Tài liệu Tự động hóa

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 x J2 q1 l1 lg1 lg2 q2 m2 J1 m1 l2 z Hình 1: Cơ cấu Robot 1. Xây dựng mô hình robot Giả thiết khớp 1 sinh ra momen M1 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: Thế năng thanh nối 1: Tọa độ tâm khối thanh nối 2: Tốc độ tâm khối thanh nối 2: →Bình phương tốc độ dài tâm thanh nối 2: Động năng thanh nối 2: Thế năng thanh nối 2: Hàm lagrange của robot: Mômen của khớp 1: Mômen của khớp 2: Đặt , Ta có mô hình toán học robot: Trong đó: : ma trận quán tính Với: : thành phần momen nhớt và hướng tâm : thành phần momen trọng lực Với: 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 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: Trong đó các ma trận H, V, G được xác định theo trên - Thuật toán điều khiển: Trong đó: sai lệch góc qay: Tham số bộ điều khiển Tao khối: Tạo khối: Mô hình bộ điều khiển trên matlab: Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn: và Kết quả mô phỏng: Góc quay khớp 1,2: Sai lệch góc quay khớp 1,2