[发明专利]利用路标点偏置位姿修正的双目视觉惯性里程计方法

说明书

本发明公开了一种利用路标点偏置位姿修正的双目视觉惯性里程计方法，首先对输入的双目图像进行包括提取特征点并进行光流追踪、IMU预积分以及关键帧筛选的数据预处理，得到筛选后的关键帧；再采用视觉和IMU的松耦合进行视觉惯性里程计快速初始化；根据BA优化目标函数、位姿变化的雅可比矩阵函数推导出路标点偏置与位姿估计误差关联关系，求出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；最后基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对深度信息误差以及位姿估计误差的消除，减少轨迹误差。

技术领域

本发明属于计算机技术领域，涉及SLAM(即时定位与地图构建)技术领域，主要涉及了一种利用路标点偏置位姿修正的双目视觉惯性里程计方法。

背景技术

对于协同SLAM系统，回环检测不仅包括单个机器人非相邻帧之间的回环测量(intra-loop closures)，还需要包括不同机器人的帧间的相对位姿变换，称之为机器人之间的回环测量(inter-loop closures)。正是由于回环检测的引入，里程计带来的累积误差才得以消除，保证了轨迹与地图长时间(long term)的正确性和稳定性。然而，异常回环的存在会使得优化器的求解结果难以收敛，因此在后端优化过程中减少或者剔除假阳性回环的影响，可以降低优化失败的概率，对SLAM系统的鲁棒性提升至关重要。

综上，针对单机器人的假阳性回环剔除方法，大多不适合在线地增量式地剔除假阳性回环，而针对多个机器人之间假阳性回环剔除相关研究较少，且这两类方法均具有对人为设置参数敏感的特点。面对上述现状与问题，需要考虑如何在协同SLAM系统中首先能够剔除单个机器人的假阳性回环测量，为多机器人之间的相对位姿变换提供可靠。

发明内容

本发明正是针对现有技术中视觉惯性里程计位姿估计精度不足的问题，提供一种利用路标点偏置位姿修正的双目视觉惯性里程计方法，首先对输入的双目图像进行包括提取特征点并进行光流追踪、IMU预积分以及关键帧筛选的数据预处理，得到筛选后的关键帧；再采用视觉和IMU的松耦合进行视觉惯性里程计快速初始化；根据BA优化目标函数、位姿变化的雅可比矩阵函数推导出路标点偏置与位姿估计误差关联关系，求出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；最后基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对深度信息误差以及位姿估计误差的消除，减少轨迹误差。

为了实现上述目的，本发明采取的技术方案是：利用路标点偏置位姿修正的双目视觉惯性里程计方法，包括以下步骤：

S1，数据预处理：对输入的双目图像进行数据预处理，得到筛选后的关键帧；所述数据预处理至少包括提取特征点并进行光流追踪、I MU预积分以及关键帧筛选；

S2，视觉惯性里程计快速初始化：采用视觉和I MU的松耦合进行视觉惯性里程计快速初始化，双目相机通过三角测量恢复特征点深度，利用PnP算法进行纯视觉的相机位姿估计，再将其与I MU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度；所述初始化至少包括陀螺仪偏置校正、初始化速度、重力向量和尺度因子；

S3，位姿估计修正：根据BA优化目标函数、双目立体视觉相机观测方程推导出路标点偏置与位姿估计误差关联关系，推导出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；

S4，后端非线性优化：基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对系统状态量进行紧耦合的非线性优化以得到更精确的定位数据；所述系统状态量包括相机位姿、I MU加速度计偏置、陀螺仪偏置、相机到I MU的外参、三维路标点的逆深度。

作为本发明的一种改进，所述步骤S1具体包括：

S11：采用特征点法估计相机位姿，提取特征点，并采用光流跟踪法匹配特征点；