基于卡尔曼滤波器融合的激光IMU激光里程计学习

SLAM||DUNK

已于 2024-03-27 08:03:21 修改

阅读量3.7k

点赞数 3

分类专栏：激光SLAM 多传感器融合文章标签：自动驾驶人工智能机器学习

于 2020-06-21 17:17:55 首次发布

本文链接：https://blog.csdn.net/qq_42700518/article/details/106888053

版权

激光SLAM 同时被 2 个专栏收录

14 篇文章 13 订阅

订阅专栏

多传感器融合

9 篇文章 2 订阅

订阅专栏

filter-lio模块

LINS
FAST-LIO

主要方案有FAST-LIO以及LINS。

LINS

该算法是直接在lego上面改的,主要针对的还是16线激光.
主要模块是:

Feature Extraction Module: 点云分割以及特征提取, 和legoloam的操作一样
LIO module: 融合IMU与激光的里程计.
Mapping Module: 局部地图优化以及回环检测与后端优化, 和legoloam一样
该算法的核心还是IMU与激光融合的LIO模块, 重点看这部分就可以了.

LIO模块运行的基本逻辑是，原始激光点云首先进入image_projection_node.cpp中执行数据预处理，比如滤除地面，聚类滤波等操作，然后将处理后的点云以及IMU数据传入lins_fusion_node.cpp 进一步执行特征提取、去畸变，以及计算里程计并融合。

1. 开始

首先理清LIO module相关的文件:
src/lins_fusion_node.cpp: LIO 节点, 构造了 fusion::LinsFusion 对象.
src/lib/Estimator.cpp: fusion::LinsFusion 类的实现, 该类相当于ROS接口, 将传感器数据通过ROS传入到算法内部中.
include/Estimator.h: fusion::LinsFusion 类的声明.
include/integrationBase.h: IMU 运动学模型相关.
include/KalmanFilter.hpp: 这里只是实现了使用IMU进行EKF状态预测的部分.
include/StateEstimator.hpp: LIO算法底层库的实现, 包含特征提取等.

重点在IMU的回调函数中，
流程:

将IMU数据与车体系对齐.

// 将IMU与车体坐标对齐  
alignIMUtoVehicle(misalign_euler_angles_, acc_raw_, gyr_raw_, acc_aligned_,
                  gyr_aligned_);

将IMU数据添加到 imuBuf_ 中.

// Add a new IMU measurement
Imu imu(imuMsg->header.stamp.toSec(), acc_aligned_, gyr_aligned_);
imuBuf_.addMeas(imu, imuMsg->header.stamp.toSec());                 // MapRingBuffer<Imu>

执行状态估计 performStateEstimation();

2. 融合的实现

核心函数 - performStateEstimation()，融合就是在这个函数中完成的。
主要逻辑：该函数在IMU的回调函数中执行，执行时，每次都去检查最老的点云数据是否被IMU数据覆盖，如果覆盖，那么循环执行IMU的IKF预测过程，接着执行点云的IKF校正过程。
在这里插入图片描述

流程:

检查数据是否充分

if (imuBuf_.empty() || pclBuf_.empty() || cloudInfoBuf_.empty() ||
   outlierBuf_.empty())
 return;

检查是否进行初始化 ,没执行初始化则执行 processFirstPointCloud();
while()循环 , 反复执行 processPointClouds();

2.1 processFirstPointCloud()

如果初始化未完成,则进入这个函数进行处理.
流程:

获取最新的分割后的点云为 distortedPointCloud, 以及外点点云 outlierMsg, 当前点云信息 cloudInfoMsg , 以及最新的imu数据.

调用下面函数完成处理

 estimator->processPCL(scan_time_, imu, distortedPointCloud, cloudInfoMsg,
 outlierPointCloud);

2.2 processPointClouds()

初始化后，在这个函数中完成完整的LIO流程。
流程：
1、获取上一次估计后第一个点云数据
2、检查最后一个IMU的时间戳是否晚于该点云时间，如果比点云的时间早，认为IMU没有覆盖两帧点云直接退出。
3、对于当前帧时间戳之前的IMU数据，循环执行IMU预测过程，
estimator->processImu(dt, imu.acc, imu.gyr);
4、执行IESKF的观测更新：

