Bài tập dài điều khiển robot công nghiệp part 2

Cho một cơ cấu robot 2DOF như hình vẽSử dụng matlab- Simulink1. Xây dựng mô hình toán học robot2. Thiết kế bộ điều khiển PID bù trọng trường. Chạy mô phỏngSố liệu của robot:Chiều dài thanh nối 1: l1 = 0,5 mChiều dài thanh nối 2: l2 = 0,4 mKhối lượng thanh nối 1: m1 = 1 KgKhối lượng thanh nối 2: m2 = 1 KgMomen quán tính khớp 1 quay quanh tâm khối: J1 = 0,5 Kgm2Momen quán tính khớp 2 quay quanh tâm khối: J2 = 0,5 Kgm2Khoảng cách từ khớp 1 đến tâm thanh nối 1: lg1...
Nội dung trích xuất từ tài liệu:
Bài tập dài điều khiển robot công nghiệp part 2Bài số 2Cho 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ỏngSố 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 rad1. 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 ướngtheo trục yKý 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ínhVới:: thành phần momen nhớt và hướng tâm: thành phần momen trọng lựcVớ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 botm1 = 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 Gg1 = 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 robotm1 = 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 VV1 = -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ốiHàm tínhfunction y = H(q,M)% thong so robotm1 = 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 khoptheta1=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 khopy = 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ểnTao 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