Bài tập mô phỏng chương 3+4

KỸ THUẬT ROBOT
Họ và tên sinh viên : Thân Đức Thảo
Lớp : 20TDHCLC3 MSSV: 105200473

I. TÍNH TOÁN ĐỘNG HỌC

- Thực hiện việc gắn các hệ tọa độ lên các khâu của tay máy robot :

Y^ 5

Y^
3

Y^ Y^
4
6

Y^ Y^
1
2

^Z
0 Y^
0

^
X
0

BẢNG THAM SỐ
DH SỬA ĐỔI
ai-1 αi-1 di θi-1
1 0 0 L1=399 θ1
2 0 -90o 0 θ2
3 L2=350 0 0 θ3
 4 L3=42 -90o L4=351 θ4
5 0 90o 0 θ5
6 0 90o 0 θ6
 Viết phương trình động học thuận

[ ]
cθ i −s θ i 0 ai−1
i−1 s θ i c α i−1 c θ i c α i−1 −s α i−1 −s α i−1 d i
iT =
sθ i s α i−1 c θ i s α i−1 c α i−1 c α i−1 d i
0 0 0 1

[ ] [ ]
cos θ1 −sin θ1 0 0 cos θ2 −sin θ2 0 0
0 sin θ1 cos θ1 0 0 1 0 0 1 0
1T = 2T =
0 0 1 399 −sin θ2 −cos θ2 0 0
0 0 0 1 0 0 0 1

[ ] [ ]
cos θ3 −sin θ3 0 350 cos θ 4 −sin θ 4 0 42
2 sin θ3 cos θ3 0 0 3 0 0 1 351
3T = 4T =
0 0 1 0 −sin θ4 −cos θ 4 0 0
0 0 0 1 0 0 0 1

[ ] [ ]
5
cos θ5 −sin θ5 0 0 cos θ6 −sinθ 6 0 0
0 0 1 0 0 0 1 0
4
5T = T=
−sin θ5 −cos θ5 0 0 −sinθ 6 −cos θ 6 0 0
0 0 0 1 6 0 0 0 1

0 0 1 2 3 4 5
6 T = 1T 2T 3T 4T 5T 6T

[ ]
r 11 r 12 r 13 p x
0 r 21 r 22 r 23 p y
6T =
r 31 r 32 r 33 pz
0 0 0 1

r 11 = C1C2C3C4C5C6 – C1S2S3C4C5C6 – S1S4C5C6 + C1C2S3S5C6 + C1S2S3C5C6

+ C1C2C3S4C6 – C1S2S3S4S6 + S1C4S6
r 21 = S1C2C3C4C5C6 – S1S2S3C4C5C6 – C1S4C5C6 + S1C2S3S5C6 + S1S2S3S4C6 +
S1C2C3S4S6 – S1S2S3S4S6 + C1C4S6
r 31 = - S2C3C4C5C6 – C2S3C4C5C6 – S2S3S5C6 + C2C3S5C6 – S2C3S4S6 –
C2S3S4S6
r 12 = - C1C2C3C4C5S6 + C1S2S3C4S5S6 + S1S4C5S6 – C1C2S3C5S6 – C1S2S3C5S6
+ C1C2C3S4C6 – C1S2S3S4C6 + S1C4C6
r 22 = - S1C2C3C4C5S6 + S1S2S3C4C5S6 + C1S4C5S6 – S1C2S3S5S6 – S1S2C3S5S6
+ S1C2C3S4C6 – S1S2S3S4C6 + C1C4C6
r 32 = S2C3C4C5S6 + C2S3C4C5S6 + S2S3S5S6 – C2C3S5S6 – S2C3S4C6 – C2S3S4C6

r 13 = - C1C2C3S4 + C1S2S3S4 – S1C4

r 23 = - S1C2C3S4 + S1S2S3S4 – C1C4

r 33 = S2C3S4 +C2S3S4

p x = 42C1C2C3 – 42C1S2S3 – 351C1C2S3 – 351C1S2C3 + 350C1C2

p y = 42S1C2C3 – 42S1S2S3 – 351S1C2S3 – 351S1S2C3 + 350S1C2

