I.

X©y dùng ph-¬ng tr×nh ®éng häc thuËn vµ ng-îc biÓu diÔn
mèi quan hÖ gi÷a hÖ to¹ ®é tay Robot (End effector) vµ hÖ täa
®é c¸c khíp (Joints)

Robot hai thanh nèi lµ robot víi 2 kh©u ph¼ng, v× vËy c¬ chÕ vµ ph¹m vi ho¹t
®éng cña robot n»m trong mÆt ph¼ng, bao gåm hai khíp quay vµ hai thanh nèi nh-
y0
h×nh vÏ: y2
x2

a2 02
y1

2
a1
x1
01
1
x0
00
Z1 Khíp 2
Khíp 1

1. ThiÕt kÕ hÖ to¹ ®é cho c¸c thanh nèi

a. Tham sè cña thanh nèi vµ khíp
X¸c ®Þnh bé th«ng sè Denavit -Hartenberg (b¶ng th«ng sè DH)
Tõ h×nh vÏ robot ta x¸c ®Þnh ®-îc c¸c th«ng sè sau:
+ §é dµi ph¸p tuyÕn chung cña khíp 1 vµ khíp 2 lµ a1

+ Gãc chÐo gi÷a hai trôc khíp 1 vµ khíp 2 lµ 1 .

+ §©y lµ khíp quay cho nªn kho¶ng c¸ch ®o däc trôc khíp ®éng 1 tõ ®-êng
vu«ng gãc chung gi÷a trôc khíp ®éng 2 vµ trôc khíp ®éng 1 tíi ®-êng vu«ng gãc
chung gi÷a khíp ®éng 1 vµ gèc robot lµ d1 = 0. T-¬ng tù ta còng dÔ dµng cã d2 = 0.

+ Gãc gi÷a hai ®-êng vu«ng gãc chung lµ 1

T-¬ng tù xÐt khíp 2 vµ tay robot

+ Ta cã ®é dµi ph¸p tuyÕn chung lµ a2

+ Gãc chÐo gi÷a hai trôc khíp 2 vµ tay robot lµ 1 .

+ Gãc gi÷a hai ®-êng vu«ng gãc chung lµ 2

VËy biÕn khíp lµ 1 vµ 2

b. ThiÕt lËp hÖ to¹ ®é

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 HÖ to¹ ®é cè ®Þnh o0x0y0z0 cã gèc o0 ®Æt t¹i t©m trôc khíp ®éng 1, chiÒu x0
h-íng tõ o0 ®ªn o1
HÖ to¹ ®é o1x1y1z1 cã gèc o1 ®Æt t¹i t©m trôc khíp ®éng 2
HÖ to¹ ®é o2x2y2z2 cã gèc o2 ®Æt t¹i t©m trôc khíp ®éng cuèi kh©u 2.
Ba trôc z0, z1, z2 vu«ng gãc víi mÆt ph¼ng tê giÊy.

2. X©y dùng ph-¬ng tr×nh ®éng häc thuËn cho robot

Ph-¬ng tr×nh ®éng häc thuËn lµ ph-¬ng tr×nh biÓu diÔn quan hÖ vÞ trÝ vµ h-íng
cña tay robot th«ng qua c¸c biÕn khíp. C¸c biÕn khíp nµy lµ gãc quay cña khíp quay
vµ ®é dÞch chuyÓn tÞnh tiÕn ®èi víi khíp tÞnh tiÕn
C¨n cø vµo ph-¬ng tr×nh ®éng häc thuËn nµy khi biÕt vÞ trÝ cña c¸c khíp ta cã
thÓ x¸c ®Þnh ®-îc vÞ trÝ vµ h-íng cña tay robot.
C¨n cø vµo c¸c th«ng sè vµ hÖ to¹ ®é ®· ®-îc thiÕt lËp ta cã b¶ng th«ng sè DH
cña robot nh- sau:
Thanh (kh©u) i i ai Di
1 * 0 l1 0
1

2 * 0 l2 0
2

*. Quan hÖ gi÷a hai khung to¹ ®é o1x1y1z1 vµ o0x0y0z0 ®-îc x¸c ®Þnh nh- sau:

- Quay xung quanh trôc z0 mét gãc 1 sao cho trôc x0 trïng víi ph-¬ng cña trôc x1
- TÞnh tiÕn däc theo trôc x1 (ph-¬ng ph¸p tuyÕn chung) mét ®o¹n b»ng a1
VËy phÐp biÕn ®æi tæng hîp nh- sau:

A1 = Rot(z0, 1 ) Trans (a1,0,0)

Trong ®ã: Rot(z0, 1 ) lµ phÐp quay quanh trôc z0 mét gãc 1

cos 1
sin 1
0 0

sin 1
cos 1
0 0
Rot(z0, 1 )=
0 0 1 0

0 0 0 1

Trans(a1,0,0) lµ phÐp tÞnh tiÕn khung to¹ ®é o0x0y0z0 theo trôc x0 mét ®o¹n b»ng a1.
1 0 0 a1

0 1 0 0
Trans(a1,0,0) =
0 0 1 0

0 0 0 1

CuuDuongThanCong.com https://fb.com/tailieudientucntt
VËy 0A1 ®-îc tÝnh nh- sau:
cos 1
sin 1
0 0 1 0 0 a1

0 sin 1
cos 1
0 0 0 1 0 0
A1 = Rot(z0, 1 ) Trans (a1,0,0) =
0 0 1 0 0 0 1 0

0 0 0 1 0 0 0 1

cos 1
sin 1
0 a 1 cos 1

