Bảng các tham số động học của robot

khâu θi di ai αi

1 0 q1 a1 90
∘

2 π q2 0 90
∘

3 q3 0 a3 0

Trong đó q 1 , q 2 , q 3 là các biến khớp , còn a 1, a 3 là các hằng số

T
Và X =[ x 1 , x 2 , x 3 ] là véc tơ biểu diễn vị trí của bàn kẹp trong hệ cố định.

q=[ q 1 , q 2 , q 3 ] là các góc xoay và tịnh tiến của các biến khớp
- Dạng tổng quát của ma trận Denavit-Hartenberg cho các khâu

[ ]
cos ⁡θi −sin ⁡θi cos ⁡α i sin ⁡θi sin ⁡α i a i cos ⁡θi
i−1 sin ⁡θi cos ⁡θi cos ⁡α i −sin ⁡α i cos ⁡θ i a i sin ⁡θ i
A i=
0 sin ⁡α i cos ⁡α i di
0 0 0 1
(2.0)

[ ]
1 0 0 a1
0 0 0 −1 0
A 1=
Ma trận Denavit-Hartenbergcủa khâu 1: 0 1 0 q1
0 0 0 1
(2.1)

[ ]
−1 0 0 0
1 0 0 1 0
A 2=
Ma trận Denavit-Hartenbergcủa khâu 2: 0 1 0 q2
0 0 0 1
(2.2)

[ ]
cos ⁡q 3 −sin ⁡q 3 0 a3 ⋅cos ⁡q3
2 sin ⁡q2 cos ⁡q3 0 a 3 ⋅sin ⁡q3
A 3=
Ma trận Denavit-Hartenbergcủa khâu 3: 0 0 1 0
0 0 0 1
(2.3)

Từ các ma trận (2.1) và (2.2) ta xác định được ma trận biến đổi tọa độ thuần
nhất của khâu 2 so với trục hệ tọa độ cố định O0 x 0 y 0 z 0 là :

[ ]
−cos ⁡q 3 sin ⁡q3 0 −a3 ⋅cos ⁡q3 +a1
0 −sin ⁡q 3 −cos ⁡q3 0 −a3 ⋅sin ⁡q 3−q 2
A 2=0 A 1 ⋅ A 2❑ A =
2
3
0 0 1 q1
0 0 0 1

- Ma trận 0 A 3 cho ta biết hướng và vị trí của khâu thao tác trong hệ tọa độ cố
định hay nói cách khác là vị trí của điểm tác động cuối và hướng của hệ tọa
độ động gắn vào khâu tại điểm tác động cuối trong hệ tọa độ cố định. Vì thế
nó còn được biểu diễn qua thông số các biến khớp ta tạm gọi là qi. Trong bài
toán cụ thể thì nó là các khớp xoay θi , với i=1 ÷ 3.

- Khi đó, ma trận (2.5) được kí hiệu thành ∘ A 3 (q)

Sử dụng các góc Cardan xác định hướng của vật rắn. Ta gọi
[ x E , y E , z E , α , β , η ] là giá trị mô tả trực tiếp vị trí và hướng của EX 2 Y 2 Z 2 so với
hệ tọa độ O0 Z 0 Y 0 Z 0. Trong đó: [ x E , y E , z E ] là các tọa độ điểm E và [α , β ,η] là
các góc quay Cardan
của EX 3 Y 3 Z 3 so với hệ tọa độ O0 Z 0 Y 0 Z 0. Do các tọa độ thao tác đều là hàm
của

[ ]
0 0
0 R n (t) r E (t )
A n (t)=
thời gian. Nên ta có thể biểu diễn: 0
T
1
(2.6)

Với: 0 A E là ma trận Cardan mô ta hướng EX 2 Y 2 Z 2 so với hệ tọa độ O0 Z 0 Y 0 Z 0.

r E là vectơ mô tả vị trí của điểm tác động cuối trong hệ tọa độ O0 Z 0 Y 0 Z 0.

0

T
❑ r E (t )=[ ¿ x E (t ),∧¿ y E (t) ,∧¿ z E (t ) ]
0

[ ]
cos ⁡β cos ⁡η −cos ⁡β sin ⁡η sin ⁡β
0
Rn=RCD = sin ⁡α sin ⁡β cos ⁡η+cos ⁡α sin ⁡η −sin ⁡α sin ⁡β sin ⁡η+ cos ⁡α cos ⁡η −sin ⁡α cos ⁡β
−cos ⁡α sin ⁡β cos ⁡η+sin ⁡α sin ⁡η cos ⁡α sin ⁡β sin ⁡η+sin ⁡α cos ⁡η cos ⁡α cos ⁡β

