Hình 3: Cấu hình robot RRTR

Bảng thông số D-H :

Cách xác định các tham số động học D-H.

Vị trí của hệ tọa độ khớp (Oxyz)i đối với hệ tọa độ khớp (Oxyz)i−1 được xác định bởi 4
tham số θi , d i, α i, a i như sau:

θi : góc quay quanh trục z i−1 để trục x i−1 trùng với trục x ' i( x i /¿ x ' i )

d i : dịch chuyển tịnh tiế dọc trục z i−1 để gốc tọa độ O i−1 chuyể đến O ' i là giao điểm của

trục x i và trục z i−1

a i : dịch chuyển dọc trục x i để điểm O ' i chuyển đến điểm O i

α i : góc quay quanh trục x i sao cho trục z ' i−1( z ' i−1 /¿ z i−1) trùng với trục z i

Bảng 1: Bảng D-H của robot RRTR

 d a α
1 1* 0 L1 0
2  2* 0 L2 π
3 0 d3* 0 0
4  4* d4 0 0

Các ma trận truyền:

Ma trận truyền khâu i so với khâu i-1:

 Cos1 Sin1 0 L1Cos1 


Sin1 Cos1 0 L1Sin1 
0
A1  
 0 0 1 0 
 
 0 0 0 1 
  Cos 2 Sin 2 0 L2Cos 2 
 
Sin 2 Cos 2 0 L2 Sin 2 
1
A2  
 0 0 1 0 
 
 0 0 0 1 

1 0 0 0
 
0 1 0 0
2
A3  
0 0 1 d3 
 
0 0 0 1

 Cos 4 Sin 4 0 0
 
Sin 4 Cos4 0 0
3
A4  
 0 0 1 d4 
 
 0 0 0 1

Ma trận truyền khâu 2, 3, 4 so với khâu 0:

 Cos(1   2 ) Sin(1   2 ) 0 L1Cos1  L2Cos (1   2 ) 

 
Sin(1  2 ) Cos(1  2 ) 0 L1Sin1  L2 Sin(1  2 )
0
A2  0 A1.1 A2   
 0 0 1 0 
 
 0 0 0 0 

 Cos(1   2 ) Sin(1   2 ) 0 L1Cos1  L2Cos (1  2 ) 


Sin(1   2 ) Cos(1   2 ) 0 L1Sin1  L2 Sin(1  2 ) 
0
A3  0 A2 . 2 A3  
 0 0 1 d3 
 
 0 0 0 1 

 Cos(1   2   4 ) Sin(1   2   4 ) 0 L1Cos1  L2Cos (1  2 ) 


 Sin(1  2   4 ) Cos(1   2   4 ) 0 L1Sin1  L2 Sin(1   2 ) 
0
A4  A3 . A4 
0 3
 0 0 1 d3  d 4 
 
 0 0 0 1 

2. Tính vận tốc điểm tác động cuối ,vận tốc góc khâu thao tác

Các thông số đầu vào :

1
*
 *
2  *
4 d *
3
, , ,
Thông số cần xác định: điểm tác động cuối P=( Px,Py,Pz),hướng của khâu cuối

Tọa độ điểm tác động cuối:

Px  0 A4  1, 4  L1Cos1  L2Cos (1  2 )

Py  0 A4  2, 4  L1Sin1  L2 Sin (1  2 )
Pz  0 A4  3, 4   d 3  d 4

Hướng của khâu thao tác có ma trận cosin chỉ hướng:

 Cos(1   2   4 ) Sin(1   2   4 ) 0
 
0
R4   Sin(1   2   4 ) Cos (1   2   4 ) 0 
 0 0 1


Vận tốc của điểm tác động cuối:

R0
d (0)
vE(0)  rE
dt