0 sin 1
cos 1
0 a 1 sin 1
A1 = (1.1)
0 0 1 0

0 0 0 1

*. Quan hÖ gi÷a hai khung to¹ ®é o2x2y2z2 vµ o1x1y1z1 ®-îc x¸c ®Þnh nh- sau:

- Quay xung quanh trôc z1 mét gãc 2 sao cho trôc x1 trïng víi ph-¬ng cña trôc x2
- TÞnh tiÕn däc theo trôc x2 (ph-¬ng ph¸p tuyÕn chung) mét ®o¹n b»ng a2
VËy phÐp biÕn ®æi tæng hîp nh- sau:
1
A2 = Rot(z1, 2 ) Trans (a2,0,0)

Rot(z1, 2 ) lµ phÐp quay quanh trôc z1 mét gãc 2

cos 2
sin 2
0 0

sin 2
cos 2
0 0
Rot(z1, 2 )=
0 0 1 0

0 0 0 1

Trans(a2,0,0) lµ phÐp tÞnh tiÕn khung to¹ ®é o1x1y1z1 theo trôc x1 mét ®o¹n b»ng a2.
1 0 0 a 2

0 1 0 0
Trans(a2,0,0) =
0 0 1 0

0 0 0 1

VËy 1A2 ®-îc tÝnh nh- sau:

cos 2
sin 2
0 0 1 0 0 a 2

1 sin 2
cos 2
0 0 0 1 0 0
A2 = Rot(z1, 2 ) Trans (a2,0,0) =
0 0 1 0 0 0 1 0

0 0 0 1 0 0 0 1

cos 2
sin 2
0 a 2
cos 2

1 sin 2
cos 2
0 a 2
sin 2
A2 = (1.2)
0 0 1 0

0 0 0 1

CuuDuongThanCong.com https://fb.com/tailieudientucntt
Ph-¬ng tr×nh ®éng häc thuËn ®-îc x¸c ®Þnh nh- sau:

cos 1
sin 1
0 a 1 cos 1
cos 2
sin 2
0 a 2
cos 2

0 sin cos 0 a 1 sin sin cos 0 a sin

A2 = 0A1 1A2 = 1 1 1 2 2 2 2

0 0 1 0 0 0 1 0

0 0 0 1 0 0 0 1

Nh©n hai ma trËn trªn vµ rót gän ta cã:

cos 1 2
sin 1 2
0 a 2
cos 1 2
a 1 cos 1

0 sin 1 2
cos 1 2
0 a 2
sin 1 2
a 1 sin 1
A2 = (1.3)
0 0 1 0

0 0 0 1

Hay:
n x
o x
a x
p x
cos 1 2
sin 1 2
0 a 2 cos 1 2
a 1 cos 1

0 n y
o y
a y
p y
sin 1 2
cos 1 2
0 a 2 sin 1 2
a 1 sin 1
A2 = =
n z
o z
a z
p z
0 0 1 0

0 0 0 1 0 0 0 1

(1.4)

Nh- vËy hÖ ph-¬ng tr×nh ®éng häc thuËn cña robot nh- sau:
nx = cos ( 1+ 1 )
ny = sin ( 1+ 1 )
nz = 0
ox = - sin ( 1+ 1 )
oy = cos ( 1+ 1 )
oz = 0
ax = 0
ay = 0
az = 1
px = a1cos 1 + a2 cos ( 1+ 1 )
py = a1sin 1 + a2 sin ( 1+ 1 )
pz = 0

3. X©y dùng ph-¬ng tr×nh ®éng häc ng-îc cho robot

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 Ph-¬ng tr×nh ®éng häc ng-îc robot nh»m x¸c ®Þnh c¸c gi¸ trÞ biÕn khíp tõ
c¸c vÞ trÝ vµ h-íng cña tay robot mong muèn. Ph-¬ng tr×nh ®éng häc ng-îc th-êng
khã gi¶i vµ kh«ng cã lêi gi¶i tæng qu¸t cho mäi robot.
Ta thÊy r»ng ma trËn 0A2 ®· biÕt, tøc lµ vÞ trÝ vµ h-íng cña khung to¹ ®é tay
robot, cÇn x¸c ®Þnh gi¸ trÞ c¸c biÕn khíp 1 vµ 2 :

n x
o x
a x
p x

0 n o a p
A2 = 0A11A2 = y y y y
(1.5)
n z
o z
a z
p z

0 0 0 1

Nh©n hai vÕ cña ph-¬ng tr×nh trªn víi ma trËn nghÞch ®¶o cña ma trËn 0A1 lµ
(0A1)-1 ta cã ph-¬ng tr×nh sau:
(0A1)-1 0A2 = 1A2
cos 1
sin 1
0 a1

sin cos 0 0
Ta cã (0A1)-1 = 1 1
(1.6)
0 0 1 0

0 0 0 1

cos 1
sin 1
0 a1 n x
o x
a x
p x

sin cos 0 0 n o a p
VËy (0A1)-1 0A2 = 1 1 y y y y

0 0 1 0 n z
o z
a z
p z

0 0 0 1 0 0 0 1

(0A1)-1 0A2 = 1A2 t-¬ng ®-¬ng víi:

n x
cos 1
n y
sin 1
o x cos 1
o y sin 1
a x
cos 1
a y
sin 1
P x cos 1
P y sin 1
a1

n x
sin 1
n y
cos 1
o x sin 1
o y cos 1
a x
sin 1
a y
cos 1
P x sin 1
P y cos 1
=
n z
oz a z
Pz

0 0 0 1

cos 2
sin 2
0 a 2
cos 2

