Tài liệu Robot song song P2 pdf

Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 1 - CHƯƠNG 2: CƠ SỞ LÝ THUYẾT CHUNG VỀ PHÂN TÍCH ĐỘNG HỌC VẬT RẮN KHÔNG GIAN 2.1 Ma trận cosin chỉ hướng 2.1.1 Định nghĩa ma trận cosin chỉ hướng của vật rắn. Cho vật rắn B và hệ qui chiếu R 0 = { } (0) (0) (0) 123 ,,eee r rr . Trong đó (0) 1 e r , (0) 2 e r , (0) 3 e r là ba vector đơn vị trên các trục Ox 0 ,Oy 0 ,Oz 0 . Ta gắn chặt vào vật rắn một hệ qui chiếu R= { } 123 ,,eee rr r với 1 e r , 2 e r , 3 e r là ba vector đơn vị trên các trục Ax,Ay,Az (Hình 2.1). Hình 2.1 Định nghĩa : Ma trận vuông cấp ba =A (0) (0) (0) 11 12 13 (0) (0) (0) 21 22 23 (0) (0) (0) 31 32 33 eeee ee eeee ee eeee ee ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ rrrr rr rrrr rr rrrr rr (2.1) được gọi là ma trận cosin chỉ hướng của vật rắn B đối với hệ qui chiếu R 0 . Nếu ta đưa vào ký hiệu : (0) (0) .cos(,) ij i i i i aee ee== rr r r , (i,j = 1,2 3) (2.2) Thì ma trận cosin chỉ hướng (2.1) có dạng: =A 11 12 13 21 22 23 31 32 33 aaa aaa aaa ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ (2.3) O e 3 (0) (0) e 1 e (0) 2 e 3 e 1 e 2 X Z Y X B A 0 0 Y Z 0 Z 1 Y 1 X 1 Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 2 - Từ định nghĩa trên, trong hệ qui chiếu R 0 ta có các hệ thức liên hệ: (0) (0) (0) 1111 122 133 (0) (0) (0) 2211 222 233 (0) (0) (0) 3311 322 333 eaeaeae eae ae ae eae ae ae =++ =++ =++ r rrr r rrr r rrr (2.4) Nếu ta ký hiệu e i là ma trận cột gồm các phần tử của vector i e r trong hệ qui chiếu R 0 1 =e 11 21 31 a a a ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ , 2 = e 12 22 32 a a a ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ , 3 = e 13 23 33 a a a ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (2.5) Thì ma trận cosin chỉ hướng (2.3) có dạng: A=[e 1 ,e 2 ,e 3 ] (2.6) Ma trận cosin chỉ hướng A còn được gọi là ma trận quay của vật rắn. 2.1.2 Một vài tính chất cơ bản của ma trận cosin chỉ hướng a) Tính chất 1: Ma trận cosin chỉ hướng là ma trận trực giao. Theo công thức (2.6) : A=[e 1 ,e 2 ,e 3 ] Vậy ma trận cosin chỉ hướng A là ma trận cột có ba cột là ba vector trực chuẩn. Do đó A là ma trận trực giao. Hệ quả: Trong 9 thành phần của ma trận cosin chỉ hướng có 3 thành phần độc lập. Do tính chất của ma trận cosin chỉ hướng là ma trận trực giao nên A.A T =E. Từ đó nhận được 6 phương trình liên hệ giữa các thành phần của ma trận cosin chỉ phương như sau: 222 11 21 31 222 12 22 32 222 13 23 33 1 1 1 aaa aaa aaa ++= ++= ++= , 11 12 21 22 31 32 11 13 21 23 31 33 12 13 22 23 32 33 0 0 0 aa aa aa aa aa aa aa aa aa + += + += + += Do vậy chỉ có ba thành phần của ma trận cosin chỉ hướng là độc lập. b) Tính chất 2: Định thức của ma trận cosin chỉ hướng det(A)=1. Từ hệ thức A.A T = E ta suy ra: det( A.A T ) = det(A).det(A T ) = det(E) = 1 Do : det( A) = det(A T ) nên to có det(A) = 1 ± . Ta có thể chứng minh det( A) = 1. Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 3 - c) Tính chất 3 : Ma trận cosin chỉ hướng có ít nhất một trị riêng 1 1 λ = . 2.1.3 Ý nghĩa của ma trận cosin chỉ hướng của vật rắn Xét hai hệ qui chiếu R 0 và R có cùng gốc O. Trong đó hệ qui chiếu R 0 ≡ Ox 0 y 0 x 0 là hệ qui chiếu cố định, hệ qui chiếu R ≡ Oxyz gắn liền với vật rắn B. Lấy một điểm P bất kỳ thuộc vật rắn B. Vị trí của điểm P được xác định bởi vector định vị P OP r= uuur r . (Hình vẽ 2.2) Hình 2.2 Ký hiệu các tọa độ của điểm P trong hệ qui chiếu động Oxyz là x P , y P , z P , các tọa độ của điểm P trong hệ qui chiếu cố định Ox 0 y 0 z 0 là (0) P x , (0) P y , (0) P z . Ta có các hệ thức sau : (0) (0) (0) (0) (0) (0) 123 PP P P rxe ye ze=++ rr r r (2.7) 123 PP P P rxeyeze=++ rr rr (2.8) Thế các biểu thức (2.4) vào hệ thức (2.8) ta được : (0) (0) (0) 11 1 21 2 31 3 (. . . ) PP rxae ae ae=+++ rrrr (0) (0) (0) 12 1 22 2 32 3 (. . . ) P yaeaeae + ++ rrr (2.10) (0) (0) (0) 13 1 23 2 33 3 (. . . ) P zaeaeae++ rrr Hay : (0) 11 12 33 1 (. . .) PPPP raxayaze=++ + rr (0) 31 32 33 2 (. . .) PPP ax ay aze++ + r (2.11) (0) 31 32 33 3 (. . .) PPP ax ay aze++ r e 3 (0) e 2 (0) e 1 (0) e 3 e 1 e 2 Z Y Y X 0 Z 0 X 0 P B Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 4 - Z Y X O θ Ψ ϕ So sánh các biểu thức (2.7) và (2.11) ta suy ra hệ phương trình : (0) 11 12 33 P PPP x ax ay az=++ (0) 31 32 33 P PPP yaxayaz=++ (2.12) (0) 31 32 33 P PPP zaxayaz=++ Hệ phương trình (2.12) có thể viết lại dưới dạng ma trận như sau : (0) 11 12 13 (0) 21 22 23 (0) 31 32 33 . P P P P P P x x aaa yaaay aaa z z ⎡⎤ ⎡ ⎤ ⎡⎤ ⎢⎥ ⎢ ⎥ ⎢⎥ = ⎢⎥ ⎢ ⎥ ⎢⎥ ⎢⎥ ⎢ ⎥ ⎢⎥ ⎣⎦ ⎣ ⎦ ⎣⎦ (2.13) Từ hệ phương trình (2.13) ta rút ra kết luận sau : Ma trận cosin chỉ hướng A biến đổi các tọa độ của điểm P bất kỳ thuộc vật rắn trong hệ qui chiếu động Oxyz sang các tọa độ của điểm P đó trong hệ qui chiếu cố định Ox 0 - y 0 z 0 . 2.2 Các ma trận quay cơ bản Ta qui ước hướng quay dương là hướng quay ngược chiều kim đồng hồ như hình vẽ (Hình 2.3). Hình 2.3 Các phép quay quanh trục x, y, z của hệ tọa độ vuông góc Oxyz được gọi là phép quay cơ bản. Ta tìm ma trận quay của phép quay quanh trục x 0 một góc ϕ (Hình 2.4). Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 5 - Hình 2.4 Theo công thức định nghĩa (2.1) ta có: 0x A (0) (0) (0) 11 1 2 13 (0) (0) (0) 21 22 23 (0) (0) (0) 31 32 33 () . . . eeee ee eeee ee eeee ee ϕ ⎡⎤ ⎢⎥ = ⎢⎥ ⎢⎥ ⎣⎦ rrrr rr rrrr rr rrrr rr (2.14) 0 () ϕ x A = 10 0 0cos sin 0sin cos ϕ ϕ ϕ ϕ ⎡⎤ ⎢⎥ − ⎢⎥ ⎢⎥ ⎣⎦ (2.15) Ma trận (2.15) được gọi là ma trận quay của phép quay cơ bản quanh trục x 0 . Bằng cách tương tự, ta xác định được các ma trận quay cơ bản quanh các trục y 0 và z 0 (Hình 2.5) 0 () ψ = y A cos 0 sin 010 sin 0 cos ψ ψ ψ ψ ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ − ⎣⎦ , 0 () θ = z A cos sin 0 sin cos 0 001 θ θ θθ − ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (2.16) Từ các công thức (2.15) và (2.16) ta dễ dàng tính được: 000 det ( ) det ( ) det ( ) ϕ ψθ == xyz AAA (2.17) e 2 Z Z Y O 0 0 Z e 2 (0) e 3 (0) 3 e ϕ Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 6 - Hình 2.5 2.3 Các tọa độ thuần nhất và ma trận biến đổi tọa độ thuần nhất Khái niệm toạ độ thuần nhất được Denavit Hartenberg đưa ra năm 1955,và hiện nay đang được dùng rất rộng rãi trong tính toán động học robot. 2.3.1 Các toạ độ thuần nhất Định nghĩa: Cho X={x 1 ,x 2 , x n } là một điểm trong không gian n chiều R n .Tập hợp (n+1) phần tử (y 1 ,y 2 , y n ,y n+1 ) với (y n+1 ≠0) và: 12 12 11 1 ; ; ; n n nn n y yy xx x yy y ++ + == = (2.18) Gọi là toạ độ thuần nhất của X. Trong kỹ thuật,người ta thường chọn (y n+1 =1). Vậy điểm P(x,y,z) trong toạ độ vật lý R 3 được biểu diễn trong toạ độ thuần nhất R 4 như sau: P=[x,y,z] T ⇔ P=[x,y,z,1] T Trong R 3 Trong toạ độ thuần nhất R 4 Nhờ khái niệm toạ độ thuần nhất trong không gian 4 chiều ta có thể chuyển bài toán cộng ma trận cột trong không gian ba chiều sang bài toán nhân ma trận trong không gian bốn chiều. Cho a r và b r là hai vector trong không gian ba chiều, ta có: e e O 2 θ 1 (0) 1 e X 0 0 Y Y 2 (0) e X O 1 (0) e X 0 3 (0) 0 Z e Z X Ψ 3 e 1 e Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 7 - a+b= 11 11 22 22 33 33 abab abab abab + ⎡⎤⎡⎤⎡ ⎤ ⎢⎥⎢⎥⎢ ⎥ +=+ ⎢⎥⎢⎥⎢ ⎥ ⎢⎥⎢⎥⎢ ⎥ + ⎣⎦⎣⎦⎣ ⎦ (2.19) Ta chuyển phép tính cộng (2.19) bằng phép tính nhân hai ma trận như sau: 11 1 1 22 2 2 3 33 3 100 010 001 000 1 11 ab b a ab b a a ab b + ⎛⎞ ⎛⎞ ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ + ⎜⎟ ⎜⎟ ⎜⎟ = ⎜⎟ ⎜⎟ ⎜⎟ + ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ ⎝⎠ ⎝⎠ (2.20) 2.3.2 Ma trận biến đổi toạ độ thuần nhất Xét vật rắn B chuyển động trong hệ qui chiếu cố định OX 0 Y 0 Z 0 . Lấy một điểm A nào đó của vật rắn B và gắn chặt vào vật rắn hệ qui chiếu AXYZ (Hình 2.6). Lấy P là một điểm bất kỳ thuộc vật rắn B. Trong hệ toạ độ vật lý OX 0 Y 0 Z 0 ta có: Hình 2.6 P AAP rrs=+ rrr (2.21) Phương trình (2.21) có thể viết dưới dạng ma trận như sau: (0) (0) 11 12 13 (0) (0) 21 22 23 (0) (0) 31 32 33 PA x P Ay P Az xx s rrr yyrrrs rrr zz s ⎛⎞⎛⎞ ⎛⎞ ⎛⎞ ⎜⎟⎜⎟ ⎜⎟ ⎜⎟ =+ ⎜⎟⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟⎜⎟ ⎜⎟ ⎝⎠ ⎝⎠ ⎝⎠⎝⎠ (2.22) O X0 Z0 Y0 Y X Z P e A (0) 3 (0) e 1 e 3 (0) e 2 e 1 r A e 2 r P S AP Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 8 - Trong đó R là ma trận cosin chỉ hướng của vật rắn B, ,, x yz s ss là toạ độ của vectơ A P s r trong hệ qui chiếu x yz A .Nếu sử dụng hệ các toạ độ thuần nhất phương trình (2.22) có thể được viết lại dưới dạng: (0) (0) 11 12 13 (0) (0) 21 22 23 (0) (0) 31 32 3 000 1 1 1 x P A y P A A z P s x rrrx s y rrr y rr rz s z ⎛⎞ ⎛⎞ ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ = ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ ⎝⎠ ⎝⎠ (2.23) Định nghĩa : Ma trận H = (0) 11 12 13 (0) 21 22 23 (0) 31 32 33 000 1 A A A rrrx rrr y rrr z ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ (2.24) được gọi là ma trận chuyển toạ độ thuần nhất của điểm P từ hệ Axyz sang hệ Ox 0 y 0 z 0 . Các ma trận quay cơ bản thuần nhất và ma trận tịnh tiến thuần nhất: Các ma trận quay cơ bản (2.15), (2.16) mở rộng ra trong hệ toạ độ thuần nhất bốn chiều có dạng như sau: A x0 (ϕ)= 10 0 0 0cos sin 0 0sin cos 0 00 0 1 ϕϕ ϕϕ ⎛⎞ ⎜⎟ − ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ (2.25) A y0 (ψ)= cos 0 sin 0 0100 sin 0 cos 0 0001 ψψ ψψ ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ − ⎜⎟ ⎝⎠ (2.26) A z0 (θ)= cos sin 0 0 sin cos 0 0 0010 0001 θθ θθ − ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ (2.27) Ngoài ra ta đưa vào khai niệm ma trận tịnh tiến thuần nhất có dạng: Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 9 - T(a,b,c)= 100 010 001 0001 a b c ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ (2.28) Trong đó ta thực hiện chuyển động tịnh tiến theo trục toạ độ x một doạn là a, theo trục toạ độ y một đoạn b, theo trục toạ độ z một đoạn c. 2.4 Các góc quay Euler và ma trận quay Euler 2.4.1 Xác định ma trận cosin chỉ hướng từ các góc Euler Vị trí của vật rắn B quay quanh điểm O cố định được xác định bởi hệ qui chiếu động Oxyz (gắn chặt vào vật rắn B) đối với hệ qui chiếu cố định Ox 0 y 0 z 0 (Hình 2.7). Giả sử giao của mặt phẳng Ox 0 y 0 và mặt phẳng Oxy là trục OK. Trục OK này được gọi là đường nút. Hình 2.7 Ta đưa vào các ký hiệu sau : - Góc giữa trục Ox 0 và OK là ψ - Góc giữa trục Oz 0 và Oz là θ - Góc giữa trục OK và Ox là ϕ Ba góc ,, ψ θϕ được gọi là góc Euler. Như thế, vị trí của vật rắn B đối với hệ qui chiếu cố định được xác định bởi ba tọa độ suy rộng ,, ψ θϕ . Phương trình chuyển động của vật rắn quay quanh một điểm cố định có dạng: (t) ψ ψ = ; (t) θ θ = ; (t) ϕ ϕ = (2.29) Từ đó suy ra, vật rắn quay quanh một điểm cố định có ba bậc tự do. K 0 X X O Z Z 0 0 Y Y θ Ψ ϕ Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian - 10 - Khi xác định vị trí của vật rắn bằng các góc Euler, ta có thể quay hệ qui chiếu cố định Ox 0 y 0 z 0 sang hệ qui chiếu động Oxyz bằng ba phép quay Euler như sau (Hình 2.8): Hình 2.8 - Quay hệ qui chiếu R 0 ≡ Ox 0 y 0 z 0 quanh trục Oz 0 một góc ψ để trục Ox 0 chuyển tới đường nút OK. Với phép quay này, hệ Ox 0 y 0 z 0 chuyển sang hệ Ox 1 y 1 z 1 với Oz 0 ≡ Oz 1 . - Quay hệ qui chiếu R 1 ≡ Ox 1 y 1 z 1 quanh trục Ox 1 ≡ OK một góc θ để trục Oz 0 ≡ Oz 1 chuyển tới trục Oz 2 ≡ Oz. Như thế hệ qui chiếu Ox 1 y 1 z 1 chuyển sang hệ qui chiếu Ox 2 y 2 z 2 với Ox 1 ≡ Ox 2 ≡ OK. - Quay hệ qui chiếu R 2 ≡ Ox 2 y 2 z 2 quanh trục Oz 2 ≡ Oz một góc ϕ để trục Ox 2 ≡ OK chuyển tới trục Ox. Với phép quay này hệ qui chiếu Ox 2 y 2 z 2 chuyển sang hệ qui chiếu Oxyz với Oz 2 ≡ Oz. Như thế, bằng phép quay Euler quanh trục Oz 0 một góc ψ , quanh trục OK một góc θ , quanh trục Oz một góc ϕ , hệ qui chiếu Ox 0 y 0 z 0 chuyển sang hệ qui chiếu Oxyz. Các ma trận quay ứng với các phép quay Euler có dạng: 0 () ψ = z A cos sin 0 sin cos 0 001 ψ ψ ψψ − ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ (2.30) () θ = K A 10 0 0cos sin 0sin cos θ θ θ θ ⎡⎤ ⎢⎥ − ⎢⎥ ⎢⎥ ⎣⎦ (2.31) X O Ψ Z ω 0 X 1 0 Y Y 1 ≡ Κ Ψ ≡ Ζ 1 ≡ X X 1 ω θ O 1 Y Y 0 Z 1 Z 2 θ Z ≡ Z 2 ω ϕ Y 2 Y X X 2 ϕ 0 . năm 1955,và hiện nay đang được dùng rất rộng rãi trong tính toán động học robot. 2.3.1 Các toạ độ thuần nhất Định nghĩa: Cho X={x 1 ,x 2 , x n } là