Trong suốt quá trình học tập vàthực hiện bài làm em luôn được sự quan tâm, hướng dẫn và giúp đỡ tận tình của thầy đãtrang bị kiến thức, kỹ năng cần thiết để thực hiện bài báo cáo đầu tiê
Tính động học thuận
- Cho trước robot và các thông số của nó:
Hình 1.1 Robot 3 bậc tự do
Bảng 1: Bảng thông số đề bài
7 0.2 0.1 0.2 0.15 0.15 Đặt hệ trục tọa độ
Hình 1 1 Các hệ trục tọa độ
Bảng 3: Bảng D-H đã thế các thông số đề bài a i−1 α i−1 d i Ɵ i
Ma trận chuyển đội từ hệ 0 sang hệ 1 :
T01=[ cos(Ɵ sin (Ɵ 0 0 1) 1) −sin cos (Ɵ 0 0 (Ɵ 1) 1) 0 0 1 0 L L 0 1 1 0 ] = [ cos sin(Ɵ (Ɵ 0 0 1) 1) −sin cos (Ɵ1 0 0 (Ɵ 1) ) 0 0.1 0 1 0.2 0 0 1 ] (1.1)
Ma trận chuyển đội từ hệ 1 sang hệ 2 :
T12=[ cos(Ɵ sin(Ɵ 0 0 2) 2) −sin cos (Ɵ1) 0 0 (Ɵ 1) 0 0 1 0 L 0 0 1 2 ] = [ cos(Ɵ sin(Ɵ 0 0 1) 1) −sin cos(Ɵ 0 0 (Ɵ 1 1) ) 0 0.2 0 1 0 0 0 1 ] (1.2)
Ma trận chuyển đội từ hệ 2 sang hệ 3 :
T23=[ cos(Ɵ sin (Ɵ 0 0 3) 3) −sin cos(Ɵ 0 0 (Ɵ 3 3) ) 0 0 1 0 L3 0 0 1 ] = [ cos sin (Ɵ (Ɵ 0 0 1) 1) −sin cos(Ɵ 0 0 (Ɵ 1) 1) 0 0.15 0 1 0 0 0 1 ] (1.3)
Ma trận chuyển đội từ hệ 3 sang hệ 4 :
Ma trận chuyển đội từ hệ 0 sang hệ 4 :
T04=[ cos sin ( ( 0 0 θ θ 123 123 ) ) − cos sin 0 0 ( ( θ θ 123 123 ) ) 0 0 1 0 L1 + L L 2∗sin 3∗cos ( θ ( 1 θ ) + 12 L3∗sin ) + L 2∗cos L 1 0 ( θ 12 ( Ɵ ) + 1 L ) + 4∗sin L 4∗cos ( θ 123 ( θ ) 123 ) ]
T04=[ cos sin ( ( 0 0 θ θ 123 123 ) ) − cos sin 0 0 ( ( θ θ 123 123 ) ) 0 0.1 0 1 0 0.15∗sin +0.15 ∗cos ( θ 12 ( θ ) 12 + ) 0.2 +0.2∗cos ∗sin 0.2 1 ( Ɵ 1 ( Ɵ ) +0.15∗sin 1 ) + 0.15∗cos ( θ 123 ( θ ) 123 ) ]
Tính động học nghịch
T04=[ cos sin ( ( 0 0 θ θ 123 123 ) ) − cos sin 0 0 ( ( θ θ 123 123 ) ) 0 0.1 0 1 0 0.15∗cos +0.15 ∗cos ( θ 12 ( θ ) + 12 0.2∗cos ) +0.2∗cos 0.2 1 ( Ɵ ( 1 Ɵ ) + 1 0.15 ) + 0.15∗cos ∗cos ( θ 123 ( θ ) 123 ) ]
T04=[ cos sin ( ( 0 0 θ θ 123 123 ) ) − cos sin 0 0 ( ( θ θ 123 123 ) ) 0 0 1 0 L1 + L L 2∗sin 3∗cos ( θ ( 1 θ ) + 12 L3∗sin ) + L 2∗cos L 1 0 ( θ 12 ( Ɵ ) + 1 L ) + 4∗sin L 4∗cos ( θ 123 ( θ ) 123 ) ]
Từ động học thuận ta có :
Kiểm chứng
Kiểm chứng động học thuận
- Tạo khối Function và nhập công thức tống quát chuyển đổi từ hệ i-1 sang i: function T=TransMatrix(anpha,a,d,theta)
T=[cos(theta),-sin(theta),0,a; sin(theta)*cos(anpha), cos(theta)*cos(anpha),-sin(anpha),-sin(anpha)*d; sin(theta)*sin(anpha), cos(theta)*sin(anpha),cos(anpha),cos(anpha)*d;
- Khai báo các thông số và tính toán các ma trận chuyển đổi:
+ Công thức tổng quát để biến đổi ma trận : syms theta1 theta2 theta3 L0 L1 L2 L3 L4
%% The transformation matrix between frame
+ Kiểm chứng bằng hình học
Hình 3 1 Vị trí của End-effector
+ Kiểm chứng bằng hình học
Hình 3.2 Vị trí của End-effector nhìn từ ngoài vào trong l 0 =0.2 l 1 +l 2 +l 3 +l 4 =¿ 0.6
Hình 3.3 Vị trí của End-effector nhìn từ trên xuống
+ Kiểm chứng bằng hình học
Hình 3.4 Vị trí của End-effector nhìn từ ngoài vào trong l 2 +l 3 +l 4 =¿ 0.5 l 2 +l 1 =¿ 0.3
Hình 3.5 Vị trí của End-effector nhìn từ trên xuống
+ Kiểm chứng bằng hình học
Hình 3.6 Vị trí của End-effector nhìn từ ngoài vào trong l 3 +l 4 =¿ 0.3 l 2 +l 1 =¿ 0.3 l 1 +l 2 +l 3 =¿ 0.45
Hình 3.7 Vị trí của End-effector nhìn từ trên xuống
Kiểm chứng động học nghịch
+ Công thức tổng quát để biến đổi ma trận : function [nghiem1, nghiem2] = DHN(P)
Ny=Py-L4*sind(theta); c_theta2=(Nx^2+Ny^2-L2^2-L3^2)/(2*L2*L3); s1_theta2=sqrt(abs(1-c_theta2^2)); s2_theta2=-sqrt(abs(1-c_theta2^2)); theta2_1=atan2d(s1_theta2,c_theta2); theta2_2=atan2d(s2_theta2,c_theta2);
%% Tính theta1 c1_theta1=(Nx*(L2+L3*cosd(theta2_1))+Ny*L3*sind(theta2_1))/
(L2^2+2*L2*L3*cosd(theta2_1)+L3^2); s11_theta1=sqrt(abs(1-c1_theta1^2)); s12_theta1=-sqrt(abs(1-c1_theta1^2)); c2_theta1=(Nx*(L2+L3*cosd(theta2_2))+Ny*L3*sind(theta2_2))/
(L2^2+2*L2*L3*cosd(theta2_2)+L3^2); s21_theta1=sqrt(abs(1-c2_theta1^2)); s22_theta1=-sqrt(abs(1-c2_theta1^2)); theta1_1=atan2d(s11_theta1,c1_theta1); theta1_2=atan2d(s12_theta1,c1_theta1); theta1_3=atan2d(s21_theta1,c2_theta1); theta1_4=atan2d(s22_theta1,c2_theta1);
%% Tính theta3 theta3_1=theta-theta1_1-theta2_1; theta3_2=theta-theta1_2-theta2_1; theta3_3=theta-theta1_3-theta2_2; theta3_4=theta-theta1_4-theta2_2;
[theta1_4 theta2_2 theta3_4]; nghiem1=[theta1_2 theta2_1 theta3_2]; nghiem2=[theta1_3 theta2_2 theta3_3]; end
- Kiểm nghiệm bằng cách tìm các góc theta từ động học nghịch rồi dùng các góc làm đầu vào của động học tính ra được vị trí ban đầu.
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P= [ 0.15 0.8 0 ]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P= [ 0.1 0.5 0.2 ]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P= [ 0.3 0.3 0.2 ]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P= [ 0.45 0.15 0.2 ]
CHƯƠNG 4: VẼ KHÔNG GIAN LÀM VIỆC CHUNG CỦA TỪNG BỘ NGHIỆM
- Để vẽ không gian làm việc của robot 3 bậc, ta sử dụng đoạn code sau: clc; clear all; close all;
L0 = 200; L1 = 100; L2 = 200; L3 = 150; L4 = 150; theta_range = -90:5:90; % Phạm vi góc theta1, theta2, theta3 tt = 0; emtry = []; emtry1 = []; emtry2 = []; for theta1 = theta_range for theta2 = theta_range for theta3 = theta_range if (theta1 + theta2 + theta3) == 0 %% Điều kiện của khâu cuối tt = tt + 1; x = L1 + cosd(theta1 + theta2 + theta3) * L4 + L3 * cosd(theta1 + theta2)
+ L2 * cosd(theta1); % động học thuận 1, y = sind(theta1 + theta2 + theta3) * L4 + L3 * sind(theta1 + theta2) + L2 * sind(theta1); % động học thuận 2, emtry(:, tt) = [x; y];
P1 = DHT(theta1, theta2, theta3); try
%% Bộ nghiệm 1 if all(abs([n1(1) - theta1, n1(2) - theta2]) < 1e-6) x1 = L1 + cosd(theta1 + theta2 + theta3)*L4+ L3*cosd(theta1 + theta2) + L2*cosd(theta1); % động học thuận 1, y1 = sind(theta1 + theta2 + theta3)*L4 + L3*sind(theta1 + theta2) +
L2*sind(theta1); % động học thuận 2, emtry1(:, tt) = [x1; y1]; end
%% Bộ nghiệm 2 if all(abs([n2(1) - theta1, n2(2) - theta2]) < 1e-6) x2 = L1 + cosd(theta1 + theta2 + theta3)*L4+ L3*cosd(theta1 + theta2) + L2*cosd(theta1); % động học thuận 1, y2 = sind(theta1 + theta2 + theta3)*L4 + L3*sind(theta1 + theta2) +
L2*sind(theta1); % động học thuận 2, emtry2(:, tt) = [x2; y2]; end catch % Xử lý trường hợp không tìm thấy nghiệm hoặc có lỗi continue; % Bỏ qua vòng lặp và tiếp tục với giá trị theta tiếp theo end end end end end figure(1); plot(emtry(1,:), emtry(2,:), '.b', 'MarkerSize', 10); figure(2); plot(emtry1(1,:), emtry1(2,:), '.g', 'MarkerSize', 10); figure(3); plot(emtry2(1,:), emtry2(2,:), '.r', 'MarkerSize', 10);
- Không gian làm việc chung
Hình 4.1: Không gian làm việc chung của robot
- Không gian làm việc của bộ nghiệm 1
Hình 4.2: Không gian làm việc của bộ nghiệm thứ 1
- Không gian làm việc của bộ nghiệm 2
Hình 4.3: Không gian làm việc của bộ nghiệm thứ 2
CHƯƠNG 5: PHƯƠNG PHÁP QUY HOẠCH QUỸ ĐẠO ĐI QUA 2 VÀ 3 ĐIỂM 5.1 Quy hoạch quỹ đạo đi qua 2 điểm:
Lập quỹ đạo chuyển động cho robot giữa 2 điểm A và B Với tọa độ và thời gian đi qua các điểm A và B là như sau: A ( 0.6,0) và B (0.4,-0.27) với tB = 2s Vận tốc khi di chuyển qua A và B lần lượt là vA =0 và vB = 0.
- Sử dụng Simulink, tạo 1 khối Matlab Function:
Hình 5.1: Thành lập khối Matlab Function
The MATLAB program TrajectoryPlanning defines a cubic trajectory with boundary conditions P0, v0, Pf, vf, and tf It calculates coefficients a10, a20, a11, a21, a12, a22, a13, and a23 Depending on the value of t, it returns the position [x,y] within the time interval [0, 2] according to the computed coefficients Outside this interval, it returns [-20, -20].
Hình 5.2: Quỹ đạo của giữa điểm A và B với trục x
Hình 5.3: Quỹ đạo của giữa điểm A và B với trục y
5.2 Quy hoạch quỹ đạo đi qua 3 điểm:
Lập quỹ đạo chuyển động cho robot giữa 3 điểm A, B và C Với tọa độ và thời gian đi qua các điểm A, B và C là như sau: A(0.6;0) với tA = 0s, B(0.4;0.-27) với tB = 2s, C(0.4;0.27) với tC = 4s Vận tốc khi di chuyển qua A, B và C lần lượt là vA = 0, vB = 0 và vC = 0.
- Sử dụng Simulink, tạo 1 khối Matlab Function:
Hình 5.4: Thành lập khối Matlab Function
+ Đoạn code tính toán các giá trị function [x,y] = TrajectoryPlanning(t,PA,vA,PB,vB,PC,vC,tf) tf1=4;tf2=6; aA10=PA(1); aA20=PA(2); aA11=vA(1); aA21=vA(2); aA12=3/tf^2*(PB(1)-PA(1))-2/tf*vA(1)-1/tf*vB(1); aA22=3/tf^2*(PB(2)-PA(2))-2/tf*vA(2)-1/tf*vB(2); aA13=-2/tf^3*(PB(1)-PA(1))+1/tf^2*(vB(1)+vA(1)); aA23=-2/tf^3*(PB(2)-PA(2))+1/tf^2*(vB(2)+vA(2)); aB10=PB(1); aB20=PB(2); aB11=vB(1); aB21=vB(2); aB12=3/tf1^2*(PC(1)-PB(1))-2/tf1*vB(1)-1/tf1*vC(1); aB22=3/tf1^2*(PC(2)-PB(2))-2/tf1*vB(2)-1/tf1*vC(2); aB13=-2/tf1^3*(PC(1)-PB(1))+1/tf1^2*(vC(1)+vB(1)); aB23=-2/tf1^3*(PC(2)-PB(2))+1/tf1^2*(vC(2)+vB(2)); aC10=PC(1); aC20=PC(2); aC11=vC(1); aC21=vC(2); aC12=3/tf2^2*(PA(1)-PC(1))-2/tf2*vC(1)-1/tf2*vA(1); aC22=3/tf2^2*(PA(2)-PC(2))-2/tf2*vC(2)-1/tf2*vA(2); aC13=-2/tf2^3*(PA(1)-PC(1))+1/tf2^2*(vA(1)+vC(1)); aC23=-2/tf2^3*(PA(2)-PC(2))+1/tf2^2*(vA(2)+vC(2)); if (t
