Chương 2: ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC ROBOT 6 BẬC TỰ DO

2.1. Cơ sở lý thuyết về mô tả vị trí của robot công nghiệp

Nhóm tác giả sử dụng phương pháp Denavit-Hartenberg để tính toán cho bài
toán động học thuận của cánh tay robot, xây dựng một cách tổng quát tọa độ và
hướng trong không gian của khâu thao tác, không chỉ cho mẫu cánh tay robot 6 bậc
tự do trong đồ án mà có thể áp dụng cho các mẫu cánh tay robot khác có cấu trúc
tương tự. Hơn nữa, do có 6 bậc tự do nên việc biến đổi, tính toán trực tiếp sẽ rất khó
khăn và phức tạp. Đối với bài toán ngược, để tìm bộ thông số biến khớp khi biết vị
trí và hướng của khâu công tác, có thể sử dụng phương pháp giải tích hoặc phương
pháp hình học:

- Phương pháp giải tích: Sử dụng các lý thuyết và công thức đại số, các mối
liên hệ giữa các góc khớp với nhau để tìm ra lời giải. Phương pháp này phù hợp với
khả năng tính toán của máy tính, có thể dùng cho nhiều mẫu robot khác nhau có
cùng cấu trúc mà không phải thay đổi hoặc thay đổi rất ít các thông số bên trong.

- Phương pháp hình học: Sử dụng các đặc điểm hình học trong kết cấu robot
để tìm giá trị các góc khớp theo các thông số độ dài đã biết, chủ yếu là dựa vào các
hệ thức lượng trong tam giác. Phương pháp này tuy đơn giản nhưng lại chỉ áp dụng
cho 1 mẫu robot nhất định, khi muốn áp dụng sang loại khác phải thay đổi nhiều
thông số.

Trong đồ án, nhóm tác giả sử dụng phương pháp giải tích để giải bài toán
động học ngược.

Phương pháp Denavit-Hartenberg:

Giả sử trong chuỗi động học của tay máy có n khâu, khâu thứ i nối khớp thứ i
và khớp thứ i+1.
 Hình 2.1. Biểu diễn các thông số động học của quy tắc D-H

Quy ước khi gắn hệ trục tọa độ lên các khâu, khớp như sau:

- Trục zi của hệ tọa độ thứ i đặt dọc theo trục khớp động thứ i+1.
- Trục xi của hệ tọa độ thứ i nằm dọc theo đường vuông góc chung của
trục zi và zi-1, hướng từ khớp động thứ i đến khớp động thứ i+1. Nếu
trục zi và zi-1 cắt nhau thì trục xi đặt vuông góc với mặt phẳng chứa
trục zi và zi-1 tại giao điểm trục zi và zi-1 cắt nhau.
- Gốc của hệ tọa độ gắn liền với khâu thứ i (hệ tọa độ thứ i) đặt tại giao
điểm giữa đường vuông góc chung của trục zi và zi-1 và trục khớp thứ
i+1. Nếu 2 trục song song thì chọn gốc tọa độ bất kì sao cho dễ tính
toán.

Có một số quy ước riêng cho các trường hợp đặc biệt:

- Đối với hệ tọa độ cơ sở, chỉ có phương của trục z0 là xác định, trục x0
có thể chọn tùy ý.
 - Đối với hệ tọa độ thứ n, chỉ có phương của trục xn là xác định, trục zn
có thể chọn tùy ý.

Sau khi thiết lập được hệ tọa độ, ta hoàn toàn có thể xác định được vị trí của
hệ tọa độ thứ i so với hệ tọa độ thứ i-1 dựa vào bộ thông số sau:

- ai: Khoảng cách giữa 2 khớp theo phương đường vuông góc chung.
- di: Khoảng cách giữa giao điểm của 2 đường vuông góc chung với trục
quay, theo phương đường vuông góc chung.
- iGóc xoắn giữa trục zi và trục zi-1 , hay còn gọi là góc xoắn của khâu.
- iGóc xoay quanh trục zi-1 mà khi đó trục xi-1 trùng với trục xi
Trong 4 thông số trên, ta thấy ai, ichỉ phụ thuộc vào kết cấu của khâu thứ i, còn di,
i phụ thuộc vào tính chất của khớp thứ i. Nếu khớp thứ i là khớp quay thì di = const,
i = variable, và ngược lại, nếu là khớp tịnh tiến thì di = variable, i = const.
Như vậy, để mô tả phép chuyển đổi giữa 2 hệ tọa độ thứ i và i-1, ta cần thực hiện 4
bước sau:
1. Quay hệ tọa độ quanh trục zi-1 một góc i
2. Tịnh tiến dọc trục zi-1 một đoạn di
3. Tịnh tiến dọc trục xi-1 (đã trùng với xi) một đoạn ai
4. Quay quanh trục xi một góc i
Tương ứng với 4 bước trên là 4 ma trận thuần nhất:
cosi  sin i 0 0
 sin  cos i 0 0
