SICE東北支部研究集会資料(2012年)

Size: px

Start display at page:

Download "SICE東北支部研究集会資料(2012年)"

しなつさくいし
9 years ago
Views:

1 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) : Tel.: (4) Fax.: (4) [email protected].. I-PENTAR, ) I-PENTAR Fig. 3) Fig. I-PENTAR

**Graduate School of Fukushima University.

2 レータの動作を外乱と見なして補償を行うまたマニピュレータ側では未知重量の物体の把持等の外乱に加え倒立振子型ロボットの揺れを外乱と見なして補償を行う本論文ではマニピュレータ側での外乱補償制御のシミュレーションについて述べる. 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 は次のように表現できる

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

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

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.

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

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 ψ]

5 () 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]

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 )

6 displacement[m] (a) Tip position in the case of PD control (b) Each angle in the case of PD control displacement[m] (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

position in the case of ESO (d) Each angle in the case of ESO 8 6 torque [Nm] 4 - (e) Each joint

7 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 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(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 displacement[m] (a) Tip position in the case of PD control (b) Each angle in the case of PD control displacement[m] (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

9 displacement [m] (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 ,. 5), Dragomir N. Nenchev,., 5, P- N

Disturbance Compensation in Pushing, Pulling, and Lifting for Load Transporting Control of a Wheeled Inverted Pendulum Type Assistant Robot Using The

10 6),, Luis Canete,., 77, 77-,.

数値計算：常微分方程式

数値計算：常微分方程式 ( ) 1 / 82 1 2 3 4 5 6 ( ) 2 / 82 ( ) 3 / 82 C θ l y m O x mg λ ( ) 4 / 82 θ t C J = ml 2 C mgl sin θ θ C J θ = mgl sin θ = θ ( ) 5 / 82 ω = θ J ω = mgl sin θ ω J = ml 2 θ = ω, ω = g l sin θ = θ ω ( )