Bài tập robot công nghiệp Chươ ng ng 1&2 Bài 1: Cho robot Stanford như hình 1 gồm 2 khớ p quay và 1 khớ p tịnh tiến. Hãy xác định: • Số bậc tự do của robot • Các khả năng xoay, tịnh tiến nào trong hệ cố định OXYZ ?
Z Y
Hình 1
X
Bài 2: Cho robot Elbow như hình 2 vớ i 6 khớ p xoay. Hãy xác định: • Số bậc tự do của robot • Các khả năng xoay, tịnh tiến nào trong hệ cố định OXYZ ?
Z
Y X Hình 2
Bài 3: ơ đồ một robot (vớ i cấu hình tối thiểu) mà khâu tác động cuối (End-effector) có khả Vẽ sơ đồ năng tịnh tiến theo phươ ng ng Y, tịnh tiến theo phươ ng ng Z, và xoay quanh phươ ng ng X.
Chươ ng ng 3: Bài 4:
điểm P biểu diễn bở i vectơ A p = [2 4 1]T . T Tịnh tiến điểm P theo vectơ h = [1 2 1] , sau đó cho điểm P quanh tr ục X c ủa h ệ tọa độ {A} một góc 0 90 . Xác định véc-tơ biểu diễn vị trí điểm P sau 2 bướ c dịch chuyển. Cho
Bài 5: Cho một khối lậ p phươ ng ng trong hệ tọa độ OXYZ cố định như hình 3. Khối này đượ c 0 quay quanh tr ục OB một góc 90 . Xác định véc-tơ biểu di ễn v ị trí điểm A (một đỉnh c ủa khối lậ p phươ ng) ng) sau khi thực hiện phép quay. Bài 6: Cho một kh ối l ậ p phươ ng ng trong hệ tọa
độ OXYZ cố định như hình 3. Tịnh tiến kh ối lậ p ng quanh tr ục OZ một góc phươ ng ng theo véc-tơ h = [1 1 1] sau đó quay khối lậ p phươ ng 0 90 (lưu ý: hướ ng ng của khối lậ p phươ ng ng cũng sẽ bị thay đổi khi quay). Xác định véc-tơ biểu diễn vị trí điểm A (một đỉnh của khối lậ p phươ ng) ng) sau khi thực hiện 2 phép biến đổi. T
Bài 7: Cho một khối lậ p phươ ng ng trong hệ tọa độ OXYZ cố định như hình 3. Quay khối lậ p 0 0 phươ ng ng quanh tr ục OZ một góc 90 sau đó quay tiế p quanh tr ục OX một góc -90 . Xác ng) sau khi thực hiện 2 định véc-tơ biểu diễn vị trí điểm A (một đỉnh của khối lậ p phươ ng) phép biến đổi. Bài 8: Cho một khối lậ p phươ ng ng trong hệ tọa độ OXYZ cố định như hình 3. Quay khối lậ p 0 phươ ng ng quanh tr ục OZ một góc 45 sau đó quay tiế p quanh véc-tơ AB (là 1 cạnh của khối 0 lậ p phươ ng) ng) một góc -90 . Xác định véc-tơ biểu diễn vị trí điểm C (một đỉnh của khối lậ p phươ ng) ng) sau khi thực hiện 2 phép biến đổi. Bài 9: Cho một khối l ậ p phươ ng ng trong hệ tọa độ {R: O-XYZ} cố định như hình 3. Quay khối lậ p phươ ng ng quanh tr ục OX một góc -450 sau đó tịnh tiến khối lậ p phươ ng ng theo véc-tơ h = [1 0 4] . Xác định véc-tơ biểu diễn vị trí phươ ng) ng) sau khi thực hiện 2 phép biến đổi. R
T
điểm A (một đỉnh của khối lậ p
Bài 10: T Một điểm P = [3 5 7] trong hệ tọa độ tham chiếu. Sau đó dịch chuyển điểm P một T khoảng cách d = [2 3 4] . Xác định vị trí mớ i của điểm P trong hệ tọa độ tham chiếu.
Bài 11: Một hệ tọa độ {A} đượ c mô tả so vớ i hệ tọa độ tham chiếu {R} bằng ma tr ận biến đổi R R thuần nhất TA. Xác định ma tr ận biến đổi thuần nhất TA sau khi dịch chuyển hệ {A} R T một khoảng cách d = [5 2 6] .
⎡0 ⎢1 R T A = ⎢ ⎢0 ⎢ ⎣0
1
0
2⎤
0
0
4
⎥ ⎥ 0 − 1 6⎥ ⎥ 0 0 1⎦
Bài 12: Cho một hệ tọa độ {A} đượ c mô tả so vớ i h ệ tọa độ tham chiếu {R} bằng ma tr ận biến đổi thuần nhất R TA. Hãy xác định các thành phần còn thiếu.
⎡? 0 − 1 ⎢? 0 0 R T A = ⎢ ⎢? − 1 0 ⎢ ⎣0 0 0
5⎤
⎥ ⎥ 2⎥ ⎥ 1⎦ 3
Bài 13: A Một vectơ p đượ c quay xung quanh tr ục Z của h ệ {A} một góc θ , và sau đó đượ c quay xung quanh tr ục X của h ệ {A} một góc φ . Hãy xác định ma tr ận quay thể hiện các phép quay này theo thứ tự đượ c cho. Bài 14: A 0 Một vectơ p đượ c quay xung quanh tr ục Z của hệ {A} một góc 30 , và sau đó đượ c quay xung quanh tr ục X của hệ {A} một góc 450. Hãy xác định ma tr ận quay thể hiện các phép quay này theo thứ tự đượ c cho. Bài 15: Cho một hệ tọa độ {B} ban đầu trùng vớ i hệ tọa độ {R}. Sau đó quay hệ tọa độ {B} xung quanh tr ục Z của nó một góc θ , và tiế p theo đó quay hệ tọa độ {B} xung quanh tr ục X của nó một góc φ . Hãy xác định ma tr ận quay để chuyển đổi vectơ từ hệ tọa độ {B} sang hệ tọa độ {R}. Bài 16: Cho một hệ tọa độ {B} ban đầu trùng vớ i hệ tọa độ {R}. Sau đó quay hệ tọa độ {B} xung 0 quanh tr ục Z của nó một góc 30 , và tiế p theo đó quay hệ tọa độ {B} xung quanh tr ục X 0 của nó một góc 45 . Hãy xác định ma tr ận quay để chuyển đổi vectơ từ hệ tọa độ {B} sang hệ tọa độ {R}.
Bài 17: Cho mối quan hệ giữa các hệ tọa độ {R}, {A}, {B}, và {C} như sau:
⎡0.866 − 0.500 0.000 11.0 ⎤ ⎢0.500 0.866 0.000 − 1.0⎥ R ⎥ T A = ⎢ ⎢0.000 0.000 1.000 8.0 ⎥ ⎢ ⎥ 0 0 1 ⎦ ⎣ 0 0.0 ⎤ ⎡1.000 0.000 0.000 ⎢0.000 0.866 − 0.500 10.0 ⎥ A ⎥ T B = ⎢ ⎢0.000 0.500 0.866 − 20.0⎥ ⎢ ⎥ 0 0 0 1 ⎣ ⎦
⎡0.866 − 0.500 0.000 − 3.0⎤ ⎢0.433 0.750 − 0.500 − 3.0⎥ C ⎥ T R = ⎢ ⎢0.250 0.433 0.866 3.0 ⎥ ⎢ ⎥ 0 0 1 ⎦ ⎣ 0 B
Xác định TC Chươ ng 4 Bài 18: Cho cơ cấu tay máy 2 bậc tự do như hình 4. Thiết lậ p: • Hệ tọa độ cho từng khâu • Bảng thông số DH • Hệ phươ ng trình động học thuận cho tay máy. Bài 19: Cho cơ cấu tay máy 3 bậc tự do như hình 5. Thiết lậ p: • Hệ tọa độ cho từng khâu • Bảng thông số DH • Hệ phươ ng trình động học thuận cho tay máy.
Hình 4
Hình 5
Bài 20: Cho cơ cấu tay máy 3 bậc tự do như hình 6. Thiết lậ p: • Hệ tọa độ cho từng khâu • Bảng thông số DH • Hệ phươ ng trình động học thuận cho tay máy. Hình 6
Bài 21: Cho cơ cấu tay máy 3 bậc tự do như hình 7 Thiết lậ p: • Hệ tọa độ cho từng khâu • Bảng thông số DH • Hệ phươ ng trình động học thuận cho tay máy. Hình 7 E
Bài 22: Cho cơ cấu tay máy có cấu hình như hình 8. Hệ toạ độ cố định là X0Y0Z0. Các kích thướ c d2=100mm, d4=100mm và biến khớ p d3=200mm. • Xác định vectơ biểu diễn vị trí điểm E trong hệ cố định. • Xác định tọa độ điểm E, nếu bi ến khớ p thứ 0 nhất có giá tr ị 30 , biến khớ p thứ hai có 0 giá tr ị 0 , biến khớ p thứ ba có giá tr ị 25mm, và ba biến kh ớ p thứ tư, thứ năm, và thứ sáu còn lại đều bằng 0
d4
Z Y X
Hình 8
Bài 23: Thiết lậ p các hệ tọa độ và xác định các tham số D-H cho robot 3-DOF trong hình 9.
Hình 9 Bài 24: Thiết lậ p các hệ tọa độ và xác định các tham số D-H cho robot 3-DOF trong hình 10.
Hình 10
Bài 25: Thiết lậ p các hệ tọa độ và xác định các tham số D-H cho robot SCARA trong hình 11.
Hình 11
Bài giải Bài 1:
5
∑
•
DOF = 6n − ipi Công thức tính bậc tự do 1 Vớ i n: số khâu động pi: số khớ p loại i Robot có 3 khâu n = 3 3 khớ p loại 5 (2 khớ p quay – 1 khớ p tịnh tiến) i = 5 ; p5 = 3 Vậy DOF = 6.3 − 5.3 = 3 Robot có 3 bậc tự do
•
Khớ p 1 quay quanh tr ục Y, khớ p 2 tịnh tiến vậy k ết hợ p 2 chuyển động này robot có thể tịnh tiến đến vị trí bất k ỳ trong mặt phẳng XOZ (tịnh tiến theo X và Z). Khớ p 3 quay quanh tr ục Y do đó End Effector có thể vươ n đến bất k ỳ điểm nào trong không gian 3 chiều. T ổng h ợ p l ại thì End Effector của robot có 3 bậc t ự do là quay tịnh tiến theo tr ục X,Y và Z.
Bài 2:
DOF = 6n −
5
∑
•
ipi Công thức tính bậc tự do 1 Robot có 6 khâu n = 6 6 khớ p loại 5 (6 khớ p quay) i = 5 ; p5 = 6 Vậy DOF = 6.6 − 5.6 = 6 Robot có 6 bậc tự do
•
Vậy End Effector của robot có 6 bậc tự do là quay quanh tr ục X,Y,Z, tịnh tiến theo tr ục X,Y,Z.
Bài 3:
Bài 4: Vị trí điểm P sau phép tịnh tiến
⎡1 ⎢0 A A B p = T B p = ⎢ ⎢0 ⎢ ⎣0
⎡1 1 0 q y ⎥ ⎢ p y ⎥ ⎢0 ⎥.⎢ ⎥ = ⎢ 0 1 q z ⎥ ⎢ p z ⎥ ⎢0 ⎥⎢ ⎥ ⎢ 0 0 1 ⎦ ⎣ 1 ⎦ ⎣0 0
0
q x ⎤ ⎡ p x ⎤
Vị trí P sau phép quay quanh tr ục X
⎡3 ⎤ 1 0 2⎥ ⎢ 4⎥ ⎢6 ⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 0 1 1 ⎥ ⎢1 ⎥ ⎢ 2 ⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 0 1 ⎦ ⎣1 ⎦ ⎣ 0 ⎦ 0
0
1 ⎤ ⎡ 2⎤
0 0 ⎡1 ⎢0 cosψ − sinψ A A B p = R B p = ⎢ ⎢0 sinψ cosψ ⎢ 0 0 ⎣0
0⎤ ⎡ p x ⎤
⎡1 ⎥ ⎢ ⎥ ⎢0 0 p y ⎥.⎢ ⎥ = ⎢ 0⎥ ⎢ p z ⎥ ⎢0 ⎥⎢ ⎥ ⎢ 1 ⎦ ⎣ 1 ⎦ ⎣0 A Vậy toạ độ điểm P sau 2 phép quay liên tiế p là p = [3
⎡ 3⎤ ⎥ ⎢ ⎥ ⎢ − 2⎥ 0 −1 0 6 ⎥.⎢ ⎥ = ⎢ ⎥ 1 0 0⎥ ⎢2 ⎥ ⎢ 6 ⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 0 1 ⎦ ⎣1 ⎦ ⎣ 1 ⎦ 0
0
0⎤ ⎡3 ⎤
− 2 6]T
Bài 5: Véctơ biều diễn điểm A A
r=
1
T .
A
p = [2 0
T
2] . Véctơ đơ n vị chỉ phươ ng tr ục quay OB 0
Vậy véctơ biểu diễn điểm A sau phép quay quanh tr ục r góc 90 là:
[1 1 1] 3 ⎡ r x2 (1 − cosϑ ) + cosϑ
⎤ r x r y (1 − cosϑ ) − r z sin ϑ r x r z (1 − cosϑ ) + r y sin ϑ 0 ⎡ p x ⎤
⎢ ⎥⎢ ⎥ 2 − + − + − − r r ( 1 cos ϑ ) r sin ϑ r ( 1 cos ϑ ) cos ϑ r r ( 1 cos ϑ ) r sin ϑ 0 A z y y z x ⎥.⎢ p y ⎥ p = A R B B p = ⎢ x y ⎢ r r (1 − cosϑ ) − r sin ϑ r r (1 − cosϑ ) + r sin ϑ r 2 (1 − cosϑ ) + cosϑ 0⎥ ⎢ p z ⎥ y y z x z ⎢ x z ⎥⎢ ⎥ 0 0 0 1⎥⎦ ⎣ 1 ⎦ ⎢⎣ ⎡ 0.3333 − 0.2440 0.9107 ⎢ 0.9107 0.3333 − 0.2440 A p=⎢ 0.3333 ⎢− 0.2440 0.9107 ⎢ 0 0 0 ⎣
0⎤ ⎡ 2 ⎤
⎡ 2.488 ⎤ 0⎥ ⎢0⎥ ⎢ 1.333 ⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 0⎥ ⎢2⎥ ⎢0.1786⎥ ⎥⎢ ⎥ ⎢ ⎥ 1 ⎦ ⎣1 ⎦ ⎣ 1 ⎦
Vậy A
T
p = [2.488 1.333 0.1786]
Bài 6: Vị trí điểm A sau phép tịnh tiến
⎡1 ⎢0 A A B p = T B p = ⎢ ⎢0 ⎢0 ⎣
⎡1 1 0 q y ⎥ ⎢ p y ⎥ ⎢0 ⎥.⎢ ⎥ = ⎢ 0 1 q z ⎥ ⎢ p z ⎥ ⎢0 ⎥⎢ ⎥ ⎢ 0 0 1 ⎦ ⎣ 1 ⎦ ⎣0 0
0
q x ⎤ ⎡ p x ⎤
⎡3⎤ 1 0 1⎥ ⎢0⎥ ⎢1⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 0 1 1⎥ ⎢ 2⎥ ⎢3⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 0 1⎦ ⎣1 ⎦ ⎣1⎦ 0
0 1⎤ ⎡ 2⎤
Vị trí A sau phép quay quanh tr ục Z
⎡cos φ − sin φ ⎢ sin φ cos φ A A B p = R B p = ⎢ 0 ⎢ 0 ⎢ 0 0 ⎣
⎡0 0 0⎥ ⎢ p y ⎥ ⎢1 ⎥.⎢ ⎥ = ⎢ 1 0⎥ ⎢ p z ⎥ ⎢0 ⎥⎢ ⎥ ⎢ 0 1 ⎦ ⎣ 1 ⎦ ⎣0 A Vậy toạ độ điểm A sau 2 phép biến đổi là p = [− 1 3 0
0⎤ ⎡ p x ⎤
− 1 0 0⎤ ⎡3⎤ ⎡− 1⎤ 0 0 0⎥ ⎢1⎥ ⎢ 3 ⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 0 1 0⎥ ⎢3⎥ ⎢ 3 ⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 0 1⎦ ⎣1⎦ ⎣ 1 ⎦ T
3]
Bài 7: Vị trí điểm P sau phép quay quanh tr ục Z
⎡cos φ − sin φ ⎢ sin φ cos φ A A B p = R B p = ⎢ 0 ⎢ 0 ⎢ 0 0 ⎣
⎡0 − 1 0 0⎥ ⎢ p y ⎥ ⎢1 0 ⎥.⎢ ⎥ = ⎢ 1 0⎥ ⎢ p z ⎥ ⎢0 0 ⎥⎢ ⎥ ⎢ 0 1 ⎦ ⎣ 1 ⎦ ⎣0 0 0
0⎤ ⎡ p x ⎤
⎡0⎤ 0 0⎥ ⎢0 ⎥ ⎢2⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 1 0⎥ ⎢ 2⎥ ⎢2⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 1 ⎦ ⎣ 1 ⎦ ⎣1 ⎦ 0
0⎤ ⎡ 2⎤
Vị trí P sau phép quay quanh tr ục X
0 0 ⎡1 ⎢0 cosψ − sinψ A A B p = R B p = ⎢ ⎢0 sinψ cosψ ⎢0 0 0 ⎣
0⎤ ⎡ p x ⎤
⎡1 0 0 0⎥ ⎢ p y ⎥ ⎢0 0 1 ⎥.⎢ ⎥ = ⎢ 0⎥ ⎢ p z ⎥ ⎢0 − 1 0 ⎥⎢ ⎥ ⎢ 1 ⎦ ⎣ 1 ⎦ ⎣0 0 0 Vậy toạ độ điểm P sau 2 phép quay liên tiế p là A p = [0 2 − 2]T
0⎤ ⎡0 ⎤
⎡0⎤ 0⎥ ⎢ 2⎥ ⎢ 2 ⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 0⎥ ⎢ 2⎥ ⎢ − 2⎥ ⎥⎢ ⎥ ⎢ ⎥ 1 ⎦ ⎣1 ⎦ ⎣ 1 ⎦
Bài 8: Sau khi quay quanh tr ục Z toa độ điểm A
⎡C φ − Sφ ⎢S C φ φ A A B p = R B p = ⎢ 0 ⎢0 ⎢0 0 ⎣
⎡0.7071 − 0.7071 0 0⎥ ⎢ p y ⎥ ⎢0.7071 0.7071 ⎥.⎢ ⎥ = ⎢ 1 0⎥ ⎢ p z ⎥ ⎢ 0 0 ⎥⎢ ⎥ ⎢ 0 1⎦ ⎣ 1 ⎦ ⎣ 0 0 0
0⎤ ⎡ p x ⎤
Sau khi xoay và tịnh tiến A về O thì tọa độ điểm B là
⎡0.7071 − 0.7071 ⎢0.7071 0.7071 A A B p = T B p = ⎢ 0 ⎢ 0 ⎢ 0 0 ⎣
− 1.414⎤ ⎡2⎤ ⎡− 1.414⎤ 0 − 1.414⎥ ⎢2⎥ ⎢ 1.414 ⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 1 − 2 ⎥ ⎢ 2⎥ ⎢ 0 ⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 1 ⎦ ⎣1 ⎦ ⎣ 1 ⎦ 0
Do chiều dài AB là 2 nên vectơ đơ n vị chỉ phươ ng là:
⎡ − 0.7071⎤ ⎢ 0.7071 ⎥ A ⎥ r=⎢ ⎢ 0 ⎥ ⎢ 1 ⎥ ⎣ ⎦ 0
Ma tr ận quay quanh tr ục xoắn r một góc -90 và tịnh tiến tr ở về vị trí cũ
⎡1.414⎤ 0 0⎥ ⎢0⎥ ⎢1.414⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 1 0⎥ ⎢ 2⎥ ⎢ 2 ⎥ ⎥⎢ ⎥ ⎢ ⎥ 0 1 ⎦ ⎣1 ⎦ ⎣ 1 ⎦ 0
0⎤ ⎡ 2⎤
− 0.5 − 0.7071 1.414⎤ ⎡ 0.5 ⎢ − 0.5 0.5 − 0.7071 1.414⎥ A ⎥ R B = ⎢ 0 2 ⎥ ⎢0.7071 0.7071 ⎢ 0 ⎥ 0 0 1 ⎦ ⎣ Ma tr ận chuyển đổi cho các phép biến đổi
− 0.5 − 0.7071 1.414⎤ ⎡0.7071 − 0.7071 ⎡ 0.5 ⎢ − 0.5 0.5 − 0.7071 1.414⎥ ⎢0.7071 0.7071 A ⎥.⎢ T B = ⎢ 0 2 ⎥⎢ 0 0 ⎢0.7071 0.7071 ⎢ 0 ⎥⎢ 0 0 1 ⎦⎣ 0 0 ⎣
− 1.414⎤ 0 − 1.414⎥ ⎥ 1 −2 ⎥ ⎥ 0 1 ⎦ 0
Toạ độ điểm C sau các phép biến đổi
⎡0 − 0.7071 − 0.7071 2.8282⎤ ⎡2⎤ ⎡ 1.414 ⎤ ⎢0 0.7071 − 0.7071 2.8282⎥ ⎢2⎥ ⎢4.2424⎥ A A B ⎥.⎢ ⎥ = ⎢ ⎥ p = T B p = ⎢ 1 0 0 0 0 2 ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢0 ⎥⎢ ⎥ ⎢ ⎥ 0 0 1 ⎦ ⎣1 ⎦ ⎣ 1 ⎦ ⎣ Bài 9: Vị trí điểm A sau các phép biến đổi
⎡1 0 ⎢0 C ψ A A B p = R B p = ⎢ ⎢0 Sψ ⎢0 0 ⎣
0
− Sψ C ψ
0
h x ⎤ ⎡ p x ⎤
0 0 ⎡1 h y ⎥ ⎢ p y ⎥ ⎢0 0.7071 0.7071 ⎥.⎢ ⎥ = ⎢ h z ⎥ ⎢ p z ⎥ ⎢0 − 0.7071 0.7071 ⎥⎢ ⎥ ⎢ 1 ⎦ ⎣ 1 ⎦ ⎣0 0 0
Bài 10: Vị trí điểm P sau phép biến đổi
⎡1 ⎢0 A A B p = T B p = ⎢ ⎢0 ⎢ ⎣0
⎡1 ⎥ ⎢ p ⎥ ⎢0 1 0 d y ⎥.⎢ y ⎥ = ⎢ 0 1 d z ⎥ ⎢ pz ⎥ ⎢ 0 ⎥ ⎢ ⎥ ⎢ 0 0 1 ⎦ ⎣ 1 ⎦ ⎣0 0
0
d x ⎤ ⎡ px ⎤
Bài 11: Ma tr ận biến đổi sau phép dịch chuyển
⎡ 5⎤ ⎥ ⎢ ⎥ ⎢ 8⎥ 1 0 3 5 ⎥.⎢ ⎥ = ⎢ ⎥ 0 1 4⎥ ⎢ 7⎥ ⎢11⎥ ⎥ ⎢ ⎥ ⎢ ⎥ 0 0 1⎦ ⎣ 1⎦ ⎣ 0 ⎦ 0 0
2⎤ ⎡ 3⎤
1 ⎤ ⎡ 2⎤
⎡ 3 ⎤ 0⎥ ⎢0⎥ ⎢− 1.4142⎥ ⎥.⎢ ⎥ = ⎢ ⎥ 4⎥ ⎢2⎥ ⎢ 5.4142 ⎥ ⎥⎢ ⎥ ⎢ 1 ⎥ 1 ⎦ ⎣1 ⎦ ⎣ ⎦
⎡1 ⎢0 R T A = ⎢ ⎢0 ⎢0 ⎣
⎡0 1 0 2⎥ ⎢1 0 0 4⎥ ⎢1 ⎥⎢ ⎥=⎢ 0 1 6 ⎥ ⎢0 0 − 1 6 ⎥ ⎢0 ⎥⎢ ⎥ ⎢ 0 0 1 ⎦ ⎣0 0 0 1 ⎦ ⎣0 0
0
5 ⎤ ⎡0
Bài 12:
⎡? 0 − 1 ⎢? 0 0 R T A = ⎢ ⎢? − 1 0 ⎢0 0 0 ⎣
1
0
2⎤
⎡a 0 − 1 3⎥ ⎢b 0 0 ⎥=⎢ 2⎥ ⎢ c − 1 0 ⎥ ⎢ 1 ⎦ ⎣0 0 0 5⎤
1
0
0
0
7⎤ 6⎥
⎥ 0 − 1 12⎥ ⎥ 0 0 1⎦
5⎤ 3⎥
⎥ 2⎥ ⎥ 1⎦
Ta có
u = [a Vì nên Vì
T
c]
b
v = [0
0
− 1]T
w = [− 1 0
T
0]
w.u = 0 ( −1).a + 0.b + 0.c = 0 ⇒ a = 0 w×u = v
Nên
⎡ i j k ⎤ r r r ⎢ ⎥ w × u = − 1 0 0 = (0.c − 0.b)i + (0.a − ( −1).c) j + ((−1).b − 0.a) k ⎢ ⎥ ⎢⎣ a b c ⎥⎦ r
r
r
r
r
r
w × u = 0i + c j + −bk = 0i + 0 j − 1k Vậy c=0 và b=1 Bài 13: Ma tr ận quay quanh tr ục Z
⎡cos θ − sin θ 0⎤ A R B = Rot ( Z , φ ) = ⎢ sin θ cos θ 0⎥ ⎢ ⎥ 0 1⎥⎦ ⎢⎣ 0 Ma tr ận quay quanh tr ục X
0 0 ⎤ ⎡1 A R B = Rot ( X ,ψ ) = ⎢0 cos φ − sin φ ⎥ ⎢ ⎥ ⎢⎣0 sin φ cos φ ⎥⎦
Ma tr ận quay liên tiế p
⎡ C θ − Sθ 0 ⎤ ⎢ ⎥ A R B = Rot ( X , φ ). Rot ( Z , θ ) = ⎢C φ Sθ C θ C φ − Sφ ⎥ ⎢⎣ Sφ Sθ Sφ C θ C φ ⎥⎦ Bài 14: Vớ i θ = 300 và φ = 450
⎡ ⎢ ⎢ 0 0 A R B = Rot ( X ,45 ). Rot ( Z ,30 ) = ⎢ ⎢ ⎢ ⎢ ⎣
3 2 2
−
1 2 6
4 2
4 6
4
4
⎤
0 ⎥
⎥ − ⎥ 2 ⎥ 2 ⎥ ⎥ 2 ⎦ 2
Bài 15: Đây là phép quay Euler Phép quay quanh tr ục Z của hệ B
⎡cos φ − sin φ ⎢ sin φ cos φ Rot ( Z , φ ) = ⎢ 0 ⎢ 0 ⎢ 0 0 ⎣
0 0
0⎤ 0⎥
⎥ 1 0⎥ ⎥ 0 1⎦
Phép quay quanh tr ục X của hệ B
0 0 ⎡1 ⎢0 cosψ − sinψ Rot ( X ,ψ ) = ⎢ ⎢0 sinψ cosψ ⎢0 0 0 ⎣
0⎤ 0⎥
⎥ 0⎥ ⎥ 1⎦
Tổng hợ p 2 phép quay liên tiế p
⎡C φ − Sφ C ψ ⎢S C φ C ψ φ A T B = Rot ( Z ,φ ). Rot ( X ,ψ ) = ⎢ Sψ ⎢0 ⎢0 0 ⎣
Sφ Sψ
0⎤
− C φ Sψ 0⎥ ⎥ C ψ 0⎥ ⎥ 0 1⎦
Bài 16: Vớ i φ = 300 và ψ = 450 Tổng hợ p 2 phép quay liên tiế p
⎡0.866 − 0.3535 0.3535 ⎢ 0.5 0.6124 − 0.6124 A ⎢ T Rot ( Z , φ ). Rot ( X , ψ ) = = B 0.7071 0.7071 ⎢ 0 ⎢ 0 0 0 ⎣ Bài 17: Ta có C
T B =C T R R T A A T B
Vậy
- 0.75 0.433 - 1.634 ⎤ ⎡ 0.5 ⎢ 0.75 0.125 - 0.6495 11.343 ⎥ C ⎥ T B = ⎢ ⎢0.433 0.6495 0.625 - 2.5752⎥ ⎢ 0 ⎥ 0 0 1 ⎣ ⎦ Vì
⎡ C R BT M − C R BT C q ⎤ ⎢ ⎥ C −1 B T B = TC = ⎢ LLL M LLL ⎥ ⎢0 0 0 M ⎥ 1 ⎣ ⎦ Nên
0.75 0.433 − 8.8053⎤ ⎡ 0.5 ⎢− 0.75 0.125 0.6495 − 4.316 ⎥ B ⎥ TC = ⎢ ⎢ 0.433 − 0.6495 0.625 6.4653 ⎥ ⎢ 0 ⎥ 0 0 1 ⎣ ⎦
0⎤ 0⎥
⎥ 0⎥ ⎥ 1⎦
Bài 18: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH
Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
⎡0 ⎢1 0 A1 = ⎢ ⎢0 ⎢0 ⎣
Khâu a α0 d θ0 1 0 90 d1 90 2 0 0 d2 0
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
⎡1 ⎢0 1 A2 = ⎢ ⎢0 ⎢0 ⎣
0
0
1
0
0⎤
1
0
0
⎥ 0 1 d 2⎥ ⎥ 0 0 1⎦
Phươ ng trình động học thuận 0 p = 0T 2 p 2
0⎥
⎥ 1 0 d 1⎥ ⎥ 0 0 1⎦
Ma tr ận chuyển từ hệ 2 về hệ 0
⎡0 ⎢1 0 0 1 T 2 = A1 A2 = ⎢ ⎢0 ⎢0 ⎣
0⎥
0⎤
0
0 1
d 2⎤
0 0
⎥ d 1⎥ ⎥ 1⎦
1
0
0 0
0⎥
Bài 19: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH
Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
⎡ − Sθ 1 ⎢ C θ 0 A1 = ⎢ 1 ⎢ 0 ⎢ 0 ⎣
Khâu a α0 d θ0 0 1 0 90 0 90 +θ1 0 2 0 90 L+d1 180 0 3 a3 0 0 90 +θ3
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
⎡− 1 ⎢0 1 A2 = ⎢ ⎢0 ⎢0 ⎣
0
0
Ma tr ận chuyển từ hệ 3 về hệ 0 0
0
1
2
T 3 = A1 A2 A3
Phươ ng trình động học thuận 0 p = 0 T 3 p 3
0⎤
0
Sθ 1
1
0
0
0
⎥ 0⎥ ⎥ 1⎦
0⎥
Ma tr ận chuyển từ hệ 3 về hệ 2(khâu 3)
⎡− Sθ 3 ⎢ C θ 2 A3 = ⎢ 3 ⎢ 0 ⎢ 0 ⎣
⎤ 1 1 0 ⎥ ⎥ 1 0 L + d 1⎥ ⎥ 0 0 1 ⎦ 0
0 C θ 1
− C θ 3 − Sθ 3 0 0
− a3 Sθ 3 ⎤ 0 a3C θ 3 ⎥ ⎥ 1 0 ⎥ ⎥ 0 1 ⎦ 0
Bài 20: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH
α0
Khâu a 1 0 2 a2 3 a3
Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
⎡C θ 1 ⎢S θ 0 A1 = ⎢ 1 ⎢ 0 ⎢ 0 ⎣
θ0 θ1 θ2 θ3
d 90 d1 0 0 0 0
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
⎡C θ 2 ⎢S θ 1 A2 = ⎢ 2 ⎢ 0 ⎢ 0 ⎣
− Sθ 2
0
C θ 2
0
0
1
0
0
a 2 C θ 2 ⎤
⎥ 0 ⎥ ⎥ 1 ⎦
Ma tr ận chuyển từ hệ 3 về hệ 0 0
0
1
2
T 3 = A1 A2 A3
Phươ ng trình động học thuận 0 p = 0 T 3 p 3
Sθ 1
0
− C θ 1
1
0
0
0
0⎥
⎥ d 1 ⎥ ⎥ 1⎦
Ma tr ận chuyển từ hệ 3 về hệ 2 (khâu 3)
⎡C θ 3 ⎢S θ 1 A2 = ⎢ 3 ⎢ 0 ⎢ 0 ⎣
a 2 Sθ 2 ⎥
0⎤
0
− Sθ 3
0
a3C θ 3 ⎤
C θ 3
0
0
1
0
0
⎥ 0 ⎥ ⎥ 1 ⎦
a3 Sθ 3 ⎥
Bài 21: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH Khâu a 1 E 2 0
Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
⎡− Sθ 1 ⎢ C θ 0 A1 = ⎢ 1 ⎢ 0 ⎢ 0 ⎣
α0
d θ0 0 90 0 90 +θ1 0 L+d2 0
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
⎡1 ⎢0 1 A2 = ⎢ ⎢0 ⎢0 ⎣
⎤ 1 0 0 ⎥ ⎥ 0 1 L + d 2 ⎥ ⎥ 0 0 1 ⎦ 0
0
0
Ma tr ận chuyển từ hệ 2 về hệ 0 0
T 2 = 0 A1 1 A2
Phươ ng trình động học thuận 0 p = 0 T 2 p 2
0 C θ 1 0
Sθ 1
1
0
0
0
− E .Sθ 1 ⎤ E .C θ 1 ⎥ ⎥ 0 ⎥ ⎥ 1 ⎦
Bài 23: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH Khâu a α0 d θ0 1 0 90 d1 θ1 2 0 0 d2 θ3 Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
⎡C θ 1 ⎢S θ 0 A1 = ⎢ 1 ⎢ 0 ⎢ 0 ⎣
0
Sθ 1
0
− C θ 1
1
0
0
0
0⎤
⎡C θ 3 ⎢S θ 1 A2 = ⎢ 3 ⎢ 0 ⎢ 0 ⎣
0⎥
⎥ d 1 ⎥ ⎥ 1⎦
Ma tr ận chuyển từ hệ 2 về hệ 0 0
T 2 = A1 A2 0
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
1
Phươ ng trình động học thuận 0 p = 0 T 2 p 2
− Sθ 3
0
C θ 3
0
0 0
0⎤ 0⎥
⎥ 1 d 2 ⎥ ⎥ 0 1⎦
Bài 24: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH
Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
α0
Khâu a 1 0 2 a2 3 a3
⎡C θ 1 ⎢S θ 0 A1 = ⎢ 1 ⎢ 0 ⎢ 0 ⎣
θ0 θ1 θ2 θ3
d 90 0 0 d1 0 0
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
⎡C θ 2 ⎢S θ 1 A2 = ⎢ 2 ⎢ 0 ⎢ 0 ⎣
− Sθ 2
0
C θ 2
0
0
1
0
0
a 2 C θ 2 ⎤
⎡C θ 3 ⎢S θ 2 A3 = ⎢ 3 ⎢ 0 ⎢ 0 ⎣
a 2 Sθ 2 ⎥
⎥ d 1 ⎥ ⎥ 1 ⎦
T 3 = A1 A2 A3 0
Phươ ng trình động học thuận
1
2
p = T 3 p
0
Sθ 1
0
− C θ 1
1
0
0
0
0⎤ 0⎥
⎥ 0⎥ ⎥ 1⎦
Ma tr ận chuyển từ hệ 3 về hệ 2 (khâu 3)
Ma tr ận chuyển từ hệ 3 về hệ 0 0
0
0
3
− Sθ 3
0
a3 C θ 3 ⎤
C θ 3
0
0
1
0
0
⎥ 0 ⎥ ⎥ 1 ⎦
a3 Sθ 3 ⎥
Bài 25: Đặt các hệ toạ độ lên tay máy
Bảng thông số DH
Ma tr ận chuyển từ hệ 1 về hệ 0 (khâu 1)
Khâu a α0 1 a1 0 2 a2 0 3 0 0
⎡C θ 1 ⎢S θ 0 A1 = ⎢ 1 ⎢ 0 ⎢ 0 ⎣
d θ0 0 θ1 0 θ2 d3 0
Ma tr ận chuyển từ hệ 2 về hệ 1 (khâu 2)
⎡C θ 2 ⎢S θ 1 A2 = ⎢ 2 ⎢ 0 ⎢ 0 ⎣
− Sθ 2
0
C θ 2
0
0
1
0
0
a 2 C θ 2 ⎤ a 2 Sθ 2 ⎥
⎥ 0 ⎥ ⎥ 1 ⎦
Ma tr ận chuyển từ hệ 3 về hệ 0 0
T 3 = 0 A1 1 A2 2 A3
Phươ ng trình động học thuận
0
p = 0 T 3 3 p
− Sθ 1
0
a1C θ 1 ⎤
C θ 1
0
0
1
0
0
⎥ 0 ⎥ ⎥ 1 ⎦
a1 Sθ 1 ⎥
Ma tr ận chuyển từ hệ 3 về hệ 2 (khâu 3)
⎡1 ⎢0 2 A3 = ⎢ ⎢0 ⎢0 ⎣
0
0
1
0
0⎤ 0⎥
⎥ 0 1 d 3 ⎥ ⎥ 0 0 1⎦
