Về phương pháp giải bài toán động lực học ngược robot song song

Điều khiển – Cơ điện tử - Truyền thông 
 N. V. Khang, N. V. Quyền, N. N. Hà, “Về phương pháp giải bài toán robot song song.” 200 
VỀ PHƯƠNG PHÁP GIẢI BÀI TOÁN ĐỘNG LỰC HỌC NGƯỢC 
ROBOT SONG SONG 
Nguyễn Văn Khang1, Nguyễn Văn Quyền1, Nguyễn Ngọc Hà2* 
Tóm tắt: Trong báo cáo này trình bày hai phương pháp giải bài toán động lực 
học ngược robot song song là phương pháp sử dụng nhân tử Lagrange và phương 
pháp thu gọn về các tọa độ suy rộng độc lập. Sau khi trình bày lý thuyết, đã tiến 
hành tính toán mô phỏng một số ví dụ về giải bài toán động lực học ngược robot 
song song phẳng 3RPR. 
Từ khóa: Động học ngược, Động lực học ngược, Robot song song. 
1. MỞ ĐẦU 
Các robot song song là các hệ nhiều vật có cấu trúc mạch vòng [1-3]. Như đã biết, 
phương trình vi phân - đại số mô tả chuyển động của hệ nhiều vật có cấu trúc mạch vòng 
có dạng [1] 
 , Ts M s s b s s g s s  τ Φ λ (1) 
 f s 0 (2) 
Gọi a
n
a
 q là véc tơ các tọa độ khớp chủ động, z
n
 z là véc tơ tọa độ các khớp 
suy rộng dư (bao gồm các tọa độ khớp bị động và có thể cả tọa độ thao tác). Ký hiệu: 
, , ,s
T
nT T
a s a z
n n n 
s q z s (3) 
Trong các phương trình (1) và (2) ta có: 
, , , ,
, , ,
s s s
s s s
n n n rr T r
s
n n n
s


M s f s
f
g s b s,s
s
 
Φ λ
Φ τ
Bài toán động lực học ngược được phát biểu dưới dạng: Cho biết quy luật chuyển động 
của khâu thao tác , mt x x x và phương trình liên kết , f x q 0 , 
,n r q f . Xác định mô men (hoặc lực) dẫn động của khâu dẫn a
n
 τ cần thiết 
để tạo ra chuyển động mong muốn của khâu thao tác. 
Trong các tài liệu [3-9] đã trình bày việc áp dụng các phương pháp nguyên lý công ảo, 
phương trình Lagrange dạng nhân tử để giải bài toán động lực học robot song song. Trong 
bài báo này áp dụng phương pháp tách câú trúc để thiết lập phương trình vi phân đại số 
của các robot song song [10-11]. Sau đó, trình bày việc tính toán so sánh hai phương pháp 
giải bài toán động lực học ngược robot song song. 
2. PHƯƠNG PHÁP GIẢI BÀI TOÁN 
ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG 
2.1. Động lực học ngược dựa trên phương trình Lagrange dạng nhân tử 
Theo phương pháp này, phương trình liên kết (2) được sử dụng để giải bài toán động 
học ngược. Khi giải xong bài toán này ta được 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san ACMEC, 07 - 2017 201
 , , , 0,1,...,k k kt t t k N s s s  (4) 
Phương trình (1) dùng để tính các nhân tử Lagrange, các lực và mô men cần thiết của 
các khâu dẫn. Trước hết, ta viết lại phương trình (1) dưới dạng 
 1 , , Tst M s s p s s s  τ Φ λ (5) 
Ta tách 
s
n phương trình (5) thành hai nhóm, nhóm thứ nhất gồm 
a
n phương trình có 
chứa mô men (hay là lực) của khâu phát động, nhóm thứ hai gồm 
z
n phương trình còn lại 
 1 1 11 , , Ta a z a at M s q M s z p s s s  τ Φ λ (6) 
 2 2 21 , , Ta a z zt M s q M s z p s s s  Φ λ (7) 
 Trong đó 
1 1
2 2
a z
a z
M s M s
M s
M s M s
 (8) 
,
T
aT
Ts a z s
z
s
s
s
Φ
Φ Φ Φ Φ
Φ
 (9) 
