首页 > 代码库 > Google开源SLAM软件cartographer中使用的UKF滤波器解析

Google开源SLAM软件cartographer中使用的UKF滤波器解析

  在Google开源SLAM软件cartographer中,相对《SLAM for dummies》使用了更为复杂、性能更好的Scan匹配与UKF算法,这里简单介绍下cartographer中使用的UKF算法。

  (一)滤波器参数设定

      constexpr static FloatType kAlpha = 1e-3;

      constexpr static FloatType kKappa = 0.;

      constexpr static FloatType kBeta = 2.;

      constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;

      constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);

      constexpr static FloatType kCovWeight0 = kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);

      constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));

      constexpr static FloatType kCovWeightI = kMeanWeightI;

  (二)关键成员变量

      GaussianDistribution<FloatType, N> belief_;

     const std::function<StateType(const StateType& state, const StateType& delta)>  add_delta_;

      const std::function<StateType(const StateType& origin, const StateType& target)>  compute_delta_;

  (三)核心函数处理流程

  1Predict函数

  该函数与UKF标准算法基本相同。利用预测函数g更新状态变量x及其协方差。具体到SLAM场景下,一般说来,预测函数是基于IMU传感器感知数据或者机器人的移动执行指令,来更新Lidar的位姿。

  (1)检查协方差是否为对称正定矩阵,如果不是,则退出,否则计算状态变量x的均值,协方差的平方根,这一步是计算复杂度最高的一步。

  (2)基于均值,协方差平方根构造2N+1个Sigma点集,N为状态变量x的维度:

   (3)对Sigma点集中每个点进行的均值执行非线性预测函数g的变换,计算其均值及协方差,并确认协方差为对称矩阵。预测函数仅计算Translation之和,不计算方向角之和,因此在从状态变量x估计Lidar位姿时需要在方向角上乘以IMU估计并维护的方向角。

  (4)协方差加上原始协方差,作为预测变量的协方差,均值作为预测变量的均值。

  以上步骤又称为UT变换。

  2Observe函数

  Observe函数总在Predict函数之后调用,利用观测函数h进一步修正状态变量x及其协方差,以及Kalman增益。具体到SLAM场景下,一般说来,观测函数指观测值即Lidar扫描扇区的值与Lidar位姿之间的函数关系。在cartographer中,由于观测值直接使用Scan-Submap匹配修订了Lidar位姿,因此不存在闭合形式的观测函数h。为了解决这个问题,cartographer采用了Scan-Submap匹配之后得到的Lidar位姿修订值与系统状态变量均值的矢量差作为预测函数h。因此,对应的观测过程与标准Kalman滤波有所差别。

  (1)检查协方差是否为对称正定矩阵,如果不是,则退出,否则获得预测变量的均值及协方差。计算协方差的平方根。

  (2)基于均值,协方差平方根构造2N+1个Sigma点集,N为状态变量x的维度:

  (3)对Sigma点集中每个点进行的均值执行非线性观测函数h的变换,得到,计算的均值作为观测变量的均值,及的协方差,并确认为对称矩阵。协方差加上协方差作为观测变量的协方差。在cartographer中,参见PoseTracker::AddPoseObservation函数,观测函数h表示为:,其中表示Scan-Submap匹配得到的位姿,减号表示矢量差(即包含translation差和方位角orientation差)。

  (4)计算预测变量与观测变量之间的协方差。

  (5)计算Kalman增益,基于Kalman增益更新经过观测过程后修正的系统状态变量x的均值及其协方差。

  (四)背后的核心算法

  见百度文库《UKF滤波算法》,网址为http://wenku.baidu.com/link?url=er-Uo_5unEZGIcyg14wzZydaB5cqaUE-yAQddxCC7rS_rBtBBtsFgtVkVETRWZTs30B3bYkffqGaOzquxRT-MKFd3yibcHMfhs0BKtjjOCy。

      需要带有完整公式的PDF格式内容,请E-mail:huangkui@lbsense.com

Google开源SLAM软件cartographer中使用的UKF滤波器解析