M HNH NG HC V NG LC HC CA TAY MY DI NG 5 BC T DO KINEMATIC AND DYNAMIC MODELS OF THE 5 DOF MOBILE MANIPULATORS

Phan Tn Tng Khoa C Kh, i Hc Bch Khoa, Tp H Ch Minh, Vit Nam.

Email phtantung@yahoo.com Tel: 84-08-8637897

BN TM TT Tay my di dng nm bc t do gm c mt tay my phng ba bc t do (c ba khu v ba khp bn l) gn trn mt xe robot di ng hai bc t do (c hai bnh xe quay c lp vi nhau) l i tng nghin cu ca bi bo ny. M hnh ng hc v ng lc hc ca tay my di ng c xy dng theo hng phn chia thnh hai h con mt h con l tay my v mt h con l xe robot. Vic xy dng m hnh ng hc v ng lc hc trong bao gm nhiu h con s gip cho cc h phng trnh m t c h n gin hn. Cc m hnh ng hc v ng lc hc ny l c s thc hin vic iu khin tay my di ng. Cc kt qu m phng trn my tnh chng minh tnh ng n ca cc m hnh ng hc v ng lc hc c ngh. ABSTRACT The five DOF mobile manipulator that includes a three DOF manipulator and a two wheeled mobile platform is the study subject of this paper. The kinematic and dynamic models of the mobile manipulator are divided into two subsystems one for the mobile platform and another for the manipulator. In fact, the equations of the subsystems are more simple than for the whole. The kinematic and dynamic models are based for control the mobile manipulator. The sumulation results on the computer show the effectiveness of the proposed models. 1. TNG QUAN Ngy nay, cc robot hn c ng dng kh ph bin trong cc thao tc hn trong mi trng nguy him v c hi cho sc kho ca cng nhn. M hnh tay my hn di ng gm c mt xe robot di ng c lp mt tay my l i tng nghin cu chnh ca bi bo ny. Trong cc cng trnh c cng b trn th gii c lin quan n tay my di ng, chng ta c th k n cc bi bo ca Seraji[3] pht trin mt m hnh ng hc cho tay my di ng theo hng xem ton b xe v tay my l mt h thng duy nht. Yoo v cc cng s[7] pht trin xy dng m hnh ng hc ca tay my di ng da trn c s hm Lagrange. Yamamoto[3] cng ngh mt m hnh ng hc cho tay my di ng da trn cc hm ng lng. C th tm tt cc vn lin quan n m hnh ng hc v ng lc hc ca tay my di ng c cc nh nghin cu kho st theo cc xu hng sau: Xem ton b c h (bao gm xe robot di ng v tay my) nh mt th thng nht. S dng cc phng trnh nng lng Lagrange xy dng m hnh hnh ng hc.

Xem c h nh l s tp hp ca cc h con. S dng cc phng trnh nng lng Lagrange xy dng m hnh hnh ng hc cho tng h con. Theo cch ny th phi tm mi lin h gia cc h con vi nhau. Vic xy dng m hnh ng hc v ng lc hc cho tay my di ng nm bc t do l rt cn thit cho vic iu khin chuyn ng ca tay my di ng. Trong bi bo ny, chng ti gii thiu mt m hnh ng hc v ng lc hc ca tay my di ng theo hng mt h thng bao gm nhiu h con phc v cho qu trnh iu khin khng tp trung (decentralized control). 2. M HNH NG HC Chng ta xem xt mt tay my di ng nh s trn hnh 1.
Y YE E y L3 yE
1

ma trn Jacobian ca c cu tay my ba khu c xc nh nh sau

L3S123 L2 S12 L1S1 L3S123 L2 S12 J = L3C123 + L2C12 + L1C1 L3C123 + L2C12 1 1 L3S123 L3C123 1

vi

S1 = sin (1 ); S12 = sin (1 + 2 ); S123 = sin (1 + 2 + 3 ); C1 = cos(1 ); C12 = cos(1 + 2 ); C123 = cos(1 + 2 + 3 ); L1, L2 v L3 l chiu di ca ba khu ca tay my. Vn tc ca im E[XE YE E] l & & & T trong h 0 c xc nh V X Y
E

bi phng trnh (2)

VE = VP + WP 0 Rot1 1 p E + 0Rot1 1VE

(2)

3 2 1
xE p

vE

pE

J3

L2
J2

L1 YP
P J1

vP

