Giáo trình IUH Kỹ thuật robot Chương 2 Động học

Chương Động học Xét theo quan điểm khí tay máy biểu diễn chuỗi khâu cứng (khâu) nối với khớp quay khớp tịnh tiến Một đầu chuỗi khâu nối ràng buộc với phần đế, khâu tác động cuối gắn vào đầu lại Chuyến động tạo cấu trúc kết hợp chuyển động thành phần khâu so với khâu trước Do đó, để tay máy thực thao tác đối tượng khơng gian cần phải mơ tả vị trí hưóng khâu tác động cuối Trong chương trình bày vấn đề liên quan tới phương trình động học thuận dựa vào tốn học đại số tuyến tính Vị trí hướng khâu tác động cuối biểu diễn hàm số biến khớp cấu trúc khí so với hệ tọa độ tham chiếu Các cấu trúc động học chuỗi hở chuỗi đóng đề cập chương Phần cuối chương đề cập toán động học ngược, toán biến khớp xác định theo tư cho trước khâu tác dộng cuối 2.1 Tư ♦ vật • thể rắn Một vật thể cứng mơ tả khơng gian vị trí hưởng (tư thế) so với hệ tọa độ tham chiếu Như Hình 2.1, đặt o — xyz hệ tọa độ tham chiếu trực giao, X, y, z véc-tơ đơn vị trục hệ tọa độ Vị trí điểm O’ vật thể cứng so với hệ tọa độ o — xyz mô tả sau: o' = o'xx + o'yy + Ĩzz Trong đó, c/, , o’z, biểu thị thành phần véc-tơ O' € B3 dọc theo trục hệ tọa độ; vị trí O' viết gọn dưởi dạng véc-tơ (3x1) sau: o / °y (2.1) °'z Để mô tả hướng vật thể cứng, vật thể cứng, gắn hệ tọa độ trực giao biểu diễn véc-tơ đơn vị so với hệ tọa độ tham chiếu Hệ tọa độ đặt vật thể cứng O' — x'y'z' gốc ơ' x' ,y', z' véc-tơ đơn vị trục hệ tọa độ Các véc-tơ biểu diễn dối vói hệ tọa độ tham chiếu o — xyz theo phương trình: 29 Hình 2.1: Vị trí hướng vật thể cứng V = Tj -r + x' y + x'zz y' = ýxz + y'yy + y'zz (2.2) z' = z'xx + z'yy + z'z z Các thành phần véc-tơ đưn vị cosin hướng trục hệ tọa độ O' — x'y'z' đối vói hệ quy chiếu o — xyz 2.2 Ma trận quay Ba véc-tơ đơn vị (2.2) mô tả hướng vật thể so vói hệ tọa độ tham chiếu có the kết hợp ma trận (3 X 3) XX y X z'Tx x'Ty y'Ty z'Ty X /Tz_ y /7' z z~/7' z (2.3) R gọi ma trận quay Lưu ý véc-tơ cột ma trận R trực giao nên ta có: X1 y' — y'1 Z = z'1 X = Ngoài chuẩn dơn vị chúng: 11 X IT XJ — y iT yi _ — 11 zJT zJ _ — 11 Do R một, ma trận trực giao, hay: if R = h (2.4) Trong đó, /3 nia trận đơn vị (3 X 3) (2.4) viết lại cách nhân vế ma trận nghịch đảo R \ ta có: Rt = R~} 30 (2.5) Chuyển vị ma trận R nghịch đảo Hình 2.2: Phép quay hệ o — xyz góc quanh trục z 2.2.1 Các phép quay Thông qua phép quay hệ tọa độ tham chiếu quanh trục tọa độ, ta có the nhận hệ tọa độ Các phép quay dương chúng quay theo ngược chiều kim đồng hồ quanh trục tương đối Giả sử hệ quy chiếu o — xyz quay góc a quanh trục z (Hình 2.2), gọi O' — x'y'z' hệ tọa độ quay Các véc-tơ đơn vị hệ tọa độ mơ tả thành phần chúng đối vói hệ quy chiếu cosa sỉna y' = —sina' cosa ụ ’0‘ Do đó, ma trận quay hệ O' - x'y'z' so vói khung o — xyz là: co sa Sỉĩia —stna 01 cosa Tương tự, phép quay góc (3 quanh trục y là: cosị3 —si,n(3 31 (2.6) góc quanh trục X tương ứng sin/3 0 cos(3_ (2.7) ■ cosy —siny siny cosy (2.8) Hình 2.3: Biểu diễn điểm p hai hệ tọa độ khác Các ma trận hữu ích việc mơ tả phép quay quanh trục không gian Từ (2.6) - (2.8) ta rút tính chất sau: Rk(-ứ) = Rpti) k = x,y,z (2.9) Ma trận quay R có ý nghĩa mặt hình học tức ma trận quay R mô tả phép quay quanh trục không gian để cho sau quay trục hệ tọa độ quy chiếu trùng với trục tương ứng hệ tọa độ gắn vật the 2.2.2 Biểu diễn véc-tơ Để hiểu ý nghĩa hình học ma trận quay, ta xem xét trường hợp gốc tọa độ hệ gắn vật cứng trùng vói gốc hệ tọa độ quy chiếu (Hình 2.3) Với ớ' = 0, biểu thị véc-tơ rỗng có kích thước (3 X 1) Một điểm p khơng gian biễu diễn theo cách sau: Đối với hệ tọa độ o — xyz ~Px Py J>Z- Dối với hệ tọa dộ O' — x'y'z' 32 Hình 2.4: Biểu diễn điểm p hệ tọa độ quay Vì p p' biểu diễn điểm p hai hệ tọa dộ khác nhau, nên ta có: p = pxx + pyy + Pzz y' Khi (2.3) viết lại là: p = Rp' (2.10) Ma trận quay R ma trận chuyển đối tọa độ véc-tơ hệ tọa độ o — x'y'z' sang tọa độ véc-tơ hệ tọa độ o — xyz Từ (2.4), (2.10) viết lại: P = rtp (2.11) Ví dụ 2.1 Xét hai hệ tọa độ có chung diểm gốc tọa độ quay với góc a quanh trục z Dặt p p' véc-tơ tọa độ điểm p hệ o — xyz hệ o — x'y’z' tương ứng (Hình 2.4) Mối hên hệ tọa độ điểm p hai hệ tọa độ dược mô tả sau: px = p'rcosa — p' siĩiơ Py — Pr.sina — PyCOsa Pz — pí 33 Do đó, ma trận (2.6) biểu diễn khơng hướng hệ tọa độ so vởi hệ tọa độ lại mà biểu diễn chuyển đổi véc-tơ từ hệ tọa độ sang hệ tọa độ cịn lại có chung gốc tọa độ Hình 2.5: Phép quay véc-tơ 2.2.3 Phép quay véc-tơ Một ma trận quay xem toán tử ma trận cho phép quay véc-tơ góc cho trước quanh trục không gian Thực tế p' véc-tơ hệ quy chiếu o — xyz\ tích ma trận R véc-tơ p' (Rpr) tạo véc-tơ p có độ lớn p' xoay p' theo ma trận R Ví dụ 2.2 Xét véc-tơ p thu cách quay véc-tơ p' mặt phẳng xy góc a quanh trục z hệ tọa độ quy chiếu (Hình 2.5) Đặt (p'x,p'yìp'z) tọa độ véc-tơ p' véc-tơ p có thành phần sau px = p'xcosa — PySÌna Py — p'xsina — p'yCosa Pz = p'z Có thể thấy p biểu diễn p = Rz(à)p' Trong Rz() ma trận quay giống (2.6) Tóm lại, mặt hình học, ma trận quay có ba ý nghĩa sau : 34 • Ma trận quay mô tả hướng hai hệ tọa độ ; véc-tơ cột cosin hướng trục hệ tọa độ quay so vởi hệ tọa độ ban đầu • Ma trận quay biểu diễn chuyển đổi tọa độ tọa độ điểm biểu diễn hai hệ tọa độ khác (có chung gốc tọa độ) • Ma trận quay toán tử cho phép quay véc-tơ hệ tọa độ 2.3 Tống hợp ma trận quay Giả sử ba hệ tọa độ o - xoyozG, o - 3712/1^1, o - x2y‘2Z2 có chung gốc tọa độ o véc-tơ p mơ tả vị trí điểm chung khơng gian mơ tả ba hệ tọa độ p°, p1, p2 tương ứng Trước tiên, ta xem xét mối quan hệ p2 véc-tơ p hệ tọa độ p1 véc-tơ hệ tọa độ Nếu Rị biểu diễn ma trận quay hệ tọa độ i so với hệ tọa độ j, ta có = (2.12) Tương tự, ta có pu = R°p (2.13) pa = R°2p2 (2.14) Mặt khác, thay (2.12) vào (2.13) sử dụng (2.14) ta có R'Ì = R°ÌR\ (2.15) Như Rị (2.15) biểu diễn ma trận quay hệ tọa độ so với hệ tọa độ 0, ma trận thu nhờ thực hai phép quay liên tiếp thông qua hai ma trận quay (tức ma trận quay hệ tọa độ so với hệ tọa độ ) (tiíc ma trận quay hệ tọa độ so với hệ tọa độ 1) Một cách tống quát ta có Rị = («■)-’ = (fí’)r (2.16) Lưu ý, kết ma trận quay cuối khác phép quay theo hệ tọa độ hay theo hệ tọa độ cố định từ đầu (Hình 2.6 Hình 2.7 kết sau hai phép quay theo hệ tọa độ theo hệ tọa độ cố định) Ví dụ 2.3 Xét vật có gắn hệ tọa độ dó Hình 2.6 cho thấy kết cuối cìing hai phép quay khác quay theo thứ tự khác (trục z trước trục y trước) khác hai phép quay phép quay khung Mỗi phép quay Hình 2.6 dựa vào hệ tọa độ (tức tọa độ tương đối) Trong đó, Hình 2.7 kết hai phép quay (theo thứ tự z trước y trước), phép quay dược thực theo tọa độ cố định ban đầu (tức lặ tọa dộ tuyệt, dối) 35 Hình 2.6: Các phép quay hên tiếp vật thể quanh trục hệ tọa độ (hệ tọa độ tương đối) Hình 2.7: Các phcp quay liên tiếp vật thổ quanh trục hệ tọa độ cố định (hệ tọa độ tuyệt đối) 2.4 Góc Euler Các ma trận quay mơ tả hướng hệ tọa độ so vởi hệ tọa độ tham chiếu, thực tế chúng đặc trưng phần tử không độc lập có ràng buộc có liên hệ với Nghĩa có thơng số độc lập đủ để mô tả hướng vật the cứng không gian Mộ ba thông số biểu diễn hướng vật thể 36 Hình 2.8: Biểu diễn góc Euler ZY z khơng gian góc Euler ộ — 1!) ip]T Có hai góc Euler góc ZYz góc ZYX hay cịn gọi góc Roll-Pitch-Yaw 2.4.1 Góc ZYZ Phép quay mơ tả góc ZYz thu cách thực kết hợp ba phép quay tương ứng với ba ma trận quay /?£ ((£>), /?y'(d), Rzii[iỊjỴ tức quay quanh trục z góc 7?, sau hệ tọa độ đó, tiếp tục quay quanh trục Ị/' góc ứ, sau tiếp tục quay quanh trục z" gốc ỉ/> Ta có ma trận quay mơ tả góc ZYz tích ma trận quay thành phần: = 'C^C^ - SVSÝ -C^CtS* - SvCt, C,Sý + -Sy,cX + Cvc\ -S^Cyp cv (2.17) Cơng tlìiíc hữu ích việc giải tốn ngược, xác định góc Euler theo ma trận quay cho trước 2.4.2 7"11 7"12 7" 13 r2i r22 7-23 T31 7’32 7’33 Góc RPY (Roll-Pitch-Yaw) Một Góc Euler khác xuất phát từ việc biểu diễn hưởng ngành hàng khơng, dó góc ZYX hay cịn gọi góc Roll-Pitch-Yaw để biểu thị thay đoi cao độ máy bay Trong trường hợp góc ộ = [ự? ứ ì/)]T biểu diễn phép quay xác định so với hệ tọa độ cố định gắn vào khối tâm máy bay (Hình 2.9) W) - Rz^)Ry^Rz(ý;) 'C^ơỡ C^S.Ộ-S^, c^c4> + Q c c c A7 Q C C áoCỳ , 37 (2.18) y Hình 2.9: Biểu diễn góc Roll-Pitch-Yaw Với ma trận quay cho trước R= Tn Ca /12 H3 'ĩ'22 r23 7'31 r32 r33J Ta giải tốn ngược để tìm góc Euler Với 19 phạm vi (—7r/2, 7t/2) ta có góc Euler (p = Atan2(r21, rn) 19 = Atan2(—rẲi X/r‘ị2 + rịị) (2.19) 'Ip = Ato2(r32, r33) Với 19 phạm vi (tt/2, 3vr/2) ta có (Ị) = Ataĩi2(-r2ỵ, -rn) ứ = Atan2(-r3i, -y/rị2 + rị3) (2.20) lị) = Atan2(—r32, -T33) Các đáp án (2.19), (2.20) suy biến Cj) — 0; trường hợp này, xác định tổng sai lệch ộ lị) 2.5 Góc trục Để thuận tiện việc hoạch định quỹ đạo, hướng khâu tác động cuối tay máy robot, cần thiết phải biểu diễn phép quay quanh trục khơng gian với góc cho trước 38 KHỚP í KHÂU i Hình 2.17: Kết nối liên kết chuỗi với hai liên kết Dựa vào bảng thơng số bước 7, tính ma trận chuyển đổi với z = l, ,n Tính ma trận chuyển đổi tồn hệ tay máy T V (ợ) = Aỵ 4"-1 để tìm hướng vị trí hệ tọa độ n so vói hệ tọa độ 10 Vói T(f 77 cho trước, tính hàm động học thuận theo cơng thức 7ịè (ợ) = TỊịTỊỊT1' đe tìm vị trí hướng cùa hệ tọa độ khâu tác động cuối so với hệ tọa độ gắn phần đế 2.7.3 Tay máy chuỗi kín Phương pháp tính tốn động học thuận dựa vào quy tắc D-H áp dụng cho tay máy có dặc điểm có cấu tạo chuỗi nhiều khâu hở (chuỗi hở) Tuy nhiên phương pháp có the mơ rộng trường hợp tay máy có chuỗi kín Xét tay máy chuỗi kín cấu tạo từ 71 + khâu Vì có vịng kín bên trong, nên số lượng khớp nối l phải lớn n\ đặc biệt, hiểu số lượng vịng kín trị số ỉ — n Như Hình 2.1 7, khâu tới khâu i nối liên tiếp thông qua i khớp nối dầu tiên, xem một, chuỗi hở Kể từ điểm này, khớp i + 1' nối khâu i với khâu i + T khớp i + 1" nối khâu i với khâu i + 1" Giả sử trục khớp i + 1' i + 1" trùng Dù không the hình khâu i + 1' i + 1" khâu thành phần chuỗi động học kín Dặc biệt khâu i + 1' cịn nối vởi khâu i + 2' thông qua khớp i + 2' tiếp tục nối khâu j qua khớp j Tương tự khâu i + 1" dược nối với khâu Z + 2" thông qua khớp i + 2" tiếp tục khâu k thông qua khớp k Cuối cùng, khâu j k nối với khớp j + đe tạo thành chuỗi kín Thơng thường j k Đe dặt hệ tọa độ lên khâu khác áp dụng quy tắc D-H, cần phải tính tới chuỗi động học kín Như Hình 2.18, chuỗi động học tay máy có vịng kín khớp i kết thúc vịng kín khớp j Tại khớp i khớp j cấu trúc động học có dạng sơ đồ hình vói hai nhánh mở khóp i hai nhánh đóng lại khớp j Từ khâu tới khâu ị chuỗi động học hở, từ khâu ỉ tới khâu j chuỗi động học kín, từ khâu j tới khâu cuối chuỗi dộng học hở Lưu ý hệ tọa độ í chọn vói trục Zị trùng với trục cua khớp i + T i + 1" 48 i +-1" Hình 2.18: Chuyển đổi toạ độ chuỗi động học kín Vị trí hướng hệ tọa độ j so vói hệ tọa độ ỉ biểu diễn cách kết hợp ma trận chuyển đổi sau: y~'(%) Aý?') = A*+1, (2.42) Trong q' — [Ợí+I/ Tương tự vị trí hướng hệ tọa độ k so với hệ tọa độ i cho công thức sau A’k (