574 Nguyễn Văn Khang, Nguyễn Thành Công
VCM2012
Về hai phương pháp giải bài toán động lực học ngược robot song song
On two methods for calculating inverse dynamics
of parallel manipulator
Nguyễn Văn Khang, Nguyễn Thành Công
Trường đại học Bách Khoa Hà Nội
Email: ,
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 – phương
pháp sử dụng nhân tử Lagrange và phương pháp thu gọn tọa độ. Sau khi trình bày lý thuyết, đã tiến hành tính
toán mô phỏng số một thí dụ về giải bài toán động lực học ngược robot song song phẳng 3RPR.
Abstract
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. 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]








T
n
T 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
n n n r
r T r
s
M s f Φ s λ
 
   
   

   
, , ,
s s s
n n n

lực) của khâu dẫn động
a
n
a
τ

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ấu trúc để thiết lập phương
trình vi phân đại số của các robot song song [10-
12]. 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. Giải bài toán độ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 [10-11]






, , , 0,1, ,

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 n
z
phương trình còn lại














1 1 1
1
, ,
T
a a z a a
t   M s q M s z p s s
τ Φ s λ
 
(6)



 
 
 
 
1 1
2 2
a z
a z
M s M s
M s
M s M s
 
 

 
 
 
(8)
 
 


 
,
T
a
T
s a z s
T
z






2 2 2
1
, ,
T
z 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



1 1 1
1
, ,
T
a a a z a
t
τ M s q M s z p s s Φ s λ
   
 

(12)
Sơ đồ tính mô men (hay lực) phát động của robot
song song
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


t
x
và


Bước 3 : Tính các nhân tử Lagrange (hay các phản
lực liên kết) từ phương trình (11)
Bước 4 : Tính các mô men phát động từ phương
trình (12)

3. Giải bài toán độ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ệ. Xét các phương trình
liên kết (2)




, , , ,
a
z
n



s a a z
Φ s s Φ s q Φ s z 0
d d d
  
(15)
Viết lại phương trình (1), ta có








,
T
s
Φ s λ M s s b s s g s τ
   
 
(16)
Chuyển vị hai vế của phương trình (16) ta được







 
 
(18)
Chú ý đến công thức (15),


s
Φ s s 0
d

, từ (18) ta
suy ra






,
T
T
M s s b s s g s
τ s 0
d
 
   
 
 
 
(19)

a
a
z a
E
q
s q
z
Φ s Φ s
d
d d
d

 
 
 
 
 
 
 

 
 
 
(22)
Nếu ta đưa vào ký hiệu
 
   
1
a
n


(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
d
 
   
 
 
 
(26)
Do
1 2
, , ,
n
a
a a a




,
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
[ , ( ( ) ( )) ]
( ( ) ( ))
a
T T
na z a
z
T
a z a z


 
 
 








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


t
x




, ( , ), ( )
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)

4. Thí dụ áp dụng
Trong thí dụ áp dụng, ta xét chuyển động của
robot song phẳng 3RPR (H. 1). Robot song phẳng
3RPR có 3 bậc tự do (k=3). Chọn các tọa độ suy
rộng dư là
T
T T T
a p
s q q x
 

 
 
. Trong đó
a
q
là các
tọa độ suy rộng độc lập (hay các tọa độ suy rộng
của các khâu chủ động),
p

a p P
u x
u y
u
q q x
q
q
q j
   
 
   
 
   
 
  
   
 
   
 
 
   
 
   

* Khâu chủ động là
1 1 2 2 3 3
, ,
A B A B A B

1 1

có dạng
1 1 1 1 1 1
PP PA A B B P
  
   

(31)
Chiếu (31) lên hai trục tọa độ x và y ta được
1 2 1
1 2 1
1 3
cos cos +
2 3 6
1 3
sin sin +
2 3 6
P
P
x u l h
y u l h
p
q j
p
q j
   
 
 
  
 
 

 
2
2 2 2
, ,
m l C

x

P
2


2


2

F
2

A
2

 

1
1 1 1
, ,
C m l

A
1

B
3

B
2

B
1

 
3
2 2 2
, ,
C m l


3

P
3

u


x
P

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 577
Mã bài: 132

Phương trình của vòng động học thứ nhất
2 2 2 3 3 3 2
P A B B A PP

2 2 2 2 2 3 3 3 3 3 3 2
0
P A A B B B B A A P P P
     
     
(33)
Sau khi biến đổi ta được
1 1 2 2 2 2 2 2
PP PP P A A B B P
   
    
(34)
Chiếu (34) lên hai trục tọa độ x và y ta được
2 2 2
2 2 2
1 3
cos cos
2 3 6
1 3

