28 1 2011 1 : 1000 8152(2011)01 0108 05 Control Theory & Applications Vol. 28 No. 1 Jan. 2011 1, 2, 2 (1., 110016; 2., 110016) :,. 6..,. : ; ; : TP273 : A Relative dynamic modeling and ormation control o multiple unmanned helicopters WANG Zheng 1, HE Yu-qing 2, HAN Jian-da 2 (1. Graduate school o Chinese Academy o Sciences, Shenyang Liaoning 110016, China; 2. State Key Laboratory o Robotics,Shenyang Institute o Automation, Chinese Academy o Sciences, Shenyang Liaoning 110016, China) Abstract: Based on the concept o relative dynamics and leader-ollower ormation strategy, a new ormation control algorithm is developed to partially solve the ormation problem. Firstly, the relative dynamics is derived by combining the 6-DOF rigid body relative dynamics with the individual unmanned helicopter dynamics. Secondly, with the cascade control structure which includes inner and outer loops, a tracking controller is designed by using the techniques o approximate eedback linearization and the extended high gain observer. Finally, simulations o the proposed method with two unmanned helicopter systems are conducted to test the desired tracking perormance and easible disturbance rejection. Key words: unmanned helicopter; ormation control; relative dynamics 1 (Introduction) ( ),. (L-F) [1] [2]., ( ),., Follower ; U.C. Berkeley Aerobot Team Mesh,, [3].,.,, [4]. [5],.,. : 1) ; 2). 2 (Problems statement) (leader) (ollower). oxyz 1.,,.,., p l oxyz, p l x y z., p l, p l R 3. : 2009 11 13; : 2010 05 07. : (RLZ200806).
1 : 109 (L-F), : ẋ l = (x l, u, u l l). (1) : x l L-F, u F, u l l L. : F L u l l. (1), F u, ul l., L 6 ( 3 (5)). : 1) p L-F, L-F ; 2) p u i = κ i (x li) (i = 1, 2,, p) p. 3 (Relative dynamics o unmanned helicopters)., : / 3 [7]. θ m θ t T m T t ; / T m, T t a 1, b 1 6 i i, τ i i ; 6,, 2. / [7] i i = [ 1] T u 4 + ε(u 1, u 2, u 3 ), (2) τi i = [u 1 u 2 u 3 ] T. 1 Fig. 1 Coordinates o leader-ollowing unmanned helicopters : 0 ε 2 0 u 1 ε(u 1, u 2, u 3 ) = ε 1 0 ε 3 u 2, 0 u 3 ε 4 (u 4 ) u 1 = z mr T m b 1, ε 1 = 1/z mr, u 2 = z mr T m a 1, ε 2 = 1/z mr, u 3 = (k 0 + k T T 1.5 m + x tr T t ), ε 3 = 1/x tr, u 4 = T m, ε 4 (T m )=k 0 +k T T 1.5 m. T m, T t. a 1, b 1,. z mr z, x tr x, 1. k 0, k T,. 1 L-F [6] : ( ) Leader, Leader,, Leader,, L-F. Leader L-F [6]., L-F. [2]. 2 Fig. 2 Structure o unmanned helicopters relative dynamics, 6 / 3, 2
110 28., 6, Leader l l τ l l. ( 1), L-F : p l = R T (p l p ), (3) Rl = R T R l. p l L F, R l L x ly l z l F x y z. (3), ṗ l = R T υ l R T υ Ω(ω )p l, (4) Θ l = ψ 1 ωl l ψ 2 ω. : ωi i, ωi i Ω(ωi) 3 i 3, ωi i u = Ω(ωi)u, i i = l or ; Θl L F. R T υ l R T υ L-F ; Ω(ω )p l ; ψ 1 ωl l ψ 2ω L-F ; ψ 1 ψ 2 Ṙi = R i Ω(ωi), i : 1 sϕ lsθl /cθl sθl cθl /cϕ l ψ 1 = 0 cϕ l sϕ l, 0 sϕ l/cθl cϕ l/cθl cφ l/cθl sφ l/cθl 0 ψ 2 = sφ l cφ l 0. sθl cφ l/cθl sθl sφ l/cθl 1 (5)(6) L-F. 3 1 x = [p l υ l Θ l ω ] T ; F 4 u = [u 1 u 2 u 3 u 4 ] T ; L 6 w = [ l l /m l ω l l] T. : (x)= ẋ = (x) + g 1 (x)w + g 2 (x)u, (7) υ l Ω 2 (ω )p l Ω(p l)(j ) 1 Ω(ω )J ω 2Ω(ω )υ l ψ 2 ω, (J ) 1 Ω(ω )J ω R g 1 (x) = l/m l 0 0 ψ 1, I 3 3 /m Ω(p g 2 (x) = l)(j ) 1. 0 (J ) 1 4 (Formation control),,, 3. (4) 1 (4) 2, p l = Rl l l /m l /m Ω 2 (ω )p l Ω( ω )p l 2Ω(ω )ṗ l, (5) Θ l = ψ 1 ωl l ψ 2 ω, ω = (J ) 1 (τ Ω(ω )J ω ). R l l l /m l /m L-F ; Ω 2 (ω )p l Ω( ω )p l L- F ; Ω(ω )ṗ l ; m i J i i ; i i τ i i. 6 (5) (2)., Follower / = [ 1] T u 4 + ε(u 1, u 2, u 3 ), (6) τ = [u 1 u 2 u 3 ] T, 3 Fig. 3 Structure o tracking control ϕ l θ l,. ϕ l θ l, ϕ l θ l.,, ϕ l, θ l u = [u 1 u 2 u 3 u 4 ] T 6. (7) 2 Θ l = ψ 2 ω +ψ 2 (J ) 1 Ω(ω )J ω ψ 2 (J ) 1 u i,,
1 : 111 u i = J ψ 1 2 [(K 1 e Θ + K 2 ė Θ ) + ψ 2 ω ˆp l = ˆυ l + H 1 (ε)(p l ˆp l), ψ 2 (J ) 1 Ω(ω )J ω ], (8) ˆυ l = ẑl + H 2 (ε)(p l ˆp l), ẑ l = ˆσ + ˆβ(ˆx o, η) + ˆα(ˆx o, η)u o + (11). e Θ = Θ l d Θ l, ė Θ = Θ l d Θ H 3 (ε)(p l, l ˆp l), ˆσ = H 4 (ε)(p l ˆp l). Θ l d Θ l d. K 1 K 2,. (7) ( ),., ϕ l, θ l u 4, u o = [ ϕ l θ l u 4 ] T, 3,, ω, τ φ l. l l [ 0] T, (7) Rl, ϕ l θ l. ŵ l l /m l, Rl( l l /m l ŵ).,, ε(u 1, u 2, u 3 ), [8]., ε(u 1, u 2, u 3 ) Rl( l l /m l ŵ), : p l = Ω 2 (ω )p l Ω(p l)(j ) 1 Ω(ω )J ω 2Ω(ω )ṗ l + Ω(p l)(j ) 1 τ + R lŵ + 1 m (0, 0, 1) T u 4 + δ, ϕ l = u o1, θ l = u o2, u 4 = u o3. (9) : u o = [u o1 u o2 u o3 ] T. δ = R l( l l /m l ŵ) + ε(u 1, u 2, u 3 )., u o = α 1 (x o, η)[k 3 (p l d p l) + K 4 (ṗ l d ṗ l) + K 5 ( p l d p l) β(x o, η)], (10) x o = [p l ṗ l] T, η = [φ l τ ω ] T, β(x o, η)= Ω 2 (ω )ṗ l +Ω[(J ) 1 Ω(ω )J ω ]ṗ l 2Ω(ω ) p l Ω[(J ) 1 τ ]ṗ l, 0 α(x o, η) = Rl ŵ R l ŵ 0 ϕ l θl 1, m K 3, K 4 K 5., ϕ l θ l ϕ l d θ l d, 3.,, : ˆp l ˆυ l, ẑl ˆυ l. α i /ε i H i (ε) = α i /ε i, i = 1,, 4. α i /ε i α i s 4 + α 1 s 3 + α 2 s 2 + α 3 s 1 + α 4 Hurwitz. ε > 0. ˆα( ) ˆβ( ),,, ˆα( ) = α( ), ˆβ( ) = β( ). ˆσ. (11), H i (ε)(p l ˆp l). (9), u o = ˆα 1 (ˆx o, η)[κ(ˆx o ) ˆσ ˆβ(ˆx o, η)], (12) κ(ˆx o ) (10), κ(ˆx o ) = K 3 (p l d ˆp l) + K 4 (ṗ l d ˆυ l ) + K 5 ( p l d ẑ l ), (13).,,. u o =satˆα 1 (ˆx o, η)[κ(ˆx o ) ˆσ ˆβ(ˆx o, η)]}. (14) : sat( ), sat(x) = min1, M}sgn x, : M, ±M. [9]. 5 (Simulation), L-F p l0 = (5, 5, 5) T Θl0 = (0, 0, 0.1) T. p l d = (2, 2, 2) T, Θl d = (0, 0, 0) T. K 1 = 5 I 3 3, K 2 = 10 I 3 3, K 3 = I 3 3, K 4 = 2 I 3 3, K 5 = 2 I 3 3, ε = 0.01, M = 10 3, α 1 = 4, α 2 = 6, α 3 = 4, α 4 = 1, I., 10 s L, F,
112 28 l l /m = ŵ = (0, 0, 9.8) T, ω l l = ˆω l l = (0, 0, 0) T. 10 s, L, l l /m = (1, 1, 9.8) T,, ŵ = (0, 0, 9.8) T,., L-F 4, L, F 10 s 10 s. 5. : 10 sf, 10 sf L. 4 L-F Fig. 4 Leader and ollower s trajectories... (Reerences): [1] CHEN XP, SERRANI A, OZBAY H. Control o leader-ollower ormations o terrestrial UAVs[C] //Proceedings o the 42nd IEEE Conerence on Decision and Control. Maui, HI: IEEE, 2003: 498 503. [2] SHAW E, HEDRICK J K. Controller design or string stable heterogeneous vehicle strings[c] //Proceedings o the 46th IEEE Conerence on Decision and Control. New Orleans, LA: IEEE, 2007: 6157 6164. [3] SHAW E, CHUNG H, HEDRICK JK, et al. Unmanned helicopter ormation light experiment or the study o mesh stability[m] //Lecture Notes in Economics and Mathematical Systems. Berlin/Heidelberg: Springer, 2007, 588: 37 56. [4] VIDAL R, SHAKERNIA O, SASTRY S. Distributed ormation control with omnidirectional vision-based motion segmentation and visual servoing[j]. Robotics and Automation Magazine, 2004, 11(4): 14 20. [5] WONG H, KAPILA V, SPARKS A G. Adaptive output eedback tracking control o spacecrat ormation[j]. International Journal o Robust and Nonlinear Control, 2002, 12(2): 117 139. [6] HE Y Q, HAN J D. Decentralized receding horizon control or multiple unmanned helicopters considering dynamics model[c] //Proceedings o the 48th IEEE Conerence on Decision and Control. Shanghai: IEEE, 2009: 8351 8356. [7] BEJAR M, OLLERO A, CUESTA F. Modeling and control o autonomous helicopters[m] //Lecture Notes in Control and Inormation Sciences. Berlin/Heidelberg: Springer, 2007, 353: 1 29. [8] KOO TJ, SASTRY S, Output tracking control design o a helicopter model based on approximate linearization[c] //Proceedings o the 37th IEEE Conerence on Decision and Control. Tampa, FL: IEEE, 1998: 3635 3640. [9] FREIDOVICH L B, KHALIL H K, Robust eedback linearization using extended high-gain observers[c] //Proceedings o the 45th IEEE Conerence on Decision and Control. San Diego, CA: IEEE, 2006: 983 988. : 5 Fig. 5 Relative positions and relative velocities 6 (Conclusion).,. (1982 ),,,, E-mail: wzheng@sia.cn; (1980 ),,,,, E-mail: heyuqing@sia.cn; (1968 ),,,,, / /, E-mail: jdhan@sia.cn.