robot cong nghiep chuong 4

1

ChươngChương 4 4 -- Robot Robot côngcông nghinghiệệpp TS TS PhanPhan TTấấnn TTùùngng

ChươngChương 4 4 ĐĐộộngng hhọọcc vvịị trtríí Robot Robot

1. Bài toán động học (vị trí) thuận (forward kinematics)

• Tay máy là 1 chuổi động hở bao gồm các khâu và khớp

• Các thông số không thay đổi giá trị khi tay máy hoạt động gọi là THAM SỐ ( Vd: chiều dài các khâu )

• Các thông số thay đổi giá trị khi tay máy hoạt động gọi là BIẾN KHỚP(Vd: góc hợp bởi 2 khâu tại 1 khớp)

• Các khớp thường dùng trong tay máy là KHỚP TRƯỢT và KHỚP QUAY(là khớp loại 5)

• Bài toán thuận mô tà vị trí và hướng của điểm End Effector dưới dạnghàm số của các biến khớp

2


Hàm số mô tả vị trí của điểm End Effector

Với

Vd1: Cơ cấu tay máy như hình bên

pqTp nn ).(00 =

⎥⎦

⎤⎢⎣

⎡=

1000)(

00000 pwvuqT

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++−

=

10000100

00

)( 2112121

2112121

21

21

0 θθθθθθθ

θθθθθθθ

SaSaCSCaCaSC

qT

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++−

==

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

10

1000

.

10000100

00

).(

1

211

211

2112121

2112121

21

21

21

21

22

00 θθθ

θθθ

θθθθθθθ

θθθθθθθ

SaSaCaCa

SaSaCSCaCaSC

pqTzyx

pE

E

E

3

ChươngChương 4 4 -- Robot Robot côngcông nghinghiệệpp TS TS PhanPhan TTấấnn TTùùngng1.1 Qui tắc Denavit Hartenberg

Hệ toạ độ gắn lên các khâu như sau:

• Trục Zi đặt dọc theo trục khớp i+1

• Trục Xi đặt dọc theo phương pháp tuyến chung giữa Zi-1 và Zi, hướngtừ khớp i đến khớp i+1

• Trục Yi vuông góc với Xi và Yi theo qui tắc bàn tay phải

• Gốc toạ độ Oi là giao của trục Zi và pháp tuyến chung của trục Zi-1 và Zi

• Gốc toạ độ Oi’ là giao của trục Zi-1 và pháp tuyến chung của trục Zi-1 vàZi

Các thông số Denavit Hartenberg

• Khoảng cách giữa 2 khớp liên tiếp theo phương Xi là ai (tham số)

• Khoảng cách giữa 2 khớp liên tiếp theo phương Zi-1 là di (tham số hoặc biến khớp)

• Góc quay quanh trục Xi giữa trục Zi-1 và trục Zi là αI (tham số)

• Góc quay quanh trục Zi-1 giữa trục Xi-1 và trục Xi là θI (tham số hoặc biến khớp)

4


Định nghĩa hệ toạ độ và các thông số Denavit Hartenberg

5


⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=−

1000100

0000

int1

i

ii

ii

i

dCSSC

A θθ

θθ

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

=

10000000

001

int

ii

ii

i

i CSSC

a

Aαα

αα

Xác định ma trận chuyển đổi từ hệ toạ độ thứ i vế hệ toạ độ thứ i-1

Ma trận chuyển đổi từ hệ toạ độ thứ i về hệ toạ độ trung gian int. Xoayquanh trục Xint góc αI sau đó tịnh tiến theo trục Xint đoạn ai .

Ma trận chuyển đổi từ hệ trung gian int về hệ i-1. Xoay quanh trục Zi gócθi sau đó tịnh tiến theo trục Zint đoạn di .

6


⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

−

== −−

10000

intint

11

iii

iiiiiii

iiiiiii

ii

ii

dCSSaSCCCSCaSSCSC

AAAαα

θαθαθθ

θαθαθθ

Vậy ma trận chuyển đổi từ hệ i về hệ i-1 là

Tóm lại tại khâu thứ i ta có ma trận chuyển đổi từ hệ thứ i về hệ i-1 nhưtrên với các thông số Denavit Hartenberg được xác định trong bảngthông số DH như sau

7


Phương trình động học thuận (vị trí) của tay máy là

pAAAApTp nn

ni

inn

112

11

000 ...... −−==Vd2: Xét cơ cấu tay máy như hình bên

Bảng thông số DH của tay máy

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

10000100

00

222

222

2

2

21 θθθ

θθθ

SaCSCaSC

A

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

10000100

00

111

111

1

1

10 θθθ

θθθ

SaCSCaSC

A

Ma trận chuyển đổi từ hệ 2 về hệ 1 và từ hệ 1 về hệ 0

8


( ) ( )( )

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++−++−+−−

==

10000100

00

1212121212121

1212121212121

12

12

21

10

20 θθθθθθθθθθθθθ

θθθθθθθθθθθθθ

SaSCCSaSSCCSCCSCaSSCCaCSSCSSCC

AAA

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++−

==

10000100

00

1212121

1212121

12

12

20

20 θθθθθθθ

θθθθθθθ

SaSaCSCaCaSC

AT

Vậy ma trận chuyển đổi từ hệ toạ độ 2 về hệ toạ độ 0 là

Phương trình đông học (vị trí) của tay máy

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++−

==

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

10

1000

.

10000100

00

.

1

211

211

2112121

2112121

21

21

21

21

22

00 θθθ

θθθ

θθθθθθθ

θθθθθθθ

SaSaCaCa

SaSaCSCaCaSC

pTzyx

pE

E

E

9


Các bước khi xác định ma trận chuyển đổi theo phương pháp DenavitHartenberg

Qui ước khâu thứ i nằm giữa khớp thứ i và i+1

Bước 1: xác định các đường tâm trục các khớp

10


Bước 2: xác định kích thước của pháp tuyến chung giữa đường tâm trụckhớp thứ i và i+1 và gọi là ai.

11


Nếu 2 trục khớp thứ i và i +1 song song thì có thể chọn ai bất kỳ. Nhưngtốt nhất nên chọn ai giao với ai-1

12


Nếu đường tâm 2 trục khớp thứ i và i+1 cắt nhau thì đoạn ai trở thành 1 điểm và có pháp tuyến chung vuông góc với trục khớp i và i +1

13


Bước 3: xác định gốc cuẩ hệ toạ độ Oi là giao của đường ai và trục khớpthứ i +1. Gốc của hệ toạ độ trung gian Hi là giao của đường ai và trụckhớp i.

14


Nếu trục khớp i và i+1 cắt nhau thì Hi ≡ Oi

15


Với khớp 1 (khớp nối giá cố định) thì chọn O0 ≡ H1

16


Bước 4: Xác định hệ toạ độ đầu tiên (hệ 0) đặt tại khớp 1 của khâu 1. Chọn trục Z0 của hệ toạ độ đầu tiên có phương trùng với trục khớp 1

17


Chọn trục X0 của hệ toạ độ đầu tiên theo phương pháp tuyến chunghướng từ điểm H1 đến điểm O1

18


Chọn trục Y0 của hệ toạ độ đầu tiên hợp với trục X0 và Z0 vừa chọn theoquy tắc bàn tay phải

19


Bước 5: Xác định hệ toạ độ thứ i của khâu thứ i (đặt tại khớp i+1). Chọntrục Zi của hệ toạ độ thứ i có phương trùng với trục khớp i+1

20


Chọn trục Xi của hệ toạ độ thứ i theo phương pháp tuyến chung hướngtừ điểm Hi đến điểm Oi

21


Nếu đường tâm 2 trục khớp thứ i và i+1 cắt nhau thì trục Xi có phươngvuông góc trục khớp i và khớp i+1và tốt nhất là hợp với trục Xi-1 một gócnhọn

22


Chọn trục Yi của hệ toạ độ thứ i hợp với trục Xi và Zi vừa chọn theo quytắc bàn tay phải

23


Bước 6: xác định hệ toạ độ trung gian của khâu thứ i (đặt tại khớp thứ i). Hệ toạ độ trung gian có gốc toạ độ là điểm Hi, trục Xint ≡ Xi, trục Zint ≡ Zi-1, trục Yint hợp với trục Xint và Zint theo qui tắc bàn tay phải. Góc αI là góchợp bởi trục Zi-1 và Zi có chiều dương là chiều dương quay quanh trục Xi từ Zi-1 đếnZi

24


Góc θi hợp bởi trục Xi-1 và trục Xi. Chiều dương là chiều dương quay quanh trục Zi-1 từXi-1 đến trục Xi.

25


Định nghĩa kích thước ai: iii OHa =

26


Định nghĩa kích thước di:1−= iii OHd

27


1.2 Không gian hoạt động của robot

• Không gian hoạt động của robot (workspace) là vùng mà điểm tác độngcuối có thể di chuyển đến được.

• Lưu ý cần phân biệt:

• Không gian có thể với đến được (reachable workspace) là vùng mà điểmtác động cuối có thể di chuyển đến được mà khôngquan tâm đến hướng.

• Không gian làm việc có tính đến hướng (dexterous workspace) là tậpcon của không gian có thể với đến được (reachable workspace).

28


2. Phương trình động học của các robot điển hình

2.1 Robot Stanford

Robot Stanford có 6 bậc tự do. Robot có 6 khớp ( 5 khớp xoay và 1 khớp trượt ).

29


30


31


32


33


2.2 Robot Elbow

34


35


36


37


38


2.3 Robot Puma

39


40


41


42


3. Bài toán động học ngược (inverse kinematics )

• Bài toán động học thuận tay máy cho phép xác định vị trí và hướng củaphần công tác (điểm tác động cuối hay end effector) theo các biến khớp.

• Bài toán động học ngược tay máy cho phép xác định các biến khớptheo vị trí và hướng của phần công tác.

• Bài toán ngược có thể không có lời giải, có nhiều lời giải và có thể cólời giải bằng toán học nhưng không thực hiện được trong thực tế vì lý do kết cấu không cho phép thực hiện.

43


3.1 Cách giải trực tiếp

Giả sử điểm E có toạ độ (xE yE), chiều dài các khâu là a1, a2 và các biếnkhớp là θ1, θ2 . Ta có hệ phương trình sau:

)cos(.cos. 21211 θθθ ++= aaxE

)sin(.sin. 21211 θθθ ++= aayE

44


Bình phương 2 vế rồi cộng lại (nhớ áp dụng công thức cộng) ta có:

21

22

21

22

2 ..2cos

aaaayx EE −−+

=θVì

22

2 cos1sin θθ −±=Nên

)cos,(sin2tan 222 θθθ A=Và ta có

2222221

1sin)cos(sin

EE

EE

yxxayaa

+−+

=θθθ

2222221

1sin)cos(cos

EE

EE

yxyaxaa

+++

=θθθ

)cos,(sin2tan 111 θθθ A=

45


3.2 Trường hợp robot n bậc tự do

Ma trận chuyển hệ toạ độ từ hệ n về hệ 0 là

nn

n AAAT 12

11

00 .... −=Ta lập được hệ phương trình: Nếu 6 bậc tự do:

nn

nn

nn

nn

nn

TTA

TTA

TTA

1211

2

2112

1

1011

0

.....................−−−

−−

−

−

=

=

=

65

651

54

62

611

21

61

601

10

.....................

TTA

TTA

TTA

=

=

=

−

−

−

Dựa vào các phương trình trên ta giải ra các biến khớp

46


Ví dụ cơ cấu 2 bậc tự do

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

10000100

00

222

222

2

2

21 θθθ

θθθ

SaCSCaSC

A

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

10000100

00

111

111

1

1

10 θθθ

θθθ

SaCSCaSC

A

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

==

10000100

00

1000

2121

2121

20

20 E

E

Ezzz

Eyyy

Exxx

yCSxSC

zwvuywvuxwvu

AT θθθθ

θθθθ

47


⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

−

=−

1000010000

0

11

11 1

11

0 θθ

θθ

CSaSC

A ⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −==−

1000

1

M

LLLMLLL

M qRRTT

ATB

ATB

A

AB

BA

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

−

=−

10000100

00

.

1000010000

0

2121

2121

11

11 1

201

10 E

E

yCSxSC

CSaSC

TA θθθθ

θθθθ

θθ

θθ

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡+−

−+−

=−

10000100

00

1122

1122 1

201

10 θθθθ

θθθθ

CySxCSaSyCxSC

TA EE

EE

48


Vì2

12

011

0 ATA =−

Đồng nhất hoá 2 vế ta có

21

22

21

22

2 ..2cos

aaaayx EE −−+

=θ 22

2 cos1sin θθ −±=


2222221

1sin)cos(sin

EE

EE

yxxayaa

+−+

=θθθ

2222221

1sin)cos(cos

EE

EE

yxyaxaa

+++

=θθθ


49


• Hệ phương trình động học ngược của robot Elbow

Xem sách Robot công nghiệp của GS TSKH Nguyễn Thiện Phúc trang126 - 130

• Hệ phương trình động học ngược của robot Stanford

Xem sách Robot công nghiệp của GS TSKH Nguyễn Thiện Phúc trang130 - 134

HẾT CHƯƠNG 4

robot cong nghiep chuong 4

Documents