sin 2
cos 2
0 a 2
sin 2
(1.7)
0 0 1 0

0 0 0 1

C©n b»ng hai vÕ cña hai ma trËn t¹i cét thø 4 ta ®-îc hÖ ph-¬ng tr×nh sau:
P x cos 1
P y sin 1
a1 a 2
cos 2
(1.8)
P x sin 1
P y cos 1
a 2
sin 2

Víi c¸c th«ng sè Px, Py vµ a1 , a2 ®· biÕt gi¶i hÖ ph-¬ng tr×nh nµy ta ®-îc c¸c
biÕn khíp 1 vµ 2 nh- sau:

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 P x cos 1
P y sin 1
a 2
cos 2
a1
Ta cã: (1.9)
P x sin 1
P y cos 1
a 2
sin 2

B×nh ph-¬ng ai vÕ 1.9, céng vÕ theo vÕ ta ®-îc:

2 2 2 2
Px Py a1 2 a 1 a 2 cos 2
a2

2 2 2 2
Px Py a1 a 2
VËy 2
ar cos (1.10)
2 a 1a 2

B×nh ph-¬ng hai vÕ vµ céng vÕ theo vÕ cña 1.8 ta ®-îc:

2 2 2 2
a1 Px Py 2 a 1 P x cos 1
P y sin 1
a2

2 2 2 2
Px Py a1 a 2
P x cos 1
P y sin 1
2a1

2 2 2 2
Px Py Px Py a1 a2
cos 1
sin 1
2 2 2 2 2 2
Px Py Px Py 2a1 Px Py

2 2 2 2
Px Py a1 a 2 Px
1
arcsin arcsin (1.11)
2 2 2 2
2a1 Px Py Px Py

2 2 2 2
Px Py a1 a 2 Px
1
arcsin arcsin
2 2 2 2
2a1 Px Py Px Py
VËy: (1.12)
2 2 2 2
Px Py a1 a 2
2
ar cos
2 a 1a 2

II. X©y dùng quan hÖ gi÷a tèc ®é cña c¸c khíp vµ tèc ®é cña tay
robot

§Ó x©y dùng quan hÖ gi÷a tèc ®é cña khíp vµ tèc ®é cña tay robot ta cÇn ph¶i
thiÕt lËp ph-¬ng tr×nh ®éng lùc häc cho robot.

Gäi v lµ tèc ®é chuyÓn ®éng tÞnh tiÕn cña t©m vËt thÓ, lµ tèc ®é gãc, m lµ khèi
l-îng vËt thÓ, Jt lµ m«men qu¸n tÝnh cña vËt thÓ.
Sö dông ph-¬ng ph¸p Lagrange ®Ó thiÕt lËp ph-¬ng tr×nh ®éng lùc häc cho robot. Gi¶
thiÕt khíp 1 sinh ra m«men M1 t¸c dông gi÷a bÖ vµ thanh nèi 1, khíp 2 sinh ra m«men
M2 t¸c dông gi÷a thanh nèi 1 vµ 2. Träng lùc cã h-íng theo trôc y.

1. TÝnh tèc ®é cña mét ®iÓm bÊt kú trªn robot

Nh- trong phÇn I ta cã phÐp biÕn ®æi to¹ ®é tõ thanh nèi 2 ®Õn hÖ to¹ ®é gèc
0
lµ A2

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 0
A2 = 0A1 1A2 (2.1)
Trong ®ã: A1 lµ phÐp biÕn ®æi to¹ ®é tõ thanh nèi 1 ®Õn hÖ to¹ ®é gèc, lµ hµm
cña biÕn 1

cos 1
sin 1
0 a 1 cos 1

0 sin 1
cos 1
0 a 1 sin 1
A1 =
0 0 1 0

0 0 0 1

1
A2 lµ phÐp biÕn ®æi to¹ ®é tõ thanh nèi 2 ®Õn hÖ to¹ ®é thanh nèi 1, lµ
hµm cña biÕn 2

cos 2
sin 2
0 a 2
cos 2

1 sin 2
cos 2
0 a 2
sin 2
A2 =
0 0 1 0

0 0 0 1

cos 1 2
sin 1 2
0 a 2
cos 1 2
a 1 cos 1

sin cos 0 a sin a 1 sin

Vµ 0A2 = 1 2 1 2 2 1 2 1

0 0 1 0

0 0 0 1

Gi¶ thiÕt biÓu diÔn mét ®iÓm bÊt kú nµo ®ã trªn thanh nèi i nh- sau
T
i
r x i , y i , z i ,1 (2.2)

VËy vÞ trÝ cña ®iÓm ®ã trong hÖ to¹ ®é gèc o0x0y0z0 lµ:

cos 1 2
sin 1 2
0 a 2
cos 1 2
a 1 cos 1
x i

sin 1 2
cos 1 2
0 a 2
sin 1 2
a 1 sin 1
y i
0
r
0
A i
i
r = (2.3)
0 0 1 0 zi

0 0 0 1 1

i
V× thanh nèi lµ vËt thÓ cøng, nªn quan hÖ r trong thanh nèi i lµ h»ng sè, hay:
i
d r
0 (2.4)
dt

LÊy ®¹o hµm ph-¬ng tr×nh (2.3) vµ chó ý ®Õn (2.4) ta cã ph-¬ng tr×nh nh- sau:
T
0 i 2 0
0 d r d 0 i 0 d r A i i
r ( A i) r A i i r
dt dt dt i 1 i