// Update the iterative-ESKF using a new PCL
  estimator->processPCL(scan_time_, imu, distortedPointCloud, cloudInfoMsg,
                        outlierPointCloud)

大概的流程就是这样了，可以看到最后数据都流向了StateEstimator类的processImu()和processPCL()两个函数中，下面学习这两个函数。

3. StateEstimator类

globalState_ 是当前机体坐标系(IMU系)相对于世界坐标系的状态。
filter_->state_ 即StatePredictor类的state_表示robocentric的状态(k+1帧想对于k帧的状态)，IESKF估计的就这个状态。
在processScan()中，performIESKF()执行完IESKF的校正过程后，会得到后验robocentric状态，然后执行integrateTransformation()更新到全局状态中：

  void integrateTransformation() {
    GlobalState filterState = filter_->state_;
    globalState_.rn_ = globalState_.qbn_ * filterState.rn_ + globalState_.rn_;
    globalState_.qbn_ = globalState_.qbn_ * filterState.qbn_;
    globalState_.vn_ =
        globalState_.qbn_ * filterState.qbn_.inverse() * filterState.vn_;
    globalState_.ba_ = filterState.ba_;
    globalState_.bw_ = filterState.bw_;
    globalState_.gn_ = globalState_.qbn_ * filterState.gn_;
  }

然后对滤波器进行重置：

filter_->reset(1)

将执行这段代码：

      V3D covPos = INIT_POS_STD.array().square();   // 初始位姿协方差 
      double covRoll = pow(deg2rad(INIT_ATT_STD(0)), 2);
      double covPitch = pow(deg2rad(INIT_ATT_STD(1)), 2);
      double covYaw = pow(deg2rad(INIT_ATT_STD(2)), 2);

      M3D vel_cov =
          covariance_.block<3, 3>(GlobalState::vel_, GlobalState::vel_);
      M3D acc_cov =
          covariance_.block<3, 3>(GlobalState::acc_, GlobalState::acc_);
      M3D gyr_cov =
          covariance_.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_);
      M3D gra_cov =
          covariance_.block<3, 3>(GlobalState::gra_, GlobalState::gra_);

      covariance_.setZero();
      covariance_.block<3, 3>(GlobalState::pos_, GlobalState::pos_) =
          covPos.asDiagonal();  // pos    位置的协方差设置为初始默认值
      covariance_.block<3, 3>(GlobalState::vel_, GlobalState::vel_) =
          state_.qbn_.inverse() * vel_cov * state_.qbn_;  // vel
      covariance_.block<3, 3>(GlobalState::att_, GlobalState::att_) =
          V3D(covRoll, covPitch, covYaw).asDiagonal();  // att   姿态的协方差设置为初始默认值
      covariance_.block<3, 3>(GlobalState::acc_, GlobalState::acc_) = acc_cov;
      covariance_.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_) = gyr_cov;
      covariance_.block<3, 3>(GlobalState::gra_, GlobalState::gra_) =
          state_.qbn_.inverse() * gra_cov * state_.qbn_;

      state_.rn_.setZero();
      state_.vn_ = state_.qbn_.inverse() * state_.vn_;
      state_.qbn_.setIdentity();
      state_.gn_ = state_.qbn_.inverse() * state_.gn_;
      state_.gn_ = state_.gn_ * 9.81 / state_.gn_.norm();

重置协方差矩阵covariance_，重置robocentric状态。

3.1 状态预测 processImu()

switch (status_) {
      case STATUS_INIT:
        break;
      case STATUS_FIRST_SCAN:   // 第一帧-第二帧之间
        preintegration_->push_back(dt, acc, gyr);    // 进行预积分  
        filter_->time_ += dt;
        acc_0_ = acc;
        gyr_0_ = gyr;
        break;
      case STATUS_RUNNING:     // 第二帧后 
        filter_->predict(dt, acc, gyr, true);
        break;
      default:
        break;
    }