p z = - 42S2C3 – 42C2S3 + 351S2S3 – 351C2C3 – 350C2 + 399

Gắn toạ độ { e } với End-effector :

Y^ e
^Z e

^
X
Ma trận 0e T có dạng: 0e T =06T 6eT

[ ][ ]
r 11 r 12 r 13 p x −1 0 0 0
0 r 21 r 22 r 23 p y 0 −1 0 0
eT =
r 31 r 32 r 33 pz 0 0 1 L5
0 0 0 1 0 0 0 1

[ ]
−r 11 −r 12 r 13 L5 r 13 + p x
0 −r 21 −r 22 r 23 L5 r 23 + p y
eT =
−r 31 −r 32 r 33 L5 r 33+ p z
0 0 0 1

Từ các kết quả trên, ta có các phương trình động học thuận tính toán các tọa độ X e, Y e , Z e,
của khâu chấp hành cuối mô tả trong hệ tọa độ {0} theo các biến khớp của robot.
X e = L5 r 13+ p x

Y e = L5 r 23+ p y

Z e = L5 r 33+ p z

 Giải bài toán động học ngược

[ ] [ ]
cos θ1 −sin θ 1 0 0 cos θ2 0 −sin θ 2 0
0 −1 sin θ1 cos θ 1 0 0 1 -1 −sin θ2 0 −cos θ 2 0
1T = 2T =
0 0 1 −399 0 1 0 0
0 0 0 1 0 0 0 1

[ ] [ ]
cos θ3 sinθ 3 0 −350 cos θ 3 cos θ4 0 −sin θ4 −42cos θ4
2 -1 −sin θ3 cos θ3 0 350 sin θ3 3 -1 −sin θ4 0 −cos θ 4 42 sinθ 4
3T = 4T =
0 0 1 0 0 0 1 −351
0 0 0 1 0 0 0 1

[ ] [ ]
cos θ5 0 −sinθ 5 0 cos θ6 0 −sinθ 6 0
4 -1 −sin θ5 0 −cos θ5 0 5 -1 −sin θ6 0 −cos θ6 0
5T = 6T =
0 1 0 0 0 1 0 0
0 0 0 1 0 0 0 1

- Khớp 1,2,3
 [ ]
r 11 r 12 r 13 px
r 21 r 22 r 23 py
0
1 T −1 = 12T 23T
r 31 r 32 r 33 pz
0 0 0 1

[ ][ ]
cos θ1 −sin θ 1 0 0 r 11 r 12 r 13 px
−sin θ1 cos θ 1 0 0 r 21 r 22 r 23 py
=
0 0 1 −399 r 31 r 32 r 33 pz
0 0 0 1 0 0 0 1

[ ]
C θ2 C θ3−S θ 2 S θ3 −C θ2 S θ 3−S θ2 C θ3 0 350 C θ2
0 0 1 0
= −S θ C θ −C θ S θ S θ S θ −C θ C θ 0 −350 S θ2
2 3 2 3 2 3 2 3
0 0 0 1

(1,4) = n1 = C1Px + S1Py = 350C2 = m1 (1)

(2,4) = n2 = -S1Px + C1Py = 0 = m2 (2)
(3,4) = n3 = Pz – 399 = -350S2 = m3 (3)
S1 Py S1 Py
Từ (2) có 2 khả năng C 1 = Px hoặc C 1 = - Pz

Py Py
 tanθ1 = Px hoặc tanθ1 = - Pz
 θ1 = arctan2 (Py,Px) hoặc θ1 = arctan2(-Py,Pz)
 θ1є [0,90],[-180,-90]