& & vi VP = X P YP 0 T l vect vn tc di ca im P (cng l v tr ca khp J1 ) trong & h 0, WP = 0 0 P T l vect vn tc gc ca im P trong h 0, 0Rot1 l ma trn quay chuyn i t h 1 v h 0, 1pE l vect v tr ca im E trong h 1.

p
b

X O Xp XE

Hnh 1 S ca tay my di ng Mt h ta vung gc c gn c nh vo xe robot (h xoy) c k hiu l h 1. C c h xe v tay my chuyn ng trong h c nh (h XOY) c k hiu l h 0. Mc tiu ca phn ny l tm ra mi lin h gia vn tc ca u t do ca khu 3 v vn tc ca cc khu dn (tc l hai bnh xe v ba khp bn l). Vn tc ca im E[xE yE E] trong h 1 c xc nh bi phng trnh (1)
1

cos P sin P 0 Rot1 = sin P cos P 0 0 0 1 L1C1 + L2 C12 + L3C123 1 p E = L1 S1 + L2 S12 + L3 S123 E
0

Phng trnh biu din mi quan h gia vn tc di vP v vn tc gc P ca thn xe vi vn tc gc ca bnh xe tri L, bnh xe phi R l:
R 1 / r = 1 / r L b / r vP b / r P

(3)

& VE = J & Trong VE = x E

1

(1)

& yE

& & & & T ca im E trong h 1, = 1 2 3 l vn tc gc ca cc khp J1, J2 v J3, v J l

& E

l vn tc

vi r l bn knh bnh xe v b l mt na khong cch gia hai bnh xe. Cui cng, vn tc di v vn tc gc ca im E trong h 0 c xc nh bi h phng trnh (4) nh sau

& & v E = X E cos E + YE sin E & =

E E

(4)

Kt hp vi iu kin non-holonomic ca xe robot hai bnh ta c phng trnh sau:

3. M HNH NG LC HC Mc ny ch trnh by m hnh ng lc hc ca xe robot. Do c im ca tay my hn l hnh dng tay my c bo ton khi hot ng nn chng ta xem ch nh l mt phn khi lng ca xe robot. Phng trnh ng lng ca xe robot tng ng vi khi tm ca xe l:
K= 1 2 1 2 1 1 2 2 mv P + I P + I W R + IW L (5) 2 2 2 2

1 1 & 1 &2 &2 &2 & m X P + YP2 + I 2 + I W R + L P 2 2 2 & & & & + m c d P YP cos P X P sin P (9) T phng trnh (3) vn tc gc ca bnh xe bn tri v bnh xe bn phi c th biu din bng cc phng trnh sau: K=

1 (v P + b P ) r & 1 L = (v P b P ) r & R =

(10) (11)

vi m l tng khi lng ca xe robot, I l tng m-men qun tnh ca xe robot i vi trc thng ng i qua khi tm ca xe, IW l m-men qun tnh ca bnh xe robot i vi trc thng ng i qua khi tm ca xe. Tng khi lng ca xe robot bao gm khl lng ca thn xe mc cng vi khi lng hai bnh xe mw v c biu th bi phng trnh sau:

Thay cc phng trnh (10) v (11) vo phng trnh (9) v sp xp li ta c phng trnh ng lng ca xe robot nh sau:

m = mc + 2mw

(6)

2I & 2 & 1 & & K = m + 2w X P + YP2 + mc d P (YP cos P r 2 I & 1 & X P sin P ) + I + w2 2 P 2 2c (12) Phng trnh Lagrange i vi h nonholonomic cho bi phng trnh sau
d K dt qi & K q = Fi a1i i

Tng m-men qun tnh ca xe robot i vi trc thng ng i qua khi tm ca xe bao gm m-men qun tnh ca thn xe robot i vi trc thng ng i qua khi tm ca xe Ic cng vi m-men qun tnh ca hai bnh xe v hai roto ca ng c DC i vi trc thng ng i qua khi tm ca chng l Im v c biu th bi phng trnh sau: I = I c + 2mw b 2 + 2 I m (7)

(13)

vi qi l ta Lagrange, Fi l lc Lagrange, l nhn t Lagrange, ali l cc tha s. Thay phng trnh (12) vo phng trnh (13) ta c cc phng trnh sau

2I m + 2w r

vi mwb2 l i lng quy i m-men qun tnh Im v trc thng ng i qua khi tm ca 2 &2 & & xe. Vi ch rng v 2 = X P + YP2 P = 2 P P & 2 = & 2 v 2 = 2 th phng trnh (5) c
R R L L

th vit li nh sau: 1 & 2 &2 1 & 1 &2 &2 K = m X P + YP + I 2 + I W R + L P 2 2 2 (8)