Víi kh¸i niÖm vÕt cña ma trËn (Trace), b×nh ph-¬ng tèc ®é cña ®iÓm ®ã ®-îc
tÝnh nh- sau:

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 T T T
2 0 2 0
A A
v
2
i
0
r
0
r Trace
0
r
0
r Trace
i
i
i
r
i
j
i
r (2.5)
i 1 i j 1 j

Sau khi biÕn ®æi ta ®-îc:

2 2 0 0 T
A T A
v
2
t
Trace
i i
r
i
r
i
i j (2.6)
j 1 k 1 i j

2. §éng n¨ng cña robot ®-îc tÝnh nh- sau

§éng n¨ng cña mét ®iÓm bÊt kú trong thanh nèi i
2 2 0 0 T
1 2 1 A i i i
T A i
dK 2
v i dm Trace r r j k dm
2 2 j 1 k 1 j k

Do ®ã ®éng n¨ng cña thanh nèi i ®-îc tÝnh b»ng tÝch ph©n theo toµn bé thanh
nèi i
2 2 0 0 T
1 A i i
T A
Ki = dK 2
Trace
i
r r dm
i
j k

thanh i
2 j 1 k 1 j thanh i k

T
i i
Ký hiÖu Ji = r r dm lµ ma trËn qu¸n tÝnh gi¶
thanh i

i i i i i 2 i
r x dm ry r x dm rz r x dm r x dm
thanh i thanh i thanh 2 thanh i
i i i i i 2 i
rx r y dm r y dm rz r y dm r y dm
thanh i thanh i thanh 2 thanh i
Ji
i i i i i 2 i
rx r z dm ry r z dm r z dm r z dm
thanh i thanh i thanh 2 thanh i
i i i
r x dm r y dm r z dm dm
thanh i thanh i thanh 2 thanh i

Cã thÓ viÕt l¹i ma trËn Ji nh- sau:

I ixx I iyy I izz

I ixy I ixz m ix i
2
I ixx I iyy I izz
I ixy I iyz m iy i
Ji 2
I ixx I iyy I izz
I ixz I iyz m iz i
2
m ix i m iy i
m iz i m i

i i i i T
Trong ®ã r r x , r y , r z ,1 lµ b¸n kÝnh vect¬ biÓu diÔn träng t©m cña kh©u i
trong hÖ to¹ ®é i.
C¸c phÇn tö cña Ji ®-îc tÝnh nh- sau:

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 i 2 i 2
I ixx ry r z dm
thanh i

i i
I ixy rx r y dm
thanh i

i
m ix i r x dm
thanh i

C¸c thµnh phÇn cßn l¹i ®-îc tÝnh t-¬ng tù

Tæng ®éng n¨ng cña robot ®-îc tÝnh nh- sau:
n 2 2 2 0 0 T
1 A A
K K i
Trace
i
Ji
i
j k (2.7)
i 1 2 i 1 j 1 k 1 j k

Hay
2 2 2
1
K Trace h jk j k
2 i 1 j 1 k 1

trong ®ã thµnh phÇn ma trËn hjk ®-îc tÝnh theo:

n 0 0 T
A i
A i
h jk
Trace Ji
i 1 j k

0
A
Do i
0 (j > i) nªn cã thÓ viÕt hjk nh- sau:
j

n 0 0 T
A i
A i
h jk
Trace Ji
i max j,k j k

Do hjk = hkj nªn ma trËn vu«ng H( ) lµ ma trËn ®èi xøng, ®ång thêi ®éng n¨ng lµ

mét ®¹i l-îng d-¬ng víi 0 , nªn c¸c thµnh phÇn cña H( ) lµ c¸c sè d-¬ng.

3. ThÕ n¨ng cña robot ®-îc tÝnh nh- sau

ThÕ n¨ng cña thanh nèi thø i ®-îc x¸c ®Þnh theo biÓu thøc:
T 0 i
Pi -m i
g A i
r

Víi g = [ gx gy gz 1]T lµ vÐct¬ gia tèc träng tr-êng

mi lµ khèi l-îng thanh nèi thø i
i
r lµ vÐct¬ vÞ trÝ thanh nèi i trong hÖ to¹ ®é cña thanh nèi thø i

Tæng thÕ n¨ng cña hÖ thèng lµ:

2
0 i
T
P = m ig A i
r
i 1

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 i
Ta thÊy m i
r lµ cét thø t- cña ma trËn Ji nªn ta cã tæng thÕ n¨ng nh- sau:
2
T 0 T
P g A iJ i 0 0 0 1 (2.8)
i 1

4. M« h×nh ®éng lùc häc robot

Hµm Lagrange cña robot lµ:
n

L= K i
Pi
i 1

Thay lÇn l-ît c¸c biÓu thøc vµo vµ biÕn ®æi ta cã:
2 i i 0 0 . . n
1 A A 0 i
L Trace
i
Ji
i
j k m ig
T
A i
r (2.9)
2 i 1 j 1 k 1 j k i 1

Ph-¬ng tr×nh Lagrange cã d¹ng nh- sau:

d L L
F Mi
. .
dt
i i

FMi - ®éng lùc, h×nh thµnh trong khíp ®éng thø i khi thùc hiÖn chuyÓn ®éng.
.
L K
Ta cã: . .
H

.. .
.
d L
.
H H
dt

T
. .
L 1 P
H
2

Do ®ã ph-¬ng tr×nh ®éng lùc häc cã d¹ng:

.
T
. .
.
1 P
H H H FM
2

T
. . . .
. . .
1 K
Ta ký hiÖu: V , H H H q gäi lµ thµnh phÇn
2

m«men nhít vµ h-íng t©m.

