Chương 2 ĐỘNG HỌC ROBOT

2.1. Vị trí và hướng của vật rắn trong không gian

Hệ tọa độ vật:
Biểu diễn matrận
__ ^ ^ ^
P  a x i  by j  c z k

x
__  y
P 
z
 
 w

x y z
ax  ; by  ; cz 
w w w
 1 0 0 dx 
Tịnh tiến: 0 1 0 d y 
T 
0 0 1 dz 
 
0 0 0 1
Quay:
n x ox ax 0 
n oy a y 0 
F  y

nz oz az 0 
 
0 0 0 1

Quay và tịnh tiến

 nx ox ax Px 
n oy ay 
Py 
F  y

 nz oz az Pz 
 
0 0 0 1
Hệ tọa độ gốc: Oyxz với các véc tơ đơn vị là x, y, z
Vị trí và định hướng của của vật rắn trong không gian
 gắn lên hệ quy chiếu địa phương O’x’y’z’
 o' x 

o'  o' y 
O'  o' x x  o' y y  o' z z 
o' z 
Hướng của vật <== các x'  xx' x  x 'y y  xz' z
véc tơ đơn vị x’, y’, z’: y '  y x' x  y 'y y  y z' z
z '  z x' x  z 'y y  z z' z

Các thành phần của các véc tơ đơn vị (x’x, x’y, x’z) là cosin chỉ
phương của các trục của hệ tọa độ địa phương so với hệ quy
chiếu chung.
X’x = X’.cos(X’,X),…

 x' x y' x z' x 


R  x' y ' z '   x' y y' y z' y 
 x' z y' z z ' z 
 Ma trận quay
Quay quanh trục z

cos  sin  0
Rz ( )   sin  cos 0
 0 0 1

 cos  0 sin  
R y (  )   0 1 0 
 sin  0 cos  

1 0 0 
cos   sin   0 R ( )  0 cos 
 sin  
      x 
x'   sin  ; y '   cos ; z '  0 0 sin  cos 
 0   0  1
Ví dụ
 2.2 Mô tả tối thiểu của hướng
•Góc Euler:
Ba lần quay quanh ba trục của ba hệ quy chiếu khác nhau
• Ví dụ một bộ góc ơle là zyz, nghĩa là quay quanh trục z, quay
quanh trục y, rồi lại quay quanh trục z, tức là trong một bộ góc ơle
có thể quay quanh một trục tối đa 2 lần, song phải là 2 lần không
liên tiếp. Vậy khởi xuất nếu một trục quay có thể có mặt hai lần thì
ban đầu sẽ có bộ 6 lần quay, quanh 6 trục x, y, z, x, y, z.
• Có ba khả năng chọn trục quay đầu tiên hoặc x, hoặc y, hoặc z.
• Có hai khả năng chọn trục quay thứ hai, chọn 2 trong 3 trục trên
trừ trục đã chọn ở bước trước, vì hai trục quay giống nhau không
được thực hiện liên tục.
• Có hai khả năng chọn trục quay lần ba vì có thể chọn lặp lại trục
đầu tiên và còn một trục chưa dùng lần nào.
• Vậy số khả năng của phép quay ơle là k = 3.2.2 = 12)
Phép quay ơle ZYZ

REUL  Rot ( z ,  ).Rot ( y ,  ).Rot ( z , )

, ,,

c c c  s s  c c s  s c c s 
 
  s c c  c s  s c s  c c s s 
  s c s s c 

 Euler RPY

Quay ba lần quanh ba trục Xoay vật liên tiếp ba lần

của ba hệ quy chiếu khác quanh ba trục của hệ quy
nhau, chiếu ban đầu.

đối tượng quay là hệ quy đối tượng quay trong

