[发明专利]同时优化外参数的视觉惯性里程计系统

说明书

对于机器人系统中的视觉惯性里程计，可以通过高效的初始化来提高最终定位的性能。本文提出了一种基于单目视觉惯性系统的初始化方法，该方法可以同时优化相机与惯性测量单元之间的外部参数。首先，通过视觉惯性信息估计旋转外参数、陀螺仪偏差、绝对尺度和重力矢量。然后，在视觉惯性测量信息对齐阶段优化平移外参数，并通过之前的状态估计计算得到速度。当旋转角度不够大时，由于加速度计偏差对初始化几乎没有影响，因此在初始化阶段不进行估计，而是在后端优化中进行处理。对比实验结果验证了该方法的有效性。

技术领域

本发明属于计算机视觉与移动机器人的技术领域，特别是涉及需要同时优化外参数的视觉惯性里程计系统

背景技术

VO通过安装在机器人上的相机采集图像数据，根据图像特征和运动约束进行位姿估计，不会产生积累误差。但当视觉传感器数据采样率和机器人运动速度比较快时，会造成部分图像缺失而引起估计精度降低，同时单目相机也存在尺度不确定的问题。与视觉传感器不同，惯性传感器具有成本低、尺寸小等特点，能够以较高的频率采集物体的运动信息，非常适合应用在物体做高速运动的场景，对于短期运动预测精度较好，不会受到运动突然变化的影响，但其误差随时间累积严重。当今比较流行的解决方式是将视觉与IMU结合，通过IMU恢复出视觉的绝对尺度，同时利用视觉信息对IMU的偏差进行矫正。

发明内容

本文提供了一种方法，能够在视觉惯性里程计系统初始化时同时优化相机和IMU的外参数，这将会大大提高机器人定位与建图的精度。

本文提出了一种基于单目视觉惯性系统的初始化方法，该方法可以同时优化相机与惯性测量单元之间的外部参数。首先，通过视觉惯性信息估计旋转外参数、陀螺仪偏差、绝对尺度和重力矢量。然后，在视觉惯性测量信息对齐阶段优化平移外参数，并通过之前的状态估计计算得到速度。当旋转角度不够大时，由于加速度计偏差对初始化几乎没有影响，因此在初始化阶段不进行估计，而是在后端优化中进行处理。对比实验结果验证了该方法的有效性。

本发明提出的同时优化外参数的视觉惯性里程计系统包括：

同时优化外参数的视觉惯性里程计系统，其特征包括以下步骤：

第1，系统描述

第1.1，系统框架

所提出方法的系统框架，特别是视觉惯性对齐模块，如附图1所示。首先，通过运动恢复结构算法(SFM)获得两个连续相机帧之间的旋转和平移，该算法仅用于获得相机关键帧之间关系的。然后，采用IMU预积分算法来获得IMU两帧之间的差量，并在之前获得估计变量的基础上估算当前IMU关键帧的测量值。通过运动恢复结构和IMU预积分算法获得的相关关键帧的旋转和利用经典算法，使用收敛准则获得外部参数的旋转部分。其次，估计陀螺仪偏差、平移外参数、绝对尺度和重力向量，并重新纠正重力的方向。最后，每一帧的速度可以通过先前的状态估计计算得到。加速度计偏差通过后端VIO紧耦合优化进行补偿。

第1.2，IMU预积分

IMU的测量数据包括三轴角速度和三轴线性加速度偏差b和白噪声n影响加速度和角速度的精度；测量值和理想值之间的关系如下：

w_b和a_b分别为和的实际值，和分别为线性加速度和角速度的偏差。n_w和n_a分别为w_b和a_b的高斯白噪声，服从高斯分布。是世界坐标系到IMU坐标系的旋转矩阵，g^w是世界坐标系下的重力向量，所以表示的是重力向量在IMU坐标系下的值。

忽略噪声，IMU连续两帧和在时间间隔[t_k，t_k+1]内的位移、速度和旋转表示如下：