Trường hợp ,
z z
r n Φ là ma trận chính quy. Từ phương trình (7) ta được 
 2 2 21 , ,Tz a a z t s M s q M s z p s s Φ λ (10) 
1 2 2 2
1
, ,T
z a a z
t
s M s q M s z p s s λ Φ (11) 
Thế (11) vào phương trình (6) ta được phương trình xác định 
a
τ 
 1 1 11 , , Ta a a z at M s q M s z p s s s τ Φ λ (12) 
Các bước giải bài toán động lực học ngược theo phương pháp thứ nhất 
Bước 1: Giải bài toán động học ngược. Cho biết tx và f x,q = 0 . Tính 
 , ,t t ts s s  
Bước 2: Từ phương trình vi phân đại số mô tả chuyển động của robot song song, tính 
các ma trận , ,sM s ,b s ,s s g s Φ 
Bước 3: Tính các nhân tử Lagrange từ phương trình (11) 
Bước 4: Tính các mô men phát động từ phương trình (12) 
2.2. Động lực học ngược dựa trên các phương trình vi phân thu gọn về các tọa độ tối 
thiểu 
Ý tưởng của phương pháp này là: Khử các tọa độ suy rộng dư z và các nhân tử 
Lagrange λ , biến đổi hệ phương trình vi phân đại số (1) và (2) về hệ phương trình vi phân 
thường với các tọa độ là các thành phần của véc tơ 
a
q , số lượng phương trình bằng số bậc 
tự do của hệ. 
Điều khiển – Cơ điện tử - Truyền thông 
 N. V. Khang, N. V. Quyền, N. N. Hà, “Về phương pháp giải bài toán robot song song.” 202 
 Xét các phương trình liên kết (2) 
 , , , ,z an nra a f s f q z 0 f z q (13) 
Giả sử số lượng các tọa độ dư bằng số các phương trình liên kết bổ sung r=na . Từ 
phương trình (13) ta suy ra: 
 s  


f
f s s s 0
s
Φ (14) 
Gọi s là nghiệm của hệ phương trình đại số tuyến tính (14), ta có: 
 s a a z   s s s q s z 0Φ Φ Φ (15) 
Viết lại phương trình (1), ta có: 
 ,Ts s M s s b s s g s Φ λ τ (16) 
Chuyển vị hai vế của phương trình (16) ta được: 
 ,
T
T
s
M s s b s s g s λ Φ τ (17) 
Nhân hai vế của phương trình (17) với s ta được: 
 ,
T
T
s
  
s M s s b s s g s s λ Φ τ (18) 
Chú ý đến công thức (15), s  s s 0Φ , từ (18) ta suy ra 
 ,
T
T 
M s s b s s g s s 0  τ (19) 
Mặt khác từ phương trình (15) ta có: 
 1z a a 
 z s s qΦ Φ (20) 
Chú ý rằng véc tơ 
a
q có thể viết lại dạng 
a
a n a
  q E q (21) 
Kết hợp hai phương trình (20) và (21) ta có: 
 1
a
na
a
z a

 
 
Eq
s q
z s sΦ Φ
 (22) 
Nếu ta đưa vào ký hiệu 
 1
a
n
z a
E
R s
s sΦ Φ
 (23) 
Thì phương trình (22) có dạng a  s R s q (24) 
Từ (24) ta suy ra: a s R s q  (25) 
Thế biểu thức (24) vào phương trình (19) ta có 
 ,
T
T
a
 
M s s b s s g s R s q 0  τ (26) 
Do 
1 2
, ,...,
na
a a a
q q q   là các biến phân độc lập, nên từ phương trình (26) ta suy ra: 
 ,T R s M s s b s s g s 0  τ (27) 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san ACMEC, 07 - 2017 203
Phương trình (27) có thể viết lại dưới dạng 
 ,T T R s R s M s s b s s g s τ (28) 
Từ phương trình (23) ta có: 
 1
1
[ , ( ( ) ( )) ]
( ( ) ( ))
aT T
na z a
z
T
a z a z
R s E s s
s s
τ
τ Φ Φ
τ
τ Φ Φ τ
 (29) 
