đồ án Điều khiển robot

Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệgiữa hệ toạ độ tay Robot End effector và hệ toạ độ các khớp Joints.. Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ

Đề bài

Cho một cơ cấu Robot 2 thanh nối đợc truyền động bởi động cơ một chiều

Động cơ một chiều đợc cấp điện từ một bộ khuếch đại điện áp

1 Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệgiữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints)

2 Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay Robot

3 Viết hàm MATLAB thực hiện các phơng trình khi tay Robot di chuyển

từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng Đồ thị tốc

độ đặt trớc của tay Robot dọc theo quỹ đạo cho ở hình 2

4 Thiết kế “bộ điều khiển bù trọng lực” cho Robot

5 Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1thay đổi nhảy cấp từ 0 đến 1 rad

Bảng thông số của Robot:

5 Momen quán tính khớp 1 quay quanh tâm khối (J1) 0,528 Kgm2

6 Momen quán tính khớp 2 quay quanh tâm khối (J2) 0,19 Kgm2

7 Khoảng cách từ khớp 1 đến tâm khối 1 (lg1) 0,25 m

8 Khoảng cách từ khớp 2 đến tâm khối 2 (lg2) 0,15 m

9 Hằng số momen của động cơ khớp 1, 2 (KM) 0,5 Nm/A

11 Momen lớn nhất của động cơ khớp 1 (M1max) 1,2 Nm

12 Momen lớn nhất của động cơ khớp 2 (M2max) 0,7 Nm

13 Tốc độ lớn nhất của động cơ khớp 1, 2 (ωmax) 90 rad/s

14 Momen quán tính của động cơ (JĐ) 0,0004 Kgm2

1 1

0 2

0 n

p a o n

p a o n

p a o n T

z z z z

y y y y

x x x x

E 0

Theo đó ta xác định đợc các ma trận biến đổi A của robot nh sau:

0 0

d c

s 0

sθ a s

cθ c

cθ sθ

cθ a s

sθ c

sθ cθ

A

i i

i

i i i i i

i i

i i i i i i i

i -1 i

αα

αα

αα

0 1 0 0

sθ l 0 cθ sθ

cθ l 0 sθ cθ

1 1 1

1

1 0

0 1 0 0

sθ l 0 cθ sθ

cθ l 0 sθ cθ

2 2 2

2

2 1

++

++

+

−+

=

1 0 0 0

p a o n

p a o n

p a o n

1 0

0 0

0 1

0 0

θ θ s l sθ l 0 θ θ c θ θ s

θ θ c l cθ l 0 θ θ s θ

θ c T

z z z z

y y y y

x

ã x x x

2 1 2 1 1 2

1 2

1

2 1 2 1 1 2

1 2

1

E 0

1

θ θ sin θ

θ s

θ θ cos θ

θ c

+

=+

+

=+

P là điểm thuộc hệ toạ độ gắn với tay robot, có vị trí đợc xác định bằngvectơ cột thứ t của 0 T E

Từ (2) ta đợc phơng trình động học thuận biểu diễn mối quan hệ giữa hệ tọa

độ tay Robot và hệ tọa độ các khớp:

=

++

=

0 z

) θ sin(θ l sinθ l y

) θ cos(θ l

cosθ l x

T

2 1 2 1 1 T

2 1 2

1 1 T

p a o n

p a o n

p a o n T

z z z z

y y y y

x

· x x x

E 0

Theo (1) ta cã:

2

1 1

0 E

0 1 0 0

s l 0 c

s

c l 0 s c

1 0 0 0

0 f f f

0 f f f

0 f f f

2 2 2

2

2 2 2

2

33 32 31

23 22 21

13 12 11

θθ

θ

θθ

p a o n

p a o n

p a o n

1 0 0 0

0 1 0 0

0 0 cθ sθ -

l - 0 sθ cθ

1 0 0 0

f f f f

f f f f

f f f f T A

z z z z

y y y y

x

· x x x 1

1

1 1

1

34 33 32 31

24 23 22 21

14 13 12 11

E 0 1 1 0

− +

− +

−

− +

+ +

+

=

1 0

0 0

p a

o n

c p s p c

a s a c

o s o c

n s n

l s p c p s

a c a s

o c o s

n c n

z z

z z

1 y 1 x 1

y 1 x 1 y 1 x 1 y 1 x

1 y 1 x y

1 x y

1 x 1

y 1 x

θ θ

θ θ

θ θ

θ θ

θ θ

θ θ

θ θ

θ θ

C©n b»ng vÕ tr¸i vµ ph¶i cña (5), ta cã:

2 2 14

sθ l f

cθ l f





=+

−

+

=+

⇔

2 2 1 y 1 x

2 2 1 1 y 1 x

sθ l cθ p sθ p

cθ l l sθ p cθ p

y x 2

2 2 2

p y

p x

1 θ s θ c

Bình phơng hai vế của hai phơng trình trong (6) và cộng vế ta có:

2 1

2 2

2 1 2 2 2

l 2l

) l (l ) y (x

) l (l ) y (x

) l (l ) y (x 2 ) l l y (x

2

2 1 2 2

4 2

4 1 2 2 2 2

2 2

2 1 2 2

+++

−+

++

=

+++

−+

++

=

) l (l ) y (x k

) l (l ) y (x 2 ) l l y (x k

2 2

2 1 2 2 2

4 2

4 1 2 2 2 2

2 2

2 1 2 2 1

ta có:

) k , atan2(k

Thay (7) vào (6) ta đợc:

1

2 2

2 1 2 2 1 1

2l

l l x y ysθ

Giải ra ta có:

) k , atan2(k x)

