Bảng thông số động học của Robot D-H như sau:

i αi-1 ai-1 di θi
1 0 0 d=0.5 θ1*
2 900 0 0 θ2*
3 0 e=0.4 0 θ3*
4 0 f=0.25 0 0

Câu 1: Đặt cấu hình các khung tọa độ cho các khớp và điểm công tác ( từ frame
{0} đến {4})

[ ]
1 0 0 0
0 1 0 0
Khung {0} : 0 0 1 0
0 0 0 1

[ ]
C ϴ1 −S ϴ1 0 ai −1
S ϴ1 C α C ϴ1 C α −Sα −Sα d i
Khung {1} : i−1 i−1 i−1 i−1

S ϴ 1 Sα i−1
C ϴ 1 Sα
i−1
Cα i−1
C α di
i−1

0 0 0 1

[ ]
C ϴ2 −S ϴ2 0 a i−1
S ϴ2 C α C ϴ2 C α −S α −Sα d i
Khung {2} : i−1 i−1 i−1 i−1

S ϴ 2 Sα i−1
C ϴ 2 Sα
i−1
Cα i−1
C α di
i−1

0 0 0 1

[ ]
C ϴ3 −S ϴ3 0 a i−1
S ϴ3 C α C ϴ3 C α −S α −S α di
Khung {3} : i−1 i−1 i−1 i−1

S ϴ3 S α i−1
C ϴ3 S α
i−1
Cα i−1
C α di
i−1

0 0 0 1

[ ]
C ϴ4 −S ϴ4 0 a i−1
S ϴ4Cα C ϴ4 C α −S α −S α di
Khung {4} : i−1 i−1 i−1 i−1

S ϴ4 S α i−1
C ϴ4 S α i−1
Cα i−1
C α di
i−1

0 0 0 1
Cấu hình tọa độ khung
Câu 2: Xác định giới hạn và vẽ không gian công tác của robot này
Giới hạn => các góc xoay = 360°

Hình vẽ không gian công tác của Robot RRR

Câu 3: Xác định ma trận liên hệ giữa các thanh nối và tìm i−1iT , 04T , 03T (end –
effector, vị trí cuối của tay máy trong không gian công tác)

[ ]
C θi −S θ i 0 ai−1
i−1 S θi .C α i−1 C θi . C α i−1 −S α i−1 d i . S α i−1
iT =
S θi . S α i−1 C θi . S α i−1 C α i−1 d i .C α i−1
0 0 0 1

[ ][ ]
C θ1 −S θ1 0 a0 C θ1 −S θ 1 0 0
S θ1 cα 0 C θ1 c α 0 −s α 0 −s α 0 d 1 = S θ1 C θ 1 0 0
T =01¿ ¿
S θ1 s α 0 C θ 1 s α 0 c α 0 c α 0 d1 0 0 1 0.5
0 0 0 1 0 0 0 1
 [ ][ ]
C θ2 −S θ 2 0 a1 C θ2 −S θ 2 0 0
S θ2 cα 1 C θ2 c α 1 −s α 1 −s α 1 d 2 −1 0
T =12¿ ¿= 0 0
S θ2 s α 1 C θ2 s α 1 c α 1 c α 1 d2 S θ2 C θ 2 0 0
0 0 0 1 0 0 0 1

[ ][ ]
C θ3 −S θ 3 0 a2 C θ3 −S θ 3 0 0.4
S θ3 cα 2 C θ3 c α 2 −s α 2 −s α 2 d 3
T =23¿ ¿ = S θ 3 C θ3 0 0
S θ3 s α 2 C θ 3 sα2 c α 2 c α2 d 3 0 0 1 0
0 0 0 1 0 0 0 1

[ ] [ ]
C θ4 −S θ4 0 a3 1 0 0 0.25
S θ 4 cα 3 C θ4 c α 3 −s α 3 −s α 3 d 4 0 1 0 0
T =34 ¿ ¿ =
S θ 4 s α 3 C θ 4 sα 3 c α 3 c α 3 d4 0 0 1 0
0 0 0 1 0 0 0 1

