Proceedings VCM 2012 78 về hai phương pháp giải bài toán động lực học ngược robot song song

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 đ

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: nvankhang@mail.hut.edu.vn, ntcongbk@gmail.com

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 s 

M s s b s s g s τ Φ s λ (1)

 

f s 0 (2)

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

T T

Trong các phương trình (1) và (2) ta có

  n s n s, r, T  n s r, r

s

M s    f  Φ s    λ 

s

f

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 xx t ,x m và phương trình liên kết

 , 

f x q 0, qn,fr Xác định mô men (hay

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]

     t k , t k , t k ,k 0 ,1, ,K

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t, ,  T s 

M s s p s s  τ Φ s λ (5)

Ta tách n s phương trình (5) thành hai nhóm, nhóm thứ nhất gồm n a 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    

M s q M s z p s s τ Φ s λ (6)

  2     2     2    

1 , , T

M s q M s z p s s Φ s λ (7)

Trong đó

        

    

M s

(8)

 

,

T a T

z

Φ s

Φ s (9)

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

Xét trường hợp rn Φ z, z là ma trận chính quy

Khi đó từ phương trình (7) ta suy ra

    2     2     2  

1 , ,

T

Φ s λM s q M s z p s s (10)

  1  2    2    2  

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     1    

1 , , 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à f x,q = 0  Tính s     t ,s t ,s t

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

   , s   ,

M s ,b s , s Φ s g s

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ơ qa, 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)

   ,  , r, n z, n a

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=n z Từ phương trình (13)

ta suy ra

 

s

f

s

 (14)

Gọi ds là nghiệm của hệ phương trình đại số tuyến

tính (14), ta có

Φ s dsΦ s dq Φ s dz0 (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

   ,   T

T s

λ Φ M s sb s s g s τ

Nhân hai vế của phương trình (17) với ds ta được

   ,   T

T s

λ Φ sd M s sb s s g s τ ds (18)

Chú ý đến công thức (15), Φ ss d s 0, từ (18) ta

suy ra

   ,   T T

Mặt khác từ phương trình (15) ta có

   

1

d    d

(20)

Chú ý rằng véc tơ dqa có thể viết lại dạng

a

d  d

(21) Kết hợp hai phương trình (20) và (21) ta có

   

1

a

n a

a

E q

d

 

Nếu ta đưa vào ký hiệu

 

   

1

a

n

E

R s

(23)

Thì phương trình (22) có dạng

  a

d  d

(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

Do 1, 2, ,

na

từ phương trình (26) ta suy ra

     ,  

T

R s M s s b s s g s  τ 0

Phương trình (27) có thể viết lại dưới dạng

       ,  

R s τR s M s s b s s g s

    (28)

Từ phương trình (23) ta có

1

[ , ( ( ) ( )) ] ( ( ) ( ))

a

z T





 

 

τ

τ

(29)

Thế (29) vào phương trình (28) ta được

Động

học

ngược

Xác định



(pt 11)

Xác định

momen phát động (pt 12)

       

1

, +( ( ) ( ))

T

a

T

τ 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 và f x,q = 0  , tính s     t ,s t ,s t

Bước 2 : Tính các ma trận     1 

Φ 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)

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

a p

s q q x  Trong đó qa 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), z qp

x

 

 

    là các tọa độ suy rộng phụ thuộc (với qp là các tọa độ suy rộng của

các khâu bị động và x là tọa độ suy rộng của bàn

máy động) 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à P A P A P A1 1, 2 2, 3 3

P

u

q q

     

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

P

u

q q

     

a) Thiết lập các phương trình vi phân –đại số

Phương trình xác định trọng tâm bàn máy động P

có dạng

P PP AA B B P

   

(31)

Chiếu (31) lên hai trục tọa độ x và y ta được

P

P

p

p

     

     

(32)

  2

2, ,2 2

m l C

x P2

2

2

F2

A2

  2

1, ,1 1

m l C

u2

  1

2 , 2 , 2

P1

u1

1

 1

F1

  1

1 , 1 , 1

A1

B3

B2

B1

  3

2 , 2,2

 3

P3 u3

 3

F 3

  3

1 , 1,1

C m l A3

h



P

y

H 1 Robot song phẳng 3RPR

yP

xP

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

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 P P

P A A B B B B A A P P P 

     

(33) Sau khi biến đổi ta được

P PP P P A A B B P

    

(34) Chiếu (34) lên hai trục tọa độ x và y ta được

P

P

p

p

       

      

