ĐẶT HỆ TRỤC TỌA ĐỘ

BẢNG THÔNG SỐ D-H:

Joint i αi ai di θi

1 +90
0
0 d1 θ1

2 0 a2 d2 θ2

3 +90
0
0 0 θ3

4 −90
0
0 d4 θ4

5 +90
0
0 d5 θ5

6 0 0 d6 θ6

d 1=0.3 m; a2 =0.45 m; d 2=0.004 m; d 5=−0.1835 m; d 6=0.228 m

FORWARD KINEMATICS
Do kết quả ma trận T quá dài nên tụi em có gửi kèm 2 file .txt để thầy có thể test trên
matlab ạ.

INVERSE KINEMATICS
Ma trận T:

[ ]
ux v x w x q x
0 uy v y wy qy
6T =
uz v z w z qz
0 0 0 1

Xét 3 khâu cuối:

{
T
P= ⃗
OP = [ p x p y pz ] =q−d 6 w+|d 5|. z 4
0
4
T (1)
4 P = BP= [ 0 0 d4]
3
⃗

Với:
 []
ux
u=x6 = u y
uz

[]
vx
v= y 6= v y
vz

[]
wx
w=z 6= w y
wz

q là tọa độ điểm Q:
T
q=[q x q y q z ]

Từ hệ tọa độ đã đặt:
z 4 =s θ 6 . u+ c θ6 . v (2)

Từ (1) và (2), suy ra:

{
p x =q x −d 6 . wx +¿ d 5∨.(s θ6 .u x +c θ 6 . v x )
p y =q y −d 6 . w y + ¿ d 5∨.(s θ6 . u y +c θ 6 . v y )
p z=q z −d 6 . w z +¿ d 5∨.(s θ 6 . u z +c θ 6 . v z )

Tìm θ1 :
0 −1 0 1 3
( 1 R ) . 4 P= 3 R . 4 P
 [ ][ ]
p x . c θ 1 + p y . s θ1 d 4 . s (θ2 +θ3 )+a 2 . c θ 2
→ p z−d 1 = a2 . s θ2−d 4. c (θ2 +θ3 ) (3)
p x . s θ 1− p y . c θ1 d2

p x . s θ 1− p y . c θ1=d 2

px py d2
→ . s θ1− . c θ1 =
√p x
2
+ py
2
√p x
2
+ py
2
√p x
2
+ py
2

¿
px py py
Đặt : =cφ và =sφ → tan φ=
√p x
2
+ py
2
√p x
2
+ py
2 px

d2
→ cφ . s θ 1−sφ .c θ1=
√p x
2
+py
2

d2
→ s ( θ1−φ )=
√p x
2
+ py
2

{ (√ )
d2
θ1=arcsin 2 2
+φ
px + p y
→

(√ )
d2
θ 1=1800−arcsin 2 2
+φ
px + py

Tìm θ3 :
Từ (3)
2 2
→ 2 a2 d 4 s θ 3= px + p y +¿
2 2
Đặt K= p x + p y +¿ ¿

Có 2 giá trị θ3

→
{ θ 3=arcsinK
θ 3=180 °−arcsinK

Tìm θ2 :
Từ (3):

→
[ a2 + d 4 s θ 3 d4 c θ3 c θ2
−d 4 c θ 3 a2 +d 4 s θ3 s θ2
= x 1 y 1
][ ] [
p c θ + p sθ
p z−d 1 ]
 → θ2=atan 2(s θ2 , c θ2 )

Tìm θ 4 ,θ5 :

[ ]
ux v x w x
3 0 T 0 T 5 0
5
R(θ 4 ,θ 5)= R ( θ1 , θ2 ,θ3 ) . R . R ( ¿ θ 6)Với R = u y v y w y ¿
3 6 6 6
uz v z w z

[ ][ ]
c θ 4 . c θ5 −s θ4 c θ 4 . s θ 5 ¿ a12 ¿
→ s θ 4 .c θ5 c θ 4 s θ4 . s θ5 = ¿ a22 ¿ (4 )
−s θ 5 0 c θ5 a31 a32 a33

Thực hiện tính toán trong Matlab:

{
a 12=s θ 6 (u x c θ 1 c θ23 +u y s θ 1 c θ23 +u z s θ23 )+ c θ6 (v x c θ1 c θ23 + v y s θ1 c θ23 + v z s θ23)
a 22=−s θ 6 (−u x s θ 1+u y c θ1 )−c θ6 (−v x s θ1 +v y c θ1)
a31=c θ6 ( u x c θ1 s θ23 +u y s θ1 s θ23−uz c θ23 )−s θ6 (v x c θ 1 s θ23 +v y s θ1 s θ 23−v z c θ23 )
a33=w z c θ1 s θ23+ w y s θ1 s θ 23−w z c θ 23

{
→ θ 4=atan 2(−a12 , a22 )
θ 5=atan 2(−a31 , a33)

Kiểm tra θ6 :
Thực hiện tính toán trong Matlab:
a 32=s θ 6 (u x c θ 1 s θ23 +u y s θ1 s θ23−u z c θ 23)+c θ6 (v x c θ1 s θ23+ v y s θ 1 s θ23−v z c θ23)

Từ (4):
→ s θ 6 ( u x c θ1 s θ23+ u y s θ 1 s θ23−u z c θ 23) + c θ6 ( v x c θ1 s θ23+ v y s θ 1 s θ23−v z c θ23 )=0

−v x c θ 1 s θ23 +v y s θ1 s θ23−v z c θ23

→ tan θ6 =
ux c θ 1 s θ23 +u y s θ 1 s θ23−u z c θ 23