77 (..3) 77- Simulation of Disturbance Compensation Control of Dual Manipulator for an Inverted Pendulum Robot Using The Extended State Observer Luis Canete Kenta Nagano, Takuma Sato, Luis Canete,Takayuki Takahashi *, ** *Fukushima University, **Graduate School of Fukushima University. : (Human Support Robot) (Inverted Pendulum) (Dual Manipulator) (Extended State Observer) : 96-96 Tel.: (4)548-559 Fax.: (4)548-559 E-mail: kenta@rb.sss.fukushima-u.ac.jp.. I-PENTAR, ) I-PENTAR Fig. 3) Fig. I-PENTAR
レータの動作を外乱と見なして補償を行う ま た マニピュレータ側では未知重量の物体の把 持等の外乱に加え 倒立振子型ロボットの揺れ を外乱と見なして補償を行う 本論文ではマニ ピュレータ側での外乱補償制御のシミュレーショ ンについて述べる. Front view 拡張状態オブザーバ 本章では 一般的な n 次の非線形システムに 対する拡張状態オブザーバ 4) (Extended State Side view Observer 以降 ESO) について述べる Fig. Inverted Pendulum Type Assistant Robot(I-PENTAR) システムの入力を u(t) 出力を y(t) として 一入出力の n 次の非線形システムを考える y n (t) = f (y (n ) (t), y (n ) (t),, y(t), w(t)) +bu(t) () ここで w(t) は有界な外乱 b は定数であり f は外乱を含んだシステムすべての動特性である 以降 記述を簡単にするため f (y (n ) (t), y (n ) (t),, y(t), w(t)) を f と記述する ここで h = f という h を定義する すると式 () は状態空間 モデルとして次のように表現できる Fig. 8 D.O.F dual manipulator. x = x... 研究目的 x n = xn 倒立振子型ロボットにマニピュレータを搭載 x n = xn+ + bu し様々なタスクを行わせる場合 未知の外乱に x n+ = h(x, u, w, w ) 加え マニピュレータの動作がロボット本体に影 y = x 響を与えることが予想される しかし 対象と するマニピュレータがこれらの外乱と比較して 非力で軽量な場合 マニピュレータを搭載した ロボットを正確にモデリングし 制御を行うこ とは得策ではない そこで 本研究では倒立振 子型ロボットにおけるマニピュレータの動作の 影響に対して 制御系を本体側とマニピュレー タ側の つに分離し それぞれで外乱の補償を 行い制御系間の情報のやりとりを最小限にする ここで x = [x, x,, xn+ ]T は状態変数で ある 式 () における xn は f であり その拡張 状態は xn+ と表され オブザーバは xn+ 用い ることで y と f を推定することができる この ようなオブザーバを拡張状態オブザーバ (ESO) と呼ぶ ESO はモデル化されていないダイナミ クスに加わる外乱と y を推定することが可能で ある 制御系を提案する 外乱補償については 本体 側では段差等の外部からの外乱に加え マニピュ () 式 () のシステムにおいて 入力として u と y を与えた ESO は次のように表現できる
ˆx = ˆx + l (x ˆx ). ˆx n = ˆx n + l n (x ˆx ) ˆx n = ˆx n+ + l n (x ˆx ) + bu ˆx n+ = l n+ (x ˆx ) (3) ˆx = [ˆx, ˆx,, ˆx n+ ] T l i (i =,,,, n+) ESO u = u ˆf b (4) ˆf ˆf = f () y (n) (t) u (5) y ESO u PD 3. Fig. 3 Table q = [ψ, θ w, θ, θ ] T M(q) q + C(q, q) + V q + G(q) = Eτ w + T τ + T τ + Dτ d (6) M(q) q C(q, q) V q G(q) Fig. 3 Model for dual manipulatar and I-PENTAR Table Control variables and parameters Symbol Unit Description ψ rad Inclination angle of CoG θ w rad Rotational angle of wheel θ rad Rotational angle of upper link θ rad Rotational angle of lower link M g Kg Mass of body m w Kg Mass of wheel m Kg Mass of upper link m Kg Mass of lower link l g m Length between the origin of body coordinates and CoG l m Distance of gravity of upper link l m Distance ofgravity of lower link r w m Radius of wheel τ w Nm Motor torque of wheel τ Nm Motor torque of upper link τ Nm Motor torque of lower link g m/s Gravity acceleration 4. ESO PD 4. 5[s] 8[s] θ.5[rad] θ [rad] 3
.8.8.6.4.6.4.. Fig. 4 Disturbance compensation control in the joint space by PD control 5[s] [s] [Nm] 4. PD ESO Fig. 4 Fig. 5 Fig. 4 PD.[rad] Fig. 5 ESO PD ESO PD 5. Fig. 5 Disturbance compensation control in the joint space by ESO ESO PD 5. P w P s P e (7) P d (x d, y d ) = [x d, y d ] P w (x w, y w ) = [r w θ w, r w ] P s (x s, y s ) = [l ws sin ψ, l ws cos ψ] P e (x e, y e ) = [x d, y d ] (7) (8) l ws = l body l se = (x e x s ) + (y e y s ) l we = (x e x w ) + (y e y w ) (8) Fig. 6 4
() q d = J T (JJ T + λ I) dp d dt () λ λ = { when ˆσn > ɛ ɛ ˆσ n otherwise (3) Fig. 6 Model for the calculation of desired value θ d = cos ( l ws + l se l we l ws l se ) cos ( l arm + l se l arm l arm l se θ d = π cos ( l arm + l arm l se l arm l arm ) ) (9) P e (x e, y e ) x e = r w θ w + l body sin ψ + l arm sin(θ ψ) +l arm sin(θ + θ ψ) y e = r w + l body cos ψ l arm cos(θ ψ) l arm cos(θ + θ ψ) () (3) ɛ ˆσ n J I-PENTAR 5) ( q b = J T (JJ T + λ I) Pe ψ ψ + P ) e θw (4) θ w ()(4) q d = q d q b (5) θ, θ J = [ xp θ y p θ ] x p θ y p θ x p θ = l arm cos(θ ψ) +l arm cos(θ + θ ψ) x p θ = l arm cos(θ + θ ψ) y p θ = l arm sin(θ ψ) +l arm sin(θ + θ ψ) y p θ = l arm sin(θ + θ ψ) () 5 5. [s] 3[s] P d (x d, y d ) = [,.] P d (x d, y d ) = [.,.7] 5[s] [s] [Nm] 5[Nm]
.8 3.7.6.5 displacement[m].4.3. -. - -. -3 (a) Tip position in the case of PD control (b) Each angle in the case of PD control.8 3.7.6.5 displacement[m].4.3. -. - -. -3 (c) Tip position in the case of ESO (d) Each angle in the case of ESO 8 6 torque [Nm] 4 - (e) Each joint torque in the case of ESO Fig. 7 Simulation results in the case of applying step disturbance to each joint of manipulator 6
5.3 5.3. PD ESO Fig. 7(a) Fig. 7(c) PD ESO Fig. 7(b) Fig. 7(d). ESO Fig. 7(e) Fig. 7(a) Fig. 7(b) PD.7[m] Fig. 7(c) Fig. 7(d) ESO.[m] τ = 3[Nm],τ = 3[Nm] Fig.7(e) ESO. ESO 5.4 5.3. PD ESO Fig. 8(a) Fig. 8(c) PD ESO Fig. 8(b) Fig. 8(d) ESO Fig. 8(e) Fig. 8(a) Fig. 8(b) PD Fig. 8(b).6[m] Fig. 8(c) Fig. 8(d) ESO PD.[m] Fig.8(e) ESO. ESO 5.4 PD ESO x [s] [s] P d (x d, y d ) = [,.] P d (x d, y d ) = [.,.7] ESO Fig. 9(a) Fig. 9(b) 7
.8 3.7.6.5 displacement[m].4.3. -. - -. -3 (a) Tip position in the case of PD control (b) Each angle in the case of PD control.8 3.7.6.5 displacement[m].4.3. -. - -. -3 (c) Tip position in the case of ESO (d) Each angle in the case of ESO 8 6 torque [Nm] 4 - (e) Each joint torque in the case of ESO Fig. 8 Simulation results in the case of applying step disturbance to robot body of manipulator 8
displacement [m].8.7.6.5.4.3.. -. 3 - (a) Tip position in the case of ESO 6. ESO ESO ESO I-PENTAR - -3 (b) Each angle in the case of ESO Fig. 9 Examination of the initial response Fig. 9(a) Fig. 9(b) x 6) ),, Luis Canete,.,, P-I4,. ) Luis Canete,Takayuki Takahashi. Disturbance Compensation in Pushing, Pulling, and Lifting for Load Transporting Control of a Wheeled Inverted Pendulum Type Assistant Robot Using The Extended State Observer, Intelligent Robots and Systems, October 7-, Vilamoura,Algarve Portugal,. 3),,,,. - -, 9, P-G6 9. 4),,,.., Vol. 8, No., pp. 86-93,. 5), Dragomir N. Nenchev,., 5, P- N-7 5. 9
6),, Luis Canete,., 77, 77-,.