Luan an tien si_Nam

7/29/2019 Luan an tien si_Nam

1/155

B GIO DC V O TO VIN KHOA HCV CNG NGH VIT NAM

VIN CHC

TRN HONG NAM

GII BI TON NGC NG HC, NG LCHC V IU KHIN TRT RBT D

DN NG DA TRN THUT TON HIUCHNH GIA LNG VC TTA SUY RNG

LUN N TIN S K THUT

H Ni - 2010


2/155

B GIO DC V O TO VIN KHOA HCV CNG NGH VIT NAM

VIN CHC

TRN HONG NAM

GII BI TON NGC NG HC,NG LC HC V IU KHIN TRT RBT

DDN NG DA TRN THUT TONHIU CHNH GIA LNG VC T

TA SUY RNG

Chuyn ngnh: Chc k thutM s ngnh: 62 52 02 01

LUN N TIN S K THUT

Ngi hng dn khoa hc :1. GS.TSKH Nguyn Vn Khang2.PGS.TS Nguyn Phongin

H Ni 2010


3/155

LI CAM OAN

Ti xin cam oan y l cng trnh nghin cu ca ring ti v cha ccng b trong bt c cng trnh no khc. Cc s liu, kt qu nu trong lun nl trung thc.

Tc gi lun n

Trn Hong Nam


4/155

MC LC

Trang

Li cam oan

Mc lc I

Danh mc cc k hiu, ch vit tt thng s dng III

Danh mc cc bng VI

Danh mc cc hnh v v th VII

Mu 1

Chng 1. Tnh ton ng hc ngc rbt d dn ng bng thut

ton hiu chnh gia lng vc tta suy rng 81.1 Gii bi ton ng hc thun rbt d dn ng bng phng php

ma trn Denavit-Hartenberg 81.1.1 Cc tham sng hc v ma trn Denavit-Hartenberg 101.1.2 Phng trnh xc nh v tr v hng ca khu thao tc 121.1.3 Bi ton p dng 141.2 Ma trn ta nghch o 161.2.1 nh ngha 161.2.2 Ma trn ta nghch o Moore-Penrose 161.2.3 Phng php nhn t Lagrange 181.2.4Nghim tng qut ca phng trnh i s tuyn tnh 191.3 Gii bi ton ng hc ngc rbt d dn ng bng phng php

hiu chnh gia lng vc tta suy rng 201.3.1 Phng php khai trin Taylor 201.3.2 Cc cng thc xc nh vc tvn tc v vc tgia tc suy rng 211.3.3 Cc cng thc xc nh vc tta suy rng 221.3.4 nh gi sai s 261.4 Cc bi ton p dng 261.5 Kt lun chng 1 43Chng 2 Tnh ton ng lc hc ngc rbt d dn ng

trong khng gian thao tc da trn thut tonhiu chnh gia lng vc tta suy rng 44

2.1 Dng thc Lagrange loi 2 ca h nhiu vt 442.2 Gii bi ton ngc ng lc hc rbt d dn ng trong khng

gian thao tc 47


5/155

II

2.3 Cc bi ton p dng 522.4 Kt lun chng 2 69Chng 3 iu khin trt rbt d dn ng da trn thut ton s

hiu chnh gia lng vc tta suy rng 703.1 Csl thuyt n nh Lyapunov 703.1.1 H phi tuyn v cc im cn bng 703.1.2 Khi nim n nh 723.1.3 Phng php trc tip Lyapunov 743.2 Bi ton iu khin chuyn ng ca rbt 783.3 iu khin trt rbt d dn ng 803.4 Cc bi ton p dng 853.5 Kt lun chng 3 102Chng 4. Bi ton ngc ng hc, ng lc hc

v iu khin trt rbt BKHN-MCX-04 103

4.1 Tng quan v phng php o chnh xc b mt cc chi tit my 1034.2 Kt cu ca r bt o BKHN-MCX-04 1044.3 Tnh ton ng hc ngc 1064.4 Tnh ton ng lc hc ngc 1114.5 iu khin trt rbt BKHN-MCX-04 1224.6 Th nghim 1264.6.1 Cu to ca h thng th nghim 1264.6.2 Nguyn l hot ng ca h thng th nghim 1274.6.3 H thng iu khin 1274.6.4 Kt qu th nghim 1314.7 Kt lun chng 4 133

Kt lun chung 134

Danh mc cng trnh ca tc gi 136

Ti liu tham kho 137


6/155

III

DANH MC K HIU,CHVIT TT THNG SDNG

ai Khong dch chuyn dc trc xi (chiu di ca khu th i)i Gc quay quanh trc xi.A(z,i) Ma trn bin i ta bi php quay cbn xung quanh trc z.A(z,di) Ma trn bin i ta bi php tnh tin dc trc z.A(x,ai) Ma trn bin i ta bi php tnh tin dc trc x.A(x,i) Ma trn bin i ta bi php quay xung quanh trc x.Ai Ma trn cosin ch hng ca khu th i.A

+ Ta nghch o ca ma trn A.

