Proceedings VCM 2012 74 thiết kế bộ điều khiển bám quỹ đạo cho robot bằng phương pháp

Thiết kế bộ điều khiển bám quỹ đạo cho robot bằng phương pháp Jacobian xấp xỉ thích nghi Design adaptive controller for robot motion tracking using approximate Jacobian matrix approach

Thiết kế bộ điều khiển bám quỹ đạo cho robot bằng phương pháp

Jacobian xấp xỉ thích nghi Design adaptive controller for robot motion tracking using approximate

Jacobian matrix approach

Thái Hữu Nguyên1, Nguyễn Phạm Thục Anh2

1 Trường ĐHSPKT Vinh, 2 Trường ĐHBK Hà Nội e-Mail: thainguyenktv@yahoo.com

Tóm tắt

Bài báo trình bày về phương pháp thiết kế bộ điều khiển bám quỹ đạo cho robot theo phương pháp Jacobian xấp xỉ thích nghi khi động học và động lực học không biết chính xác Ý tưởng chính là đưa vào một vector trượt thích nghi sử dụng tốc độ tay máy ước lượng Các thông số động học không chính xác của tốc độ tay máy ước lượng và ma trận Jacobian được cập nhật bởi một luật cập nhật thông số động học Cơ sở phân tích sự ổn định được dựa trên hàm điều khiển Lyapunov Vị trí điểm tác động cuối của robot sẽ hội tụ đến vị trí nhiệm vụ mong muốn trong một không gian hữu hạn ngay cả khi động học và ma trận Jacobian là không chắc chắn Kết quả của bộ điều khiển được kiểm chứng trên mô hình robot 3 thanh nối và được mô phỏng trên phần mềm matlab-Simulink

Abstract:

This paper presents an adaptive approach for motion tracking of robots by using Jacobian matrix under assumption that kinematic and dynamic parameters are uncertain These parameters can be estimated and the Jacobian matrix are calculated by an update law The basis of the stability analysis is based on the Lyapunov control function The actual position of the robot converges to the desired task position in workspace The efectiveness of the control approach has been confirmed on a 3-link planar robot and simulated in Matlab-Simulink

Ký hiệu

Ký hiệu Đơn vị Ý nghĩa

M,C,G,S Các ma trận

Chữ viết tắt

1; 12; 123

c c c Cos các biến góc tương ứng

1; 12; 123

s s s Sin các biến góc tương ứng

kj

C Các thành phần của ma trận C

kj

S Các thành phần của ma trận S

1 Đặt vấn đề

Trong hầu hết các nghiên cứu về điều khiển robot

trước đây thường giả thiết các thông số động học

của robot (chiều dài các thanh nối, khoảng cách từ

trục quay tới trọng tâm…) có thể đo chính xác

Nhưng thực tế luôn có sai số đo, hơn nữa khi

robot thao tác gắp các vật dụng chưa xác định

trước, sẽ dẫn đến các thông số này thay đổi theo

quá trình thao tác Sự bất định của thông số động

học có thể dẫn đến hai vấn đề như sau: (1) trong bài toán động học ngược vị trí, việc tính toán từ vị trí tay máy sang các biến khớp sẽ không chính xác, (2) trong bài toán động học ngược tốc độ, ma trận Jacoby là hàm của chiều dài thanh nối sẽ không chính xác Nếu ta dùng các bộ điều khiển chuyển động trong không gian làm việc truyền thống sử dụng ma trận Jacoby với sự không biết chính xác động học hoặc những thay đổi không biết trước của đối tượng công tác sẽ dẫn đến sai lệch quỹ đạo Mặt khác, trong hầu hết các phương pháp điều khiển truyền thống cũng giả định các thông số động lực học của Robot như khối lượng, mô men quán tính, các hệ số ma sát là biết chính xác Vì vậy lại tạo nên những bất lợi mới cho điều khiển bám chính xác quỹ đạo khi áp dụng điều khiển truyền thống không thích nghi Lúc này một trong những vấn đề được đặt ra để giải quyết các hạn chế trên đó là thiết kế bộ điều khiển ma trận Jacoby xấp xỉ thích nghi để điều khiển bám theo quỹ đạo chuyển động mong muốn khi không biết chính xác động học và động lực học robot Như vậy, áp dụng luật điều khiển này sẽ làm cho robot

có một mức độ linh hoạt cao trong việc xử lý

những thay đổi không biết trước và sự không biết

chính xác động học và động lực học của nó

