自动驾驶与机器人中的SLAM技术--第8章--紧耦合LIO系统

weixin_47046769

已于 2024-08-28 14:33:05 修改

阅读量412

点赞数 25

文章标签：自动驾驶机器人人工智能

于 2024-08-27 16:27:10 首次发布

本文链接：https://blog.csdn.net/weixin_47046769/article/details/141501245

版权

8.1 紧耦合的原理和优点

广义上，只要状态估计系统考虑了各传感器内在的性质，而非模块化地将它们的输出进行融合，就可以称为紧耦合LIO系统。
在松耦合LIO系统中，如果一个模块失效，就必须在逻辑上识别出它的失效，再想办法恢复成正常状态。而在紧耦合LIO系统中，一个模块的工作状态能够直接反映到另一个模块中，帮助它们更好地约束自身的工作维度。

8.2 基于IEKF的LIO系统

8.2.1 IEKF状态变量和运动方程

将高维状态变量统一记作x，它定义在一个高维流形空间M：

$x=\left \{ p,v,R,b_{g},b_{a},g\right \} ^{\top}$

误差状态 $\delta x\in \mathbb{R}^{18}$ ，运动过程包括IMU离散时间的递推过程（8.2）和方差更新过程 $P_{pred}=FPF^{\top}+Q$ ，Q为噪声矩阵，P为上一时刻的状态协方差，F由运动误差的线性形式给出（3.47）。在两个雷达数据之间要多次递推IMU数据，记递推后的状态估计为 $x_{pred},P_{pred}$ 。

8.2.2 观测方程中的迭代过程

在紧耦合LIO系统中，把雷达的ICP、NDT残差作为观测方程，写入EKF模型中。由于ICP和NDT需要迭代，应该为EKF的观测过程增加迭代过程。

从 $x_{0},P_{0}$ 出发，不断迭代观测模型，计算出本次迭代的 $\delta x$ ，进而得到下一次的 $x,P$ ，最终收敛。
现在考虑第k次迭代，工作点是 $x_{k},P_{k}$ ，计算本次增量 $\delta x_{k}$ ，根据式8.5求出第k次迭代的切空间变换雅可比矩阵 $J_{k}$ ，把运动过程求出的 $P_{pred}$ 用 $J_{k}$ 转换为 $P_{k}$ ：

$P_{k}=J_{k}P_{pred}J_{K}^{\top}$

这样做的好处是不用每次都更新协方差矩阵 $P$ ，只需要初始 $P_{pred}$ 和 $J_{k}$ 就可以求 $P_{k}$ 。迭代器的更新过程为：

$K_{k}=P_{k}H_{k}^{\top}(H_{k}P_{k}H_{k}^{\top}+V)^{-1}$
$\delta x_{k}=K_{k}(z-h(x_{k}))$

如果滤波器收敛，对P进行更新：

$P_{k+1}=(I-K_{k}H_{k})J_{k}P_{pred}J_{k}^{\top}$

如果IEKF结束迭代，则将 $P_{k+1}$ 投影至结束时刻的切空间中，保持整个IEKF的一致性。

8.2.3 高维观测的等效处理

NDT或ICP的残差维度很高，当残差维度为m时， $H_{k}\in\mathbb{R}^{m\times18 }$ ，这时卡尔曼增益 $K_{k}$ 中的 $(H_{k}P_{k}H_{k}^{\top}+V)^{-1}$ 是对m*m维矩阵求逆，显然需要避免高维矩阵的求逆。使用sherman-morrison-woodbury恒等式（SMW恒等式）来变换卡尔曼增益，有：

$K_{k}=(P_{k}^{-1}+H_{k}^{\top}V^{-1}H_{k})^{-1}H_{k}^{\top}V^{-1}$

这样就把求逆操作的维度变成了18*18维。通过公式推导向读者更加清晰地介绍NDT与卡尔曼滤波之间的联系。卡尔曼增益的 $\delta x$ 为：

$\delta x_{k}=K_{k}(z-h(x_{k}))$

而NDT、ICP等最小二乘的增量为：

$\sum_{i}(J_{i}^{\top}\Sigma_{i}^{-1}J_{i})\triangle x=-\sum_{i}J_{i}^{\top}\Sigma_{i}^{-1}e_{i}$

