bài tập lớn robotics đề 3

Ma trận biến đổi tọa độ độ thuần nhất o A n biểu diễn trạng thái khâu thao tác có thể xác định từ từ cấu trúc động học robot... Các ma trận 31 góc trên bên phải cho 3 phương trình độc l

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

1. Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot

2. Giải bài toán động học

3. Tính toán tĩnh học

4. Tính toán động lực học

5. Chọn quy luật điều khiển thích hợp

1

LỜI NÓI ĐẦU

Ngày nay khi mà nền công nghiệp ngày càng phát triển chúng ta sử dụng robot ngày càng phổ biếntrong nhiều lĩnh vực như cơ khí gia công,công nghệ hàn tự động,robot thám hiểm,

Một trong những robot được biết phát triển,chế tạo sớm vẫn được sử dụng cho đến ngày nay là robot 3bậc tự do RRR Robot 3 bậc tự do RRR được ứng dụng nhiều trong công nghệ hàn tự động trên mặtphẳng.Hiện nay trên trong nước đã ứng dụng robot này trong công nghệ sản xuất tàu thủy,oto

Từ ứng dụng thực tế của robot 3 bậc tự do RRR em đã tìm hiểu và làm bài tập lớn môn “Robotics” với đề tài “Nghiên cứu robot 3 bậc tự do RRR”

Hà Nội, ngày 30 tháng 05 năm 2012

Sinh viên thực hiện

ĐỖ VĂN THẢO

3

DANH MỤC CÁC KÝ HIỆU VÀ CÁC TỪ VIẾT TẮT

θi : Góc quay quanh trục zi-1 để trục xi-1 chuyển đến trục xi theo qui tắc bàn tay phải

di : Khoảng cách Oi-1 và Oi theo trục zi-1.

αi : Góc xoay đưa trục zi-1về zi quanh zi theo quy tắc bàn tay phải

ai : Khoảng dịch chuyển giữa 2 trục khớp động kề nhau

X 3

Y 0

Z 0

X 0 O

HỌC ROBOT

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

1.1.1 Đặt hệ quy chiếu

- Hệ trục tọa độ OX0Y0Z0 đặt tại khâu đế, trục OZ0 có hướng dọc trục khớp động 1, trục

OX0 hướng thẳng đứng, trục OY0 xác định theo quy tắc bàn tay phải

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

Theo DH, tại mỗi khớp ta gắn một hệ trục toạ độ, quy ước về cách đặt hệ toạ

độ này như sau:

ý.

i+1, hướng từ điểm trục của khớp i tới khớp i+1 Nếu hai trục song song thìi

x có thể chọn bất kỳ là đường vuông góc chung hai trục khớp Trong trường

quy tắc bàn tay phải).

- Trục yi được xác định theo xi và zi theo quy tắc bàn tay phải

 : góc giữa 2 đường vuông góc chung Là góc quay quanh trục zi-1 để trục xi-1 chuyển đến trục xi

theo qui tắc bàn tay phải

Hình 1.2 Quy tắc bàn tay phải

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

Danavit-Hartenbeg như sau :

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

Ma trận biến đổi tọa độ thuần nhất Denavit - Hartenberg :

cos cos sin sin sin cos

sin cos cos sin cos sin

Ma trận biến đổi tọa độ độ thuần nhất o A n biểu diễn trạng thái khâu thao tác có thể xác định từ từ cấu trúc động học robot o A n có thể nhận được bằng cách nhân liên tiếp các ma trận biến đổi tọa độthuần nhất ứng với các phép dịch chuyển hệ tọa độ từ hệ trục cố điịnh tới hệ trục tọa độ gắn với

00

10

10

0)1cos(

0)1sin(

0)1sin(

0)1cos(

1 0

d

q q

q q

00

01

00

)2sin(

20)2cos(

)2sin(

)2cos(

20)2sin(

)2cos(

2

q a

q q

00

01

00

)3sin(

30)3cos(

)3sin(

)3cos(

30)3sin(

)3cos(

3

q a

q q

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

0A q  A t (1.4)Trong đó

00

12

30

23

23

)

(

2 23

23 23

2 1 23

1 1

23 1 23

1

2 1 23

1 1

23 1 23

1

3

2 2

1 1

0 3 0

d S a S a C

S

C S a C S a C S

S C

S

C C a C C a S S

C C

C A A A q A

00

),,(),,(),,(

),,(),,(),,(

),,(),,(),,()

