Điều khiển tựa thụ động robot phẳng 6 bậc tự do

CHÀO MỪNG NGÀY THÀNH LẬP TRƯỜNG 01/4/2018 
 54 Tạp chí Khoa học Công nghệ Hàng hải Số 54 - 4/2018 
ĐIỀU KHIỂN TỰA THỤ ĐỘNG ROBOT PHẲNG 6 BẬC TỰ DO 
PASSIVITY-BASED CONTROL OF 6-DOF PLANAR ROBOT 
HOÀNG MẠNH CƯỜNG, NGUYỄN HOÀNG HẢI 
Viện Cơ khí, Trường ĐHHH Việt Nam 
Tóm tắt 
Trong bài báo này, các tác giả tập trung nghiên cứu đưa ra quy luật điều khiển đối tượng 
robot phẳng 6 bậc tự do, nhằm điều khiển robot thực hiện chuyển động theo quỹ đạo định 
trước. Muốn vậy, trước hết, việc thiết lập phương trình vi phân chuyển động của robot đã 
được thực hiện nhờ áp dụng phương trình Lagrange loại II, sau đó, phương pháp điều 
khiển tựa thụ động đã được nghiên cứu để đưa ra các quy luật điều khiển. Để thấy được 
tác động của quy luật điều khiển lên robot, việc tính toán mô phỏng số đã được thực hiện 
và kết quả đưa ra mô tả việc bám quỹ đạo định trước của các biến khớp dưới tác dụng của 
quy luật điều khiển đưa ra. 
Từ khóa: Robot phẳng, robot 6 bậc tự do, thụ động, thiết kế điều khiển, Lyapunov, mô hình toán. 
Abstract 
This article presents the control of a 6-dof planar robot so that the robot follows prescribed 
trajectories. First, the mathematical model of the robot is established based on Lagrange 
equation type II. Then, a nonlinear controller is designed by exploiting the passivity of the 
system and the asymptotic stability of the control system is shown by using Lyapunov’s 
theorem. The effectiveness of the proposed controller is illustrated by a set of simulations 
in which the joints are able to track desired trajectories. 
Keywords: Planar robot, 6-dof robot, passivity, controller design, Lyapunov, mathematical model. 
1. Mở đầu 
Robot công nghiệp được sử dụng phổ biến trong các hệ thống sản xuất linh hoạt, đã có nhiều 
cấu hình robot được nghiên cứu ứng dụng trong sản xuất và được đề cập trong nhiều tài liệu [1, 2, 
3, 6]. Theo [1] tay máy robot có số bậc tự do nhiều hơn số tọa độ tối thiểu, xác lập nên vị trí và 
hướng của khâu thao tác, theo yêu cầu của bài toán công nghệ được gọi là robot dư dẫn động. Như 
vậy, robot phẳng có 6 bậc tự do là một dạng của robot dư dẫn động. Nhờ tính dư dẫn động mà robot 
loại này có khả năng tránh được các vật cản, các điểm kỳ dị, các giới hạn của biến khớp, Việc 
nghiên cứu các bài toán liên quan đến robot dư dẫn động, đã được nhiều tác giả quan tâm nghiên 
cứu [1, 4, 5], trong các công trình đó các tác giả đã đề cập đến các bài toán động học thuận, động 
học ngược và điều khiển trượt đối với robot phẳng 5 và 6 bậc tự do, về áp dụng luật điều khiển tựa 
thụ động cho điều khiển robot cũng có nhiều công trình đề cập đến [8, 9], tuy nhiên các công trình 
này lại không đề cập đến các robot dư dẫn động. Trong bài báo này, nhóm tác tác giả tập trung 
nghiên cứu thuật toán điều khiển tựa thụ động nhằm điều khiển robot phẳng 6 bậc tự do thực hiện 
chuyển động bám theo quỹ đạo định trước. 
2. Thiết lập phương trình động lực học 
Xét mô hình robot có dạng như trong Hình 1, trong đó các khâu được coi là các thanh thẳng 
đồng chất với các kích thước O0O1 = a1, O1O2 = a2, O2O3 = a3, O3O4 = a4, O4O5 = a5, O5O6 = a6, gọi 
m1, m2, m3, m4, m5, m6 tương ứng là khối lượng các khâu, khi đó: 
Hình 1. Mô hình robot phẳng 6 khâu 
Theo [1], biểu thức động năng của vật rắn được xác định bởi công thức: 
CHÀO MỪNG NGÀY THÀNH LẬP TRƯỜNG 01/4/2018 
Tạp chí Khoa học Công nghệ Hàng hải Số 54 - 4/2018 55 
1
. ( ).
2
TT q M q q (1)
Trong đó: 
 (0) (0) (0) ( ) (0)
