(Luận văn học viện tài chính aof) robot planar 2 bậc tự do ( 2dof) – tính số bậc tự do của robot

Đề H Tính số bậc tự robot Vẽ hệ trục tọa độ gắn liền khâu theo quy tắc Denavit-Hartenberg (DH) Lập bảng D-H, tính ma trận D-H: Tìm vị trí điể thao tác biểu diễn theo tọa độ khớp Xác định hướng khâu thao tác Tìm vị trí điểm tác động cuối E Tính vận tốc góc khâu Tìm gia tốc điểm tác động cuối Tính gia tốc khâu thao tác Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc khâu thao tác Tính tọa độ khớp Tính vận tốc, gia tốc dài khâu tương tứng với khớp tịnh tiến 10 Tính vận tốc góc, gia tốc góc khâu tương ứng với khớp quay 11.Khảo sát toán tĩnh học robot 12.Thiết kế quỹ đạo chuyển động 13.Khảo sát động lực học robot 14 Thiết kế mơ hình điểu khiển robot 15 Lập trình tính tốn, vẽ đồ thị phần mềm MAPLE, MATLAB, mô điều khiển robot MATLAB, SIMULINK Nguyễn Văn Nguyên – MSSV 20159868 Page - Tính số bậc tự robot F = 3(2-2) +1+1=2 robot có bậc tự Joint H Ta xây dựng bảng thông số D-H q1 q2 0 a1 a2 0 - Các ma trận D-H cosq −sinq1 sinq cosq A1 = 0 0 [ cosq 12 −sinq 12 sinq 12 cosq 12 A2 = 0 0 [ a1 cosq cosq −sinq a1 sinq1 sinq2 cosq , A2= 0 0 0 ] [ a1 cosq +a2 cosq 12 a1 sinq +a2 sinq 12 0 ] - Điểm tác động thao tác cuối Nguyễn Văn Nguyên – MSSV 20159868 Page a2 cosq2 a2 sinq2 0 ] X E =a1 cosq1 + a2 cosq 12 Y E=a1 sinq1+ a2 sinq 12 ZE= - Tính ma trân Jacobi tịnh tiến Của khâu −a1 A = 0 → c1 0 0 0 0 [ ] ][ ] ] [ ] 0 1 ❑ c1 r = [ −a1 ] [ [ a cosq 1 0 a1 sinq 1 0 1 0 H cosq1 −sinq sinq cosq1 Ac 1= 0 0 cosq1 −sinq A0c 1= sinq1 0 cosq1 0 a1 cosq1 a1 sinq1 →r c 1= −a1 sinq −a1 0 T J T = a1 ; ❑J T 1= sinq1 cosq 0 0 [ ] [ −a1 0 a1 cosq1 a1 sinq a1 cosq1 0 - Ma trận Jacobi tịnh tiến khâu Nguyễn Văn Nguyên – MSSV 20159868 Page ] −a2 2 A = Ta có: c 0 , A0c 2= A 02 A2c 0 0 0 [ ] 0 cosq12 −sinq 12 sinq 12 cosq12 Ac 2= 0 0 [ [ cosq12 0 0 [ J = a1 cosq1 + −a2 0 a2 −a2 sinq 12 sinq12 2 J 0T = a a2 a cosq + cos 12 cos12 2 0 ] [ −a sinq 1− a2 a sinq 12 a1 cosq 1+ cos12 2 −a2 a2 sinq12 cos 12 2 −a1 sinq1− ] ] H a2 cosq12 a →r c 2= a1 sinq + sinq 12 0 T ❑ T1 a2 cosq12 a2 a sinq 1+ sinq12 cosq12 −sinq 12 a1 cosq 1+ → Ac 2= sinq12 [ ][ a1 cosq 1+ a2 cosq12 0 a sinq 1+ a2 sinq 12 1 0 1 0 ] ] Từ ma trận truyền biến biến đổi tạo độ ta suy ma trận quay khâu khâu để tính ma trận jacobi quay hai khâu - Tính ma trận jacobi quay khâu cosq1 −sinq1 cosq1 sinq1 0 T A → R = sinq1 cosq1 ,❑ R c1= −sinq cosq1 0 0 c1 c1 [ ] [ Nguyễn Văn Nguyên – MSSV 20159868 Page ] −sinq −cosq1 0 R˙ c 1= cosq1 −sinq1 0 0 [ ] cosq sinq −sinq −cosq1 r T ~ ta lại có: ω1=❑R c1 R˙ c 1= −sinq1 cosq cosq −sinq1 θ˙1 0 0 [ ][ ] −1 T ~ ωr1=❑0 R Tc1 R˙ 0c 1= 0 → ω r1=[ 0 θ˙ ] 0 [ ] 0 ω = → J R 1= 0 → J TR 1= 0 0 θ˙ r [] [ ] [ ] - Tính ma trận jacobi quay khâu cosq12 −sinq 12 cosq 12 sinq 12 0 T A → R = sinq 12 cosq12 → ❑ R c 2= −sinq12 cosq 12 0 0 c2 [ c2 ] H ] [ [ −sinq 12 −cosq12 ˙R0c 2= cosq12 −sinq12 ( θ˙1 + θ˙2 ) 0 ] 0 −1 r T r ~ ˙ ˙ ˙ ω2=❑R c2 Rc 2= 0 ( θ1 + θ 2)→ω 2= 0 0 θ˙1+ θ˙2 [ ] [ ] [ ] 0 J R 2= 0 → J TR = 0 0 1 [ ] - Tính tensor quán tính khâu khâu 0 m1 a21 12 0 m2 a22 12 [ ] [ ] - ⊖c = 0 0 ,⊖c 2= m1 a1 12 0 0 m2 a2 12 - Áp dụng phương trình lagrange loại II tính ma trận khối lượng M Nguyễn Văn Nguyên – MSSV 20159868 Page M ( q )=¿ m11= ( m3 +m ) a + m3 a +m a a cosq 2 2 2 2 m2 m2 a1 a2 cosq2 + a2 m21=m 12 m12= m22= m2 a M ( q )= [ m11 m12 m21 m22 ] Từ ma trận khối lượng ta suy ma trân lực quán tính coriolis quán tính ly tâm: c1 c2 [] C ( q , q˙ ) q= ˙ c 1=−m2 a1 a2 sinq2 q˙1 q˙ − c 1= m2 a1 a sinq2 q˙ 2 H m2 a a sinq2 q˙ 22 2 - Tính lực hai khâu п=п1 +п2 , g=[ −g ] T п1=m1 g y c1 п2 =m g y c2 , G ( q )= G1 ∂∏ ∂∏ ,G1= ,G 2= ∂ q1 ∂q G2 [ ] ( m2 +m ) g a cosq + m2 g a cos (q + q ) → G 1= G 2= 2 1 2 m2 g a2 cos ( q 1+ q2 ) Tính lực suy rộng lực không n Q1=∑ ( J TTi1 F i + J TRi1 M i) =J TT 12 F 2+ J TR 12 M i=1 Nguyễn Văn Nguyên – MSSV 20159868 Page [ ¿ − a1 sinq + Fx a2 a2 sinq 12 a1 cosq1 + cosq 12 F y + [ 0 ] 2 Mz ][ ] [] Q1=(−a1 sinq 1−a2 sinq 12 ) F x + ( a1 cosq +a2 cosq 12) F y + M z n Q2=∑ ( J TTi1 F i + J TRi1 M i )=J TT 22 F 2+ J TR 22 M i=1 Q 2= [ Fx −a2 a2 sinq 12 cosq 12 F y + [ 0 ] 2 Mz ][ ] [] Q 2=( −a2 sinq 12 ) F x + ( a2 cosq12 ) F y + M z Thế đại lượng ta tính tốn vào phương trình Lagrange loại II ta phương trình vi phân chuyển động: M ( q ) q+C ( q , q ) q+ ă G ( q ) +Q=U U =[ U U … U n ] T H U i=τ i= F mi với khớptịnh tiến M mi với khớp quay { Phương trình vi phân chuyển động: [( [ m1 m m m m2 m1 +m2 a 21+ c2 a1 a2 m2+ a 22 qă + c a a 2+ a22 q−m +m2 g a1 cosq1 ă q a1 a2 sinq q˙ 1+ a1 a2 sinq q 3 2 ] [ ) ] ( ) m2 m2 m2 m2 m2 c a1 a2 + a2 qă1 + a2 qă2 + a1 a2 sinq q˙ + g a2 cos ( q1 +q )( −a2 sinq12 ) F x + ( a2 cosq 12 ) F y + M z=M m 2 3 2 ] II Khảo sát toán tĩnh học robot Nguyễn Văn Nguyên – MSSV 20159868 Page Ta có véc tơ biểu diễn lực momen hệ T 2 ❑ P = [ −m2 g ] , r =[−a2 M 3,2= [ 0 ] −a2 0] , r = T T H { T F3,2 =[ −F ] ❑ c2 [ T ] 0 , 0 T F 23,2=❑0 R−1 F 3,2 =❑ R F 3,2 −1 0 T P = ❑ R P2 = ❑ R P2 Áp dụng phương trình cân lực trọng tọa độ khâu thứ ta sau: { 2 F2,1 =F3,2 −P2 2 2 2 M 2,1=M 3,2−❑~r F3,2 −r c P2 - Ta đặt thông số cho robot nhứ sau: Với thông số: m1 = 60kg; l1 = 2m; lg1=1m; J1=80kg.m2; m2 = 40kg; l2 = 1.5m; lg2 = 0.75m; J2=30kg.m2; g=9.8m/s2; Nguyễn Văn Nguyên – MSSV 20159868 Page Hình Robot Planar DOF Xây dựng mơ hình động lực học robot Phương pháp Euler-lagrange M H Trong đó: L = ∑Ki - ∑Pi với: ∑Ki : tổng động ∑Pi : tổng Với robot Planar 2DOF ta cú: M = H(q)qă + V(q, q ) + G(q) Trong đó: H = [H11 H12;H21 H22]; V = [V1; V2] G = [g1; g2] Với : H11 = m1*lg1^2 + J1 + m2*(l1^2 + lg2^2 + 2*l1*lg2*cos(q2)) + J2; H21 = m2*(lg2*lg2 +l1*lg2*cos(q2)) + J2; Nguyễn Văn Nguyên – MSSV 20159868 Page H12 = H21; H22 = m2*lg2*lg2 + J2; h122 = -1*m2*l1*lg2*sin(q2); h112 = -1*m2*l1*lg2*sin(q2); h211 = m2*l1*lg2*sin(q2); V1 = h122*q˙ 2^2 + 2*h112*(q˙ 1)*(q˙ 2) V2 = h211*(q˙ 1)^2 g1 = m1*g*lg1*cos(q1) + g*(l1*cos(q1) + lg2*cos(q1+q2)) g2 = m2*g*lg2*cos(q1+q2) Mơ hình robot thể qua sơ đồ khối: H Hình 2.Mơ hình động học Robot Mô simulink  Viết m-file cho tham số robot Dưới chương trình m-file viết cho giá trị tham số robot % thong so robot m1=60;l1=2;lg1=1;J1=80; m2=40;l2=1.5;lg2=0.75;J2=30; g=9.8; Dưới chương trình m-file viết cho thơng số dạng biến % thông s? d?ng bi?n syms m1 m2 l1 l2 lg1 lg2 J1 J2 g; syms q1(t) q2(t); H11 = m1*lg1^2 + J1 + m2*(l1^2 + lg2^2 + 2*l1*lg2*cos(q2)) + J2; H21 = m2*(lg2*lg2 +l1*lg2*cos(q2)) + J2; H12 = H21; H22 = m2*lg2*lg2 + J2; H = [H11 H12;H21 H22]; syms X1 X2; Nguyễn Văn Nguyên – MSSV 20159868 Page 10 l1^2*lg2^2*m2^2 + J2*l1^2*m2 + J2*lg1^2*m1 + J1*lg2^2*m2 l1^2*lg2^2*m2^2*cos(u(3))^2 + lg1^2*lg2^2*m1*m2) q2cc = (u(2)*(m2*l1^2 + 2*m2*cos(u(3))*l1*lg2 + m1*lg1^2 + m2*lg2^2 + J1 + J2))/(J1*J2 + l1^2*lg2^2*m2^2 + J2*l1^2*m2 + J2*lg1^2*m1 + J1*lg2^2*m2 l1^2*lg2^2*m2^2*cos(u(3))^2 + lg1^2*lg2^2*m1*m2) - (u(1)*(m2*lg2^2 + l1*m2*cos(u(3))*lg2 + J2))/(J1*J2 + l1^2*lg2^2*m2^2 + J2*l1^2*m2 + J2*lg1^2*m1 + J1*lg2^2*m2 - l1^2*lg2^2*m2^2*cos(u(3))^2 + lg1^2*lg2^2*m1*m2) H Hình Khối H-1*X - Khối V có đầu vào dq; q2 tương ứng với đầu vào fcn u(1), u(2); u(3) Với: V1 = -2*m2*l1*lg2*sin(u[3])*u[1]*u[2]-m2*l1*lg2*sin(u[3])*u[2]*u[2] V2 = m2*l1*lg2*sin(u[3])*u[1]*u[1] Nguyễn Văn Nguyên – MSSV 20159868 Page 12 Hình Khối V - Khối G có đầu vào q1, q2 tương ứng với đầu vào fcn u(1), u(2) Với: g1 = m1*g*lg1*cos(u[1])+g*(l1*cos(u[1])+lg2*cos(u[1]+u[2])) g2 = m2*g*lg2*cos(u[1]+u[2]) Hình Khối G Mơ mơ hình robot simulink với đầu vào step, pulse, ramp H Hình mơ simulink robot với đầu vào step, pulse, ramp Đầu đầu vào khối robot step: Nguyễn Văn Nguyên – MSSV 20159868 Page 13 H Hình 8: góc pha q1 đầu vào dạng step Nguyễn Văn Nguyên – MSSV 20159868 Page 14 H Hình 9: góc q2 đầu vào dạng step Thiết kế điều khiển cho robot Mạch điều khiển cho robot: Nguyễn Văn Nguyên – MSSV 20159868 Page 15 Hình 10: mạch điều khiển robot + Khối động học nghịch: H Hình 11: sơ đồ khối động học nghịch Ta có: q2= acos((Px^2+Py^2-l1^2-l2^2/2*l1*l2) Hàm fcn có dạng: ((u(1)^2+u(2)^2-l1^2-l2^2)/2*l1*l2) q1 = atan2(A,B) đó : A=cos(q1)=[Px(l1+l2*cos(q2))+Py*l2*sin(q2)]/[(l1+l2cos(q2))^2+(l2*sin(q2))^2] B=sin(q1)=[Px(l1+l2*cos(q2))-Py*l2*sin(q2)]/[(l1+l2cos(q2))^2+(l2*sin(q2))^2] Nguyễn Văn Nguyên – MSSV 20159868 Page 16 + khối động học thuận Hình 12: sơ đồ khối động học thuận robot Ta có: x = l1*cosq1 + l2*cos(q1+q2) y = l1*sinq1 + l2*sin(q1+q2) Ta suy khối fcn có dạng: Fcn: l1*cos(u(1)) + l2*cos(u(1)+u(2)) Fcn1: l1*sin(u(1)) + l2*sin(u(1)+u(2)) H + khối đặt quỹ đạo chuyển động đường thẳng Đặt quỹ đạo chuyển động robot : y = -x + 3.5 đó: x = -0.2*t + 3.5 y = -x + 3.5 với: -0.2 vận tốc theo phương x ( 3.5; ) tọa độ ban đầu robot + Đặt quỹ đạo đường tròn Nguyễn Văn Nguyên – MSSV 20159868 Page 17 Hình 13: sơ đồ đặt vị trí theo đường thẳng Đặt quỹ đạo trịn có phương trình là: (x-1.5)^2 + y^2 = 2^2 Phương trình chuyển động robot: x = 1.5 +2*cos(0.2*t) y = 2*sin(0.2*t) Hình 14: sơ đồ đặt quỹ đạo đường tron với: bán kính đường trịn 0.2 vận tốc góc chuyển động Phương pháp P-D bù trọng trường a Sơ đồ mô H Hinh 15: sơ đồ mạch điều khiển phương phá PD bù TT Chọn Kp = 50000, Kd = 100000; b Kết mô + đặt quỹ đạo thẳng Nguyễn Văn Nguyên – MSSV 20159868 Page 18 1.6 1.4 1.2 góc pha q [rad] 0.8 q1 q2 0.6 0.4 0.2 -0.2 -0.4 thoi gian t [s] 10 Hình 16: đồ thị góc pha q(t) 3.6 3.4 3.2 toa x [m] 2.8 2.6 2.4 2.2 H 1.8 1.6 thoi gian t [s] 10 10 Hình 17: đồ thị x(t) 1.8 1.6 toa y [m] 1.4 1.2 0.8 0.6 0.4 0.2 0 thoi gian t [s] Hình 18: đồ thị y(t) Nguyễn Văn Nguyên – MSSV 20159868 Page 19 Hình 19: so sánh quỹ đạo thực quỹ đạo đặt đồ thị y(x) H Nguyễn Văn Nguyên – MSSV 20159868 Page 20 + đặt quỹ đạo tròn goc pha q [rad] 1.5 q1 q2 0.5 -0.5 Thoi gian t [s] 10 Hình 20: đồ thị góc pha q(t) 3.5 2.5 H toa x [m ] 1.5 0.5 Thoi gian t [s] 10 10 Hình 21: đồ thị x(t) 2.5 toa y [m ] 1.5 0.5 0 Thoi gian t [s] Hình 22: đồ thị y(t) Nguyễn Văn Nguyên – MSSV 20159868 Page 21 Hình 23: so sánh quỹ dạo thực quỹ đạo đặt y(x)  Nhận xét: điều khiển P-D bù trọng trường điều khiển thiết lập trạng thái ổn định nhanh, thời gian độ ngắn, sai lệch tĩnh nhanh làm cho robot hoạt động cách xác đạt chất lượng tốt Phương pháp momen tính tốn a Sơ đồ mơ H Sơ đồ simulink cho điều khiển momen tính tốn Hình 24: sơ đồ điều khiển momen tính tốn Trong sơ đồ này, qd lấy từ động học nghịch, góc đặt robot Nguyễn Văn Nguyên – MSSV 20159868 Page 22 Trong sơ đồ trên, khối ĐK khối có dạng: chọn Kp = 32 chọn Kd = b Kết mô + quỹ đạo đường thẳng 1.6 1.4 1.2 goc pha q [rad] 0.8 q1 q2 0.6 0.4 0.2 -0.2 -0.4 Thoi gian t [s] H 10 Hình 25: đồ thị góc pha q(t) 3.5 toa x [m] 2.5 1.5 Thoi gian t [s] Hình 26: đồ thị x(t) Nguyễn Văn Nguyên – MSSV 20159868 Page 23 10 2.5 toa y [m] 1.5 0.5 -0.5 Thoi gian t [s] Hình 27: đồ thị y(t) H Hình 28: so sánh quỹ dạo đặt quỹ đạo thực y(x) + quỹ đạo đường tròn Nguyễn Văn Nguyên – MSSV 20159868 Page 24 10 goc pha q [rad] 1.5 q1 q2 0.5 -0.5 Thoi gian t [s] 10 Hình 29: đồ thị góc pha q(t) 3.5 toa x [m] 2.5 1.5 H 0.5 Thoi gian t [s] 10 Hình 30: đồ thị x(t) 2.5 toa y [m] 1.5 0.5 0 Thoi gian t [m] Hình 31: đồ thị y (t) Nguyễn Văn Nguyên – MSSV 20159868 Page 25 10 Hình 32: so sánh quỹ đạo thực quỹ đạo đặt y(x) Nhận xét: điều khiển momen tính tốn điều khiển tính nhanh, xác Tuy nhiên, so với điều khiển PD bù trọng trường điều khiển momen tính tốn có chất lượng hơn, thời gian H Nguyễn Văn Nguyên – MSSV 20159868 Page 26