一种视觉惯性里程计系统技术方案

本发明专利技术提供一种视觉惯性里程计系统，包括数据采集模块和数据处理模块，数据采集模块用关于采集图像信息、角速度和加速度；数据处理模块用于执行以下预处理和后端优化算法；本发明专利技术算法使用视觉/惯性紧组合导航技术，对比传统视觉里程计，该视觉惯性里程计系统可以在高动态情况下稳定运行且有较好的性能水平；相比于基于松组合的视觉惯性里程计，本发明专利技术能够充分传感器的信息，在使用相同水平的传感器时发挥出更好的性能；定位精度可以达到分米级，无需依赖卫星信号。需依赖卫星信号。需依赖卫星信号。

【技术实现步骤摘要】
一种视觉惯性里程计系统


[0001]本专利技术涉及视觉惯性导航
，尤其涉及一种视觉惯性里程计系统。

技术介绍

[0002]随着无人机、无人车以及移动机器人的井喷式发展，导航技术成为了制约无人平台广泛应用的瓶颈技术之一。SLAM技术一直被广泛用于无人机、无人车以及移动机器人的导航，即自主导航领域，SLAM是“Simultaneous Localization And Mapping”的缩写，译为同步定位与地图构建。经过了三十多年的发展，视觉SLAM——即使用视觉传感器作为主要信息输入的SLAM技术，由于其成本的低廉且图像信息更为丰富，逐渐成为了SLAM研究热点之一。
[0003]视觉SLAM算法框架主要分为视觉里程计和地图构建两大模块，视觉里程计输出系统的位置姿态信息，地图构建模块通过结合视觉里程计的信息和图像信息完成地图构建。其中，单目视觉SLAM是使用单目相机作为主要传感器的SLAM技术，拥有系统结构简单、成本较低的优势，但是由于单目相机不能得到绝对深度信息，因而实现难度也是最大的；且只使用相机的视觉里程计对于高动态运动情况(即有较大的旋转运动)，容易丢失特征点而导致里程计失效。
[0004]故而提高定位精度，特别是高动态情况下的定位精度，成为了视觉里程计的重要研究方向。在应用需求的牵引下，使用视觉/惯性组合导航技术的视觉惯性里程计，特别是视觉与微惯性传感器的组合，逐渐发展成为当前自主导航及机器人领域的研究热点。视觉/惯性组合导航具有显著的优点：(1)微惯性器件和视觉传感器具有体积小、成本低的优点，随着制造技术的不断进步，器件越来越小，且成本越来越低。(2)不同于卫星和无线电导航，视觉和惯性导航均不依赖外部设施支撑，可以实现自主导航。(3)惯性器件和视觉器件具有很好的互补性，惯性导航误差随时间累积，但是在短时间内可以很好地跟踪载体快速运动，保证短时间的导航精度；而视觉导航在低动态运动中具有很高的估计精度，且引入了视觉闭环矫正可以极大地抑制组合导航误差，两者的组合可以更好地估计导航参数。
[0005]视觉/惯性组合导航技术以组合方式可以分为松组合和紧组合：松组合把视觉传感器和惯性传感器分别作为独立的模块进行组合，两个模块分别使用视觉导航算法和惯性导航算法来获得系统的导航信息，然后再对两个信息进行融合滤波，来达到更好的导航结果；紧组合则是使用两个传感器的原始数据，然后构建一个基于最小二乘法的优化框架，把所有数据都放入框架内进行优化，最后得到导航结果。松组合的优点是结构简单，实现难度较低，但缺点是性能较低，达不到很好的精度；紧组合可以达到很好的性能水平，但是结构复杂，实现难度大。

技术实现思路