1 1
( ) .
i i i i i
p p
T T i T
i T T R i C i R
i i
m
  M q J J J A I A J (2) 
Với Ai, JTi, JRi (i = 1, 2, , 6) tương ứng là ma trận côsin chỉ hướng, ma trận Jacobi tịnh tiến 
và ma trận Jacobi quay của các khâu robot, ICi (i = 1, 2, , 6) là tenxơ quán tính khối các khâu của 
robot đối với hệ trục đặt tại khối tâm Ci và gắn chặt vào vật. 
Thế năng của hệ được cho bởi công thức: 
 (0)
1
.
i
p
i C
i
m y
  (3) 
Với cơ hệ chịu các liên kết hôlônôm có n bậc tự do, vị trí của cơ hệ được xác định bởi các tọa 
độ suy rộng q1, q2,, qn, áp dụng phương trình Lagrange loại II [1], ta có: 
T T T
d T T
dt
    
   
f
q q q
 (4) 
Trong đó: 
  1
T
nq q q K , 1
T
nQ Q
 f K (5) 
Thay (1) và (3) vào (4), ta nhận được hệ phương trình có dạng: 
 ( ). ( , ). ( ) M q q C q q q G q τ
 (6) 
Trong đó: 
( ) 1 ( )
( , ) ( ) ( )
2
T
n n
  
   
  
M q M q
C q q E q q E
q q
 (7) 
 ( )
T
 
 
G q
q
 (8) 
Phương trình (6) là phương trình vi phân mô tả chuyển động của robot phẳng n bậc tự do, 
trong đó  là mômen tác dụng tại các khâu của robot. 
3. Thiết kế quy luật điều khiển tựa thụ động cho robot dư dẫn động 
Xét hệ robot dư dẫn động được mô tả bằng phương trình (6). Bài toán điều khiển đặt ra là xác 
định tín hiệu điều khiển  sao cho các biến trạng thái q bám theo quỹ đạo qd cho trước. 
Ta đưa vào véc tơ sai lệch có dạng: 
 q d
 e q q 
Khi đó, tín hiệu điều khiển được chọn có dạng: 
 ( ) ( , ) ( ) τ M q v C q q v G q u
 (9) 
Trong đó: 
d q v q Ke 
Bằng cách đặt: 
q q r q v e Ke 
Và thay (9) vào (6) sẽ thu được: 
 ( ) ( , ) M q r C q q r u
 (10) 
Chọn hàm dự trữ có dạng: 
1
( )
2
TS r M q r 
Dễ dàng thấy rằng: 
CHÀO MỪNG NGÀY THÀNH LẬP TRƯỜNG 01/4/2018 
 56 Tạp chí Khoa học Công nghệ Hàng hải Số 54 - 4/2018 
1 1
( ) ( ) ( 2 )
2 2
T T T T TS r M q r r M q r r M C r r u r u 
Như vậy hệ là thụ động với tín hiệu vào u và tín hiệu ra r. Bây giờ ta chọn u có dạng: 
( )r r q q u K r K e Ke& 
Với Kr ma trận đường chéo xác định dương. Chọn hàm Lyapunov có dạng: 
1 1
( )
2 2
T T
q r qV r M q r e KK e (11) 
Đạo hàm của hàm Lyapunov trong (11) là: 
1 1
( ) ( ) 2 ( ( ) 2 ( , )) 2
2 2
 ( ) ( ) 2
T T T T T T
q r q r q r q
T T T T
q q r q q q r q q r q q r q
V 
r M q r r M q r e KK e r M q C q q r r K r e KK e
e Ke K e Ke e KK e e K e e KK Ke
Như vậy, hệ ổn định tiệm cận tại điểm 0q q e e& , hay nói theo cách khác q bám được theo 
quỹ đạo qd cho trước với quy luật điều khiển: 
( ) ( , ) ( ) ( )r q q τ M q v C q q v G q K e Ke (12) 
Trong đó K và Kr là các ma trận xác định dương. 
4. Một số kết quả tính toán mô phỏng số 
Để tính toán số ta cho các tham số của hệ các giá trị như sau: 
 a1 = 0,4(m), a2 = 0,35(m), a3 = 0,3(m), a4 = 0,2(m), a5 = 0,1(m), a6 = 0,1(m) 
m1 = 10(kg), m2 = 5(kg), m3 = 2(kg), m4 = 1(kg), m5 = 1(kg), m6 = 1(kg).
Hướng của khâu thao thác được giữ thẳng đứng, điểm định vị P của khâu thao tác dịch chuyển 
theo quỹ đạo là một đường tròn, được mô tả bởi phương trình: 
 0,6 0,2cos(2 ), 0,6 0,2sin(2 )P Px t y t 
