B GIO DC V O TO

VIN KHOA HC V CNG NGH

VIT NAM

Cng trnh c hon thnh ti:

VIN C HC - VIN KHOA HC V CNG NGH VIT NAM

VIN C HC
Ngi hng dn khoa hc :
1. GS. TSKH. Nguyn Vn Khang, Trng H Bch khoa H Ni
2. PGS. TS. Nguyn Phong in, Trng H Bch khoa H Ni
TRN HONG NAM
Phn bin 1:
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
M s

: C hc k thut
: 62.52.02.01

TM TT LUN N TIN S K THUT

H NI 2010

Phn bin 2:
Phn bin 3:

Lun n s c bo v trc hi ng chm lun n cp nh nc

hp ti Vin C Hc - Vin Khoa hc v Cng ngh Vit Nam

Vo hi .. gi

ngy thng

nm 2010

C th tm hiu lun n ti :
- Th vin Quc gia
- Th vin Vin C hc Vit Nam

CC CNG TRNH KHOA HC CNG B:

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.

M U
1. Tnh cp thit ca ti nghin cu
Theo cc ti liu v rbt, 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. Nh tnh d dn ng m rbt d dn ng c kh nng trnh c
cc im k d, cc gii hn ca bin khp, cc vt cn
Khi nghin cu rbt ta phi gii quyt cc bi ton v ng hc, ng lc
hc v iu khin. Trong cc bi ton ny th cc bi ton ngc l cc bi
ton kh, nht l i vi cc bi ton ngc ca rbt d dn ng. Bi ton
ngc ca rbt d dn ng nc ta hy cn t c nghin cu. Do vic
nghin cu, tm ra phng php mi gii bi ton ngc l vic lm cp
thit v v vy tc gi chn ti nghin cu l: 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.
2. Mc ch nghin cu
Xy dng mt thut ton a li chnh xc cao khi gii cc bi ton
ngc ng hc, ng lc hc v iu khin dng trt rbt d dn ng.
3. i tng v ni dung nghin cu
i tng nghin cu ca lun n l cc rbt d dn ng.
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.
4.

Cc phng php nghin cu

Phng php t ng ha thit lp cc phng trnh ng hc v ng

lc hc ca h nhiu vt.
Phng php m phng s da trn phn mm a nng MATLAB v
MAPLE.
Phng php thc nghim.

5. Nhng ng gp mi ca lun n
xut thut ton hiu chnh gia lng vc t ta suy rng v p
dng n gii cc bi ton ngc ng hc, ng lc hc v iu khin
chuyn ng ca rbt d dn ng bng phng php trt. tin hnh gii
mt s v d minh ha chng t tnh u vit ca phng php gii bi ton
ngc khi s dng thut ton hiu chnh gia lng vc t ta suy rng so
vi khi gii bi ton m khng s dng thut ton hiu chnh gia lng vc t
ta suy rng.

6. B cc ca lun n
Lun n c 142 trang. Ngoi cc phn m u, kt lun chung, ti liu
tham kho, cc cng trnh lin quan n lun n, ni dung chnh ca lun n
c trnh by trong 4 chng :
Chng 1: Tnh ton ng hc ngc rbt d dn ng bng thut ton
hiu chnh gia lng vc t ta suy rng.
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.
Chng 3: iu khin trt rbt d dn ng da trn thut ton s
hiu chnh gia lng vc t ta suy rng.
Chng 4: ng lc hc v iu khin trt rbt o BKHN-MCX-04.
L chng p dng cc kt qu nghin cu l thuyt cho mt m hnh rbt o
BKHN-MCX-04 mi c ch to.
CHNG 1
TNH TON NG HC NGC RBT D DN NG
BNG THUT TON HIU CHNH GIA LNG
VCT TA SUY RNG
T vic gii bi ton ng hc thun ta xc nh c quan h
x = f(q)
(1.1)
cn khi gii bi ton ngc ta phi xc nh quan h hnh thc c suy ra t
biu thc (1.1) di dng
q = f-1(x)
(1.2)
1.1 Phng php khai trin Taylor
Trong cc cun sch [48, 51] trnh by mt thut ton s nh sau:
q( t k +1 ) = q( t k ) + q& ( t k )t
(1.3)