[0006]为了在保证导航精度的同时，降低算法复杂度，提高处理效率，本专利技术提供一种视觉惯性里程计系统，包括数据采集模块和数据处理模块，所述数据采集模块用关于采集图
像信息、角速度和加速度；所述数据处理模块用于执行以下预处理和后端优化算法：
[0007]预处理：
[0008]对采集的所述图像信息、角速度和加速度进行时间同步对齐；
[0009]针对当前图像信息，提取图像Harris角点，跟踪相邻帧与所述当前图像信息的图像Harris角点匹配的特征点；
[0010]利用所述采集的角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵PVQ；
[0011]计算所述相邻帧的预积分增量，以及误差传递矩阵；
[0012]后端优化：
[0013]判断所述当前图像信息的特征点，与前一帧图像匹配的特征点之间的相对位移是否超过阈值，如是，判定所述当前图像信息为关键帧，边缘化滑动窗口内最老的图像帧，并将该关键帧存入所述滑动窗的最新位置；若否，则丢弃前一图像帧，并将所述当前图像信息和前一图像帧之间的预积分增量，转换为所述当前图像信息与前二图像帧之间的预积分增量；
[0014]构建由边缘化的先验信息、测量残差、视觉重投影误差构成的目标函数，通过优化目标函数来得到系统状态向量的最优解，所述系统状态向量包括系统的位置、姿态、速度、加速度计误差和、陀螺仪误差；基于最优解对应的系统状态向量得到导航信息。
[0015]进一步的，所述数据采集模块包括相机和惯性传感器，所述相机用于所述采集图像信息，所述惯性传感器用于所述采集角速度和加速度。
[0016]进一步的，所述利用所述角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵，包括通过如下积分运算得到：
[0017][0018]所述为k+1时刻世界坐标系下的的位置，所述为k时刻的世界坐标系下的位置，所述为k时刻世界坐标系下的的速度，所述为k+1时刻世界坐标系下的的速度，所述Δt
k
为k时刻到k+1时刻的时间差，所述为Body自身坐标系(body
‑
axis coordinate system)到Word世界坐标系的旋转矩阵，所述和分别为所述采集的加速度和角速度，所述g
w
为重力向量，所述和所述分别为加速度偏差和角速度偏差。
[0019]进一步的，所述计算所述相邻帧的预积分增量，通过如下预积分增量函数得到：
[0020][0021]式中，所述所述所述分别为式(1)中位置速度旋转矩阵的预积分增量真值；所述所述所述分别为所述数据采集模块测量值得到的预积分增量；所述所述为位置P矩阵的误差预积分增量；所述为速度V矩阵的误差预积分增量；所述为转移Q矩阵的误差预积分增量。
[0022]进一步的，所述目标函数包括：
[0023][0024]所述‖r
p
‑
J
p
X‖2为边缘化的先验信息，所述为测量残差，所述为视觉重投影误差。
[0025]进一步的，所述通过优化目标函数来得到系统状态向量的最优解包括：
[0026]当目标函数中的系统状态向量X有一个增量ΔX后，目标函数有最小值，构造一个关于增量ΔX的增量公式：
[0027][0028]上式中，为预积分误差项的协方差，为误差项r
B
关于所有状态向量X的Jacobian矩阵，为视觉观测的误差协方差矩阵；
[0029]对上述增量公式进行简化：
[0030](Λ
p
+Λ
B
+Λ
C
)ΔX＝b
p
+b
B
+b
C
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0031]其中，Λ
p
，Λ
B
，Λ
C
为Hessian矩阵；
[0032]通过最小二乘法求解上述简化后的增量公式，可得当前时刻系统状态向量X的最优解。
[0033]进一步的，所述方法还包括：
[0034]在进行首次预处理之后，且在首次进行后端优化处理之前，还需要进行一次初始化处理：首先利用Global SFM算法，求得当前收到的所有帧的位姿和所有路标点在世界坐标系本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种视觉惯性里程计系统，其特征在于，包括数据采集模块和数据处理模块，所述数据采集模块用关于采集图像信息、角速度和加速度；所述数据处理模块用于执行以下预处理和后端优化算法：预处理：对采集的所述图像信息、角速度和加速度进行时间同步对齐；针对当前图像信息，提取图像Harris角点，跟踪相邻帧与所述当前图像信息的图像Harris角点匹配的特征点；利用所述采集的角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵PVQ；计算所述相邻帧的预积分增量，以及误差传递矩阵；后端优化：判断所述当前图像信息的特征点，与前一帧图像匹配的特征点之间的相对位移是否超过阈值，如是，判定所述当前图像信息为关键帧，边缘化滑动窗口内最老的图像帧，并将该关键帧存入所述滑动窗的最新位置；若否，则丢弃前一图像帧，并将所述当前图像信息和前一图像帧之间的预积分增量，转换为所述当前图像信息与前二图像帧之间的预积分增量；构建由边缘化的先验信息、测量残差、视觉重投影误差构成的目标函数，通过优化目标函数来得到系统状态向量的最优解，所述系统状态向量包括系统的位置、姿态、速度、加速度计误差和、陀螺仪误差；基于最优解对应的系统状态向量得到导航信息。2.如权利要求1所述的视觉惯性里程计系统，其特征在于，所述数据采集模块包括相机和惯性传感器，所述相机用于所述采集图像信息，所述惯性传感器用于所述采集角速度和加速度。3.如权利要求1所述的视觉惯性里程计系统，其特征在于，所述利用所述角速度和加速度进行积分，得到当前时刻的位置速度旋转矩阵，包括通过如下积分运算得到：所述为k+1时刻世界坐标系下的的位置，所述为k时刻的世界坐标系下的位置，所述为k时刻世界坐标系下的的速度，所述为k+1时刻世界坐标系下的的速度，所述Δt
k
为k时刻到k+1时刻的时间差，所述为Body自身坐标系到Word世界坐标系的旋转矩阵，所述和分别为所述采集的加速度和角速度，所述g
w
为重力向量，所述和所述分别为加速度偏差和角速度偏差。4.如权利要求1所述的视觉惯性里程计系统，其特征在于，所述计算所述相邻帧的预积
分增量，通过如下预积分增量函数得到：式中，所述所述所述分别为式(1)中位置速度旋转矩阵的预积分...

【专利技术属性】
技术研发人员：董子乐，余崇圣，李梦，陈方圆，王建波，
申请(专利权)人：重庆知至科技有限公司，
类型：发明
国别省市：