chiếu phép quay này là vật thể)
• Gọi A là ma trận điểm biểu diễn điểm mút véc tơ cần biến hình
trong cả hai hệ quy chiếu.
• Phép quay vật so với hệ quy chiếu cố định liên tiếp:
RRPY  R( z,  ) R( y' , ) R( x" , ) (1)
• Hay gọi A1 là ảnh của A qua ánh xạ đó ta có:
A1  A.R( z,  )R( y' , ) R( x" , ) (2)
• Sau khi quay vật đi lần thứ nhất bởi phép thực hiện bình thường
vì trục z lúc này là trục cơ bản. Lần quay thứ hai quanh trục y’
không có ma trận quay vì y’ lúc này là trục bất kì, ta phải làm
trùng nó với một trục của hệ quy chiếu rồi sử dụng phép quay có
bản quanh trục y cũ, sau đó trả kết quả lại như sau:
A.R( z,  ) R 1 ( z,  ) R( y,  ) R( z,  )  A.R(y,  )R(z,  ) (3)
• Lúc này trục x” lại là trục bất kì, để có ma trận quay ta lại phải
làm trùng trục quay trước khi quay, sau khi quay bằng ma trận
quay tiêu chuẩn trả kết quả lại như sau:
A.R(y,  )R(z,  )R -1 (z,  )R -1 ( y,  ) R( x, ) R( y,  ) R( z,  )  A.R( x, ) R( y,  ) R( z,  ) (4)
Vậy biểu thức đạt được cuối cùng ở đây chính là một trình tự
ngược lại với (2). Biểu thức (2) biểu thi phép quay RPY còn
(4) biểu thị Euler.
 Phép biến đổi thuần nhất:
• Chuyển động = tịnh tiến + quay

c  s 0 0 c  0  s 0 1 0 0 0
s c 0 0 0 1 0 0 0 c  s 0
Rot ( z, )   
; Rot ( y,  )   ; Rot ( x,  )  
0 0 1 0 s 0 c 0 0 s c 0
     
0 0 0 1 0 0 0 1 0 0 0 1

1 0 0 M
0 1 0 N 
Trans( M , N , P)  
0 0 1 P
 
0 0 0 1
2.3 Bài toán động học
thuận của tay máy
Bài toán động học thuận

• Mục đích của bài toán động học thuận là xác

định vị trí của khâu tác động cuối của tay máy
khi biết các biến khớp của tay máy.
Các bước thực hiện bài toán động học thuận
cho tay máy:
 Để định vị và định hướng từng khâu trên cánh tay cũng như
khâu tác động sau cùng người ta phải gắn các hệ tọa độ suy rộng lên
từng khâu, cả cơ cấu có một hệ quy chiếu chung nối với giá cố định,
hệ quy chiếu này có chức năng vừa để mô tả định vị, định hướng
khâu tác động sau cùng của tay máy, vừa để mô tả đối tượng tác
động của tay máy mà nó cần nhận diện. Việc xây dựng các hệ quy
chiếu này cần có tính thống nhất cao, đòi hỏi tính xác định duy nhất.
Sau đây sẽ xem xét quy tắc DH là một quy tắc điển hình.
Quy tắc Denavit-Hartenberg (D-H)
i: là góc quay của thanh nối thứ i,
di: là độ lệch khâu,
ai: là độ dài đường vuông góc chung giữa Zi-1
và Zi ,
αi: là góc vặn của thanh nối.
ai: khoảng cách giữa hai khớp theo phương đường
vuông góc chung.
di : khoảng cách giữa giao điểm của hai đường vuông
góc chung với trục quay, tính theo phương của đường
vuông góc chung.
i là góc quay quanh trục xi để zi-1 đến trùng với zi.
i là góc quay quanh trục zi-1 để xi-1 đến trùng với xi.
 Biến đổi hệ quy chiếu Oi-1
trùng với hệ quy chiếu Oi:
• Tịnh tiến Oi-1  Ói (di
theo trục zi-1)
• Quay O’i quanh trục z’i
bằng ma trận quay R(z’i, i)

 R ( Zi, i ).R (0,0, di)

i 1
Ai '
ci  si 0 0 
s ci 0 0 
  i 
 0 0 1 di 
 
 0 0 0 1 
•Tịnh tiến O’i theo trục
x’i một lượng ai
•Quay Ói ở bước trên
quanh trục x’i góc để
hoàn thiện.

1 0 0 ai 
0 ci  si 0 

i'
Ai
0 si ci 0
 
0 0 0 1
• Ma trận biến hình tổng hợp đạt đựơc bằng cách nhân hai
ma trận trên có dạng:

ci  si ci si si ai ci 

s ci ci  ci si 
ai si 
i 1 i 1 i '
Ai (qi )  Ai ' Ai   i
0 si ci di 
 
0 0 0 1 
Một số ví dụ ứng dụng quy tắc DH:

Ví dụ 1 : Robot 2 thanh nối phẳng

Ví dụ 2 :
Tay máy ba khâu phẳng
Tay máy ba khâu phẳng:
• Thay các thông số tương ứng vào các ma trận mẫu tổng quát nói
trên nhận được ma trận biến hình cho từng bước như sau:
ci  si 0 ai ci 
s c 
0 ai s i 
Aii1 (i )   i i

0 0 1 0 
 
0 0 0 1 

• Khi nhân các ma trận này với nhau có ma trận chuyển đổi tổng
hợp:
c123  s123 0 a1c1  a2 c12  a3c123 
s 
c123 0 a1s1  a2 s12  a3 s123 
T3 (q)  A1 A2 A3 
0 0 1 2  123

0 0 1 0 
 
0 0 0 1 
• Ở đây kí hiệu: c123  cos(1  2  3 )
Ví dụ 3 : Robot 3 bậc tự do

Z3
Z0 Z1 Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
Joint i i ai di i
1 0 a0 0 0

2 -90 a1 0 1

3 0 0 d2 2
 C i  C i S i S i S i ai C i 
 S C i C i  S i C i ai S i 
Ti i 1  i
 0 S i C i di 
 
 0 0 0 1 

cosθ0  sinθ0 0 a0 cos 0  cosθ1 0  sin 1 a1 cos1 

 sinθ  sinθ a1 sin 1 
cosθ0 0 a0 sin  0  T 21 
0 cos1
T 01 
1
0
 0 1 0 0 
 0 0 1 0   
   0 0 0 1 
 0 0 0 1 

cosθ 2  sinθ 2 0 0
sin  cos 2 0 0 
T 23  2
T03  (T 01)(T 21)(T 23)
 0 0 1 d2 
 
 0 0 0 1
Ví dụ 4 : Tay máy tọa độ cầu:
Sơ đồ động và bảng thông số DH cho thấy như
hình vẽ:
 • Vì z0 và z1 cắt nhau nên d1 = 0. Từ bảng thông số DH có các
ma trận chuyển vị thành phần như sau:

c1 0  s1 0 c2 0 s2 0 1 0 0 0
s 0 0 
s 0 c  0  c2 0  1 0
A10 (1 )   1 1 0 A1 ( )   2 A32 (d 3 )  
 0 1 0
2 2
0 d2  0 0 1 d3 
0 1 0
 
   
0 0 0 1 0 0 0 1
0 0 0 1

• Nhân các ma trận trên với nhau có ma trận chuyển vị tổng

hợp:
c1c2  s1 c1s2 c1s2 d3  s1d 2 
s c c s s s s d  c d 
T3 (q)  A1 A2 A3 
0 0 1 2  1 2 1 1 2 1 2 3 1 2

 s2 0 c2 c2 d 3 
 
 0 0 0 1 
Ví dụ 5 : Robot Stanford
 Z7
Z6 Z4
X7 Z5

X6 X4
X3
Z3 Z2
X5 Z1
X2

X1 Y1
• DH Parameters of Stanford Arm

i ai di i i
1 0 d1 90° 1
2 0 d2 90° 2
3 0 d3 (var) 90° 90°
4 0 0 90° 4
5 0 d5 0° 5
6 0 d6 0 6
 #  d a 
1 1 0 0 -90

2 2 d2 0 90

3 0 d3 0 0

4 4 0 0 -90

5 5 0 0 90
Fig. 2.31 The frames of the Stanford Arm.
6 6 0 0 0
Ví dụ 6 : Robot
SCARA

Khâu i i Li Di

1 1* 0 L1 H

2  2* 1800 L2 0

3 0 0 0 d 3*

4  4* 0 0 d4
 C1  S1 0 L1C1 
S C1 0 L1S1 
T01  R( z ,1 ).T (l1 ,0,0)   1
0 0 1 H 
 
0 0 0 1 
C2 S2 0 L2C2 
S C2 0 L2 S 2 
T12  R( z , 2 ).T (l2 ,0,0)   2
0 0 1 0 
 
0 0 0 1 
1 0 0 0
0 1 0 0 
T23  T (0,0, d3 )  
0 0 1 d3 
 
0 0 0 1

C4  S4 0 0
S C4 0 0 
T34  R( z , 4 )   4
0 0 1 d4 
 
0 0 0 1
 C12  S12 0 L1C1  L2C12 
S C12 0 L1S1  L2 S12 
T02  T01.T12   12
 0 0 1 H 
 
 0 0 0 1 
C12  S12 0 L1C1  L2C12 
S C 0 L S  L S 
T03  T01.T12.T23   12 12 1 1 2 12 

0 0  1  d3  H 
 
0 0 0 1 

C123  S123 0 L1C1  L2C12 

S C 0 L S  L S 
T04  T01.T12 .T23.T34   123 123 1 1 2 12 

 0 0 1  d3  d 4  H 
 
 0 0 0 1 
Ví dụ 7 : Robot Puma
Puma
Example: Puma 560
Example: Puma 560

Joint i i i ai(mm) di(mm)

1 1 0 0 0
2 2 -90 0 d2
3 3 0 a2 d3
4 4 90 a3 d4
5 5 -90 0 0
6 6 0 0 0
 Forearm of a PUMA

d4
x4
x3
a3 z4
y3 x6
x5
y5 z6

Spherical joint
Example: Puma 560
Different Configuration
Link Coordinate Parameters
PUMA 560 robot arm link coordinate parameters

Joint i i i ai(mm) di(mm)

1 1 -90 0 0
2 2 0 431.8 149.09
3 3 90 -20.32 0
4 4 -90 0 433.07
5 5 90 0 0
6 6 0 0 56.25
Example: Puma 560
Bài tập 1
Cho cơ cấu robot như hình vẽ
• Xây dựng khung toạ độ thanh nối theo Denavit-Hartenberg.
• Tìm ma trận xác định hướng và vị trí điểm cuối của tay Robot.
Giải thích ý nghĩa của ma trận này ?
• Xác định vị trí và hướng tay khi : d1=30 cm, 1=60o, a2 = 40cm,
2=0o, a3 = 25cm, 3=30o,a4 = 35cm, 4=60o ?
Giải :
Xây dựng khung tọa độ thanh nối theo D-H như sau :

Joint i i i ai(cm) di(cm)

1 1 90 0 d1
2 2 0 a2 0
3 3 0 a3 0
4 4 0 a4 0
 Giải :
Viết các ma trận chuyển đổi như sau :

c1 0 s1 0  c2  s2 0 a 2 c2 
s  
0  c1 0  2  s2 c2 0 a2 s2 
A0 
1  1
A1 
0 1 0 d1  0 0 1 0 
   
0 0 0 1 0 0 0 1 

c3  s3 0 a3c3   c4  s4 0 a 4 c4 
s c3  
0 a3 s3  4  s4 c4 0 a4 s4 
A2 
3  3
A3 
0 0 1 0  0 0 1 0 
   
0 0 0 1  0 0 0 1 
 Nhân các ma trận

c1c2  c1s2 s1 a2c1c2 

c s  s1s2  c1 a2 s1c2 
A0  A0 A1 
2 1 2  2 1

 s2 c2 0 a2 s2  d1 
 
 0 0 0 1 

c1c23  c1s23 s1 a2c1c2c3  a3 s2 s3  a2c1c2 

c s  s s  c1 a3 s1c23  a2 s1c2 
A0  A0 A2 
3 2 3  23 1 1 23 
 s23 c23 0 a3 s23  a2 s2  d1 
 
 0 0 0 1 
 A04  A01. A12 . A23 . A34  A03 A34 
c1c234  c1s234 s1 a4c1c234  a3c1c2 c3  a3 s2 s3  a2 c1c2 
c s  s s  c a s c  a s c  a s c 
  234 1 1 234 1 4 1 234 3 1 23 2 1 2 
 s234 c234 0 a4 s234  a3 s23  a2 s2  d1 
 
 0 0 0 1 
Thay số với : d1=30 cm, 1=60o, a2 = 40cm, 2=0o, a3 = 25cm,
3=30o,a4 = 35cm, 4=60o

 1 3 25 3  80 
0   0
2 2 4  0,5 0,866 30,825 
  
0 
3

1 75  80 3  0
  0,866  0,5 53,391
A0  
4
 

2 2 4
 1 0 0 77,5 
 0 
155
1 0 0 0 0 1 
 2 
0 0 0 1 
Lưu ý
Ma trận D-H

ci  si ci si si ai ci 

s ci ci  ci si 
ai si 
i 1 i 1 i '
Ai (qi )  Ai ' Ai   i
0 si ci di 
 
0 0 0 1 
Bài tập 2
• 1. Xây dựng khung toạ độ thanh nối và lập bảng thông số theo
Denavit-Hartenberg.
• 2. Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
• 3. Xác định vị trí và hướng tay khi :
• d1=30 cm, 1=60o, a1 = 40cm, d2=30cm, d3 = 40cm ?
d3

d2

a1
d1


Giải :
Xây dựng khung tọa độ thanh nối theo D-H như sau :

Joint i i i ai(cm) di(cm)

1 1 0 a1 d1
2 -90 -90 0 d2
3 0 0 0 d3
Giải :
Viết các ma trận chuyển đổi như sau :

c1  s1 0 a1c1  0 0 1 0
s c 
0 a1s1   1 0 0 0 
A1 
0  1 1
A21  
0 0 1 d1   0 1 0 d2 
   
0 0 0 1  0 0 0 1

1 0 0 0
0 1 0 0 
A3  
2
0 0 1 d3 
 
0 0 0 1
Nhân các ma trận

 s1 0 c1 a1c1 
 c 0 s1 a1s1  
A0  A0 A1 
2 1 2  1

 0 1 0 d 2  d1 
 
 0 0 0 1 

 s1 0 c1 c1d 3  a1c1 
 c 0 
s1 s1d 3  a1s1 
A0  A0 A2 
3 2 3  1

 0 1 0 d 2  d1 
 
 0 0 0 1 
  s1 0 c1 c1d 3  a1c1 
 c 0 
s1 s1d 3  a1s1 
A0  A0 A2 
3 2 3  1

 0 1 0 d 2  d1 
 
 0 0 0 1 
Thay số với : d1=30 cm, 1=60o, a1 = 40cm, d2=30cm, d3 = 40cm

 3 1 
 0 40 
 2 2 

A0  
3 1
0
3
40 3 
 2 2 
 0 1 0 60 
 
 0 0 0 1 
Bài tập 3
• 1. Xây dựng khung toạ độ thanh nối và lập bảng thông số theo
Denavit-Hartenberg.
• 2. Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
• 3. Xác định vị trí và hướng tay khi : d1=30 cm, 1=60o, a2 =
40cm, 2=90o, d3 = 20 cm ?

d3


a2
d1


Giải :
Xây dựng khung tọa độ thanh nối theo D-H như sau :

Joint i i i ai(cm) di(cm)

1 1 90 0 d1
2 2 -90 a2 0
3 0 0 0 d3
Giải :
Viết các ma trận chuyển đổi như sau :

c1 0 s1 0 c2 0  s2 a2 c2 
s  
0  c1 0  1  s2 0 c2 
a2 s 2 
A1 
0  1
A2 
0 1 0 d1  0 1 0 0 
   
0 0 0 1 0 0 0 1 

1 0 0 0
0 1 0 0 
A32  
0 0 1 d3 
 
0 0 0 1
Nhân các ma trận

c1c2  s1  c1s2 a2c1c2 

c s c1  s1s2 a2c2 s1  
A0  A0 A1 
2 1 2  2 1

 s2 0 c2 a2 s2  d1 
 
 0 0 0 1 

c1c2  s1  c1s2 a2c1c2  c1s2 d 3 

c s c1  s1s2 
 s2 s1d 3  a2c2 s1 
A0  A0 A2 
3 2 3  2 1

 s2 0 c2 c2 d 3  a2 s2  d1 
 
 0 0 0 1 
 c1c2  s1  c1s2 a2c1c2  c1s2 d 3 
c s c1  s1s2 
 s2 s1d 3  a2c2 s1 
A0  A0 A2 
3 2 3  2 1

 s2 0 c2 c2 d 3  a2 s2  d1 
 
 0 0 0 1 
Thay số với : d1=30 cm, 1=60o, a2 = 40cm, 2=90o, d3 = 20 cm

 3 1 
0    10 
 2 2 
A0  0  10 3 
1 3

3
 2 2 
1 0 0 70 
 
0 0 0 1 
Bài tập 4
• 1. Xây dựng khung toạ độ thanh nối và lập bảng thông số theo
Denavit-Hartenberg.
• 2. Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
• 3. Xác định vị trí và hướng tay khi : d1=30 cm, 1=60o, d2 =
40cm, 2=90o, d3 = 20 cm ?
Giải :
Xây dựng khung tọa độ thanh nối theo D-H như sau :

Joint i i i ai(cm) di(cm)

1 1 -90 0 d1
2 2 90 0 d2
3 0 0 0 d3
Giải :
Viết các ma trận chuyển đổi như sau :

c1 0  s1 0   c2 0 s2 0
s 0 c 0  s 0  c2 0 
A1  
0 1 1  A2 
1  2

 0  1 0 d1  0 1 0 d2 
   
0 0 0 1 0 0 0 1

1 0 0 0
0 1 0 0 
A32  
0 0 1 d3 
 
0 0 0 1
Nhân các ma trận

c1c2  s1 c1s2  s1d 2 

c s c1 s1s2 c1d 2 
A0  A0 A1 
2 1 2  2 1

  s2 0 c2 d1 
 
 0 0 0 1 

c1c2  s1 c1s2 c1s2 d 3  s1d 2 

c s c1 s1s2 
s2 s1d 3  c1d 2 
A0  A0 A2 
3 2 3  2 1

  s2 0 c2 c2 d 3  d1 
 
 0 0 0 1 
 c1c2  s1 c1s2 c1s2 d 3  s1d 2 
c s c1 s1s2 
s2 s1d 3  c1d 2 
A0  A0 A2 
3 2 3  2 1

  s2 0 c2 c2 d 3  d1 
 
 0 0 0 1 
Thay số với : d1=30 cm, 1=60o, d2 = 40cm, 2=90o, d3 = 20 cm

 3 1 
0  10  20 3 
 2 2 

A0  0
3 1 3
10 3  20 
 2 2 
 1 0 0 30 
 
 0 0 0 1 
2.5 ĐỘNG HỌC NGƯỢC
Inverse Kinematics
 Ví dụ

Tìm :
y
θ  arctan( )
x
(x , y)
Trong tính toán:
y arctan2() specifies that it’s in the
θ  arctan 2( ) first quadrant
x
Y

S
1 Finding S:
X

S  (x 2  y 2 )
Cơ cấu hai khâu phẳng:

(x , y) Given: l1, l2 , x , y

2 Find: 1, 2

l2

(x , y)

1 l1
Phương pháp hình học
(x , y) Using the Law of Cosines:
l2
2
c 2  a 2  b 2  2ab cos C
( x  y )  l1  l2  2l1l2 cos(180  θ 2 )
2 2 2 2

cos(180  θ 2 )   cos(θ 2 )
x 2  y 2  l1  l2
2 2
l1
 cos(θ 2 ) 
2l1l2
1
 x 2  y 2  l12  l2 2 
θ 2  arccos 

Using the Law of Cosines:  2 l l
1 2 
Redundant since 2 could be in the
sin B sin C
 first or fourth quadrant.
b c
sin θ1 sin(180  θ 2 ) sin(θ 2 )
  Redundancy caused since 2 has two
l2 x y
2 2
x 2  y2 possible values

θ1  θ1  α  l sin(θ )   y
θ1  arcsin  2 2 
 arctan 2 
 y  x y 
2 2
x
α  arctan 2   
x
The Algebraic Solution
l2 (x , y) 
2

c1  cosθ1
c1 2  cos(θ 2  θ1 )
l1 (1) x  l1 c1  l2 c1 2
(2) y  l1 s1  l2 sin 1 2
1
(3) θ  θ1  θ 2

(1) 2  (2) 2  x 2  y 2 
 2 2
) 
 l1 c1  l2 (c1 2 ) 2  2l1l2 c1 (c1 2 )  l1 s1  l2 (sin 1 2 ) 2  2l1l2 s1 (sin 1 2 )
2 2 2 2
)
 l1  l2  2l1l2 c1 (c1 2 )  s1 (sin 1 2 ) )