(

33 32

31

23 22

21

13 12

11 3

0

ze c

c c

ye c

c c

xe c

c c

t A

Chú ý : Xem kí hiệu viết tắt

Ma trận (1.6) là ma trận cosin chỉ hướng xác định theo một trong các phép quay Yaw, Cardan hoặc Euler

Roll-Pitch-Phương trình (1.4) chính là phương trình động học dạng ma trận của robot Nó bao gồm 16 phương trình vô hướng, trong đó có 4 phương trình tầm thường Các ma trận 31 góc trên bên phải cho 3 phương trình độc lập, biểu diễn về vị trí của điểm thao tác thuộc khâu thao tác.Các ma trận 33 ở góc trên bên trái cho 9 phương trình, mô tả về hướng của khâu thao tác Tuy nhiên do các điều kiện trực dao nên trong 9 phương trình đó chỉ có 3 phương trình độc lập Phương trình (1.4) trên được sử dụng rất nhiều trong các bài toán động học thuận và động học ngược.

9

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

2.1 BÀI TOÁN ĐỘNG HỌC THUẬN

Mục tiêu của bài toán động học thuận là tính toán vị trí và hướng của khâu thao tác dưới dạng

hàm của các biến khớp hay chính là cho biết góc quay của các khớp ta tìm hướng và vị trí của khâu

thao tác so với hệ tọa độ cố định

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

Ta chọn quy luật chuyển động bất kì cho robot theo thời gian như sau :























































2 4 3

7 8 2

12 2 1

2 2 6 3

4 7 4 2

12 3 1

2 2 2

t q

t q

t q

t t q

t t q

t t q











(2.1) Xét phương trình (1.4) thay các giá trị q1,q2,q3 ta có tọa độ của khâu thao tác chính là 3 phần

tử đầu tiên của (1.5) :































































29 0 4

7 4

3 sin

2 80 2

5

1 2

1 1

25 0 1

2 3

4 7

4 3

co s 12

3

28 0

2 5 1 2 11 co s 12 3 s i n 2 5 0 2 3 4 7 4 3 co s 12 3 co s 28 0

2 5

1 2 11

co s 12

3

co s

2 5 0 2

3

2 2

2 23

2 2

2 2

2 1

1

2 2

2 2

2 1

23 1

t t

t t

d S

a S

a z

t t

t t

t t

t t

C S

a C

S a

y

t t

t t

t t

t t

C C

a C

C a

x

E

E

E









(2.2)

Hướng của bàn kẹp có thể được xác định từ các góc Euler, các góc Cardan, các góc Roll –

Pitch – Yaw Ở trong đồ án này tôi sử dụng 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 Chi tiết thực hiện phép quay này có thể tham khảo trong giáo

trình robot công nghiệp Dưới đây chỉ đưa ra công thức chính để thực hiện phép quay này



































) cos(

) cos(

) cos(

sin sin

) sin(

) cos(

)) sin(

) cos(

) cos(

) sin(

) cos(

) cos(

) sin(

) cos(

cos sin

) sin(

) sin(

) sin(

) cos(

) cos(

) sin(

) sin(

) sin(

) sin(

) cos(

) cos(

) cos(



























































cd

R

để tính được các góc α, β, η ta so sánh ma trận chỉ hướng của (1.5) và ma trận chỉ

hướng của (1.6) giải các hệ phương trình ta có :

    

    





 









 



 

arctan

arctan

arctan

arctan

arctan

arctan

arctan

arctan

23

23

12

13 123

11

31

23

23

12

11

23

11

31















C

C

C

C

C

C

C

2.1.2 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]

] 3 , 2 , 1

q     T

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

2 2

1 1

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

) 3 2

( 3

2 1

2 )

3 2

( 1

3

2 1

2 )

3 2

( 1

3

2 23

2 1 2

1 23

1 23

1

2 1 2

1 23

1 23

1

q C a q

q C

a z

V

q S S q

C C a

q q

S S q

C C a

y V

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

23

123

123

1

123

11231

0

1 23

1 23

1 23

1 23 1

1 23 1 23

23 1 23

1

1

23 23 1 23

1

23 23

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

S C q C S C

S

C S S S

C

S C S C

1

10

23

1230

23 23

23 23

S q C q

S q q

C q q

23

) 12 2

( 1

) 12 2

( 1

23 23

23 23

t q

t C

C q

t S

S q

z y

2.1.3 Ứng dụng phần mềm Matlab, Maple vẽ quỹ đạo chuyển động của khâu thao tác cuối

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

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

cuối như sau :

Hình 2.1 Quỹ đạo chuyển động khâu thao tác theo hướng trục X

11

Hình 2.2 Quỹ đạo chuyển động khâu thao tác theo hướng trục Y

Hình 2.3 Quỹ đạo chuyển động khâu thao tác cuối theo hướng Z

Hình 2.4 Quỹ đạo chuyển động của khâu thao tác trong không gian

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ìmgiá 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ầnbiế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

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 E

9

1 12

(2.7)

2.2.2 Khảo sát bài toán động học ngược của robot tìm quy luật chuyển động của các khâu

Ta có phương trình ma trận (xem 1.4):

)()