Các thông số của bộ điều khiển được chọn như sau: 
K = Diag [50, 40, 40, 30, 20, 20]; Kr = Diag [50, 30, 10, 10, 4, 4]; 
Với các số liệu như trên, sau khi tính toán mô phỏng ta được các kết quả được cho như trong 
các Hình 2 - 7, trong đó các đường nét liền là đường quỹ đạo mong muốn qd của các tọa độ khớp, 
các đường nét đứt là các đường quỹ đạo thực q(t) của các tọa độ khớp khi robot được điều khiển 
theo quy luật tựa thụ động. Từ các kết quả mô phỏng cho thấy, dù trạng thái của robot ban đầu ở vị 
trí khá xa so với vị trí đặt trước, nhưng với quy luật điều khiển đưa ra, chỉ sau thời gian ngắn khoảng 
từ 0 đến 1 giây, robot đã bám sát quỹ đạo mong muốn và chuyển động ổn định, điều đó cho thấy 
quy luật điều khiển đã thiết kế hoạt động hiệu quả và ổn định. 
Hình 2. Đồ thị góc quay của khâu 1 trong không gian khớp 
CHÀO MỪNG NGÀY THÀNH LẬP TRƯỜNG 01/4/2018 
Tạp chí Khoa học Công nghệ Hàng hải Số 54 - 4/2018 57 
Hình 3. Đồ thị góc quay của khâu 2 trong không gian khớp 
Hình 4. Đồ thị góc quay của khâu 3 trong không gian khớp 
Hình 5. Đồ thị góc quay của khâu 4 trong không gian khớp 
Hình 6. Đồ thị góc quay của khâu 5 trong không gian khớp 
Hình 7. Đồ thị góc quay của khâu 6 trong không gian khớp 
5. Kết luận 
Trong bài báo này, các tác giả đã tập trung nghiên cứu phương pháp điều khiển tựa thụ động 
để thiết kế quy luật điều khiển cho robot phẳng 6 bậc tự do nhằm điều khiển robot thực hiện chuyển 
động bám theo các quỹ đạo đã định trước. Một số kết quả đạt được của công trình này là đã đưa ra 
được quy luật điều khiển cho robot và việc tính toán mô phỏng đã được thực hiện để kiểm chứng 
tính ổn định và hiệu quả của quy luật điều khiển này. 
TÀI LIỆU THAM KHẢO 
[1] Nguyễn Văn Khang, Chu Anh Mỳ, Cơ sở rôbốt công nghiệp, NXB Giáo dục Việt Nam, 2011. 
[2] Đào Văn Hiệp, Kỹ thuật rôbốt, NXB Khoa học và kỹ thuật, 2006. 
[3] Nguyễn Mạnh Tiến, Điều khiển rôbốt công nghiệp, NXB Khoa học và kỹ thuật, 2007. 
[4] Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng Nam, “Về một thuật toán điều 
khiển trượt robot dư dẫn động”, Tạp chí tin học và điều khiển học, tập 24, No.3, Tr.269-280, 2008 
[5] Nguyễn Quang Hoàng, Nguyễn Văn Khang, Trần Hoàng Nam, “Bài toán động học ngược robot 
dư dẫn động có chú ý đến sự cố kẹt khớp”, Tuyển tập Hội nghị Cơ học toàn quốc, Tập 2, Tr.282-
290, NXB Khoa học tự nhiên và Công nghệ, Hà Nội, 2009. 
CHÀO MỪNG NGÀY THÀNH LẬP TRƯỜNG 01/4/2018 
 58 Tạp chí Khoa học Công nghệ Hàng hải Số 54 - 4/2018 
[6] Bruno Siciliano, Oussama Khatib (Editors), Springer Handbook of Robotics, Springer Berlin 
Heidelberg, 2008. 
[7] Hassan K. Khalil, Nonlinear Systems, Pearson, 2002. 
[8] Bruno Siciliano, Luigi Villani, “A New Passivity-Based Control Technique for Safe Patient-Robot 
Interaction in Haptics-Enabled Rehabilitation Systems”, IEEE/RSJ International Conference on 
Intelligent Robots and Systems (IROS) Congress Center Hamburg Sept 28 - Oct 2, Hamburg, 
Germany, pp. 4556-4561, 2015. 
[9] Lucas C.Neves, Gabriel V.Paim, Isabelle Queinnec, Ubirajara F.Moreno, Edson R.De Pieri, 
“Passivity and Power Based Control of a Robot with Parallel Architecture”, Preprints of the 18th 
IFAC World Congress Milano (Italy) August 28 - September 2, pp. 14608 - 14613. 2011. 
Ngày nhận bài: 06/11/2017 
Ngày nhận bản sửa: 06/02/2018 
Ngày duyệt đăng: 09/02/2018