功能包简介：

功能包名称：LiDAR_IMU_Init

github地址：https://github.com/hku-mars/LiDAR_IMU_Init

效果展示b站地址：https://www.bilibili.com/video/BV1ZS4y127mW?spm_id_from=333.337.search-card.all.click

功能包简介：
LiDAR_IMU_Init 是一个鲁棒的、实时的激光雷达与惯导系统的初始化方法。所提出的方法校准了 LiDAR 和 IMU 之间的时间偏移和外部参数，以及重力矢量和 IMU 偏差。
该方法不需要任何目标或额外的传感器、特定的结构化环境、先验环境点图或外部和时间偏移的初始值。

功能包解决了以下关键问题：

• 从 FAST-LIO2 修改的稳健的 LiDAR 里程计（Fast LO）。
• 无需任何硬件设置即可在 LiDAR 和 IMU 之间进行快速且稳健的时间偏移和外部参数校准。
• 支持多种激光雷达类型：机械旋转激光雷达（Hesai、Velodyne、Ouster）和固态激光雷达（Livox Avia/Mid360）。
• 无缝合并到 FAST-LIO2，作为强大的初始化模块。

论文重点部分解读

对于大多数的 LiDAR-惯性里程计，精确的初始状态（包括LiDAR和6轴IMU之间的时间偏移和外参变换）非常重要，通常被视为先觉条件。但是，这些信息在自己组装的激光雷达惯性系统中可能没办法直接获得，往往需要提前进行额外的时空标定过程，整个标定过程耗时费力。

本文提出了LI-Init：一个完整并且实时的 LiDAR-惯性系统初始化过程。

该过程通过将由 LiDAR 测量估计出的状态与 IMU 测量得到的状态进行对齐，从而标定出 LiDAR 和 IMU 之间的时间偏移、外参、重力向量和 IMU 偏置。

已经将该方法封装成初始化模块，能够自动检测所收集到的数据的运动激励程度。并在运行中校准时间偏移、外部、重力矢量和 IMU bias，然后用作高质量的初始状态值用于实时 LiDAR 惯性里程计系统。

该初始化模块能够用于不同类型的 LiDAR-惯性组合中，并且已经集成到系统 FAST-LIO2 当中。

本文的具体贡献如下：

• 提出了一种高效、准确、不需要专用时间同步硬件的时间标定方法来估计未知但恒定的 Lidar-惯性之间的时间偏移，该方法基于互相关和统一时空优化。
• 提出了一种新的优化范式来进行空间初始化，并提出了一种评估数据的运动激励程度的方法。通过进一步将由 LiDAR 估计出的状态与 IMU 降噪后的测量值对齐，初始化过程能够自动提取初始化所需的数据，并实时估计外参、重力向量、陀螺仪偏置和加速度计偏置。
• 在多种类型的 LiDAR-惯性组合上进行了实验，并验证了整个初始化过程的效率和精度。该方法是已知的第一个开源的三维 LiDAR-惯性系统时空初始化算法，同时支持机械旋转激光雷达和非重复扫描式激光雷达。
  
  实验平台包括 IMU、激光雷达（可以是三种）、相机（用来记录）

框架概述

由于IMU只有在运动中才能够得到激励，因此整个初始化过程是一种基于运动的方法，这意味着需要足够的运动激励。

整体的框架流程如下图所示：

LiDAR 里程计模块
框架中使用的 LiDAR 里程计是通过修改 FAST-LIO2 得到的。

LiDAR 里程计在 LiDAR 扫描中使用匀速模型（既包括角速度也包括线速度）来预测 LiDAR 运动和对 LiDAR 运动畸变进行补偿。通过将输入的 LiDAR 帧拆分为几个子帧，提高 LIDAR 里程计的帧率，来缓解匀速模型与实际传感器运动之间的不匹配。

如果 LiDAR 里程计没有失效（比如退化），并且所估计的 LiDAR 角速度和线速度满足评估标准，则认为激励足够。这种情况下，就可以将 LiDAR 里程计的输出和对应的 IMU 原始测量数据送入到初始化模块。