& ( t k ) c xc nh t cng thc

Trong q
q& ( t k ) = J 1 (q& ( t k ))x& ( t k )

(1.4)

Th (1.4) vo (1.3) ta c
q( t k +1 ) = q( t k ) + J 1 (q& ( t k ))x& ( t k )t

(1.5)

Kt qu tnh ton vc t ta suy rng theo (1.5) l kh th. Do ta

phi tm cch ci tin cng thc (1.5) c chnh xc cao hn.
1.2 Cc cng thc xc nh vc t vn tc v vc t gia tc suy rng
T bi ton ng hc thun, ta c h thc
x = f (q)

(1.6)
2

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

f
x& =
q& = J (q)q&
q

(1.7)

Trong :
f1
q
f 1
= ...
J (q) =
q f m
q1

f1
q 2
...
f m
q 2

f1
q n

... ...
f m
...
q n

...

(1.8)

Gi s J(q) c hng y . Theo [41, 56], ta chn ta nghch o ca

J(q) di dng

J + (q) = J T (q) J (q)J T (q)

(1.9)

Khi t biu thc (1.7) ta suy ra cng thc tnh vc t vn tc suy rng:
q& ( t k ) = J + (q( t k ))x& ( t k )

(1.10)

V suy ra:
&&( t ) = J + (q( t ))&x&( t ) + J& + (q( t ))x& ( t )
q

(1.11)
&+

p dng c cng thc (1.11) cn phi tnh c J (q( t )) .

T biu thc (1.9) ta suy ra :
J + (q( t ) )J (q( t ) )J T (q( t ) ) = J T (q( t ) )

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

(1.12)

J& + (q )J (q )J T (q ) + J + (q ) J& (q )J T (q ) + J (q )J& T (q ) = J& T (q )

