CHƯƠNG III: XÂY DỰNG ĐỘNG HỌC THUẬN VÀ ĐỘNG HỌC NGƯỢC CHO ROBOT 4 BẬC TỰ DO.

khâu tác động cuối tay máy. Bài toán động học thuận có nội dung gần giống như bài

toán phân tích động học cơ cấu nên người ta thường gọi “Bài toán phân tích động học”

Bài toán động học ngược: Ngược lại với nội dung của bài toán thuận, bài toán động

học ngược cho trước vị trí và định hướng của điểm tác động cuối mong muốn dưới

dạng một quy luật chuyển động nào đó trong không gian. Vấn đề là tìm tập hợp các

chuyển vị, vận tốc, gia tốc của các biến khớp tương ứng đó để điểm tác động đạt vị thế

mong muốn với các đặc tính chuyển động theo yêu cầu. Trong thực tế, bài toán động

học ngược gần giống như bài toán tổng hợp động học cơ cấu nghĩa là bài toán chỉ cho

trước yêu cầu hoặc quy luật chuyển động của khâu cuối ta phải xác định cơ cấu tay

máy và quy luật chuyển động của các khâu thành viên nên người ta thường gọi với tên

gọi khác là “Bài toán tổng hợp”. Giải bài toán động học ngược nhằm mục đích phục

vụ bài toán điều khiển quỹ đạo, điều khiển tối ưu…

Với bài toán động học thuận, trong mọi trường hợp ta xác định được một nghiệm

duy nhất, nghĩa là với mỗi tập giá trị biến khớp qi cho trước ta chỉ xác định được duy

nhất một tập nghiệm mô tả vị trí và hướng của cơ cấu tác động cuối.

Trong khi đó với bài toán động học ngược ta có thể xác định được một nghiệm,

nhiều nghiệm hay cũng có thể là không có nghiệm thỏa mãn tùy thuộc vào vị trí của cơ

cấu tác động cuối. Trong trường hợp quy luật chuyển động của cơ cấu tác động cuối

nằm trong vùng không gian hoạt động của tay máy ta có thể xác định được nhiều tập

nghiệm. Tại vị trí biên vùng không gian hoạt động của Robot ta xác định được duy

nhất một nghiệm, bài toán vô nghiệm khi luật chuyển động của cơ cấu tác động cuối

không nằm trong vùng hoạt động của Robot.

Ta có thể mô tả nội dung các bài toán động học tay máy thông qua sơ đồ:



22



Hình 3. 1: Sơ đồ khối động học Robot

3.2. Phương pháp nghiên cứu các bài toán động học Robot

Robot công nghiệp thường là cơ cấu hở, gồm một chuỗi các khâu nối với nhau bằng

các khớp động, khâu đầu tiên được nối với giá cố định (chân đế). Các khớp động này

có thể là khớp quay hoặc khớp tịnh tiến. Để Robot có thể thao tác linh hoạt theo mục

tiêu đặt ra thì cấu trúc chuỗi động của nó phải đảm bảo sao cho điểm mút của khâu

cuối đi theo một quỹ đạo cho trước nào đó và bản thân các khâu các khớp có khả năng

thay đổi hướng một cách dễ dàng phù hợp với công việc. Khâu cuối cùng thường là

bàn kẹp hoặc khâu gắn liền với dụng cụ làm việc (mỏ hàn, camera, súng phun sơn, dao

cắt). Do đó khi nghiên cứu Robot ta cần quan tâm không những vị trí của nó mà còn

phải quan tâm hướng của khâu cuối cùng trong hệ tọa độ cơ sở.

Khi nghiên cứu các bài toán động học Robot công nghiệp, người ta có thể dùng

nhiều phương pháp khác nhau: phương pháp vẽ hình, phương pháp giải tích vector, …

Tuy nhiên, một trong những phương pháp phổ biến, đơn giản và hiệu quả nhất hiện

nay là dùng ma trận biến đổi thuần nhất Denavit Hartenberg. Theo phương pháp này

để xác định vị trí và hướng của cơ cấu tác động cuối Robot so với hệ tọa độ cố định, ta

gắn vào khâu tác động cuối một hệ tọa độ động n và gắn mỗi khâu động một hệ tọa độ

động khác (từ khâu n đến khâu n-1) theo một quy tắc gọi là quy tắc Denavit

Hartenberg, sau đó ta xác định các thông số của khâu, khớp (thông số Denavit

Hartenberg) của Robot và biểu diễn mối quan hệ giữa các hệ tọa độ động gắn trên

23



khâu thông qua các thông số này dưới dạng ma trận (4x4), nhờ đó mà người ta xác

định được vị trí và hướng của cơ cấu tác động cuối. Như vậy thông qua các phép tính