2 2

 l1  l2  2l1l2 c 2
2 2 Only Note :
Unknown
 x  y  l1  l2 2 2 2 2
 cos(ab)  (cos a)(cos b)  (sin a)(sin b)
 θ 2  arccos 
 sin( ab)  (cos a)(sin b)  (cos b)(sin a)
 2l1l2 
 x  l1 c1  l2 c1 2 Note :
 l1 c1  l2 c1c 2  l2 s1s2 cos(ab)  (cos a)(cos b)  (sin a)(sin b)
 c1 (l1  l2 c 2 )  s1 (l2 s2 ) sin( ab)  (cos a)(sin b)  (cos b)(sin a)

y  l1 s1  l2 sin 1 2 We know what 2 is from the previous

 l1 s1  l2 s1c 2  l2 s 2 c1 slide. We need to solve for 1 . Now
 c1 (l2 s 2 )  s1 (l1  l2 c 2 ) we have two equations and two
unknowns (sin 1 and cos 1 )
x  s1 (l 2 s 2 )
c1 
(l1  l 2 c 2 )
x  s1 (l 2 s 2 ) Substituting for c1 and
y (l 2 s 2 )  s1 (l1  l 2 c 2 ) simplifying many times
(l1  l 2 c 2 )


1
(l1  l 2 c 2 )

