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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
22/155
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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:
7/29/2019 Luan an tien si_Nam
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 .
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
28/155
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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)
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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)
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
+=
= +
7/29/2019 Luan an tien si_Nam
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)
7/29/2019 Luan an tien si_Nam
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]
7/29/2019 Luan an tien si_Nam
41/155
28
Hnh 1.8. Cc c tnh chuyn ng ca khu 2
Hnh 1.9. Cc c tnh chuyn ng ca khu 3
Hnh 1.10. Cc c tnh chuyn ng ca khu 4
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]
7/29/2019 Luan an tien si_Nam
42/155
29
Hnh 1.11. Cc c tnh chuyn ng ca khu 5
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]
7/29/2019 Luan an tien si_Nam
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 :
Hnh 1.16. 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]
7/29/2019 Luan an tien si_Nam
44/155
31
Hnh 1.17. Cc c tnh chuyn ng ca khu 2
Hnh 1.18. Cc c tnh chuyn ng ca khu 3
Hnh 1.19. Cc c tnh chuyn ng ca khu 4
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]
7/29/2019 Luan an tien si_Nam
45/155
32
Hnh 1.20. Cc c tnh chuyn ng ca khu 5
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]
7/29/2019 Luan an tien si_Nam
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]
Hnh 1.23. 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.24. Dng chuyn ng ca rbt theo kt qu tnh ton
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).
7/29/2019 Luan an tien si_Nam
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]
Hnh 1.25. Sai s theo trc x ca im thao tc
0 2 4 6 8 10 12 14 16 18 20-20
-15
-10
-5
0
5
x 10-3
time [s]
ey
[m]
Hnh 1.26. Sai s theo trc y ca im thao tc
0 2 4 6 8 10 12 14 16 18 20-4
-2
0
2
4x 10
-15
time [s]
e
[rad]
Hnh 1.27. Sai s gc nh hng ca bn kp
7/29/2019 Luan an tien si_Nam
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]
Hnh 1.28. Dng chuyn ng ca rbt theo kt qu tnh ton
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]
Hnh 1.29. Sai s theo trc x ca im thao tc
0 2 4 6 8 10 12 14 16 18 20-6
-4
-2
0
2
4
6x 10
-16
time [s]
ey
[m]
Hnh 1.30. Sai s theo trc y ca im thao tc
7/29/2019 Luan an tien si_Nam
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]
Hnh 1.31. 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.32. Dng chuyn ng ca rbt theo kt qu tnh ton
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
7/29/2019 Luan an tien si_Nam
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]
Hnh 1.33. Sai s theo trc x ca im thao tc
0 2 4 6 8 10 12 14 16 18 20-0.15
-0.1
-0.05
0
0.05
time [s]
ey
[m]
Hnh 1.34. Sai s theo trc y ca im thao tc
0 2 4 6 8 10 12 14 16 18 20-2
-1
0
1
2x 10
-15
time [s]
e
[ra
d]
Hnh 1.35. Sai s gc nh hng ca bn kp
7/29/2019 Luan an tien si_Nam
51/155
38
-0.5 0 0.5 1
0
0.2
0.4
0.6
0.8
1
x [m]
y[m]
Hnh 1.36. Dng chuyn ng ca rbt theo kt qu tnh ton
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]
Hnh 1.37. Sai s theo trc x ca im thao tc
0 1 2 3 4 5 6 7 8 9 10-6
-4
-2
0
2
4
6x 10
-16
time [s]
ey
[m]
Hnh 1.38. Sai s theo trc y ca im thao tc
7/29/2019 Luan an tien si_Nam
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
]
Hnh 1.39. 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.40. Dng chuyn ng ca rbt theo kt qu tnh ton
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).
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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)
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
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)
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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)
7/29/2019 Luan an tien si_Nam
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 & .
7/29/2019 Luan an tien si_Nam
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).
7/29/2019 Luan an tien si_Nam
64/155
51
KT THC
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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:
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
71/155
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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:
7/29/2019 Luan an tien si_Nam
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)
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
Ma trn Jacobi tnh tin
+++
=
=
000000
000ClClCaClCaCa
000SlSlSaSlSaSa
12331233122123312211
12331233122123312211
3C3T
q
rJ
7/29/2019 Luan an tien si_Nam
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
Ma trn Jacobi tnh tin
=
=
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
7/29/2019 Luan an tien si_Nam
78/155
65
++++
++++
=
0
SlSaSaSaSa
ClCaCaCaCa
12345512344123312211
12345512344123312211
5Cr
Ma trn Jacobi tnh tin
=
=
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
Ma trn Jacobi tnh tin
7/29/2019 Luan an tien si_Nam
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
7/29/2019 Luan an tien si_Nam
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
Hnh 2.18. Mmen trn khp ng th 2
7/29/2019 Luan an tien si_Nam
81/155
68
0 2 4 6 8 103
4
5
6
7
8
9
10
Thoi gian(s)
Momenkhop3(Nm)
Momen3
Hnh 2.19. Mmen trn khp ng th 3
0 2 4 6 8 10-2.5
-2
-1.5
-1
-0.5
0
0.5
1
Thoi gian(s)
Momenkhop4(Nm)
Momen4
Hnh 2.20. Mmen trn khp ng th 4
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
Hnh 2.21. Mmen trn khp ng th 5
7/29/2019 Luan an tien si_Nam
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
Hnh 2.22. Mmen trn khp ng th 6
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.
7/29/2019 Luan an tien si_Nam
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).
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
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.
7/29/2019 Luan an tien si_Nam
86/155
73
Top Related