LiDAR-惯性初始化
在初始化过程中，首先通过移动 IMU 测量数据以对齐 LiDAR 里程计数据，从而标定 IMU 和 LiDAR 里程计之间的时间偏移。时间偏移标定完成之后，进行进一步的优化，以进一步标定时间偏移，并标定外参、 IMU 偏置和重力矢量。

初始化之后的状态便可以输入到紧耦合的 LIO 系统中（比如FAST-LIO2），通过融合后续的 LiDAR 数据和 IMU 数据进行在线的状态估计。

雷达里程计相关计算

匀速模型

LiDAR 里程计和建图部分（仅仅使用 LiDAR）建立在匀速模型上，该模型假设角速度和线速度在时间间隔tk和tk+1 内为常亮，连续的两个雷达帧之间，即：

其中， Δt 是两次扫描的时间间隔，状态矢量 x ，噪声 w 和离散状态转移函数 f 被定义如下

其中GRL，GPL表示 LiDAR 在世界坐标系（也就是第一帧 LiDAR 体坐标系）下的位姿
GVL表示 LiDAR 在世界坐标系下的线速度
WL表示 LiDAR 在 LiDAR 自身坐标系下的角速度
GVL和WL分别被建模为高斯噪声nv，nw驱动的随机游走过程

实际上，传感器运动的速度可能并不是常量。为了减轻模型误差的影响，我们将激光雷达扫描输入分为多个比较小的连续子帧，在每个子帧的时间范围内，传感器运动与匀速模型更加一致。

基于误差状态的迭代卡尔曼滤波ESIKF

基于匀速公式中表示的流形系统，使用基于误差状态的迭代卡尔曼滤波器（ESIKF）来估计系统状态。ESIKF的预测步骤包括状态预测和协方差传播，表示如下

其中，P 和 Q分别是状态估计和过程噪声 w的协方差矩阵。Fx 和 Fw 分别表示如下

x~表示误差状态。

激光雷达运动补偿

在我们考虑的问题中，IMU 数据和 LIDAR 数据是不同步的，因此采用 IMU 辅助运动补偿方法是不可行的。

在接收到时间戳tk+1时刻的新激光雷达帧后，为了补偿运动失真，将在时间点pj属于tk到tk+1时刻采样的每个点投影到该帧的最后时刻的坐标系下。由于前面是匀速模型假设，所以可以认为该帧的位姿变换为上一帧的位姿变换。那么则可以根据时间信息求得每个点的位姿，然后投影到最后一个点上去。

图a是没有做运动补偿的3维点云结果
图b是用上面方法做了运动补偿的3维点云结果
图c和d是图b的细节部分

LiDAR-惯性初始化相关计算

上面的 LiDAR 里程计在每帧扫描结束的时候，即tk时刻，输出雷达的角速度和线速度。同时IMU提也供了原始测量值，即带有时间戳 Ti的角速度和线性加速度。
这些数据被积累，并且通过激励准则反复被访问。
一旦收集到足够激励的数据，就会调用初始化模块，该模块最终输出时间偏移、外参、IMU bias 、重力矢量

数据预处理

IMU原始测量结果受到噪声 nwi和nai 的影响。IMU测量模型为：

其中 wgti和agti是IMU角速度和线加速度的真值。

​同样，来自 LiDAR 里程计的估计 角速度和线加速度也包含噪声。

为了减轻这些高频噪音的影响，通常使用一种 非零相低通滤波器（a non-causal zero phase low-pass filter） 来过滤噪声，而无需引入任何滤波延迟。

零相位滤波器是通过向后和向后运行Butterworth低通滤波器来实现，从而产生了噪声衰减的IMU测量 和 噪声衰减 LiDAR 估计

从 LiDAR 里程计的估计 角速度和线速度中，可以通过非因果中心差分（non-causal central difference）的方法可以得到 LiDAR 的角加速度和线加速度
最终的 LiDAR 里程计数据可以用一个集合表示：

同理，从噪声衰减的陀螺仪测量，可以得到 IMU 的角加速度，也可以用集合表示：

