[发明专利]一种面向视觉惯性定位的基于地图的一致高效滤波算法

权利要求书

1.一种面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的算法通过以下系统来实现，所述的系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，所述的局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；所述的地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；所述的基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考系和地图参考系的相对变换，进而计算得到机器人在地图参考系下的状态。

2.根据权利要求1所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的基于局部特征观测的MSOC-S-IKF状态更新模块。

3.根据权利要求2所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的IMU用于为系统提供实时的旋转角速度和线加速度；所述的状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将系统的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量及其对应的协方差；获得的状态变量与协方差信号传递给基于局部特征观测的MSOC-S-IKF状态更新模块；所述的特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给基于局部特征观测的MSOC-S-IKF状态更新模块；所述的基于局部特征观测的MSOC-S-IKF状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用该发明所提出的不变卡尔曼滤波结合多状态约束来更新状态变量与协方差。

4.根据权利要求3所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的状态变量及其对应的协方差包含以下变量：

1)系统当前时刻t本体的状态：包括由旋转矩阵和平移向量组成的位姿，线速度

2)局部参考系下的特征点

3)IMU传感器的关于角速度与线加速度的偏差构成变量

4)系统过去s个时刻的本体位姿s为所设定的滑动窗口的大小，位姿由表示朝向的旋转矩阵和表示位置的向量组成。用于基于局部特征观测的MSOC-S-IKF状态更新模块；

5)局部里程计参考系和地图参考系之间的位姿变换^LT_G，由表示朝向的旋转矩阵^LR_G和表示位置的向量^Lp_G组成，该变量需要在基于地图的定位模块中的利用PnP初始化增广变量模块加入到状态变量中；

6)地图关键帧的位姿位姿由表示朝向的旋转矩阵和表示位置的向量组成，该变量在基于地图的定位模块中将地图关键帧位姿加入系统状态模块加入到状态变量中。