toán các ma trận dạng (4x4) ta dễ dàng xác định được vị trí và hướng của cơ cấu tác

động cuối hay một khâu bất kỳ nào đó trên robot.

3.2.1. Quy tắc gắn các hệ tọa độ lên các khâu

Khi nghiên cứu động học Robot, người ta thường dùng quy tắc Denavit Hartenberg

(DH). Theo quy tắc này thông qua việc gắn các hệ tọa độ lên các khâu ta có thể xác

định được các ma trận biểu biến đổi biểu thị mối quan hệ giữa các hệ tọa độ với nhau

nhờ các phép biến đổi thuần nhất. Nhờ đó mà mà ta xác định được vị thế của điểm tác

động cuối so với hệ tọa độ gốc.

Xét hai khâu kế tiếp nhau của robot là khâu thứ i-1 và khâu thứ i được liên kết với

nhau thông qua khớp i.

Nguyên tắc gắn các hệ tọa độ lên các khâu là:

 Nếu chuỗi có n khâu thì lập được n hệ trục tọa độ.

Gốc hệ tọa độ thứ i được đặt tại tâm của khớp thứ i( là khớp nối giữa khâu i-1 và

khâu i)

zi của hệ tọa độ Oixiyizi trùng với trục của khớp thứ i

 Nếu khớp là khớp quay thì trục khớp là trục quay

 Nếu khớp là khớp trượt thì trục khớp trùng với phương trượt

Với khớp trụ trục khớp trùng với trục quay

Với khớp cầu : thường biến đổi tương đương rồi sau đó gắn trục tọa độ

Trục xi của hệ tọa độ Oixiyizi trùng với phương của vector vuông góc chung giữa

zi và zi+1 ( nghĩa là chọn trùng phương với tích với tích có hướng [ziX zi+1] )

 Nếu phương của zi và zi+1 là chéo nhau hoặc cắt nhau thì phương vuông góc

chung xác định duy nhất

 Nếu zi // với zi+1 : có vô số đường vuông góc chung, thường chọn x i trùng với

xi+1



24



Sau khi đã xác định được gốc tọa độ O i ,dựa vào phương chiều các trục z i và trục xi

ta có thể xác định trục yi bằng quy tắc bàn tay phải

Tương tự như cách xây dựng trên ta xác định được hệ tọa độ O i-1xi-1yi-1zi-1 được gắn

liền với khâu i-1



Hình 3. 2: Sơ đồ thiết lập hệ tọa độ lên các khâu

3.2.2. Các thông số động học Denavit Hartenberg

Bằng việc gắn các hệ tọa độ O ixiyizi và Oi-1xi-1yi-1zi-1 ta xác định các thông số

Denavit Hartenberg(DH) . Thông qua các tham số động học Denavit Hartenberg này ta

có thể biểu thị mối quan hệ giữa hệ tọa độ O ixiyizi và Oi-1xi-1yi-1zi-1 bằng các phép biến

đổi thuần nhất. Các thông số động học Denavit Hartenberg đó là:



25



 αi : Góc giữa trục





Zi và trục Zi + 1 xác định theo trục X i



a i : Khoảng cách từ trục Zi đến trục Zi + 1 đo theo trục X i



 di : là Khoảng cách từ trục





X i − 1 đến trục X i đo theo trục Z i



θ i : Góc giữa trục X i − 1 và trục X i xác định theo trục Zi



3.2.3. Ma trận biến đổi giữa các hệ tọa độ

Trong bốn thông số động học Denavit Hartenberg (DH) thì thông số a i và αi là

hai thông số của khâu. Hai thông số này luôn là hằng số độ lớn của chúng phụ thuộc

vào hình dáng, vị trí tương đối giữa khâu thứ i và khâu thứ i. Hai thông số d i và θi được

gọi là thông số của khớp, chúng phụ thuộc vào loại của khớp. Trong mỗi trường hợp

thì một trong hai thông số này là hằng số thông số còn lại là ẩn số. Nếu khớp là khớp

quay thì thông số θi là ẩn số và ngược lại nếu khớp là khớp trượt thì thông số d i là ẩn

số.

Để có thể chuyển hệ tọa độ O i-1xi-1yi-1zi-1 về hệ tọa độ Oixiyizi ta lần lượt thực hiện

bốn chuyển động cơ bản:

 Đầu tiên ta quay hệ tọa độ Oi-1xi-1yi-1zi-1 quanh trục zi-1 một góc θ i.

 Tịnh tiến dọc trục zi-1 một đoạn di.



26



Hình 3. 3: Sơ đồ chuyển đổi hệ tọa độ Oi-1xi-1yi-1zi-1 về hệ tọa độ Oixiyizi

 Tịnh tiến dọc dọc xi một đoạn