- Do ma trận 0 A 3 (q) biểu diễn vị trí và hướng của khâu thao tác trong hệ tọa
độ cố định thông qua biến khớp qi (Ma trận trạng thái khâu thao tác theo cấu
trúc động học). Còn ma trận 0 A E (t) cũng mô tả vị trí và hướng của khâu thao
tác thông qua hệ tọa độ khâu thao tác. Ở đây ta chọn cách biểu diễn thông
qua các góc Cardan.
0 0
A (q)= A 3 (t) 0 0
Từ đó ta có PT động học ROBOT có dạng: A 3 (q)= A n (t) 3
(2.9)
Từ các hệ thức Ta xây dựng được hệ 3 phương trình độc lập như sau:
0 0
¿ f 1 =❑ A 3 (q)[1 , 4]−❑ A 3 (t)[1 , 4 ]=−a3 cos ⁡q 3+ a1−x E
¿ f 2=❑0 A 3 (q) [2 , 4 ]−❑0 A3 (t)[2, 4 ]=−a3 sin ⁡q 3−q 2− y E
0 0
¿ f 3=❑ A3 (q)[3 , 4]−❑ A 3 (t)[3 , 4 ]=q 1−z E
Thông số cần xác định: điểm tác động cuối E=( x E , y E , z E ), và hướng của khâu
tác động cuối so với hệ tọa độ cơ sở.

