Động Học Vận Tốc pot

5 Động Học Vận Tốc 5.1 Ma trận Skew Symmetric Ma trận skew symmetric giúp cho việc đơn giản nhiều việc tính toán. Định nghĩa 1: Một ma trận S được gọi là skew symmetric nếu và chỉ nếu 0=+ SS T (5.1) Từ (5.1), ta thấy rằng các phần tử trên đường chéo 0= ii s , và các phần tử ngoài đường chéo jiss jiij ≠−= , . Vì vậy, S chỉ chứa 3 thành phần độc lập có dạng như sau:           − − − = 0 0 0 12 13 23 ss ss ss S (5.2) Ta biểu diễn tập hợp mọi ma trận 3x3 skew symmetric bằng ký hiệu SS(3). Nếu T zyx aaa ),,(=a , ta định nghĩa ma trận skew symmetric )(aS như sau:           − − − = 0 0 0 )( 3 xy xz y aa aa as aS (5.3) Ví dụ 5.1: Ta biểu diễn 3 vectơ tọa độ đơn vị cơ sở ji, và k . TTT )1,0,0(;)0,1,0(;)0,0,1( === kji Theo định nghĩa, ma trận skew symmetric tương ứng là           − =           − =           −= 000 001 010 )(; 001 000 100 )(; 010 100 000 )( kSjSiS (5.4) 5.2 Tính chất ma trận skew symmetric: 1 (a) Tính chất 1: tuyến tính (Linearity) Với mọi vectơ 3 , Rba ∈ và 2 số vô hướng α và β ta có )()()( baba SSS βαβα +=+ (5.5) (b) Tính chất 2: Với mọi vectơ T zyx ppp ),,(=p papa x =)(S (5.6) NL: kyxyxjyxyxiyxyx yyy xxx kji )()()( 122131132332 321 321 −+−+−=           == yxc x θ sinyxc = Nếu )3(SOR∈ và 3 , Rba ∈ baba RRR x x =)( (5.7) Biểu thức (5.7) chỉ đúng khi R là trực giao Từ (5.6)-(5.7) ta có: ba ba ba baba )( )( )()( )()( RS R RRR RRRRS T TT = = = = x x x (5.8) Ta có một kết quả rất quan trọng sau: )()( aa RSRRS T = (5.9) Vế trái của (5.9) biểu diễn một phép biến đổi tương đương của ma trận )(aS ; có nghĩa là, sự biểu diễn ma trận )(aS trong hệ tọa độ được quay bởi R thì giống với ma trận skew symmetric )( aRS tương ứng với vectơ a quay bởi R . Giả sử )3()( SORR ∈= θ . Vì R trực giao với mọi θ ta có: IRR T =)()( θθ (5.10) Vi phân (5.10) ta có 2 0)()( =+ θ θθ θ d dR RR d dR T T (5.11) Định nghĩa ma trận T R d dR S )(: θ θ = (5.12) θ θθ θ d dR RR d dR S T T TT )()( =       = (5.13) Biểu thức (5.11) được viết lại như sau: 0=+ T SS (5.14) Vậy ma trận S định nghĩa bởi (5.12) là skew symmetric Nhân 2 vế của (5.12) cho R cho )( θ θ SR d dR = (5.15) Biểu thức (5.15) rất quan trọng. Nó có ý nghĩa rằng việc tính toán vi phân của ma trận quay R tương đương với việc nhân ma trận quay đó với một ma trận skew symmetric S . Hơn nữa, hầu hết các trường hợp ma trận R là ma trận quay cơ bản hay tích số của các ma trận quay cơ sở. Ví dụ 5.2: Xét ma trận quay cơ sở θ ,x RR = )( 010 100 000 cossin0 sincos0 001 sincos cossin0 000 iSR d dR S T =           −=           −           − −−== θθ θθ θθ θθ θ (5.16) θ θ θ , , )( x x RiS d dR =⇒ (5.17) Tương tự, θ θ θ , , )( y y RjS d dR = và θ θ θ , , )( z z RkS d dR = (5.18) Ví dụ 5.3: Xét ma trận quay θ ,k R quanh trục k bất kỳ, ta có 3 θ θ θ , , )( k k RkS d dR = (5.19) 5.3 Vận tốc góc: trường hợp tổng quát Xét vận tốc góc quay quanh trục tùy ý. Giả sử ma trận quay R thay đổi theo thời gian, nghĩa là, )3()( SOtRR ∈= , ta có: )()()( tRtStR =  (5.20) Vì )(tS là skew symmetric, nó có thể biểu diễn như ))(( tS ω đối với vectơ duy nhất )(t ω . Vectơ )(t ω là vận tốc góc của hệ tọa độ quay đối với hệ tọa độ cố định tại thời điểm t . )())(()( tRtStR ω =  (5.21) Ví dụ 5.4: Giả sử )(, )( tx RtR θ = )())(()()( tRtStRiS dt d d dR R ωθ θ θ ===⇒   (5.22) trong đó θω  i= 5.4 Cộng vận tốc góc Khảo sát vận tốc góc của 2 hệ tọa độ 1111 zyxo và 2222 zyxo chuyển động tương đối với hệ tọa độ cố định 0000 zyxo . Ta có )()()( 1 2 0 1 0 2 tRtRtR = (5.23) Theo kết quả (5.22), vi phân (5.23) ta được 0 2 0 2 1 2 0 1 1 2 0 1 0 2 )( RSRRRRR ω =+=  (5.24) Tính số hạng thứ 1: 0 2 01 2 0 1 01 2 0 1 )()( RSRRSRR aa ωω ==  (5.25) Tính số hạng thứ 2: 0 2 10 1 1 2 0 1 10 1 1 2 0 1 0 1 10 1 1 2 10 1 1 2 0 1 )( )()( )( RRS RRRSRRRSR RSRRR b b T b b ω ωω ω = == =  (5.26) Từ (5.24) ta thu được { } 0 2 10 1 00 2 0 2 )()()( RRSSRS ba ωωω += (5.27) Vì )()()( baSbSaS +=+ ta có 10 1 00 2 ba R ωωω += (5.28) 4 Kết quả trên có thể được mở rộng cho bất kỳ hệ trục tọa độ 11 2 0 1 0 − = n nn RRRR (5.29) 000 )( nnn RSR ω =  (5.30) Trong đó 10 1 3 4 0 3 2 3 0 2 1 2 0 1 0 1 0 − − +++++= n nnn RRRR ωωωωωω (5.31) 5.5 Vận tốc thẳng của một điểm gắn vào một hệ tọa độ chuyển động Giả sử điểm p được gắn cứng vào hệ tọa độ 1111 zyxo và 1111 zyxo quay tương đối với hệ tọa độ 0000 zyxo . Ta có 10 1 0 )( ptRp = (5.32) Vận tốc điểm đó được tính như sau: 0000 10 1 0 10 1 10 1 0 )( )()( )()( ppS ptRS ptRptRp x ωω ω == = +=    (5.33) Lưu ý: trong biểu thức (5.33), 0 1 =p  vì tọa độ của điểm đó không đổi đối với 1111 zyxo . Bây giờ, xét ma trận thuần nhất biểu diễn chuyển động tương đối của 1111 zyxo đối với 0000 zyxo phụ thuộc thời gian:       = 10 )()( )( 0 1 0 1 0 1 tOtR tH (5.34) Để đơn giản việc ký hiệu ta bỏ tham số t và các chỉ số ORpp += 10 (5.35) Vi phân (5.35) ta được vr ORpS OpRp += += += x ω ω     1 10 )( (5.36) trong đó 1 Rpr = là vectơ từ 1 O đến p được biểu diễn theo hướng của hệ tọa độ 0000 zyxo , và v là tốc độ tại đó gốc 1 O sẽ dịch chuyển. 5.6 Xây dựng ma trận Jacobian 5 Xét tay máy n khâu với các biến khớp n qq , , 1 . Đặt       = 10 )()( )( 00 0 qOqR qT nn n (5.37) biểu diễn phép biến đổi từ hệ tọa độ cơ cấu tác động cuối đối với hệ tọa độ nền, và T n qqq ), ,( 1 = là vectơ biến khớp. Khi robot chuyển động, i q , )( 0 qO n và )( 0 qR n thay đổi theo thời gian. Mục tiêu phần này là tìm mối liên hệ giữa vận tốc thẳng và vận tốc góc của cơ cấu tác động cuối với vận tốc khớp )(tq  . Đặt T nnn RRS )()( 000  = ω (5.38) biểu thị vận tốc góc 0 n ω của cơ cấu tác động cuối, và đặt 00 nn Ov  = (5.39) biểu thị vận tốc thẳng của cơ cấu tác động cuối. Ta tìm biểu thức có dạng qJ qJv n vn   ω ω = = 0 0 (5.40) hay qJ v n n n  0 0 0 =       ω (5.41) trong đó v J và ω J là các ma trận 3xn, và Jacobian n J J J v n x6 0 ∈       = ω , n là số khâu Ta sẽ tìm một biểu thức Jacobian đơn giản cho bất kỳ tay máy nào. (1) Vận tốc góc Đặt 1−i i ω là vận tốc góc khâu i tương đối với hệ tọa độ 1111 −−−− iiii zyxo gây bởi chuyển động quay của khớp i . k i i ii i i qzq  == − − − 1 1 1 ω (5.42) Nếu khớp i là khớp trượt, chuyển động của hệ tọa độ i đối với hệ tọa độ )1( −i là chuyển động tịnh tiến, nghĩa là 0 1 = −i i ω Vì vậy, vận tốc góc toàn bộ của cơ cấu tác động cuối, 0 n ω , trong hệ tọa độ nền được xác định như sau: 6 ∑ − −− =+++= n i iiinnnn zqRqRqq 1 0 1 0 1 0 12211 0  ρρρρω kkk (5.43) trong đó 1= i ρ đối với khớp quay, và 0= i ρ đối với khớp trượt, vì k 0 1 0 1 −− = ii Rz Dĩ nhiên T z )1,0,0( 0 0 = , và phần tử [ ] 101 − = nn zzJ ρρ ω 5.6.2 Vận tốc thẳng Vận tốc thẳng của cơ cấu tác động cuối chỉ là 0 n O  , ta có i n i i n n q q O O   ∑ = ∂ ∂ = 1 0 0 (5.44) Cột thứ i của v J được cho như sau: i n vi q O J ∂ ∂ = 0 (5.45) Cột thứ i của Jacobian có thể tìm được bằng cách giữ tất cả khớp cố định trừ khớp thứ i và kích hoạt khớp i với vận tốc đơn vị. Bây giờ ta xét 2 trường hợp: khớp quay và khớp trượt (1) Trường hợp 1: khớp trượt       ++ =                   = = =       − − − −− −− − − 10 101010 10 0 1 10 1 0 110 11 10 1 0 00 i i ii i ni o n i n i n i i i ii o i i n i ii n nn OORORR OROROR TTT T OR (5.46) Như vậy 0 1 10 1 00 − − − ++= i i ii i nin OORORO Nếu chỉ có khớp i chuyển động thì i n O và 0 1−i O là hằng số. Hơn nữa, nếu khớp i là khớp trượt thì ma trận quay 0 1−i R cũng là hằng số. Cuối cùng, theo qui ước D-H thì T iiiiii i i dssacaO ),,,( 1 = − . Các ma trận quay 0 i R và 0 1−i R cũng là hằng số đối với i d . Ta có 7 0 1 0 11 1 1 0 1 0 0 −−− − − =           =           ∂ ∂ = ∂ ∂ = ∂ ∂ iiii i ii ii i i i i i i i ii n zdRd d sa ca d ROR dq O  (5.47) hay 1 − = ivi zJ (2) Trường hợp 2: khớp quay Ta có ii q θ = . Vì 0 i R không là hằng số đối với i θ , ta thu được [ ] )( ))(( ])[( )()( 0 1 00 1 0 1 00 1 10 1 00 1 10 1 0 1 00 1 10 1 0 10 1 00 −− −− − −− − −−− − − − − −= −= += += ∂ ∂ + ∂ ∂ = + ∂ ∂ = ∂ ∂ inii inii i ii i niii i iiii i niii i i i i i ni i i ii i ni i n i OOz OOzS ORORzS ORzSORzS OROR ORORO x θ θ θ θθ θθ θθ     (5.48) Tính 10 1 0 1 10 1 0 1 10 1 0 1 0 1 10 1 0 1 0 1 )( )( ))(( )( 0 − −− − −− − −−− − − −− = = = =           − =           ∂ ∂ i iiii i iiii i ii T iii i iii iii ii i i ii ii i i ORzS ORkRS ORRkSR OkSR ca sa R d sa ca R θ θ θ θ θ θ      (5.49) Vì vậy, )( 11 −− −= inivi OOzJ x (5.50) 8 Công thức (5.50) được minh họa trên hình 5.1 Hình 5.1: Chuyển động của cơ cấu tác động cuối do khâu i Kết hợp Jacobian góc và Jacobian thẳng - Nữa trên của Jacobian v J được cho như sau: [ ] vnvv JJJ 1 = (5.52) Nếu khớp i là khớp quay, phần tử cột thứ i được tính: )( 11 −− −= inivi OOzJ x (5.53) Nếu khớp i là khớp trượt, 1− = ivi zJ (5.54) - Nữa dưới của Jacobian ω J được cho như sau: [ ] n JJJ ωωω 1 = (5.55) Nếu khớp i là khớp quay, phần tử cột thứ i được tính: 1− = ii zJ ω (5.56) Nếu khớp i là khớp trượt, 0= i J ω (5.57) 9 Như vậy, Jacobian cho tay máy n khâu có dạng như sau: [ ] n JJJJ 21 = (5.58) trong đó phần tử cột thứ i :       − = − −− 1 11 )( i ini i z OOz J x , nếu khớp i là khớp quay (5.59)       = − 0 1i i z J , nếu khớp i là khớp trượt (5.60) Công thức trên làm cho việc xác định Jacobian của bất kỳ tay máy nào trở nên đơn giản vì có sẳn tất cả các đại lượng cần tính một khi bài toán động học thuận tính được. Các đại lượng để tính Jacobian chỉ là vectơ đơn vị i z và tọa độ của các gốc n OO , , 1 . Kết quả trên rất quan trọng khi ta cần tính vận tốc của khối tâm trên các khâu để tính phương trình động lực học của chuyển động. Ví dụ 5.5: Xét tay máy 3 khâu phẳng (hình 5.2). Tính vận tốc thẳng v và vận tốc góc ω . Hình 5.2: Tìm vận tốc khâu 2 của robot 3 khâu phẳng. Ta có [ ] qJJJ v  321 =       ω (5.61) trong đó các cột của Jacobian được xác định theo công thức (5.59) và (5.60) với n O được thay bằng c O : 10 [...]...J 1 = z0 x (Oc − O0 ) J 2 = z1 x (Oc − O1 ) (5.62) J3 = 0 Vì vận tốc của khâu 2 không bị ảnh hưởng bởi sự chuyển động của khâu 3 (chỉ xét về mặt động học) Thật vậy, phản lực của khâu 3 sẽ ảnh hưởng đến sự chuyển động của khâu 2; vấn đề này sẽ được xét trong phần khảo sát động lực học Ví dụ 5.6: Xét tay máy 2 khâu phẳng như hình vẽ Ma trận Jacobian có dạng như sau:  z x...  0   0  0   1   (5.65) Ta thấy rằng 2 hàng đầu tiên của (5.65) là Jacobian vận tốc thẳng của O2 tương đối với nền Hàng thứ 3 là Jacobian vận tốc thẳng theo hướng z0 , bằng 0 trong trường hợp này Ba hàng cuối biểu diễn vận tốc góc của hệ tọa độ cuối cùng, chỉ đơn giản là phép quay   quanh trục thẳng đứng với tốc độ θ1 + θ 2 Ví dụ 5.7: Tay máy SCARA Jacobian là một ma trận 6x4 vì SCARA chỉ... kỳ dị cho tay máy là quan trọng vì nhiều lý do: (1) Các điểm kỳ dị biểu diễn các vị trí tại đó hướng của chuyển động không tính được (2) Tại các điểm kỳ dị, vận tốc bao của cơ cấu tác động cuối có thể tương đương với vận tốc khớp không bị bao (3) Tại các điểm kỳ dị, lực bao của cơ cấu tác động cuối có thể tương đương với mômen quay của khớp không bị bao (4) Các điểm kỳ dị thường (không luôn luôn) tương... = 0   0  1   − a2 s12 a2 c12 0 0 0 1 5.7 Kỳ dị (Singularities) 12 0 0 0 0  −1 0   0 0 0 0  0 − 1  (5.67)   Jacobian J (q) ∈ (6xn) xác định một ánh xạ giữa q (vận tốc khớp) và X = ( v, ω)T (vận tốc cơ cấu tác động cuối):   X = J (q)q (5.68) (5.68) xác định một phép biến đổi tuyến tính vi phân giữa dq và dX dX = J (q) dq (5.69) Các vi phân này có thể có thể nghĩ như là việc xác định... bất kỳ giá trị của dq1 và dq2 không làm thay đổi dy Nên bất kỳ vectơ dX có thành phần thứ 2 khác không biểu diễn hướng không thể đạt được của chuyển động tức thời 5.8.1 Tách các điểm kỳ dị 13 Ta thấy rằng có thể tính được một tập các phương trình động học thuận cho tay máy bất kỳ bằng cách gắn cứng một hệ tọa độ vào từng khâu theo cách bất kỳ, tính các phép biến đổi homogenous và nhân chúng lại với... điểm trong không gian làm việc tay máy không thể đạt tới khi có sự thay đổi nhỏ của tham số khâu, như chiều dài khâu, độ dời, v.v (6) Gần các điểm kỳ dị sẽ không tồn tại nghiệm duy nhất cho bài toán động học nghịch Trong trường hợp như thế có thể vô nghiệm hay vô số nghiệm Ví dụ 5.8: Xét phương trình hệ thống hai chiều 1 1 dX = Jdq =   dq 0 0  (5.70) tương đương với 2 phương trình dx = dq1 + dq2... thì vị trí của tay máy, về bản chất là các đại lương hình học, độc lập với việc gán các hệ tọa độ đó Điều này cho phép ta tách việc xác định các vị trí kỳ dị, cho các tay máy khớp cầu, thành 2 bài toán đơn giản hơn Bài toán 1 là xác định vị trí kỳ dị cho cánh tay, bao gồm 3 khâu hay nhiều hơn, bài toán thứ 2 là xác định kỳ dị cho cổ tay từ chuyển động của cổ tay khớp cầu Xét tay máy có 3 bậc tự do cho . mối liên hệ giữa vận tốc thẳng và vận tốc góc của cơ cấu tác động cuối với vận tốc khớp )(tq  . Đặt T nnn RRS )()( 000  = ω (5.38) biểu thị vận tốc góc 0 n ω của cơ cấu tác động cuối, và đặt 00 nn Ov  = . khâu để tính phương trình động lực học của chuyển động. Ví dụ 5.5: Xét tay máy 3 khâu phẳng (hình 5.2). Tính vận tốc thẳng v và vận tốc góc ω . Hình 5.2: Tìm vận tốc khâu 2 của robot 3 khâu. chuyển động không tính được. (2). Tại các điểm kỳ dị, vận tốc bao của cơ cấu tác động cuối có thể tương đương với vận tốc khớp không bị bao. (3). Tại các điểm kỳ dị, lực bao của cơ cấu tác động