本发明公开了一种机器人自主定位与导航方法及系统。其中，方法的实现包括：采集机器人在当前位置的RGB图像；将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走。本发明仅仅利用了视觉传感器完成了机器人的自主定位与导航，设备结构简单、成本较低、操作简便、路径规划实时性高，可用于无人驾驶、室内定位导航等多个领域。
1.一种机器人自主定位与导航方法，其特征在于，包括：
(2)将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
(3)在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走；
(2.1)使用聚类算法对所述室内三维点云地图中SIFT特征点的描述子进行聚类，得到词汇树；
(2.2)获取机器人当前位置的第一RGB图像和机器人接收到的第二RGB图像，并对所述第一RGB图像以及所述第二RGB图像进行SIFT特征点提取，然后使用聚类算法得到视觉词汇；
(2.3)进行2D-3D检索，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，根据该2D特征点对应的视觉词汇，在所述词汇树中查找与该2D特征点具有相同单词的候选三维匹配点；
(2.4)进行2D-3D匹配，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，计算该2D特征点与对应的候选三维匹配点在特征描述子空间上的距离；
(2.5)找到与该2D特征点对应的候选三维匹配点中在特征描述子空间上距离最近和距离次近的两个三维点，若最近距离与次近距离的比值小于第一预设阈值，则该2D特征点与在特征描述子空间上距离该2D特征点距离最近的目标三维点相匹配；
(2.6)对于与该2D特征点匹配的目标三维点，查找所述目标三维点在欧式空间上的若干个最近邻三维点；
(2.7)对每一个最近邻三维点，进行3D-2D匹配，在2D图像中查找与该最近邻三维点具有相同词汇的候选2D特征点，计算该最近邻三维点与候选2D特征点在特征描述子空间上的距离；
(2.8)找到与该最近邻三维点对应的候选2D特征点中在特征描述子空间上距离最近和距离次近的两个2D特征点，若最近距离与次近距离的比值小于第二预设阈值，则该最近邻三维点与在特征描述子空间上距离该最近邻三维点距离最近的目标2D特征点相匹配；
(2.9)检查当前找到的三维点和二维点的匹配个数，若达到第三预设阈值则执行步骤(2.10)，否则，执行步骤(2.3)；
(2.10)通过得到的3D与2D的匹配对计算得到机器人当前在室内三维点云地图中的位置和目的地在室内三维点云地图中的位置，进而得到八叉树地图内的机器人当前所处的位置和目的地的位置。
2.根据权利要求1所述的方法，其特征在于，所述室内三维点云地图按照如下方式构建：采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，机器人的行进路线为闭合路线；
利用所述RGB图像和深度图像构建室内三维点云地图。
3.根据权利要求2所述的方法，其特征在于，所述利用所述RGB图像和深度图像构建室内三维点云地图，具体包括：(S1)将获取的起始帧图像作为第一帧图像，将与所述起始帧图像相邻的下一帧图像作为第二帧图像；
(S2)提取所述第一帧图像与所述第二帧图像的特征点，分别计算所述第一帧图像与所述第二帧图像上的关键点，针对关键点周围的像素点，计算所述第一帧图像的特征描述子以及所述第二帧图像的特征描述子；
(S3)采用所述第一帧图像中的特征点到所述第二帧图像的最近邻特征点距离与次近邻特征点距离的比值来对所述第一帧图像的特征描述子与所述第二帧图像的特征描述子进行匹配，得到所述第一帧图像与所述第二帧图像之间的特征点匹配对；
(S4)根据所述第一帧图像对应的RGB图像和深度图像以及所述第二帧图像对应的RGB图像和深度图像，利用针孔相机模型计算出所述第一帧图像对应的RGB图像和所述第二帧图像对应的RGB图像中的二维点所对应的三维点；
(S5)根据所述第一帧图像与所述第二帧图像之间的特征点匹配对对应的三维点，利用所述第一帧图像与所述第二帧图像之间的几何关系以及非线性优化方法估计出所述第一帧图像与所述第二帧图像之间的旋转平移矩阵，得到所述机器人从所述第一帧图像到所述第二帧图像之间的位姿变换；
(S6)将所述第二帧图像作为新的第一帧图像，将与所述第二帧图像相邻的下一帧图像作为新的第二帧图像，检测所述新的第一帧图像是否与所述起始帧图像重合，若不重合，则执行步骤(S2)，否则，执行步骤(S7)；
(S7)将封闭路径上的所有帧图像对应的机器人位姿构建一个姿态图，所述姿态图的节点代表机器人在每一个帧图像上的姿态，边表示节点之间的位姿变换，然后利用非线性优化方法对所述姿态图进行优化，得到机器人在每一帧图像的位姿，进而得到每一帧图像对应的三维点云图像；
(S8)拼接每一帧图像对应的三维点云图像，形成室内三维点云地图，并将所述室内三维点云地图转化为八叉树地图。
4.根据权利要求3所述的方法，其特征在于，所述步骤(3)具体包括：
(3.1)将所述八叉树地图以及在所述八叉树地图内的机器人当前所处的位置和目的地的位置进行投影，得到二维平面的栅格地图；
(3.2)获取所述栅格地图中机器人当前所处的位置的投影点作为起点的所有邻近像元，并计算各邻近像元的F值，存入栈或队列中，其中F值表示邻近像元到目的地的距离值；
(3.3)将栈或队列中F值最小的点作为新起点，并将所有处理过的像元和栈或队列中的像元标记为已处理；
(3.4)检测新起点是否为目的地点，若不是，则获取新起点周围未经过处理的邻近像元的F值并存入栈或队列中，然后执行步骤(3.3)，否则执行步骤(3.5)；
(3.5)将从机器人当前所处位置的投影点到目的地位置的投影点之间的所有新起点连接起来作为从机器人当前位置到目的地位置的最优路径；
(3.6)驱动机器人按所述最优路径进行行走，最终到达目的地位置完成导航任务。
5.一种机器人自主定位与导航系统，其特征在于，包括：
图像采集模块，用于采集机器人在当前位置的RGB图像；
重定位模块，用于将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
导航模块，用于在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走；
聚类模块，用于使用聚类算法对所述室内三维点云地图中SIFT特征点的描述子进行聚类，得到词汇树；
所述聚类模块，还用于获取机器人当前位置的第一RGB图像和机器人接收到的第二RGB图像，并对所述第一RGB图像以及所述第二RGB图像进行SIFT特征点提取，然后使用聚类算法得到视觉词汇；
2D-3D检索模块，用于进行2D-3D检索，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，根据该2D特征点对应的视觉词汇，在所述词汇树中查找与该2D特征点具有相同单词的候选三维匹配点；
2D-3D匹配模块，用于进行2D-3D匹配，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，计算该2D特征点与对应的候选三维匹配点在特征描述子空间上的距离；
所述2D-3D匹配模块，还用于找到与该2D特征点对应的候选三维匹配点中在特征描述子空间上距离最近和距离次近的两个三维点，若最近距离与次近距离的比值小于第一预设阈值，则该2D特征点与在特征描述子空间上距离该2D特征点距离最近的目标三维点相匹配；
3D-2D检索模块，用于对于与该2D特征点匹配的目标三维点，查找所述目标三维点在欧式空间上的若干个最近邻三维点；
3D-2D匹配模块，用于对每一个最近邻三维点，进行3D-2D匹配，在2D图像中查找与该最近邻三维点具有相同词汇的候选2D特征点，计算该最近邻三维点与候选2D特征点在特征描述子空间上的距离；
所述3D-2D匹配模块，还用于找到与该最近邻三维点对应的候选2D特征点中在特征描述子空间上距离最近和距离次近的两个2D特征点，若最近距离与次近距离的比值小于第二预设阈值，则该最近邻三维点与在特征描述子空间上距离该最近邻三维点距离最近的目标
第二检测模块，用于检查当前找到的三维点和二维点的匹配个数，若没有达到第三预设阈值则返回执行所述2D-3D检索模块及后续操作，直到匹配个数达到第三预设阈值；
重定位子模块，用于在匹配个数达到第三预设阈值时，通过得到的3D与2D的匹配对计算得到机器人当前在室内三维点云地图中的位置和目的地在室内三维点云地图中的位置，进而得到八叉树地图内的机器人当前所处的位置和目的地的位置。
所述图像采集模块，还用于采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，机器人的行进路线为闭合路线；
室内地图构建模块，用于利用所述RGB图像和深度图像构建室内三维点云地图。
7.根据权利要求6所述的系统，其特征在于，所述室内地图构建模块包括：
选取模块，用于将获取的起始帧图像作为第一帧图像，将与所述起始帧图像相邻的下一帧图像作为第二帧图像；
特征点提取模块，用于提取所述第一帧图像与所述第二帧图像的特征点，分别计算所述第一帧图像与所述第二帧图像上的关键点，针对关键点周围的像素点，计算所述第一帧图像的特征描述子以及所述第二帧图像的特征描述子；
特征点匹配模块，用于采用所述第一帧图像中的特征点到所述第二帧图像的最近邻特征点距离与次近邻特征点距离的比值来对所述第一帧图像的特征描述子与所述第二帧图像的特征描述子进行匹配，得到所述第一帧图像与所述第二帧图像之间的特征点匹配对；
三维点匹配模块，用于根据所述第一帧图像对应的RGB图像和深度图像以及所述第二帧图像对应的RGB图像和深度图像，利用针孔相机模型计算出所述第一帧图像对应的RGB图像以及所述第二帧图像对应的RGB图像中的二维点所对应的三维点；
位姿确定模块，用于根据所述第一帧图像与所述第二帧图像之间的特征点匹配对对应的三维点，利用所述第一帧图像与所述第二帧图像之间的几何关系以及非线性优化方法估计出所述第一帧图像与所述第二帧图像之间的旋转平移矩阵，得到所述机器人从所述第一帧图像到所述第二帧图像之间的位姿变换；
第一检测模块，用于将所述第二帧图像作为新的第一帧图像，将与所述第二帧图像相邻的下一帧图像作为新的第二帧图像，检测所述新的第一帧图像是否与所述起始帧图像重合，若不重合，则返回执行所述特征点提取模块的操作，直到所述新的第一帧图像与所述起始帧图像重合；
优化模块，用于将封闭路径上的所有帧图像对应的机器人位姿构建一个姿态图，所述姿态图的节点代表机器人在每一个帧图像上的姿态，边表示节点之间的位姿变换，然后利用非线性优化方法对所述姿态图进行优化，得到机器人在每一帧图像的位姿，进而得到每一帧图像对应的三维点云图像；
地图构建子模块，用于拼接每一帧图像对应的三维点云图像，形成室内三维点云地图，并将所述室内三维点云地图转化为八叉树地图。
8.根据权利要求7所述的系统，其特征在于，所述导航模块包括：
投影模块，用于将所述八叉树地图以及在所述八叉树地图内的机器人当前所处的位置和目的地的位置进行投影，得到二维平面的栅格地图；
获取计算模块，用于获取所述栅格地图中机器人当前所处的位置的投影点作为起点的所有邻近像元，并计算各邻近像元的F值，存入栈或队列中，其中F值表示邻近像元到目的地的距离值；
第三检测模块，用于将栈或队列中F值最小的点作为新起点，并将所有处理过的像元和栈或队列中的像元标记为已处理，并检测新起点是否为目的地点，若不是，则获取新起点周围未经过处理的邻近像元的F值并存入栈或队列中，然后执行所述第三检测模块及后续操作；
路径规划模块，用于在新起点为目的地点时，将从机器人当前所处位置的投影点到目的地位置的投影点之间的所有新起点连接起来作为从机器人当前位置到目的地位置的最优路径；
导航子模块，用于驱动机器人按所述最优路径进行行走，最终到达目的地位置完成导航任务。
一种机器人自主定位与导航方法及系统
技术领域
[0001] 本发明属于计算机视觉、机器人技术领域，更具体地，涉及一种机器人自主定位与导航方法及系统。
背景技术
[0002] 室内机器人自主定位与导航是最近几年出现的一个比较热门的研究领域。主要包含以下三个部分：1)室内场景地图的重建与机器人状态的估计；2)机器人与目标对象重定位；3)机器人路径规划。对于室内机器人自主定位与导航来说，主要问题在于传感器的选择(即外界信息的获取)以及机器人的定位导航(即外界信息的处理)。目前是利用激光雷达或者惯性测量原件与视觉传感器结合来完成这一任务，但是所需成本较高。随着相机的普及与计算机视觉理论的发展，使用基于纯视觉的方法来完成这一任务成为主流的研究方向。
[0003] 机器人定位与建图(Simultaneous Localization and Mapping，SLAM)，是指搭载传感器的机器人，在移动时建立对环境的描述模型，同时估计自己的运动。SLAM同时包含定位与建图两个问题，被认为是实现机器人自主性的关键问题之一，对机器人的导航、控制、任务规划等领域有重要的研究意义。
[0004] 目前，对于实时定位与地图重建方法比较成熟的方案是激光雷达SLAM，即利用激光雷达传感器建立二维地图，进而完成机器人导航任务。然而该方法使用的扫描设备结构复杂、价格昂贵且不易操作，并且不能够实时重现室内三维场景，无法对地图产生直观的感受。
[0005] 图定位(Image-based localization)能够从一张新拍的图片中精确定位相机相对于已有三维场景的位置(position)和旋转角(orientation)。随着行人导航、机器人导航、增强现实、运动恢复结构(Structure from Motion，SFM)等技术的发展，图定位受到了越来越多的关注。
[0006] 使用实时定位与地图重建SLAM或运动恢复结构的技术中，由于生成的点云部分或全部点带有相应的特征点描述子，在此情景下图定位的实现方法一般分为三步，2D图像特征点和特征点描述子提取，对特征点点云和2D图像进行描述子邻近搜索和配对，利用基于随机抽样一致性方法(Random Sample Consensus，RANSAC)的n点透视问题解法(n-point perspective pose problem，pnp)来估计相机相对于离线地图的位姿。
[0007] 然而，由于真实场景下点云数量巨大，以及室内环境中重复人造物体的干扰，导致图定位技术在精度和运行速度上难以取得令人满意的效果，因此研究改进的特征点点云到2D图像的特征点匹配算法，提高图定位技术的精度和运行速度在理论和实际应用上都具有很高的价值。
[0008] 机器人路径规划算法是根据机器人对环境感知得到的数据以及构建得到的地图模型，自行规划出一条安全的运行路线，并且高效地完成任务。地图构建路径规划技术是按照机器人自身传感器搜索的障碍物信息，将机器人周围区域划分为不同的网格空间(如自由空间和限制空间等)，计算网格空间的障碍物占有情况，再依据一定规则确定最优路径。
[0009] 目前，地图构建技术已引起机器人研究领域的广泛关注，成为移动机器人路径规划的研究热点之一。但机器人传感器信息资源有限，使得网格地图障碍物信息很难计算与处理，同时由于机器人要动态快速地更新地图数据，在网格数较多、分辨率较高时难以保证路径规划的实时性。因此，地图构建方法必须在地图网格分辨率与路径规划实时性上寻求平衡。
[0010] 随着机器人理论、计算机视觉理论的完善和视觉传感器的发展与普及，基于纯视觉的室内机器人定位和导航技术取得了很大的进展。因此，研究基于RGB-D相机的室内机器人定位导航策略，不仅有很强的理论意义，而且具有非常广阔的应用前景。
发明内容
[0011] 针对现有技术的以上缺陷或改进需求，本发明提供了一种机器人自主定位与导航方法及系统，使得机器人能够独立完成从室内地图构建到机器人自身及目标重定位，再到机器人路径规划这一任务。由此解决现有技术中，设备结构复杂、价格昂贵、不易操作、数据量巨大、路径规划实时性较低等技术问题。
[0012] 为实现上述目的，按照本发明的一个方面，提供了一种机器人自主定位与导航方法，包括：
[0013] (1)采集机器人在当前位置的RGB图像；
[0014] (2)将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
[0015] (3)在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走。
[0016] 优选地，所述室内三维点云地图按照如下方式构建：
[0017] 采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，机器人的行进路线为闭合路线；
[0018] 利用所述RGB图像和深度图像构建室内三维点云地图。
[0019] 优选地，所述利用所述RGB图像和深度图像构建室内三维点云地图，具体包括：
[0020] (S1)将获取的起始帧图像作为第一帧图像，将与所述起始帧图像相邻的下一帧图像作为第二帧图像；
[0021] (S2)提取所述第一帧图像与所述第二帧图像的特征点，分别计算所述第一帧图像与所述第二帧图像上的关键点，针对关键点周围的像素点，计算所述第一帧图像的特征描述子以及所述第二帧图像的特征描述子；
[0022] (S3)采用所述第一帧图像中的特征点到所述第二帧图像的最近邻特征点距离与次近邻特征点距离的比值来对所述第一帧图像的特征描述子与所述第二帧图像的特征描述子进行匹配，得到所述第一帧图像与所述第二帧图像之间的特征点匹配对；
[0023] (S4)根据所述第一帧图像对应的RGB图像和深度图像以及所述第二帧图像对应的RGB图像和深度图像，利用针孔相机模型计算出所述第一帧图像对应的RGB图像和所述第二帧图像对应的RGB图像中的二维点所对应的三维点；
[0024] (S5)根据所述第一帧图像与所述第二帧图像之间的特征点匹配对对应的三维点，利用所述第一帧图像与所述第二帧图像之间的几何关系以及非线性优化方法估计出所述第一帧图像与所述第二帧图像之间的旋转平移矩阵，得到所述机器人从所述第一帧图像到所述第二帧图像之间的位姿变换；
[0025] (S6)将所述第二帧图像作为新的第一帧图像，将与所述第二帧图像相邻的下一帧图像作为新的第二帧图像，检测所述新的第一帧图像是否与所述起始帧图像重合，若不重合，则执行步骤(S2)，否则，执行步骤(S7)；
[0026] (S7)将封闭路径上的所有帧图像对应的机器人位姿构建一个姿态图，所述姿态图的节点代表机器人在每一个帧图像上的姿态，边表示节点之间的位姿变换，然后利用非线性优化方法对所述姿态图进行优化，得到机器人在每一帧图像的位姿，进而得到每一帧图像对应的三维点云图像；
[0027] (S8)拼接每一帧图像对应的三维点云图像，形成室内三维点云地图，并将所述室内三维点云地图转化为八叉树地图。
[0028] 优选地，所述步骤(2)具体包括：
[0029] (2.1)使用聚类算法对所述室内三维点云地图中SIFT特征点的描述子进行聚类，得到词汇树；
[0030] (2.2)获取机器人当前位置的第一RGB图像和机器人接收到的第二RGB图像，并对所述第一RGB图像以及所述第二RGB图像进行SIFT特征点提取，然后使用聚类算法得到视觉词汇；
[0031] (2.3)进行2D-3D检索，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，根据该2D特征点对应的视觉词汇，在所述词汇树中查找与该2D特征点具有相同单词的候选三维匹配点；
[0032] (2.4)进行2D-3D匹配，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，计算该2D特征点与对应的候选三维匹配点在特征描述子空间上的距离；
[0033] (2.5)找到与该2D特征点对应的候选三维匹配点中在特征描述子空间上距离最近和距离次近的两个三维点，若最近距离与次近距离的比值小于第一预设阈值，则该2D特征点与在特征描述子空间上距离该2D特征点距离最近的目标三维点相匹配；
[0034] (2.6)对于与该2D特征点匹配的目标三维点，查找所述目标三维点在欧式空间上的若干个最近邻三维点；
[0035] (2.7)对每一个最近邻三维点，进行3D-2D匹配，在2D图像中查找与该最近邻三维点具有相同词汇的候选2D特征点，计算该最近邻三维点与候选2D特征点在特征描述子空间上的距离；
[0036] (2.8)找到与该最近邻三维点对应的候选2D特征点中在特征描述子空间上距离最近和距离次近的两个2D特征点，若最近距离与次近距离的比值小于第二预设阈值，则该最近邻三维点与在特征描述子空间上距离该最近邻三维点距离最近的目标2D特征点相匹配；
[0037] (2.9)检查当前找到的三维点和二维点的匹配个数，若达到第三预设阈值则执行步骤(2.10)，否则，执行步骤(2.3)；
[0038] (2.10)通过得到的3D与2D的匹配对计算得到机器人当前在室内三维点云地图中的位置和目的地在室内三维点云地图中的位置，进而得到八叉树地图内的机器人当前所处的位置和目的地的位置。
[0039] 优选地，所述步骤(3)具体包括：
[0040] (3.1)将所述八叉树地图以及在所述八叉树地图内的机器人当前所处的位置和目的地的位置进行投影，得到二维平面的栅格地图；
[0041] (3.2)获取所述栅格地图中机器人当前所处的位置的投影点作为起点的所有邻近像元，并计算各邻近像元的F值，存入栈或队列中，其中F值表示邻近像元到目的地的距离值；
[0042] (3.3)将栈或队列中F值最小的点作为新起点，并将所有处理过的单元和栈或队列中的单元标记为已处理；
[0043] (3.4)检测新起点是否为目的地点，若不是，则获取新起点周围未经过处理的邻近像元的F值并存入栈中，然后执行步骤(3.3)，否则执行步骤(3.5)；
[0044] (3.5)将从机器人当前所处位置的投影点到目的地位置的投影点之间的所有新起点连接起来作为从机器人当前位置到目的地位置的最优路径；
[0045] (3.6)驱动机器人按所述最优路径进行行走，最终到达目的地位置完成导航任务。
[0046] 按照本发明的另一方面，提供了一种机器人自主定位与导航系统，包括：
[0047] 图像采集模块，用于采集机器人在当前位置的RGB图像；
[0048] 重定位模块，用于将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
[0049] 导航模块，用于在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走。
[0050] 优选地，所述图像采集模块，还用于采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，机器人的行进路线为闭合路线；
[0051] 所述系统还包括：
[0052] 室内地图构建模块，用于利用所述RGB图像和深度图像构建室内三维点云地图。
[0053] 优选地，所述室内地图构建模块包括：
[0054] 选取模块，用于将获取的起始帧图像作为第一帧图像，将与所述起始帧图像相邻的下一帧图像作为第二帧图像；
[0055] 特征点提取模块，用于提取所述第一帧图像与所述第二帧图像的特征点，分别计算所述第一帧图像与所述第二帧图像上的关键点，针对关键点周围的像素点，计算所述第一帧图像的特征描述子以及所述第二帧图像的特征描述子；
[0056] 特征点匹配模块，用于采用所述第一帧图像中的特征点到所述第二帧图像的最近邻特征点距离与次近邻特征点距离的比值来对所述第一帧图像的特征描述子与所述第二帧图像的特征描述子进行匹配，得到所述第一帧图像与所述第二帧图像之间的特征点匹配对；
[0057] 三维点匹配模块，用于根据所述第一帧图像对应的RGB图像和深度图像以及所述第二帧图像对应的RGB图像和深度图像，利用针孔相机模型计算出所述第一帧图像对应的RGB图像以及所述第二帧图像对应的RGB图像中的二维点所对应的三维点；
[0058] 位姿确定模块，用于根据所述第一帧图像与所述第二帧图像之间的特征点匹配对对应的三维点，利用所述第一帧图像与所述第二帧图像之间的几何关系以及非线性优化方法估计出所述第一帧图像与所述第二帧图像之间的旋转平移矩阵，得到所述机器人从所述第一帧图像到所述第二帧图像之间的位姿变换；
[0059] 第一检测模块，用于将所述第二帧图像作为新的第一帧图像，将与所述第二帧图像相邻的下一帧图像作为新的第二帧图像，检测所述新的第一帧图像是否与所述起始帧图像重合，若不重合，则返回执行所述特征点提取模块的操作，直到所述新的第一帧图像与所述起始帧图像重合；
[0060] 优化模块，用于将封闭路径上的所有帧图像对应的机器人位姿构建一个姿态图，所述姿态图的节点代表机器人在每一个帧图像上的姿态，边表示节点之间的位姿变换，然后利用非线性优化方法对所述姿态图进行优化，得到机器人在每一帧图像的位姿，进而得到每一帧图像对应的三维点云图像；
[0061] 地图构建子模块，用于拼接每一帧图像对应的三维点云图像，形成室内三维点云地图，并将所述室内三维点云地图转化为八叉树地图。
[0062] 优选地，所述重定位模块包括：
[0063] 聚类模块，用于使用聚类算法对所述室内三维点云地图中SIFT特征点的描述子进行聚类，得到词汇树；
[0064] 所述聚类模块，还用于获取机器人当前位置的第一RGB图像和机器人接收到的第二RGB图像，并对所述第一RGB图像以及所述第二RGB图像进行SIFT特征点提取，然后使用聚类算法得到视觉词汇；
[0065] 2D-3D检索模块，用于进行2D-3D检索，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，根据该2D特征点对应的视觉词汇，在所述词汇树中查找与该2D特征点具有相同单词的候选三维匹配点；
[0066] 2D-3D匹配模块，用于进行2D-3D匹配，对于所述第一RGB图像以及所述第二RGB图像中的每个2D特征点，计算该2D特征点与对应的候选三维匹配点在特征描述子空间上的距离；
[0067] 所述2D-3D匹配模块，还用于找到与该2D特征点对应的候选三维匹配点中在特征描述子空间上距离最近和距离次近的两个三维点，若最近距离与次近距离的比值小于第一预设阈值，则该2D特征点与在特征描述子空间上距离该2D特征点距离最近的目标三维点相匹配；
[0068] 3D-2D检索模块，用于对于与该2D特征点匹配的目标三维点，查找所述目标三维点在欧式空间上的若干个最近邻三维点；
[0069] 3D-2D匹配模块，用于对每一个最近邻三维点，进行3D-2D匹配，在2D图像中查找与该最近邻三维点具有相同词汇的候选2D特征点，计算该最近邻三维点与候选2D特征点在特征描述子空间上的距离；
[0070] 所述3D-2D匹配模块，还用于找到与该最近邻三维点对应的候选2D特征点中在特征描述子空间上距离最近和距离次近的两个2D特征点，若最近距离与次近距离的比值小于第二预设阈值，则该最近邻三维点与在特征描述子空间上距离该最近邻三维点距离最近的目标2D特征点相匹配；
[0071] 第二检测模块，用于检查当前找到的三维点和二维点的匹配个数，若没有达到第三预设阈值则返回执行所述2D-3D检索模块及后续操作，直到匹配个数达到第三预设阈值；
[0072] 重定位子模块，用于在匹配个数达到第三预设阈值时，通过得到的3D与2D的匹配对计算得到机器人当前在室内三维点云地图中的位置和目的地在室内三维点云地图中的位置，进而得到八叉树地图内的机器人当前所处的位置和目的地的位置。
[0073] 优选地，所述导航模块包括：
[0074] 投影模块，用于将所述八叉树地图以及在所述八叉树地图内的机器人当前所处的位置和目的地的位置进行投影，得到二维平面的栅格地图；
[0075] 获取计算模块，用于获取所述栅格地图中机器人当前所处的位置的投影点作为起点的所有邻近像元，并计算各邻近像元的F值，存入栈或队列中，其中F值表示邻近像元到目的地的距离值；
[0076] 第三检测模块，用于将栈或队列中F值最小的点作为新起点，并将所有处理过的单元和栈或队列中的单元标记为已处理，并检测新起点是否为目的地点，若不是，则获取新起点周围未经过处理的邻近像元的F值并存入栈中，然后执行所述获取计算模块及后续操作；
[0077] 路径规划模块，用于在新起点为目的地点时，将从机器人当前所处位置的投影点到目的地位置的投影点之间的所有新起点连接起来作为从机器人当前位置到目的地位置的最优路径；
[0078] 导航子模块，用于驱动机器人按所述最优路径进行行走，最终到达目的地位置完成导航任务。
[0079] 总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要有以下的技术优点：
[0080] (1)按照本发明实施的室内机器人定位与导航策略完全依赖于计算机视觉和机器人技术，其过程相较已有方法得到简化，从数据输入到模型输出仅在地图构建时需要驱动机器人，其它过程全部实现了自动化处理。
[0081] (2)实验设备价格适中、易于操作，所需要的数据形式单一、采集方便。
[0082] (3)使用一种2D-3D搜索和3D-2D搜索复合的主动搜索方法来做点云和2D图像特征点匹配，能够实现高精度的匹配和较少的计算时间，具有较好的实用性。
[0083] (3)整体策略能够仅以视觉信息为基础，完成机器人从构图到定位再到导航的任务，方案的完整度、可行性等指标上相较于以往的方案都有很大的改善。
附图说明
[0084] 图1为本发明实施例公开的一种机器人自主定位与导航方法的流程示意图；
[0085] 图2为本发明实施例公开的另一中机器人自主定位与导航方法的流程示意图；
[0086] 图3为本发明实施例中室内三维地图构建和估计机器人位姿的流程图；
[0087] 图4为本发明实施例中机器人自身以及目标重定位的流程图。
具体实施方式
[0088] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0089] 本发明公开的方法涉及到特征点匹配、多视图几何、图优化、SLAM、图片重定位、路径规划，可以直接用于基于视觉传感器的机器在室内环境中建立三维地图，进而完成对自身以及目标位置的重定位和路径规划任务。因此该方法能够用于无人驾驶汽车、室内导航等多个领域。
[0090] 如图1所示为本发明实施例公开的一种机器人自主定位与导航方法的流程示意图。在图1所示的方法中，包括以下操作：
[0091] (1)采集机器人在当前位置的RGB图像；
[0092] (2)将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
[0093] 作为一种可选的实施方式，室内三维点云地图按照如下方式构建：
[0094] 采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，机器人的行进路线为闭合路线；
[0095] 利用上述RGB图像和深度图像构建室内三维点云地图。
[0096] (3)在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走。
[0097] 如图2所示为本发明实施例公开的另一种机器人自主定位与导航方法的流程示意图，从图2可以看出，作为输入的RGB图像、深度图像和目的地位置RGB图像数据需要经过室内地图构建、机器人和目标重定位、路径规划等几个步骤，最终完成室内机器人自主导航任务。其具体实施方式如下：
[0098] (1)采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，行进路线为封闭路线；
[0099] 机器人A处于一个室内环境中，驱动机器人使其在该环境中缓慢移动，同时驱动机器人上的RGB-D相机对机器人行径路线上的所有位置进行拍摄得到一系列RGB图像数据和深度图像数据。值得注意的是，因为机器人需要实时地建立室内环境地图所以需要很大的计算量，因此为了获取精确的地图应当使得机器人行进尽量缓慢。同时为了使得地图优化部分能够顺利完成，应当尽量使得机器人在行进过程中形成封闭的回路。
[0100] 本发明实施例中对数据的形式不做限定，机器人所处场景的RGB-D数据可以是一段连续的视频也可以是连续的多帧图像，但是应该保证每一个关键帧都有其对应的深度图像。
[0101] (2)利用RGB图像和深度图像构建室内三维点云地图；
[0102] 优选地，在本发明的一个实施例中，机器人自身位姿估计和室内场景地图构建SLAM采用RGB-D SLAM算法。除此之外，本发明实施例中使用的SLAM方法还可以任意选用能够建立稠密三维地图的SLAM方法如实时稠密跟踪和构图(Dense Tracking and Mapping in Real-Time，DTMP)、稠密视觉里程计(Dense Visual Odometry and SLAM，DVO)等。
[0103] 优选地，在本发明的一个实施例中，步骤(2)的实现具体包括：
[0104] (2.1)将获取的起始帧图像作为第一帧图像，将与起始帧图像相邻的下一帧图像作为第二帧图像；
[0105] (2.2)提取第一帧图像与第二帧图像的特征点，分别计算第一帧图像与第二帧图像上的关键点，针对关键点周围的像素点，计算第一帧图像的特征描述子以及第二帧图像的特征描述子；
[0106] 其中，为了提取彩色图像的SIFT特征，在机器人拍摄的第一帧与其相邻的下一帧的两幅RGB图像上分别计算关键点，针对这些关键点周围的像素点计算各自的SIFT描述子，这些描述子能够表达每一幅图像的特征。
[0107] (2.3)采用第一帧图像中的特征点到第二帧图像的最近邻特征点距离与次近邻特征点距离的比值来对第一帧图像的特征描述子与第二帧图像的特征描述子进行匹配，得到第一帧图像与第二帧图像之间的特征点匹配对；
[0108] 其中，为了得到第一帧与第二帧之间的匹配关系，对两关键帧的特征描述子用最近邻算法进行匹配，计算得到两关键帧之间对应的匹配对。
[0109] (2.4)根据第一帧图像对应的RGB图像和深度图像以及第二帧图像对应的RGB图像和深度图像，利用针孔相机模型计算出第一帧图像对应的RGB图像和第二帧图像对应的RGB图像中的二维点所对应的三维点；
[0110] (2.5)根据第一帧图像与第二帧图像之间的特征点匹配对对应的三维点，利用第一帧图像与第二帧图像之间的几何关系以及非线性优化方法估计出第一帧图像与第二帧图像之间的旋转平移矩阵，得到机器人从第一帧图像到第二帧图像之间的位姿变换；
[0111] 其中，旋转平移矩阵表示两关键帧的相对位姿变换，也即表示机器人从第一帧到相邻下一帧之间的位姿变换。
[0112] (2.6)将第二帧图像作为新的第一帧图像，将与第二帧图像相邻的下一帧图像作为新的第二帧图像，检测新的第一帧图像是否与起始帧图像重合，若不重合，则执行步骤(2.2)，否则，执行步骤(2.7)；
[0113] (2.7)将封闭路径上的所有帧图像对应的机器人位姿构建一个姿态图，该姿态图的节点代表机器人在每一个帧图像上的姿态，边表示节点之间的位姿变换，然后利用非线性优化方法对上述姿态图进行优化，得到机器人在每一帧图像的位姿，进而得到每一帧图像对应的三维点云图像；
[0114] (2.8)拼接每一帧图像对应的三维点云图像，形成室内三维点云地图，并将室内三维点云地图转化为八叉树地图。
[0115] 由于后续的机器人导航无法通过三维点云图像来完成，所以需要将得到的稠密室内三维点云地图进行转化得到八叉树地图，将空间分解为相互连接且不重叠的空间单元，并且能够得到每一个单元中是否有物体占据，从而利于机器人导航任务的进行。如图2所示为本发明实施例中室内三维地图构建和估计机器人位姿的流程图。
[0116] (3)采集机器人在当前位置的第一RGB图像，将第一RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的第二RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
[0117] 其中，预存储的第二RGB图像以及机器人当前位置的第一RGB图像可以通过以下方式获得：实验人员B在室内环境中的任意一处位置拍摄RGB图像，将该RGB图像发送给机器人，同时机器人A在当前位置也拍摄RGB图像。值得注意的是RGB图像的拍摄尽量选取带有纹理的图像。
[0118] 机器人和目的地的定位采用同样的方式，对当前的场景拍摄一张照片，通过图定位技术即可确定机器人相对于三维点云地图的初始位置和目的地相对于三维点云地图的位置。
[0119] 优选地，在本发明的一个实施例中，图片特征点提取采用SIFT角点检测算法，除此之外，本发明中使用的特征点还可以任意选用具有局部显著性和稳定性的特征如二值鲁棒尺度不变特征点(Binary Robust Invariant Scalable Keypoints，BRISK)、加速分割试验特征(Features from Accelerated Segment Test，FAST)和加速稳健特征(Speed Up Robust Feature，SURF)等。
[0120] 优选地，在本发明的一个实施例中，2D、3D点匹配采用2D-3D匹配结合3D-2D匹配的复合方法进行，除此之外，本发明实施例中使用的2D、3D点匹配可以任意选用能够成功找到准确、数量足够的2D、3D匹配的方法，如使用特征描述子直接进行树搜索查找近邻点、或只使用2D-3D匹配或3D-2D匹配进行检索。
[0121] 优选地，在本发明的一个实施例中，步骤(3)的实现具体包括：
[0122] (3.1)使用聚类算法对室内三维点云地图中SIFT特征点的描述子进行聚类，得到词汇树；
[0123] 优选地，在本发明的一个实施例中，可以使用近似K均值聚类算法对室内三维点云SIFT特征点的描述子进行聚类，得到一个词汇树。
[0124] (3.2)获取机器人当前位置的第一RGB图像和机器人接收到的第二RGB图像，并对第一RGB图像以及第二RGB图像进行SIFT特征点提取，然后使用聚类算法得到视觉词汇；
[0125] (3.3)进行2D-3D检索，对于第一RGB图像以及第二RGB图像中的每个2D特征点，根据该2D特征点对应的视觉词汇，在上述词汇树中查找与该2D特征点具有相同单词的候选三维匹配点；
[0126] (3.4)进行2D-3D匹配，对于第一RGB图像以及第二RGB图像中的每个2D特征点，计算该2D特征点与对应的候选三维匹配点在特征描述子空间上的距离；
[0127] (3.5)找到与该2D特征点对应的候选三维匹配点中在特征描述子空间上距离最近和距离次近的两个三维点，若最近距离与次近距离的比值小于第一预设阈值，则该2D特征点与在特征描述子空间上距离该2D特征点距离最近的目标三维点相匹配；
[0128] 优选地，在本发明的一个实施例中，进行2D-3D匹配，对图片中每一个2D特征点，先使用线性搜索检索候选三维匹配点，然后使用距离检测决定候选点是否与该2D特征点匹配。具体包括以下操作：
[0129] (3.4.1)从第一RGB图像或第二RGB图像中读取一个2D特征点；
[0130] (3.4.2)在词汇树较低层查找与该2D特征点具有相同词汇的所有候选三维匹配点；
[0131] 优选地，在本发明的一个实施例中选择在词汇树第3层查找候选三维匹配点。
[0132] (3.4.3)使用线性搜索在候选三维匹配点中找到与2D特征点在特征描述子空间上的最近点和次最近点，并计算他们与2D特征点的距离；
[0133] (3.4.4)若(3.4.3)中计算得到的最近距离和次最近距离的比值小于第一预设阈值，则认为该2D特征点和与其在特征描述子空间上距离最近的候选三维点相匹配，记入匹配。否则认为该2D特征点无三维点与其匹配。
[0134] (3.6)对于与该2D特征点匹配的目标三维点，查找目标三维点在欧式空间上的若干个最近邻三维点；
[0135] (3.7)对每一个最近邻三维点，进行3D-2D匹配，在2D图像中查找与该最近邻三维点具有相同词汇的候选2D特征点，计算该最近邻三维点与候选2D特征点在特征描述子空间上的距离；
[0136] (3.8)找到与该最近邻三维点对应的候选2D特征点中在特征描述子空间上距离最近和距离次近的两个2D特征点，若最近距离与次近距离的比值小于第二预设阈值，则该最近邻三维点与在特征描述子空间上距离该最近邻三维点距离最近的目标2D特征点相匹配；
[0137] 优选地，在本发明的一个实施例中，对前述每一个最近邻三维点，进行3D-2D匹配，具体方法为在图片中查找与前述最近邻三维点相匹配的所有候选2D特征点，然后使用距离检测决定候选2D特征点是否与该最近邻三维点相匹配。具体包括以下操作：
[0138] (3.7.1)从(3.6)中获得的所有最邻近三维点中读入一个三维点；
[0139] (3.7.2)查找该三维点在(3.1)得到的词汇树的较高层中对应的词汇，并找到属于该词汇的所有2D特征点，成为该三维点的候选2D匹配特征点；
[0140] 优选地，在本发明的一个实施例中使用词汇树的第5层查找词汇。
[0141] (3.7.3)使用线性搜索在候选2D特征点中找到与三维点在特征描述子空间上距离最近点和次最近点，并计算它们与三维点在前述空间上的距离；
[0142] (3.7.4)若(3.7.3)中计算得到的最近距离和次最近距离的比值小于第二预设阈值，则认为前述三维点和与其在特征描述子空间上距离最近的候选2D特征点相匹配，记入匹配；否则回到(3.7.1)，重新读入另外一个邻近三维点并进行3D-2D匹配检索。
[0143] (3.9)检查当前找到的三维点和二维点的匹配个数，若达到第三预设阈值则执行步骤(3.10)，否则，执行步骤(3.3)；
[0144] (3.10)通过得到的3D与2D的匹配对计算得到机器人当前在室内三维点云地图中的位置和目的地在室内三维点云地图中的位置，进而得到八叉树地图内的机器人当前所处的位置和目的地的位置。
[0145] 优选地，在本发明的一个实施例中，对离线构建的三维点云和对场景拍摄的图片进行特征点匹配，并对N个匹配对使用基于6点直接线性变换算法(Direct Linear Transform，DLT)的随机抽样一致性算法(Random Sample Consensus，RANSAC)方法来估计当前相机位姿。如图3所示为本发明实施例中机器人自身以及目标重定位的流程图。
[0146] (4)在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按上述最优路径行走。
[0147] 优选地，可以利用A*算法求解得到最佳路径从而驱动机器人完成导航。
[0148] 优选地，在本发明的一个实施例中，步骤(4)的实现具体包括：
[0149] (4.1)将八叉树地图以及在八叉树地图内的机器人当前所处的位置和目的地的位置进行投影，得到二维平面的栅格地图，该栅格地图每一个单元为一个正方形，代表了现实环境中的一定空间；
[0150] (4.2)获取栅格地图中机器人当前所处的位置的投影点作为起点的所有邻近像元，并计算各邻近像元的F值，存入栈或队列中，其中F值表示邻近像元到目的地的距离值；
[0151] 其中，F值的计算通常的做法是起点到当前点的距离加上当前点到目的地点的距离。
[0152] (4.3)将栈或队列中F值最小的点作为新起点，并将所有处理过的单元和栈或队列中的单元标记为已处理；
[0153] (4.4)检测新起点是否为目的地点，若不是，则获取新起点周围未经过处理的邻近像元的F值并存入栈中，然后执行步骤(4.3)，否则执行步骤(4.5)；
[0154] (4.5)将从机器人当前所处位置的投影点到目的地位置的投影点之间的所有新起点连接起来作为从机器人当前位置到目的地位置的最优路径；
[0155] (4.6)驱动机器人按上述最优路径进行行走，最终到达目的地位置完成导航任务。
[0156] 在本发明的一个实施例中，公开了一种机器人自主定位与导航系统，其中，该系统包括：
[0157] 图像采集模块，用于采集机器人在当前位置的RGB图像；
[0158] 重定位模块，用于将当前位置的RGB图像与室内三维点云地图进行特征点匹配，确定机器人在室内三维点云地图中的当前位置；将预存储的目的地RGB图像与室内三维点云地图进行特征点匹配，确定目的地在室内三维点云地图中的目的地位置；
[0159] 导航模块，用于在室内三维点云地图中搜索从当前位置到目的地位置的最优路径，并驱动机器人按所述最优路径行走。
[0160] 作为一种可选的实施方式，上述图像采集模块，还用于采集机器人在行进路线上拍摄得到的RGB图像和深度图像，其中，机器人的行进路线为闭合路线；
[0161] 作为一种可选的实施方式，该系统还包括：室内地图构建模块，用于利用所述RGB图像和深度图像构建室内三维点云地图。
[0162] 其中，各模块的具体实施方式可以参照方法实施例的描述，本发明实施例将不做复述。
[0163] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
