【技术实现步骤摘要】
利用路标点偏置位姿修正的双目视觉惯性里程计方法
[0001]本专利技术属于计算机
，涉及SLAM(即时定位与地图构建)
，主要涉及了一种利用路标点偏置位姿修正的双目视觉惯性里程计方法。
技术介绍
[0002]对于协同SLAM系统，回环检测不仅包括单个机器人非相邻帧之间的回环测量(intra
‑
loop closures)，还需要包括不同机器人的帧间的相对位姿变换，称之为机器人之间的回环测量(inter
‑
loop closures)。正是由于回环检测的引入，里程计带来的累积误差才得以消除，保证了轨迹与地图长时间(long term)的正确性和稳定性。然而，异常回环的存在会使得优化器的求解结果难以收敛，因此在后端优化过程中减少或者剔除假阳性回环的影响，可以降低优化失败的概率，对SLAM系统的鲁棒性提升至关重要。
[0003]综上，针对单机器人的假阳性回环剔除方法，大多不适合在线地增量式地剔除假阳性回环，而针对多个机器人之间假阳性回环剔除相关研究较少，且这两类方法均具有对人...
【技术保护点】
【技术特征摘要】
1.利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于，包括以下步骤：S1，数据预处理：对输入的双目图像进行数据预处理，得到筛选后的关键帧；所述数据预处理至少包括提取特征点并进行光流追踪、IMU预积分以及关键帧筛选；S2，视觉惯性里程计快速初始化：采用视觉和IMU的松耦合进行视觉惯性里程计快速初始化，双目相机通过三角测量恢复特征点深度，利用PnP算法进行纯视觉的相机位姿估计，再将其与IMU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度；所述初始化至少包括陀螺仪偏置校正、初始化速度、重力向量和尺度因子；S3，位姿估计修正：根据BA优化的目标函数、双目立体视觉相机观测方程推导出路标点偏置与位姿估计误差关联关系，推导出光流跟踪特征点过程中路标点的逆深度偏置，利用关联关系对位姿估计进行修正；S4，后端非线性优化：基于紧耦合的后端非线性优化方法，构建优化误差函数迭代求解，实现对系统状态量进行紧耦合的非线性优化以得到更精确的定位数据；所述系统状态量包括相机位姿、IMU加速度计偏置、陀螺仪偏置、相机到IMU的外参、三维路标点的逆深度。2.如权利要求1所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S1具体包括：S11：采用特征点法估计相机位姿，提取特征点，并采用光流跟踪法匹配特征点；S12：采用IMU预积分方法对测量值之间的相对状态量进行处理，给出预积分状态的协方差矩阵以及雅可比矩阵，实现与绝对位姿的解耦，构建误差函数对关键帧位姿的迭代优化；所述相对状态量至少包括当前相对于上一时刻的姿态和速度、位移；S13：关键帧筛选：满足以下两个条件之一就判定为关键帧：(a)根据视差判断，通过计算光流跟踪的Shi
‑
Tomasi角点的视差来进行关键帧筛选，若视差大于设定阈值，则将其判定为关键帧；(b)根据特征点数量判断，若图像帧跟踪的特征点数量没有超过设定阈值，将其设定为关键帧。3.如权利要求2所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S12中，通过IMU预积分方法，给出预积分状态的协方差矩阵以及雅可比矩阵，即推导出t+δt时刻雅可比矩阵J
t+δt
的迭代关系：其中，J代表IMU测量值的误差的雅可比矩阵；I代表单位矩阵；F代表线性误差系数。4.如权利要求2所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S2中，对陀螺仪偏置校正，得出陀螺仪偏置b
ω
：其中，是b
ω
是对应雅可比矩阵对的偏导；是前者的转置矩阵，代表克罗内克积；代表k
‑
1帧到k+1帧的旋转矩阵；代表第k帧对应IMU坐标系到上一帧的旋转矩阵；代表第k+1帧对应IMU坐标系到帧的旋转矩阵，vec代表矩阵向量化；
利用Cholesky分解求解下式，恢复出待优化变量即可得到初始化后的速度、重力向量和尺度因子：其中，I代表单位矩阵；代表参考帧下的重力向量；代表k帧对应速度；代表k+1帧对应速度；s代表尺度因子；和代表相邻两IMU的预积分项；代表参考帧到第k帧IMU的旋转矩阵；代表第k帧相机到参考帧的旋转矩阵；Δt
k
代表间隔时间；代表IMU到相机的旋转矩阵。5.如权利要求3或4所述利用路标点偏置位姿修正的双目视觉惯性里程计方法，其特征在于：所述步骤S3中推导路标点偏置与位姿估计误差关联关系具体为：将相机位姿以及三维点同时作为优化变量，非线性最小二乘问题被建模为最小化重投影误差，即集束调整(Bundle Adjustment,BA)，BA...
【专利技术属性】
技术研发人员：张小国，张圣旻，张梓涵，王慧青，邓奎刚，
申请(专利权)人：东南大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。