Ci Cosin (qi).C12n Cosin(q1+q2++qn).C(q, )iq& iq& Vc tcc lc Coriolis v lc hng tm ca m hnh thc.

),( qqC & iq& Vc tcc lc Coriolis v lc hng tm ca biu khin.

),(~

qqC & Sai s gia C(q, )iq& v ),( qqC & .di Khong dch chuyn gc ta Oi-1 v Oi.d Vc tnhiu.Di Ma trn chuyn i ta t O0x0y0z0 v h ta Oixiyizi.

ke Sai s dch chuyn ca bn kp (khng gian thao tc).ke& Sai s vn tc ca bn kp.ke&& Sai s gia tc ca bn kp.

e(t) Vc tsai s bm theo gc quay.)t(e& Sai s bm theo vn tc quay.

Sai s ca php tnh.epxi Gia tc gc ca khu th i.f S bc t do ca ch.g(q) Vc tlc trng trng suy rng.

)( qg Vc tcc lc trng trng ca biu khin.)(~ qg Sai s gia g(q) v )( qg .

i-1Hi Ma trn bin i ta t h trc Oi-1xi-1yi-1zi-1 v Oixiyizi.

h=t Khong thi gian ca bc tnh ton.li V tr trng tm khu th i i vi h trc ta khu i. Nhn t Lagrange.


7/155

IV

i Hng s dng th i.mi Khi lng ca cht im th i.M(q) Ma trn khi lng suy rng.

)( qM Momen khi lng ca biu khin.

)(~ qM Sai s gia M(q) v )( qM .Ii Tenxqun tnh khi ca vt rn th i.Izi Momen qun tnh i vi trc z ca khu th i.JTi Ma trn Jacobi tnh tin ca vt rn th i.JRi Ma trn Jacobi quay ca vt rn th i.J(q) Ma trn Jacobi.J

+(q) Ta nghch o ca ma trn Jacobi.)(qJ +& o hm bc nht ca J

+(q).k H s khuych i .

k H s khuych i ca hm sat.Ks H s khuych i dng trt.Kpd H s khuych i t l - o hm.omi Vn tc gc ca khu th i.Px, Py, Pz Cc ta x, y, z ca im thao tc P. Th nng ca h.

q

Thnh phn lc suy rng ca cc lc c th.

q Vc tta suy rng ca robot.

q& o hm bc nht ca q theo thi gian.q&& o hm bc 2 ca q theo thi gian.q0 Vc tta suy rng ti thi im ban u.

0q& o hm bc nht theo thi gian ca q0.

0q&& o hm bc 2 theo thi gian ca q0.

0~q Gi tr gn ng ca q0.q0 S gia ca q0.qk+1 Gi tr ca q ti thi im t = tk+1.

1k+

q& o hm bc 1 theo thi gian ca qk+1.

1k+q&& o hm bc 2 ca qk+1.

1k~

+q Gi tr gn ng ca qk+1.qi_dot o hm bc nht ca qi theo thi gian.qi_2dot o hm bc 2 ca qi theo thi gian .qd Vc tta suy rng mong mun.

dq& o hm ca vc tsuy rng mong mun theo thi gian.


8/155

V

rCi Vc tnh v trng tm ca vt rn th i.sgn Hm du.sat Hm bo ha.Si Sin(qi).S

12..nSin(q

1+q

2++q

n).

i Gc quay ca trc xi-1 xung quanh trc zi-1.x Vc tnh v ca khu thao tc.x=f(q) Phng trnh xc nh ta x theo ta suy rng q.xd V tr mong mun ca bn kp.

dx& Vn tc chyn ng mong mun ca bn kp.

dx&& Gia tc chuyn ng mong mun ca bn kp.x* Trng thi cn bng.

dx~ Sai s bm ca bin x.

d

~x Vc tsai s bm.vr

Vc thnh hc ca vn tc.vi Vc ti s ca vn tc khi tm ca vt rn th i.V(x) Hm Lyapunov.i Vn tc gc ca vt rn th i.

i~ Ton t sng ca vc tvn tc gc th i. Lc/momen trn khp ng .T ng nng ca ton h.u Quy lut iu khin.DH Denavit-Hartenberg.MP Moore-Penrose.PD T l - vi phn.PID T l - vi phn tch phn.


9/155

VI

DANH MC CC BNG

Trang

Bng 1.1 Thng s DH rbt 5 khu ng 14Bng 2.1 Cc tham sng hc ca rbt Scara 52Bng 2.2 Bng thng sng lc rbt SCARA 4 bc t do 56Bng 2.3 Cc thng sng lc hc ca rbt 5 khu ng 60Bng 2.4 Cc thng sng hc ca rbt 5 khu ng 60Bng 2.5 Cc thng s DH rbt 6 khu ng 62Bng 2.6 Cc thng sng lc rbt 6 khu ng 62Bng 2.7 Cc gi tr thng sng lc hc ca rbt 6 khu ng 67Bng 3.1 Cc thng sng lc rbt 4 khu ng 85Bng 3.2 Cc thng sng lc rbt SCARA 93

Bng 4.1 Thng s hnh hc ca rbt 105Bng 4.2 Bng tham sng hc D-H 106Bng 4.3 V tr khi tm khu i ca rbt trn hng 114Bng 4.4 Cc thng sng lc ca rbt o 119Bng 4.5 Cc kt quo thc nghim 131


10/155

VII

DANH MC CC HNH V V TH

TrangHnh 0.1 Rbt trn dy chuyn ca trung tm sn xut linh hot 1Hnh 0.2 Rbt phc v my phay CNC 2Hnh 0.3 Rbt Mitsubishi RV-2AJ 2Hnh 0.4 Hnh nh mt s loi rbt 3

Hnh 1.1 S thit lp h ta cc khu 9Hnh 1.2 S rbt dng chui 13Hnh 1.3 Rbt 5 khu ng 14Hnh 1.4 S khi tnh ton A+ theo phng php Moore-Penrose 17Hnh 1.5 S khi gii bi ton ng hc ngc 25Hnh 1.6 Rbt phng 5 khu ng 26

Hnh 1.7 Cc c tnh chuyn ng ca khu 1 27Hnh 1.8 Cc c tnh chuyn ng ca khu 2 28Hnh 1.9 Cc c tnh chuyn ng ca khu 3 28Hnh 1.10 Cc c tnh chuyn ng ca khu 4 28Hnh 1.11 Cc c tnh chuyn ng ca khu 5 29Hnh 1.12 Sai s v tr ca im thao tc theo trc x 29Hnh 1.13 Sai s v tr ca im thao tc theo trc y 29Hnh 1.14 Sai s gc nh hng ca bn kp 30Hnh 1.15 Dng chuyn ng ca rbt theo kt qu tnh ton 30Hnh 1.16 Cc c tnh chuyn ng ca khu 1 30

Hnh 1.17 Cc c tnh chuyn ng ca khu 2 31Hnh 1.18 Cc c tnh chuyn ng ca khu 3 31Hnh 1.19 Cc c tnh chuyn ng ca khu 4 31Hnh 1.20 Cc c tnh chuyn ng ca khu 5 32Hnh 1.21 Sai s v tr ca im thao tc theo trc x 32Hnh 1.22 Sai s v tr ca im thao tc theo trc y 32Hnh 1.23 Sai s gc nh hng ca bn kp 33Hnh 1.24 Dng chuyn ng ca rbt theo kt qu tnh ton 33Hnh 1.25 Sai s v tr ca im thao tc theo trc x 34

Hnh 1.26 Sai s v tr ca im thao tc theo trc y 34Hnh 1.27 Sai s gc nh hng ca bn kp 34Hnh 1.28 Dng chuyn ng ca rbt theo kt qu tnh ton 35Hnh 1.29 Sai s v tr ca im thao tc theo trc x 35Hnh 1.30 Sai s v tr ca im thao tc theo trc y 35Hnh 1.31 Sai s gc nh hng ca bn kp 36Hnh 1.32 Dng chuyn ng ca rbt theo kt qu tnh ton 36


11/155

VIII

Hnh 1.33 Sai s v tr ca im thao tc theo trc x 37Hnh 1.34 Sai s v tr ca im thao tc theo trc y 37Hnh 1.35 Sai s gc nh hng ca bn kp 37Hnh 1.36 Dng chuyn ng ca rbt theo kt qu tnh ton 38Hnh 1.37 Sai s v tr ca im thao tc theo trc x 38Hnh 1.38 Sai s v tr ca im thao tc theo trc y 38Hnh 1.39 Sai s gc nh hng ca bn kp 39Hnh 1.40 Dng chuyn ng ca rbt theo kt qu tnh ton 39Hnh 1.41 Rbt 6 khu ng 40Hnh 1.42 th gc quay ca cc khp 41Hnh 1.43 th vn tc gc cc khp 41Hnh 1.44 th gia tc gc cc khp ng 41Hnh 1.45 th sai s v tr trong khng gian thao tc 42Hnh 1.46 th sai s vn tc trong khng gian thao tc 42

Hnh 1.47 th sai s gia tc trong khng gian thao tc 42

Hnh 2.1 44Hnh 2.2 Thut ton gii bi ton ng lc hc ngc

trong khng gian thao tc 51Hnh 2.3 S kt cu rbt scara 52Hnh 2.4 th chuyn ng ca cc khp ng rbt scara 53Hnh 2.5 th vn tc chuyn ng ca cc khp ng rbt scara 54Hnh 2.6 th gia tc chuyn ng ca cc khp ng rbt scara 54Hnh 2.7 th sai s v tr bn kp rbt scara 55

Hnh 2.8 th sai s vn tc bn kp rbt scara 55Hnh 2.9 th sai s gia tc bn kp rbt scara 56Hnh 2.10 Mmen trn khp ng th 1 57Hnh 2.11 Mmen trn khp ng th 2 58Hnh 2.12 Lc trn khp ng th 3 58Hnh 2.13 Mmen trn khp ng th 4 59Hnh 2.14 Rbt phng 5 khu ng 59Hnh 2.15 th cc mmen ng ctheo thi gian 61Hnh 2.16 Rbt phng 6 khu ng 61

Hnh 2.17 Mmen trn khp ng th 1 67Hnh 2.18 Mmen trn khp ng th 2 67Hnh 2.19 Mmen trn khp ng th 3 68Hnh 2.20 Mmen trn khp ng th 4 68Hnh 2.21 Mmen trn khp ng th 5 68


12/155

IX

Hnh 2.22 Mmen trn khp ng th 6 69

Hnh 3.1 Cc khi nim vn nh 72Hnh 3.2 Gc khng n nh 72Hnh 3.3 Trng thi hi t khng n nh 73

Hnh 3.4 S phn k trng thi khi chuyn ngdc theo cc ng nng lng thp 76

Hnh 3.5 S hi tn tp bt bin ln nht 77Hnh 3.6 S hot ng ca rbt 78Hnh 3.7 S tng qut ca h thng iu khin khng gian khp 79Hnh 3.8 S tng qut ca h thng iu khin khng gian thao tc 79Hnh 3.9 Siu khin rbt 80Hnh 3.10 Hm sat 83Hnh 3.11 Hm arctan 83

Hnh 3.12 S tnh ton v m phng iu khin rbt 84Hnh 3.13 Rbt 4 khu ng 85Hnh 3.14 th nhiu tc ng trn khp ng 1 86Hnh 3.15 th nhiu tc ng trn khp ng 2 86Hnh 3.16 th nhiu tc ng trn khp ng 3 87Hnh 3.17 th nhiu tc ng trn khp ng 4 87Hnh 3.18 Momen iu khin ti khp 1 88Hnh 3.19 Momen iu khin ti khp 2 88Hnh 3.20 Momen iu khin ti khp 3 89Hnh 3.21 Momen iu khin ti khp 4 89

Hnh 3.22 th sai s suy rng s1 90Hnh 3.23 th sai s suy rng s2 90Hnh 3.24 th sai s suy rng s3 91Hnh 3.25 thi sai s suy rng s4 91Hnh 3.26 th ta x(t) trong khng gian thao tc theo thi gian 92Hnh 3.27 th ta y(t) trong khng gian thao tc theo thi gian 92Hnh 3.28 Rbt Scara 93Hnh 3.29 th nhiu tc ng trn khp ng 1 94Hnh 3.30 th nhiu tc ng trn khp ng 2 94

Hnh 3.31 th nhiu tc ng trn khp ng 3 95Hnh 3.32 th nhiu tc ng trn khp ng 4 95Hnh 3.33 Momen iu khin ti khp 1 96Hnh 3.34 Momen iu khin ti khp 2 96Hnh 3.35 Lc iu khin ti khp 3 97Hnh 3.36 Momen iu khin ti khp 4 97Hnh 3.37 th sai s suy rng s1 98


13/155

X

Hnh 3.38 th sai s suy rng s2 98Hnh 3.39 th sai s suy rng s3 99Hnh 3.40 thi sai s suy rng s4 99Hnh 3.41 th ta x(t) trong khng gian thao tc theo thi gian 100Hnh 3.42 th ta y(t) trong khng gian thao tc theo thi gian 100Hnh 3.43 th ta z(t) trong khng gian thao tc theo thi gian 101Hnh 3.44 th gc quay

trong khng gian thao tc theo thi gian 101

Hnh 4.1 M hnh rbt o BKHN-MCX-04 104Hnh 4.2 Sng hc rbt o BKHN-MCX-04 105Hnh 4.3 Quo nh trc ca im E (ng xon c) 109Hnh 4.4 th bin khp q 109Hnh 4.5 th vn tc gc cc khp q& 110

Hnh 4.6 th gia tc gc cc khp q&& 110Hnh 4.7 V tr trng tm cc khu 114Hnh 4.8 Quo im E 119Hnh 4.9 V tr cc khu trong qu trnh chuyn ng 120Hnh 4.10 Tr s ca bin khp 2, 3 v 4 120Hnh 4.11 Vn tc gc ca ng c dn ng khu 2, 3 v 4 121Hnh 4.12 Gia tc gc ca ng cdn ng khu 2, 3 v 4 121Hnh 4.13 Mmen dn ng cn thit cho cc khu ca rbt 122Hnh 4.14 Quo nh trc ca im E (ng xon c) 123

Hnh 4.15 th ta xE(t) trong khng gian thao tc theo thi gian 124Hnh 4.16 th ta yE(t) trong khng gian thao tc theo thi gian 125Hnh 4.17 th ta zE(t) trong khng gian thao tc theo thi gian 125Hnh 4.18 M hnh th nghim rbt o BKHN-MCX-04 126Hnh 4.19 Siu khin ton b rbt 128Hnh 4.20 Siu khin tng khp ca BKHN-MCX-04 129Hnh 4.21 S nguyn l ca mch iu khin 1 trc 130Hnh 4.22 Quo thc nghim ca im cui E 132


14/155

1

MU

1. L do la chn tiCng nghip ha v hin i ha nn sn xut l mt ch trng ln ca

t nc chng ta hin nay. Vi xu th chung ca th gii, c thy mnhs nghip cng nghip ha v hin i ha cn u tin p dng cc tin b cakhoa hc k thut vo i sng sn xut. Trong vn quan trng nht lphi tng nhanh lng tng ha vo cc qu trnh sn xut cng nghip. ycng l mt i hi cp bch lin quan n vic gii phng con ngi khi snng nhc, s nhm chn ca cng vic (do s lp i lp li cc thao tc camt cng vic gin n no ), s nguy him ca mi trng lao ng nh snng bc ti cc l hi, s ly lan ca cc bnh him ngho ti cc csy t,s nhim do bi bm ca cc hm m, s nguy him dui y i dng v

trn khng gian v tr c th khc phc c nhng vn va nu, cccng ty cc nc c nn sn xut pht trin a cc rbt vo cc dychuyn sn xut ca mnh. Di y l mt s hnh nh v cc rbt v ni ngdng ca chng m chng ta thng gp

Hnh 0.1. Rbt trn dy chuyn ca mt trung tm sn xut linh hot.


15/155

2

Hnh 0.2 rbt trn dy chuyn ca my phay cnc.

hnh 0.2. Rbt phc v my phay CNC.

Hnh 0.3. Rbt Mitsubishi RV-2AJ


16/155

3

a. b. c.Hnh 0.4. Mt s loi rbt

Ti cc l phn ng ht nhn (a) Thm him i dng (b) Khm ph v tr (c)

Trong cc ti liu v rbt [25, 28, 32, 39] ngi ta a ra mt s khinim v rbt cng nghip. Di y l mt s khi nim (nh ngha) inhnh:

Rbt cng nghip l mt my tng linh hot c s dng thayth tng phn hoc ton b cc hot ng cbp v hot ng tr tu cacon ngi vi nhiu kh nng thch nghi khc nhau.

Rbt cng nghip l mt ccu chuyn i tng c th chng trnhha, lp li cc chng trnh, tng hp cc chng trnh t ra trn cctrc ta c kh nng nh v, nh hng, di chuyn cc i tng vtcht, chi tit, dng c g lp, dao ct theo nhng chng trnh thayi, chng trnh ha nhm thc hin cc chng trnh cng nghkhc nhau.

Rbt cng nghip l mt tay my vn nng hot ng theo chng trnhv c th lp trnh li hon thnh v nng cao hiu qu hon thnh ccnhim v khc nhau trong cng nghip, nh vn chuyn nguyn vt liu,chi tit, thit b hoc cc dng c chuyn dng khc.

Nhng chic rbt cng nghip u tin c ch to vo nm 1956 bi

cng ty Unimation ca George Devol v Joseph F. EngelbergerM. Cc rbtu tin ny ch yu c dng vn chuyn cc vt th trong mt phm vinh. Tnh nng lm vic ca rbt ngy cng c hon thin v nng cao hn,nht l kh nng nhn bit v v x l cc thng tin. Cc rbt ngy nay ctrang b thm cc loi cm bin khc nhau nhn bit mi trng xung quanhv nhcc thnh tu to ln trong cc lnh vc nhiu khin hc, tin hc v


17/155

4

in t hc m cc rbt c thm nhiu tnh nng c bit. Nhvy m ccrbt cng nghip c mt v tr quan trng trong cc dy chuyn sn xuthin i, nht l trong cc dy chuyn sn xut tng linh hot (FMS). Do

cc nc ang y mnh vic sn xut cc rbt cng nghip ng dng vocc ngnh cng nghip, i u trong lnh vc ch to rbt cng nghip hinnay l Nht Bn, kn l M, c, , Php, Anh v Hn Quc.

Theo [9, 22, 25, 28, 32, 44, 56, 58], cu to ca rbt thng c 3 bphn ch yu, l:

Tay my (manipulator) B phn dn ng B phn iu khin

Trong tay my l b phn ckh quan trng, ng vai tr l mt bphn chp hnh ca rbt. Tay my cu to bi cc khu v cc khp nhm m

phng theo nguyn tc hot ng ca bn tay con ngi.B phn dn ng gm cc ng c(c th l ng cin, kh nn hoc

thy lc) to nn chuyn ng cho cc khp ca tay my.B phn iu khin gi vai tr quan trng nh l b no ca con ngi.

Biu khin c dng iu khin cc hot ng ca rbt. B phn iukhin thng c thc hin thng qua mt h thng chng trnh iu khin -mi chng trnh m nhn mt nhim v c th.

c th khai thc, s dng mt cch hiu qu cc rbt c trangb, cng nh c th tin hnh nghin cu, th nghim, ch to cc rbt mip ng c nhu cu i hi ngy cng cao ca nn cng nghip hin i th

vic nghin cu rbt ang c cc c s sn xut, cc nh khoa hc, cctrng hc i hc, cao ng quan tm.

Khi nghin cu v rbt chng ta thng phi gii quyt cc bi ton vng hc, ng lc hc v iu khin. Trong cc bi ton ny th bi ton ngcm c bit l cc bi ton ngc ca rbt d dn ng l bi ton kh v hinnay ang cn t c nghin cu nc ta. V vy tc gi chn cho mnh tiGii bi ton ngc ng hc, ng lc hc v iu khin trt rbt d dnng da trn thut ton hiu chnh gia lng vc tta suy rng nhm citin phng php s gii bi ton ngc c hin nay.

2. i tng v ni dung nghin cui tng nghin cu ca lun n l cc rbt d dn ng. Rbt loi

ny c s bc t do ln hn s ta xc nh v tr v hng ca bn kp, do c s dng kh linh hot trong cc nhim v thao tc phc tp. Ni dungnghin cu l kho st bi ton ng hc ngc, bi ton ng lc hc ngcv bi ton iu khin trt rbt d dn ng.


18/155

5

3. Tng quan v vn nghin cuVic nghin cu cc bi ton lin quan n rbt d dn ng cng

c cc tc gi nc ngoi nghin cu v trnh by trong cc cng trnh khoa

hc ca mnh. Theo [15, 18, 20, 43, 47, 57, 59, 65] th mt rbt c gi l ddn ng khi s ta suy rng nhiu hn s ta ti thiu xc lp nn v trv hng ca khu thao tc theo ng yu cu ca bi ton cng ngh. Nhtnhd dn ng m rbt loi ny c kh nng trnh c cc vt cn, cc im kd, cc gii hn ca bin khp, , ngha l cc rbt d dn ng c tnh linhhot cao hn hn so vi cc rbt chun.

Bi ton ng lc hc v bi ton iu khin rbt l cc bi ton c ngha thc tin cao, n lm cscho vic nghin cu ch to ra cc loi rbtmi c bit l cc rbt d dn ng, mt lnh vc m hin cn c t nghincu nc ta.

Trong cc ti liu [9, 22, 25, 28, 32, 44, 55] ni v rbt hin nay, nhnchung cc tc gi trnh by kh y v cc bi ton ng hc, ng lchc v iu khin rbt chun.

Bi ton ng hc c chia lm 2 nhm l nhm bi ton ng hcthun v nhm bi ton ng hc ngc. Nhm bi ton ng hc thun cnhim v xc nh v tr v hng ca khu thao tc khi bit c s cu trcca rbt v cc quy lut chuyn ng ca cc khp ng. Bi ton ny cgii da vo php bin i ta theo phng php Denavit-Hartenberg. Kt quthu nhn c l mt ma trn m t hng v phng trnh xc nh ta cabn kp. Nhm bi ton ng hc ngc l nhm bi ton xc nh cc c tnh

chuyn ng ca cc khp ng rbt to ra c quy lut chuyn ngmong mun nhm thc hin mt nhim v c th no . Vic gii cc bi tonng hc ngc thng l phc tp v kh khn hn so vi bi ton ng hcthun. gii quyt bi ton ny thng thng ta phi thit lp phng trnhm t quan h gia ta ca khu thao tc (bn kp) vi cc gc khp (y lkt qu ca bi ton thun) ri da vo phng trnh chuyn ng mong mun tm ra cc c tnh chuyn ng cho cc khp. Trong trng hp s khpdn ng ng bng vi s ta xc nh nn cu hnh ca rbt th vnguyn tc ta u gii c v cng c trnh by kh y trong cc tiliu tham kho nu. Trong trng hp khi s khp dn ng nhiu hn s ta

ti thiu xc lp v tr ca khu thao tc (lin quan ti bi ton ngc carbt d dn ng) th bi ton s c nhiu nghim v dng bi ton ny cn tc trnh by trong cc ti liu, nht l cc ti liu bng ting Vit.

Bi ton ng lc hc cng c 2 nhm l nhm bi ton ng lc hcthun v nhm bi ton ng lc hc ngc. Nhim v ca bi ton ng lchc ngc l thit lp phng trnh vi phn chuyn ng cho cc khu ng carbt t xc nh lc/mmen tc ng trn cc khp. Vic thit lp phng


19/155

6

trnh vi phn chuyn ng trnh by trong cc ti liu [9, 15, 22, 25, 28, 32, 38,39, 44, 55, 64, 67] u da trn cscc phng trnh Newton-Euler hoc ccphng trnh Lagrange loi 2 cho h nhiu vt.

Vi bi ton ng lc hc ngc ta bit hoc la chn c scu trc ca rbt, theo yu cu ca bi ton cng ngh m ta c phngtrnh chuyn ng ca khu thao tc t ta phi xc nh phng trnh nglc hc ca rbt. xy dng phng trnh ng lc hc ta phi bit c ccc tnh chuyn ng ca cc khu, v vy ta li phi gii bi ton ng hcngc. Kt qu gii bi ton ng hc ngc sc s dng gii bi tonng lc hc ngc.

Nhim v ch yu ca bi ton iu khin l duy tr chuyn ng cakhu thao tc theo 1 quo mong mun no c xc nh trc theo yucu ca cng ngh [28, 38]. thc hin c nhim v ny ta c th thc hin

bng 2 cch l iu khin chuyn ng ca cc khp ng (iu khin khnggian khp) v iu khin chuyn ng ca bn kp (iu khin khng gian thaotc). Vi phng php iu khin khng gian khp ta tin hnh iu khinchuyn ng ca cc khp theo v tr mong mun m ta xc nh trc cn phng php iu khin khng gian thao tc th ta phi iu khin khu thao tcng v tr mong mun do phi tnh ton lng chuyn ng cho cc khptrong qu trnh iu khin v vy m qu trnh iu khin thng chm hn dokhi lng tnh ton nhiu khi iu khin. Trong lun n ca mnh, tc gi lachn phng n iu khin trong khng gian cc khp. thc hin theophng n ny ta phi xc nh trc cc c tnh chuyn ng ca cc khptrn cschuyn ng nh trc ca khu thao tc, ngha l ta li phi gii libi ton ng hc ngc ca rbt. Kt qu ca bi ton ng hc ngc l uvo cho bi ton iu khin trong khng gian khp. Trong qu trnh iu khinta phi m bo tnh n nh chuyn ng ca h thng. Theo [27, 63] th iukhin dng trt l phng php iu khin c tnh n nh cao d cho trn hthng trong qu trnh lm vic c th c cc nhiu tc ng ngu nhin v ccc sai s do cu trc a li cho nn tc gi vn dng phng php iu khindng trt cho vic iu khin chuyn ng trong khng gian khp i vi ccrbt d dn ng,

Cc bi ton nu trn u c mt im chung l u phi tin hnh giibi ton ng hc ngc v y l mt bi ton kh i vi cc rbt d dnng v vy m di s hng dn ca GS.TSKH Nguyn Vn Khang chng xy dng Thut ton hiu chnh gia lng vc t ta suy rng lm cs gii bi ton ng hc ngc v xy dng cc thut ton gii bi ton nglc hc ngc v bi ton iu khin chuyn ng trong khng gian khp. nc ta vic s dng phng php s gii cc bi ton ngc ca rbt, nhtl i vi rbt d dn ng hy cn t c nghin cu.


20/155

7

4. Cu trc ca lun nCu trc ca lun n gm: Phn mu, 4 chng ni dung v phn kt

lun chung.

Chng 1: Tnh ton ng hc ngc rbt d dn ng bng thutton hiu chnh gia lng vc t ta suy rng. Trong chng ny tc gigii thiu khi nim v ma trn ta nghch o v ng dng ca ma trn tanghch o gii cc bi ton h phng trnh i s tuyn tnh trong trnghp s lng phng trnh ca h t hn s lng cc n s cn tm. Sau trnh by mt thut ton s gii bi ton ng hc ngc ca rbt d dn ng.

Chng 2: Tnh ton ng lc hc ngc rbt d dn ng trongkhng gian thao tc da trn thut ton hiu chnh gia lng vc tta suyrng. Ni dung ch yu ca chng l tp trung xy dng mt thut ton tnh ton cc lc/mmen tc ng ln cc khp ng ca rbt d dn ngtrong khng gian thao tc.

Chng 3: iu khin trt rbt ddn ng da trn thut ton shiu chnh gia lng vc t ta suy rng. Trong chng ny gii thiu slc v csl thuyt n nh trong iu khin v ni dung bi ton iu khinchuyn ng. Ni dung chnh ca chng 3 l trnh by mt thut ton gii biton iu khin chuyn ng trong khng gian khp theo phng php iukhin dng trt.

Chng 4: ng lc hc v iu khin trt rbto BKHN-MCX-04.Trong chng ny p dng l thuyt trnh by trong 3 chng u gii bi

ton ng hc ngc, ng lc hc ngc v iu khin mt rbt o mi cch to ti Trng i Hc Bch Khoa H Ni. Trn m hnh rbt t ch to,d dng xc nh cc tham sng hc, ng lc hc ca rbt.


21/155

Chng 1

TNH TON NG HC NGC RBT DDN NG

BNG THUT TON HIU CHNH GIA LNGVC T TA SUY RNG

Trong cc ti liu v rbt ngi ta thng k hiu vc tta suy rng(vc tm t cu hnh ca rbt) bi [ ]Tn21 q,...,q,q=q v k hiu vc txcnh v tr v hng ca bn kp trong h quy chiu c nh bi

[ ]Tm21 x,...,x,x=x . T bi ton ng hc thun ca rbt ta xc nh c hthc

)(qfx = (1.1)

Trong .R,;R mn fxq Khi nm = rbt c gi l rbt chun, cnkhi nm < rbt c gi l rbt d dn ng.

Ni dung ca bi ton ng hc ngc l: Cho bit x = x(t), tm ).t(qq = Nh vy trong bi ton ng hc ngc chng ta phi thit lp c quan hhnh thc:

)(1 xfq = (1.2)Cc phng php gii bi ton ng hc ngc rbt c phn thnh

hai nhm: nhm cc phng php gii tch v nhm cc phng php s. Vics dng cc phng php gii tch gii bi ton ng hc ngc ca rbt ddn ng c trnh by trong cc ti liu v rbt d dn ng [57, 59, 66].

Trong khi vic s dng cc phng php s gii bi ton ng hc ngcca rbt d dn ng cn t c nghin cu.

Trong chng ny da trn cng thc khai trin Taylor chng ti xutmt thut ton mi gii bi ton ng hc ngc rbt d dn ng. Thut ton xut c gi l thut ton hiu chnh gia lng vc tta suy rng. Sau p dng thut ton xut gii bi ton ng hc ngc ca mt vi rbtc th, nhm minh ha cho kh nng tnh ton ca thut ton.

1.1Gii bi ton ng hc thun rbt ddn ng bng phng php matrn Denavit Hartenberg

nghin cu ng hc ca rbt, Denavit v Hartenberg [28, 38, 39,44] xut phng n gn h trc ta ln cc khu ca rbt, tchuyn i ta ca im thao tc v h ta gn lin vi h quy chiu cnh. H ta Denavit Hartenberg c xy dng nh sau:

Xt 2 khu k tip nhau ca rbt l khu th i1v khu th i nh hnh1.1. Gc iO ca h trc ta iiii zyxO c gn lin vi khu th i (h ta


22/155


23/155

10

trc 0x th do khng c khp ng th 0 nn ta c th chn ty , min ln phi vung gc vi trc 0z .

i vi h trc ta nnnn

zyxO gn vi khu th n, ta thy do khng ckhp ng th n+1 nn theo quy c trn th ta khng xc nh ctrc nz . Tr c nz khng c xc nh duy nht trong khi trc nx li cchn theo php tuyn ca trc .1nz Trong trng hp ny nu khp n lkhp quay th ta c th chn trc .z// 1nn z Ngoi ra c th chn ty sao cho hp l vi php chuyn dch trc.

Khi khp th i l khp tnh tin, v nguyn tc ta c th chn trc1iz mt cch ty . Tuy nhin trong nhiu trng hp ngi ta thng

chn trc 1iz dc theo trc ca khp tnh tin ny.

1.1.1 Cc tham sng hc v ma trn Denavit-HartenbergSau khi thit lp xong cc h ta , ta thy v tr ca h ta iiii zyxO

so vi h ta 1i1i1i1-i zyxO c xc nh bi 4 tham s sau y:

1. i l gc quay trc 1ix xung quanh trc 1iz theo chiu ngc chiukim ng h phng ca cc trc ta 1ix v ix trng nhau.

2. 'i1-ii OOd = l khong dch chuyn tnh tin dc theo trc 1iz gc

ta 1-iO chuyn n'

iO - giao im ca trc ix vi 1iz (hnh 1.1).

3.i

a l khong dch chuyn dc theo trci

x a 'i

O ti imi

O .

4. i l gc quay quanh trc ix sao cho trc 1iz chuyn n trc iz .

Nh vy vi cc php di trc nh nu trn, ta c tha h quychiu 1i1i1i1-i zyxO v trng vi h quy chiu iiii zyxO . Do vy 4 tham s i ,di, ai, i nu trn c gi l 4 tham sng hc Denavit Hartenberg.

Trong 4 tham s nu trn th cc tham s ia v i lun l cc hng s, ln ca chng ph thuc vo hnh dng v s ghp ni ca cc khu th iv i-1.Hai tham s cn li i v id th mt s l hng s cn tham s kia s l bin sph thuc vo loi khp ca khp ng th i+1. Nu khp th i+1 l khp quay

th i l bin, id l hng s. Cn khi khp l khp tnh tin th ngc li.Nh vy ta c th dng 4 php bin i cbn sau y chuyn ta

t h ta 1i1i1i1-i zyxO v h ta iiii zyxO :

- Quay xung quanh trc 1iz mt gc i .

- Dch chuyn tnh tin dc trc 1iz mt on di.


24/155

11

- Dch chuyn tnh tin dc trc ix mt on ai.

- Quay xung quanh trc ix mt gc . i

Ta k hiu ma trn ca php bin i ta t h ta 1i1i1i1-i

zyxO

sang h ta iiii zyxO l i1i H . S dng cc ta thun nht [25, 28], ta suy

ra ma trn ca php bin i ny s l tch ca 4 ma trn v c dng nh sau:

Theo [28, 38], nu quay h ta xung quanh trc 1iz mt gc i th matrn bin i cc ta s l:

=

=

1000

0100

00cossin

00sincos

1000

zaaa

yaaa

xaaa

ii

ii

)0(

O333231

)0(O232221

)0(

O131211

),z(

1

1

1

iA

Cn khi dch chuyn tnh tin theo trc 1iz mt on id th ma tr n bini s l :

=

=

1000

100

0010

0001

1000i

)0(

O333231

)0(O232221

)0(

O131211

)d,z(

1

1

1

i dzaaa

yaaa

xaaa

A

bc dch chuyn tnh tin theo trci

x mt oni

a th ma trn chuyn ita s l:

=

=

1000

0100

0010

001

1000

i

)0(

O333231

)0(

O232221

)0(O131211

)d,x(

1

1

1

i

a

zaaa

yaaa

xaaa

A

v bc quay xung quanh trc ix mt gc i , ma trn chuyn i l:

=

=

1000

0cossin0

0sincos0

0001

1000

zaaa

yaaa

xaaa

ii

ii

)0(

O333231

)0(

O232221

)0(

O131211

),x(

1

1

1

iA

Nh vy nu dng h ta Denavit Hartenberg th ma trn chuyn ita i

1i H t h 1i1i1i1-i zyxO sang h ta iiii zyxO l:


25/155

12

=

=

1000

0cossin0 0sincos0

0001

1000

0100 0010

001

1000

100 0010

0001

1000

0100 00cossin

00sincos

.

ii

ii

i

i

ii

ii

),x()a,x()d,z(),z(i1i

iiii

a

d

AAAAH

Ly tch ca 4 ma trn trn, ta c mt ma trn c gi l ma trnDenavitHartenberg v ta c kt qu nh sau:

=

1000

dcossin0

sinasincoscoscossin

cosasinsincossincos

iii

iiiiiii

iiiiiii

i

1i H (1.3)

Vi vic s dng cng thc (1.3) ta s thc hin c vic chuyn i ta t h ta ny qua h ta khc (t khu ng hc ny qua khu nghc khc). Khi , cng thc chuyn ta t h ta { }1i1i1i1-i zyxO sangh ta { }iiii zyxO l:

[ ]

=

1

z

y

x

1

z

y

x

1i

1i

1i

i1i

i

i

i

H (1.4)

1.1.2Phng trnh xc nh vtr v hng ca khu thao tcXt mt m hnh tng qut ca mt rbt c n khu nh trn hnh v 1.2.

Nh trnh by trn, ma trn iH cho ta bit :

V tr ca im iO trong h quy chiu { }1-i1-i1-i1-i1i zyxOR .

Hng ca vt rn iB trong h quy chiu { }1-i1-i1-i1-i1i zyxOR .

Nh vy, bng cch chuyn dn h quy chiu { }nnnnn zyxOR t nOv 1-nO , 2-nO , v cui cng l v h quy chiu cnh { }00000 zyxOR ta sxc nh c v tr ca im gc nO v hng ca khu ng th n trong hquy chiu cnh { }00000 zyxOR .


26/155

13

Hnh 1.2

p dng lin tip cc php bin i i vi rbt n khu ta c:

n21n

1n

2

1

1

0

n

0

n ...... HHHHHHHD === (1.5)

=

1000

zaaa

yaaa

xaaa

)0(

P333231

)0(

P232221

)0(

P131211

nD (1.6)

Ma trn nD cho ta bit v tr ca im P v hng ca khu thao tc (bnkp) ca rbt i vi h quy chiu cnh 0R .

Nh vy, khi bit c cc c tnh hnh hc ca cc khu v cc quylut chuyn ng ca cc khp l ta hon ton c th xc nh c v tr vhng ca khu thao tc (bn kp).

POn

0O

1O

2O

iO2nO

1nO


27/155

14

1.1.3Bi ton p dngV d 1.1. Xc nh v tr ca im thao tc v hng ca bn kp ca

rbt 5 khu ng (hnh 1.3).

Hnh 1.3. Rbt 5 khu ng

K hiu O0O1 = a1; O1O2 = a2; O2O3 = a3; O3O4 = a4; O4O5 = a5.

Ta c bng tham s DH ca rbt nh sau:

Bng 1.1 Thng s DH rbt 5 khu ng

Khu d a 1 q1 0 a1 02 q2 0 a2 0

3 q3 0 a3 04 q4 0 a4 05 q5 0 a5 0

Qua tnh ton ta thu c cc kt qu nh sau:

=

1000

0100 Sa0CS

Ca0SC

1111

1111

1H ;

=

1000

0100 Sa0CS

Ca0SC

2222

2222

2H ;

1q

2q

3q 5q

0y

1O

5O 4q

0x

1x

2x 3x

4x 5x

1y

5y

0O

3O

4O 2O


28/155


29/155

16

Trong ma trn Jacobi J(q) l mt ma trn ch nht. V vy, khi giibi ton ng hc ngc ta cn phi x l cc ma trn ta nghch o. Di ytrnh by mt s khi nim v ma trn ta nghch o [41].

1.2Ma trn ta nghch o1.2.1 nh ngha

Cho mt ma trn ch nht mxnRA . Ma trn nxmRX sc gi l matrn ta nghch o (ma trn nghch o suy rng) ca ma trn A khi v ch khibn phng trnh iu kin sau y c tha mn [41, 45]:

AXA = A (1.7)XAX = X (1.8)(AX)T = AX (1.9)(XA)T = XA (1.10)

1.2.2Ma trn ta nghch o Moore - PenroseTa nghch o ca ma trn thng dng l ta nghch o Moore-

Penrose (MP), c k hiu l A+ [62].

Vi A l ma trn cmxn, nu A c hng y , th ta nghch o +A ca ma trn Ac xc nh nh sau [41]:

>

=

n:

V A c hng y , nn rank(A) = n, ma trn (AA)nxn l kho.

Thay T1T )( AAAA + = ln lt vo v tri ca 4 phng trnh iu kin,ta c:

[ ] [ ] ( ) AAAAAAAA)(AAAAAA)(AAAAAT1T1T1T1T

====+ T1

++++++ ==== AAAAAAAAAAAAAAAAAA 1T1T1T1TT )()())(

[ ] [ ] ( )[ ]

[ ] ( )[ ] [ ] ( )[ ] ========

+

TT1TT1TTT1T

T11T1T

AAAAA)(AAAAAAA)(AAA

AAAAA)(AAAAA)A(AAA

T11

T1TTT1TT)(

[ ] [ ] [ ]

[ ] AAAAAA

AAAAAAAAAAAAAA

+

+

==

====1TT

1TTTT1TTTT1TT )()()()(

Nh vy ta c s khi tnh ton ta nghch o ca mt ma trn theoMoore Penrose :

Hnh 1.4. S khi tnh ton A+ theo phng php Moore-Penrose

So snh m, n

1TT )( + = AAAA T1T )( AAAA + =1+ = AA

nmA

+A

m = n

m > nm < n


31/155

18

1.2.3Phng php nhn tLagrangeXt phng trnh i s

Ax = y (1.12)

Trong ;R nmA cc vc t: mn R,R yx vi m < n.

Ta s xc nh nghim ca phng trnh (1.12), vi iu kin nghim nys lm cc tiu ha mt hm mc tiu no . y ta s chn hm mc tiu cdng ton phng ca bin x:

Wxxx T

2

1)(g = (1.13)

Vi Wnxn l ma trn trng si xng, xc nh dng v c la chnmt cch thch hp. S dng phng php nhn t Lagrange, ta xt hm mc

tiu mi c dng:

)(2

1),(g TT* AxyWxxx += (1.14)

trong mR c gi l nhn t Lagrange. ),(g* x t cc tiu, iukin cn phi c l:

0AWxx

==

T

T*g(1.15)

0yAx =+=

T*

g (1.16)

T (1.15) ta suy ra :

AWx T1= (1.17)

Th (1.17) vo (1.16), ta c:

yAAWyAAW 1T1T1 )(0 ==+

Th biu thc ca vo (1.17), ta tm c nghim ca phng trnh thamn iu kin cc tiu hm mc tiu (1.13)

yAAWAWx 1T1T1 )( = (1.18)

C th thy, trong trng hp W = In (ma trn n v cp n), nghim thuc ca phng php nhn t Lagrange cng chnh l nghim thu c theophng php Moore-Penrose.


32/155

19

Ta cng thy, nghim theo phng php Moore-Penrose l nghim ng

vi hm mc tiu22

1

2

1

2

1)(g

2

TTx

xxxIxWxxxn

T ==== cc tiu, tng ng

vi nghim c chun nh nht trong tp nghim ca (1.12)

1.2.4Nghim tng qut ca phng trnh i stuyn tnhXt phng trnh i s

Ax = y (1.19)

Trong mnnm R,R,R yxA vi m < n. Ta s chng minh rngnghim c dng:

A)zAIyAx ++ += n( (1.20)

Vi In l ma trn n v cp n, z l mt vc tty thuc Rn, +A l matrn ta nghch o ca A.

Tht vy, khi thay (1.20) vo v tri ca (1.19), ta c:

y0.zyzAAAAyAAWAAW

zAAIAyAAA)zAIyAAAx11