atan2(y,

trong đó:

) l (l ) y (x

2

2 1 2 2

) , ( 2 tan a ) x , y ( 2 tan a

2 1 2

3 1 1

ααθ

ααθ

(10)

Với :

+

−+

=

+++

−+

++

=

) l (l ) y (x k

) l (l ) y (x k

) l (l ) y (x 2 ) l l y (x k

2 2

2 1 2 2 3

2 2

2 1 2 2 2

4 2

4 1 2 2 2 2

2 2

2 1 2 2 1

Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của tay Robot

Đạo hàm hai vế phơng trình động học thuận theo thời gian ta có:

+

=

++

−

−

=

) θ cos(θ )l

θ θ ( cosθ l θ y

) θ sin(θ )l θ θ ( sinθ l θ x

2 1 2

2 1 1 1 1

2 1 2 2 1 1 1 1

+

+

−+

1 2

2 1 2

1 1

2 1 2 2

1 2 1 1

θ

θ ) θ cos(θ l

) θ cos(θ l

cosθ l

) θ sin(θ l ) θ sin(θ l sinθ l y

+

+

−+

−

−

=

) θ cos(θ l

) θ cos(θ l

cosθ l

) θ sin(θ l ) θ sin(θ l sinθ l J

2 1 2

2 1 2

1 1

2 1 2 2

1 2 1 1

(J: ma trận Jacobien)

Phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay Robot

đợc biểu diễn dới dạng:

) θ (θ sin l θ sin l ) θ (θ cos l θ cos l

) θ (θ sin l )

θ (θ cos l

2 1 2 2

1 2

−

−

++

=

−

( )J l 2 cos( 1 2)l 1 sin( )1 l 2 sin( 1 2)l 1 cos( )1

Câu 3 Viết hàm MATLAB thực hiện các ph ơng trình ở câu 1 và

2 và vẽ đ ờng biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0] đến [0,0:0,4] theo một đ ờng thẳng Đồ thị tốc độ của tay Robot đã cho nh hình 2

d(2)=d2*360/(2*pi) %Goc theta 2 tinh theo do.

v=[vx;vy]; % Vecto van toc theo truc x va y.

vd=inv(J)*v; % Van toc goc cua khop 1 va 2 tinh theo radian/s

d Vẽ đờng biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0 m] đến [0,0:0,4 m] theo một đờng thẳng

Dựa vào dạng đồ thị tốc độ ở hình 2 ta có quỹ đạo chuyển động của tayrobot là quỹ đạo 2-1-2 Mặt khác tay Robot di chuyển từ vị trí [0,4: 0,0 m] đến[0,0: 0,4 m] theo một đờng thẳng thực chất là chuyển động tịnh tiến theo cả haitrục x và y

Xét khoảng thời gian từ 0ữ0,25s, tay Robot di chuyển theo quỹ đạo bậc 2:

=

11 12 1

10 11

2 12 1

a t 2a (t) x

a t a t a (t) x

=

11 12 1

10 11

2 12 1

b t 2b (t) y

b t b t b (t) y