[ ][ ][ ]
c 1 −s 1 0 0 c2 −s 2 0 0 c 3 −s 3 0 0.4
0 0 1 2 s c1 0 0 0 0 −1 0 s 3 c 3 0 0
T = 3¿ 1T 2T T = 3¿ 1 ¿¿
0 0 1 0.5 s 2 c 2 0 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

[ ]
c1 c 2 c 3−c1 s 2 s 3 −c 1 c 2 s3−c1 s 2 c3 s1 0.4 c 1 c 2
s c c −s s s −s 1 c 2 s3−s 1 s2 c 3 −c 1 0.4 s1 c 2
T =03¿ 1 2 3 1 2 3 ¿
s 2 c 3+ c2 s 3 −s 2 s3 + c2 c 3 0 0.4 s 2+ 0.5
0 0 0 1

[ ][ ]
c1 c 2 c 3−c 1 s2 s 3 −c 1 c 2 s3−c1 s 2 c 3 s1 0.4 c 1 c 2 1 0 0 0.25
s c c −s s s −s1 c 2 s3−s 1 s2 c 3 −c 1 0.4 s1 c 2 0 1 0 0 ¿
T =04 ¿ 03T 34T = 1 2 3 1 2 3
s2 c 3+ c 2 s3 −s 2 s 3+ c2 c 3 0 0.4 s 2+ 0.5 0 0 1 0
0 0 0 1 0 0 0 1