{
x E= 0 A3 [1 , 4 ]=−a3 cos ⁡q 3+ a1
y E =0 A 3 [2, 4]=−a3 sin ⁡q 3−q 2
* Ta có tọa độ của điểm tác động cuối:
z E =0 A 3 [3 , 4]=q 1
(2.11)
Và các thông số hình học:
a1= 50 cm, a3 = 30 cm

Động Học Ngược

Từ bài toán động học thuận ta có được hệ phương trình :

{
x E= 0 A3 [1 , 4 ]=−a3 cos ⁡q 3+ a1
y E =0 A 3 [2, 4]=−a3 sin ⁡q 3−q 2
z E =0 A 3 [3 , 4]=q 1

Bài toán đặt ra là : cho biết các tọa độ [ x E y E z E ] và nhiệm vụ của ta là phải
tìm
T
lại các biến khớp q=[ q 1 q 2 q 3 ]

Với các x E , y E , z E , a1 , a3 đã biết.

Giả sử ta phải cho quỹ đạo của điểm tác động cuối E là

{
x E=40 cm
y E =40+ 30⋅ sin ⁡(2 t)⋅cos ⁡(t)cm
z E =30+ 40⋅ sin ⁡(2 t)⋅sin ⁡( t)cm
(2.32)

Từ phương trình vị trí điểm E (2.31) thì ta có thể tìm được các biến khớp qi

{
q1=z E

cos ⁡q3 =
a 1−x E
⇒ sin ⁡q 3=
√ 2
a3−( a1−x E )
2

a3 a3
như sau : ⇒ q3 =a tan ⁡2 ( sin ⁡q3 , cos ⁡q 3 )
√ 2
q2 =−a3 ⋅sin ⁡q 3− y E=− a 3−( a 1−x E ) − y E
2

( 2.33)

{
q 1=z E =30+ 40 sin ⁡(2 t)sin ⁡(t )

cos ⁡q3=
50−40 1 √8
= ⇒ sin ⁡q3 = ⇒ q 3=19.5
∘
30 3 3
Thay các giá trị trên vào ta được:
q2 =−√ 30 −( 50 −40 )−40−30 sin ⁡(2 t)cos ⁡(t)
2 2 2

⇒ q 2=−40−30 sin ⁡(2 t)cos ⁡(t)

(2.34)
 T
0
Fi , i−1 =[ F x F y F z ] là lực do khâu i-1 tác dụng lên khâu i ở khớp thứ I trong
hệ tọa độ cơ bản
T
0
M i, i−1=[ M x M y M z ] là mô men do khâu i-1 tác dụng lên khâu i ở khớp
thứ i trong hệ tọa độ cơ bản.
T
0
Pi=[ P x P y P z ] là trọng lực của khâu i trong hệ tọa độ cơ bản.

r i= Ri r i là vector có gốc là O0 nối với Oi trong hệ tọa độ cơ bản..

0 0

Ri là ma trận quay biến đổi từ hệ tọa độ 0 đến hệ tọa độ thứ i

0 0 1 i−1
Ri = R1 R2 …

[ ]
ai
i
r i= d i sin ⁡α i là vector có gốc Oi−1 nối với Oi trong hệ tọa độ khâu i.
di cos ⁡α i

r ci= A i r ci là vector có gốc O0 nối với C i trong hệ tọa độ cơ bản .

0 0 i

r ci : là vector có gốc Oi nối với C i trong hệ tọa độ khâu i

1

[ ] [ ]
3267 0
⇒ F 1 ,0= 0 , M 1 ,1= −12+3267 q 1
0 156 s3 +592 q 2−61.44

Mà −1 ≤sin ⁡(q 3)≤1 ; q 2 ≤ 0 , 8. a2 ; q 1≤ 0.8 . a0 (chiều dài khâu đế )

do đó lực và momen lớn nhất ở khâu 1 là :

[ ] [ ]
3267 0
F 1, 0= 0 , M 1, 0 = 2863 ⇒ M 1, 0 max ≈ 2900(N . m)
0 498

+ Động Lực học

Ta có biểu thức động lực học như sau:
T
M (q) q̈+ C(q , q̇) q̇+G(q)=U + J E F

- Xác định thế năng của robot

Chọn gốc tính thế nắng tại mặt đất ta có :
3
Π=∑ ❑mi X Ci g
i=1
δΠ
Π 1=m1 X C 1 g=m1 g 0.51 ⇒ g 1= =0
δ q1
δΠ
Π 2=m2 X C 2 g=m2 g 0.87 ⇒ g 2= =0
δ q2
δΠ
Π 3=m3 X C 3 g=m3 g ( 0.87−0.39C 3 ) ⇒ g 3= =m3 g 0.39 S3
δ q3
⇒ Π =0.51 m1 g+0.87 m2 g+ m3 g (−0.39 C 3+ 0.87 )

[ ]
0
⇒ G(q)= 0
0.39 m3 g S3

- Ma trân Jacobi tịnh tiến và Jacobi quay

r ci là khoảng cách từ gốc tọa độ Oi đến khối tâm i của từng khâu ..

J Ti là ma trận Jacobi tịnh tiến của khâu i:

∂ ( 0 r Ci )
J Ti =
∂q

[ ][ ] [ ] [ ]
1 0 0 0.87 −0.36 0.51

[ ]
0.51 0 0 0
0 0 1 0 0 −1 0 0 0 0
r c 1= A 1 r c 1= = ⇒ r c 1= 0 ⇒ J T 1= 0 0 0
0 1 0 q1 0 q1
q1 1 0 0
0 0 0 1 1 1

[ ][ ] [ ]
−1 0 0 0.87 0.87

[ ]
0
0.87
0 0 2 0 −1 0 −q 2 −0.68 0.68−q2 0
r c 2= A 2 r c 2= = ⇒ r c 2= 0.68−q 2
0 0 −1 q 1 0 q1
q1
0 0 0 1 1 1

[ ]
0 0 0
⇒ J T 2= 0 −1 0
1 0 0
 [ ]
S1 ( a 3 S3 +2 q 2)

[ ]
2
−0.21 −C 1 ( a3 S 3+2 q 2 )
0 0 3
r c 3= A 3 r c3 =
[ −C 3 S3 0
−S3 −C 3 −S1
−0.6 C 3 +0.87 )
−0.6 S3−q2 ¿ ]
¿ 0 ¿ 0 ¿ 1 ¿ q1 ¿ 0 ¿ 0 ¿ 0 ¿ 1 ¿
0
−0.03
1
= 2
a C
0
⇒ r c3

d 1− 3 3
2
1

[ ]
0 S1 0.39 S 3
0 −1 −0.39C 3
⇒ J T 3=
a 3 S3
1 0
2

J Ri là ma trận Jacobi quay của khâu i:

[ ] [ ]
∂ ( ω 'i ) ' 0 T 0 1 0 0 0 0 0
'
J Ri= ώ = R 1 Ṙ1= 0 0 −1 ⇒ω 1= 0 0 0
∂ q̇ 1
0 1 0 0 0 0

[ ] [ ] [] [ ]
0 0 0 0 0 0 0 0 0 0
0 ' 0 T0 ' 0
⇒ J R 1= 0 0 0 ώ 2= R 2 Ṙ 2= 0 0 0 ⇒ω 2= 0 ⇒ J R 2= 0 0 0
0 0 0 0 0 0 0 0 0 0

[ ] [] [ ]
0 −q̇ 3 0 0 0 0 0
' 0 T0 ' 0
ώ = R Ṙ3 = q̇ 3 0 0 ⇒ ω3= 0 ⇒ J R 3= 0 0 0
3 3
0 0 0 q̇3 0 0 1

Xác định động năng của Robot

Ma trận ten xơ quán tính của hai khâu 1, 2 và 3 với trục gắn vào khối tâm
song song với hệ trục của khâu cũng tương ứng là hệ quán tính chính trung
tâm

[ ]
J xx 0 0
'
có dạng ΘCi = 0 J yy 0
0 0 J zz

Với J xx= ∫ B ( y 2 + z 2) dm ; J yy= ∫ B ( z 2 + x 2 ) dm; J zz=∫ B ( x 2+ y2 ) dm

 [ ][ ]
J 1 xx 0 0 34 0 0
'
ΘC 1 = 0 J 1 yy 0 = 0 34.27 0
0 0 J 1 zz 0 0 2.5

[ ][ ]
J 2 xx 0 0 2.23 0 0
'
Θ = 0 J 2 yy 0 = 0
C2 0.11 0
0 0 J 2 zz 0 0 2.19

[ ][ ]
J 3 xx 0 0 0.34 0 0.75
'
Θ = 0 J 3 yy 0 = 0
C3 4.6 0
0 0 J 3 zz 0.75 0 4.36

Biểu thức động năng, ma trân khối lượng M (q) :

{∑ }
3
1 1
T = q̇ T ❑ ( J TTi mi J Ti + J TRi Θ'i J Ri ) q̇= q̇T M (q ) q̇
2 i =1 2

[ ]
3 m11 m12 m13
M (q)=∑ ❑ ( J TTi mi J Ti + J TRi Θ'i J Ri ) = m21 m22 m23
i=1
m31 m32 m33

Với các thành phần như sau :

m11=m1 +m2+ m3
m12=m21=m13=m31=0
m22=m2 +m3
m23=m32=0.39 m3 C3
m33=4.36+0.1521 m3

- Xác định các ma trận Clioris

[]
c1 3
C (q , q̇ )=[ c j ( q , q̇) ]3 ×1= c 2 c j=∑ ❑(k ,l , j) ( q̇k q̇ l ) ;
k=1
c3
c 1=(1 ,1 , 1) q̇ 1 q̇ 1+(1 , 2, 1) q̇ 1 q̇2 +(1 , 3 , 1) q̇1 q̇ 3+(2 ,1 , 1) q̇ 2 q̇1 +(2 , 2 ,1) q̇2 q̇2 +(2 ,3 , 1) q̇ 2 q̇3
+(3 ,1 , 1) q̇3 q̇ 1+(3 , 2 ,1) q̇3 q̇2 +(3 ,3 , 1) q̇3 q̇3

(1 ,1 , 1)=
δ mlj δ mkl
−
δ qk δ q j )
δ m11−
δ q1
−
δ q1
=
] [ ]
δ m11 δ m11 −1 δ m11
2 δ q1
=0
 (1 , 2, 1)=
[
1 δ m11 δ m21 δ m12 1 δ m11
2 δ q2
−
δ q1
−
δ q1
=
2 δ q2
=0
] [ ]
(1 ,3 , 1)=
[
1 δ m11 δ m31 δ m13 1 δ m11
2 δ q3
−
δ q1
−
δ q1
=
2 δ q3
=0
] [ ]
(2 ,1 , 1)=
[
1 δ m21 δ m11 δ m21 1 −δ m11
2 δ q1
−
δ q2
−
δ q1
=
2 δ q2
=0
] [ ]
(2, 2 , 1)=
[
1 δ m21 δ m21 δ m22
2 δ q2
−
δ q2
−
δ q1
=0
]
(2 , 3 , 1)=
[
1 δ m21 δ m31 δ m23
2 δ q3
−
δ q2
−
δ q1
=0
]
(3 , 1 ,1)=
[
1 δ m31 δ m11 δ m31 −1 δ m11
2 δ q1
−
δ q3
−
δ q1
=
2 δ q3
=0
]
(3 ,2 , 1)=
[
1 δ m31 δ m21 δ m32
2 δ q2
−
δ q3
−
δ q1
=0
]
(3 , 3 , 1)=
[
1 δ m31 δ m31 δ m33
2 δ q3
−
δ q3
−
δ q1
=0
]
⇒ c 1=0
c 2=(1 ,1 , 2) q̇ 1 q̇ 1+(1 , 2, 2) q̇1 q̇2 +( 1, 3 , 2) q̇ 1 q̇ 3+(2 , 1 ,2) q̇2 q̇1 +(2 ,2 , 2) q̇ 2 q̇ 2+(2 , 3 ,2) q̇2 q̇3
+(3 ,1 , 2) q̇ 3 q̇ 1+(3 , 2 , 2) q̇3 q̇ 2+(3 , 3 ,2) q̇3 q̇ 3

Làm tương tự ta cũng có c 2=0

c 3=(1 ,1 , 3) q̇ 1 q̇1 +(1 , 2 ,3) q̇1 q̇ 2+(1 ,3 , 3) q̇1 q̇3 +(2 ,1 , 3) q̇ 2 q̇1 +(2 , 2 ,3) q̇ 2 q̇ 2+(2 , 3 ,3) q̇2 q̇3

+(3 , 1 ,3) q̇3 q̇ 1+(3 , 2 ,3) q̇3 q̇ 2+(3 , 3 ,3) q̇3 q̇ 3

(1, 1 , 3)=
[
1 δ m13 δ m13 δ m11 −1 δ m11
2 δ q1
−
δ q1
−
δ q3
=
] [ ]
2 δ q3
=0

(1 , 2 ,3)=
[
1 δ m13 δ m23 δ m12
2 δ q2
−
δ q1
−
δ q3
=0
]
(1 , 3 , 3)=
[
1 δ m13 δ m33 δ m13
2 δ q3
−
δ q1
−
δ q3
=
−δ m33
δ q1
=0
][ ]
(2 , 1, 3)=
[
1 δ m23 δ m13 δ m21
2 δ q1
−
δ q2
−
δ q3
=0
]
(2 , 2 ,3)=
[
1 δ m23 δ m23 δ m22 1 −δ m22
2 δ q2
−
δ q2
−
δ q3
=
2 δ q3
=0
] [ ]
 ( 2 ,3 , 3)=
[
1 δ m23 δ m33 δ m23 1 −δ m33
2 δ q3
−
δ q2
−
δ q3
=
2 δ q2
=0
] [ ]
(3 ,1 , 3)=
[
1 δ m33 δ m13 δ m31 1 δ m33
2 δ q1
−
δ q3
−
δ q3
=
2 δ q1
=0
] [ ]
(3 , 2 ,3)=
[
1 δ m33 δ m23 δ m32
2 δ q2
−
δ q3
−
δ q3
=−
δ m23
δ q3
= 0.2m3 S 3
] [ ]
( 3 ,3 , 3)=
[
1 δ m33 δ m33 δ m33 1 −δ m33
2 δ q3
−
δ q3
−
δ q3
=
2 δ q3
=0
] [ ]
⇒ c 3=0.39 m3 S 3 q̇ 3 q̇2

[][
c1
⇒ C= c 2 =
c3
0
0.2 S 3 m3 q̇3 q̇ 2 ]
a
Q=B q̇+ D si ⁡gn q̇ + F

[ ]
a3 S 3
δ r ip δ rE 3
+ M i=[ −Fx −Fy −Fz ] =¿=[ −Fx −Fy −Fz ] −a3 C 3
T
⇒ Q3 =F ip
δ qi δ q3
0
¿ Fy a 3 C 3−Fx a 3 S3

[ ]
δr 0
⇒Q=F ip ip + M i=
T
0
δ qi
Fy a3 C3−Fx a3 S 3

- Xác định lực suy rộng

Lực suy rộng gồm lực ma sát khô,ma sát nhớt,lực hoạt động không thế có
dạng như sau:
a
¿ Q=B q̇+ D si ⁡gn q̇ + F

[ ]
δ r ip δr a3 S 3
+ M i=[ −Fx −Fy −Fz ] E 3 =¿=¿ [ −Fx −Fy −Fz ] −a3 C 3
T
¿ ⇒ Q3=F ❑ip
δ qi δ q3
0
¿∧Fy a3 C3−Fx a3 S 3

[ ]
T δ r ip 0
⇒ Q=F ❑ip + M i= 0 =[0]
δ qi
Fy a3 C3 −Fx a3 S 3
Từ đó ta có hệ phương trình phân của robot:

[ ][ ] [ ][ ] [ ]
m1+ m2 +m3 0 0 q̈1 0 q̇1 0
0 m2 +m3 0 , 2 m3 C 3 q̈2 + 0 q̇2 + 0
0 0 ,2 m3 C3 1, 82+0 , 04 m3 q̈3 0 , 2 m3 S 3 q̇3 q̇2 q̇3 0 ,39 g S 3

[ ][ ]
U1 0
¿ U2 + 0
U 3 Fy a 3 C 3−Fx a 3 S3