nx = C1Px + S1Py
ny = -S1Px + C1Py
nz = Pz – 399
nx =350C 2
{n =−350 S 2
z

2 2 2
n x =350 C 2
{ 2 => nx2 + nz2 = 3502
nz =3502 S 22

n x =350 C 2
{ n y =0
=> nx + ny = 350C2
 n x +n y
 Cosθ2 = , Sinθ2 = √ 1−cos θ 22
350
 θ2 = arctan2 (sinθ2,cosθ2)

[ ]
r 11 r 12 r 13 p x
2 -1 r 21 r 22 r 23 p y
3T . = 01T 12T
r 31 r 32 r 33 p z
0 0 0 1

[ ][ ]
cos θ3 sinθ 3 0 −350 cos θ 3 r 11 r 12 r 13 px
−sin θ3 cos θ3 0 350 sin θ3 r 21 r 22 r 23 py
=
0 0 1 0 r 31 r 32 r 33 pz
0 0 0 1 0 0 0 1

[ ]
C θ1 C θ2 −S θ 1 S θ2 0 0
Sθ C θ C θ 1 S θ2 0 0
= 1 2
0 0 1 399
0 0 0 1

nx = C3Px + S3Py – 350C3

ny = -S3Px + C3Py + 350S3

nz = Pz -399
C 3 Px+ S 3 Py+ Pz−399
 350
= Cosθ3

Sinθ3 = √ 1−cos θ 23
 θ3 = arctan2(sinθ3,cosθ3)
- Khớp 4,5,6
3 0 0
6 R = ( 3 R )T .6 R

[ ]
C θ 4 C θ5 C θ6 + S θ4 S θ6 S θ 4 C θ6 −C θ4 C θ 5 S θ6 −C θ 4 S θ5 42
−S θ5 C θ6 S θ5 S θ6 −C θ5 351
C θ4 S θ6 −S θ 4 C θ5 C θ6 S θ4 C θ 5 S θ6 +C θ 4 C θ6 S θ4 S θ5 0
0 0 0 1

[ ]
C θ 4 C θ5 C θ6 + S θ4 S θ6 S θ 4 C θ6 −C θ4 C θ 5 S θ6 −C θ 4 S θ5
−S θ5 C θ6 S θ5 S θ6 −C θ5
C θ4 S θ6 −S θ 4 C θ5 C θ6 S θ4 C θ 5 S θ6 +C θ 4 C θ6 S θ4 S θ5
 [ ][ ]
C θ 1 C θ 2 C θ 3−C θ1 S θ2 S θ3 S θ1 C θ2 C θ3−S θ1 S θ2 S θ3 −S θ2 C θ3−C θ2 S θ3 r 11 r 12 r 13
¿ −C θ1 C θ 2 S θ3 −C θ1 S θ 2 C θ 3 −S θ1 C θ2 C θ3−S θ1 S θ 2 C θ 3 S θ2 S θ 3−C θ2 C θ3 r 21 r 22 r 23
−S θ1 C θ1 0 r 31 r 32 r 33

C4S5 = - (C1C2C3-C1S2S3)r13 – (S1C2C3-S1S2S3)r23 + (S2C3+C2S3)r33

C5 = (C1C2S3 + C1S2C3)r13 + (S1C2S3+S1S2C3)r23 – (S2S3-C2C3)r33
S4S5 = -S1r13 + C1r23
S5
 Tanθ5 = , Sinθ5 = √ 1−cos θ 25
C5
 θ5 = arctan2( ¿

√ 1−[ ( C 1 C 2 S 3+C 1 S 2C 3 ) r 13
2
+ ( S 1C 2 S 3+ S 1 S 2C 3 ) r 23− ( S 2 S 3−C 2C 3 ) r 33] ,(C1C2
S3 + C1S2C3)r13 + (S1C2S3+S1S2C3)r23 – (S2S3-C2C3)r33)
S 4 S5
= tanθ 4
C4 S5

 θ 4 = arctan2(-S1r13 + C1r23, - (C1C2C3-C1S2S3)r13 – (S1C2C3-S1S2S3)r23 +

(S2C3+C2S3)r33)
-S5C6 = -(C1C2S3+C1S2C3)r11 – (S1C2S3+S1S2C3)r21 + (S2S3-C2C3)r31
S5S6 = -(C1C2S3+C1S2C3)r12 – (S1C2S3+S1S2C3)r22 + (S2S3-C2C3)r32
S5C6 θ
 θ6 = - = tan ( 6 )
S 5S6 −1
 θ6 = arctan2(-(C1C2S3+C1S2C3)r12 – (S1C2S3+S1S2C3)r22 + (S2S3-C2C3)r32 ,
(C1C2S3+C1S2C3)r11 – (S1C2S3+S1S2C3)r21 + (S2S3-C2C3)r31)