[发明专利]一种同步定位与地图构建方法和系统

说明书

本发明公开了一种同步定位与地图构建方法和系统。本发明利用特征点法初步求取两帧间的位姿变换关系，然后将它作为直接法优化的初始值。本发明检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；根据相邻帧图像间特征点的对应关系，计算出本质矩阵；根据所述本质矩阵计算出相邻帧图像的位姿变化关系；获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，对像素点重投影，优化位姿变化关系的光度误差；根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。本发明可以解决非线性优化中容易陷入局部最优值的问题，使系统在特征点不足或光照不稳定的应用场景中能够有较好的稳定性。

技术领域

本发明涉及无人机定位与地图构建领域，尤其是一种同步定位与地图构建(Simultaneous Localization And Mapping，SLAM)方法和系统。

背景技术

未知环境下同步定位与建图，是一个智能移动设备实现自主移动的基础关键能力，也只有在定位的前提下，才能进一步导航规划，控制底层驱动实现动力系统的类人行走，由此才能进行其他的工作。传统的视觉SLAM多基于特征点或者直接比较相邻帧之间的像素差异。

特征点法需要依赖重复性较强的特征提取器，关键点提取、描述子、匹配耗时长，需要正确的特征匹配，才能正确地计算相机运动。在纹理缺失的环境中，特征点容易丢失，导致算法失效。

直接法直接比较相邻帧之间的像素差异，而不对像素本身进行描述，所以需要去除因为光线对像素灰度的影响。直接法的理论基础是灰度一致性假设，即假设相机运动的同时，相邻帧的图像像素信息在完全相同的光线条件下得到的。灰度不变假设设定同一个空间点的像素灰度在各个图像中是保持固定。灰度不变假设是一个很强的假设，在移动机器人实际的运动中，满足此假设的对环境条件的要求很苛刻而很难满足。而且由于物体的材质不同，像素会在像素平面中出现高光和阴影部分。当相机自动调整曝光参数时，图像整体的亮度自动变化。此时灰度不变假设都是不成立的。

由此可知，传统的视觉SLAM方法存在以下不足：

1、特征点信息利用率低，不足以满足稳定的图像匹配跟踪。

2、基于特征点的SLAM算法需要环境中有丰富的点特征。

3、基于直接法的SLAM算法基于灰度不变假设，是一种强假设,易受光照变化影响。

发明内容

本发明的发明目的在于：针对上述存在的问题，提供一种同步定位与地图构建方法，以在在快速运动情况下需求时不再有严重的特征丢失，而且降低对灰度不变假设的依赖，在纹理不足的场景也有较好的稳定性。

本发明采用的技术方案如下：

一种同步定位与地图构建方法，包括：

检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；根据相邻帧图像间特征点的对应关系，计算出本质矩阵；根据所述本质矩阵计算出相邻帧图像的位姿变化关系；获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上；优化位姿变化关系的光度误差；根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。

上述方法把特征点检测和匹配与相邻帧图像之间的像素梯度信息联系起来，即便于相邻关键帧的信息提取与匹配，又具备对光度误差计算的简单便捷特点，对特征点依赖低，不易丢帧，对灰度不变性依赖低。

进一步的，所述根据相邻帧图像间特征点的对应关系，计算出本质矩阵，包括：根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。

进一步的，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵采用五点算法计算。