“观察力较强的读者应该能看出这两个公式的一致性”。上式左侧矩阵求逆之后，就和SMW变换后的卡尔曼增益一致了。这句话怎么理解？

考虑一组点云的NDT配准。设一共有N个点，考虑第j个点的配准情况，它与目标体素产生一个残差 $r_{j}$ ，此残差的信息矩阵为该体素中的正态分布参数 $\Sigma_{j}^{-1}$ 。它产生的平方误差为 $e_{j}=r_{j}^{\top}\Sigma_{j}^{-1}r_{j}$ 。按照第七章NDT的知识求出雅可比矩阵：

$J_{j}=[\frac{\partial r_{j}}{\partial t},0_{3},\frac{\partial r_{j}}{\partial R},0_{3},0_{3},0_{3} ]$

此时滤波器中的 $H_{k}$ 矩阵理想状态下应有N个雅可比矩阵组成N*18维矩阵，噪声矩阵 $V$ 则是由 $\Sigma_{j}^{-1}$ 组成的对角矩阵块。由于 $V$ 是对角矩阵块，于是卡尔曼增益公式中的 $H_{k}^{\top}V^{-1}H_{k}$ 可以写成求和形式：

$H_{k}^{\top}V^{-1}H_{k}=\sum_{j}(J_{j}^{\top}\Sigma_{j}^{-1}J_{j})$

而 $H_{k}^{\top}V^{-1}$ 乘以 $z-h(x_{k})$ 可以得到：

$H_{k}^{\top}V^{-1}(z-h(x_{k}))=\sum_{j}(J_{j}^{\top}\Sigma_{j}^{-1}r_{j})$

关于协方差更新公式 $P_{k+1}=(I-K_{k}H_{k})J_{k}P_{pred}J_{k}^{\top}$ ，将 $K$ 代入，写成：

$P_{k+1}=(I-(P_{k}^{-1}+H_{k}^{\top}V^{-1}H_{k})^{-1}H_{k}^{\top}V^{-1}H_{k})J_{k}P_{pred}J_{k}^{\top}$

即 $P_{k+1}=(I-(P_{k}^{-1}+\sum_{j}(J_{j}^{\top}\Sigma_{j}^{-1}J_{j}))^{-1}\sum_{j}(J_{j}^{\top}\Sigma_{j}^{-1}J_{j}))J_{k}P_{pred}J_{k}^{\top}$

这样整个IEKF的解算就完全和NDT挂钩了，可以把紧耦合LIO系统看成带IMU预测的高维NDT或ICP。

8.3 实现基于IEKF的LIO系统

主要流程和松耦合LIO一致。

void LioIEKF::ProcessMeasurements(const MeasureGroup &meas) {
    LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();
    measures_ = meas;

    if (imu_need_init_) {
        // 初始化IMU系统
        TryInitIMU();
        return;
    }

    // 利用IMU数据进行状态预测
    Predict();

    // 对点云去畸变
    Undistort();

    // 配准
    Align();
}

来看一下是如何更新观测的。

在每次观测迭代中，求出 $H_{k}^{\top}V^{-1}H_{k}$ ， $H_{k}^{\top}V^{-1}r_{k}$ ， $J_{k}$ ， $P_{k}$ ， $Q_{k}$ ，其中

$Q_{k}=(P_{k}^{-1}+H_{k}^{\top}V^{-1}H_{k})^{-1}$

然后可以求出 $\delta x$ 。

在更新名义状态变量函数中，更新 $x=\left \{ p,v,R,b_{g},b_{a},g\right \} ^{\top}$ ，如果在迭代过程中收敛，则退出，并更新协方差矩阵 $P_{k+1}$ ，然后把 $P_{k+1}$ 投影到收敛时刻的切空间中。

/**
     * NDT观测函数，输入一个SE3 Pose, 返回本书(8.10)中的几个项
     * HT V^{-1} H
     * H^T V{-1} r
     * 二者都可以用求和的形式来做
     */
using CustomObsFunc = std::function<void(const SE3& input_pose, Eigen::Matrix<S, 18, 18>& HT_Vinv_H,
                                             Eigen::Matrix<S, 18, 1>& HT_Vinv_r)>;
