62 Phạm Trường Tùng

ĐIỀU KHIỂN QUỸ ĐẠO ROBOT BỐN BẬC TỰ DO

CONTROLLING THE PATH OF 4 DOF ROBOT

Phạm Trường Tùng

Trường Đại học Phạm Văn Đồng; pttung@pdu.edu.vn

Tóm tắt - Robot 4 bậc tự do dạng RRR.R là dạng robot có nhiều ưu
điểm hơn so với các robot 4 bậc tự do khác nhờ khả năng định
hướng và vị trí một cách linh hoạt. Bài báo giới thiệu cách thức điều
khiển điểm tác động cuối của robot bám theo một quỹ đạo trong
không gian Descarte cho trước bằng cách tính toán ma trận biến đổi
thuần nhất bằng phương pháp biến đổi tọa độ. Trên cơ sở ma trận
thuần nhất, tính toán động học ngược của robot và thiết kế quỹ đạo
biến khớp của robot để cơ cấu chấp hành cuối bám theo quỹ đạo đã
cho. Với mô hình động lực học robot, bài báo giới thiệu một bộ điều
khiển theo phương pháp điều khiển trượt (slide mode control) để điều
khiển các khớp theo quỹ đạo biến khớp vừa xác định. Các kết quả
được kiểm tra mô phỏng bằng phần mềm Matlab.
Abstract - The 4DOF (degree of freedom) robot RRR.R has more
advantages than other 4DOF robots through flexible orientation
and position. This paper presents a method to control the end
effector of 4DOF robot to follow the path that is desired on
Descarter-coordinates by calculating the homogeneous matrix.
On the basis of homogeneous matrix, the robot’s inverse
kinematic dynamics is calculated and the robot‘s joint space
trajectory is designed. On the basis of dynamics, this paper
proposes a controller that is designed by slide mode control
theory to control joints according to the determination of joint
trajectories. The results are simulated by using Matlab.

Từ khóa - robot; 4DOF; quỹ đạo biến khớp; quỹ đạo cung tròn; Key words - robot; 4DOF; joint trajector; arc path; slide mode
điều khiển trượt. control.

1. Đặt vấn đề phẳng O0X0Y0 là β0, góc hợp giữa tia ORB với giao tuyến
Robot 4DOF có kết cấu 4 khớp quay RRR.R bị hạn chế mặt phẳng α và mặt phẳng O0X0Y0 là βf, góc hợp giữa hướng
bởi khả năng định hướng trong không gian (so với các của cơ cấu chấp hành cuối với giao tuyến mặt phẳng α và
robot có số bậc tự do cao hơn), do đó khó thực hiện được mặt phẳng O0X0Y0 là β(t). Điểm E thể hiện vị trí hiện tại của
các nguyên công đòi hỏi khắt khe về vị trí và hướng của cơ cơ cấu chấp hành cuối của robot. Cơ cấu chấp hành cuối di
cấu chấp hành cuối (như nguyên công sơn, hàn…). Trong chuyển theo hướng của cung tròn từ A đến B (Hình 2).
khi đó, quỹ đạo cung tròn trong không gian là dạng quỹ đạo
thường được sử dụng trong thực tế (như trong nguyên công (t)
hàn ống). Bài viết giải quyết bài toán định vị trí và hướng 
B OR
của robot bám theo một cung tròn trong không gian nằm x
Z0 y
trong trường công tác của robot và thiết kế bộ điều khiển để E

điều khiển robot theo quỹ đạo đã thiết kế.  z
A
 Y0
2. Kết quả nghiên cứu O0
2.1. Bài toán đặt ra 
Cho robot 4DOF có bốn khớp quay, cho một cung 

tròn được định hướng nằm trong mặt phẳng vuông góc
X0
với mặt phẳng O0X0Z0 gắn trên đế của robot và nằm trong
trường công tác của robot. Điều khiển cơ cấu chấp hành Hình 2. Xác định các giá trị bài toán
cuối của robot bám theo cung tròn và có phương của
vector hướng đi qua tâm cung tròn.
(t)
f
 R
B OR
B OR
Z0 
 E

A
 Y0

O0 A
 Hình 3. Mô hình điểm tác động cuối của robot trên quỹ đạo
Gắn vào điểm OR một hệ trục tọa độ ORxyz, sao cho
X0
mặt phẳng ORxy trùng với mặt phẳng α, trục ORx trùng
Hình 1. Mô hình bài toán với đường ORE, chiều của ORx là chiều của vector ORE.
2.2. Giải bài toán Như vậy ma trận biến đổi thuần nhất mô tả vị trí và
Gọi góc giữa hai mặt phẳng α và mặt phẳng O0X0Z0 là φ, hướng của hệ tọa độ ORxyz so với hệ tọa độ O0X0Y0Z0 là:
góc hợp giữa tia ORA với giao tuyến mặt phẳng α và mặt H = Trans(P).Rot(z, φ).Rot(x,900).Rot(z,β(t)).
ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(96).2015, QUYỂN 1 63
   
