(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp

(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp(Đề tài NCKH) Điều khiển, giám sát Robot Magician ứng dụng vận chuyển sản phẩm trong công nghiệp

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM BÁO CÁO TỔNG KẾT ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN ĐIỀU KHIỂN, GIÁM SÁT ROBOT MAGICIAN ỨNG DỤNG VẬN CHUYỂN SẢN PHẨM TRONG CÔNG NGHIỆP MÃ SỐ ĐỀ TÀI SV2021-123 Thuộc nhóm ngành khoa học : Ứng dụng Sinh viên thực : Đỗ Hoàng Việt Nam, Nữ : Nam Dân tộc : Kinh Lớp, khoa : 17151CL2, Khoa Đào Tạo Chất Lượng Cao Năm thứ : Ngành học : Công Nghệ Điều Khiển Tự Động Hóa Người hướng dẫn : PGS.TS Nguyễn Minh Tâm TP Hồ Chí Minh, Tháng 9, Năm 2021 MỞ ĐẦU 1 Tổng quan tình hình nghiên cứu thuộc lĩnh vực đề tài Phương pháp nghiên cứu Đối tượng phạm vi nghiên cứu Ý nghĩa khoa học thực tiễn đề tài CHƯƠNG 1: TỔNG QUAN 1.1 Đặt vấn đề 1.2 Mục tiêu giới hạn đề tài 1.3 Nội dung nghiên cứu CHƯƠNG 2: Cơ sở lý thuyết 2.1 Khái niệm cánh tay robot 2.2 Cấu tạo cánh tay robot 2.3 Khái niệm động học robot 2.3.1 Tính tốn động học thuận 2.3.2 Tính tốn động học ngược 2.4 Phương pháp quy hoạch quỹ đạo Robot 2.4.1 Giới thiệu 2.4.2 Hoạch định quỹ đạo hàm bậc 2.5 Lý thuyết tránh vật cản 10 2.5.1 Giới thiệu 10 2.5.2 Bài toán tránh vật cản 11 2.5.2.1 Quá trình phát va chạm 11 2.5.2.2 Giới thiệu phương pháp Potential Field 12 2.5.2.3 Thông số trường hấp dẫn Potential Field 13 2.5.2.4 Thông số trường kháng cự Potential Field 15 2.5.2.5 Tính tốn gradient descent 16 CHƯƠNG 3: Thiết kế phần cứng 18 3.1 Yêu cầu thiết kế robot 18 3.2 Thiết kế khí cho robot 18 3.3 Lựa chọn thiết bị phần cứng 21 3.3.1 Chọn động 21 3.3.2 Chọn vi điều khiển 24 3.3.3 Chọn driver điều khiển động 26 3.3.4 Các chi tiết phần cứng khác 28 CHƯƠNG 4: Thiết kế thi công phần mềm 30 4.1 Yêu cầu thực robot 30 4.2 Tính tốn thơng số tránh vật cản 30 4.2.1 Xác định vật cần gắp tính tốc độ 30 4.2.2 Xác định vật cản tính tốn khoảng cách đến robot: 31 4.2.3 Phát xử lý robot bị mắc kẹt 31 4.3 Thiết kế điều khiển 32 4.4 Thực điều khiển robot GUI điều khiển 35 CHƯƠNG 5: Kết đạt 37 5.1 Kết phần cứng 37 5.2 Kết mô phần mềm 37 CHƯƠNG 6: Kết luận hướng phát triển 40 6.1 Kết luận 40 6.2 Hướng phát triển 40 TÀI LIỆU THAM KHẢO 41 PHỤ LỤC 42 DANH MỤC HÌNH ẢNH Hình 1.1 Biểu đồ thể tăng trưởng ngành Robot qua năm 2013-2022 Hình 2.3.1 Mơ hình robot Magician Hình 2.3.2 Khớp động học robot Hình 2.4.1 Qũy đạo chuyển động 10 Hình 2.4.2 Qũy đạo vận tốc 10 Hình 2.5.1 Gradient trường kháng cự không liên tục 16 Hình 2.5.2 Gradient descent từ điểm ban đầu qinit đến giá trị cục nhỏ 17 Hình 3.2 Thiết kế khớp cánh tay robot 19 Hình 3.2 Thiết kế khớp 2,3 20 Hình 3.2.4 Thiết kế hồn thiện cánh tay robot 21 Hình 3.3.1 Động bước 22 Hình 3.3 Sơ đồ cánh tay robot 23 Hình 3.3.3 ArduinoMega 2560 24 Hình 3.3.4 Sơ đồ chân Arduino Mega 2560 25 Hình 3.3.5 Mạch điều khiển động bước TB6600 26 Hình 3.3.6 Sơ đồ kết nối Arduino với driver động 28 Hình 3.3.7 Các chi tiết phần cứng khác 28 Hình 4.3.1 Lưu đồ tổng quát chương trình điều khiển 33 Hình 4.3.2 Lưu đồ tổng quát chương trình hàm ngắt Timer5 34 Hình 4.3.3 Qũy đạo đường cua góc quay theta 35 Hình 4.4.1 GUI điều khiển Robot 35 Hình 5.1 Hình ảnh robot 37 Hình 5.2.1 Robot vị trí xuất phát 38 Hình 5.2.2 Robot tránh hết vật cản đường 38 Hình 5.2.3 Robot gắp vật chuyển động 39 Hình 5.2.4 Robot gắp vật né vật cản lúc để trở điểm đầu 39 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM THÔNG TIN KẾT QUẢ NGHIÊN CỨU CỦA ĐỀ TÀI Thông tin chung: - Tên đề tài: Điều khiển, giám sát Robot Magician ứng dụng thuật toán tránh vật cản - Chủ nhiệm đề tài: Đỗ Hoàng Việt Mã số SV: 17151163 - Lớp: 17151CL2B Khoa: Đào Tạo Chất Lượng Cao - Thành viên đề tài: Stt Họ tên MSSV Lớp Khoa Trần Phùng Bảo Trâm 17151146 17151CL2A CLC Nguyễn Thị Ngọc Thi 17151131 17151CL2B CLC Nguyễn Tiến Đức 17151069 17151CL2A CLC Phạm Phùng Huỳnh Đình Đại 17151059 17151CL2B CLC - Người hướng dẫn: PGS.TS Nguyễn Minh Tâm Mục tiêu đề tài: Thiết kế mơ hình robot Magician vận hành ổn định, định vị gắp vật, đồng thời bổ sung tính né vật cản Tính sáng tạo: Robot cơng nghiệp lúc né vật cản gặp điểm local minimum đứng yên, robot làm việc trở lại dỡ bỏ vật cản, đề tài nhóm đề xuất giải pháp đường cho robot để khỏi điểm local minimum di chuyển tiếp đến điểm mong muốn Kết nghiên cứu: Qua thời gian nghiên cứu, tìm hiểu nhóm hoàn thành nội dung sau: - Thiết kế lắp ráp hồn thiện mơ hình robot Magician - Quy hoạch quỹ đạo nghiệm để robot di chuyển đến vị trí mong muốn với tốc độ độ xác cao - Mơ q trình né vật cản gắp vật robot Đóng góp mặt giáo dục đào tạo, kinh tế - xã hội, an ninh, quốc phòng khả áp dụng đề tài: Đưa giải pháp có tính khả thi cao tốn tối ưu quỹ đạo đường kết hợp né vật cản robot Công bố khoa học SV từ kết nghiên cứu đề tài (ghi rõ tên tạp chí có) nhận xét, đánh giá sở áp dụng kết nghiên cứu (nếu có): TP Hồ Chí Minh, Ngày 01 tháng 10 năm 2021 SV chịu trách nhiệm thực đề tài (kí, họ tên) Nhận xét người hướng dẫn đóng góp khoa học SV thực đề tài (phần người hướng dẫn ghi): Ngày 01 tháng 10 năm 2021 Người hướng dẫn (kí, họ tên) MỞ ĐẦU Tổng quan tình hình nghiên cứu thuộc lĩnh vực đề tài “Robot Modeling and Control” tác giả Mark W Spong, Seth Hutchinson, and M Vidyasaga xuất lần thứ vào tháng 12 năm 2005 “Improved Manipulator Obstacle Avoidance Path Planning Based on Potential Field Method” tác giả Ming Zhao Xiaoqing Lv xuất ngày 22 tháng năm 2020 Phương pháp nghiên cứu Nghiên cứu sở khoa thực tiễn cấu tạo loại robot thực tế Nghiên cứu phương pháp tính động học thuận nghịch robot Mô chuyển động thực robot ứng dụng thuật toán tránh vật cản phần mềm Matlab/Simulink Đối tượng phạm vi nghiên cứu Đối tượng nghiên cứu đề tài Robot Magician Nghiên cứu tổng quan cấu tạo, nguyên lý hoạt động thuật toán tránh vật cản cho robot Vận dụng kiến thức học để lựa chọn sử dụng điều khiển cho cánh tay robot Ý nghĩa khoa học thực tiễn đề tài Kết nghiên cứu đề tài giúp hiểu rõ cấu tạo vận hành robot thực tế Bên cạnh đó, hiểu thêm tầm quan trọng sử dụng điều khiển để đảm bảo cho robot hoạt động xác 1|Page CHƯƠNG : TỔNG QUAN CHƯƠNG 1: TỔNG QUAN 1.1 Đặt vấn đề Cách mạng công nghiệp 4.0 phát triển mạnh mẽ mang đến cho nhân loại nói chung Việt Nam nói riêng nhiều hội để thay đổi mặt kinh tế quốc nội Trong phải kể đến ngành cơng nghiệp Tự động hóa Robot có bước phát triển vượt bậc, đem đến xác, suất, tốc độ sản xuất cho doanh nghiệp, công ty suốt năm trở lại Trên giới có nhiều hãng sản xuất robot cơng nghiệp khác Nhiều quốc gia giới đầu nghiên cứu lĩnh vực như: Nhật Bản, Mỹ, Úc, Nga… ứng dụng rộng rãi từ công xưởng, ngồi vũ trụ, đáy biển hay lịng đất… Theo thống kê Liên đoàn Robot học quốc tế (IFR) phát hành Báo cáo tăng trưởng ngành Robot, cho thấy giá trị bán hàng toàn cầu hàng năm vào khoảng 16,5 tỷ đô la vào năm 2018 IFR cho biết 422.000 robot xuất xưởng năm 2018, tăng 6% so với năm 2017 Tuy nhiên, tập đoàn cho biết năm 2019 giảm so với mức kỷ lục năm 2018, dự kiến tăng trưởng trung bình 12% năm từ năm 2020 đến năm 2022 Hình 1.1 Biểu đồ thể tăng trưởng ngành Robot qua năm 2013-2022 2|Page CHƯƠNG : TỔNG QUAN Qua số liệu thấy, việc nghiên cứu, ứng dụng chế tạo robot điều cần thiết để phát triển ngành tương lai Trong dự án này, nhóm lựa chọn đề tài “Điều khiển, giám sát Robot Magician ứng dụng thuật tốn tránh vật cản” để tìm hiểu ngun lý hoạt động cách sử dụng thuật toán Artificial Potential Field để vừa điều khiển xác điểm đầu cuối đồng thời tránh xuất vật cản vùng làm việc robot 1.2 Mục tiêu giới hạn đề tài Mục tiêu đề tài Vận dụng kiến thức học để tính tốn động học thuận, động học nghịch, quỹ hoạch quỹ đạo cho Robot Magician để đạt mục tiêu tránh vật cản đạt vị trí điều khiển mong muốn Nghiên cứu tổng quan cấu tạo, phương pháp tính tốn động học, mơ chuyển động thực để điều khiển xác robot Từ tính tốn động học, lực tác động vào robot, tính tốn chuyển động vật cản, từ quy hoạch quỹ đạo đường cho robot để đạt setpoint cách nhanh chóng xác Giới hạn đề tài Tính tốn động học mơ chuyển động tránh vật cản robot công cụ Matlab/Simulink 1.3 Nội dung nghiên cứu Chương 2: Cơ sở lý thuyết Chương 3: Thiết kế phần cứng Chương 4: Thiết kế thi công phần mềm Chương 5: Kết đạt Chương 6: Kết luận hướng phát triển 3|Page TÀI LIỆU THAM KHẢO H Asada and J.A Cro-Granito Kinematic and static characterization of wrist joints and their optimal design In Proc IEEE Conf on Robotics and Automation, St Louis, 1985 H Asada and J-J Slotine Robot Analysis and Control Wiley, New York, 1986 Jerome Barraquand and Jean-Claude Latombe Robot motion planning: A distributed representation approach International Journal of Robotics Research, 10(6):628–649, December 1991 D Bertsekas Nonlinear Programming Athena Scientifific, Belmont, MA, second edition, 1999 41 | P a g e PHỤ LỤC clc ss=1; L1 = 102.03; L2 = 177.5; L3 = 190; L4 = 81.3; L7 = 30.6; L6 = 0; L5=L1; linkLen = [L1;L2;L3;L4;L5]; %lengths of each link P0_EE = [250;150;0]; q = ikmagician(P0_EE(1),P0_EE(2),P0_EE(3)); %q start %draw object v_count = 1; t = 1; y0 = -200; v0 = 25; x = -250; z = 20; l = 30; w = 30; h = 30; pt_wait_pick = [x-w/2;200;100]; ptPick = [x-w/2;200;z+l]; ptPlace = [-x-w/2;150;z+l]; Sr = dist(pt_wait_pick,ptPick); Vr = (33.33333333/5)*5; Tr = Sr/Vr; 42 | P a g e S_start_pick = v0 * Tr; ptObject = draw_object(x,y0,z,l,w,h); start_gd = 1; chuky=1; while(true) switch ss case tStart = tic; P0_EE_goal = pt_wait_pick; case P0_EE_goal = ptPick; case P0_EE_goal = ptPlace; chuky = 2; otherwise break end qGoal = ikmagician(P0_EE_goal(1),P0_EE_goal(2),P0_EE_goal(3)); %q goal rhoNot = 50; %: defines the distance of influence of the obstacle % TRAJECTORY tv=0:.04:1; qd0 = [0,1,0]; %velocity qd1 = [0,0,0]; qtraj=jtraj_magician(q(1:3,1),qGoal(1:3,1),tv,qd0,qd1); P = size(qtraj,1); Virtual_numerator = 1; 43 | P a g e Virtual_denominator = 5; %divide qtraj into 'Virtual_denominator' parts qVirtual_target = qtraj(ceil(Virtual_numerator/Virtual_denominator*P),:); %take the first part oVirtual_target_temp fkmagician(qVirtual_target(1),qVirtual_target(2),qVirtual_target(3)); = oVirtual_target = oVirtual_target_temp(numel(linkLen),:); % oGoal = computeOrigins(qGoal); %robot goal DH origins oGoal = fkmagician(qGoal(1),qGoal(2),qGoal(3)); %robot goal DH origins q_recent = zeros(numel(q),4); %recent positions of robot arm % Parameters (should change these) %zeta: vector parameter that scales the forces for each degree-of-freedom zeta = [0.01;0.03;0.02;0.01;0.01]; alpha = 5; %step size for each iteration In motion planning problems, the choice for alpha is often made on an ad hoc or empirical basis, such as the distance to the nearest obstacle or goal d = 100; %d: the distance that defines the transition from conic to parabolic %eta: vector parameter that scales the repulsive forces for each degree-of-freedom eta_test = 300000; eta = [eta_test;eta_test;eta_test;eta_test;eta_test]; inLocalMinimum = false; eps_m = alpha+alpha/10; %epsilon_m: limit for step sizes for determining local minima fail = 1; tt = 1; %setup figure figure(1);clf; view(140,30) 44 | P a g e hold on %draw_obstacle ptObstacles_none = draw_obstacle(0,100,0,1,1,1); ptObstacles1 = draw_obstacle(170,200,0,30,40,60); ptObstacles2 = draw_obstacle(20,250,0,40,40,100); ptObstacles3 = draw_obstacle(-130,250,220,30,30,30); ptObstacles = [ptObstacles1;ptObstacles2;ptObstacles3]; %ptObstacles = [ptObstacles_none]; %draw robot hGline = line([0,0],[0,0],[0,1],'color','g','linewidth',4); % draw robot link setpoint hGpts = plot3([0,0],[0,0],[0,1],'og','markersize',12); % draw robot joint setpoint hRline = line([0,0],[0,0],[0,1],'color','b','linewidth',4); % draw robot link current hRpts = plot3([0,0],[0,0],[0,1],'ob','markersize',12); % draw robot joint current harr= quiver(0,0,1,2,'color',[0,0.8,0],'linewidth',2); % atrractive force hrep= quiver(0,0,1,2,'color',[0,0,1],'linewidth',2); % repulsive force Obline = line([0,0],[0,0],[0,1],'color','k','linewidth',2); % draw object to be picked up hold off hTitle = title(num2str(0)); set(hTitle,'fontsize',24); set(gca,'fontsize',20) hClosestLine = zeros(numel(ptObstacles(:,2))*numel(linkLen),1); for j = 1:numel(ptObstacles(:,2))*numel(linkLen); hClosestLine(j) = line([0,0],[0,0],[0,1],'color','m'); set(hClosestLine(j),'marker','.'); end Freps = zeros(numel(ptObstacles(:,2))*numel(linkLen),3); 45 | P a g e RepsPts = zeros(numel(ptObstacles(:,2))*numel(linkLen),3); %updateArm_3d(oGoal,hGline,hGpts); % draw robot setpoint updateObject_3d(ptObject,Obline); % draw object maxIters = 1000; for iteration = 1:maxIters %each iteration performs gradient descent one time oR = fkmagician(q(1),q(2),q(3)); %x,y,z robot delta=divelo(oR(5,:),oGoal(5,:)); oErr=delta(5,1)^2; %calulate error qVirtual_target = qtraj(ceil((Virtual_numerator/Virtual_denominator)*P),:); oVirtual_target_temp fkmagician(qVirtual_target(1),qVirtual_target(2),qVirtual_target(3)); = oVirtual_target = oVirtual_target_temp(numel(linkLen),:); delta_virtual=divelo(oR(5,:),oVirtual_target); oErr_virtual=delta_virtual(5,1)^2; % error of current point and virtual point for j = 1:numel(ptObstacles(:,1)) %calulate repulsive force and determine points on the robot closest to the obstacle [Frep,clpts] = frepPtFloatingPoint(q, ptObstacles(j,:), eta, rhoNot); for n = 1:numel(q) idx = (j-1)*numel(q)+n; % Draw the shortest line from the obstacle to the robot set(hClosestLine(idx), 'Xdata',[clpts(n,1),ptObstacles(j,1)],'Ydata',[clpts(n,2),ptObstacles(j,2)],'Zdata',[clpts (n,3),ptObstacles(j,3)]); Freps(idx,:) = Frep(n,:); % repulsive force RepsPts(idx,:) =clpts(n,:); % points on the robot closest to the obstacle 46 | P a g e end end if ss == Fatt = fatt(q, oVirtual_target_temp,zeta,d); % calulate attractive force start_gd = 1; elseif ss == && So S_start_pick start_gd = 0; elseif ss == clc ss=1; L1 = 102.03; L2 = 177.5; L3 = 190; L4 = 81.3; L7 = 30.6; L6 = 0; L5=L1; linkLen = [L1;L2;L3;L4;L5]; %lengths of each link P0_EE = [250;150;0]; q = ikmagician(P0_EE(1),P0_EE(2),P0_EE(3)); %q start %draw object v_count = 1; t = 1; y0 = -200; v0 = 25; x = -250; z = 20; l = 30; w = 30; h = 30; pt_wait_pick = [x-w/2;200;100]; 47 | P a g e ptPick = [x-w/2;200;z+l]; ptPlace = [-x-w/2;150;z+l]; Sr = dist(pt_wait_pick,ptPick); Vr = (33.33333333/5)*5; Tr = Sr/Vr; S_start_pick = v0 * Tr; ptObject = draw_object(x,y0,z,l,w,h); start_gd = 1; chuky=1; while(true) switch ss case tStart = tic; P0_EE_goal = pt_wait_pick; case P0_EE_goal = ptPick; case P0_EE_goal = ptPlace; chuky = 2; otherwise break end qGoal = ikmagician(P0_EE_goal(1),P0_EE_goal(2),P0_EE_goal(3)); %q goal rhoNot = 50; %: defines the distance of influence of the obstacle 48 | P a g e % TRAJECTORY tv=0:.04:1; qd0 = [0,1,0]; %velocity qd1 = [0,0,0]; qtraj=jtraj_magician(q(1:3,1),qGoal(1:3,1),tv,qd0,qd1); P = size(qtraj,1); Virtual_numerator = 1; Virtual_denominator = 5; %divide qtraj into 'Virtual_denominator' parts qVirtual_target %take the first part = qtraj(ceil(Virtual_numerator/Virtual_denominator*P),:); oVirtual_target_temp fkmagician(qVirtual_target(1),qVirtual_target(2),qVirtual_target(3)); = oVirtual_target = oVirtual_target_temp(numel(linkLen),:); % oGoal = computeOrigins(qGoal); %robot goal DH origins oGoal = fkmagician(qGoal(1),qGoal(2),qGoal(3)); %robot goal DH origins q_recent = zeros(numel(q),4); %recent positions of robot arm % Parameters (should change these) %zeta: vector parameter that scales the forces for each degree-of-freedom zeta = [0.01;0.03;0.02;0.01;0.01]; alpha = 5; %step size for each iteration In motion planning problems, the choice for alpha is often made on an ad hoc or empirical basis, such as the distance to the nearest obstacle or goal d = 100; %d: the distance that defines the transition from conic to parabolic %eta: vector parameter that scales the repulsive forces for each degree-offreedom eta_test = 300000; eta = [eta_test;eta_test;eta_test;eta_test;eta_test]; inLocalMinimum = false; eps_m = alpha+alpha/10; %epsilon_m: limit for step sizes for determining local minima 49 | P a g e fail = 1; tt = 1; %setup figure figure(1);clf; view(140,30) hold on %draw_obstacle ptObstacles_none = draw_obstacle(0,100,0,1,1,1); ptObstacles1 = draw_obstacle(170,200,0,30,40,60); ptObstacles2 = draw_obstacle(20,250,0,40,40,100); ptObstacles3 = draw_obstacle(-130,250,220,30,30,30); ptObstacles = [ptObstacles1;ptObstacles2;ptObstacles3]; %ptObstacles = [ptObstacles_none]; %draw robot Fatt = fatt(q, oVirtual_target_temp,zeta,d); % calulate attractive force x_ob_picked = oR(5,1); y_ob_picked = oR(5,2); z_ob_picked = oR(5,3); ptObject = draw_object(x_ob_picked+l/2,y_ob_picked+w/2,z_ob_pickedh,l,w,h); start_gd = 1; end if(ss == 1) || (ss == 2) timerVal(iteration) = toc(tStart); if(timerVal(iteration)>t) y = y0 + v0*t; 50 | P a g e ptObject = draw_object(x,y,z,l,w,h); t = t + 0.1; end elseif(ss==3) t=0; end updateObject_3d(ptObject,Obline); center_object = [ptObject(1,1)+l/2; ptObject(1,2)+w/2; ptObject(1,3)+h,]; So = dist(ptPick,center_object); %update drawing updateArm_3d(oR,hRline,hRpts); % draw attractive force set(harr, 'Xdata',oR(:,1),'Ydata',oR(:,2),'Zdata',oR(:,3),'Udata',Fatt(:,1),'Vdata',Fatt(:,2),'Wdata ',Fatt(:,3)); % draw repulsive force set(hrep, 'Xdata',RepsPts(:,1),'Ydata',RepsPts(:,2),'Zdata',RepsPts(:,3),'Udata',Freps(:,1),'Vdat a',Freps(:,2),'Wdata',Freps(:,3)); set(hTitle,'String',[num2str(iteration),' of ', num2str(maxIters), ' error = ',num2str(oErr)]) drawnow if oErr < 0.001 % if current point = target point -> stop robot ss = ss+1; if(ss==4) ss=1; end break end if oErr_virtual < alpha*100 % if current point close virtual point -> set a new virtual point if Virtual_numerator < Virtual_denominator 51 | P a g e Virtual_numerator = Virtual_numerator + ; end end %map the workspace forces to joint torques tau = zeros(3,1); for ic = 1:numel(q) % torques Fatt tau = tau + Fatt(ic,:)'; end for jc = 1:numel(ptObstacles(:,1)) for ic = 1:numel(q) idx = (jc-1)*numel(q)+ic; % torques Fatt - torques Freps tau = tau + Freps(idx,:)'; end end %Gradient descent algorithm if start_gd ==1 o_temp= fkmagician(q(1),q(2),q(3)); o_now = o_temp(numel(linkLen),:); amin = min(alpha, 1/2*max(abs(o_now-oVirtual_target))); o_now = o_now+(amin*tau/norm(tau))'; q = ikmagician(o_now(1),o_now(2),o_now(3)); o_now_full(tt,:) = o_now; oVirtual_target_full(tt,:) = oVirtual_target; tt = tt + 1; end hold on 52 | P a g e plot3(o_now(1),o_now(2),o_now(3),'or','markersize',2); plot3(oVirtual_target(1),oVirtual_target(2),oVirtual_target(3),'diamond','markersize' ,10); axis([-350 350 -350 350 350]); %detect a local minimum q_recent = [q_recent(:,2:end) q]; if iteration > inLocalMinimum = (distance(q_recent(1:3,4),qVirtual_target') > eps_m && distance(q_recent(1:3,1),q_recent(1:3,2)) < eps_m && distance(q_recent(1:3,1),q_recent(1:3,3)) < eps_m && distance(q_recent(1:3,1),q_recent(1:3,4)) < eps_m); end if inLocalMinimum qtraj=jtraj_magician(q(1:3,1),qGoal(1:3,1),tv,qd0,qd1); Virtual_numerator = ceil(rand*Virtual_denominator); end end end 53 | P a g e 54 | P a g e ... tài: Điều khiển, giám sát Robot Magician ứng dụng thuật toán tránh vật cản - Chủ nhiệm đề tài: Đỗ Hoàng Việt Mã số SV: 17151163 - Lớp: 17151CL2B Khoa: Đào Tạo Chất Lượng Cao - Thành viên đề tài: ... lựa chọn đề tài ? ?Điều khiển, giám sát Robot Magician ứng dụng thuật toán tránh vật cản” để tìm hiểu nguyên lý hoạt động cách sử dụng thuật toán Artificial Potential Field để vừa điều khiển xác... tránh vật cản 2.5.1 Giới thiệu Trong công nghiệp dây chuyền lớn vận hành làm việc công suất cao, đặc biệt dây chuyền sử dụng robot Trong q trình vận hành, đơi gặp vài cố robot bị chặn vật cản (con