Luan An Tien Si - Nam

B GIO DC V O TO
VIN KHOA HC
V CNG NGH VIT NAM
VIN C HC
TRN HONG NAM
GII BI TON NGC NG HC, NG LC

HC V IU KHIN TRT RBT D
DN NG DA TRN THUT TON HIU
CHNH GIA LNG VC T TA SUY RNG
LUN N TIN S K THUT
H Ni - 2010
B GIO DC V O TO
VIN KHOA HC
V CNG NGH VIT NAM
VIN C HC
TRN HONG NAM
GII BI TON NGC NG HC,

NG LC HC V IU KHIN TRT RBT
D DN NG DA TRN THUT TON
HIU CHNH GIA LNG VC T
TA SUY RNG
Chuyn ngnh: C hc k thut
M s ngnh: 62 52 02 01
LUN N TIN S K THUT
Ngi hng dn khoa hc :

1. GS.TSKH Nguyn Vn Khang
2. PGS.TS Nguyn Phong in
H Ni 2010
LI CAM OAN
Ti xin cam oan y l cng trnh nghin cu ca ring ti v cha c
cng b trong bt c cng trnh no khc. Cc s liu, kt qu nu trong lun n
l trung thc.
Tc gi lun n
Trn Hong Nam
MC LC
Trang
Li cam oan
Mc lc
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
M u
Chng 1. Tnh ton ng hc ngc rbt d dn ng bng thut

ton hiu chnh gia lng vc t ta suy rng
1.1
1.1.1
1.1.2
1.1.3
1.2
1.2.1
1.2.2
1.2.3
1.2.4
1.3
1.3.1
1.3.2
1.3.3
1.3.4
1.4
1.5
Gii bi ton ng hc thun rbt d dn ng bng phng php

ma trn Denavit-Hartenberg
Cc tham s ng hc v ma trn Denavit-Hartenberg
Phng trnh xc nh v tr v hng ca khu thao tc
Bi ton p dng
Ma trn ta nghch o
nh ngha
Ma trn ta nghch o Moore-Penrose
Phng php nhn t Lagrange
Nghim tng qut ca phng trnh i s tuyn tnh
Gii bi ton ng hc ngc rbt d dn ng bng phng php
hiu chnh gia lng vc t ta suy rng
Phng php khai trin Taylor
Cc cng thc xc nh vc t vn tc v vc t gia tc suy rng
Cc cng thc xc nh vc t ta suy rng
nh gi sai s
Cc bi ton p dng
Kt lun chng 1
Chng 2
Tnh ton ng lc hc ngc rbt d dn ng

trong khng gian thao tc da trn thut ton
2.1 Dng thc Lagrange loi 2 ca h nhiu vt

2.2 Gii bi ton ngc ng lc hc rbt d dn ng trong khng
gian thao tc
8
10
12
14
16
16
16
18
19
20
20
21
22
26
26
43
44
44
47
II
2.3
2.4
Cc bi ton p dng
Kt lun chng 2
Chng 3
3.1
3.1.1
3.1.2
3.1.3
3.2
3.3
3.4
3.5
iu khin trt rbt d dn ng da trn thut ton s

52
69
70
C s l thuyt n nh Lyapunov
H phi tuyn v cc im cn bng
Khi nim n nh
Phng php trc tip Lyapunov
Bi ton iu khin chuyn ng ca rbt
iu khin trt rbt d dn ng
Cc bi ton p dng
Kt lun chng 3
70
70
72
74
78
80
85
102
Chng 4. Bi ton ngc ng hc, ng lc hc

v iu khin trt rbt BKHN-MCX-04
103
4.1
4.2
4.3
4.4
4.5
4.6
4.6.1
4.6.2
4.6.3
4.6.4
4.7
103
104
106
111
122
126
126
127
127
131
133
Tng quan v phng php o chnh xc b mt cc chi tit my

Kt cu ca r bt o BKHN-MCX-04
Tnh ton ng hc ngc
Tnh ton ng lc hc ngc
iu khin trt rbt BKHN-MCX-04
Th nghim
Cu to ca h thng th nghim
Nguyn l hot ng ca h thng th nghim
H thng iu khin
Kt qu th nghim
Kt lun chng 4
Kt lun chung
134
Danh mc cng trnh ca tc gi
136
Ti liu tham kho
137
III
DANH MC K HIU,
CH VIT TT THNG S DNG
ai
i
A(z,i)
A(z,di)
A(x,ai)
A(x,i)
Ai
A+
Ci
C12n
C(q, q& i ) q& i
(q, q& ) q&
C
i
~
C(q, q& )
di
d
Di
ek
Khong dch chuyn dc trc xi (chiu di ca khu th i)

Gc quay quanh trc xi.
Ma trn bin i ta bi php quay c bn xung quanh trc z.
Ma trn bin i ta bi php tnh tin dc trc z.
Ma trn bin i ta bi php tnh tin dc trc x.
Ma trn bin i ta bi php quay xung quanh trc x.
Ma trn cosin ch hng ca khu th i.
Ta nghch o ca ma trn A.
Cosin (qi).
Cosin(q1+q2++qn).
Vc t cc lc Coriolis v lc hng tm ca m hnh thc.
e& k
Sai s vn tc ca bn kp.
&e&k
Sai s gia tc ca bn kp.

Vc t sai s bm theo gc quay.
Sai s bm theo vn tc quay.
Sai s ca php tnh.
Gia tc gc ca khu th i.
S bc t do ca c h.
Vc t lc trng trng suy rng.
Vc t cc lc trng trng ca b iu khin.
Sai s gia g(q) v g (q) .
Ma trn bin i ta t h trc Oi-1xi-1yi-1zi-1 v Oixiyizi.
Khong thi gian ca bc tnh ton.
V tr trng tm khu th i i vi h trc ta khu i.
Nhn t Lagrange.
e(t)
e& ( t )
epxi
f
g(q)
g (q)
~
g (q)
i-1
Hi
h=t
li
Vc t cc lc Coriolis v lc hng tm ca b iu khin.

(q, q& ) .
Sai s gia C(q, q& ) v C
i
Khong dch chuyn gc ta Oi-1 v Oi.

Vc t nhiu.
Ma trn chuyn i ta t O0x0y0z0 v h ta Oixiyizi.
Sai s dch chuyn ca bn kp (khng gian thao tc).
IV
i
mi
M(q)
(q)
M
~
M (q)
Ii
Izi
JTi
JRi
J(q)
J+(q)
J& + (q)
k
k
Ks
Kpd
omi
Px, Py, Pz
q
q
q&
&&
q
q0
q& 0
&& 0
q
~
q
0
q0
qk+1
q& k +1
&& k +1
q
~
q
k +1
qi_dot
qi_2dot
qd
q& d
Hng s dng th i.
Khi lng ca cht im th i.
Ma trn khi lng suy rng.
Momen khi lng ca b iu khin.
(q) .
Sai s gia M(q) v M
Tenx qun tnh khi ca vt rn th i.

Momen qun tnh i vi trc z ca khu th i.
Ma trn Jacobi tnh tin ca vt rn th i.
Ma trn Jacobi quay ca vt rn th i.
Ma trn Jacobi.
Ta nghch o ca ma trn Jacobi.
o hm bc nht ca J+(q).
H s khuych i .
H s khuych i ca hm sat.
H s khuych i dng trt.
H s khuych i t l - o hm.
Vn tc gc ca khu th i.
Cc ta x, y, z ca im thao tc P.
Th nng ca h.
Thnh phn lc suy rng ca cc lc c th.
Vc t ta suy rng ca robot.
o hm bc nht ca q theo thi gian.
o hm bc 2 ca q theo thi gian.
Vc t ta suy rng ti thi im ban u.
o hm bc nht theo thi gian ca q0.
o hm bc 2 theo thi gian ca q0.
Gi tr gn ng ca q0.
S gia ca q0.
Gi tr ca q ti thi im t = tk+1.
o hm bc 1 theo thi gian ca qk+1.
o hm bc 2 ca qk+1.
Gi tr gn ng ca qk+1.
o hm bc nht ca qi theo thi gian.
o hm bc 2 ca qi theo thi gian .
Vc t ta suy rng mong mun.
o hm ca vc t suy rng mong mun theo thi gian.
V
rCi
sgn
sat
Si
S12..n
i
x
x=f(q)
xd
x& d
&x& d
x*
~
xd
~
xd
r
v
vi
V(x)
i
~
T
u
DH
MP
PD
PID
Vc t nh v trng tm ca vt rn th i.
Hm du.
Hm bo ha.
Sin(qi).
Sin(q1+q2++qn).
Gc quay ca trc xi-1 xung quanh trc zi-1.
Vc t nh v ca khu thao tc.
Phng trnh xc nh ta x theo ta suy rng q.
V tr mong mun ca bn kp.
Vn tc chyn ng mong mun ca bn kp.
Gia tc chuyn ng mong mun ca bn kp.
Trng thi cn bng.
Sai s bm ca bin x.
Vc t sai s bm.
Vc t hnh hc ca vn tc.
Vc t i s ca vn tc khi tm ca vt rn th i.
Hm Lyapunov.
Vn tc gc ca vt rn th i.
Ton t sng ca vc t vn tc gc th i.
Lc/momen trn khp ng .
ng nng ca ton h.
Quy lut iu khin.
Denavit-Hartenberg.
Moore-Penrose.
T l - vi phn.
T l - vi phn tch phn.
VI
DANH MC CC BNG
Bng 1.1
Bng 2.1
Bng 2.2
Bng 2.3
Bng 2.4
Bng 2.5
Bng 2.6
Bng 2.7
Bng 3.1
Bng 3.2
Bng 4.1
Bng 4.2
Bng 4.3
Bng 4.4
Bng 4.5
Trang
Thng s DH rbt 5 khu ng
14
Cc tham s ng hc ca rbt Scara
52
Bng thng s ng lc rbt SCARA 4 bc t do
56
Cc thng s ng lc hc ca rbt 5 khu ng
60
Cc thng s ng hc ca rbt 5 khu ng
60
Cc thng s DH rbt 6 khu ng
62
62
Cc thng s ng lc rbt 6 khu ng
Cc gi tr thng s ng lc hc ca rbt 6 khu ng 67
85
Cc thng s ng lc rbt SCARA
93
Thng s hnh hc ca rbt
105
Bng tham s ng hc D-H
106
V tr khi tm khu i ca rbt trn h ng
114
Cc thng s ng lc ca rbt o
119
131
Cc kt qu o thc nghim
VII
DANH MC CC HNH V V TH
Hnh 0.1
Hnh 0.2
Hnh 0.3
Hnh 0.4
Rbt trn dy chuyn ca trung tm sn xut linh hot

Rbt phc v my phay CNC
Rbt Mitsubishi RV-2AJ
Hnh nh mt s loi rbt
Hnh 1.1
Hnh 1.2
Hnh 1.3
Hnh 1.4
Hnh 1.5
Hnh 1.6
Hnh 1.7
Hnh 1.8
Hnh 1.9
Hnh 1.10
Hnh 1.11
Hnh 1.12
Hnh 1.13
Hnh 1.14
Hnh 1.15
Hnh 1.16
Hnh 1.17
Hnh 1.18
Hnh 1.19
Hnh 1.20
Hnh 1.21
Hnh 1.22
Hnh 1.23
Hnh 1.24
Hnh 1.25
Hnh 1.26
Hnh 1.27
Hnh 1.28
Hnh 1.29
Hnh 1.30
Hnh 1.31
Hnh 1.32
S thit lp h ta cc khu
S rbt dng chui
Rbt 5 khu ng
S khi tnh ton A+ theo phng php Moore-Penrose
S khi gii bi ton ng hc ngc
Rbt phng 5 khu ng
Cc c tnh chuyn ng ca khu 1
Sai s v tr ca im thao tc theo trc x
Sai s v tr ca im thao tc theo trc y
Sai s gc nh hng ca bn kp
Dng chuyn ng ca rbt theo kt qu tnh ton
Trang
1
2
2
3
9
13
14
17
25
26
27
28
28
28
29
29
29
30
30
30
31
31
31
32
32
32
33
33
34
34
34
35
35
35
36
36
VIII
Hnh 1.33
Hnh 1.34
Hnh 1.35
Hnh 1.36
Hnh 1.37
Hnh 1.38
Hnh 1.39
Hnh 1.40
Hnh 1.41
Hnh 1.42
Hnh 1.43
Hnh 1.44
Hnh 1.45
Hnh 1.46
Hnh 1.47
Hnh 2.1
Hnh 2.2
Hnh 2.3
Hnh 2.4
Hnh 2.5
Hnh 2.6
Hnh 2.7
Hnh 2.8
Hnh 2.9
Hnh 2.10
Hnh 2.11
Hnh 2.12
Hnh 2.13
Hnh 2.14
Hnh 2.15
Hnh 2.16
Hnh 2.17
Hnh 2.18
Hnh 2.19
Hnh 2.20
Hnh 2.21

Rbt 6 khu ng
th gc quay ca cc khp
th vn tc gc cc khp
th gia tc gc cc khp ng
th sai s v tr trong khng gian thao tc
th sai s vn tc trong khng gian thao tc
th sai s gia tc trong khng gian thao tc
37
37
37
38
38
38
39
39
40
41
41
41
42
42
42
44
Thut ton gii bi ton ng lc hc ngc

trong khng gian thao tc
S kt cu rbt scara
th chuyn ng ca cc khp ng rbt scara
th vn tc chuyn ng ca cc khp ng rbt scara
th gia tc chuyn ng ca cc khp ng rbt scara
th sai s v tr bn kp rbt scara
th sai s vn tc bn kp rbt scara
th sai s gia tc bn kp rbt scara
Mmen trn khp ng th 1
Lc trn khp ng th 3
Rbt phng 5 khu ng
th cc mmen ng c theo thi gian
Rbt phng 6 khu ng
51
52
53
54
54
55
55
56
57
58
58
59
59
61
61
67
67
68
68
68
IX
Hnh 2.22
69
Hnh 3.1
Hnh 3.2
Hnh 3.3
Hnh 3.4
Cc khi nim v n nh
Gc khng n nh
Trng thi hi t khng n nh
S phn k trng thi khi chuyn ng
dc theo cc ng nng lng thp
S hi t n tp bt bin ln nht
S hot ng ca rbt
S tng qut ca h thng iu khin khng gian khp
S tng qut ca h thng iu khin khng gian thao tc
S iu khin rbt
Hm sat
Hm arctan
S tnh ton v m phng iu khin rbt
Rbt 4 khu ng
th nhiu tc ng trn khp ng 1
Momen iu khin ti khp 1
th sai s suy rng s1
th sai s suy rng s2
th sai s suy rng s3
thi sai s suy rng s4
th ta x(t) trong khng gian thao tc theo thi gian
th ta y(t) trong khng gian thao tc theo thi gian
Rbt Scara
Lc iu khin ti khp 3
th sai s suy rng s1
72
72
73
Hnh 3.5
Hnh 3.6
Hnh 3.7
Hnh 3.8
Hnh 3.9
Hnh 3.10
Hnh 3.11
Hnh 3.12
Hnh 3.13
Hnh 3.14
Hnh 3.15
Hnh 3.16
Hnh 3.17
Hnh 3.18
Hnh 3.19
Hnh 3.20
Hnh 3.21
Hnh 3.22
Hnh 3.23
Hnh 3.24
Hnh 3.25
Hnh 3.26
Hnh 3.27
Hnh 3.28
Hnh 3.29
Hnh 3.30
Hnh 3.31
Hnh 3.32
Hnh 3.33
Hnh 3.34
Hnh 3.35
Hnh 3.36
Hnh 3.37
76
77
78
79
79
80
83
83
84
85
86
86
87
87
88
88
89
89
90
90
91
91
92
92
93
94
94
95
95
96
96
97
97
98
Hnh 3.38
Hnh 3.39
Hnh 3.40
Hnh 3.41
Hnh 3.42
Hnh 3.43
Hnh 3.44
Hnh 4.1
Hnh 4.2
Hnh 4.3
Hnh 4.4
Hnh 4.5
Hnh 4.6
Hnh 4.7
Hnh 4.8
Hnh 4.9
Hnh 4.10
Hnh 4.11
Hnh 4.12
Hnh 4.13
Hnh 4.14
Hnh 4.15
Hnh 4.16
Hnh 4.17
Hnh 4.18
Hnh 4.19
Hnh 4.20
Hnh 4.21
Hnh 4.22
th sai s suy rng s2

th sai s suy rng s3
thi sai s suy rng s4
th ta x(t) trong khng gian thao tc theo thi gian
th ta y(t) trong khng gian thao tc theo thi gian
th ta z(t) trong khng gian thao tc theo thi gian
th gc quay
trong khng gian thao tc theo thi gian
98
99
99
100
100
101
M hnh rbt o BKHN-MCX-04

S ng hc rbt o BKHN-MCX-04
Qu o nh trc ca im E (ng xon c)
th bin khp q
th vn tc gc cc khp q&
&&
th gia tc gc cc khp q
V tr trng tm cc khu
Qu o im E
V tr cc khu trong qu trnh chuyn ng
Tr s ca bin khp 2, 3 v 4
Vn tc gc ca ng c dn ng khu 2, 3 v 4
Gia tc gc ca ng c dn ng khu 2, 3 v 4
Mmen dn ng cn thit cho cc khu ca rbt
th ta xE(t) trong khng gian thao tc theo thi gian
th ta yE(t) trong khng gian thao tc theo thi gian
th ta zE(t) trong khng gian thao tc theo thi gian
M hnh th nghim rbt o BKHN-MCX-04
S iu khin ton b rbt
S iu khin tng khp ca BKHN-MCX-04
S nguyn l ca mch iu khin 1 trc
Qu o thc nghim ca im cui E
104
105
109
109
110
110
114
119
120
120
121
121
122
123
124
125
125
126
128
129
130
132
101
M U
1. L do la chn ti
Cng 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 th y mnh
s nghip cng nghip ha v hin i ha cn u tin p dng cc tin b ca
khoa hc k thut vo i sng sn xut. Trong vn quan trng nht l
phi tng nhanh lng t ng ha vo cc qu trnh sn xut cng nghip. y
cng l mt i hi cp bch lin quan n vic gii phng con ngi khi s
nng nhc, s nhm chn ca cng vic (do s lp i lp li cc thao tc ca
mt cng vic gin n no ), s nguy him ca mi trng lao ng nh s
nng bc ti cc l hi, s ly lan ca cc bnh him ngho ti cc c s y 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, cc
cng ty cc nc c nn sn xut pht trin a cc rbt vo cc dy
chuyn sn xut ca mnh. Di y l mt s hnh nh v cc rbt v ni ng
dng ca chng m chng ta thng gp
Hnh 0.1. Rbt trn dy chuyn ca mt trung tm sn xut linh hot.
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
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 khi

nim v rbt cng nghip. Di y l mt s khi nim (nh ngha) in
hnh:
Rbt cng nghip l mt my t ng linh hot c s dng thay
th tng phn hoc ton b cc hot ng c bp v hot ng tr tu ca
con ngi vi nhiu kh nng thch nghi khc nhau.
Rbt cng nghip l mt c cu chuyn i t ng c th chng trnh
ha, lp li cc chng trnh, tng hp cc chng trnh t ra trn cc
trc ta c kh nng nh v, nh hng, di chuyn cc i tng vt
cht, chi tit, dng c g lp, dao ct theo nhng chng trnh thay
i, chng trnh ha nhm thc hin cc chng trnh cng ngh
khc nhau.
Rbt cng nghip l mt tay my vn nng hot ng theo chng trnh
v c th lp trnh li hon thnh v nng cao hiu qu hon thnh cc
nhim 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. Engelberger M. Cc rbt
u tin ny ch yu c dng vn chuyn cc vt th trong mt phm vi
nh. 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 c
trang b thm cc loi cm bin khc nhau nhn bit mi trng xung quanh
v nh cc thnh tu to ln trong cc lnh vc nh iu khin hc, tin hc v
in t hc m cc rbt c thm nhiu tnh nng c bit. Nh vy m cc

rbt cng nghip c mt v tr quan trng trong cc dy chuyn sn xut
hin i, nht l trong cc dy chuyn sn xut t ng linh hot (FMS). Do
cc nc ang y mnh vic sn xut cc rbt cng nghip ng dng vo
cc ngnh cng nghip, i u trong lnh vc ch to rbt cng nghip hin
nay l Nht Bn, k n l M, c, , Php, Anh v Hn Quc.
Theo [9, 22, 25, 28, 32, 44, 56, 58], cu to ca rbt thng c 3 b
phn ch yu, l:
Tay my (manipulator)
B phn dn ng
B phn iu khin
Trong tay my l b phn c kh quan trng, ng vai tr l mt b
phn 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 c in, 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.
B iu khin c dng iu khin cc hot ng ca rbt. B phn iu
khin 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 trang
b, cng nh c th tin hnh nghin cu, th nghim, ch to cc rbt mi
p 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, cc
trng hc i hc, cao ng quan tm.
Khi nghin cu v rbt chng ta thng phi gii quyt cc bi ton v
ng hc, ng lc hc v iu khin. Trong cc bi ton ny th bi ton ngc
m c bit l cc bi ton ngc ca rbt d dn ng l bi ton kh v hin
nay ang cn t c nghin cu nc ta. V vy tc gi chn cho mnh ti
Gii bi ton ngc ng hc, ng lc hc v iu khin trt rbt d dn
ng da trn thut ton hiu chnh gia lng vc t ta suy rng nhm ci
tin phng php s gii bi ton ngc c hin nay.
2. i tng v ni dung nghin cu
i 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 dung
nghin cu l kho st bi ton ng hc ngc, bi ton ng lc hc ngc
v bi ton iu khin trt rbt d dn ng.
3. Tng quan v vn nghin cu