d S

a S

a

t y

C S a C

S a

t x

C C a C

C a

E E

1 2

3

9 2

3

1

1 2 2

3

2 23

2 1 23

1

2 1 23

1

(2.8)Dựa vào cấu tạo hình học của robot ta xác định được q1 như sau:

13

x,arctan2(y

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 2

23 2 (12 1) 9

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

1 12

d t P

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 trình sau:

P S a S

a

P C a C

a

2 23

2 23

2 3

2 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 :

2 2 3 2

2 a a a C Px Py

322

3

2 2

a a P P

C  x  y  

 (2.11)Mặt khác ta có :

3

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

32,

1artan23

2 2 2 3

a a

a a P P C

P a

C a S S a C

P S S a a

C a C

) 2 3

( 3

3 ) 2 3

(

3 2

3 2

3 2 3

2

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

2 2

3 2

2

3 3

2

2 3 2 2 3

3 3 3

3 2

2 3 2 2 3

3 3

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

(2.14)Vậy ta tính được q2:

2

3 3

3 2

3 3

2 , 2 3 2 2 3

3 3 3

3 2 artan2

2

C a a a

a

S a P C a a P C a a a

a

S a P C a a P

2.2.3 Ứng dụng phần mềm mô phỏng số

Sử dụng phần mềm maple ta có đồ thị biểu diễn các góc khớp quay như sau:

X 3

Y 0

Z 0

X 0 O

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, và mômen M :

Hình 3.1 Sơ đồ tính toán tĩnh học

17

     

0 0

02

10

r 002

2r

0

02

3r

0

1

0

r

00a2r ]