Thế (29) vào phương trình (28) ta được: 
1
,
+( ( ) ( ))
T
a
T
z a z
R s M s s b s s g s
s s
 τ
Φ Φ τ
 (30) 
Các bước giải bài toán động lực học ngược theo phương pháp thứ hai : 
Bước 1: Giải bài toán động học ngược. Cho biết tx và f x,q = 0 tính 
 , ,t t ts s s  
Bước 2: Tính các ma trận 1, ,z a z
 s s , sΦ Φ Φ , ( , ), ( )R s ,M s b s s g s 
Bước 3: Tính các mô men (hay lực) của các khâu dẫn động theo công thức (30) 
3. ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG 3RPR 
Trong mục này, ta xét chuyển động của robot song phẳng 3RPR (Hình 1). Robot song 
phẳng 3RPR có 3 bậc tự do (k=3). Ta sẽ giải bài toán động lực học ngược robot song 
phẳng 3RPR trong hai trường hợp như sau: 
* Khâu chủ động là 
1 1 2 2 3 3
, ,PA PA PA * Khâu chủ động là 
1 1 2 2 3 3
, ,AB AB AB 
1 1
2 2
3 3
, ,
P
a p P
u x
u y
u


 
q q x 
1 1
2 2
3 3
, ,
P
a p P
u x
u y
u


 
q q x 
3.1. Thiết lập phương trình vi phân – đại số mô tả chuyển động của robot 
Sáu phương trình liên kết của robot: 
1 2 1 1 2 1
1 3 1 3
cos cos + , sin sin +
2 3 6 2 3 6P P
x u l h y u l h
  
2 2 2 2 2 2
1 3 1 3
cos cos , sin sin
2 3 6 2 3 6P P
x c u l h y u l h
  
3 2 3 3 2 3
1 3 3 1 3
cos sin , sin cos
2 2 3 2 2 3P P
c
x u l h y c u l h  
Điều khiển – Cơ điện tử - Truyền thông 
 N. V. Khang, N. V. Quyền, N. N. Hà, “Về phương pháp giải bài toán robot song song.” 204 
Hình 1. Robot song song phẳng 3RPR. 
Sử dụng phương pháp tách cấu trúc, tách robot thành các cấu trúc con như Hình 2, 5. 
Từ đó, với mỗi chân của robot, sử dụng phương trình Lagrange loại 2 và sử dụng 
phương trình Newton-Euler cho bàn máy động, ta thu được 9 phương trình vi phân 
2 2
1 1 2 1 1 2 1 2 1 1 1 1 1 2 1 1
1 1 1 2 1 1 1 2 1
2
2 1 2 1 1 2 1 1 1 1 1 1
1 1
2 cos
4 2
1 1
sin cos
2 2
sin cos sin
ml m u I I m u u m l m u g
X u l Y u l
m u m u m g F X Y
  
  
   
  

2 2
1 1 2 2 1 2 2 2 2 2 2 1 1 2 2 2
2 2 2 2 2 2 2 2 2
1 1
2 cos
4 2
1 1
sin cos
2 2
m l m u I I m u u m l m u g
X u l Y u l
  
  
  
2
2 2 2 2 2 2 2 2 2 2 2 2
sin cos sinm u m u m g F X Y     
2 2
1 1 2 3 1 2 3 2 3 3 3 1 1 2 3 3
3 3 3 2 3 3 3 2 3
1 1
2 cos
4 2
1 1
sin cos
2 2
m l m u I I m u u m l m u g
X u l Y u l
  
  
  
2
2 3 2 3 3 2 3 3 3 3 3 3
sin cos sinm u m u m g F X Y     
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san ACMEC, 07 - 2017 205
1 2 3 1 2 3
,
P P
mx X X X my mg Y Y Y   
1 1 2 2
3 3
sin cos sin cos3
6 6 6 6
3
cos sin
X Y X Y
I h
X Y
 
