Bài tập lớn ROBOTIC docx

BTL môn ROBOTICS Mục lục Chƣơng XÂY DỰNG CẤU TRÚC,THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT 1.1 Xây dựng cấu trúc robot 1.2 Thiết lập phƣơng trình động học robot Chƣơng BÀI TOÁN ĐỘNG HỌC 2.1 Bài toán động học thuận 2.2 Bài toán động học ngƣợc Chƣơng TÍNH TỐN TĨNH HỌC 3.1 Tính lực dẫn động khớp đảm bảo cân tĩnh Chƣơng TÍNH TỐN ĐỘNG LỰC HỌC 4.1 Xây dựng cấu trúc động lực học 4.2 Cơ sở lý thuyết 4.3 Xây dựng bảng tham số động học 4.4 Ma trận jacobi khâu 4.5 Ma trận khối lƣợng robot 4.6 Ma trận ly tâm quán tính coriolits 4.7 Thế robot 4.8 Phƣơng trình vi phân chuyển động khâu Chƣơng CHỌN BỘ ĐIỀU KHIỂN Phụ lục Code maple BTL môn ROBOTICS Chƣơng XÂY DỰNG CẤU TRÚC THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT 1.1 XÂY DỰNG CẤU TRÚC ROBOT 1.1.1 Đặt hệ quy chiếu Hình 1.1 Mơ hình robot hệ trục tọa độ - Hệ trục tọa độ OX0Y0Z0 đặt khâu đế, trục OZ0 có hƣớng dọc trục khớp động 1, trục OX0 nằm mặt phẳng vng góc với OZo có hƣớng từ xuống, trục OY0 xác định theo quy tắc bàn tay phải - Hệ trục tọa độ OX1Y1Z1 khớp động 2, trục OZ1 đặt dọc trục khớp động 2, trục OX1 vng góc với OZ0,OZ1 có hƣớng dọc theo khâu 1, trục OY1 xác định theo quy tắc bàn tay phải - Hệ trục tọa độ OX2Y2Z2 đặt trục khớp động 3, trục OZ2 đặt dọc trục khớp động 3, trục OX2 vng góc với OZ1 OZ2 hƣớng từ OZ1 sang OZ2, trục OY2 xác định theo quy tắc bàn tay phải - Hệ trục tọa độ OX3Y3Z3 đặt khâu thao tác, trục OX3 hƣớng theo hƣớng khâu OZ3 song song với trục OZ2, trục OY3 xác định theo quy tắc bàn tay phải BTL môn ROBOTICS 1.1.2 Thiết lập thơng số Denavit-Hartenbeg Từ mơ hình hệ trục tọa độ ta xây dựng đƣợc bảng thông số DanavitHartenbeg nhƣ sau : Bảng 1.1: Bộ thông số Denavit-Hartenbeg Khâu θi αi di θ1 900 a1 d1 θ2 a2 θ3 a3 Trong đó: θi góc quay quanh Zi-1 đển biến Xi-1 thành Xi αi góc quay quanh Xi để biến Zi-1 thành Zi Các biến khớp θ1, θ2, θ3, đặt biến khớp tƣơng ứng q1,q2,q3 Các ma trận biến đổi tọa độ Denavit-Hartenbeg dựa vào thông số : cos(q1 )  sin( q ) A1      0 sin(q1 ) a1 cos(q1 )  cos(q1 ) a1 sin(q1 )    d1  0  (1.1) cos(q2 )  sin( q2 )  sin(q ) cos(q ) 2 A2    0   0 a2 cos(q2 ) a2 sin(q2 )      (1.2) cos(q3 )  sin(q3 )  sin(q ) cos(q ) 3 A3    0   0 a3 cos(q3 ) a3 sin(q3 )      (1.3) BTL môn ROBOTICS 1.2 THIẾT LẬP PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT Phƣơng trình động học robot nhận đƣợc dạng ma trận nhƣ sau : A3 (q)  A3 (t ) (1.4) Trong C1C 23  C1 S 23 S1 a3C1C 23  a2 C1C  a1C1  S C  0  23  S1 S 23  C1 a3 S1C 23  a2 S1C  a1 S1  A3 (q)  A1 A2 A3   S 23 C 23 a3 S 23  a2 S  d1  (1.5)   0   Trong C1,C2,S1,S2,C23 S23 lần lƣợt viết tắt cos(q1), cos(q2), sin(q1), sin(q2), cos(q2+q3), sin(q2+q3)  c11( ,  , ) c ( ,  , ) A3 (t )   21 c31( ,  , )   c12 ( ,  , ) c22 ( ,  , ) c13 ( ,  , ) c23 ( ,  , ) c32 ( ,  , ) c33 ( ,  , ) xe  ye   ze   1 Trong cij(α,β,ɳ) phần tử ma trận Cardan cos( ) cos( )  cos( ) sin( ) sin(  )    sin( ) sin(  ) cos( )  cos( ) sin( )  sin( ) sin(  ) sin    cos  cos( )  sin( ) cos( ) Rcd     cos( ) sin(  ) cos( )  cos( ) sin( )) cos( ) sin(  ) sin    sin   cos( ) cos( ) cos( )    Ta có phƣơng trình dạng ma trận nhƣ sau: c11( ,  , ) c12 ( ,  , ) c13( ,  , ) c ( ,  , ) c ( ,  , ) c ( ,  , ) 22 23  21 c31( ,  , ) c32 ( ,  , ) c33( ,  , )  0  xe  C1C23  C1S 23 S1 a3C1C23  a2C1C2  a1C1  ye  S1C23  S1S 23  C1 a3 S1C23  a2 S1C2  a1S1    ze  S 23 C23 a3 S 23  a2 S  d1  (1.6)    1  0  BTL mơn ROBOTICS Chƣơng BÀI TỐN ĐỘNG HỌC 2.1 BÀI TOÁN ĐỘNG HỌC THUẬN  Xây dựng quy luật chuyển động, vị trí khâu thao tác ma trận hƣớng Chọn thông số chiều dài khâu nhƣ sau: d1=100 mm, a1=200 mm ,a2=200 mm , a3 = 200mm Và chọn quy luật chuyển động khâu nhƣ sau:    q1  t  t q1   t    2    q  2t  t  q   t  3      q  t  t q   t    Đồ thị biến đổi biến khớp: với 0(s)≤t≤5(s) (2.1) Đồ thị q1(t) BTL môn ROBOTICS Đồ thị q2(t) Đồ thị q3(t) BTL môn ROBOTICS Từ phƣơng trình 1.6 ta có :  xE  a3C1C23  a2C1C2  a1C1   y E  a3 S1C23  a2 S1C2  a1S1 z  a S  a S  d 23 2  E (2.2) Thay giá trị biến vào ta có: Hƣớng bàn kẹp đƣợc xác định từ góc Cardan, ký hiệu tƣơng ứng α, β, γ quay lần lƣợt quanh trục x-y-z cos( ) cos( )  cos( ) sin( ) sin(  )    sin( ) sin(  ) cos( )  cos( ) sin( )  sin( ) sin(  ) sin    cos cos( )  sin( ) cos( ) Rcd     cos( ) sin(  ) cos( )  cos( ) sin( )) cos( ) sin(  ) sin    sin  cos( ) cos( ) cos( )    để tính đƣợc góc α, β, η ta so sánh ma trận hƣớng (1.5) ma trận hƣớng (1.6) giải hệ phƣơng trình ta có : BTL mơn ROBOTICS    c32  ,  ,    C1     arctan  c  ,  ,    arctan  S       1  31     C1 S 23    c  ,  ,     3    arctan  21    ,    c  ,  ,    arctan  C C      2   11   23        c13  ,  ,  S 23     arctan    arctan   2  2    c11  ,  ,   c12  ,  ,    C1 S 23   S1C 23        arctan  c32  ,  ,    arctan   C1    S     c  ,  ,        31     C1 S 23   c 21  ,  ,      3     ,    arctan    c  ,  ,    arctan   C C     2  23   11         c13  ,  ,  S 23     arctan    arctan     2  2     c11  ,  ,   c12  ,  ,     C1 S 23   S1C 23     Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác Từ phần ta xây dựng đƣợc quy luật chuyển nhƣ tìm đƣợc tọa độ khâu thao tác cuối, biến khớp đạo hàm cấp theo t biết : q  [q1 , q2 , q3 ]T     q  [q1 , q2 , q3 ] T Vận tốc góc khâu thao tác: R A3= A1 A2 A3   E 0 rE  1  (2.3) Vận tốc khâu thao tác đạo hàm vị trí khâu thao tác theo thời gian:     VE= r E= xE , y E , z E  T        VEx  x E  a3 S1C 23q1  C1 S 23 (q2  q3 )   a2 S1C2 q1  C1 S q2   a1 S1q1          VEy  y E  a3 C1C 23q1  S1 S 23 (q2  q3 )   a2 C1C2 q1  S1 S q2   a1C1q1      VEz  z E  a3 C 23 (q2  q3 )   a2 C2 q2 (2.4) Thay (2.1) vào (2.4) ta tìm đƣợc vấn tốc khâu thao tác cuối Vận tốc góc khâu thao tác:  ~  T   E  RE RE    z   y   z x y   x  0  BTL môn ROBOTICS    S1C23q1  C1S 23q23  qCC S S q    1 23 23 23   C23q23     q1S1S 23  q23C1C23 C1q1   C1C 23     C1S 23q1  S1C23q23 S1q1   C1 S 23    S 23q23   S1      q1 q2 C1  q3C1     q1   q2 S1  q3 S1 S1C 23  S1 S 23  C1 S 23  C 23        (q2  q3 )C1     (q2  q ) S1     (2.5) Suy vận tốc góc khâu thao tác:      E  32 13 21 T  [(q2  q3 )S1  (q2  q3 )C1 q1 ]T 11     x  (q2  q3 ) S1  (3  12 t ) sin(t  t )  11      y  (q2  q3 )C1  (3  t ) cos(t  t ) 12     z  q1   t  (2.6)  Ứng dụng phần mềm Matlab, Maple vẽ quỹ đạo chuyển động khâu thao tác cuối Quỹ đạo điểm khâu thao tác Sử dụng phần mềm Maple ta vẽ đƣợc đồ thị quỹ đạo chuyển động khâu thao tác cuối nhƣ sau : Chuyển động điểm cuối E theo phương X BTL môn ROBOTICS Chuyển động điểm cuối E theo phương Y Chuyển động điểm cuối theo phương Z 2.2 BÀI TOÁN ĐỘNG HỌC NGƢỢC Bài tốn động học ngƣợc thơng thƣờng cho biết trƣớc vị trí khâu thao tác u cầu tìm giá trị biến khớp ứng với vị trí Ở tiểu luận robot bậc tự kiểu RRR ta không cần biết hƣớng khâu thao tác mà tìm đƣợc góc quay tƣơng ứng 10 BTL môn ROBOTICS   Fx    0      F3,   Fy   m3 g     Fz           Mx     My   ~  F 0 M r3  y 3,      Mz        Fx   F  m g   y    Fz   Fx      ~ m g   m3 g  rc      Fz    a3C1C 23  Trong : r3  R3 r3   a3 S1 S 23     a3 S 23    (3.1)  a3   C1C 23   a  rc  R3 rc    S1C 23      a3 S  23     C1C 23  C1 S 23 S1 S C  23  S1 S 23  C1 Chú ý : R3  R1 R2 R3 =  S C 23 23   (3.2)       Thay (3.2) vào (3.1) ta tìm đƣợc M 3, :      Fx     Fx  0  F   m g    F  m g    F3,   y    y   Fz     Fz         0 0 ~0 ~  M 3,  M E ,3  r3 F3,  rc P3   a3 a    S 23  S1C 23    M x   2  a3 S 23 a3 S1 S 23   Fx         a3   F  m g     a3 S  a3C1C 23   y C1C 23  m3 g     M y    a3 S 23  23      M   a S S   Fz   a   a3 C1C 23 a3       z   23  S1C 23  C1C 23        Fx  0    F3,   Fy  m3 g    Fz          Mx  a3 ( Fy  m3 g ) S 23  a3 FzS1C 23  a3 m3 gS 23    M   My  a3 FxS 23  a3 FzS1C 23   3,   Mz  a FzS C  a ( Fy  m g )C C  a3 m gC C   23 3 23 23       (3.3) 14 BTL mơn ROBOTICS  Tính lực momen khâu tác dụng lên khâu khớp Hệ phƣơng trình cân dạng mà trận khảo sát hệ tọa độ sở :  F2,1  F3,  P2  0 0 0 r2 rc  M 2,1  M 3,  ~ F2,1  ~ P2  Trong đó: 0 C1 R2  R R2   S1  0  1 a2C1C2  r2  R2 r2   a2 S1C2     a2 S    (3.4) S1  C  C1   S     S2 C2 0 C1C 0   S1C   1  S    C1 S  S1 S C2 S1   C1      a2   C1C2   a  rc  R2 rc    S1C2      a2 S      (3.5) Thay (3.5) (3.3) vào (3.4) ta đƣợc:  Fx   0 F2,1   Fy  m3  m2 g        Fz    1    Mx  a3 Fy  m3 g S 23  a3 FzS1C 23  a3 S 23m3 g  a2 S ( Fy  m2 g  m3 g )  a2 FzS1C  a S m2 g    2 0   My  a2 FxS 23  a3 FzC1C 23  a2 FxS  a2 FzC1C  M 2,1      Mz  a3 FxS 1C 23  a3 FyC1C 23  a3 m3 gC1C 23 ( Fy  m3 g )  a3 m3 gC1C 23  Fx a2 S1C  a2 Fy  m2 g  m3 g C1C  a2 m2 gC1C     2        Tính lực momen khâu tác dụng lên khâu đế khớp Hệ phƣơng trình cân dạng mà trận khảo sát hệ tọa độ sở :  F1,0  F2,1  P1  0 0 0 r1 rc  M 1,0  M 2,1  ~ F1,0  ~1 P1  (3.7) Trong : 15 BTL mơn ROBOTICS C1 R1   S1  0  S1   C1     a1C1  r1  R r   a1 S1        1  a1   C1   a  rc  R2 rc    S1          (3.8) Thay (3.8) (3.6) vào (3.7) ta đƣợc hệ phƣơng trình:  Fx Fx       0  F  m  m g   m g    F  m  m  m g  3  F1,   y     y         Fz Fz           m1       g   d1 Fy   m2  m3           M 0 M   d1Fx  ,1  1,           Fx   0  F  m  m  m g   y  F1,     Fz    a3    m2 g  0  M 1, x  Mx   Fya  m3 g  S 23  a3FzS1C23  a S   m3 g  Fy         m1     a FzS1C2  d1 Fy   m2  m3   g         0  M 1, y  My  a FxS 23  a3FzC1C23  a FxS  a FzC1C2  d1Fx 0 a3  M 1, z  Mz  a3FxS1C23  a3FyC1C23  m3 gC1C23  Fxa S1C2   mg     a Fy   m3 g C1C2     (3.9) 16 BTL mơn ROBOTICS Chƣơng TÍNH TỐN ĐỘNG LỰC HỌC 4.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC Vì khâu coi nhƣ đồng chất tiết diện ngang không đáng kể nên ta có trọng tâm khâu nằm trung điểm 4.2 Cơ sở lý thuyết Động robot có dạng: T T q M ( q ) q  qT b( q, q ) 2 Trong : b(q, q) = M (q)q , b  b1 bn  T Phƣơng trình Lagrange loại II: 17 BTL môn ROBOTICS T T T d  T   T            dt  q   q   q  T  T  T  qT M ( q )     M (q)q q  q  T d  T     M (q)q  M (q)q dt  q  Sử dụng định lý đạo hàm riêng theo vector tích hai ma trận ta có: T   qT b  T  (q b)   (b  I n )  qT  q q  q q  (4.1) Trong đó:  b1 I n    qT T T T  (b  I n )  e1T en        b1e1 I n bn en I n    q   bn I n   b1 bn   bT   M (q)q   qT M (q) T (4.2) Mặt khác:  qT  b       M q q   qT  M q  q  I n   M q  q   qT  q      q q q    qT   M (q) I n   qT M (q) (4.3) Thay (2) (3) vào (1) ta đƣợc: T  T  T  qT M ( q )     M (q)q , q  q  T d  T     M (q)q  M (q)q dt  q  (4.4) Tính tốn tƣơng tự ta có: T   qT b   T b   (qT b)   (b  I n )  qT   0 q  q q  q q   q    M (q) q   qT (q  I n )  M (q )   M (q )q   qT  q  q q    M (q ) T  M (q) q  ( q  I n )    qT (q  I n )  q q  (4.5) Thay (4) (5) vào phƣơng trình lagrange II ta đƣợc: 18 BTL mơn ROBOTICS T T      M (q) M (q )q  M (q )q   qT (q  I n )      2 q   q  M (q) M (q)  ( I n  q) q T  M (q)    M (q ) v ( q, q )   ( I n  q)   ( q  I n )   q  C ( q, q ) q  q  q     Đặt : T Ta có:   M (q) v ( q, q )  M ( q ) q   (q  I n )  q  q  (4.6) Theo định lý đạo hàm toàn phần đạo hàm riêng ta có: M (q)  M (q) ( I n  q) q Thay vào (6) ta đƣợc: T  M (q)    M (q ) v ( q, q )   ( I n  q)   ( q  I n )   q  C ( q, q ) q  q  q     T  M (q)  M (q ) C ( q, q )  ( I n  q)   (q  I n )  q  q  Ma trận ly tâm coriolis có dạng: T  M (q)  M ( q) C ( q, q )  ( I n  q)   (q  I n )  q  q  Khi phƣơng trình vi phân chuyển đông khâu : T    M ( q ) q  C ( q, q ) q      q  4.3 Xây dựng bảng tham số động lực học Bảng 4.1 Bảng mơ tả vị trí trọng tâm khối lượng mơ men qn tính khối khâu Khâu Vị trí trọng tâm xC zC yC 0 a  0 a  2 0 a  Khối lƣợng I xx Ma trận mơmen qn tính I yy I xy I yz I zz m1 I1 x I1 y I1z 0 I zx m2 I2x I2y I2z 0 m3 I 3x I3y I 3z 0 19 BTL môn ROBOTICS 4.3 Ma trận Jacobi khâu Tạo độ trọng tâm khâu i hệ tọa độ tính nhƣ sau : rci  ri  0Ri i rci Với i rci tọa độ trọng tâm khâu i hệ tọa độ i rci tọa độ trọng tâm khâu i hệ tọa độ i Ri ma trận quay biến đổi hệ thành hệ i ri tọa độ gốc tọa độ i hệ tọa độ Ta có ma trận tọa độ trọng tâm khâu nhƣ sau : a1 cos(q1 ) C1 rC1   a1 sin(q1 )    S1      0 d1    0 rC rC a   a1   cos(q1 )   S1     02    a1 sin(q1)     C1    2     d1           a2 C1C2  a1C1  C1C2   a2 S1C2  a1 S1    S1C2     a S  d1   S     a2   a2  S1     C1C2  a1C1  2   C1      l2 S1C         a2 S  d      2       C1 S  S1 S C2 a2C1C2  a3C1C23  a1C1  C1C23   a2 S1C2  a3 S1C23  a1S1    S1C23       S 23 a2 S  a3 S 23  d1    (4.7)  C1S 23  S1S 23 C23 (4.8) a   a3   C1C23  q2C1C2  a1C1  S1      02    a S C  a3 S C  a S   C1       2 23 1     a3   S 23  a2 S  d1         Từ (4.7) (4.8) (4.9) ta có ma trận Jacobi tịnh tiến khâu : JT1 JT  a1  S1  a r  C1   C1  q       a2 S1C2  S1a1 1 r  C1   a2C1C2  C1a1  q      0  0  0   a2C1 S 2  a2 S1 S 2 a2 C 2  (4.10)  0  0  0   (4.11) 20 BTL môn ROBOTICS JT   a2 S1C2  a3 S1C23  C1a1  r  C1   a2C1C2  a3C1C23  C1a1   q    1  a3C1S 23  a3C1 S 23  2  1  a2 S1S  a3 S1 S 23  a3 S1 S 23  2  1 a2C2  a3C23 a3C23   2   a2C1S  (4.12) Cũng từ ma trận Denanvit- Hartenberg ta xác định đƣợc ma trận cosin hƣởng khâu (xem 1.1, 1.5, 3.5) : Ma trận cosin hƣớng khâu : C1 R1   S1  0  S1   C1     Ma trận cosin hƣớng khâu : C1C R   S 1C   S2   C1 S  S1 S C2 S1   C1     Ma trận cosin hƣớng khâu 3: C1C 23 R3   S1C23   S 23   C1 S 23  S1 S 23 C 23 S1   C1     Tốn tử sóng vector vận tốc góc khâu 1:   ~   1`  R1T R1  q1  0     q1 0  0 0  0   Suy vận tốc góc ma trận Jacobian khâu : ~   1`  0 q1 T  J R1q J R1 0 0  1    0 0    q 1 0   (4.13) Tốn tử sóng vector vận tốc góc khâu 2:  ~ `  RT R   q   2 2  q2C1    q1  q2 S1   q2C1    q2 S1     21 BTL môn ROBOTICS Suy vận tốc góc ma trận Jacobian khâu : ~`     2  q2 S1  q2C1 q1 T  J R q J R2 0 S1      C1  q  1  0 0  0  (4.14) Tốn tử sóng vector vận tốc góc khâu 3:  ~ `  RT R     3 q1 3  q2C1  q3C1     q1   q2 S1  q3 S1    (q2  q3 )C1     (q2  q ) S1     Suy vận tốc góc ma trận Jacobian khâu : ~      3`  S1 (q2  q3 )  (q2  q3 )C1 q1 T  J R3q J R3 0 S1 3     C1  q  1  S1   C1     (4.15) 4.5 Ma trận khối lƣợng suy rộng robot  m11 m12  T T M   (mi JTi JTi  J Ri Ai I i AiT J Ri ) =  m21 m22 i 1 m  31 m32 m13   m23  m33   Trong đó: m12=m13=m31=m21=0 22 BTL mơn ROBOTICS 4.6 Ma trận ly tâm coriolis :  c11 T   M (q)  M (q) C ( q, q )  ( I n * q)   (q * I n )  =  c21 q  q  c  31 c12 c22 c32 c13   c23  c33    c1       C (q, q).q   c2  c   3 Sử dụng phần mềm Maple nhân ta đƣợc :  Trong : diffq i  qi 23 BTL môn ROBOTICS 4.7 Thế robot   m1 gd1  m2 g (d1  l2 S2 )  m3 g (d1  l2 S2  l3S23 ) Từ suy :  g1     g   g2  q    g3  Trong : 4.8 Phuơng trình vi phân chuyển động khâu Thế biểu thức vào phƣơng trình Lagrange loại hai : M (q)q  C (q, q)q  g (q)   Ta nhận đƣợc hệ phƣơng trình vi phân chuyển động robot ba khâu không gian : Khâu : 24 BTL môn ROBOTICS Khâu : Khâu : Trong  d qi  qi  diffq i  q 25 BTL môn ROBOTICS Chƣơng CHỌN BỘ ĐIỀU KHIỂN Nhiêm vụ toán điều khiển tìm quy luật lực/ mơ men động điện tạo tác dụng lên khâu để đảm bảo robot chạy theo quy luật q d(t) cho trƣớc, nhằm thực số nhiệm vụ Trên sở chuyển động mong muốn qd(t) đƣợc định nghĩa trƣớc chuyển động robot đƣợc đo cảm biến đặt khớp, điều khiển có nhiệm vụ đƣa lực/mômen cần thiết lực/mômen tác động làm cho robot thực chuyển động mong muốn cách ổn định xác Sơ đồ khối điều khiển cho robot có dạng nhƣ hình 5.1 d Bộ điều khiển PD q d (t)  Robot q(t) _ Cảm biến Hình 5.1 Sơ đồ điều khiển robot Để có đƣợc luật điều khiển đáp ứng yêu cầu vừa nêu, thông thƣờng luật điều khiển dựa động lực học ngƣợc đƣợc sử dụng Với luật điều khiển lực/mô men phận dẫn động đƣợc tính nhƣ sau:   M (q ).q  C (q q ).q  g (q ) (5.1) Giả thiết thành phần mômen trọng lực G(q) đƣợc bù hoàn toàn, sơ đồ hệ thống điều khiển phản hồi với cấu trúc điều khiển PD có dạng đơn giản nhƣ sau: 26 BTL môn ROBOTICS q Kp  q RB q Kp q Tín hiệu đặt vị trí qd đƣợc so sánh với vị trí thực khớp q, sai lệch đƣợc đặt vào khâu khuếch đại với hệ số Kp Tín hiệu khâu tỉ lệ đƣợc cộng đại số với tín hiệu tỉ lệ với tốc độ khớp đặt cấu chấp hành robot:  dk  K p  K D (5.2) Hay viết với khớp thứ i ta có:  dki  K pi i  Kdi i (5.3) Trong :   qd  q sai số vị trí khớp robot   qd  q sai số tốc độ khớp robot K p  diag ( K p1 , K p , , K pn ) ma trận đƣờng chéo hệ số khuếch đại khớp hợp riêng Kd  diag ( Kd1 , Kd , , Kdn ) ma trận đƣờng chéo hệ số đạo hàm khớp hợp riêng Hệ thống điều kiển với cấu trúc điều khiển (3…) ổn định tuyệt đối toàn cục thật vậy, chọn hàm lyapunov có dạng : VL  ( T K p   q T Hq ) Hàm VL biểu thị tổng lƣợng hệ thống robot: thành phần (5.4) T  K p tỉ lệ T (q H q ) động robot KP H ma trận hệ số dƣơng; nên hàm VL > với q  qd lƣợng đầu vào thành phần Tính đạo hàm cấp hàm VL ta đƣợc: 27 BTL môn ROBOTICS 1 1 VL   T K p   T K p  q T Hq  q T Hq  q T Hq 2 2 (5.5) Do tính đối xứng thành phần  T K p , q T Hq nên (4.5) đƣợc rút gọn lại nhƣ sau: VL   T K p  q T Hq  q T Hq (5.6) Thay phƣơng trình dạng tổng quát vào phƣơng trình với giả thiết khơng có thành phần mơmen trọng lực G(p) ta có : VL   T K p  q T Hq  q T [  C (q , q )q ] (5.7) Sử dụng thuộc tính phƣơng trình động lực học áp dụng luật điều khiển (4.1), Phƣơng trình (4.7) đƣợc biến đổi nhhuw sau: 1 VL  q T K p  q T H (q )q  q T C (q , q )q  q T K p  q T [ H (q )  C (q , q )]q 2 (5.8) 1 H (q )  C (q , q ) ma trận đối xứng ngƣợc nên H (q )  C (q , q ) = với 2 q nên từ (4.7) ta nhận đƣợc : Do ma trận VL  q T K p q  Bất đẳng thức cho thấy hệ thông ổn định tuyệt đối mức độ dƣơng VL phụ thuộc vào Kp mức độ âm VL phụ thuộc vào KD Do tăng tốc độ hội tụ cách tăng giá trị ma trận KD 28 ... a1S1    ze  S 23 C23 a3 S 23  a2 S  d1  (1.6)    1  0  BTL môn ROBOTICS Chƣơng BÀI TOÁN ĐỘNG HỌC 2.1 BÀI TOÁN ĐỘNG HỌC THUẬN  Xây dựng quy luật chuyển động, vị trí khâu thao tác... Chuyển động điểm cuối E theo phương X BTL môn ROBOTICS Chuyển động điểm cuối E theo phương Y Chuyển động điểm cuối theo phương Z 2.2 BÀI TỐN ĐỘNG HỌC NGƢỢC Bài tốn động học ngƣợc thơng thƣờng cho biết...   Đồ thị biến đổi biến khớp: với 0(s)≤t≤5(s) (2.1) Đồ thị q1(t) BTL môn ROBOTICS Đồ thị q2(t) Đồ thị q3(t) BTL mơn ROBOTICS Từ phƣơng trình 1.6 ta có :  xE  a3C1C23  a2C1C2  a1C1   y