3

Động Học Thuận:

Qui Tắc Denavit-Hartenberg
Bước 1: Xác định các trục khớp và đặt tên tương ứng z0  z n−1 .
Bước 2: Xác lập hệ tọa độ nền. Đặt gốc của hệ tọa độ này tại bất kỳ điểm nào trên trục
z0 . Các trục x0 và y0 được chọn thỏa qui tắc tam diện thuận.

Lặp i = 1,..., n − 1 lần thực hiện bước 3 đến bước 5.

Bước 3: Xác định các gốc Oi là giao điểm của đường vuông góc chung giữa zi và zi −1

với zi . Nếu zi giao với zi −1 , đặt Oi tại giao điểm này. Nếu zi song song với zi −1 , đặt Oi

tại bất kỳ vị trí nào trên zi sao cho thuận tiện.

Bước 4: Xác định xi dọc theo đường vuông góc chung giữa zi −1 và zi đi qua Oi , hoặc

theo hướng vuông góc với mặt phẳng tạo bởi zi −1 và zi nếu zi −1 và zi giao nhau.

Bước 5: Xác định yi thỏa qui tắc tam diện thuận.

Bước 6: Xác định hệ tọa độ tác động cuối on xn yn z n . Giả sử khớp n là khớp quay, đặt

z n = a dọc theo hướng z n−1 . Xác định gốc On bất kỳ trên z n sao cho thuận tiện, thường

là tâm của bộ kẹp hay tại đầu dụng cụ mà tay máy phải mang. Đặt y n = s theo hướng

kẹp và đặt xn theo s x a . Nếu dụng cụ kẹp không đơn giản thì đặt xn và yn tạo thành
tam diện thuận.

1
Bước 7: Lập bảng tham số ch các khâu trên robot

ai : khoảng cách theo phương x i từ Oi đến giao điểm của các trục x i và z i −1 .

d i : khoảng cách theo phương zi −1 từ Oi −1 đến giao điểm của các trục x i và z i −1 , d i thay

đổi khi khớp i là khớp trượt.

α i : là góc quay quanh trục x i từ z i −1 đến z i .

θ i : là góc quay quanh trục z i −1 từ x i −1 đến x i .

Bước 8: Từ các ma trận biến đổi thuần nhất A i bằng cách thay các tham số trên vào.

Bước 9: Tính Tn = A1  An . Ma trận này cho ta biết được vị trí và hướng đối với hệ tọa
0

độ nền của dụng cụ gắn trên khâu cuối.

Ví dụ 3.1:

Hình 3.1: Tay máy hai khâu phẳng.

2
Bảng tham số khâu cho robot 2 khâu đồng phẳng

Khâu ai αi di θi
1 a1 0 0 θ1
2 a2 0 0 θ2

c1 − s1 0 a1c1 
s c1 0 a1s1 
A1 =  1
0 0 1 0 
 
0 0 0 1 
c 2 − s2 0 a 2 c2 
s c2 0 a2 s2 
A2 =  2
0 0 1 0 
 
0 0 0 1 

Ma trận biến đổi thuần nhất

c12 − s12 0 a1c1 + a2 c12 

s c12 0 a1s1 + a2 s12 
T20 = A1 A2 =  12
0 0 1 0 
 
0 0 0 1 

Lưu ý rằng 2 thành phần đầu của cột cuối cùng của T20 là vị trí x và y của O2 .

x = a1c1 + a2 c12
y = a1s1 + a2 s12

Phần quay của T20 cho hướng của o2 x2 y2 z 2 đối với hệ tọa độ nền.

3
Ví dụ 3.2: Cổ tay khớp cầu (Spherical Wrist)

Trục z3 , z 4 , z5 đồng quy tại điểm O . Tay máy Stanford có cổ tay thuộc dạng này.

Hình 3.2: Gán hệ trục tọa độ cho cổ tay khớp cầu.

Bảng 3.2: Tham số DH cho cổ tay khớp cầu.

Khâu ai αi di θi
4 0 -90 0 θ4
5 0 90 0 θ5
6 0 0 d6 θ6

Ta thấy rằng ba biến khớp cuối θ 4 , θ 5 , θ 6 là các góc Euler φ , θ ,ψ tương ứng đối với hệ

tọa độ O3 x3 y3 z3 . Ta có

c 4 0 − s4 0 c5 0 − s5 0 c6 − s6 0 0
s 0 c4 0 s 0 c5 0 s c6 0 0 
A4 =  4 ; A5 = 
5
; A6 = 
6

0 −1 0 0 0 −1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1

4
 c4 c5c6 − s4 s6 − c4 c5 s6 c 4 s5 c4 s5 d 6 
 s4 s5 d 6 