x l 2 s 2  s1 (l12  l 2 2  2l1l 2 c 2 ) ) Notice this is the law of cosines
and can be replaced by x2+ y2

y(l1  l 2 c 2 )  x l 2 s 2  y(l1  l 2 c 2 )  x l 2 s2 
s1  θ1  arcsin  

x 2  y2  x 2
 y 2

Algebraic solution
Consider a 3-link manipulator

We can derive
kinematic equations:

B
WT T T T T
0
3
0
1
1 2
2 3
Algebraic solution
D-H transformation 
X3


Y0 Y3

 
Y1 Y2 X 2


X1 
X0

i i-1 i-1 di i

1 0 0 0 1

2 0 L1 0 2

3 0 L2 0 3
 Algebraic Solution

 c1  s1 0 a0  c1  s1 0 0

 s c c1c 0  s 0  s 0 d1   s1 c1 0 0
0
T   1 0

1
 s1s 0 c1s 0 c 0 c 0 d1   0 0 1 0
   
 0 0 0 1   0 0 0 1
c 2  s 2 0 L1  c 3  s 3 0 L2 
 s c 2 0 0   s c 3 0 0 
1
T   2 2
T   3
2
 0 0 1 0
3
 0 0 1 0
   
 0 0 0 1  0 0 0 1
 Algebraic Solution
