[发明专利]基于稀疏直接法的视觉惯性里程计位姿估计方法

权利要求书

1.基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，具体包括如下步骤：

步骤一：在载体上安装俯视单目相机VO和惯性传感器IMU；对VO和IMU采集到的数据进行时间同步；

步骤二：对VO和IMU进行联合初始化，并同步VO和IMU的轨迹；

步骤三：对第k+1时刻VO采集到的图像建立N层图像金字塔，由上往下依次对图像金字塔的每一层进行光度补偿，从而更新该第k+1时刻图像；

步骤四：建立滑动窗口，将更新后的第k+1时刻图像放入窗口，根据更新后的第k+1时刻图像，计算在第k+1时刻时VO的位姿，若该位姿与上一个关键帧对应的VO位姿之间的差值大于等于预设的阈值，则计算窗口中第y个关键帧中每个像素点的梯度值，并由大到小排列，选择前300个像素点作为第y个关键帧的路标点，y＝1,2,…,M,M为滑动窗口中关键帧的总个数，根据每一个关键帧中所有路标点的梯度值计算每一个关键帧对滑动窗口的贡献值，将贡献值最小的关键帧边缘化，同时将更新后的第k+1时刻图像作为一个关键帧；基于更新后的第k+1时刻图像与上一个关键帧之间的VO视觉残差、更新后的第k+1时刻图像与上一个关键帧之间的IMU惯性残差以及上一次优化计算得到的先验残差，建立基于窗口的非线性最小二乘函数，并求解该非线性最小二乘函数，得到第k+1时刻里程计位姿的最优估计值，将被边缘化的图像帧对应的测量信息转换为先验信息约束加入到滑动窗口，得到用于下一次优化计算的先验残差，并继续将下一个时刻的图像放入滑动窗口中进行计算；若第k+1时刻的VO位姿与上一个关键帧对应的VO位姿之间的差值小于预设的阈值，则删除第k+1时刻图像，并继续将下一个时刻的图像放入滑动窗口中进行计算；

所述步骤三具体为：

步骤3.1：对第k+1时刻VO采集到的原始图像建立一个N层图像金字塔，该图像金字塔的分辨率由下往上逐渐降低，最底层为分辨率最高的原始图像；

步骤3.2：在该图像金字塔的顶层图像的中心选择一块大小为J×L的矩形区域作为模板，计算该模板的平均灰度值，将该平均灰度值作为第k+1时刻图像金字塔的灰度值，得到第k+1时刻图像金字塔与第k时刻图像金字塔的灰度值的差值，将该差值作为第k+1时刻图像金字塔中每一层图像的光度补偿值，从而对图像金字塔中每一层图像进行更新。

2.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤二中同步VO和IMU的轨迹包括将IMU坐标系转换到VO坐标系下，具体为：

步骤2.1:计算相邻时刻之间IMU测得旋转积分相邻时刻之间VO测量到的旋转并建立如下几何约束条件：

其中，表示四元数乘法，q_bc为VO和IMU之间的旋转外参；

步骤2.2：根据q_bc，计算得到在VO坐标系下从k时刻到k+1时刻IMU的旋转为：

3.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中，计算第k+1时刻VO的位姿具体为：将更新后的第k+1时刻图像中每个像素点的梯度值由大到小排列，选择前300个点作为该图像的路标点，根据这些路标点的最小化光度误差计算第k+1时刻VO位姿的李代数，并通过稀疏直接法求解该李代数，得到第k+1时刻VO的位姿。

4.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中，VO视觉残差为第k+1时刻图像中所有路标点灰度值之和与上一个关键帧中所有路标点灰度值之和的差值。

5.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中IMU惯性残差为，对第k+1时刻图像与上一个关键帧之间IMU测得的数据进行预积分，从而得到IMU误差传递函数，利用IMU的误差传递函数处理预积分量中的噪声，从而得到IMU惯性残差。