Bài tập lớn ROBOTIC docx

Bài toán động học thuận 4.5 Ma trận khối lượng của robot 4.6 Ma trận ly tâm và quán tính coriolits 4.7 Thế năng của robot 4.8 Phương trình vi phân chuyển động của các khâu Chương 5

Trang 1

Xây dựng cấu trúc robot

1.2 Thiết lập phương trình động học robot

Chương 2 BÀI TOÁN ĐỘNG HỌC

2.1 Bài toán động học thuận

4.5 Ma trận khối lượng của robot

4.6 Ma trận ly tâm và quán tính coriolits

4.7 Thế năng của robot

4.8 Phương trình vi phân chuyển động của các khâu

Chương 5 CHỌN BỘ ĐIỀU KHIỂN

Phụ lục Code maple

Trang 2

BTL môn ROBOTICS

2

Chương 1 XÂY DỰNG CẤU TRÚC THIẾT LẬP HỆ PHƯƠNG TRÌNH ĐỘNG HỌC

ROBOT 1.1 XÂY DỰNG CẤU TRÚC ROBOT

1.1.1 Đặt hệ quy chiếu

Hình 1.1 Mô hình robot và hệ trục tọa độ

- Hệ trục tọa độ OX 0 Y 0 Z 0 đặt tại khâu đế, trục OZ 0 có hướng dọc trục khớp động 1, trục

OX 0 nằm trong mặt phẳng vuông góc với OZ o và có hướng từ trên xuống, trục OY 0 xác

định theo quy tắc bàn tay phải

- Hệ trục tọa độ OX 3 Y 3 Z 3 đặt tại khâu thao tác, trục OX 3 hướng theo hướng khâu 3 OZ 3

song song với trục OZ 2 , trục OY 3 xác định theo quy tắc bàn tay phải

Trang 3

BTL môn ROBOTICS

3

1.1.2 Thiết lập bộ thông số Denavit-Hartenbeg

Từ mô hình và hệ trục tọa độ ở trên ta xây dựng được bảng thông số

θi là góc quay quanh Zi-1 đển biến Xi-1 thành Xi

αi là góc quay quanh Xi để biến Zi-1 thành Zi

Các biến khớp là θ1, θ2, θ3, đặt các biến khớp tương ứng là q1,q2,q3

Các ma trận biến đổi tọa độ thuần nhất Denavit-Hartenbeg dựa vào bộ thông số

00

01

0

)sin(

)cos(

0)sin(

)cos(

)sin(

0)cos(

1

1 1 1 1

1 0

d

q a q q

00

01

00

)sin(

0)cos(

)sin(

)cos(

0)sin(

)cos(

2 2 2

2

2 2 2

2 2

q a q

00

01

00

)sin(

0)cos(

)sin(

)cos(

0)sin(

)cos(

3 3 3

3

3 3 3

3 3

q a q

q

A (1.3)

Bảng 1.1: Bộ thông số Denavit-Hartenbeg

Trang 4

BTL môn ROBOTICS

4

1.2 THIẾT LẬP PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT

Phương trình động học robot nhận được trong dạng ma trận như sau :

)()

3 0

t A q

0 0

0

)

(

1 2 2 23 3 23

23

1 1 2 1 2 23 1 3 1 23

1 23

1

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

1 3

2 2

1 1

0

3

0

d S a S a C

S

S a C S a C S a C S

S C

S

C a C C a C C a S S

C C

C A A A

0 0

) , , ( )

, , ( )

, , (

) , , ( )

, , ( )

, , (

) , , ( )

, , ( )

(

33 32

31

23 22

21

13 12

11 3

0

ze c

c c

ye c

c c

xe c

c c

)cos(

sinsin

)sin(

)cos(

))sin(

)cos(

)sin(

)

cos(

)cos(

)sin(

)cos(

cossin

)sin(

)cos(

)sin(

)

sin(

)sin(

)cos(

0 0

) , , ( ) , , ( )

,

(

) , , ( ) , , ( )

,

(

) , , ( ) , , ( )

,

