Điều khiển phân tán robot bầy đàn tránh vật cản dùng mạng api socket

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA ĐIỀU KHIỂN PHÂN TÁN ROBOT BẦY ĐÀN TRÁNH VẬT CẢN DÙNG MẠNG API SOCKET GVHD: TS VŨ VĂN PHONG SVTH: HOÀNG VIỆT HƯNG VŨ HỒNG THÁI TRẦN VĂN TÀI SKL009117 Tp.Hồ Chí Minh, tháng 8/2022 BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MƠN TỰ ĐỘNG ĐIỀU KHIỂN - - ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: ĐIỀU KHIỂN PHÂN TÁN ROBOT BẦY ĐÀN TRÁNH VẬT CẢN DÙNG MẠNG API SOCKET GVHD: TS VŨ VĂN PHONG SVTH: HOÀNG VIỆT HƯNG 18151185 VŨ HỒNG THÁI 18151242 TRẦN VĂN TÀI 18151239 Tp Hồ Chí Minh, tháng năm 2022 BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN - - ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: ĐIỀU KHIỂN PHÂN TÁN ROBOT BẦY ĐÀN TRÁNH VẬT CẢN DÙNG MẠNG API SOCKET GVHD: TS VŨ VĂN PHONG SVTH: HOÀNG VIỆT HƯNG 18151185 VŨ HỒNG THÁI 18151242 TRẦN VĂN TÀI 18151239 Tp Hồ Chí Minh, tháng năm 2022 TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN o0o Tp HCM, ngày 24 tháng 07 năm 2022 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên Sinh viên 1: Vũ Hồng Thái MSSV:18151242 Họ tên Sinh viên 2: Hoàng Việt Hưng MSSV:18151185 Họ tên Sinh viên 3: Trần Văn Tài MSSV:18151239 Chuyên ngành: Công nghệ Kỹ thuật Điều khiển Tự động hóa Hệ đào tạo: Đại học quy Khóa: 2018 Lớp: 181511 I TÊN ĐỀ TÀI: ĐIỀU KHIỂN PHÂN TÁN ROBOT BẦY ĐÀN TRÁNH VẬT CẢN DÙNG MẠNG API SOCKET II NHIỆM VỤ Các số liệu ban đầu: - Tham khảo hệ robot bầy đàn, mobile robot yêu cầu phương pháp điều khiển bầy đàn sử dụng giới - Tiến hành tìm hiểu, nghiên cứu khoa học robot bầy đàn Nội dung thực hiện: - Tìm hiểu nghiên cứu thuật toán điều khiển bầy đàn với robot di động - Tìm hiểu thuật tốn điều khiển robot dẫn đầu bám theo quỹ đạo tham chiếu - Tính tốn mơ động học robot di động thuật tốn điều khiển đội hình - Thiết kế phần cứng lập trình nhúng điều khiển robot với board Jetson nano - Xử lý ảnh xác định vị trí hướng robot di động với thư viên OpenCV i - Truyền thông, giao tiếp liệu robot di động qua mạng wifi API Socket - Điều khiển tập hợp đội hình robot, di chuyển đội hình di chuyển đội hình tránh vật cản - Thiết kế giao diện giám sát điều khiển robot Python với PyQt5 III NGÀY GIAO NHIỆM VỤ: 20/02/2022 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 30/07/2022 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: TS Vũ Văn Phong CÁN BỘ HƯỚNG DẪN BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN TS Vũ Văn Phong ii PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Sinh viên thực hiện: Họ tên: Vũ Hồng Thái MSSV: 18151242 Lớp: 181511A Họ tên: Hoàng Việt Hưng MSSV: 18151185 Lớp: 181511A Họ tên: Trần Văn Tài MSSV: 18151239 Lớp: 181511A Ngành: Công nghệ kỹ thuật Điều khiển Tự động hóa Tên đề tài: ĐIỀU KHIỂN ROBOT BẦY ĐÀN TRÁNH VẬT CẢN DÙNG MẠNG API SOCKET Giáo viên hướng dẫn: TS Vũ Văn Phong NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Về nội dung đề tài khối lượng thực Ưu điểm Khuyết điểm Ý kiến khác Tp HCM, ngày 06 tháng 08 năm 2022 Giáo viên hướng dẫn TS Vũ Văn Phong iii PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Sinh viên thực hiện: Họ tên: Vũ Hồng Thái MSSV: 18151242 Lớp: 181511A Họ tên: Hoàng Việt Hưng MSSV: 18151185 Lớp: 181511A Họ tên: Trần Văn Tài MSSV: 18151239 Lớp: 181511A Ngành: Công nghệ kỹ thuật Điều khiển Tự động hóa Tên đề tài: ĐIỀU KHIỂN ROBOT BẦY ĐÀN TRÁNH VẬT CẢN DÙNG MẠNG API SOCKET Giáo viên phản biện: TS Trần Vi Đô NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Về nội dung đề tài khối lượng thực Ưu điểm Khuyết điểm Đề nghị cho bảo vệ hay không? Đánh giá loại Điểm……………….(Bằng chữ:………………………………….……………….) Tp HCM, ngày 06 tháng 08 năm 2022 Giáo viên phản biện TS Trần Vi Đô iv TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN o0o Tp HCM, ngày 24 tháng 07 năm 2022 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên Sinh viên 1: Vũ Hồng Thái MSSV:18151242 Họ tên Sinh viên 2: Hoàng Việt Hưng MSSV:18151185 Họ tên Sinh viên 3: Trần Văn Tài MSSV:18151239 Tuần/Ngày 15/02 – 25/02 Nội dung Lựa chọn đề tài thực (Điều khiển Robot bầy đàn dùng mạng API socket) 25/02 – 15/03 Tìm hiểu tổng quan đề tài Tìm tài liệu, báo khoa học đề tài robot bầy đàn điều 10/03 – 30/04 khiển đội hình robot Xây dựng mơ hình tốn mobile robot 01/04 – 01/05 Lựa chọn thiết bị thiết kế phần cứng 15/05 – 15/06 Tìm hiểu xây dựng thuật tốn điều khiển đội hình cho xe 16/06 – 07/07 Tìm hiểu xây dựng thuật tốn tránh vật cản v Xác nhận GVHD Tìm hiểu, xây dựng giao tiếp truyền thông robot với 01/06 – 01/07 robot với máy tính mạng wifi với API Socket Python 01/06 – 01/07 15/06 – 15/07 Xây dựng thuật toán xử lý ảnh với Camera để xác định vị trí hướng robot Thiết kế giao diện điều khiển Robot máy tính PyQt5 15/06 – 15/07 Tiến hành chạy thử, kiểm tra sửa lỗi 15/07 – 08/08 Viết báo cáo word, thiết kế PowerPoint Chuẩn bị bảo vệ đồ án GV HƯỚNG DẪN (Ký ghi rõ họ tên) TS Vũ Văn Phong vi TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN o0o Tp HCM, ngày 24 tháng 07 năm 2022 LỜI CAM ĐOAN Tôi tên là: Vũ Hồng Thái MSSV:18151242 Tôi tên là: Hồng Việt Hưng MSSV:18151185 Tơi tên là: Trần Văn Tài MSSV:18151239 Ngành học: Công Nghệ Kỹ Thuật Điều Khiển Và Tự Động Hóa Chúng tơi xin cam kết báo cáo thực dựa vào kinh nghiệm kết thực tế suốt trình học tập thực đề tài mà có, khơng chép hay giả mạo kết có báo cáo Chúng tơi thực dựa vào số tài liệu tham khảo đoạn trích hình ảnh sử dụng đính kèm tài liệu tham khảo họ, công khai theo quy tắc Nhóm thực đề tài Hồng Việt Hưng Vũ Hồng Thái vii Trần Văn Tài PHỤ LỤC L23]; u=inv(G2)*(p-F2*u1); error=e; y = u; Matlab Function: Bám quỹ đạo xe function [y,vw1,eror] = fcn(delta,theta0_present,k,vw) %#codegen A=[cos(theta0_present) sin(theta0_present) 0; -sin(theta0_present) cos(theta0_present) 0; 0 1]; e=A*delta; V_c=[vw(1)*cos(e(3))+k(1)*e(1); vw(2)+k(2)*vw(1)*e(2)+k(3)*vw(1)*sin(e(3))]; Matrix=[cos(theta0_present) 0; sin(theta0_present) 0; 1]; u=Matrix*V_c; y = u; vw1=V_c; eror = e; Code mơ thuật tốn tránh vật cản xe: Matlab function: Thuật toán tránh vật cản function y= fcn(sum_r,d,posi_Q,x_R,y_R,anpha) %#codegen rv=sum_r(1)+sum_r(2)+sum_r(3); M=[cos(anpha) -sin(anpha) posi_Q(1); sin(anpha) cos(anpha) posi_Q(2); BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN PHỤ LỤC 0 0; 0 1]; N=[x_R;y_R;0;1]; Vitri=inv(M)*N; xr_dot=(d/abs(d))*Vitri(2)+Vitri(1)*(rv^2-Vitri(1)^2-Vitri(2)^2); yr_dot=-(d/abs(d))*Vitri(1)+Vitri(2)*(rv^2-Vitri(1)^2-Vitri(2)^2); theta_r=atan(yr_dot/xr_dot); u=[xr_dot;yr_dot;theta_r]; y = u; Matlab function: Phương trình quỹ đạo Robot function [y,eror] = fcn(delta,theta0_present,k,v,w) %#codegen A=[cos(theta0_present) sin(theta0_present) 0; -sin(theta0_present) cos(theta0_present) 0; 0 1]; e=A*delta; V_c=[v*cos(e(3))+k(1)*e(1); w+k(2)*v*e(2)+k(3)*v*sin(e(3))]; Matrix=[cos(theta0_present) 0; sin(theta0_present) 0; 1]; u=Matrix*V_c; y = u; eror = e; BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN PHỤ LỤC Code mơ thuật tốn tránh vật cản xe trì đội hình: Matlab function: Tính quỹ đạo chất điểm function y= fcn(sum_r,d,posi_Q,x_R,y_R,anpha) %#codegen rv=sum_r(1)+sum_r(2)+sum_r(3); M=[cos(anpha) -sin(anpha) posi_Q(1); sin(anpha) cos(anpha) posi_Q(2); 0 0; 0 1]; N=[x_R;y_R;0;1]; Vitri=inv(M)*N; xr_dot=(d/abs(d))*Vitri(2)+Vitri(1)*(rv^2-Vitri(1)^2-Vitri(2)^2); yr_dot=-(d/abs(d))*Vitri(1)+Vitri(2)*(rv^2-Vitri(1)^2-Vitri(2)^2); theta_r=atan(yr_dot/xr_dot); u=[xr_dot;yr_dot;theta_r]; y = u; Matlab function: Bám quỹ đạo robot function [y,vw1,eror] = fcn(delta,theta0_present,k,v,w) %#codegen A=[cos(theta0_present) sin(theta0_present) 0; -sin(theta0_present) cos(theta0_present) 0; 0 1]; e=A*delta; V_c=[v*cos(e(3))+k(1)*e(1); w+k(2)*v*e(2)+k(3)*v*sin(e(3))]; BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN PHỤ LỤC Matrix=[cos(theta0_present) 0; sin(theta0_present) 0; 1]; u=Matrix*V_c; vw1=V_c; y = u; eror = e; Matlab function: Thuật tốn đội hình function [y,error] = fcn(x1,y1,theta1,x2,y2,theta2,x3,y3,theta3,u1,k,target) %#codegen d=0.09; L12_x=x1-x2-d*cos(theta2); L12_y=y1-y2-d*sin(theta2); L13_x=x1-x3-d*cos(theta3); L13_y=y1-y3-d*sin(theta3); L23_x=x2-x3-d*cos(theta3); L23_y=y2-y3-d*sin(theta3); L12=sqrt(L12_x^2+L12_y^2); L13=sqrt(L13_x^2+L13_y^2); L23=sqrt(L23_x^2+L23_y^2); mega12=atan(L12_y/L12_x)-theta1+pi; mega13=atan(L13_y/L13_x)-theta1+pi; mega23=atan(L23_y/L23_x)-theta2+pi; beta12=theta1-theta2; beta13=theta1-theta3; beta23=theta2-theta3; BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN PHỤ LỤC si12=beta12+mega12; si13=beta13+mega13; si23=beta23+mega23; G2=[cos(si12) d*sin(si12) 0; -sin(si12)/L12 d*cos(si12)/L12 0 0; cos(si13) d*sin(si13); -cos(mega23) F2=[-cos(mega12) cos(si23) d*sin(si23)]; 0; sin(mega12)/L12 -1; -cos(mega13) 0; 0]; p= [k(1)*(target(1)-L12); k(2)*(target(2)-mega12); k(3)*(target(3)-L13); k(4)*(target(4)-L23)]; e=[L12; mega12; L13; L23]; u=inv(G2)*(p-F2*u1); error=e; y = u; Code hàm chương trình xử lý Python: Python: Thuật toán bầy đàn G=np.matrix([ [m.cos(gama12) , 0.09*m.sin(gama12) ,0 [-m.sin(gama12)/L12_3 , 0.09*m.cos(gama12)/L12_3 , BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN ,0 ], ,0 ], PHỤ LỤC [0 , , m.cos(gama13) [-m.cos(psi23_3) , , m.cos(gama23) ]) F=np.matrix([[-m.cos(psi12_3) , 0], [ m.sin(psi12_3)/L12_3 ,-1], [-m.cos(psi13_3) [0 , 0], , 0] ]) p=np.matrix([[k1*(L12d_3-L12_3)], [k2*(psi12d_3-psi12_3)], [k3*(L13d_3-L13_3)], [k4*(L23d_3-L23_3)]]) bien_tinh=p-np.dot(F,u1) u23=np.dot(np.linalg.inv(G),bien_tinh) delta_L12=L12d_3-L12_3 delta_psi12=psi12d_3-psi12_3 delta_L13=L13d_3-L13_3 delta_L23=L23d_3-L23_3 Python: Di chuyển bầy đàn if mode_tich_1==0: x1_present=float(self.ui.position_x1_now.text()) y1_present=float(self.ui.position_y1_now.text()) theta1_present = float(self.ui.theta1_now.text()) xR=float(self.ui.R_target_x.text()) yR=float(self.ui.R_target_y.text()) if mode_R_run==0: Xr_run=Xr_run+0.04*m.cos(np.deg2rad(theta_r))*0.02 Yr_run=Yr_run+0.04*m.sin(np.deg2rad(theta_r))*0.02 BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN , 0.09*m.sin(gama13)], , 0.09*m.sin(gama23)] PHỤ LỤC k1=float(self.ui.K1_setup.text()) k2=float(self.ui.K2_setup.text()) k3=float(self.ui.K3_setup.text()) if np.sqrt((xR-Xr_run)**2+(yR-Yr_run)**2) color2yp): theta = BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN PHỤ LỤC elif (color1xp == color2xp) and (color1yp < color2yp): theta = math.pi elif (color2xp > color1xp): theta = math.atan((color1yp-color2yp)/(color1xp-color2xp)) + math.pi/2 elif (color2xp < color1xp): theta = math.atan((color1yp-color2yp)/(color1xp-color2xp)) + math.pi*3/2 # if theta >math.pi/2: # print(color1xp,color2xp,color1yp,color2yp) elif (((color1xp-color4xp)**2 + (color1yp-color4yp)**2) < ((color1xp-color2xp)**2 + (color1yp-color2yp)**2)) and (((color1xp-color4xp)**2 + (color1yp-color4yp)**2) < ((color3xp-color2xp)**2 + (color3yp-color2yp)**2)) and (((color1xp-color4xp)**2 + (color1yp-color4yp)**2) < ((color3xp-color4xp)**2 + (color3yp-color4yp)**2)): x = round((color1x+color4x)/2,3) y = round((color1y+color4y)/2,3) x_p = round((color1xp+color4xp)/2) y_p = round((color1yp+color4yp)/2) if (color1xp == color4xp) and (color1yp > color4yp): theta = elif (color1xp == color4xp) and (color1yp < color4yp): theta = math.pi elif (color4xp > color1xp): theta = math.atan((color1yp-color4yp)/(color1xp-color4xp)) + math.pi/2 elif (color4xp < color1xp): theta = math.atan((color1yp-color4yp)/(color1xp-color4xp)) + math.pi*3/2 # if theta >math.pi/2: # print(color1xp,color4xp,color1yp,color4yp) elif (((color3xp-color4xp)**2 + (color3yp-color4yp)**2) < ((color1xp-color2xp)**2 + (color1yp-color2yp)**2)) and (((color3xp-color4xp)**2 + (color3yp-color4yp)**2) < ((color3xp-color2xp)**2 + (color3yp-color2yp)**2)) and (((color3xp-color4xp)**2 + (color3yp-color4yp)**2) < ((color1xp-color4xp)**2 + (color1yp-color4yp)**2)): x = round((color3x+color4x)/2,3) y = round((color3y+color4y)/2,3) x_p = round((color3xp+color4xp)/2) BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN PHỤ LỤC y_p = round((color3yp+color4yp)/2) if (color3xp == color4xp) and (color3yp > color4yp): theta = elif (color3xp == color4xp) and (color3yp < color4yp): theta = math.pi elif (color4xp > color3xp): theta = math.atan((color3yp-color4yp)/(color3xp-color4xp)) + math.pi/2 elif (color4xp < color3xp): theta = math.atan((color3yp-color4yp)/(color3xp-color4xp)) + math.pi*3/2 else: x = round((color3x+color2x)/2,3) y = round((color3y+color2y)/2,3) x_p = round((color3xp+color2xp)/2) y_p = round((color3yp+color2yp)/2) if (color3xp == color2xp) and (color3yp > color2yp): theta = elif (color3xp == color2xp) and (color3yp < color2yp): theta = math.pi elif (color2xp > color3xp): theta = math.atan((color3yp-color2yp)/(color3xp-color2xp)) + math.pi/2 elif (color2xp < color3xp): theta = math.atan((color3yp-color2yp)/(color3xp-color2xp)) + math.pi*3/2 theta = round(theta*180/math.pi,2) return theta, x, y, x_p, y_p BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN S K L 0