Vic 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 d
dn ng khi s ta suy rng nhiu hn s ta ti thiu xc lp nn v tr
v hng ca khu thao tc theo ng yu cu ca bi ton cng ngh. Nh tnh
d dn ng m rbt loi ny c kh nng trnh c cc vt cn, cc im k
d, cc gii hn ca bin khp, , ngha l cc rbt d dn ng c tnh linh
hot 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 c s cho vic nghin cu ch to ra cc loi rbt
mi c bit l cc rbt d dn ng, mt lnh vc m hin cn c t nghin
cu nc ta.
Trong cc ti liu [9, 22, 25, 28, 32, 44, 55] ni v rbt hin nay, nhn
chung cc tc gi trnh by kh y v cc bi ton ng hc, ng lc
hc v iu khin rbt chun.
Bi ton ng hc c chia lm 2 nhm l nhm bi ton ng hc
thun v nhm bi ton ng hc ngc. Nhm bi ton ng hc thun c
nhim v xc nh v tr v hng ca khu thao tc khi bit c s cu trc
ca rbt v cc quy lut chuyn ng ca cc khp ng. Bi ton ny c
gii da vo php bin i ta theo phng php Denavit-Hartenberg. Kt qu
thu nhn c l mt ma trn m t hng v phng trnh xc nh ta ca
bn 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 ng
mong mun nhm thc hin mt nhim v c th no . Vic gii cc bi ton
ng hc ngc thng l phc tp v kh khn hn so vi bi ton ng hc
thun. gii quyt bi ton ny thng thng ta phi thit lp phng trnh
m t quan h gia ta ca khu thao tc (bn kp) vi cc gc khp (y l
kt 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 khp
dn ng ng bng vi s ta xc nh nn cu hnh ca rbt th v
nguyn tc ta u gii c v cng c trnh by kh y trong cc ti
liu 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 ca
rbt d dn ng) th bi ton s c nhiu nghim v dng bi ton ny cn t
c 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 hc
thun v nhm bi ton ng lc hc ngc. Nhim v ca bi ton ng lc
hc ngc l thit lp phng trnh vi phn chuyn ng cho cc khu ng ca
rbt t xc nh lc/mmen tc ng trn cc khp. Vic thit lp phng
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 c s cc phng trnh Newton-Euler hoc cc
phng trnh Lagrange loi 2 cho h nhiu vt.
Vi bi ton ng lc hc ngc ta bit hoc la chn c s
cu trc ca rbt, theo yu cu ca bi ton cng ngh m ta c phng
trnh chuyn ng ca khu thao tc t ta phi xc nh phng trnh ng
lc hc ca rbt. xy dng phng trnh ng lc hc ta phi bit c cc
c tnh chuyn ng ca cc khu, v vy ta li phi gii bi ton ng hc
ngc. Kt qu gii bi ton ng hc ngc s c s dng gii bi ton
ng lc hc ngc.
Nhim v ch yu ca bi ton iu khin l duy tr chuyn ng ca
khu thao tc theo 1 qu o mong mun no c xc nh trc theo yu
cu 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 khng
gian khp) v iu khin chuyn ng ca bn kp (iu khin khng gian thao
tc). Vi phng php iu khin khng gian khp ta tin hnh iu khin
chuyn 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 tc
ng v tr mong mun do phi tnh ton lng chuyn ng cho cc khp
trong qu trnh iu khin v vy m qu trnh iu khin thng chm hn do
khi lng tnh ton nhiu khi iu khin. Trong lun n ca mnh, tc gi la
chn phng n iu khin trong khng gian cc khp. thc hin theo
phng n ny ta phi xc nh trc cc c tnh chuyn ng ca cc khp
trn c s chuyn ng nh trc ca khu thao tc, ngha l ta li phi gii li
bi ton ng hc ngc ca rbt. Kt qu ca bi ton ng hc ngc l u
vo cho bi ton iu khin trong khng gian khp. Trong qu trnh iu khin
ta phi m bo tnh n nh chuyn ng ca h thng. Theo [27, 63] th iu
khin dng trt l phng php iu khin c tnh n nh cao d cho trn h
thng trong qu trnh lm vic c th c cc nhiu tc ng ngu nhin v c
cc sai s do cu trc a li cho nn tc gi vn dng phng php iu khin
dng trt cho vic iu khin chuyn ng trong khng gian khp i vi cc
rbt d dn ng,
Cc bi ton nu trn u c mt im chung l u phi tin hnh gii
bi ton ng hc ngc v y l mt bi ton kh i vi cc rbt d dn
ng 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 c s
gii bi ton ng hc ngc v xy dng cc thut ton gii bi ton ng
lc 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, nht
l i vi rbt d dn ng hy cn t c nghin cu.
4. Cu trc ca lun n
Cu trc ca lun n gm: Phn m u, 4 chng ni dung v phn kt
lun chung.
Chng 1: Tnh ton ng hc ngc rbt d dn ng bng thut
ton hiu chnh gia lng vc t ta suy rng. Trong chng ny tc gi
gii thiu khi nim v ma trn ta nghch o v ng dng ca ma trn ta
nghch o gii cc bi ton h phng trnh i s tuyn tnh trong trng
hp 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 trong
khng gian thao tc da trn thut ton hiu chnh gia lng vc t ta suy
rng. 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 ng
trong khng gian thao tc.
Chng 3: iu khin trt rbt d dn ng da trn thut ton s
hiu chnh gia lng vc t ta suy rng. Trong chng ny gii thiu s
lc v c s l thuyt n nh trong iu khin v ni dung bi ton iu khin
chuyn ng. Ni dung chnh ca chng 3 l trnh by mt thut ton gii bi
ton iu khin chuyn ng trong khng gian khp theo phng php iu
khin dng trt.
Chng 4: ng lc hc v iu khin trt rbt o 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 c
ch to ti Trng i Hc Bch Khoa H Ni. Trn m hnh rbt t ch to,
d dng xc nh cc tham s ng hc, ng lc hc ca rbt.
Chng 1
TNH TON NG HC NGC RBT D DN NG
BNG THUT TON HIU CHNH GIA LNG
VC T TA SUY RNG
Trong cc ti liu v rbt ngi ta thng k hiu vc t ta suy rng
T
(vc t m t cu hnh ca rbt) bi q = [q1 , q 2 ,..., q n ] v k hiu vc t xc
nh v tr v hng ca bn kp trong h quy chiu c nh bi
T
x = [x 1 , x 2 ,..., x m ] . T bi ton ng hc thun ca rbt ta xc nh c h
thc
x = f (q)
(1.1)
n
m
Trong q R ; x, f R . Khi m = n rbt c gi l rbt chun, cn
khi m < n rbt c gi l rbt d dn ng.
Ni dung ca bi ton ng hc ngc l: Cho bit x = x(t), tm q = q( t ).
Nh vy trong bi ton ng hc ngc chng ta phi thit lp c quan h
hnh thc:
q = f 1 ( x )
(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. Vic
s dng cc phng php gii tch gii bi ton ng hc ngc ca rbt d
dn 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 ngc
ca rbt d dn ng cn t c nghin cu.
Trong chng ny da trn cng thc khai trin Taylor chng ti xut
mt 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 t ta suy rng. Sau
p dng thut ton xut gii bi ton ng hc ngc ca mt vi rbt
c th, nhm minh ha cho kh nng tnh ton ca thut ton.
1.1 Gii bi ton ng hc thun rbt d dn ng bng phng php ma
trn 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, t
chuyn i ta ca im thao tc v h ta gn lin vi h quy chiu c
nh. H ta Denavit Hartenberg c xy dng nh sau:
Xt 2 khu k tip nhau ca rbt l khu th i1 v khu th i nh hnh
1.1. Gc O i ca h trc ta O i x i y i z i c gn lin vi khu th i (h ta
th i) v c t ti giao im ca trc khp ng th i+1 vi ng vung

gc chung ca cc trc khp ng th i v th i+1. Trng hp 2 trc khp
ng giao nhau th gc ta l im giao nhau , cn nu chng song song
nhau th gc ta l im bt k trn trc khp ng th i+1.
Trc z i ca h ta th i nm dc theo trc khp ng th i+1.
Trc x i ca h ta th i nm dc theo ng vung gc chung ca 2
trc khp ng l khp th i v i+1, c hng t khp ng th i ti khp ng
th i+1. Trong trng hp 2 trc khp ng giao nhau th hng ca trc x i
trng vi hng tch vc t z i z i 1 .
Trc y i c chn sao cho h ta O i x i y i z i l h ta thun.
zi-1
i
Khu i
i 1
zi
i+1
Khp th i
Khu i-1
z i 1
Khp th i+1
O i
ai
di
x i 1
O i1
i
Oi
zi
yi
xi
Hnh 1.1 S thit lp h ta cc khu

Vi cch thit lp cc trc ca h ta nh trnh by th h quy chiu
i khi khng c xc nh mt cch duy nht, v vy ta cn b sung thm mt
s iu kin nh sau:
i vi h ta O 0 x 0 y 0 z 0 - l h ta c nh nm dc theo trc ca
khp ng th 1, c trc z 0 c xc lp theo nguyn tc trn, cn
10
trc x 0 th do khng c khp ng th 0 nn ta c th chn ty , min l

n phi vung gc vi trc z 0 .
i vi h trc ta O n x n y n z n gn vi khu th n, ta thy do khng c
khp ng th n+1 nn theo quy c trn th ta khng xc nh c
trc z n . Trc z n khng c xc nh duy nht trong khi trc x n li c
chn theo php tuyn ca trc z n1 . Trong trng hp ny nu khp n l
khp quay th ta c th chn trc z n // z n 1 . 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 trc
z i 1 mt cch ty . Tuy nhin trong nhiu trng hp ngi ta thng
chn trc z i 1 dc theo trc ca khp tnh tin ny.
1.1.1 Cc tham s ng hc v ma trn Denavit-Hartenberg
Sau khi thit lp xong cc h ta , ta thy v tr ca h ta O i x i y i z i

so vi h ta O i -1 x i 1 y i 1 z i 1 c xc nh bi 4 tham s sau y:
1. i l gc quay trc x i 1 xung quanh trc z i 1 theo chiu ngc chiu
kim ng h phng ca cc trc ta x i 1 v x i trng nhau.
2. d i = O i -1 O i' l khong dch chuyn tnh tin dc theo trc z i 1 gc
ta O i-1 chuyn n O i' - giao im ca trc x i vi z i 1 (hnh 1.1).
3. a i l khong dch chuyn dc theo trc x i a O i' ti im O i .
4. i l gc quay quanh trc x i sao cho trc z i 1 chuyn n trc z i .
Nh vy vi cc php di trc nh nu trn, ta c th a h quy
chiu O i -1 x i 1 y i 1 z i 1 v trng vi h quy chiu O i x i y i z i . Do vy 4 tham s i ,
di, ai, i nu trn c gi l 4 tham s ng hc Denavit Hartenberg.
Trong 4 tham s nu trn th cc tham s a i v i lun l cc hng s,
ln ca chng ph thuc vo hnh dng v s ghp ni ca cc khu th i v i-1.
Hai tham s cn li i v d i th mt s l hng s cn tham s kia s l bin s
ph thuc vo loi khp ca khp ng th i+1. Nu khp th i+1 l khp quay
th i l bin, d i l hng s. Cn khi khp l khp tnh tin th ngc li.
Nh vy ta c th dng 4 php bin i c bn sau y chuyn ta
t h ta O i-1 x i 1 y i 1 z i 1 v h ta O i x i y i z i :
- Quay xung quanh trc z i 1 mt gc i .
- Dch chuyn tnh tin dc trc z i 1 mt on di.
11
- Dch chuyn tnh tin dc trc x i mt on ai.

- Quay xung quanh trc x i mt gc i .
Ta k hiu ma trn ca php bin i ta t h ta O i-1 x i 1 y i 1 z i 1
sang h ta O i x i y i z i l i 1 H i . 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 z i 1 mt gc i th ma
trn bin i cc ta s l:
A ( z , )
i
a 11
a
= 21
a 31
a 12
a 13
a 22
a 32
0
a 23
a 33
0
x (O0 ) cos i
y (O0 ) sin i
=
z (O0 ) 0

1 0
1
sin i
cos i
0
0
0 0
0 0
1 0
0 1
Cn khi dch chuyn tnh tin theo trc z i 1 mt on d i th ma trn bin

i s l :
A ( z ,d )
i
a11
a
= 21
a31
a12
a13
a22
a32
0
a23
a33
0
xO( 0 ) 1
yO( 0 ) 0
=
z O( 0 ) 0

1 0
1
0
0
0 1 di
0 0 1
0 0
1 0
bc dch chuyn tnh tin theo trc x i mt on a i th ma trn chuyn i

ta s l:
A ( x ,d )
i
a11
a
= 21
a31
a12
a13
a22
a23
a32
0
a33
0
xO( 0 ) 1
yO( 0 ) 0
=
z O( 0 ) 0

1 0
1
0 0 ai
1 0 0
0 1 0
0 0 1
v bc quay xung quanh trc x i mt gc i , ma trn chuyn i l:
A ( x , )
i
a 11
a
= 21
a 31
a 12
a 13
a 22
a 32
0
a 23
a 33
0
0
x (O0 ) 1
(0)
y O 0 cos i
=
z (O0 ) 0 sin i

0
1 0
1
0
sin i
cos i
0
0
0
Nh vy nu dng h ta Denavit Hartenberg th ma trn chuyn i

ta i 1 H i t h O i -1 x i 1 y i 1 z i 1 sang h ta O i x i y i z i l:
12
i 1
H i = A ( z,i ) .A ( z,di ) A ( x ,ai ) A ( x ,i )
cos i
sin
i
=
0
sin i
cos i
0
0
0 0 1
0 0 0
1 0 0
0 1 0
0 1
0 0
0 1 d i 0
0 0 1 0
0 0
1 0
0 0 ai 1
0
1 0 0 0 cos i
0 1 0 0 sin i
0 0 1 0
0
0
sin i
cos i
0
0
0
Ly tch ca 4 ma trn trn, ta c mt ma trn c gi l ma trn

DenavitHartenberg v ta c kt qu nh sau:
cos i
sin
i
i 1
Hi =
0
sin i cos i
cos i cos i
sin i
sin i sin i
cos i sin i
cos i
a i cos i
a i sin i
di
(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 ng
hc khc). Khi , cng thc chuyn ta t h ta {O i-1 x i 1 y i 1z i 1 } sang
h ta {O i x i y i z i } l:
x i
x i 1
y
y
i = [ i 1 H i ] i 1
zi
z i 1

1
1
(1.4)
1.1.2 Phng trnh xc nh v tr v hng ca khu thao tc
Xt mt m hnh tng qut ca mt rbt c n khu nh trn hnh v 1.2.

Nh trnh by trn, ma trn H i cho ta bit :
V tr ca im O i trong h quy chiu R i 1 {O i-1 x i-1 y i-1 z i-1 } .
Hng ca vt rn Bi trong h quy chiu R i1 {O i-1 x i-1 y i-1z i-1 }.
Nh vy, bng cch chuyn dn h quy chiu R n {O n x n y n z n } t O n
v O n -1 , O n -2 , v cui cng l v h quy chiu c nh R 0 {O 0 x 0 y 0 z 0 } ta s
xc nh c v tr ca im gc O n v hng ca khu ng th n trong h
quy chiu c nh R 0 {O0 x 0 y 0 z 0 }.
13
Oi
O2
On 2
O n 1
On P
O1
O0
Hnh 1.2
p dng lin tip cc php bin i i vi rbt n khu ta c:
D n = 0 H n = 0 H 1 1 H 2 ...n 1 H n = H 1 H 2 ...H n
a 11
a
D n = 21
a 31
a 12
a 13
a 22
a 23
a 32
0
a 33
0
x (P0 )
y (P0 )
z (P0 )
(1.5)
(1.6)
Ma trn D n cho ta bit v tr ca im P v hng ca khu thao tc (bn

kp) ca rbt i vi h quy chiu c nh R 0 .
Nh vy, khi bit c cc c tnh hnh hc ca cc khu v cc quy
lut chuyn ng ca cc khp l ta hon ton c th xc nh c v tr v
hng ca khu thao tc (bn kp).
14
1.1.3 Bi ton p dng

V d 1.1. Xc nh v tr ca im thao tc v hng ca bn kp ca
rbt 5 khu ng (hnh 1.3).
x2
O3
x3
O2
O4
x1
q3
O1
y5
q4
O5
q5
q2
x5
x4
y0
y1
q1
O0
x0
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
q1
q2
q3
q4
q5
Khu
1
2
3
4
5
d
0
0
0
0
0
0
0
0
0
0
a
a1
a2
a3
a4
a5
Qua tnh ton ta thu c cc kt qu nh sau:

C 1
S
H1 = 1
0
S1
C1
0
0
0 a 1 C1
0 a 1S1
;
1
0
0
1
C 2
S
H2 = 2
0
S2
C2
0
0
0 a 2C 2
0 a 2S 2
;
1
0
0
1
15
C 3
S
H3 = 3
0
S3
C3
0
C 5
S
H5 = 5
0
S5
0
C5
0
0
0 a 3C3
C 4
S
0 a 3S 3
; H4 = 4
0
1
0
0
1
0
S4
C4
0
0
0 a 4C4
0 a 4S4
;
1
0
0
1
0 a 5C5
0 a 5S5
.
1
0
0
1
Trong ta s dng cc k hiu vit tt: Si = sin(qi), Ci = cos(qi).

V ta c ma trn chuyn i l:
D 4 = H1H 2 H 3 H 4 H 5 =
C12345
S
= 12345
0
S12345
C12345
0
0
0 a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + a 5 C12345

0 a 1S1 + a 2S12 + a 3S123 + a 4S1234 + a 5S12345 .
1
0
0
1
vi S12i = sin(q1+q2+qi); C12i = cos(q1+q2++qi).

Nh vy ta ca im thao tc P trong h ta c nh l
xP = a1cos(q1)+a2cos(q1+q2)+a3cos(q1+q2+q3)+a4cos(q1+q2+q3+q4)+
+ a5cos(q1+q2+q3+q4+q5).
yP = a1sin(q1)+a2sin(q1+q2)+a3sin(q1+q2+q3)+a4sin(q1+q2+q3+q4)+
+a5sin(q1+q2+q3+q4+q5).
zP= 0.
Ma trn cosin ch hng ca bn kp i vi h ta c nh l
C12345
A = S12345
S12345
C12345
0
0
0 .
Ta nhn thy khi m < n th phng trnh (1.1) s l mt h m phng

trnh i s phi tuyn ca n n. o hm biu thc (1.1) theo thi gian, ta c
x& = J (q)q&
16
Trong ma trn Jacobi J(q) l mt ma trn ch nht. V vy, khi gii

bi ton ng hc ngc ta cn phi x l cc ma trn ta nghch o. Di y
trnh by mt s khi nim v ma trn ta nghch o [41].
1.2 Ma trn ta nghch o
1.2.1 nh ngha
Cho mt ma trn ch nht A R . Ma trn X R nxm s c gi l ma

trn ta nghch o (ma trn nghch o suy rng) ca ma trn A khi v ch khi
bn phng trnh iu kin sau y c tha mn [41, 45]:
mxn
AXA = A
XAX = X
(AX)T = AX
(XA)T = XA
(1.7)
(1.8)
(1.9)
(1.10)
1.2.2 Ma trn ta nghch o Moore - Penrose
Ta nghch o ca ma trn thng dng l ta nghch o MoorePenrose (MP), c k hiu l A+ [62].

Vi A l ma trn c mxn, nu A c hng y , th ta nghch o A +
ca ma trn A c xc nh nh sau [41]:
A T ( AA T ) 1
A + = A 1
( A T A) 1 A T
m<n
m=n
(1.11)
m>n
Chng minh. chng minh ma trn nu trn l ta nghch o, ta phi

kim tra xem n c tha mn 4 iu kin t (1.7) n (1.10) trn hay khng.
a. Trng hp m < n:
V A c hng y , nn ma trn (AA)Tmxm l kh o.
Khi thay ln lt A + = A T ( AA T ) 1 vo v tri ca cc phng trnh iu
kin, ta s c:
AA + A = A[A T ( AA T ) 1 ]A = AA T ( AA T ) 1 A = A
A + AA + = [A T ( AA T ) 1 ]A[A T ( AA T ) 1 ] = [A T ( AA T ) 1 ][AA T ( AA T ) 1 ]
= A T ( AA T ) 1 = A +
( AA + ) T = [AA T ( AA T ) 1 ] = I T = I = AA T ( AA T ) 1 = AA +
T
17
( A + A) T = [A T ( AA T ) 1 A ] = A T [( AA T ) 1 ] A = A T [( AA T ) T ] A =
T
= A T [AA T ] A = A + A
b. Trng hp m = n:
1
Ma trn A c hng y , nn n l ma trn vung khng suy bin v

A = A 1 , ta c AA + = A + A = I , thay vo 4 phng trnh iu kin trn, ta d
dng nhn thy rng chng c tha mn.
+
c. Trng hp m > n:
V A c hng y , nn rank(A) = n, ma trn (AA)nxn l kh o.
Thay A + = ( A T A) 1 A T ln lt vo v tri ca 4 phng trnh iu kin,
ta c:
AA + A = A[(A T A) 1 A T ]A = A[A 1 (A T ) 1 A T ]A = AA 1 (A T ) A T A = A
1
A + AA + = A + A[( A T A) A T )] = A + A[A 1 ( A T ) 1 A T ] = A + [AA 1 ( A T ) 1 ] = A +
( AA + ) T = [A(A T A) 1 A T ] = A[A 1 (A T ) 1 ] A T = A[A 1 (A 1 )] A T =

T
A[A 1 (A 1 ) T ]A T = A[A 1 (A T )]A T = A[(A T A) 1 ] = A (A T A )

T
]A
1 T
( A + A ) T = [( A T A ) 1 A T A ] = A T [( AA T ) 1 ] A = A T [( AA T ) T ] A =
T
= A T [AA T ] A = A + A
1
Nh vy ta c s khi tnh ton ta nghch o ca mt ma trn theo

Moore Penrose :
A mn
m<n
So snh m, n
m>n
m=n
A + = A T ( AA T ) 1
A + = A 1
A + = ( A T A ) 1 A T
A+
Hnh 1.4.
S khi tnh ton A+ theo phng php Moore-Penrose
18
1.2.3 Phng php nhn t Lagrange
Xt phng trnh i s
Ax = y
(1.12)
Trong A R mn ; cc vc t: x R n , y R m vi m < n.
Ta s xc nh nghim ca phng trnh (1.12), vi iu kin nghim ny
s lm cc tiu ha mt hm mc tiu no . y ta s chn hm mc tiu c
dng ton phng ca bin x:
1
g(x) = x T Wx
2
(1.13)
Vi Wnxn l ma trn trng s i xng, xc nh dng v c la chn

mt cch thch hp. S dng phng php nhn t Lagrange, ta xt hm mc
tiu mi c dng:
1
g * (x, ) = x T Wx + T (y Ax)
2
(1.14)
trong R m c gi l nhn t Lagrange. g * (x, ) t cc tiu, iu

kin cn phi c l:
T
g *
= Wx A T = 0
(1.15)
g *
= Ax + y = 0

(1.16)
T (1.15) ta suy ra :
x = W 1 A T
(1.17)
Th (1.17) vo (1.16), ta c:
AW 1 A T + y = 0 = ( AW 1 A T ) 1 y
Th biu thc ca vo (1.17), ta tm c nghim ca phng trnh tha
mn iu kin cc tiu hm mc tiu (1.13)
x = W 1 A T ( AW 1 A T ) 1 y
(1.18)
C th thy, trong trng hp W = In (ma trn n v cp n), nghim thu

c ca phng php nhn t Lagrange cng chnh l nghim thu c theo
phng php Moore-Penrose.
19
Ta cng thy, nghim theo phng php Moore-Penrose l nghim ng

2
x
1 T
1 T
1 T
vi hm mc tiu g(x) = x Wx = x I n x = x x =
cc tiu, tng ng
2
2
2
2
vi nghim c chun nh nht trong tp nghim ca (1.12)
1.2.4 Nghim tng qut ca phng trnh i s tuyn tnh
Xt phng trnh i s
Ax = y
(1.19)
Trong A R mn , x R n , y R m vi m < n. Ta s chng minh rng

nghim c dng:
x = A + y + (I n A + A)z
(1.20)
Vi In l ma trn n v cp n, z l mt vc t ty thuc Rn, A + l ma

trn ta nghch o ca A.
Tht vy, khi thay (1.20) vo v tri ca (1.19), ta c:
Ax = A[A + y + (I n A + A)z ] = AA + y + A (I n A + A)z =
= AW 1 A T ( AW 1 A T ) 1 y + ( A AA + A )z = y + 0.z = y
Ngc li, ta gi x l 1 nghim trong tp nghim ca (1.19), ngha l
Ax=y, biu din x di dng
x = A + Ax + (I n A + A)x = A + y + (I n A + A)x
(1.21)
iu ny chng t (1.20) l nghim tng qut hn nghim (1.21) m vn

tha mn (1.19). Tuy nhin mi nghim ca (1.19) u c th biu din di
dng (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 phng
php nhn t Lagrange. Ta xt hm mc tiu dng ton phng:
1
g ( x) = ( x z ) T W ( x z )
2
(1.22)
Vi Wnxn l ma trn trng s i xng xc nh dng c la chn mt

cch thch hp. Xt hm mc tiu mi:
1
g * (x, ) = (x z ) T W (x z ) + T (y Ax)
2
hm g * (x, ) t cc tiu
(1.23)
20
g *
= W (x z ) A T = 0
(1.24)
g *
= Ax + y = 0

(1.25)
T (1.24), ta suy ra
x = W 1 A T + z
(1.26)
Thay (1.26) vo (1.25), ta c

AW 1 A T Az + y = 0 = ( AW -1 A T ) -1 (y - Az)
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)
x = W1AT (AW1AT )1 y W1AT (AW1AT )1 Az+ z =
W1AT (AW1AT )1 y + [In W1AT (AW1AT )1 A]z = A+y + (In A+ A)z
(1.27)
C th thy, nghim (1.27) thu c gm 2 thnh phn: thnh phn th

nht l thnh phn c chun nh nht, cn thnh phn th hai tha mn tnh cht
A (I n A + A )z = ( A AA + A)z = 0 chnh l thnh phn ca vc t z chiu
ln 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 s W = In
(chnh l ma trn ta nghch o Moore-Penrose )
1.3 Gii bi ton ng hc ngc rbt d dn ng bng phng php
1.3.1 Phng php khai trin Taylor

Mt 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:
q(t k +1 ) = q(t k ) + q& (t k )t
(1.28)
Trong q& (t k ) c xc nh t cng thc

q& (t k ) = J 1 (q& (t k ))x& (t k )
(1.29)
Th (1.29) vo (1.28) ta c
q(t k +1 ) = q(t k ) + J 1 (q& (t k ))x& (t k )t
(1.30)
21
Vic tnh ton theo cng thc (1.30) cho kt qu kh th (iu ny s

c thy r trong v d phn 1.4). Trong lun n ny chng ti tm cch ci
tin cng thc (1.30) nhn c mt thut ton mi c chnh xc cao hn.
1.3.2 Cc cng thc xc nh vc t vn tc v vc t gia tc suy rng

K hiu q = [q1 , q 2 ,..., q n ] l vc t ta suy rng (vc t m t cu
T
hnh ca rbt), x = [x 1 , x 2 ,..., x m ] l vc t xc nh v tr v hng ca bn

kp trong h quy chiu c nh. T bi ton ng hc thun ta c h thc
T
(1.31)
x = f (q)
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 qu o mong mun cho
trc no , ngha l chng ta phi xc nh cc gi tr ca q to ra gi tr
mong mun cho x. Nh vy bi ton ng hc ngc chng ta phi thit lp
c quan h:
q = f 1 ( x )
(1.32)
Trc ht o hm 2 v ca (1.31) theo thi gian, ta c :
x& =
f
q& = J (q)q&
q
(1.33)
Trong J(q) l ma trn Jacobi c mxn, vi :
f 1
q
f 1
J (q) =
= ...
q f m
q 1
f 1
q 2
...
f m
q 2
f 1
q n
... ...
f m
...
q n
...
(1.34)
Gi s hng ca J(q) l m, theo [41, 57] ta chn ma trn ta nghch o

ca ma trn ch nht J(q) di dng
J + (q) = J T (q)[J (q)J T (q)] ,