由于 IMU 的频率通常高于 LiDAR 的频率，因此两个序列Ii和Lk的大小不相同。为了解决此问题，在同一时间段内提取 LIDAR 和 IMU 数据，并通过在每个LiDAR 里程计时刻 tk上对IMU数据进行线性插值来进行下采，如下图。下采样的 IMU 数据表示为Ik。

通过互相关（Cross-Correlation）进行时间初始化

在大多数情况下，由于 LIDAR-惯性 里程计模块在接收之前不可避免地传输和处理延迟

这是LIDAR数据 Lk和IMU线性插值后的数据 Ik之间存在的未知但恒定的偏移ItL

因此如果将 IMU 测量Ik提前ItL。将与 LIDAR 里程计Lk对齐。

由于 LIDAR 数据和 IMU 数据在离散时间tk中，因此将 IMU 数据提前的操作基本上是在离散的步长
其中 Δ t 是两次 LiDAR 扫描之间的时间间隔。

具体而言，对于角速度，我们有：

由于陀螺仪bias(bw) 通常非常小，可以忽略不计。如果不管 RL，可以发现
wik+d和wLk的大小应该一样。

使用互相关来量化其数量之间的相似性。然后，可以通过以下优化问题中求解出d：

即通过在Lk的索引范围内枚举偏移d

统一的外参的旋转部分和时间标定

互相关方法对噪声和小尺度的陀螺仪偏置是鲁棒的。

但是上面求d的方法的一个明显缺陷是时间偏移的校准分辨率要大于 LiDAR 里程计的一个采样时间间隔Δ t，因此无法识别任何出小于Δ t的偏移残差

设ItL为LiDAR 里程计wL和IMU数据wI之间的总偏移量，则有：

那么将 IMU 测量前移ItL与 LiDAR 里程计对齐

离散形式就是：


如上图所示，我们可以通过假设在δt 这一段非常小的时间段内角加速度是常量来插值得到

即

最后，基于上式的约束，统一时空的优化问题可以表述为：

由于存在约束该问题可以通过 Ceres Solver 进行迭代求解

外参的平移部分和重力初始化

在上一节中，获得了外参的旋转部分RL、陀螺仪偏置bw和时间偏移ItL。在本节中，我们固定这些值，然后进行外参的平移部分、重力向量和加速偏置的标定。

首先，我们使用之前标定好的时间偏移和偏移残差 将 IMU 数据 与 LIDAR 的数据对齐。现在假定它与雷达数据完全对齐而没有时间偏移。

最后，可以通过以下优化问题共同估计外参的平移部分、加速度计偏置和重力向量：

数据累积评估

文中所提出的初始化方法依赖于 LiDAR-惯性装置的足够激励（即装置需要进行足够的运动）。因此，该系统应能够评估是否有足够的运动激励来执行初始化。

在实践中，我们发现只需要评估外参旋转部分和外参平移部分对应的雅可比就足够了，因为外参的激励往往需要复杂的运动，这些运动同样也足够激励其他状态。

使用Jr来表示外参旋转部分IRL对应的雅克比，使用Jt来表示外参平移部分IPL对应的雅克比

然后可以通过以下两个矩阵的秩来评估激励程度：

更定量地说，激励程度由上面两个矩阵的奇异值表示。

基于此原则，我们开发了一个评估程序，该程序可以指导用户如何移动其设备以获得足够的激励。我们通过雅可比矩阵的奇异值来量化激励，并设置一个阈值来评估激励是否足够。

核心代码