&& && &2 X P mc d P sin P + P cos P ( + L )cos P sin = R P r (14) 2I & && & m + 2w Y&P + mc d P cos P 2 sin P P r ( + L )sin P + cos = R P r (15)

 && P b = ( R L ) r (16) Vi L v R l m-men xon ca bnh tri v phi, d l khong cch t im P n khi r tm ca xe robot, c = . Vit li cc phng 2b trnh (14,15, 16) ta c I & && mc d Y&P cos P X P sin P + I + w2 2c && & & M v (qv )qv + Cv (qv , qv )qv = E (qv ) v AT (qv ) (17) vi qv = [ X P YP P ] T
0 m+ 2I w r2 mc dsin P mc dcos P Iw I+ 2 2c

hn. Mc 4 trnh by cch thit k b iu khin sao cho tay my hn di ng thc hin c yu cu trn.

4.1 nh ngha sai s

Gi s c mt im hn tham chiu di chuyn trn qu o hn vi vn tc bng vn tc hn. Nu im hn trn tay my hn di ng khng trng vi im hn tham chiu th tn ti cc sai s v tr gia chng. Mc tiu ca b iu khin l lm cho im hn trn tay my hn chuyn ng theo im hn tham chiu v sai s v tr s dn v gi tr 0. Da trn s hnh 2, chng ta nh ngha cc sai s v tr nh sau
e1 cos E E E = e2 = sin E e3 0 sin E cos E 0 0 X R X E 0 YR YE 1 R E (18)

2I w m+ 2 r M v (qv ) = 0 -mc dsin P

mc dcos P

& 0 0 mc d P cos P & & C v (qv , qv ) = 0 0 mc d P sin P 0 0 0 cos P 1 E (qv ) = sin P r b cos P sin P b
v = R L

YR Y YR YE E R vE e3 vR YE
E

e2

E XE XR

e1

2I & & & && = m + 2w X P cos P + YP sin P P + mc d P r

XE XR

Hnh 2 S dng xc nh sai s e1, e2, e3 Tay my hn di ng l mt c cu c nm bc t do, trong khi rng buc ca im hn ch c ba. Do chng ta to thm hai rng buc ph bng cch buc tay my hn khng thay i hnh dng khi hot ng. iu ny tng ng vi vic im E phi lun lun trng vi mt im M c nh trn xe robot. Nu tay my thay i hnh dng th s tn ti sai s gia hai im ny. Da trn s hnh 3, chng ta nh ngha cc sai s v tr nh sau
e4 cos M = e5 = sin M e6 0 sin M cos M 0 0 X E X M 0 YE YM 1 E M

Phng trnh (17) l m hnh ng lc hc ca xe robot din t mi lin h gia m-men xon tc ng ln hai bnh xe vi gia tc v vn tc ca xe robot.

4. THIT K B IU KHIN
kim chng cc m hnh ng hc v ng lc hc ngh nh trn, chng ta p dng cc m hnh ny u khin mt tay my di ng dng trong lnh vc hn, trong im hn (sng hn) xem nh t ti u t do ca khu 3. Nhim v ca tay my hn l phi di chuyn theo qu o ca ng hn sao cho m bo vn tc hn (chn bng 7,5 mm/s) v khu 3 phi lun lun vung gc vi ng

EM

(19)

y M(xM,yM)

e5

E(xE,yE) vE e4

T & & V1 = E M E M

(25)

Hnh dang ban au cua tay may

& V1 0 th vn tc ca xe robot phi tha phng trnh sau:

x

D e6
P

vP

v P = v E cose6 + D P + k 4 e4 P = E + v E sine6 + k 5 e5 + k 6 e6
vi k4, k5, k6 l cc hng s dng.

(26)

P(xP,yP)

Hnh 3 S dng xc nh sai s e4, e5, e6

4.4 Thit k lut iu khin bng m hnh ng lc hc cho xe robot

Ta chn phng php iu khin thch nghi iu khin xe robot vi cc i lng phi c lng l khi lng xe robot. Ta c: cos P & qv = sin P 0 0 vp 0 = S (qv )v(t ) p 1

4.2 Thit k lut iu khin bng m hnh ng hc cho tay my

Hm Lyapunov chn nh sau 1 T V0 = E E E E 2 o hm ca hm Lyapunov l (20)

(27)

& & T V0 = E E E E

(21)

Thay phng trnh (27) vo phng trnh (17) v k hiu li ta c:

& V0 0 th phng trnh sau y phi tha

