Tài liệu Bài tập môn robot có lời giải xây dựng mô hình robot

Bài số 2 Cho một cơ cấu robot 2DOF như hình vẽ Sử dụng matlab- Simulink 1. Xây dựng mô hình toán học robot 2. Thiết kế bộ điều khiển PID bù trọng trường. Chạy mô phỏng Số liệu của robot: Chiều dài thanh nối 1: l1 = 0,5 m Chiều dài thanh nối 2: l2 = 0,4 m Khối lượng thanh nối 1: m1 = 1 Kg Khối lượng thanh nối 2: m2 = 1 Kg Momen quán tính khớp 1 quay quanh tâm khối: J1 = 0,5 Kgm2 Momen quán tính khớp 2 quay quanh tâm khối: J2 = 0,5 Kgm2 Khoảng cách từ khớp 1 đến tâm thanh nối 1: lg1 = 0,25 m Khoảng cách từ khớp 2 đến tâm thanh nối 2: lg2 = 0,2 m Góc ban đầu của các thanh nối: θ1 = 0 rad ; θ2 = Л/6 rad Góc đặt của các thanh nối: θ1 = Л/3 rad ; θ2 = Л/2 rad y m2 J2 l1 lg1 J1 1 z 2 l2 lg2 m1 Hình 1: Cơ cấấu Robot x 1. Xây dựng mô hình robot Giả thiết khớp 1 sinh ra momen M 1 tác dụng giữa bệ và thanh nối 1; khớp 2 sinh ra momen M2 tác dụng giữa thanh nối 1 và 2; trọng lực có hướng theo trục y Ký hiệu động năng và thế năng của thanh nối i là Ki và Pi (i=1,2) Ta có: Động năng thanh nối 1: . 2 . 2 1 1 2 K1  m1l g 1  1  J1  1 2 2 Thế năng thanh nối 1: P1  m1 glg1 sin 1 Tọa độ tâm khối thanh nối 2:  x2  l1cos1  lg 2cos(1 + 2 )   y2  l1 sin 1  l g 2sin(1 + 2 ) Tốc độ tâm khối thanh nối 2: . . . .  x 2  l1 sin 1  1  lg 2sin(1 + 2 )( 1   2 ) . . . .  y 2  l1cos1  1  l g 2cos(1 + 2 )( 1   2 )  →Bình phương tốc độ dài tâm thanh nối 2: . 2 . 2 . 2 v  x2  y2  l  1  l 2 2 2 1 2 g2 . . . 2 . . ( 1   2 )  2l1lg 2cos 2 ( 1   1  2 ) 2 Động năng thanh nối 2: K2  . . 1 1 m2v22  J 2 ( 1   2 ) 2 2 2 . 2 . . . 2 . . . . 1 1 2 2 2 K 2  m2 [l1  1  l g 2 ( 1   2 )  2l1lg 2cos 2 ( 1   1  2 )]  J 2 ( 1   2 ) 2 2 2 Thế năng thanh nối 2: P2  m2 g [l1 sin 1  l g 2 sin(1   2 )] Hàm lagrange của robot: L  ( K1  K 2 )  ( L1  L2 ) . 2 . 2 1 1 2  m1l g 1  1  J1  1  2 2 . 2 . . . 2 . . 1 2 2 2  m2 [l1  1  l g 2 ( 1   2 )  2l1l g 2cos 2 ( 1   1  2 )]  2 . . 1  J 2 ( 1   2 ) 2  m1 gl g 1 sin 1  m2 g[l1 sin 1  l g 2 sin(1   2 )] 2 Mômen của khớp 1: M1  d L L ( . )( ) dt    1 1 ..  [m1l g21  J1  m2 (l12  l g22  2l1l g 2cos 2 )  J 2 ]1 [m2 (l 2 g2 .. . . .  l1lg 2cos 2 )  J 2 ] 2  m2l1l g 2 sin  2 (21  2   22 )  m1 gl g 1cos1  m2 g [l1 sin 1  l g 2cos(1   2 )] Mômen của khớp 2: M2  d L L ( . )( ) dt   2 2  [m2 (l 2 g2 ..  l1l g 2cos 2 )  J 2 ]1  ( m l 2 2 g2 M 1  1  M  q     M 2   2 , Ta có mô hình toán học robot: .. . M  H ( q ) q  V ( q, q )  G ( q ) Trong đó: H11 H12  H   H 21 H 22  : ma trận quán tính .  J 2 ] 2  m2l1l g 2 sin  2 12  m2 gl g 2cos(1   2 ) Đặt .. Với:  H11  m1l g21  J1  m2 (l12  lg22  2l1l g 2cos 2 )  J 2  2  H12  H 21  m2 (lg 2  l1lg 2cos 2 )  J 2  2  H 22  m2lg 2  J 2 . . .   2  m l l sin  (   2   ) 2 1 g 2 2 2 1 2   V   . 2   m2l1l g 2 sin  2 1  : thành phần momen nhớt và hướng tâm  g1  G  g 2  : thành phần momen trọng lực Với:   g1  m1 gl g 1cos1  m2 g[(l1cos1  l g 2cos(1   2 )]   g 2  m2 glg 2cos(1   2 ) Thực hiện mô phỏng trên matlab: Mô hình trên matlab: Trong đó: - Hàm tính G: function y = G(q) % thong so ro bot m1 = 1; m2 = 1; l1 = 0.5; l2 = 0.4; g = 9.8; J1 = 0.5; J2 = 0.5; lg1= l1/2; lg2 = l2/2; % ma tran G g1 = m1*g*lg1*cos(q(1))+m2*g*[l1*cos(q(1))+lg2*cos(q(1)+q(2))]; g2 = m2*g*lg2*cos(q(1)+q(2)); y = [g1;g2]; - Hàm tính V: function y = V(q,dq) % thong so robot m1 = 1; m2 = 1; l1 = 0.5; l2 = 0.4; g = 9.8; J1 = 0.5; J2 = 0.5; lg1= l1/2; lg2 = l2/2; % ma tran V V1 = -m2*l1*lg2*sin(q(2))*[dq(2)^2+2*dq(1)*dq(2)]; V2 = m2*l1*lg2*sin(q(2))*dq(1)^2; y = [V1;V2]; H 1 ( q ) - Khối H 1 (q ) Hàm tính function y = H(q,M) % thong so robot m1 = 1; m2 = 1; l1 = 0.5; l2 = 0.4; g = 9.8; J1 = 0.5; J2 = 0.5; lg1 = l1/2; lg2 = l2/2; % bien khop theta1=q(1); theta2=q(2); % ma tran H(q) H11=m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(theta2))+J2; H12=m2*(lg2^2+l1*lg2*cos(theta2))+J2; H21=H12; H22=m2*lg2^2+J2; H = [H11 H12;H21 H22]; % ma tran nghich dao H^(-1)(q) Hinv = inv(H); % Gia toc khop y = Hinv * M; Tạo khối robot (Create Subsystem) 2. Thiết kế bộ điều khiển PD bù trọng trường - Mô hình toán học robot: .. . M  H (q ) q  V ( q, q )  G ( q ) Trong đó các ma trận H, V, G được xác định theo trên - Thuật toán điều khiển: . M dk  K P e  K D q  G ( q) Trong đó: sai lệch góc qay: Tham số bộ điều khiển K 0  KP   1  0 K1  e  qd  q Tao khối: K 0  KD   2  0 K 2  Tạo khối: Mô hình bộ điều khiển trên matlab:  / 3  qd     / 2  0  q0     / 6  Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn: 150  KP    0 15 Kết quả mô phỏng: và 3.50  KD    0 3.5  Góc quay khớp 1,2: Sai lệch góc quay khớp 1,2