2 Mô hình toán học của robot

Phương trình động lực học tổng quát của robot n

bậc tự do [1]:

2

q M  q q 



Trong đó: q[ ,q q1 2, ,q n]TR nlà các biến khớp;

M(q) n n

R  là ma trận quán tính, R n là mô

men đặt lên trục các khớp của robot, G(q)Rnlà

thành phần trọng lực của robot, n n

S(q, )q  R  là ma

trận nghiêng đối

Phương trình động học thuận vị trí của robot n

DOF có dạng:

( )

X h q (2)

VớiXRnbiểu diễn từ vị trí và hướng của cơ cấu

tác động cuối trong không gian Đề các, được tính

toán hình học hoặc theo phương pháp D-H

Phương trình biểu diễn quan hệ giữa tốc độ tay

máy X và tốc độ khớpq: X J q q

(3)

( ) n

J q R là ma trận Jacobian

Xét mô hình robot 3 thanh nối được biểu diễn như

hình H1 Trong đó: ilà góc quay của các khớp

nối (i 1, 2,3)mi là khối lượng thanh nối i; l i là

chiều dài thanh nối i; lgi là khoảng cách từ khớp i

khối tâm thanh nối i; j i là mô men quán tính của

thanh nối i đối với trục qua khối tâm của thanh

nối; i là mô men tác dụng của khớp i; v i là vận

tốc dài của khối tâm thanh nối i; x y i, i là toạ độ

khối tâm thanh nối i; ( , ) x y là toạ độ điểm p (cơ

cấu tác động cuối); là hướng của cơ cấu tác

động cuối so với phương ngang

H.1: Cấu trúc robot phẳng 3 thanh nối

Phương trình động học thuận:

( )

X h q vớiX x y, ,T là vị trí và hướng của tay máy q q q q1, 2, 3Tlà vị trí khớp (biến khớp)

ta có:

1 2 3







(4)

Ma trận Jacobian:dX J dq

1 1 3

Θ

d dx

dt dt

d dy

J





  

với: Θ   1, 2, 3T

 

1 1 2 12 3 123 2 12 3 123 3 123

1 1 2 12 3 123 2 12 3 123 3 123

Θ

(5) Phương trình động lực học được thiết lập từ phương trình Lagrange:



(6)

Tính toán các thông số của mô hình robot H.1:

1 1 1 1 2 1 1 2 12

3 1 1 2 12 3 123

 

2 Θ 2 lg2 12 3 (2 12 lg3 123)

 

3 Θ 3 lg3 123

G m g c (7)

11 Θ m lg1 1 1 2 1 lg2 2 lg1 2 2

2 3 1 2 lg 3 2 1 2 2 2 lg 1 3 23 2 lg 2 3 3 3

22 Θ 2lg2 2 3 2 lg3 2 lg2 3 3 3

33 Θ 3lg3 3

12 Θ 21 Θ 2 lg2 2 lg1 2 2 2

3 2 lg3 1 2 2 21 3 23 2 lg2 3 3 3

13 Θ 31 Θ 3 lg 3 1 lg 3 23 2 lg 3 3 3

23 32 3 3 2 3 3 3

M Θ M Θ m lg l lg c J

(8)

l3

l2

l1

m1

m2

m3

1

2

3

y

x P(x,y)

   

1 Θ, 2 1lg2 2 2 21 2

V    m l s   

3 1 2 2 2 1 2

1 3 23 2 3 1 2 3

2 3 3 3 1 2 3

2

