Dieu khien robot final
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (2.49 MB, 74 trang )
TRƯỜNG ĐẠI HỌC GIAO THƠNG VẬN TẢI
KHOA CƠ KHÍ
BỘ MƠN KỸ THUẬT MÁY
BÀI TẬP LỚN
ĐIỀU KHIỂN ROBOT
Giáo viên hướng dẫn:
PGS.TS. Đinh Thị Thanh Huyền
Lớp:
Cơ điện tử 2
Khóa:
59
Nhóm sinh viên thực
hiện:
Nhóm 4
Phạm Trung Thanh Tùng
MSV: 181302168
Đồn Quang Vinh
MSV: 181300483
Nguyễn Đình Trường
MSV: 181301033
Nguyễn Trung Kiên
MSV: 181300827
Hà Nội, Tháng 05 Năm 2022
CHƯƠNG 1: ĐỘNG LỰC HỌC ROBOT
Hình 1.1 Mơ hình cánh tay robot
Giới thiệu về rơ bốt.
● Khối lương và chiều dài khâu:
1.1 Đế Robot
Vật liệu : gang xám.
Khối lượng: 76,11 kg.
2
Hình 1.2 Đế của robot.
1.2 Khâu 1 :
● Vật liệu : thép cacbon.
● Khối lượng: 18,61 Kg
3
Hình 1.3 khâu 1 của robot.
1.3 khâu 2:
● vật liệu: thép cacbon.
● Khối lượng: 14,99 Kg
4
Hình 1.4 khâu 2 của robot.
1.4 khâu 3:
● vật liệu: thép cacbon.
● Khối lượng: 14,83 Kg
5
Hình 1.5 khâu 3 của robot.
1.5 khâu 4:
● vật liệu: thép cabon.
● Khối lượng: 5,14 Kg.
6
Hình 1.6 Khâu 4 của robot.
1.6 Chọn hệ quy chiếu, gắn hệ trục tọa độ suy rộng lên các khâu và lập bảng DH.
Hình 1.7 Hệ trục của cánh tay Robot.
7
● Bảng DH:
i
1
0
0
0
2
90o
0
L1
3
0o
L2
0
4
-180o
L3
0
5
90o
0
L4
90+
180
● Từ bảng DH ta có ma trận thuần nhất của các khâu:
Do cách chọn trục sai so với quy tắc của bảng ĐH nên tính lại ta có:
=Rot(X,90o) trans(0,L1,0)Rot(Z,2)
8
● Vecto tọa độ trọng tâm (bỏ qua khâu 0)
Trọng tâm khâu 1: x1= 0; y1= 0; z1= 0,04 (m).
1
rG 0 0 0,04 1
Trọng tâm khâu 2: x2=0,14 ; y2=-0,01; z2=-0,02 (m).
2
rG 0,14 -0,01 -0,02 1
Trọng tâm khâu 3: x3= 0.09; y3=0; z3=0 (m).
3
rG 0,09 0 0 1
Trọng tâm khâu 4: x4=0; y4=-0,; z4=0 (m).
4
rG 0 -0,1 0, 1
● Vecto gia tốc trọng trường
● Momen quán tính tại trọng tâm (kg.m2)
Khâu 1:
Ixx1= 0,1203
Iyy1= 0,1174
Izz1= 0,0663
Khâu 2:
Ixx2= 0,0243
Ixy2= - 0,0207
Ixz2= - 0,0425
Iyy2= 0,4329
9
Iyz2= 0,0021
Izz2= 0,429
Khâu 3:
Ixx3= 0,0283
Ixy3= - 0,0016
Iyy3= 0,2099
Izz3= 0,2003
Khâu 4:
Ixx4= 0,0837
Iyy4= 0,004
Izz4= 0,0818
● Ma trận giả quán tính được tính theo cơng thức:
1.7 Lập phương trình động lực học.
Áp dụng phương trình ĐLH cho Robot 4 bậc tự do ( n=4) và khai triển chi tiết ta
được:
10
11
12
13
● Code Matlab thiết lập hệ phương trình vi phân chuyển động của Robot:
clc;
syms q1 q2 q3 q4 qd1 qd2 qd3 qd4 qdd1 qdd2 qdd3 qdd4 m1 m2 m3 m4 L1 L2
L3 L4 g dx1 dy1 dz1 dx2 dy2 dz2 dx3 dy3 dz3 dx4 dy4 dz4
%Vector gia toc trong truong
Vg=[0;0;g;1];
%Vector toa do khoi tam trong he quy chieu cua khau
rRG1=[0;0; 0.04;1];
rRG2=[0.14; -0.01 ; -0.02; 1];
rRG3=[0.14; 0; 0; 1];
rRG4=[0; -0.1; 0; 1];
% ma tran gia quan tinh cua cac khau
Ixx1=0.12;
Iyy1=0.12;
Izz1=0.07;
Ixy1=0;
Ixz1=0;
Iyx1=0;
Izx1=0;
Iyz1=0;
Ixx2=0.02;
Ixy2=-0.02;
Ixz2=-0.04;
Iyx2=-0.02;
Iyy2=0.43;
Iyz2=0;
14
Izx2=-0.04;
Izy2=0;
Izz2=0.43;
Ixx3=0.03;
Iyy3=0.21;
Ixz3=0;
Iyx3=0;
Ixy3=0;
Iyz3=0;
Izx3=0;
Izy=0;
Izz3=0.2;
Ixx4=0.08;
Ixy4=0;
Ixz4=0;
Iyx=0;
Iyy4=0;
Iyz4=0;
Izx4=0;
Izy4=0;
Izz4=0.08;
dx1=0; dy1=0; dz1=-0.04; dx2=-0.14; dy2=0.01; dz2=0.02;dx3=-0.09; dy3=0;
dz3=0; dx4=0; dy4=0.1; dz4=0;
L1=0.13; L2=0.3; L3=0.2;
L4=0.16;g=9.81;m1=18.61 ;m2=14.99;m3=14.83;m4=5.14;
J1=[(-Ixx1+Iyy1+Izz1)/2+m1*dx1*dx1 Ixy1+m1*dx1*dy1 Ixz1*m1*dx1*dz1 m1*dx1; Ixy1+m1*dx1*dy1 (Ixx1-Iyy1+Izz1)/2+m1*dy1*dy1
15
Iyz1+m1*dy1*dz1 -m1*dy1; Ixz1+m1*dx1*dz1 Iyz1+m1*dy1*dz1 (Ixx1+Iyy1Izz1)/2+m1*dz1*dz1 -m1*dz1; -m1*dx1 -m1*dy1 -m1*dz1 m1];
J2=[(-Ixx2+Iyy2+Izz2)/2+m2*dx2*dx2 Ixy2+m2*dx2*dy2 Ixz2*m2*dx2*dz2 m2*dx2; Ixy2+m2*dx2*dy2 (Ixx2-Iyy2+Izz2)/2+m2*dy2*dy2
Iyz2+m2*dy2*dz2 -m2*dy2; Ixz2+m2*dx2*dz2 Iyz2+m2*dy2*dz2 (Ixx2+Iyy2Izz2)/2+m2*dz2*dz2 -m2*dz2; -m2*dx2 -m2*dy2 -m2*dz2 m2];
J3=[(-Ixx3+Iyy3+Izz3)/2+m3*dx3*dx3 Ixy3+m3*dx3*dy3 Ixz3*m3*dx3*dz3 m3*dx3; Ixy3+m3*dx3*dy3 (Ixx3-Iyy3+Izz3)/2+m3*dy3*dy3
Iyz3+m3*dy3*dz3 -m3*dy3; Ixz3+m3*dx3*dz3 Iyz3+m3*dy3*dz3 (Ixx3+Iyy3Izz3)/2+m3*dz3*dz3 -m3*dz3; -m3*dx3 -m3*dy3 -m3*dz3 m3];
J4=[(-Ixx4+Iyy4+Izz4)/2+m4*dx4*dx4 Ixy4+m4*dx4*dy4 Ixz4*m4*dx4*dz4 m4*dx4; Ixy4+m4*dx4*dy4 (Ixx4-Iyy4+Izz4)/2+m4*dy4*dy4
Iyz4+m4*dy4*dz4 -m4*dy4; Ixz4+m4*dx4*dz4 Iyz4+m4*dy4*dz4 (Ixx4+Iyy4Izz4)/2+m4*dz4*dz4 -m4*dz4; -m4*dx4 -m4*dy4 -m4*dz4 m4];
T01=[cos(q1) -sin(q1) 0 0; sin(q1) cos(q1) 0 0; 0 0 1 0; 0 0 0 1];
T12=[cos(q2) -sin(q2) 0 0; 0 0 -1 L1; sin(q2) cos(q2) 0 0; 0 0 0 1];
T23=[cos(q3) -sin(q3) 0 L2; sin(q3) cos(q3) 0 0; 0 0 1 0; 0 0 0 1];
T34=[sin(q4) cos(q4) 0 L3; cos(q4) -sin(q4) 0 0; 0 0 1 0; 0 0 0 1];
T02=T01*T12;
T03=T02*T23;
T04=T03*T34;
%phuong trinh đong luc hoc
F1= trace(diff(T01,q1)*J1*transpose(diff(T01,q1)))*qdd1+...
trace(diff(T02,q1)*J2*transpose(diff(T02,q1)))*qdd1+...
trace(diff(T02,q2)*J2*transpose(diff(T02,q1)))*qdd2+...
trace(diff(T03,q1)*J3*transpose(diff(T03,q1)))*qdd1+...
trace(diff(T03,q2)*J3*transpose(diff(T03,q1)))*qdd2+...
trace(diff(T03,q3)*J3*transpose(diff(T03,q1)))*qdd3+...
trace(diff(T04,q1)*J4*transpose(diff(T04,q1)))*qdd1+...
trace(diff(T04,q2)*J4*transpose(diff(T04,q1)))*qdd2+...
16
trace(diff(T04,q3)*J4*transpose(diff(T04,q1)))*qdd3+...
trace(diff(T04,q4)*J4*transpose(diff(T04,q1)))*qdd4+...
trace(diff(diff(T01,q1),q1)*J1*transpose(diff(T01,q1)))*qd1*qd1+...
2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q1)))*qd1*qd2+...
trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q1)))*qd2*qd2+...
trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q1)))*qd1*qd1+...
2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q1)))*qd1*qd2+...
2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q1)))*qd1*qd3+...
trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q1)))*qd2*qd2+...
2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q1)))*qd2*qd3+...
trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q1)))*qd3*qd3+...
trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q1)))*qd1*qd1+...
2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q1)))*qd1*qd2+...
2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q1)))*qd1*qd3+...
2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q1)))*qd1*qd4+...
trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q1)))*qd2*qd2+...
2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q1)))*qd2*qd3+...
trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q1)))*qd3*qd3+...
2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q1)))*qd3*qd4+...
trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q1)))*qd4*qd4-...
m1*transpose(Vg)*diff(T01,q1)*rRG1-...
m2*transpose(Vg)*diff(T02,q1)*rRG2-...
m3*transpose(Vg)*diff(T03,q1)*rRG3-...
m4*transpose(Vg)*diff(T04,q1)*rRG4;
F2=trace(diff(T02,q1)*J2*transpose(diff(T02,q2)))*qdd1+...
trace(diff(T02,q2)*J2*transpose(diff(T02,q2)))*qdd2+...
17
trace(diff(T03,q1)*J3*transpose(diff(T03,q2)))*qdd1+...
trace(diff(T03,q2)*J3*transpose(diff(T03,q2)))*qdd2+...
trace(diff(T03,q3)*J3*transpose(diff(T03,q2)))*qdd3+...
trace(diff(T04,q1)*J4*transpose(diff(T04,q2)))*qdd1+...
trace(diff(T04,q2)*J4*transpose(diff(T04,q2)))*qdd2+...
trace(diff(T04,q3)*J4*transpose(diff(T04,q2)))*qdd3+...
trace(diff(T04,q4)*J4*transpose(diff(T04,q2)))*qdd4+...
trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q2)))*qd1*qd1+...
2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q2)))*qd1*qd2+...
trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q2)))*qd2*qd2+...
trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q2)))*qd1*qd1+...
2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q2)))*qd1*qd2+...
2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q2)))*qd1*qd3+...
trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q2)))*qd2*qd2+...
2*trace(diff(diff(T03,q3),q2)*J3*transpose(diff(T03,q2)))*qd3*qd2+...
trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q2)))*qd3*qd3+...
trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q2)))*qd1*qd1+...
2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q2)))*qd1*qd2+...
2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q2)))*qd1*qd3+...
2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q2)))*qd1*qd4+...
trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q2)))*qd2*qd2+...
2*trace(diff(diff(T04,q3),q2)*J4*transpose(diff(T04,q2)))*qd2*qd3+...
2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q2)))*qd2*qd4+...
trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q2)))*qd3*qd3+...
2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q2)))*qd3*qd4+...
trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q2)))*qd4*qd4+...
18
-m2*transpose(Vg)*diff(T02,q2)*rRG2-...
m3*transpose(Vg)*diff(T03,q2)*rRG3-...
m4*transpose(Vg)*diff(T04,q2)*rRG4;
F3= trace(diff(T03,q1)*J3*transpose(diff(T03,q3)))*qdd1+...
trace(diff(T03,q2)*J3*transpose(diff(T03,q3)))*qdd2+...
trace(diff(T03,q3)*J3*transpose(diff(T03,q3)))*qdd3+...
trace(diff(T04,q1)*J4*transpose(diff(T04,q3)))*qdd1+...
trace(diff(T04,q2)*J4*transpose(diff(T04,q3)))*qdd2+...
trace(diff(T04,q3)*J4*transpose(diff(T04,q3)))*qdd3+...
trace(diff(T04,q4)*J4*transpose(diff(T04,q3)))*qdd4+...
trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q3)))*qd1*qd1+...
2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q3)))*qd1*qd2+...
2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q3)))*qd1*qd3+...
trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q3)))*qd2*qd2+...
2*trace(diff(diff(T03,q3),q2)*J3*transpose(diff(T03,q3)))*qd3*qd2+...
trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q3)))*qd3*qd3+...
trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q3)))*qd1*qd1+...
2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q3)))*qd1*qd2+...
2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q3)))*qd1*qd3+...
2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q3)))*qd1*qd4+...
trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q3)))*qd2*qd2+...
2*trace(diff(diff(T04,q3),q2)*J4*transpose(diff(T04,q3)))*qd3*qd2+...
trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q3)))*qd3*qd3+...
2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q3)))*qd3*qd4+...
trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q3)))*qd4*qd4-...
m3*transpose(Vg)*diff(T03,q3)*rRG3- m4*transpose(Vg)*diff(T04,q3)*rRG4;
19
F4= trace(diff(T04,q1)*J4*transpose(diff(T04,q4)))*qdd1+...
trace(diff(T04,q2)*J4*transpose(diff(T04,q4)))*qdd2+...
trace(diff(T04,q3)*J4*transpose(diff(T04,q4)))*qdd3+...
trace(diff(T04,q4)*J4*transpose(diff(T04,q4)))*qdd4+...
trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q4)))*qd1*qd1+...
2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q4)))*qd1*qd2+...
2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q4)))*qd1*qd3+...
2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q4)))*qd1*qd4+...
trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q4)))*qd2*qd2+...
2*trace(diff(diff(T04,q3),q2)*J4*transpose(diff(T04,q4)))*qd2*qd3+...
2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q4)))*qd2*qd4+...
trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q4)))*qd3*qd3+...
2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q4)))*qd3*qd4+...
trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q4)))*qd4*qd4-...
m4*transpose(Vg)*diff(T04,q3)*rRG4;
F1=simplify(F1)
F2=simplify(F2)
F3=simplify(F3)
F4=simplify(F4)
● Từ matlab ta có phương trình vi phân chuyển động (đã thu gọn) của robot:
F1 =(2662337822617180897453*qdd1)/1125899906842624000000 +
(56286100732867145429*qdd1*cos(2*q2))/45035996273704960000 +
(37829124300660220784889*qd2^2*cos(q2))/36028797018963968000000 (257*qdd1*cos(2*q2 + 2*q3 - q4))/2500 + (657*qdd1*cos(2*q2 + 2*q3 - 2*q4))/
10000 + (5906705098477029*qdd1*sin(2*q2))/144115188075855872 +
(810107500971404828791*qd2^2*sin(q2))/36028797018963968000000 +
(5693934785881723169*qdd1*cos(2*q2 + 2*q3))/22517998136852480000 (3341*qdd2*sin(q2 + q3 - q4))/50000 - (3341*qdd3*sin(q2 + q3 - q4))/50000 +
(3341*qdd4*sin(q2 + q3 - q4))/50000 + (307151*qdd2*sin(q2 + q3))/1000000 +
20
