一种融合视觉里程计与物理里程计的室内机器人定位方法

摘要

本发明公开了一种融合视觉里程计与物理里程计的室内机器人定位方法，通过采集的图像提取ORB特征进行图像匹配、相机位姿估计、闭环检测精确定位机器人。本发明加入了视觉传感器在已知环境中对机器人进行闭环检测，以此消除基于粒子滤波的物理里程计在全局中的累计误差，把里程计的全局误差变为了阶段性的累加，并在此基础上构建了闭合的地图。本发明融合视觉里程计后有效解决物理里程计误差累计问题，能使机器人在已知环境中进行自定位和精确重定位，且增加的运算量不大，能保证效率和实时性，精度满足室内导航需求，是现阶段解决大环境下机器人定位不准的有效方法。

权利要求

1.一种融合视觉里程计与物理里程计的室内机器人定位方法，其特征在于，该方法实现的步骤如下：步骤1、采用相机获取彩色和深度图像；

步骤2、对得到的连续两幅图像提取ORB特征，计算每个orb特征点的描述子，通过相邻图像间的特征匹配，估计相机位姿变化，估计相机位姿变化的步骤是：1)结合深度图像，得到有效特征点处的深度信息；2)根据特征点的orb特征和深度值进行匹配，RANSAC算法剔除错误点对；3)求得相邻图像间的旋转矩阵R和平移矩阵T，估计相机的位姿变换；

步骤3、在机器人移动过程中，选取相邻几帧图像中共同的特征点最多、匹配最好的图像为关键帧，同时保存每个关键帧所对应的机器人轨迹和激光数据；

步骤4、当机器人移动到已知环境中，首先在离线训练的BoW词典中寻找与当前帧匹配的特征点，重定位机器人，然后通过TF计算机器人当前位姿，最后把闭环检测重定位的机器人位姿信息以ros的消息机制发布出去；

步骤5、根据扩展卡尔曼滤波器，订阅闭环检测的视觉里程计消息和AMCL粒子滤波实时定位的机器人位姿优化得到机器人精确的实时位姿，以此消除物理里程计累计的全局误差；其中，每一次局部闭环检测，都能消除机器人物理里程计的累计误差，使得全局误差始终处于阶段性的累加；

步骤6、最终当机器人回到初始位置时，全局的闭环检测，会优化整个运动轨迹和所有关键帧的位姿，再利用保存的激光数据构建栅格地图，完成整个同时定位与地图构建的流程。

说明书

一种融合视觉里程计与物理里程计的室内机器人定位方法

技术领域

[0001] 本发明涉及室内移动机器人自主定位精度的方法，具体涉及一种融合视觉里程计与物理里程计的室内机器人定位方法。

背景技术

[0002] 在自主移动机器人智能导航技术的相关研究中，机器人在未知环境下的同时定位与地图构建(SLAM)技术作为关键性的技术，兼具工程和学术上的双重价值，已然成为近二十年来该领域的研究热点。在这种趋势下，学者们提出了多种解决SLAM问题的方法，也应用了多种传感器来解决SLAM中的环境感知问题。

[0003] SLAM技术首先要解决的问题是选择适当的传感器系统来实现机器人实时定位。实际应用中，基于激光雷达这类在测距范围和方位角上都具有较高精确度的传感器是人们优先选用的传感器，同时还需要使用红外线、超声波、IMU、视觉传感器、里程计等多种传感器来辅助定位以提供定位精度。然而，多传感器融合一直是SLAM界的技术难点，目前能有效融合并产品化的SLAM方法基本没有。对于室内移动机器人，考虑到实际的使用场景以及现今的发展状况，除了使用激光雷达和物理里程计，再加入视觉里程计来提高定位精度，是目前室内移动机器人SLAM技术真正产品化阶段最优的解决方案。

[0004] 现有技术通过改进的蒙特卡洛粒子滤波和物理里程计的定位方法能满足机器人在室内结构简单且面积不大的环境中，然而物理里程计是通过两个时间段的位移增量来进行计算，它只考虑局部运动，因此误差会不断叠加累计，直到漂移过大，无法消除，尤其在轮子打滑或者倾斜时会导致定位误差更大。

发明内容

[0005] 有鉴于此，本发明提供了一种融合视觉里程计与物理里程计的室内机器人定位方法，通过采集的图像提取ORB特征进行图像匹配、相机位姿估计、闭环检测精确定位机器人。

