[发明专利]基于单目相机的VIO快速联合初始化方法

权利要求书

1.基于单目相机的VIO快速联合初始化方法，其特征在于，包括如下步骤：

(1)用ORB-SLAM处理几秒钟的视频以获得初始的位姿和几个关键帧；

(2)通过步骤(1)中的关键帧与IMU预积分构建代价函数计算陀螺仪偏差；

(3)通过解算连续帧间的线性模型求取重力加速度计偏置、重力加速度、重力加速度校准与重力加速度计偏置；

重力加速度计偏置估计具体为：两个连续关键帧之间的运动用两帧间所测量的预积分ΔR，Δv，Δp来描述，由预积分方程：

i和i+1分别是时刻，和分别是IMU的i+1时刻在世界坐标系下的旋转、速度和平移，ΔR_i，i+1、Δv_i，i+1和Δp_i，i+1分别是IMU的i和i+1时刻间的世界坐标系下的旋转、速度和平移增量，Δt_i，i+1是时间差，和分别是平移相对于角速度和加速度的雅各比矩阵，和分别是速度相对于角速度和加速度的雅各比矩阵，是旋转相对于角速度的雅各比矩阵，b_a和b_g表示加速度计和陀螺仪偏置，g_w是世界坐标系中的重力加速度矢量，w是世界表系，b是IMU坐标系，b_i和b_i+1分别是第i和i+1时刻的IMU坐标系；在世界参考坐标系中定义IMU测量得到的旋转平移以及速度

根据两个连续关键帧的已测得旋转对陀螺仪偏差进行估计，对于所有连续关键帧对，通过最小化陀螺仪计算IMU预积分和ORB-SLAM计算的相对旋转之间的差值来优化陀螺仪偏置：

N是关键帧的数量，是从IMU帧到世界帧的旋转矩阵，从相机坐标转到世界坐标系的旋转矩阵由ORB-SLAM进行计算，ΔR_i，i+1是由预积分计算出来的两个连续关键帧之间旋转矩阵，用高斯牛顿法对式(2)进行解算；

重力加速度估计具体为：获得陀螺仪偏差之后通过补偿陀螺仪偏置再用预积分准确计算出相机的位置、速度与旋转，由ORB-SLAM计算的摄像机轨迹的尺度值是一个相对值，所以当实际估计从IMU到相机的帧间位移时需要引入一个尺度值s，和分别是相机到世界坐标系下的平移和旋转，是IMU和相机之间的平移参数；

结合公式(3)和公式(1)，忽略加速度偏置得到：

目标是用线性方程组估计重力和未知尺度，为了减少运算的复杂性，运算过程中考虑三个连续关键帧之间的两个关系(4)和(3)中的速度关系以消除速度，并且分离变量如下：

将三个连续关键帧(5)之间的所有关系叠加到测量模型A_3(N-2)×4x_4×1＝B_3(N-2)×1中，通过奇异值分解SVD来求解模型以得到比例因子s^*和重力矢量因为有3(N-2)个方程和4个未知数，至少需要4个关键帧；

(4)通过步骤(3)中的重力加速度校准与参数分离求出尺度与速度信息。