相关配置参数

    nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4);
    nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
    nh.param<string>("map_file_path", map_file_path, "");
    nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar");
    nh.param<string>("common/imu_topic", imu_topic, "/livox/imu");
    nh.param<double>("mapping/filter_size_surf", filter_size_surf_min, 0.5);
    nh.param<double>("mapping/filter_size_map", filter_size_map_min, 0.5);
    nh.param<double>("cube_side_length", cube_len, 200);
    nh.param<float>("mapping/det_range", DET_RANGE, 300.f);
    nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1);
    nh.param<double>("mapping/acc_cov", acc_cov, 0.1);
    nh.param<double>("mapping/grav_cov", grav_cov, 0.001);
    nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001);
    nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001);
    nh.param<double>("preprocess/blind", p_pre->blind, 1.0);
    nh.param<int>("preprocess/lidar_type", lidar_type, AVIA);
    nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
    nh.param<bool>("preprocess/feature_extract_en", p_pre->feature_enabled, 0);
    nh.param<bool>("initialization/cut_frame", cut_frame, true);
    nh.param<int>("initialization/cut_frame_num", cut_frame_num, 1);
    nh.param<int>("initialization/orig_odom_freq", orig_odom_freq, 10);
    nh.param<double>("initialization/online_refine_time", online_refine_time, 20.0);
    nh.param<double>("initialization/mean_acc_norm", mean_acc_norm, 9.81);
    nh.param<double>("initialization/data_accum_length", Init_LI->data_accum_length, 300);
    nh.param<vector<double>>("initialization/Rot_LI_cov", Rot_LI_cov, vector<double>());
    nh.param<vector<double>>("initialization/Trans_LI_cov", Trans_LI_cov, vector<double>());
    nh.param<bool>("publish/path_en", path_en, true);
    nh.param<bool>("publish/scan_publish_en", scan_pub_en, 1);
    nh.param<bool>("publish/dense_publish_en", dense_pub_en, 1);
    nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, 1);
    nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
    nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
    nh.param<int>("pcd_save/interval", pcd_save_interval, -1);

订阅雷达消息

    ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
        nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
         nh.subscribe(lid_topic, 200000, standard_pcl_cbk);

订阅imu消息

    ros::Subscriber sub_imu = nh.subscribe<sensor_msgs::Imu>
            (imu_topic, 200000, boost::bind(&imu_cbk, _1, pubIMU_sync));

发布的消息

    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered", 100000);
    ros::Publisher pubLaserCloudFullRes_body = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered_body", 100000);
    ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_effected", 100000);
    ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
            ("/Laser_map", 100000);
    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
            ("/aft_mapped_to_init", 100000);
    ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
            ("/path", 100000);

同步测量值

