1 PHẦN 1: XÂY DỰNG CẤU TRÚC ĐỘNG HỌC Để xây dựng cấu trúc động học robot, ta thực nội dung sau: - Xác định cấu trúc động học robot: Vẽ sơ đồ động học, tính số bậc tự - Thiết lập mơ hình tốn học - Lựa chọn tham số động học tính tốn số 1.1 Vẽ sơ đồ động học, tính số bậc tự Đây robot khâu với cấu hình RRR Số bậc tự robot xác định sau: Trong đó: f - bậc tự cấu fi - số bậc tự chuyển động cho phép khớp i k - số khớp hệ, n - số khâu động hệ λ - số bậc tự không gian cấu thực chuyển động fc - số ràng buộc thừa fp - số bậc tự thừa Cụ thể mơ hình robot ta có: f = ( robot có bậc tự ) 1.2  Xây dựng hệ tọa độ khảo sát Cơ sở lý thuyết Việc gắn hệ tọa độ có vai trị quan trọng thiết lập hệ phương trình động học robot Qui tắc thiết lập hệ tọa độ Denavit-Hartenberg tóm lược sau: Từ khấu đế - gốc, khâu khớp đánh số liên tiếp Gốc xem khâu 0, khâu cuối khâu tác động cuối Ngoại trừ gốc khâu cuối, khâu còn lại đều bao gồm hai khớp Khớp thứ i liên kết khâu thứ i với khâu i-1 Dựng đường vng góc chung giữa trục khớp kề Ngoại trừ gốc khâu cuối, trục mỗi khớp (i) đều gắn với đường vng góc chung, với trục khớp động thứ (i-1) trục khớp động thứ (i +1) 3 Thiết lập hệ tọa độ gốc, ví dụ z0 dọc theo trục khớp động thứ nhất, x0 chọn vng góc với z0,, trục y0 xác định theo qui tắc bàn tay phải Thiết lập hệ tọa độ bàn kẹp khâu thứ n thỏa mãn x n vng góc với trục khớp liền trước Trục zn chọn hướng tiếp cận khâu cuối 5.Gắn hệ tọa độ Đề tại khớp cuối tất cả khâu sau: - Trục zi chọn dọc theo hướng trục khớp động thứ (i+1), - Trục xi chọn dọc theo đường vng góc chung giữa hai trục zi-1 zi , hướng từ zi-1 sang zi Nếu trục song song, xi chọn đường vng góc chung trục Trong trường hợp trục cắt nhau, gốc chọn tại giao điểm hướng trục xi xác định qua tích hữu hướng zi-1 x zi, - Trục yi xác định theo qui tắc bàn tay phải Xác định thông số khâu biến khớp ai, αi, θi, di, Có n+1 hệ tọa độ cho tay máy n bậc tự do.Tuy nhiên, hệ qui chiếu bở sung xác định, chúng liên hệ với hệ tọa độ ma trận chuyển đổi Chú ý rằng John Craig theo quan niệm khác; ông gắn hệ tọa độ thứ i tại khớp đầu khâu i, Ma trận phép biến đổi đồng cũng khác  Áp dụng vào robot Mơ hình xây dựng phần mềm solidworks với kết cấu sau:  Bảng thông số động học Khâu qi di i q1* d1 90 * q2 a2 * q3 a3 *:biến khớp 1.3 Tính ma trận truyền động học Ma trận i-1Hi có dạng i-1 Hi = R(z,i).Tp(0,0,di).Tp(ai,0,0).R(x,i) (Theo đề i = 1,2,3) Dạng tổng quát ma trận truyền Denavit-Hartenberg  sin q i cos  i sin qi sin  i cos q i cos  i  cos qi sin  i sin  i cos  i cos q i  sin q i  i-1 Hi =    0 a i cos q i  a i sin q i   di    Nên ta có: Ma trận H1 mơ tả vị trí hướng khâu thứ với gốc cố định: cos q1 sin q H1      0 sin q1  cos q 0 0 0  d1   1 Ma trận H2 mô tả vị trí hướng khâu thứ hai so với khâu thứ nhất: cos q  sin q H2       sin q cos q 0 a cos q  a sin q      Ma trận H3 mô tả vị trí hướng khâu thứ ba so với khâu thứ hai: cos q  sin q H3       sin q cos q 0 a cos q  a sin q      Ma trận biến đởi tọa độ th̀n 0Hi có dạng : Hn = 0H1.1H2…n-1Hn =H1.H2…Hn n ký hiệu hệ tọa độ (theo đề n = 1,2,3) Nên ta có: Ma trận 0H3 mơ tả vị trí hướng khâu thứ ba so với giá cố định H3= H1.H2.H3 C1C 23 S C H   23  S 23    C1 S 23 S1  S1 S 23  C1 C 23 0 C1 [a C  a C 23 ]  S1 [a C  a C 23 ]   d  a S  a S 23    Trong ký hiệu: -Ci: cos biến khớp qi - Si: sin biến khớp qi - Sij: sin tổng biến khớp sin(qi+qj) - Cij: sin tổng biến khớp cos(qi+qj) Việc tính xây dựng mà trận truyền thực matlab với chương trình sau: % hàm khởi tạo biến: syms t a2 a3 d1 d2 pi real; syms nx sx ax px ny sy ay py nz sz az pz real; %hàm tính ma trận DH function A=DH(q,alpha,a,d) A = [cos(q) -sin(q)*cos(alpha) sin(q)*sin(alpha) a*cos(q); sin(q) cos(q)*cos(alpha) -cos(q)*sin(alpha) a*sin(q); sin(alpha) cos(alpha) d; 0 1]; End %hàm tính ma trận truyền syms q1 q2 q3 t a2 a3 d1 d2 pi real; A=cell(3,1); A{1}=DH(q1,pi/2,0,d1); A{2}=DH(q2,0,a2,0); A{3}=DH(q3,0,a3,0); Ma trận 0H3 cho ta biết hướng vị trí khâu cuối (end-effactor) hệ tọa độ cố định O0 x0 y0 z0 mơ tả hướng vị trí hệ tọa độ O3 x3 y3 z3 hệ tọa độ cố định O0 x0 y0 z0 1.4 Thiết lập phương trình động học Hn (q) = 0H1(q) 1H2(q) …n-1Hn(q) n ký hiệu hệ tọa độ H3 (q)=   R3 (q) 0 rE (q)   0  c11 (q) c12 (q) c13 (q)  R3 (q)  c21 (q) c22 (q) c23 (q)   c31 (q) c32 (q) c33 (q)    rE (q)  x(q) y ( q ) z ( q ) T q  q1 q2 q3  T  c11 (q) c12 (q) c ( q ) c ( q ) 22 H n (q)   21 c31 (q) c32 (q)   C1C 23  C1 S 23 S C  S1 S 23 H   23  S 23 C 23   x( q )  c 23 (q) y (q)  c33 (q) z (q)    S1 C1 [a C  a C 23 ]   C1 S1 [a C  a C 23 ]   d  a S  a S 23    c13 (q) Gọi 0H3(t) ma trận mô tả vị trí hướng “ điểm tác động cuối ” khâu cuối:  c11 (t ) c12 (t ) c13 (t ) c (t ) c (t ) c (t ) 22 23 H (t )   21 c 31 (t ) c 32 (t ) c 33 (t )  0  x E (t )  y E (t )  z E (t )    Hệ phương trình động học dạng ma trận: 0H3 (q) = 0H3 (t) C1C 23 S C  23  S 23   1.5  C1 S 23 S1  S1 S 23  C1 C 23 0 C1 [a C  a C 23 ]   c11 (t ) c12 (t ) c13 (t ) C1 [a C  a C 23  c 21 (t ) c 22 (t ) c 23 (t )   d  a S  a S 23  = c31 (t ) c32 (t ) c33 (t )   0   x E (t )  y E (t )  z E (t )    Lựa chọn tham số động học tính tốn số Các thơng số di, xác định từ hình dáng robot Theo thiết kế ta có: d1=140 cm a2=150 cm a3=152 cm PHẦN : KHẢO SÁT BÀI TOÁN ĐỘNG HỌC 2.1 Bài toán động học thuận a Bài toán thuận vị trí So sánh hai ma trận ta tính góc Cardan: sin(roty )  a13  cos(roty )    sin (roty )  sin(rotx)   a23 cos(roty )    a33 cos(rotx)  cos(roty )   a12 sin(rotz )   cos(roty )   a11 cos(rotz )  cos(roty )   b Bài toán thuận vận tốc 3 𝑖=1 𝑖=1 𝜕𝑓 𝛼 𝜕𝑓 𝛼 ∑ 𝑝̇ 𝑖 = − ∑ 𝑞̇ , 𝛼 = ̅̅̅̅ 1,3 𝜕𝑝 𝑖 𝜕𝑞 𝑖 𝑖  f1  p   f  p     f  p1  f1 p2 f p2 f p2 f1   f1    q p3 p  1  f     f p3   p2     q1           f   p3   f  q1 p3    10 f1 d f d f d f1  q3   q   1 f    q3   q2        f   q3    q3   0, sin(alpha1), 0, 0, cos(alpha1), 0, d1 1] theta2=sym(input('theta2=')); alpha2=sym(input('alpha2=')); a2=sym(input('a2=')); d2=sym(input('d2=')); A2 =[ cos(theta2), -cos(alpha2)*sin(theta2), sin(alpha2)*sin(theta2), cos(alpha2)*cos(theta2), -sin(alpha2)*cos(theta2), a2*cos(theta2) sin(theta2), a2*sin(theta2) 0, sin(alpha2), 0, 0, cos(alpha2), 0, d2 1] theta3=sym(input('theta3=')); alpha3=sym(input('alpha3=')); a3=sym(input('a3=')); d3=sym(input('d3=')); A3 =[ cos(theta3), -cos(alpha3)*sin(theta3), sin(alpha3)*sin(theta3), cos(alpha3)*cos(theta3), -sin(alpha3)*cos(theta3), a3*cos(theta3) sin(theta3), a3*sin(theta3) 0, sin(alpha3), 0, 0, cos(alpha3), 0, 1] A =A1*A2*A3 fprintf('\nbai toan nguoc\n') A01 =inv(A1) 15 d3 A02 =inv(A2) T1 =A01A3 T2 =A02A01A3 T01=A2A3 16 PHẦN : TÍNH TỐN ĐỘNG LỰC HỌC 3.1 Tính đại lượng động lực học robot - Lực tác dụng lên khâu robot ( dạng vecto ):  i RCi i F i ,i 1 i F i 1,i i P i  i i i i i i i i  RCi  M i ,i 1  M i 1,i ( r i  r Ci )  F i ,i 1  r Ci ( F i 1,i )  - Áp dụng phương trình động học Euler ta được:  i Fi ,i 1 i F i 1,i i P i  mi vCi    i M i ,i 1 i M i 1,i (i r i i r Ci ) i F i ,i 1  i r Ci i F i 1,i  iCi iBi  iBi iCi iBi  - Sử dụng ma trận quay để biểu diễn hệ tọa độ:  i 1 Fi ,i 1  i 1Ri i F i ,i 1   i 1 i 1 i  M i ,i 1  Ri M i ,i 1  - Mặt khác ta có: vCi  JTi (q)q : vCi   xCi yCi zCi  T q  1d23     xCi   1  y J Ti   Ci  1  zCi   1 T xCi d yCi d zCi d xCi   3  yCi   3  zCi   3  17 i  J Ri (q)q T i  ix iy iz     ix ix ix    d 3     iy iy iy  J Ri    d 3   1  iz iz iz    d 3   1 aCi  vCi  JTi (q)q  J Ti (q )q  Ci  i  J Ri (q)q  J Ri (q)q Thay tất cả giá trị vào hệ phương trình (1) ta được: mi JTi q  mi JTi q  Fi a  Fi c  (2)  Ci J Ri (q)q  Ci J Ri (q)q  BiCiBi  M ia  M ic  Trong đó: Fa – Lực hoạt động, Fc – lực liên kết Giải thuật chương trình tính tốn - Giải thuật tinh tốn trình bày sơ đồ phía - Về chương trình tính tốn: robot đơn giản, việc tính tốn thực bằng tay.Một số phép tốn như: Nhân ma trân,ma trận nghịch đảo… thực bằng phần mềm Matlap 18 Sử dụng phần mềm Maple tính toán kết quả: c1  1s1 1  J R1 (q)q   s1  1c1    0  1    0   d   [ (c1  1s1 )1  0  3    0 0   1    2  J R (q)q  0 0   d   [ 0 ]   0 0  3     19 ( s1  1c1 )1 0] 0 0  1    3  J R (q)q  0 0   d   [ 0 3 ]   0  3     3.2 Thiết lập phương trình vi phân chuyển động Áp dụng phương trình vi phân chuyển động robot sử dụng phương trình Newton – Euler:  i Fi ,i 1 i F i 1,i i P i  mi vCi    i M i ,i 1 i M i 1,i (i r i i r Ci ) i F i ,i 1  i r Ci i F i 1,i  iCi iBi  iBi iCi iBi   Xét khâu thứ FE ,3  300 300 0 N M E ,3  0 500 Nm P3  0 m3 g 0  0 100 0 ( chọn m3=10kg ) FE ,3  R0 FE ,3 c13   s13    c13 P3  R00 P3   s13    M E ,3  R0 M E ,3  s13 0 300 c13 0 300  300(c13  s13 ) 300(c13  s13 ) 0   1      s13 0   c13 0  100  100s13   1     c13   s13    100c13 0  s13 0   c13 0    0 500    500   Lực: 20 3 F3,2 3 F E ,3 3 P  m3vC   150 s13 (1  3 )  300(c13  s13 )   100 s13  300(c  s )    100c   100  150c (   )  F3,2   13 13  13  13          0       300c13  200 s13  15000 s13 (1  3 )   0,3c13  0, s13  15s13 (1  3 )      F3,2   400c13  300 s13  15000c13 (1  3 )   0, 4c13  0,3s13  15c13 (1  3 )      0     Momen: M 3,2 3 M E ,3 (3 r 3 r C ) 3 F 3,2  r C 3 F E ,3  3C 3B3  3B3 3C 3B3 M E ,3  R0 M E ,3 3 F3,2  s13 0   c13 0    0 500    500   0 3      3    0    3 0  3 0 0  0 0  r3   a3 0; 3rC   a3 / 0; 3r3  3rC  a3 / 0 0  0 0  0  0 0 0   0  ; r  0 r  r C3   a3 / 2  0,15 C  0,15  0 a3 / 0 0,15   0 0,15        C 3 c13   s13     0,3c13  0, s13  15s13 (1  3 )  300(c13  s13 )       0, 4c13  0,3s13  15c13 (1  3 )  ; FE ,3  300(c13  s13 )      0     0 3    ;   3    3  J xx    J yx  J zx  J xy J yy J zy J xz   J yz  ; Trong đó: J zz   21 J xx   ( y  z )dm  0; J yy   ( x  z )dm  a3 a2 m3 ; J zz   ( y  x )dm  m3 ; 4 J xy  J yx    xydm  0; J xz  J zx    xzdm  0; J zy  J yz    yzdm  C 3  0   0   0     0   0 0 0, 225     0 0, 225   a3   m3   a3 m3 0  0,3c13  0, s13  15s13 (1  3 )  0   0 (3 r 3 r C ) 3 F 3,2   0,15 0, 4c13  0,3s13  15c13 (1  3 )     0 0,15     (3 r 3 r C ) 3 F  0 0,15.[0, 4c13  0,3s13  15c13 (1  3 )]   0  300(c13  s13 )  0 0 r C  F E ,3   0,15 300(c13  s13 )   0 45(c13  s13 )   0 0,15       C 3 3,2 0 0 0 0 0, 225 B       0 0, 2253      0 0, 225 3     B 3C 3B 3 0   3 0  3 0  0 0    0 0, 2253   0 0 0, 225     0   0 0 0, 225 3  0     0        0 0     3    Vậy momen: M 3,2 M 3,2 0  0,3(c13  s13 )        0,3(c  s )        0 13 13          0 0,15.[0, 4c13  0,3s13  15c13 (1  3 )] 45(c13  s13 )  0, 2253          0,3(c13  s13 )      0,3(c13  s13 )  0,15.[0, 4c13  0,3s13  15c13 (1  3 )] - 45(c13  s13 )  0, 2253    22  Xét khâu thứ  F2,1 2 F 3,2 2 P  m2vC    M 2,1 2 M 3,2 ( r  r C ) 2 F 2,1  r C 2 F 3,2  2C 2B  2B 2C 2B  3 F3,2 M 3,2  0,3c13  0, s13  15s13 (1  3 )     0, 4c13  0,3s13  15c13 (1  3 )  N     0,3(c13  s13 )      0,3(c13  s13 )  0,15.[0, 4c13  0,3s13  15c13 (1  3 )] - 45(c13  s13 )  0, 2253    P2  0 m2 g 0  0 100 0 ( chọn m2=10kg ) c3 F3,2  R3 F3,2   s3     s3 c3 0   0,3c13  0, 2s13  15s13 (1  3 )     0, 4c13  0,3s13  15c13 (1  3 )     1     0,3c133  0, s133  0, 2c13 s3  15s133 (1  3 ) 0,3s133  0, 2c133  0, 2c13c3  15c133 (1  3 )      0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t 0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t 0 c1  s1 0   P2  R00 P2   s1 c1 0  0,1  0,1s1 0,1c1 0  0,1sin 3t 0,1cos3t 0    0   1    vC  [ d2 c1.1  s1.d2 2 d2 s1.1  c1.d 2 0]= 30t cos3t 10sin 3t 30t sin 3t 10cos3t  vC  30cos3t  90t sin 3t 30cos3t 90t cos3t  60sin 3t  Lực: 23 F2,1  F 3,2  P  m2 vC   0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t   0,1sin 3t  30 cos 3t  90t sin 3t  0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t    0,1cos 3t   10   F2,1   30 cos 3t          90t cos 3t  60sin 3t  0       0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t   F2,1   0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t     900t cos 3t  600sin 3t   Momen: M 2,1 2 M 3,2 (2 r 2 r C ) 2 F 2,1  r C 2 F 3,2  2C 2B  2B 2C 2B 2 0,3cos 7t  0, 2cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300cos 3t  900t sin 3t   F2,1   0,3sin 7t  0, 2cos 5t cos 2t  74,8cos 7t  300,1cos 3t     900t cos 3t  600sin 3t   M 3,2  R M 3,2 2 3 c3   s3     s3 c3 0  0,3(c13  s13 )    0  0,3(c13  s13 )   0,15.[0, 4c13  0,3s13  15c13 (1  3 )] - 45(c13  s13 )  0, 2253     0,3(c133  s133 ) 0,3(c133  s133 ) 0,15.[0, 4c13  0,3s13  15c13 (1  3 )] - 45(c13  s13 )  0, 2253      0,3(cos 7t  sin 7t ) 0,3(cos 7t  sin 7t ) 56, cos 5t  45sin 5t  F3,2  0,3cos 7t  0, 2cos5t sin 2t  74,8sin 7t 0,3sin 7t  0, 2cos5t cos 2t  74,8cos 7t 0 0  3  0  ;   0    0  3  0    0    0 0    0 0    0 0    => M 2,1 2 M 3,2 (2 r  r C ) 2 F 2,1  r C 2 F 3,2 r2  0 d2 ; 3rC  0 d2 / 2  0 10t ; r2  rC  0 10t  r 2 r C  10t 0  10t 0  10t 0 ; r C   10t 0      0  0 0 0     24 10t  0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t   0  0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t    0  900t cos 3t  600sin 3t     10t (0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t ) 10t (0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )  ( r  r C ) 2 F 0  10t 2,1  0  10t   0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t    10t 0  0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t   r C  F 3,2       0      10t (0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t ) 10t (0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t ) 0 2 Vậy momen:  0,3(cos 7t  sin 7t )   10t (0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t )  M 2,1   0,3(cos 7t  sin 7t )    10t (0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t )        56, cos 5t  45sin 5t        10t (0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t )   10t (0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )        2 0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, 4cos 5t cos 2t  149, 6cos 7t  300,1cos 3t )   0,3(cos 7t  sin 7t )  10t (0, 6cos 7t  0, 4cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300cos 3t  900t sin 3t )  M 2,1      56, 2cos 5t  45sin 5t    Xét khâu thứ  F1,0 1 F 2,1 1 P  m1vC1    M1,0 1 M 2,1 (1 r 1 r C1 ) 1 F 1,0  r C1 1 F 2,1  1C1 1B1  1B1 1C1 1B1  0,3cos 7t  0, 2cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300cos 3t  900t sin 3t   F2,1   0,3sin 7t  0, 2cos 5t cos 2t  74,8cos 7t  300,1cos 3t  N   900t cos 3t  600sin 3t   0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, 4cos 5t cos 2t  149, 6cos 7t  300,1cos 3t )   0,3(cos 7t  sin 7t )  10t (0, 6cos 7t  0, 4cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300cos 3t  900t sin 3t )  M 2,1      56, 2cos 5t  45sin 5t   P  0 m1 g 0  0 100 0 ( chọn m1=10kg ) 25 1 0  0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t   F2,1  1R2 F2,1  0 1  0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t    0    900t cos 3t  600sin 3t    0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t    900t cos 3t  600sin 3t     0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t    c1 s1    1 P  R0 P   0 1  0,1   0,1s1 0,1c1    0,1sin 3t 0,1cos 3t  1      s1 c1       T vC1  [501s1 501s1 0]= 150sin 3t 150sin 3t 0 vC1   450cos3t 450cos3t 0 Lực: F1,0 1 F 2,1 1 P  m1vC1  0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t   F1,0   900t cos 3t  600sin 3t     0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t    0,1sin 3t   450 cos 3t     10  450 cos 3t       0,1cos 3t        0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  900t sin 3t  4800 cos 3t   F1,0   900t cos 3t  600sin 3t  4500 cos 3t     0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300, cos 3t   Momen: 1 M1,0 1 M 2,1 (1 r 1 r C1 ) 1 F 1,0  r C1 1 F 2,1  1C1 1B1  1B1 1C1 1B1 0,3cos 7t  0, 2cos 5t sin 2t  74,8sin 7t  900t sin 3t  4800cos 3t   F1,0   900t cos 3t  600sin 3t  4500cos 3t     0,3sin 7t  0, 2cos 5t cos 2t  74,8cos 7t  300, 2cos 3t   26 0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, cos 5t cos 2t  149, cos 7t  300,1cos 3t ) 1 0    M 2,1  1R2 M 2,1  0 1 0,3(cos 7t  sin 7t )  10t (0, cos 7t  0, cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )     0    56, cos 5t  45sin 5t    0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, cos 5t cos 2t  149, cos 7t  300,1cos 3t )     56, cos 5t  45sin 5t   0,3(cos 7t  sin 7t )  10t (0, cos 7t  0, cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )    0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t   F2,1   900t cos 3t  600sin 3t     0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t   T T  18sin 3t  27t cos 3t  1   18cos 3t  27t sin 3t         (c1  1s1 )1  3(cos 3t  3t sin 3t )    1  ( s1  1c1 )1   3(sin 3t  3t cos 3t )  ;       0     0 3(sin 3t  3t cos 3t )    1   0 3(cos 3t  3t sin 3t )    3(sin 3t  3t cos 3t ) 3(cos 3t  3t sin 3t )    r1   a1 0; 1rC1   a1 / 0   0,05 0; 3r3  3rC  0,05 0 1 0  0  0 0 0  ; r  0 r  r C1   0, 05 C1  0, 05  0 0, 05 0 0, 05        J xx  C1   J yx  J zx  J xy J yy J zy J xz   J yz  ; Trong đó: J zz   J xx   ( y  z )dm  a12 a2 m1  0, 25; J yy   ( x  z )dm  m1  0, 25; J zz   ( y  x )dm  0; 4 J xy  J yx    xydm  0; J xz  J zx    xzdm  0; J zy  J yz    yzdm  0 0 0, 25   C1   0, 25    0 0   27 0  0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  900t sin 3t  4800 cos 3t  0 0  (1 r 1 r C1 ) 1 F 1,0   0, 05  900t cos 3t  600sin 3t  4500 cos 3t   0 0, 05   0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300, cos 3t       0, 05(0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300, cos 3t )  1 ( r  r C1 )  F 1,0      0, 05(900t cos 3t  600sin 3t  4500 cos 3t )   0  0,3cos 7t  0, cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t  0 0  r C1  F 2,1   0, 05  900t cos 3t  600sin 3t   0 0, 05   0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t      0, 05(0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t )  1 r C1  F 2,1      0, 05(900t cos 3t  600sin 3t )    C1 T T 0  18sin 3t  27t cos 3t  0, 25( 18sin 3t  27t cos 3t )  0, 25  B1   0, 25 0  18cos 3t  27t sin 3t    0, 25(18cos 3t  27t sin 3t )          0  0      0 3(sin 3t  3t cos 3t )  0, 25 0  3(cos 3t  3t sin 3t )     B1 C1 B1   0 3(cos 3t  3t sin 3t )   0, 25  3(sin 3t  3t cos 3t )      3(sin 3t  3t cos 3t ) 3(cos 3t  3t sin 3t )   0 0      0  3(cos 3t  3t sin 3t )    0  3(sin 3t  3t cos 3t )   0 0     0, 75(sin 3t  3t cos 3t ) 0, 75(cos 3t  3t sin 3t )       1 Vậy momen: 28 0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, cos 5t cos 2t  149, cos 7t  300,1cos 3t )    M 1,0   56, cos 5t  45sin 5t   0,3(cos 7t  sin 7t )  10t (0, cos 7t  0, cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )      0, 25(18sin 3t  27t cos 3t )    0, 05(0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300, cos 3t )    0, 25(18cos 3t  27t sin 3t )           0, 05(900t cos 3t  600sin 3t  4500 cos 3t )        0, 05(0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300,1cos 3t )      0, 05(900t cos 3t  600sin 3t )   0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, cos 5t cos 2t  149, cos 7t  300,1cos 3t )  4,5sin 3t  6, 67t cos 3t )      56, cos 5t  45sin 5t  0,1(0,3sin 7t  0, cos 5t cos 2t  74,8 cos 7t  300, cos 3t )  0, 25(18cos 3t  27t sin 3t )  0,3(cos 7t  sin 7t )  10t (0, cos 7t  0, cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )  0,1( 900t cos 3t  600sin 3t )  225cos 3t )    Tóm lại: lực/momen điều khiển U là: 3 F3,2 0,3c13  0, s13  15s13 (1  3 )  0,3cos 5t  74,8sin 5t     0, 4c13  0,3s13  15c13 (1  3 )    0,3sin 5t  74, 6sin 5t        0     M 3,2  0,3(cos5t  sin 5t )    0,3(cos5t+sin 5t )     56, 2cos5t  45sin 5t )    0,3cos 7t  0, 2cos 5t sin 2t  74,8sin 7t  0,1sin 3t  300cos 3t  900t sin 3t   F2,1   0,3sin 7t  0, 2cos 5t cos 2t  74,8cos 7t  300,1cos 3t     900t cos 3t  600sin 3t   0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, 4cos 5t cos 2t  149, 6cos 7t  300,1cos 3t )   0,3(cos 7t  sin 7t )  10t (0, 6cos 7t  0, 4cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300cos 3t  900t sin 3t )  M 2,1      56, 2cos 5t  45sin 5t   0,3cos 7t  0, 2cos 5t sin 2t  74,8sin 7t  900t sin 3t  4800cos 3t   F1,0   900t cos 3t  600sin 3t  4500cos 3t     0,3sin 7t  0, 2cos 5t cos 2t  74,8cos 7t  300, 2cos 3t   0,3(cos 7t  sin 7t )  10t (0, 6sin 7t  0, cos 5t cos 2t  149, cos 7t  300,1cos 3t )  4,5sin 3t  6, 67t cos 3t )    M1,0   56, cos 5t  45sin 5t  0,1(0,3sin 7t  0, cos 5t cos 2t  74,8cos 7t  300, cos 3t )  0, 25(18cos 3t  27t sin 3t )   0,3(cos 7t  sin 7t )  10t (0, cos 7t  0, cos 5t sin 2t  149, 6sin 7t  0,1sin 3t  300 cos 3t  900t sin 3t )  0,1(900t cos 3t  600sin 3t )  225cos 3t )    29 ... ĐỘNG HỌC Để xây dựng cấu trúc động học robot, ta thực nội dung sau: - Xác định cấu trúc động học robot: Vẽ sơ đồ động học, tính số bậc tự - Thiết lập mơ hình tốn học - Lựa chọn tham số động học. .. =inv(A2) T1 =A01A3 T2 =A02A01A3 T01=A2A3 16 PHẦN : TÍNH TỐN ĐỘNG LỰC HỌC 3.1 Tính đại lượng động lực học robot - Lực tác dụng lên khâu robot ( dạng vecto ):  i RCi i F i ,i 1 i F i 1,i i P i... tham số động học tính tốn số Các thơng số di, xác định từ hình dáng robot Theo thiết kế ta có: d1=140 cm a2=150 cm a3=152 cm PHẦN : KHẢO SÁT BÀI TOÁN ĐỘNG HỌC 2.1 Bài toán động học thuận a Bài