bool IESKF<S>::UpdateUsingCustomObserve(IESKF::CustomObsFunc obs) {
    // H阵由用户给定

    SO3 start_R = R_;
    Eigen::Matrix<S, 18, 1> HTVr;
    Eigen::Matrix<S, 18, 18> HTVH;
    Eigen::Matrix<S, 18, Eigen::Dynamic> K;
    Mat18T Pk, Qk;

    for (int iter = 0; iter < options_.num_iterations_; ++iter) {
        // 调用obs function
        obs(GetNominalSE3(), HTVH, HTVr);

        // 投影P
        Mat18T J = Mat18T::Identity();
        J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log());
        Pk = J * cov_ * J.transpose();

        // 卡尔曼更新
        Qk = (Pk.inverse() + HTVH).inverse();  // 这个记作中间变量，最后更新时可以用
        dx_ = Qk * HTVr;
        // LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm();

        // dx合入名义变量
        Update();

        if (dx_.norm() < options_.quit_eps_) {
            break;
        }
    }

    // update P
    cov_ = (Mat18T::Identity() - Qk * HTVH) * Pk;

    // project P
    Mat18T J = Mat18T::Identity();
    Vec3d dtheta = (R_.inverse() * start_R).log();
    J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta);
    cov_ = J * cov_ * J.inverse();

    dx_.setZero();
    return true;
}

在第七章中的ndt_inc.cc文件中增加一个接口，计算所需的几个矩阵，并返回结果。计算流程和原始NDT一样，只是不需要计算更新量。由于NDT的点数明显多于预测方程，可能导致估计结果向NDT倾斜，所以给信息矩阵 $\Sigma^{-1}$ 添加一个乘积因子（0.01），让更新部分更加平衡。

void IncNdt3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr) {
    assert(grids_.empty() == false);
    SE3 pose = input_pose;

    // 大部分流程和前面的Align是一样的，只是会把z, H, R三者抛出去而非自己处理
    int num_residual_per_point = 1;
    if (options_.nearby_type_ == NearbyType::NEARBY6) {
        num_residual_per_point = 7;
    }

    std::vector<int> index(source_->points.size());
    for (int i = 0; i < index.size(); ++i) {
        index[i] = i;
    }

    int total_size = index.size() * num_residual_per_point;

    std::vector<bool> effect_pts(total_size, false);
    std::vector<Eigen::Matrix<double, 3, 18>> jacobians(total_size);
    std::vector<Vec3d> errors(total_size);
    std::vector<Mat3d> infos(total_size);

    // gauss-newton 迭代
    // 最近邻，可以并发
    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
        auto q = ToVec3d(source_->points[idx]);
        Vec3d qs = pose * q;  // 转换之后的q

        // 计算qs所在的栅格以及它的最近邻栅格
        Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));

        for (int i = 0; i < nearby_grids_.size(); ++i) {
            Vec3i real_key = key + nearby_grids_[i];
            auto it = grids_.find(real_key);
            int real_idx = idx * num_residual_per_point + i;
            /// 这里要检查高斯分布是否已经估计
            if (it != grids_.end() && it->second->second.ndt_estimated_) {
                auto& v = it->second->second;  // voxel
                Vec3d e = qs - v.mu_;

                // check chi2 th
                double res = e.transpose() * v.info_ * e;
                if (std::isnan(res) || res > options_.res_outlier_th_) {
                    effect_pts[real_idx] = false;
                    continue;
                }

                // build residual
                Eigen::Matrix<double, 3, 18> J;
                J.setZero();
                J.block<3, 3>(0, 0) = Mat3d::Identity();                   // 对p
                J.block<3, 3>(0, 6) = -pose.so3().matrix() * SO3::hat(q);  // 对R

                jacobians[real_idx] = J;
                errors[real_idx] = e;
                infos[real_idx] = v.info_;
                effect_pts[real_idx] = true;
            } else {
                effect_pts[real_idx] = false;
            }
        }
    });

    // 累加Hessian和error,计算dx
    double total_res = 0;
    int effective_num = 0;

    HTVH.setZero();
    HTVr.setZero();

    const double info_ratio = 0.01;  // 每个点反馈的info因子

    for (int idx = 0; idx < effect_pts.size(); ++idx) {
        if (!effect_pts[idx]) {
            continue;
        }

        total_res += errors[idx].transpose() * infos[idx] * errors[idx];
        effective_num++;

        HTVH += jacobians[idx].transpose() * infos[idx] * jacobians[idx] * info_ratio;
        HTVr += -jacobians[idx].transpose() * infos[idx] * errors[idx] * info_ratio;
    }

    LOG(INFO) << "effective: " << effective_num;
}