1
m<n
(1.35)
Khi t biu thc (1.33) ta suy ra cng thc tnh vc t vn tc suy

rng:
q& ( t ) = J + (q( t ) )x& ( t )
(1.36)
Nh th nu bit c x(t), ta tm c x& (t ) , bit c hm f(q(t)) ta tm

c J + (q( t ) ) . Theo (1.36) ta tnh c q& ( t ) . Vn y l tm cch xc
nh q(t). Vn ny s c trnh by trong mc 1.3.3.
22
o hm 2 v ca (1.36) theo thi gian, ta c cng thc xc nh vc

t gia tc suy rng
&&( t ) = J + (q( t ) )&x&( t ) + J& + (q( t ) )x& ( t )
(1.37)
q
c th p dng c cng thc (1.37) ta cn phi tnh c J& + (q( t ) ) .
Theo l thuyt ta c th o hm ma trn J + (q( t ) ) theo thi gian. Tuy nhin rt
kh xc nh biu thc gii tch ca ma trn ny, do ta phi tm mt thut
ton xc nh J& + (q( t ) ) . T biu thc (1.35) ta suy ra :
J + (q( t ) )J (q( t ) )J T (q( t ) ) = J T (q( t ) )
o hm 2 v ca (1.38) theo thi gian, ta c
J& + (q )J (q )J T (q ) + J + (q )[J& (q )J T (q ) + J (q )J& T (q )] = J& T (q )
(1.38)
(1.39)
T (1.39) ta suy ra
1
J& + (q ) = {J& T (q ) J + (q )[J& (q )J T (q ) + J (q )J& T (q )]}[J (q )J T (q )]
(1.40)
Ma trn J& (q ) c tnh bng cch o hm trc tip cc phn t ca ma

trn J (q ) theo thi gian. Th biu thc (1.40) vo (1.37) ta tm c gia tc
&&( t ) .
q
1.3.3 Cc cng thc xc nh vc t ta suy rng
S dng cc cng thc (1.36), (1.37) v (1.39) ta s xc nh c q& v

&& , nu nh xc nh c q( t ) v x(t) ti thi im kho st. Nh vy,
q
&& trc ht ta cn xc nh q(t). Thut ton xc nh q(t) nh
xc nh q& v q
sau:
Chia khong thi gian lm vic ca rbt [0, T] thnh N khong bng
nhau
t =
T
, ta c t k +1 = t k + t vi k = 0, 1, , N-1
N
p dung khai trin Taylor i vi qk+1 quanh gi tr qk , ta c

1
&& k (t ) 2 + ...
q k +1 = q( t k + t ) = q k + q& k t + q
2
(1.41)
Th biu thc (1.36) vo (1.41) v b qua cc v cng b bc 2 ta c:

q k +1 = q k + J + (q k )x& k t vi k = 0, 1, , N-1
(1.42)
23
T , ta c cc bc tnh ton nh sau:

1. Tm q 0 .
2. Tnh J (q 0 ), J + (q 0 ), J& (q 0 ) .
&&(t = 0) = q
&& 0 theo (1.37).
3. Tnh q& ( t = 0) = q& 0 theo (1.36) v tnh q
&& k +1 theo (1.36) v (1.37).
4. Tnh q k +1 theo (1.40), ri tnh q& k +1 , q
Ta thy vic tnh q k+1 theo (1.42) l kh th. V vy nng cao chnh
xc, ta cn c mt thut ton xc nh q k +1 chnh xc hn nh di y.
a. Hiu chnh gia lng vc t ta suy rng ti thi im t0
~
Gi s t 0 = 0 l thi im u. Ta c th xc nh gi tr gn ng q
0
ca q 0 bng cch v (hoc bng thc nghim). Sau p dng khai trin Taylor
tm gn ng tt hn ca q 0 nh sau:
~ + q
Gi s ly q = q
0
Theo phng trnh (1.31), ta c :

~ + q ) = f (q
~ ) + f (q
~ )q + ...
x 0 = f (q 0 ) = f (q
0
0
0
0
0
q
T suy ra
~ )q x f (q
~ )
J (q
0
0
0
0
(1.43)
Gii phng trnh i s tuyn tnh (1.43) vi n q 0 , ta c
~ )[x f (q
~ )] .
q 0 = J + (q
0
0
0
~ bi q
~ * := q
~ + q ri th
Nu q 0 th ta tnh li gi tr mi ca q
0
0
0
0
vo (1.43) v li gii phng trnh ny. Qu trnh lp c dng li khi
q 0 < .
V khi ta gn gi tr tm c l q0
~*
q =q
0
(1.44)
Bit c nghim q 0 ca phng trnh (1.29) ti thi im t 0 = 0 . Thay

&&(0) = q
&& 0 .
vo (1.36), (1.37) ta tm c q& (0) = q& 0 v q
Gi tr sai s cho php c xc nh theo yu cu v chnh xc ca
bi ton.
24
b. Hiu chnh gia lng vc t ta suy rng ti thi im tk+1

~ theo cng thc (1.42)
Trc tin ta xc nh gi tr gn ng q
k +1
~ = q + J + (q )x& t
q
k +1
k
k
k
(1.45)
Sau ta tm biu thc gn ng tt hn ca q k+1

~ + q
q k +1 = q
k +1
k +1
Ta cn xc nh q k+1
(1.46)
Th (1.46) vo phng trnh (1.31), ta c :

~ + q ) = f (q
~ ) + f (q
~ )q + ...
x k +1 = f (q k +1 ) = f (q
k +1
k +1
k +1
k +1
k +1
q
T suy ra
~ )q
J (q
k +1
k +1
~ )
x k +1 f (q
k +1
(1.47)
~ ) l ma trn c mxn, q l vc t c n phn t. Do

Ch rng J (q
k +1
k+1
s n ln hn s phng trnh (m<n), nghim c chun nh nht ca phng
trnh (1.47) c dng :
~ )[x f (q
~ )]
q = J + (q
(1.48)
k +1
Sau ly
~ =q
~
q
k +1
k +1
k +1
+ q k +1
k +1
k +1
(1.49)
Nu q k +1 th thay (1.47) vo (1.48) v tip tc gii phng trnh

(1.47). Nu q k +1 < th ta ly
~
(1.50)
q k +1 = q
k +1
&& k+1 theo cng thc
Bit c q k +1 , ta tnh q& k +1 theo cng thc (1.36) v q
(1.37).
Trn hnh 1.5 trnh by s khi gii bi ton ng hc ngc rbt d

dn ng bng phng php hiu chnh gia lng vc t ta suy rng.
25
Cho x = f (q) , x = x( t ) , J (q ) =
f
(q) , t0, q 0 ,
q
N, T
h = t = T/N
k: = 0
~ := q
t k := t 0 , q
k
0
~ ) , x = x( t ) , f = f (q
~ )
Tnh J + (q
k
k
k
k
k
~ )(x f )
q k = J + (q
k
k
k
~
~
q := q + q
k
~ := q
~ + J + (q )x& t
q
k
k 1
k 1
k 1
q k <
Sai
ng
Xut kt qa qk
k := k + 1
Sai
kN
ng
KT THC
Hnh 1.5.
S khi gii bi ton ng hc ngc
26
1.3.4 nh gi sai s
nh gi sai s ca phng php ta a vo cc cng thc xc nh

sai s ca dch chuyn, vn tc v gia tc
e( t k ) = x( t k ) x k = x( t k ) f (q k )
e& ( t k ) = x& ( t k ) x& k = x& ( t k ) J (q k )q& k
&& k
&e&( t k ) = &x&( t k ) &x& k = &x&( t k ) J& (q k )q& k J (q k )q
(1.51)
Trong e( t k ) = [e1 ( t k ) e 2 ( t k ) L e m ( t k )] .
T
ln ca cc chun ca cc vc t e( t k ), e& ( t k ), &e&( t k ) cho bit chnh

xc ca phng php hiu chnh gia lng vc t ta suy rng.
Trong chng trnh tnh ton ta s dng chun Euclid
e( t k ) = e12 ( t k ) + e 22 ( t k ) + L + e 2m ( t k )
thy r chnh xc ca phng php s ngh, ta xt cc v d sau.
Trong cc v d y c bit cn ch n cc th nh gi sai s. l cc
kt qu nghin cu mi ca chng ti.
1.4
Cc bi ton p dng
V 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.
x1
q3
x2
q4
x3
q2
q1
O
q5
x4
x5
x
Hnh 1.6. Rbt phng 5 khu ng

Cho 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)
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 tr
v 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*S1
yp = a5*C12345 + a4*C1234 + a3*C123 + a2*C12 + a1*C1
Do ma trn Jacobi l:
J11 = a5C12345+a4C1234+a3C123+a2C12+a1C1
J12 = a5C12345+a4C1234+a3C123+a2C1
J13 = a5C12345+a4C1234+a3C123
J14 = a5C12345+a4C1234
J15 = a5C12345
J21 = -a5S12345-a4S1234-a3S123-a2S12-a1S1
J22 = -a5S12345-a4S1234-a3S123-a2S12
J23 = -a5S12345-a4S1234-a3S123
J24 = -a5S12345-a4S1234
J25 = -a5S12345
S dng chng trnh gii bi ton ng hc ngc cho trng hp c s
dng thut ton hiu chnh gia lng v khng s dng thut ton hiu chnh gia
lng vi cc bc thi gian tnh ton khc nhau, ta thu c cc th gc
quay, vn tc gc v gia tc gc ca cc khp ng cng cc sai s v tr do cc
phng php tnh a li nh sau:
a.
Chn bc thi gian tnh ton h = t = 0.001 (s)
Khi khng s dng thut ton hiu chnh gia lng vc t ta suy
rng
4
q1 [rad]
3
q1 [1/s]
q 1 [1/s 2]
1
0
-1
-2
Hnh 1.7.
5
time [s]
10
28
1.5
1
0.5
0
-0.5
q2 [rad]
-1
q2 [1/s]
-1.5
-2
q 2 [1/s 2]
1
Hnh 1.8.
5
time [s]
10
2
q3 [rad]
q3 [1/s]
q3 [1/s 2]
0
-1
-2
-3
0
Hnh 1.9.
5
time [s]
10
5
q [rad]
4
q [1/s]
4
q4 [1/s
2]
1
0
-1
-2
-3
5
time [s]
10
Hnh 1.10. Cc c tnh chuyn ng ca khu 4
29
4
q [rad]
5
q [1/s]
5
q [1/s 2]
-2
-4
-6
-8
-10
0
5
time [s]
10

-4
x 10
2
0
-2
ex [m]
-4
-6
-8
-10
-12
-14
5
time [s]
10
Hnh 1.12. Sai s v tr ca im thao tc theo trc x

-4
x 10
2
0
ey [m]
-2
-4
-6
-8
-10
-12
5
time [s]
10
Hnh 1.13. Sai s v tr ca im thao tc theo trc y
30
-15
x 10
e [rad]
4
2
0
-2
-4
5
time [s]
10
Hnh 1.14. Sai s gc nh hng ca bn kp

1
0.8
y [m]
0.6
0.4
0.2
-0.4
-0.2
0.2
0.4
x [m]
0.6
0.8
Hnh 1.15. Dng chuyn ng ca rbt theo kt qu tnh ton

Khi c s dng thut ton hiu chnh gia lng vi sai s cho php
= 10-6 (rad), kt qu nhn c l :
4
q [rad]
1
q [1/s]
1
q [1/s 2]
1
0
-1
-2
5
time [s]
10
31
1.5
1
0.5
0
-0.5
q2 [rad]
-1
q2 [1/s]
-1.5
-2
0
q2 [1/s2]
1
5
time [s]
10

2
1
0
-1
q3 [rad]
q3 [1/s]
-2
q3 [1/s 2]
-3
0
5
time [s]
10

6
q4 [rad]
q4 [1/s]
q4 [1/s 2]
2
0
-2
-4
5
time [s]
10
32
4
2
0
-2
-4
q5 [rad]
-6
q5 [1/s]
-8
q5 [1/s2]
-10
0
5
time [s]
10

-16
x 10
ex [m]
2
0
-2
-4
-6
5
time [s]
10
Hnh 1.21. Sai s theo trc x ca im thao tc

-15
x 10
ey [m]
1
0
-1
5
time [s]
Hnh 1.22. Sai s theo trc y ca im thao tc
10
33
-15
2.5
x 10
e [rad]
1.5
1
0.5
0
-0.5
-1
5
time [s]
10
0.8
y [m]
0.6
0.4
0.2
-0.4
-0.2
0.2
0.4
x [m]
0.6
0.8

Qua cc hnh 1.12, 1.13, 1.14, 1.21, 1.22, 1.23 ta nhn thy bc thi
gian tnh ton h = 0.001(s) th mc chnh xc ca phng php tnh khi c s
dng 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 hiu
chnh gia lng vc t ta suy rng (ch t ti 10-4, hnh 1.12, 1.13).
34
Chn bc thi gian tnh ton h = t = 0.01 (s)

Trng hp tnh ton khng s dng thut ton hiu chnh gia lng
vc t ta suy rng ta c:
0.005
0
ex [m]
-0.005
-0.01
-0.015
-0.02
-0.025
10
time [s]
12
14
16
18
20

-3
x 10
ey [m]
-5
-10
-15
-20
10
time [s]
12
14
16
18
20

-15
x 10
2
e [rad]
b.
-2
-4
10
time [s]
12
14
16
18
20
35
0.8
y [m]
0.6
0.4
0.2
-0.4
-0.2
0.2
0.4
x [m]
0.6
0.8

Khi c s dng thut ton hiu chnh gia lng vc t ta suy rng
-16
x 10
ex [m]
2
0
-2
-4
-6
10
time [s]
12
14
16
18
20

-16
x 10
ey [m]
2
0
-2
-4
-6
10
time [s]
12
14
16
18
20
36
-15
x 10
e [rad]
-1
10
time [s]
12
14
16
18
20
0.8
y [m]
0.6
0.4
0.2
-0.4
-0.2
0.2
0.4
x [m]
0.6
0.8

Nh vy khi tng t = h ln (gim s im tnh ton xung) ta thy khi s
dng 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 m
bo c s chnh xc v qu o mong mun cho im thao tc E (hnh 1.32).
Cn khi khng s dng thut ton hiu chnh, chnh xc ch t ti 10-3 (hnh
1.25, 1.26), v vy m sai s tch ly ln v qu o chuyn ng ca im E s
khng c chnh xc. Trn hnh 1.28 ta thy khi khng s dng thut ton hiu
chnh gia lng vc t ta suy rng, sau mt thi gian tnh ton, do sai s
tch ly m im E ca khu thao tc khng vch nn 1 ng trn nh nhim
37
v cng ngh t ra. Trn hnh 1.32 do c s dng thut ton hiu chnh vc t
ta suy rng nn im E vn lun chuyn ng trn qu o chng trnh.
Vi bc thi gian tnh ton h = t = 0.05 (s)
Khi khng s dng thut ton hiu chnh gia lng vc t ta suy
rng
0.05
ex [m]
-0.05
-0.1
-0.15
10
time [s]
12
14
16
18
20

0.05
ey [m]
0
-0.05
-0.1
-0.15
10
time [s]
12
14
16
18
20

-15
x 10
1
e [rad]
c.
-1
-2
10
time [s]
12
14
16
18
20
38
0.8
y [m]
0.6
0.4
0.2
-0.5
0.5
x [m]

Khi c s dng thut ton hiu chnh gia lng vc t ta suy rng
ta nhn c cc kt qu nh sau :
-16
x 10
ex [m]
-2
-4
5
time [s]
10

-16
x 10
4
2
ey [m]
0
-2
-4
-6
5
time [s]
10
39
-15
x 10
e [rad]
-1
5
time [s]
10

1
0.8
y [m]
0.6
0.4
0.2
-0.4
-0.2
0.2
0.4
x [m]
0.6
0.8

Qua cc kt qu tnh ton c c cc trng hp ca v d 1.2 ta nhn
thy khi dng thut ton hiu chnh gia lng vc t ta suy rng gii bi
ton ng hc ngc th mc chnh xc ca cc kt qu tnh ton s cao hn
nhiu 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 t ta suy rng th chng ta c th gim bt khi lng
tnh ton xung (do c th tng bc thi gian tnh ton ln) cn phng php
tnh ton m khng s dng thut ton th khng th c (xem cc hnh 1.28,
1.32, 1.36, 1.40).
40
V d 1.3. Xc nh quy lut chuyn ng ca cc khp ng thuc rbt

6 khu ng c s kt cu nh hnh v 1.41.
x2
x3
O3
q4
O2
O4
q3
q5
x1
O1
y1
q6
x5
q1
O6
y0
q2
y6
x4
O5
x6
O0
x0
Hnh 1.41. Rbt 6 khu ng
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 c nh =900,
T s kt cu ca rbt ta c:
xp = a6*C123456 + a5*C12345 + a4*C1234 +a3*C123 + a2*C12 +a1*C1
yp = a6*S123456 + a5*S12345 + a4*S1234 + a3*S123 + a2*S12 + a1*S1
Do ma trn Jacobi l:
J11 = -a6S123456-a5S12345-a4S1234-a3S123-a2S12-a1S1
J12 = -a6S123456-a5S12345-a4S1234-a3S123-a2S12
J13 = -a6S123456-a5S12345-a4S1234-a3S123
J14 = -a6S123456-a5S12345-a4S1234
J15 = -a6S123456-a5S12345
J16 = -a6S123456
J21 = a6C123456+a5C12345+a4C1234+a3C123+a2C12+a1C1
J22 = a6C123456+a5C12345+a4C1234+a3C123+a2C1
J23 = a6C123456+a5C12345+a4C1234+a3C123
J24 = a6C123456+a5C12345+a4C1234
J25 = a6C123456+a5C12345
J26 = a6C123456
41
Goc quay cua cac khop (rad)
Chn bc chia thi gian h = 0.001(s), chn sai s = 10 6 (rad). S dng

chng trnh gii bi ton ng hc ngc, ta thu c th v tr, vn tc gc
v gia tc gc ca cc khp v sai s tnh ton theo thut ton xut.
2
q1
1
q2
q3
q4
q5
-1
q6
-2
Van toc goc cua cac khop (rad/s)
Hnh 1.42.
4
6
Thoi gian(s)
10
th gc quay ca cc khp (rad)
1
q'1
0.5
q'2
q'3
q'4
q'5
-0.5
q'6
-1
4
6
Thoi gian(s)
10
Hnh 1.43. th vn tc gc cc khp (rad/s)

Gia toc goc cac khop (rad/s 2)
q" 1
q" 2
q" 3
q" 4
-1
q" 5
q" 6
-2
4
6
Thoi gian(s)
10
Hnh 1.44. th gia tc gc cc khp ng (rad/s2)
Sai so vi tri trong khong gian thao tac (m, rad)
42
-13
x 10
Sai so theo truc x
Sai so theo truc y

Sai so goc dinh huong
1
0
-1
4
6
Thoi gian(s)
10
Sai so van toc trong khong gian thao tac (m/s)
Hnh 1.45. th sai s v tr trong khng gian thao tc

-15
x 10
-1
Sai so truc x
Sai so truc y
-2
4
6
Thoi gian(s)
10
Sai so gia toc trong khong gian thao tac (m/s 2)
Hnh 1.46. th sai s vn tc trong khng gian thao tc

-13
x 10
Sai so truc x
Sai so truc y
3
2
1
0
-1
4
6
Thoi gian(s)
10
Hnh 1.47. th sai s gia tc trong khng gian thao tc
43
Cc hnh t 1.42 n 1.44 cho ta bit s thay i ca v tr, vn tc v gia

tc ca cc khp. c bit l cc hnh t 1.45 n 1.47 ch ra sai s ca cc gc
quay, vn tc gc v gia tc gc ca cc khp khi tnh theo thut ton hiu
chnh vc t ta suy rng, cc sai s ny l rt b: sai s i vi cc gc quay
nh hn 4x10-13 rad, i vi cc vn tc gc nh hn 2x10-15 rad/s, i vi gia
tc 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 t ta suy rng.
1.5 Kt lun chng 1
Trong 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 hng
dn ca GS. Nguyn Vn Khang. Chng ti ngh gi tn ca phng php s
ny l thut ton hiu chnh gia lng vc t ta 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 d c trnh
by trong chng ny nhm minh ha cho thut ton v chnh xc ca thut
ton cng nh th hin r tnh u vit ca phng php ny so vi phng php
tnh ton khng s dng thut ton hiu chnh gia lng vc t ta 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 th
s dng thut ton ny tnh ton ng lc hc ngc cc c cu rbt phc
tp, bi v vic tnh ton ng lc hc cc rbt phc tp i qu nhiu php
tnh do m sai s ca phng php c mt vai tr ht sc quan trng.
Chng 2
TNH TON NG LC HC NGC RBT D DN NG
TRONG KHNG GIAN THAO TC DA TRN THUT TON HIU CHNH
GIA LNG VC T TA SUY RNG
Trong chng ny cc phng trnh Lagrange loi 2 c s dng thit
lp cc phng trnh vi phn chuyn ng ca rbt. Cho bit chuyn ng ca
khu thao tc, tm biu thc ca lc/mmen ca cc khu dn ng l ni dung
chnh ca bi ton ng lc hc ngc. Nh th bi ton ng lc hc ngc rbt
phi da trn bi ton ng hc ngc v c lin h mt thit vi bi ton iu
khin rbt. Trong chng ny da trn thut ton hiu chnh gia lng vc t ta
suy rng, trnh by mt thut ton gii bi ton ng lc hc ngc rbt d
dn ng trong khng gian thao tc.
2.1 Dng thc Lagrange loi 2 ca h nhiu vt
Xt c h gm p vt rn, chu r lin kt hlnm. S bc t do ca c h l
f = 6p r . Trong on ny da trn khi nim ma trn Jacobi, ta thit lp dng
thc Lagrange loi 2 cho h nhiu vt
hlnm [17, 55].
zi
K hiu cc ta suy rng l
yi
z0
q = [ q 1 ,K , q f ] T
(2.1)
Ci
V tr ca mi vt rn Bi trong h
Bi
quy chiu c nh R 0 = Ox 0 y 0 z 0 c xc
xi
nh bi vc t xc nh v tr khi tm v
y0
ma trn csin ch hng ca vt rn.
R0
rC = rC (q, t ), A i = A i (q, t )
(2.2)
x0
Hnh 2.1
n gin, di y ta ch xt cc
h hlnm gi v dng. Khi biu thc (2.2) c dng
rC = rC (q), A i = A i (q)
(2.3)
Theo [17] trng thi vn tc ca vt rn Bi c xc nh bi vn tc khi
tm v vn tc gc ca n
drC rC
~ =A
& AT
(2.4)
vC =
=
q& ,
i
i
i
i
dt
q
i
45
Nu ta a vo cc k hiu ma trn Jacobi tnh tin J Ti v ma trn Jacobi

quay J R i [17] theo cc cng thc
rCi
i
q
q&
th vn tc khi tm vt rn v vn tc gc ca n c th vit di dng sau
v Ci = J Ti (q)q& ,
i = J R i (q)q&
J Ti =
, J Ri =
(2.5)
(2.6)
T biu thc tnh ng nng ca mt vt rn trong [17, 64, 67], ta suy ra

biu thc ng nng ca h p vt rn.
p
1 p
1 p
T = Ti = m i v TCi v Ci + iT I i i
(2.7)
2 i=1
2 i=1
i =1
Trong (2.7) th Ii l ma trn ca tenx qun tnh ca vt rn th i i vi
khi tm Ci ca n trong h qui chiu c nh R0.
Ch . Trong tnh ton thc hnh c khi ngi ta thay vic tnh biu thc
p
i =1
T
i
I i i bng biu thc
(i )T
i
I i( i ) i( i ) . Trong I i( i ) l ma trn ca tenx qun
tnh ca vt rn i vi khi tm Ci ca n trong h qui chiu ng C i x i y i z i gn