Rot (z,i )   i 
 0 0 1 0
 
 0 0 0 1
 1 0 0 0
0 1 0 0
Trans (0,0,d i )   
0 0 1 di 
 
0 0 0 1
1 0 0 ai 
0 1 0 0
Trans (a i ,0,0)   
0 0 1 0
 
0 0 0 1
1 0 0 0
 0 cos   sin  i 0
Rot (x, i )   i 
 0 sin  i cos  i 0
 
0 0 0 1

Nhân 4 ma trận thuần nhất tương ứng cho 4 bước biến đổi trên, ta thu được

Ai 1,i
ma trận thể hiện mối quan hệ về vị trí và hướng giữa hai hệ trục tọa độ liền
kề.
Ai 1,i  Rot ( z ,i ).Trans(0,0,d i ).Trans(a i ,0,0).Rot ( x,  i )
cosi  sin i cos  i sin i sin  i ai cosi 
 sin  cos i cos  i  cos i sin  i ai sin i 
Ai 1,i  i 
 0 sin  i cos  i di 
 
 0 0 0 1 

2.2. Xây dựng mô hình động học cánh tay robot

2.2.1. Động học thuận

Động học nghiên cứu chuyển động các khâu của Robot về phương diện hình
học, không quan tâm tới các lực và moment gây ra chuyển đông. Động học Robot là
bài toán quan trọng trong phục vụ tính toán và thiết kế robot. Nhiệm vụ chủ yếu của
bài toán động học thuận là xác định vị trí và hướng của khâu công tác dưới dạng
hàm của các biến khớp.

Hệ tay máy Robot Fanuc MATE 100 có cấu trúc gồm 6 khâu, 6 khớp và một
khâu là giá. Mô hình cấu trúc của hệ có dạng (RRRRRR) quay-quay-quay-quay-
quay-quay được thể hiện trong hình dưới.

Hình 2.5. Mô hình động học của hệ.

 Ma trận biến đổi thuần nhất khâu công tác

Theo cách tiếp cận Dennavit – Hartenberg, để xác định các quan hệ động học
(vị trí , vận tốc) chúng ta cần xác định các ma trận chuyển đổi thuần nhất. Trên mô
hình cơ hệ, các tọa độ đã được định nghĩa theo phương pháp này. Bảng tham số
Dennavit – Hartenberg (D – H) như sau:

Bảng 2.1. Bảng tham số D-H

Khâu θi di ai αi

1 q1 d1 a1
2
2 q2 0 a2 0

3 q3 0 a3
2

4 q4 d4 0
2

5 q5 0 0 
2
6 q6 d6 0 0

Theo quy tắc D-H, ta xác định được ma trận biến đổi thuần nhất:

C1 0  S1 a1C1 
S 0 C1 a1S1 
A01   1
0 1 0 d1 
 
0 0 0 1 

C2 S2 0 a2 C 2 
S C2 0 a2 S 2 
A12   2
0 0 1 0 
 
0 0 0 1 
 C3 0  S3 a3C3 
S 0 C3 a3 S3 
A23   3
0 1 0 0 
 
0 0 0 1 

 C4 0 S4 0
S 0 C4 0 
A34   4
0 1 0 d4 
 
0 0 0 1

C5 0  S5 0
S 0 C5 0 
A45   5
0 1 0 0
 
0 0 0 1

C6  S6 0 0
S C6 0 0 
A56   6
0 0 1 d6 
 
0 0 0 1

Từ các ma trận chuyển đổi tọa độ thuần nhất, ta suy ra vị trí khâu thao tác so
với hệ tọa độ gốc O 0xyz bằng phép nhân ma trận, thực hiện trên phần mềm Matlab,
code tính toán được trình bày trong phụ lục I:

 nx ox ax px 
n oy ay p y 
A06  A01. A12 . A23 . A34 . A45 . A46   y
 nz oz az pz 
 
0 0 0 1