0 ) 0 ( y

0 ) 0 ( y

V (0,25) x

0 ) 0 ( x

4 , 0 ) 0 ( x

y 12 11 10

max x max

x 12 11 10

V 2 5 , 0 V b

0 b

0 b

V 2 5 , 0 V a

0 a

4 , 0 a

20 21

2

a (t) x

a 0,25) (t

a (t) x

20 21

2

b (t) y

b 0,25) (t

b (t) y



Từ điều kiện biên suy ra:

max y 21

max x 21

V b

V a

=

= Xét khoảng thời gian từ 0,75ữ1s, robot di chuyển theo quỹ đạo bậc 2:

=

31 32

3

30 31

2 32

3

a 0,75) (t

2a (t) x

a 0,75) (t

a 0,75) (t

a (t) x

−

=

31 32

3

30 31

2 32

3

b 0,75) (t

2b (t) y

b 0,75) (t

b 0,75) (t

b (t) y

Dựa vào các điều kiện biên có:

0 (1) y

V (0,75) y

0 (1) x

V (0,75) x

3

ymax 3

3

xmax 3

max y 31

max x 32

max x 31

V 2 b

V b

V 2 a

V a

0 (1) x

3

3

=

= ⇒

0,4 b

0,25 V

0,25 V

2

0 a 0,25 V

0,25 V

2

30 ymax

2 ymax

30 xmax

2 xmax

=+

ì+

ì

−

=+

ì+

(0,75) x

(0,25) x

(0,25) x

2 3

1 2

2 30

xmax 1

20

0,5V a

(0,75) x

a

0,125V 0,4

(0,25) x

(0,75) y

(0,25) y

(0,25) y

2 3

1 2

2 30

ymax 1

20

0,5V a

(0,75) y

b

0,125V (0,25)

y b

Vậy ta có:

(0,4 0,125V 0,5V ) 0 0,25

V 0,25 V

−

(0,125V 0,5V ) 0,4 0,25

V 0,25 V

−

s m 5333 , 0

V xmax =−

s m 5333 , 0

V ymax =Thay vào các hệ số của phơng trình quỹ đạo chuyển động ở trên ta đợc ph-

ơng trình quỹ đạo chuyển động của robot nh sau:

t 0,5333 0,75

t 1,0666

3333 , 0 0,25 t

0,5333

0,4 1,0666t

t

2

1 t

1 t 0,75

0,75 t

0,25

0,25 t

t 5333 , 0 0,75 t

0666 , 1

0,0667 0,25

t 0,5333

1,0666t t

2

1 t

1 t 0,75

0,75 t

0,25

0,25 t

Kết quả thu đợc khi chạy chơng trình với 200 điểm quỹ đạo đợc thể hiệntrên hình vẽ 3.

>> cdtay(200)

Hình 3 Quỹ đạo chuyển động của tay robot

Để xác định tốc độ và vị trí các khớp, cần tính đợc các giá trị x,y rời rạc củatay Robot sau đó thông qua phơng trình động học ngợc xác định góc quay θ1 và

θ2 Biểu diễn θ1 và θ2 trên đồ thị thời gian, ta đợc đồ thị biểu diễn vị trí của khớp

Đạo hàm chuyển động của từng khớp theo thời gian ta sẽ đợc dạng đờng vận tốccủa khớp

Quỹ đạo chuyển động khớp:

y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy=-2.1332*(t-0.75)+0.5333;

Hình 4 Quỹ đạo chuyển động của các khớp

Chơng trình tính toán và vẽ đồ thị vận tốc chuyển động của khớp:

vx=2.1332*(t-0.75)-0.5333;

y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy=-2.1332*(t-0.75)+0.5333;

ylabel('Van toc goc(do/s)');

Chạy chơng trình với số điểm vẽ N=150 ta đợc:

>>vkhop(150);

Hình 5 Đồ thị vận tốc chuyển động của khớp

Câu 4 Thiết kế bộ điều khiển bù trọng lực cho robot

a Xây dựng phơng trình động lực học

Phơng trình động lực học của hệ thống:

12 11

h h

h h q H

2

2 2

2 2

2 2 2 1

2 2

2 1 2 1

2 1 1 11

cos 6 , 0 2555 , 2

19 , 0 cos

15 , 0 4 , 0 2 15 , 0 4 , 0 5 528 , 0 25 , 0 10

J c l l 2 l l m J l m h

θ

θ+

=

++

++

+

=

++

++

2 2 2 1

2 2 2 12

cos 3 , 0 3025 , 0

19 , 0 cos

15 , 0 4 , 0 15 , 0 5

J c l l l m h

θ

θ+

=

++

=

++

=

2

2 2 2 1

2 2 2 21

cos 3 , 0 3025 , 0

J c l l l m h

θ+

=

++

=

3025 , 0

J l m

2 2 22

l l m

s l l m s

l l m q

, q C

1 2 2 1 2

2 1 2 2 1 2 2

2 2 1 2

θ

θθ

1 m gl c m g l c l c

12 2 2

2 m gl c

g =

b Xây dựng luật điều khiển

Các hệ thống điều khiển có phản hồi nhằm tạo ra khả năng chống nhiễu tốt.Nhng khi cần bám sát quỹ đạo với tốc độ và gia tốc lớn thì chúng không đáp ứng

đợc Cơ chế điều khiển có bù cho phép giảm sai số quỹ đạo

Việc xác định tín hiệu điều khiển sao cho hệ thống ổn định xung quanh

điểm cân bằng dựa trên phơng pháp ổn định Lyapunov

Gọi vector trạng thái của hệ thống là [ T T]T

1 ,

q

với mọi q,ε ≠0, Kp là ma trận chéo xác định dơng

( )ε ( ) ( ) pε

T T

2

1 q q H q , q

Câu 5: Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 radian đến 1 radian

Mô phỏng bằng SIMULINK nh hình vẽ

Hình 10 Mô phỏng hệ bằng Simulink

Hình 11 Kết quả mô phỏng - đáp ứng đầu

H×nh 12 KÕt qu¶ m« pháng, sai lÖch tÜnh cña hÖ

Tµi liÖu tham kh¶o

[1] NguyÔn M¹nh TiÕn “Bµi gi¶ng m«n Robot cao häc n¨m 2002”

[2] Lorenzo Sciavicco, Bruno Sciliano “Modeling and Control of Robot Manipulators”.