(35)
Phương trình của vòng động học thứ hai
1 1 1 3 3 3 1
P A B B A P P
. Hoàn toàn tương tự như vòng động
học thứ nhất ta được
3 2 3
3 2 3
1 3
cos sin
2 2 3
3 1 3
sin cos
2 2 3
P
P
c
x u l h
y c u l h
q j
q j
 


   







Từ hình vẽ ta có thể xác định véc tơ vị trí khối tâm
2 khâu
   
1 2
1 1
1 1
1 1
1 1 1 1
1
cos
2
cos
1
sin , sin
2
0
0
C C
l
u
l ur r
q
q
q q
 
 
 
 
 
 

2


2

F
2

A
2

 
2
1 1 1
, ,
C m l

u
2

B
2
Y
2
X
2
y

H. 3 Chân thứ hai
y

C m lA
3

X
3
Y
3
H. 4 Chân thứ ba


P
x
y
P
1
X
1
Y
1
X
3
X
2
Y
3
Y
2

1 1 1
, ,
C m lA
1

yX
1

Y
1

B
1

xH. 2 Chân thứ nhất
578 Nguyễn Văn Khang, Nguyễn Thành Công
VCM2012
 
 
 
 

l
l
u
u
r
J
q
r
J
q
q
q
q q
q q
 
 

 
 

 
 
 
 

 
 
 
 
 

 
 
 
 
 
 
1 2
1 1
1 1
1 2
1 1
0 0 0 0
0 0 , 0 0
1 0 1 0
R R
ω ω
J J
q q
   
   
 
   
   
   
 
   
   
 
(42)
Gọi

Thay các giá trị ở (40), (42) và (43) vào biểu thức
             
     
1 1 2 2 1 1 1
2 2 2
1 1 1 1 1 1 1
1 2
1 1 1
T T T
T
T T T T R C R
R C R
m m
M(q) J J J J J I J
J I J
  


Ta được ma trận khối lượng của hệ hai khâu
2 2
1 1 2 1 1 2
2
1
0
4
0
m l m u I I
m
M(q)
 




 
 
  

(45)
Từ đó suy ra
 
2 1 1
2 1 1
2 0
0
m u u
m u
C q,q
q
 
 

 

 



(46)
Thế năng của robot có dạng
1 1 2 1 1

q
q
 
 

 

 








 


  


 





 
 

sin cos
2
1
cos sin
2
x u l u
y u l u
d q dq q d
d q dq q d
 


   





 
 


  





 
(49)



 


 



 




 
 

  
(50)
Từ (50) ta tính được ma trận f
*
1 1 2 1 1 1 2 1
*
1
1 1 1 1 1
1 1
sin cos
2 2
cos sin
X u l Y u l

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
1
2
4
1
cos
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
q q
q t
q q
 


   





 
 


Bây giờ ta chuyển sang thiết lập phương trình vi
phân chuyển động của bàn máy động (H. 5). Giả
sử bàn máy động là một tam giác đều, đồng chất.
Gọi m và I lần lượt là khối lượng và mô men quán
tính của bàn máy động đối với trục đi qua khối
tâm P và vuông góc với mặt phẳng hình vẽ.
Ta có véc tơ tọa độ suy rộng


T
P P
x yx
j
 .
Chuyển động của bàn máy động là chuyển động
song phẳng. Từ hình vẽ 5 ta thấy, bàn máy động
chịu tác dụng của các phản lực liên kết là
1 1 2 2 3 3
, , , , ,
X Y X Y X Y
. Do đó áp dụng phương trình vi
Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 579
Mã bài: 132

phân chuyển động của vật rắn chuyển động song
phẳng ta có

1 2 3
1 2 3

 
 
 
  
   

   
 
 
   
 
 
 
 
 
   
 



(53)
Tóm lại đối với robot phẳng 3 RPR ta có hệ các
phương trình vi phân – đại số như sau :
Sáu phương trình liên kết
1 2 1
1 2 1
2 2 2
2 2 2
1 3
cos cos +

   
 
 
  
 
 
 
 
 
   
   
 
 
  
 
 
 
 
 
   
   
 
 
    
 
 
 
 
 
   




 
 


   





 
(54)
Chín 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 1 1
2 cos sin cos
4 2 2 2
sin cos sin
m l 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
q q q t q q
q q q q
       
   

       
 


