(Luận văn thạc sĩ) định vị và tránh vật cản cho robot tự hành sử dụng camera kinect

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 LUẬN VĂN THẠC SĨ TRẦN VĂN LUÂN ĐỊNH VỊ VÀ TRÁNH VẬT CẢN CHO ROBOT TỰ HÀNH SỬ DỤNG CAMERA KINECT NGÀNH: KỸ THUẬT ĐIỆN TỬ - 605270 S K C0 4 Tp Hồ Chí Minh, tháng 10/2014 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 LUẬN VĂN THẠC SĨ TRẦN VĂN LUÂN ĐỊNH VỊ VÀ TRÁNH VẬT CẢN CHO ROBOT TỰ HÀNH SỬ DỤNG CAMERA KINECT NGÀNH: KỸ THUẬT ĐIỆN TỬ- 605270 Hướng dẫn khoa học: TS NGƠ VĂN THUN Tp Hồ Chí Minh, tháng 10/2014 LÝ LỊCH KHOA HỌC I LÝ LỊCH SƠ LƯỢC: Họ & tên: Trần Văn Luân Giới tính: Nam Ngày, tháng, năm sinh: 24-07-1988 Nơi sinh: Đồng Nai Quê quán: Quảng Trị Dân tộc: Kinh Chỗ riêng địa liên lạc: ấp 1, xã Bàu Lâm, huyện Xuyên Mộc, tỉnh Bà Rịa – Vũng Tàu Điện thoại quan: Điện thoại nhà riêng: 0933449605 E-mail: tranvanluan07118@gmail.com II QUÁ TRÌNH ĐÀO TẠO: Trung học chuyên nghiệp: Hệ đào tạo: Thời gian đào tạo từ …/…đến …/ … Nơi học (trường, thành phố): Ngành học: Đại học: Hệ đào tạo: Chính quy Thời gian đào tạo từ 10/2007 đến 01/ 2012 Nơi học (trường, thành phố): Đại học Sư Phạm Kỹ Thuật thành phố Hồ Chí Minh Ngành học: CN Điện Tự Động Tên đồ án, luận án môn thi tốt nghiệp: Thiết kế điều khiển hệ thống lắc ngược quay Ngày & nơi bảo vệ đồ án, luận án thi tốt nghiệp: Đại học Sư Phạm Kỹ Thuật thành phố Hồ Chí Minh Người hướng dẫn: TS Ngơ Văn Thun i III Q TRÌNH CƠNG TÁC CHUYÊN MÔN KỂ TỪ KHI TỐT NGHIỆP ĐẠI HỌC: Thời gian Nơi công tác Công việc đảm nhiệm 2/2012 Công Ty TNHH Nidec Copal VN Kĩ sư thiết kế điện 3/2013 Trường CĐ Kỹ Thuật Cao Thắng TP.HCM Giảng viên ii LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu tơi với hướng dẫn Ts Ngô Văn Thuyên Các số liệu, kết nêu luận văn trung thực chưa cơng bố cơng trình khác Tp Hồ Chí Minh, ngày tháng năm 2014 (Ký tên ghi rõ họ tên) Trần Văn Luân iii LỜI CẢM TẠ Đầu tiên, xin gửi lời cám ơn chân thành đến TS Ngô Văn Thuyên Thầy trực tiếp hướng dẫn tơi cách tận tình chu đáo từ xây dựng lúc thực hoàn thành đề tài Trong trình làm đề tài này, thầy ln theo sát tiến trình thực tơi có định hướng giải vấn đề khó khăn gặp phải Đồng thời, xin chân thành gửi lời cảm ơn đến tồn thể q Thầy Cơ trường Đại học Sư Phạm Kỹ Thuật TP Hồ Chí Minh giảng dạy, hướng dẫn tạo điều kiện, môi trường học tập tốt cho tơi Bên cạnh đó, gia đình ln chỗ dựa nguồn động viên vô to lớn tiếp sức cho tơi suốt q trình nghiên cứu đề tài Xin chân thành cảm ơn! Tp Hồ Chí Minh, ngày tháng năm 2014 (Ký tên ghi rõ họ tên) Trần Văn Luân iv TÓM TẮT Định vị, tránh vật cản tìm đường tối ưu cho robot ba yêu cầu cần thiết cho robot tự hành Robot cần đồ để thực tìm đường tối ưu Khi vị trí robot khơng thể xác định, robot di chuyển để tìm vật mốc định vị Định vị toán điều khiển di chuyển robot di động Nếu robot khơng biết vị trí mơi trường robot khó đưa định thực việc tiếp theo.Một robot cần phải có vài nhận biết vị trí mơi trường để hành động cách xác Đề tài trình bày thuật tốn định vị điều khiển di chuyển cho robot di động sử dụng camera vật mốc Phương pháp hình học sử dụng để xác định vị trí robot từ tọa độ vật mốc mơi trường Thuật tốn tìm đường D* áp dụng để tìm đường tối ưu cho robot Giải thuật điều hướng lai ghép sử dụng để giúp robot điều hướng di chuyển theo đường tối ưu mà robot tìm Cảm biến sử dụng cho việc nhận dạng xác định khoảng cách từ robot đến vật mốc camera Kinect Để nhận dạng xác định tâm vật mốc thuật toán định vị sử dụng hàm thị giác máy tính thư viện mã nguồn mở OpenCV, thuật toán SURF Cảm biến IMU sử dụng để xác dịnh góc lệch robot Bộ lọc Kalman sử dụng cho việc kết hợp thông tin nội thông tin đo đạc từ bên ngồi để xác định vị trí cho robot di động Kết thực nghiệm cho thấy robot có di chuyển tới mục tiêu với phương pháp trình bày Điều cho thấy phương pháp đề xuất đáng tin cậy hiệu v ABSTRACT Localization, obstacle avoidance and path planning are three fundamental problems of robotic Robots need a map to perform actions like path-planing When positioning robot is not available, robot will move to research the landmark for localization Localization is one of the fundamental problems in mobile robot navigation If a robot does not know where it is, it can be difficult to determine what to next The robot will most likely need to have at least some idea of where it is to be able to operate and act successfully This thesis presented localization and navigation algorithm for mobile robot using camera and landmarks The geometric method was used to determine the position of the robot from the coordinate of landmarks in the environment The path finding algorithm D* was applied to find out positions which the robot can move from the start point to the goal Integrated navigation algorithm is used to help the robot moving in the direction of the optimal path that the robot has found The algorithm has been implemented on a mobile robot platform with two differential drive wheels The sensor used for localization and determination distance from robot to landmarks was a Kinect camera To be able to recognize and determine the center position of landmarks, the localization algorithm utilized computer vision functions in OpenCV Library, SURF algorithm A IMU sensor used to determine bias of robot The Kalman filter combined available internal and external sensory informations to determine position of the mobile robot The experimental results showed that the robot could control moving to the target with methods above Trajectory of robot was close with the ideal line and the ground truth vi M CL C Trang Lý l ch cá nhân i L iii C m t iv Tóm t t v M c l c vii Danh sách ch vi t t t x Danh sách hình xi Danh sách b ng xiv Ch ng T NG QUAN 1.1 T ng quan chung v l nh v c nghiên c u 1.2 M tài 1.3 Ph m vi nghiên c u 1.4 Ph ng pháp th c hi n 1.5 N i dung c Ch tài LÝ THUY T nh v ng 2.1.1 Gi i thi u nh v d mc nh v d a vào nh n d ng v t m c b 2.2 B ng th ng b cho robot 10 2.2.1 B d ng Topo 10 2.2.2 B d ng hình h c 2D 11 2.2.3 B d ng m ng 2D 11 2.3 Gi i thu t tìm i u 13 2.3.1 Gi i thu t Dijkstra 13 2.3.2 Thu t toán A-star 15 vii 2.3.3 Gi i thu t D-star 16 2.4 Các ph ng pháp tránh v t c n 17 2.4.1 Ph ng pháp Bug2 18 2.4.2 Ph ng pháp Vecto Field Histogram (VFH) 18 2.4.3 Ph ng pháp APF (Artificial Potential Field) 19 2.5 Camera Kinect 22 2.5.1 Gi i thi u 22 sâu 23 2.5.3 Ph ng pháp phát hi n v t c n v i Kinect 25 2.6 C m bi n IMU 30 2.6.1 Gi i thi u 30 2.6.2 Các nguyên nhân gây sai s c m bi n IMU 31 Ch ng THI T K GI I THU U KHI N ROBOT 33 3.1 Gi i thu u n robot 33 3.2 Gi i thu uh 3.3 Ph ng lai ghép (Integrated navigation algorithm) 34 uh ng tr ng th 36 3.3.1 Tr ng l c hút 36 3.3.2 Tr ng l y 37 3.3.3 L c d n robot 38 nh v n t c lái vi sai cho robot v i tr Ch ng ng th 40 NH V CHO ROBOT TRÊN B 2D 42 4.1 Ph nh v cho robot 42 4.2 Ph ng pháp nh n d ng SURF 43 4.2.1 Phát hi m n i b t 43 4.2.2 Mô t m 44 4.2.3 So sánh b mô t 46 4.3 Nh n d ng v t m c v i Kinect dùng gi i thu t SURF 46 4.4 B l c Kalman 46 Ch ng K T QU THI CÔNG VÀ TH C NGHI M 50 viii PHỤ LỤC // Chương trình phát vật ứng dụng thư viện PCL cho camera Kinect void CRobotDlg::OnBnClickedButtonRun() { UpdateData(TRUE); m_sliderTilt.SetPos(CL_MOTOR_MIN_ANGLE); setMotorAngle(-15000); Sleep(5); pass_.setFilterFieldName ("z"); pass_.setFilterLimits (0.4, 1.5); grid_.setLeafSize (0.03, 0.03, 0.03); seg_.setOptimizeCoefficients (true); seg_.setModelType (pcl::SACMODEL_PLANE); seg_.setMethodType (pcl::SAC_RANSAC); seg_.setMaxIterations (2000); seg_.setDistanceThreshold (0.02); //2cm ec.setClusterTolerance (0.05); // 3cm ec.setMinClusterSize (20); ec.setMaxClusterSize (1000); KinectThread(); } void CRobotDlg::KinectThread() { local local_; std::string device_id(""); pcl::Grabber* intf = new pcl::OpenNIGrabber (device_id, pcl::OpenNIGrabber:: OpenNI_QVGA_30Hz,pcl::OpenNIGrabber:: OpenNI_QVGA_30Hz); boost::function cloud_cb = boost::bind(&local::cloud_callback,&local_, _1); boost::signals2::connection cloud_connection = intf->registerCallback (cloud_cb); intf->start (); while (!viewer->wasStopped()) { if (local_.cloud_) { FPS_CALC ("drawing"); local_.getLatestCloud(); if(planeWall) SetDlgItemText(IDC_EDIT_OBS, "Yes"); else SetDlgItemText(IDC_EDIT_OBS, "No"); std::stringstream ss; ss begin(); it != temp_cloud>end(); ++it) { float x = it->x; float y = (it->y)*cosTheta - (it->z)*sinTheta; float z = (it->y)*sinTheta + (it->z)*cosTheta + 0.09f; y = (y - 0.60f); CloudType point; point.x = x; point.y = - y; point.z = z; cloud_rotated->push_back(point); } pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); seg_.setInputCloud (cloud_rotated); seg_.segment (*inliers, *coefficients); extract_.setInputCloud (cloud_rotated); extract_.setIndices (inliers); extract_.setNegative (planeWall); extract_.filter (*cloud_plane); pcl::getMinMax3D(*cloud_plane, minPlane, maxPlane); if((maxPlane.y > 0.15)&&(maxPlane.zindices.begin (); pit != it->indices.end (); ++pit) { cloud_cluster->points.push_back (temp_cloud2>points[*pit]); cloud_cluster_temp->points.push_back (temp_cloud2>points[*pit]); } pcl::getMinMax3D (*cloud_cluster_temp, min_pt_temp, max_pt_temp); disTemp = min_pt_temp.z; sizeMat = matObsInfo->size(); matObsInfo[0].resize(sizeMat + 1, CloudType(0, 0, 0)); matObsInfo[1].resize(sizeMat + 1, CloudType(0, 0, 0)); matObsInfo[0][countNum] = min_pt_temp; matObsInfo[1][countNum] = max_pt_temp; Trang 68 if(disTemp < closest_z ) { closest_z = disTemp; *cloud_cluster_closest = *cloud_cluster_temp; } countNum ++; } numObs = countNum; viewer->setBackgroundColor (0, 0, 0); viewer->removePointCloud("cloud 1"); viewer->removePointCloud("cloud 2"); viewer->removePointCloud("cloud 3"); viewer->removePointCloud("cloud 4"); viewer->removeShape("Line"); if(countNum) { pcl::getMinMax3D (*cloud_cluster_closest, min_pt, max_pt); viewer->addLine(max_pt, min_pt, 255, 0, 255, "Line"); std::stringstream ss1, ss2; ss1 -0.23 && matObsInfo[0][i].x < 0.23)) || (matObsInfo[0][i].x < -0.23 && matObsInfo[1][i].x > 0.23)) { if(matObsInfo[0][i].z < 1.2) checkPath++; if(matObsInfo[0][i].z < RANGEOBSTACLE)checkObstacle ++; disOnPath = matObsInfo[0][i].z; if(closestOnPath > disOnPath) { closestOnPath = disOnPath; minOnPathTemp = matObsInfo[0][i]; maxOnPathTemp = matObsInfo[1][i]; } } } if(checkObstacle) obstacle_flag = true; else obstacle_flag = false; if(checkPath) { freePath = false; minOnPath = minOnPathTemp; maxOnPath = maxOnPathTemp; } else Trang 69 { freePath = true; minOnPath.x = 0; minOnPath.y = 0; minOnPath.z = 0; maxOnPath.x = 0; maxOnPath.y = 0; maxOnPath.z = 0; } pcl::visualization::PointCloudColorHandlerCustom single_color1(cloud_cluster, 0, 255, 0); viewer->addPointCloud (cloud_cluster, single_color1, "cloud 1"); pcl::visualization::PointCloudColorHandlerCustom single_color2(cloud_cluster_closest, 255, 0, 0); viewer->addPointCloud (cloud_cluster_closest, single_color2, "cloud 2" ); pcl::visualization::PointCloudColorHandlerCustom single_color3(cloud_plane, 0, 0, 255); viewer->addPointCloud (cloud_floor, single_color3, "cloud 3" ); if(planeWall) { pcl::visualization::PointCloudColorHandlerCustom single_color4(cloud_plane, 255, 255, 0); viewer->addPointCloud (cloud_wall, single_color4, "cloud 4" ); } viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud 1"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud 2"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud 3"); if(planeWall) viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud 4"); viewer->spinOnce(); viewer->initCameraParameters (); return (cloud_cluster); } }; Trang 70 PHỤ LỤC // Chương trình điều hướng Robot với trường void CRobotDlg::Navigation_APF(int x_robot,int y_robot,int x_obs, int y_obs,int x_target,int y_target,int Rv) { double phiRep, phiAtt,temp1,temp2,tu,mau,Frep,Fatt; double Krep =0.0007; double Katt =0.005; double dxrep = x_obs-x_robot ; double dyrep = y_obs-y_robot ; phiRep= atan(dyrep/dxrep)+180.0; double dxatt= x_target - x_robot ; double dyatt= x_target - y_robot ; phiAtt= atan(dyatt/dxatt); Frep = Krep*(1/temp2-1/Rv)*(1/temp2-1/Rv); Fatt= Katt*temp1; temp1= sqrt(dxatt*dxatt+dyatt*dyatt); temp2= sqrt (dxrep*dxrep+dyrep*dyrep); tu =Fatt*sin(phiAtt)+ Frep*sin(phiRep); mau =Fatt*cos(phiAtt)+ Frep*cos(phiRep); desired_angle=atan(tu/mau)*180/PI; } Trang 71 PHỤ LỤC // Chương trình nhận dạng vật mốc dùng phương pháp SURF ứng dụng thư viện OpenCV vector x_point_detect; vector y_point_detect; int x_mid_point_detect; int y_mid_point_detect; int x_mid_point_surf ; int y_mid_point_surf ; Context context; ImageGenerator imageGen; const Size frameSize(640, 480); Mat bgrMat(frameSize, CV_8UC1); vt_detect localizatin_surf(Mat input_image_detect) vt_detect vtor_detect; vtor_detect.n=0; x_point_detect.clear(); y_point_detect.clear(); // Initialize context object XnStatus nRetVal = XN_STATUS_OK; nRetVal = context.Init(); { // Defauly output mode XnMapOutputMode outputMode; outputMode.nXRes = 640; outputMode.nYRes = 480; outputMode.nFPS = 30; // Create an imageGenerator node nRetVal = imageGen.Create(context); nRetVal = imageGen.SetMapOutputMode(outputMode); nRetVal = imageGen.GetMirrorCap().SetMirror(true); // Start generating nRetVal = context.StartGeneratingAll(); // Get the first frame from the camera Mat mat(frameSize,CV_8UC1, (unsigned char*)imageGen.GetImageMap()); // To hold each frame IplImage *frame = cvCreateImage(frameSize, 8, ) ; Mat img_object ; Mat img_scene ; int Ok_detect = ; while(!Ok_detect){ Mat mat(frameSize, CV_8UC1,(unsigned char*) imageGen.GetImageMap()); cvtColor(mat, bgrMat, CV_GRAY2RGB); frame = cvCloneImage(&(IplImage)bgrMat); cvSaveImage ( "image_detect.jpg" , frame); cvtColor(input_image_detect,img_object,CV_RGB2GRAY ); cvtColor(bgrMat,img_scene,CV_RGB2GRAY ); if( !img_object.data || !img_scene.data) { std::cout max_dist ) max_dist = dist; std::vector< DMatch > good_matches; for( int i = 0; i < descriptors_object.rows; i++ ) { if( matches[i].distance < 3*min_dist ){ good_matches.push_back( matches[i]); } } Mat img_matches; drawMatches( img_object, keypoints_object, img_scene, keypoints_scene,good_matches, img_matches, Scalar::all(-1), Scalar::all(1),vector(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); std::vector obj; std::vector scene; for( size_t i = 0; i < good_matches.size(); i++ ) { obj.push_back( keypoints_object[ good_matches[i].queryIdx].pt ); scene.push_back( keypoints_scene[ good_matches[i].trainIdx].pt ); } Mat H = findHomography( obj, scene, RANSAC ); std::vector obj_corners(4); obj_corners[0] = Point(0,0); obj_corners[1] = Point( img_object.cols, ); obj_corners[2] = Point( img_object.cols, img_object.rows ); obj_corners[3] = Point( 0, img_object.rows ); std::vector obj_corners_detect(4); obj_corners_detect[0] = Point(0,0); Trang 73 obj_corners_detect[1] = Point( img_scene.cols, ); obj_corners_detect[2] = Point( img_scene.cols,img_scene.rows ); obj_corners_detect[3] = Point( 0,img_scene.rows ); for (int i=0; i