Bài giảng Động học vị trí Robot

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

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

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ạng hàm số của các biến khớp

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

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

(

n ).

0 = 0 qTp

Với

0 qT )(

u 0

v 0

w 0

p 1

⎡ ⎢ ⎣

⎤ ⎥ ⎦

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

θ 1

θθ 21

S − θθ 21 C

Ca 1 Sa 1

θ 1

θθ 21

0 qT )(

θθ 21 0 0

0 0 1 0

Ca + 2 Sa + 2 0 1

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

C ⎡ θθ 21 ⎢ S θθ ⎢ 21 0 ⎢ ⎢ 0 ⎣

θ 1

θθ 21

θ 1

θθ 21

x y

S − θθ 21 C

0 0

Ca 1 Sa 1

Ca 2 Sa 2

Ca 1 Sa 1

Ca 2 Sa 2

θ 1

θθ 21

θ 1

θθ 21

2 ).

qT ( 2

z E 1

θθ 21 0 0

1 0

+ + 0 1

0 1

+ + 0 1

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎤ ⎥ ⎢ ⎥ ⎢ . ⎥ ⎢ ⎥ ⎢ ⎣ ⎦

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

C ⎡ θθ 21 ⎢ S θθ ⎢ 21 0 ⎢ ⎢ 0 ⎣

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

1.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ướng

từ 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 nhỏ nhất 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 nhỏ nhất 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 (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 (biến khớp)

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

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

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

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. Xoay quanh trục Xint góc αI sau đó tịnh tiến theo trục Xint đoạn ai .

int

A i

1 0 0 0

0 C i α S i α 0

0 S − i α C i α 0

a i 0 0 1

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

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 .

S i θ

− C

1 −

int

i θ S i θ 0 0

i θ 0 0

1 0

d i 1

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

C ⎡ ⎢ ⎢ ⎢ ⎢ ⎣

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

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

i θ

−

i θ

int

1 −

1 i = − A

A i

int

SS i i αθ SC i i αθ C

Ca i Sa i d

CS − i i αθ CC i i αθ S i α 0

i α 0

i 1

C ⎡ i θ ⎢ S i θ ⎢ 0 ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

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ảng thông số DH như sau

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

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

1 −

AApTp

i ...

...

A

=

pA n

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

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

θ 2

θ 1

A 2

A 1

S − θ 1 C θ 1 0

0 0 1

Ca 1 Sa 1 θ 1 0

S − θ 2 C θ 2 0

0 0 1

Ca 2 Sa 2 θ 2 0

C ⎡ θ 2 ⎢ S θ ⎢ 2 0 ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

C ⎡ θ 1 ⎢ S θ ⎢ 1 0 ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

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

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

−

)

θθ 2

θ 1

( CCa 2 ( CSa

) )

Ca 1 Sa 1

CC 1 CS θθ 1 2

SS θθ 1 2 SC 1

θθ 2

( SC 1 CC 1

θθ 2

CS + θθ 1 2 SS θθ 1 2

θθ 2

SS θθ 1 2 SC 1

θ 1

A 2

0 1 AA 2 1

+ 0

θθ 2 0

− 0

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

S θθ 1 2

θ 1

θθ 2 1

− C

0 Ca 2 Sa 2

Ca 1 Sa 1

θ 1

0 T 2

A 2

θθ 2 1 S θθ 1 2 0 0

θθ 1 2 0 0

1 0

+ θθ 2 1 0 1

C ⎡ ⎢ ⎢ ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

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

θ 1

θθ 21

θ 1

θθ 21

0 0

x y

0 0

S − θθ 21 C

Ca 1 Sa 1

Ca 2 Sa 2

Ca 1 Sa 1

Ca 2 Sa 2

θ 1

θθ 21

θ 1

θθ 21

2 . pT 2

z E 1

θθ 21 0 0

1 0

+ + 0 1

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

C ⎡ θθ 21 ⎢ S θθ ⎢ 21 0 ⎢ ⎢ 0 ⎣

⎡ ⎤ ⎥ ⎢ ⎥ ⎢ . 0 ⎥ ⎢ ⎥ ⎢ 1 ⎣ ⎦

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

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

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

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

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

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

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

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

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

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 có phương vuông góc với trục khớp i và i +1

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

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

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

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

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

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

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

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

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

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

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

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 theo quy tắc bàn tay phải

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

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

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

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

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

Nếu đường tâm 2 trục khớp thứ i và i+1 cắt nhau thì trục Xi có phương vuô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óc nhọn

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

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 quy tắc bàn tay phải

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

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óc hợp bởi trục Zi-1 và Zi có chiều dương là chiều dương quay quanh trục Xi

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

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.

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

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

OHa = i i

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

Định nghĩa kích thước di:

OHd = i

1−

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

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 động cuố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ểm tá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ập con của không gian có thể với đến được (reachable workspace).

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

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 ).

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

2.2 Robot Elbow

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

2.3 Robot Puma

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

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ủa phầ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ớp theo 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.

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

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ến khớp là θ1, θ2 . Ta có hệ phương trình sau:

cos

cos(

)

a 1 sin.

2 .

sin(

+ θθ 2 )

xE yE

θ 1 + θ 1

a 1

1 + θθ 2

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

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

2 E

2 2

cos

=θ 2

2 2 y a − E 1 aa .2 . 1

Vì

sin

cos

±=

−

θ 2

Nên

tan

(sin2

cos

)

θ A=

θ 2

2 Và ta có

(

cos

sin

a 1

θ 2

sin

θ 1

(

sin

a 1

θ 2

cos

θ 1

E y

) y E y + ) x +

− 2 E + 2 E

)