在LIO层面，沿用松耦合的框架，只需要将之前松耦合的配准函数改写为紧耦合的形式。
        在NDT类内部使用IEKF预测的初始估计位姿，计算出所需的 $H_{k}^{\top}V^{-1}H_{k}$ ， $H_{k}^{\top}V^{-1}r_{k}$ 。
$H_{k}\in\mathbb{R}^{(N\times3)\times18}$ 由第k次迭代的N个雅可比矩阵构成。
         $V^{-1}\in\mathbb{R}^{(N\times3)\times(N\times3)}$ 是由N个信息矩阵 $\Sigma^{-1}$ 组成的对角矩阵。
         $r_{k}\in\mathbb{R}^{N\times3}$ 是由N个NDT残差构成。
至此，紧耦合LIO的本质在于把NDT残差以及信息矩阵作为观测，对IEKF进行更新。

	紧耦合LIO	松耦合LIO
预测	使用IMU读数进行初始状态估计	同
观测	以初始状态估计（首次）和更新位姿（迭代）为基准的NDT残差和信息矩阵	NDT配准后的位姿估计
更新	多次迭代，直到更新量dx满足要求每次迭代都会以上一次更新的位姿来重新求NDT观测量	一次更新

void LioIEKF::Align() {
    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix().cast<float>());
    scan_undistort_ = scan_undistort_trans;

    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);

    // voxel 之
    pcl::VoxelGrid<PointType> voxel;
    voxel.setLeafSize(0.5, 0.5, 0.5);
    voxel.setInputCloud(current_scan_);

    CloudPtr current_scan_filter(new PointCloudType);
    voxel.filter(*current_scan_filter);

    /// the first scan
    if (flg_first_scan_) {
        ndt_.AddCloud(current_scan_);
        flg_first_scan_ = false;

        return;
    }

    // 后续的scan，使用NDT配合pose进行更新
    LOG(INFO) << "=== frame " << frame_num_;

    ndt_.SetSource(current_scan_filter);
    ieskf_.UpdateUsingCustomObserve([this](const SE3 &input_pose, Mat18d &HTVH, Vec18d &HTVr) {
        ndt_.ComputeResidualAndJacobians(input_pose, HTVH, HTVr);
    });

    auto current_nav_state = ieskf_.GetNominalState();

    // 若运动了一定范围，则把点云放入地图中
    SE3 current_pose = ieskf_.GetNominalSE3();
    SE3 delta_pose = last_pose_.inverse() * current_pose;

    if (delta_pose.translation().norm() > 1.0 || delta_pose.so3().log().norm() > math::deg2rad(10.0)) {
        // 将地图合入NDT中
        CloudPtr current_scan_world(new PointCloudType);
        pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
        ndt_.AddCloud(current_scan_world);
        last_pose_ = current_pose;
    }

    // 放入UI
    if (ui_) {
        ui_->UpdateScan(current_scan_, current_nav_state.GetSE3());  // 转成Lidar Pose传给UI
        ui_->UpdateNavState(current_nav_state);
    }

    frame_num_++;
    return;
}

8.4 基于预积分的LIO系统

8.4.1 预积分LIO系统的原理

预积分因子可以更方便地整合到现有的优化框架中，从开发到实现都更为便捷。

预积分因子通常关联两个关键帧的高维状态 $(R,p,v,b_{a},b_{g})$ ，在转换为图优化问题时，把顶点分开处理（如上图），这样做虽然顶点种类较多，但边的维度较低。
对上一个关键帧添加一个先验因子（来自此关键帧前一个关键帧的位姿协方差），在两帧之间添加预积分因子和零偏随机游走因子，在下一个关键帧中添加NDT观测的位姿约束。

在LioPreinteg类中，包含一个预积分器，和一个增量NDT的里程计。

#ifndef SAD_CH8_LIO_PREINTEG_H
#define SAD_CH8_LIO_PREINTEG_H

#include <livox_ros_driver/CustomMsg.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>

