[发明专利]一种融合特征点法和直接法的快速单目视觉里程计导航定位方法

说明书

本发明公开了一种融合特征点法和直接法的快速单目视觉里程计导航定位方法，包括：S1、开启视觉里程计并获取第一帧图像I₁，转化为灰度图，并提取ORB特征点，构建初始化关键帧；S2、判断是否已初始化；若已初始化，则转到步骤S6，否则转到步骤S3；S3、定义参考帧和当前帧，提取ORB特征，并进行特征匹配；S4、用并行线程同时求单应矩阵H和基础矩阵F，计算判断模型得分RH，若RH大于阈值时选用单应性矩阵H，否则选用基础矩阵F，并根据选用的模型估计相机运动；S5、求相机位姿和初始3D点；S6、判断是否已提取特征点，若未提取特征点，则使用直接法进行跟踪，否则用特征点法进行跟踪；S7、完成初始相机位姿估计。通过本发明可以更为精确的进行导航定位。

技术领域

本发明属于导航定位的技术领域，涉及一种融合特征点法和直接法的快速单目视觉里程计导航定位方法。

背景技术

SLAM(simultaneous localization and mapping,即时定位与地图构建)指的是机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。视觉里程计作为视觉SLAM方法的重要组成部分，在很大程度上决定了视觉SLAM方法的精度和速度。

视觉里程计主要采取2种计算方式：特征点法和直接法。特征法首先提取图像特征点和描述子，进行特征点匹配后通过最小化重投影误差计算机器人位姿。直接法相对于特征法，由于不需要提取图像特征，执行速度较快；对图像的光度误差鲁棒性较高，但是对相机内参要求很高，当存在几何噪声时算法性能下降较快；在发生图像运动模糊的情况下依然可以实现相机定位，但对大基线运动的鲁棒性较差。

现有技术中，并没有将融合特征点法和直接法同时使用进行导航定位的技术方案，所以如何克服难点，基于特征点法与直接法的特点，融合特征点法与直接法，是本领域技术人员研究的方向之一。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供了一种融合特征点法和直接法的快速单目视觉里程计导航定位方法，通过本方法得到的结果相比于传统的单目视觉里程计对计算硬件要求更低，计算时间大幅缩短。

为了达到上述目的，本发明采用以下技术方案：

本发明提供的一种融合特征点法和直接法的快速单目视觉里程计导航定位方法，包括下述步骤：

S1、开启视觉里程计并获取第一帧图像I₁，转化为灰度图，并提取ORB特征点，构建初始化关键帧；

S2、判断是否已初始化；若已初始化，则转到步骤S6，否则转到步骤S3；

S3、定义参考帧和当前帧，提取ORB特征，并进行特征匹配；

S4、用并行线程同时求单应矩阵H和基础矩阵F，计算判断模型得分R_H，若R_H大于阈值时选用单应矩阵H，否则选用基础矩阵F，并根据选用的模型估计相机运动；

S5、求相机位姿和初始3D点；

S6、判断是否已提取特征点，若未提取特征点，则使用直接法进行跟踪，否则用特征点法进行跟踪；

S7、完成初始相机位姿估计；

S8、若通过一种模型完成了初始相机的位姿估计，则进一步跟踪局部地图，即和当前帧相关联的地图点做联合优化，获得一个较为准确的相机位姿；

S9、获取下一帧图像并转到步骤S2。

作为优选的技术方案，步骤S3中，采用FAST角点提取ORB特征点，具体方法为：

S3.1.1、在图像中选取像素p，假设它的亮度为I_p；