Động học thuận của tay máy robot

CHƯƠNG 2: XÂY DỰNG MÔ H NH TOÁN HỌC CỦA TAY MÁY ROBOT

2.1. Động học thuận, động học ngược của tay máy Robot hai bậc tự do

2.1.1. Động học thuận của tay máy robot

Các khái niệm cơ bản.

2.1.1.1.

Để biểu diễn một điểm bất kỳ trong không gian 3 chiều, người ta thường dùng vector điểm (point vector) với một hệ quy chiếu chọn trước.

⃗⃗ ⃗ Với:

- ⃗⃗ là vector điểm biểu diễn điểm V(a,b,c) trong hệ quy chiếu đã chọn;

- ⃗ là các vector đơn vị của hệ quy chiếu.

Nếu quan tâm cả về định vị và định hướng ta phải biểu diễn vector v trong không gian 4 chiều với suất vector là một ma trận cột:

[ ]

Với w là một hằng số thực nào đó. Nếu w = 1, ta có:

[ ] [ ]

Khi đó, tọa độ của điểm V trùng với tọa độ vật lý trong không gian 3 chiều, và hệ trục tọa độ sử dụng w = 1 được gọi là hệ tọa độ thuần nhất.

Ta quy ước một số vector đặc biệt như sau:

[0 0 0 0]T là vector không xác định.

[0 0 0 n]T với n ≠ 0 là vector không, tr ng với gốc tọa độ.

[x y z 0]T là vector chỉ hướng.

[x y z 1]T là vector điểm trong hệ tọa độ thuần nhất.

Gọi u là vector biểu diễn điểm U cần biến đổi. h là vector dẫn được biểu diễn bằng ma trận H gọi là ma trận biến đổi, khi đó ta có:

⃗

⃗ là vector biểu diễn điểm U sau khi biến đổi bằng ma trận biến đổi H Phép biến đổi tịnh tiến (translation):

Giả sử cần tịnh tiến một điểm U có tọa độ thuần nhất u = [x y z 1]T theo vector dẫn h = [a b c 1]T, ta có ma trận chuyển đổi H như sau:

( ) [

] (2.1)

Vector v biểu diễn điểm U sau khi biến đổi tịnh tiến được xác định như sau:

[

] [ ] [ ]

Như vậy bản chất của phép biến đổi tịnh tiến là phép cộng vector giữa vector điểm cần tịnh tiến và vector dẫn.

Phép quay (rotation) quanh các trục tọa độ:

Giả sử ta cần quay một điểm hoặc một vật thể quanh một trục tọa độ nào đó (trục x, y hoặc z) một góc qo, tương tự phép tịnh tiến ta lần lượt có các ma trận biến đổi sau:

( ) [

] (2.2)

( ) [

] (2.3)

( ) [

] (2.4)

Giả sử cho điểm U biểu diễn bởi vector u quay quanh trục z một góc α, ta có vector v biểu diễn điểm U sau khi quay là:

v = Rot(z,α).u

Nếu tiếp tục quay điểm U quanh trục y một góc β, ta có tọa độ mới là:

w = Rot(y,β).v = Rot(y,β). Rot(z,α).u

Chú ý: việc đổi thứ tự quay sẽ cho kết quả cuối cùng khác nhau.

Phép quay tổng quát:

Bây giờ chúng ta nghiên cứu phép quay quanh một vector k bất kỳ một góc , với ràng buộc duy nhất là vector k phải trùng gốc với gốc của một hệ tọa độ được xác định trước.

Ta khảo sát một hệ tọa độ C gắn lên điểm tác động cuối (bàn tay – End effector) của robot (hình 2.1). Hệ C gồm 4 vector Cx, Cy, Cz, Co được biểu diễn bởi.

[ ]

Với Cx = [nx ny nz 0]T; Cy = [Ox Oy Oz 0]T; Cz = [ax ay az 0]T

Hình 2.1. Hệ tọa độ gắn trên khâu chấp hành cuối

Hệ toạ độ gắn liền với điểm chấp hành cuối của robot có các vectơ đơn vị chỉ phương các trục như sau:

a - vector có hướng tiếp cận với vật thể (approach);

O - vector có hướng mà theo đó các ngón tay nắm vào khi cầm nắm đối tượng (occupation);

n - vector pháp tuyến với (O,a) (normal).

Các bước cơ bản để lập phương trình động học của tay máy robot tổng 2.1.1.2.

quát.