/// 部分类直接使用ch7的结果
#include "ch3/static_imu_init.h"
#include "ch4/imu_preintegration.h"
#include "ch7/loosely_coupled_lio/cloud_convert.h"
#include "ch7/loosely_coupled_lio/measure_sync.h"
#include "ch7/ndt_inc.h"

#include "common/math_utils.h"
#include "tools/ui/pangolin_window.h"

namespace sad {

/**
 * 第8章 基于预积分系统的LIO
 * 框架与前文一致，但之前由IEKF处理的部分变为预积分优化
 */
class LioPreinteg {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    struct Options {
        Options() {}
        bool with_ui_ = true;  // 是否带着UI
        bool verbose_ = true;  // 打印调试信息

        double bias_gyro_var_ = 1e-2;           // 陀螺零偏游走标准差
        double bias_acce_var_ = 1e-2;           // 加计零偏游走标准差
        Mat3d bg_rw_info_ = Mat3d::Identity();  // 陀螺随机游走信息阵
        Mat3d ba_rw_info_ = Mat3d::Identity();  // 加计随机游走信息阵

        double ndt_pos_noise_ = 0.1;                   // NDT位置方差
        double ndt_ang_noise_ = 2.0 * math::kDEG2RAD;  // NDT角度方差
        Mat6d ndt_info_ = Mat6d::Identity();           // 6D NDT 信息矩阵

        sad::IMUPreintegration::Options preinteg_options_;  // 预积分参数
        IncNdt3d::Options ndt_options_;                     // NDT 参数
    };

    LioPreinteg(Options options = Options());
    ~LioPreinteg() = default;

    /// init without ros
    bool Init(const std::string& config_yaml);

    /// 点云回调函数
    void PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr& msg);
    void LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr& msg);

    /// IMU回调函数
    void IMUCallBack(IMUPtr msg_in);

    /// 结束程序，退出UI
    void Finish();

   private:
    bool LoadFromYAML(const std::string& yaml_file);

    /// 处理同步之后的IMU和雷达数据
    void ProcessMeasurements(const MeasureGroup& meas);

    /// 尝试让IMU初始化
    void TryInitIMU();

    /// 利用IMU预测状态信息
    /// 这段时间的预测数据会放入imu_states_里
    void Predict();

    /// 对measures_中的点云去畸变
    void Undistort();

    /// 执行一次配准和观测
    void Align();

    /// 执行预积分+NDT pose优化
    void Optimize();

    /// 将速度限制在正常区间
    void NormalizeVelocity();

    /// modules
    std::shared_ptr<MessageSync> sync_ = nullptr;
    StaticIMUInit imu_init_;

    /// point clouds data
    FullCloudPtr scan_undistort_{new FullPointCloudType()};  // scan after undistortion
    CloudPtr current_scan_ = nullptr;

    // optimize相关
    NavStated last_nav_state_, current_nav_state_;  // 上一时刻状态与本时刻状态
    Mat15d prior_info_ = Mat15d::Identity();        // 先验约束
    std::shared_ptr<IMUPreintegration> preinteg_ = nullptr;

    IMUPtr last_imu_ = nullptr;

    /// NDT数据
    IncNdt3d ndt_;
    SE3 ndt_pose_;
    SE3 last_ndt_pose_;

    // flags
    bool imu_need_init_ = true;
    bool flg_first_scan_ = true;
    int frame_num_ = 0;

    MeasureGroup measures_;  // sync IMU and lidar scan
    std::vector<NavStated> imu_states_;
    SE3 TIL_;  // Lidar与IMU之间外参

    Options options_;
    std::shared_ptr<ui::PangolinWindow> ui_ = nullptr;
};

}  // namespace sad

#endif  // FASTER_LIO_LASER_MAPPING_H

预积分LIO系统的流程和紧耦合LIO系统一致，包含IMU初始化，预测，去畸变，配准。
在预测阶段，使用预积分得到位姿估计，不同于IEKF的IMU预测阶段。

void LioPreinteg::Predict() {
    imu_states_.clear();
    imu_states_.emplace_back(last_nav_state_);

    /// 对IMU状态进行预测
    for (auto &imu : measures_.imu_) {
        if (last_imu_ != nullptr) {
            preinteg_->Integrate(*imu, imu->timestamp_ - last_imu_->timestamp_);
        }

        last_imu_ = imu;
        imu_states_.emplace_back(preinteg_->Predict(last_nav_state_, imu_init_.GetGravity()));
    }
}