r
lin vo vt Bi, cn i( i ) l vc t i s ca i trong h qui chiu C i x i y i z i .
Th cc biu thc (2.6) vo biu thc (2.7) ta c
1 p
1 p
T
T
T = m i (J Ti q& ) J Ti q& + (J Ri q& ) I i J R i q&
2 i=1
2 i=1
p
1
= q& T [ (m i J TTi J Ti + J TRi I i J Ri )]q&
2
i =1
By gi ta a vo k hiu
M (q) = (m i J TT J T + J TR I i J R
p
i =1
(2.8)
(2.9)
Biu thc (2.9) c gi l ma trn khi lng suy rng. Khi biu thc
ng nng ca h nhiu vt c dng rt gn nh sau:
1
T = q& T M (q)q&
(2.10)
2
Trong
46
&q&1
q& 1
q1
m11 (q) K m1f (q)
&& = M
K
K , q = M , q& = M , q
M (q) = K
(2.11)

&q& f
q& f
q f
m f 1 (q) K m ff (q)
Biu thc ng nng (2.10) c th vit li di dng nh sau
1 f f
T = m jk (q 1 ,K, q f )q& k q& j
(2.12)
2 j=1 k =1
o hm biu thc ng nng (2.12) theo cc vn tc suy rng q& j v cc ta
suy rng qj ta c
f
T
T 1 f f m ik
= m ij q& j ,
=
q& k q& j
q& i j=1
q i 2 j=1 k =1 q i
T suy ra
f
f m
d T f
= m ij&q& j + ij q& k q& j

dt q& i j=1
j=1 k =1 q k
Th cc biu thc trn vo phng trnh Lagrange loi 2
d T T
( j = 1,K, f )
(2.13)
+ j
dt q& j q j
q j
ta c
f
f
1
+ m jk ,l m kl , j q& k q& l + g j (q) = j

2
j=1
j=1 k =1
Trong ta s dng cc k hiu

m jk
m kl
m jk ,l =
, m kl , j =
, g j (q) =
q l
q j
q j
f
m &q&
ij
m ij,k
( j = 1,K, f ) (2.14
(2.15)
Do ma trn khi lng M (q) l ma trn i xng, nn ta c

= m jk ,i = m ki , j . Do phng trnh (2.14) c th vit li di dng nh sau
1 f f
(m lj,k + m jk ,l m kl, j )q& k q& j = + i (i = 1,K, f ) (2.16)
2 k =1 l=1
q i
j=1
Phng trnh (2.14) hoc (2.16) thng c gi l dng thc Lagrange loi
2 ca h nhiu vt.
Nu ta a vo k hiu
h ij,k (q) = m ij,k + m ik , j m kj,i
(2.17)
f
m ij&q& j +
47
Th phng trnh (2.14) c th vit li di dng

f
m
j=1
&&
& &
ij (q )q j + h ij, k q k q j + g i (q ) + i
j=1 k =1
(i = 1,K, f )
(2.18)
thun tin cch vit ta a vo k hiu

f
c ij (q, q& ) = h ij,k (q)q& k
(2.19)
k =1
khi
f
h
j=1 k =1
& &
& &
ij, k (q )q k q j = c ij (q, q )q j
(2.20)
j=1
Phng trnh vi phn (2.18) by gi c dng

f
m
j=1
ij
(q)&q& j + c ij (q, q& )q& j + g i (q) = i

j=1
(i = 1,K, f )
Phng trnh (2.21) c th vit li di dng ma trn

&& + C(q, q& )q& + g (q) =
M (q)q
(2.21)
(2.22)
trong :
qRn
l vc t ta suy rng,
M (q) R nn l ma trn khi lng, i xng xc nh dng,
C(q, q& ) R nn l ma trn lin quan n lc coriolis v lc ly tm,
g(q) R n
l vc t lc do trng lc,
Rn
l vc t lc dn ng t cc ng c.
Trong cc bi ton iu khin rbt ngi ta thng hay s dng phng
trnh vi phn chuyn ng dng (2.22).
Vic thit lp phng trnh vi phn chuyn ng ca rbt ngy nay c
tin hnh mt cch t ng nh cc phn mm. trng i hc Bch khoa H
Ni, nhm nghin cu ca GS. Nguyn Vn Khang xy dng phn mm
ROBODYN [49] thit lp t ng cc phng trnh vi phn chuyn ng ca
rbt. Trong lun n ny s dng phn mm ROBODYN thit lp phng trnh
vi phn chuyn ng ca cc rbt.
2.2 Gii bi ton ngc ng lc hc rbt d dn ng trong khng gian

thao tc
Mt 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 ca
rbt c th lm vic theo mt chng trnh nh trc. Bi ton ny c gi
l bi ton ng lc hc ngc. Bi ton ng lc hc ngc mt mt phi da vo
48
cc thut ton ca bi ton ng hc ngc, mt khc c lin quan mt thit ti bi

ton iu khin chuyn ng ca rbt.
Trong phn ny, da trn thut ton hiu chnh gia lng vc t ta suy
rng gii bi ton ng hc ngc trnh by mt thut ton s gii cc bi ton
ng 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 di
dng
x = f(q)
(2.24)
n
m
trong q R l vc t cha cc bin khp, x R l vc t cha v tr tm ca
bn kp trong mt h ta c nh v hng ca bn kp.
T
T
x = [x 1 x 2 K x m ]
q = [q1 q 2 K q n ]
f
x& = q& = J (q)q&
(2.25)
q
trong J(q) l ma trn Jacobi c m n c xc nh theo (1.34). Gi s hng
ca J(q) l m, theo [41, 45] ta chn
J + (q) = J T (q)[J (q)J T (q)]1
(2.26)
+
Ma trn J (q) 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
q& = J + (q)x&
(2.27)
&& = J + (q)&x& + J& + (q)x&
q
(2.28)
d
vi J& + (q) = J + (q).
dt
Cc cng thc (2.27) v (2.28) cho php ta xc nh c vc t vn tc v
&& , nu nh bit q(t) ti thi im kho st v quy lut
vc t gia tc suy rng q& v q
chuyn ng ca bn kp x( t ), x& ( t ), &x&( t ) .
S dng phng php hiu chnh gia lng vc t ta suy rng trnh
&& nh sau:
by trong chng 1, chng ta c cc bc xc nh q, q& , q
Bc 1. Gii h phng trnh i s tuyn tnh
~ ) q x ( t ) f ( q
~ )
(2.29)
J (q
0
0
0
c
~ )[x( t ) f (q
~ )] .
q 0 = J + (q
(2.30)
0
0
49
Bc 2. iu chnh nghim
~ := q
~ + q .
q
0
0
0
(2.31)
Bc 3. Kim tra iu kin sai s

Nu q 0 , l sai s cho php, chuyn sang bc 4, ngc li th ta tr
li bc 1.
~ .
Bc 4. Ly nghim q 0 = q
0
Nu khng ch quan tm n nghim bnh phng ti thiu ca phng trnh
(2.25) th biu thc nghim (2.27) c th c vit li nh sau:
q& = J + (q)x& + [E J + (q)J (q)]z 0
(2.32)
trong z 0 R n l mt vect ty v E R nn l mt ma trn n v.
Vic chn vc t z0 cho php ta khai thc c u im (kh nng trnh vt
cn, trnh va chm vi gii hn khp, trnh cu hnh k d) ca rbt d dn ng
so vi rbt chun. Trong phn ny, trnh va chm vo gii hn khp ta s chn
z0 hm sau y t cc tiu
2
1 n q i qi
(2.33)
(q) = c i
2 i =1 q iM q im
Vi qiM (qim) l k hiu ca gii hn ln nht (nh nht), qi l gi tr gia
ca khong lm vic ca khp, v cc trng s dng ci > 0. Do , cc tiu ca
hm ny s lm cho cc bin khp di chuyn v gi tr trung gian, tnh d dn ng
s c khai thc gi cho cc bin khp gn gi tr gia ca khong lm vic
ca rbt, trnh c va chm vi cc gii hn khp.
Ta ly
(2.34)
z 0 = (q )
trong : (q) : gradient ca (q ) , hng s m nu (q ) cn cc tiu ho.
T (2.32) suy ra
&& = J + (q)&x& + J& + (q)x&
q
(2.35)
d +
[J (q)J (q)]z 0
dt
&&( t k ) ti thi im tk, xp x
Sau khi tnh c cc gi tr ca q ( t k ), q& ( t k ), q
ban u cho thi im tip theo t k +1 = t k + t c chn nh sau
+ [E J + (q)J (q)]z& 0
q k +1 = q k + J + (q)x& k t + [E J + (q k )J (q k )]z 0 t .
50
Sau thut ton iu chnh gia lng sai s vect ta suy rng c p
dng tinh chnh gi tr xp x trn.
Phng trnh ng lc ca tay my rbt c th nhn c nh s dng
phng trnh Lagrng loi 2 nh sau
&& + C(q, q& )q& + g (q) = ,
M (q)q
(2.36)
&& ca bi ton ng hc ngc, t
S dng file s liu cc vc t q, q& , q
phng trnh (2.36) ta c th xc nh mmen/lc cn thit tng ng vi chuyn
ng mong mun x(t) ca bn kp.
Cc bc tin hnh tnh mmen/lc ng c khi bn kp chuyn ng theo
mt 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
&& ca cc khp t chuyn ng ca bn kp x( t ), x& ( t ), &x&( t ) .
rng q, q& , q
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).
51
Xc nh f ( q ), M ( q ), C ( q , q& ), g ( q )
Cho x=x(t), t0, q0, N, T
J (q ) =
f
( q ); h = t = T / N
q
~ =q
k:=0; tk:=t0; q
k
0
~ ), x = x ( t ), f = f ( q
~ )
Tnh J + ( q
k
k
k
k
~ )( x f )
q k = J + (q
k
k
k
~ =q
~ + q
q
k
k
k
~ := q
~ + J + (q
~ ) x& t
q
k
k 1
k 1
k 1
q k <
Sai
ng
~ ) x& ; q
~ ) &x& + J& + ( q
~ ) x&
&& k = J + ( q
q& k = J + ( q
k
k
k
k
k
Tnh M ( q k ), C ( q k , q& k ), g ( q k )
&& k + C ( q k , q& k ) q& k + g ( q k )
= M (q k )q
Xut kt qu k
k := k + 1
Sai
kN
ng
KT THC
Hnh 2.2. Thut ton gii bi ton ng lc hc ngc trong khng gian thao tc
52
2.3 Cc bi ton p dng

V 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 bi

ton cn c 3 ta xc nh v tr ca bn kp v vy trong trng hp ny n l
rbt d dn ng (d 1 khp dn ng).
T s kt cu ca rbt ta c bng cc tham s ng hc:
Bng 2.1 Cc tham s ng hc ca rbt Scara
d
a
Khu
q1
d1
a1
q2
a2
q3
q4
d4
53
Gii bi ton ng hc thun ta c:

0 a 2 cos(q 1 + q 2 ) + a 1 cos(q 1 )
cos(q 1 + q 2 q 4 ) sin(q1 + q 2 q 4 )
sin(q + q q ) cos(q + q q ) 0 a sin(q + q ) + a sin(q )
1
2
4
1
2
4
2
1
2
1
1
D4 =
0
0
1
d 4 q 3 + d 1
0
0
0
1
x P = a 2 cos(q1 + q 2 ) + a 1 cos(q1 )
y P = a 2 sin(q 1 + q 2 ) + a 1 sin(q1 )
z P = d 4 q 3 + d 1
suy ra ta ca tm bn kp v ma trn Jacobi l:
a 1S1 a 2S12 a 2S12 0 0
J = a 1C1 + a 2 C12
a 2 C12
0 0
0
0
1 0
Chn bc thi gian h = 0.001 giy; sai s cho php 10 6 t dng
chng trnh gii bi ton ng hc ngc ta nhn c th gc quay, vn tc
gc, gia tc gc v cc sai s chuyn ng ca bn kp l:
3
Vi tri cac khau (rad, m)
2
1
0
q1(rad)
-1
q2(rad)
q3(m)
-2
-3
q4(rad)
0
Hnh 2.4.
4
6
Thoi gian(s)
10
th chuyn ng ca cc khp ng rbt Scara
54
0.4
Van toc cac khau (rad/s, m/s)
0.3
0.2
0.1
0
-0.1
q.1(rad/s)
-0.2
q.2(rad/s)
q.3(m/s)
-0.3
-0.4
q.4(rad/s)
0
Hnh 2.5.
4
6
Thoi gian(s)
10
th vn tc chuyn ng ca cc khp ng rbt Scara
Gia toc cac khau (rad/s 2, m/s 2)
0.6
0.4
0.2
0
-0.2
q..1(rad/s 2)
-0.4
q..2(rad/s 2)
q..3(m/s 2)
-0.6
-0.8
Hnh 2.6.
q..4(rad/s 2)
0
4
6
Thoi gian(s)
10
th gia tc chuyn ng ca cc khp ng rbt Scara
Sai so vi tri trong khong gian thao tac (m)
55
-15
x 10
-2
-4
-6
Sai so truc x
Sai so truc y
Sai so truc z
0
Hnh 2.7.
4
6
Thoi gian(s)
10
th sai s v tr bn kp ca rbt Scara
Sai so van toc trong khong gian thao tac (m/s)
-16
x 10
1.5
1
0.5
0
-0.5
-1
Sai so truc x
Sai so truc y
Sai so truc z
-1.5
-2
Hnh 2.8.
4
6
Thoi gian(s)
10
th sai s vn tc bn kp ca rbt Scara
Sai so gia toc trong khong gian thao tac (m/s 2)
56
-16
x 10
2
1
0
-1
Sai so truc x
Sai so truc y
Sai so truc z
-2
-3
Hnh 2.9.
4
6
Thoi gian(s)
10
th sai s gia tc bn kp ca rbt Scara
Bng 2.2. Bng thng s ng lc rbt Scara 4 bc t do

Mmen qun tnh khi tng khu
V tr trng tm (so vi
Khu
gc to gn trn mi
Khi
khp)
lng
xC
yC
zC
-(a1-l1x)
-l1z
-(a2-l2)
(tnh i vi h to t ti
trng tm tng khu v song song
vi to khp)
Ixx
Iyy
Izz
Ixy
Ixz
Iyz
m1
I1x
I1y
I1z
m2
I2x
I2y
I2z
-l3
m3
I3x
I3y
I3z
-(d4-l4)
m4
I4x
I4y
I4z
Vi bng tham s ng lc hc nh trong bng 2.2, thc hin chng trnh

ng lc hc ngc ta xc nh c biu thc tnh mmen/lc tc ng trn cc
khp ng ca rbt:
57
1 = (m1.l12+Iz1+m2.a12+2.m2.l2.a1.cos(q2)+m2.l22+Iz2+m3.a22+2.m3.a2.a1.cos(q2)
+m3.a12+Iz3+m4.a22+2.m4.a2.a1.cos(q2)+m4.a12+Iz4). q1_2dot +(m2.l2.a1.cos(q2) +
m2.l22+Iz2+m3.a22+m3.a2.a1.cos(q2)+Iz3+m4.a22+m4.a2.a1.cos(q2)+Iz4). q2_2dot Iz4.q4_2dot -a1.sin(q2).(m2.l2+m3.a2+m4.a2). q2_2dot .q1_dota1.sin(q2).(m2.l2+m3.a2+m4.a2).(q2_dot+q1_dot).q2_dot;
2 = (m2.l2.a1.cos(q2)+m2.l22+Iz2+m3.a22+m3.a2.a1.cos(q2)+Iz3+m4.a22+
m4.a2.a1.cos(q2)+Iz4). q1_2dot +(m2.l22+Iz2+m3.a22+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 s ng 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.m2)
Ta v c th bin thin ca mmen/lc theo thi gian trn cc khp ng
l:
0.4
Momen khop 1 (Nm)
0.2
0
-0.2
-0.4
Momen1
-0.6
Hnh 2.10.
4
6
Thoi gian(s)
10
th mmen tc ng trn khp 1
58
0.3
Momen2
Momen khop 2 (Nm)
0.2
0.1
0
-0.1
-0.2
-0.3
-0.4
4
6
Thoi gian(s)
10
Hnh 2.11. th mmen tc ng trn khp 2

-29.2
Luc 3
-29.25
Luc khop 3
(N)
-29.3
-29.35
-29.4
-29.45
-29.5
-29.55
-29.6
4
6
Thoi gian(s)
Hnh 2.12. th lc tc ng trn khp 3
10
59
0.04
Momen4
Momen khop 4 (Nm)
0.02
0
-0.02
-0.04
-0.06
4
6
Thoi gian(s)
10
Hnh 2.13. th mmen tc ng trn khp 4

Qua cc th trn ta thy rng kt qu gii phn ng hc ngc xc
nh v tr, vn tc v gia tc ca cc khp ng rbt Scara c chnh xc kh
cao (cc hnh: 7, 8, 9), sai s v tr b hn 10-14 cn sai s ca vn tc v ca gia
tc b hn 10-15, v vy m khi thay vo biu thc tnh ton gi tr ca
lc/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 n
khp ni vi khu trc l li, khi lng mi, v c mmen qun tnh i vi
khi tm Izi, (i = 1,2,...,5). Mt s thng s ca rbt c cho trong bng 2.3.
B
q4
q1
q3
C3
q2
C4
C2
g
C1
D
E
q5
Hnh 2.14.
S rbt phng 5 khu ng
60
Bn kp (khu 5) c yu cu chuyn ng theo mt qu o trn c

phng trnh l
x = 0.8 + 0.1cos(2 t )
y = 0.8 + 0.1sin( 2 t )
v bn kp phi lun to vi phng thng ng 1 gc = 1 (rad) .
Bng 2.3 Cc thng s ng lc hc ca rbt 5 khu ng
Khu
m [kg]
Iz
[kgm2]
a [m]
l [m]
1
10
2
7.5
3
5
4
4
5
1
0.11 0.10 0.03 0.02 0.02

0.55 0.50 0.45 0.40 0.20
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 s ng hc ca rbt 5 khu ng

Khu
1
2
3
4
5
q1
q2
q3
q4
q5
d
0
0
0
0
0
a
a1
a2
a3
a4
a5
0
0
0
0
0
Ta s s dng kt qu ca bi ton ng hc ngc thu nhn c v d

1.2 ca chng 1.
Vi chng trnh ROBOTDYN ta s thit lp c phng trnh vi phn
chuyn ng ca cc khu, song do phng trnh qu di m khng trnh by c
th trong lun n ny. Vi cc thng s ng lc hc cho trong bng 2.3 ta tnh
ton v v c th mmen tc ng trn cc khp ng l:
61
160
u1
140
u2
120
u3
u4
80
u5
Moment [Nm]
100
60
40
20
0
-20
-40
10
time [s]
15
20
Hnh 2.15. th cc mmen ng c theo thi gian

Da trn th cc mmen ng c, ngi k s d dng la chn cc ng
c pht ng ph hp lp trn cc khu ca rbt.
V d 2.3. Xc nh phng trnh ng lc hc ca cc khu thuc rbt
phng 6 khu ng nh hnh 2.16.
O3
x2
O4
O2
q3
x1
q4
x4
q5
q2
y0
O1
q1
O
y6
O5
O6
q6
x0
x6
x5
Hnh 2.16. Rbt phng 6 khu ng

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)
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 s ng lc ca rbt 6 khu ng
nh di y
Bng 2.5
Cc thng s DH rbt 6 khu ng
Khu
1
2
3
4
5
6
Bng 2.6
q1
q2
q3
q4
q5
q6
1
2
3
4
5
6
yC
0
0
0
0
0
0
a
a1
a2
a3
a4
a5
a6
0
0
0
0
0
0
V tr trng tm (so vi
gc to gn trn Khi
Khu
mi khp)
lng
xC
- (a1 - l1)
- (a2 - l2)
- (a3 - l3)
- (a4 - l4)
- (a5 - l5)
- (a6 - l6)
d
0
0
0
0
0
0
zC
0
0
0
0
0
0
m1
m2
m3
m4
m5
m6
Mmen qun tnh khi tng khu

(tnh i vi h to t ti trng
tm tng khu v song song vi to
khp)
Ixx Iyy
Izz
Ixy Ixz
Iyz
Ix1 Iy1
Iz1
0
0
0
Ix2 Iy2
Iz2
0
0
0
Ix3 Iy3
Iz3
0
0
0
Ix4 Iy4
Iz4
0
0
0
Ix5 Iy5
Iz5
0
0
0
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 lun
n, c th tham kho trong ph lc km theo):
Khu 1
To khi tm C1
rC1 = [l1C1
l1S1
0]
Ma trn Jacobi tnh tin ca khu 1
63
l1S1 0 0 0
rC1
J T1 =
= l1 C 1 0 0 0
q
1
0 0 0
Vn tc gc
T
1 = [0 0 q& 1 ]
Ma trn Jacobi quay
0 0 0 0 0
1
J R1 =
= 0 0 0 0 0
q&
1 0 0 0 0
Khu 2
0 0
0 0
0 0
0
0
To khi tm C2
T
rC 2 = [a 1C1 + l 2 C12 a 1S1 + l 2 S12 0]
Ma trn Jacobi tnh tin
a 1S1 l 2S12 l 2S12 0 0 0 0
rC 2
J T2 =
= a 1C1 + l 2 C12
l 2 C12 0 0 0 0
1
0
0 0 0 0
Vn tc gc khu 2
T
2 = [0 0 q& 1 + q& 2 ]
Ma trn Jacobi quay
0 0 0 0 0 0
2
JR2 =
= 0 0 0 0 0 0
q&
1 1 0 0 0 0
Khu 3
To khi tm C3
rC 3 = [a 1C1 + a 2 C12 + l 3 C123
a 1S1 + a 2 S12 + l 3S123
0]
J T3
a 1S1 a 2S12 l 3S123

r
= C 3 = a 1C1 + a 2 C12 + l 3 C123

q
a 2S12 l 3S123
l 3S123
a 2 C12 + l 3 C123
l 3 C123
0 0 0
0 0 0
0 0 0
64
Vn tc gc khu 3
3 = [0 0 q& 1 + q& 2
Ma trn Jacobi quay
0 0
3
J R3 =
= 0 0
q&
1 1
+ q& 3 ]
0 0 0 0
0 0 0 0
1 0 0 0
Khu 4
To khi tm C4
rC 4 = [a 1C1 + a 2 C12 + a 3 C123 + l 4 C1234 a 1S1 + a 2 S12 + a 3S123 + l 4 S1234
J 11 J 12 J 13 J 14 0 0
rC 4
J T4 =
= J 21 J 22 J 23 J 24 0 0
q
0
0
0 0 0
0
Vi:
J11 = -a1S1-a2S12-a3S123-l4S1234
J12 = -a2S12-a3S123-l4S1234
J13 = -a3S123-l4S1234
J14 = -l4S1234
J21 = a1C1+a2C12+a3C123+l4C1234
J22 = a2C12+a3C123+l4C1234;
J23 = a3C123+l4C1234
J24 = l4C1234
Vn tc gc khu 4
T
4 = [0 0 q& 1 + q& 2 + q& 3 + q& 4 ]
Ma trn Jacobi quay
0 0 0 0 0 0
4
JR4 =
= 0 0 0 0 0 0
q&
1 1 1 1 0 0
Khu 5
To khi tm C5
0]
65
a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + l 5 C12345

rC 5 = a 1S1 + a 2S12 + a 3S123 + a 4S1234 + l 5S12345
0
J 11 J 12 J 13 J 14 J 15 0
rC 5
J T5 =
= J 21 J 22 J 23 J 24 J 25 0
q
0
0
0
0
0 0
Vi:
J11 = -a1S1-a2S12-a3S123-a4S1234-l5S12345
J12 = -a2S12-a3S123-a4S1234-l5S12345
J13 = -a3S123-a4S1234-l5S12345
J14 = -a4S1234-l5S12345
J15 = -l5S12345
J21 = a1C1+a2C12+a3C123+a4C1234+l5C12345
J22 = a2C12+a3C123+ a4C1234+l5C12345
J23 = a3C123+ a4C1234+l5C12345
J24 = a4C1234+l5C12345
J25 = l5C12345
Vn tc gc khu 5
T
5 = [0 0 q& 1 + q& 2 + q& 3 + q& 4 + q& 5 ]
Ma trn Jacobi quay
0 0 0 0 0 0
5
J R5 =
= 0 0 0 0 0 0
q&
1 1 1 1 1 0
Khu 6
To khi tm C6
a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + a 5 C12345 + l 6 C123456
rC 6 = a 1S1 + a 2S12 + a 3S123 + a 4S1234 + a 5S12345 + l 6S123456
0
66
J T6
J 11
rC 6
=
= J 21
q
0
J 12
J 13
J 14
J15
J 22
J 23
J 24
J 25
J 16
J 26
Vi:
J11 = -a1S1-a2S12-a3S123-a4S1234-a5S12345-l6S123456
J12 = -a2S12-a3S123-a4S1234-a5S12345-l6S123456
J13 = -a3S123-a4S1234- a5S12345-l6S123456
J14 = -a4S1234-a5S12345-l6S123456
J15 = -a5S12345-l6S123456
J16 = -l6S123456;
J21 = a1C1+a2C12+a3C123+a4C1234+a5C12345+l6C123456
J22 = a2C12+a3C123+ a4C1234+ a5C12345+l6C123456
J23 = a3C123+ a4C1234+ a5C12345+l6C123456
J24 = a4C1234+ a5C12345+l6C123456
J25 = a5C12345+l6C123456
J26 = l6C123456;
Vn tc gc khu 6
6 = [0 0 q& 1 + q& 2 + q& 3 + q& 4 + q& 5 + q& 6 ]
Ma trn Jacobi quay
J R6
0 0 0 0 0 0
6
=
= 0 0 0 0 0 0
q&
1 1 1 1 1 1
Ma trn tensor qun tnh

I x1
I1 = 0
0
I x 4
I4 = 0
0
I y1
0
0
I y4
0
0
I x 2
0 , I2 = 0
0
I z1
0
I x 5
0 , I5 = 0
0
I z 4
0
I y2
0
0
I y5
0
0
I x 3
0 , I3 = 0
0
I z 2
0
I x 6
0 , I6 = 0
0
I z 5
0
I y3
0
0
I y6
0
0
0 ,
I z 3
0
0
I z 6
67
Bng 2.7 Cc gi tr thng s ng lc hc ca rbt 6 khu ng