Một robot bất kỳ có thể xem là tập hợp của các khâu (links) gắn liền với các khớp (joints). Trên mỗi khâu ta gắn một hệ tọa độ và sử dụng các phép biến đổi thuần nhất (phép quay, tịnh tiến) để mô tả vị trí tương đối và hướng giữa các hệ tọa độ này.

Denavit J. đã gọi một phép biến đổi thuần nhất giữa 2 khâu liền nhau được mô tả bằng một ma trận A. A1 mô tả vị trí tương đối và hướng của khâu đầu tiên (so với hệ tọa độ gốc); A2 mô tả vị trí tương đối và hướng của khâu thứ 2 so với khâu thứ nhất. Như vậy, vị trí tương đối và hướng của khâu thứ 2 so với hệ tọa độ gốc được mô tả bởi ma trận:

T2 = A1A2 Tương tự đối với khâu thứ n:

Tn = A1A2…An

Tích của các ma trận A là ma trận T, thường có 2 chỉ số trên và dưới. Chỉ số trên là số thứ tự khâu đầu tiên, bỏ qua nếu chỉ số đó bằng 0; chỉ số dưới thể hiện số thứ tự của khâu cuối cùng. Nếu một robot có 6 khâu có thể có 6 bậc tự do và có thể định vị trí, định hướng trong vùng vận động của nó (range of motion). Ba bậc tự do xác định

vị trí thuần túy và 3 bậc tự do còn lại xác định hướng mong muốn. Ta có ma trận mô tả vị trí và hướng của khâu chấp hành cuối:

T6 = A1A2A3A4A5A6 (2.5) Đối với khâu chấp hành cuối (bàn tay robot):

Hình 2.2. Các vector định vị và định hướng của bàn tay máy

Khảo sát bàn tay robot như hình 3.2. Giả sử ta đặt gốc tọa độ của hệ mô tả tại điểm giữa của các ngón tay. Gốc tọa độ này được mô tả bởi vector p (xác định vị trí của bàn tay so với tọa độ gốc). Ba vector đơn vị mô tả hướng của bàn tay được xác định như sau:

- Vector có hướng mà theo đó bàn tay sẽ tiếp cận đến đối tượng là vector a (approach);

- Vector có hướng mà theo đó các ngón tay nắm vào nhau khi cầm nắm đối tượng là vector o (Occupation);

- Vector còn lại là vector pháp tuyến (normal) có quan hệ sau:

⃗

Và ma trận biến đổi T6 sẽ gồm các thành phần sau:

[ ] (2.6)

Ma trận R (3x3) là ma trận trực giao, biểu diễn hướng của khâu chấp hành cuối đối với hệ tọa độ cơ bản.

Bộ thông số DH (Denavit-Hartenberg):

Một robot nhiều khâu cấu thành từ các khâu nối tiếp nhau thông qua các khớp động. Gốc chuẩn của robot là khâu số 0 và không tính vào số các khâu. Khâu 1 nối với khâu chuẩn qua khớp số 1 và không có khớp ở đầu mút của khâu cuối cùng. Bất kỳ khâu nào cũng được đặc trưng bởi 2 thông số sau:

- ln là độ dài pháp tuyến chung

- αn là góc giữa các trục trong mặt phẳng vuông góc với ln

Thông thường người ta gọi ln là chiều dài và ln là góc xoắn của một khâu (hình

2.3). Phổ biến là 2 khâu liên kết với nhau ở chính trục của các khớp (hình 2.4).

Hình 2.3. Chiều dài và góc xoắn của một khâu

Hình 2.4. Các thông số cơ bản của một khâu (q, d, l và α)

Mỗi trục sẽ có 2 pháp tuyến với nó tương ứng với 2 khâu trước và sau. Khoảng cách giữa 2 pháp tuyến này được gọi là dn -đặc trưng cho vị trí tương đối giữa 2 khâu liên tiếp nhau; αn là góc giữa 2 pháp tuyến của một khớp đo trong mặt phẳng vuông góc với trục.

Các thông số ln, αn, dn và qn được gọi là bộ thông số DH.

Trường hợp khớp quay thì qn là biến khớp; trường hợp khớp tịnh tiến thì dn là biến khớp và ln bằng 0.

Xác định các ma trận Ai.

Để xác định các ma trận Ai – đặc trưng cho quan hệ giữa các hệ tọa độ nối tiếp nhau, tại mỗi khớp thứ i ta thực hiện trình tự các phép biến đổi thuần nhất sau:

- Quay quanh zi-1 một góc qi;

Khớp n Khớp n+1

ln

αn Khâu n

Khớp n-1 Khớp n Khớp n+1

Khâu n-2

Khâu n-1

Khâu n