[0006] 一种融合视觉里程计与物理里程计的室内机器人定位方法，该方法实现的步骤如下：

[0007] 步骤1、采用相机获取彩色和深度图像；

[0008] 步骤2、对得到的连续两幅图像提取ORB特征，计算每个orb特征点的描述子，通过相邻图像间的特征匹配，估计相机位姿变化；

[0009] 步骤3、在机器人移动过程中，选取相邻几帧图像中共同的特征点最多、匹配最好的图像为关键帧，同时保存每个关键帧所对应的机器人轨迹和激光数据；

[0010] 步骤4、当机器人移动到已知环境中，首先在离线训练的BoW词典中寻找与当前帧匹配的特征点，重定位机器人，然后通过TF计算机器人当前位姿，最后把闭环检测重定位的机器人位姿信息以ros的消息机制发布出去；

[0011] 步骤5、根据扩展卡尔曼滤波器，订阅闭环检测的视觉里程计消息和AMCL粒子滤波实时定位的机器人位姿优化得到机器人精确的实时位姿，以此消除物理里程计累计的全局误差；其中，每一次局部闭环检测，都能消除机器人物理里程计的累计误差，使得全局误差始终处于阶段性的累加；

[0012] 步骤6、最终当机器人回到初始位置时，全局的闭环检测，会优化整个运动轨迹和所有关键帧的位姿，再利用保存的激光数据构建栅格地图，完成整个同时定位与地图构建的流程。

[0013] 进一步地，所述估计相机位姿变化的步骤是：1)结合深度图像，得到有效特征点处的深度信息；2)根据特征点的orb特征和深度值进行匹配，RANSAC算法剔除错误点对；3)求得相邻图像间的旋转矩阵R和平移矩阵T，估计相机的位姿变换。

[0014] 有益效果：

[0015] 本发明加入了视觉传感器在已知环境中对机器人进行闭环检测，以此消除基于粒子滤波的物理里程计在全局中的累计误差，把里程计的全局误差变为了阶段性的累加，并在此基础上构建了闭合的地图。与传统的SLAM方法相比，本发明融合视觉里程计后有效解决物理里程计误差累计问题，能使机器人在已知环境中进行自定位和精确重定位，且增加的运算量不大，能保证效率和实时性，精度满足室内导航需求，是现阶段解决大环境下机器人定位不准的有效方法。

附图说明

[0016] 图1为本发明的融合定位方法流程图；

[0017] 图2为本发明融合视觉里程计和物理里程计的定位过程示意图。

具体实施方式

[0018] 下面结合附图并举实施例，对本发明进行详细描述。

[0019] 如附图1和2所示，本发明提供了一种

[0020] 步骤1、采用华硕的深度相机Xtion获取彩色和深度图像；

[0021] 步骤2、对得到的连续两幅图像提取ORB特征，计算每个orb特征点的描述子，通过相邻图像间的特征匹配，估计相机位姿变化：1)结合深度图像，得到有效特征点处的深度信息；2)根据特征点的orb特征和深度值进行匹配，RANSAC算法剔除错误点对；3)求得相邻图像间的旋转矩阵R和平移矩阵T，估计相机的位姿变换；

[0022] 步骤3、在机器人移动过程中，选取相邻几帧图像中共同的特征点最多、匹配最好的图像为关键帧，同时保存每个关键帧所对应的机器人轨迹和激光数据；

[0023] 步骤4、当机器人移动到已知环境中，首先在离线训练的BoW词典中寻找与当前帧匹配的特征点，重定位机器人，然后通过TF计算机器人当前位姿，最后把闭环检测重定位的机器人位姿信息以ros的消息机制发布出去；

[0024] 步骤5、根据扩展卡尔曼滤波器，订阅闭环检测的视觉里程计消息和AMCL粒子滤波实时定位的机器人位姿优化得到机器人精确的实时位姿，以此消除物理里程计累计的全局误差；其中，每一次局部闭环检测，都能消除机器人物理里程计的累计误差，使得全局误差始终处于阶段性的累加；

[0025] 步骤6、最终当机器人回到初始位置时，全局的闭环检测，会优化整个运动轨迹和所有关键帧的位姿，再利用保存的激光数据构建栅格地图，完成整个同时定位与地图构建的流程。

[0026] 采用本方法构建出的闭合地图实际宽为86.4m，高为38.4m。

[0027] 综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。