The kinematics of the example seen before
are: c s 0 l c l c 
123 123 1 1 2 12
s c123 0 l1s1  l2 s12 
W T  3T 
B 0  123

 0 0 1 0 
 
 0 0 0 1 
Assume goal point is specified by 3 numbers:
c  s 0 x
s c 0 
y
WT 
B  
0 0 1 0
 
0 0 0 1
 Algebraic Solution
By comparison, we get c  c123 s  s123
the four equations: x  l1c1  l2 c12
y  l1s1  l2 s12

Summing the square

x  y  l  l  2l1l2c2
2 2
1
2 2
2
of the last 2 equations:
From here we get an x  y l l
2 2 2 2

expression for c2 c2  1 2
2l1l2

And finally: s2   1  c 2
2   2  A tan 2( s2 , c2 ).
 Algebraic Solution
• The arc cosine function does not behave
well as its accuracy in determining the
angle is dependant on the angle (
cos(q)=cos(-q)).
• When sin(q) approaches zero, division by
sin(q) give inaccurate solutions.
• Therefore an arc tangent function which
is more consistent is used.

  A tan 2(s , c )
 Algebraic Solution
Using c12=c1c2-s1s2 and s12= c1s2-c2s1:

x  k1c1  k 2 s1
y  k1s1  k 2 c1