Hình 2. Chân thứ nhất. 
Hình 3. Chân thứ hai. 
Hình 4. Chân thứ ba. 
Hình 5. Bàn máy động. 
3.2. Mô phỏng số động lực học ngược 
Các tham số của robot song song phẳng 3RPR [7] được cho trong bảng 1. Quy luật 
chuyển động của bàn máy: 
0.15 3 0.025 1 sin , 0.15 0.025 cos , 1 cos
3 3 12 3P P
x t y t t
Bảng 1. Các tham số của robot. 
 i=1 i=2 i=3 
i
P
x m 
0 
0.3 3 0.15 3 
i
P
y m 
0 0 0.45 
1 2 1 2
0.2 , 0.3 3 , 3, 1.5, 5l l m h m m m m kg 
Các kết quả tính toán trên phần mềm MATLAB cho trên các hình từ hình 6 đến hình 
13. Trong đó, đưa ra hai phương án: Phương án 1 mômen dẫn động đặt vào các khâu quay 
Điều khiển – Cơ điện tử - Truyền thông 
 N. V. Khang, N. V. Quyền, N. N. Hà, “Về phương pháp giải bài toán robot song song.” 206 
nối giá, phương án 2 lực dẫn động đặt tại các khâu chuyển động tịnh tiến tương đối. Ta 
thấy, các công suất của từng động cơ trong hai trường hợp khác nhau nhưng tổng công 
suất trong hai trường hợp là như nhau. 
Về hiệu quả của hai phương pháp tính, nếu chọn số bước tính toán N=150, ta có thể so 
sánh thời gian tính toán theo hai phương pháp như bảng 2. 
Bảng 2. Thời gian tính toán của hai phương pháp. 
Phương pháp dựa trên phương trình 
Lagrange dạng nhân tử 
Truyền động bằn mô men 0.235(s) 
Truyền động bằng lực 0.234(s) 
Phương pháp dựa trên các phương trình 
vi phân thu gọn về tọa độ tối thiểu 
Truyền động bằng mô men 0.218(s) 
Truyền động bằng lực 0.212(s) 
3.2.1. Kết quả động lực học ngược dựa trên phương trình Lagrange dạng nhân tử 
a) Trường hợp dẫn động mô men 
0 0.5 1 1.5 2 2.5 3
-30
-20
-10
0
10
20
30
t(s)
T
o
rq
u
e
(N
m
)
torque1
torque2
torque3
Hình 6. Đồ thị mô men các động cơ. 
0 0.5 1 1.5 2 2.5 3
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
t(s)
T
o
ta
l 
P
o
w
e
r(
W
)
Hình 7. Đồ thị tổng công suất. 
b) Trường hợp dẫn động bởi lực 
0 0.5 1 1.5 2 2.5 3
-60
-40
-20
0
20
40
60
80
t(s)
F
o
rc
e
(N
m
)
force1
force2
force3
Hình 8. Đồ thị lực dẫn động. 
0 0.5 1 1.5 2 2.5 3
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
t(s)
T
o
ta
l 
P
o
w
e
r(
W
)
Hình 9. Đồ thị tổng công suất. 
3.2.2. Động lực học ngược dựa trên phương pháp thu gọn về tọa độ tối thiểu 
a) Trường hợp dẫn động mô men 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san ACMEC, 07 - 2017 207
0 0.5 1 1.5 2 2.5 3
-30
-20
-10
0
10
20
30
t(s)
T
o
rq
u
e
(N
m
)
torque1
torque2
torque3
Hình 10. Đồ thị mômen các động cơ. 
0 0.5 1 1.5 2 2.5 3
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
t(s)
T
o
ta
l 
P
o
w
e
r(
W
)
Hình 11. Đồ thị tổng công suất. 
b) Trường hợp dẫn động bởi lực 
0 0.5 1 1.5 2 2.5 3
-60
-40
-20
0
20
40
60
80
t(s)
F
o
rc
e
(N
m
)
force1
force2
force3
Hình 12. Đồ thị lực dẫn động. 
0 0.5 1 1.5 2 2.5 3
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
t(s)
T
o
ta
l 
P
o
w
e
r(
W
)
Hình 13. Đồ thị tổng công suất. 
4. KẾT LUẬN 
Tính toán động lực học ngược robot song song là một bài toán quan trọng trong việc 
điều khiển robot song song. Có nhiều phương pháp giải bài toán động lực học ngược, 
trong bài báo này, chúng tôi sử dụng 2 phương pháp đó là: phương pháp dựa trên phương 
trình Lagrange dạng nhân tử và phương pháp dựa trên phương trình vi phân thu gọn về tọa 
độ khớp chủ động. 
Dựa vào kết quả mô phỏng số ta thấy rằng, sử dụng cả hai phương pháp đều đạt độ 
chính xác cao hay sai số nhỏ (khoảng 10-13 mm). Tuy nhiên, sử dụng phương pháp dựa 
trên phương trình vi phân thu gọn về tọa độ khớp chủ động thì thời gian tính toán nhỏ hơn 
sử dụng phương pháp dựa trên phương trình Lagrange dạng nhân tử. Các kết quả trùng 
khớp với phương pháp truy hồi truyền thống trước đây [6-7]. 
TÀI LIỆU THAM KHẢO 
[1]. Nguyễn Văn Khang, Động lực học hệ nhiều vật , NXB KHKT, Hà Nội 2007. 
[2]. J.P Merlet, “ Parallel robots” Springer-Verlag, 2006. 
[3]. L-W Tsai, “Robot analysis”, The mechanics of serial and parallel manipulator. John 
Wiley & Sons, Inc, 1999. 
Điều khiển – Cơ điện tử - Truyền thông 
 N. V. Khang, N. V. Quyền, N. N. Hà, “Về phương pháp giải bài toán robot song song.” 208 
