y x J2 1 l1 lg1 lg2 2 m2 J1 m1 l2 z Hình 1: Cơ cấu 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: l 1 = 0,5 m Chiều dài thanh nối 2: l 2 = 0,4 m Khối lượng thanh nối 1: m 1 = 1 Kg Khối lượng thanh nối 2: m 2 = 1 Kg Momen quán tính khớp 1 quay quanh tâm khối: J 1 = 0,5 Kgm 2 Momen quán tính khớp 2 quay quanh tâm khối: J 2 = 0,5 Kgm 2 Khoảng cách từ khớp 1 đến tâm thanh nối 1: l g1 = 0,25 m Khoảng cách từ khớp 2 đến tâm thanh nối 2: l g2 = 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 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 M 2 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à K i và P i (i=1,2) Ta có: Động năng thanh nối 1: 1 2 2 . . 2 1 1 1 1 1 1 1 2 2 g K m l J θ θ = + Thế năng thanh nối 1: 1 1 1 1 sin g P m gl θ = Tọa độ tâm khối thanh nối 2: 2 1 1 2 1 2 2 1 1 2 1 2 os os( + ) sin sin( + ) g g x l c l c y l l θ θ θ θ θ θ = +    = +   Tốc độ tâm khối thanh nối 2: . . . . 2 1 1 2 1 1 2 1 2 . . . . 1 1 2 1 1 2 1 2 2 sin sin( + )( ) os cos( + )( ) g g x l l y l c l θ θ θ θ θ θ θ θ θ θ θ θ  = − − +    = + +  →Bình phương tốc độ dài tâm thanh nối 2: 2 2 2 2 . . . . . . . . 2 2 2 2 2 1 1 2 1 1 2 2 1 2 1 2 22 ( ) 2 os ( ) g g v x y l l l l c θ θ θ θ θ θ θ = + = + + + + Động năng thanh nối 2: . . 2 2 2 2 2 2 2 1 1 1 ( ) 2 2 K m v J θ θ = + + 2 2 . . . . . . . . 2 2 2 2 1 1 2 1 1 2 2 2 2 1 2 1 2 2 2 1 1 1 [ ( ) 2 os ( )] ( ) 2 2 g g K m l l l l c J θ θ θ θ θ θ θ θ θ = + + + + + + Thế năng thanh nối 2: 2 2 1 1 2 1 2 [ sin sin( )] g P m g l l θ θ θ = + + Hàm lagrange của robot: 1 2 1 2 ( ) ( )L K K L L= + − + 1 2 2 . . 2 1 1 1 1 2 2 . . . . . . 2 2 2 1 1 2 1 1 2 2 1 2 1 2 2 . . 2 2 2 1 1 1 2 1 1 2 1 21 1 1 2 2 1 [ ( ) 2 os ( )] 2 1 ( ) sin [ sin sin( )] 2 g g g g g m l J m l l l l c J m gl m g l l θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ = + + + + + + + + + + − − + + Mômen của khớp 1: 1 . 1 1 ( ) ( ) d L L M dt θ θ ∂ ∂ = − ∂ ∂ 1 2 2 2 1 1 2 1 2 1 2 2 2 1 . . . 2 2 2 2 1 2 2 2 2 2 1 2 2 1 2 2 1 1 1 2 1 1 2 1 2 [ ( 2 os ) ] [ ( os ) ] sin (2 ) os [ sin os( )] g g g g g g g g m l J m l l l l c J m l l l c J m l l m gl c m g l l c θ θ θ θ θ θ θ θ θ θ θ θ = + + + + + + + + − + + + + + Mômen của khớp 2: 2 . 2 2 ( ) ( ) d L L M dt θ θ ∂ ∂ = − ∂ ∂ . 2 2 2 2 2 1 2 2 2 1 2 2 2 2 2 1 2 2 1 2 2 1 2 [ ( os ) ] ( ] sin os( ) g g g g g m l l l c J m l J m l l m gl c θ θ θ θ θ θ θ = + + + + + + + Đặt 1 2 M M M   =     , 1 2 q θ θ   =     Ta có mô hình toán học robot: . ( ) ( , ) ( )M H q q V q q G q= + + Trong đó: 11 12 21 22 H H H H H   =     : ma trận quán tính Với: 1 2 2 2 11 1 1 2 1 2 1 2 2 2 2 12 21 2 2 1 2 2 2 2 22 2 2 2 ( 2 os ) ( os ) g g g g g g H m l J m l l l l c J H H m l l l c J H m l J θ θ  = + + + + +   = = + +   = +   . . . 2 2 1 2 2 2 1 2 . 2 2 1 2 2 1 sin ( 2 ) sin g g m l l V m l l θ θ θ θ θ θ   − +   =       : thành phần momen nhớt và hướng tâm 1 2 g G g   =     : thành phần momen trọng lực Với: 1 1 1 1 2 1 1 2 1 2 2 2 2 1 2 os [( os os( )] os( ) g g g g m gl c m g l c l c g m gl c θ θ θ θ θ θ = + + +    = +   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]; - Khối 1 ( )H q − Hàm tính 1 ( )H q − 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: . ( ) dk P D M K e K q G q= − + Trong đó: sai lệch góc qay: d e q q= − Tham số bộ điều khiển 1 1 0 0 P K K K   =     Tao khối: 2 2 0 0 D K K K   =     Tạo khối: Mô hình bộ điều khiển trên matlab: /3 / 2 d q π π   =     0 0 /6 q π   =     Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn: 150 0 15 P K   =     và 3.50 0 3.5 D K   =     Kết quả mô phỏng: Góc quay khớp 1,2: [...]...Sai lệch góc quay khớp 1 ,2  . 1 2 2 2 11 1 1 2 1 2 1 2 2 2 2 12 21 2 2 1 2 2 2 2 22 2 2 2 ( 2 os ) ( os ) g g g g g g H m l J m l l l l c J H H m l l l c J H m l J θ θ  = + + + + +   = = + +   = +   . . . 2 2 1 2 2 2. tốc độ dài tâm thanh nối 2: 2 2 2 2 . . . . . . . . 2 2 2 2 2 1 1 2 1 1 2 2 1 2 1 2 22 ( ) 2 os ( ) g g v x y l l l l c θ θ θ θ θ θ θ = + = + + + + Động năng thanh nối 2: . . 2 2 2 2 2 2 2 1 1. 1: 1 . 1 1 ( ) ( ) d L L M dt θ θ ∂ ∂ = − ∂ ∂ 1 2 2 2 1 1 2 1 2 1 2 2 2 1 . . . 2 2 2 2 1 2 2 2 2 2 1 2 2 1 2 2 1 1 1 2 1 1 2 1 2 [ ( 2 os ) ] [ ( os ) ] sin (2 ) os [ sin os( )] g g g g g g g g m l