然后进行去畸变。

void LioIEKF::Undistort() {
    auto cloud = measures_.lidar_;
    auto imu_state = ieskf_.GetNominalState();  // 最后时刻的状态
    SE3 T_end = SE3(imu_state.R_, imu_state.p_);

    if (options_.save_motion_undistortion_pcd_) {
        sad::SaveCloudToFile("./data/ch7/before_undist.pcd", *cloud);
    }

    /// 将所有点转到最后时刻状态上
    std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto &pt) {
        SE3 Ti = T_end;
        NavStated match;

        // 根据pt.time查找时间，pt.time是该点打到的时间与雷达开始时间之差，单位为毫秒
        math::PoseInterp<NavStated>(
            measures_.lidar_begin_time_ + pt.time * 1e-3, imu_states_, [](const NavStated &s) { return s.timestamp_; },
            [](const NavStated &s) { return s.GetSE3(); }, Ti, match);

        Vec3d pi = ToVec3d(pt);
        Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;

        pt.x = p_compensate(0);
        pt.y = p_compensate(1);
        pt.z = p_compensate(2);
    });
    scan_undistort_ = cloud;

    if (options_.save_motion_undistortion_pcd_) {
        sad::SaveCloudToFile("./data/ch7/after_undist.pcd", *cloud);
    }
}

在配准时，使用预积分给出的预测位姿作为NDT里程计的输入，同时使用NDT的输出作为观测值进行优化。

void LioPreinteg::Align() {
    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix().cast<float>());
    scan_undistort_ = scan_undistort_trans;

    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);

    // voxel 之
    pcl::VoxelGrid<PointType> voxel;
    voxel.setLeafSize(0.5, 0.5, 0.5);
    voxel.setInputCloud(current_scan_);

    CloudPtr current_scan_filter(new PointCloudType);
    voxel.filter(*current_scan_filter);

    /// the first scan
    if (flg_first_scan_) {
        ndt_.AddCloud(current_scan_);
        preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
        flg_first_scan_ = false;
        return;
    }

    // 后续的scan，使用NDT配合pose进行更新
    LOG(INFO) << "=== frame " << frame_num_;
    ndt_.SetSource(current_scan_filter);

    current_nav_state_ = preinteg_->Predict(last_nav_state_, imu_init_.GetGravity());
    ndt_pose_ = current_nav_state_.GetSE3();

    ndt_.AlignNdt(ndt_pose_);

    Optimize();

    // 若运动了一定范围，则把点云放入地图中
    SE3 current_pose = current_nav_state_.GetSE3();
    SE3 delta_pose = last_ndt_pose_.inverse() * current_pose;

    if (delta_pose.translation().norm() > 0.3 || delta_pose.so3().log().norm() > math::deg2rad(5.0)) {
        // 将地图合入NDT中
        CloudPtr current_scan_world(new PointCloudType);
        pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
        ndt_.AddCloud(current_scan_world);
        last_ndt_pose_ = current_pose;
    }

    // 放入UI
    if (ui_) {
        ui_->UpdateScan(current_scan_, current_nav_state_.GetSE3());  // 转成Lidar Pose传给UI
        ui_->UpdateNavState(current_nav_state_);
    }

    frame_num_++;
}

优化过程中，建立关于上一关键帧【v0_pose，v0_vel，v0_bg，v0_ba】和下一关键帧【v1_pose，v1_vel，v1_bg，v1_ba】的8个顶点，如上图图优化模型所示，为相应顶点添加边。

预积分因子：9维多元边，关联6个顶点【p1，v1，bg1，ba1，p2，v2】，残差项有【er，ev，ep】，共9维。雅可比矩阵为9*24维。

陀螺仪零偏随机游走：3维二元边，关联2个顶点【bg1，bg2】，残差项有【bg2-bg1】，共3维。雅可比矩阵为3*6维。

加速度计零偏随机游走：3维二元边，关联2个顶点【ba1，ba2】，残差项有【ba2-ba1】，共3维。雅可比矩阵为3*6维。