P
G gäi lµ thµnh phÇn m«men träng lùc.

CuuDuongThanCong.com https://fb.com/tailieudientucntt
Khi ®ã ph-¬ng tr×nh ®éng lùc häc nh- sau:
.. .

H V , G M

M«men cña khíp i ®-îc x¸c ®Þnh nh- sau:

n .. n n . .

M i
H ij
q j
V ijk j k G i
j 1 j 1 k 1

0 0 T
2
A p
A p
Trong ®ã: h ij
Trace Jp (i, j = 1,2)
p max( i , j ) j j

0
j j 2 0
2
. A p
A p
. .

Vi , Trace Jp k m
(i, j, k =1,2)
j 1 k 1 p max i, j,k j k

0
n
T
A p p
g i
m pg r
p 1 j

Hay viÕt d-íi d¹ng ma trËn

.. .

M H( ) V , G

h 11 h 12
Trong ®ã: H
h 21
h 22

1 2 1 1 2 1
m 1l1 0 0 m 1l1 m 1l 2 0 0 m 2l2
3 2 3 2
0 0 0 0 0 0 0 0
J1 ; J2
0 0 0 0 0 0 0 0
1 1
m 1l1 0 0 m 1
m 2l2 0 0 m 2
2 2

0 0 T 0 0 T
A 1
A 1
A 2
A 2
h 11 Trace J1 Trace J2
1 1 1 1

1 2 4 2 2
m 1l1 m 2l2 m 2 l 2 cos 2
3 3

0 0 T 0 0 T
A 2
A 2
A 2
A 2
1 2 1 2
h 12 h 21
Trace J2 Trace J2 m 2 l1 m 2l2
1 2 2 1
3 2

0 0 T
A 2
A 2
1 2
h 22
Trace J2 m 2l2
2 2
3

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 . v1
V ,
v 2

20 0 . . 20 0 . . 20 0 . .
A 1
A 1
A 2
A 2
A 2
A 2
V1 Trace J1 1 1 Trace J2 1 2 Trace J2 2 2

1 1 1 2 1 1 2 2 1

2
. . .
1 2 2
V1 m 2
sin 2
l2 2 m 2
sin 2
l1 1 2
2

2 0 0 . . 2 0 0 . . .
2
A 2
A 2
A 2
A 2
1 2
V2 Trace J 2 1 2 Trace J 2 2 2 m 2
sin 2
l2 1

2 1 2 2 2 2
2

g1
Ma trËn V
g 2

1/2 1/2

A 0 T 0
g1 = -m 1
0 g 0 0
1
m 2
0 g 0 0
1
0 1
0

1 1

1 1
m 1 gl 1
m 2 gl 1
cos 1
m 2 gl 2
cos 1 2
2 2

1/2

T 0
g2 = m 2
0 g 0 0 m 2 gl 2
cos 1 2

2
0

VËy ph-¬ng tr×nh ®éng lùc häc cña robot lµ:

.. .

M H( ) V , G

1 2 4 2 2 1 2 1 2 ..
m 1l1 m 2l2 m 2 l 2 cos m 2 l1 m 2l2
M 2
1 3 3 3 2 1
..
M 1 2 1 2 1 2
2
m 2 l1 m 2l2 m 2l2 2
3 2 3
2
. . .
1 2 2
m sin l 2 m sin l 1 2 1 1
2 2 2 2 2 1
2 m 1 gl 1
m 2 gl 1
cos 1
m 2 gl 2
cos 1 2

.
2 2 2
1 2
m 2
sin 2
l2 1 m 2 gl 2
cos 1 2
2

Víi c¸c m«men khíp cho tr-íc ta gi¶i hÖ ph-¬ng tr×nh vi ph©n bËc hai nµy th×
. . .. ..

thu ®-îc c¸c biÕn khíp lµ gãc 1

vµ 2
, vËn tèc gãc 1
, 2
, gia tèc gãc 1
, 2

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 5. Quan hÖ gi÷a tèc ®é cña khíp vµ tèc ®é cña tay robot
BiÓu diÔn tay robot trªn thanh nèi 2 nh- sau:
2 T
r tay 0 , 0 , 0 ,1

VËy vÞ trÝ cña ®iÓm ®ã trong hÖ to¹ ®é gèc o0x0y0z0 lµ:

cos 1 2
sin 1 2
0 a 2
cos 1 2
a 1 cos 1
0

sin 1 2
cos 1 2
0 a 2
sin 1 2
a 1 sin 1
0
0
r tay
0
A 2
2
r tay =
0 0 1 0 0

0 0 0 1 1

a 2
cos 1 2
a 1 cos 1

a 2
sin 1 2
a 1 sin 1

B×nh ph-¬ng tèc ®é cña tay robot lµ:

T T
2 0 2 0
2 0 T 0 0 0 A A
v tay
r tay r tay Trace r tay
r tay = Trace 2
i
2
r tay
2
j
2
r tay
i 1 i j 1 j

2 2
a 1 sin 1
a 2
sin 1 2
a 1 cos 1
a 2
cos 1 2

Khai triÓn vËn tèc cña tay robot theo hai thµnh phÇn trôc x vµ trôc y vµ ta cã thÓ
viÕt r»ng:
2 2 2
v tay u v

Trong ®ã:
v = a1 sin 1
+ a2 sin( 1 2
)

u = a1 cos 1
+ a2 cos( 1 2
)

§¹o hµm theo thêi gian ta ®-îc:

v =a1 cos  + a2 cos( ) (+  )
1 1 1 2 1 2

u = -a1 sin  - a2 sin( ) (+  )

