(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 Thuyên i LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu 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 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 5.1 K t qu thi cơng mơ hình robot di 5.2 T ng 50 u n robot 51 5.3 K t qu t o b d ng m ng 2D cho robot 52 5.4 K t qu ph nh v 53 5.5 K t qu tránh v t c n 56 5.5.1 K t qu mô ph ng tránh v t c n 56 5.5.2 K t qu th c nghi m tránh v t c n 58 Ch ng K T LU N 62 6.1 K t qu 6.2 H c 62 ng phát tri n c tài 62 TÀI LI U THAM KH O 63 PH L C 65 ix T ng quan T NG QUAN 1.1 T nh v ên c Ngày này, th gi i l nh v c nghiên c ng, c ã phát tri n lo i robot d ch v chuyên dùng ph c v qu c phòng, an ninh, hay robot ch bóng … V i s phát tri c kh e, robot d ch v tb nv nh v có th ng di chuy ,v ng r ng ng thông minh hi c thi t k robot có th di chuy n xây d ng b ã v ng xung ng có th di chuy n theo m nh l nh ho c c ch i c d ng, ví d ch ng tránh v t nh v b l c ho c c v trí hi n t i [1] [3] quanh [4] [5] ng ngày ng ph c t n có th t l n robot ln c n có m t b ình ì robot di di chuy c ã tr nên ph bi n c s di chuy n c a v t th mơi có th d n nh n bóng M t s c s di chuy n c a qu bóng ã robot tr c thi t k ng d ng ph bi n (Rehabilitation robot), robot y t (Medical Robot), robot hút b i (Robot Vacuum), robot giúp viêc nhà (Home Assistant Robot AR), robot h tr i b nh t t (daily life mobile robot assistant Care-O-bot III) Hình 1.1 Trong kho ã có nh ng ho ã có nh liên quan nhi u tin c m bi l c ti nv u ch v c v robot Các nghiên c u v robot ng h ng l c h c, thi t k qu u n phát tri u c ta o, x lý thông c bi t, u n truy n th ng u u ng nghiên c u n s d ng m Trang t ph , thu t T ng quan u n t thích nghi c áp d ng t i h c c ã c p nghiên c u c hay vi n Khoa h c công ngh Cùng v i k t qu v th c nghi m, nhi u công trình nghiên c u khoa h c v ã c cơng b t p chí khoa h c k thu c ho c báo cáo t i H i ngh khoa h c qu c t v robot Tuy nhiên ho ng nghiên c u c ta v n nh ng h n ch nh nh, nghiên c u v robot ng d ng th c t nhi ph n mô ph n ch d ng l i c thi t k phát tri robot thơng minh nh m ph c v tồn có th phát tri m ng l i ng d ng v i V m t lý thuy t, c c lo àn h t ng k thu t cịn h n ch , ngành cơng nghi p h tr cho công nghi n, thi t b ki m tra, ki quy mô th nh ch ng h n h d ng Vì v phát tri n lo i robot ng hi n v n d ng l i vi c phát tri n lo i robot công nghi p ti p n ng phát tri n robot ng cho s phát tri n n n công nghi p robot ph c v ng ng d ng th nhi th mn ib th ng d ng k thu t x lý ng th i t o ti c ta h c tr ng K thu t mang i nh ng k thu t c m bi n truy n th ng có giúp camera tr thành m t c a robot Thông qua k thu t này, robot có th phát hi n v t c n có th tránh v t c n m t cách an toàn nh t [3] m ng nghiên c u v ng th i robot có th ng u ng di chuy n nh v bi c robot có th di chuy n ng ph c t p r ng l n vi c xây d ng gi i thu u n t robot r t c n thi t M t yêu c u t t ph ng th c th kh nh ho c b c p cho robot [3] quan tr n c a robot t iv im ng, tránh v t c n c di chuy n m t cách an toàn t Trang ct nh v cung m t công vi c 1] [3] [5] Khi T ng quan có th di chuy nh th c hi n nhi m v khác mà không va ch m tr ng i t tránh v t c n cho robot t hành ng [4] nh v t yêu c u c n nghiên c u cho robot di ng (a) Robot hút b i (b) Robot giúp viêc (c) Robot h tr nhà i b nh t t Hình 1.1 M t vài robot thông minh h tr vi c nhà 1.2 M ài Xây d ng mơ hình robot t hành ph i có kh ng c v t c n có qng kh ng tìm M t c ph c v n d ng m ng 2D ng thơng minh, có th i, giúp vi ho c làm m t s công vi u ng di chuy n Ngồi ra, robot có tài phát tri n m t robot t s d ng l nh v có th c ng d n du khách ịng ho c t e, hút b i, lau nhà, … 1.3 Ph ên c Ph m vi nghiên c u c tài gi i h n nh ng ng d ng nhà Lu n ch y u t p trung nghiên c u v nh v tránh v t c n cho robot Quá trình thu th p, x lý d li u Camera Kinect ki m nghi m thu t toán hi n máy tính 1.4 tài là: Trang c th c 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