tan

(sin

θ A=

θ 2 2 x E cos θ 2 2 x E cos θ 1

θ 1

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

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à n 1 .... −

0 T n

A n

1 AA 1 2

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

1 T = n

1 T = 6

01 − TA n 1 11 − TA T n n 2 .......... .......... .

01 − TA 1 6 11 − TA T 2 6 6 .......... .......... .

−

2 T n

1 − T n

1 − A n 1 −

51 − TA 5 6

5 T 6

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

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

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

S θ 2

θ 2

A 2

θ 2 S θ 2 0

− C θ 2 0

0 1

Ca 2 Sa 2 θ 2 0

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

θ 1

0 0 0

0 S − θ 1 C

A 1

θ 1 0 0

1 0

1 Ca 1 Sa 1 θ 1 0 1

C ⎡ ⎢ ⎢ ⎢ ⎢ 0 ⎣ C ⎡ θ 1 ⎢ S θ ⎢ 1 0 ⎢ ⎢ 0 ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦ x

w x

S θθ 21

− C

0 T 2

A 2

y z

v v

θθ 21 S θθ 21 0

θθ 21 0

0 1

y E 0

E 1

z 0

w y w z 0

C ⎡ ⎢ ⎢ ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎢ u ⎢ u ⎢ ⎢ ⎣

−

T R B

A AT qR B

a 1

1 A =− T B

S θ 1 C

− 0

−

LLLMLLL 000 M

1 =− A 1

⎡ ⎢ ⎢ ⎢ ⎣

⎤ ⎥ ⎥ ⎥ ⎦

θ 1 S θ 1 0

θ 1 0

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎢ ⎢ ⎢ ⎢ ⎣

a 1

S θθ 21

S θ 1 C

− C

− 0

−

01 − TA 1 2

θθ 21 0

θθ 21 S θθ 21 0

E 0

θ 1 S θ 1 0

θ 1 0

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

θ 2

0 S − C

0 0 0

C ⎤ ⎡ ⎢ ⎥ ⎢ ⎥ . ⎢ ⎥ ⎢ ⎥ 1 ⎦ ⎣ Cx E −

0 a − 1 θ 1 Cy E

θ 1

01 − TA 1 2

θ 2 0 0

1 0

0 Sy + E θ 1 Sx + E 0 1

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎡ ⎢ ⎢ ⎢ ⎢ ⎣ C ⎡ ⎢ S ⎢ ⎢ ⎢ ⎣

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

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

Vì

01 − TA 1 2

1 A = 2

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

−

2 E

2 2

cos

sin

cos

±=

−

=θ 2

θ 2

tan

2 cos

)

θ A=

2 2 y a − E 1 . .2 aa 1 (sin2 , θ 2

θ 2

(

cos

sin

a 1

θ 2

sin

θ 1

(

sin

a 1

θ 2

cos

θ 1

E y

y ) E y + x ) +

− 2 E + 2 E

)

tan

(sin

θ A=

θ 2 2 x E cos θ 2 2 x E cos θ 1

θ 1

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

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

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

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

Bài giảng Động học vị trí Robot - TS. Phan Tấn Tùng