1 1 1 2 1 2

Nh- vËy u vµ v chÝnh lµ c¸c vÐc t¬ vËn tèc cña tay r« bèt chiÕu theo c¸c
ph-¬ng x vµ y.
v y
= v = [a1 cos 1
+ a2 cos( 1 2
) ] 
1
+a2 cos( 1 2
) 2

vx = u = -[a1 sin 1
+ a2 sin( 1 2
) ] 
1
- a2 sin( 1 2
) 2

Tõ hÖ ph-¬ng tr×nh trªn ta cã quan hÖ gi÷a tèc ®é c¸c khíp vµ tèc ®é cña tay r« bèt
nh- sau:

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 sin( ) cos( )
 = 1 2
v + 1 2
vx
1 y
a 1 sin 2
a 1 sin 2

a 1 sin a sin( ) a 1 cos a cos( )

 = 1 2 1 2
v
1 2 1 2
vx
2 y
a 1a 2
sin 2
a 1a 2
sin 2

III. ViÕt hµm MATLAB thùc hiÖn c¸c ph-¬ng tr×nh ë phÇn 1 vµ 2, vÏ
®-êng biÓu diÔn vÞ trÝ vµ tèc ®é cña khíp khi tay robot di
chuyÓn tõ vÞ trÝ [0.4; 0.0 m] ®Õn [0; 0.4m] theo mét quü ®¹o ®-êng
th¼ng. §å thÞ tèc ®é ®Æt tr-íc cña tay robot däc theo quü
®¹o cho tr-íc

Khi robèt di chuyÓn tõ vÞ trÝ (0.4; 0.0m) ®Õn (0.0; 0.4m) theo mét ®-êng th¼ng
khi ®ã: vËn tèc cña tay robot, quü ®¹o chuyÓn ®éng, px, py, pz lµ c¸c ®¹i l-îng ®· biÕt;
thêi gian chuyÓn ®éng t = 1s. CÇn tÝnh tèc ®é cña c¸c khíp 1 vµ 2 theo t.

1. Ch-¬ng tr×nh tÝnh to¸n ®éng häc ng-îc

-Hµm tÝnh gãc 1
:
function y = teta_1(px,py)
l1 = 0.4; % chieu dai cua thanh noi 1 (m)
l2 = 0.3; % chieu dai cua thanh noi 2 (m)
% goc cua khop mot (rad)
y = asin((l1^2+px^2+py^2-l2^2)/(2*l1))- asin(px/sqrt(px^2 + py^2));
-Hµm tÝnh gãc 2
:
function y=teta_2(px,py)
l1=.4; % chieu dai cua thanh noi 1 (m)
l2=.3; % chieu dai cua thanh noi 2 (m)
% goc cua khop hai (rad)
y = acos((px^2+py^2-l1^2-l2^2)/(2*l1*l2));

2. Ch-¬ng tr×nh tÝnh to¸n vËn tèc ®é cña c¸c khíp theo tèc ®é tay
r« bèt
-Hµm tÝnh tèc ®é gãc cña thanh 1:
function y = dhone(vy,vx,t1,t2)
l1=.4;
l2=.3;
% vy van toc cua tay robot theo phuong y
% vx van toc cua tay robot theo phuong x
% t1 goc quay cua khop 1
% t2 goc quay cua khop 2
y = sin(t1+t2)*vy/(l1*sin(t2))+cos(t1+t2)*vx/(l1*sin(t2));
-Hµm tÝnh tèc ®é gãc cña thanh 2:
function y = dhtwo(vy,vx,t1,t2)
l1=.4;
l2=.3;
y = -(l1*sin(t1)+l2*sin(t1+t2))*vy/(l1*l2*sin(t2)) -

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 (l1*cos(t1)+l2*cos(t1+t2))*vx/(l1*l2*sin(t2));
3 X©y dùng mèi quan hÖ gi÷a vx,vy,px vµ py theo thêi gian t

Py Vtay (m/s)

0.4m

0 0.4m Px 0 0.25 0.75 1 t(s)

