77 (..3) 77- A study on disturbance compensation control of a wheeled inverted pendulum robot during arm manipulation using Extended State Observer Luis Canete Takuma Sato, Kenta Nagano,Luis Canete,Takayuki Takahashi *, ** *Fukushima University, **Graduate School of Fukushima University : (Extended State Observer) (Disturbance Compensation Control) (A Wheeled Inverted Pendulum Robot) : 96-96 Tel.:(4)548-559 Fax.: (4)548-559 E-mail: takukma@rb.sss.fukushima-u.ac.jp., ) 4 I-PENTAR(Inverted PENdulum Type Assistant Robot). I-PENTAR I-PENTAR Fig. Table I-PENTAR 7.5[kg] 3) Pushing Pulling 4) I-PENTAR Fig. Fig.
Pick and Place Tool handling Hand over Front view Side view Fig. Inverted Pendulum Type Assistant Robot(I-PENTAR) Table Hardware Specification of I-PENTAR Size.9.34[W].3[D] [m] Weight 39. [kg] D.O.F Mobie() Arm(6) Waist() Sensor Gyro sensor(), Encoder(9) Actuator DC motor() OS ARTLinux(Sampling time:[ms]) Interface FPGA(PC4) Fig. Target tasks of I-PENTAR using its robot arm. 3. ( ESO:Extended State Observer ) 5, 6) u R y R SISO(Single-Input Single-Outoput) () I-PENTAR I-PENTAR I-PENTAR y (n) (t) = f(y (n ) (t),, y(t), w(t)) bu(t) () w b h f () ẋ = x. ẋ n = x n ẋ n = x n bu ẋ n = h(x, u, w, ẇ) y = x () x = [x, x,, x n ] T R n x n = 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 R n l = [l, l,, l n ] T R n () x 3. ESO (4) u = u ˆf b (4) ESO () (4) (5) y (n) (t) = f( ) ˆf u u (5) () (5) n 3. I-PENTAR I-PENTAR τ w I-PENTAR ESO ˆf θw ˆf b 4 b 3 PD PD ˆ, ˆθw d, θ wd u B θw u θw τw B ˆ, ˆ I-PENTAR SYSTEM d, d A θw A C θw C ESO L θw ˆ θw L ˆ ESO Fig. 3 Block diagram of the control system. I-PENTAR () (6) [ ] [ f (, θ =, θ ] [ ] w, τ d ) b 3 w f θw (,, θ w, τ d ) b 4 τ w (6) I-PENTAR τ w τ w u u θw u u θw u u θw f ( ) u θw u f θw ( ) (7) (8) ESO 7, 8) = f (,,, u θ, τ d ) b 3 u (7) = f θw (,,, u, τ d ) b 4 u θw (8) I-PENTAR Fig. 3 A = A θw = B = b3 B θw = b4 C = C θw = [ ] 3
l arm l θ θ l arm l l body l g r ω θ ω Z W X W Σ W Fig. 4 DOF model of I-PENTAR. L R 3 L θw R 3 4. I- PENTAR I-PENTAR I-PENTAR 4. I-PENTAR 3 I-PENTAR Fig.4 M g M M l arm = l l arm = l q [,, θ, θ ] T R 4 Lagrange (9) M(q) R 4 4 H(q, q) R 4 G(q) R 4 τ w τ τ τ d I-PENTAR I-PENTAR E = T = T = D = M (q)( H(q, q) G(q) T τ T τ Dτ d ) f(q, q) R 4 τ w u u θw () q = f(q, q) M (q)eτ w = f(q, q) M (q)e(u u θw ) () ESO () f(q, q) M (q)eu θw ESO () f(q, q) M (q)eu 4. () 5 a i (i =,, 5) () θ d (t) PD t ( ) t f ( ) θ θ f θ d (t) = 5 a i t i () i= M(q) q H(q, q) G(q) = Eτ w T τ T τ Dτ d (9) θ d (t ) = θ θd (t ) = θd (t ) = θ d (t f ) = θ f θd (t f ) = θ d (t f ) = () 4
I-PENTAR ESO.5 θ d θ d [s] [rad] θ d θ d Fig.5 [s] [rad] θ d θ d Fig.6 5[s].5[kg] [s].5 -.5 Link swing up motion Link swing down motion 5 5 5 3 35 Fig. 5 Input trajectories θ d and θ d in case..5.5 -.5 Link swing up motion Link swing down motion Link oscillating motion 5 5 5 3 35 θ d θ d Fig. 6 Input trajectories θ d and θ d in case. 4.3 Fig.7 Fig.8 4.4 3 - r w =.3[m] r w =-.8[m] θ θ Fig.7 Fig.8 ESO 9) - Link weight is increased -3 Link weight is returned 5 5 5 3 35 Fig. 7 Simulation result in case. 5
.5.5 -.5 r w =.9[m] Link weight is increased - r w =-.9[m] -.5 Link weight is returned - 5 5 5 3 35 θ θ Fig. 8 Simulation result in case. 5. 4. I-PENTAR 3 3) 5. I-PENTAR ( RODO:Reduced Order Disturbance Observer) ) x [,,, ] T (6) I-PENTAR (3) (4) [ ] [ ] [ ] [ ] ẋ A D x B = τ d τ d y = [ ] x τ d τ w (3) (4) (3) (4) τ d RODO (5) (6) ż = LDz ( LDL LA)x LBτ w (5) ˆτ d = z Lx (6) L R 4 τ d (7) ) τ d M g gl g (7) d (7) ˆτ d ) ) 5.. 4. 6
3 - - r w =.6[m] Link weight is increased r w =-.5[m] Link weight is returned -3 5 5 5 3 35 θ θ.5..5 -.5 -. -.5 -. -.5 -.3 5 5 5 3 35 d Fig. 9 Simulation result when using RODO for d calculation in case..5.5 -.5 r w =.6[m] Link weight is increased - r w =-.7[m] -.5 Link weight is returned - 5 5 5 3 35 θ θ Fig. Simulation result when using RODO for d calculation in case. Fig.9 Fig. [..] d Fig. 5.. Fig.9 Fig. Fig. Fig. Plot of and d using RODO in case. 5. 5. ( ) Fig.4 I-PENTAR I-PENTAR 7
.5.5 -.5 Link weight is increased Link weight is returned - 5 5 5 3 35 θ θ Fig. Simulation result when using arm and pendulum parameters for d calculation in case. (8) M g L g ()g M L (, θ )g L g l g sin M L (, θ, θ )g = (8) L (l body sin l sin(θ )) L (l body sin l arm sin(θ ) l sin(θ θ )) (8) (9) ( = tan M ) l S θ M l S (θ θ ) M M l C θ M l C (θ θ ) (9) M M g l g (M M )l body M M M sin θ S θ cos θ C θ (9) I-PENTAR d 5.. 4. Fig. Fig.3 d Fig.4.8.6.4. -. -.4 -.6 Link weight is increased -.8 Link weight is returned 5 5 5 3 35 θ θ Fig. 3 Simulation result when using arm and pendulum parameters for d calculation in case..5 -.5 -. -.5 -. -.5 5 5 5 3 35 d Fig. 4 Plot of and d in case. I-PENTAR τ w Fig.5 5.. Fig. Fig.3 Fig.5 τ w 8
Torque[Nm] 6 4 - -4 τ w dd -6 τw w/o -8-5 5 5 3 35.5.5 -.5 - r w =-.9[m] Link weight is increased Link weight is returned -.5 5 5 5 3 35 θ θ r w =.8[m] Fig. 5 Plot of τ w in case. Table Values use for simulating effects of erroneous parameters. M g [%] M [%] M [%] l g [%] l [%] l [%] l body [%] 5.3 5. (9) d Table 5.3. 4. Fig.6 Fig.7 Fig. 6 Simulation result when using d considering parameter error in case..8.6.4. -. -.4 r w =-.6[m] -.6 -.8 Link weight is increased - Link weight is returned 5 5 5 3 35 θ θ r w =.6[m] Fig. 7 Simulation result when using d considering parameter error in case. d Fig.8 5.3. Fig.6 Fig.7 4. Table 9
.5 -.5 -. -.5 -. -.5 5 5 5 3 35 d Fig. 8 Plot of and d in case. 6. I-PENTAR I-PENTAR ( ) ),,,, : EMIEW : ( & ),, Vol.8, No.[P-I3], (8). ),,, : RIBA,, Vol., No.[A-E4], (). 3),, Luis Canete, :,, Vol., No.[P-I4], (). 4) Luis Canete and 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, IEEE/RSJ International Conference on Intelligent Robots and Systems, Vol.7, No., (). 5) Q. Zheng, L.Q. Gao, and Z. Gao: On validation of extended state observer through analysis and experimentation, Journal of Dynamic Systems, Measurement, and Control, Vol.34, No.,pp.455. 455.6, (). 6),,, :,, Vol.8, No.,pp.86 93, (). 7) R. Miklosovic and Z. Gao. A dynamic decoupling method for controlling high performance turbofan engines. In Proc. of the 6th IFAC World Congress, pp.4 8, (5). 8) Z. Shen and G. Zhiqiang. Active disturbance rejection control for non-minimum phase systems. In 9th Chinese Control Conference, pp.666 67. IEEE, (). 9),, Luis Canete, :, 77, Vol.77, No., (). ) Luis Canete and Takayuki Takahashi: A disturbance compensation method for pushing control of an inverted pendulum robot, the 9TH Annual Conference of the Robotics Society of Japan, Vol., No.[I3-8], ().