nn

=+=+=

=+=+=+

++++

)()(

)((1TT

Ngc li, ta gi x l 1 nghim trong tp nghim ca (1.19), ngha lAx=y, biu din x di dng

A)xAIyAA)xAIAxAxnn

++++ +=+= (( (1.21)

iu ny chng t (1.20) l nghim tng qut hn nghim (1.21) m vntha mn (1.19). Tuy nhin mi nghim ca (1.19) u c th biu din didng (1.21), do (1.20) chnh l nghim tng qut ca phng trnh (1.19) cho.

Mt cch tip cn khc gii phng trnh (1.19) l p dng phngphp nhn t Lagrange. Ta xt hm mc tiu dng ton phng:

)()(2

1)(g T zxWzxx = (1.22)

Vi Wnxn l ma trn trng si xng xc nh dng c la chn mtcch thch hp. Xt hm mc tiu mi:

)()()(2

1),(g TT* AxyzxWzxx += (1.23)

hm ),(g* x t cc tiu


33/155

20

0AzxWx

==

T

T*

)(g

(1.24)

0yAx

=+=

T*g (1.25)

T (1.24), ta suy ra

zAWx += T1 (1.26)

Thay (1.26) vo (1.25), ta c

Az)yAAW0yAzAAW 1 -()( 1-T1-T ==+

Thay biu thc nhn c vo (1.26) ta tm c nghim ca phng

trnh tha mn iu kin cc tiu ca hm mc tiu (1.22)

[ ] zAAIyAzA)AAWAWIy)AAWAWzAz)AAWAWy)AAWAWx

)(((

((

n

1T1T1

n

1T1T1

1T1T11T1T1

++

+=+

=+=(1.27)

C th thy, nghim (1.27) thu c gm 2 thnh phn: thnh phn thnht l thnh phn c chun nh nht, cn thnh phn th hai tha mn tnh cht

0zAAAAzAAIA == ++ )()( n chnh l thnh phn ca vc t z chiuln khng gian b (null space) ca A.

Thc t, trong bi ton ng hc ngc rbt d dn ng ta thng s

dng biu thc nghim theo phng php Lagrange vi ma trn trng sW = In(chnh l ma trn ta nghch o Moore-Penrose )

1.3Gii bi ton ng hc ngc rbt d dn ng bng phng phphiu chnh gia lng vc tta suy rng

1.3.1Phng php khai trin TaylorMt s tc gi quan tm n vic s dng phng php s gii bi

ton ng hc ngc ca rbt [48, 52, 54]. Trong cc cun sch [48, 52] trnh by mt thut ton s nh sau:

( ) ( ) ( ) tttt kk1k +=+ qqq & (1.28)

Trong ( )ktq& c xc nh t cng thc

( ) ( )( ) ( )kk1

k ttt xqJq &&&= (1.29)

Th (1.29) vo (1.28) ta c

( ) ( ) ( )( ) ( ) ttttt kk1

k1k +=

+ xqJqq && (1.30)


34/155

21

Vic tnh ton theo cng thc (1.30) cho kt qu kh th (iu ny sc thy r trong v dphn 1.4). Trong lun n ny chng ti tm cch citin cng thc (1.30) nhn c mt thut ton mi c chnh xc cao hn.

1.3.2 Cc cng thc xc nh vc tvn tc v vc tgia tc suy rngK hiu [ ]Tn21 q,...,q,q=q l vc t ta suy rng (vc tm t cu

hnh ca rbt), [ ]Tm21 x,...,x,x=x l vc txc nh v tr v hng ca bnkp trong h quy chiu cnh. T bi ton ng hc thun ta c h thc

)(qfx = (1.31)Trong iu khin rbt, chng ta thng gp yu cu ca bi ton cng

ngh l phi iu khin bn kp chuyn ng theo 1 quo mong mun chotrc no , ngha l chng ta phi xc nh cc gi tr ca q to ra gi tr

mong muncho x. Nh vy bi ton ng hc ngc chng ta phi thit lpc quan h:)(1 xfq = (1.32)

Trc ht o hm 2 v ca (1.31) theo thi gian, ta c :

qqJqq

fx &&& )(=

= (1.33)

Trong J(q) l ma trn Jacobi cmxn, vi :

==

n

m

2

m

1

m

n

1

2

1

1

1

q

f...

q

f

q

f............q

f...

q

f

q

f

)(q

fqJ (1.34)

Gi s hng ca J(q) l m, theo [41, 57] ta chn ma trn ta nghch oca ma trn ch nht J(q) di dng

[ ] 1)()()()( + = qJqJqJqJ TT , m < n (1.35)

Khi t biu thc (1.33) ta suy ra cng thc tnh vc tvn tc suy

rng:( ) )t()t()t( xqJq && += (1.36)

Nh th nu bit c x(t), ta tm c ( )tx& , bit c hm f(q(t)) ta tm

c ( ))t(qJ + . Theo (1.36) ta tnh c )t(q& . Vn y l tm cch xcnh q(t). Vn ny sc trnh by trong mc 1.3.3.


35/155

22

o hm 2 v ca (1.36) theo thi gian, ta c cng thc xc nh vctgia tc suy rng

( ) ( ) )t()t()t()t()t( xqJxqJq &&&&&& ++ += (1.37)

c th p dng c cng thc (1.37) ta cn phi tnh c ( ))t(qJ +& .Theo l thuyt ta c tho hm ma trn ( ))t(qJ + theo thi gian. Tuy nhin rtkh xc nh biu thc gii tch ca ma trn ny, do ta phi tm mt thutton xc nh ( ))t(qJ+& . T biu thc (1.35) ta suy ra :

( ) ( ) ( ) ( ))t()t()t()t( TT qJqJqJqJ =+ (1.38)

o hm 2 v ca (1.38) theo thi gian, ta c

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )qJqJqJqJqJqJqJqJqJ TTTT &&&& =++ ++ (1.39)

T (1.39) ta suy ra

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]{ } ( ) ( )[ ] 1TTTT ++ += qJqJqJqJqJqJqJqJqJ &&&& (1.40)

Ma trn ( )qJ& c tnh bng cch o hm trc tip cc phn t ca matrn ( )qJ theo thi gian. Th biu thc (1.40) vo (1.37) ta tm c gia tc

)t(q&& .

1.3.3Cc cng thc xc nh vc tta suy rng

S dng cc cng thc (1.36), (1.37) v (1.39) ta s xc nh c q& vq&& , nu nh xc nh c )t(q v x(t) ti thi im kho st. Nh vy, xc nh q& v q&& trc ht ta cn xc nh q(t). Thut ton xc nh q(t) nhsau:

Chia khong thi gian lm vic ca rbt [0, T] thnh N khong bngnhau

N

Tt = , ta c ttt k1k +=+ vi k = 0, 1, , N-1

p dung khai trin Taylori vi qk+1 quanh gi trqk, ta c

...)t(2

1t)tt( 2kkkk1k +++=+=+ qqqqq &&& (1.41)

Th biu thc (1.36) vo (1.41) v b qua cc v cng b bc 2 ta c:

( ) tkkk1k +=+

+ xqJqq & vi k = 0, 1, , N-1(1.42)


36/155

23

T, ta c cc bc tnh ton nh sau:

1. Tm 0q .

2. Tnh )(),(),( 000 qJqJqJ&+

.3. Tnh 0)0t( qq && == theo (1.36) v tnh ( ) 00t qq &&&& == theo (1.37).

4. Tnh 1k+q theo (1.40), ri tnh 1k+q& , 1k+q&& theo (1.36) v (1.37).

Ta thy vic tnh 1k+q theo (1.42) l kh th. V vy nng cao chnhxc, ta cn c mt thut ton xc nh 1k+q chnh xc hn nh di y.

a. Hiu chnh gia lng vc tta suy rng ti thi im t0

Gi s 0t 0 = l thi im u. Ta c th xc nh gi tr gn ng 0~q

ca 0q bng cch v (hoc bng thc nghim). Sau p dng khai trin Taylor tm gn ng tt hn ca 0q nh sau:

Gi s ly 000~ qqq +=

Theo phng trnh (1.31), ta c :

...)~()~()~()( 0000000 +

+=+== qqq

fqfqqfqfx

T suy ra

)~

()~

( 0000 qfxqqJ (1.43)Gii phng trnh i s tuyn tnh (1.43) vi n 0q , ta c

[ ])~()~( 000 qfxqJq 0 =+ .

Nu 0q th ta tnh li gi tr mi ca 0~q bi 00

*0

~:~ qqq += ri thvo (1.43) v li gii phng trnh ny. Qu trnh lp c dng li khi


37/155

24

b. Hiu chnh gia lng vc tta suy rng ti thi im tk+1

Trc tin ta xc nh gi tr gn ng 1k~

+q theo cng thc (1.42)

( ) t~

kkk1k +=+

+ xqJqq&

(1.45)Sau ta tm biu thc gn ng tt hn ca 1k+q

1k1k1k~

+++ += qqq (1.46)Ta cn xc nh 1k+q

Th (1.46) vo phng trnh (1.31), ta c :

( ) ( ) ( ) ( ) ...~~~ 1k1k1k1k1k1k1k +

+=+== +++++++ qqq

fqfqqfqfx

T suy ra

( ) ( )1k1k1k1k ~~ ++++ qfxqqJ (1.47)

Ch rng ( )1k~

+qJ l ma trn cmxn, 1k+q l vc tc n phn t. Do sn ln hn s phng trnh (m


38/155

25

Hnh 1.5. S khi gii bi ton ng hc ngc

Sai

Sai

Cho )(qfx = , )t(xx = , ( ) )(qq

fqJ

= , t0, 0q , N, T

k: = 0

0k t:t = , 0k :~

qq =

Tnh )~( kqJ+ , )t( kk xx = , )

~( kk qff =

kkk

kkkk

~:~))(~(

qqq

fxqJq

+=

= +


39/155

26

1.3.4 nh gi sai snh gi sai s ca phng php ta a vo cc cng thc xc nh

sai s ca dch chuyn, vn tc v gia tc

kkkkkkkk

kkkkkk

kkkkk

)()()t()t()t(

)()t()t()t(

)()t()t()t(

qqJqqJxxxe

qqJxxxe

qfxxxe

&&&&&&&&&&&&

&&&&&

==

====

(1.51)

Trong [ ]Tkmk2k1k )t(e)t(e)t(e)t( L=e .

ln ca cc chun ca cc vc t )t(),t(),t( kkk eee &&& cho bit chnhxc ca phng php hiu chnh gia lng vc tta suy rng.

Trong chng trnh tnh ton ta s dng chun Euclid

)t(e)t(e)t(e)t( k2mk

22k

21k +++= Le

thy r chnh xc ca phng php s ngh, ta xt cc v d sau.Trong cc v dy c bit cn ch n cc thnh gi sai s. l cckt qu nghin cu mi ca chng ti.

1.4 Cc bi ton p dngV d 1.2. Xc nh quy lut chuyn ng ca cc khu ng ca rbt

phng 5 khu ng c kt cu nh hnh v 1.6.

q1

O

E

x

y

q2

q3q4

q5

x1

x2x3

x4

x5

Hnh 1.6. Rbt phng 5 khu ngCho bit chiu di ca cc khu l:

a1 = 0.55 (m); a2 = 0.50 (m); a3 = 0.45 (m); a4 = 0.40 (m); a5 = 0.20 (m).

Phng trnh chuyn ng ca im E l:

xE = 0.8+0.1cos(2t) (m); yE = - 0.8+0.1sin(2t) (m)


40/155

27

Bn kp ca rbt phi lun to vi phng thng ng 1 gc =1(rad).

trng hp ny, do c 5 ta suy rng cn s ta xc nh v trv hng ca bn kp l 3 cho nn y l rbt d dn ng (d 2 ta suy

rng)T s kt cu ta c :

xp = a5*S12345 + a4*S1234 +a3*S123 + a2*S12 +a1*S1yp = a5*C12345 + a4*C1234 + a3*C123 + a2*C12 + a1*C1

Do ma trn Jacobi l:J11 = a5C12345+a4C1234+a3C123+a2C12+a1C1J12 = a5C12345+a4C1234+a3C123+a2C1J13 = a5C12345+a4C1234+a3C123J14 = a5C12345+a4C1234

J15 = a5C12345J21 = -a5S12345-a4S1234-a3S123-a2S12-a1S1J22 = -a5S12345-a4S1234-a3S123-a2S12J23 = -a5S12345-a4S1234-a3S123J24 = -a5S12345-a4S1234J25 = -a5S12345

S dng chng trnh gii bi ton ng hc ngc cho trng hp c sdng thut ton hiu chnh gia lng v khng s dng thut ton hiu chnh gialng vi cc bc thi gian tnh ton khc nhau, ta thu c cc th gcquay, vn tc gc v gia tc gc ca cc khp ng cng cc sai s v tr do ccphng php tnh a li nh sau:

a. Chn bc thi gian tnh ton h = t = 0.001 (s) Khi khng sdng thut ton hiu chnh gia lng vc tta suy

rng

Hnh 1.7. Cc c tnh chuyn ng ca khu 1

0 1 2 3 4 5 6 7 8 9 10-2

-1

0

1

2

3

4

time [s]

q1

[rad]

q1

[1/s]

q1

[1/s2]


41/155

28




0 1 2 3 4 5 6 7 8 9 10-3

-2

-1

0

1

2

3

4

5

time [s]

q4

[rad]

q4

[1/s]

q4 [1/s2]

0 1 2 3 4 5 6 7 8 9 10-3

-2

-1

0

1

2

time [s]

q3

[rad]q3 [1/s]

q3[1/s 2]

0 1 2 3 4 5 6 7 8 9 10-2

-1.5

-1

-0.5

0

0.5

1

1.5

time [s]

q2

[rad]

q2

[1/s]q

2[1/s2]


42/155

29


0 1 2 3 4 5 6 7 8 9 10-14

-12

-10

-8

-6

-4

-2

0

2x 10

-4

time [s]

ex

[m]

Hnh 1.12. Sai s v tr ca im thao tc theo trc x

0 1 2 3 4 5 6 7 8 9 10-12

-10

-8

-6

-4

-2

0

2

4x 10

-4

time [s]

ey

[m]

Hnh 1.13. Sai s v tr ca im thao tc theo trc y

0 1 2 3 4 5 6 7 8 9 10-10

-8

-6

-4

-2

0

2

4

time [s]

q5

[rad]

q5

[1/s]q

5[1/s2]


43/155

30

0 1 2 3 4 5 6 7 8 9 10-4

-2

0

2

4

6

8x 10

-15

time [s]

e

[rad]

Hnh 1.14. Sai s gc nh hng ca bn kp

-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

x [m]

y[m]

Hnh 1.15. Dng chuyn ng ca rbt theo kt qu tnh ton

Khi c sdng thut ton hiu chnh gia lng vi sai scho php = 10-6(rad), kt qu nhn c l :


0 1 2 3 4 5 6 7 8 9 10-2

-1

0

1

2

3

4

time [s]

q1

[rad]

q1

[1/s]

q1

[1/s2]


44/155

31




0 1 2 3 4 5 6 7 8 9 10-4

-2

0

2

4

6

time [s]

q4

[rad]q

4[1/s]

q4[1/s

2]

0 1 2 3 4 5 6 7 8 9 10-3

-2

-1

0

1

2

time [s]

q3

[rad]q

3[1/s]

q3[1/s

2]

0 1 2 3 4 5 6 7 8 9 10-2

-1.5

-1

-0.5

0

0.5

1

1.5

time [s]

q2

[rad]q

2[1/s]

q2[1/s

2]


45/155

32


0 1 2 3 4 5 6 7 8 9 10-6

-4

-2

0

2

4

6x 10

-16

time [s]

ex

[m]

Hnh 1.21. Sai s theo trc x ca im thao tc

0 1 2 3 4 5 6 7 8 9 10

-1

0

1

2

3x 10

-15

time [s]

ey

[m]

Hnh 1.22. Sai s theo trc y ca im thao tc

0 1 2 3 4 5 6 7 8 9 10-10

-8

-6

-4

-2

0

2

4

time [s]

q5

[rad]q5

[1/s]

q5[1/s

2]


46/155

33

0 1 2 3 4 5 6 7 8 9 10-1

-0.5

0

0.5

1

1.5

2

2.5x 10

-15

time [s]

e

[rad]


-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

x [m]

y[m]


Qua cc hnh 1.12, 1.13, 1.14, 1.21, 1.22, 1.23 ta nhn thy bc thigian tnh ton h = 0.001(s) th mc chnh xc ca phng php tnh khi c sdng thut ton hiu chnh gia lng vc t ta suy rng t c l 10-15 -10-16 (hnh 1.21 1.23) cao hn nhiu so vi khi khng s dng thut ton hiuchnh gia lng vc tta suy rng (cht ti 10-4, hnh 1.12, 1.13).


47/155

34

b. Chn bc thi gian tnh ton h = t = 0.01 (s) Trng hp tnh ton khng sdng thut ton hiu chnh gia lng

vc tta suy rng ta c:

0 2 4 6 8 10 12 14 16 18 20-0.025

-0.02

-0.015

-0.01

-0.005

0

0.005

time [s]

ex

[m]


0 2 4 6 8 10 12 14 16 18 20-20

-15

-10

-5

0

5

x 10-3

time [s]

ey

[m]


0 2 4 6 8 10 12 14 16 18 20-4

-2

0

2

4x 10

-15

time [s]

e

[rad]



48/155

35

-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

x [m]

y[m]


Khi c sdng thut ton hiu chnh gia lng vc tta suy rng

0 2 4 6 8 10 12 14 16 18 20-6

-4

-2

0

2

4

6x 10

-16

time [s]

ex

[m]


0 2 4 6 8 10 12 14 16 18 20-6

-4

-2

0

2

4

6x 10

-16

time [s]

ey

[m]



49/155

36

0 2 4 6 8 10 12 14 16 18 20-1

0

1

2

3x 10

-15

time [s]

e

[rad]


-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

x [m]

y[m]


Nh vy khi tng t = h ln (gim sim tnh ton xung) ta thy khi sdng thut ton hiu chnh gia lng vc t ta suy rng th chnh xc

tnh ton vn t kh cao (sai s 10-15

10-16

, hnh 1.29 1.31), v vy m mbo c s chnh xc v quo mong mun cho im thao tc E (hnh 1.32).Cn khi khng s dng thut ton hiu chnh, chnh xc cht ti 10-3 (hnh1.25, 1.26), v vy m sai s tch ly ln v quo chuyn ng ca im E skhng c chnh xc. Trn hnh 1.28 ta thy khi khng s dng thut ton hiuchnh gia lng vc t ta suy rng, sau mt thi gian tnh ton, do sai stch ly m im E ca khu thao tc khng vch nn 1 ng trn nh nhim


50/155

37

v cng nght ra. Trn hnh 1.32 do c s dng thut ton hiu chnh vc tta suy rng nn im E vn lun chuyn ng trn quo chng trnh.

c. Vi bc thi gian tnh ton h = t = 0.05 (s) Khi khng sdng thut ton hiu chnh gia lng vc tta suy

rng

0 2 4 6 8 10 12 14 16 18 20-0.15

-0.1

-0.05

0

0.05

time [s]

ex

[m]


0 2 4 6 8 10 12 14 16 18 20-0.15

-0.1

-0.05

0

0.05

time [s]

ey

[m]


0 2 4 6 8 10 12 14 16 18 20-2

-1

0

1

2x 10

-15

time [s]

e

[ra

d]



51/155

38

-0.5 0 0.5 1

0

0.2

0.4

0.6

0.8

1

x [m]

y[m]


Khi c sdng thut ton hiu chnh gia lng vc tta suy rngta nhn c cc kt qu nhsau :

0 1 2 3 4 5 6 7 8 9 10-4

-2

0

2

4x 10

-16

time [s]

ex

[m]


0 1 2 3 4 5 6 7 8 9 10-6

-4

-2

0

2

4

6x 10

-16

time [s]

ey

[m]



52/155

39

0 1 2 3 4 5 6 7 8 9 10-1

0

1

2

3x 10

-15

time [s]

e

[rad

]


-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

x [m]

y[m]


Qua cc kt qu tnh ton c c cc trng hp ca v d 1.2 ta nhnthy khi dng thut ton hiu chnh gia lng vc tta suy rng gii biton ng hc ngc th mc chnh xc ca cc kt qu tnh ton s cao hnnhiu so vi khi khng s dng thut ton (10-14, 10-15, 10-16 so vi 10-4, 10-3,10-2) xem cc hnh 1.12, 1.13, 1.14, 1.21, 1.22, 1.23, 1.25, 1.26, 1.27, 1.29, 1.30,1.31, 1.33, 1.34, 1.35, 1.37, 1.38, 1.39). c bit khi s dng thut ton hiu

chnh gia lng vc tta suy rng th chng ta c th gim bt khi lngtnh ton xung (do c th tng bc thi gian tnh ton ln) cn phng phptnh ton m khng s dng thut ton th khng thc (xem cc hnh 1.28,1.32, 1.36, 1.40).


53/155

40

V d 1.3. Xc nh quy lut chuyn ng ca cc khp ng thuc rbt6 khu ng c s kt cu nh hnh v 1.41.

Hnh 1.41. Rbt 6 khu ng

0x

1x

1y

2x3x

4x

5x 6x

6y

1q

2q

3q

4q

5q

6q 0y

O0

O1

O2

O3

O4

O5O6

Bit : Kch thc ca cc khu l:

a1 = a2 = 0.30m; a3 = a4 =a5 = 0.40m; a6 = 0.25m

Phng trnh chuyn ng ca im thao tc l:xp = 0.8+0.2cos(2t); yp = -0.8+0.2sin(2t) ;

Khu thao tc luoon to vi phng nm ngang 1gc cnh =900,T s kt cu ca rbt ta c:

xp = a6*C123456 + a5*C12345 + a4*C1234 +a3*C123 + a2*C12 +a1*C1yp = a6*S123456 + a5*S12345 + a4*S1234 + a3*S123 + a2*S12 + a1*S1

Do ma trn Jacobi l:J11 = -a6S123456-a5S12345-a4S1234-a3S123-a2S12-a1S1J12 = -a6S123456-a5S12345-a4S1234-a3S123-a2S12J13 = -a6S123456-a5S12345-a4S1234-a3S123J14 = -a6S123456-a5S12345-a4S1234J15 = -a6S123456-a5S12345

J16 = -a6S123456J21 = a6C123456+a5C12345+a4C1234+a3C123+a2C12+a1C1J22 = a6C123456+a5C12345+a4C1234+a3C123+a2C1J23 = a6C123456+a5C12345+a4C1234+a3C123J24 = a6C123456+a5C12345+a4C1234J25 = a6C123456+a5C12345J26 = a6C123456


54/155

41

Chn bc chia thi gian h = 0.001(s), chn sai s 610= (rad). S dngchng trnh gii bi ton ng hc ngc, ta thu c th v tr, vn tc gcv gia tc gc ca cc khp v sai s tnh ton theo thut ton xut.

0 2 4 6 8 10-2

-1

0

1

2

Thoi gian(s)

Gocquaycuacackhop(rad)

q1

q2

q3

q4

q5

q6

Hnh 1.42. th gc quay ca cc khp (rad)

0 2 4 6 8 10

-1

-0.5

0

0.5

1

Thoi gian(s)Van

tocgoccuacac

khop

(ra

d/s)

q'1

q'2

q'3

q'4

q'5

q'6

Hnh 1.43. th vn tc gc cc khp (rad/s)

0 2 4 6 8 10-2

-1

0

1

2

Thoi gian(s)

Giatoc

goccac

khop

(ra

d/s

2)

q"1

q"2

q"3

q"4

q"5

q"6

Hnh 1.44. th gia tc gc cc khp ng (rad/s2)


55/155

42

0 2 4 6 8 10-1

0

1

2

3

4x 10

-13

Thoi gian(s)Sa

isov

itritrong

khongg

ian

th

ao

tac

(m,

rad)

Sai so theo truc x

Sai so theo truc ySai so goc dinh huong

Hnh 1.45. th sai s v tr trong khng gian thao tc

0 2 4 6 8 10-2

-1

0

1

2x 10-15

Thoi gian(s)Saisovantoctrongkhonggianthaotac(m/s

)

Sai so truc x

Sai so truc y

Hnh 1.46. th sai s vn tc trong khng gian thao tc

0 2 4 6 8 10-1

0

1

2

3

4x 10

-13

Thoi gian(s)Saisogiatoctrongkhonggianthaotac(m/s2)

Sai so truc x

Sai so truc y

Hnh 1.47. th sai s gia tc trong khng gian thao tc


56/155

43

Cc hnh t 1.42 n 1.44 cho ta bit s thay i ca v tr, vn tc v giatc ca cc khp. c bit l cc hnh t 1.45 n 1.47 ch ra sai s ca cc gcquay, vn tc gc v gia tc gc ca cc khp khi tnh theo thut ton hiu

chnh vc tta suy rng, cc sai s ny l rt b: sai si vi cc gc quaynh hn 4x10-13 rad, i vi cc vn tc gc nh hn 2x10-15 rad/s, i vi giatc gc nh hn 4x10-13 rad/s2.

Qua hai v d 1.2 v 1.3 ta thy sai s ca phng php rt b. iu chng t tnh u vit ca thut ton hiu chnh gia lng vc tta suy rng.

1.5Kt lun chng 1Trong chng mt trnh by mt thut ton s gii bi ton ng hc

ngc ca rbt d dn ng. Thut ton ny c hon thin di s hngdn ca GS. Nguyn Vn Khang. Chng ti ngh gi tn ca phng php s

ny l thut ton hiu chnh gia lng vc tta suy rng.Theo thut ton trn, mt phn mm tnh ton ng hc rbt d dn

ng c xy dng da trn phn mm MATLAB. Cc v dc trnhby trong chng ny nhm minh ha cho thut ton v chnh xc ca thutton cng nh th hin r tnh u vit ca phng php ny so vi phng phptnh ton khng s dng thut ton hiu chnh gia lng vc tta suy rng.y l cc v d mi, cha c trong cc sch chuyn kho.

Qua cc v d ta thy chnh xc ca thut ton kh cao. Do c ths dng thut ton ny tnh ton ng lc hc ngc cc ccu rbt phctp, bi v vic tnh ton ng lc hc cc rbt phc tp i qu nhiu phptnh do m sai s ca phng php c mt vai tr ht sc quan trng.


57/155

Chng 2

TNH TON NG LC HC NGC RBT DDN NG

TRONG KHNG GIAN THAO TC DA TRN THUT TON HIU CHNHGIA LNG VC T TA SUY RNG

Trong chng ny cc phng trnh Lagrange loi 2 c s dng thitlp cc phng trnh vi phn chuyn ng ca rbt. Cho bit chuyn ng cakhu thao tc, tm biu thc ca lc/mmen ca cc khu dn ng l ni dungchnh ca bi ton ng lc hc ngc. Nh th biton ng lc hc ngc rbt

phi da trnbi ton ng hc ngc v c lin h mt thit vi bi ton iukhin rbt. Trong chng ny da trn thut ton hiu chnh gia lng vc tta suy rng, trnh by mt thut ton gii bi ton ng lc hc ngc rbt ddn ng trong khng gian thao tc.

2.1 Dng thc Lagrange loi 2 ca h nhiu vtXt ch gmp vt rn, chu rlin kt hlnm. S bc t do ca ch l

r6p = . Trong on ny da trn khi nim ma trn Jacobi, ta thit lp dngthc Lagrange loi 2 cho h nhiu vthlnm [17, 55].

K hiu cc ta suy rng lT

f1 ]q,,q[ K=q (2.1)

V tr ca mi vt rn Bi trong hquy chiu cnh 0000 zyOxR = c xcnh bi vc txc nh v tr khi tm vma trn csin ch hng ca vt rn.

)t,(),t,( iiCC ii qAAqrr == (2.2)

n gin, di y ta ch xt cch hlnm gi v dng. Khi biu thc (2.2) c dng

)(),( iiCC ii qAAqrr == (2.3)

Theo [17] trng thi vn tc ca vt rn Bic xc nh bi vn tc khitm v vn tc gc ca n

i

T

iii

CC

C~,

dt

dii

iAAq

q

rrv =

== && (2.4)

x0

z0

R0 y0

Hnh 2.1

xi

yi

zi

Ci

Bi


58/155

45

Nu ta a vo cc k hiu ma trn Jacobi tnh tin iTJ v ma trn Jacobi

quay iRJ [17] theo cc cng thc

qJ

qrJ

&=

= iR

CT i

ii , (2.5)

th vn tc khi tm vt rn v vn tc gc ca n c th vit di dng sauqqJqqJv && )(,)( iii RiTC == (2.6)

T biu thc tnh ng nng ca mt vt rn trong [17, 64, 67], ta suy rabiu thc ng nng ca h p vt rn.

===

+==p

1iii

Ti

p

1iC

TCi

p

1ii 2

1m

2

1TT ii Ivv (2.7)

Trong (2.7) th Ii l ma trn ca tenxqun tnh ca vt rn th i i vikhi tm Ci ca n trong h qui chiu cnh R0.Ch . Trong tnh ton thc hnh c khi ngi ta thay vic tnh biu thc

=

p

1iii

T

i I bng biu thc )i(i)i(iT)i(i I . Trong )i(iI l ma trn ca tenxqun

tnh ca vt rn i vi khi tm Ci ca n trong h qui chiu ng iiii zyxC gn

lin vo vt Bi, cn)i(

i l vc ti s ca ir

trong h qui chiu iiii zyxC .

Th cc biu thc (2.6) vo biu thc (2.7) ta c

( ) ( )

( )

=

==

+=

+=p

1iRi

TRT

TTi

T

p

1iRi

TR

p

1iT

TTi

]m[2

12

1m2

1T

iiii

iiii

qJIJJJq

qJIqJqJqJ

&&

&&&&(2.8)

By gita a vo k hiu

( )=

+=p

1iRi

T

RT

T

Ti iiiim)( JIJJJqM (2.9)

Biu thc (2.9) c gi l ma trn khi lng suy rng. Khi biu thcng nng ca h nhiu vt c dng rt gn nh sau:

qqMq && )(21T T= (2.10)

Trong


59/155

46

=

=

=

=

f

1

f

1

f

1

ff1f

f111

q

q

,

q

q

,

q

q

,

)(m)(m

)(m)(m

)(

&&

M

&&

&&

&

M

&

&M

K

KKK

K

qqq

qq

qq

qM (2.11)

Biu thc ng nng (2.10) c th vit li di dng nh sau

( )= =

=f

1j

f

1kjkf1jk qqq,,qm2

1T &&K (2.12)

o hm biu thc ng nng (2.12) theo cc vn tc suy rng jq& v cc ta

suy rng qj ta c

= ==

=

= f

1j

f

1kjk

i

ik

i

f

1jjij

i

qqq

m

2

1

q

T,qm

q

T&&&

&

T suy ra

= ==

+=

f

1j

f

1kjk

k

ijf

1jjij

i

qqq

mqm

q

T

dt

d&&&&

&

Th cc biu thc trn vo phng trnh Lagrange loi 2

( )f,,1jqq

T

q

T

dt

dj

jjj

K&

=+

=

(2.13)

ta c

)f,,1j()(gqqm21mqm jjf

1j

f

1klkj,kll,jk

f

1jjij K&&&& ==+ + = == q (2.14

Trong ta s dng cc k hiu

j

j

j

klj,kl

l

jk

l,jk q)(g,

q

mm,

q

mm

=

=

= q (2.15)

Do ma trn khi lng )(qM l ma trn i xng, nn ta c

j,kii,jkk,ij mmm == . Do phng trnh (2.14) c th vit li di dng nh sau

( ) )f,,1i(q

qqmmm2

1qm

ii

f

1k

f

1l jkj,kll,jkk,lj

f

1j jij

K&&&& =+

=++

= == (2.16)

Phng trnh (2.14) hoc (2.16) thng c gi l dng thc Lagrange loi2 ca h nhiu vt.

Nu ta a vo k hiu

i,kjj,ikk,ijk,ij mmm)(h +=q (2.17)


60/155

47

Th phng trnh (2.14) c th vit li di dng

)f,,1i()(gqqhq)(m iif

1j

f

1kjkk,ij

f

1jjij K&&&& =+++

= ==

qq (2.18)

thun tin cch vit ta a vo k hiu

=

=f

1kkk,ijij q)(h)(c && qqq, (2.19)

khi

== =

=f

1jjij

f

1j

f

1kjkk,ij q),(cqq)(h &&&& qqq (2.20)

Phng trnh vi phn (2.18) by gic dng

)f,,1i()(gq)(cq)(m iif

1jjij

f

1jjij K&&&& ==++

==

qqq,q (2.21)

Phng trnh (2.21) c th vit li di dng ma trnqgqqqCqqM =++ )(),()( &&&& (2.22)

trong :nRq l vc tta suy rng,

nnR)( qM l ma trn khi lng, i xng xc nh dng,nnR),( qqC & l ma trn lin quan n lc coriolis v lc ly tm,

nR)( qg l vc tlc do trng lc,n

R l vc tlc dn ng t cc ng c.Trong cc bi ton iu khin rbt ngi ta thng hay s dng phngtrnh viphn chuyn ng dng (2.22).

Vic thit lp phng trnh vi phn chuyn ng ca rbt ngy nay ctin hnh mt cch tng nhcc phn mm. trng i hc Bch khoa H

Ni, nhm nghin cu ca GS. Nguyn Vn Khang xy dng phn mmROBODYN [49] thit lp tng cc phng trnh vi phn chuyn ng carbt. Trong lun n ny s dng phn mm ROBODYN thit lp phng trnhvi phn chuyn ng ca cc rbt.

2.2 Gii bi ton ngc ng lc hc rbt ddn ng trong khng gianthao tcMt trong cc bi ton quan trng khi tnh ton thit k rbt l xc nh cc

lc/mmen dn ng cn thit cho cc khu dn ca rbt khu thao tc carbt c th lm vic theo mt chng trnh nh trc. Bi ton ny c gil bi ton ng lc hc ngc. Bi ton ng lc hc ngc mt mt phi da vo


61/155

48

cc thut ton ca bi ton ng hc ngc, mt khc c lin quan mt thit ti biton iu khin chuyn ng ca rbt.

Trong phn ny, da trn thut ton hiu chnh gia lng vc t ta suyrng gii bi ton ng hc ngc trnh by mt thut ton s gii cc bi tonng lc hc ngc cho rbt d dn ng trong khng gian thao tc.

Mi lin h gia v tr ca bn kp vi cc bin khp c th biu din didng

x = f(q) (2.24)

trong nRq l vc tcha cc bin khp, mRx l vc tcha v tr tm cabn kp trong mt h ta cnh v hng ca bn kp.

[ ]Tm21 xxx K=x [ ]T

n21 qqq K=q o hm 2 v ca (2.24) theo thi gian, ta c

qqJqq

fx &&& )(=

= (2.25)

trong J(q) l ma trn Jacobi c nm c xc nh theo (1.34). Gi s hngca J(q) l m, theo [41, 45] ta chn

1TT )]()()[()( + = qJqJqJqJ (2.26)Ma trn )(qJ + c gi l ma trn ta nghch o ca ma trn J(q). T

(2.25), nu ch quan tm n nghim bnh phng ti thiu, ta c

xqJq && )(+= (2.27)o hm 2 v ca (2.27) theo thi gian, ta c

xqJxqJq &&&&&& )()( ++ += (2.28)

vi ).(dt

d)( qJqJ ++ =&

Cc cng thc (2.27) v (2.28) cho php ta xc nh c vc tvn tc vvc tgia tc suy rng q& v q&& , nu nh bit q(t) ti thi im kho st v quy lutchuyn ng ca bn kp )t(),t(),t( xxx &&& .

S dng phng php hiu chnh gia lng vc tta suy rng trnhby trong chng 1, chng ta c cc bc xc nh q, q& , q&& nh sau:Bc 1. Gii h phng trnh i s tuyn tnh

)~()t()~( 000 qfxqqJ (2.29)c

)]~()t()[~( 000 qfxqJq =+ . (2.30)


62/155

49

Bc 2. iu chnh nghim

000~:~ qqq += . (2.31)

Bc 3. Kim tra iu kin sai sNu 0q , l sai s cho php, chuyn sang bc 4, ngc li th ta trli bc 1.

Bc 4. Ly nghim 00~qq = .

Nu khng ch quan tm n nghim bnh phng ti thiu ca phng trnh(2.25) th biu thc nghim (2.27) c thc vit li nh sau:

0)]()([)( zqJqJExqJq++ += && (2.32)

trong n0 Rz l mt vectty vnnR E l mt ma trn n v.

Vic chn vc tz0 cho php ta khai thc c u im (kh nng trnh vtcn, trnh va chm vi gii hn khp, trnh cu hnh k d) ca rbt d dn ngso vi rbt chun. Trong phn ny, trnh va chm vo gii hn khp ta s chnz0 hm sau y t cc tiu

=

=n

1i

2

imiM

iii qq

qqc

2

1)(q (2.33)

Vi qiM (qim) l k hiu ca gii hn ln nht (nh nht), iq l gi tr giaca khong lm vic ca khp, v cc trng s dng ci > 0. Do , cc tiu cahm ny s lm cho cc bin khp di chuyn v gi tr trung gian, tnh d dn ngsc khai thc gi cho cc bin khp gn gi tr gia ca khong lm vicca rbt, trnh c va chm vi cc gii hn khp.

Ta ly)(0 qz = (2.34)

trong : )(q : gradient ca )(q , hng s m nu )(q cn cc tiu ho.

T (2.32)suy ra

00 )]()([dtd)]()([

)()(

zqJqJzqJqJE

xqJxqJq

++

++

+

+=

&

&&&&&&

(2.35)

Sau khi tnh c cc gi tr ca )t(),t(),t( kkk qqq &&& ti thi im tk, xp xban u cho thi im tip theo ttt k1k +=+ c chn nh sau

t)]()([t)(q 0kkkk1k ++=++

+ zqJqJExqJq & .


63/155

50

Sau thut ton iu chnh gia lng sai s vectta suy rng c pdng tinh chnh gi tr xp x trn.

Phng trnh ng lc ca tay my rbt c th nhn c nh s dngphng trnh Lagrng loi 2 nh sau

qgqqqCqqM =++ )(),()( &&&& , (2.36)

S dng file s liu cc vc t qqq &&& ,, ca bi ton ng hc ngc, tphng trnh (2.36) ta c th xc nh mmen/lc cn thit tng ng vi chuynng mong mun x(t) ca bn kp.

Cc bc tin hnh tnh mmen/lc ng ckhi bn kp chuyn ng theomt quy lut x(t) cho trc nh sau:

1. Gii bi ton ng hc ngc, xc nh cc ta , vn tc v gia tc suy

rng qqq &&& ,, ca cc khp t chuyn ng ca bn kp )t(),t(),t( xxx &&& .2. S dng phng trnh (2.36) tnh c cc mmen/lc ca cc ng c

dn.Ton b thut ton xc nh mmen/lc dn ng cc khu rbt d dn

ng trong khng gian thao tc c minh ha nh trn s khi (hnh 2.2).


64/155

51

KT THC


65/155

52

2.3Cc bi ton p dngV d 2.1. Xc nh lc/mmen, phng trnh chuyn ng ca rbt Scara

nh hnh 2.3 khi quy lut chuyn ng ca bn kp c dng:xp = 0.30+0.08cos(t) (m), yp = 0.30+0.08sin(t) (m), zp = 0.05cos(t) (m),

Hnh 2.3. S kt cu rbt Scara

Rbt Scara c 4 khp dn ng dng RRTR trong khi theo yu cu ca biton cn c 3 ta xc nh v tr ca bn kp v vy trong trng hp ny n lrbt d dn ng (d 1 khp dn ng).

T s kt cu ca rbt ta c bng cc tham sng hc:Bng 2.1 Cc tham sng hc ca rbt Scara

Khu d a

1 q1 d1 a1 02 q2 0 a2

3 0 q3 0 0

4 q4 d4 0 0


66/155

53

Gii bi ton ng hc thun ta c:

+++++

++++

=

1000

dqd100)qsin(a)qqsin(a0)qqqcos()qqqsin(

)qcos(a)qqcos(a0)qqqsin()qqqcos(

D134

11212421421

11212421421

4

134P

11212P

11212P

dqdz

)qsin(a)qqsin(ay

)qcos(a)qqcos(ax

+=

++=

++=

suy ra ta ca tm bn kp v ma trn Jacobi l:

+

=

0100

00CaCaCa

00SaSaSa

J 12212211

12212211

Chn bc thi gian h = 0.001 giy; sai s cho php 610 t dngchng trnh gii bi ton ng hc ngc ta nhn c th gc quay, vn tcgc, gia tc gc v cc sai s chuyn ng ca bn kp l:

0 2 4 6 8 10-3

-2

-1

0

1

2

3

Thoi gian(s)

Vitricackhau(rad,m)

q1(rad)

q2(rad)

q3(m)

q4(rad)

Hnh 2.4. th chuyn ng ca cc khp ng rbt Scara


67/155

54

0 2 4 6 8 10-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

Thoi gian(s)

Vantoccackhau(rad/s,m/s)

q.1(rad/s)

q.2(rad/s)

q.3(m/s)

q.4(rad/s)

Hnh 2.5. th vn tc chuyn ng ca cc khp ng rbt Scara

0 2 4 6 8 10-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

Thoi gian(s)

G

iatoccackhau(rad/s2,m/s2)

q..1(rad/s2)

q..2(rad/s2)

q..3(m/s2)

q..4(rad/s2)

Hnh 2.6. th gia tc chuyn ng ca cc khp ng rbt Scara


68/155

55

0 2 4 6 8 10-6

-4

-2

0

2x 10

-15

Thoi gian(s)

Saiso

vitritrongkhonggianthaotac(m)

Sai so truc x

Sai so truc y

Sai so truc z

Hnh 2.7. th sai s v tr bn kp ca rbt Scara

0 2 4 6 8 10-2

-1.5

-1

-0.5

0

0.5

1

1.5

2x 10

-16

Thoi gian(s)

Saiso

vantoctrongkhonggianthao

tac(m/s)

Sai so truc x

Sai so truc y

Sai so truc z

Hnh 2.8. th sai s vn tc bn kp ca rbt Scara


69/155

56

0 2 4 6 8 10-3

-2

-1

0

1

2

3x 10

-16

Thoi gian(s)Saisogiatoctrongkhonggianthaotac(m/s2)

Sai so truc x

Sai so truc y

Sai so truc z

Hnh 2.9. th sai s gia tc bn kp ca rbt Scara

Bng 2.2. Bng thng sng lc rbt Scara 4 bc t do

V tr trng tm (so vi

gc to gn trn mi

khp)

Mmen qun tnh khi tng khu

(tnh i vi h tot ti

trng tm tng khu v song songvi to khp)

Khu

xC yC zC

Khi

lng

Ixx Iyy Izz Ixy Ixz Iyz

1 -(a1-l1x) 0 -l1z m1 I1x I1y I1z 0 0 0

2 -(a2-l2) 0 0 m2 I2x I2y I2z 0 0 0

3 0 0 -l3 m3 I3x I3y I3z 0 0 0

4 0 0 -(d4-l4) m4 I4x I4y I4z 0 0 0

Vi bng tham sng lc hc nh trong bng 2.2, thc hin chng trnhng lc hc ngc ta xc nh c biu thc tnh mmen/lc tc ng trn cckhp ng ca rbt:


70/155

57

1 = (m1.l12+Iz1+m2.a1

2+2.m2.l2.a1.cos(q2)+m2.l22+Iz2+m3.a2

2+2.m3.a2.a1.cos(q2)+m3.a1

2+Iz3+m4.a22+2.m4.a2.a1.cos(q2)+m4.a1

2+Iz4). q1_2dot +(m2.l2.a1.cos(q2) +m2.l2

2+Iz2+m3.a22+m3.a2.a1.cos(q2)+Iz3+m4.a2

2+m4.a2.a1.cos(q2)+Iz4). q2_2dot -Iz4.q4_2dot -a1.sin(q2).(m2.l2+m3.a2+m4.a2). q2_2dot .q1_dot-a1.sin(q2).(m2.l2+m3.a2+m4.a2).(q2_dot+q1_dot).q2_dot;

2 = (m2.l2.a1.cos(q2)+m2.l22+Iz2+m3.a2

2+m3.a2.a1.cos(q2)+Iz3+m4.a22+

m4.a2.a1.cos(q2)+Iz4). q1_2dot +(m2.l22+Iz2+m3.a2

2+Iz3+m4.a22+Iz4). q2_2dot -

Iz4. q4_2dot +a1.sin(q2).(m2.l2+m3.a2+m4.a2).q1_dot2;

3 = (m3+m4). q3_2dot -m3.g-m4.g;

4 = -Iz4. q1_2dot -Iz4. q2_2dot +Iz4. q4_2dot;

Vi b cc gi tr tham sng lc hc :m1 = 10; m2 = 5; m3 = 2; m4 = 1 (kg)a1 = 0.22; a2 = 0.40; d4 = 0.09 (m)l1 = 0.15; l2 = 0.16; l3 = 0.19; l4 = 0.04 (m)Iz1 = 0.21; Iz2 = 0.20; Iz3 = 0.18; Iz4 = 0.15 (kg.m

2)

Ta vc th bin thin ca mmen/lc theo thi gian trn cc khp ngl:

0 2 4 6 8 10-0.6

-0.4

-0.2

0

0.2

0.4

Thoi gian(s)

Momenkhop1(Nm)

Momen1

Hnh 2.10. th mmen tc ng trn khp 1


71/155


72/155

59

0 2 4 6 8 10-0.06

-0.04

-0.02

0

0.02

0.04

Thoi gian(s)

Momenkhop4(Nm)

Momen4

Hnh 2.13. th mmen tc ng trn khp 4

Qua cc th trn ta thy rng kt qu gii phn ng hc ngc xcnh v tr, vn tc v gia tc ca cc khp ng rbt Scara c chnh xc khcao (cc hnh: 7, 8, 9), sai s v tr b hn 10-14 cn sai s ca vn tc v ca giatc b hn 10-15, v vy m khi thay vo biu thc tnh ton gi tr calc/mmen cng s cho ta kt qu vi chnh xc cao.

V d 2.2. Gii bi ton ng lc hc ngc ca mt rbt phng 5 khu

ng (hnh 2.14). Khu i c chiu di ai, khi tm Ci, khong cch t khi tm nkhp ni vi khu trc l li, khi lng mi, v c mmen qun tnh i vikhi tm Izi, (i = 1,2,...,5). Mt s thng s ca rbt c cho trong bng 2.3.

q1

O

g

E

x

y

A

B C

q2

q3 q4

C1

C2

C3C4

q5

D

Hnh 2.14. S rbt phng 5 khu ng


73/155

60

Bn kp (khu 5) c yu cu chuyn ng theo mt qu o trn cphng trnh l

)t2sin(1.08.0y

)t2cos(1.08.0x

+=

+=

v bn kp phi lun to vi phng thng ng 1 gc )rad(1= .

Bng 2.3 Cc thng sng lc hc ca rbt 5 khu ng

Khu 1 2 3 4 5m [kg] 10 7.5 5 4 1

Iz[kgm2]

0.11 0.10 0.03 0.02 0.02

a [m] 0.55 0.50 0.45 0.40 0.20l[m] 0.15 0.16 0.19 0.20 0.10

Ta c bng cc thng s DH ca rbt nh di y:

Bng 2.4 Cc thng sng hc ca rbt 5 khu ng

Khu d a

1 q1 0 a1 0

2 q2 0 a2 0

3 q3 0 a3 04 q4 0 a4 0

5 q5 0 a5 0

Ta s s dng kt qu ca bi ton ng hc ngc thu nhn c v d1.2 ca chng 1.

Vi chng trnh ROBOTDYN ta s thit lp c phng trnh vi phnchuyn ng ca cc khu, song do phng trnh qu di m khng trnh by c

th trong lun n ny. Vi cc thng sng lc hc cho trong bng 2.3 ta tnhton v vc th mmen tc ng trn cc khp ng l:


74/155

61

0 5 10 15 20-40

-20

0

20

40

60

80

100

120

140

160

time [s]

Moment[Nm]

u1

u2

u3

u4

u5

Hnh 2.15. th cc mmen ng ctheo thi gian

Da trn th cc mmen ng c, ngi k s d dng la chn cc ngcpht ng ph hp lp trn cc khu ca rbt.

V d 2.3. Xc nh phng trnh ng lc hc ca cc khu thuc rbtphng 6 khu ng nh hnh 2.16.

Hnh 2.16. Rbt phng 6 khu ng

O1 O6

O2

O3

O4

O5

O

x0

y0

x1

x2

x4

x5

x6

y6

q1

q2

q3q4

q5

q6

Phng trnh ta chuyn ng ca im thao tc c dng:xP = 0.8+0.2cos(2t) (m); yP = -0.8+0.2sin(2t) (m)


75/155

62

Bn kp to vi vi phng nm ngang 1 gc l 900.Ta k hiu O0O1 = a1, O1O2 = a2, O2O3 = a3, O3O4 = a4, O4O5 = a5, O5O6 = a6;

O0C1 = l1, O1C2 = l2, O2C3 = l3, O3C4 = l4, O4C5 = l5, O5C6 = l6.Ta c cc bng thng s DH v thng sng lc ca rbt 6 khu ng

nh di yBng 2.5 Cc thng s DH rbt 6 khu ng

Bng 2.6 Cc thng sng lc rbt 6 khu ng

V tr trng tm (so vigc to gn trnmi khp)

Mmen qun tnh khi tng khu(tnh i vi h tot ti trngtm tng khu v song song vi to khp)

Khu

xC yC zC

Khilng

Ixx Iyy Izz Ixy Ixz Iyz1 - (a

1- l

1) 0 0 m

1I

x1I

y1I

z10 0 0

2 - (a2 - l2) 0 0 m2 Ix2 Iy2 Iz2 0 0 03 - (a3 - l3) 0 0 m3 Ix3 Iy3 Iz3 0 0 04 - (a4 - l4) 0 0 m4 Ix4 Iy4 Iz4 0 0 05 - (a5 - l5) 0 0 m5 Ix5 Iy5 Iz5 0 0 06 - (a6 - l6) 0 0 m6 Ix6 Iy6 Iz6 0 0 0

Qua thc hin chng trnh tnh ton ta thu c cc kt qu chnh nh sau(do phng trnh vi phn chuyn ng qu di nn khng trnh by trong bn lunn, c th tham kho trong ph lc km theo):

Khu 1

To khi tm C1[ ]T11111C 0SlCl=r

Ma trn Jacobi tnh tin ca khu 1

Khu d a 1 q1 0 a1 02 q2 0 a2 0

3 q3 0 a3 04 q4 0 a4 0

5 q5 0 a5 06 q6 0 a6 0


76/155

63

=

=

000001

00000Cl

00000Sl

11

11

1C1T

q

rJ

Vn tc gc[ ]T11 q00 &=

Ma trn Jacobi quay

=

=

000001

000000

0000001

1Rq

J

&

Khu 2

To khi tm C2[ ]T12211122112C 0SlSaClCa ++=r

Ma trn Jacobi tnh tin

+

=

=

000001

0000ClClCa

0000SlSlSa

12212211

12212211

2C2T

q

rJ

Vn tc gc khu 2[ ]T212 qq00 && +=

Ma trn Jacobi quay

=

=

000011

000000

0000002

2Rq

J

&

Khu 3

To khi tm C3[ ]T1233122111233122113C 0SlSaSaClCaCa ++++=r


+++

=

=

000000

000ClClCaClCaCa

000SlSlSaSlSaSa

12331233122123312211

12331233122123312211

3C3T

q

rJ


77/155

64

Vn tc gc khu 3[ ]T3213 qqq00 &&& ++=

Ma trn Jacobi quay

=

=

000111

000000

0000003

3Rq

J

&

Khu 4

To khi tm C4[ ]T12344123312211123441233122114C 0SlSaSaSaClCaCaCa ++++++=r


=

=

000000

00JJJJ00JJJJr

24232221

14131211

4C4T

qJ

Vi:J11 = -a1S1-a2S12-a3S123-l4S1234J12 = -a2S12-a3S123-l4S1234J13 = -a3S123-l4S1234J14 = -l4S1234J21 = a1C1+a2C12+a3C123+l4C1234

J22 = a2C12+a3C123+l4C1234;J23 = a3C123+l4C1234J24 = l4C1234

Vn tc gc khu 4

[ ]T43214 qqqq00 &&&& +++=

Ma trn Jacobi quay

=

=

001111

000000

0000004

4R

q

J

&

Khu 5To khi tm C5


78/155

65

++++

++++

=

0

SlSaSaSaSa

ClCaCaCaCa

12345512344123312211

12345512344123312211

5Cr


=

=

000000

0JJJJJ

0JJJJJr

2524232221

1514131211

5C5T

qJ

Vi:J11 = -a1S1-a2S12-a3S123-a4S1234-l5S12345J12 = -a2S12-a3S123-a4S1234-l5S12345

J13 = -a3S123-a4S1234-l5S12345J14 = -a4S1234-l5S12345J15 = -l5S12345J21 = a1C1+a2C12+a3C123+a4C1234+l5C12345J22 = a2C12+a3C123+ a4C1234+l5C12345J23 = a3C123+ a4C1234+l5C12345J24 = a4C1234+l5C12345J25 = l5C12345

Vn tc gc khu 5

[ ]

T

543215 qqqqq00&&&&&

++++= Ma trn Jacobi quay

=

=

011111

000000

0000005

5Rq

J

&

Khu 6

To khi tm C6

+++++

+++++

=

0

SlSaSaSaSaSa

ClCaCaCaCaCa

123456612345512344123312211

123456612345512344123312211

6Cr



79/155

66

=

=

000000

JJJJJJ

JJJJJJr

262524232221

161514131211

6C6T

qJ

Vi:J11 = -a1S1-a2S12-a3S123-a4S1234-a5S12345-l6S123456J12 = -a2S12-a3S123-a4S1234-a5S12345-l6S123456J13 = -a3S123-a4S1234- a5S12345-l6S123456J14 = -a4S1234-a5S12345-l6S123456J15 = -a5S12345-l6S123456J16 = -l6S123456;J21 = a1C1+a2C12+a3C123+a4C1234+a5C12345+l6C123456

J22 = a2C12+a3C123+ a4C1234+ a5C12345+l6C123456J23 = a3C123+ a4C1234+ a5C12345+l6C123456J24 = a4C1234+ a5C12345+l6C123456J25 = a5C12345+l6C123456J26 = l6C123456;

Vn tc gc khu 6

[ ]T6543216 qqqqqq00 &&&&&& +++++=

Ma trn Jacobi quay

=

=

111111

0000000000006

6Rq

J

&

Ma trn tensor qun tnh

=

1z

1y

1x

1

I00

0I0

00I

I ,

=

2z

2y

2x

2

I00

0I0

00I

I ,

=

3z

3y

3x

3

I00

0I0

00I

I ,

=

4z

4y

4x

4

I00

0I000I

I ,

=

5z

5y

5x

5

I00

0I000I

I ,

=

6z

6y

6x

6

I00

0I000I

I


80/155

67

Bng 2.7 Cc gi tr thng sng lc hc ca rbt 6 khu ng

Khu 1 2 3 4 5 6

m (kg) 10 5 2 1 1 1Iz (kgm2) 0.11 0.10 0.03 0.02 0.01 0.01

a (m) 0.40 0.35 0.30 0.20 0.10 0.1l (m) 0.20 0.16 0.15 0.1 0.05 0.05

Khi rbt 6 khu ng c cc gi tr thng sng lc hc nh trong bng2.7 ta c c cc kt qu tnh ton mmen cc khp ng c trnh by trn cchnh t 2.17 n 2.22. Da trn cc th ny, ngi k s d dng la chn ccng cpht ng lp trn cc khu ca rbt.

0 2 4 6 8 1055

60

65

70

75

Thoi gian(s)

Momenkhop1(Nm)

Momen1

Hnh 2.17. Mmen trn khp ng th 1

0 2 4 6 8 1025

26

27

28

29

30

31

32

Thoi gian(s)

Momenkhop2(Nm)

Momen2



81/155

68

0 2 4 6 8 103

4

5

6

7

8

9

10

Thoi gian(s)

Momenkhop3(Nm)

Momen3


0 2 4 6 8 10-2.5

-2

-1.5

-1

-0.5

0

0.5

1

Thoi gian(s)

Momenkhop4(Nm)

Momen4


0 2 4 6 8 10-0.55

-0.5

-0.45

-0.4

-0.35

-0.3

-0.25

-0.2

Thoi gian(s)

M

omenkhop5(Nm)

Momen5



82/155

69

0 2 4 6 8 100.25

0.3

0.35

0.4

0.45

0.5

0.55

0.6

Thoi gian(s)

Momenkhop6(Nm)

Momen6


2.4. Kt lun chng 2

Da trn thut ton hiu chnh gia lng vc t ta suy rng trongchng 1, y trnh by mt thut ton tnh ton lc/mmen ca cc khu dnng trong khng gian thao tc. T gip cho ngi k s thit k chn cc ngclp vo rbt. S dng thut ton gii bi ton ng lc hc ngc rbt trnh

by trong lun n ny cho php chng ta rt ngn thi gian tnh ton xung rt

nhiu m vn m bo c chnh xc cao (do sai s trong bi ton ng hcngc l rt nh). Cc v d trnh by trong chng l cc v d mi, cha c trongcc sch chuyn kho.

Vic gii bi ton ng lc hc ngc l mt khu quan trng trong viciu khin chuyn ng ca rbt. Vn ny sc trnh by trong chng 3.


83/155

Chng 3

IU KHIN TRT RBT DDN NGDA TRN THUT TON HIU CHNH GIA LNG

VC T TA SUY RNG

iu khin chuyn ng rbt l bi ton quan trng trong k thut rbt.C nhiu phng php iu khin chuyn ng rbt nhiu khin PD, iukhin PID, iu khin trt, iu khin m [59, 63, 66, 69, 70, 72]. Trongchng ny gii thiu slc v khi nim n nh chuyn ng v iu khinchuyn ng rbt. Trng tm ca chng l trnh by iu khin trt rbt ddn ng da trn thut ton hiu chnh gia lng vc t ta suy rng [19,50, 51].

3.1 Csl thuyt n nh Lyapunovn nh l mt khi nim quan trng trong iu khin [4, 64, 67]. Mt h

khng n nh l khng th chp nhn c v mt ng dng. V mt trcquan, mt h gi l n nh nu n xut pht t mt im no ln cn imhot ng th n s lun xung quanh im hot ng . L thuyt n nhLyapunov c nu hai phng php phn tch n nh l phng php tuyn tnhho v phng php trc tip. Trong phm vi ca lun n ny, ta ch s dngphng php trc tip (hay cn gi l phng php hm Lyapunov).

3.1.1H phi tuyn v cc im cn bnga. H phi tuyn

Mt hng lc phi tuyn l h c phng trnh trng thi c miu tbng h cc phng trnh vi phn phi tuyn sau

)t,(yfy =& (3.1)Trong :

nRf l hm vctphi tuyn, nRy l vcttrng thi. S trng thi n c gi l cp (order)

ca h.

V mt trc quan phng trnh ny ging nh phng trnh trng thi camt h khng iu khin. Tuy nhin, n chnh l phng trnh trng thi tngqut dng cho bi ton phn tch ca tt c cc h. Xt i tng iu khin cphng trnh trng thi

)t,,( uyfy =& Nu chn lut iu khin l )t,(ygu = , ta thu c h [ ]t),t,(, ygyfy =&

do ta c th vit li di dng (3.1).


84/155

71

H tuyn tnh l trng hp ring ca h phi tuyn. Lc (3.1) cvit di dng:

yAy )t(=& , vi A(t) l ma trn nxn.

Ty thuc vo vic h c ph thuc hay khng ph thuc vo bin thigian m h phi tuyn c chia thnh h tnm v h phi tnm theo nhngha di y.

nh ngha 3.1. H phi tuyn (3.1) c gi l tnm nu fkhng phthuc tng minh vo thi gian, ngha l phng trnh trng thi c dng.

)(yfy =& (3.1b)

Ngc li, ta gi l hphi tnm.

Ch rng, nh ngha trn cng nh phng trnh trng thi (3.1) pdng cho mt h c phn hi. Do , tnh tnm hay phi tnm l ph thuc

vo ci tng iu khin ln biu khin. V d, nu i tng iu khinc th coi gn ng l tnm song biu khin li phi tnm th c h kns l phi tnm.

S khc nhau cbn gia h tnm v h phi tnm l quo trngthi ca h tnm khng ph thuc vo gi tr thi im u. Do , qu trnhphn tch tnh n nh ca h s d dng hn. Trong ti liu ny, n gin chtrnh by vic nghin cu n nh ca cc h tnm.

b.im cn bngnh ngha 3.2. Mt trng thi y*c gi l trng thi cn bngca

h (hay im cn bng) nu mt khi y(t) = y* n s nm nguyn ti trng thi y*.

Nh vy im cn bng y* s tho mn phng trnh f(y*) = 0.Khi y*0, ta c thi bin a im cn bng ca h v trng vi

gc trng thi nh sau:t x = yy* ri thay vo biu thc (3.1b), ta c:

)(*)(* xgyxfyyx =+== &&& K hiu li f(x) = g(x), t phng trnh trn ta suy ra :

)(xfx =& (3.2)

Do , ta s nghin cu tnh n nh ca h quanh im gc x* = 0.Trong nhiu bi ton thc t, chng hn nh bi ton iu khin rbt

bm theo quo, ta quan tm n tnh n nh ca h quanh mt quo phthuc vo thi gian. Bng phng php i bin, ta c th bin i bi ton c t ra (l tnm hoc phi tnm) thnh bi ton nghin cu sn nhca mt h phi tnm quanh im cn bng.


85/155

72

3.1.2Khi nim n nhTrong phn ny ta phn tch cc khi nim n nh, khng n nh, n

nh tim cn v n nh s m ca nghim cn bng x* = 0 cah tnm

(3.2).Ta k hiu Br l min cu cn Sr l mt cu tm O bn knh r, BR l min

cu cn SR l mt cu tm O bn knh R (R > r).a. n nh v khngn nhnh ngha 3.3. Trng thi cn bng x* = 0c gi l n nh nu0R> , 0r> sao cho, nu ||x(0)|| < r th ||x(t)|| < R vi 0t . Ngc li,

im x* = 0 l im khng n nh (hnh 3.1).

O

Br BR

1

2

x1

x2

Sr

SR

Hnh 3.1 im gc O l: im n nh (1) im khng n nh (2)

Bn cht ca khi nim n nh Lyapunov trn l: quo ca h cth tin n gn im gc mt cch ty , min l v tr ban u gn gc mtkhong nh.

Ch rng, trong h phi tuyn, h khng n nh khng c ngha l hlun lun di chuyn ra xa gc n v cng. Trn hnh 3.3, quo h di chuyntim gn gc ty cui cng tim cn theo mt chu trnh. (Nu xt mt hnhtrn nm bn trong chu trnh th cui cng h si ra khi hnh trn ).

Hnh v cng ni ln s khc nhau cbn gia n nh v khng n nh. Cth quo ca h vn nm quanh im cn bng mt khong cch hu hnno , nhng n khng thgn im cn bng mt cch ty .

b. n nh tim cn v n nh sm

Trong nhiu trng hp, ch dng khi nim n nh Lyapunov l cha . lm v d, ta xt mt v tinh b nhiu nh bt ra khi v tr cn bng ca n.


86/155

73

Luan an tien si_Nam

Documents

Transcript of Luan an tien si_Nam