Trong đó:
n x  C1 ((C4C5C6  S 4 S6 )(C2C3  S 2 S3 )  S5 (C2 S3  C3 S 2 ))  S1 (C 4 S 6  S 4C5C6 ) (2.1)
n y  S1 ((S4 S6  C6C5C4 )(S 2 S3  C2C3 )  S5 (C2 S3  C3 S 2 ))  C1 ( S6C4  S 4C5C6 ) (2.2)
nz  S 23 S 4 S6  C6 (C23 S5  S 23C4C5 ) (2.3)
ox  C1 (( S 4C6  C4C5 S6 )(C2C3  S 2 S3 )  S5 (C2 S3  C3 S2 ))  S1 (C4C6  C5 S4 S6 ) (2.4)
o y  S1 ((C 4 S 6  C6 S 4 )( S 2 S 3 C2C3 )  S5 (C2 S3  C3 S 2 ))  C1 (C4C5 S 6  C4C6 ) (2.5)
oz  S6 (C23 S5  S23C4C5 )  S 23C6 S 4 (2.6)
ax  C1 ( S5C4 (C2C3  S 2 S3 )  C5 (C2 S3  C3 S 2 ))  S1 S 4 S5 (2.7)
a y  S1 (C4 S5 ( S 2 S3  C2C3 )  C5 (C2 S3  C3 S 2 ))  C1S 4 S5 (2.8)
az  S 23C4 S5  C23C5 (2.9)
px  C1 (a1  d 4 S23  a2C2  d 6 S 23C5  a3 (C2C3  S 2 S3 )  d 6 (C4 S 2 S3 S5  C2C3C4 S5 ))  d 6 S1S 4 S5 (2.10)
p y  S1 (a1  d 4 S23  a2C2  d 6 S 23C5  a3 (C2C3  S 2 S3 )  d 6 (C4 S 2 S3 S5  C2C3C4 S5 ))  d 6C1S 4 S5 (2.11)
pz  d1  d 4C23  a3 S 23  a2 S 2  (d 6S23S45 ) / 2  d 6C23C5  (d 6 S 4 5 S 23 ) / 2 (2.12)

Với
Si  sin(q i ), Ci  cos(q i ), S 23  sin(q 2  q3 ), C23  cos(q 2  q3 ),S45  sin(q 4  q5 )
Có thể ghi lại như sau :

A RE 
A06   E
0 1 

A là ma trận mô tả hướng và RE là véc tơ mô tả vị trí khâu thao tác trong hệ

tọa độ cơ sở.

T
RE   pxE p yE pzE  (2.13)
 nx   ,  ,   ox   ,  ,   a x   ,  ,   
 
AE   n y   ,  ,   oy   ,  ,   a y   ,  ,    (2.14)
 nz   ,  ,   oz   ,  ,   az   ,  ,   
C C C S  S  S C C S  C  S S 
 
AE   S C S S  S  C C S S  C  C S  (2.15)
  S C  S C C 
 
Với α, β, γ là các góc quay của khâu thao tác lần lượt theo ba trục x, y, z.
 Bằng cách thay các biến khớp q(t) vào ma trận A06 ta nhận được vị trí và
hướng của khâu công tác. Như vậy bài toán động học thuận được giải hoàn toàn.

Thông thường bài toán động học thuận được áp dụng để tìm miền làm việc
cho Robot khi biết giới hạn góc quay của khác khâu Robot.

2.2.2. Động học ngược

Nội dung của bài toán động học thuận là cho biết chuyển động của các toạ độ
khớp, ta cần phải xác định chuyển động của các toạ độ thao tác. Ngược lại trong bài
toán động học ngược, cho biết chuyển đông của các toạ độ thao tác, ta cần xác định
chuyển động của các toạ độ khớp.

Mục đích của bài toán nhằm xác định các góc i (i  1..6) để tay máy nắm bắt
được đối tượng tại một vị trí đã cho theo một hướng xác định. Do đó dữ liệu vào vị
trí đầu mỏ hàn và hướng mỏ hàn R

Nhìn chung, việc giải bài toán động học ngược cho robot 6 bậc tự do là rất
khó khăn, tuy nhiên, vẫn có những phương pháp giải để đơn giản hóa bài toán hơn.

Từ bài toán động học thuận, ta có ma trận mô tả vị trí điểm công tác E như
sau:

 nx ox ax px 
n oy ay p y 
AE   y
 nz oz az pz 
 
0 0 0 1

Đối với các robot có sáu khớp với ba khớp cuối cùng giao nhau tại một điểm,
có thể tách vấn đề động học ngược thành hai vấn đề đơn giản hơn. Chúng được biết
đến tương ứng, như động học nghịch đảo, và động học định hướng nghịch đảo. Nói
chính xác hơn, bài toán động học nghịch đảo có thể được chia thành hai bài toán
đơn giản hơn. Bước đầu tiên là tìm vị trí giao nhau của trục cổ tay, việc tiếp theo là
tìm hướng của cổ tay. Vị trí cổ tay có thể được tìm thấy bằng cách giải các phương
trình, trong đó px, py, pz, ax, ay, az là các phần tử của ma trận chuyển.

 Tính góc q1