[4]. Th. Geike.; J. McPhee, “Inverse dynamic analysis of parallel manipulators with full 
mobility”, Mechanism and Machine Theory 38 (2003) 549 – 562. 
[5]. W. A. Khan.; V.N. Krori.; S.K. Saha.; J. Angeles, “ Recursive kinematics and inverse 
dynamics for a planar 3 R parallel manipulator”, Journal of Dynamic Systems, 
Measuremwnt, and Control 127 (2005), pp. 529-536. 
[6]. S. Staicu, D. Zhang, R. Rugescu, “ Dynamic modeling of a 3-DOF parallel 
manipulator using recursive matrix relations”, Robotica 24 (2006), pp.125-130. 
[7]. S. Staicu, “Power requirement comparision in the 3-RPR planar parallel robot 
dynamics”, Mechanism and Machine Theory 44 (2009) 1045 – 1057. 
[8]. S. Staicu, “Inverse dynamics of the 3-PRR planar parallel robot”, Robotics and 
Autonomous Systems 57 (2009), pp. 556-563. 
[9]. Do Thanh Trung, Jens Kotlarski, Bodo Heimann and Tobias Ortmainer, “ A new 
program to automatically generate the kinematic and dynamic equations of general 
robots in symbolic form”, Proc. of the ISRM 2009, Bach Khoa Publishing House 
2009, pp 122-128. 
[10]. Nguyen Van Khang, “Inverse dynamics of constrained multibody systems using the 
projection matrix”, Vietnam Journal of Mechanics, 35 (2013). 
[11]. Nguyễn Văn Khang, Chu Anh Mỳ. Cơ sở robot công nghiệp ,NXB Giáo dục, Hà Nội 
2011. 
ABSTRACT 
ON THE METHODS FOR INVERSE DYNAMICS ANALYSIS 
OF PARALLEL ROBOTS 
 In this paper, two methods using Lagrange multiplier and using coordinate 
reduction are proposed for calculating inverse dynamics of parallel manipulator. 
After addressing the principles of two methods, an example – a 3 RPR planar 
parallel manipulator – is demonstrated for the efficiency of the proposed methods in 
the analysis of inverse dynamic problem. 
Keywords: Inverse Kinematics, Inverse Dynamics, Parallel Robots. 
Nhận bài ngày 20 tháng 5 năm 2017 
Hoàn thiện ngày 10 tháng 07 năm 2017 
Chấp nhận đăng ngày 20 tháng 07 năm 2017 
Địa chỉ: 1 Trường Đại học Bách khoa Hà Nội; 
 2 Trường Đại học Kỹ thuật Công nghiệp – Đại học Thái Nguyên. 
 * Email: nguyenngocha.osc@gmail.com