第一帧-第二帧之间时此时滤波器还没有初始化，会进行预积分用于估计之后的匹配初值，第二帧后滤波器初始化了就开始执行ESKF的预测传播过程。
预测步骤如下：
首先对robocentric状态进行传播

    GlobalState state_tmp = state_;
    V3D un_acc_0 = state_tmp.qbn_ * (acc_last - state_tmp.ba_) + state_tmp.gn_;
    V3D un_gyr = 0.5 * (gyr_last + gyr) - state_tmp.bw_;
    Q4D dq = axis2Quat(un_gyr * dt);
    state_tmp.qbn_ = (state_tmp.qbn_ * dq).normalized();
    V3D un_acc_1 = state_tmp.qbn_ * (acc - state_tmp.ba_) + state_tmp.gn_;
    V3D un_acc = 0.5 * (un_acc_0 + un_acc_1);

    // State integral
    state_tmp.rn_ = state_tmp.rn_ + dt * state_tmp.vn_ + 0.5 * dt * dt * un_acc;
    state_tmp.vn_ = state_tmp.vn_ + dt * un_acc;

误差状态的均值由于恒定为0，所以不需要传播，只需要更新误差状态的协方差矩阵：

   if (update_jacobian_) {
      MXD Ft =
          MXD::Zero(GlobalState::DIM_OF_STATE_, GlobalState::DIM_OF_STATE_);
      Ft.block<3, 3>(GlobalState::pos_, GlobalState::vel_) = M3D::Identity();

      Ft.block<3, 3>(GlobalState::vel_, GlobalState::att_) =
          -state_tmp.qbn_.toRotationMatrix() * skew(acc - state_tmp.ba_);
      Ft.block<3, 3>(GlobalState::vel_, GlobalState::acc_) =
          -state_tmp.qbn_.toRotationMatrix();
      Ft.block<3, 3>(GlobalState::vel_, GlobalState::gra_) = M3D::Identity();

      Ft.block<3, 3>(GlobalState::att_, GlobalState::att_) =
          - skew(gyr - state_tmp.bw_);
      Ft.block<3, 3>(GlobalState::att_, GlobalState::gyr_) = -M3D::Identity();

      MXD Gt =
          MXD::Zero(GlobalState::DIM_OF_STATE_, GlobalState::DIM_OF_NOISE_);
      Gt.block<3, 3>(GlobalState::vel_, 0) = -state_tmp.qbn_.toRotationMatrix();
      Gt.block<3, 3>(GlobalState::att_, 3) = -M3D::Identity();
      Gt.block<3, 3>(GlobalState::acc_, 6) = M3D::Identity();
      Gt.block<3, 3>(GlobalState::gyr_, 9) = M3D::Identity();
      Gt = Gt * dt;

      const MXD I =
          MXD::Identity(GlobalState::DIM_OF_STATE_, GlobalState::DIM_OF_STATE_);
      F_ = I + Ft * dt + 0.5 * Ft * Ft * dt * dt;

      // jacobian_ = F * jacobian_;
      covariance_ =
          F_ * covariance_ * F_.transpose() + Gt * noise_ * Gt.transpose();
      covariance_ = 0.5 * (covariance_ + covariance_.transpose()).eval();
    }

3.2 观测校正 processPCL()

在这里插入图片描述第一帧和第二帧直接点云匹配求解速度信息，然后初始化滤波器，初始化后执行processScan()来执行滤波器的校正环节。

3.2.1 processSecondScan()

在这里插入图片描述
源码中用预积分求解帧间预测的部分有点问题，linState_.gn_ 此时是 $(0.0, 0.0, - G 0)$ , 但是实际上应该要使用在第一帧坐标系下的重力表示，代码中这么做可能是默认为初始位姿是垂直地面向下的。
下面这一部分代码是通过预积分求解second-scan与first-scan之间的预测位姿：

// Calculate relative transform, linState_, using ICP method
    V3D pl;
    Q4D ql;
    V3D v0, v1, ba0, bw0;
    ba0.setZero();
    ql = preintegration_->delta_q;
    pl = preintegration_->delta_p +
         0.5 * linState_.gn_ * preintegration_->sum_dt *
             preintegration_->sum_dt -
         0.5 * ba0 * preintegration_->sum_dt * preintegration_->sum_dt;

对于于预积分有: $\alpha^k_{k+1}=R_W^{k}(p_{k+1}^W-p_k^W+\frac{1}{2}g^W\Delta t^2-v_k^W\Delta t)\\\gamma_{k+1}^k=q_W^k\otimes q_{k+1}^W$ 认为第一帧速度为0。