R3 O63   s4 c5c6 + c4 s6 − s4 c5 s6 + c4 c6 s 4 s5
T63 = A4 A5 A6 =  6 =
0 1   − s5 c 6 s5 s 6 c5 c5 d 6 
 
 0 0 0 1 

3 3
So sánh phần ma trận quay R6 của T6 với phép biến đổi góc Euler. Điều đó cho thấy

rằng vai trò θ 4 , θ 5 , θ 6 hoàn toàn giống với các góc Euler φ , θ ,ψ đối với hệ tọa độ

O3 x3 y3 z3 .

NL: Các góc Euler

Xét hệ tọa độ cố định O0 x0 y0 z0 và hệ tọa độ quay O1 x1 y1 z1 nhận được bởi việc thực hiện
3 phép quay sau:

(1) Quay quanh trục z một góc φ ;

(2) Quay quanh trục y hiện hành một góc θ ;

(3) Quay quanh trục z hiện hành một góc ψ

Hình 3.3: Sự biểu diễn các góc Euler

5
Ma trận biến đổi

R10 = Rz ,φ R y ,θ Rz ,ψ
cφ − sφ 0  cθ 0 sθ  cψ − sψ 0
=  sφ cφ 0  0 1 0   sψ cψ 0
 0 0 1 − sθ 0 cθ   0 0 1
cφcθcψ − sφsψ − cφcθsψ − sφcψ cφsθ 
=  sφcθcψ + cφsψ − sφcθsψ + cφcψ sφsθ 
 − sθcψ sθsψ cθ 

Bây giờ xét bài toán xác định các góc φ , θ ,ψ khi cho trước ma trận quay

 r11 r12 r13 

R = r21 r22 r23 
r31 r32 r33 

Giả sử rằng cả hai phần tử r13 , r23 đều không bằng 0. Có nghĩa là sθ ≠ 0 và vì thế cả

r31 , r32 đều không bằng 0. Nếu cả r13 , r23 đều không bằng 0 thì r33 ≠ ±1 và ta có cθ = r33 ,

sθ = ± 1 − r332 , như vậy ta có

(
θ = A tan r33 , 1 − r332 ) (3.1)

(
hay θ = A tan r33 ,− 1 − r332 ) (3.2)

Tùy vào dấu của các tham số mà hàm Atan sẽ chọn góc phần tư cho góc θ . Nếu cả hai
tham số bằng 0, thì hàm Atan không xác định.

Nếu r13 = r23 = 0 , thì r33 = ±1 và r31 = r32 = 0 . Vì vậy R có dạng

6
  r11 r12 0
R = r21 r22 0 
 0 0 ± 1

Nếu r33 = 1 thì cθ = 1 và sθ = 0 , kết quả là θ = 0 . Trong trường hợp này R10 trở thành

cφcψ − sφsψ − cφsψ − sφcψ 0 c(φ + ψ ) − s (φ + ψ ) 0  r11 r12 r13 

R10 =  sφcψ + cφsψ − sφsψ + cφcψ 0 =  s(φ + ψ ) c(φ + ψ ) 0 = r21 r22 r23 
 0 0 1  0 0 1  r31 r32 r33 

⇒ φ + ψ = A tan(r11 , r21 ) = A tan(r11 ,−r12 )

Có vô số nghiệm trong trường hợp này. Ta có thể lấy φ = 0 , và xác định ψ .

Nếu r33 = −1 , thì cθ = −1 và sθ = 0 , kết quả là θ = π . Ta có

− c(φ − ψ ) − s(φ − ψ ) 0  r11 r12 r13 

R =  s(φ − ψ )
0
1 c(φ − ψ ) 0 = r21 r22 r23 
 0 0 1  r31 r32 r33 

⇒ φ − ψ = A tan(− r11 ,− r21 ) = A tan(− r11 ,− r12 )

Cũng có vô số nghiệm trong trường hợp này.

7
4

Động học nghịch (Inverse kinematics)

Phát biểu bài toán

Cho trước một ma trận biến đổi homogenous 4x4 sau:

R O
H =  (4.1)
0 1 

Tìm một hay tất cả nghiệm của phương trình sau

Tn0 (q1 ,  , qn ) = H (4.2)

trong đó

Tn0 (q1 ,  , qn ) = A1 (q1 )  An (qn ) (4.3)

và H biểu diễn vị trí và hướng mong muốn của cơ cấu tác động cuối, và ta phải tìm giá

trị các góc khớp q1 ,  , qn sao cho Tn (q1 ,  , qn ) = H .

0

Phương trình (4.2) đưa đến việc giải 12 phương trình phi tuyến với n ẩn sau:

Tij = (q1 ,  , qn ) = hij , i = 1,2,3 , j = 1,...,4 (4.4)