ai



 Cuối cùng ta quay quanh trục



xi



một góc



α



i



Ma trận biến đổi hệ tọa độ Oi-1xi-1yi-1zi-1 về hệ tọa độ Oixiyizi được xác định bằng tích

ma trận biến đổi của bốn chuyển động cơ bản trên là:



Tii−1 = ��t(z, �



�



).����� (0, 0, ��) . ����� (��, 0, 0). ���(



xi , � )

�



Quay hệ tọa độ Oi-1xi-1yi-1zi-1 quanh trục zi-1 một góc θi thì ma trận biến đổi của hệ tọa

độ là:



 cθ i − sθ i

 sθ cθ

i

Rot ( zi −1 ,θ ) =  i

 0

0



0

 0

Tịnh tiến theo trục



1

0

Trans(0,0, di ) = 

0



0



0

0÷

÷

0÷

÷

1



một đoạn



(3.1)



di thì



ma trận biến đổi của hệ tọa độ là :



0 0

0 0÷

÷

1 di ÷

÷

0 1



0

1

0

0



Tịnh tiến dọc theo



zi − 1



0

0

1

0



xi



một đoạn



(3.2)



a i thì ma trận biến đổi của hệ tọa độ là :



27



1

0

Trans (ai ,0,0) = 

0



0



0

1

0

0



0

0

1

0



ai 

0÷

÷

0÷

÷

1



(3.3)



Quay quanh trục xi một góc αi thì ma trận biến đổi của hệ tọa độ là:



0

1 0

 0 cα − sα

i

i

Rot ( x, α i ) = 

 0 sα i cα i



0

0 0



0

0÷

÷

0÷

÷

1



(3.4)



Như vậy ma trận biến đổi hệ tọa độ Oi-1xi-1yi-1zi-1 về hệ tọa độ Oixiyizi là:



T = Rot ( zi −1 ,θ ) . Trans(0,0, d i ) . Trans (ai ,0,0) . Rot ( x,α i )



i−1

i



 cθ i

 sθ

i −1

 i

iT =

 0



 0



− sθ i

cθ i

0

0



 cθi − sθ i

 sθ cθ

i

i −1

 i

iT =

 0

0



0

 0



0

0

1

0



0 0  1

0 0÷  0

÷.

1 0÷  0

÷

0 1  0



0 1

0÷ 0

÷. 

0÷ 0

÷

1 0



0

1

0

0



0  1

1 0 0 ÷ 0

÷. 

0 1 di ÷  0

÷

0 0 1  0

0 0



0 0 ai   1 0

1 0 0 ÷  0 cα i

÷. 

0 1 0 ÷  0 sα i

÷

0 0 1  0 0



0 0  1 0

0

0 0 ÷  0 cα i −sα i

÷. 

1 di ÷  0 sα i cα i

÷

0 1  0 0

0



− sqi

0

ai −1 

 cqi

 cα sq c cq − sα

− d i sα i −1 ÷

i −1

i −1

 i −1 i α i −1 i

÷

iT =

 sα i −1sq1 sα i −1cq1 cα i −1 di cα i −1 ÷



÷

0

0

1 

 0



28



0

− sα i

cα i

0



0

0÷

÷

0÷

÷

1



0

0÷

÷

0÷

÷

1



(3.5)



Ma trận



i−1

i



T được gọi là ma trận biến đổi thuần nhất ma trận có dạng :

i−1

i



T=



Trong đó :



Ri (ma trận 3x3): Ma trận quay

pi ( ma trận 3x1): Véc tơ tịnh tiến



Theo phép chuyển đổi thuần nhất giữa các hệ tọa độ, ta xác định được vị trí và

hướng của cơ cấu tác động cuối so với hệ tọa độ gốc được mô tả bằng ma trận tổng hợ p :

0

n



T=



Như vậy khi biết đặc tính hình học của các khâu và các quy luật chuyển động

của các khớp ta hoàn xác định hướng và vị trí của khâu thao tác.

3.3. Bài toán động học robot

3.3.1. Bài toán động học thuận

Mục đích của bài toán động học thuận là tìm ra vị trí và hướng của cơ cấu tác

động cuối khi đã biết giá trị các biến khớp. Thông thường giá trị biến khớp này được

xác định dưới dạng hàm cho trước biến đổi theo thời gian �� = ��(�), ta cần xác định vị

trí và hướng của khâu tác động cuối tương ứng với giá trị các biến khớp. Để giải bài

toán động học thuận ta chỉ cần lập phương trình động học của robot bằng phương pháp

DH.

a. Gắn hệ tọa độ lên các khâu

 Chọn hệ tọa độ gốc



