(Đồ án tốt nghiệp) xe tự động di chuyển dựa vào vật mốc dùng xử lý ảnh

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HCM KHOA ĐIỆN - ĐIỆN TỬ ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ - TRUYỀN THÔNG XE TỰ ĐỘNG DI CHUYỂN DỰA VÀO VẬT MỐC DÙNG XỬ LÝ ẢNH GVHD: TS.NGUYỄN THANH HẢI SVTH: PHẠM GIA TRÍ MSSV: 12141237 SVTH: HUỲNH TRỌNG TRƯỜNG MSSV: 12141247 SKL004340 Tp Hồ Chí Minh, tháng 07/2016 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 ĐIỆN TỬ - CÔNG NGHIỆP - ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ TÀI: XE TỰ ĐỘNG DI CHUYỂN DỰA VÀO VẬT MỐC DÙNG XỬ LÝ ẢNH GVH D SVTH MSSV SVTH : TS Nguyễn Thanh Hải : Phạm Gia Trí : 12141237 : Huỳnh Trọng Trường : 12141247 MSSV Tp Hồ Chí Minh – 7/2016 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 ĐIỆN TỬ - CÔNG NGHIỆP - ĐỒ ÁN TỐT NGHIỆP NGÀNH KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ TÀI: XE TỰ ĐỘNG DI CHUYỂN DỰA VÀO VẬT MỐC DÙNG XỬ LÝ ẢNH GVHD : TS Nguyễn Thanh Hải SVTH : Phạm Gia Trí MSSV : 12141237 SVTH : Huỳnh Trọng Trường MSSV : 12141247 Tp Hồ Chí Minh – 7/2016 TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN-ĐIỆN TỬ BỘ MƠN ĐIỆN TỬ CƠNG NGHIỆP CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC Tp HCM, ngày 01 tháng 04 năm 2016 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Chuyên ngành: Hệ đào tạo: Khóa: I TÊN ĐỀ TÀI: XE TỰ ĐỘNG DI CHUYỂN DỰA VÀO VẬT MỐC DÙNG XỬ LÝ ẢNH II NHIỆM VỤ Các số liệu ban đầu: - Đọc tài liệu trước - Dựa vào mơ hình robot trước Nội dung thực hiện: - Gia cơng khung xe - Thiết kế mạch công suất - Kết nối tồn hệ thống camera, máy tính, Arduino, mạch cơng suất - Tìm hiểu hệ điều hành ROS - Tìm hiểu thuật tốn PID, thuật tốn hướng rẽ - Lập trình Arduino điều khiển động dùng PID - Tìm hiểu lý thuyết xử lý ảnh, thư viện OpenCV - Tìm hiểu, viết chạy thử nghiệm chương trình dị line dùng xử lý ảnh - Tìm hiểu, viết chạy thử nghiệm chương trình nhận dạng vật mốc - Kết hợp dò line nhận dạng vật mốc gởi tín hiệu điều khiển cho Arduino định hướng xe - Chạy thử nghiệm, cân chỉnh hệ thống - Viết báo cáo luận văn - Báo cáo đề tài tốt nghiệp ii III NGÀY GIAO NHIỆM VỤ: IV NGÀY HOÀN THÀNH NHIỆM VỤ: V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: 01/04/2016 07/07/2016 TS Nguyễn Thanh Hải CÁN BỘ HƯỚNG DẪN BM ĐIỆN TỬ CÔNG NGHIỆP iii TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN-ĐIỆN TỬ BỘ MƠN ĐIỆN TỬ CƠNG NGHIỆP CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC Tp HCM, ngày 01 tháng 04 năm 2016 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Phạm Gia Trí Lớp: 12141DT1C Họ tên sinh viên 2: Huỳnh Trọng Trường Lớp: 12141DT1C Tên đề tài: XE TỰ ĐỘNG DI CHUYỂN DỰA VÀO VẬT MỐC DÙNG XỬ LÝ ẢNH Tuần/ngày Tuần Gặ Tuần Cà (U (fil Tuần Tìm Me Tuần Viế hơ Tuần Tìm Ng Ng Tuần Tìm Ar Cà Tuần 7, Ng chư Tuần Tìm xử Tuần 10 Kế Tuần 11 Tuần 12 Ch Viế Viế Tuần 13 Ch GV HƯỚNG DẪN (Ký ghi rõ họ tên) iv LỜI CAM ĐOAN Đề tài 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 Người thực đề tài Phạm Gia Trí Huỳnh Trọng Trường v LỜI CẢM ƠN Chúng em xin gởi lời cảm ơn sâu sắc đến Thầy Nguyễn Thanh Hải - Trưởng môn điện tử công nghiệp, lời cảm ơn sâu sắc đến thầy Nguyễn Tấn Như thuộc môn điện tử công nghiệp trực tiếp hướng dẫn tận tình giúp đỡ tạo điều kiện để hoàn thành tốt đề tài Chúng em xin gởi lời chân thành cảm ơn thầy cô Khoa Điện-Điện Tử tạo điều kiện tốt cho chúng em hoàn thành đề tài Chúng em gởi lời đồng cảm ơn đến bạn lớp 12141DT1 chia sẻ trao đổi kiến thức kinh nghiệm quý báu thời gian thực đề tài Cuối cùng, chúng xin gởi lời cảm ơn đến cha mẹ, người cho chúng sống để chúng ăn học phát triển ngày hôm Xin chân thành cảm ơn! Người thực đề tài Phạm Gia Trí Huỳnh Trọng Trường vi MỤC LỤ Trang bìa Nhiệm vụ đồ án Lịch trình Cam đoan Lời cảm ơn Mục lục Liệt kê hình vẽ Liệt kê bảng vẽ Tóm tắt CHƯƠNG TỔNG QUAN 1.1 Đặt vấn đề 1.2 Mục tiêu 1.3 Nội dung nghiên cứu 1.4 Giới hạn 1.5 Bố cục CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 Tổng quan Robot 2.2 Tổng quan ROS (Robot Operating System) 2.2.1Giới thiệu hệ 2.2.2 Mô hình ROS 2.3 Giới thiệu xử lý ảnh 2.3.1Hệ thống xử lý 2.3.2Những vấn đề c 2.3.3Tổng quan th 2.3.4Phép biến đổi đ 2.4 Thuật toán PID điều khiển tốc độ động 2.4.1Điều khiển 2.4.2Đặc tính điều 2.5 Thuật toán điều khiển hướng robot CHƯƠNG TÍNH TỐN VÀ THIẾT KẾ vii PHỤ LỤC set_turn_angle(); } if (count > 3) { mode = ; } return; } /********************************************************************/ void setup () { / setup the pwm input reader pinMode(PWM_l_a, OUTPUT); pinMode(PWM_l_b, OUTPUT); pinMode(PWM_r_a, OUTPUT); pinMode(PWM_r_b, OUTPUT); //Setup parameter for timer for left motor Timer3.attachInterrupt (timer_3_interrupt_for_left_motor); Timer3.start (100000); //100ms //Setup parameter for timer for right motor Timer4.attachInterrupt (timer_4_interrupt_for_right_motor); Timer4.start (100000); //100ms //Setup parameter for timer Timer5.attachInterrupt (timer_5_interrupt_for_turn_corner); Timer5.start (1000000); //1000ms = 1s //Setup PID parameters PID_leftMotor.SetMode (AUTOMATIC); PID_rightMotor.SetMode (AUTOMATIC); PID_leftMotor.SetTunings (consKp, consKi, consKd); PID_leftMotor.SetOutputLimits (-255,255); PID_rightMotor.SetTunings (consKp, consKi, consKd); PID_rightMotor.SetOutputLimits (-255,255); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC nh.initNode(); nh.subscribe(receive_data1); nh.subscribe(receive_data2); nh.advertise(send_data); } /********************************************************************/ void loop () { //Function for following line DataSend = 0; if (mode == 0) { selectMover(); } //Update the PID value //Update the value of the current setpoint of the left and right motor // For left motor if (leftPIDOutput < 0) { digitalWrite(PWM_l_b,LOW); analogWrite(PWM_l_a, abs(leftPIDOutput)); } else { if (leftPIDOutput > 0) { digitalWrite(PWM_l_a,LOW); analogWrite(PWM_l_b, leftPIDOutput); } else { digitalWrite(PWM_l_a,LOW); digitalWrite(PWM_l_b,LOW); } } //For right motor BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC if (rightPIDOutput < 0) { digitalWrite(PWM_r_b,LOW); analogWrite(PWM_r_a, abs(rightPIDOutput)); } else { if (rightPIDOutput > 0) { digitalWrite(PWM_r_a,LOW); analogWrite(PWM_r_b, rightPIDOutput); } else { digitalWrite(PWM_r_a,LOW); digitalWrite(PWM_r_b,LOW); } } //Spin ROS for updating system data_send.data = DataSend; send_data.publish(&data_send); nh.spinOnce(); } /*****************************Followingline****************************/ void selectMover() { if (status_robot == 1) robotMover( ); // TH lech line else if (status_robot == -1) robotMover( ); else if (status_robot == 0) robotMover( ); // TH line giua else if (status_robot == 2) { // TH dung nga robotMover( ); mode = 1; } else robotMover( ); // TH dung ko line DataSend = 1; // cho phep doc vat mau BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC } void robotMover (int action) { switch (action) { case 0:// không di chuyển motorControlSpeed(0, 0); break; case 1:// thẳng motorControlSpeed(1, 1); break; case 2:// lùi lại motorControlSpeed(-1,-1); break; case 3:// rẽ trái motorControlSpeed(1.0,0.5); break; case 4:// rẽ phải motorControlSpeed(0.5,1.0 ); break; default: action = 0; } } void motorControlSpeed(double speedR, double speedL) { leftSetPoint = speedL; rightSetPoint = speedR; } void set_turn_angle() BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC { double vr, vl, v, L = 44, R = 4.5; //centimet double radian_angle; sec_itr = 2; radian_angle = (turn_angle * pi) / 180; radian_angle = radian_angle / sec_itr; if (radian_angle > 3) vr = vl = 0; else if (radian_angle > 0) { vl = 0; v = radian_angle * L / 2; vr = (2 * v + radian_angle * L) / ((2 * R) * (2 * pi)); } else if (radian_angle < 0) { vr = 0; v = - radian_angle * L / 2; vl = (2 * v - radian_angle * L) / ((2 * R) * (2 * pi)); } else { v = 18; vr = (2 * v + radian_angle * L) / ((2 * R) * (2 * pi)); vl = (2 * v - radian_angle * L) / ((2 * R) * (2 * pi)); } rightSetPoint = vr; leftSetPoint = vl; } - Code chương trình xử lý máy tính: #include #include #include "opencv2/core/core.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC #include #include #include #include #include #include #include #include #include using namespace cv; using namespace std; float pos_objx, angleData = 360; SiftFeatureDetector detector; SiftDescriptorExtractor extractor; FlannBasedMatcher matcher; Mat img_obj, img_sceneObj, img_sceneLine; Mat img_sceneObj_g; Mat img_object1, img_object_g1; Mat img_object2, img_object_g2; Mat img_object3, img_object_g3; vector keypoints_object1; vector keypoints_object2; vector keypoints_object3; vector keypoints_scene; Mat descriptors_object1; Mat descriptors_object2; Mat descriptors_object3; Mat descriptors_scene; Mat img_chose; vector keypoints_chose; std::vector< DMatch > good_matchesChose; void detectObject() { BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC cvtColor (img_sceneObj, img_sceneObj_g, CV_BGR2GRAY); detector.detect (img_sceneObj_g, keypoints_scene); extractor.compute (img_sceneObj_g, keypoints_scene, descriptors_scene); drawKeypoints(img_sceneObj,keypoints_scene,img_sceneObj,Scalar::all(1),DrawMatchesFlags::DRAW_RICH_KEYPOINTS); std::vector< DMatch > matches1; std::vector< DMatch > matches2; std::vector< DMatch > matches3; matcher.match( descriptors_object1, descriptors_scene, matches1 ); matcher.match( descriptors_object2, descriptors_scene, matches2 ); matcher.match( descriptors_object3, descriptors_scene, matches3 ); double min_dist1 = 70; for( int i = 0; i < descriptors_object1.rows; i++ ) { double dist = matches1[i].distance; if( dist < min_dist1 ) min_dist1 = dist; } std::vector< DMatch > good_matches1; for( int i = 0; i < descriptors_object1.rows; i++ ) { if( matches1[i].distance < 3.0*min_dist1 ) { good_matches1.push_back( matches1[i]); } } double min_dist2 = 60; for( int i = 0; i < descriptors_object2.rows; i++ ) { double dist = matches2[i].distance; if( dist < min_dist2 ) min_dist2 = dist; } std::vector< DMatch > good_matches2; for( int i = 0; i < descriptors_object2.rows; i++ ) { if( matches2[i].distance < 3.0*min_dist2 ) { good_matches2.push_back( matches2[i]); } } double min_dist3 = 60; for( int i = 0; i < descriptors_object3.rows; i++ ) { double dist = matches3[i].distance; BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC if( dist < min_dist3 ) min_dist3 = dist; } std::vector< DMatch > good_matches3; for( int i = 0; i < descriptors_object3.rows; i++ ) { if( matches3[i].distance < 3.0*min_dist3 ) { good_matches3.push_back( matches3[i]); } } Mat img_displayed; Mat img_chose; vector keypoints_chose; std::vector< DMatch > good_matchesChose; // tim max good_match good_matchesChose = good_matches1; if (good_matches2.size() > good_matchesChose.size()) { good_matchesChose = good_matches2; } else if (good_matches3.size() > good_matchesChose.size()) { good_matchesChose = good_matches3; } // ve cac duong thang va xoa cac diem khong tot if (good_matchesChose.size() == good_matches1.size()) { img_chose = img_object1; keypoints_chose = keypoints_object1; } else if (good_matchesChose.size() == good_matches2.size()) { img_chose = img_object2; keypoints_chose = keypoints_object2; } else { img_chose = img_object3; keypoints_chose = keypoints_object3; } drawMatches( img_chose, keypoints_chose, img_sceneObj, keypoints_scene, good_matchesChose, img_displayed, Scalar::all(-1), Scalar::all(-1), vector(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); cout