& M v v + Cv v = E v
vi

(28)

& EE = KE E

(22)

Vi K = diag(k1 k 2 k 3 ) v k1, k2, k3 l cc hng s dng. Kt hp cc phng trnh (2), (18) v (22) ta c vect vn tc ca ba khp bn l ca tay my l :

M v = S T M vf S v M v l i lng c lng

& C v = S T M vf S + Cvf S v C v l i lng c lng E = ST E Phng trnh (28) vi cc i lng c c lng c vit li nh sau: & M v v * +Cv v = E ( q v ) v vi ~ & & v* = vd + K v v (30) (31) (29)

& & = J 1 0 Rot11 ( A 1 ( AA 1 + K ) E E + VR VP WP 0 Rot1 1 p E )

(23)

4.2 Thit k lut iu khin bng m hnh ng hc cho xe robot

Hm Lyapunov chn nh sau V1 = 1 T EM EM 2 (24)

~ v = vd v

o hm ca hm Lyapunov l

v Kv l matrn cho 2x2 bao gm cc hng s. Thay cc phng trnh (30) v (31) vo

phng trnh (29) ta c:

~ ~ 1 ~ & ~ & v + K v v = M v M v v + Cv v

&T ~ v = v T UM v 1W
(32)

(39)

~ ~ Vi M v = M v M v v Cv = Cv Cv t
v = mc mw Ic Iw Im

5. KT QU M PHNG 5.1 Kt qu m phng vi m hnh ng hc

mi li2 T

m
i i =1 i =1

th phng trnh (28) c vit li nh sau

& M v v + Cv v = W v

(33)

Vi cc b iu khin ca xe robot v tay my biu th bi phng trnh (23) v (26), chng ta c kt qu m phng trn my tnh nh hnh 4. Kt qu m phng cho thy tay my chuyn ng theo ng qu o hn v xe robot chuyn ng gi cho hnh dng tay my khng thay i.
1.6 1.4

vi
&2 & v P P d W = d v & P P & 2v P && 2b 2 P 0 && P & 2v P r && 2 P r2
2

m Tay may Quy ao cua iem han tham chieu Xe robot

0 && 2 P

&& & & v P h P sin + 2 cos P 0

0 && P

1.2 1 0.8 0.6

Vi ch l v = v th phng trnh (32) c vit li l ~ ~ 1 ~ & v + K v v = M v W v vi (34)

0.4 0.2 0 -0.2 0 0.5 1 1.5 2

Quy ao cua xe robot

Hnh 4 Kt qu m phng biu din qu trnh tay my di ng hn lm vic.

(35)

~ v = v v
Hm Lyapunov chn nh sau ~ ~ ~ ~ Vv = v T U v + vT 1 v

(36)

Vi U l ma trn cho 2x2 xc nh dng, l ma trn cho 7x7 xc nh dng. o hm ca hm Lyapunov l

1 ~ ~ ~ ~ & & Vv = 2v T UK v v + 2 v T UM v W + vT 1 v ~ (37) Lut iu khin thch nghi cho xe robot l

Cc th t hnh 5 n hnh 9 biu th cc thng s ca tay my hn di ng khi thc hin chuyn ng theo qu o hn. Khi bt u hot ng, v tr ca im hn trn tay my khng trng vi im hn tham chiu, do tn ti cc sai s e1, e2, e3. Tuy nhin sau khong 2,5s th hai im ny trng nhau.
16 14 12 10 8 6 4 2 0 -2

e1 (mm) e2 (mm) e3 (o)

& v = E 1 M v v * +C v v Lut cp nht l

(38)

Sai so

5 6 7 Thi gian (s)

10

Hnh 5 th biu din sai s e1, e2 v e3 trong 10 giy u tin.

4 2 0 Sai so -2 -4 -6 -8 -10 -12 0 1 2 3 4 e4(mm) e5(mm) e6(o) 5 6 7 Thi gian (s) 8 9 10

my biu th bi phng trnh (23), (38) v (39), chng ta c kt qu m phng trn my tnh nh hnh 10. Kt qu m phng cho thy tay my chuyn ng theo ng qu o hn v xe robot chuyn ng gi cho hnh dng tay my khng thay i.

Quy ao han tham chieu

Hnh 6 th biu din sai s e4, e5 v e6 trong 10 giy u tin.

Quy ao han tham chieu
Quy ao xe robot

Tay may

Xe robot

Quy ao sung han Sung han