(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

P

P

c



    



    

(36)

Để thiết lập phương trình vi phân chuyển động

của robot, ta sử dụng phương pháp tách cấu trúc

Ta sẽ tách robot thành các cơ cấu con có cấu trúc

đơn giản hơn như các hình (2-5)

Để thiết lập phương trình vi phân chuyển động

của chân thứ nhất (H 2), ta chọn tọa độ suy rộng

như sau

  1  

1 1

T

u

q  q (37)

Phương trình Lagrange loại 2 dạng ma trận của hệ hai khâu

M q q + C q,q q + g q = f   (38)

Từ hình vẽ ta có thể xác định véc tơ vị trí khối tâm

2 khâu

1 cos

1

2

0 0

l

u

q

q

(39)

Từ (39) ta tính được các ma trận Jacobi tịnh tiến

P1

  2

2 , 2 , 2

x P2

2

2

F2

A2

  2

1 , 1 , 1

u2 B2

Y2

X2

y

H 3 Chân thứ hai

y

x P1

  3

2 , 2 , 2

3

P3 u3

 3

B3

  3

1 , 1 , 1

A3

X3

Y3

H 4 Chân thứ ba



P

x

y

P1

X1 Y1

X3

X2

Y3

Y2

xP

yP

h

H 5 Bàn máy động

  1

2 , 2 , 2

P1

u1

 1

1

F1

  1

1 , 1 , 1

A1

y

X1 Y1

B1

x

H 2 Chân thứ nhất

   

 

   

 

1

1

2

2

1

1

1

1

1

1

2 1

2

sin cos

C

T

C

T

l l

u u

r

J

q

r

J

q

q q

(40)

Các véc tơ vận tốc góc của hai khâu

  1   1

   

   

(41)

Từ (41) ta tính được các ma trận Jacobi quay

   

 

   

 

Gọi I I1, 2 lần lượt là mô men quán tính của các

khâu P A A B1 1, 1 1 đối với trục đi qua khối tâm của

chúng và vuông góc với mặt phẳng hình vẽ Từ đó

suy ra các ma trận mô men quán quán tính khối có

dạng

(43)

Thay các giá trị ở (40), (42) và (43) vào biểu thức

             

     

1 1 1 1 1 1 1

1 1 1

T

R C R

J I J



Ta được ma trận khối lượng của hệ hai khâu

2

1

0 4

0

m

M(q)



(44)

Theo [12] ta có công thức ma trận Coriolis

2

T



(45)

Từ đó suy ra

2 1 1

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

1

sin



     

Từ đó suy ra

 

1

1

cos 2

sin

T

m l m u g

m g

g q

q q



(47)

Công ảo của các lực không có thế

d t dq  d  d  d (48)

Từ hệ thức

x u  l  q y u  l  q

ta suy ra

1 1 2 1 1 1 1

1

2 1

2



      



     

(49)

Thay (49) vào (48) ta được

1

1 sin 2 1 cos 2

q dq



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



    

(50)

Từ (50) ta tính được ma trận f *

*

1

(51) Thế các biểu thức (44), (46), (47), (51) vào phương trình (38) ta được phương trình vi phân chuyển động của hai khâu gốc P1

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



     

       

(52)

2

m u m u q m g q FX q Y 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ẽ

P P

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

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

3

c cos sin

P

P



       



      





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

2

P

P

P

P

P

c x

p

p

p

p

     

     

       

      

P



      

(54)

Chín phương trình vi phân

2





2

m u m u q m g q F X q Y q (55)

2







1 2 3

1 2 3

3

P

P



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









b) Mô phỏng số bài toán động lực học ngược

robot song phẳng 3RPR

Bảng 1 Thông số kỹ thuật của robot 3RPR

Để mô phỏng số ta cho các thông số kỹ thuật của

robot song phẳng 3RPR trên bảng 1 [7]

Công thức tính mô men quán tính khối có dạng

như sau

2

2

0.1687[ ] 8

mh

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

t

p

p p

j j



    



   

Trong đó

 

0

12

t

p j



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

mômen dẫn động đặt vào các khâu quay 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 K=150, ta có thể so sánh thời

gian tính toán theo hai phương pháp như bảng

sau :

Phương pháp dựa

trên phương trình

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

men

0.218(s)

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

-30

-20

-10

0

10

20

30

t(s)

torque1 torque2 torque3

H

6 Đồ thị mô men các động cơ

-4 -3 -2 -1 0 1 2

t(s)

power1 power2 power3

H 7 Đồ thị công suất các động cơ

-3 -2.5 -2 -1.5 -1 -0.5 0 0.5

t(s)

H 8 Đồ thị tổng công suất các động cơ

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

-60 -40 -20 0 20 40 60 80

t(s)

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

-3

-2

-1

0

1

2

3

t(s)

power1 power2 power3

H 10 Đồ thị công suất các động cơ

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

t(s)

H 11 Đồ thị tổng công suất các động cơ

Phương pháp giải bài toán độ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

Trường hợp dẫn động mô men

-30

-20

-10

0

10

20

30

t(s)

torque1 torque2 torque3

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

-4 -3 -2 -1 0 1 2

t(s)

power1 power2 power3

H 13 Đồ thị công suất các động cơ

-3 -2.5 -2 -1.5 -1 -0.5 0 0.5

t(s)

H 14 Đồ thị tổng công suất các động cơ

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

-60 -40 -20 0 20 40 60 80

t(s)

force1 force2 force3

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

0 0.5 1 1.5 2 2.5 3

-3

-2

-1

0

1

2

3

t(s)

power1 power2 power3

H 16 Đồ thị công suất các động cơ

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

t(s)

H 17 Đồ thị tổng công suất các động cơ

5 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ử

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 planar 3 R parallel manipulator Journal of

Dynamic Systems, Measuremwnt, and Control, Vol 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 Vol

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 Proceeding 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, Vol 35 (2013)

[11] Nguyen Thanh Cong: Tính toán động lực học ngược robot song song sử dụng phương pháp tách cấu trúc Đồ án tốt nghiệp đại học, Trường

Đại học Bách khoa Hà Nội 2012

[12] Nguyen Van Khang: Chu Anh My: Cơ sở robot công nghiệp NXB giáo dục, Hà Nội 2011

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

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

Nguyễn Thành Công, sinh

năm 1989, tốt nghiệp ngành

cơ điện tử Trường ĐHBKHN

2012 Từ 2013 học cao học và nghiên cứu sinh tại Lab “Các

hệ thống cơ điện tử thông minh”, Soongsil University,

nghiên cứu: Động lực học và Điều khiển robot, Dao động

và Cơ điện tử