2
2 2 2 2 2 2 2 2 2 2 2 2
sin cos sin
m u m u m g F X Y
q q q q
    



(55)
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
2
2 3 2 3 3 2 3 3 3 3 3 3
1 1 1 1
2 cos sin cos
4 2 2 2
sin cos sin
m l m u I I m u u ml m u g X u l Y u l
m u m u m g F X Y
q q q t q q
q q q q
       
   
   
          

   
   

         
   
   
   
   
    
       





b) Mô phỏng số bài toán động lực học ngược
robot song phẳng 3RPR
i=1 i=2 i=3
x
Pi
[m] 0
0.3
3
0.15
3

y
Pi
[m] 0 0 0.45
h[m]

8
C C
m l m l
I kg m I kg m
mh
I kg m
   
 

Giả sử bàn máy động chuyển động theo quy luật
[7]
 
 
0
0
0
1 sin
3
cos
3
1 cos
3
P P
P P
x x R t
y y R t
t
p
p
p



 
0 0
0
0.15 3 , 0.15, 0.025
, 0,3
12
P P
x m y R m
t
p
j
  


 





580 Nguyễn Văn Khang, Nguyễn Thành Công
VCM2012
Bài toán đặt ra là xác định mô men (hay lực) và
công suất của khâu dẫn động cần thiết để tạo ra
chuyển động của bàn máy động.
Một phần 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 17.
Trong đó đưa ra hai phương án: Phương án 1

Truyền động
bằng lực
0.212(s)
Bảng 2. Thời gian tính toán của hai phương pháp

Phương pháp giải bài toán động lực học ngược
dựa trên phương trình Lagrange dạng nhân tử
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)
Torque(Nm)torque1
torque2
torque3
H
. 6 Đồ thị mô men các động cơ 0 0.5 1 1.5 2 2.5 3
-4


Trường hợp dẫn động lực

0 0.5 1 1.5 2 2.5 3
-60
-40
-20
0
20
40
60
80
t(s)
Force(Nm)force1
force2
force3

H. 9 Đồ thị lực dẫn động các động cơ

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 581
Mã bài: 132 0 0.5 1 1.5 2 2.5 3
-3
-2
-1

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)
Torque(Nm)torque1
torque2
torque3

H. 12 Đồ thị mô men các động cơ

0 0.5 1 1.5 2 2.5 3
-4
-3
-2
-1
0
1
2
t(s)
Power(W)


40
60
80
t(s)
Force(Nm)force1
force2
force3

H. 15 Đồ thị lực các động cơ

582 Nguyễn Văn Khang, Nguyễn Thành Công
VCM2012
0 0.5 1 1.5 2 2.5 3
-3
-2
-1
0
1
2
3
t(s)
Power(W)power1
power2
power3

phương trình Lagrange dạng nhân tử.
Sử dụng phương pháp giải bài toán động học
robot song song bằng phương pháp số [10-11] và
kết hợp với phương pháp giải bài toán động lực
học ngược trình bầy trong báo cáo này, nhóm
nghiên cứu của chúng tôi đã tiến hành tính toán
động học ngược, động lực học ngược và điều
khiển nhiều robot song song phẳng và robot song
song không gian. Các kết quả nghiên cứu sẽ được
trình bày trong các công trình công bố sắp tới.

Lời cảm ơn
Công trình này được sự tài trợ về kinh phí của Đề
tài nghiên cứu khoa học cấp trường về robot song
song của Trường Đại học Bách Khoa Hà Nội.

Tài liệu tham khảo
[1] Nguyen Van Khang: Động lực học hệ nhiều vật.
NXB khoa học và Kỹ thuật, 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.
[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

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 583
Mã bài: 132
Nguyễn Văn Khang, TS.
1973 (CHLB Đức), TSKH.
1986 (CHLB Đức), PGS Cơ
học 1991 (ĐHBKHN), GS
Cơ học 1996 (ĐHBKHN).
Lĩnh vực nghiên cứu: Động
lực học và điều khiển hệ
nhiều vật/robot, Dao động
tuyến tính và phi tuyến, Điều
khiển các hệ cơ điện tử. Các
giáo trình giảng dạy: Động lực học hệ nhiều vật,
Động lực học phi tuyến và hỗn độn, Dao động kỹ
thuật, Cơ học kỹ thuật, Động lực học và Điều
khiển robot.
Seoul, Korea. Lĩnh vực
nghiên cứu: Động lực học và
Điều khiển robot, Dao động
và Cơ điện tử.