Thiết kế, mô phỏng tay robot mitsubishi MZ07 6 bậc tự do và tính toán động học ngược bằng matlab, có điều khiển bằng PID, PI, PD( full code, bản vẽ 3d solid, simulink matlab)
TỔNG QUAN VỀ ROBOT ABB IRB 2400
Tổng quan về ROBOT NACHI-MZ07-01
Robot MZ07-01 có kích thước như một cánh tay con người.
Có thể làm việc trong môi trường chật hẹp.
Cánh tay robot có thể chạm đến khoảng cách 723 mm
Robot có trọng lượng nhẹ và kích thước nhỏ nên có thể đặt bên trong máy gia công, treo trên cao.
Bộ phận điều khiển được tích hợp trong cánh tay Robot Thuận lợi cho việc nối dây điện, dây hơi.
Là robot có tốc độ di chuyển nhanh Sử dụng tủ điều khiển CFD.
Có thể kết nối vài Robot trong một dây chuyền làm việc.
Mô hình cánh tay robot
Thông số kỹ thuật
Khả năng chạm đến cánh tay 723 mm.
Gắn kết: gắn trên sàn nhà, trần Bậc tự do: 6 bậc,5 khớp Độ lặp lại vị trí : ±0,03 mm (bề mặt bích cổ tay) tuân theo tiêu chuẩn ISO
Ứng dụng
Các khâu và thông số trên Solidwork
TÍNH TOÁN THÔNG SỐ ĐỘNG HỌC
Gắn hệ tọa độ lên từng khâu
Đặt hệ trục tọa độ cho Robot
Bảng D-H
Khâu i αi−1 (rad) ai−1 (m) di(m) θi (rad)
Bộ thông số ai, ∝ � , � � và � � được gọi là bộ thông số Denavit – Hartenberg (DH) Khi các hệ quy chiếu được gắn trên Robot bộ thông số DH được xác định chi tiết như sau:
ai: Khoảng cách từ trục Zi-1đến trục Zi đo dọc theo trục Xi-1
∝ � : Góc giữa trục Zi-1và trục Zixác định theo trục Xi-1
di: Khoảng cách từ trục Xi-1đến trục Xi đo theo trục Zi
� � : Góc giữa trục Xi-1và trục Xixác định theo trục Zi
2 3 Các ma trận biến đổi thuần nhât:
Trên cơ sở đã đặt các hệ quy chiếu mô tả tất cả các khâu của Robot, ta có thể thiết lập mối quan hệ giữa các hệ quy chiếu liên tiếp nhưa bởi ma trận biến đổi thuần nhất với các đặc điểm sau:
- Quay quanh trục Xi-1một góc xoắn αi-1. - Tịnh tiến dọc trục Xi-1một đoạn ai-1. - Quay quanh trục Zimột gócθ i - Tịnh tiến dọc trục Zi một đoạn di. Ta có thể viết biểu thức toán học như sau: i−1 iT= Rot (X,α i−1 ).Trans (α i−1 ,0 ,0).Rot (Z,θ 2 ) Trans (0,0 ,di) Trong đó:
Biến đổi biểu thức (1.1), ta có: i−1 iT = Rot(X, α i−1 ) Trans (αi−1, 0, 0) Rot (Z, θi) Trans (0, 0,d i ) i−1 iT cθi −sθi 0 ai−1 cα i−1 �θi cα i−1 �θi −sαi−1 − d i sαi−1 sα i−1 �θ i sα i−1 �θ i cα i−1 d i cα i−1
Ma trận biến đổi tổng quát từ hệ i-1 sang hệ i:
+)Ma trận biến đổi tổng thể thuần nhất T là:( 4 hàng-4 cột)_”Sử dụng phần mềm Matlab”
Xác định vector biểu diễn trọng tâm của từng khâu và vector gia tốc trọng trường
Vector biểu diễn trọng tâm của khâu i trong hệ quy chiếu gốc:
��� � = ��� ��� � �� 1 Vector gia tốc trọng trường:
Giải bài toán cụ thể:
�= �� �� � � 0 � Vector biểu diễn trọng tâm của từng khâu trong hệ quy chiếu gốc: rRG1=[-dx1; -dy1; -dz1; 1]; rRG2=[-dx2; -dy2; -dz2; 1]; rRG3=[-dx3; -dy3; -dz3; 1]; rRG4=[-dx4; -dy4; -dz4; 1]; rRG5=[-dx5; -dy5; -dz5; 1]; rRG6=[-dx6; -dy6; -dz6; 1];
Vector tọa độ trọng tâm
Vector gia tốc trọng trường g=[0 0 −9.81 1] �
Ma trận giả quán tính được tính theo công thức:
2.4 Phương trinh động lực học:
∂q i j r G j Tư do va khai triên ta đươc:
2.5 Chương trinh Matlab: clc; symst real%th?i gian s symsq1 q2 q3 q4 q5 q6%radian
%theta1= ; theta2= ; theta3= ; theta4= ; theta5= ; theta6= ;
%q1=theta1; q2=theta2; q3=theta3; q4=theta4-90; q5=theta5; q6=theta6+30;
%syms qdd1 qdd2 qdd3 qdd4 qdd5 qdd6 qd1 qd2 qd3 qd4 qd5 qd6
%syms d1 d2 d3 d4 d5 d6 real qdd1=q1/(t*t); qdd2=q2/(t*t); qdd3=q3/(t*t); qdd4=q4/(t*t); qdd5=q5/(t*t); qdd6=q6/(t*t); qd1=q1/t; qd2=q2/t; qd3=q3/t; qd4=q4/t; qd5=q5/t; qd6=q6/t; d10; d2=0; d3=0; d4=0; d5=0; d6P;
%syms al1 al2 al3 al4 al5 al6 real al1=0; al2=-90; al3=0; al4=-90; al5; al6=-90;
% Vector gia toc trong truong Vg=[0; 0; -9.81; 1];
% Vector toa do khoi tam trong he quy cua khau rRG1=[-34.01; -0.03; 100.15; 1]; rRG2=[-66.87; -35.24; 0.01; 1]; rRG3=[-17.2; 151.27; 5.26; 1]; rRG4=[-24.31; 0.34; -105.3; 1]; rRG5=[-143.77; -0.08; -0.45; 1]; rRG6=[-4.25; 0; -56.4; 1];
% Ma tran gia quan tinh Ixx14761283.51; Iyy15712413.46; Izz1c485332.7;
J2=[ (-Ixx2+Iyy2+Izz2)/2+m2*L2^2/4 0 0 m2*L2/2 ; 0 (Ixx2-Iyy2+Izz2)/2 0 0;
J4=[ (-Ixx4+Iyy4+Izz4)/2+m4*L4^2/4 0 0 m4*L4/2 ; 0 (Ixx4-Iyy4+Izz4)/2 0 0;
% Ma tran bien doi thuan nhat T1 = [cos(q1), -sin(q1)*cos(al1), sin(q1)*sin(al1), a1*cos(q1); sin(q1), cos(q1)*cos(al1), -cos(q1)*sin(al1), a1*sin(q1);
0, 0, 0, 1]; disp('Ma tr?n bi?n ð?i t?ng th? T01 là:'); disp(T1);
T2 = [cos(q2), -sin(q2)*cos(al2), sin(q2)*sin(al2), a2*cos(q2); sin(q2), cos(q2)*cos(al2), -cos(q1)*sin(al2), a2*sin(q2);
0, 0, 0, 1]; disp('Ma tr?n bi?n ð?i t?ng th? T12 là:'); disp(T2);
T3 = [cos(q3), -sin(q3)*cos(al3), sin(q1)*sin(al3), a3*cos(q3); sin(q3), cos(q3)*cos(al3), -cos(q1)*sin(al3), a3*sin(q3);
0, 0, 0, 1]; disp('Ma tr?n bi?n ð?i t?ng th? T23 là:'); disp(T3);
T4 = [cos(q4), -sin(q4)*cos(al4), sin(q4)*sin(al4), a4*cos(q4); sin(q4), cos(q4)*cos(al4), -cos(q4)*sin(al4), a4*sin(q4);
0, 0, 0, 1]; disp('Ma tr?n bi?n ð?i t?ng th? T34 là:'); disp(T4);
T5 = [cos(q5), -sin(q5)*cos(al5), sin(q5)*sin(al5), a5*cos(q5); sin(q5), cos(q5)*cos(al5), -cos(q5)*sin(al5), a5*sin(q5);
0, 0, 0, 1]; disp('Ma tr?n bi?n ð?i t?ng th? T45 là:'); disp(T5);
T6 = [cos(q6), -sin(q6)*cos(al6), sin(q6)*sin(al6), a6*cos(q6); sin(q6), cos(q6)*cos(al6), -cos(q6)*sin(al6), a6*sin(q6);
0, 0, 0, 1]; disp('Ma tr?n bi?n ð?i t?ng th? T56 là:'); disp(T6);
% Ma tr?n bi?n ð?i t?ng th? t? g?c ð?n kh?p cu?i T06 = simplify(T1 * T2 * T3 * T4 * T5 * T6);
% Hi?n th? k?t qu? disp('Ma tr?n bi?n ð?i t?ng th? T06 là:'); disp(T06);%here q = [q1; q2; q3; q4; q5; q6]; qd = [qd1; qd2; qd3; qd4; qd5; qd6]; qdd = [qdd1; qdd2; qdd3; qdd4; qdd5; qdd6];
% Tính ma tr?n kh?i lý?ng disp('ma tran khoi luong');
% Véc tõ các bi?n s? disp('vector J:');
%F1 total1=trace(diff(T1,q1)*J1*transpose(diff(T1,q1)))*qdd1 %1
+trace(diff(T2,q1)*J2*transpose(diff(T2,q1)))*qdd1 %2 +trace(diff(T2,q2)*J2*transpose(diff(T2,q1)))*qdd2 %3 +trace(diff(T3,q1)*J3*transpose(diff(T3,q1)))*qdd1 %4 +trace(diff(T3,q2)*J3*transpose(diff(T3,q1)))*qdd2 %5 +trace(diff(T3,q3)*J3*transpose(diff(T3,q1)))*qdd3 %6 +trace(diff(T4,q1)*J4*transpose(diff(T4,q1)))*qdd1 %7 +trace(diff(T4,q2)*J4*transpose(diff(T4,q1)))*qdd2 %8 +trace(diff(T4,q3)*J4*transpose(diff(T4,q1)))*qdd3 %9 +trace(diff(T4,q4)*J4*transpose(diff(T4,q1)))*qdd4 %10 +trace(diff(T5,q1)*J5*transpose(diff(T5,q1)))*qdd1 %11 +trace(diff(T5,q2)*J5*transpose(diff(T5,q1)))*qdd2 %12 +trace(diff(T5,q3)*J5*transpose(diff(T5,q1)))*qdd3 %13 +trace(diff(T5,q4)*J5*transpose(diff(T5,q1)))*qdd4 %14 +trace(diff(T5,q5)*J5*transpose(diff(T5,q1)))*qdd5 %15 +trace(diff(T6,q1)*J6*transpose(diff(T6,q1)))*qdd1 %16 +trace(diff(T6,q2)*J6*transpose(diff(T6,q1)))*qdd2 %17 +trace(diff(T6,q3)*J6*transpose(diff(T6,q1)))*qdd3 %18 +trace(diff(T6,q4)*J6*transpose(diff(T6,q1)))*qdd4 %19 +trace(diff(T6,q5)*J6*transpose(diff(T6,q1)))*qdd5 %20 +trace(diff(T6,q6)*J6*transpose(diff(T6,q1)))*qdd6;%21 total3=-m1*transpose(Vg)*diff(T1,q1)*rRG1 %1
-m2*transpose(Vg)*diff(T2,q1)*rRG2 %2 -m3*transpose(Vg)*diff(T3,q1)*rRG3 %3 -m4*transpose(Vg)*diff(T4,q1)*rRG4 %4 -m5*transpose(Vg)*diff(T5,q1)*rRG5 %5 -m6*transpose(Vg)*diff(T6,q1)*rRG6;%6 total2=trace(diff(diff(T1,q1),q1)*J1*transpose(diff(T1,q1)))*qd1*qd1 %1
+trace(diff(diff(T2,q1),q1)*J2*transpose(diff(T2,q1)))*qd1*qd1 %2+trace(diff(diff(T2,q2),q1)*J2*transpose(diff(T2,q1)))*qd1*qd2 %3+trace(diff(diff(T2,q1),q2)*J2*transpose(diff(T2,q1)))*qd2*qd1 %4
+trace(diff(diff(T2,q2),q2)*J2*transpose(diff(T2,q1)))*qd2*qd2 %5+trace(diff(diff(T3,q1),q1)*J3*transpose(diff(T3,q1)))*qd1*qd1 %6+trace(diff(diff(T3,q2),q1)*J3*transpose(diff(T3,q1)))*qd1*qd2 %7+trace(diff(diff(T3,q3),q1)*J3*transpose(diff(T3,q1)))*qd1*qd3 %8+trace(diff(diff(T3,q1),q2)*J3*transpose(diff(T3,q1)))*qd2*qd1 %9+trace(diff(diff(T3,q2),q2)*J3*transpose(diff(T3,q1)))*qd2*qd2 %10+trace(diff(diff(T3,q3),q2)*J3*transpose(diff(T3,q1)))*qd2*qd3 %11+trace(diff(diff(T3,q1),q3)*J3*transpose(diff(T3,q1)))*qd3*qd1 %12+trace(diff(diff(T3,q2),q3)*J3*transpose(diff(T3,q1)))*qd3*qd2 %13+trace(diff(diff(T3,q3),q3)*J3*transpose(diff(T3,q1)))*qd3*qd3 %14+trace(diff(diff(T4,q1),q1)*J4*transpose(diff(T4,q1)))*qd1*qd1 %15+trace(diff(diff(T4,q2),q1)*J4*transpose(diff(T4,q1)))*qd1*qd2 %16+trace(diff(diff(T4,q3),q1)*J4*transpose(diff(T4,q1)))*qd1*qd3 %17+trace(diff(diff(T4,q4),q1)*J4*transpose(diff(T4,q1)))*qd1*qd4 %18+trace(diff(diff(T4,q1),q2)*J4*transpose(diff(T4,q1)))*qd2*qd1 %19+trace(diff(diff(T4,q2),q2)*J4*transpose(diff(T4,q1)))*qd2*qd2 %20+trace(diff(diff(T4,q3),q2)*J4*transpose(diff(T4,q1)))*qd2*qd3 %21+trace(diff(diff(T4,q4),q2)*J4*transpose(diff(T4,q1)))*qd2*qd4 %22+trace(diff(diff(T4,q1),q3)*J4*transpose(diff(T4,q1)))*qd3*qd1 %23+trace(diff(diff(T4,q2),q3)*J4*transpose(diff(T4,q1)))*qd3*qd2 %24+trace(diff(diff(T4,q3),q3)*J4*transpose(diff(T4,q1)))*qd3*qd3 %25+trace(diff(diff(T4,q4),q3)*J4*transpose(diff(T4,q1)))*qd3*qd4 %26+trace(diff(diff(T4,q1),q4)*J4*transpose(diff(T4,q1)))*qd4*qd1 %27+trace(diff(diff(T4,q2),q4)*J4*transpose(diff(T4,q1)))*qd4*qd2 %28+trace(diff(diff(T4,q3),q4)*J4*transpose(diff(T4,q1)))*qd4*qd3 %29+trace(diff(diff(T4,q4),q4)*J4*transpose(diff(T4,q1)))*qd4*qd4 %30+trace(diff(diff(T5,q1),q1)*J5*transpose(diff(T5,q1)))*qd1*qd1 %31+trace(diff(diff(T5,q2),q1)*J5*transpose(diff(T5,q1)))*qd1*qd2 %32+trace(diff(diff(T5,q3),q1)*J5*transpose(diff(T5,q1)))*qd1*qd3 %33+trace(diff(diff(T5,q4),q1)*J5*transpose(diff(T5,q1)))*qd1*qd4 %34
+trace(diff(diff(T5,q5),q1)*J5*transpose(diff(T5,q1)))*qd1*qd5 %35+trace(diff(diff(T5,q1),q2)*J5*transpose(diff(T5,q1)))*qd2*qd1 %36+trace(diff(diff(T5,q2),q2)*J5*transpose(diff(T5,q1)))*qd2*qd2 %37+trace(diff(diff(T5,q3),q2)*J5*transpose(diff(T5,q1)))*qd2*qd3 %38+trace(diff(diff(T5,q4),q2)*J5*transpose(diff(T5,q1)))*qd2*qd4 %39+trace(diff(diff(T5,q5),q2)*J5*transpose(diff(T5,q1)))*qd2*qd5 %40+trace(diff(diff(T5,q1),q3)*J5*transpose(diff(T5,q1)))*qd3*qd1 %41+trace(diff(diff(T5,q2),q3)*J5*transpose(diff(T5,q1)))*qd3*qd2 %42+trace(diff(diff(T5,q3),q3)*J5*transpose(diff(T5,q1)))*qd3*qd3 %43+trace(diff(diff(T5,q4),q3)*J5*transpose(diff(T5,q1)))*qd3*qd4 %44+trace(diff(diff(T5,q5),q3)*J5*transpose(diff(T5,q1)))*qd3*qd5 %45+trace(diff(diff(T5,q1),q4)*J5*transpose(diff(T5,q1)))*qd4*qd1 %46+trace(diff(diff(T5,q2),q4)*J5*transpose(diff(T5,q1)))*qd4*qd2 %47+trace(diff(diff(T5,q3),q4)*J5*transpose(diff(T5,q1)))*qd4*qd3 %48+trace(diff(diff(T5,q4),q4)*J5*transpose(diff(T5,q1)))*qd4*qd4 %49+trace(diff(diff(T5,q5),q4)*J5*transpose(diff(T5,q1)))*qd4*qd5 %50+trace(diff(diff(T5,q1),q5)*J5*transpose(diff(T5,q1)))*qd5*qd1 %51+trace(diff(diff(T5,q2),q5)*J5*transpose(diff(T5,q1)))*qd5*qd2 %52+trace(diff(diff(T5,q3),q5)*J5*transpose(diff(T5,q1)))*qd5*qd3 %53+trace(diff(diff(T5,q4),q5)*J5*transpose(diff(T5,q1)))*qd5*qd4 %54+trace(diff(diff(T5,q5),q5)*J5*transpose(diff(T5,q1)))*qd5*qd5 %55+trace(diff(diff(T6,q1),q1)*J6*transpose(diff(T6,q1)))*qd1*qd1 %56+trace(diff(diff(T6,q2),q1)*J6*transpose(diff(T6,q1)))*qd1*qd2 %57+trace(diff(diff(T6,q3),q1)*J6*transpose(diff(T6,q1)))*qd1*qd3 %58+trace(diff(diff(T6,q4),q1)*J6*transpose(diff(T6,q1)))*qd1*qd4 %59+trace(diff(diff(T6,q5),q1)*J6*transpose(diff(T6,q1)))*qd1*qd5 %60+trace(diff(diff(T6,q6),q1)*J6*transpose(diff(T6,q1)))*qd1*qd6 %61+trace(diff(diff(T6,q1),q2)*J6*transpose(diff(T6,q1)))*qd2*qd1 %62+trace(diff(diff(T6,q2),q2)*J6*transpose(diff(T6,q1)))*qd2*qd2 %63+trace(diff(diff(T6,q3),q2)*J6*transpose(diff(T6,q1)))*qd2*qd3 %64
+trace(diff(diff(T6,q4),q2)*J6*transpose(diff(T6,q1)))*qd2*qd4 %65 +trace(diff(diff(T6,q5),q2)*J6*transpose(diff(T6,q1)))*qd2*qd5 %66 +trace(diff(diff(T6,q6),q2)*J6*transpose(diff(T6,q1)))*qd2*qd6 %67 +trace(diff(diff(T6,q1),q3)*J6*transpose(diff(T6,q1)))*qd3*qd1 %68 +trace(diff(diff(T6,q2),q3)*J6*transpose(diff(T6,q1)))*qd3*qd2 %69 +trace(diff(diff(T6,q3),q3)*J6*transpose(diff(T6,q1)))*qd3*qd3 %70 +trace(diff(diff(T6,q4),q3)*J6*transpose(diff(T6,q1)))*qd3*qd4 %71 +trace(diff(diff(T6,q5),q3)*J6*transpose(diff(T6,q1)))*qd3*qd5 %72 +trace(diff(diff(T6,q6),q3)*J6*transpose(diff(T6,q1)))*qd3*qd6 %73 +trace(diff(diff(T6,q1),q4)*J6*transpose(diff(T6,q1)))*qd4*qd1 %74 +trace(diff(diff(T6,q2),q4)*J6*transpose(diff(T6,q1)))*qd4*qd2 %75 +trace(diff(diff(T6,q3),q4)*J6*transpose(diff(T6,q1)))*qd4*qd3 %76 +trace(diff(diff(T6,q4),q4)*J6*transpose(diff(T6,q1)))*qd4*qd4 %77 +trace(diff(diff(T6,q5),q4)*J6*transpose(diff(T6,q1)))*qd4*qd5 %78 +trace(diff(diff(T6,q6),q4)*J6*transpose(diff(T6,q1)))*qd4*qd6 %79 +trace(diff(diff(T6,q1),q5)*J6*transpose(diff(T6,q1)))*qd5*qd1 %80 +trace(diff(diff(T6,q2),q5)*J6*transpose(diff(T6,q1)))*qd5*qd2 %81 +trace(diff(diff(T6,q3),q5)*J6*transpose(diff(T6,q1)))*qd5*qd3 %82 +trace(diff(diff(T6,q4),q5)*J6*transpose(diff(T6,q1)))*qd5*qd4 %83 +trace(diff(diff(T6,q5),q5)*J6*transpose(diff(T6,q1)))*qd5*qd5 %84 +trace(diff(diff(T6,q6),q5)*J6*transpose(diff(T6,q1)))*qd5*qd6 %85 +trace(diff(diff(T6,q1),q6)*J6*transpose(diff(T6,q1)))*qd6*qd1 %86 +trace(diff(diff(T6,q2),q6)*J6*transpose(diff(T6,q1)))*qd6*qd2 %87 +trace(diff(diff(T6,q3),q6)*J6*transpose(diff(T6,q1)))*qd6*qd3 %88 +trace(diff(diff(T6,q4),q6)*J6*transpose(diff(T6,q1)))*qd6*qd4 %89 +trace(diff(diff(T6,q5),q6)*J6*transpose(diff(T6,q1)))*qd6*qd5 %90 +trace(diff(diff(T6,q6),q6)*J6*transpose(diff(T6,q1)))*qd6*qd6;%91 F1=total1+total2+total3; fprintf('F1= '); disp(F1);
%F2 total4=trace(diff(T2,q1)*J2*transpose(diff(T2,q2)))*qdd1 %2
+trace(diff(T2,q2)*J2*transpose(diff(T2,q2)))*qdd2 %3 +trace(diff(T3,q1)*J3*transpose(diff(T3,q2)))*qdd1 %4 +trace(diff(T3,q2)*J3*transpose(diff(T3,q2)))*qdd2 %5 +trace(diff(T3,q3)*J3*transpose(diff(T3,q2)))*qdd3 %6 +trace(diff(T4,q1)*J4*transpose(diff(T4,q2)))*qdd1 %7 +trace(diff(T4,q2)*J4*transpose(diff(T4,q2)))*qdd2 %8 +trace(diff(T4,q3)*J4*transpose(diff(T4,q2)))*qdd3 %9 +trace(diff(T4,q4)*J4*transpose(diff(T4,q2)))*qdd4 %10 +trace(diff(T5,q1)*J5*transpose(diff(T5,q2)))*qdd1 %11 +trace(diff(T5,q2)*J5*transpose(diff(T5,q2)))*qdd2 %12 +trace(diff(T5,q3)*J5*transpose(diff(T5,q2)))*qdd3 %13 +trace(diff(T5,q4)*J5*transpose(diff(T5,q2)))*qdd4 %14 +trace(diff(T5,q5)*J5*transpose(diff(T5,q2)))*qdd5 %15 +trace(diff(T6,q1)*J6*transpose(diff(T6,q2)))*qdd1 %16 +trace(diff(T6,q2)*J6*transpose(diff(T6,q2)))*qdd2 %17 +trace(diff(T6,q3)*J6*transpose(diff(T6,q2)))*qdd3 %18 +trace(diff(T6,q4)*J6*transpose(diff(T6,q2)))*qdd4 %19 +trace(diff(T6,q5)*J6*transpose(diff(T6,q2)))*qdd5 %20 +trace(diff(T6,q6)*J6*transpose(diff(T6,q2)))*qdd6;%21 total6=-m1*transpose(Vg)*diff(T1,q2)*rRG1 %1
-m2*transpose(Vg)*diff(T2,q2)*rRG2 %2 -m3*transpose(Vg)*diff(T3,q2)*rRG3 %3 -m4*transpose(Vg)*diff(T4,q2)*rRG4 %4 -m5*transpose(Vg)*diff(T5,q2)*rRG5 %5 -m6*transpose(Vg)*diff(T6,q2)*rRG6;%6 total5=trace(diff(diff(T1,q1),q1)*J1*transpose(diff(T1,q2)))*qd1*qd1 %1
+trace(diff(diff(T2,q1),q1)*J2*transpose(diff(T2,q2)))*qd1*qd1 %2+trace(diff(diff(T2,q2),q1)*J2*transpose(diff(T2,q2)))*qd1*qd2 %3
+trace(diff(diff(T2,q1),q2)*J2*transpose(diff(T2,q2)))*qd2*qd1 %4+trace(diff(diff(T2,q2),q2)*J2*transpose(diff(T2,q2)))*qd2*qd2 %5+trace(diff(diff(T3,q1),q1)*J3*transpose(diff(T3,q2)))*qd1*qd1 %6+trace(diff(diff(T3,q2),q1)*J3*transpose(diff(T3,q2)))*qd1*qd2 %7+trace(diff(diff(T3,q3),q1)*J3*transpose(diff(T3,q2)))*qd1*qd3 %8+trace(diff(diff(T3,q1),q2)*J3*transpose(diff(T3,q2)))*qd2*qd1 %9+trace(diff(diff(T3,q2),q2)*J3*transpose(diff(T3,q2)))*qd2*qd2 %10+trace(diff(diff(T3,q3),q2)*J3*transpose(diff(T3,q2)))*qd2*qd3 %11+trace(diff(diff(T3,q1),q3)*J3*transpose(diff(T3,q2)))*qd3*qd1 %12+trace(diff(diff(T3,q2),q3)*J3*transpose(diff(T3,q2)))*qd3*qd2 %13+trace(diff(diff(T3,q3),q3)*J3*transpose(diff(T3,q2)))*qd3*qd3 %14+trace(diff(diff(T4,q1),q1)*J4*transpose(diff(T4,q2)))*qd1*qd1 %15+trace(diff(diff(T4,q2),q1)*J4*transpose(diff(T4,q2)))*qd1*qd2 %16+trace(diff(diff(T4,q3),q1)*J4*transpose(diff(T4,q2)))*qd1*qd3 %17+trace(diff(diff(T4,q4),q1)*J4*transpose(diff(T4,q2)))*qd1*qd4 %18+trace(diff(diff(T4,q1),q2)*J4*transpose(diff(T4,q2)))*qd2*qd1 %19+trace(diff(diff(T4,q2),q2)*J4*transpose(diff(T4,q2)))*qd2*qd2 %20+trace(diff(diff(T4,q3),q2)*J4*transpose(diff(T4,q2)))*qd2*qd3 %21+trace(diff(diff(T4,q4),q2)*J4*transpose(diff(T4,q2)))*qd2*qd4 %22+trace(diff(diff(T4,q1),q3)*J4*transpose(diff(T4,q2)))*qd3*qd1 %23+trace(diff(diff(T4,q2),q3)*J4*transpose(diff(T4,q2)))*qd3*qd2 %24+trace(diff(diff(T4,q3),q3)*J4*transpose(diff(T4,q2)))*qd3*qd3 %25+trace(diff(diff(T4,q4),q3)*J4*transpose(diff(T4,q2)))*qd3*qd4 %26+trace(diff(diff(T4,q1),q4)*J4*transpose(diff(T4,q2)))*qd4*qd1 %27+trace(diff(diff(T4,q2),q4)*J4*transpose(diff(T4,q2)))*qd4*qd2 %28+trace(diff(diff(T4,q3),q4)*J4*transpose(diff(T4,q2)))*qd4*qd3 %29+trace(diff(diff(T4,q4),q4)*J4*transpose(diff(T4,q2)))*qd4*qd4 %30+trace(diff(diff(T5,q1),q1)*J5*transpose(diff(T5,q2)))*qd1*qd1 %31+trace(diff(diff(T5,q2),q1)*J5*transpose(diff(T5,q2)))*qd1*qd2 %32+trace(diff(diff(T5,q3),q1)*J5*transpose(diff(T5,q2)))*qd1*qd3 %33
+trace(diff(diff(T5,q4),q1)*J5*transpose(diff(T5,q2)))*qd1*qd4 %34+trace(diff(diff(T5,q5),q1)*J5*transpose(diff(T5,q2)))*qd1*qd5 %35+trace(diff(diff(T5,q1),q2)*J5*transpose(diff(T5,q2)))*qd2*qd1 %36+trace(diff(diff(T5,q2),q2)*J5*transpose(diff(T5,q2)))*qd2*qd2 %37+trace(diff(diff(T5,q3),q2)*J5*transpose(diff(T5,q2)))*qd2*qd3 %38+trace(diff(diff(T5,q4),q2)*J5*transpose(diff(T5,q2)))*qd2*qd4 %39+trace(diff(diff(T5,q5),q2)*J5*transpose(diff(T5,q2)))*qd2*qd5 %40+trace(diff(diff(T5,q1),q3)*J5*transpose(diff(T5,q2)))*qd3*qd1 %41+trace(diff(diff(T5,q2),q3)*J5*transpose(diff(T5,q2)))*qd3*qd2 %42+trace(diff(diff(T5,q3),q3)*J5*transpose(diff(T5,q2)))*qd3*qd3 %43+trace(diff(diff(T5,q4),q3)*J5*transpose(diff(T5,q2)))*qd3*qd4 %44+trace(diff(diff(T5,q5),q3)*J5*transpose(diff(T5,q2)))*qd3*qd5 %45+trace(diff(diff(T5,q1),q4)*J5*transpose(diff(T5,q2)))*qd4*qd1 %46+trace(diff(diff(T5,q2),q4)*J5*transpose(diff(T5,q2)))*qd4*qd2 %47+trace(diff(diff(T5,q3),q4)*J5*transpose(diff(T5,q2)))*qd4*qd3 %48+trace(diff(diff(T5,q4),q4)*J5*transpose(diff(T5,q2)))*qd4*qd4 %49+trace(diff(diff(T5,q5),q4)*J5*transpose(diff(T5,q2)))*qd4*qd5 %50+trace(diff(diff(T5,q1),q5)*J5*transpose(diff(T5,q2)))*qd5*qd1 %51+trace(diff(diff(T5,q2),q5)*J5*transpose(diff(T5,q2)))*qd5*qd2 %52+trace(diff(diff(T5,q3),q5)*J5*transpose(diff(T5,q2)))*qd5*qd3 %53+trace(diff(diff(T5,q4),q5)*J5*transpose(diff(T5,q2)))*qd5*qd4 %54+trace(diff(diff(T5,q5),q5)*J5*transpose(diff(T5,q2)))*qd5*qd5 %55+trace(diff(diff(T6,q1),q1)*J6*transpose(diff(T6,q2)))*qd1*qd1 %56+trace(diff(diff(T6,q2),q1)*J6*transpose(diff(T6,q2)))*qd1*qd2 %57+trace(diff(diff(T6,q3),q1)*J6*transpose(diff(T6,q2)))*qd1*qd3 %58+trace(diff(diff(T6,q4),q1)*J6*transpose(diff(T6,q2)))*qd1*qd4 %59+trace(diff(diff(T6,q5),q1)*J6*transpose(diff(T6,q2)))*qd1*qd5 %60+trace(diff(diff(T6,q6),q1)*J6*transpose(diff(T6,q2)))*qd1*qd6 %61+trace(diff(diff(T6,q1),q2)*J6*transpose(diff(T6,q2)))*qd2*qd1 %62+trace(diff(diff(T6,q2),q2)*J6*transpose(diff(T6,q2)))*qd2*qd2 %63
+trace(diff(diff(T6,q3),q2)*J6*transpose(diff(T6,q2)))*qd2*qd3 %64 +trace(diff(diff(T6,q4),q2)*J6*transpose(diff(T6,q2)))*qd2*qd4 %65 +trace(diff(diff(T6,q5),q2)*J6*transpose(diff(T6,q2)))*qd2*qd5 %66 +trace(diff(diff(T6,q6),q2)*J6*transpose(diff(T6,q2)))*qd2*qd6 %67 +trace(diff(diff(T6,q1),q3)*J6*transpose(diff(T6,q2)))*qd3*qd1 %68 +trace(diff(diff(T6,q2),q3)*J6*transpose(diff(T6,q2)))*qd3*qd2 %69 +trace(diff(diff(T6,q3),q3)*J6*transpose(diff(T6,q2)))*qd3*qd3 %70 +trace(diff(diff(T6,q4),q3)*J6*transpose(diff(T6,q2)))*qd3*qd4 %71 +trace(diff(diff(T6,q5),q3)*J6*transpose(diff(T6,q2)))*qd3*qd5 %72 +trace(diff(diff(T6,q6),q3)*J6*transpose(diff(T6,q2)))*qd3*qd6 %73 +trace(diff(diff(T6,q1),q4)*J6*transpose(diff(T6,q2)))*qd4*qd1 %74 +trace(diff(diff(T6,q2),q4)*J6*transpose(diff(T6,q2)))*qd4*qd2 %75 +trace(diff(diff(T6,q3),q4)*J6*transpose(diff(T6,q2)))*qd4*qd3 %76 +trace(diff(diff(T6,q4),q4)*J6*transpose(diff(T6,q2)))*qd4*qd4 %77 +trace(diff(diff(T6,q5),q4)*J6*transpose(diff(T6,q2)))*qd4*qd5 %78 +trace(diff(diff(T6,q6),q4)*J6*transpose(diff(T6,q2)))*qd4*qd6 %79 +trace(diff(diff(T6,q1),q5)*J6*transpose(diff(T6,q2)))*qd5*qd1 %80 +trace(diff(diff(T6,q2),q5)*J6*transpose(diff(T6,q2)))*qd5*qd2 %81 +trace(diff(diff(T6,q3),q5)*J6*transpose(diff(T6,q2)))*qd5*qd3 %82 +trace(diff(diff(T6,q4),q5)*J6*transpose(diff(T6,q2)))*qd5*qd4 %83 +trace(diff(diff(T6,q5),q5)*J6*transpose(diff(T6,q2)))*qd5*qd5 %84 +trace(diff(diff(T6,q6),q5)*J6*transpose(diff(T6,q2)))*qd5*qd6 %85 +trace(diff(diff(T6,q1),q6)*J6*transpose(diff(T6,q2)))*qd6*qd1 %86 +trace(diff(diff(T6,q2),q6)*J6*transpose(diff(T6,q2)))*qd6*qd2 %87 +trace(diff(diff(T6,q3),q6)*J6*transpose(diff(T6,q2)))*qd6*qd3 %88 +trace(diff(diff(T6,q4),q6)*J6*transpose(diff(T6,q2)))*qd6*qd4 %89 +trace(diff(diff(T6,q5),q6)*J6*transpose(diff(T6,q2)))*qd6*qd5 %90 +trace(diff(diff(T6,q6),q6)*J6*transpose(diff(T6,q2)))*qd6*qd6;%91 F2=total4+total5+total6; fprintf('F2= '); disp(F2);
%F3 total7=trace(diff(T3,q1)*J3*transpose(diff(T3,q3)))*qdd1 %4
+trace(diff(T3,q2)*J3*transpose(diff(T3,q3)))*qdd2 %5 +trace(diff(T3,q3)*J3*transpose(diff(T3,q3)))*qdd3 %6 +trace(diff(T4,q1)*J4*transpose(diff(T4,q3)))*qdd1 %7 +trace(diff(T4,q2)*J4*transpose(diff(T4,q3)))*qdd2 %8 +trace(diff(T4,q3)*J4*transpose(diff(T4,q3)))*qdd3 %9 +trace(diff(T4,q4)*J4*transpose(diff(T4,q3)))*qdd4 %10 +trace(diff(T5,q1)*J5*transpose(diff(T5,q3)))*qdd1 %11 +trace(diff(T5,q2)*J5*transpose(diff(T5,q3)))*qdd2 %12 +trace(diff(T5,q3)*J5*transpose(diff(T5,q3)))*qdd3 %13 +trace(diff(T5,q4)*J5*transpose(diff(T5,q3)))*qdd4 %14 +trace(diff(T5,q5)*J5*transpose(diff(T5,q3)))*qdd5 %15 +trace(diff(T6,q1)*J6*transpose(diff(T6,q3)))*qdd1 %16 +trace(diff(T6,q2)*J6*transpose(diff(T6,q3)))*qdd2 %17 +trace(diff(T6,q3)*J6*transpose(diff(T6,q3)))*qdd3 %18 +trace(diff(T6,q4)*J6*transpose(diff(T6,q3)))*qdd4 %19 +trace(diff(T6,q5)*J6*transpose(diff(T6,q3)))*qdd5 %20 +trace(diff(T6,q6)*J6*transpose(diff(T6,q3)))*qdd6;%21 total9=m3*transpose(Vg)*diff(T3,q3)*rRG3 %3
-m4*transpose(Vg)*diff(T4,q3)*rRG4 %4 -m5*transpose(Vg)*diff(T5,q3)*rRG5 %5 -m6*transpose(Vg)*diff(T6,q3)*rRG6;%6 total8=trace(diff(diff(T3,q1),q1)*J3*transpose(diff(T3,q3)))*qd1*qd1 %6
+trace(diff(diff(T3,q2),q1)*J3*transpose(diff(T3,q3)))*qd1*qd2 %7+trace(diff(diff(T3,q3),q1)*J3*transpose(diff(T3,q3)))*qd1*qd3 %8+trace(diff(diff(T3,q1),q2)*J3*transpose(diff(T3,q3)))*qd2*qd1 %9+trace(diff(diff(T3,q2),q2)*J3*transpose(diff(T3,q3)))*qd2*qd2 %10+trace(diff(diff(T3,q3),q2)*J3*transpose(diff(T3,q3)))*qd2*qd3 %11
+trace(diff(diff(T3,q1),q3)*J3*transpose(diff(T3,q3)))*qd3*qd1 %12+trace(diff(diff(T3,q2),q3)*J3*transpose(diff(T3,q3)))*qd3*qd2 %13+trace(diff(diff(T3,q3),q3)*J3*transpose(diff(T3,q3)))*qd3*qd3 %14+trace(diff(diff(T4,q1),q1)*J4*transpose(diff(T4,q3)))*qd1*qd1 %15+trace(diff(diff(T4,q2),q1)*J4*transpose(diff(T4,q3)))*qd1*qd2 %16+trace(diff(diff(T4,q3),q1)*J4*transpose(diff(T4,q3)))*qd1*qd3 %17+trace(diff(diff(T4,q4),q1)*J4*transpose(diff(T4,q3)))*qd1*qd4 %18+trace(diff(diff(T4,q1),q2)*J4*transpose(diff(T4,q3)))*qd2*qd1 %19+trace(diff(diff(T4,q2),q2)*J4*transpose(diff(T4,q3)))*qd2*qd2 %20+trace(diff(diff(T4,q3),q2)*J4*transpose(diff(T4,q3)))*qd2*qd3 %21+trace(diff(diff(T4,q4),q2)*J4*transpose(diff(T4,q3)))*qd2*qd4 %22+trace(diff(diff(T4,q1),q3)*J4*transpose(diff(T4,q3)))*qd3*qd1 %23+trace(diff(diff(T4,q2),q3)*J4*transpose(diff(T4,q3)))*qd3*qd2 %24+trace(diff(diff(T4,q3),q3)*J4*transpose(diff(T4,q3)))*qd3*qd3 %25+trace(diff(diff(T4,q4),q3)*J4*transpose(diff(T4,q3)))*qd3*qd4 %26+trace(diff(diff(T4,q1),q4)*J4*transpose(diff(T4,q3)))*qd4*qd1 %27+trace(diff(diff(T4,q2),q4)*J4*transpose(diff(T4,q3)))*qd4*qd2 %28+trace(diff(diff(T4,q3),q4)*J4*transpose(diff(T4,q3)))*qd4*qd3 %29+trace(diff(diff(T4,q4),q4)*J4*transpose(diff(T4,q3)))*qd4*qd4 %30+trace(diff(diff(T5,q1),q1)*J5*transpose(diff(T5,q3)))*qd1*qd1 %31+trace(diff(diff(T5,q2),q1)*J5*transpose(diff(T5,q3)))*qd1*qd2 %32+trace(diff(diff(T5,q3),q1)*J5*transpose(diff(T5,q3)))*qd1*qd3 %33+trace(diff(diff(T5,q4),q1)*J5*transpose(diff(T5,q3)))*qd1*qd4 %34+trace(diff(diff(T5,q5),q1)*J5*transpose(diff(T5,q3)))*qd1*qd5 %35+trace(diff(diff(T5,q1),q2)*J5*transpose(diff(T5,q3)))*qd2*qd1 %36+trace(diff(diff(T5,q2),q2)*J5*transpose(diff(T5,q3)))*qd2*qd2 %37+trace(diff(diff(T5,q3),q2)*J5*transpose(diff(T5,q3)))*qd2*qd3 %38+trace(diff(diff(T5,q4),q2)*J5*transpose(diff(T5,q3)))*qd2*qd4 %39+trace(diff(diff(T5,q5),q2)*J5*transpose(diff(T5,q3)))*qd2*qd5 %40+trace(diff(diff(T5,q1),q3)*J5*transpose(diff(T5,q3)))*qd3*qd1 %41
Chương trình Matlab
Tính chọn động cơ cho các trục khớp Ta có: Khối lương các khâu
Chọn bộ truyền bánh răng song song Chọn hộp giảm tốc cho tất cả các khớp đều có hệ số giảm tốc la 10:1 Hiệu suất của bộ dẫn động η = ηbr η ol 2 ηkn
Trong đó: η br = 0.95; η ol 2 = 0.99; η kn = 1
3.2 Tính chọn động cơ cho khớp 6:
Chọn số vòng quay lớn nhất của khớp 3 la � 3 = 166 �ℎ/�ℎ Công suất tính toán trên trục khớp:
9,55.10 3 = 0,0596 �� = 59,6 � Công suất cần thiết trên khớp 3:
� 59,60,93 = 64,1 (�)Số vòng quay trên trục động cơ: � ��6 = 166.10 = 1660 ��/�ℎChọn động cơ thỏa mãn điều kiện
CHỌN ĐỘNG CƠ
Tính chọn động cơ
Tính chọn động cơ cho các trục khớp Ta có: Khối lương các khâu
Chọn bộ truyền bánh răng song song Chọn hộp giảm tốc cho tất cả các khớp đều có hệ số giảm tốc la 10:1 Hiệu suất của bộ dẫn động η = ηbr η ol 2 ηkn
Trong đó: η br = 0.95; η ol 2 = 0.99; η kn = 1
Tính chọn động cơ cho khớp 6
Chọn số vòng quay lớn nhất của khớp 3 la � 3 = 166 �ℎ/�ℎ Công suất tính toán trên trục khớp:
9,55.10 3 = 0,0596 �� = 59,6 � Công suất cần thiết trên khớp 3:
� 59,60,93 = 64,1 (�)Số vòng quay trên trục động cơ: � ��6 = 166.10 = 1660 ��/�ℎChọn động cơ thỏa mãn điều kiện
=≫Ta chọn động cơ của hãng YASKAWASGM7J-01A7A61 (100W- 3000RPM)
Các thông số cơ bản cho việc tính toán:
Motor moment of inertia - Mômen quán tính động cơ: J = 0,0739.10 -4 kg.m 2
Torque constant - Hằng số momen (Nm/A): K m = 0,413
Hệ số sức phản điện động(Vs/rad): K b =V n = 200 1660= 0,12 Rated current - Dòng điện định mức(A): Idm= 0,85 Coil Resistance - Điện trở phần ứng(Ω): R = 104,8 ± 10%
Viscous Friction Coefficients(Nm/rad/s): Bm = 0.152
Tính chọn động cơ cho khớp 5
Chọn số vòng quay lớn nhất của khớp 5 la �5 = 91 �ℎ/�ℎ Công suất tính toán trên trục khớp:
9,55.10 3 = 0,175 �� = 175 � Công suất cần thiết trên khớp 3:
� 175 0,93 = 188,17 (�) Số vòng quay trên trục động cơ: � ��5 = 91.10 = 910 ��/�ℎ Chọn động cơ thỏa mãn điều kiện
=≫Ta chọn động cơ của hãng YASKAWASGM7J-02A7A (200W- 3000RPM)
Các thông số cơ bản cho việc tính toán:
Motor moment of inertia - Mômen quán tính động cơ: J = 0,333.10 -4 kg.m 2
Torque constant - Hằng số momen (Nm/A): Km = 0,444
Hệ số sức phản điện động(Vs/rad): K b =V n 0 910 = 0,22 Rated current - Dòng điện định mức(A): I dm = 1,6 Coil Resistance - Điện trở phần ứng(Ω): R = 96 ± 10%
Viscous Friction Coefficients(Nm/rad/s): B m = 0.152
Tính chọn động cơ cho khớp 4
Chọn số vòng quay lớn nhất của khớp 4 la � 4 = 91 �ℎ/�ℎ Công suất tính toán trên trục khớp:
9,55.10 3 = 0,616 �� = 616 � Công suất cần thiết trên khớp 3:
� 616 0,93 = 662,36 (�) Số vòng quay trên trục động cơ: ���3 = 91.10 = 910 ��/�ℎ Chọn động cơ thỏa mãn điều kiện
=≫Ta chọn động cơ của hãng YASKAWA
Các thông số cơ bản cho việc tính toán:
Motor moment of inertia - Mômen quán tính động cơ: J = 1,77.10 -4 kg.m 2
Torque constant - Hằng số momen (Nm/A): Km = 0,584
Hệ số sức phản điện động(Vs/rad): K b =V n 200 910 = 0,22 Rated current - Dòng điện định mức(A): I dm = 4,4
Coil Resistance - Điện trở phần ứng(Ω): R = 88,6 ± 10%
Viscous Friction Coefficients(Nm/rad/s): Bm = 0.152
Tính chọn động cơ cho khớp 3
Chọn số vòng quay lớn nhất của khớp 3 la n3 = 86 vg/ph Công suất tính toán trên trục khớp:
9,55.10 3 = 1,95 (KW) = 1950 (W) Công suất cần thiết trên khớp 3:
� 50 0,93 = 2096,7 (�) Số vòng quay trên trục động cơ: ���3 = 86.10 = 860 ��/�ℎ Chọn động cơ thỏa mãn điều kiện
=≫Ta chọn động cơ của hãng YASKAWA SGM7G-30A7C (2.5kW -1500RPM)
Các thông số cơ bản cho việc tính toán:
Motor moment of inertia - Mômen quán tính động cơ: J = 46.10 -4 kg.m 2
Torque constant - Hằng số momen (Nm/A): Km = 0,848
Hệ số sức phản điện động(Vs/rad): Kb =V n 200 860= 0,232 Rated current - Dòng điện định mức(A): Idm = 19,6 Coil Resistance - Điện trở phần ứng(Ω): R = 56
Hệ số cản(Nms/rad): B m = 0.252
Tính chọn động cơ cho khớp 2
Chọn số vòng quay lớn nhất của khớp 2 la n 2 = 63 vg/phCông suất tính toán trên trục khớp:
9,55.10 3 = 1,933 (KW) = 1933 (W) Công suất cần thiết trên khớp 2:
� 33 0,93 = 2078,5 (�) Số vòng quay trên trục động cơ: ���2 = 63.10 = 630 ��/�ℎ Chọn động cơ thỏa mãn điều kiện
=≫Ta chọn động cơ của hãng YASKAWA SGM7G-30A7C (2.5kW - 1500RPM)
Các thông số cơ bản cho việc tính toán:
Motor moment of inertia - Mômen quán tính động cơ: J = 46.10 -4 kg.m 2
Torque constant - Hằng số momen (Nm/A): Km = 0,848
Hệ số sức phản điện động(Vs/rad): Kb =V n 200 860= 0,232 Rated current - Dòng điện định mức(A): Idm = 19,6 Coil Resistance - Điện trở phần ứng(Ω): R = 56
Hệ số cản(Nms/rad): Bm = 0.252
Tính chọn động cơ cho khớp 1
Chọn số vòng quay lớn nhất của khớp 1 la n 1 = 75 vg/ph Công suất tính toán trên trục khớp:
9,55.10 3 = 3,817 (��) = 3817 (�) Công suất cần thiết trên khớp 1:
0,93 = 4104,3 (�) Số vòng quay trên trục động cơ: n sb1 = 75.10 = 750 vg/ph lChọn động cơ thỏa mãn điều kiện
=≫Ta chọn động cơ của hãng YASKAWASGM7G-44A7C (4,4kW – 1500RPM)
Các thông số cơ bản cho việc tính toán:
Motor moment of inertia - Mômen quán tính động cơ: J = 75,4.10 -4 kg.m 2
Torque constant - Hằng số momen (Nm/A): Km = 0,934
Hệ số sức phản điện động(Vs/rad): Kb =V n 200 750 = 0,26 Rated current - Dòng điện định mức(A): I dm = 32,8 Coil Resistance - Điện trở phần ứng(Ω): R = 59
Hệ số cản(Nms/rad): B m = 0,302
THIẾT KẾ BỘ ĐIỀU KHIỂN
Sử dụng Matlab Simulink xây dựng mô hình điều khiển
4.2 Sư dụng Matlab Simulink xây dựng mô hinh điều khiển:
Thiết kế bộ điều khiển PI,PD và PID cho hệ thống
-Trong đó tín hiệu đầu vao U(s) đươc xác định bởi:
U(s) = Kp(� � � − �(�)) - KDS�(s) (4.1) -Trong đó: KP la hệ số tỉ lệ va KD la hệ số vi phân.
Hình:Sơ đồ khối của hệ thống điều khiên PD mạch kín cho một khớp của Robot
- Sử dụng phép biến đổi Laplace cho hai vế của phương trình (2.1) va kết hơp với phương trình (4.1) ta có phương trình biêu diễn hệ thống mạch kín như sau:
- Hệ thống điều khiên mạch kín nay phải ổn định với tất cả các giá trị KP, KD dương va các nhiễu đươc giới hạn Từ đó ta xác định đươc ham sai số như sau:
= � ��� � 2 + � Ω(�) ��� + �� � � � � � � + Ω � 1 �(�) (3.4) - Sai số trong giai đoạn xác lập đươc xác định theo công thức sau ess = ��(�) = �� � (4.3) �
- Ta có thê xác định các thông số của bộ điều khiên như sau:
��� = S2 + 2wζS + w2 (4.4) Cân bằng các hệ số của phương trình (4.4), ta có:
- Khi thêm thanh phần tích phân vao bộ điều khiên PD cúng ta thu đươc bộ điều khiên PID Từ đó, ta có ham truyền của bộ điều khiên PID như sau:
Hình 4.8: Sơ đồ khối của hệ thống điều khiên PID mạch kín cho một khớp của Robot - Hệ thống điều khiên kín với bộ điều khiên PID la hệ thống bậc 3
2 � �(�) (4.7) Trong đó Ω 2 � la đa thức đặc trưng:
Ω2 � = JeffS2 + (Beff + KKD)S2 + KKPS + KKI (4.8)
- Sử dụng tiêu chuẩn Routh-Hurwitz cho đa thức đặc trưng (4.8) ta xác định đươc hệ thống sẽ ổn định khi các hệ số đều dương va thỏa mãn điều kiện:
Tín hiệu vao của góc
4.3-1.Thiết kế bộ điều khiển PI
Sơ đồ simscape hệ thống điều khiên PID của từng khớp
Kết quả của các khâu
+Phản ứng của hệ thống rất ổn định, không có quá độ đáng kê hoặc dao động.
+Thời gian đáp ứng tương đối nhanh, hệ thống nhanh chóng hội tụ về tín hiệu tham chiếu mới.
+Sai số trạng thái ổn định tiệm cận về 0, cho thấy bộ điều khiên PI có khả năng kiêm soát tốt hệ thống.
+Các tham số của bộ điều khiên PI (hệ số tỉ lệ va hằng số tích phân) dường như đươc lưa chọn va điều chỉnh thích hơp. Điều nay giúp đạt đươc đáp ứng nhanh, ổn định va chính xác so với tín hiệu tham chiếu.
4.3-2.Thiết kế bộ điều khiển PID
Sơ đồ simscape hệ thống điều khiên PID của từng khớp
Tín hiệu vao của các khâu khi có PID
4.3-3.Thiết kế bộ điều khiển PD
Sơ đồ simscape hệ thống điều khiên PID của từng khớp
Tín hiệu vao của các khâu khi có PD
+ Đáp ứng của hệ thống rất ổn định, không có hiện tương quá độ đáng kê hay dao động.
+ Thời gian đáp ứng nhanh, hệ thống nhanh chóng hội tụ về tín hiệu tham chiếu mới.
+ Sai số trạng thái ổn định gần như bằng 0, cho thấy bộ điều khiên PID có khả năng kiêm soát tốt hệ thống.
4.4 Thiết kế bộ điều khiển gán điểm cực sư dụng phương trinh trạng thái
Ta có được các phương trình như sau: Đối với khâu 1:
= B m6 θ m6 − K 6 u 6 + d 6 Ta xác định được ma trận:
− d 1 d 2 d 3 d 4 d 5 d 6 Trong đó ta xác định được các thông số sau:
Jm 1 = 75,4.10 −4 ;Jm 2 = 46 10 −4 ;Jm 3 = 2,35 10 −4 ; Jm 4 = 0,97 10 −4 ;Jm 5 = 0,025 10 −4 ;Jm 6 = 0,025 10 −4 ; J11 c,56; J12 = 0; J13 = 0; J14 = 0; J15 = 0; J16 = 0;
Thay số vào phương trình trên ta được:
Thiết kế bộ điều khiển gán điểm cực sử dụng phương trình trạng thái 82
0 0 0 0 0 0 1,554 d1 7,987 d2 1,321 d3 7,267 d4 8,8967 �5 8,1566 d 6 Xây dựng code trên Matlab ta thu được kết quả như sau:
4.4.1 Chương trình con functionxd_nonlinear=mimo_nonlinear1(t,x_nonlinear,K)
%bo dieu khien gan diem cuc u1=-K1*x_nonlinear; u2=-K2*x_nonlinear; u3=-K3*x_nonlinear; u4=-K4*x_nonlinear; u5=-K5*x_nonlinear; u6=-K6*x_nonlinear;
%cac bien trang thai x1=x_nonlinear(1); x2=x_nonlinear(2); x3=x_nonlinear(3); x4=x_nonlinear(4); x5=x_nonlinear(5); x6=x_nonlinear(6); x7=x_nonlinear(7); x8=x_nonlinear(8); x9=x_nonlinear(9); x10=x_nonlinear(10); x11=x_nonlinear(11); x12=x_nonlinear(12);
%phuong trinh xd1=x7; xd2=x8; xd3=x9; xd4=x10; xd5=x11; xd6=x12; xd7=0.469*x7+0.024*u1; xd8=2.01*x8+0.121*u2; xd9=0.33*x9+0.02125*u3; xd10=1.104*x10+0.0478*u4; xd11=1.352*x11+0.0411*u5; xd12=1.239*x12+0.032*u6; xd_nonlinear=[xd1;xd2;xd3;xd4;xd5;xd6;xd7;xd8;xd9;xd10;xd11;x12]; end
4.4.2 Chương trình chính điểm cực clc; clear; symsd1 d2 d3 d4 d5 d6; miu=[0;0;0;0;0;0;0.5;0.5;0.5;0.5;0.5;0.5]; x0=[0;0;0;0;0;0;0.05;0.05;0.05;0.05;0.05;0.05];
% xac dinh cac bien trang thai cua he kin tuyen tinh xt=zeros(length(times),12);%bien xt chua gia tri cua bien trang thai theo thoi gian
% Tính giá tri cua bien trang thai tai tung thoi diem fori=1:length(times) xt(i,:)=(expm((A-B*K)*times(i))*x0)';
% xac dinh cac bien trang thai cua he kin phi tuyen
[T,xt_non]=ode45(@(t,x_nonlinear)mimo_nonlinear1(t,x_nonlinear,K),times,x 0);
%do thi ket qua mo phong so sanh gua he phi tuyen va tuyen tinh figure(1); subplot(2,2,1); plot(times,xt(:,1),'-',times,xt_non(:,1),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,1),times,0) subplot(2,2,2); plot(times,xt(:,7),'-',times,xt_non(:,7),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(2); subplot(2,2,1); plot(times,xt(:,2),'-',times,xt_non(:,2),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,2),times,0) subplot(2,2,2); plot(times,xt(:,8),'-',times,xt_non(:,8),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(3); subplot(2,2,1); plot(times,xt(:,3),'-',times,xt_non(:,3),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,3),times,0) subplot(2,2,2); plot(times,xt(:,9),'-',times,xt_non(:,9),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(4); subplot(2,2,1); plot(times,xt(:,4),'-',times,xt_non(:,4),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,4),times,0) subplot(2,2,2); plot(times,xt(:,10),'-',times,xt_non(:,10),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(5); subplot(2,2,1); plot(times,xt(:,5),'-',times,xt_non(:,5),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,5),times,0) subplot(2,2,2); plot(times,xt(:,11),'-',times,xt_non(:,11),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(6); subplot(2,2,1); plot(times,xt(:,6),'-',times,xt_non(:,6),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,6),times,0) subplot(2,2,2); plot(times,xt(:,12),'-',times,xt_non(:,12),' ','LineWidth',2); gridon; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); end
Nhận xét:Bộ điều khiển cho tín hiệu điều khiển ở khâu 1, khâu 3, khâu 4, khâu 5, khâu 6 ổn định bám sát theo đường phi tuyến- Bộ điều khiển ở khâu 2 chưa bám sát đường phi tuyến
TỔNG KẾT VÀ KẾT LUẬN
Những kết quả có được
- Thông qua phần mềm MATLAB đã có thể xây dựng được các chương trình tính toán động học, động lực học và điều khiển Robot chuyển động dựa trên các phép toán.
-Tìm được mối liên hệ giữa các biến khớp và vị trí toạ độ điểm cuối từ đó xây dựng được quỹ đạo chuyển động thích hợp.
-Xây dựng được bài toán điều khiển và mô phỏng quỹ đạo chuyển động khâu thao tác đáp ứng tín hiệu đặt mong muốn với công cụ Simulink-SimMechanics tích hợp trên Matlab.
Định hướng phát triển trong tương lai
- Các kết quả có được sẽ là bước đệm cho tư duy về thiết kế Robot, do vậy ý tưởng của nhóm trong tương lai sẽ cho ra sản phẩm Robot hàn hoàn thiện thực tế và ngoài ra có thể ứng dụng nó vào công việc hội họa với mong muốn cho raRobot viết chữ hay vẽ tranh…
Mô hinh chạy mô phỏng
