[发明专利]一种基于最大后验IMU的线特征SLAM初始化方法

权利要求书

1.一种基于最大后验IMU的线特征SLAM初始化方法，其特征在于步骤如下：

步骤1：采用LSD算法对相机拍摄图像进行边缘提取，得到若干条带；

步骤2、添加LBD描述子：计算出每个条带对应的特征向量BD_j，将特征向量BD_j与条带并将生成LBD描述子；

步骤3、线特征初始化：将连续三帧图像对应的LBD描述子构建一个三焦点张量的矩阵并求解，取数值最小的一组解得到位姿信息获得轨迹的先验信息，完成位姿初始化；

步骤4、IMU初始化：以轨迹的先验信息进行IMU数据的优化；

将所有轨迹和关键帧的位姿固定，仅仅优化IMU的相关参数：

其中,s也就是整个地图的尺度；R_wg∈SO₍₃₎是重力加速度方向，表示为g＝R_wgg_ig_i＝(0,0,G)^T,表示IMU的加速度和角速度偏差，是所有的关键帧对应的无尺度的速度；

相对于真值s*v表示的速度,使用速度同时，在初始化的过程中偏置的值固定不变，将重力加速度纳入到优化公式中；所有的测量值都是IMU的变量，同时用L_i,j为IMU从第i帧到第j帧之间的预积分，L_0,k表示在初始化窗口的连续关键帧之间的预积分；

制定一个最大后验估计：

p(χ_k|L_0:k)∝p(L_0:k|χ_k)p(χ_k)

其中，p(L_0:k|χ_k)表示在给定第k帧IMU数据的情况下,从0到k帧的可能的IMU测量值分布可能性；p(χ_k)为IMU状态的先验；

将其分解为：

找到参数使后验概率最大，意味着使其负对数的值更小：

IMU的预积分都是服从高斯分布的，则MAP问题等价于：

其中，r_p和为先验值和IMU测量值的残差，而∑_p和为他们的协方差；

优化的过程中；

定义旋转矩阵，速度，位移的误差如下：

步骤5、视觉惯性联合初始化：将视觉初始位姿与IMU初始化结果即旋转矩阵，速度，位移的误差作一阶线性相似，对其求解最小二乘得到速度，加速度和尺度信息，完成初始化。