Đang tải... (xem toàn văn)
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ê
Trang 1BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HCMKHOA ĐIỆN-ĐIỆN TỬ
Lớp T4 - Tiết 7-11
Thành phố Hồ Chí Minh, tháng 10 năm 2023
Trang 2Lời cảm ơn
Em xin được bày tỏ lời cảm ơn chân thành đến thầy giáo TS Trần Đức Thiện đãtrực tiếp giúp đỡ, hướng dẫn em hoàn thành bài tập 1 này 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ên.
Do trình độ nghiên cứu còn hạn chế và trong quá trình thực hiện đề tài không tránhkhỏi những sai sót, hạn chế rất mong thầy bỏ qua Và em rất mong nhận được sự quantâm, góp ý của thầy để nhóm hoàn thiện tốt hơn bài báo cáo cuối kỳ này cũng như rút kinhnghiệm cho bài báo cáo đồ án tốt nghiệp sắp tới Em xin chân thành cảm ơn!
Trang 3Chương 4 Vẽ không gian làm việc chung từng bộ nghiệm 23
Chương 5 Phương pháp quy hoạch quỹ đạo qua 2 và 3 điểm 27
5.1 Quy hoạch quỹ đạo đi qua 2 điểm 27
5.2 Quy hoạch quỹ đạo đi qua 3 điểm 30
Chương 6 Kết luận 33
TÀI LIỆU THAM KHẢO 34
Trang 4Danh sách hình ả
Hình 1.1 Robot 3 bậc tự do 6
Hình 1.2 Đặt hệ trục tọa độ 7
YHình 3.1 Vị trí của end-effector 13
Hình 3.2 Vị trí của end-effector nhìn từ ngoài vào trong 13
Hình 3.3 Vị trí của end-effector nhìn từ trên xuống 14
Hình 3.4 Vị trí của end-effector nhìn từ ngoài vào trong 14
Hình 3.5 Vị trí của end-effector nhìn từ trên xuống 15
Hình 3.6 Vị trí của end-effector nhìn từ ngoài vào trong 15
Hình 3.7 Vị trí của end-effector nhìn từ trên xuống 16
Hình 4.1 Không gian làm việc chung của Robot 25
Hình 4.2 Không gian làm việc của bộ nghiệm thứ nhất 25
Hình 4.3 Không gian làm việc của bộ nghiệm thứ hai 26
Hình 5.1 Thành lập khối matlab-funtion qua 2 điểm 27
Hình 5.2 Quỹ đạo của 2 điểm A và B với trục x 29
Hình 5.3 Quỹ đạo của 2 điểm A và B với trục y 29
Hình 5.4 Thành lập khối matlab-funtion qua 3 điểm 30
Hình 5.5 Quỹ đạo của 3 điểm A,B,C với trục x 32
Hình 5.6 Quỹ đạo của 3 điểm A,B,C với trục y 32
Trang 5Danh sách bảng
Bảng 1 Bảng thông số đề bài 6Bảng 2 Bảng D-H của robot 7Bảng 3 Bảng D-H đã thế các thông số 7
Trang 6CHƯƠNG 1: 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 thông số:
Bảng 1: Bảng thông số đề bài
Trang 7Đặt hệ trục tọa độ
Hình 1 1 Các hệ trục tọa độBảng D-H:
Trang 8CHƯƠNG 1: ĐỘNG HỌC THUẬN
- Ta có :
Ma trận chuyển đội từ hệ 0 sang hệ 1 :
T 01=[cos(Ɵ1) −sin(Ɵ1) 0 L 1sin (Ɵ1)cos(Ɵ 1) 0 0
0 0 0 1 ]=[cos(Ɵ1) −sin (Ɵ1) 0 0.1sin(Ɵ 1)cos(Ɵ1) 0 0
Ma trận chuyển đội từ hệ 1 sang hệ 2 :
T 12=[cos(Ɵ2) −sin (Ɵ1) 0 L 2sin(Ɵ 2)cos(Ɵ1) 0 0
0 0 0 1 ]=[cos(Ɵ 1) −sin(Ɵ 1) 0 0.2sin(Ɵ 1)cos(Ɵ 1) 0 0
Ma trận chuyển đội từ hệ 2 sang hệ 3 :
T 23=[cos(Ɵ 3) −sin(Ɵ 3) 0 L3sin (Ɵ3)cos(Ɵ 3) 0 0
Ma trận chuyển đội từ hệ 0 sang hệ 4 :
T 04=[cos(θ123) −sin(θ123) 0 L1+L 3∗cos(θ12)+L2∗cos(Ɵ1)+L 4∗cos(θ123)
sin(θ123) cos(θ123) 0 L 2∗sin(θ1)+L3∗sin(θ12)+L 4∗sin(θ123)
T 04=[cos(θ123) −sin(θ123) 0 0.1+0.15∗cos(θ12)+0.2∗cos(Ɵ1)+0.15∗cos(θ123)
sin(θ123) cos(θ123) 0 0.15∗sin(θ12)+0.2∗sin(Ɵ1)+0.15∗sin(θ123)
Trang 9Px =0.1+0.15∗cos(θ12)+0.2∗cos(Ɵ1)+0.15∗cos(θ123)Py=0.15∗sin(θ12)+0.2∗sin(Ɵ1)+0.15∗sin(θ123)Pz=0.2
- Với θ1=0,θ2=0, θ3=0
- Với θ1=90,θ2=0, θ3=0
=[ l1
1 ]=[0.10.50.21 ]
- Với θ1=0,θ2=90, θ3=0
1 ]=[0.30.30.21 ]
- Với θ1=0,θ2=0, θ3=90
1 ]=[0.450.150.2
1 ]
Trang 10CHƯƠNG 2: TÍNH ĐỘNG HỌC NGHỊCH
T 04=[cos(θ123) −sin(θ123) 0 0.1+0.15∗cos(θ12)+0.2∗cos(Ɵ1)+0.15∗cos(θ123)
sin(θ123) cos(θ123) 0 0.15∗cos(θ12)+0.2∗cos(Ɵ1)+0.15∗cos(θ123)
T 04=[cos(θ123) −sin(θ123) 0 L1+L 3∗cos(θ12)+L2∗cos(Ɵ1)+L 4∗cos(θ123)
sin(θ123) cos(θ123) 0 L 2∗sin(θ1)+L3∗sin(θ12)+L 4∗sin(θ123)
Tính θ2
+ Ta có: θ1+θ2+θ3=θ=0
+ Đặt {nx=Px−l1−l4c θ123=l2c θ1+l3c θ12ny=Py−l4s θ123=l2s θ1+l3s θ12 (2.1)
Trang 11→ c θ1= nx.(l2+l3c θ2)+ny l3 s θ2
l22+2l2l3c θ2+l32 s θ22+l32 c θ22(2.15)
→ c θ1=nx.(l2+l3c θ2)+ny.l3 s θ2l22
+2l2l3c θ2+l32 (2.16)
¿ 0,35 (0,2+0,15)+00,22
Trang 12CHƯƠNG 3: KIỂM CHỨNG3.1 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:
- 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
L0 = 0.15; L1 = 0.15 ; L2 = 0.25; L3 = 0.25 ; L4 = 0.15;%% The individual transformation:
%% The transformation matrix between frameT04= (T01*T12*T23*T34)
px=(T04(1,4))py=(T04(2,4))pz=(T04(3,4))
Trang 13- Với θ1=0,θ2=0, θ3=0
+ Kiểm chứng bằng hình học
Hình 3 1 Vị trí của End-effector
- Với θ1=90,θ2=0, θ3=0
=[ l1
1 ]=[0.10.50.21 ]
+ 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
l1+l2+l3+l4=¿0.6
Trang 14Hình 3.3 Vị trí của End-effector nhìn từ trên xuống
- Với θ1=0,θ2=90, θ3=0
1 ]=[0.30.30.21 ]
+ 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
l2+l1=¿0.3
Trang 15Hình 3.5 Vị trí của End-effector nhìn từ trên xuống
- Với θ1=0,θ2=0, θ3=90
1 ]=[0.450.150.2
Trang 16Hình 3.7 Vị trí của End-effector nhìn từ trên xuống
−Với :θ1=0o;θ2=60o;θ3=−60o;
l1+l2+l3=¿0.45
Trang 173.2 Kiểm chứng động học nghịch
+ Công thức tổng quát để biến đổi ma trận :
%% Thông số hệ thống
L0 = 150; L1 = 150; L2 = 250; L3 = 250; L4 = 150;epsilon = 1e-6;
%% Tính t1,t2,t3%% Tính theta2Px=P(1);
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
s21_theta1=sqrt(abs(1-c2_theta1^2));s22_theta1=-sqrt(abs(1-c2_theta1^2));
Trang 18theta1_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;%% Bộ nghiệm
[theta1_1 theta2_1 theta3_1]; [theta1_4 theta2_2 theta3_4];
nghiem1=[theta1_2 theta2_1 theta3_2];nghiem2=[theta1_3 theta2_2 theta3_3];
- 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.
Trang 19−Với :θ1=0o;θ2=0o;θ3=0o;ta có:P=[ 0.800.15]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P=[ 0.800.15]
Trang 20−Với :θ1=90o;θ2=0o;θ3=0o;ta có:P=[0.10.50.2]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P=[0.10.50.2]
Trang 21−Với :θ1=0o;θ2=90o;θ3=0o;ta có:P=[0.30.30.2]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P=[0.30.30.2]
Trang 22−Với :θ1=0o;θ2=0o;θ3=90o;ta có:P=[0.450.150.2]
->Ta thấy 2 bộ nghiệm đều ra tính ra vị trí P=[0.450.150.2 ]
Trang 23CHƯƠ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, theta3tt = 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];
%% Động học nghịch P1 = DHT(theta1, theta2, theta3); try
[n1, n2] = DHN(P1); %% 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];
Trang 24end %% 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
endend
Trang 25- 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
Trang 26- 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
Trang 27CHƯƠNG 5: PHƯƠNG PHÁP QUY HOẠCH QUỸ ĐẠO ĐI QUA 2 VÀ 3 ĐIỂM5.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 quacá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ểnqua 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
Trang 28- Chương trình matlab:
a12=3/tf^2*(Pf(1)-P0(1))-2/tf*v0(1)-1/tf*vf(1);a22=3/tf^2*(Pf(2)-P0(2))-2/tf*v0(2)-1/tf*vf(2);a13=-2/tf^3*(Pf(1)-P0(1))+1/tf^2*(vf(1)+v0(1));a23=-2/tf^3*(Pf(2)-P0(2))+1/tf^2*(vf(2)+v0(2));if (t<=2)
x=a10+a11*t+a12*t^2+a13*t^3; y=a20+a21*t+a22*t^2+a23*t^3;else
x=-20; y=-20;endreturn
-Kết quả:
Trang 29Hì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
Trang 305.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 quacá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ị
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);
Trang 31if (t<=2)
x=aA10+aA11*t+aA12*t^2+aA13*t^3; y=aA20+aA21*t+aA22*t^2+aA23*t^3;
elseif t<=4
x=aB10+aB11*t+aB12*t^2+aB13*t^3; y=aB20+aB21*t+aB22*t^2+aB23*t^3;
elseif t<=6
x=aC10+aC11*t+aC12*t^2+aC13*t^3; y=aC20+aC21*t+aC22*t^2+aC23*t^3;
x=10; y=10;
endreturn
Trang 32- Kết quả
Hình 5 1 Quỹ đạo của giữa điểm A và B với trục x
Hình 5.2: Quỹ đạo của giữa điểm A và B với trục y
Trang 33CHƯƠNG 5 KẾT LUẬN
Trong quá trình nghiên cứu và thực hiện báo cáo này, em đã xem xét và nghiên cứu ba chủđề quan trọng liên quan đến tính động học của robot và quy hoạch quỹ đạo Em đã khám phá sự khác biệt giữa tính động học thuận và động học nghịch của robot, thực hiện quy hoạch quỹ đạo cho robot thông qua việc sử dụng thuật toán, và cũng đã xem xét cách vẽ không gian cho từng bộ nghiệm để biểu diễn môi trường của robot.
Tính Động Học Thuận và Động Học Nghịch đã được thảo luận để hiểu rõ hơn về cách robot di chuyển một cách linh hoạt và hiệu quả trong môi trường đã định hướng, cũng nhưkhả năng của nó trong việc thích nghi và vượt qua các trở ngại.
Quy Hoạch Quỹ Đạo Robot đã được trình bày thông qua việc tạo ra quỹ đạo bằng cách kếthợp mô hình động học của robot và thuật toán Điều này giúp robot di chuyển theo một đường dẫn thuận lợi và an toàn để đạt được mục tiêu cụ thể trong không gian.
Vẽ Không Gian Cho Từng Bộ Nghiệm đã giúp hiểu rõ hơn môi trường của robot và cách nó tương tác với nó thông qua việc biểu diễn các điểm đầu, điểm giao đoạn (nếu có), và điểm đích trong không gian.
- Tóm lại, động học thuận và động học nghịch là hai khía cạnh quan trọng của việc điềukhiển robot 3 bậc tự do, trong khi quy hoạch quỹ đạo và không gian trạng thái đóng vai tròquan trọng trong việc lập kế hoạch và hiểu hành vi của robot trong không gian làm việc.
Trang 34TÀI LIỆU THAM KHẢO
[1] Nguyễn Trường Thịnh (2014), Giáo trình Kỹ thuật robot, Trường Đại học Sư phạm
Kỹ thuật TP.HCM.
[2].Modelling, Planning and Control, 2009.
[3].Matlab và simulink dành cho kỹ sư điều khiển tự động của thầy Nguyễn Phùng Quang.