Bài tập 4.1: Cho tay máy cấu hình dạng RR gồm khâu chuyển động quay tương mặt phẳng sơ đồ hình vẽ Y L1 Hãy xác định phương trình chuyển động điểm E khâu tọa độ điểm so với hệ sở θ1 X Cách 1: Sử dụng ma trận chuyển (quay tịnh tiến) Thực phép quay/tịnh tiến quanh hệ tọa độ thời Y2 = T10 T21 X2 E 02 Y1 T0(q) = R(Z0, θ1) T(X’0, L1) R(Z1, θ2) T(X’1, L2) T20 E X1 Zθ0 Y0 Y’1 X1 θ2 r0 01 θ1 T  Cosθ - Sinθ  Sinθ Cosθ 1 =   0   0 L1Cosθ  L1Sinθ  0  ;    X’1 X0  Cosθ - Sinθ  Sinθ Cosθ 2 T2 =   0   0 L 2Cosθ  L Sinθ  0     - Ma trận thể phương trình chuyển động điểm E:  Cos (θ + θ ) - Sin(θ1 + θ )  Sin(θ + θ ) Cos(θ + θ ) 2 0 T (q) = T1 T2 =   0  0  0 L1Cosθ + L2 Cos (θ + θ )  L1Sinθ + L2 Sin(θ + θ )  0     Chú ý áp dụng công thức: Cos(A ± B) = cosA.cosB + sinA.sinB Sin(A ± B) = sinA.cosB ± cosA.sinB - Tạo độ điểm E hệ sở: giả sử điểm E trùng gốc O2 (rE = [0, 0, 0, 1]T)  cos (θ + θ ) - sin(θ + θ ) L1cosθ + L2 cos (θ + θ )     L1cosθ + L2 cos (θ + θ )   sin(θ + θ ) cos (θ + θ ) L sinθ + L sin(θ + θ )     L sinθ + L sin(θ + θ )  1 2    =  1 rE =      0  0      0 1   1    Cách 2: Do toán đơn giản nên tá áp dụng công thức dạng tổng quát ma trận chuyển đổi từ khâu hệ sở : E r0 L1 θ1  n0 (q ) T (q ) =   θ1+ θ2 02 Y0 X2 Y2 θ2 L2 01 X0 O ( q) a (q) P (q)   0  Trong đó: n0 ( q ) hướng X2 hệ sở O (q ) hướng Y2 hệ sở a (q) hướng Z2 hệ sở P (q ) vị trí gốc O2 hệ sở  nx  1.Cos(θ1 + θ )   Cos(θ1 + θ )   ax        n0 (q) =  ny  = 1.Sin (θ1 + θ )  =  Sin (θ1 + θ )  ; a ( q) =  ay  =  0  0    1     0 n   z   az     Ox   − Sin(θ1 + θ )   Px   L1Cosθ + L2Cos (θ + θ )        O (q ) =  Oy  =  Cos(θ1 + θ )  ; P ( q) =  Py  = L1Sinθ + L2 Sin(θ + θ )    0       0  Oz    Pz   Thay vào phương trình ta được: T (q) =  Cos (θ1 + θ ) - Sin(θ + θ )  Sin(θ + θ ) Cos(θ + θ ) 2   0  0  0 L1Cosθ1 + L2Cos(θ + θ )  L1Sinθ1 + L2 Sin(θ1 + θ )     Cách 3: Dùng hình học giải tích để thực phép biến đổi tọa độ mặt phẳng Y0 X2 Y2 E θ1+θ2 YE 02 L2 θ2 Y1 01 L1 00 θ1 X1 XE X0 Điểm tác động cuối E (E ≡ 02) biểu diễn hệ (0xyz)2 : rE2 = [ 0, 0, 0, 1] T Điểm E biểu diễn hệ sở (0xyz)0 : rE0 = [X E , YE , Z E , 1] T Trong : X E = L1 cos θ1 + L2 cos(θ1 + θ ) YE = L1 sin θ1 + L2 sin(θ + θ ) ZE = Vậy toa độ điểm E hệ sở : rE0 =  L1 cos θ + L2 cos(θ + θ )   L sin θ + L sin(θ + θ )  2         E Bài tập 4.2: Cho tay máy dạng RR gồm khâu có chiều dài L1, L2 chuyển động quay tương sơ đồ hình vẽ Hãy xác định phương trình chuyển động điểm E khâu tọa độ điểm L2 θ1 θ2 L1 Giải : dùng phép biến đổi D-H X2 - Xây dựng hệ tọa độ hình vẽ thực phép dịch chuyển: Y2 R(Z0, θ1) T(X0, L1) R(X1, α1) R(Z’1, θ2) T(X’1, L2) - Xác định thống số D-H: Z2 Z0 Khớpθ idiaiα i1θ10L19002θ20L20o E Y0 θ1 Y1 L1 X0 Z1 - L2 θ2 α1 X1 Xác định ma trận D-H dựa vào công thức: Aii-1  cos θ i sin θ i =      cos θ1 sin θ A1 =     − sin θ i cos α i sinθ i sin α i cosθ i cos α i − cos θ i sinα i sinα i cos α i 0 sinθ1 − cos θ1 0 T20 = A10 A12 = L1 cos θ1  L1 sin θ1  ;     cos θ1 cos θ sin θ cos θ   sin θ   cos θ i  sin θ i   di   cos θ sin θ A2 =     − cos θ1 sin θ sinθ1 − sin θ1 sin θ − cos θ1 cos θ 0 − sinθ cos θ 0 0 L2 cos θ  L2 sin θ     L1 cos θ1 + L2 cos θ1 cos θ  L1 sin θ1 + L2 sin θ1 cos θ   L2 sin θ   Điểm E biểu diển hệ sở:  L1 cos θ1 + L2 cos θ1 cos θ   L sin θ + L sin θ cos θ  2  rE =    L2 sin θ     E Bài tập 4.3: Cho tay máy dạng RP gồm khâu chuyển động quay tịnh tiến tương (hình vẽ) Khâu khâu di trượt vuông góc với Hãy xác định phương trình chuyển động điểm E thông số θ, d theo tọa độ X, Y Y L T10 rE0 = T10 rE2 ;  Cθ  Sθ     = : − Sθ Cθ 0  Cθ  Sθ     = T10 rE2 =  Cθ   Sθ 0  =   0 0   0 1  1  0 0    0    0 XE Tọa độ điểm E: YE ZE 0 − Sθ 0 L d 0 X1 L θ 00 X0 − Sθ L Cθ  Cθ L Sθ  0     + L Cθ  + L Sθ  − d Sθ + L Cθ = = = Y1 Y0 0   − d Sθ     d Cθ L Sθ d    =    0       1   Cθ X E L Cθ  θ1 GIẢI: a) Bài toán thuận: • Xác định phương trình chuyển động điểm E - Thiết lập hệ tọa độ hình vẽ - Thực phép biến đổi tọa độ: T10 = R(Z0, θ) T(X’0, L) Như phương trình chuyển động E thuộc khâu 2: rE0 d     d Cθ + L Sθ Từ hệ phương trình ta tính toán xác định giá trị X E(t) YE(t) liên tục quĩ đạo của điểm E rE0 Và ta được: X 2E + YE2 + Z 2E = = L2 + d • Xác định vận tốc điểm E: E V  − sin θ E = T r  cos θ = θ     VE0 = − cos θ − sin θ 0  − θ d cos   ( Yθ + L sin θ ) E  Y  L cosθ  d     =  − θ ( d sinEθ + L cos θ )  0  0         Y1 0  1    −L sinθ  0  X 2E + YE2 + Z 2E = θ 2(L2 + d ) d X1 Y1 L b) Bài toán nghịch: (Xác định thông số θ, d theo X, Y) 0O θ X X X Từ phương trình chuyển động điểm E thuộc khâu ta được: X E = L cosθ − d sinθ YE = L sinθ + d cosθ (4.2a) Vấn đề toán nghịch xác định thông số θi, di để thỏa mãn quĩ đạo cho trước điểm E (XE, YE) Đến người ta dùng thuật toán biến đổi kỹ phân tích, đoán nghiệm… • Xác định khoảng dịch chuyển di : toán đơn giản ta tiến hành theo cách: dựa trực tiếp vào cấu hình tay máy (hình vẽ) bình phương vế (4.2a) ta được: X E2 + YE2 = L2 + d Vậy ta : X E2 d = + • Xác định góc θi : YE2 − L2 (*) L cos θ − X E d Y − d cos θ sin θ = E L ⇒ cos θ = X E + d sin θ L Y − L sin θ cos θ = E d ⇒ sin θ = sin θ = Từ hệ (4.2a) ta rút ra: cosθ = tgθ = sin θ cosθ = YE L − X E d YE d + X E L ⇒ YE d + X E L L2 + d YE L − X E d L2 + d  YE L − X E d  ÷ ÷  YE d + X E L  θ = arctg  (**) Chú ý dấu (**) Hệ phương trình (*) (**) giúp ta xác định liệu điều khiển thời điểm t bất kỳ: di (t ) = X i2 (t ) + Yi2 (t ) − L2  LY i (t ) − X i (t ) X i2 (t ) + Yi (t ) − L2 ÷  θ i (t ) = arctg  Y (t ) X (t ) + Y (t ) − L2 + L.X (t ) ÷ i i i  i  Bài tập 4.4: Cho tay máy dạng RR gồm khâu chuyển động quay tương không gian sơ đồ hình vẽ E L2 θ2 L1 Z0 Hãy xác định phương trình chuyển động điểm E khâu θ1 Y0 X0 Y3 - Xây dựng hệ tọa độ hình vẽ - Xác định ma trận chuyển theo phép biến đổi: Z’1 Y2 Z2 ≡Z3 E θ2 X3 T30 = R(Zo, θ1) T(Zo, L1) R(Y2, θ2) = X2 Z0 T10 T21 T32 X’1 Y1 θ1 X0 Y0 X1 E d3 θ2 Bài tập 4.5: d1 θ1 Cho tay máy có khâu dạng RRT sơ đồ hình vẽ Cả khâu đồng phẳng, khâu khâu trượt tương Hãy thiết lập hệ phương trình động học robot xác dịnh tọa độ điểm E khâu cuối Z3 Y3 X3 d3 Y1≡Z2 Y’1 θ2 Z1 α1 α2 X1≡X2 X’1 Z0 Y0 θ1 X0 Z3 Y3 X3 d3 Z’1 θ2 Z2 α1 α2 Z1 θ1 Y’1 X’1≡X2 Y1 Y0 X1 X0 L2 E θ2 L1 θ1 θ2 θ2 θ3 L2 L3 Z0 θ1 θ1 Y0 Y0 X0 X0 Y3 Z0≡ Z1 Y2 Z2 Y2 θ3 X3 Y3 Z0≡ Z1 Z2 θ2 θ2 θ3 X4 X2 Y1 Y0 X1 Z3 ≡ Z4 X3 Y1 θ1 Z4 Z3 X2 X0 L3 L1 L1 Z0 θ3 L2 E L3 Y0 θ1 X0 X1 θ3 L2 θ2 L1 θ1 10 X4 Y4 X3 Y3 Y2 θ3 03 Y1 Y2 θ2 X1 θ1 00 X0 θ3 L3 L2 θ2 X1 Z0 θ2 02 θ1 00≡01 03 Y0 02 X1 θ3 Y1 X2 Y0 X3 Y3 X0 L4 E Y0 L1 θ1 b a X 11 X2 θ3 L3 L2 θ2 θ3 E Z1 L1 L4 θ1 X1 Z0 L3 L2 E θ2 Z0 Z1 Y0 L b θ1 Y1 X1 01 a 00 X0 θ3 L3 L4 L2 θ2 E θ3 L3 L1 Z0 L12 θ2 E θ1 L1 Y0 θ1 b a X0 12 θ3 Y3 Z2 ≡Z3’ E Z0 ≡Z1 X3 Z3 Y2 θ2 θ1 X2 Y1 Y0 X0 X1 13 ... d3 Y1≡Z2 Y 1 θ2 Z1 1 α2 X1≡X2 X 1 Z0 Y0 1 X0 Z3 Y3 X3 d3 Z 1 θ2 Z2 1 α2 Z1 1 Y 1 X 1 X2 Y1 Y0 X1 X0 L2 E θ2 L1 1 θ2 θ2 θ3 L2 L3 Z0 1 1 Y0 Y0 X0 X0 Y3 Z0≡ Z1 Y2 Z2 Y2 θ3 X3 Y3 Z0≡ Z1 Z2... L4 E Y0 L1 1 b a X 11 X2 θ3 L3 L2 θ2 θ3 E Z1 L1 L4 1 X1 Z0 L3 L2 E θ2 Z0 Z1 Y0 L b 1 Y1 X1 01 a 00 X0 θ3 L3 L4 L2 θ2 E θ3 L3 L1 Z0 L12 θ2 E 1 L1 Y0 1 b a X0 12 θ3 Y3 Z2 ≡Z3’ E Z0 ≡Z1 X3 Z3... X4 X2 Y1 Y0 X1 Z3 ≡ Z4 X3 Y1 1 Z4 Z3 X2 X0 L3 L1 L1 Z0 θ3 L2 E L3 Y0 1 X0 X1 θ3 L2 θ2 L1 1 10 X4 Y4 X3 Y3 Y2 θ3 03 Y1 Y2 θ2 X1 1 00 X0 θ3 L3 L2 θ2 X1 Z0 θ2 02 1 00≡ 01 03 Y0 02 X1 θ3 Y1 X2