Các hệ trục tọa độ của robot được xây dựng trên quy ước hệ tọa độ Denavit-

Hartenberg.
Hệ tọa độ thì gốc của hệ tọa độ thứ i gắn liền với chính khâu thứ i đó và được đặt
tại giao điểm của đường vuông góc chung giữa hai trục khớp động thứ i + 1 và
khớp động thứ i với chính trục khớp động thứ i + 1. Trong trường hợp hai trục của
khớp động giao nhau thì gốc tọa độ sẽ được lấy trùng với chính giao điểm đó. Còn
nếu hai trục song song với nhau thì gốc tọa độ được chọn là điểm bất kỳ trên trục
khớp động i+1.
Trục Zi của hệ tọa độ thứ i nằm dọc theo trục khớp động thứ i+1.
Trục Xi của hệ tọa độ thứ i nằm dọc theo đường vuông góc chung hướng từ khớp
động i đến khớp động i+1. Trường hợp hai trục giao nhau, hướng trục Xi sẽ trùng
với hướng vecto ZixZi-1, tức là vuông góc cới mặt phẳng chứa Zi, Zi-1.
Bộ thông số DH bao gồm các thông số cơ bản giữa hai khâu liên tiếp nhau. Cụ thể
là:
a i: Khoảng cách giữa 2 khớp liên tiếp theo phương X i

d i: Khoảng cách giữa 2 khớp liên tiếp theo phương Zi−1

α i: Góc quay quanh trục X i giữa Zi−1 và Zi

θi : Góc quay quanh trục Zi−1 giữa X i−1 và X i

Khâu θi di ai αi Biến khớp

1 θ1 d1 0 900 θ1

2’ θ2 0 a2 0 θ2

2 −θ2 0 a2 ‘ 900 θ2

3 −θ3 d3 a3 0 θ3

4 θ4 d4 0 0 θ4

1.2 Thiết lập phương trình động học

Trên cơ sở đã được xây dựng được các hệ tọa độ với hai khâu động liên tiếp
nhau và bộ thông số DH, có thể thiết lập mối quan hệ giữa hai hệ tọa độ liên tiếp
nhau theo các bước sau:
Quay quanh trục Zi-1 một góc θi
Tịnh tiến dọc trục Zi-1 một khoảng di
Tịnh tiến dọc trục Xi-1 (đã trùng với Xi) một khoảng ai
Quay quanh trục Xi-1 một góc αi
Ta được ma trận chuyển từ gốc tọa độ Oi về Oi-1
Bốn bước này được thể hiện bằng tích các ma trận thuần nhất sau:
Ai i 1 Ai  R( z ,i ) .T(0,0,di ) .T(a i ,0,0) .R(x,i )

Các ma trận ở vế phải được tính theo công thức của các phép biến đổi ma trân:
Quay quanh trục OX một góc α:
1 0 0 0
0 cos   sin  i 0 
R  x,  i    i

0 sin  i cos  i 0
 
0 0 0 1

Quay quanh trục OZ một góc θ:

cos i  sin  i 0 0
 sin  cos i 0 0 
R  z , i    i

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

Tịnh tiến theo phương P( Px , Py , Pz)T :

1 0 0 Px 
0 1 0 Py 
Tp  
0 0 1 Pz 
 
0 0 0 1

Áp dụng công thức này ta có ma trận biến đổi sau:

 cos   sin  0 0 1 0 0 0  1 0 0 ai  1 0 0 0
 sin  cos  0 0   0 1 0 0   0 1 0 0   0 cos   sin  0 

 0 0 1 0 0 0 1 di  0 0 1 0   0 sin  cos  0
       
Ai =  0 0 0 1 x 0 0 0 1  x 0 0 0 1  x 0 0 0 1

 cos   cos  .sin  sin  .sin  ai .cos  

 sin  cos  .cos   sin  .cos  ai .sin  

 0 sin  cos  di 
 
= 0 0 0 1 
Thay các thông số tương ứng các khâu vào ta có:
Ma trận chuyển đổi hệ trục tọa độ 1 thành hệ trục tọa độ 0:

cos(1 ) 0 sin(1 ) 0
 sin( ) 0  cos(1 ) 0 
0
A1   1

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

Ma trận chuyển đổi hệ trục tọa độ 2’ thành hệ trục tọa độ 1:

cos( 2 ) sin( 2 ) 0 a2 cos( 2 ) 

 sin( )  cos( ) 0 a2 sin( 2 ) 
1
A2'   2 1

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

Ma trận chuyển đổi hệ trục tọa độ 2 thành hệ trục tọa độ 2’:

 cos( 2 ) 0  sin( 2 ) a2 'cos( 2 ) 

  sin( ) 0  cos( 2 )  a2 'sin( 2 ) 
2
A3   2

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

Ma trận chuyển đổi hệ trục tọa độ 3 thành hệ trục tọa độ 2:

cos(3 ) sin( 3 ) 0 a3 cos(3 ) 

 sin( ) cos( ) 0  a3 sin(3 ) 
2
A3   3 3

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

Ma trận chuyển đổi hệ trục tọa độ 4 thành hệ trục tọa độ 3:

  cos( 4 )  sin( 4 ) 0 0
 sin( ) cos( ) 0 0 
3
A4   4 4

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

Ma trận chuyển đổi hệ trục tọa độ 4 thành hệ trục tọa độ 0:

cos(1  3   4 ) sin(1  3   4 ) 0 a2 'cos(1 )  a2 cos(1 ) cos 2  a3 cos(1  3 ) 

 sin(     )  cos(     ) 0 a 'sin( )  a cos( )sin   a sin(   ) 
