robot planar 2 bậc tự do ( 2DOF) – tính số bậc tự do của robot

Đề 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ự Ta xây dựng bảng thông số D-H Joint i di i q1 q2 0 a1 a2 0 - Các ma trận D-H - Điểm tác động thao tác cuối ZE= Nguyễn Văn Nguyên – MSSV 20159868 Page - Tính ma trân Jacobi tịnh tiến Của khâu → → ; - Ma trận Jacobi tịnh tiến khâu Ta có: , → → 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 , ta lại có: = - Tính ma trận jacobi quay khâu Nguyễn Văn Nguyên – MSSV 20159868 Page - Tính tensor quán tính khâu khâu - , - Áp dụng phương trình lagrange loại II tính ma trận khối lượng M 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: - Tính lực hai khâu Tính lực suy rộng lực khơng Nguyễn Văn Nguyên – MSSV 20159868 Page 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: Phương trình vi phân chuyển động: II Khảo sát tốn tĩnh học robot Ta có véc tơ biểu diễn lực momen hệ ,, , Nguyễn Văn Nguyên – MSSV 20159868 Page Áp dụng phương trình cân lực trọng tọa độ khâu thứ ta sau: - 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; 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 Trong đó: L = ∑Ki - ∑Pi với: ∑Ki : tổng động ∑Pi : tổng Nguyễn Văn Nguyên – MSSV 20159868 Page Với robot Planar 2DOF ta có: M = H(q) + V(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; 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*^2 + 2*h112*()*() V2 = h211*()^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ình 2.Mơ hình động học Robot Mô simulink Nguyễn Văn Nguyên – MSSV 20159868 Page  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; X = [X1;X2]; K =H*X h122 = -1*m2*l1*lg2*sin(q2); h112 = -1*m2*l1*lg2*sin(q2); h211 = m2*l1*lg2*sin(q2); V1 = h122*X2^2 + 2*h112*(X1)*(X2) V2 = h211*(X1)^2 g1 = m1*g*lg1*cos(q1) + g*(l1*cos(q1) + lg2*cos(q1+q2)) g2 = m2*g*lg2*cos(q1+q2) Hz= inv(H)*X Ma trận X ma trận 2x1, xem tín hiệu tổng hợp đưa vào để nhân với khối H khối H-1 Ví dụ: mơ hình robot X=M–V–G Hình ảnh bên khối robot có đầu vào M đầu q Trong khối robot mơ hình động lực học: Nguyễn Văn Ngun – MSSV 20159868 Page Hình Sơ đồ simulink khối robot Trong khối H-1*X; V; G khối function ( f(u) ) - Khối H-1*X có đầu vào M1, M2 q2 tương ứng với đầu vào khối fcn u(1), u(2), u(3) Với: q1cc = (u(1)*(m2*lg2^2 + 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(2)*(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) 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ình Khối H-1*X Nguyễn Văn Nguyên – MSSV 20159868 Page - 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] 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 Nguyễn Văn Nguyên – MSSV 20159868 Page 10 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 11 Hình 8: góc pha q1 đầu vào dạng step Nguyễn Văn Nguyên – MSSV 20159868 Page 12 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 13 Hình 10: mạch điều khiển robot + Khối động học nghịch: 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 14 + 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)) + 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 Nguyễn Văn Nguyên – MSSV 20159868 Page 15 Hình 13: sơ đồ đặt vị trí theo đường thẳng + Đặt quỹ đạo đường tròn Đặ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ô 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 16 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 do x [m] 2.8 2.6 2.4 2.2 1.8 1.6   thoi gian t [s] 10 Hình 17: đồ thị x(t)   1.8 1.6 toa do 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 17 10 Hình 19: so sánh quỹ đạo thực quỹ đạo đặt đồ thị y(x) Nguyễn Văn Nguyên – MSSV 20159868 Page 18 + đặ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   toa do x  [m ] 2.5 1.5 0.5   Thoi gian t [s] 10 Hình 21: đồ thị x(t) 2.5   toa do 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 19 10 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ô 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 20 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] 10 Hình 25: đồ thị góc pha q(t) 3.5   toa do 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 21 10 2.5   toa do y [m] 1.5 0.5 ­0.5   Thoi gian t [s] Hình 27: đồ thị y(t) 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 22 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 do x [m] 2.5 1.5 0.5   Thoi gian t [s] 10 Hình 30: đồ thị x(t) 2.5   toa do 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 23 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 Nguyễn Văn Nguyên – MSSV 20159868 Page 24 ... l1 ^2* lg2 ^2* m2 ^2* cos(u(3)) ^2 + lg1 ^2* lg2 ^2* m1*m2) - (u (2 ) *(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) 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. .. 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ình