where k1=l1+l2c2 and k2=l2s2.

To solve these eqs, set
r=+ k12+k22 and =Atan2(k2,k1).
 Algebraic Solution
k1

l2
 2 k2
l1
Then: k1=r cos  , k2=r sin  , and we can
write:
x/r= cos  cos 1 - sin  sin 1
y/r= cos  sin 1 + sin  cos 1
or: cos(+1) = x/r, sin(+1) =y/r
 Algebraic Solution
Therefore:
+1 = atan2(y/r,x/r) = atan2(y,x)
And so:
1 = atan2(y,x) - atan2(k2,k1)
Finally, 3 can be solved from:
1+ 2+ 3 = 
Bài tập
Xây dựng ct tính thuận và ngược
 2.5. Bài toán động học ngược của tay máy:
• Bài toán thuận động học nhằm xác định định vị
và định hướng của phần công tác khi cho trước các
biến khớp. Bài toán ngược cho trước vị trí và định
hướng của khâu tác động sau cùng đòi hỏi phải xác
định bộ thông số tọa độ suy rộng để đảm bảo
chuyển động cho trước của phần công tác.
• Đối với tay máy có kết cấu dạng chuỗi động hở,
nếu cho trước bộ thông số biến khớp thì vị trí và
định hướng của phần công tác xác định duy nhất,
điều này không đúng với các tay máy có cấu trúc
dạng chuỗi động kín.
• Đối với các tay máy dạng chuỗi động hở, ứng với
một bộ thông số mô tả định vị và định hướng của
phần công tác khi giải bài toán ngược có thể xảy ra
các trường hợp:
– Có thể có nhiều lời giải khác nhau;
– Các phương trình đồng nhất thức thường có
dạng phi tuyến, siêu việt, thường không cho lời
giải đúng;
– Có thể gặp nghiệm vô định vì có các liên kết
thừa giống kiểu kết cấu siêu tĩnh;
– Có thể có lời giải toán học, song lời giải này
không chấp nhận được về mặt vật lí do các yếu
tố về kết cấu của cấu trúc không đáp ứng được.
 Nhìn chung khi số bậc tự do càng lớn