=>
0
4T= [ c 1 c 2 c3 - c 1 s 2 s 3 - c 1 c 2 s 3 - c1 s2 c3 s1 ¿+0.4 c1 c2 ¿ s1 c 2 c 3 - s1 s2 s3 ¿ - s 1 c2 s3 - s1 s 2 c 3¿ - c1 ¿ 0.25( s1 c2 c3−s1 s2 s3 ¿ +0

Câu 4. Xác định vị trí tay khi thực hiện thao tác lần lượt quay quanh các trục
z 1 , z 2 , z 3 các góc θ1=-45° -i; θ2=90° ; θ3=30° +i (với i =13)

θ1=−58° ; θ2=90° ; θ1=43 ° .

T= [ c 1 c 2 c3 - c 1 s 2 s 3 - c 1 c 2 s 3 - c1 s2 c3 s1 ¿+0.4 c1 c2 ¿ s1 c 2 c 3 - s1 s2 s3 ¿ - s 1 c2 s3 - s1 s 2 c 3¿ - c1 ¿ 0.25( s1 c2 c3−s1 s2 s3 ¿ +0

0
4
 [ ]
−0.3614 −0.3875 −0.8480 −0.0904
0 −0.5784 0.6202 −0 .5299 0.1446
4T =
0.7314 −0.6820 0 1.0828
0 0 0 1

Do đó hướng và vị trí của tay máy sau lần lượt các phép quay là
A T
❑ P ¿[−0.0904 , 0.1446 ,1.0828 ]

5. Giá trị các biến khớp thay đổi thế nào khi bộ phận công tác của tay máy di
chuyển từ vị trí ban đầu sang vị trí mới có tọa độ ❑A P ¿[0.3333 , 0.1565 , 0.4835]T (Giải
bài toán động học ngược để tìm giá trị các biến khớp).
Ta có:

0
4 T= [ c 1 c 2 c3 - c 1 s 2 s 3 - c 1 c 2 s 3 - c1 s2 c3 s1 ¿+0.4 c1 c2 ¿ s1 c 2 c 3 - s1 s2 s3 ¿ - s 1 c2 s3 - s1 s 2 c 3¿ - c1 ¿ 0.25( s1 c2 c3−s1 s2 s3 ¿ +0

để tìm biến khớp để ma trận biến đổi dạng

[ ]
r 11 r 12 r 13 Px
0 r 21 r 22 r 23 P y
4T =
r 31 r 32 r 33 P z
0 0 0 1

P x =0.25 C1 ( C2 C3−S2 S3 ) + 0.4 C1 C2

P y =0.25S1 ( C2 C3−S2 S3 ) + 0.4 S1 C2

P z=0.25 C1 ( S2 C3−C2 S3 ) +0.4 S2 +0.5

Ta xét phương trình sau:

-nhân cả 2 vế của P xvới S1 và P y với C1 thì ta có :
P x S1=0.25 C1 S1 ( C2 C3−S2 S3 ) +0.4 C1 C2 S1

P y C1 =0.25 S1 C1 ( C2 C3−S2 S3 ) +0.4 S1 C2 C1

→ P y C1 −P x S1 =0.25 S1 C1 ( C2 C3−S2 S3 ) +0.4 S1 C2 C1 −0.25 C1 S1 ( C2 C3−S2 S3 ) +0.4 C1 C2 S1

↔ P y C1 −P x S1 =0
S1 Py Py
↔C =
Px
↔ tanθ 1= P ↔θ1= Atan 2(P y , Px )
1 x
↔θ1=25 ,15=± 250 9'

Câu 6. Hãy tính tốc độ của hệ {3} dưới dạng hàm của tốc độ các khớp, viết trong
hệ {3} và hệ {0}, từ đó xây dựng ma trận Jacobian J 3 x3của Robot này.

[ ] [ ]
c1 −s1 0 0
c1 −s1 0
0 s c 0 0 0
R =
ta có 1 T = 1 1 →1 s1 c1 0
0 0 1 0.5
0 0 1
0 0 0 1

[ ]
c1 s1 0
1
→ R = −s 1 c1 0
0
0 0 1

[ ]
1

[ ] [ ]
c 2 −s 2 0 0
c2 −s2 0 c2 0 s2
0 0 −1 0 1 2
T= →2R = 0 0 −1 →1R = −s2 0 c2
s2 c2 0 0
s2 c 2 0 0 −1 0
2 0 0 0 1

[ ] [ ] [ ]
c3 −s3 0 0.4
c3 −s3 0 c3 s3 0
s3 c3 0 0 2 3
2
3T = →3R = s3 c3 0 →2R = −s3 c3 0
0 0 1 0
0 0 1 0 0 1
0 0 0 1

[ ] [ ]
1 0 0 0.25
1 0 0
3 0 1 0 0 3
T =4 ¿ ¿ →4 R = 0 1 0 = 34 R = I
0 0 1 0
0 0 1
0 0 0 1

[]
1
0
1
❑
˙ 0
❑
1
ω 1= R . ω0 + θ1 . 1Z = 0
0 θ˙1

[]
2
s 2 θ˙1
2 1 2
❑ ω 2= R . ω1 + θ̇ . Z = c 2 θ˙1
❑ 2 2

1 θ˙2
 [ ][ ]
3
c3 s2 θ̇1 +s3 c 2 θ̇1 θ̇1 S23
3
❑ ω 3= R . ω2 + θ̇ . Z = −s3 s 2 θ̇1−s3 c2 θ̇1 = θ˙1 C 23
2
❑
3
3 3

2 θ̇2+ θ̇3 θ̇2 + θ̇3

1
❑ 1 v =R . ¿¿

v 2 = 1 R ¿)
2 2

[ ][ ] [ ] [ ]
c2 0 s2 0 0 0
= −s 2 0 c 2
. 0 . 0 = 0
0 0 1 θ̇1 0 0

v 3 = 2 R ¿)
3 3

[ ][ ][ ][ ]
c3 s3 0 θ˙1 . s2 0.4 0.4 θ̇2 . s 3
= −s 3 c3 0 . θ̇1 . c 2 . 0 = 0.4 θ̇2 . c3
0 0 1 θ̇2 0 −0.4 θ̇2

v 3=3 R v 3=1 R 2 R 3 R v 3
0 0 3 0 1 2 3

[ ][ ][ ][ ]
c 1 −s 1 0 c2 −s2 0 c3 −s3 0 0.4 θ˙2 . s3
¿ s1 c1 0 0 0 −1 s3 c3 0 0.4 θ̇2 .c 3
0 0 1 2 2 s c 0 0 0 1 −0.4 θ̇2

[ ][ ]
c 1 c 2 c 3−c 1 c 2 s 3 −c1 c2 s3−c1 s2 cc s1 0.4 θ̇2 . s 3
¿ s 1 c 2 c 3−s 1 s 2 s 3 −s 1 c 2 s3−s 1 s 2 c 3 −c 1 0.4 θ̇2 . c3
s 2 c 3 +c2 s3 −s2 s3 +c 2 c 3 0 −0.4 θ̇2

[ ]
0.4 θ˙2 s 3 c 1 c (2+3) −0.4 θ̇2 c3 c 1 s (2+3)−0.4 θ̇2 s1
¿ −0.4 θ˙2 s3 s 1 c (2+3)−0.4 θ̇2 c3 s1 s(2+3) +0.4 θ̇2 c1
0.4 θ̇2 s 3 s(2+3) + 0.4 θ̇2 c3 c(2+3)
Ma trận Jacobian viêt trong hệ {3}:

[ ]
0 0.4 s 3 0
3
J (Θ )= 0 0.4 c3 0
0 0.4 0

Ma trận Jacobian viêt trong hệ {0}:

[ ]
0 0.4 s 3 c 1 c (2+3) −0.4 c 3 c 1 s (2+3) −0.4 s 1 0
0
J (Θ )= 0 −0.4 s 3 s 1 c (2+3)−0.4 c3 s 1 s (2+3) +0.4 c1 0
0 0.4 s 3 s (2+3) +0.4 c3 c(2+3) 0
Câu 8. Giả sử khớp đầu tiên của robot cần đi từ góc 15 ° qua -45° rồi trở về góc 15
° . Thiết kế hai đoạn quỹ đạo bậc 3 sao cho gia tốc liên tục biết thời gian cho mỗi
đoạn quỹ đạo là 6.5 (s)
-Quỹ đạo bậc 3 khi đi từ góc 15 ° qua -45° thì :
quỹ đạo θ(t) trơn cần thỏa mãn các ràng buộc
θ(0)=θ15 ° ; θ̇ ( 0 ) =0
θ ( t f ) =θ−45° . θ̇ ( t f ) =0

Các ràng buộc này có thể thỏa mãn bằng một đa thức bậc 3
2 3
θ ( t )=a 0+ a1 t+a 2 t + a3 t

Lúc này các ràng buộc và hệ số tìm được là :

θ0 =a0 a 0=θ0
2
θ f =a0 +a1 t f +a 2 t f + a3 t f
3
a1 = 0

3
0 = a1 a 2= 2 (θf −θ0 )
tf
−2
0 = a 1+2 a 2 t f +3 a 3 t 2f a 3= 3
(θ f −θ0 )
tf

Với θ0 =15° và θ f =−45 ° trong thời gian 6.5(s) thì ta tính được các hệ số của đa thức
bậc 3 là :
a 0=15

a 1=0

a 2=−4.26

a 3=0.44

Vậy quỹ đạo vị trí, tốc độ, gia tốc tương ứng là :
2 3
θ ( t )=15−4.26 t + 0.44 t

θ̇ ( t )=−8.52+1.32 t .
2

θ̈ ( t )=−8.52+2.62 t
-Quỹ đạo bậc 3 khi đi từ góc -45 ° qua 15° thì :
quỹ đạo θ(t) trơn cần thỏa mãn các ràng buộc
θ(0)=θ− 45° ; θ̇ ( 0 ) =0
θ ( t f ) =θ15 ° . θ̇ ( t f )=0

Các ràng buộc này có thể thỏa mãn bằng một đa thức bậc 3
2 3
θ ( t )=a 0+ a1 t+a 2 t + a3 t

Lúc này các ràng buộc và hệ số tìm được là :

θ0 =a0 a 0=θ0
2
θ f =a0 +a1 t f +a 2 t f + a3 t f
3
a1 = 0

3
0 = a1 a 2= 2 (θf −θ0 )
tf
−2
0 = a 1+ a2 t f + a3 t 2f a 3= 3
(θ f −θ0 )
tf

Với θ0 =−45 ° và θ f =15 ° trong thời gian 6.5(s) thì ta tính được các hệ số của đa thức
bậc 3 là :
a 0=−45

a 1=0

a 2=4.26

a 3=−0.44

Vậy quỹ đạo vị trí, tốc độ, gia tốc tương ứng là :
2 3
θ ( t )=−45+4.26 t −0.44 t

θ̇ ( t )=8.52 t−1.32t .
2

θ̈ ( t )=8.52−2.64 t .