(

33 32

31

23 22

21

13 12

11

ze c

c

ye c

c

xe c

0 0

23 23

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

d S a S a C

S

S a C S a C S a C S S C S

C a C C a C C a S S C C C

(1.6)

Trang 5

BTL môn ROBOTICS

5

Chương 2 BÀI TOÁN ĐỘNG HỌC 2.1 BÀI TOÁN ĐỘNG HỌC THUẬN

 Xây dựng quy luật chuyển động, vị trí khâu thao tác và ma trận chỉ hướng

Chọn thông số chiều dài các khâu như sau:

13

23

22

12

11

8

13

3

122

4

11

2 2 2

t q

t t q

Trang 7

BTL môn ROBOTICS

7

Từ phương trình 1.6 ta có :(2.2)

Thay các giá trị của biến vào ta có:

Hướng của bàn kẹp có thể được xác định từ các góc Cardan, ký hiệu tương ứng là α, β, γ quay lần lượt quanh các trục x-y-z

)cos(

sinsin

)sin(

)cos(

))sin(

)cos(

)sin(

)

cos(

)cos(

)sin(

)cos(

cossin

)sin(

)cos(

)sin(

)

sin(

)sin(

)cos(

3

1 1 2 1 2 23

1

3

1 1 2 1 2 23

1

3

d S a S

a

z

S a C S a C

S

a

y

C a C C a C

Trang 8

arctan ,

, ,

,

, , arctan

arctan ,

,

, , arctan

arctan ,

,

, , arctan

2

3 , 2 khi

arctan ,

, ,

,

, , arctan

arctan ,

,

, , arctan

arctan ,

,

, , arctan

2 23 1 2 23 1

23 2

12 2 11

13

23 1

23 1 11

21

1 1 31

32

2 23 1 2 23 1

23 2

12 2 11

13

23 1

23 1 11

21

1 1 31

C

S c

c c

C C

S C c

c

S

C c

c

C S S

C

S c

c c

C C

S C c

c

S

C c

c

 Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác

Từ phần trên ta đã xây dựng đƣợc quy luật chuyển cũng nhƣ tìm đƣợc tọa độ của

khâu thao tác cuối, các biến khớp và đạo hàm các cấp theo t đã biết :

T

q q q

q[ 1, 2, 3]

],,[q1 q2 q3

q    TVận tốc góc của khâu thao tác:

E

E r R A A

A (2.3) Vận tốc của khâu thao tác chính là đạo hàm vị trí khâu thao tác theo thời gian:

VE=r E = T

E E

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

)(

q C a q q C a z V

q C a q S S q C C a q q S S q C C a y V

q S a q S C q C S a q q S C q C S a x V

E Ez

E Ey

E Ex

Vận tốc góc của khâu thao tác:

0

~

x y

x z

y z T

E E

Trang 9

1 1 23 23 1 1 23 1 23

23 1 23 1 1

1 1 23 1 23 23 1 1 23 23 1 1 23 1

q S q

C

q S q C S q S C q

S S C C

q

q C C C q S S q q S C q C S

23 23 1 23 1

C S

C S S S

C

S C S C

0

)(

0

1 3 1 2 1 3 1 2

1 3 2 1

S q S q C q C q

S q q q

C q q q

2 1

3 2

2 1

3 2

2

11

)4

1cos(

)12

113()(

)4

1sin(

)12

113()(

t q

t t t C

q q

t t t S

q q

z

y x

Quỹ đạo điểm khâu thao tác

Sử dụng phần mềm Maple ta vẽ đƣợc đồ thị quỹ đạo chuyển động của khâu thao

tác cuối nhƣ sau :

Chuyển động điểm cuối E theo phương X

Trang 10

BTL môn ROBOTICS

10

Chuyển động điểm cuối E theo phương Y

Chuyển động điểm cuối theo phương Z

2.2 BÀI TOÁN ĐỘNG HỌC NGƯỢC

Bài toán động học ngược thông thường cho biết trước vị trí của khâu thao tác yêu

cầu tìm giá trị các biến khớp ứng với vị trí đó Ở tiểu luận này robot 3 bậc tự do kiểu RRR

ta không cần biết hướng của khâu thao tác mà vẫn có thể tìm được các góc quay tương

ứng

Trang 11

BTL môn ROBOTICS

11

2.2.1 Xây dựng quy luật chuyển động của khâu thao tác cuối

Ta chọn quy luật chuyển động bất kì của khâu thao tác E của robot như sau:

t y

t x

E

30100100

60600

3 0

t A q

100

60600

1 2 2 23 3

1 1 2 1 2 23 1 3

t z

d S a S a

t y

S a C S a C S a

t x

C a C C a C C a

E E

Y q

X

Y q

E E E E

60600

100arctan

arctan

)tan(

1 1

Nhân phương trình 1 với C1 và phương trình 2 với S1 ta được phương trình:

1 1

1 2 2 23

3C a C a (600 60t)C 100tS

a      (2.9) Đặt:

30100

100)