thì bài toán ngược càng khó giải, số nghiệm
toán học lại càng nhiều, khi đó để chọn được
nghiệm điều khiển đòi hỏi phải loại bỏ các
nghiệm không phù hợp dựa trên cơ sở các
ràng buộc về giới hạn hoạt động của các
khớp. Việc lựa chọn phương pháp để giải bài
toán ngược cũng là một vấn đề, cho đến nay
không có phương pháp tổng quát nào có thể
áp dụng cho tất cả các robot. Sau đây giới
thiệu một số ví dụ bài toán ngược tay máy của
các cơ cấu đã giải bài toán thuận ở mục trước.
• 2.5.1. Cơ cấu ba khâu phẳng:
• Dựa trên kết quả đã triển khai ở bài toán thuận, ta đã có phương trình động học
của tay máy này dưới dạng ma trận đồng nhất (4.4):

c123  s123 0 a1c1  a2 c12  a3c123 

s c 0 a1s1  a2 s12  a3 s123 
T3 (q)  A1 A2 A3  
0 0 1 2 123 123

0 0 1 0 
 
0 0 0 1 
• Ma trận định vị và định hướng phần tác động sau cùng trên cánh tay được cho trước
trong bài toán ngược dưới dạng như sau:

 a11 a12 a13 a14 