这里初始化感觉也是有问题的：

 filter_->initialization(scan_new_->time_, r1, v1, ba0, bw0, imu_last_.acc,
                         imu_last_.gyr);

这里应该是初始化robocentric状态的初始值，位移p应该设置为0，而这里传入了r1,此外，重力应该设置为第2帧下的观测值，但这里这里并没有重新初始化重力。

4.2.2 processScan()

在这里插入图片描述重点是： performIESKF()

FAST-LIO

核心处理线程是 LaserMapping::Run()，该线程中，首先通过LaserMapping::SyncPackages()去提取出一帧雷达数据与包围的IMU数据的集合，然后将数据传入到 ImuProcess::Process()执行卡尔曼滤波的预测以及点云的去畸变，

初始化

首先关注最核心的估计器的初始化，fastlio使用的估计器是esekf类：

template <typename state, int process_noise_dof, typename input = state, typename measurement = state,
          int measurement_noise_dof = 0>
class esekf {...}

所使用的对象是：esekfom::esekf<state_ikfom, 12, input_ikfom> kf_;
即所使用的状态state是state_ikfom，process_noise_dof为12，input_ikfom为input_ikfom。
在bool LaserMapping::InitROS(ros::NodeHandle &nh)中，对kf_进行了初始化：

    kf_.init_dyn_share(
        get_f, df_dx, df_dw,
        [this](state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data) { ObsModel(s, ekfom_data); },
        options::NUM_MAX_ITERATIONS, epsi.data());

其中init_dyn_share()如下：

    void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in,
                        measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n]) {
        f = f_in;
        f_x = f_x_in;
        f_w = f_w_in;
        h_dyn_share = h_dyn_share_in;

        maximum_iter = maximum_iteration;
        for (int i = 0; i < n; i++) {
            limit[i] = limit_vector[i];
        }

        x_.build_S2_state();
        x_.build_SO3_state();
        x_.build_vect_state();
    }

在执行ImuProcess::Process时，如果没有初始化imu_need_init_==false,那么会去执行ImuProcess::IMUInit完成初始化，ImuProcess::IMUInit初始化的流程是：
1、通过迭代的方式计算 mean_acc_、mean_gyr_、cov_acc_、cov_gyr_。
2、初始化ieskf的状态：

state_ikfom init_state = kf_state.get_x();
 init_state.grav = S2(-mean_acc_ / mean_acc_.norm() * common::G_m_s2);

 init_state.bg = mean_gyr_;
 init_state.offset_T_L_I = Lidar_T_wrt_IMU_;
 init_state.offset_R_L_I = Lidar_R_wrt_IMU_;
 kf_state.change_x(init_state);

上面将初始状态中角速度偏置设置为mean_gyr_，重力设置为了在当前IMU坐标系下的重力观测。
3、初始化协方差矩阵

    esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();
    init_P.setIdentity();
    init_P(6, 6) = init_P(7, 7) = init_P(8, 8) = 0.00001;
    init_P(9, 9) = init_P(10, 10) = init_P(11, 11) = 0.00001;
    init_P(15, 15) = init_P(16, 16) = init_P(17, 17) = 0.0001;
    init_P(18, 18) = init_P(19, 19) = init_P(20, 20) = 0.001;
    init_P(21, 21) = init_P(22, 22) = 0.00001;
    kf_state.change_P(init_P);

ieskf预测环节

ImuProcess::UndistortPcl()

在ImuProcess::Process()中，初始化完毕后，就会执行ImuProcess::UndistortPcl(), 在这个函数里执行ieskf的状态预测以及点云的畸变去除。
在ImuProcess::UndistortPcl()中，会对于所有IMU遍历执行：kf_state.predict(dt, Q_, in);,其中dt表示时间差，in为传感器输入信号：

// 传感器输入-加速度信息与角速度信息
 in.acc = acc_avr;
 in.gyro = angvel_avr;
 // 输入噪声-传感器观测噪声+bias随机游走噪声
 Q_.block<3, 3>(0, 0).diagonal() = cov_gyr_;
 Q_.block<3, 3>(3, 3).diagonal() = cov_acc_;
 Q_.block<3, 3>(6, 6).diagonal() = cov_bias_gyr_;
 Q_.block<3, 3>(9, 9).diagonal() = cov_bias_acc_;
 kf_state.predict(dt, Q_, in);

