Tài liệu hạn chế xem trước, để xem đầy đủ mời bạn chọn Tải xuống
1
/ 25 trang
THÔNG TIN TÀI LIỆU
Thông tin cơ bản
Định dạng
Số trang
25
Dung lượng
1,69 MB
Nội dung
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