Trong đó P P  X OR .i  YOR . j  Z OR .k Trong đó:
Vậy các ma trận biến đổi là: - Hệ tọa độ O4 mô tả hướng và vị trí của điểm cuối
của khâu thứ 4.
 1 0 0 X OR    
  0 1 0 YOR  - Hệ tọa độ E với các vector hướng n, o, a mô tả
Trans ( P )   (1) hướng và vị trí điểm tác động cuối gắn trên robot
0 0 1 Z OR  (ví dụ như đầu mũi hàn…)
 
0 0 0 1 
Như vậy ta có:
 cos( )  sin( ) 0 0  0 0 1 R 
 sin( ) cos( ) 0 0 0 0 1 a   
Rot ( z ,  )   (2) 0 1 0 0  base 0 1 0 0 
0 0 1 0 4
TTool   
; Pobj   
  1 0 0 0 1 0 0 0 
 0 0 0 1    
 0 0 0 1   
 1 0 0 0 0 0 0 1 
 
 0 0 1 0 Để tìm vị trí góc quay của các khớp, ta giải phương
Rot ( x, 90 )  
0   (3)
 trình (6) với các biến khớp, do đó ta có:
 0 1 0 0
 0 0 0 1 T4= H(t)basePobj4TTool-1 (7)
 
Như vậy theo (7):
 cos(  (t ))  sin(  (t )) 0 0
 sin(  (t )) cos(  (t )) 0 0 C (t ) C S ( t ) C S X OR   R  a  C (t ) C 
Rot ( z ,  (t ))   (4)  C S S  ( t ) S C YOR   R  a  C (t ) S 
0 0 1 0 T4    (t )  Ro
 
 0 0 0 1   S (t ) C (t ) 0 ZOR   R  a  S  ( t ) 
 0 0 0 1 
Vậy ma trận biến đổi thuần nhất là:
 C  ( t ) C C S  ( t ) S XO  bot 4 bậc tự do kết cấu kiểu RRR.R có mô hình động học
 R
như sau [1]:
C S  S  ( t ) S C YO 
  (t )  (5)
H (t )   
R
a2 a3 a4

 S (t ) C (t ) 0 ZO 
 R

 0 0 0 1 
Y1 Y2 Y3 Y4

 O3

Trong đó: O1 X1 O2 X2 X3 O4 X4

Cβ(t) = cos(β(t)); Sβ(t) = sin(β(t)); Cφ = cos(φ); Sφ = Z1 Z2 Z3 Z4

sin(φ);
d1 Z0
Vị trí đến của tay máy được mô tả bằng phương trình
sau [2,5,6]: Y0

T44TTool = H(t)basePobj (6) O0 X0

Trong đó:
- T4 là ma trận biến đổi thuần nhất mô tả hướng và Hình 5. Mô hình động học robot
vị trí của hệ tọa độ gắn trên khâu chấp hành cuối so Với mô hình động học như vậy, phương trình động
với hệ tọa độ gốc. học của robot như sau:
- 4TTool là ma trận biến đổi thuần nhất mô tả hướng C1C234 C1 S234 S1 a 4 C1C234  a3C1C23  a2 C1C2 
và vị trí của điểm tác động cuối của công cụ đối  
 S1C234 S1 S234 C1 a 4 S1C234  a3 S1C23  a2 S1C2 
với hệ tọa độ gắn trên khâu chấp hành cuối. T4   
- H(t) là ma trận biến đổi thuần nhất, là một hàm  S234 C234 0 a 4 S 234  a3 S23  a2 S2  d1 
theo thời gian, mô tả hệ tọa độ làm việc OR của  0 
 0 0 1 
đối tượng đối với hệ tọa độ gốc của robot.
Giả sử vị trí khâu chấp hành cuối nằm ở vị trí thỏa
- basePobj là ma trận biến đổi thuần nhất mô tả hướng
mãn điều kiện tồn tại nghiệm, theo [1] ta có nghiệm toán
và vị trí của cơ cấu chấp hành cuối so với hệ tọa độ
học của robot là:
làm việc OR.
1   arctan 2(ax , a y )
Điểm tác động cuối của robot được mô tả như Hình 4.  2 =arctan2(A,B)
 2 2 2 2
 =arctan2(  1  C 2 , Px  Py  a3  a2 ) (8)
B 3 3

 2
a3  a2 2
 4 = 234   2  3