Gäi v1 lµ vËn tèc cña tay r« bèt trªn ®o¹n [0 0.25], v2 lµ vËn tèc cña tay r«
bèt trªn ®o¹n [0.25 0.75], v3 lµ vËn tèc cña tay r« bèt trªn ®o¹n [0.75 1].
Ta cã:
v1=4vot.
v2=vo.
v3=-4vo(t-1) víi v0 lµ vËn tèc ban ®Çu.
Gäi s1 lµ qu·ng ®-êng mµ tay r« bèt dich chuyÓn ®-îc trong kho¶ng thêi gian
[0 0.25], s2 lµ qu·ng ®-êng mµ tay r« bèt dich chuyÓn ®-îc trong kho¶ng thêi
gian [0.25 0.75], i s3 lµ qu·ng ®-êng mµ tay r« bèt dich chuyÓn ®-îc trong kho¶ng
thêi gian [.75 1]
Ta cã:
s1= vo/8.
s2= vo/2.
s3= vo/8.
VËy s1+ s2+ s3=3vo/4
Khi tay r« bèt di chuyÓn tõ ®iÓm (0.4 0) ®Õn (0 0.4) qu·ng ®-îc ®i ®-îc lµ:
0.4 2 . Suy ra: vo=1.6 2 /3. Thay vµo c¸c c«ng thøc trªn ta ®-îc:
v1=6.4 2 t/3.
v2=1.6 2 /3
v3=-6.4 2 (t-1)/3 .
ChiÕu vÐc t¬ vËn tèc lªn hai trôc x vµ y ta ®-îc:
v1x=-6.4t/3.
v1y=6.4t/3.
v2x=-1.6/3.
v2y=1.6/3.

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 v3x=6.4(t-1)/3 .
v3y=-6.4(t-1)/3 .
VÞ trÝ cña tay r« bèt khi di chuyÓn t-¬ng øng lµ:
p1x=0.4-3.2t2/3.
p1y=3.2t2/3.
p2x=(1.4-1.6t)/3.
p2y=(1.6t-0.2)/3.
p3x=6.4(t2/2-t+1/2)/3.
p3y=-6.4(t2/2-t+5/16)/3.
Trªn c¬ së c¸c ph-¬ng tr×nh nµy,ch-¬ng tr×nh tÝnh vµ vÏ ®å thÞ vÞ trÝ, tèc ®é c¸c
khíp ®-îc viÕt nh- sau:
% Vi tri va toc do cac khop trong khoang thoi gian tu 0 den 0.25
t=0:.01:.24;
vx=-6.4*t/3; % toc do cua tay ro bot theo truc x
vy=6.4*t/3; % toc do cua tay ro bot theo truc y
px=0.4-3.2*t.^2/3; % vi tri cua tay ro bot theo truc x
py=3.2*t.^2/3; % vi tri cua tay ro bot theo truc y
t1=tone(px,py); % vi tri cua khop 1
t2=ttwo(px,py); % vi tri cua khop 2
dh1=dhone(vy,vx,t1,t2); % toc do goc cua khop 1
dh2=dhtwo(vy,vx,t1,t2); % toc do goc cua khop 2
tm=t;
o1=t1;
o2=t2;
d1=dh1;
d2=dh2;
% Vi tri va toc do cac khop trong khoang thoi gian tu 0.25 den 0.75
t=0.25:.01:.74;
vx=-1.6/3;
vy=1.6/3;
px=1.4/3-1.6*t/3;
py=1.6*t/3-0.2/3;
t1=tone(px,py);
t2=ttwo(px,py);
dh1=dhone(vy,vx,t1,t2);
dh2=dhtwo(vy,vx,t1,t2);
th=t;
o3=t1;
o4=t2;
d3=dh1;
d4=dh2;
% Vi tri va toc do cac khop trong khoang thoi gian tu 0.75 den 1
t=.75:.01:1;
vx=6.4*(t-1)/3;
vy=-6.4*(t-1)/3;
px=6.4*(t.^2/2- t + 1/2)/3;
py=-6.4*(t.^2/2- t + 5/16)/3;

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 t1=tone(px,py);
t2=ttwo(px,py);
dh1=dhone(vy,vx,t1,t2);
dh2=dhtwo(vy,vx,t1,t2);
o5=t1;
o6=t2;
d5=dh1;
d6=dh2;
tb=t;
% Vi tri va toc do cac khop trong khoang thoi gian tu 0 den 1
t=[tm th tb];
om=[o1 o3 o5];
oh=[o2 o4 o6];
dm=[d1 d3 d5];
dh=[d2 d4 d6];
plot(t,om,'b') % Vi tri cua khop 1.
pause
plot(t,dm,'c') % Toc do cua khop 1.
pause
plot(t,oh,'r') % Vi tri cua khop 2
pause
plot(t,dh,'k') % Toc do cua khop 2
- Ch¹y ch-¬ng tr×nh ta cã kÕt qu¶ nh- sau:
§å thÞ vÞ trÝ gãc quay cña khíp 1:

§å thÞ vËn tèc gãc quay cña khíp 1

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 §å thÞ vÞ trÝ gãc quay cña khíp 2:

§å thÞ vËn tèc gãc quay cña khíp 2:

IV. ThiÕt kÕ bé ®iÒu khiÓn ®éng häc ng-îc

Tõ ph-¬ng tr×nh ®éng lùc häc cña robot:

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 .. .

M H( ) V , G

1 2 4 2 2 1 2 1 2 ..
m 1l1 m 2l2 m 2 l 2 cos m 2 l1 m 2l2
M 2
1 3 3 3 2 1
..
M 1 2 1 2 1 2
2
m 2 l1 m 2l2 m 2l2 2
3 2 3
2
. . .
1 2 2
m sin l 2 m sin l 1 2 1 1
2 2 2 2 2 1
2 m 1 gl 1
m 2 gl 1
cos 1
m 2 gl 2
cos 1 2

.
2 2 2
1 2
m 2
sin 2
l2 1 m 2 gl 2
cos 1 2
2

d1 1
Sai lÖch vÞ trÝ: e dt

d2 2

. .
. . .
d1 1
Sai lÖch tèc ®é: e dt
. .

d2 2

.. ..
.. .. ..
d1 1
Sai lÖch gia tèc: e dt
.. ..

d2 2

.. .. ..

VËy: d e

.. ..

e d H
1
V G M ®Æt V G N

.. ..

§Æt U d H
1
N M VËy e U

Tõ ph-¬ng tr×nh trªn ta cã ®-îc m«men cña khíp cÇn ®iÒu khiÓn:
..
1
M dk H d U N

S¬ ®å cÊu tróc cña hÖ thèng ®iÒu khiÓn nh- sau:

.
..
N( , )
d
H Robot ..

Bé §K phô

Bé ®iÒu khiÓn vßng trong Bé ®iÒu khiÓn vßng ngoµi (®¶m

(khö phi tuyÕn) b¶o ®é æn ®Þnh, chÊt l-îng qu¸
tr×nh qu¸ ®é, chÕ ®é tÜnh)

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 TiÕp theo ta ®i ®Õn thiÕt kÕ bé ®iÒu khiÓn U