0
A4  0 A1.1 A2 . 2 A3 . 3 A4   1 3 4 1 3 4 2 1 2 1 2 3 1 3 

 0 0 1 d1  d 3  d 4  a2 sin( 2 ) 
 
 0 0 0 1 

1.3 Giải phương trình động học.

Phương trình động học .

Vị trí điểm thao tác :

 xE  a2 'cos(1 )  a2 cos(1 ) cos  2  a3 cos(1  3 )


 yE  a2 'sin(1 )  a2 cos(1 ) sin  2  a3 sin(1   3 )
 z  d  d  d  a sin( )
 E 1 3 4 2 2

Hướng của khâu thao tác:

cos(1  3   4 ) sin(1  3   4 ) 0
RE  R4   sin(1   3   4 )  cos(1   3   4 ) 0 
0 
 0 0 1

1.4 Không gian làm việc của tay máy

Tọa độ làm viẹc của tay máy được xây dựng theo hệ tọa độ gốc (OXYZ)
Hình chiếu không gian làm việc trên OXY
Hình chiếu không gian làm việc trên OXZ
Không gian làm việc OXYZ
 Đặt tọa độ các khớp quay -90 -> 90

Code
syms theta1 theta2 theta3 theta4
syms d1 d4
syms a2 a2_o a3
syms theta alpha a d
A=[cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta)
a*cos(theta); sin(theta) cos(alpha)*cos(theta)
-sin(alpha)*cos(theta) a*sin(theta) ; 0 sin(alpha) cos(alpha) d ;
0 0 0 1]
A01=subs(A,[theta,d,alpha,a],[theta1,d1,pi/2,0])
A12=subs(A,[theta,d,alpha,a],[theta2,0,0,a2])
A22=subs(A,[theta,d,alpha,a],[-theta2,0,pi/2,a2_o])
A23=subs(A,[theta,d,alpha,a],[-theta3,0,0,a3])
A34=subs(A,[theta,d,alpha,a],[theta4,d4,0,0])

A04=simplify(A01*A12*A22*A23*A34)

xe=A04(1,4)
ye=A04(2,4)
ze=A04(3,4)

a2=1200;
a3= 1180;
d4= 1550;
d1= 2422.5;
a2_o= 272.38;

x=zeros(1000);y=zeros(1000);z=zeros(1000);

i=1;
for theta1=-pi/2:0.3:pi/2
for theta2=-pi/2:0.3:pi/2
for theta3=-pi/2:0.3:pi/2
x(i)= a2_o*cos(theta1) +
a2*cos(theta1)*cos(theta2) + a3*cos(theta1)*cos(theta3) -
a3*sin(theta1)*sin(theta3)
y(i)= a2_o*sin(theta1) +
a2*cos(theta2)*sin(theta1) + a3*cos(theta1)*sin(theta3) +
a3*cos(theta3)*sin(theta1)
z(i)= d1 - d4 + a2*sin(theta2)
i=i+1;
end
end
end

plot3(x,y,z,'k*')
hold on;
 Khâu θi di ai αi Biến khớp

0 0 d0 0 900 d0

1 θ1 d1 0 900 θ1

2’ θ2 0 a2 0 θ2

2 −θ2 0 a2 ‘ 900 θ2

3 −θ3 d3 a3 0 θ3

4 θ4 d4 0 0 θ4

Ma trận chuyển đổi hệ trục tọa độ 0 thành hệ trục tọa độ gốc:

 1 0 0 0
0 0 1 0 
g
A0  
0 1 0 d0 
 
0 0 0 1

Ma trận chuyển đổi hệ trục tọa độ 1 thành hệ trục tọa độ 0:

cos(1 ) 0 sin(1 ) 0
 sin( ) 0  cos(1 ) 0 
0
A1   1

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

Ma trận chuyển đổi hệ trục tọa độ 2’ thành hệ trục tọa độ 1:

cos( 2 ) sin( 2 ) 0 a2 cos( 2 ) 

 sin( )  cos( ) 0 a2 sin( 2 ) 
1
A2'   2 1

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

Ma trận chuyển đổi hệ trục tọa độ 2 thành hệ trục tọa độ 2’:

 cos( 2 ) 0  sin( 2 ) a2 'cos( 2 ) 

  sin( ) 0  cos( 2 )  a2 'sin( 2 ) 
2
A3   2

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

Ma trận chuyển đổi hệ trục tọa độ 3 thành hệ trục tọa độ 2:

cos(3 ) sin( 3 ) 0 a3 cos(3 ) 

 sin( ) cos( ) 0  a3 sin(3 ) 
2
A3   3 3

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

Ma trận chuyển đổi hệ trục tọa độ 4 thành hệ trục tọa độ 3:

  cos( 4 )  sin( 4 ) 0 0
 sin( ) cos( ) 0 0 
3
A4   4 4

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

Ma trận chuyển đổi hệ trục tọa độ 4 thành hệ trục tọa độ gốc:

0
A4  g A0 . 0 A1.1 A2 . 2 A3 . 3 A4
cos(1  3   4 ) sin(1   3   4 ) 0 a2 'cos(1 )  a2 cos(1 )cos  2  a3 cos(1  3 ) 
 sin(     )  cos(     ) 0 d1  d 3  d 4  a2 sin( 2 ) 
0
A4   1 3 4 1 3 4 
 0 0 1 d 0  a2 'sin(1 )  a2 cos(1 )sin  2  a3 sin(1  3 ) 
 
 0 0 0 1 