00a3[r M

T

C1 1 T C2

2 T

C3 3 T

1

1

2 2 T

3 3 E3

a d

Mz My Mx FZ

FY FX

F

i

T E

- 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

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

0 3 0

23

23 1

23 1

3

3 3

0 3 0

2 3 2 3 2 3

và 3

3 3

S a

S S a

C C a r

R r

S a

S S a

C C a r R

23 1 23

23 1 23 3 3 2

, 3

3 2 , 3

2 3 ) ( 3

3 3

3 ) 2 3 ( 3

C gC m a C g m Fy a C FzS a Mz

C FzC a FxS a My

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

F g m F F F

z y x

(3.3)

- 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

000

1000

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

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

0 2 0

2

2 1

2 1

2

2 2

0

2

0

2 2 2 2 2 2

và 2

2 2

S a

C S a

C C a r

R r

S a

C S a

C C a r R

1

2 1 2

2 1 2 2 23 2

, 3 0 2 0 0 1 , 2 0 0

3 1

,

2

0

2 2

2

2 2

2 ) 2 2 (

C C g m g m Fy a C FxS a

C FzC a FxS a

C FzS a S g m a g m Fy a M P r F r

M

M

F g m m F F g

m F

g m F

F

F

c

z y x

z y

2 2

1 23

1 23

1 23

1

2 1 2

23 1 23

2 1 3

2 2 23

1 23

2 3

2

3 3

3

2 2

3 2

2 2

2 3

2

3 3

C C g m g m Fy a C

S Fxa C

gC m a C

FyC a

C FxS a

Mz

C FzC a

FxS a

C FzC a

FxS a

My

C FzS a

Fy g

m g m S a C

FzS a

S a g m Fya

Mx M

F

g m m

F

F F

z y

x

gọi hệ trên là (3.6)

- Tính lực và momen của khâu 1 tác dụng lên khâu đế 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ở :

1 0 1 , 2 0 0

, 1 0

~

~

P r F r M

M

P F

F

c (3.7)Trong đó :

0100

0

1 1

1 1

và 0

1

0

2

2 2

0 2

0 1

1 1

0 1

r R r

d r R

2 1 3 2 1

0 0

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

19

3 2

2 1

23 1

3 23

1 23

1 0

, 1 0

2 1

2

2 3 1

23 0

, 1 0

2 1

3 2

2

2 3 1

23 3

0 , 1 0

1 2

3 0

, 1 0

2 2

2 2

3 3

3

1 2

2 3

2

2 1 3

2 1

2

2 2

3 2

3 3

C C

g m

g m

F y a

C S

F x a C

g C m

a C

F y C a

C

F xS a

Mz z

M

F x d

C

F zC a

F xS a

C

F zC a

F x S a

My y

M

g m

m m

F y d

C

F zS a

F y g

m g

m S

a C

F zS a

S a

g m

F y a Mx

x M

F

g m

m m

F F

z y

(3.9)

X 3

Y 0

Z 0

X 0 O

4.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC

21

( ) ( )

T T

q M q q q M q

q q

T T

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

Từ cấu tạo hình học của robot và bảng Denavit-Hartenberg (xem bảng 1.1) ta xác định đượctọa độ khối tâm của các khâu so với hệ tọa độ cố định :

1

1 1

00

100

10001

00

l

l d C S

S C

2 1 2

2 1 2 2

2 2

1 2

1 2 1

1 2 1 2 1

2

2 1

2 1 2

0020

222

S l

C S l

C C l l

a C

S

C S

S C S

S S C C

C S

a

C S a

C C a

23 1 3 2 1

23 1 3 2 1

3

23 23

1 23

1 23

1

1 23 1 23

1

23 2

23 1 2

1

23 1 2

1 3

22

2

0030

32

32

32

S l S a

C S l C S a

C C l C C a

l a C

S

C S

S C

S

S S C C

C S

a S a

C S a C S a

C C a C C a

0 0 0

0 0 0

1 1

3 2

23 1 3 23 1 3 2 1 23

1 3 2 1

23 1 3 23 1 3 2 1 23

1 3 2 1 1

3

20

22

22

C l C

l C a

S S l S

S l S S a C

C l C C a

S C l S

C l S C a C S l C S a q

0

1 1

1 1

1

S C

2

1 2

1 2

1

1 2 1 2 1 2 0

C S

C S

S C

S

S S C C

C R

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

2

1 2

1 2

1

1 2 1 23 1 3 0

C S

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:

000

1000

00

10

1

10

10

100

0

~

1 1

1 1

1 1

1 1 1 1

` 1

q

q q

S q

C

q C q

S C

S

S C R

0 0 1

0 0 0 1

4

10

2

120

02

2

12

12

1

12

121

.0

~

2 2

2 1

2 2

1 2 1 2

1 2

1 2

1

1 2 1 2

1 2

1 2

1

1 1

2 2 1 2

1

2 2 1 2 1 2

2

`

2

S q C q

S q q

C q q

S q C

q

q S C S q S C q S

S q q C C

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

S

C S S S

C

S C S C C R

00

00

2

2 2

S q

1

10

2

120

023

23

123

123

1

123

1231

.0

~

2 2

2 1

23 23

1 23 1 2

1 23

1 23

1

1 23 1 23

1 23

1 23

1

1 1

23 23 1 23

1

23 23

1 23

1 3

3

`

3

S q C q

S q q

C q q

S q C

q

q S C S q S C q S

S q q C C

q C C C q S S q q

S C q C S C

S

C S S S

C

S C

S C

C R

00

00

23

23 3

S q

i Ti Ti Ri i i i Ri i

m gl C

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 :

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ệntạ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ựchiệ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 độngmong 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ựatrê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:

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

10

BÀI T P L N ẬP LỚN ỚN

PHỤ LỤCChương trình tính toán với maple

; Khởi động chương trình và khai báo thư viện sử dụng

> restart:

> with(linalg):

> with(LinearAlgebra);

> with(tensor):

;Khai báo ma trận Denavit-Hartenberg tổng quát

> H := Matrix([[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)], [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)], [0, sin(alpha), cos(alpha), d], [0, 0, 0, 1]]):

; Tính ma trận Denavit-Hartenberg các khâu

;Thế bảng thông số DH cho mỗi khâu vào ma trận H tổng quát

;Ma trận Denavit-Hartenberg của khâu 1

> H1 := subs({a = a1, alpha = 0, d = d1, theta = theta1}, H):

; Ma trận Denavit-Hartenberg của khâu 2:

> H2 := subs({a = a2, alpha = (1/2)*Pi, d = 0, theta = theta2}, H):

> H3 := subs({a = a3, alpha = 0, d = 0, theta = theta3}, H):

II.Xác định vị trí khâu thao tác

> A_1 := Matrix(3, 3); Ac_1 := Matrix(3, 3); for i to 3 do for j to 3 do

A_1[i, j] := evalf(subs({theta1 = theta1(t), theta2 = theta2(t), theta3 = theta3(t)}, D1[i, j]));

Ac_1[i, j] := diff(A_1[i, j], t) end do end do;

> OmegaSong1 := combine(multiply(Ac_1, transpose(A_1)));