GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics BÀI TẬP LỚN: ROBOTICS PHẦN I: XÂY DỰNG CẤU TRÚC VÀ THIẾT LẬP HỆ PHƯƠNG TRÌNH ĐỘNG HỌC CỦA ROBOT 1.xây dựng cấu trúc: Cấu trúc 3D: SVTH: NGUYỄN QUANG TRẠCH Trang1 GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Mơn :Robotics thiết lập hệ phương trình động học robot: Bảng tham số DH: θ i di αi q1 h l1 q2 l2 π q3 0 q4 l4 0 Ma trận DH khâu i so với khâu thứ i-1 có dạng:  cos θi  sin θi i −1 Ai =     − sin θi cos α i sin θi sin α i cos θi  ÷ cos θi cos α i − cos θi sin α i sin θi ÷ sin α i cos α i di ÷ ÷ 0  SVTH: NGUYỄN QUANG TRẠCH Trang2 GVHD: PGS.TS PHAN BÙI KHÔI Quy ước viết tắt: Bài Tập Lớn Môn :Robotics Ci = cos θi ; Si = sin θi ; C12 = cos(θ1 + θ ); S12 = sin(θ1 + θ ); Các ma trận vị trí hướng khâu khâu thứ i so với khâu thứ i-1:  C1 − S1  S C1 A1 =  0  0  1  A3 =  0  0 0 l1C1  ÷ l1S1 ÷ h ÷ ÷ ÷   C2  S A2 =       C4  S A4 =  0  0  0 ÷ 0÷ d3 ÷ ÷ 1 S2 −C2 − S4 C4 0 0 0 l2C2  ÷ l2 S2 ÷ −1 ÷ ÷ ÷  0 ÷ 0÷ l4 ÷ ÷ 1÷  Các ma trận vị trí hướng khâu thứ i so với khâu thứ 0:  C1 − S1  S C1 A1 =  0  0  l1C1  ÷ l1S1 ÷ h ÷ ÷ ÷   C12  S 0 A3 = A1 A2 A3 =  12      C12C4 + S12 S4  S C − C12 S A4 =  12      C12  S 0 A2 = A1 A2 =  12     − S12 l1C1 + l2C12  ÷ C12 l1S1 + l2C12 ÷ −1 −q3 + h ÷ ÷ ÷ 0  −C12 S + S12C4 −C12C4 − S12 S 0 l1C1 + l2C12  ÷ l1S1 + l2C12 ÷ −1 h − q3 − l4 ÷ ÷ ÷  Hệ phương trình động học robot: SVTH: NGUYỄN QUANG TRẠCH Trang3 − S12 C12 0 l1C1 + l2C12  ÷ l1S1 + l2 S12 ÷ ÷ −1 h ÷ ÷  GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics Điểm tác động cuối mô tả ma trận: Ta có TE = A4  nx  n TE =  y  nz  0 sx sy sz ax ay ay px  ÷ py ÷ pz ÷ ÷  suy ra:  nx = C12C4 + S12 S n = S C − C S 12 12  y  nz =   sx = −C12 S + S12C4  s = −C C − S S 12 12  y  sz =    f1 = px − l1C1 + l2C12  ax =  f = p −l S +l C a y = y 1 12    f = pz − h − q3 − l4  az = −1   f = n −C C + S S  x 12 12   px = l1C1 + l2C12  f = sz  p y = l1S1 + l2C12    pz = h − q3 − l4   ⇒  f6 = ay Với:h=340(mm); l1=l2=350(mm); l4=100(mm) PHẦN II:GIẢI BÀI TOÁN ĐỘNG HỌC 1.bài tốn động học thuận: Các thơng số đầu vào :q1, q2, q3,q4 Thông số cần xác định: điểm tác động cuối P=( Px, Py,Pz),hướng khâu cuối • Tọa độ điểm tác động cuối:  px = l1C1 + l2C12   p y = l1S1 + l2C12   pz = h − q3 − l4 • Hướng khâu thao tác có ma trận cosin hướng: SVTH: NGUYỄN QUANG TRẠCH Trang4 GVHD: PGS.TS PHAN BÙI KHÔI  C12C4 + S12 S4  R4 =  S12C4 − C12 S4   Bài Tập Lớn Môn :Robotics −C12 S4 + S12C4 −C12C4 − S12 S4 0 ÷ 0÷ −1÷  a.xây dựng quy luật chuyển động khâu robot: giả sử robot chuyển từ vị trí ban đầu (A) tới vị trí cuối (B) thời gian quỹ đạo khớp robot theo đa thức bậc 3: (t)= giả thiết vận tốc điểm đầu điểm cuối 0: ; Ta có hệ phương trình: q (t ) = q = a0i = i ( A)  i  qi (tc ) = qi ( B ) = a0i + a1itc + a2itc + a3itc  & & qi (t0 ) = qi ( A) = a1i =  qi (tc ) = q & &B) = a1itc + 2a2itc + 3a3itc = i(  ⇔ a = q i ( A)  0i a =  1i  3(qi( B) − qi( A) )  a =  2i t2  −2(qi( B) − qi( A) )  a3i =  t3   Giải hệ ta hệ số phương trình quỹ đạo Bài tốn cụ thể: cho điểm A(700; 0; 240) điểm B(175; 153.11; 140) Thời gian chuyển động Từ toán ngược ta xác định thong số biến khớp: q = 0o 1( A)  q = 0o  2( A)  q3( A) =  o q4( A) =  q = 60o 1( B )  q = 30o  2( B )  q3( B ) = 100  o q4( B ) =  Quỹ đạo chuyển động khâu: Khâu 1: SVTH: NGUYỄN QUANG TRẠCH Trang5 GVHD: PGS.TS PHAN BÙI KHÔI Với hệ số:0 Bài Tập Lớn Môn :Robotics 7.2 ta có: q1(t ) = 7.2t − 0.96t Khâu 2: Với:0 q2 (t ) = 3.6t − 0.48t Khâu 3: Với:0 q3 (t ) = 12t −1.6t Khâu 4:khơng chuyển động b.tính vận tốc điểm tác động cuối vận tốc góc khâu thao tác: • Vận tốc điểm tác động cuối: v (0) E = R0 d (0) rE dt d  & & & vPx = dt Px = −l1q1S1 − l2 ( q1 + q2 ) S12  d  & & & vPy = Py = l1q1C1 − l2 (q1 + q2 ) S12 dt  d  & vPz = dt Pz = − q3  • Vận tốc góc khâu: Ma trận cosin hướng khâu so với R0:  C1 − S1   C12  ÷  R1 =  S1 C1 ÷ R2 =  S12 0  0 1÷    SVTH: NGUYỄN QUANG TRẠCH − S12 C12  C12 − S12  0 ÷  ÷ ÷ R3 =  S12 C12 ÷  −1÷ −1÷    Trang6 GVHD: PGS.TS PHAN BÙI KHÔI  C12C4 + S12 S4  R4 =  S12C4 − C12 S4   Ta có: −C12 S + S12C4 −C12C4 − S12 S Bài Tập Lớn Môn :Robotics 0 ÷ 0÷ −1÷  % & ωi = 0Ri RT i 0  −q1  &  ÷  ÷ % & & ω1 = R1 0RT =  q1 0 ÷⇒ ω1 =  ÷ 0 q ÷ 0÷    & % & ω2 = R2 RT    & & −q1 − q2   ÷  ÷ & & =  q1 + q2 0 ÷⇒ ω2 =  ÷  q + q ÷ 0÷    & &    & & −q1 − q2   ÷  ÷ % & & & ω3 = R3 0RT =  q1 + q2 0 ÷ ⇒ ω3 =  ÷  q +q ÷ 0÷    & & • ứng dụng phần mềm Maple vẽ quỹ đạo điểm tác động cuối Khởi động chương trình khai báo thư viện sử dụng > > > > Khai báo ma trận Denavit-Hartenbergtổng quát > Tính ma trận Denavit-Hartenbergcác khâu Thế bảng thơng số DH cho khâu vào ma trận H tổng quát > > SVTH: NGUYỄN QUANG TRẠCH Trang7 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics > > > > > > > > > > Tọa độ điểm tác động cuối theo thời gian: > > > Đồ thị xp theo thời gian: > Đồ thị yp theo thời gian: SVTH: NGUYỄN QUANG TRẠCH Trang8 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics > Đồ thị zp theo thời gian: > SVTH: NGUYỄN QUANG TRẠCH Trang9 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics Tọa độ khâu điểm tác động cuối: SVTH: NGUYỄN QUANG TRẠCH Trang10 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics 2.bài toán động học ngược a.xây dựng quy luật chuyển động khâu cuối: điểm tác động cuối chuyển động theo đường xoắn ốc:  xp = 450sin(π t )   yp = 450 cos(π t )  zp = t  b.giải toán động học ngược: chọn góc cardan làm góc định hướng cho khâu thao tác: ma trận cardan: cos( β ) cos(η ) − cos(α )sin( β ) sin( β )   sin(α ) sin( β ) cos(η ) + cos(α ) sin(η ) − sin(α )sin( β )sin(η ) + cos(α ) cos(η ) − sin(α ) cos( β ) Cd =   − cos(α )sin( β ) cos(η ) + sin(α ) sin(η ) cos(α ) sin( β ) sin(η ) + sin(α ) cos(η ) cos(α ) cos(β )  0  tính q3: • ta có: • pz = h − q3 − l4 suy ra: q3 = h − pz − l4 tính q2:  px = l1C1 + l2C12    p y = l1S1 + l2C12  Lấy tổng bình phương hai vế: 2 px + p y = l12 + l22 + 2l1l2 (C1C12 + S1 S12 ) 2 px + p y = l12 + l22 + 2l1l2 C2 C2 = 2 px + p y − l12 − l2 2l1l2 S = ± − C2 q2 = a tan 2( S , C2 ) • Tính q1: Cũng từ hệ:  px = l1C1 + l2C12    p y = l1S1 + l2C12  SVTH: NGUYỄN QUANG TRẠCH ta có:  px = (l1 + l2C2 )C1 + l2 S S1    p y = l2 S 2C1 + (l1 + l2C2 ) S1  Trang11 px  ÷ py ÷ pz ÷ ÷  GVHD: PGS.TS PHAN BÙI KHÔI Giải hệ ta : Bài Tập Lớn Môn :Robotics l2 ( px C2 + p y S )  C1 = 2 px + p y    S = (l1 + l2C2 ) p y − l2 S px 2  px + p y  q1 = a tan 2( S1 , C1 ) Suy : • Tính q4 : Ta có : nx = C12C4 + S12 S = C124 = cos ϕ q4 = ϕ − q1 − q4 Suy : c.ứng dụng phần mềm Matlap, Maple để tính tốn mơ số : PHẦN III : TÍNH TỐN TĨNH HỌC 1.tính lực momen dẫn động khớp • Lý thuyết: Áp dụng hai cơng thức sau Fi,i−1 = Fi+1,i − Pi % % M i ,i −1 = M i +1,i + ri × Fi ,i −1 − rci × Pi Chú ý:biểu diễn hệ trục tọa độ theo phương pháp DH thì: i   ri =  di sα i     di cα i    ri = Ri i ri rci = Ri i rci Nhận xét: -Đối với khớp tịnh tiến lực kích động tác dụng dọc theo trục khớp thứ i: (giả sử lực masat là không đáng kể) τ i = ziT−1Fi,i −1 -Đối với khớp quay moomen kích động tác dụng tại khớp thứ i SVTH: NGUYỄN QUANG TRẠCH Trang12 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics τ i = ziT−1M i ,i −1 ( ziT−1 :Véc tơ đơn vị chỉ dọc theo chiều dương của trục khớp thứ i) Ở đây: Fi +1,i :Lực tác dụng lên khâu thứ i+1 bởi khâu i tại M i +1,i Tính ma trận  l1  r1 =  ÷  ÷ h÷   0 r3 =  ÷  ÷ q ÷  3  l1C1  r1 =  l1S1 ÷  ÷  h ÷    −lc1  rc1 =  ÷  ÷  ÷     rc3 =  ÷  ÷  −l ÷  c3  0 và ;   r3 =  ÷  ÷  −q ÷  3 Tính ma trận ri  l2  r2 =  ÷  ÷ 0÷   ; rci và 0 rc3 =  ÷  ÷ l ÷  c3  0  ÷ r4 =  ÷ l ÷  4 rci ,  C12l2  r2 =  S12l2 ÷  ÷  ÷      ÷ r4 =  ÷  −l ÷  4 :  −lc  rc =  ÷  ÷  ÷   Oi M i ,i +1 = − M i +1,i :  −lc1C1  rc1 =  ÷  ÷  ÷   ri i • , :Momen tác dụng lên khâu thứ i+1 bởi khâu i,tại i • Oi Fi ,i +1 = − Fi +1,i  −lc 2C12  rc =  −lc S12 ÷  ÷  ÷     0  ÷ rc =  ÷ rc =  ÷  ÷  −l ÷ l ÷  c4   c4  %% ri rci Tính ma trận sóng : • −h l1S1    ÷ % r1 =  h −l1C1 ÷  −l S l C ÷  1 1  SVTH: NGUYỄN QUANG TRẠCH  %  r2 =   −S l  12 Trang13 0 C12l2 S12 l2  ÷ −C12l2 ÷ ÷  GVHD: PGS.TS PHAN BÙI KHÔI 0 0 %  r3 =  0  −q  0 Bài Tập Lớn Mơn :Robotics 0 ÷ q3 ÷ 0÷  0  0  ÷ % rc1 =  0 lc1C1 ÷  −l C ÷ c1   0 %  rc3 =  lc 0  • −lc 0  %  rc =  l S  c 12 0 %  rc =  lc 0  −lc 0 l4 0 0 −lc 2C12 0 ÷ 0÷ 0÷  −lc S12  ÷ lc 2C12 ÷ ÷  0 ÷ 0÷ 0÷  Trọng lực:   P = ÷  ÷  −m g ÷  ã 0ữ ữ 0ữ  % r4 =  −l     P2 =  ÷  ÷  −m g ÷      ÷ P3 =  ÷  −m g ÷   Ngoại lực tác dụng:  Fx   ÷ F =  Fy ÷ F ÷  z Mx   ÷ M = My ÷ M ÷  z Sử dụng phần mền Maple nhân được: • Tính phản lực khớp > SVTH: NGUYỄN QUANG TRẠCH Trang14   P4 =  ÷  ÷  −m g ÷   GVHD: PGS.TS PHAN BÙI KHƠI • Bài Tập Lớn Mơn :Robotics Tính momen phản lực khớp > Trong đó: F [i, i + 1]=Fi ,i +1 M [i,i+1]=M i ,i +1 • P[i]=Pi % % sr[i]= ri src[i ]= rci ; ; ; ; Khai báo véc tơ đơn vị chỉ dọc theo chiều dương của trục khớp thứ i SVTH: NGUYỄN QUANG TRẠCH Trang15 GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Mơn :Robotics > > > > • Tính lực dẫn động ở khớp đảm bảo robot cân tĩnh > PHẦN IV: TÍNH TỐN ĐỘNG LỰC HỌC 1.cơ sở lý thuyết: Khi phương trình vi phân chuyển đơng khâu : T  ∂π  & & && M ( q ) q + C ( q, q ) q +  ÷ =τ  ∂q  Trong đó: SVTH: NGUYỄN QUANG TRẠCH Trang16 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics n T T M (q ) = ∑ (mi J Ti J Ti + J Ri Ri I i RiT J Ri ) i =1 :ma trận khối lượng suy rộng robot T  ∂M (q )  M (q ) & & & C ( q, q ) = ( I n × q) −  (q × I n ) ÷ ∂q  ∂q  π : ma trận coriolis : robot 2.Bảng tham số động lực học robot khâu Vị trí trọng tâm Khối lượn g m1 Ma trận mômen quán tính I1x I1y I1z 0 −lC1 0 −lC2 0 m2 I2x I2 y I2z 0 0 −lC3 m3 I 3x I3 y I 3z 0 0 −lC4 m4 I4x I4 y I4z 0 3.ma trận Jacobi khâu Từ cấu tạo hình học robot ta xác định tọa độ khối tâm khâu so với hệ tọa độ cố định :  lC1C1   ÷ rC =  lC1S1 ÷  h ÷    l1C1 + lc 2C12  rC =  l1S1 + lc S12 ÷  ÷  ÷ h    l1C1 + lc 2C12  rC =  l1S1 + lc S12 ÷  ÷  h−q +l ÷ c3    l1C1 + lc 2C12  rC =  l1S1 + lc S12 ÷  ÷ h −l −q +l ÷ c3   a.các ma trận Jacobi tịnh tiến khâu  −lC1S1 0  ∂rC1  ÷ JT =  lC1C1 0 ÷ ∂q  0 0÷    −l1S1 − lc S12 ∂rC  = l1C1 + lc 2C12 ∂q    JT SVTH: NGUYỄN QUANG TRẠCH Trang17 −lc S12 lc 2C12 0 0 ÷ 0÷ 0÷  GVHD: PGS.TS PHAN BÙI KHƠI JT  −l1S1 − lc S12 ∂rC  = l1C1 + lc 2C12 ∂q    Bài Tập Lớn Môn :Robotics −lc S12 lc 2C12 0 0 ÷ 0÷ 0÷  JT  −l1S1 − lc S12 ∂rC  = l1C1 + lc 2C12 ∂q    −lc S12 lc 2C12 0 0 ÷ 0÷ 0÷  b.ma trận Jacobi quay khâu : 0 0 0 ∂ω1  J R1 = 0 0÷ ÷ & ∂θ  1 0 0÷   J R3 J R2 0 0 0 ∂ω2  = 0 0÷ ÷ & ∂q  ÷ 1 0 J R4 0 0 0 ∂ω4  ÷ = 0 0 0÷ & ∂q  ÷ 1 1 0 0 0 ∂ω3  ÷ = 0 0 0÷ & ∂q  ÷ 1 0 4.ma trận khối lượng suy rộng robot: n T T M (q ) = ∑ (mi J Ti J Ti + J Ri Ri I i RiT J Ri ) i =1 Sử dụng phần mềm maple nhân ta kết quả :  m11  m M (q ) =  21  m31   m41 m12 m13 m22 m23 m32 m33 m42 m43 m14  ÷ m24 ÷ m34 ÷ ÷ m44  Trong : 2 m11 = I1z + I z + I z + I z + m1l C + m2 (lC2 + l12 + 2l1lC2 C2 ) + (m3 + m4 )(l2 + l12 + 2l1l2C2 ) m12 = I z + I z + I z + m2 (l + l1lC2 C2 ) + (m3 + m4 )(l2 + l1l2C2 ) C2 m13 = m31 = m23 = m32 = m34 = m43 = 2 m22 = I z + I z + I z + m2lC2 + (m3 + m4 )l2 m21 = I z + I z + I z + m2 (lC2 + l1lC2 C2 ) + (m3 + m4 )(l22 + l1l2C2 ) m33 = m3 + m4 m41 = m14 = m42 = m24 = m44 = I z 5.Ma trận ly tâm coriolis : SVTH: NGUYỄN QUANG TRẠCH Trang18 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics  c11 c12 c c  ∂M (q )  ∂M (q ) & & & C ( q, q ) = ( I n * q) −  ( q * I n ) ÷ =  21 22 ∂q  ∂q   c31 c32   c41 c42 T c13 c23 c33 c43 c14  c24 ÷ ÷ c34 ÷ ÷ c44  Trong : & c11 = −2(m2lC2 + m3l2 + m4l2 )l1S q2 & c12 = 2(m2lC2 + m3l2 + m4l2 )l1S q2 c13 = c31 = c14 = c41 = c23 = c32 = c24 = c42 = c33 = c34 = c43 = c44 = & (m2lC2 + m3l2 + m4l2 )l1S q1 & & & & c21 = (m2lC2 (2q1 − q2 ) + ( m3 + m4 )(2q1 − q2 )l2 )l1S 2 c22 = 6.Thế robot Π = m1 gL2 + m2 gL2 + m3 g (h − q3 + lc3 ) + m4 g ( h − l4 − q3 + lc ) Từ suy :      ∂Π    g = ÷= ∂q  ( m3 + m4 ) g       7.phươ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 hệ phương trình vi phân chuyển động robot scara : Khâu : 2 2 & & [ I1z + I z + I z + I z + m1lC1 + m2 (lC2 + l11 + 2l1lC2 C2 ) + (m3 + m4 )(l2 + l12 2l1l2C2 )]q1 2 & & +[ I z + I z + I z + m2 (lC2 + l1lC2 C2 ) + (m3 + m4 )(l2 + l1l2C2 )]q2 & && & + I z q4 − 2(m2lC2 + m3l2 + m4l2 )l1S q1q2 − (m2lC2 + m3l2 + m4l2 )l1S q2 = τ1 Khâu : SVTH: NGUYỄN QUANG TRẠCH Trang19 GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics 2 & & [ I z + I z + I z + m2 (lC2 + l1lC2 C2 ) + (m3 + m4 )(l2 + l1l2C2 )]q1 2 & & & & & + ( I z + I z + I z + m2lC + (m3 + m4 )l2 )q2 + I z q4 + (m2lC2 + (m3 + m4 )l2 )l1S q12 = τ Khâu 3: & & (m3 + m4 ) q3 + ( m3 + m4 ) g = τ Khâu 4: & & & & I z q1 + I z q2 = τ PHẦN V: CHỌN LUẬT ĐIỀU KHIỂN 1.Hệ thống điều khiển phản hồi không gian khớp Chức của hệ thống điều khiển là tạo momen cần thiết để truyền động khớ p robot đảm bảo khớp robot bám theo vị trí đặt Sử dụng cấu trúc điều khiển có dạng tỉ lệ - đạo hàm (PD): (1) Trong đó: ε = qd − q :là sai số vị trí của khớp robot & & & ε = qd − q : là sai số tốc độ của khớp robot KP =diag(,,…,) :ma trận đường chéo hệ số khuếch đại khớp riêng biệt SVTH: NGUYỄN QUANG TRẠCH Trang20 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics =diag(, ,….,):ma trận đường chéo hệ số đạo hàm khớp riêng biệt Hệ thống điều khiển với cấu trúc điều khiển ổn định tuyệt đối toàn cục.thực chọn hàm Liapunov có dạng : & & VL = (ε T K pε + qT Hq ) Hàm VL biểu thị tổng lượng của hệ thống robot Thành phần lệ lượng đầu vào và thành phần là ma trận hệ số dương ; nên hàm Tính đạo hàm cấp hàm g VL = T & T ε ε KP ε VL VL = T & ε KP ε T & & q Hq ) ( VL T (ε K pε là động của robot >0 với và H q # qd 1 1 T T T g T & & & & & & & & & KP ε q q q H q q q + + H + + H T & & ε T KP ε q q T T T & & & & & 2q q q q + H + H SVTH: NGUYỄN QUANG TRẠCH Kp ) tỷ ,nhận : Do tính đối xứng của thành phần g (2) Trang21 , (3) H , (3) rút gọn dạng: (4) GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics Thay phương trình động lực học dạng tổng quát (1) vào phương trình (4) ,với G( p ) giả thiết khơng có thành phần mơmen trọng lực , nhận phương trình sau : g VL = T & ε KP ε T T T & & & & q 2q q q + H + [M-V(q, )] (5) Sử dụng thuộc tính của phương trình động lực học va áp dụng luật điều khiển (*) ,phương trình (5) biểu đổi thành dạng g & && & & qT C (q, q )q + qT H (q )qK P K D & & V L q KD ε = g T g T & ( H − C )q & & & q qT K D ε =+ Trong : (6) & && V ( q, q ) = C ( q, q ) q g H−C Do ma trận là ma trận đối xứng ngược , ,nên từ (6) nhận : g VL =- & & qT K D q ≤ g qT ( H − C ) q = (7) Bất đẳng thức (7) cho thấy hệ thống ổn định tuyệt đối Quỹ đạo chuyển động khâu: Khâu 1: Khâu 2: Khâu 3: q1(t ) = 7.2t − 0.96t q2 (t ) = 3.6t − 0.48t q3 (t ) = 12t −1.6t SVTH: NGUYỄN QUANG TRẠCH Trang22 với q GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics Khâu 4:không chuyển động 2.Các thao tác thực mô Các hệ số khuếch đại tỉ lệ đạo hàm lựa chọn sau : 0   3000  KP =  3000 ÷ ÷  0 3000 ÷    100  100 ÷ KD =  ÷  0 100 ÷   Ta có mơ hình simulink robot : Trong : , tín hiệu quỹ đạo đặt vận tốc đặt vào khớp robot đươc thiết lập phần thiết lập quỹ đạo q, v tín quỹ đọa vận tốc phản hồi lại từ robot(quỹ đạo thực) ta sử dụng khối: +khối Gain dùng để khếch đại tín hiệu đầu vào +khối mux chập tín hiệu đơn thành tín hiệu tổng hợp tách thành nhiều tín hiệu +khối input, ouput đầu vào tín hiệu SVTH: NGUYỄN QUANG TRẠCH Trang23 GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Mơn :Robotics +khối hàm biểu diễn hàm tốn học có tín hiệu vào biến tín hiệu đầu giá trị hàm Mơ hình điều khiển PD : 3.kết mơ Sau mô ta kết đồ thị đường đặc tính biến: SVTH: NGUYỄN QUANG TRẠCH Trang24 GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Môn :Robotics Mô men khớp theo thời gian Vị trí khớp theo thời gian SVTH: NGUYỄN QUANG TRẠCH Trang25 GVHD: PGS.TS PHAN BÙI KHÔI Bài Tập Lớn Mơn :Robotics Sai số vị trí khớp theo thời gian TÀI LIỆU THAM KHẢO 1.Bài giảng Robotics-PGS.TS.Phan Bùi Khôi 2.cơ sở robot công nghiệp-GS.TSKH.Nguyễn Văn Khang 3.matlap và simulink – Nguyễn Phùng Quang 4.Phần help của phần mềm Maple 5.Phần help của phần mềm Matlab SVTH: NGUYỄN QUANG TRẠCH Trang26 GVHD: PGS.TS PHAN BÙI KHÔI SVTH: NGUYỄN QUANG TRẠCH Bài Tập Lớn Môn :Robotics Trang27 ... Lớn Mơn :Robotics Tọa độ khâu điểm tác động cuối: SVTH: NGUYỄN QUANG TRẠCH Trang10 GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics 2 .bài toán động học ngược a .xây dựng quy luật chuyển động. .. Quỹ đạo chuyển động khâu: Khâu 1: SVTH: NGUYỄN QUANG TRẠCH Trang5 GVHD: PGS.TS PHAN BÙI KHÔI Với hệ s? ?:0 Bài Tập Lớn Mơn :Robotics 7.2 ta c? ?: q1(t ) = 7.2t − 0.96t Khâu 2: Với:0 q2 (t ) = 3.6t...GVHD: PGS.TS PHAN BÙI KHƠI Bài Tập Lớn Mơn :Robotics thiết lập hệ phương trình động học robot: Bảng tham số DH: θ i di αi q1 h l1 q2 l2 π q3 0 q4 l4 0 Ma