39 Fig. 2 倒立 2 輪ロボットシステム Fig. 4 倒立 2 輪ロボットモデル Table. 1 物理パラメータるそしてその角度情報がターミナルボードを介して, ディジタルコントロールボードに送られ, その情報をもとにを利用して内で演算され, 制御に必要なモータトルクの指令信号が

Size: px

Start display at page:

Download "39 Fig. 2 倒立 2 輪ロボットシステム Fig. 4 倒立 2 輪ロボットモデル Table. 1 物理パラメータるそしてその角度情報がターミナルボードを介して, ディジタルコントロールボードに送られ, その情報をもとにを利用して内で演算され, 制御に必要なモータトルクの指令信号が"

あつとしかがんじ
6 years ago
Views:

1 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 月

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 号

3 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 月

4 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 号

5 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 月

6 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 号

7 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 月

8 45 Fig. 9(a) 倒立 2 輪ロボットの移動応答 Fig. 9(d) 車輪の角速度 φ の時間応答が分かる () は本体の姿勢角速度 θ での時間応答である図より,[] に漸近していることが分かる () は車輪の角速度 φ の時間応答である本コントローラの仕様において車輪の角速度をr(t)=φ (t)=[] としたが, 図においても車輪はほぼ静止していることが分かる以上の実験の結果より, 本研究で設計した倒立輪ロボットシステムに対し, 最適ロバストサーボ制御に基づいたコントローラを設計して, 適用した結果, 十分倒立のための安定性が確保できたことが確認できた Fig. 9(b) 本体の姿勢角度 θ の時間応答 5. 結言本研究では, 最適ロバストサーボ制御法をコントローラに用いて, 倒立輪ロボットの倒立制御を試み, 提案した制御手法がこの倒立問題に有効であるかを検討したその結果, 倒立輪ロボットの安定化制御において, 最適ロバストサーボ制御は有効であることが確認された参考文献 Fig. 9(c) 本体の姿勢角速度 θ の時間応答立輪ロボットの安定化の実験結果について述べる () は, 移動距離の応答である図より車輪は秒後にほぼ静止していることがわかる () は本体の姿勢角度 θ の時間応答である図より, 姿勢角が [ ] に漸近し安定していること ) 天野耀鴻やさしいシステム制御工学, 森北出版,() ) 青木貴伸, 蔵本一峰, 森口肇, 吉田郷弘使える!, 講談社,() ) 木田隆フィードバック制御の基礎, 培風館,() ) 小林一行ハンドブック, 秀和システム,() ) 橋本洋志, 他で学ぶシステム制御の基礎, ホーム社,() 秋田高専研究紀要第 46 号

Microsoft Word - 秋田高専紀要論文kai2

Microsoft Word - 秋田高専紀要論文kai2 フライホイール型倒立振子の安定化制御工藤駿 * 木澤悟 Stbiliztion of Inverted Pendulu with Inerti Rotor Shun KUDOU * nd Storu KIZAWA ( 平成年月 5 日受理 ) Abstrt-his pper desribes the ontrol of n under tuted syste lled the Inverted