Bé ®iÒu khiÓn U cã thÓ ®iÒu khiÓn PD, PID, bé ®iÒu khiÓn ph¶n håi tr¹ng th¸i.
ThiÕt kÕ bé ®iÒu khiÓn PD.

Ph-¬ng tr×nh m¹ch vßng ngoµi nh- sau:

. ..

U K d
e K p
e e

.
.
e1 e1
Víi e , e
.
, do vËy:
e2
e2

K p1
0 K d1
0
K p
; K d
0 K p2 0 K d2

Ph-¬ng tr×nh cho 2 khíp nh- sau:

.. .

e1 K e
d1 1
K p1
e1 0
.. .

e2 K d2
e2 K p2
e2 0

TÝnh to¸n c¸c hÖ sè Kd1, Kd2, Kp1, Kp2 b»ng ph-¬ng ph¸p dùa vµo c¸c kh©u
®éng häc ®iÓn h×nh:
+ ThiÕt kÕ bé ®iÒu khiÓn cho khíp 1:

Theo ph-¬ng ph¸p ®Æt nghiÖm ta mong muèn:

2
s K d1
s K p1
s 3 s 3 0 Kp1 = 9 vµ Kd1 = -6

Ta cã m« men ®iÒu khiÓn ®-îc tÝnh nh- sau:

M®k= H(  - u ) + N d

víi u = 6* -9* 
1 1

+ ThiÕt kÕ bé ®iÒu khiÓn cho khíp 2:

T-¬ng tù ta cã: Kd2 = -6, Kp2 = 9.

CuuDuongThanCong.com https://fb.com/tailieudientucntt
V. M« pháng hÖ thèng víi bé ®iÒu khiÓn ë c©u IV cña khíp 1 thay
®æi nh¶y cÊp tõ 0 ®Õn 1 rad/s

1. ThiÕt kÕ bé ®iÒu khiÓn tèc ®é cho ®éng c¬ khíp 1

Ta cã:

-V× ta cÇn m« pháng hÖ thèng víi gãc quay 1

thay ®æi nh¶y cÊp, cho nªn ta cã
thÓ cho gãc 2
b»ng h»ng sè.

Chän nh- sau: 2

=60o , suy ra 
2
vµ 
2
®Òu b»ng kh«ng.

Thay c¸c gi¸ trÞ vµo ta ®-îc:

H11 = 3.0905 Kgm2

H22 = 0.3025 Kgm2

- HÖ sè c¶n cña ®éng c¬ vµ phô t¶i coi b»ng kh«ng.

- M« men quan tÝnh tæng hiÖu dông cña hai ®éng c¬ lµ:

+ §éng c¬ khíp 1:

Jqd1 =J® + i2 H11= .0004 + 3.0905/122 =0.0219 Kgm2

+ §éng c¬ khíp 2:

Jqd2 =J® + i2 H22= .0004 + 0.3025/122 =0.0025 Kgm2

Trong ®ã i lµ tØ sè truyÒn: i = tèc ®é cña ®éng c¬ / tèc ®é cña khíp = 12.

- Ta cã s¬ ®å hÖ thèng ®iÒu khiÓn tèc ®é quay nh- sau:

- Víi c¸c th«ng sè:

Kb = Ka = 0.5 .

Ra = 3 .

CuuDuongThanCong.com https://fb.com/tailieudientucntt
 - Sö dông bé ®iÒu khiÓn PI: C¸c th«ng sè ®-îc x¸c ®Þnh theo ph-¬ng ph¸p g¸n cùc.

- Hµm truyÒn ®¹t cña bé ®iÒu khiÓn cã d¹ng:

Wc = KP + KIs

- Hµm truyÒn ®¹t cña ®èi t-îng lµ:

W§T = K/s(s+K*Kb)

Trong ®ã: K=KM/(r*Jqd1) =0.5/(3*0.0219) = 7.6104

Hµm truyÒn ®¹t cña hÖ thèng sÏ lµ:

Wk = K*(KI + sKP)/(s3 + s2*K0 + sKP + KI)

Víi K0 = K*Kb = 3.8052.

Chän c¸c ®iÓm cùc lµ:

-s1,2 = -1; s3 =-1.8052 , ta cã c¸c tham sè cña bé ®iÒu khiÓn lµ:

KI = K0 – 2 = 1.8052

KP = 2K0 – 3 = 4.6104

- Sau khi ch¹y ch-¬ng tr×nh ta ®-îc ®å thÞ hµm qu¸ ®é cña hÖ thèng nh- sau:

CuuDuongThanCong.com https://fb.com/tailieudientucntt
2 M« pháng hÖ thèng
-S¬ ®å ®iÒu khiÓn nh- sau:

- Cho tÝn hiÖu ®Æt gãc quay cña khíp 1 lµ tÝn hiÖu step.

- Cho tÝn hiÖu ®Æt tèc ®é gãc quay cña khíp 1 lµ b»ng kh«ng.

- g1 ®-îc x¸c ®Þnh nh- sau:

g1 = m 1 gl g1
cos 1
+ m2g [l 1
cos 1
l g 2 cos( 1 2
) ]

=10*9.7*0.2*cos( )+5*9.7*(.4*cos( )+.15*cos( +pi/3))

1 1 1

- KP vµ KD ®-îc x¸c ®Þnh nh- ë trªn.

- Sau khi ch¹y ch-¬ng tr×nh ta ®-îc ®å thÞ gãc quay cña khíp nh- sau:

CuuDuongThanCong.com https://fb.com/tailieudientucntt