a a22 a23 a24 
A  21

a31 a32 a33 a34 

 
a41 a42 a43 a44 
• Nhiệm vụ của bài toán ngược phải xác định một bộ công thức tính dựa
trên các đồng nhất thức tạo ra từ hai ma trận trên.
1 ; 2 ;3
• Vì biến số nằm trong góc nên nếu giải trực tiếp hệ phương trình mô
tả định vị và định hướng là không thể. Hãy xem hệ thiết lập được
trên 2 điều kiện này:
 sin(1  2  3 )  a12
0  a
 13


0  a23

a1 cos(1 )  a2 cos(1  2 )  a3 cos(1  2  3 )  a14
a1 sin(1 )  a2 sin(1  2 )  a3 sin(1  2  3 )  a24


0  a34
• Ba phương trình đầu của hệ mô tả định hướng của khâu sau cùng, ba
phương trình sau mô tả định vị của khâu sau cùng. Vì hệ suy biến
nên thực chất còn ba phương trình, ba ẩn:

 sin(1  2  3 )  a12

a1 cos(1 )  a2 cos(1  2 )  a3 cos(1  2  3 )  a14
a sin( )  a sin(   )  a sin(     )  a
1 1 2 1 2 3 1 2 3 24
• Nếu đặt   1  2  3 , để mô tả định hướng của khâu sau cùng,
phải cho trước giá trị này.
• Vậy nếu xem đây là hệ hai phương trình hai ẩn với 1 ,2 Matlab có
thể giải ra kết quả, từ đó tính ra tuy nhiên kết quả rất dài không có
tính thực tế.
• Nếu coi điểm W là tâm của khớp quay thứ ba, hay là điểm tựa công
nghệ. Định hướng của khâu sau cùng sẽ đạt được trên cơ sở xoay
hướng khâu sau cùng phải là  từ điểm này.
• Điểm tựa công nghệ W có thể xác định bằng hình học như sau:
 pwx  p x  a3c  a1c1  a2 c12

 pwy  p y  a3 s  a1s1  a2 s12
• Bình phương hai vế phương trình này rồi cộng lại nhận được:
2
pwx  pwy
2
 a12  a22
c2 
2a1a2
• C2 phải thỏa mãn miền giá trị của hàm cosin. Tính được: s 2   1  c2
2

• Thay c2 vào hệ phương trình trên và giải ra được:

(a1  a2 c2 ) pwy  a2 s2 pwx (a1  a2 c2 ) pwx  a2 s2 pwy

s1  c1  2  A tan 2(s2 , c2 )
p p
2
wx
2
wy p p
2 2
wx wy
• Cuối cùng tính được: 3 =  - 1 - 2
• Bài toán ngược kết thúc phần xác định nghiệm toán học, cần tiếp tục căn cứ vào các
yêu cầu cụ thể chọn nghiệm điều khiển.
 2.5.2. Cơ cấu cầu:
• Phương trình động học cơ cấu cầu đã xác định trong bài toán
thuận, nếu tổng quát bài toán ngược đòi hỏi đáp ứng cả định vị và
định hướng của điểm quản lí, sẽ phải giải hệ 6 phương trình (ba định
vị, ba định hướng) để xác định các biến khớp .
• Xuất phát từ phương trình động học trong bài toán thuận:
c1c2  s1 c1s2 c1s2 d 3  s1d 2 
s c c s s s s d  c d 
T3 (q)  A1 A2 A3 
0 0 1 2  1 2 1 1 2 1 2 3 1 2

  s2 0 c2 c2 d 3 
 
 0 0 0 1 
• Ma trận mô tả định vị, định hướng của phần công tác biết trước:
 a11 a12 a13 a14 
a a a a 
A   21 22 23 24 

 a31 a32 a33 a34 

 
a 41 a 42 a 43 a 44 
• Hệ 6 phương trình ba ẩn như sau:
a12   s1
a  c s
 13 1 2


a 23  s1 s 2

c1 s 2 d 3  s1d 2  a14
s1 s 2 d 3  c1d 2  a 24


c2 d 3  a34
• Chúng ta thấy ba phương trình đầu mô tả định hướng của
phần làm việc vì vậy không liên quan gì đến tầm với d3,
mà chủ yếu liên quan đến hai bậc tự do quay. Ngược lại, ba
phương trình sau mô tả định vị nên liên quan chặt chẽ đến
tầm với d3.
• Nếu không đòi hỏi định hướng, chỉ xét hệ gồm ba phương
trình sau trong hệ