T (1.13) ta suy ra

]}[

(1.13)

1
J& + (q ) = J& T (q ) J + (q ) J& (q )J T (q ) + J (q )J& T (q ) J (q )J T (q )
(1.14)

Ma trn J& (q ) c tnh bng cch o hm trc tip cc phn t ca ma

&&( t ) .
trn J (q ) theo thi gian. Th (1.14) vo (1.11) ta tm c gia tc q
1.3 Cc cng thc xc nh vc t ta suy rng
p dung khai trin Taylor i vi qk+1 quanh gi tr qk ta c
1
&& k (t ) 2 + ...
q
(1.15)
2
Th biu thc (1.10) vo (1.14) v b qua cc v cng b bc 2 ta c :
q k +1 = q( t k + t ) = q k + q& k t +

q k +1 = q k + J + (q k )x& t vi k = 0, 1, , N-1

(1.16)

T , ta c cc bc tnh ton nh sau:

1.

Tm q 0 .

2.
3.
4.

Tnh
Tnh
Tnh

J (q 0 ), J + (q 0 ), J& (q 0 ) .
&&(t = 0 ) = q
&& 0 theo (1.11).
q& ( t = 0) = q& 0 theo (1.10) v tnh q
&& k +1 theo(1.10) v (1.11).
q k +1 theo (1.14), ri tnh q& k +1 , q

Ta thy vic tnh q k +1 theo (1.16) l kh th. V vy ta cn c mt thut ton

xc nh q k +1 chnh xc hn. Trong lun n a ra thut ton hiu chnh
gia lng vc t ta suy rng q k +1 khi bit qk . S khi ca thut ton
ny c trnh by trn hnh 1.1.
1.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, ca vn tc v ca gia tc nh sau
e(t k ) = x(t k ) xk = x(t k ) f (qk )
(1.17)
e& (t k ) = x& (t k ) x& k = x& (t k ) J(qk )q& k
&e&(t ) = &x&(t ) &x& = &x&(t ) J& (q )q& J(q )q
&&
k

k +1

Trong e( t k ) = [e1( t k ) e2 ( 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 thut ton 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 )

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.1.

S khi gii bi ton ng hc ngc

5

1.5 V d minh ha
Gii bi ton ng hc ngc ca rbt d dn ng 5 khu ng nh hnh v
q3

x1

x2

q4

x3

q2

q1
O

q5

x4

x5

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 thao tc E l:
xE = 0.8+0.1cos(2t) (m); yE = - 0.8+0.1sin(2t) (m)
Bn kp ca rbt phi lun to vi phng thng ng 1 gc =1(rad).
Qua vic p dng thut ton hiu chnh gia lng vc t ta suy rng ta tm
c quy lut chuyn ng ca cc khp ng l :
4
q1 [rad]
3

qd1 [1/s]

qdd1 [1/s 2]

1
0
-1
-2

5
time [s]

10

Hnh 1.2 Cc c tnh chuyn ng ca khu 1

1.5
1
0.5
0
-0.5
q2 [rad]

-1

qd2 [1/s]

-1.5
-2

qdd [1/s 2]
2

5
time [s]

10

Hnh 1.3. Cc c tnh chuyn ng ca khu 2

2
1
0
-1
q3 [rad]
qd3 [1/s]

-2

qdd [1/s 2]
-3

5
time [s]

10

Hnh 1.4. Cc c tnh chuyn ng ca khu 3

6

6
q4 [rad]
qd4 [1/s]

qdd [1/s 2]
4

2
0
-2
-4

5
time [s]

10

Hnh 1.5. Cc c tnh chuyn ng ca khu 4

4
2
0
-2
-4
q5 [rad]

-6

qd5 [1/s]

-8
-10

qdd5 [1/s 2]
0

5
time [s]

10

Hnh 1.6. Cc c tnh chuyn ng ca khu 5

-16

x 10

6
4

ex [m]

2
0
-2
-4
-6

5
time [s]

10

Hnh 1.7. Sai s theo trc x ca im thao tc E

-15

x 10

ey [m]

1
0

-1

5
time [s]

10

Hnh 1.8. Sai s theo trc y ca im thao tc E

-15

2.5

x 10

e [rad]

1.5
1
0.5
0
-0.5
-1

5
time [s]

10

Hnh 1.9. Sai s gc nh hng ca bn kp

7

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.10. Dng chuyn ng ca rbt theo kt qu tnh ton

Cc v d trong lun n chng t tnh u vit ca phng php gii bi
ton ngc ng hc rbt d dn ng khi s dng thut ton hiu chnh gia
lng vc t ta suy rng (t chnh xc c 10-15) so vi phng php
gii khng s dng thut ton (ch t chnh xc 10-4).
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
2.1 Phng trnh ng lc hc ca rbt
Trong cc ti liu v rbt ta c biu thc:
&& + C(q, q& )q& + g (q) =
M (q)q
trong :
l vct ta suy rng,
q Rn

M(q) R nn
C(q, q& ) R

g(q) R

Rn

n n

(2.1)

l ma trn khi lng,

l ma trn lin quan lc coriolis, lc ly tm,
l vct lc do trng lc,
l vct lc/mmen dn ng t cc ng c.

2.1

Gii bi ton ngc ng lc hc rbt d dn ng trong khng

gian thao tc
Khi tnh ton, thit k rbt ta thng phi xc nh cc lc/mmen dn
ng cn thit tc ng trn cc khu ca rbt khu thao tc ca rbt c
8

th lm vic theo mt chng trnh nh trc. Bi ton ny c gi l bi

ton ng lc hc ngc.
Mi lin h gia v tr ca bn kp vi cc bin khp c dng
x = f(q)
(2.2)
n
m
trong q R l vct cha cc bin khp, x R l vct cha v tr
tm v hng ca bn kp trong mt h ta c nh.
o hm 2 v biu thc (2.2) theo thi gian, ta c

f
q& = J (q)q&
q
T (2.3) ta c
q& = J + (q)x&
o hm 2 v ca (2.4) theo thi gian, ta c
&& = J + (q)&x& + J& + (q)x&
q
x& =

(2.3)

(2.4)
(2.5)

d
vi J& + (q) = J + (q).
dt
S dng thut ton hiu chnh gia lng vc t ta suy rng chng ta s
&& ti cc thi im khc nhau.
xc nh c q, q& , q
Do phng trnh ng lc hc ca rbt c dng (2.1), nn s dng file s
&& ca bi ton ng hc ngc th t (2.1) ta c th xc
liu cc vc t q, q& , q
nh c mmen/lc cn thit tng ng vi chuyn ng mong mun x(t)
ca bn kp.
V vy ta c cc bc tin hnh tnh mmen/lc ca ng c bn kp
chuyn ng theo mt quy lut x(t) nh trc nh sau:
1. Gii bi ton ng hc ngc xc nh cc ta , vn tc v gia
&& ca cc khp ng t phng trnh chuyn ng
tc suy rng q, q& , q
&
ca bn kp x( t ), x( t ), &x&( t ) .

S dng phng trnh (2.1) tnh cc mmen/lc ca cc ng c

dn ng.
T ta xy dng c s khi gii bi ton ng lc hc ngc da trn
thut ton hiu chnh gia lng vc t ta suy rng nh sau (hnh 2.1):
2.

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&
&
&
q& k = J + (q
=
J
(qk )&x&k + J& + (q
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.1 S khi gii bi ton ng lc hc ngc rbt d dn ng

10

CHNG 3
IU KHIN TRT RBT D DN NG
DA TRN THUT TON HIU CHNH GIA LNG
VC T TA SUY RNG
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, 49, 50].
3.1

Bi ton iu khin chuyn ng ca rbt

q2
Hnh 3.1

q3
xP

q1

Nhim v ca bi ton iu khin chuyn ng rbt l m bo cho khu

thao tc lun bm theo qu o cho trc trong khng gian thao tc.
T ta c cc dng h thng iu khin nh sau:
H thng iu khin trong khng gian khp.
H thng iu khin trong 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 n l kh m bo chnh
xc cho v tr ca khu thao tc do s tn ti cc sai lch trong c cu dn ng
(khe h ca cc khp, ma st v.v) v thiu thng tin v sai lch x x d trong
qu trnh iu khin.

11

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 xd 0
Trong x d l vc t v tr mong mun ca khu thao tc, 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 khi lng tnh ton s ln do
thi gian iu khin s lu.
3.2

iu khin trt rbt d dn ng

Phng trnh ng lc hc ca rbt c dng
&& + C(q, q& )q& + g(q) + d =
M (q)q
Trong :
M(q)
- ma trn khi lng,
C(q, q& )q& - vct cha cc lc crilis v lc ly tm,
g(q)
- vct cha cc lc do trng trng,
d
- vct cha cc lc/mmen do kch ng nhiu,

- lc/mmen ca ng c dn ng.

(3.1)

s dng iu khin dng trt, ta a vo k hiu vc t sai s bm c

xc nh bi
e( t ) = q ( t ) q d ( t )
(3.2)
v vc t sai s suy rng nh sau
s = e& ( t ) + e( t )
(3.3)
trong
= diag (1 , 2 ,..., n ) , i > 0 .
(3.4)
Nhim v ca bi ton iu khin l chuyn hm mc tiu e( t ) 0 sang
hm mc tiu s( t ) 0 khi t
e( t ) 0 s( 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 )
t suy ra
q& ( t ) q& r ( t ) = q& ( t ) q& d ( t ) + e( t ) = e& ( t ) + e( t ) = s( t )
do
&&( t ) q
&& r ( t )
s& ( t ) = q
tm lut iu khin, ta chn hm Lyapunov nh sau

12

(3.5)

(3.6)
(3.7)

1 T
s 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
2
T (3.1) v ch n (3.7) ta c
&&( t ) q
&& r ( t )) + M (q)q
&& r ( t ) + C(q, q& )q& + g (q) + d =
M (q )(q
V=

(3.8)

(3.9)

&& r d
M (q )s& = C(q, q& )q& g(q) M (q )q
T (3.6) ta suy ra
q& (t ) = s + q& r (t )
Do ta c
C(q, q& )q& = C(q, q& )s + C(q, q& )q& r
Th (3.12 ) vo (3.10), ta c
&& r C(q, q& )q& r g (q) d
M (q)s& = C(q, q& )s M (q)q

(3.10)
(3.11)
(3.12)
(3.13)

Thay (3.13) vo (3.9) ta c

1&

& = sT[ M(q)q

&&r C(q,q&)q& r g(q) d] + sT M
(3.14)
V
(q) C(q,q&)s
2

1 T &
Do tnh cht
s M (q)s s T C(q, q& )s = 0
2
Ta suy ra
& = s T [ M (q)q
&& r C(q, q& )q& r g (q) d]
V
(3.15)
Cn c vo (3.15) 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.16)
=M
r
pd
s

Trong : sgn(s) = [sgn(s1 ), sgn(s 2 ),..., sgn(s n )]T , 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

11
22
nn
22
nn
K pd = diag k 11
pd , k pd ,..., k pd v K s = diag k s , k s ,..., k s

Vi cch chn (3.16), h thc (3.15) tr thnh

~
~
& = s T K s s T K sgn(s) + s T d + s T M
&& r + C(q, q& )q& r + ~
V
(q)q
g (q) =
pd
s

i =1

ii ) 2
k (pd
si

k s(ii ) | s i | +

i =1

s i (d i + i )
i =1

13

&
V

i =1

i =1

k (pdii )s i2 ( k s(ii ) | d i + i |) | s i |

vi cc sai lch gia m hnh thc s dng trong (3.1) v cc thng s m hnh
s dng trong b iu khin (3.16) nh sau
~
(q)

M (q) = M (q) M

~
(q, q& )
(3.17)
C(q, q& ) = C(q, q& ) C
~

g (q) = g(q) g(q)

~
~
&& + C(q, q& )q& + ~
v = 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 . Thnh phn Kpd ch l thnh phn iu khin PD thm vo

rt ngn thi gian chuyn tip.
T cc vn nu trn, xy dng c s khi gii bi ton iu
khin chuyn ng rbt d dn ng trong khng gian khp theo phng
php trt nh trn hnh 3.3.
Do c im ca hm sgn(si) l khng lin tc ti gi tr si = 0, do b
iu khin s xy ra hin tng chattering. kh chattering ta thay hm du
sgn(s) bng hm bo ho sat (s / ) .
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,

ii )
thnh phn k (pd
v k s(ii ) trong phm vi sai s suy rng si nm trong khong
u ng vai tr l b iu khin PD.
x
x <1
Vi sat ( x ) =
x 1
sgn( x )

Hnh 3.2 Hm sat(x)

14

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.3.

S tnh ton v m phng iu khin rbt

15

CHNG 4
BI TON NGC NG HC, NG LC HC
V IU KHIN TRT RBT O BKHN-MCX-04
Rbt o BKHN-MCX-04 c thit k v ch to tin hnh cc bi
ton ng hc ngc, ng lc hc ngc v iu khin chuyn ng.
4.1 Kt cu ca r bt o BKHN-MCX-04
Rbt o BKHN-MCX-04 c hnh dng nh hnh 4.1 v c cc thng s
hnh hc nh trong bng 4.1.
3
4

Qu o nh trc
Qu o

5
1

i tng cn o
i

Hnh 4.1. M hnh rbt

o BKHN-MCX-04

Bng 4.1. Thng s hnh hc ca rbt

Oi-1O i (m)
Khu i
1
0.14
2
0.15
3
0.20
4
0.0
5
0.163
6
0.080
4.2 Tnh ton ng hc ngc
T hnh 4.1, ta xc nh c cc tham s DH nh trong bng 4.2.
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
16

ai
0
a2
a3
0
0

i
/2
0
0
-/2
/2

T bng tham s DH ta xc nh c cc ma trn H1, H2, H3, H4, H5 v sau

l ma trn D5 = H1 H2 H3 H4 H5
C1C 234C5 S1S5
S C C + C S
1 5
D5 = 1 234 5

S234C5

C1S234

S1C5 + C1S5C 234

S1S234

C1C5 + S1S5C 234

C 234

S5S234

d 5C1S234 + a 2C1C 2 + a 3C1C 23

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

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 C1S 234
y E = a 2 S1C 2 + a 3S1C 23 d 5S1S 234
z E = a 2 S 2 + a 3S 23 + d 5 C 234 + d1
Phng trnh (4.1) biu din quan h
x = f(q)
T (4.1) ta xc nh c ma trn Jacobi J(q)
J11 J12 J13 J 14 J 15
J (q) = J 21 J 22 J 23 J 24 J 25
J 31 J 32 J 33 J 34 J 35

(4.1)

(4.2)

(4.3)

Trong cc phn t ca ma trn J(q) c dng

J11 = -d5 S1 S234 - a3 S1 C23 - a2 S1 C2
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(q) c xc nh theo h thc (4.3) v mt qu o nh
trc ca im E, ta c th xy dng chng trnh tnh ton cc gi tr bin
&& i (i = 1,2,...,5) da trn thut gii c trnh
khp qi v cc o hm q& i , q
17

by trong chng 1.
V d im E c qu o chuyn ng l mt ng xon c bm trn mt
mt cu nh hnh 4.2.
Qu o im E

Hnh 4.2. Qu o nh trc ca im E (ng xon c)

Quy lut chuyn ng ca im E c dng
x E = 3 + 0.1 sin(2t ) sin(t / 3);
y E = 0.1 cos(2t ) sin(t / 3)

(4.4)

z E = 0.12 + 0.1 sin(t / 3)

trong cc ta c tnh theo n v o l mt.
Kt qu tnh ton bi ton ng hc ngc c biu din trn cc hnh
4.3, a, b v c. Tham s gc quay q5 l hng s v khng c nh hng n qu
o chuyn ng ca im E.

Hnh 4.3.

a. th bin khp q
b. th vn tc gc
c. th gia tc gc

4.3 Tnh ton ng lc hc ngc

B qua ma st ti cc khp v cc lc cn khc v xem lc tc ng ca
vt cn o ln khu 6 ti v tr tip xc l khng ng k.
Phng trnh ng lc c biu din di dng tng qut nh sau:
&& + C(q, q& )q& + g(q) =
M(q)q
(4.5)
18

Gi s quy lut chuyn ng ca im E (im O5) trn mt phng

{Ox 0 z 0 } c m t bi cc phng trnh ta (4.6) v c minh ha trn

hnh 4.4.

x E = 0 .2 + 0 .12 1 cos t ( m );
4

z E = 0 .14 + 0.12 sin t

(m )
4

yE = 0

(4.6)

Vi cc thng s ng lc hc xc nh theo bng 4.4 v v tr khi tm

ca cc khu theo bng 4.3
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 )
1

-(d1-l1)

-(a2-l2)

-(a3-l3)

l4

-(d5-l5)

-(a6-l6)

Bng 4.4. Cc thng s ng lc hc ca rbt o

Ixi
Iyi
mi
Izi
Khu i
(kgm2)
(kgm2)
(kg)
(kgm2)
1
2.0
4.010-3
3.010-3
1.010-3
-3
-3
2
0.9
0.210
3.010
3.010-3
3
1.2
0.510-3
3.510-3
4.010-3
-3
-3
4
1.1
0.610
2.510
3.510-3
-3
-3
5
0.5
0.710
0.210
0.310-3
-4
-4
6
0.05
0.310
0.210
0.110-4

Hnh 4.4

Hnh 4.5 (a)

19

li
(m)
0.10
0.06
0.10
0.04
0.03
0.02

Qua tnh ton, ta c c kt qu gii bi ton ng lc hc ngc nh

trn cc hnh 4.5 a, b, c v d.

b.
Hnh 4.5

c.
d.
Tr s cc bin khp 2, 3 v 4
Vn tc gc ca ng c dn ng khu 2, 3, 4
Gia tc gc ca ng c dn ng khu 2, 3, 4
Mmen dn ng cn thit cc khu ca rbt.

a.
b.
c.
d.

4.4 iu khin trt rbt BKHN-MCX-04

Gi s chi tit cn o c dng hnh cu nh trn hnh 4.2
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. Gi s phng
trnh ca ng xon c c dng
x

= 3 + 0 . 1 sin( 2 t ) sin( t / 3 )

= 0 . 1 cos( 2 t ) sin( t / 3 )

= 0 . 12 + 0 . 1 sin( t / 3 )

Cc bi ton ng hc ngc v ng lc hc ngc cho trng hp ny

c gii quyt trong phn 4.2 v 4.3. Ta s s dng cc kt qu ny cho
phn iu khin.
n nh chuyn ng ca im O5 ta chn hm Lyapunov dng
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 = e& ( t ) + e( t )
trong e(t), 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 gii bi ton iu khin trt cho rbt chng 3,
vi cc thng s ca b iu khin c chn :
Ks= diag([20,20,20,20,1,0.1]);
Kp = diag([0.2,0.2,0.2,0.2,0.02,0.01]);
= diag([50,50,50,50,30,20]);
Sai s c lng ca m hnh 5%, Nhiu d(t)=random(6,6)*0.01;
20

Ta thu c kt qu iu khin chuyn ng rbt BKHN-MCX-04 nh sau:

a.

b.

Hnh 4.6

c.

a. th ta x(t) trong khng gian thao tc

b. th ta y(t) trong khng gian thao tc.

c. th ta z(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, thi gian im O5 t c
qu o mong mun l 0.25 giy.
4.5 Th nghim
Th nghim c tin hnh 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.7 M hnh th nghim rbt o BKHN-MCX-04

4.5.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
21

4.5.2 Nguyn l hot ng ca h thng th nghim

Trn rbt, ti cc khp ng ta gn cc n led. Khi rbt chuyn ng,
camera chp li nh v lu vo b nh ca my tnh. Phn mm x l nh s
xc nh v tr tm ca cc khp v lu vo tp toadotam.txt ca cc khp.
Chng trnh iu khin rbt c xy dng bng ngn ng Visual
Basic. Thng qua dao 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.
Di y l kt qu th nghim tin hnh trn rbt BKHN-MCX-04
4.5.3 Kt qu th nghim
Gi s qu o mong mun ca bn kp ca rbt BKHN-MCX-04 c
dng l na vng trn vi phng trnh chuyn ng l
x E = 0.2 + 0.12(1 cos(t / 4) (m);
yE = 0
z E = 0.14 + 0.12 sin(t / 4)

( m)

Qua bi ton ng hc ngc ta nhn c gi tr ta im cui E v

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

Hnh 4.8

Qu o thc nghim ca im cui E

Qua kt qu th nghim ta thy s khc bit gia qu o chuyn ng

mong mun ca khu thao tc v kt qu chuyn ng thc thu c qua th
nghim l khng ng k. iu chng t thut ton m chng ti s
dng gii bi ton ng hc ngc l hon ton ph hp.
4.6 Kt lun chng 4
Trn c s rbt o t ch to, 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 khng n gin nu ta s dng cc cng c tnh ton
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.

23

KT LUN CHUNG
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 rbt.
Nhng kt qu chnh ca lun n
1. 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. 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. tham gia ch to rbt o BKHN-MCX-04 qua xc nh
c cc tham s ng hc, ng lc hc v tin hnh cc bi ton
ngc ng hc, ng lc hc v iu khin trt cho rbt t ch to
ny. tin hnh th nghim nh trn m hnh.

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.
Mt s vn c th tip tc m rng nghin cu
- 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 nht.
- Nghin cu trin khai ng dng trong thc t sn xut cc kt qu
nghin cu c.

24