[发明专利]一种单目视觉惯性组合的定位导航方法

说明书

本发明提供了一种单目视觉惯性组合的定位导航方法，包括：采集视频流和IMU数据流，并进行打包对齐；进行初始化，其包括视觉初始化、IMU初始化、确定视觉坐标系与IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI‑EKF对尺度进行求精估计；获取IMU数据流中的惯导数据，通过用互补滤波结合预积分的方法得到IMU位姿，同时跟踪视频流中的图像特征并参考IMU位姿变化量得到视觉位姿；判断视觉跟踪是否丢失，若丢失，则用IMU位姿进行运动跟踪，若未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终的相机位姿。本发明针对现有技术的不足，提高了尺度的精度，达到了在定位过程中保持高精度，高鲁棒性的效果。

技术领域

本发明涉及定位导航技术领域，特别是涉及一种单目视觉惯性组合的定位导航方法。

背景技术

近年来，随着自动驾驶汽车、增强现实、虚拟现实、机器人和无人机导航等新兴技术的发展，组合导航逐步成为定位导航领域的热门研究方向。在不同的导航传感器组合模式中，单目视觉惯性组合提供了一种廉价且极具潜力的解决方案。单目相机价格低，功耗小，且能提供丰富的环境信息，但是纯单目视觉导航方法存在尺度不确定，易受环境条件影响的缺点；惯性传感器提供自运动信息，不易受遮挡和光照的影响，但是纯惯性导航方法受其本身的漂移和积累误差的影响，只在初始化时刻附近有较好的精度，随着时间的增加，其精度会逐渐发散。通过将单目相机和惯性传感器组合起来可以使其对各自局限性进行相互补偿。

发明内容

本发明针对现有技术的不足，本发明在初始化中采用非线性优化和λI-EKF(Lamdba Iterated Extended Kalman Filter)算法对尺度进行求精估计，并采用IDSF(Double Sensor Iterated Extended Kalman Filter)算法实现了单目相机与惯性传感器的融合，提供了一种尺度精度更高，在定位过程中保持更高精度、更高鲁棒性的一种单目视觉惯性组合的定位导航方法。

本发明解决其技术问题所采用的技术方案是：

步骤一、通过单目相机采集视频流；通过IMU采集IMU数据流；

步骤二、将采集的视频流和IMU数据流进行对齐，再将对齐后的视频流和IMU数据流打包在一起；

步骤三、系统数据初始化；若初始化成功，则顺序执行步骤四和步骤五；若初始化失败，则重新进行初始化；初始化过程包括视觉初始化、IMU初始化、确定视觉坐标系和IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI-EKF对尺度进行求精估计；

步骤四、获取IMU数据流中的惯导数据，对惯导数据进行互补滤波得到姿态变化量，再结合预积分计算得到IMU位姿，并将IMU位姿变化量给步骤五作为计算当前视觉帧位姿的参考变化量；

步骤五、逐帧提取视频流中的图像特征，利用提取的图像特征进行视觉跟踪和匹配，得到视觉位姿；

步骤六、判断步骤五中的视觉跟踪是否丢失，若视觉跟踪丢失并且不能重定位，则用IMU位姿进行运动定位；若视觉跟踪未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终用于定位导航的相机位姿。

所述步骤二中的对齐包括时间戳同步；打包包括数据打包。

所述步骤三中初始化的步骤具体包括：

3.1)视觉初始化：选取特征点数目大于设定值的连续图像帧进行匹配，当前后帧匹配点对大于设定值时，进行视觉初始化；如果匹配点对小于设定值时，重新开始选择图像帧；如果可以进行视觉初始化，运用RANSAC 8点法同时计算单应矩阵和基础矩阵，择优选择计算得到相机的位姿，之后用视觉里程计持续计算相机位姿，同时对相机位姿计算得到视觉位姿；

3.2)输出视觉初始化得到的视觉位姿；