Hnh 10 Kt qu m phng biu din qu trnh tay my di ng hn lm vic. Cc th t hnh 11 n hnh 15 biu th cc thng s ca tay my hn di ng khi thc hin chuyn ng theo qu o hn. Khi bt u hot ng, v tr ca im hn trn tay my khng trng vi im hn tham chiu, do tn ti cc sai s e1, e2, e3. Tuy nhin sau khong 2,5s thi hai im ny trng nhau.
m
0.41 Quy ao iem han tham chieu 0.405 0.4 0.395 Quy ao sung han 0.39 Sung han

Hnh 7 Qu o im hn giai on u
1 0.8 0.6 0.4 0.2 0 -0.2 -0.4 -0.6 -0.8 Van toc goc khp ban le J1 Van toc goc khp ban le J2 Van toc goc khp ban le J3

Van toc goc (rad/s)

5 6 7 Thi gian (s)

10

Hnh 8 th biu din vn tc gc ca cc khp J1, J2 v J3 trong 10 giy u tin.

20
Van toc goc (v/ph)

15 10 5 0 -5

Van toc goc banh xe ben trai Van toc goc banh xe ben phai

0.385 0.275 0.28 0.285 0.29 0.295 0.3 0.305

Hnh 11 Qu o im hn giai on u
10 5 Sai so

-10

50

100

150 200 Thi gian (s)

250

300

350

0 e1(mm) e2(mm) e 3(o)

Hnh 9 th biu din vn tc gc ca bnh xe bn tri v bn phi

5.2 Kt qu m phng vi m hnh ng lc hc

-5

-10 -15 0 1 2 3 4 5 6 Thi gian (s)

10

Vi cc b iu khin ca xe robot v tay

Hnh 12 th biu din sai s e1, e2 v e3 trong 10 giy u tin

6 4 2 Sai so 0 -2 -4 -6 0 1 2 3 4 5 6 7

e4 (mm) e5 (mm) e 6 (o)

TI LIU THAM KHO

1.

10

2.

Thi gian (s)

Hnh 13 th biu din sai s e4, e5 v e6 trong 10 giy u tin

1 0.8 0.6 0.4 0.2 0 -0.2 -0.4 -0.6 -0.8 -1 x 10 4

3.

R ( N .mm ) L ( N .mm )
0 1 2 3 4 5 6 7 Thi gian (s) 8 9 10

4.

Hnh 14 th biu din m-men xon ca bnh xe tri v phi trong 10 giy u tin
10 8 6 4 2 0 -2 -4 -6 -8 -10

5.

6.
Banh xe ben trai Banh xe ben phai 0 100 200 300 400 Thi gian (s) 500 600

7.

Hnh 15 th biu din vn tc gc ca bnh xe bn tri v bn phi

6. KT LUN

Tan Tung Phan, Tan Lam Chung, Manh Dung Ngo, Hak Kyeong Kim, and Sang Bong Kim, Decentralized Control Method for a Welding Mobile Manipulator, KSME International Journal, accepted (Dec. 2004). Y. Yamamoto, Control and Coordination of Locomotion and Manipulation of a Wheeled Mobile Manipulator, Ph. D. Thesis, Department of Computer and Information Science, University of Pennsylvania, USA, 1994. H. Seraji, Configuration Control of Rover Mounted Manipulators, Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 3, pp. 2261-2266, 1995. H. Wang, T. Fukao, and N. Adachi, An Adaptive Tracking Control Approach for Nonholonomic Mobile Robot, Proceedings of the IFAC World Congress, pp. 609-615, 1999. Y. Kanayama,Two Dimensional Wheeled Vehicle Kinematic, Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 4, pp. 3079-3084, 1994. R. Fierro, and F. L. Lewis, Control of a Nonholonomic Mobile Robot: Backstepping Kinematics into Dynamics, Proceedings of the IEEE Conference on Decision and Control, Vol. 4, pp. 38053810, 1995. W. S. Yoo, J. D. Kim, and S. J. Na, A Study on a Mobile Platform Manipulator Welding System for Horizontal Fillet Joints, Transactions on Mechatronics, Vol. 11, pp. 853-868, 2001.

Bi bo ny gii thiu m hnh ng hc v ng lc hc ca tay my di ng theo hng chia thnh nhiu h con phc v cho vic iu khin khng tp trung. ng thi, bi bo cng gii thiu lut iu khin s dng cc m hnh thit lp iu khin tay my di ng vi ng dng l robot hn. Cc kt qu m phng chng t tnh ng n ca cc m hnh ngh.

Van toc goc (v/ph)

Mo men xoan