Tối ưu đường đi cho robot và di chuyển đa robot theo đội hình

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 TỐI ƯU ĐƯỜNG ĐI CHO ROBOT VÀ DI CHUYỂN ĐA ROBOT THEO ĐỘI HÌNH GVHD: TS VŨ VĂN PHONG SVTH : NGUYỄN TỪ GIA THỊNH LÊ VĂN TÙNG NGUYỄN XUÂN SÁNG SKL011583 Tp Hồ Chí Minh, tháng 7/2023 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: TỐI ƯU ĐƯỜNG ĐI CHO ROBOT VÀ DI CHUYỂN ĐA ROBOT THEO ĐỘI HÌNH GVHD: TS Vũ Văn Phong SVTH MSSV Nguyễn Từ Gia Thịnh 19151041 Lê Văn Tùng 19151028 Nguyễn Xuân Sáng 19151030 TP Hồ Chí Minh, tháng 07 năm 2023 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Ử BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN ĐỘC LẬP - TỰ DO - HẠNH PHÚC o0o -Tp HCM, tháng 07 năm 2023 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Nguyễn Từ Gia Thịnh MSSV: 19151041 Họ tên sinh viên 2: Lê Văn Tùng MSSV: 19151028 Họ tên sinh viên 3: Nguyễn Xuân Sáng MSSV: 19151030 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: 2019 Lớp: 191511B I TÊN ĐỀ TÀI: Tối ưu đường cho robot di chuyển đa robot theo đội hình II NHIỆM VỤ Các số liệu ban đầu: - Robot di động hai bánh vi sai hệ non-holonomic: động dẫn động phía sau bánh đa hướng phía trước xoay tự 360 độ - Thông số xe dự kiến: vận tốc tối đa 0.1 (m / s) - Sử dụng thiết bị Jetson Nano, cảm biến IMU 9250, vi điều khiển Arduino Mega 2560, board Jetbot Kit, động Encoder hộp số JGY-370, router Tplink MR-6400 cảm biến Rplidar A1-M8 Nội dung thực hiện: - Nghiên cứu hệ thống đa robot thuật toán tạo đồ, tối ưu đường trì đội hình - Nghiên cứu hệ điều hành robot ROS - Thiết kế phần cứng SolidWork sau triển khai thi cơng thực tế - Lập trình giải thuật cho robot mô phần mềm Matlab - Lập trình giải thuật cho robot mơ phần mềm Gazebo - Lập trình giải thuật cho robot mơ mơ hình thật - Thử nghiệm sửa lỗi hệ thống - Viết báo cáo III NGÀY GIAO NHIỆM VỤ: 05/02/2022 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 04/07/2023 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 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 04 tháng 07 năm 2023 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Nguyễn Từ Gia Thịnh MSSV: 19151041 Họ tên sinh viên 2: Lê Văn Tùng MSSV: 19151028 Họ tên sinh viên 3: Nguyễn Xuân Sáng MSSV: 19151030 Tên đề tài: Tối ưu đường cho robot di chuyển đa robot theo đội hình Xác nhận GVHD STT Thời gian Nội dung Tuần 1-3 05/02 - 26/02 Nghiên cứu hệ thống đa robot thuật toán tạo đồ, tối ưu đường đi, trì đội hình Tuần 4-6 26/02 - 17/03 Nghiên cứu hệ điều hành robot ROS Tuần 7-9 18/03 - 09/04 Thiết kế phần cứng SolidWork sau triển khai thi cơng thực tế Tuần 10-12 10/04 - 01/05 Lập trình giải thuật cho robot mô phần mềm Matlab Tuần 13-15 02/05 - 23/05 Lập trình giải thuật cho robot mô phần mềm Gazebo mơ hình thật Tuần 18-19 09/06 - 23/06 Thử nghiệm sửa lỗi hệ thống Tuần 20 24/06 - 30/06 Viết báo cáo GV HƯỚNG DẪN (Ký ghi rõ họ tên) 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Ử BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN ĐỘC LẬP - TỰ DO - HẠNH PHÚC o0o -Tp HCM, tháng 07 năm 2023 LỜI CAM ĐOAN Nhóm xin cam kết đề tài nhóm tự thực dựa vào số tài liệu trước khơng chép từ tài liệu hay cơng trình có trước Những số liệu, bảng biểu phục vụ cho việc phân tích dẫn dắt đề tài thu thập từ nguồn tài liệu khác ghi mục tài liệu tham khảo thích bên bảng biểu Ngoài ra, tài liệu diễn giải để làm rõ thêm luận điểm phân tích trích dẫn phần phụ lục thích nguồn gốc liệu Người thực đề tài Nguyễn Từ Gia Thịnh Lê Văn Tùng Nguyễn Xuân Sáng LỜI CẢM ƠN Lời nhóm em xin bày tỏ lời cảm ơn chân thành đến Ban Giám hiệu Trường Đại Học Sư Phạm Kỹ Thuật Thành Phố Hồ Chí Minh, ban chủ nhiệm Khoa Điện - Điện Tử quý thầy cô môn Điều Khiển Tự Động trang bị kiến thức, kỹ cần thiết để nhóm thực đồ án tốt nghiệp: “Tối ưu đường cho robot di chuyển đa robot theo đội hình” Đặc biệt nhóm em xin cảm ơn thầy giáo TS Vũ Văn Phong trực tiếp giúp đỡ, hướng dẫn nhóm em hồn thành đồ án Trong suốt trình học tập thực đồ án nhóm ln quan tâm, hướng dẫn giúp đỡ tận tình thầy, giáo Khoa Điện – Điện Tử Ngồi nhóm xin cảm ơn giúp đỡ người thân, anh chị, bạn bè với ý kiến đóng góp cần thiết để nhóm thực đồ án cách tốt Do trình độ nghiên cứu cịn hạn chế nên khơng tránh khỏi sai sót, mong thầy cô bỏ qua Cuối xin chúc quý thầy cô nhiều sức khoẻ, thành công công việc Chúc anh chị, bạn khoá sức khoẻ, học tập thật tốt để chuẩn bị kiến thức vững vàng tương lai Nhóm em xin chân thành cảm ơn! TP Hồ Chí Minh, tháng 07 năm 2023 Sinh viên thực Nguyễn Từ Gia Thịnh Lê Văn Tùng Nguyễn Xuân Sáng i MỤC LỤC LỜI CẢM ƠN i MỤC LỤC ii DANH SÁCH HÌNH ẢNH vi DANH SÁCH BẢNG xii TÓM TẮT xiv CHƯƠNG 1: TỔNG QUAN 1.1 Đặt vấn đề 1.1.1 Tình hình nghiên cứu ngồi nước 1.1.2 Tình hình nghiên cứu nước 1.2 Mục tiêu đề tài 1.3 Phương pháp nghiên cứu 1.4 Nội dung nghiên cứu 1.5 Giới hạn CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 Mơ hình tốn hệ robot di động hai bánh vi sai 2.1.1 Động học thuận hệ robot di động hai bánh vi sai 2.1.2 Động học nghịch hệ robot di động hai bánh vi sai 2.2 Giới thiệu hệ điều hành robot (ROS) 2.2.1 ROS filesystem level 10 2.2.2 ROS Computation graph level 11 2.2.3 ROS community level 14 2.3 Cài đặt ROS Jetson Nano 14 2.3.1 Cài đặt mạng cấu hình 14 2.3.2 Cài đặt ROS Melodic 15 2.3.3 Thêm biến môi trường cài đặt ROS Dependencies 15 2.3.4 Cài đặt không gian làm việc 15 ii 2.3.5 Cài đặt thư viện cần thiết gói chức Lidar 16 2.4 Triển khai mơ hình ROS 16 2.5 Bài toán travelling salesman problem (TSP) 18 2.6 Thuật tốn tìm đường tối ưu 20 2.6.1 Thuật toán Dijkstra’s 20 2.6.2 Thuật toán Greedy Best First Search 21 2.6.3 Thuật toán A* 21 2.7 Thuật toán giải toán TSP 22 2.7.1 Thuật toán Brute Force Search 22 2.7.2 Thuật toán tối ưu đàn kiến (ACO) 23 2.8 Gói Navigation stack ROS 24 CHƯƠNG 3: THIẾT KẾ PHẦN CỨNG 26 3.1 Yêu cầu thiết kế 26 3.2 Sơ đồ mô tả hệ thống 26 3.3 Thiết kế khung xe robot 27 3.3.1 Bottom_base 27 3.3.2 Middle_base 28 3.3.3 Control_base 28 3.3.4 Top_base 29 3.4 Tính tốn lựa chọn thiết bị 30 3.4.1 Máy tính nhúng Jetson Nano 30 3.4.2 Động DC JGY-370 31 3.4.3 Bánh xe 34 3.4.4 Driver điều khiển động 34 3.4.5 Nguồn cấp cho board Jetson Nano Driver L298N 35 3.4.6 Cảm biến gia tốc IMU-9250 35 3.4.7 Vi điều khiển Arduino Mega 2560 36 iii 3.4.8 Cảm biến khoảng cách laser scan RPLidar A1-M8 37 3.5 Sơ đồ nguyên lý hệ thống 38 3.6 Mơ hình robot thực tế 38 3.7 Điều khiển động dc sử dụng điều khiển PID 39 CHƯƠNG 4: THIẾT KẾ PHẦN MỀM 42 4.1 Tổng quan hệ thống 42 4.2 Vẽ đồ môi trường (mapping) 45 4.3.1 Thuật toán Gmapping 45 4.3.2 Ghép đồ (Map merge) 55 4.3 Định vị robot (localization) 59 4.4.1 Mơ hình chuyển động (motion model) 59 4.4.2 Mơ hình quan sát (observation model) 60 4.4.3 Thuật toán định vị Monte Carlo 60 4.4.4 Bộ lọc hạt thích nghi cho định vị (AMCL) 62 4.4 Bài toán TSP toán mở rộng 64 4.3.1 Xử lý occupancy grid map 64 4.3.2 Áp dụng thuật toán A* vào occupancy grid map xử lý 69 4.3.3 Áp dụng thuật toán ACO giải toán ITSP 71 4.3.4 Áp dụng thuật toán ACO giải toán IMTSP 72 4.3.5 Triển khai thuật toán A* ACO ROS 74 4.5 Bài tốn đội hình 78 4.4.1 Thuật toán Dynamic Window Approach (DWA) 78 4.4.2 Thuật toán di chuyển theo đội hình 83 CHƯƠNG 5: KẾT QUẢ MÔ PHỎNG VÀ THỰC NGHIỆM 86 5.1 Kết mô 86 5.1.1 Vẽ đồ với Gmapping 86 5.1.2 Bài toán ITSP 88 iv ĐỒ ÁN TỐT NGHIỆP 131 ĐỒ ÁN TỐT NGHIỆP 132 ĐỒ ÁN TỐT NGHIỆP 133 Code Arduino điều khiển tốc độ động đọc liệu từ IMU 9250 #include "I2Cdev.h" #include "MPU9250_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif #include ĐỒ ÁN TỐT NGHIỆP 134 #include #include #include #include #include #include #include #include MPU9250 mpu; #define OUTPUT_READABLE_QUATERNION #define INTERRUPT_PIN 12 // use pin on Arduino Uno & most boards #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) bool blinkState = false; bool chieutrai = 0; bool chieuphai = 0; // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer class SimplePID { private: float kp, ki; // Parameters float eprev, eintegral; // Storage float uprev,P_part,I_part; public: // Constructor SimplePID() : kp(1), ki(0), eprev(0.0), eintegral(0.0){} // A function to set the parameters void setParams(float kpIn, float kiIn){ ĐỒ ÁN TỐT NGHIỆP 135 kp = kpIn; ki = kiIn; } // A function to compute the control signal void evalu(int value, int target, float deltaT, int &pwr, int &dir){ // error int e = target - value; // integral // eintegral = eintegral + e*deltaT; // control signal P_part=kp*(e-eprev); I_part=0.5*ki*deltaT*(e+eprev); float u =uprev+P_part+I_part ; // motor power pwr = (int) fabs(u); if( pwr > 255 ){ pwr = 255; } // motor direction dir = 1; if(u0) { vx=vx+0.008; } if(vx interval) { ĐỒ ÁN TỐT NGHIỆP 141 previousMillis = currentMillis; // Publish tick counts to topics //left_wheel_tick_count.data = -posi[0]; left_wheel_tick_count.data = (2*PI*0.032*(-v1Filt[0]))/60; //left_wheel_tick_count.data = deltane; leftPub.publish( &left_wheel_tick_count ); //right_wheel_tick_count.data = posi[1]; right_wheel_tick_count.data = (2*PI*0.032*(v1Filt[1]))/60; rightPub.publish( &right_wheel_tick_count ); } // Read the position in an atomic block to avoid a potential misread int pos[NMOTORS]={0,0}; ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { for(int k = 0; k < NMOTORS; k++) { pos[k] = posi[k]; } } // time difference long currT = micros(); float deltaT = ((float) (currT - prevT))/( 1.0e6 ); if(deltaT>=0.005) { for(int k = 0; k < NMOTORS; k++) { velocity1[k] = (pos[k] - posPrev[k])/deltaT; posPrev[k] = pos[k]; } // Convert count/s to RPM for(int k = 0; k < NMOTORS; k++) { v1[k] = velocity1[k]/2150*60.0; } ĐỒ ÁN TỐT NGHIỆP 142 float target[NMOTORS]; float L = 0.138; float R = 0.032; float omegaR = (2*vx + va*L) / (2*R); float omegaL = (2*vx - va*L) / (2*R); int nR = omegaR * (30/PI); int nL = omegaL * (30/PI); target[0] = -nR; target[1] = nL; for(int k = 0; k < NMOTORS; k++) { int pwr, dir; // evaluate the control signal pid[k].evalu(v1Filt[k],target[k],deltaT,pwr,dir); // signal the motor setMotor(dir,pwr,pwm[k],in1[k],in2[k]); } prevT = currT; } previousMillisne=currentMillisne; } void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){ analogWrite(pwm,pwmVal); if(dir == 1){ digitalWrite(in1,HIGH); digitalWrite(in2,LOW); } else if(dir == -1){ digitalWrite(in1,LOW); digitalWrite(in2,HIGH); } ĐỒ ÁN TỐT NGHIỆP 143 else{ digitalWrite(in1,LOW); digitalWrite(in2,LOW); } } void updateEncoder1() { if (digitalRead(ENCODER1_PIN_A) == digitalRead(ENCODER1_PIN_B)) { posi[0]++; chieutrai=0; } else { posi[0] ; chieutrai=1; } } void updateEncoder2() { if (digitalRead(ENCODER2_PIN_A) == digitalRead(ENCODER2_PIN_B)) { posi[1]++; chieuphai=1; } else { posi[1] ; chieuphai=0; } } ĐỒ ÁN TỐT NGHIỆP 144 S K L 0