二車輪型倒立振子の姿勢制御

二車輪型倒立振子の姿勢制御

2009SE136

小西孝典

指導教員

:

陳幹

1

はじめに

本研究の目的はVstone株式会社から販売されている 二車輪型倒立振子ロボットのBeatoBalancerDuo(以下 BBD)の姿勢制御を目的とする.

2

制御対象とモデリング

BBDは二つの車輪の上部にそれぞれモータが搭載さ れており,モータの回転を摩擦によって車輪に伝達する 仕組みを持っている. またロータリーエンコーダにより 左右の車輪の回転角を, ジャイロセンサにより車体の角 速度を得て, 倒立制御を実現している[1]. BBDは2本 の単三電池を主電源としており, 実験では一本1.2[V]の 電池を用いる. 本研究ではBBDの姿勢制御鑿を行うので, 右の図1 のような簡略図を用いてに二次元平面でのモデリングを 行った. 2.1

物理定数

BeautoBalancerDuoに関する物理定数を下の表１に まとめた. 表1 物理定数 記号 名前 値 単位 Rm モータの電気抵抗 0.6818 [Ω] Kb モータの逆起電力定数 0.0014 [Vs/rad] Kt モータのトルク定数 0.0012 [Nm/A] fm モータと車体の間にある 摩擦の摩擦係数 4.9825×10−7 [ ] fw 床と車体の間にある摩擦 の摩擦係数 0 [ ] g 重力加速度 9.8100 [m/s2_] Rw 車輪の半径 0.0207 [m] Mw 車輪の質量 0.0053 [g] Mm モータの質量 0.0075 [g] Mb 車体の質量 0.1453 [kg] Jw 車輪の慣性モーメント 2.2710×10−6 [kgm2] Jm モータの慣性モーメント 3.8274×10−6 [kgm2] Jb 車体の慣性モーメント 6.6336×10−4 [kgm2] L 車輪の中心から車体の重 心までの長さ 0.0520 [m] l 車輪の中心からモータ軸 までの距離 0.0217 [m] Gr モータと車輪とのギア比 20.7 [ ] 図1 BeautoBalancerDuoのモデル図 2.2

運動方程式の導出

運動方程式の導出にはラグランジュの運動方程式を用 いる. ラグランジアン_Lを次のように定義する. L = (T1+ T2)− U (1) このとき, T1は制御対象の並進運動のエネルギー, 2は回 転運動のエネルギー, U は重力のポテンシャルエネルギ ーであり以下の式で表現することができる. T1= 1 2Mb( ˙x 2 b + ˙z 2 b) + 2( 1 2Mm( ˙x 2 m+ ˙z 2 m)) +2(1 2Mw( ˙x 2 w+ ˙z 2 w)) (2) T2= 1 2Jb ˙ ψ2+ 2(1 2JmG 2 r( ˙ψ− ˙θ) 2_{) + 2(}1 2Jw ˙ θ2) (3) U = Mbgzb+ 2(Mmgzm) + 2(Mwgzw) (4) 次に一般化力FθとFψはラグランジュの運動方程式を用 いて求め, その中に含まれる非線形項に対してマクロー リン展開を適用することで次のように表せる. Fθ= ((Mb+ Mm+ Mw)Rw2 + JmG2r+ Jw)¨θ +((MbL + Mml)Rw− JmG2r) ¨ψ (5) Fψ= ((MbL + 2Mml)Rw− 2JmG2r)¨θ +(MbL2+ 2Mml2+ Jb+ 2JmG2r) ¨ψ −(MbL + 2Mml)gψ (6) またこれらのFθとFψはモータからの外力ともいえるの で, モータから与えられるトルクとして以下のように表 現できる. Fθ= 2Gr(Kti− fmGr( ˙ψ− ˙θ) + fwθ)˙ (7)

Fψ=−2Gr(Kti− fmGr( ˙ψ− ˙θ)) (8) 次にモータの電気的振る舞いは次式で表すことができる. v = Lm˙i + Rmi + KbGr( ˙ψ− ˙θ) (9) このときコイルのインダクタンスLmは非常に小さなも のとみなせるので, Lm= 0とする. こ の 条 件 と 式 (9), (10), (11) よ り 新 た に 一 般 化 力Fθ, Fψは次のようにあらわせる. Fθ= 2G2r( KtKb Rm + fm)( ˙θ− ˙ψ) + 2Gr 2Kt Rm v (10) Fψ=−2G2r( KtKb Rm + fm)( ˙θ− ˙ψ) − 2Gr 2Kt Rm v (11) 2.3

状態空間表現

式(5), (6), (10), (11)の結果を式(12)の形にまとめる E [_¨ θ ¨ ψ ] + F [_˙ θ ˙ ψ ] + G [ θ ψ ] = Hv (12) この時,行列E, F, G, Hは以下のようになる. E = [ e11 e12 e21 e22 ] e11= ((Mb+ 2Mm+ 2Mw)R2w+ 2(JmG2r+ Jw)) e12= e21= ((MbL + 2Mml)Rw− 2JmG2r) e22= (MbL2+ 2Mml2+ Jb+ 2JmG2r) F = [ 2(fm+K_RtKb m )G 2 r −2( KtKb Rm + fm)G 2 r −2(KtKb Rm + fm)G 2 r 2(fm+K_RtKb m )G 2 r ] G = [ 0 0 0 −(MbL + Mml)g ] , H = [ _2K t Rm −2Kt Rm ] 式(12)を変形することで次の状態空間表現(13)の行列 A, B, C, xは次のように定義される. { ˙ x = Ax + Bu y = Cx (13) A = [ O2×2 I2×2 −E−1_{G −E}−1_F ] , B = [ O2×1 E−1H ] C = I2_×2 , x = [ θ ψ θ˙ ψ˙ ]

3

制御器設計

本研究では, 最適レギュレータ理論を用いてコント ローラの設計を行う[2]. こ の と き 得 ら れ た コ ン ト ー ル ゲ イ ン K と 重 み 行 列Q, Rを以下に示す. K = [ −0.1000 −39.6322 −0.1905 −5.2626] Q = diag [ 1 1 0.7 1 ] , R = 100

4

シミュレーションと実験結果

次に, 初期条件としてx = [2π 0 0 0]を与えたときの シミュレーションと, それと同じ条件の実験結果とを重 ねたグラフを以下の図2, 3, 4に示す. シミュレーショ ンと実験の時間はともに10[s]である. 図2 車輪回転角 図3 車体傾斜角 図4 入力電圧

5

おわりに

実験結果からBBDの姿勢制御ができたといえる. し かしグラフからBBDの姿勢はやや振動的であるとみら れる.これは実験データに含まれるノイズが大きいとい う理由が考えられる. そのためこれらを改善する方法と してデータ取得方法の見直しが挙げられる.

参考文献

[1] ヴィストン:『H8マイコンによる組み込みプログラ ミング入門』, オーム社(2009) [2] 川田昌克『: MATLB/Simulinkによる現代制御入門』, 東京,森北出版株式会社(2011)