OR
x Với:
y a
x
E y C1 = cos1; S1 = sin1; C2 = cos2; S2 = sin2;
O
o z C12 = cos(1 + 2); S12 = sin(1 + 2) …
z n
 a
A


Hình 4. Mô tả điểm tác động cuối của robot

64 Phạm Trường Tùng
a3C3  a2  (a3 S23  a2 S2 )  a3 S3 (a3C23  a2C2 ) ua (t ) 
1   (t ) 
m  m  K b m
A ; (10)
a3C3  a2   a3 S3 
2 2 (t ) (t )
Từ đó ta có phương trình trạng thái của hệ thống như sau:
a C  a2  (a3C23  a2C2 )  a3 S3 (a3 S23  a2 S2 )
B 3 3  dx1
a3C3  a2   a3 S3 
2 2
 dt  x2
 dx2
nx  C (t ) C ; ny  C (t ) S  C (t ) S S ; nz  S (t ) ;  (t )ua (t )   (t ) x2  (t ) Kb x1 (11)

ox  S (t ) C ; oy  S (t ) S ; oz  C (t ) ; ax  S ;  dt 
 y  x1   m
a y  C (t ) ; az  0; px  X OR   R  a C (t ) C ; 
p y  YOR   R  a C (t ) S ; pz  Z OR   R  a S (t ) Với:
K a  Ra J (t ) La dJ (t )  Ka
Px  C1 px  S1 p y  a 4 C234 ; Py  pz  d1  a 4 S234 (t )   ; (t ) 
La J (t )  K a K a dt  La J (t )
Để chuyển động các khớp được liên tục và mượt, ta
chọn quy luật vận tốc góc của điểm tác động cuối so với Ta thấy trong mô hình của đối tượng điều khiển (11)
tâm quay là OR là một hàm bậc 2 (tức  (t ), là một hàm có các tham số biến đổi tùy theo vị trí của các khớp robot
theo thời gian, do đó ta sẽ sử dụng phương pháp điều
bậc 2 hay  (t ) là hàm bậc 3) [2,5]. Do đó, ta có hàm  (t )
khiển bền vững để điều khiển. Cụ thể trong trường hợp
theo thời gian như sau:
này, ta lựa chọn phương pháp điều khiển trượt (Sliding
3 f  0   20   f t f 2  Mode Control) để thiết kế bộ điều khiển [3,4].
 (t )  0  0 t  .t Gọi:
tf 2
(9)  yd(t) là tín hiệu đặt (là một hàm biến thiên theo thời
 
0   f t f  2  f  0  3 gian).
 .t  e(t) = yd(t) – y(t) là sai lệch giữa tín hiệu đặt và tín
tf 3
hiệu điều khiển.
Với 0 và  f lần lượt là vận tốc góc tại thời gian t=0 Do mô hình là bậc 2, nên ta sử dụng hàm trượt có dạng:
và t= tf, tf là khoảng thời gian để robot di chuyển từ vị trí
de
được định vị bởi góc  0 đến góc  f . s  k1e  (12)
dt
2.3. Thiết kế bộ điều khiển robot
Với k1> 0 để phương trình s(e) có đa thức đặc tính
Xét mô hình động lực điều khiển robot sử dụng động A(p) = k1 + p là một đa thức Hurwitz.
cơ điện một chiều kích từ độc lập như sau:
Từ hàm trượt s ta có:
de d ( yd  y )
La(t) Ra(t) Uf (t) s  k1e   k1 ( yd  y )  (13)
Rf (t) dt dt
 dyd   dy 
Lf (t)  s   k1 yd     k1 y   (14)
 dt   dt 
Ua(t) eb(t)
m(t)  dyd 
Vậy mặt trượt s   k1 yd     k1 x1  x2  (15)
 dt 
Mm(t)
Jm Với hàm trượt s, nhiệm vụ của bộ điều khiển là phải
tạo ra được s0 để có e0 và điều này tương đương với:
Hình 6. Mô hình động cơ một chiều kích từ độc lập
ds
sgn( s )  0 (16)
dt
JL(t)
Chọn K > 0, từ điều kiện trượt ta được:
ds
sgn( s )   K K  0 (17)
dt
L(t) ML(t)
Từ (15), (17) ta được:
 dyd  
d  k1 yd     k1 x1  x2  
 dt   sgn( s )   K
dt
Jm
Mm(t)
m(t)  dy d d 2 y d dx1 dx2 
  k1  2
 ( k1  )  sgn( s )   K
Hình 7. Truyền động cho khớp robot  dt dt dt dt 
Ta có phương trình vi phân mô tả mối quan hệ giữa  dyd d 2 yd  dy
đầu vào hệ thống (điện áp) và đầu ra hệ thống (vận tốc   k1    k1  (t )ua (t )  (t ) K b x1
 dt dt 2  dt
góc của trục động cơ):
  (t ) x2   sgn( s )   K
ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(96).2015, QUYỂN 1 65
  k1e  yd (t)ua (t ) (t )Kb x1 (t )x2  sgn(s)  K(18)
 k1e  yd (t)ua (t) (t)Kb x1 (t)x2  K sgn(s) (19)
