38 佐藤光 * 木澤悟 Stabilization of the Wheeled Inverted Pendulum with Optimal Robust Servo System * ( 平成 年 月 日受理 ) 1. 緒言 近年, セグウェイ等に代表されるような倒立 輪ロボットの開発, 研究が盛んに行われている そこで本研究では, 実際に一から倒立 輪ロボットを製作し, そして製作したロボットをモデル化することにより, 線形制御理論を応用して, 倒立 輪ロボットの倒立の実現を目指した 倒立 輪ロボットは, 不安定なシステムかつ, モデル化においても, 非線形な運動方程式で実現される つまり, 摩擦係数等の不確定要素も多く, 物理パラメータの同定は困難な課題である, また車輪の角度や本体の姿勢等もリアルタイムで計測しながら制御する必要がある 本研究では, システムのモデル化誤差を考慮し, 倒立したまま車輪の速度を一定に保つことができるように, 最適ロバストサーボ制御法をコントローラに用いて, 倒立 輪ロボットの倒立制御を試み, 提案した制御手法がこの倒立問題に有効であることを報告する 2. 倒立 2 輪ロボットの概要 2.1 倒立 2 輪ロボットの設計 に設計した倒立 輪ロボットの設計図とそ * 秋田高専専攻科学生 Fig. 1 倒立 2 輪ロボットの設計図及び外観 の外観を示し, 以下に設計方針を示す 上部と中央部に梁を取り付けることで本体の剛性を上げた モータを下部に取り付けることで重心を低くした モータは一軸とする 2.2 倒立 2 輪ロボットのシステムの概要 に倒立 輪ロボットのシステムを示す にこのシステムの模式図を示す 実験システムは, ディジタルコントロールボード, ターミナルボード, 電源ユニット, モータアンプから構成される 輪ロボットの車輪角度はエンコーダ付き モータにより, また本体の姿勢角度はエンコーダに取り付けられた棒状の部品から随時検出され 平成 23 年 2 月
39 Fig. 2 倒立 2 輪ロボットシステム Fig. 4 倒立 2 輪ロボットモデル Table. 1 物理パラメータ る そしてその角度情報がターミナルボードを介して, ディジタルコントロールボードに送られ, その情報をもとに を利用して 内で演算され, 制御に必要なモータトルクの指令信号がディジタルコントロールボード上の コンバータを介してモータアンプに送られ, モータをコントロールする 3. 運動方程式の導出 Fig. 3 システムの模式図 倒立 輪ロボットの安定化制御を行うためには, 倒立 輪ロボットの入出力関係を表した動的システム表現が必要ここで に倒立 輪ロボットのモデル図を示す ここで用いるパラメータを に示す 運動方程式を導出するために以下のラグランジュの運動方程式を用いた d T T U F - + + =τ i () dt q i qi qi q i ここで, 運動エネルギ, 位置エネルギ, 散逸エネルギである 次節に各運動エネルギの導出を示す 本体の質量 [] 車輪 ( タイヤ ギヤ シャフト ) の質量 車軸から測った本体の重心の距離 3.1 モデルの運動方程式 ) 車輪の回転運動エネルギについて T wr = J(φ +θ ) () である ここで, 回転角速度 φ だけではなく本体の姿勢角速度 θ が加算されているのは傾斜することによっても車輪が回転するためである ) 傾斜方向の回転運動エネルギについて T Br = Iθ () ) 車輪の並進運動エネルギについて車輪 つの質量が m なので重心が車輪の中心にあると考える 車輪の動く速さを v とすると v=r(φ +θ ) [] [] 車輪の半径 [] 本体の慣性モーメント [ ] 車輪の慣性モーメント [ ] 車輪の粘性摩擦係数 φ [ ] 本体の粘性摩擦係数 θ [ ] 重力加速度 [ ] であり, これより車輪の並進運動エネルギは () 秋田高専研究紀要第 46 号
40 佐藤光 木澤悟 +φ T wt = mv = mr(θ +θ φ ) である ) 本体の並進運動エネルギについて T Bt = MV = M(V x +V y ) = M{φ r +θ (r +rlcosθ+l ) +φ (r θ +rlcosθ)} () () であるここで, ロボットの台車の位置にあるタイヤの回転中心 ( ) は x c =r(θ+φ) y c = () 振子となる倒立 輪ロボットの本体の重心 ( ) は x p =x c +Lsinθ y p =y c +Lcosθ である また, 本体の重心速度は V x = d x p =r(φ +θ )Lθ dt V y = d y p =Lθ sinθ dt cosθ である ) 全体の運動エネルギについて )~) より, 全体の運動エネルギは T=T wr +T Br +T wt +T Bt + Iθ = J(φ +θ ) + mr(θ +θ φ +φ () () ) ばよいので, F= D φ + D θ θ () ) ラグランジュの式の各項の導出 ここで q =θ としてラグランジュの式を利用す q =φ る するとラグランジュの式の各項は以下 T =(J+mr +Mr +MrLcosθ)φ θ +(J+I+mr +Mr +ML +MrLcosθ)θ () T =(J+mr +Mr )φ φ +(J+mr +Mr +MrLcosθ)θ () d T = { J+(m+M)r +MrLcosθ}φ dt θ -MrLsinθ φ d dt +{ J+I(m+M)r +ML +MrLcosθ}θ -MrLθ sinθ θ () T = { J+(m+M)r }φ φ +{ J+(m+M)r +MrLcosθ}θ -mrlθ sinθ T =(-θ θ =-MrLθ θ () rlsinθ-φ θ sinθ-φ θ rlsinθ) M MrLsinθ () T =() U =-MgLsinθ() U =() φ θ φ + M{φ r +θ (r +rlcosθ+l ) +φ (r θ +rlcosθ)} () ) ポテンシャルエネルギについて重心の位置エネルギを考えればよいのでつまり, U=MgLcosθ () ) 散逸エネルギについて車輪の摩擦および, 車軸まわりの摩擦を考慮すれ F =Dθ θ () θ φ 以上の 式をまとめると F =Dφ φ () d T T U F - + + = dt θ θ θ θ d T T U F - + + =τ dt φ φ φ φ また, これより () 平成 23 年 2 月
41 {(m+m)r +J }θ +{(m+m)r +MrLcosθ+J }θ -θ mrlsinθ+d φ =τ {(m+m)r +MrLcosθ+J }θ () +{(m+m)r +MrLcosθ+ML +J+I }θ J m φ A=τ A - τ () n である また, これに () を代入すると J m φ +(a+b)θ +D A=τ A - aφ φ (n ) () n -θ MrLsinθ-MgLsinθ+D θ θ = 式の見通しを良くするために a=(m+m)r +J b=mrl c=ml +I d=mgl とおく,() は aφ +(a+bcosθ)θ -θ mrlsinθ+d φ =τ () また, φa =n より φ aφ +(a+b)θ +D φ +J m n φ =nτ A () 次にタイヤに与えるトルク τ とモータ端子電圧 の関係を導出 に直流モータ回路を示す (a+bcosθ)φ +{a+bcosθ+c}φ -θ bsinθ-dsinθ+d θ θ = () ここで, 倒立状態にあるとき,θ および θ は小さな値をとると考えられ,θ が小さい間は () は線形化できると考えられる つまり cosθ sinθ θ θ = = = とおけば, 線形化した運動方程式は aφ +(a+b)θ +D φ φ =τ (a+b)φ +{a+b+c}θ -dθ+d θ θ = () 3.2 モータの動特性を考慮した運動方程式の導出モータの動特性を考慮してタイヤに与えられるトルク τ とモータ端子電圧 との関係を考える また, に モータのモデルを示す モータの慣性モーメント, モータ発生トルク τ A, 負荷トルク τ とすると角運動方程式は Fig. 6 直流モータ回路 電機子抵抗, 逆起電力, モータ端子電圧である インダクタンスを無視すれば次の関係が得られる V E = i+e b 逆起電力 はモータの回転速度に比例するので e b =K E φ A () () である ただし は誘起電圧定数である また, (),() より i= V E-e b = V E-K E φ A = V E-K E nφ () またモータ発生トルク τ A は電機子の電流に比例するので次式がいえる τ A =K T i ただし, はトルク定数 また,() に () を代入すると Fig. 5 DC モータ V E -K E nφ τ A =K T i=k T () 秋田高専研究紀要第 46 号
42 佐藤光 木澤悟 よって(),() よりタイヤに与えるトルクτ とモータ端子電圧 との関係は aφ +(a+b)θ =nτ A -J m n φ +D φ φ =J m n φ - K TK E n φ K TnV E である さらに () を変形すると () (a+j m n )φ +(a+b)θ + Dφ K TK E n φ = K Tn VE () 以上より, モータの動特性を考慮した運動方程式は (a+j m n )φ +(a+b)θ + Dφ K TK E n φ = K Tn VE (a+b)φ +(a+b+c)θ -dθ+d θ θ = 式の見通しを良くするために, () θ =a θ+a θ +a φ =a θ+a θ +a である ただし, φ +b V E φ +b V E Ed ED θ FG a =-, a =, a =-, F -EH F -EH F -EH Fd FD θ GH a =, a =-, a = F -EH F -EH F -EH EK T n HK T n b =, b =- R(F a -EH) R(F a -EH) () である () の運動方程式を状態方程式に変換すると θ θ θ = a a a θ + b V E () φ a a a φ b よって連続時間システムの状態方程式は x =Ax+Bu y=cx () E=a+J m n F=a+b G=D θ K TK E n H=a+b+c () である ただし, A= a a a B= b C=[ ] a a a b とおくと,() は, x=[θ θ φ ] T u=v E () Eφ Eφ +Fθ +Gφ +Hθ -dθ+d = K Tn VE () θ θ = 3.3 状態方程式の導出 () のφ を消去すると Ed ED θ =- θ FG FK θ+ θ - φ T n + V E F -EHF -EH F -EH R(F a -EH) () また,() のθ を消去すると Ed ED φ = θ GH HK θ- θ + φ T n - V E F -EHF -EH F -EH R(F a -EH) () ここで,() は ここで,θ: 本体の姿勢角度,θ : 本体の姿勢角速度, φ : 車輪の角速度とする 3.4 制御系の設計 輪倒立振子の安定化制御に対し, 外乱抑制とロバスト性に適した最適ロバストサーボ制御をコントローラに用いた 制御系の設計手順は, 線形時不変システム () に対し, 制御則は最適レギュレータの枠組みで設計するので, 次の 次形式評価関数を考える J= { xt C T Cx+ru } dt = { y (t)+ru(t)} dt () 出力 が で でない目標値に一致したとすると, 入力 も で でない一定値になる したがって () の評価関数は無限大となってしまい, このままこの評価関数を用いることができない こ 平成 23 年 2 月
43 こで新しい制御入力をv=u とし拡大制御ベクトル x e =[x T u] T を定義すると, 拡大システムは ただし x e=a e x e +B e v y=c e x e A B A e = B e = C e =[C ] () 一方, 適当なフィードバックによりサーボ系を構成して, 出力 が目標値 r に追従したとき, における定常状態での,,, の定常値をそれぞれ x, u, y, v ととすると () より x =Ax +Bu = u =v = y =Cx =r ここで定常値からの変動分を δx(t)=x(t)-x δu(t)=u(t)-u () () とおき,() から() を引き, 偏差系の状態ベクトルを δx e =[δx T δu] T のように定義すると x -x =A(x-x )+B(u-u ) u -u =v-v =v y-y =C(x-x ) δx e=a e δx e +B e v e=y-r=c e δx e () したがって, この拡大偏差システムに対してδx e となるような制御を行うと が達成され, 偏差 e=y-r はこれを実現するために次の評価関数を定義 J e = { δxet Q e δx e +r e v } dt () ただし,Q e は準正定行列 は正の定数であり, ここで,Q e =C e T C e とすると () は J e = { e (t)+r e v (t)} dt () である つまり偏差と制御入力に関する 次形式評価関数すなわち, それは, 拡大偏差系 () に対して() の評価関数を最小化し, δx e を達成する制御問題は通常の最適レギュレータを用いて実現できる ここで, サーボシステムが可制御, 可観測であるためには (A e B e ) が可 制御,(A e C e ) が可観測であることが必要十分条件である 以上, ロバストサーボ系の設計問題は最適レギュレータ問題に帰着され,() の評価関数を最小にする制御入力はリカッチ方程式 A e T P e +P e A e -r e - P e b e b e T P e +Q e = を満たす正定解 を用い v=r e b T e P e δx e =K δx e δu で与えられる ここで K e =r e b e T P e () () () はフィードバックゲインである しかしながら, 状態量が δx と δu のフィードバックで表されているため, このままでは実現できない そこで () に () を代入する v=k e δx =K e x(t)-x =Ke x(t) -K e x () δu u(t)-u u(t) u 一方,() より次のように行列に変形できる これより AB x = r C u x AB - = u C r また() は () () x AB x = () y C u これより x AB - = x u C y () に () と () を代入すれば () v=k e x -K e x AB - AB - =Ke x -Ke u u C y C r - AB - AB - =K e x =Ke x () C y-r C -e ここで K e AB -=[K K ] C 秋田高専研究紀要第 46 号
44 佐藤光 木澤悟 とおくと, もともと - AB u =v=k e x =[K K ] x C -e -e であるから() を~まで積分すると t t u=k (t)dt-k e(t)dt x =K [x] t t -K e(t)dt =K x(t)-k x()-k e(t)dt () () となり, 比例積分型の制御則が得られる 初期状態 () のとき制御則のブロック線図は のようになる ) t となった 次に最適ロバストサーボ制御に基づいたコントローラを設計するために () のリカッチ方程式において重み関数を試行錯誤的に以下のように設計した r e =Q e = () とした また, 本コントローラはサーボ仕様なので, 車輪の角速度の目標値 r(t)=φ (t) を[] とした コントローラのゲインは を用いて計算した結果, K e AB -=[k k ]=[] C () Fig. 7 最適ロバストサーボシステム 4. 実験 ここでは, 連続時間システム () に対し, 実際に倒立実験を行う 制御手法は第 章で示した最適ロバストサーボ制御を用いて行った はじめにコントローラを設計するためにモデルの物理パラメータを代入した結果,() に示した倒立 輪ロボットシステムの運動方程式は x = - - x+- u - - - () となった 次に に でプログラミングした最適ロバスト制御に基づいたブロック線図を示す 図中のフィードバックゲイン k および k は () より得られたものである また, 倒立 輪ロボットの姿勢角及び車輪の角度はロータリーエンコーダが検出しているが, 姿勢の角速度及び車輪の角速度は, 次式のフィルタ付き微分器を設計して情報を検出している G(s)= s s+s フィルタはローパスフィルタであり, ノイズ等の不要な高周波成分を取り除いている 上で記述されたプログラムは 社のディジタルボードとそのソフトウェア にて直接, 倒立 輪ロボットシステムを駆動する 次に設計した最適ロバスト制御に基づく, 倒 Fig. 8 倒立二輪ロボットのブロック線図 平成 23 年 2 月
45 Fig. 9(a) 倒立 2 輪ロボットの移動応答 Fig. 9(d) 車輪の角速度 φ の時間応答 が分かる () は本体の姿勢角速度 θ での時 間応答である 図より,[] に漸近していることが分かる () は車輪の角速度 φ の時間応答である 本コントローラの仕様において車輪の角速度をr(t)=φ (t)=[] としたが, 図においても車輪はほぼ静止していることが分かる 以上の実験の結果より, 本研究で設計した倒立 輪ロボットシステムに対し, 最適ロバストサーボ制御に基づいたコントローラを設計して, 適用した結果, 十分倒立のための安定性が確保できたことが確認できた Fig. 9(b) 本体の姿勢角度 θ の時間応答 5. 結言 本研究では, 最適ロバストサーボ制御法をコントローラに用いて, 倒立 輪ロボットの倒立制御を試み, 提案した制御手法がこの倒立問題に有効であるかを検討した その結果, 倒立 輪ロボットの安定化制御において, 最適ロバストサーボ制御は有効であることが確認された 参考文献 Fig. 9(c) 本体の姿勢角速度 θ の時間応答 立 輪ロボットの安定化の実験結果について述べる () は, 移動距離の応答である 図より車輪は 秒後にほぼ静止していることがわかる () は本体の姿勢角度 θ の時間応答である 図より, 姿勢角が [ ] に漸近し安定していること ) 天野耀鴻 やさしいシステム制御工学, 森北出版,() ) 青木貴伸, 蔵本一峰, 森口肇, 吉田郷弘 使える!, 講談社,() ) 木田隆 フィードバック制御の基礎, 培風館,() ) 小林一行 ハンドブック, 秀和システム,() ) 橋本洋志, 他 で学ぶシステム制御の基礎, ホーム社,() 秋田高専研究紀要第 46 号