其中acc_avr，angvel_avr就是IMU测量的中值：

// 中值
 angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
     0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
     0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
 acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
     0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
     0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
 // 对加速度观测进行尺度转换，保证加速度是和9.8为同一标准的
 acc_avr = acc_avr * common::G_m_s2 / mean_acc_.norm();  // - state_inout.ba;

Q_是测量噪声矩阵，cov_gyr_和cov_acc_是陀螺仪和加速度计测量噪声，它的值在IMU初始化时确定：

for (const auto &imu : meas.imu_) {
        const auto &imu_acc = imu->linear_acceleration;
        const auto &gyr_acc = imu->angular_velocity;
        cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
        cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;

        mean_acc_ += (cur_acc - mean_acc_) / N;
        mean_gyr_ += (cur_gyr - mean_gyr_) / N;
	   // 这里计算IMU的测量噪声，在后面EKF的前向传播中会使用
        cov_acc_ =
            cov_acc_ * (N - 1.0) / N + (cur_acc - mean_acc_).cwiseProduct(cur_acc - mean_acc_) * (N - 1.0) / (N * N);
        cov_gyr_ =
            cov_gyr_ * (N - 1.0) / N + (cur_gyr - mean_gyr_).cwiseProduct(cur_gyr - mean_gyr_) * (N - 1.0) / (N * N);

        N++;
    }

cov_bias_gyr_、cov_bias_acc_是角速度bias随机游走噪声和加速度bias随机游走噪声，这个值需要离线的标定去获得。
得到数值后通过下面代码进行设置就行。

void ImuProcess::SetGyrBiasCov(const common::V3D &b_g) { cov_bias_gyr_ = b_g; }
void ImuProcess::SetAccBiasCov(const common::V3D &b_a) { cov_bias_acc_ = b_a; }

kf_state.predict(dt, Q_, in)

到这里正式进入IESKF的预测过程，首先看下面的代码：

    void predict(double &dt, processnoisecovariance &Q, const input &i_in) {
        // f函数对应use-ikfom.hpp中的 get_f函数，对应fast_lio2论文公式(2)
        flatted_state f_ = f(x_, i_in);    // typedef Matrix<scalar_type, m, 1> flatted_state;
        cov_ f_x_ = f_x(x_, i_in);   
        cov f_x_final;

        Matrix<scalar_type, m, process_noise_dof> f_w_ = f_w(x_, i_in);
        Matrix<scalar_type, n, process_noise_dof> f_w_final;
        state x_before = x_;
        x_.oplus(f_, dt);   // 对状态进行传播
        ...

从前面的初始化可以知道，f()是get_f()函数，f_x()是df_dx()函数，f_w()是df_dw()函数，均定义在use-ikfom.hpp中，
get_f()计算状态的前向传播，参考如下公式：
在这里插入图片描述

Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in) {
    Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
    vect3 omega;
    in.gyro.boxminus(omega, s.bg);    // 去偏置
    vect3 a_inertial = s.rot * (in.acc - s.ba);     // 加速度去偏置，再转换到滤波器起始坐标
    for (int i = 0; i < 3; i++) {
        res(i) = s.vel[i];              // 位置的微分
        res(i + 3) = omega[i];     //  角速度 
        res(i + 12) = a_inertial[i] + s.grav[i];   // 速度的微分 
    }
    return res;
}

df_dx()计算状态传播方程关于状态的线性化jacobian矩阵，df_dw()计算状态传播方程关于测量输入和噪声的线性化jacobian矩阵，参考如下公式：
在这里插入图片描述

Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in) {
    Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero();
    cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
    vect3 acc_;
    in.acc.boxminus(acc_, s.ba);
    vect3 omega;
    in.gyro.boxminus(omega, s.bg);
    cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix() * MTK::hat(acc_);
    cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();
    Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
    Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
    s.S2_Mx(grav_matrix, vec, 21);
    cov.template block<3, 2>(12, 21) = grav_matrix;
    cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();
    return cov;
}

Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in) {
    Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
    cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
    cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
    cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity();
    cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity();
    return cov;
}