ROBOT. Dynamic Model Based Motor Control for Wheeled Mobile Robots

Transcript

1 ROBOT Vo.3, No.4 Juy, 8-446(8)4-36-7, 8 9 DMMC I DMMC TP49 A Dynamic Mode Based Motor Contro for Wheeed Mobie Robots CHEN Xiao-peng,, LI Cheng-rong, LI Gong-yan, LUO Yang-yu (. Beijing Institute of Technoogy, Beijing 8, China;. Institute of Automation, Chinese Academy of Sciences, Beijing 9, China) Abstract: A nove motor contro aw based on dynamic mode (DMMC) is proposed for wheeed mobie robot to contro its two driving motors synchronousy. First, kinematic mode and dynamic mode of the mobie robot, of which the mass center position is arbitrary, are derived, and the noninear differentia equation of speeds and torques of the two whees is derived. And then, system state equation of the mobie robot is derived based on the noninear differentia equations of speeds and torques, eectrica equations and eectrica-mechanica equations of the two driving motors. Finay, the poe pacement method is used to form a type I state feedback contro aw. Simuation shows that the DMMC controer can respond to the input instructions quicky without static error. Keywords: motor contro; mobie robot; modeing; dynamics; nonhoonomic constraint; cross couping (Introduction) PID [] [] [3] [4] [] DMMC DMMC 863 (7AA4Z7) 7--4

2 (Dynamics anaysis for mobie robots) J Z r ˆ J ˆ J r ˆT ˆT r ˆϕ ˆϕ r X ˆF x ˆF xr Y F y q = [ x c y c θ ˆϕ ˆϕ r ] T v = [ ] T ˆϕ ˆϕ r. Rẋ c V R = Rẏ c = r r d d ˆϕ () r + ˆϕ r θ Fig. Geometrics of mobie robot xoy XOY Y Y X X x θ O P c d r (x c,y c ) mz r( r cosθ + d sinθ) S(q) = r + r( cosθ d sinθ) r + ẋ c cosθ sinθ ẏ ċ = sinθ cosθ V R θ = r r cosθ + d sinθ cosθ d sinθ r + r sinθ d cosθ sinθ + d cosθ ˆϕ ˆϕ r () r( r sinθ d cosθ) r r + r + r( sinθ + d cosθ) r r + r + q = S(q)v (3) T (4) (3) q [5] A(q) q = (5) A(q) =. sinθ cosθ d cosθ sinθ r cosθ sinθ r r (6) x y z x y z mẍ c ( ˆF x + ˆF xr )cosθ + F y sinθ = mÿ c ( ˆF x + ˆF xr )sinθ F y cosθ = J z θ + ˆF x ˆF xr r + F y d = Jˆ ˆϕ + ˆF x r = ˆT Jˆ r ˆϕ r + ˆF xr r = ˆT r (7)

3 (4) (6) M q = Eτ A T (q)λ (8) M = diag{m,m,j Z, Jˆ, Jˆ r } E = λ = [ ] T F y ˆF x ˆF xr τ = [ ] T ˆT ˆT r T (9) A(q)S(q) = () (8) S T (q) () τ = S T (q)m q () (3) ().3 (3) () v = (S T (q)ms(q)) S T (q)mṡs(q)v+(s T (q)ms(q)) τ (J Z + md ˆλ )r + mr = + Jˆ (J Z + md ˆλ )r mr r = 3 ˆλ = mdr3 (J Z + md )r mr r 4 ˆλ = mdr3 (J Z + md )r + mr ( r ) + Jˆ 5 ˆλ = mdr3 (J Z + md )r + mr (J Z + md )r mr r ˆλr = (J Z + md )r + mr r + Jˆ ( + r ) ˆλr = 3 ˆλr = mdr3 (J Z + md )r + mr r + Jˆ ( + r ) 4 ˆλr = mdr3 (J Z + md )r + mr r ( r ) + Jˆ ( + r ) 5 ˆλr = mdr3 (J Z + md )r mr r () () ˆϕ = ˆλ ˆT + ˆλ ˆT r + 3 ˆλ ˆϕ + 4 ˆλ ˆϕ ˆϕ r + 5 ˆλ ˆϕ r ˆϕ r = ˆλ r ˆT + ˆλ r ˆT r + 3 ˆλ r ˆϕ + 4 ˆλ r ˆϕ ˆϕ r + 5 ˆλ (3) r ˆϕ r (4) ) (4) J Z ˆ J ˆ J r m r d r ) d 3) d = ˆϕ = ˆλ ˆT + ˆλ ˆT r ˆϕ r = ˆλ r ˆT + ˆλ r ˆT r (5)