先验因子：15维多元边，关联4个顶点【p1，v1，bg1，ba1】，残差项有【er，ep，ev，ebg，eba】，共15维。雅可比矩阵为15*15维。

NDT观测因子：6维单元边，关联1个顶点【p1】，残差项有【eR，eP】，共6维。雅可比矩阵为6*6维。

优化完毕后，把所有因子的海森矩阵组合成一个大的海森矩阵 $H\in\mathbb{R}^{30\times30}$ ，对 $H$ 进行边缘化，提取出信息矩阵，即下一次优化的先验因子的信息矩阵。

void LioPreinteg::Optimize() {
    // 调用g2o求解优化问题
    // 上一个state到本时刻state的预积分因子，本时刻的NDT因子
    LOG(INFO) << " === optimizing frame " << frame_num_ << " === "
              << ", dt: " << preinteg_->dt_;

    /// NOTE 这些东西是对参数非常敏感的。相差几个数量级的话，容易出现优化不动的情况

    using BlockSolverType = g2o::BlockSolverX;
    using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;

    auto *solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // 上时刻顶点， pose, v, bg, ba
    auto v0_pose = new VertexPose();
    v0_pose->setId(0);
    v0_pose->setEstimate(last_nav_state_.GetSE3());
    optimizer.addVertex(v0_pose);

    auto v0_vel = new VertexVelocity();
    v0_vel->setId(1);
    v0_vel->setEstimate(last_nav_state_.v_);
    optimizer.addVertex(v0_vel);

    auto v0_bg = new VertexGyroBias();
    v0_bg->setId(2);
    v0_bg->setEstimate(last_nav_state_.bg_);
    optimizer.addVertex(v0_bg);

    auto v0_ba = new VertexAccBias();
    v0_ba->setId(3);
    v0_ba->setEstimate(last_nav_state_.ba_);
    optimizer.addVertex(v0_ba);

    // 本时刻顶点，pose, v, bg, ba
    auto v1_pose = new VertexPose();
    v1_pose->setId(4);
    v1_pose->setEstimate(ndt_pose_);  // NDT pose作为初值
    // v1_pose->setEstimate(current_nav_state_.GetSE3());  // 预测的pose作为初值
    optimizer.addVertex(v1_pose);

    auto v1_vel = new VertexVelocity();
    v1_vel->setId(5);
    v1_vel->setEstimate(current_nav_state_.v_);
    optimizer.addVertex(v1_vel);

    auto v1_bg = new VertexGyroBias();
    v1_bg->setId(6);
    v1_bg->setEstimate(current_nav_state_.bg_);
    optimizer.addVertex(v1_bg);

    auto v1_ba = new VertexAccBias();
    v1_ba->setId(7);
    v1_ba->setEstimate(current_nav_state_.ba_);
    optimizer.addVertex(v1_ba);

    // imu factor
    auto edge_inertial = new EdgeInertial(preinteg_, imu_init_.GetGravity());
    edge_inertial->setVertex(0, v0_pose);
    edge_inertial->setVertex(1, v0_vel);
    edge_inertial->setVertex(2, v0_bg);
    edge_inertial->setVertex(3, v0_ba);
    edge_inertial->setVertex(4, v1_pose);
    edge_inertial->setVertex(5, v1_vel);
    auto *rk = new g2o::RobustKernelHuber();
    rk->setDelta(200.0);
    edge_inertial->setRobustKernel(rk);
    optimizer.addEdge(edge_inertial);

    // 零偏随机游走
    auto *edge_gyro_rw = new EdgeGyroRW();
    edge_gyro_rw->setVertex(0, v0_bg);
    edge_gyro_rw->setVertex(1, v1_bg);
    edge_gyro_rw->setInformation(options_.bg_rw_info_);
    optimizer.addEdge(edge_gyro_rw);

    auto *edge_acc_rw = new EdgeAccRW();
    edge_acc_rw->setVertex(0, v0_ba);
    edge_acc_rw->setVertex(1, v1_ba);
    edge_acc_rw->setInformation(options_.ba_rw_info_);
    optimizer.addEdge(edge_acc_rw);

    // 上一帧pose, vel, bg, ba的先验
    auto *edge_prior = new EdgePriorPoseNavState(last_nav_state_, prior_info_);
    edge_prior->setVertex(0, v0_pose);
    edge_prior->setVertex(1, v0_vel);
    edge_prior->setVertex(2, v0_bg);
    edge_prior->setVertex(3, v0_ba);
    optimizer.addEdge(edge_prior);

    /// 使用NDT的pose进行观测
    auto *edge_ndt = new EdgeGNSS(v1_pose, ndt_pose_);
    edge_ndt->setInformation(options_.ndt_info_);
    optimizer.addEdge(edge_ndt);

    if (options_.verbose_) {
        LOG(INFO) << "last: " << last_nav_state_;
        LOG(INFO) << "pred: " << current_nav_state_;
        LOG(INFO) << "NDT: " << ndt_pose_.translation().transpose() << ","
                  << ndt_pose_.so3().unit_quaternion().coeffs().transpose();
    }

    v0_bg->setFixed(true);
    v0_ba->setFixed(true);

    // go
    optimizer.setVerbose(options_.verbose_);
    optimizer.initializeOptimization();
    optimizer.optimize(20);

    // get results
    last_nav_state_.R_ = v0_pose->estimate().so3();
    last_nav_state_.p_ = v0_pose->estimate().translation();
    last_nav_state_.v_ = v0_vel->estimate();
    last_nav_state_.bg_ = v0_bg->estimate();
    last_nav_state_.ba_ = v0_ba->estimate();

    current_nav_state_.R_ = v1_pose->estimate().so3();
    current_nav_state_.p_ = v1_pose->estimate().translation();
    current_nav_state_.v_ = v1_vel->estimate();
    current_nav_state_.bg_ = v1_bg->estimate();
    current_nav_state_.ba_ = v1_ba->estimate();

    if (options_.verbose_) {
        LOG(INFO) << "last changed to: " << last_nav_state_;
        LOG(INFO) << "curr changed to: " << current_nav_state_;
        LOG(INFO) << "preinteg chi2: " << edge_inertial->chi2() << ", err: " << edge_inertial->error().transpose();
        LOG(INFO) << "prior chi2: " << edge_prior->chi2() << ", err: " << edge_prior->error().transpose();
        LOG(INFO) << "ndt: " << edge_ndt->chi2() << "/" << edge_ndt->error().transpose();
    }

    /// 重置预积分

    options_.preinteg_options_.init_bg_ = current_nav_state_.bg_;
    options_.preinteg_options_.init_ba_ = current_nav_state_.ba_;
    preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);

    // 计算当前时刻先验
    // 构建hessian
    // 15x2，顺序：v0_pose, v0_vel, v0_bg, v0_ba, v1_pose, v1_vel, v1_bg, v1_ba
    //            0       6        9     12     15        21      24     27
    Eigen::Matrix<double, 30, 30> H;
    H.setZero();

    H.block<24, 24>(0, 0) += edge_inertial->GetHessian();

    Eigen::Matrix<double, 6, 6> Hgr = edge_gyro_rw->GetHessian();
    H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0);
    H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3);
    H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0);
    H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3);

    Eigen::Matrix<double, 6, 6> Har = edge_acc_rw->GetHessian();
    H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0);
    H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3);
    H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0);
    H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3);

    H.block<15, 15>(0, 0) += edge_prior->GetHessian();
    H.block<6, 6>(15, 15) += edge_ndt->GetHessian();

    H = math::Marginalize(H, 0, 14);
    prior_info_ = H.block<15, 15>(15, 15);

    if (options_.verbose_) {
        LOG(INFO) << "info trace: " << prior_info_.trace();
        LOG(INFO) << "optimization done.";
    }

    NormalizeVelocity();
    last_nav_state_ = current_nav_state_;
}

weixin_47046769

关注

25
点赞
踩
13

收藏

觉得还不错? 一键收藏
0
评论
自动驾驶与机器人中的SLAM技术--第8章--紧耦合LIO系统

广义上，只要状态估计系统考虑了各传感器内在的性质，而非地将它们的输出进行融合，就可以称为紧耦合LIO系统。在松耦合LIO系统中，如果一个模块失效，就必须在逻辑上识别出它的失效，再想办法恢复成正常状态。而在紧耦合LIO系统中，一个模块的工作状态能够直接反映到另一个模块中，帮助它们更好地约束自身的工作维度。
复制链接

扫一扫