trong đó Tij , hij tương ứng là 12 phần tử có giá trị của Tn và H (vì hàng cuối cùng của cà
0

Tn0 và H đều là (0,0,0,1) , nên 4 trong số 16 phương trình từ (4.2) sẽ không có giá trị).

Ví dụ 4.1:

8
Khảo sát tay máy Stanford.

Hình 4.1: Tay máy Stanford

Giả sử rằng vị trí và hướng của hệ tọa độ trên cơ cấu tác động cuối được cho trước như
sau:

9
Để tìm các biến khớp θ1 , θ 2 , d 3 , θ 4 , θ 5 ta phải giải tập phương trình lượng giác phi tuyến
sau:

Dĩ nhiên, các phương trình trên rất khó giải trực tiếp, và vấn đề này cũng xảy ra đối với
hầu hết các tay máy robot. Vì thế, ta cẩn phải tìm các kỹ thuật hiệu quả có hệ thống để
khai thát cấu trúc động học đặc biệt của từng tay máy. Trong khi vấn đề động học thuận
luôn có lời giải duy nhất, chỉ đơn giản thay các giá trị biến khớp vào các phương trình
động học thuận, thì vấn đề động học ngược có thể không có lời giải. Ngay cả khi tồn tại
một lời giải, nó cũng có thể không là lời giải duy nhất. Hơn nữa, vì các phương trình
động học thuận nói chung là các hàm phi tuyến phức tạp đối với biến khớp, nghiệm cho
bài toán động học ngược có thể rất khó giải được ngay cả khi nghiệm tồn tại.

Để giải bài toán động học ngược ta quan tâm nhất việc tìm nghiệm dạng đóng hơn là tìm
nghiệm bằng phương pháp số vì hai lý do. Thứ nhất, trong một số ứng dụng, như đi theo
vết đường hàn được cung cấp bởi hệ thống vision, các phương trình động học ngược
phải được giải với tốc độ nhanh, khoảng 20 mili giây, và các biểu thức đóng cho nghiệm
trực tiếp thì thực tế hơn là dùng phương pháp số. Thứ hai, các phương trình động học

10
nói chung có nhiều nghiệm. Tìm lời giải bằng biểu thức đóng cho phép ta đưa ra các qui
tắc chọn nghiệm đặc biệt trong một số nghiệm khả dĩ.

Chọn nghiệm cho bài toán động học ngược phụ thuộc vào yếu tố toán học và công nghệ.
Ví dụ, chuyển động của khớp quay có thể bị giới hạn nhỏ hơn một vòng quay 360 0 đưa
đến việc không phải tất cả nghiệm của phương trình động học đều có được vị trí vật lý
thật tương ứng trên tay máy.

Ta sẽ giả sử rằng với vị trí và hướng cho trước sao cho phương trình (4.2) tồn tại
nghiệm. Khi một nghiệm được xác định bởi phương trình toán học, nó phải được kiểm
tra thêm xem có thỏa mãn mọi rằng buộc trên khoảng chuyển động khả dĩ của khớp.

4.2 Tách động học

Dù bài toán động học ngược là rất khó, đối với tay máy 6 khớp, có ba khớp cuối đồng
quy tại một điểm, ta có thể tách bài toán động học ngược thành hai bài toán đơn giản
hơn là động học ngược vị trí và động học ngược hướng. Cụ thể, đối với tay máy 6 bậc tự
do với cổ tay khớp cầu, bài toán động học ngược có thể tách thành hai bài toán, đó là tìm
vị trí giao điểm các trục cổ tay (tâm cổ tay), và sau đó tìm hướng của cổ tay.

Ta biểu diễn (4.2) thành hai hệ phương trình như sau:

R60 (q1 ,  , q6 ) = R
(4.5)
O60 (q1 ,  , q6 ) = O

trong đó O và R là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ tọa độ cố
định bên ngoài (world coordinate system). Ta phải giải bài toán trên đối với các ẩn
q1 ,  , q6 .

Giả sử các trục khớp cầu z3 , z 4 và z5 giao nhau tại Oc và vì vậy gốc O4 và O5 gán theo

11
qui tắc D-H sẽ luôn ở tâm cổ tay Oc . Thường O3 cũng ở tại Oc , nhưng điều này không
nhất thiết là như vậy đối với phương pháp này. Điểm quan trọng đối với động học ngược

là chuyển động của ba khâu quanh 3 trục không làm thay đổi vị trí Oc , và như vậy, vị trí
tâm cổ tay là một hàm của chỉ ba biến khớp đầu tiên.

Giải thuật:

Đối với các tay máy thuộc loại này bài toán động học ngược có thể giải theo giải thuật
sau:

Bước 1: Tìm q1 , q2 , q3 sao cho tâm cổ tay Oc có tọa độ cho trước như sau:

0 
O = O − d 6 R 0
0
c (4.6)
1

0
Bước 2: dùng các biến khớp đã tìm được trong bước 1, thay vào R3 .

Bước 3: Tìm các góc Euler ứng với ma trận quay

12
 R63 = ( R30 ) −1 R = ( R30 )T R (4.7)

4.3 Động học ngược vị trí: giới thiệu một phương pháp hình học

Ta giới hạn phương pháp động học ngược vào phương pháp hình học vì hai lý do. Thứ
nhất, hầu hết các thiết kế tay máy hiện hành thì đơn giản về mặt động học, thường năm
loại cơ bản và có cổ tay khớp cầu. Thực tế, một phần là do khó khăn trong việc tìm giải
thuật chung cho bài toán động học ngược; thứ hai, có ít kỹ thuật có thể xử lý bài toán
động học ngược đối với các vị trí tùy ý.

4.3.1 Tay máy 3 khớp bản lề

Hình 4.2: Tay máy 3 khớp quay (khỉu tay)

Hình 4.3: Chiếu tâm cổ tay lên mặt phẳng x0-y0

13
Từ phép chiếu trên hình 4.2, ta có

θ1 = A tan( xc , yc ) (4.8)

xác định với mọi ( xc , yc ) ≠ (0,0) và có duy nhất nghiệm sao cho

x y
cos θ = ; sin θ = (4.9)
x +y
2 2
x + y2
2

Nghiệm thứ 2 là θ1 = π + A tan( xc , yc ) , sẽ dẫn đến có nghiệm khác cho θ 2 , θ 3

Các nghiệm θ1 đều hợp lệ ngoại trừ xc = yc = 0 . Trong trường hợp này, (4.8) không xác
định và tay máy ở vị trí kỳ dị (singular configuration), trên hình 4.3

Hình 4.4: Vị trí kỳ dị (vô số nghiệm)

Nếu khâu được bố trí lệch ( d ≠ 0 ) , hình 4.5 thì tâm cổ tay không thể giao với z0 . Trong

trường hợp này, thường có 2 nghiệm cho θ1 gọi là tay phía trái (left arm) và tay phía
phải (right arm), hình 4.6 và hình 4.7.

14
Hình 4.5: Tay máy 3 khớp quay có vai lệch

Hình 4.6: Vị trí tay phía trái

15
 Hình 4.7: Vị trí tay phía phải

Từ hình vẽ ta thấy rằng, nghiệm thứ 1 tương ứng với vị trí tiếp cận phía trái

θ1 = φ + α (4.10)

Trong đó φ = A tan( xc , yc ) ; α = A tan( r 2 − d 2 , d ) = A tan( xc2 + yc2 − d 2 , d )

Nghiệm thứ 2 tương ứng với ví trí tay máy tiếp cận phía phải

θ1 = A tan( xc , yc ) + A tan(− r 2 − d 2 , d ) (4.11)

hay θ1 = α + β

trong đó

α = A tan( xc , yc ) ;

β = γ + π
 ⇒ β = A tan(− r 2 − d 2 , d )
γ = A tan( r − d , d )
2 2

16
Vì cos(θ + π ) = − cos θ và sin(θ + π ) = − sin(θ )

Để tìm θ 2 , θ 3 khi biết trước θ1 , ta xét mặt phẳng tạo bởi khâu 2 và khâu 3 (hình 4.8). Vi
chuyển động của khâu 2 và 3 trong mặt phẳng. (còn tiếp, trình bày sau)

Hình 4.8: Chiếu lên mặt phẳng tạo bởi khâu 2 và 3

Hình 4.9: Bốn lời giải cho động học nghịch vị trí tay máy PUMA

17
Hình 4.2: Các động cơ cổ tay được bố trí cân bằng với hệ thống và truyền động bằng
các ống đồng trục

Hình 4.3: Cơ cấu cổ tay đồng quy tại một điểm (tâm cổ tay)

18
4.4 Động học nghịch hướng

Ta đã dùng phương pháp hình học để giải bài toán vị trí nghịch. Phương pháp này cho ta

giá trị của ba biến khớp đầu tiên θ1 , θ 2 và θ 3 ứng với một vị trí tâm cổ tay cho trước. Bài
toán động học ngược hướng bây giờ trở thành bài toán tìm giá trị của ba biến khớp cuối

cùng ứng với hướng cho trước đối với hệ tọa độ o3 x3 y3 z3 . Đối với cổ tay khớp cầu, bài
toán này trở thành bài toán tìm các góc Euler ứng với ma trận quay cho trước R . Cụ thể
là, ta tìm 3 góc Euler dùng các phương trình (3.1)-(3.2), sau đó thay các góc

θ4 = φ
θ5 = θ
θ6 = ψ

19