(Đồ án hcmute) 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 SKL 0 4 Tp Hồ Chí Minh, tháng 07/2016 an 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 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 an 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 an 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: Phạm Gia Trí Huỳnh Trọng Trường Kỹ thuật Điện - Điện tử Đại học quy 2012 MSSV: 12141237 MSSV: 12141247 Mã ngành: 52510302 Mã hệ: Lớp: 12141DT1C 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 an 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 an 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 MSSV: 12141237 Họ tên sinh viên 2: Huỳnh Trọng Trường Lớp: 12141DT1C MSSV: 12141247 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 Xác nhận GVHD Nội dung Tuần Tuần Gặp giáo viên hướng dẫn, chọn đề tài Cài đặt chương trình cơng cụ để sử dụng cho đề tài (Ubuntu 14.04, ROS Indigo) Tìm hiểu ROS phần (filesystem, stack, package, service ) Tuần Tìm hiểu ROS phần nâng cao (Computation Graph, Node, Message, Topic, Bags ) Viết số chương trình ví dụ sử dụng ROS để làm quen với cơng cụ ROS Tìm hiểu encoder, pwm, thuật toán PID Nghiên cứu thuật toán điều khiển hướng robot Nghiên cứu, viết chương trình Arduino điều khiển động Tuần Tuần Tuần Tuần 7, Tuần Tuần 10 Tuần 11 Tìm hiểu giao tiếp node máy tính với node Arduino Cài đặt gói hỗ trợ driver cho camera, chụp ảnh vật mẫu Nghiên cứu thuật tốn tìm điểm đặc trưng SIFT Viết chương trình xử lý ảnh vật mẫu Tìm hiểu phương pháp xử lý ảnh dị line Viết chương trình xử lý ảnh dị line Tuần 12 Kết hợp chương trình Chạy thử mơ hình robot Chỉnh sửa hồn thiện chương trình Viết luận văn Viết luận văn Tuần 13 Chỉnh sửa hoàn thành luận văn GV HƯỚNG DẪN (Ký ghi rõ họ tên) iv an 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 an (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an vi (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh MỤC LỤC Trang bìa i Nhiệm vụ đồ án ii Lịch trình iv Cam đoan v Lời cảm ơn vi Mục lục vii Liệt kê hình vẽ ix Liệt kê bảng vẽ xii Tóm tắt xiii 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.1 Giới thiệu hệ điều hành ROS 2.2.2 Mơ hình ROS 2.3 Giới thiệu xử lý ảnh 10 2.3.1 Hệ thống xử lý ảnh 10 2.3.2 Những vấn đề xử lý ảnh 11 2.3.3 Tổng quan thư viện OpenCV 14 2.3.4 Phép biến đổi đặc trưng bất biến tỉ lệ – SIFT xử lý ảnh 17 2.4 Thuật toán PID điều khiển tốc độ động 24 2.4.1 Điều khiển khâu vi tích phân tỉ lệ PID 24 2.4.2 Đặc tính điều khiển P, I, D 25 2.5 Thuật toán điều khiển hướng robot 27 CHƯƠNG TÍNH TỐN VÀ THIẾT KẾ 30 vii (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 3.1 Giới thiệu 30 3.2 Thiết kế sơ đồ khối 30 3.2.1 Khối nguồn 30 3.2.2 Khối camera 32 3.2.3 Khối PC xử lý liệu ảnh 32 3.2.4 Khối xử lý trung tâm 34 3.2.5 Khối chấp hành 36 3.3 Sơ đồ nguyên lý mạch điện hệ thống 43 CHƯƠNG THI CÔNG HỆ THỐNG 44 4.1 Thi công phần mạch điện 44 4.2 Thi công phần khung robot 45 4.2.1 Khung robot 45 4.2.2 Các phận nhựa 46 4.2.3 Linh kiện khí 47 4.2.4 Bố trí phận robot 48 4.3 Lập trình hệ thống 48 4.3.1 Lưu đồ giải thuật 48 4.3.2 Phần mềm lập trình Arduino 61 4.4 Hướng dẫn vận hành mơ hình robot 68 CHƯƠNG KẾT QUẢ_NHẬN XÉT_ĐÁNH GIÁ 72 5.1 Kết 72 5.1.1 Mơ hình Robot 72 5.1.2 Hoạt động Robot 72 5.2 Nhận xét, đánh giá 74 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 77 6.1 Kết luận 77 6.2 Hướng phát triển 77 TÀI LIỆU THAM KHẢO 78 PHỤ LỤC 80 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an viii (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 83 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 84 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 85 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 86 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 87 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 88 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh an 89 (Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh(Do.an.hcmute).xe.tu.dong.di.chuyen.dua.vao.vat.moc.dung.xu.ly.anh 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