4 DMMC (DMMC agorithm) DMMC C m C mr L L r R R r (3) I τ τ r I I r U U r T T r T m T mr E E r T c T cr 3. T ϖ T ϖr f ϖ f ϖr I c I cr ϖ ϖ r i (3) ϖ = λ T + λ T r + 3 λ ϖ + 4 λ ϖ ϖ r + 5 λ ϖr τ = L τ r = L r R R r ϖ r = λ r T + λ r T r + 3 λ r ϖ + 4 λ r ϖ ϖ r + 5 λ r ϖr T m = C m I T mr = C mr I r (6) E = C m ϖ E r = C mr ϖ r λ = i ˆλ λ r = i ˆλ U E = R (I + τ İ ) U r E r = R r (I r + τ r İ r ) r λ = i ˆλ λ r = i ˆλ T c = C c I c T cr = C cr I cr r λ = /i 3 ˆλ 3 λ r = /i 3 ˆλ T ϖ = f ϖ ϖ T ϖr = f ϖr ϖ r r (7) 4 λ = /i 4 ˆλ 4 λ r = /i 4 ˆλ T = T m T c T ϖ T r = T mr T cr T ϖr r λ = /i 5 ˆλ 5 λ r = /i 5 ˆλ r (6) (8) (8) İ = I E + U τ R τ R τ İ r (t) = I r (t) E r (t) + U r (t) τ r R r τ r R r τ r Ė = λ Cm I (t) + λ λ C m C mr I r (t) + E 4 C (t) + λ λ C m E E r (t) + m C mr C Er (t) mr λ Cm I c λ C m C mr I cr λ f ϖ E C m λ f ϖr E r (t) C mr Ė r (t) = λ r C m C mr I + λ r C λ r C mr mri r (t) + C E 4 + λ r λ r E E r (t) + Er (t) m C m C mr λ r C m C mr I c λ r CmrI cr C mr f ϖ λ r E λ r f ϖr E r (t) C m (9) (9) İ τ R τ I Ė λ Cm λ f ϖ λ C m C mr C m f ϖr R λ τ = C E mr İ r (t) + I τ r R r τ r r (t) U U r Ė r (t) λ r C m C mr C mr f R r τ r ϖ λ r λ r Cmr λ r f E r (t) ϖr C m () λ E 4 + C (t) + λ λ C m E E r (t) + m C mr C Er (t) λ Cm I c λ C m C mr I cr mr λ r C mr E 4 + λ r λ r E E r (t) + Er (t) λ r C m C mr I c λ r C C m C mri cr mr C m

5 y = v = [ ] T ˆϕ ˆϕ r y = ˆϕ I = ic m E ˆϕ r I ic mr r (t) E r (t) () y = τ = [ ] T ˆT ˆT r I y = ˆT = ic m E () ˆT r ic mr I r (t) E r (t) () 3. DMMC () ẋx = Ax + Bu y = Cx τ R τ A = λ r C m C mr C mr f ϖ λ r λ r Cmr λ r f ϖr C m (3) λ C m λ f ϖ λ C m C mr λ C m f ϖr C mr τ r R r τ r R τ B = R r τ r C = ic m C = ic m ic mr ic mr x = [ I I r (t) E r (t) ] T u = [U U r ] T y = y y = y (4) E y r (3) I [6] ξ = y r y = y r Cx (5) ẋx(t) = ξ ĀA x(t) + Bu(t) + B r y r (t) (6) (t) ξ (t) ĀA = A B = B B r = (7) C I (6) u = K x = [ K x ξ K ξ K [ ĀA ] x (8) ξ B ] K LQG (8) (6) ẋx(t) = A BK x K ξ x(t) + y r (t) ξ (t) C ξ (t) I y = [ C ] x(t) ξ (t) (9) (9) r DMMC ) J Z ˆ J ˆ J r m r d ) C m C mr i R R r L L r f ϖ f ϖr I c I cr 3) (4) ˆλ (7) 4) (4) A B C 5) (7) [ ĀA B ] LQG K 6) y r 7) 9) 7) x (5) ξ 8) (8) PWM

6 ) DMMC LQG I 4 (Simuation resuts) DMMC MATLAB C m =.444 Nm/A R =.3 Ω L = 3.4e 4 H I c =.363 A i = 9. J m = 7.398e 6 kg m r =.56 m d =.5 m =.75 m m = 3 kg J Z =.5645 kg m J = J r = 3.986e 5 kg m f ϖ = 5.453e 6 Nm/rad v ref = [.8.3 ] T rad/sv ref = [.8.3 ] T sin(πt) rad/s K ĀA 4/s (3) m (9) () Simuink Fig. Resuts of two simuation methods DMMC I 5 (Concusion) DMMC DMMC I DMMC

7 DSP (References) [] Huang L, Yu W, Jhajharia S K. Speed contro of differentiay driven wheeed mobie robots tracking and synchronization[a]. Proceedings of the IEEE Instrumentation and Measurement Technoogy Conference[C]. Piscataway, NJ, USA: IEEE, [] Borenstein J, Koren Y. Motion contro anaysis of a mobie robot[j]. Journa of Dynamic Systems, Measurement and Contro, Transactions ASME, 987, 9(): [3] Otten D. Buiding MITEE mouse III: Part [J]. Circuit Cear Ink Magazine, 99, (5): [4] Jones J L, Fynn A M, Seiger B A. Mobie Robots: Inspiration to Impementation[M]. Natick, MA, UK: AK Peters, Ltd., 999. [5] Sarkar N, Yun X, Kumar V. Contro of mechanica systems with roing constraints: Appication to dynamic contro of mobie robots[j]. The Internationa Journa of Robotics Research, 994, 3(): [6] [M] 6 Katsuhiko Ogata. Modern Contro Engineering[M]. Beijing: Tsinghua University Press, [] [M] Sun Zeng-qi, Zhang Zai-xing, Deng Zhi-dong. Inteigent Contro Theory and Technoogy[M]. Beijing: Tsinghua University Press, [] Srinivas M, Patnaik L M. Adaptive probabiities of crossover and mutation in genetic agorithms[j]. IEEE Transacitions on Systems, Man and Cybernetics, 994, 4(4): [] [J]. 4(5) Wang Lei, Shen Ting-zhi, Zhao Yang. An improved adaptive genetic agorithm[j]. Systems Engineering and Eectronics,, 4(5):