Từ (19), ta được
 
(t )ua (t )  K sgn(s)  k1 e yd  (t ) x2  (t )Kb x1 (20)
Ta được:
1
ua (t )   K sgn(s)  k1e  yd (t)x2 (t)Kb x1  (21)
(t )
2.4. Kết quả
Các kết quả mô phỏng bằng phần mềm Matlab cho
thấy bộ điều khiển được thiết kế hoạt động tốt, thời gian
đáp ứng nhanh, sai lệch tĩnh nhỏ trong phạm vi cho phép. Hình 11. Vị trí góc và vận tốc góc trên trục khớp 4
Tuy nhiên, hiện có hiện tượng Chatterring khi sử dụng Mô phỏng với cung tròn có tọa độ tâm OR là (0.6m; 0m;
hàm sign(s). Hiện tượng Chatterring giảm hẳn khi ta sử 0.2m); bán kính R = 0.25m; cung tròn giới hạn bởi
dụng hàm bão hòa sat(s) thay cho sign(s) [4].  0  1350 và  f  1800 . Các thông số của robot lần lượt là
d1 = 0.1m; a2 = 0.3m; a3 = 0.3m; a4 = 0.1m; a = 0.05m.
Thông số động cơ: Ka = 0.01; Kb = 0.01; Ra = 3 ;
La = 0.05H; Jm = 1N.m. Vận tốc ban đầu 0 = 0.02 rad/s; vận
tốc cuối  f = 0. Các hệ số K = 1 và k1 = 1. Kết quả như sau:
Kết quả mô phỏng ta thấy sai số của cơ cấu chấp hành
cuối e = 6,6.10-5m.

Hình 8. Vị trí góc và vận tốc góc trên trục khớp 1

Hình 12. Quỹ đạo điểm tác động cuối của robot

Hình 9. Vị trí góc và vận tốc góc trên trục khớp 2

Hình 13. Sai số trên điểm tác động cuối của robot

3. Kết luận
Kết quả nghiên cứu đã hoàn thành việc thiết kế quỹ
đạo của robot 4DOF định hướng và vị trí theo một cung
Hình 10. Vị trí góc và vận tốc góc trên trục khớp 3 tròn cho trước nằm trong trường công tác của robot. Trên
66
cơ sở quỹ đạo các biến khớp tìm được, tác giả đã thiết kế
bộ điều khiển kiểu slide mode để điều khiển các động cơ
trên các khớp của robot. Kết quả tính toán được mô phỏng
bằng phần mềm Matlab với các cung tròn khác nhau và
cho chất lượng điều khiển khả quan.
Tuy nhiên, bộ điều khiển kiểu slide mode có nhược
điểm là sự dao động của tín hiệu điều khiển lớn, dễ gây
hư hỏng cho các thiết bị điện và cơ khí. Chính vì vậy, tác
giả sẽ có những đề xuất giải pháp điều khiển khác để khắc
phục nhược điểm này.
Nhìn chung với kết quả đạt được, việc thiết kế và điều
khiển quỹ đạo theo bài toán trên đối với các robot 4DOF kết
cấu RRR.R như đã mô tả là hợp lý. Giải thuật của bài toán này
có thể áp dụng vào thực tế đối với các robot thực hiện các
nguyên công yêu cầu về điều khiển quỹ đạo theo cung tròn.
(BBT nhận bài: 25/08/2015, phản biện xong: 10/09/2015)
Phạm Trường Tùng
TÀI LIỆU THAM KHẢO
[1] Phạm Trường Tùng, Điều khiển đồng bộ robot 4 bậc tự do, Luận
văn thạc sĩ kĩ thuật, Đại học Đà Nẵng, 2011.
[2] Phạm Đăng Phước, Robot công nghiệp, NXB. Xây dựng, Hà Nội
2007.
[3] Nguyễn Doãn Phước – Phan Xuân Minh – Hán Thành Trung, Lý
thuyết điều khiển phi tuyến, NXB. Khoa học và kĩ thuật, Hà Nội
2008.
[4] Jean –Jacques E.Slotine – Weiping Li, Applied Nonlinear Control,
Prentice Hall, New Jersey 1991.
[5] Reza-N.Jazar, Theory of applied Robotics, Springer New York
Dordrecht Heidelberg London, New York 2006.
[6] Henry W.Stone, Kinematic Modeling Identification, and control of
robotic Manipulators, Kluwer Academic Publishers, United State
of America 1987.