Khâu n+1 qn-1

qn

qn+1

dn zn-1 xn-1

ln

ln

On xn

zn

ln

- Tịnh tiến dọc theo zi-1 một khoảng di; - Tịnh tiến dọc theo xi-1 – xi một khoảng li; - Quay quanh xi một góc xoắn αi.

Do đó ta có:

Ai = Rot (z, qi). Trans (0, 0, di).Trans (li, 0, 0).Rot (x, αi) (2.7) Áp dụng các công thức biến đổi thuần nhất (2.1), (2.2), (2.3) và (2.4) ta được:

[

] [

] [

]

[

] (2.8)

Đối với khớp tịnh tiến (ai = 0, i = 0) thì Ai có dạng:

[

] (2.9)

Đối với khâu đi theo khớp quay thì d, l, α là hằng số, và A sẽ là hàm số theo biến khớp q. Đối với khâu di chuyển theo khớp tịnh tiến thì a, q, α là hằng số, và A là hàm số theo biến số d.

Xác định T6 theo các ma trận Ai.

Ta đã biết T6 mô tả theo hệ tọa độ gốc được xác định như công thức (2.5). Trong trường hợp mô tả T6 theo hệ tọa độ trung gian thứ (n-1).

∏ (3.10)

Trường hợp tổng quát, xét quan hệ của robot với một thiết bị khác. Giả sử hệ tọa độ cơ bản của robot quan hệ với hệ tọa độ của thiết bị đó bởi phép biến đổi Z. và khâu chấp hành cuối của robot gắn thêm một công cụ có quan hệ bằng phép biến đổi E như hình 2.5:

Hình 2.5. Định vị và định hướng tay máy robot trường hợp tổng quát

Vị trí và hướng của công cụ mô tả theo hệ tọa độ của thiết bị sẽ là ma trận X được xác định bởi: X = ZT6E.

Quan hệ này được thể hiện qua toán đồ sau:

Hình 2.6. Toán đồ chuyển vị của tay máy robot trường hợp tổng quát

Từ toán đồ này có thể rút ra:

T6 = Z-1XE-1 (2.11)

Phương trình động học của tay máy robot 2 bậc tự do.

2.1.1.3.

Khảo sát tay máy robot có 2 khâu phẳng như hình 2.7. Ta gắn lên các hệ trục tọa độ như sau: các trục z, z1 vuông góc với mặt phẳng tờ giấy. Hệ tọa độ cơ sở Oxyz và hệ O1x1y1z1 gắn lên khâu số 2 như hình vẽ.

Chọn các biến khớp q1, q2 là góc quay của các khâu 1 và khâu 2 như hình vẽ. Giả sử khâu chấp hành cuối được gắn tại điểm mút P của khâu số 2 có tọa độ P(x,y). cũng chính là tọa độ của vật thể. Phương trình động học của tay máy robot là quan hệ giữa tọa độ của vật thể trong hệ tọa độ cơ sở [x y] và các biến khớp [q1 q2]:

q1,q2

y x



 





.

Hình 2.7. Tay máy robot 2 bậc tự do Ta lập bảng thông số DH của tay máy robot này như sau:

Bảng 2.1. Bộ thông số DH của tay máy robot 2 bậc tự do

Khâu i αi ai di

1 q1* 0 l1 0

2 q2*

0 l2 0

Trong đó, qi là các biến khớp (dấu * để chỉ các biến khớp).

Áp dụng công thức (2.8) ta tính được các ma trận Ai như sau:

[

] (2.12)

[

] (2.13)

Ký hiệu: C1 = cosq1; C2 = cosq2; S1 = sinq1; S2 = cosq2. Khi đó ta có:

[

] [

]

[

]

Y

X Yo

Xo q1

m1 lc1

lc2

l1

l2 P

X2

Y2

q2 X1

Y1

m2

g

Từ ma trận T2 ta có tọa độ điểm mút P của khâu thứ 2 (cũng chính là gốc của hệ tọa độ đặt trên khâu chấp hành cuối) như sau:

( ) ( ) (Pz = 0)

Hay:

( ) (2.13) ( ) (2.14) Hệ phương trình (2.13) và (2.14) chính là các phương trình động học thuận của tay máy robot 2 bậc tự do.

Từ 2 phương trình trên, ta suy ra:

[ ̇

̇] [ ( ) ( )

( ) ( ) ] [ ̇̇ ] ( ) [ ̇̇ ] (2.15) Trong đó:

- * + là vector góc quay các khớp của robot;

- ( ) ( )

là ma trận Jacobi của tay máy robot.