Khu
m (kg)
Iz (kgm2)
a (m)
l (m)
1
10
0.11
0.40
0.20
2
5
0.10
0.35
0.16
3
2
0.03
0.30
0.15
4
1
0.02
0.20
0.1
5
1
0.01
0.10
0.05
6
1
0.01
0.1
0.05
Khi rbt 6 khu ng c cc gi tr thng s ng lc hc nh trong bng

2.7 ta c c cc kt qu tnh ton mmen cc khp ng c trnh by trn cc
hnh t 2.17 n 2.22. Da trn cc th ny, ngi k s d dng la chn cc
ng c pht ng lp trn cc khu ca rbt.
75
Momen khop 1 (Nm)
Momen1
70
65
60
55
4
6
Thoi gian(s)
10
Hnh 2.17. Mmen trn khp ng th 1

32
Momen khop 2 (Nm)
31
30
29
28
27
Momen2
26
25
4
6
Thoi gian(s)
10
68
10
Momen khop 3 (Nm)
9
8
7
6
Momen3
5
4
3
4
6
Thoi gian(s)
10

1
Momen khop 4 (Nm)
0.5
0
-0.5
-1
-1.5
Momen4
-2
-2.5
4
6
Thoi gian(s)
10

-0.2
Momen5
Momen khop 5 (Nm)
-0.25
-0.3
-0.35
-0.4
-0.45
-0.5
-0.55
4
6
Thoi gian(s)
10
69
0.6
Momen6
Momen khop 6 (Nm)
0.55
0.5
0.45
0.4
0.35
0.3
0.25
4
6
Thoi gian(s)
10
2.4. Kt lun chng 2

Da trn thut ton hiu chnh gia lng vc t ta suy rng trong
chng 1, y trnh by mt thut ton tnh ton lc/mmen ca cc khu dn
ng trong khng gian thao tc. T gip cho ngi k s thit k chn cc ng
c lp 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 hc
ngc l rt nh). Cc v d trnh by trong chng l cc v d mi, cha c trong
cc sch chuyn kho.
Vic gii bi ton ng lc hc ngc l mt khu quan trng trong vic
iu khin chuyn ng ca rbt. Vn ny s c trnh by trong chng 3.
Chng 3
IU KHIN TRT RBT D DN NG
DA 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 nh iu khin PD, iu
khin PID, iu khin trt, iu khin m [59, 63, 66, 69, 70, 72]. Trong
chng ny gii thiu s lc v khi nim n nh chuyn ng v iu khin
chuyn ng rbt. Trng tm ca chng l trnh by iu khin trt rbt d
dn ng da trn thut ton hiu chnh gia lng vc t ta suy rng [19,
50, 51].
3.1 C s l thuyt n nh Lyapunov
n 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 trc
quan, mt h gi l n nh nu n xut pht t mt im no ln cn im
hot ng th n s lun xung quanh im hot ng . L thuyt n nh
Lyapunov c nu hai phng php phn tch n nh l phng php tuyn tnh
ho v phng php trc tip. Trong phm vi ca lun n ny, ta ch s dng
phng php trc tip (hay cn gi l phng php hm Lyapunov).
3.1.1 H phi tuyn v cc im cn bng
a. H phi tuyn
Mt h ng lc phi tuyn l h c phng trnh trng thi c miu t
bng h cc phng trnh vi phn phi tuyn sau
y& = f (y , t )
(3.1)
Trong :
f R n l hm vct phi tuyn,
y R n l vct trng 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 ca
mt h khng iu khin. Tuy nhin, n chnh l phng trnh trng thi tng
qut dng cho bi ton phn tch ca tt c cc h. Xt i tng iu khin c
phng trnh trng thi
y& = f (y, u, t )
Nu chn lut iu khin l u = g (y, t ) , ta thu c h y& = f [y , g(y , t ), t ]
do ta c th vit li di dng (3.1).
71
H tuyn tnh l trng hp ring ca h phi tuyn. Lc (3.1) c

vit di dng:
y& = A( t )y , vi A(t) l ma trn nxn.
Ty thuc vo vic h c ph thuc hay khng ph thuc vo bin thi
gian m h phi tuyn c chia thnh h tnm v h phi tnm theo nh
ngha di y.
nh ngha 3.1. H phi tuyn (3.1) c gi l tnm nu f khng ph
thuc tng minh vo thi gian, ngha l phng trnh trng thi c dng.
y& = f (y )
(3.1b)
Ngc li, ta gi l h phi tnm.

Ch rng, nh ngha trn cng nh phng trnh trng thi (3.1) p
dng cho mt h c phn hi. Do , tnh tnm hay phi tnm l ph thuc
vo c i tng iu khin ln b iu khin. V d, nu i tng iu khin
c th coi gn ng l tnm song b iu khin li phi tnm th c h kn
s l phi tnm.
S khc nhau c bn gia h tnm v h phi tnm l qu o trng
thi ca h tnm khng ph thuc vo gi tr thi im u. Do , qu trnh
phn tch tnh n nh ca h s d dng hn. Trong ti liu ny, n gin ch
trnh by vic nghin cu n nh ca cc h tnm.
b. im cn bng
nh ngha 3.2. Mt trng thi y* c gi l trng thi cn bng ca
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 th i bin a im cn bng ca h v trng vi
gc trng thi nh sau:
t x = y y* ri thay vo biu thc (3.1b), ta c:
x& = y& y& * = f (x + y*) = g(x)
K hiu li f(x) = g(x), t phng trnh trn ta suy ra :
(3.2)
x& = f (x)
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 qu o, ta quan tm n tnh n nh ca h quanh mt qu o ph
thuc 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 s n nh
ca mt h phi tnm quanh im cn bng.
72
3.1.2 Khi nim n nh
Trong 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 ca h 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 khng n nh
nh ngha 3.3. Trng thi cn bng x* = 0 c gi l n nh nu
R > 0 , r > 0 sao cho, nu ||x(0)|| < r th ||x(t)|| < R vi t 0 . Ngc li,
im x* = 0 l im khng n nh (hnh 3.1).
x1
O
Br
1
BR
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: qu o ca h c
th tin n gn im gc mt cch ty , min l v tr ban u gn gc mt
khong nh.
Ch rng, trong h phi tuyn, h khng n nh khng c ngha l h
lun lun di chuyn ra xa gc n v cng. Trn hnh 3.3, qu o h di chuyn
t im gn gc ty cui cng tim cn theo mt chu trnh. (Nu xt mt hnh
trn nm bn trong chu trnh th cui cng h s i ra khi hnh trn ).
Hnh v cng ni ln s khc nhau c bn gia n nh v khng n nh. C
th qu o ca h vn nm quanh im cn bng mt khong cch hu hn
no , nhng n khng th gn im cn bng mt cch ty .
b. n nh tim cn v n nh s m
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.
73
Nu h l n nh, ty theo cng ca nhiu (v tr sai lch ban u ca v

tinh), v tinh s n nh v xung quanh im ban u. Tuy nhin, m bo
cht lng, v tinh phi tin dn v v tr ban u ca n. V th, cn phi t ra
khi nim v n nh tim cn.
nh ngha 3.4. Mt im cn bng x* = 0 l n nh tim cn (hnh
3.2) nu n n nh, v ngoi ra, r > 0 m ||x(0)|| < r cho ta x( t ) 0 khi
t .
x1
O
3
Br
BR
x2
Sr
SR
Hnh 3.2 im gc O l im n nh tim cn

Lc ny, trn Hnh 3.2, min cu Br c gi l min ht (domain of
attraction) ca im cn bng. V tt c cc qu o xut pht t bn trong Br
cui cng u hi t v im gc.
Mt im cn bng ch n nh Lyapunov nhng khng n nh tim cn
th c gi l n nh bin (marginally stable).
Nh vy, im cn bng ca h l n nh tim cn nu n l n nh v
l hi t. Tuy nhin, mt im cn bng c th hi t nhng khng nht thit l
n nh. Hnh 3.3 minh ho mt trng hp nh vy. Mi qu o xut pht t
x2
C
r=1
x1
Hnh 3.3.
Trng thi hi t khng n nh
74
mt im khc 0 nm bn trong hnh trn 1 n v tin n ng cong C trc

khi hi t v gc. Do , n khng n nh theo ngha Lyapunov.
Trong cc ng dng thc t, nu ch dng khi nim n nh tim cn l
cha . V n nh tim cn c th hi t v 0 trong mt khong thi gian v
cng. Do , ta cn c cc khi nim khc phn nh tc hi t v 0 ca
h, l khi nim n nh s m (exponential stability).
nh ngha 3.5. Mt im cn bng x* = 0 l n nh s m nu tn ti

hai gi tr dng v m:
t 0, x( t ) x(0) e t trong hnh cu Br no bao quanh gc.
Gi tr c gi l tc ca hi t s m.
Khi nim n nh s m l khi nim cht hn khi nim n nh tim
cn.
c. n nh cc b v n nh ton cc
Cc khi nim a ra phn trc nhm mc ch phn tch hnh vi cc
b ca h xung quanh im cn bng (cc trng thi ban u xut pht gn im
cn bng). V cn phn tch tnh n nh ca h ti cc gi tr trng thi ban u
cch xa im cn bng, nn cn a ra khi nim n nh ton cc.
Nu tnh n nh tim cn (hoc n nh s m) l ng vi bt k trng
thi ban u no, im cn bng c gi l n nh tim cn (hoc n nh s
m) ton cc.
Trong h tuyn tnh, c ba khi nim: n nh tim cn, n nh bin, v
khng n nh. n nh tim cn trong h tuyn tnh lun ng ngha vi n
nh s m ton cc. Khng n nh trong h tuyn tnh lun ng ngha vi
qu o ca h tin ra v cng theo thi gian.
3.1.3 Phng php trc tip Lyapunov
tng ca phng php trc tip Lyapunov l xut pht t vic m
rng khi nim c nng trong c hc. Nu mt c h chu hao ph nng lng,
th sau mt khong thi gian, h s tr v im cn bng. Vi mt h tng qut,
ta s tm cch a ra mt hm tng t c nng v phn tch s bin i theo
thi gian ca hm .
a. Hm xc nh dng v hm Lyapunov
nh ngha 3.6. Mt hm v hng v lin tc V(x) c gi l hm
xc nh dng cc b nu V(0) = 0 v trong mt min cu BR0,
x 0 V(x) > 0 . Nu V(0) = 0 v tnh cht trn ng trn ton khng gian
trng thi th V(x) c gi l hm xc nh dng ton cc.
Hm V lc ny l hm c cc tiu duy nht ti x = 0. Trn thc t, nu ta
cho mt hm bt k c cc tiu duy nht trong mt min cu no . Bng cch
75
thm vo mt hng s, ta c th xy dng c mt hm xc nh dng cc b

trong min cu . o hm ca hm theo thi gian vn khng thay i.
Tng t, ta a ra mt s khi nim lin quan.
Hm V(x) l hm xc nh m nu V(x) l hm xc nh dng.
Hm V(x) l hm bn xc nh dng nu V(0) = 0 v V(x ) 0; x 0 .
Hm V(x) l hm bn xc nh m nu V(x) l hm bn xc nh dng.
nh ngha 3.7. Trong mt min cu BR0, nu hm V(x) l xc nh

& (x) theo thi gian da
dng c cc o hm ring lin tc v c o hm V
& (x) 0 ) th V(x) s c
trn phng trnh vi phn (3.2) l bn xc nh m ( V
gi l hm Lyapunov ca h (3.2).
Ta c th s dng cc nh l sau xt v s n nh ca h thng.
& (x) c hiu l o hm theo thi gian ca hm
Trong cc nh l ny th V
V(x) da vo h phng trnh vi phn (3.2).
b. nh l Lyapunov cho n nh cc b
Nu trong mt min cu ng BR0, tn ti hm v hng V(x) c o
hm ring cp 1 lin tc tho mn:
V(x) xc nh dng (cc b trong BR0)
& (x) bn xc nh m (cc b trong BR0)
V
th im cn bng x* = 0 l n nh.
Nu V(x) xc nh dng v V& (x) l xc nh m (cc b trong BR0) th
im cn bng x* = 0 s n nh tim cn.
c. nh l Lyapunov cho n nh ton cc
Gi s tn ti mt hm v hng V(x) c o hm bc nht lin tc v
tho mn
V(x) xc nh dng
& (x) xc nh m
V
V(x) khi x
th im cn bng ti gc l n nh tim cn ton cc.
n nh ton cc ngoi vic m rng min cu ng BR0 ra ton b
khng gian trng thi ta cn cn mt tnh cht ca hm V(x), l tnh cht
khng b chn theo mi hng (radially unbounded). Tnh cht ny nhm mc
ch bo m ng chu tuyn (contour) V(x) = V lun l mt ng cong
ng. Hnh 3.4 minh ho trng hp ng chu tuyn l ng cong m. C
th thy, mc d trng thi i theo ng gim ca V(x), x vn phn k ra xa
im gc.
76
Ch :
Vi mt h cho, c th tn ti nhiu hm Lyapunov khc nhau.
Vi mt h cho, vic la chn hm Lyapunov ny c th cho ta kt
qu chnh xc hn khi chn cc hm khc.
nh l Lyapunov l iu kin , ngha l nu ta s dng mt hm
khng tho mn tt c cc iu kin ca nh l th ta cha th kt
lun c bt c iu g v tnh n nh hay khng n nh ca h.
x2
x(t)
x1
V(x) = V3
V(x) = V2
V(x) = V1
V1 > V2 > V3
Hnh 3.4. S phn k trng thi khi chuyn ng
dc theo cc ng nng lng thp
d. nh l tp bt bin (invariant set)
iu kin v n nh tim cn ca h phi tuyn l mt iu kin rt quan
trng. Tuy nhin, iu kin ny i hi tnh xc nh m ca V& (x) , trong khi
trn thc t, ta thng ch nhn c V& (x) l bn xc nh m. Nh s ng gp
ca La Salle trong nh l tp bt bin, ta vn c th kt lun v tnh n nh
tim cn ca h. Khi nim tp bt bin l s khi qut ho ca khi nim im
cn bng.
Tp G gi l tp bt bin (invariant set) ca mt h ng hc nu mi
qu o ca h xut pht t mt im trong G s lun trong G.
im cn bng, min ht u l tp bt bin. Mt tp bt bin tm thng
khc l ton b khng gian trng thi. Vi h tnm, bt k qu o no trong
khng gian trng thi u l tp bt bin.
77
e. nh l tp bt bin cc b
Xt h tnm dng x& = f (x) .
Vi f l mt hm lin tc, V(x) l hm v hng c o hm cp 1 lin
tc. Gi s
Vi l > 0 no , min l c nh ngha bi V(x) < l l b chn
& (x) 0 x l
V
& (x) = 0 v M l tp bt bin ln
Gi R l tp cc im thuc l vi V
nht trong R. Th mi nghim x(t) xut pht t trong l u hi t v M khi

t .
Ch : M l tp bt bin ln nht theo ngha n l hp ca tt c cc tp
& (x) = 0 th V
& (x) s lun bng
bt bin trong R. Nu R l tp bt bin (mt khi V
0) th M = R
tng ca nh l trn l hm Lyapunov V dn dn s khng gim na
&
(tc V(x) hi t n 0).
Hnh di y minh ho khi nim s hi t n tp bt bin M
V=l
l
x1
xo
R
M
x2
Hnh 3.5.
S hi t n tp bt bin ln nht M
H qu
Xt h tnm (3.2) vi f lin tc, V(x) l hm v hng c o hm

ring lin tc. Gi s trong ln cn ca im gc
V(x) l hm xc nh dng cc b
& (x) bn xc nh m
V
Tp R nh ngha bi V& (x) = 0 ch gm qu o x = 0.
Th im cn bng x* = 0 l n nh tim cn.
Ngoi ra, min lin thng ln nht l (c nh ngha bi V(x) < l) nm
trong l mt min ht ca im cn bng.
78
Ch :
& (x) trong nh l n
H qu trn thay th iu kin xc nh m ca V
nh tim cn cc b ca Lyapunov bng iu kin bn xc nh m. Tuy
nhin kt hp vi iu kin th 3 v qu o trong R.
Min lin thng ln nht l l mt min ht ca im cn bng, n
khng nht thit l min ht duy nht, v hm V khng phi l duy nht.
Tp khng cn l min ht. nh l trn cng khng bo m l l
tp bt bin. C th c qu o xut pht bn ngoi l v bn trong
nhng cui cng li ra bn ngoi .
f. nh l tp bt bin ton cc
Xt h tnm (3.2) vi f lin tc, V(x) l hm v hng c o hm cp
1 lin tc. Gi s :
& (x) 0 trong ton khng gian trng thi
V
V(x) khi x .
& (x) = 0 v M l tp bt bin ln nht
Nu R l tp hp cc im c V
trong R, th tt c cc nghim u hi t tim cn ton cc v M khi t

3.2 Bi ton iu khin chuyn ng ca rbt
Nhim v ca bi ton iu khin chuyn ng ca rbt l m bo

khu thao tc chuyn ng bm theo qu o cho trc trong khng gian thao
tc
q3
q2
xP
q1
Hnh 3.6
R0
S hot ng ca rbt
Cho bit chuyn ng ca khu thao tc xd. T ta xc nh c quan h:

x d = f (q) q d = f 1 (x d )
(3.3)
c th c c chuyn ng ca im thao tc P theo phng trnh
nh trc, ta c th dng cc dng iu khin di y:
79
- H thng iu khin trong khng gian khp (hnh 3.7),

- H thng iu khin trong khng gian thao tc (hnh 3.8).
xd ng hc qd
ngc
B iu
khin
C cu dn
ng
ng
c
Tay
my
q
Cm
bin
Hnh 3.7.
xd
S tng qut ca h thng iu khin khng gian khp
B iu
khin
C cu dn
ng
ng c
Bn kp
rbt
Cm bin
Hnh 3.8.
S tng qut ca h thng iu khin khng gian thao tc
a. Bi ton iu khin trong khng gian khp

Bi ton ny c phn thnh 2 bi ton nh
Bi ton ng hc ngc: Cho xd , tm q = f 1 (x d )
H thng iu khin trong khng gian khp c thit k m bo v
tr khp q lun bm theo v tr mong mun qd, sao cho lng sai lch
q q d min
u im ca phng php ny l b iu khin tc ng trc tip n h
thng truyn ng ca cc khp. Nhc im ca phng php l h thng iu
khin ny kh m bo chnh xc ca v tr khu thao tc x x d min, do
s tn ti cc sai lch trong c cu dn ng (khe h ca cc khp, ma st v.v),
hn na thiu thng tin v x x d trong qu trnh iu khin
b. Bi ton iu khin trong khng gian thao tc

H thng iu khin trong khng gian thao tc c chc nng lm cho sai
s gia x v xd bng khng.
x x d min
80
Trong x d l vc t v tr khu thao tc mong mun, x l vc t phn

hi v tr thc t ca khu thao tc.
u im ca h thng iu khin ny l n tc ng trc tip ti cc bin
ca khng gian thao tc x. Nhc im l trong qu trnh iu khin ta phi
ng thi gii bi ton ng hc ngc, do khi lng tnh ton ln, thi
gian iu khin lu.
3.3 iu khin trt rbt d dn ng
Phng trnh ng lc hc ca rbt dng chui n bc t do c dng
&& + C(q, q& )q& + g(q) + d =
M (q)q
(3.4)
Trong :
M(q)
- ma trn khi lng c nn,
C(q, q& )q& - vct c n cha cc lc crilis v lc ly tm,
g(q)
- vct c n cha cc lc do trng trng,
d
- vct cha cc lc/mmen do kch ng nhiu,
- vct cha cc lc/mmen ca ng c dn ng.

S khi iu khin rbt c dng
d
ng
lc hc
q d (t)
Hnh 3.9.
Rbt
q(t)
S iu khin rbt
Ta a vo cc vc t sai s bm ca vc t ta suy rng v ca vn

tc suy rng
e( t ) = q ( t ) q d ( t )
(3.5)
e& ( t ) = q& ( t ) q& d ( t )
theo [63], s dng phng php iu khin trt ta a vo vc t sai s suy

rng nh sau
s = e& ( t ) + e( t )
(3.6)
trong
= diag(1 , 2 ,..., n ) , i > 0
(3.7)
Mc tiu ca bi ton iu khin l e( t ) 0 c chuyn sang hm mc
tiu s( t ) 0 khi t
e( t ) 0 s( t ) 0 khi t
(3.8)
81
Nu s = 0 th h phng trnh vi phn (3.6) c dng

(i = 1 n)
e& i ( t ) + i e i ( t ) = 0
T suy ra
de i ( t )
de
= i e i ( t ) i = i dt
dt
ei
Tch phn phng trnh trn ta c
e i ( t ) = e i (0)e t
Do nu s i ( t ) 0 th e i ( t ) 0 khi t
By gi ta tm lut iu khin sao cho s i ( t ) 0 khi t
Ta t
q& r ( t ) = q& d ( t ) e( t )
i
suy ra
(3.9)
(3.10)
q& ( t ) q& r ( t ) = q& ( t ) q& d ( t ) + e( t )

(3.11)
= e& ( t ) + e( t ) = s( t )
t ta c
&&( t ) q
&& r ( t )
s& ( t ) = q
(3.12)
tm lut iu khin, ta chn hm Lyapunov nh sau
1
(3.13)
V = s T M (q)s
2
o hm V theo thi gian t ta c
& = s T M (q)s& + 1 s T M
& (q)s
V
(3.14)
2
T (3.4) v ch n (3.12) ta c
&&( t ) q
&& r ( t )) + M (q)q
&& r ( t ) + C(q, q& )q& + g (q) + d =
M (q )(q
&& r d
M (q )s& = C(q, q& )q& g (q) M (q )q
(3.15)
T (3.11) ta suy ra
q& (t ) = s + q& r (t )
(3.16)
Do ta c
C(q, q& )q& = C(q, q& )s + C(q, q& )q& r
(3.17)
Th (3.17 ) vo (3.15) c
&& r C(q, q& )q& r g(q) d
M(q)s& = C(q, q& )s M(q)q
(3.18)
Thay (3.18) vo (3.14) ta c
1 &
& = s T [ M (q)q
&& r C(q, q& )q& r g(q) d] + s T M
(q) C(q, q& )s (3.19)
V
2
1 &
(q)s s T C(q, q& )s = 0
Do tnh cht s T M
2
82
Ta suy ra
& = s T [ M (q)q
&& r C(q, q& )q& r g (q) d ]
(3.20)
V
Cn c vo (3.20) ta chn lut iu khin (mmen cn thit m bo
chuyn ng theo chng trnh) nh sau
(q, q& )q& + g (q) K s K sgn(s)
(q)q
&& r + C
(3.21)
=M
r
pd
s
Trong :
T
sgn(s) = [sgn(s 1 ), sgn(s 2 ),..., sgn(s n )] , v Kpd, Ks l cc ma trn thc i xng
xc nh dng, K pd = K Tpd > 0 , K s = K sT > 0
n gin ta chn hai ma trn ny c dng l cc ma trn ng cho
nn
} v K s = diag{k11s , k s22 ,..., k snn }
K pd = diag{k 11pd , k 22
pd ,..., k pd
Vi cch chn (3.21), h thc (3.20) tr thnh
~
~
& = s T K pd s s T K s sgn(s) + s T d + s T M
&& r + C(q, q& )q& r + ~
V
(q)q
g (q) =
i =1
i =1
i =1
i =1
i =1
= k (pdii )s i2 k s(ii ) | s i | + s i (d i + i ) k (pdii )s i2 (k s(ii ) | d i + i |) | s i |

vi cc sai lch gia m hnh thc s dng trong (3.4) v cc thng s m hnh
s dng trong b iu khin (3.21) nh sau
~
(q)
M (q) = M (q) M
~
(q, q& )
(3.22)
C(q, q& ) = C(q, q& ) C
~
g (q) = g (q) g (q)
~
~
&& r + C(q, q& )q& r + ~
v i = M (q)q
g (q) i
& 0 th ta phi chn cc phn t ca Ks sao cho
Nh vy m bo V
k s(ii ) > d i + i . Kpd ch l thnh phn iu khin PD c a thm vo rt

ngn thi gian chuyn tip.
Do c im ca hm sgn(si) l khng lin tc ti gi tr si = 0, do b
iu khin trn s xy ra hin tng chattering. Mt trong nhng cch khc
phc thng thy l lm trn tnh khng lin tc ca b iu khin bng cch
thay hm du sgn(s) bng hm bo ho (hnh 3.10).
sat (s / ) = [sat (s1 / ), sat (s 2 / ),..., sat (s n / )]T .
Sai s iu khin trong trng hp ny phi chp nhn tng ln. Lc ny,
thnh phn k (pdii ) v k s(ii ) trong phm vi sai s suy rng si nm trong khong u
ng vai tr l b iu khin PD.
83
x
Vi sat ( x ) =
sgn( x )
x <1
x 1
Hnh 3.10. Hm sat

Ngoi ra, ngi ta c th dng hm arctan (c cn chnh t l) thay
cho hm sign (hnh 3.11)
Hnh 3.11. Hm arctan

Ta c th s dng thut ton s trong vic gii bi ton iu khin rbt d
dn ng trong Matlab/Simulink vi s thut gii nh trn hnh v 3.12.
84
Cho bit
&& + C(q, q& )q& + g (q) + d =
M (q)q
&& d ( t ), q& d ( t ), q d ( t ), q( t 0 ), q& ( t 0 )
q
T
, K pd , K s , T, h =
N
tk : =t0
e( t k ) = q ( t k ) q d ( t k )
e& ( t k ) = q& ( t k ) q& d ( t k )
s( t k ) = e& ( t k ) + e( t k )
In, v th q( t k ) , q& ( t k )
e( t k ), s( t k ) ; k=0,1,...N)
q& r ( t k ) = q& d ( t k ) e( t k )
&& r ( t k ) = q
&& d ( t k ) e& ( t k )
q
(q( t ), q& ( t ) )q& ( t ) +
(q( t k ) )q
&& r ( t k ) + C
(t k ) = M
k
k
r
k
+ g (q( t k ) ) K pd s( t k ) K s sgn (s( t k ) )
tk+1 := tk + h
Gii PTVP chuyn ng t t = t k t = t k +1 = t k + h
T
vi bc tch phn t =
N
&& + C(q, q& )q& + g (q) + d( t ) = ( t k )
M (q)q
Thu c q( t k +1 ) , q& ( t k +1 )
k:=k+1
Sai
kN
ng
KT THC
Hnh 3.12
S tnh ton v m phng iu khin rbt
85
3.4 Cc bi ton p dng

