2
輪移動ロボットの追従制御
2016SC020細江皓介 指導教員：陳幹1
はじめに
移動ロボットの制御はパスフォローイング制御,トラッ キング制御などに分類される.本論文では,移動ロボット のトラッキング制御について安定性を考える際に,安定と なるべき別の関数を考慮することでコントローラを設計 した.2
移動ロボットのモデリング
図1 移動ロボットの概略図 図1は制御対象の移動ロボットの概略図である.移動ロ ボットの運動学は次のように定義される.先行車の運動学 モデルも同様に示され,本論文では下付き文字のpは先行 車に関するものである. ˙ η = xy˙˙ ˙ ϕ = [ cos ϕ 0 sin ϕ 0 0 1 ] [ v ˙ ϕ ] = [ r 2cos ϕ r 2cos ϕ r 2sin ϕ r 2sin ϕ r 2b − r 2b ] [ ˙ θr ˙ θl ] (1) 表1は移動ロボットの物理パラメータである.[1]より, 移動ロボットの動力学モデルは次のように表される. M ¨θ + C( ˙η) ˙θ + D ˙θ = τ (2) M = [ r2 4b2(mb2+ I) + Iw r 2 4b2(mb2− I) r2 4b2(mb 2− I) r2 4b2(mb 2+ I) + I w ] C( ˙η) = [ 0 r2b2mcd ˙ϕ −r2 2bmcd ˙ϕ 0 ] , D = [ d11 0 0 d22 ] θ = [ θr θl ] T , τ = [ τr τl ] T m = mc+ 2mw I = mcd2+ 2mwb2+ Ic+ 2Im 表1 パラメータ 2つの車輪の中点 P 0 [m] 移動ロボットの重心 Pc [m] P 0− Pc間の距離 d [m] 2つの車輪の距離 2b [m] 車輪の半径 r [m] 車体の重量 mc [kg] 車輪の重量 mw [kg] 車輪の回転角度 θr, θl [rad] トルク τr, τl [N m] 車体の重心回りの慣性モーメント Ic [kgm2] 車輪の回転軸回りの慣性モーメント Iw [kgm2] 車輪の重心回りの慣性モーメント Im [kgm2] 減衰係数 dii3
運動学モデルの追従制御
移動ロボットのコントローラを設計するにあたり用いる 追従偏差は次のように示される. [ x e ye ϕe ] = [ cos ϕ sin ϕ 0 − sin ϕ cos ϕ 0 0 0 1 ] [ x p− x yp− y ϕp− ϕ ] ここで, ˙ye について移動ロボットが平衡な状態を考え, 別の式を導入することを考える.xe, ϕeの代わりに, ¯xe = xe− c1ϕy˙ e([2]参照), e = vpϕeを用いてコントローラを設 計する. リアプノフ関数は次のものを選択する. V1= 1 2x¯e 2+1 2y 2 e+ 1 2e 2 (3) 次 の 速 度 入 力 av, 姿 勢 角 の 角 速 度 の 入 力aw に つ い て, ˙V1のv, ˙ϕに代入したときにV˙1 ≤ 0となるものを考 える.c1, c2, c3, c4は正の設計パラメータである. av = h + c2x¯e aw= ˙ϕp+ 1 vp (yesin ϕe ϕe + ˙vpϕe+ c3e), ϕe̸= 0 (aw= ˙ϕp+ ye vp , ϕe= 0)(aw= c4ye, vp= 0) (4) h = vpcos ϕe− c1a˙wye+ c1aw(xeaw− vpsin ϕe) vp> 0, c1, c2, c3, c4> 0 av, aw を V˙1 の v, ˙ϕ に 代 入 す る と, ˙V1 = −c2x¯e2 − c1ϕ˙2ye2− c3e2となる. 車輪の角速度入力θrc˙ , ˙θlcは次のように速度入力av,姿 勢角の角速度入力awを用いて得られる. ˙ θc= [ ˙ θrc ˙ θlc ] = [ 1 r b r 1 r − b r ] [ av aw ] (5) 14
動力学モデルの追従制御
動力学モデルに対して, 運動学モデルで得られたコン トローラを適用するためには, ˙θ → ˙θc, t → ∞を満たす 必要がある.ここでバックステッピング法に基づき, ¯v = v− av, ˙¯ϕ = ˙ϕ− awを定義する. これに伴い次のzも定義 する. z = [ z1 z2 ] = ˙θ− ˙θc= [ ˙ θr− ˙θrc ˙ θl− ˙θlc ] = [ 1 r b r 1 r − b r ] [ ¯ v ˙¯ ϕ ] = [ ˙ θr− (1rav+braw) ˙ θl− (1rav−braw) ] (6) 次のリアプノフ関数を考える.これを用いてシステムの安 定性を示すことができれば, ˙θ→ ˙θc, t→ ∞が保証される. V2= 1 2x¯e 2+1 2y 2 e+ 1 2e 2+1 2z 2 1+ 1 2z 2 2 (7) 次の車輪の角加速度入力ua, ubについて, ˙V2のθ¨r, ¨θlに 代入したとき, ˙V2≤ 0となるようなものを考える.c5, c6は 正の設計パラメータである. ua= 1 ra˙v+ b ra˙w+ r 2x¯e+ r 2bvpe− c5z1 ub= 1 ra˙v+ b ra˙w+ r 2x¯e− r 2bvpe− c6z2 (8) ua, ubをV˙2のθ¨r, ¨θlに代入するとV˙2=−c2x¯e2−c1ϕ˙2y2e− c3e2− c5z21− c6z22となる. トルク入力を得るために,式(2)の動力学モデルから,¨θ = uとして次のフィードバックを導入する. τ = M u + C( ˙η) ˙θ + D ˙θ (9) [3]によって,uに対して直接 u = [ua ub] T を代入した 場合,移動ロボットの実際の追従を保証することができ ないと述べられている.ここで[3],[4]で提案されている u = [ua ub] T + K( ˙θc− ˙θ)という仮想入力を導入する.K は正の設計パラメータc7を用いてK = diag(c7, c7)と示 される.5
シミュレーション
物 理 パ ラ メ ー タ の 値 は,b = 0.75, r = 0.15, d = 0.3, mc = 30, mw = 1, Ic = 15.625, Iw = 0.005, Im = 0.0025, d11 = d22 = 0である.設計パラメータは c1 = 0.1, c2 = 1, c3 = 5, c4 = 3, c5 = 1, c6 = 1, c7 = 10 である.図2,3 は運動学モデルのコントローラのシミュ レーション結果を示すものである.初期値は(x, y, ϕ) = (0, 0,π 2), (xp, yp, ϕp) = (0.45, 0, π 2)であり,先行車はvp= 0.7でY 軸方向に直進している.図4は同様のセットアッ プでトルク入力を得た結果を示している.6
おわりに
シミュレーション結果より,先行車が直進する場合,先行 車に対し追従車はほぼ誤差を残さず追従することができて いる. 図2 移動ロボットの座標 図3 追従偏差 図4 トルク入力参考文献
[1] K. D. Do, Z. P. Jiang, and J. Pan “Simultane-ous Tracking and Stabilization of Mobile Robots: An Adaptive Approach,” IEEE TRANSACTION ON AUTOMATIC, VOL. 49, NO. 7, JULY 2004, pp1147-1152
[2] Zhong-Ping Jiang, Henk Nijmeijer “Tracking Con-trol of Mobile Robots: A Case Study in Backstep-ping,” Automatica, Vol. 33, No. 7, 1997, pp1393-1399
[3] R. Fierro and F. L. Lewis “Control of a Nonholo-nomic Mobile Robot: Backstepping Kinematics into Dynamics,” Journal of Robotic Systems 14(3), 1997, pp149-163
[4] Huiyu Wang, Takanori Fukao, and Norihiko Adachi “An Adaptive Tracking Control Approach for Non-holonomic Mobile Robot,” 1999 IFAC, 14th Trien-nial World Congress, Beijing, P.R China, pp8184-8189
