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

权利要求书

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

第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]内的位移、速度和旋转表示如下：

把公式(1)代入公式(2)，并从变换到得到IMU预积分公式如下：

公式(3)中，和分别为两帧之间的位移、速度旋转差量；由公式可以看出惯性测量单元的当前关键帧的测量值与之前帧无关，仅由和决定；

第2，估计旋转外参数

第2.1，运动恢复结构

视觉初始化使用运动恢复结构(SFM)方法估计滑动窗口内的所有关键帧的位姿；首先，将第一帧相机帧作为第一个参考关键帧，然后选择一个与当前帧具有更多公共点和足够视差的帧作为第二个参考关键帧；通过五点法或八点法估算当前帧和参考关键帧之间的旋转和平移；然后，利用三角化和PnP算法以及捆集调整(BA)获得滑动窗口内所有关键帧的位姿；在此基础上，得到第k个和第k+1个摄像机关键帧之间的旋转和缩放平移

第2.2，旋转外参数

根据坐标系之间的变换，可得到以下关系：

其中由IMU陀螺仪在IMU坐标系下积分得到，通过视觉初始化得到，是需要估计的旋转外参数矩阵；

把以上旋转矩阵用四元数表示得到以下公式：

将Q₁和Q₂定义为四元数的左乘和右乘，得到以下公式：

变换后得到：

随着关键帧数量的增加，得到以下超线性方程组：

其中N为关键帧数量的最大值，为剔除重投影误差的权重，利用终止准则可以得到旋转外参数；

第3，视觉惯性初始化

第3.1，陀螺仪偏差校正

在相机坐标系下，设置第一个参考关键帧后，通过SFM算法得到第k帧和参考关键帧之间的旋转然后可以由以下公式得到：

对于滑动窗口内的两个IMU关键帧和和可由公式(9)得到，相对旋转由公式(3)获得；因此陀螺仪偏差的目标方程表示如下：

其中：

因此，最小化公式(10)可以得到陀螺仪偏差b_w；相对位移和相对速度根据即时计算得出的b_w进行更新；

第3.2，平移外参数、绝对尺度和重力向量

通过以上分析，所有的参数都可以从IMU坐标系转换到相机坐标系下：

其中s是尺度因子，是缩放的位移；绝对尺度和缩放尺度之间的关系如下：

所以(3)式可写成如下形式：

通过公式(11)(12)(13)，和可以被表示为：

假设关键帧的下标i，i+1，i+2分别为1，2，3，并把(15)式的两个式子放在一起得到：

其中：

通过公式(16)，利用最小二乘算法可得到平移外参数重力向量和绝对尺度s；

第3.3，重力方向矫正

估计出的重力向量受加速度计偏差的影响，假设重力向量的幅值是一个常量，重力向量的方向由于偏差而发生改变；因此，此时矫正重力向量可以设为两个自由度：

其中为重力向量的幅值，w₁b₁和w₂b₂是分别来自两个正交方向的扰动；b为扰动的方向，w为对应的b方向的扰动量；

把公式(18)代入(16)得到：

其中：

通过求解公式(20)，重力向量被重新矫正；在此基础上，和坐标系下的所有状态变量都可以变换到世界坐标系下；此外，平移和地图点的坐标可以根据得到的绝对尺度s进行缩放；

第3.4，重力方向矫正

重力和绝对尺度得到后，每一关键帧的速度可以由公式(2)和(15)计算得到；在视觉惯性初始化时没有估计加速度计偏差b_a，因为它对初始化的影响并不是很大，并且会在后端的VIO紧耦合优化中被估计出来。