V d 3.1. iu khin rbt phng 4 khu ng theo yu cu:
Chuyn ng ca im thao tc P c dng :
xP = 0.8+0.1cos(2t) (m); yP = -0.8+0.1sin(2t) (m)
Cc thng s ng lc rbt 4 c cho trong bng 3.1
y0
y2
y1
O
T
C
q2
T1
C
O
y4
x2
q3
x1
Bng 3.1
O
P(x,y,z
q4
x3
x0
q1
Hnh 3.13.
y3 T4
C
T3
x4
Rbt 4 khu ng
Khu
1
2
3
4
m[kg]
10
5
2
1
Iz[kgm2]
0.11
0.10
0.03
0.02
a[m]
0.25
0.30
0.45
0.40
l[m]
0.15
0.16
0.19
0.20
Chn bc thi gian cho bi ton ng hc ngc v chu k ly mu ca

b iu khin h = Tc = 0.001 (s), sai s cho php trong bi ton ng hc ngc
= 10 6 (rad).
Cc thng s ca b iu khin = diag(100,100,50,50) ,
K s = diag(30,10,10,5) , K pd = diag(2,2,2,2) , = 0.2. y, ta a thm khu
bo ha vo b iu khin vi mc ch gii hn mmen iu khin ca ng
c lun nm trong gii hn -100 100 (Nm).
86
th nhiu d(t) trn cc khp ng c cho nh cc hnh di

3
Nhieu1
Nhieu khop 1 (Nm)
2
1
0
-1
-2
-3
4
6
Thoi gian(s)
10
Hinh 3.14. th cc nhiu tc ng trn khp 1
6
Nhieu2
Nhieu khop 2 (Nm)
4
2
0
-2
-4
4
6
Thoi gian(s)
10
87
Nhieu khop 3 (Nm)
3
2
1
0
-1
-2
-3
Nhieu3
0
4
6
Thoi gian(s)
10
Nhieu khop 4 (Nm)
2
1
0
-1
-2
-3
-4
Nhieu4
0
4
6
Thoi gian(s)
10
88
p dng phn mm Matlab ta tnh c mmen pht ng t ti cc

khp (cc hnh 3.18 n 3.21). Trn cc hnh t 3.22 n 3.25 l th cc sai
s suy rng si. Trn cc hnh 3.26 v 3.27 l th ta chuyn ng thc ca
khu thao tc x(t), y(t) v th chuyn ng mong mun xd, yd theo chng
trnh. Ta thy s chnh lch ch xy ra trong khong thi gian t 0 n 1 giy.
Sau qu o iu khin bm st qu o theo chng trnh.
100
momen1
Momen khop 1 (Nm)
80
60
40
20
0
-20
4
6
Thoi gian(s)
10
Hnh 3.18. Mmen iu khin ti khp 1 (Nm)

100
momen2
Momen khop 2 (Nm)
80
60
40
20
0
-20
4
6
Thoi gian(s)
10
89
100
momen3
Momen khop 3 (Nm)
80
60
40
20
0
-20
4
6
Thoi gian(s)
10

30
momen4
Momen khop 4 (Nm)
25
20
15
10
5
0
-5
4
6
Thoi gian(s)
10
90
Sai so suy rong 1 (rad/s)
s1
0
-5
-10
-15
4
6
Thoi gian(s)
10
Hnh 3.22. th sai s suy rng s1
15
s2
10
5
0
-5
-10
4
6
Thoi gian(s)
10
91
5
0
-5
s3
-10
-15
-20
-25
4
6
Thoi gian(s)
10

5
s4
0
-5
-10
-15
4
6
Thoi gian(s)
10
92
Vi tri tren truc x (m)
0.9
0.8
0.7
0.6
xd
x
0.5
4
6
Thoi gian(s)
10
Hnh 3.26. th ta x(t) trong khng gian thao tc
-0.65
yd
y
Vi tri tren truc y (m)
-0.7
-0.75
-0.8
-0.85
-0.9
4
6
Thoi gian(s)
10
Hnh 3.27. th ta y(t) trong khng gian thao tc
93
V d 3.2.
iu khin Rbt Scara theo yu cu:
Chuyn ng ca im thao tc P c dng :

xP = 0.3+0.8cos(t) (m); yP = 0,3+0.08sin(t) (m);
= sin( 2t ) (rad).
2
Cc thng s ng lc ca rbt c cho trong bng 3.2
Hnh 3.28.
Rbt Scara
Bng 3.2 Cc thng s ng lc rbt Scara

Khu
1
2
3
4
m[kg]
10
5
2
1
Iz[kgm2] a[m] d[m]

0.21
0.22
0.20
0.40
0.18
0.15
0.09
l[m]
0.15
0.16
0.19
0.04
94
th nhiu d(t) nh hnh di

3
Nhieu1
Nhieu khop 1 (Nm)
2
1
0
-1
-2
-3
Hnh 3.29.
4
6
Thoi gian(s)
10
th nhiu trn khp ng 1
6
Nhieu2
Nhieu khop 2 (Nm)
4
2
0
-2
-4
Hnh 3.30.
4
6
Thoi gian(s)
10
95
Nhieu khop 3 (N)
3
2
1
0
-1
-2
-3
Nhieu3
0
Hnh 3.31.
4
6
Thoi gian(s)
10
Nhieu khop 4 (Nm)
2
1
0
-1
-2
-3
-4
Nhieu4
0
Hnh 3.32.
4
6
Thoi gian(s)
10
Chn bc thi gian cho bi ton ng hc ngc v chu k ly mu ca

b iu khin h = Tc = 0.001 (s), sai s cho php trong bi ton ng hc
ngc = 10 6 (rad).
Cc thng s ca b iu khin = diag(5,5,5,5) , K s = diag(10,10,40,10) ,
K pd = diag(2,2,2,2) , = 0.05.
96
T ta thu c th ta ca bn kp trong khng gian thao tc, sai

s suy rng s v mmen iu khin ti cc khp.
10
Momen khop 1 (Nm)
momen1
-5
4
6
Thoi gian(s)
10

15
Momen khop 2 (Nm)
momen2
10
-5
4
6
Thoi gian(s)
10
97
10
luc 3
Luc khop 3 (N)
0
-10
-20
-30
-40
4
6
Thoi gian(s)
10
Hnh 3.35. Lc iu khin ti khp 3 (N)

10
momen4
Momen khop 4 (Nm)
5
0
-5
-10
-15
-20
4
6
Thoi gian(s)
10
98
0.2
s1
0
-0.2
-0.4
-0.6
-0.8
4
6
Thoi gian(s)
10
Hnh 3.37. Sai s suy rng s1

0.2
s2
0
-0.2
-0.4
-0.6
-0.8
4
6
Thoi gian(s)
10
99
0
-0.2
-0.4
-0.6
-0.8
-1
4
6
Thoi gian(s)
10
5
s4
Sai so suy rong 3 (m/s)
s3
4
3
2
1
0
-1
4
6
Thoi gian(s)
10
100
0.45
xd
Vi tri tren truc x (m)
0.4
0.35
0.3
0.25
0.2
4
6
Thoi gian(s)
10
Hnh 3.41. th ta x(t) trong khng gian thao tc
Vi tri tren truc y (m)
0.4
0.35
0.3
0.25
yd
y
0.2
4
6
Thoi gian(s)
10
Hnh 3.42. th ta y(t) trong khng gian thao tc
101
0.3
Vi tri tren truc z (m)
0.25
0.2
zd
0.15
0.1
0.05
0
-0.05
4
6
Thoi gian(s)
10
Hnh 3.43. th ta z(t) trong khng gian thao tc

2
phid
Vi tri goc quay (rad)
phi
1
-1
-2
4
6
Thoi gian(s)
10
Hnh 3.44. th gc quay (t) ca bn kp trong khng gian thao tc (rad)

Tng t nh v d trc, qua cc hnh v (t hnh 3.29 n hnh 3.44)
ta thy thi gian cn thit bn kp ca rbt bm c qu o mong mun
l vo khong 1 giy v mc d c cc sai s v cu trc v c cc nhiu trn
cc khp nhng rbt vn bm theo qu o mong mun.
102
3.5 Kt lun chng 3

Da trn thut ton hiu chnh gia lng vc t ta suy rng trnh
by chng 1, trong chng ny trnh by vic vn dng phng php iu
khin trt vo bi ton iu khin chuyn ng rbt d dn ng trong khng
gian khp. Chng ti chn phng n iu khin rbt d dn ng trong
khng gian khp v phng n ny c thi gian iu khin nhanh hn so vi
phng n iu khin trong khng gian thao tc do khng phi gii bi ton
ng hc ngc trong qu trnh iu khin, do ph hp hn vi iu khin
thi gian thc. Kt qu mi ca chng ny l trong phn gii bi ton ng
hc ngc s dng thut ton hiu chnh vc t ta suy rng. Kt qu gii
bi ton ng hc ngc l u vo ca bi ton iu khin chuyn ng trong
khng gian khp. Ngoi ra cc v d trnh by trong chng ny l cc v d
mi.
Chng 4
BI TON NGC NG HC, NG LC HC
V IU KHIN TRT RBT O BKHN-MCX-04
phc v cho lun n mt rbt o c thit k ch to ti B mn
C kh chnh xc v Quang hc Trng i hc Bch khoa H ni, ch tr thit
k, ch to l TS. Nguyn Vn Vinh. Rbt o ny c quy nh gi l rbt
o BKHN-MCX-04. Chng ti c tham gia mt phn thit k ch to v tham
gia thc hin cc th nghim ti Trng i hc Bch khoa H ni. Trn c s
rbt ch to, trong chng ny p dng cc thut ton trnh by trong cc
chng 1, 2 v 3 tnh ton phn tch ng hc ngc, ng lc hc ngc v
iu khin chuyn ng rbt o BKHN-MCX-04.
4.1 Tng quan v phng php o chnh xc hnh hc ca chi tit my
Trong lnh vc o lng, kim tra th o lng v kim tra t ng ang
rt pht trin. a nhng thnh tu ca cc ngnh in t v tin hc p dng
vo vic ghp ni gia my tnh v my cng tc cho php chuyn thng s
liu o vo my tnh x l v a ra kt qu ca php o mt cch nhanh
chng. Cc thit b o ta nh: My o ta cc, ta c cc
hon thin cc kh nng o lng vn c ca chng, c bit trong cc php o
gin tip c khi lng x l thng tin ln. Vi kh nng xc nh kch thc,
hnh dng, v tr mt cch d dng, vi chnh xc cao trong mt khong thi
gian ngn v kh nng tip cn i tng phong ph, vi nhng phn mm x
l thch hp, c th o nhng b mt phc tp. My o ta gp phn quan
trng trong vic m bo nng cao cht lng sn phm v em li hiu qu
cao.
Tuy nhin, trong cc trng hp sau y th my o ta cc khng
th p ng c:
- Nhng vt o c kch thc ln, nng, khng th di chuyn n bn
o ca my o ta cc.
- Khi cn o nhng chi tit lp rp vo my cc cng trnh m khng
th no tho ra c.
- o cc v tr c s hn ch v khng gian nh o bn trong khoang
my, o trong nhng chi tit dng ng v.v
Cc rbt o khc phc c nhng hn ch nu trn. Vi kt cu gn
nh, tnh c ng cao, d mang vc, tm vi rng. Thay vo vic phi di chuyn
chi tit trn bn o ca my o ta cc, rbt o c th tip cn i tng
o trong mi iu kin ca khng gian o.
Rbt o theo ta cc c th o c cc chi tit c hnh dng c bn
nh: hnh trn, hnh tr, mt phng, ng thng v.vtrong phm vi kch thc
104
200x200x200mm ca cc chi tit. Khi thc hin php o, trc ht cn t u

o ca rbt vo v tr ban u ri cho chy chng trnh, cc khp s c
iu khin v di chuyn u o n v tr cn o v ng thi c tn hiu phn
hi khi u o chm vo b mt chi tit cn o.
4.2 Kt cu ca r bt o BKHN-MCX-04
Kt cu ca rbt o BKHN-MCX-04 c minh ha bi m hnh trn
hnh 4.1. Rbt c thit k vi su khu ng v cc khp quay, to thnh
mt h c cu trc ng hc mch h c biu din trn hnh 4.2. Rbt c
su ng c servo dn ng c lp. ng c th nht dn ng khu 1 quay
xung quanh trc c nh theo phng thng ng. Trc quay ca cc ng c
dn ng cc khu 2, 3 v 4 c b tr song song vi nhau v cng vung gc
vi trc quay ng c dn ng khu 5. Cc thng s thit k ( di) ca cc
khu ca rbt c cho trong bng 4.1.
3
Quo
onh
nhtrc
Qu
i tng
i tng cn o
Hnh 4.1. M hnh rbt o BKHN-MCX-04

Trong khi bn ng c u tin c nhim v a im O5 (tm trc ca
khp quay gia khu 5 v khu 6) dch chuyn theo mt qu o khng gian
c nh trc theo yu cu v php o, ng c th 5 lm thay i hng ca
105
khu 5 cho ph hp vi hng ca b mt vt cn o. ng c th 6 c nhim

v dn ng khu 6 (que thm d) mi d (c v tr ti im O6) tip xc vi
b mt vt cn o. Vi phng n thit kt nh vy, rbt o BKHN-MCX-04
c kh nng to ra cc chuyn ng ca mi d mt cch linh hot, ph hp vi
cc php o cc b mt theo cc phng khc nhau.
O3 O4
q4
x2
x3
O2
x4
q3
y1
E O5
q5
q2
O1
z5
q6
x1
x5
z0
z4
O6
x6
y0
q1
x0
Hnh 4.2. S ng hc rbt o BKHN-MCX-04

Bng 4.1. Thng s hnh hc ca rbt
Khu i
1
2
3
4
5
6
Khong cch Oi-1O i (m)

0.14
0.15
0.20
0.0
0.163
0.080
106
4.3 Tnh ton ng hc ngc

Hnh 4.2 m t cc h ta ng gn vi cc khu da trn quy tc
chn h trc ta ca Denavit-Hartenberg. Cc im gc Oi (i = 1,2..., 5) ca
cc h trc ta u nm trong cng mt mt phng thng ng.
Nu xt tng th ton b h vi 6 dn ng, v tr v hng ca khu 6
(khu thao tc) c xc nh trn h ta c nh theo 5 tham s: ba ta
-cc ca im gc O5 v hai gc quay xc nh hng (th d cc gc q5, q6).
Nh vy s d dn ng ca rbt l 1.
Do c im kt cu ca rbt c phn tch trn, vic iu khin cc
bin khp qi (i = 1,2..., 5) im O5 chuyn ng trn mt qu o nh trc
( thun tin ta k hiu O5 E) c vai tr quan trng i vi cc thao tc o.
Ch rng s thay i tr s ca gc quay q5 s khng c nh hng n v tr
ca im E nhng s lm thay i hng ca khu 5 v to ra s thay i hng
ca khu cui 6. Bi vy, nhim v phn tch ng hc ngc ca rbt c
trnh by trong mc ny tp trung vo vic tnh ton cc tr s ca cc bin khp
qi v cc o hm q& i , &q& i (i = 1,2,K,5) da trn mt qu o nh trc ca im
E.
Bng 4.2. Bng tham s ng hc DH
Khu i
1
2
3
4
5
i
q1
q2
q3
q4
q5
di
d1
0
0
0
d5
ai
0
a2
a3
0
0
i
/2
0
0
-/2
/2
T hnh 4.2 ta c th thit lp c bng cc tham s ng hc DH nh

trong bng 4.2, trong cc hng s d1 = OO1 , a 2 = O1O 2 , a 3 = O 2 O 3 , d 5 = O 4 O 5
Cc gi tr li (i=1,2,...,5) c cho trong bng 4.1. T bng tham s DH ta c
cc ma trn bin i h trc
C1
S
H1 = 1
0
S1
0 C1
1
0
C 2
S
0
, H 2 = 2
0
d1
1
0
S2
C2
0
0
0 a 2C2
C 3
S
0 a 2S2
, H 3 = 3
0
1
0
0
1
0
S3
C3
0
0
0 a 3C3
0 a 3 S3
1
0
0
1
107
C 4
S
H4 = 4
0
S4
C4
0
C 5
S
0
, H 5 = 5
0
0
1
0
S5
C5
0
0
d5
trong ta s dng k hiu rt gn Si = sin(q i ) , C i = cos(q i ) . Ma trn chuyn

ta mt im t h ta c nh {Ox 0 y 0 z 0 } sang h ta {O 5 x 5 y 5 z 5 } c
dng
D5 = H1 H2 H3 H4 H5
C1C 234 C5 S1S5 C1S234 S1C5 + C1S5 C 234 d 5 C1S234 + a 2 C1C 2 + a 3 C1C 23
S C C + C S S S
C1C 5 + S1S5 C 234 d 5S1S234 + a 2S1C 2 + a 3S1C 23
1 234 5
1 5
1 234
S234 C5
C 234
S5S234
d1 + d 5 C 234 + a 2S2 + a 3S23
0
0
0
1
trong ta s dng cc k hiu rt gn: Sijk = sin(q i + q j + q k ) v
C ijk = cos(q i + q j + q k ) . T cc phn t ca ma trn D5 ta xc nh c cc ta

ca im E trn h ta c nh
x E = a 2 C1C 2 + a 3 C1C 23 d 5 C1S234
y E = a 2S1C 2 + a 3S1C 23 d 5S1S234
(4.1)
z E = a 2S2 + a 3S23 + d 5 C 234 + d1

Phng trnh (4.1) biu din quan h
x = f (q)
(4.2)
trong x = [x E , y E , z E ] v q = [q q , q 2 , q 3 , q 4 , q 5 ] . T (4.1) ta xc nh c
ma trn Jacobi J(q) biu din quan h x& = J (q) q& nh sau
T
x E
q
1
x y E
J (q) =
=
q q1
z
E
q1
x E
q 2
y E
q 2
z E
q 2
x E
q 3
y E
q 3
z E
q 3
x E
q 4
y E
q 4
z E
q 4
x E
q 5 J
11
y E
= J 21
q 5
z E J 31
q 5
Trong cc phn t ca ma trn J(q) c dng

J11 = -d5 S1 S234 - a3 S1 C23 - a2 S1 C2
J12
J13
J14
J 22
J 23
J 24
J 32
J 33
J 34
J15
J 25
J 35
(4.3)
108
J12 = -d5 C1 C234 - a3 C1 S23 - a2 C1 S2

J13 = -d5 C1 C234 - a3 C1 S23
J14 = -d5 C1 C234
J15 = 0
J21 = -d5 C1 S234 + a3 C1C23 + a2 C1C2
J22 = -d5 S1C234 + a3 S1 S23 + a2 S1 S2
J23 = -d5 S1C234 + a3 S1 S23
J24 = -d5 S1C234
J25 = 0
J31 = 0
J32 = -d5 S234 + a3 C23 + a2 C2
J33 = -d5 S234 + a3 C23
J34 = -d5 S234
J35 = 0
Vi ma trn J c xc nh theo h thc (4.3) v mt qu o nh trc
ca im E, ta c th xy dng mt chng trnh tnh ton cc gi tr bin khp
qi v cc o hm q& i , &q& i (i = 1,2,K,5) da trn thut gii c trnh by trong
chng 1.
minh ha cho vic tnh ton ng hc ngc bng tnh ton m phng
s, ta xt mt th d ng vi im E c qu o l mt ng xon c bm trn
mt mt cu. Quy lut chuyn ng c dng
t
x E = 3 + 0.1sin(2t ) sin
3
t
(4.4)
y E = 0.1cos(2t ) sin
3
t
z E = 0.12 + 0.1sin
3
trong cc ta c tnh theo n v mt.
Qu trnh tnh ton tr s cc bin khp c thc hin trn chng trnh
tnh MATLAB trong khong thi gian tnh 0 t (s) vi bc thi gian h =
15
0.001 (s) v sai s tnh ton = 10-6 (rad). Cc kt qu tnh ton c biu din
trn cc hnh 4.4, 4.5 v 4.6, trong cc th gc quay qi c biu din trn
hnh 4.4, hnh 4.5 l th vn tc gc q& i v hnh 4.6 cha cc th gia tc gc
109
&q& i . Nh phn tch trn, tham s gc quay q5 l hng s v khng c nh

hng n qu o chuyn ng ca im E. Cc kt qu ny c th s dng
trong vic iu khin ng hc v l d liu cho phn tch ng lc hc ngc.
Qu o
Qu
o im
im E
Hnh 4.3. Qu o nh trc ca im E (ng xon c)
Hnh 4.4. th bin khp q
110
Hnh 4.5. th vn tc gc q&
&&
Hnh 4.6. th gia tc gc q
111
4.4 Tnh ton ng lc hc ngc
Nh cp ti trong cc chng trc, mc tiu ca bi ton phn

tch ng lc hc ngc l xc nh tr s ca cc mmen ng c ng vi mt
quy lut chuyn ng cho trc (v qu o v vn tc) ca im E. Mc d
khu th 6 (que o) dng thao tc o v c iu khin c lp vi phn
cn li ca h, khu ny vn c nh hng khi tnh ton ng lc hc. Bi vy
tt c 6 khu ca rbt phi c tnh n trong cc phng trnh vi phn
chuyn ng.
Tr s mmen ng c = [1 , 2 , 3 , 4 , 5 , 6 ] c xc nh t cc
phng trnh ng lc m t chuyn ng ca rbt o. Trong trng hp b
qua ma st ti cc khp v cc lc cn khc, ng thi coi lc tc ng ca vt
cn o ln khu 6 ti v tr tip xc l nh khng ng k, phng trnh ng
lc c biu din di dng tng qut
T
&& + C(q, q& )q& + g (q) =

M (q)q
(4.5)
thit lp cc phng trnh ny ta s dng cc k hiu:

khi lng ca khu i
mi
T
ri = [x C , y C , z C ]
v tr khi tm khu i trn h c nh {Ox 0 y 0 z 0 }
i
r = [x , y , z (Ci ) ]
v i = r&i
i
vn tc khi tm khu i
vct vn tc gc khu i biu din trn h c nh
i( i )
vct vn tc gc khu i biu din trn h ng
(i)
i
(i )
Ci
(i )
Ci
ri
q
i
J Ri (q) =
q&
Ai
J Ti (q) =
(i )
i
v tr khi tm khu i trn h ng {Ox i y i z i }
ma trn Jacobi tnh tin ca khu i

ma trn Jacobi quay ca khu i
ma trn csin ch hng ca khu i
ma trn qun tnh khi ca khu i ti khi tm

ca khu v tnh trn h ta ng
Ii
ma trn qun tnh khi ca khu i ti khi tm
ca khu v tnh trn h ta c nh
Cc s hng trong phng trnh (4.5) c xc nh theo cc bc sau
y:
112
1. Xc nh cc ma trn csin ch hng Ai

Cc ma trn bin i ta ca cc khu i vi h ta c nh
C1
S
D1 = H1 = 1
0
0 S1
0
0 C1 0
,
1
0
d1
0
0
1
C1C 2
S C
D 2 = D1H 2 = 1 2
S2
C1S2
S1S2
C2
0
C1C 23
S C
D3 = D 2 H 3 = 1 23
S23
C1S23
S1S23
C 23
0
C1C 234
S C
D 4 = D3 H 4 = 1 234
S234
S1
C1
0
0
C1C 234 S1S5

S C + C S
1 5
D5 = 1 234
S234 C5
C1S234
S1S234
C 234
0
S1
a 2 C1C 2
C1 a 2S1C 2
0
a 2S2 + d1
0
1
S1 a 2 C1C 2 + a 3 C1C 23
C1 a 2S1C 2 + a 3S1C 23
0
a 2S2 + a 3S23 + d1
0
1
C1S234
S1S234
C 234
0
a 2 C1C 2 + a 3 C1C 23
a 2S1C 2 + a 3S1C 23
a 2S2 + a 3S23 + d1
S1C 5 + C1S5 C 234

C1C5 + S1S5 C 234
C5S234
0
a 2 C1C 2 + a 3C1C 23 d 5 C1S234

a 2S1C 2 + a 3S1C 23 d 5S1S234
a 2S2 + a 3S23 + d1 + d 5 C 234
Ma trn csin ch hng Ai c xc nh t 9 thnh phn ca ma trn

3x3 pha trn bn tri ca cc ma trn bin i ta Di. Vct ta rOi ca
im gc Oi ca h ta ng nm trong vct 3x1 pha trn bn phi ca Di.
0
C1 0 S1
A1 = S1 0 C1 , rO = 0

d1
0 1
0
1
113
C1C 2
A 2 = S1C 2
S2
C1S2
S1S2
C2
C1C 23
A 3 = S1C 23
S23
C1S23
C1C 234
A 4 = S1C 234
S234
S1
S1
a 2 C1C 2
C1 , rO = a 2S1C 2
a 2S2 + d1
0
2
S1
a 2 C1C 2 + a 3 C1C 23
C1 , rO = a 2S1C 2 + a 3S1C 23
d1 + a 2S2 + a 3S23
0
S1S23
C 23
C1
0
C1S234
a 2 C1C 2 + a 3 C1C 23
S1S234 , rO = a 2S1C 2 + a 3S1C 23
d1 + a 2S2 + a 3S23
C 234
C1C 234 C5 S1S5

A 5 = S1C 234 C 5 + C1S5
S234 C5
C1S234
S1S234
C 234
S1C 5 + C1S5 C 234

C1C5 + S1S5 C 234
S5S234
d 5 C1S234 + a 2 C1C 2 + a 3C1C 23

rO = d 5S1S234 + a 2S1C 2 + a 3S1C 23
d1 + d 5 C 234 + a 2S2 + a 3S23

5
C6S1S5 S6C1S234 + C6C5C1C234 S6S1S5 C6C1S234 S6C5C1C234 S1C5 + C1S5C234

A6 = C6S5C1 S6S1S234 + C6C5S1C234 S6S5C1 C6S1S234 S6C5S1C234 C1C5 + S1S5C234
C5C6S234 + S6C234
S5S234
C5S6S234 + C6C234
a6C6S1S5 + a 2C1C2 (a 6S6C1 d5C1 )S234 + a 6C6C5C1C234 + a3C1C23
rO = a6C6C1S5 + a 2S1C2 (a 6S6S1 d5S1 )S234 + a 6C6C5S1C234 + a3S1C23
a 6C5C6S234 + (a6S6 + d5 )C234 + a3S23 + a 2S2 + d1

6
2. Xc nh ta khi tm ca cc khu theo cng thc tng qut

ri = rOi + A i ri( i ) ,
(i = 1,2,K,6)
(4.6)
trong vct rOi v ma trn A i c xc nh t bc 1. Vct ri( i ) c cho

trc t cc thng s ng lc hc ca rbt nh trn hnh 4.7 v trong bng
4.3.
114
O3 O4
x2
x3
l3
C4
O2
l4
C3
x4
C2
C5
O5
l5
y1
l2
z4
l6
x1
O1
C6
O6
C1
x6
l1
O
Hnh 4.7. V tr trng tm cc khu
Bng 4.3. V tr khi tm khu i ca rbt trn h ng
V tr trng tm
Khu i
x (Cii )
y (Cii )
z (Cii )
-(d1-l1)
-(a2-l2)
-(a3-l3)
l4
-(d5-l5)
-(a6-l6)
3. Tnh ton ma trn Jacobi tnh tin ca cc khu da trn cc o hm

ring theo bin q, th d nh
0 0 0 0 0 0
r1
J T1 =
= 0 0 0 0 0 0
q
0 0 0 0 0 0
115
J T2
l 2S 2 C 2
r2
=
= l 2 C1 C 2
q
0
l 2 S 2 C1
l 2S1S2
l2C2
J T3
l 3S1C123 a 2S1C 2
r3
=
= l 3 C1C 23 + a 2 C1C 2
q
0
0 0 0 0
0 0 0 0
0 0 0 0
l 3 C1S23 a 2 C1S2
l 3S1S23 a 2S1S2
l 3 C 23 + a 2 C 2
l 3 C1S23
l 3S1S23
l 3 C 23
0 0 0
0 0 0
0 0 0
4. Tnh ton vn tc gc ca cc khu

Trc ht ta thit lp biu thc ton t sng ca cc vct vn tc gc
0 0 q& 1
~ (1) = A T A
& = 0 0 0
1
1
1
q& 1 1 0
~ ( 2)
q& 2
0
& = q&
= A T2 A
0
2
2
C 2 q& 1 S2 q& 1
~ ( 3)
0
T &
= A 3 A 3 = q& 2 + q& 3
C 23q& 1
C 2 q& 1
S2 q& 1
q& 2 q& 3
0
S23q& 1
( 4)
T &
~
C 234 q& 1
4 = A4 A4 =
q& 2 + q& 3 + q& 4
C 23q& 1
S23q& 1
C 234 q& 1
0
S234 q& 1
q& 2 q& 3 q& 4

S234 q& 1
~ (5) = AT A
& =
5
5
5
0
S5S234q& 1 C5 (q& 2 + q& 3 + q& 4 )
q& 5 + C234q& 1
= S5S234q& 1 + C5 (q& 2 + q& 3 + q& 4 )

0
C5S234q& 1 + S5 (q& 2 + q& 3 + q& 4 )
q& 5 C234q& 1
C5S234q& 1 S5 (q& 2 + q& 3 + q& 4 )
0
T ta suy ra vn tc gc ca cc khu
S234q& 1
S23q& 1
S2q& 1
0
1(1) = q& 1 , (22) = C2q& 1 , 3(3) = C23q& 1 , (44) = (q& 2 q& 3 q& 4 )
q& 2 + q& 3
q& 2
0
C234q& 1
116
C5S234q& 1 S5 (q& 2 + q& 3 + q& 4 )
5(5) =
q& 5 + C234q& 1
S5S234q& 1 + C5 (q& 2 + q& 3 + q& 4 )

C5C6S234q& 1 + S6C234q& 1 S5C6 (q& 2 + q& 3 + q& 4 ) + S6q& 5
= C5S6S234q& 1 + C6C234q& 1 + S5S6 (q& 2 + q& 3 + q& 4 ) + C6q& 5
&
&
&
&
&
q6 + S5S234q1 + C5 (q2 + q3 + q4 )
(6)
6
5. Tnh ton cc ma trn Jacobi J Ri

Trc ht ta tnh ton cc ma trn Jacobi quay J (Rii ) tng ng vi i( i ) nh
sau
0 0 0 0 0 0
1(1)
= 1 0 0 0 0 0 ,
J =
q&
0 0 0 0 0 0
S 2 0 0 0 0 0
(22 )
( 2)
= C 2 0 0 0 0 0 ,
JR2 =
q&
0 1 0 0 0 0
(1)
R1
( 3)
R3
J (R44)
S23 0 0 0
3( 3)
=
= C 23 0 0 0
q&
0 1 1 0
0
S 234 0
(44 )
=
= 0 1 1
q&
C 234 0
0
S5
0
C5
0 0
0 0 ,
0 0
0 0 0
1 0 0 ,
0 0 0
S5
0
C5
J (R55)
S 234 C 5
5( 5 )
=
= C 234
q&
S234S5
J (R66)
S6 C 234 + S 234 C 5 C 6
(66 )
=
= C 6 C 234 S 234 C 5S6
q&
S234S5
S5
0
C5
0 0
1 0 ,
0 0
S5 C 6
S5S 6
C5
S5 C 6
S5S 6
C5
S5 C 6
S5S 6
C5
S6
C6
0
0
0
Ma trn Jacobi quay J Ri tng ng vi i c xc nh t J (Rii ) theo h thc
117
i (A i i( i ) )
i( i )
J Ri =
=
= Ai
= A i J (Rii )
q&
q&
q&
(4.7)
6. Xc nh cc phn t ca ma trn khi lng M (q) ca rbt t h thc

tng qut sau
M (q) = [J TTi m i J Ti + J TRi I i J Ri ]

6
(4.8)
i =1
n gin ha cc cng thc tnh nhng khng mt tnh tng qut, ta

coi cc trc ca h ta ng l cc trc qun tnh chnh, khi ma trn
I i( i ) bao gm cc phn t l hng s v c biu din di dng ma trn ng
cho
I xi
I =0
0
(i)
1
0
I yi
1
0
0
I zi
(4.9)
Ch rng ma trn qun tnh khi Ii ca khu i ti im khi tm tnh

trn h ta c nh c xc nh bi h thc
I i = A i I i( i ) A iT
(4.10)
Th cc h thc (4.7) v (4.9) vo phng trnh (4.8) ta nhn c
M (q) = [J TTi m i J Ti + J (Rii ) T A iT A i I i( i ) A iT A i J (Rii ) ] = [J TTi m i J Ti + J (Rii ) T I i( i ) J (Rii ) ]

6
i =1
i =1
(4.11)
Th cc biu thc nhn c ca J Ti (bc 3), J (Rii ) (bc 5), cc gi tr

cho trc ca mi v ma trn I i(i ) vo phng trnh (4.11) ta nhn c biu thc
ca 66 phn t ca ma trn khi lng M (q) . Do cng thc ca M (q) c
vit di dng tch v tng ca cc ma trn nn vic tnh ton c th thc hin
kh thun li nh mt chng trnh tnh c chc nng x l cc biu thc bng
ch nh chng trnh tnh ton a nng MAPLE.
7. Xc nh cc phn t ca ma trn C(q, q& )
Nh trnh by trong chng II, cc thnh phn cij ca ma trn C(q, q& )
c xc nh t cc phn t ca ma trn M (q) theo h thc sau
C ij =
1 m ij m ik m jk
+
q j
q i
2 q k
trong i = 1,2,...,6 v j = 1,2,...,6.
q& k
(4.12)
118
Tuy nhin, biu thc ca cc phn t ca M (q) v C(q, q& ) l nhng biu
thc gii tch kh di v phc tp nn khng c trnh by chi tit thm y.
8. Xc nh cc phn t ca vc t g(q) t biu thc th nng ca h
Th nng ca h c th xc nh nh cng thc

6
= m i g T ri
(4.13)
i =1
trong vct ri c xc nh t bc 2, vc t gia tc trng trng tnh trn

T
h c nh g = [ 0,0, g ] vi g 9.81 m/s 2 . S dng h thc (4.13), th nng
ca h c vit di dng
= m1gl1 + m 2 g (l 2S 2 + d1 ) + m 3 g (l 3S23 + a 2S 2 + d1 )
+ m 4 g(l 4 C 234 + a 3S23 + a 2S2 + d1 )
+ m 5 g(l 5 C 234 + a 3S23 + a 2S2 + d1 )
+ m 6 g (l 6S6 C 234 + l 6 C 6 C 5S 234 + a 3S 23 + a 2S 2 + d 5 C 234 + d1 )
T , ta suy ra cc thnh phn ca vct g(q) theo cng thc

g(q) =
q
(4.14)
di dng sau
g1 = 0
g 2 = m 2 gl 2 C 2 + m 3 g(l 3 C 23 + a 2 C 2 )
m 4 g (l 4S234 a 3 C 23 a 2 C 2 ) m 5 g(l 5S234 a 3 C 23 a 2 C 2 )
m 6 g (l 6S6S234 l 6 C 6 C 5 C 234 a 3 C 23 a 2 C 2 + d 5S 234 )
g 3 = m 3 gl 3 C 23 m 4 g (l 4S234 + a 3 C 23 ) m 5 g (l 5S234 a 3 C 23 )
m 6 g(l 6S6S 234 l 6 C 6 C 5 C 234 a 3 C 23 + d 5 C 234 )
g 4 = m 4 gl 4S 234 m 5 gl 5S234 m 6 g (l 6S6S234 l 6 C 6 C 5 C 234 + d 5S234 )
g 5 = m 6 gl 6 C 6S5S234
g 6 = m 6 gl 6 (C 6 C 234 C 5S6S 234 )
Cui cng, ta tnh ton c cc tr s mmen ca ng c ca rbt sau
khi tt c cc s hng ca v phi ca phng trnh (4.5) c xc nh.
minh ha cho vic tnh ton ng lc hc ngc bng tnh ton s, ta xt mt
th d ng vi qu o chuyn ng ca im E theo dng cung trn phng.
Bng 4.4 cung cp cc tr s ca cc thng s ng lc hc ca rbt phc v
cho tnh ton s.
119
Bng 4.4. Cc thng s ng lc hc ca rbt o (i chiu vi hnh 4.7)

Khu i
1
2
3
4
5
6
mi
Ixi
Iyi
Izi
li
(kg)
(kgm2)
(kgm2)
(kgm2)
(m)
2.0
0.9
1.2
1.1
0.5
0.05
4.010-3
0.210-3
0.510-3
0.610-3
0.710-3
0.310-4
3.010-3
3.010-3
3.510-3
2.510-3
0.210-3
0.210-4
1.010-3
3.010-3
4.010-3
3.510-3
0.310-3
0.110-4
0.10
0.06
0.10
0.04
0.03
0.02
Quy lut chuyn ng trn mt phng {Ox 0 z 0 } ca im E (im O5)

c m t bi cc phng trnh ta (4.15) v minh ha trn hnh 4.8.

x E = 0.2 + 0.121 cos t (m)

4
(4.15)
yE = 0
z E = 0.14 + 0.12 sin t (m)

4
trong thi gian chuyn ng ht mt na ng trn l 4 (s). V tr ban u
(ti thi im t = 0) ca rbt c xc nh bi cc tr s bin khp
q 20 = 1.0472, q 30 = 3.5511, q 40 = 2.1206, q 50 = 2.0 (rad). Do chuyn ng ca im
E hon ton trn mt phng, bin khp ca khu 1 c n nh q1 = 0.
Hnh 4.8. Qu o im E (na ng trn trn mt phng {Ox 0 z 0 })
120
O2
E O5
O1
O4
Hnh 4.9. V tr cc khu v cc khp ti mt s v tr chuyn ng
Hnh 4.10. Tr s ca bin khp 2, 3 v 4
121
Hnh 4.11. Vn tc gc ca ng c dn ng khu 2, 3 v 4
Hnh 4.12. Gia tc gc ca ng c dn ng khu 2, 3 v 4
122
Hnh 4.13. Mmen dn ng cn thit cc khu ca rbt

Kt qu tnh ton ng hc ngc c trnh by trn cc hnh 4.9 - 4.12.
Nh phn tch ti mc 4.2, bin khp q5 gi gi tr hng s trong sut qu
trnh chuyn ng ca im E. Vi cc kt qu tnh ton ng hc ngc i
vi cc bin khp nh trn v vi khp 6 chuyn ng c lp theo quy lut
q 6 = / 2, q& 6 = 0, &q& 6 = 0, ta c th tnh ton c mmen dn ng cn thit ti
cc khp 1, 2,...,6 (dn ng cc khu 1, 2,...,6 tng ng) t phng trnh vi
phn chuyn ng thit lp c. Kt qu tnh ton s c biu din trn
hnh 4.13.
4.5 iu khin trt rbt BKHN-MCX-04

tin hnh iu khin rbt chuyn ng theo mt quy lut nh trc
theo yu cu ca bi ton cng ngh, trc tin ta phi gii bi ton ng hc
&& da trn thut ton hiu chnh
ngc xc nh gi tr ca cc vc t q, q& , q
gia lng vc t ta suy rng c trnh by trong chng 1, sau gii
bi ton ng lc hc ngc xc nh phng trnh chuyn ng ca rbt
&& qua xc nh cng sut cn
v gi tr lc/mmen theo cc gi tr ca q, q& , q
thit ca ng c in.
123
Theo nguyn l hot ng ca rbt BKHN-MCX-04, o mt chi tit

no ta phi xc nh trc qu o cn thit iu khin im O5 chuyn
ng theo qu o y nhm a que o tip xc vi chi tit cn o.
Gi s chi tit cn o c dng hnh cu nh hnh di y
Qu o im E
Hnh 4.14
o vt th dng hnh cu ny ta c th tin hnh iu khin im O5

chuyn ng theo 1 ng xon c trn b mt ca hnh cu. Phng trnh ca
ng xon c c dng
t
x E = 3 + 0.1sin(2t ) sin
3
t
y E = 0.1cos(2t ) sin
3
t
z E = 0.12 + 0.1sin
3
Bi ton ng hc ngc v ng lc hc ngc cho trng hp ny
c gii quyt trong phn 4.3 v 4.4, ta s s dng cc kt qu ny cho phn
iu khin.
Nh trong phn iu khin chuyn ng ca rbt trnh by chng
3 v theo l thuyt iu khin dng trt c trnh by trong [63], n nh
chuyn ng ca im O5 ta chn hm Lyapunov c dng
124
1
V = s T M (q)s
2
Trong M(q) l ma trn khi lng ca rbt, s l sai s suy rng trong
iu khin dng trt, vi s c xc nh bi
s = e& ( t ) + e( t )
trong biu thc trn th e(t) v e& ( t ) l sai lch v tr v sai lch vn tc ca
cc khp ng
e(t) = q(t) qd(t)
e& ( t ) = q& ( t ) q& d ( t )
= diag(1 , 2 ,..., 6 )
S dng chng trnh iu khin trt c xy dng theo thut ton tnh
ton gii bi ton iu khin rbt chng 3 v phn simulink trong Matlab,
ta thu c kt qu iu khin chuyn ng rbt BKHN-MCX-04 theo cc
thng s ca b iu khin c chn l:
Ks= diag([20,20,20,20,1,0.1]); Kpd = diag([0.2,0.2,0.2,0.2,0.02,0.01]);
= diag([50,50,50,50,30,20]);
Nhiu d(t) = random(6,6)*0.01;
Hnh 4.15
th ta xE(t) trong khng gian thao tc
125
Hnh 4.16
th ta yE(t) trong khng gian thao tc.
Hnh 4.17
th ta zE(t) trong khng gian thao tc.
Qua kt qu trn ta thy rng im cn iu khin O5 ca rbt BKHNMCX-04 bm kh tt theo qu o mong mun, ch sau mt khang thi gian
0.25 giy im O5 bm c qu o mong mun.
126
4.6 Th nghim
Qua cc phn l thuyt trnh by ta thy cc bi ton ngc ng lc
hc v iu khin rbt d dn ng u lin quan n vic bi bi ton ng
hc ngc. Kt qu ca bi ton ng lc hc v iu khin hon ton ph
thuc vo chnh xc ca bi ton ng hc ngc, v vy trong phn th
nghim ca mnh chng ti chn vic th nghim nhm kim tra chnh xc
ca vic gii bi ton ng hc ngc. M hnh th nghim c dng nh hnh
nh di y:
Hnh 4.18 M hnh th nghim rbt o BKHN-MCX-04
4.6.1 Cu to ca h thng th nghim

B th nghim gm c cc phn nh sau:
Rbt BKHN-MCX-04
Camera thu nhn hnh nh hot ng ca rbt
Knh lc mu
My tnh
127
4.6.2 Nguyn l hot ng ca h thng th nghim

Trn rbt, ti cc khp ng ta gn cc n led pht ngun nh sng
mu , pha trc camera ta gn knh lc mu c nhim v ch cho nh sng
mu i qua v tc ng vo camera. Trong qu trnh lm vic c th c cc
nhiu v mu sc t h thng tc ng ln camera, loi cc nhiu sc ny ta
dng phn mm lc nhiu c vit bng ngn ng Matlab. Sau khi lc i cc
nhiu ta cn li cc m sng pht ra t cc n led - v tr ca cc khp m ta
quan tm. Phn mm c nhim v xc nh v tr tm ca cc m sng. Khi xc
nh c cc tm ca m sng, th on thng ni cc tm chnh l cc khu
ca rbt thi im kho st.
Khi rbt chuyn ng, cc n led gn lin vi cc khp ng cng s
thay i v tr ca n. S thay i v tr ca cc khp ng ny s c camera
thu nhn, chp li nh v lu vo b nh ca my tnh. Phn mm x l nh s
xc nh tm ca cc khp v lu vo tp toadotam.txt ca cc khp. Mi nh
s c 2n s ta tm, trong c n gi tr ta theo trc x v n gi tr ta
theo trc y ca cc tm ( vi n l s cc n led chiu sng, y chnh l s cc
khp ng). Khi c ta ca cc tm sng, phn mm s tnh ton ra gc
gia cc khu qua cc on thng ni lin cc m sng v lu vo tp
Goc.txt l b gc gia cc khu.
iu khin chuyn ng ca rbt ta xy dng mt phn mm iu
khin vit bng ngn ng Visual Basic. Thng qua giao din ca chng trnh
ta nhp vo cc gi tr gc quay ca cc khp ng m ta tnh ton c qua bi
ton ng hc ngc. Chng trnh iu khin s iu khin cc khp rbt
quay vi cc gc tng ng. Cn phn mm x l nh s ghi v lu li cc gi
tr gc quay thc v ta ca cc tm khp cng nh v c qu o chuyn
ng ca khu thao tc.
4.6.3 H thng iu khin

Rbt o gm 5 khp iu khin chuyn ng v 1 khp iu khin vic
o lng. Mi mt khp c qun l bi 1 b vi x l. Mi b vi x l c
chc nng iu khin khp kn, va c nhim v iu khin ng c va c
nhim v o lng. Cc b vi x l s c ni chung bus v c ni vi my
tnh qua cng RS232. Mi mt b vi x l s c mt a ch xc nh ta c
th d dng iu khin.
Tay my hot ng theo nguyn tc t cc bn v CAD khi thit k b
mt cc chi tit my ta xc nh qu o iu khin theo cc im cn o. Trn
qu o iu khin o, ta s xc lp chng trnh iu khin cho cc khp
dch chuyn cc khu ca rbt theo cc im trong khng gian lm vic ca
thit b. Mun xc nh v tr ca mt im no trong khng gian, ta pht
lnh iu khin cho tng khp t my tnh. Lnh iu khin s c truyn
128
xung b vi x l. Khp no c iu khin th b vi x l ca khp s nhn

c lnh v bt u iu khin ng c. V tr ca cc khp s c xc nh
nh h thng o gc bi cc encoder. Nh ta s xc nh c v tr ca u
o. Trong sut qu trnh iu khin, v tr ca u o lun c cp nht trong
my tnh. Khi u o n ng v tr m ta mong mun th v tr ca u o
cng nh v tr ca cc khp s c lu li trong my tnh.
Xut pht t nguyn tc hot ng ca rbt o, ta a ra cc yu cu
sau :
- Xy dng h vi x l thc hin 2 chc nng iu khin ng c ng thi
x l tn hiu phn hi t h o.
- Xy dng h truyn ng in thc hin chc nng iu khin ng c
mt chiu (c o chiu).
- Xy dng h x l tn hiu phn hi t h o, ng thi nng cao phn
gii ca h thng o.
- Truyn thng a vi x l kt ni gia h iu khin ca tng khp vi
my tnh.
T yu cu trn ta c s iu khin cho ton h thng nh trn hnh 4.
19 v s iu khin ca tng khp nh trn hnh 4.20
Hnh 4.19 S iu khin ton b rbt
129
Hnh 4.20 s iu khin tng khp ca BKHN-MCX-04

Trong cc h thng iu khin trn ta dng vi iu khin thuc h
MCS-51 ca Intel, c th l chip AT89C2051 ca hng Atmel. S nguyn l
ca mch iu khin trc c dng nh trn hnh 4.21.
VCC
R1
100
SW1
RESET
D8
D6
D5
10K
C3
C
R2
C2
Y1
CRY STAL
J4
EA/VPP
RST
XTAL1
XTAL2
10
ALE/PROG
PSEN
VCC
AT89C52
4
3
2
1
P2.0/A8
P2.1/A9
P2.2/A10
P2.3/A11
P2.4/A12
P2.5/A13
P2.6/A14
P2.7/A15
P1.0/T2
P3.0/RXD
P1.1/T2-EX P3.1/TXD
P1.2
P3.2/INT0
P1.3
P3.3/INT1
P1.4
P3.4/T0
P1.5
P3.5/T1
P1.6
P3.6/WR
P1.7
P3.7/RD
P0.0/AD0
P0.1/AD1
P0.2/AD2
P0.3/AD3
P0.4/AD4
P0.5/AD5
P0.6/AD6
P0.7/AD7
EN-THUOC
31
9
19
18
D7
C1
1
2
3
4
5
6
7
8
D4
D3
D2
39
38
37
36
35
34
33
32
D1
U1
GND
20
VCC
VCC
40
VCC
U7A
VCC
U12C
30
29
10
11
12
13
14
15
16
17
21
22
23
24
25
26
27
28
14
D10
3
74HC00
10
U5E
13
12
EN-THC
11
74HC00
74HC00
U8B
U12D
F-HIGH
F-LOW
RIGHT
LEFT
PWM
RXD-TXD
1
2
LED-FL
LED-FH VCC
INT0-THC
11
74HC00
11
8
9
12
QD
QC
QB
QA
105
C4
11
8
9
12
VCC
VCC
CLKB
CLKA
R92
R91
R02
R01
11
13
14
2
3
1
14
7
6
3
2
S nguyn l ca mch iu khin 1 trc
VCC
12
74HC126
F-LOW
F-HIGH
74HC126
74LS90
1
14
2Y 1
2Y 2
1Y 1
1Y 2
L298
VCC1
VCC2
1EN
2EN
1E
2E
2A1
2A2
1A1
1A2
CLKB
CLKA
74LS90
7
6
3
2
VCC
R92
R91
R02
R01
QD
QC
QB
QA
12V
9
4
6
11
1
15
10
12
5
7
GND
10
VCC
5
74HC04
GND
10
VCC
5
10
13
1
14
U4
D13
DIODE
74HC126
U9A
Hnh 4.21
7
D11
DIODE
U10B
12V
VCC
D14
DIODE
D12
DIODE
12V
1
2
5
4
3
2
1
EN-QUAY
DC
130
131
4.6.4 Kt qu th nghim
Di y l kt qu ca th nghim m chng ti tin hnh trn rbt
BKHN-MCX-04
Gi s qu o mong mun ca bn kp ca rbt BKHN-MCX-04 c
dng l na vng trn phng vi phng trnh ca n l

x E = 0.2 + 0.121 cos t (m)

4
yE = 0
z E = 0.14 + 0.12 sin t (m)

4
Gi tr ta im cui E v cc ta suy rng ca cc khp ng
c xc nh t thc nghim v c lu li theo bng di y.
Bng 4.5 Cc kt qu o thc nghim
TT
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
t (s)
0
0.2
0.4
0.6
0.8
1.0
1.2
1.4
1.6
1.8
2.0
2.2
2.4
2.6
2.8
3.0
3.2
3.4
3.6
3.8
4.0
xE (m)
0.2000
0.2015
0.2059
0.2131
0.2229
0.2351
0.2495
0.2655
0.2829
0.3012
0.3200
0.3388
0.3571
0.3745
0.3905
0.4049
0.4171
0.4269
0.4341
0.4385
0.4400
yE (m)
0.1405
0.1578
0.1776
0.1935
0.2110
0.2239
0.2376
0.2459
0.2546
0.2575
0.2605
0.2575
0.2546
0.2459
0.2376
0.2239
0.2110
0.1935
0.1776
0.1578
0.1400
q2 (rad)
1.0472
1.0957
1.1396
1.1767
1.2045
1.2212
1.2261
1.2195
1.2025
1.1763
1.1425
1.1023
1.0569
1.0076
0.9557
0.9027
0.8501
0.7997
0.7532
0.7122
0.6783
q3 (rad)
3.5511
3.5814
3.6379
3.7157
3.8077
3.9070
4.0085
4.1089
4.2068
4.3013
4.3922
4.4791
4.5616
4.6389
4.7099
4.7733
4.8272
4.8696
4.8987
4.9125
4.9095
q4 (rad)
2.1206
2.1564
2.1711
2.1622
2.1294
2.0750
2.0030
1.9177
1.8234
1.7236
1.6213
1.5192
1.4194
1.3242
1.2355
1.1549
1.0843
1.0248
0.9776
0.9432
0.9218
132
Hnh 4.22
Qu o thc nghim ca im cui E
133
Qua kt qu th nghim ta thy sai lch ln nht gia kt qu tnh ton

v kt qu thc nghim l 0.1%. iu chng t thut ton m chng ti s
dng gii bi ton ng hc ngc l hon ton ph hp.
4.7 Kt lun chng 4
Trn c s rbt o t ch to, trong chng ny tin hnh xc nh
cc tham s ng hc, ng lc hc ca rbt. Sau tin hnh tnh ton
ng hc ngc, ng lc hc ngc v iu khin rbt bm theo qu o cn
o. y l mt loi rbt c cu trc ng hc khng gian nn vic thit lp cc
phng trnh vi phn chuyn ng kh phc tp. Do vic tnh ton ng lc
hc ngc v iu khin chuyn ng l khng n gin nu ta s dng cc
cng c c. Nh thut ton hiu chnh gia lng vc t ta suy rng, bi
ton ny c gii mt cch kh nhanh chng.
Cc kt qu ca chng ny mt mt chng minh tnh cht ng n ca
cc thut ton trong cc chng trc, mt khc nu ra mt kh nng ch to v
s dng rbt trong o lng chnh xc cc chi tit v cm cc chi tit my.
134
KT LUN CHUNG
thit k ch to cc rbt phc v cho vic y mnh cng nghip ha
v hin i ha nn cng nghip th vic nghin cu ng hc, ng lc hc v
iu khin cc rbt l mt cng vic cn thit v ht sc quan trng.
Trong lun n ca mnh, tc gi nghin cu cc bi ton ng hc,
ng lc hc v iu khin chuyn ng ca rbt d dn ng. y l cc
dng bi ton c bn nht ca vic phn tch v tng hp cc rbt. C th tm
tt li mt s kt qu chnh m chng ti t c nh sau:
Nhng kt qu chnh ca lun n
1. Da trn c s ca cng thc khai trin Taylor, xy dng thut ton
hiu chnh gia lng vc t ta suy rng tnh ton bng s bi
ton ng hc ngc rbt d dn ng. So vi cc thut ton s c
thut ton ny tnh ton c chnh xc cao hn.
2. xy dng mt thut ton gii bi ton ng lc hc ngc rbt d
dn ng trong khng gian thao tc. T xy dng mt thut ton gii
quyt bi ton iu khin rbt d dn ng trong khng gian trng thi
theo phng php iu khin dng trt.
3. Da trn cc phn mm a nng MATLAB v MAPLE xy dng cc
chng trnh tnh ton ng hc ngc, ng lc hc ngc v iu
khin trt rbt d dn ng da trn thut ton hiu chnh gia lng
vc t ta suy rng. S dng cc chng trnh tnh ton ny tnh
ton nhiu th d minh ha nhm chng minh kh nng ca thut ton v
s chnh xc (sai s rt b) ca thut ton.
4. Vi s gip ca TS. Nguyn Vn Vinh, tham gia ch to rbt o
BKHN-MCX-04. xc nh c cc tham s ng hc, ng lc hc
ca rbt t ch to ny. S dng phn mm xy dng trn mi trng
MATLAB v MAPLE tin hnh tnh ton ng hc ngc, ng lc
hc ngc v iu khin trt rbt o BKHN-MCX-04. tin hnh th
nghim nh trn m hnh ny.
Cc bi ton ng hc ngc, ng lc hc ngc v iu khin chuyn
ng rbt u c lin quan n bi ton xc nh cc ta suy rng cho nn
khi chng ta xy dng thut ton hiu chnh gia lng vc t ta suy
rng l chng ta c mt cng c hu ch gii quyt cc bi ton c bn v
ng hc ngc, ng lc hc ngc v iu khin rbt. V vy c th ni
thut ton hiu chnh gia lng vc t ta suy rng l kt qu c bn nht
ca lun n ny.
135
Mt s vn c th tip tc m rng nghin cu

- Do c bc t do cn d m khi c mt khp ng no b kt th rbt
vn c kh nng thc hin cc yu cu ca bi ton cng ngh v vy ta
c th u t nghin cu v hin tng kt khp trong qu trnh lm vic
ca Rbt d dn ng.
- Nghin cu ng dng cc phng php iu khin khc nhau la chn
gii php iu khin ti u.
- Nghin cu trin khai ng dng trong thc t sn xut cc kt qu
nghin cu c.
136
DANH MC CNG TRNH CA TC GI

1. Nguyn Vn Khang, L c t, Trn Hong Nam: V mt phng
php s gii bi ton ng hc ngc robot dng chui. Tuyn tp Hi
ngh C hc ton quc ln th 8, Tp 1, Tr. 250-259. NXB Bch khoa,
H Ni 2007.
2. Nguyn Vn Khang, Nguyn Quang Hong, L c t, Trn Hong
Nam : V mt thut ton iu khin trt robot d dn ng. Tp ch
Tin hc v iu khin hc, Tp 24 (2008), No.3, Tr.269-280.
3. Nguyn Quang Hong, Nguyn Vn Khang, Trn Hong Nam: Bi ton
ng hc ngc rbt d dn ng c ch n s c kt khp. Tuyn
tp Hi ngh C hc ton quc, Tp 2, Tr. 282-290, NXB Khoa hc t
nhin v Cng ngh, H Ni 2009.
4. Nguyn Vn Khang, Nguyn Quang Hong, Trn Hong Nam: V bi
ton ng lc hc ngc rbt d dn ng. Tuyn tp Hi ngh Khoa
hc Cng ngh C kh ch to ton quc ln th hai, Phn ban T ng
ha v C in t, Tr 41-48, H Ni 2009.
5. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang
Nam : In fluence of trajectories on the joint torques of kinematically
redundant manipulators. Vietnam Journal of Mechanics, vol. 29 (2007),
No.2, pp. 65-72.
6. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran
Hoang Nam : Inverse kinematic and dynamic analysis of redundant
measuring manipulator BKHN-MCX-04. Vietnam Journal of
Mechanics, vol. 32 (2010), No.1, pp. 15-26.
137
TI LIU THAM KHO

Ti liu ting Vit
1. Tun Anh: Bi ton ngc ng lc hc rbt d dn ng. n tt
nghip i hc - i hc Bch Khoa H Ni 2006.
2. R. H. Bishop: C in t, Tp 1 ( Phm Anh Tun v cc ng nghip
bin dch). NXB i hc Quc gia, H Ni 2006.
3. Phm Thng Ct: iu khin rbt n bc t do c nhiu tham s bt
nh trong khng gian Cc. Tuyn tp Hi ngh C hc ton quc
nm 2009, Tp 1, Tr. 195-204.
4. Phan Nguyn Di, Nguyn Vn Khang, Sanh: n nh chuyn ng
trong k thut. Nh xut bn Khoa hc v K thut, H ni 1986.
5. Nguyn Vn o : n nh ca cc h ng lc. Nh xut bn i hc
Quc gia H ni, H ni 1998.
6. L c t: V iu khin phi tuyn chuyn ng ca Robot d dn ng
trong khng gian thao tc. n tt nghip i hc, i hc Bch Khoa
H Ni 2008.
7. B. Heimann, W. Gerth, K. Popp : C in t. Nh xut bn Khoa hc v
k thut, H ni 2008 (Ngi dch: Nguyn Vn Khang, Nguyn Phong
in, Nguyn Quang Hong, Hong Minh Sn).
8. Nguyn Cng Hin, Nguyn Hong Mai: iu khin trt thch nghi
cho chuyn ng tay my. Tuyn tp Hi ngh khoa hc ton quc ln
th 6 v T ng ho, H Ni 2005.
9. o Vn Hip: K thut Rbt (in ln th 3). NXB Khoa hc v K
thut, H Ni 2006.
10. Nguyn Trn Hip, Phm Thng Ct: iu khin rbt theo nguyn l
trt s dng mng nron. Tp ch Tin hc v iu khin hc, Tp 24,
S 3, Tr.236-246.
11. Nguyn Th Hon, Phm Phu: C s phng trnh vi phn v l thuyt
n nh, Nh xut bn Gio dc, 2003.
12. Nguyn Quang Hong, Nguyn Vn Khang: V mt thut ton iu
khin trt rbt cng nghip. Tuyn tp Hi ngh C hc ton quc ln
th 8, Tp 1, Tr. 191- 199, H Ni 2007.
138
13. Nguyn Quang Hong, Nguyn Vn Khang, Trn Hong Nam: Bi ton
ng hc ngc rbt d dn ng c ch n s c kt khp. Tuyn
tp Hi ngh C hc ton quc, Tp 2, Tr. 282-290, H Ni 2009.
14. F. Holzweissig, H. Dresig : Gio trnh ng lc hc my. Nh xut bn
Khoa hc k thut, H ni 2001, (Ngi dch: Nguyn Vn Khang, V
Lim Chnh, Phan Nguyn Di).
15. L Minh Huy: Bi ton ngc v iu khin rbt d dn ng. n
tt nghip i hc, i hc Bch Khoa H ni 2009.
16. Nguyn Vn Khang: C hc k thut, Nh xut bn Gio dc, H Ni.
2009.
17. Nguyn Vn Khang: ng lc hc h nhiu vt. Nh xut bn Khoa hc
v K thut, H Ni 2007.
18. Nguyn Vn Khang, L c t, Trn Hong Nam: V mt phng
php s gii bi ton ng hc ngc robot dng chui. Tuyn tp Hi
ngh C hc ton quc ln th 8, Tp 1, Tr. 250-259. NXB Bch khoa,
H Ni 2007.
19. Nguyn Vn Khang, Nguyn Quang Hong, L c t, Trn Hong
Nam : V mt thut ton iu khin trt robot d dn ng. Tp ch
Tin hc v iu khin hc, Tp 24 (2008), No.3, Tr.269-280.
20. Nguyn Vn Khang, Nguyn Quang Hong, Trn Hong Nam: V bi
ton ng lc hc ngc rbt d dn ng. Tuyn tp Hi ngh khoa
hc cng ngh c kh ch to ton quc ln th hai, H Ni 2009.
21. Li Khc Li, Nguyn Vit Anh: Lp trnh Matlab v ng dng. Nh
xut bn Khoa hc v K thut, H ni 2006.
22. T Duy Lim: Rbt v h thng cng ngh rbt ha. NXB Khoa hc
v K thut, H Ni 2004.
23. V nh Nam: ng dng ca iu khin trt trong Robot cng nghip.
n tt nghip i hc - i hc Bch Khoa H Ni 2008.
24. inh Vn Phong: Phng php s trong C hc (in ln th 2). NXB
Khoa hc v K thut, H Ni 2006
25. Nguyn Thin Phc: Rbt cng nghip. Nh xut bn Khoa hc v K
thut, 2004.
26. Nguyn Don Phc: L thuyt iu khin nng cao. Nh xut bn Khoa
hc v K thut, 2005.
139
27. Nguyn Don Phc: L thuyt iu khin phi tuyn. Nh xut bn Khoa
hc v K thut, H Ni 2006.
28. Phm ng Phc: Rbt cng nghip. Nh xut bn Xy dng, H Ni
2007.
29. Nguyn Phng, Nguyn Th Hng Giang: C s t ng ha trong
ngnh c kh, Nh xut bn Khoa hc v K thut, H ni 2005.
30. Nguyn Ngc Thun, H Qunh : ng dng ma trn trong k thut. Nh
xut bn Khoa hc v k thut, H ni 1978.
31. Nguyn Phng Quang: Matlab/Simulink dnh cho k s iu khin t
ng. Nh xut bn Khoa hc v K thut, 2004.
32. L Hoi Quc, Chung Tn Lm: Nhp mn rbt cng nghip. Nh xut
bn Khoa hc v K thut, 2002.
33. Trn Th San (bin dch): C s nghin cu v sng to robot. Nh xut
bn Thng k, H Ni 2003.
34. Sanh : C hc gii tch. Nh xut bn Bch khoa H Ni, H ni
2008.
35. Sanh: C hc k thut (2 tp). NXB Gio dc, H Ni 2008.
36. Sanh, inh Vn Phong, Nguyn Thanh Thy, Nguyn Nht Quang,
Phan Mnh Dn, ngKhoa: Cng ngh tin hc v mt s bi ton
C k thut. K yu hi ngh c in t ton quc ln th 2, Tr. 182189. H Ni 2004.
37. Nguyn Hoi Sn, Thanh Vit, Bi Xun Lm : ng dng Matlab
trong tnh ton k thut. Nh xut bn i hc Quc gia Tp. H Ch
Minh 2002.
38. Nguyn Mnh Tin: iu khin rbt cng nghip. Nh xut bn Khoa
hc v K thut, H Ni 2007.
39. Trnh Quang Vinh, Nguyn ng Bnh, Phm Thnh Long: Rbt cng
nghip, Nh xut bn Khoa hc v k thut, H Ni 2008.
40. J. Wittenburg: ng lc hc h vt rn, (Ngi dch Nguyn ng Anh).
Nh xut bn Xy dng, H Ni 2000.
Ti liu ting Anh
41. T.L. Boulion, P.L. Odell : Generalized Inverse matrices . Wiley
Interscience, New York 1971.
140
42. H. Bruyninckx , O. Khatib : Gauss Principle and the Dynamics of

Redundant and Constrained Manipulators. Proceedings of the 2000
IEEE International Conference on Robotics & Automation, San
Francisco, CA , April 2000, pp. 2563-2568.
43. G.S. Chirikjian and J.W. Burdick: The Kinematics of Hyper-Redundant
Robot Locomotion. IEEE Transactions on Robotics and Automation,
Vol. 11, No. 6, Dec. 1995, pp. 781-793.
44. J. J. Craig: Introduction to Robotics: Mechanics and Control. Pearson
Prentice Hall, New Jersey 2005.
45. G.H. Golub and C.F Van Loan : Matrix computations (Third Edition).
The Johns Hopkins University Press, 1996.
46. Nguyen Quang Hoang, Nguyen Van Khang : On kinematic inverse and
control of redundant manipulators under consideration of jammed joint.
Proceeding of the ISRM 2009
47. P. Hsu, John Hauser, and Shankar Sastry: Dynamic Control of
Redundant Manipulators. Journal of Robotic Systems, 1989. pp 157168
48. R. Kelly, V. Statibanez : Control of Robot Manipulator in Joint Space.
Springer, London 2005.
49. Nguyen Van Khang, Do Thanh Trung : A contribution to the dynamic
simulation of robot manipulator with the software ROBODYN Vietnam
Journal of mechanics, vol. 26 (2004), No. 4, pp. 215-225.
50. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang
Nam : In fluence of trajectories on the joint torques of kinematically
redundant manipulators. Vietnam Journal of Mechanics, vol. 29 (2007),
N2, pp. 65-72.
51. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran
Hoang Nam : Inverse kinematic and dynamic analysis of redundant
measuring manipulator BKHN-MCX-04. Submitted to Vietnam Journal
of Mechanics.
52. T.R. Kurfess (Editor): Robotics and Automation Handbook. CRC Press,
2005.
53. D.C. Lay: Linear Algebra and Its Applications. Pearson Addison
Wesley, 2008.
141
54. F.L. Lewis, D.M. Dawson, C.T. Abdallah: Robot Manipulator Control:
Theory and Practice Second Edition. Marcel Dekker, Inc. Second
Edition, 2004.
55. R.H. Murray, Zexiang Li, S.S. Sastry, A. Makenratical: Introduction to
Robotic Manipulation. CRS Press, Boca Rator 1994.
56. R.K. Mittal, I.J. Nagrath: Robotics and Control. Tata Mcgraw-Hill
Publishing Company Limited, New Delhi, 2003.
57. Y. Nakamura : Advanced Robotics/ Redudancy and Optimination.
Addison Wesley Publishing Company, Reading 1991.
58. S.B. Niku : Introduction to Robotics. Analysis, Systems, Appliacation.
Prentice Hall, Inc. 2000.
59. R.V. Patel, F. Shadpey : Control of redundant robot manipulator,
Springer, Berlin 2005.
60. R. P. Paul: Robot
Manipulators/Mathamatics, Programming, and
Control. MIT Press, Cambridge Mass. 1981.
61. W. Perruquetti and J.P.B Barbot: Sliding Mode Control in Engineering.
Marcel D Dekker Inc., New York 2002.
62. C. R. Rao, S. K. Mitra : Generalized Inverse and Applications. John
Wiley & Sons Inc, 1971.
63. J.J.E. Slotine and W. Li: Applied Nonlinear Control. Prentice Hall,
Englewood Cliffs, New Jersey 1991.
64. R.J. Schilling : Fundamentals of robotics, Analysis and Control. Prentice
hall, Englewood Cliffs, New Jersey, 1990.
65. B. Sicilano: Kinematic Control of Redundant Robot Manipulators: A
Tutorial. Journal of Intelligent and Robotics Systems, 1990.
66. L. Sciavicco and B. Siciliano: A Solution Algorithm to the Inverse
Kinematic Problem for Redundant Manipulators. IEEE Journal of
Robotics and Automation, 1988, pp. 1046-1058
67. L. Sciavicco, B. Siciliano: Modelling and Control of Robot Manipulators
(2nd Edition), Springer-Verlag, London 2000.
68. G. Schreiber, C. Ott, G. Hirzinger: Interactive Redundant Robotics:
Control of the Inverted Pendulum With Null space Motion. Proceedings
of the 2001 IEEE/RSJ International Conference on Intelligent Robots and
Systems, Maui, Hawaii, USA, Oct. 29 Nov. 03, 2001, pp. 158-164.
142
69. M.W. Spong, S. Hutchinson, M. Vidyasagar: Robot Modeling

and Control. John Wiley and Sons Inc., New York 2006.
70. A. Visioli: Practical PID Control. Springer-Verlag, London 2006.
71. Q-G. Wang, Zhen Ye, Wen-Jian Cai, Chang-Chieh Hang: PID Control
for Multivariable Processes. Springer-Verlag, Berlin Heidelberg 2008.
72. N. Yagiz: Robust control of a spatial robot using sliding modes.
Mathematical & Computational Applications, Vol. 7, 2002, No. 3, pp.
219-228.

Luan An Tien Si - Nam

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Luan An Tien Si - Nam

Uploaded by

Copyright:

Available Formats

B GIO DC V O TO

TRN HONG NAM

GII BI TON NGC NG HC, NG LC

LUN N TIN S K THUT

TRN HONG NAM

GII BI TON NGC NG HC,

LUN N TIN S K THUT

Ngi hng dn khoa hc :

Trn Hong Nam

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

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

Gii bi ton ng hc thun rbt d dn ng bng phng php

Tnh ton ng lc hc ngc rbt d dn ng

2.1 Dng thc Lagrange loi 2 ca h nhiu vt

iu khin trt rbt d dn ng da trn thut ton s

Chng 4. Bi ton ngc ng hc, ng lc hc

Tng quan v phng php o chnh xc b mt cc chi tit my

Danh mc cng trnh ca tc gi

Ti liu tham kho

Khong dch chuyn dc trc xi (chiu di ca khu th i)

Sai s gia tc ca bn kp.

Vc t cc lc Coriolis v lc hng tm ca b iu khin.

Khong dch chuyn gc ta Oi-1 v Oi.

Tenx qun tnh khi ca vt rn th i.

Rbt trn dy chuyn ca trung tm sn xut linh hot

Sai s v tr ca im thao tc theo trc x

Thut ton gii bi ton ng lc hc ngc

Mmen trn khp ng th 6

th sai s suy rng s2

M hnh rbt o BKHN-MCX-04

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

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

Trong cc ti liu v rbt [25, 28, 32, 39] ngi ta a ra mt s khi

in t hc m cc rbt c thm nhiu tnh nng c bit. Nh vy m cc

3. Tng quan v vn nghin cu

nghin cu ng hc ca rbt, Denavit v Hartenberg [28, 38, 39,

th i) v c t ti giao im ca trc khp ng th i+1 vi ng vung

Hnh 1.1 S thit lp h ta cc khu

trc x 0 th do khng c khp ng th 0 nn ta c th chn ty , min l

Sau khi thit lp xong cc h ta , ta thy v tr ca h ta O i x i y i z i

- Dch chuyn tnh tin dc trc x i mt on ai.

Cn khi dch chuyn tnh tin theo trc z i 1 mt on d i th ma trn bin

bc dch chuyn tnh tin theo trc x i mt on a i th ma trn chuyn i

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

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

H i = A ( z,i ) .A ( z,di ) A ( x ,ai ) A ( x ,i )

Ly tch ca 4 ma trn trn, ta c mt ma trn c gi l ma trn

Vi vic s dng cng thc (1.3) ta s thc hin c vic chuyn i ta

1.1.2 Phng trnh xc nh v tr v hng ca khu thao tc

Xt mt m hnh tng qut ca mt rbt c n khu nh trn hnh v 1.2.

Ma trn D n cho ta bit v tr ca im P v hng ca khu thao tc (bn

1.1.3 Bi ton p dng

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

Trong ta s dng cc k hiu vit tt: Si = sin(qi), Ci = cos(qi).

0 a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + a 5 C12345

vi S12i = sin(q1+q2+qi); C12i = cos(q1+q2++qi).

Ta nhn thy khi m < n th phng trnh (1.1) s l mt h m phng

Trong ma trn Jacobi J(q) l mt ma trn ch nht. V vy, khi gii

Cho mt ma trn ch nht A R . Ma trn X R nxm s c gi l ma

1.2.2 Ma trn ta nghch o Moore - Penrose

Ta nghch o ca ma trn thng dng l ta nghch o MoorePenrose (MP), c k hiu l A+ [62].