Thực hiện nhân phương trình (2.10) với S 1, phương tình (2.11) với C 1, ta thu
được:
C1 p y  S1 p x  d 6 S 4 S5
(2.16)
Thực hiện nhân phương tình (2.7) với S 1, phương trình (2.8) với C 1, ta thu
được:
C1a y  S1ax  S 4 S5
(2.17)
Từ (2.16) và (2.17), ta có:
C1 p y  S1 px  d 6C1a y  d 6 S1ax
 C1 (p y  d 6 a y )  S1 ( px  d 6 ax )
 q1  a tan 2   px  d 6 ax  , (p y  d 6 a y ) 

 Tính góc q4, q5, q6

Ta sử dụng ma trận quay RPY trong (2.15) để tính 3 góc còn lại:
RRPY  Rot ( z , ), Rot ( y, ), Rot ( x, )
C C C S  S  S C C S  C  S S 
 
 RRPY   S C S S  S  C C S S  C  C S 
 S C  S C C 
  (2.30)
Mặt khác, ta lại có:
 C1C23 S1 C1S23 
R30  R10 R21 R32   S1C23 C1  S 23 S1 
 
  S23 0 C23  (2.31)

C4C5C6  S 4 S6 C6 S 4  C4C5 S 6 C4 S5 

 R63   R 
0 1
3 .R60   S4C5C6  C4 S6 C4C6  C5 S 4 S6  S 4 S5 
 C6 S5  S5 S 6 C5  (2.32)
So sánh 2 ma trận quay trong biểu thức (2.30) và (2.32) , ta có:
RRPY (1,3)  R 36 (1,3) (2.33)
RRPY (2,3)  R 36 (2,3) (2.34)
RRPY (3,1)  R 36 (3,1) (2.35)
RRPY (3,2)  R 36 (3,2) (2.36)
RRPY (3,3)  R 36 (3,3) (2.37)
Chia biểu thức (2.43) cho biểu thức (2.42) có :

RRPY (2,3) R 36 (2,3)

 3
RRPY (1,3) R 6 (1,3)
RRPY (2,3)  S4 S5 S 4
    tan q4
RRPY (1,3) C4 S5 C4
 q4  a tan 2( RRPY (2,3),R RPY (1,3))
Tính toán góc q6 tương tự :
Chia biểu thức (2.36) cho biểu thức (2.35) ;
RRPY (3,2) R 36 (3,2)

RRPY (3,1) R 36 (3,1)
RRPY (3,2)  S5 S6
 
RRPY (3,1) S5C6

 TH1 :
sin(q 5 )  0
  RRPY (3,2) S6
   tanq 6
RRPY (3,1) C6
 q6  a tan 2( RRPY (3,2), RRPY (3,1))

 TH2 :
sin(q 5 )  0
RRPY (3,2) S6
   tanq 6
 RRPY (3,1) C6
 q6  a tan 2( RRPY (3,2),  RRPY (3,1))
Mặt khác, ta lại có:
R63 (3,3)  C5 (2.38)
Từ biểu thức (2.37), (2.38), ta có:

 S5  1  C52  1  RRPY
2
(3,3) (2.39)
Chia biểu thức (2.39) cho biểu thức (2.38):

1  RRPY
2
(3,3) S5
  tan q5 (2.40)
RRPY (3,3) C5
 q5  a tan 2( 1  RRPY
2
(3,3), RRPY (3,3)) (2.41)
Như vậy, ta có 2 trường hợp ứng với các giá trị của sin(q5):
 q4  a tan 2( RRPY (2,3), RRPY (1,3))

q5  a tan 2( 1  RRPY (3,3), RRPY (3,3))
2
(2.42)
 q  a tan 2( R (3, 2),  R (3,1))
 Với sin(q5)  0 thì 
6 RPY RPY

 q4  a tan 2( RRPY (2,3), RRPY (1,3))


q5  a tan 2( 1  RRPY (3,3), RRPY (3,3))
2
(2.43)
 q  a tan 2(  R (3,2), R (3,1))
 Với sin(q5)  0 thì 
6 RPY RPY

Tổng kết lại, góc tại mỗi khớp được tính theo các công thức sau:

q1  a tan 2   px  d 6 ax  , (p y  d 6 a y ) 
Góc khớp q4, q5, q6 được tính dựa vào ma trận RPY mô tả hướng AE  :
 q4  a tan 2( RRPY (2,3), RRPY (1,3))

q5  a tan 2( 1  RRPY (3,3), RRPY (3,3))
2

 q  a tan 2( R (3, 2),  R (3,1))

 Với sin(q5)  0 thì 
6 RPY RPY

 q4  a tan 2( RRPY (2,3), RRPY (1,3))


q5  a tan 2( 1  RRPY (3,3), RRPY (3,3))
2

 q  a tan 2(  R (3,2), R (3,1))

 Với sin(q5)  0 thì 
6 RPY RPY