if (sync_packages(Measures)) {

处理imu数据

p_imu->Process(Measures, state, feats_undistort);

在激光雷达 FOV 中分割地图

lasermap_fov_segment();

对于一帧中的特征点进行下采样

            downSizeFilterSurf.setInputCloud(feats_undistort);
            downSizeFilterSurf.filter(*feats_down_body);
            t1 = omp_get_wtime();
            feats_down_size = feats_down_body->points.size();

初始化地图kdtree

            if (ikdtree.Root_Node == nullptr) {
                if (feats_down_size > 5) {
                    ikdtree.set_downsample_param(filter_size_map_min);
                    feats_down_world->resize(feats_down_size);
                    for (int i = 0; i < feats_down_size; i++) {
                        pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
                    }
                    ikdtree.Build(feats_down_world->points);
                }
                continue;
            }
            int featsFromMapNum = ikdtree.validnum();
            kdtree_size_st = ikdtree.size();

icp和迭代卡尔曼滤波更新

            normvec->resize(feats_down_size);
            feats_down_world->resize(feats_down_size);
            euler_cur = RotMtoEuler(state.rot_end);


            pointSearchInd_surf.resize(feats_down_size);
            Nearest_Points.resize(feats_down_size);
            int rematch_num = 0;
            bool nearest_search_en = true;
            t2 = omp_get_wtime();

迭代状态估计

            std::vector<M3D> body_var;
            std::vector<M3D> crossmat_list;
            body_var.reserve(feats_down_size);
            crossmat_list.reserve(feats_down_size);


            double t_update_start = omp_get_wtime();

            for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) {
                match_start = omp_get_wtime();
                laserCloudOri->clear();
                corr_normvect->clear();
                total_residual = 0.0;

计算测量值的雅克比矩阵和H矩阵和测量向量

				MatrixXd Hsub(effect_feat_num, 12);
                MatrixXd Hsub_T_R_inv(12, effect_feat_num);
                VectorXd R_inv(effect_feat_num);
                VectorXd meas_vec(effect_feat_num);

                Hsub.setZero();
                Hsub_T_R_inv.setZero();
                meas_vec.setZero();

                for (int i = 0; i < effect_feat_num; i++) {
                    const PointType &laser_p = laserCloudOri->points[i];
                    V3D point_this_L(laser_p.x, laser_p.y, laser_p.z);

                    V3D point_this = state.offset_R_L_I * point_this_L + state.offset_T_L_I;
                    M3D var;
                    calcBodyVar(point_this, 0.02, 0.05, var);
                    var = state.rot_end * var * state.rot_end.transpose();
                    M3D point_crossmat;
                    point_crossmat << SKEW_SYM_MATRX(point_this);

                    /*** get the normal vector of closest surface/corner ***/
                    const PointType &norm_p = corr_normvect->points[i];
                    V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);

                    R_inv(i) = 1000;
                    laserCloudOri->points[i].intensity = sqrt(R_inv(i));

                    /*** calculate the Measurement Jacobian matrix H ***/
                    if (imu_en) {
                        M3D point_this_L_cross;
                        point_this_L_cross << SKEW_SYM_MATRX(point_this_L);
                        V3D H_R_LI = point_this_L_cross * state.offset_R_L_I.transpose() * state.rot_end.transpose() *
                                     norm_vec;
                        V3D H_T_LI = state.rot_end.transpose() * norm_vec;
                        V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
                        Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(
                                H_R_LI), VEC_FROM_ARRAY(H_T_LI);
                    } else {
                        V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
                        Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, 0, 0, 0, 0, 0, 0;
                    }

                    Hsub_T_R_inv.col(i) = Hsub.row(i).transpose() * 1000;
                    /*** Measurement: distance to the closest surface/corner ***/
                    meas_vec(i) = -norm_p.intensity;
                }

迭代卡尔曼更新

                H_T_H.block<12, 12>(0, 0) = Hsub_T_R_inv * Hsub;
                MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + state.cov.inverse()).inverse();
                K = K_1.block<DIM_STATE, 12>(0, 0) * Hsub_T_R_inv;
                auto vec = state_propagat - state;
                solution = K * meas_vec + vec - K * Hsub * vec.block<12, 1>(0, 0);

状态更新

                state += solution;

                rot_add = solution.block<3, 1>(0, 0);
                T_add = solution.block<3, 1>(3, 0);


                if ((rot_add.norm() * 57.3 < 0.01) && (T_add.norm() * 100 < 0.015))
                    flg_EKF_converged = true;

                deltaR = rot_add.norm() * 57.3;
                deltaT = T_add.norm() * 100;

                euler_cur = RotMtoEuler(state.rot_end);

收敛判断和协方差更新

                if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))) {
                    if (flg_EKF_inited) {
                        /*** Covariance Update ***/
                        G.setZero();
                        G.block<DIM_STATE, 12>(0, 0) = K * Hsub;
                        state.cov = (I_STATE - G) * state.cov;
                        total_distance += (state.pos_end - position_last).norm();
                        position_last = state.pos_end;
                        if (!imu_en) {
                            geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                                    (euler_cur(0), euler_cur(1), euler_cur(2));
                        } else {
                            //Publish LiDAR's pose, instead of IMU's pose
                            M3D rot_cur_lidar = state.rot_end * state.offset_R_L_I;
                            V3D euler_cur_lidar = RotMtoEuler(rot_cur_lidar);
                            geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                                    (euler_cur_lidar(0), euler_cur_lidar(1), euler_cur_lidar(2));
                        }
                        VD(DIM_STATE) K_sum = K.rowwise().sum();
                        VD(DIM_STATE) P_diag = state.cov.diagonal();
                    }
                    EKF_stop_flg = true;
                }
                solve_time += omp_get_wtime() - solve_start;

                if (EKF_stop_flg) break;

发布里程计

publish_odometry(pubOdomAftMapped);