O0 x 0 y0 z0 : Gốc tọa độ O0 trên trục khớp thứ nhất, trục xo



đặt theo phương của khâu 2, chiều hướng từ trục khớp thứ 2 sang trục khớp thứ

3 (Hình 3.4)



29



 Hệ tọa độ



O1x1y1z1 có gốc tọa độ O1 đặt tại giao điểm của trục khớp thứ nhất



và trục khớp thứ 2, trục



x1 đặt theo phương của khâu 2, chiều hướng từ trục



khớp thứ 2 sang trục khớp thứ ba



 Hệ tọa độ



O2 x 2 y2z 2



có gốc tọa độ



O2 đặt trùng với gốc tọa độ O1 , trục z 2



hướng dọc theo phương của trục khớp thứ hai, chiều hướng từ trong ra ngoài



 Đặt hệ tọa độ



O3 x 3 y3z3 có gốc O3



khớp thứ 4, trục



đặt tại giao điểm trục khớp thứ 3 và trục



z3 hướng theo phương trục khớp thứ ba, chiều hướng từ trong



ra ngoài.



 Đặt hệ tọa độ



O 4 x 4 y4 z 4 có gốc O4 trên trục khớp thứ 4, trục z 4 hướng theo



phương trục khớp thứ 4, chiều hướng từ gốc



 Đặt hệ tọa độ



rrr

n,o,a



O4 đến bàn tay nắm bắt.



r

tại tâm bàn tay nắm bắt, chiều trục a



tượng nắm bắt.



30



hướng về phía đối



Hình 3. 4: Sơ đồ gắn hệ tọa độ lên các khâu

Khoảng cách giữa các gốc tọa độ gắn lên các khâu của Robot theo mô hình đã xây

dựng là:



a1



a2

= 300 (cm);



=200 (cm);



a3



a4



=130 (cm);



= 130;



(cm); a= 75 (cm)



b.Bảng thông số DH

Bảng 2. 1: Bảng thông số DH Robot 4 bậc tự do

i

1

2

3

4

5



α i−1

0



900

0



900

0



di



θi



a1



q1



0



0



q2



a2



a



900 + θ3



0



a3 + q4



0



0



a4



0



ai− 1

0



c. Các ma trận chuyển đổi

31



Bằng việc thay thế các thông số động học Denavit Hartenberg vào biểu thức tính ma

trận



Ti i − 1



(2.5) ta lần lượt xác định được các ma trận chuyển đổi



Ti i − 1

Trong đó



là ma trận chuyển đổi giữa hệ trục tọa độ i-1 với hệ trục tọa đội. Để đơn



giản cho việc trình bày ta sử dụng các kí hiệu



 cq1 − sq1

 sq cq

1

 1

 0

0



0

T1 =  0

0

1

0

3

T4 = 

0



0



T10 T21 T32 T43 T54 .



0

0

1

0



sqi = sin(qi )



0

 cq2 − sq2 0

0÷ 1  0

0

−1

÷ T =

2

 sq2 cq2 0

a1 ÷

÷



1

0

0

 0

;



0 0

0



1

÷

0 −1 −(a3 + q4 ) ÷ 4  0

T =

÷ 5 0

1 0

0

÷



0 0

1

;

0



0

1

0

0



và



cqi = cos(qi )



0

 − sq3 − cq3

0 ÷ 2  cq3 − sq3

÷ T =

3

 0

0÷

0

÷



1

0

 0

;



0 a2 

0 0÷

÷

1 a÷

÷

0 1



0 0

0 0÷

÷

1 a4 ÷

÷

0 1



Ma trận biểu thị mối quan hệ giữa hệ tọa độ cuối so với hệ tọa độ gốc là:



T50 = T10 .T21.T32 .T43 .T54

 − cq1.s (q2 + q3 ) sq1 cq1.c(q2 + q3 ) (a3 + a4 + q4 ).cq1.c(q2 + q3 ) + a.sq1 + a2 .cq1.cq2 



÷

0

 − sq1.s (q2 + q3 ) −cq1 sq1.c (q2 + q3 ) (a3 + a4 + q4 ).sq1.c (q2 + q3 ) − a.cq1 + a2 .cq2 .sq1 ÷(3.6)

T5 =

 c(q2 + q3 )

÷

0

s (q2 + q3 )

a1 + (a3 + a4 + q4 ).s (q2 + q3 ) + a2 .sq2



÷

0

0

0

1







Với



s(q2 + q3 )



= sin(



q2 + q3 c(q2 + q3 ) = cos(q2 + q3 )

);



Cân bằng các phần tử tương ứng của hai ma trận với nhau ta có phương trình động học

của Robot 4 bậc tự do là:



32