[

m l l s

 

2

2 2 1 2 2 1

3 2 3 3 3 1 2 3

1 2 2 1 1 3 23 1

lg

[

]

2 2 lg



 





3 3 2 3 23 1 2 1 2

V Θ, m l lg s   2  

(9)

Ta có: Θ,  Θ,  1 (Θ)

2

S    C    M (10) là ma trận

3 3 đối xứng lệch Tính ma trận CΘ,như sau:

 

 

 

 

1 2 3

Θ

Θ

T kj

kj

c

c

với: i j k , , 1, 2, 3

2

ikj

c

Từ công thức trên ta tính được:

 

11 2 1 2 2 2 3 1 2 2

1 3 23 2 1 3 23 2 3 3 3

[











 

12 2 1 2 2 1 2 3 1 2 2

1 3 23 1 2

1 3 23 2 3 3 3

[



 



 



 

13 3 1 3 23 2 3 3 1

2 3

)







 

 

21 2 1 2 2 1 3 1 2 2

1 3 23 1 2 3 3 3

l

[ ]







 

22 Θ, 3 2lg3 3 3

C    m l s 

 

23 Θ, 3 2lg3 3( 1 2 3)

C    m l s   

31 3 1 3 23 2 3 3 1

2 3 3 2

lg ]













 

32 Θ, 3 2lg3 3( 1 2)

C   m l s  

32 Θ, 0

(12)

 

11 2 1 2 2 2 3 1 2 2 2

1 3 23 2 3 2 3 3 3

2 g

[ ]



12 21 2 1 2 2 2

3 1 2 2 2 1 3 23 2 3 2 3 3 3

m l l s l

M

M

l





13 31 3 1 3 23 2 3

2 3 3 3

l

[ ]

g

l

s





 



M22 Θ  2m l3 2lg3 3 3s 

23 Θ 32 Θ 3 2lg3 3 3

M M  m l s 

 

32 Θ 0

M  (13) Thay vào CΘ,và M(Θ) vào (10) ta có:

11 0; 22 0; 33 0;

12 12 2 1 2 2 1 2

3 1 2 2 1 2 1 3 23 1 2 3

1

2

13 31 3 1 3 23 1 2 3

2 3 23 1 2 3

1

1

2

23 32 3 2 3 3 1 2 3

1

2

S  S  m l s     thay các hệ số vào ta có được phương trình động lực học của tay máy robot 3 thanh như (6)

3 Cơ sở lý thuyết

Bộ điều khiển bám quỹ đạo cho robot theo phương pháp Jacobian xấp xỉ thích nghi khi động học và động lực học không biết chính xác Ý tưởng chính

là đưa vào một vector trượt thích nghi sử dụng tốc

độ tay ước lượng Các thông số động học không chính xác của tốc độ tay ước lượng và ma trận Jacobian được cập nhật bởi một luật cập nhật

thông số động học

Đặt Xr Xd α(X  X )d (14)

Trong đó: X hay X – Xd được đo bởi một sensor vị trí, Xd Rnlà quỹ đạo của tay máy và

n

d

dt

X   là tốc độ đặt của tay máy

Đạo hàm (14) ta được: X r Xd α(X Xd)

(15)

d

dX dt

X 

là tốc độ thực của tay và d d

d dt

X  X

là gia tốc đặt của tay

Ta có: X J q, L qY q, qL (16)

Trong đó: LR fchứa các thông số động học,

J q, L R  là ma trận Jacobian và   n f

Y q,q R 

Khi các thông số động học không biết chính xác

thì ta có:Xˆ Jˆ q,L qˆ Y q, q Lˆ

(17)

Trong đó: Xˆlà tốc độ ước lượng của tay máy,

ˆ q,ˆ R

J L   là ma trận Jacobian xấp xỉ và

f

ˆ R

L  chứa các thông số động học chưa biết Lˆsẽ

được cập nhật bởi một luật cập nhật thông số được

định nghĩa sau Vector trượt thích nghiˆsx:

ˆs X X J L qX  q LX

(18) đặt: 1 

r ˆ q, ˆ r

q J L X (19)

Trường hợp này ta thừa nhận là robot làm việc

trong không gian hữu hạn sao cho ma trận

Jacobian xấp xỉ không suy biến từ (19) ta có:

r ˆ q,L Xˆ r ˆ q,ˆ r

q J  J L X

(20) Với:Jˆ 1 q,Lˆ  Jˆ1     q,L Jˆ ˆ q  , ˆ ˆL J1 q, ˆL

Ta định nghĩa vector trượt s trong không gian

khớp:

 

1

s q  q Jˆ q,Lˆ ˆs

(21)

Và s q qr (22)

Thay (21) và (22) vào phương trình (1), ta được:

1

2



1

(23) Tổng số hạng 4 số cuối của phương trình (23) có

thể viết ở dạng:

r r

1

2

Z q, , , U

q q q





  

(24)

Trong đó:URpchứa các thông số động lực học

chưa biết của robot, Z q q q q q ( , ( , ,   r, r)  R(n p ).

Thay (24) vào (23) ta được:

1

2

q q q





  

(25) Luật điều khiển thích nghi trên cơ sở ma trận

Jacobian xấp xỉ được đễ xuất bởi Cheah [TL-3]:

q

ˆ ,ˆ Kˆ Z q, , , ˆ

T

T





  

(26)

trong đó:X X Xd,

d

  

d p

K , K , K là các ma trận đường chéo cấp n xác định dương

Các thông số động học ước lượng L ˆcủa mà trận

Jacobian Jˆ q,Lˆ được cập nhật (update) bởi luật

ˆ

(27) Và các thông số động lực học U ˆ được ước lượng bởi luật

cập nhật sau:

T

r r

ˆ

U  q q  q q

(28)

Trong đó:RRf f , n n

N  R  là các ma trận đường chéo có hệ số dương Thay (26) vào (25) thu được:

1 ( ) [ ( ) ( , )] ( , , , ) 2

ˆ ( , )(ˆ ) ˆ ( , )ˆ ˆ 0

r r





(29)

ở đây:UUUˆ Chọn hàm Lyapunov sau:

1 1

( )







(30)

Đạo hàm phương trình (30) theo thời gian thu được:

1 1

ˆ











(31) Thay M q s  từ phương trình (29), Lˆ

từ phương trình (27) và Uˆ

từ phương trình (28), sử dụng thuộc tính 2 của phương trình động lực học và phương trình (31) thu được:

T

s ˆ q,ˆ K ˆ s ˆ q,ˆ ( K

K X) X K αK

X

  







T

T T

d p

s q, K s q, (K

K X) X K αK

L Y q, K K X K

L Y q, K K

ˆ ˆ

X ˆ

X

q X

























(32)

Từ các phương trình (14), (16), (18) có:

 

s  X  X  q  (33)

Trong đó:      ˆ ˆ

Y q,q   L Y q,q L L X X (34)

Thay (33) vào (32) thu được:

 

 

T

α Y q, L K α X

Y q, L K α X K X 0



          





       





(35)

Từ (30),(34) và (35), theo tiêu chuẩn ổn định

Lyapunov, hàm V luôn dương và đạo hàm của V

luôn âm

Có thể rút ra kết luận sau: Trong một không

gian làm việc hữu hạn sao cho ma trận Jacobian

xấp xỉ là không suy biến thì luật điều khiển thích

nghi Jacobian xấp xỉ (26) và các luật cập nhật

thông số (27) và (28) dùng cho hệ thống robot (1)

sẽ làm hộ tụ vị trí và sai số bám tốc độ Nghĩa là:

0

d

XX  và X Xd  0 khi t   Ngoài ra, tốc

độ ược lượng của tay robot cũng hội tụ về tốc độ

thực của tay, nghĩa là: Xˆ X khi t  

4 Áp dụng thuật toán thích nghi sử dụng ma trận

Jacoby xấp xỉ cho robot phẳng 3 thanh nối

Để kiểm định tính hiệu quả của thuật toán điều

khiển đề xuất, thuật toán được áp dụng cho Robot

3DOF Plana Các thông số thực của robot như sau:

1

m m m 0 2 k g , l l l

l 1m , lg lg lg

2

momen quán tính các thanh nối:

1 2

J  J 

2

1 1 3

m l

J

12



+ Phương trình biểu diễn quan hệ giữa tốc độ tay

và tốc độ khớp:X J q, L q (36)

1 2 3

X  x y   qq q q    là

tốc độ tay và tốc độ khớp;   3 3

J q, L R  là ma trận

1 2 3

LL , L , L  R là các thông số

động học của robot ta có:

1 1 2 12 3 123 2 12 3 123 3 123

1 1 2 12 3 123 2 12 3 123 3 123

J q, L



(37)

Viết lại (36) dưới dạng khác: X Y q, qL(38);

với:

1 1 12 1 2 123 1 2 3

1 1 12 1 2 123 1 2 3

Y q,

q





(39)

Giả thiết góc nghiêng φconstnên   0 do đó

các phần tử hàng thứ 3 của ma trận Y q, q  0

+ Các vectorX  r,Xr,X s q qˆ,ˆx, r, r, s, , X,s  X

3

R được định nghĩa ở mục 3

+ Biểu diễn   r     r

1

2



  

G q  Z q,q q q   , , U

(40)

Trong đó: p

U  R chứa các thông số động lực học chưa biết của robot,   n p

r r

Z q,q q q  , , R 

Để tìm đượcZ q, , q q q   r , r và U, ta thay thế các ma

trận M q ,  M  q ,S q,q, G q đã xác định trong

mục 2 vào phương trình (40) Sau một số phép biến đổi tìm được:

1 1 1 1 1 2 1 2 1 2 2 2 2

Um lg , m lg J , m l , m l , m lg , m lg ,



2 1 2 2 3 1 3 1 3 2 3 2 3 1 2

, m l lg , J , m l , m l , m l , m l , m l l ,

T

3 3 3 3 3 1 3 3 2 3 3

, m lg , m lg , m l lg , m l lg , J   R



r r

Z q,q q q  , , R + Dùng luật điều khiển đã nêu trong mục 3:

T

T

q

ˆ , K

ˆ

ˆ ˆ ˆ Z q, , , ˆ



  

   (41)

T

ˆ

(42)

T

r r

ˆ

,

q, ,

U   q q q   (43)

Trong đóK , K , K, Rd p R3 3vàNR18 18 , 5 ma trận đều là ma trận đường chéo dương;

q, R

ˆT ˆ

J L   là ma trận Jacobian xấp xỉ;

18

ˆ R

U  là vector ước lượng của vector Uˆ , L ˆ R3

là vector ước lượng của vectorLˆ

Sơ đồ cấu trúc mô phỏng hệ thống

H.2 Sơ đồ mô phỏng hệ điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi

H.3 Sơ đồ cấu trúc khối Subsystem1

H.4 Sơ đồ cấu trúc khối Subusytem2

Quỹ đạo chuyển động mong muốn:

d

d

d

π φ 2















Để điều khiển tay robot bám theo quỹ đạo mong

muốn trên, thay đổi hệ số kP trong khoảng

500, 2500 và thay đổi hệ số kd trong khoảng

5, 20, ta chọn được các ma trận hệ số kP và kd

cho chất lượng hệ thống tốt nhất:

p

1500 0 0

K 0 1500 0

0 0 1500

d

5 0 0

K 0 5 0

0 0 5

  

Các ma trận K, R, N được chọn như sau:

10 0 0

K 0 10 0

0 0 10

;

0.1 0 0

R 0 0.1 0

0 0 0.1

;

18 18

N  R  là ma trận đường chéo có các phần tử

đường chéo chính bằng 0.1

Kết quả mô phỏng: Như hình H5, H6

H.5 Tọa độ x của điểm tác động cuối

H.6 Tọa độ y của điểm tác động cuối

5 Kết luận

Từ các phân tích lý thuyết theo tiêu chuẩn ổn định

Lyapunov cho thấy bộ điều khiển thích nghi sử

dụng ma trận Jacoby xấp xỉ đảm bảo sự bám chính

xác quỹ đạo cho Robot ngay cả khi các thông số

hệ thống bất định Các kết quả mô phỏng đã kiểm định hệ thống điều khiển là ổn định, các tín hiệu vị trí thực của tay máy robot hội tụ về các tín hiệu vị trí đặt với tốc độ hội tụ nhanh và sai số bám nhỏ Ảnh hưởng của các ma trận hệ số kP và kd tới chất lượng hệ thống:

+ Giữ nguyên kd : khi tăng kP trong khoảng

500, 2500 thì sai số bám giảm và thời gian quá

đọ giảm và khi giảm kP quá trình diễn ra ngược lại

+ Giữ nguyên kP : khi tăng kdtrong khoảng

5, 20 thì sai số bám tăng, thời gian quá độ giảm

và khi giảm kd quá trình diễn ra ngược lại

Tài liệu tham khảo

[1] Jonh j.Craig: Induction to Robotics (Mechanics and Control) Printed USA, 2005

[2] R Kelly, V Santibáñez and A Loría; Control

of Robot Manipulators in Joint Space

Springer-Verlag London Limited 2005 [3] Sadao Kawammura Mikhail Svinin (Eds);

Advances in Robot Control Springer-Verlag Berlin

Heidelberg 2006

Thái Hữu Nguyên sinh năm

1974 Nhận bằng thạc sỹ về Tự

động hóa của trường Đại học

Kỹ thuật Công nghiệp Thái Nguyên năm 2005 Từ năm

1996 đến nay là giảng viên của Khoa điện trường Đại học Sư phạm Kỹ thuật Vinh Hiện nay

là NCS thuộc bộ môn điều khiển tự động trường Đại học Bách khoa Hà Nội Hướng nghiên cứu là

mô hình hóa và điều khiển robot công nghiệp

Nguyễn Phạm Thục Anh sinh

năm 1968 Nhận bằng Tiến sỹ năm 2002 của trường Đại học Ritsumei kan Nhật bản Từ năm 1991 đến nay là giảng viên trường Đại học bách khoa

Hà Nội Hương nghiên cứu chính là điều khiển các hệ thống phi tuyến