60600(

d t P

a tS C

t P

y x

Kết hợp với phương trình 3 của hệ (2.8) và phương trình (2.9) ta được hệ phương

P C a C a

2 2 23 3

(2.10)

Bình phương hai vế của hai phương trình (2.10) sau đó cộng hai phương trình lại

với nhau ta được phương trình :

Trang 12

BTL môn ROBOTICS

12

2 2 3 3 2 2 3 2 2

2 2 2 2 3 2 3 2 2 3 2 2 3 2 3 2 2 3 2 2

2 2 2 23 2 23 3 2 2 3 2 2

2

)(

2

)(

2

y x

P P C a a a a

P P S C S S C S C S S C C C a a a a

P P S S C C a a a a

2 3 2 2 2 2 3

2 a a

a a P P

S  Vậy ta tính được q3:

32(

1artan

2 2 2

3 3

a a

a a P P C

P S S a a C a C

)(

2 3 3 2 3 3 2

3 2 3 2 3 3 2

(2.13) Giải hệ phương trình trên ta có nghiệm như sau:

3 3 3

3 2 2

3 2 3 2 2 2 3

3 3 3

3 2 2

2

C a a a

a

S a P C a a P S

C a a a a

S a P C a a P C

x y

y x

3 3 3

3 2 3

2 3 2 2 2 3

3 3 3

3 2 2

22

artan

C a a a a

S a P C a a P C

a a a a

S a P C a a P

Trang 13

BTL môn ROBOTICS

13

TÍNH TOÁN TĨNH HỌC 3.1 Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh

Theo đầu bài ta có các lực tác dụng vào khâu thao tác tại điểm E gồm các vector lực FE3,

002r

0

02r

0

r

00ar ]

00a[r M

T

1 C1

1 T 2

C2 2 T

3 C3

3 T

1

T 2

2 2 T

3 3 3 T E3

a a

M M M F

Z Y X

 Tính lực và momen của khâu 3 tác dụng lên khâu 2 tại khớp 3

Hệ phương trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :

P r F r M

M

P F

F

c E

E

Trang 14

3 3 0 3

3 0 2

,

3

0

3 3

g m F

F r

Mz My

Mx M

F

g m F

F g

m F

F

F F

c z

y x z y x z

y x

23 1 3

3 3 3 0 3 0

23 3

23 1 3

23 1 3 3 3 3 0 3 0

22

2

và

S a

C S a

C C a

r R r S

a

S S a

C C a r R

1 23

1

1 23 1 23

1

C S

S C

S

S S C C

0 2

2

0 2

2 2

0

0 0

0

~

~ 0

0

3

23 1 3 23 1 3

23 1 3 23

3

23 1 3 23

3

3 23

1 3 23 1 3

23 1 3 23

3

23 1 3 23 3

3 0 3 0 2 , 3 0 3 0 3

C C

a C S a

C C

a S

a

C S

a S

a

F

g m F F

C C a S S a

C C a S

a

S S a S a

M

P r F r M

M

F

g m F

F g

m F F

F F

z y x

z

y

x

c E

z y x

23 1 3

23 1 3 23 3

23 3 3 23 1 3 23 3 3

2

,

3

3 2

,

3

0

2)

(

2

1)

(

C gC m

a C C g m Fy a C FzS a Mz

C FzS a FxS a My

gS m a C

FzS a S g m Fy a Mx

M

F

g m F

F

z y

x

(3.3)

Trang 15

BTL môn ROBOTICS

15

 Tính lực và momen của khâu 2 tác dụng lên khâu 1 tại khớp 2

Hệ phương trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :

P r F r M

M

P F

00

01000

2 2

1 2 1 2 1

1 2 1 2 1 2

2

2 2 1 1

1 1

2 1 1 0 2 0

C S

S C S

S S C C C C

S

S C C S

S C

R R R

2 1 2

2 2 2 0 2 0

2 2

2 1 2

2 1 2 2 2 2

0

2

0

22

2

và

S a

C S a

C C a

r R r

S a

C S a

C C a r R

2 1 2 2 2 23 1 3 23 2

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

,

2

0

2 3

1 ) (

2

1 )

( 2

1

C gC m a C C g m g m Fy a C S a F C gC m a g m F C gC m a C FyC a C FxS a

Mz

C FzC a FxS a C FzC a FxS a My

g m S a C FzS a g m g m F S a g m S a C FzS a S g m F a Mx M

F

g m m

F

x y

y y

z

y

x

 Tính lực và momen của khâu 1 tác dụng lên khâu 0 đế tại khớp 1

Hệ phương trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :

0 0 , 1

0 1

0 1 , 2

0 0

P r F

r M

M

P F

F

c

(3.7)

Trong đó :

Trang 16

BTL môn ROBOTICS

16

010

0

1 1

2

và0

1 1 1 1

2 2 2 0 2 0 1

1

1 1 1 1 1 0 1 0

S a C a

r R r S

a

C a r R

2

1321

00

1 , 2 0 0

,

1

0

1 2 3 1

2 3 0

,

1

0

Fx d

g

m m m Fy d M

M

F

g m m m F

F g

m F

g m m F

F F

z y

x z

y x

2 1 23

1 3 23

1 23

1 0

,

1

0

2 1 2

23 1 23

3 2 2 23

1 23

3 0

,

1

0

1 2 3 0

,

1

0

22

33

3

12

23

2

1321

2

223

2

33

C C g m g m Fy a

C S Fxa C

gC m

a C FyC a C FxS a Mz z

M

Fx d C FzC a FxS a C FzC a FxS a My y

M

g

m m m Fy d C FzS a

Fy g m g m S a C FzS a S

a g m Fya Mx

x

M

F

g m m m F

F F

z y

x

(3.9)

Trang 17

BTL môn ROBOTICS

17

Chương 4 TÍNH TOÁN ĐỘNG LỰC HỌC 4.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC

Vì các khâu coi như thanh đồng chất tiết diện ngang không đáng kể nên ta có trọng tâm

mỗi khâu nằm tại trung điểm của nó

Trang 18

n n

b I q

q M q q q M q

q q

T T

Trang 19

n n

( )( ) M q ( n )

Trang 20

BTL môn ROBOTICS

20

4.3 Ma trận Jacobi của các khâu

Tạo độ trọng tâm của khâu i trong hệ tọa độ 0 tính nhƣ sau :

i ci

i i

ci r R r

Với 0r ci là tọa độ trọng tâm khâu i trong hệ tọa độ i

i r ci là tọa độ trọng tâm khâu i trong hệ tọa độ i

0R i là ma trận quay biến đổi hệ 0 thành hệ i

0r i là tọa độ của gốc tọa độ i trong hệ tọa độ 0

Ta có các ma trận tọa độ trọng tâm của các khâu nhƣ sau :

1 1

1

1 1

1

0

) 1 sin(

2

) cos(

2 0

0 2 0 1 0 0

0 )

sin(

.

) cos(

.

d q a q a a

C S

S C

d

q a

1 1 2 1 2 2

2 2

1 2 1 2 1

1 2 2

1 1 2 1 2

2

0

2

2 0

0 2

C S l

C a C C a a

C S

C S S C S

S S C C C

d S a

S a C S a

C a C C a

1 1 23 1 3 2 1 2

1 1 2 1 2 23 1 3 3

23 23

1 23

1 23 1

1 23 1 23 1

1 23 3 2 2

1 1 23 1 3 2 1

2

1 1 23 1 3 2 1

2

3

0

222

0

020

d S a S a

S a C S a C S a

C a C C q C C a a

C S

S C

S

S S C C

C d

S a S a

S a C S a C

S

a

C a C C a C

002

1 1 1 1

1

S a

10

02

12

1

02

12

1

2 2

2 1 2 1

1 2 1 2

2 1 2 1

1 2 1 2

1 2

C a

S S a a

C C C a

S C a a

S C S a

q

r

Trang 21

3 2 2

23 1 3 23

1 3 2 1 2 1

1 23 1 3 2 1 2

23 1 3 23

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

1

3

2

1 2

1 0

2

1 2

1

C a C

a C a

S S a S

S a S S a a C C C a C C a

S C a S

C a S C a a C C S a C S a

q

r

Cũng từ các ma trận Denanvit- Hartenberg ta xác định được các ma trận cosin chỉ hưởng

của các khâu (xem 1.1, 1.5, 3.5) :

Ma trận cosin chỉ hướng của khâu 1 :

0

1 1

1 0

C S

S C

1 2 1 2 1

1 2 1 2 1 2 0

C S

S C S

S S C C C R

Ma trận cosin chỉ hướng của khâu 3:

1 23 1 23 1

1 23 1 23 1 3 0

C S

S C

S

S S C C

C R

Toán tử sóng của vector vận tốc góc của khâu 1:

00

~

1

1 1

1

`

q R

000

0001

0

~

1 2 1 2

1 2 1

1 2 1 2

2

` 2

S q C q

S q q

C q q R

Trang 22

00

1

1 2

S q

0

)(

0

~

1 3 1 2 1 3 1 2

1 3 2 1

3 3

` 3

S q S q C q C q

S q q q

C q q q

0

1 1

1 1 3

S S q

Trang 23

,

(

c c

c q

Trang 24

g g g q g

Trong đó :

4.8 Phuơng trình vi phân chuyển động của các khâu

Thế các biểu thức vào phương trình Lagrange loại hai :

M q qC q q qg q 

Ta nhận được hệ phương trình vi phân chuyển động của robot ba khâu trong không gian :

Khâu 1 :

Trang 25

q q d

i

i i

Trang 26

BTL môn ROBOTICS

26

Chương 5 CHỌN BỘ ĐIỀU KHIỂN

Nhiêm vụ của bài toán điều khiển là tìm ra quy luật của lực/ mô men do các động

cơ điện tạo ra tác dụng lên các khâu để đảm bảo robot chạy đúng theo quy luật qd(t) cho

trước, nhằm thực hiện một số nhiệm vụ nào đó Trên cơ sở chuyển động mong muốn qd(t)

được định nghĩa trước và chuyển động hiện tại của robot được đo bởi các cảm biến đặt tại

khớp, bộ điều khiển có nhiệm vụ đưa ra các lực/mômen cần thiết các lực/mômen này tác

động làm cho robot thực hiện chuyển động mong muốn một cách ổn định và chính xác Sơ

đồ khối của bộ điều khiển cho robot có dạng như hình 5.1

d

 _

Để có được luật điều khiển đáp ứng các yêu cầu vừa nêu, thông thường luật điều

khiển dựa trên động lực học ngược được sử dụng Với luật điều khiển này lực/mô men của

các bộ phận dẫn động được tính như sau:

( ) ( ) ( )

M q q C q q q g q

Giả thiết thành phần mômen trọng lực G(q) được bù hoàn toàn, sơ đồ hệ thống điều

khiển phản hồi với cấu trúc điều khiển PD có dạng đơn giản như sau:

Trang 27

Tín hiệu đặt vị trí qd được so sánh với vị trí thực của khớp q, sai lệch được đặt vào

khâu khuếch đại với hệ số Kp Tín hiệu ra của khâu tỉ lệ được cộng đại số với tín hiệu tỉ lệ

với tốc độ của khớp và đặt cơ cấu chấp hành của robot:

q H q là động năng của robot KP và H là các ma trận

hệ số dương; nên hàm VL > 0 với q q d

Tính đạo hàm cấp 1 hàm VL ta được:

K p

RB

Trang 28

V  K   q Hqq  C q q q (5.7)

Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển (4.1),

Phương trình (4.7) được biến đổi nhhuw sau:

Bất đẳng thức trên cho thấy rằng hệ thông ổn định tuyệt đối mức độ dương của VL phụ

thuộc vào Kp và mức độ âm của VL phụ thuộc vào KD Do đó tăng tốc độ hội tụ bằng cách

tăng giá trị ma trận KD

Định dạng
Số trang	28
Dung lượng	2,47 MB