《自动驾驶与机器人中的SLAM技术》ch7：基于 ESKF 的松耦合 LIO 系统

基于 ESKF 的松耦合 LIO 系统

1 坐标系说明

2 松耦合 LIO 系统的运动和观测方程

3 松耦合 LIO 系统的数据准备

3.1 CloudConvert 类

3.2 MessageSync 类

4 松耦合 LIO 系统的主要流程

4.1 IMU 静止初始化

4.2 ESKF 之运动过程——使用 IMU 预测

4.3 使用 IMU 预测位姿进行运动补偿

4.4 松耦合系统的配准部分

基于 ESKF 的松耦合 LIO 系统

这里我们实现一个相对简单的案例：使用第 3 章介绍的 ESKF，配合 7.3.2 中的增量 NDT 里程计，实现松耦合的 LIO 系统。整个系统的流程如下图所示，可以观察到其中的滤波器部分和点云配准部分是相对解耦的！

1 坐标系说明

2 松耦合 LIO 系统的运动和观测方程

由于整个 LIO 运行在 IMU 坐标系中，状态变量的运动方程与《自动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航中介绍的 ESKF 的运动方程保持一致，我们不再展开叙述。同时，雷达里程计（LO）的输出位姿，可直接视为对状态变量 $R$ , $p$ 的观测。这个过程实际和第 3 章的 ESKF 中谈到的 GNSS 观测是一样的。

LO 对 $R$ 的观测可以直接写成对误差状态 $\delta\theta$ 的观测，从而省去前面的链式法则推导，简化整个线性化过程。LO 的旋转观测方程为（类似于把误差状态归入名义状态的方程）：

$R_{\mathrm{LO}}=R\mathrm{Exp}(\delta\theta),$

其中 $R$ 为该时刻的名义状态， $\delta\theta$ 为误差状态， $R_{LO}$ 为观测位姿。由于在观测过程中，名义状态 $R$ 是确定的。我们不妨将 $R_{LO}$ 直接视为对 $\delta\theta$ 的观测。我们对该方程稍作变换，可以写为：

$z_{\delta\theta}=h(\delta\theta)=\delta\theta=\mathrm{Log}\left(R^\top R_{\mathrm{LO}}\right)$

此时， $h(\delta\theta)$ 是对 $\delta\theta$ 的直接观测，即 $h(\delta\theta)=\delta\theta$ ，所以 $h(\delta\theta)$ 关于 $\delta\theta$ 的雅可比矩阵，即旋转部分的雅可比矩阵为单位阵：

$\frac{\partial h(\delta\theta)}{\partial\delta{\theta}}=I_{3\times3}.$

LO 的平移观测方程为（类似于把误差状态归入名义状态的方程）：

$p_{\mathrm{LO}}=p+\delta p$

类似的，我们对该方程稍作变换，可以写为：

$z_{\delta p}=h(\delta p)=\delta p=p_{\mathrm{LO}}-p$

因此平移部分的雅可比矩阵也为单位阵：

$\frac{\partial h(\delta p)}{\partial\delta\boldsymbol{p}}=I_{3\times3}。$

由于 LO 观测数据为 6 维的 $[p,R]$ ，故 $赫$ 矩阵的维度为 $6*18$ ，具体形式如下：

$H=\frac{\partial h(\delta x)}{\partial \delta x}=\begin{bmatrix}I_{3\times3}&0_{3\times3}&0_{3\times3}&0_{3\times3}&0_{3\times3}&0_{3\times3}\\0_{3\times3}&0_{3\times3}&I_{3\times3}&0_{3\times3}&0_{3\times3}&0_{3\times3}&0_{3\times3}\end{bmatrix}$

这样就避免了再从名义状态到误差状态进行转换的过程，可以直接得到对误差状态的雅可比矩阵。注意当我们这样做时，原本 ESKF 中 $\delta x = K(zh(x_{\mathrm{pred}}))$ 部分的更新量（innovation） $zh(x)$ 也应该写成流形的形式：

$zh(x)=[p_\text{LO}-p,\text{Log}(R^\top R_\text{LO})]^\top$

即得到：

$\delta x =K(zh(x_{\mathrm{pred}}))=K([p_\text{LO}-p,\text{Log}(R^\top R_\text{LO})]^\top)$

该式表明了从直观上来看，LO 的 $R_{LO}$ ， $p_{LO}$ 主要是在观测阶段通过卡尔曼增益 $钾$ 作用于误差状态变量中。

3 松耦合 LIO 系统的数据准备

松耦合的代码实现主要分为三个部分：

我们需要将 IMU 数据与激光数据进行同步。激光通常使用 10Hz 的频率，而 IMU 通常是更高的 100Hz。我们希望能够统一处理两个激光数据之间的那 10 个 IMU 数据。
我们需要处理激光的运动补偿，而运动补偿需要有激光测量时间内的位姿数据来源，正好可以用 ESKF 对每个 IMU 数据的预测值。
我们应该从 ESKF 中拿到预测的位姿数据，交给里程计算法，再将里程计配准之后的位姿放入 ESKF 中。

3.1 CloudConvert 类

CloudConvert 类负责将各种格式的点云转化为 PCL 格式的点云。以 livox_ros_driver::CustomMsg 类型点云为例，输入 msg， 输出时间单位为毫秒(ms)、跳点处理后的 PCL 格式点云 pcl_out。代码如下：

/// 带ring, range等其他信息的全量信息点云
struct FullPointType {
    PCL_ADD_POINT4D;        //宏定义来自 PCL，为 FullPointType 添加了四维坐标点（x,y,z,w）。其中，前三个是空间坐标，而 w 是填充位（通常为 1.0，用于齐次坐标）。
    float range = 0;        //点的距离（通常是到传感器的距离）
    float radius = 0;       //点的半径（有时可以表示与传感器的水平距离，具体视应用而定）
    uint8_t intensity = 0;  //点的强度值（反射强度）
    uint8_t ring = 0;       //点的扫描线编号
    uint8_t angle = 0;      //点的角度值
    double time = 0;        //点的时间戳
    float height = 0;       //点的高度信息

    inline FullPointType() {}
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void CloudConvert::Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, FullCloudPtr &pcl_out) {
    AviaHandler(msg);
    *pcl_out = cloud_out_;
}

void CloudConvert::AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
    cloud_out_.clear();
    cloud_full_.clear();
    int plsize = msg->point_num;

    cloud_out_.reserve(plsize);
    cloud_full_.resize(plsize);

    std::vector<bool> is_valid_pt(plsize, false); // 标记哪些点是有效的
    std::vector<uint> index(plsize - 1);
    for (uint i = 0; i < plsize - 1; ++i) {
        index[i] = i + 1;  // 从1开始
    }

    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const uint &i) {
        // 0x30: 00110000
        // 0x10: 00010000、0x00: 00000000
        // 只保留置信度优和中的点
        if ((msg->points[i].line < num_scans_) &&
            ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) {
            // 跳点比例
            if (i % point_filter_num_ == 0) {
                cloud_full_[i].x = msg->points[i].x;
                cloud_full_[i].y = msg->points[i].y;
                cloud_full_[i].z = msg->points[i].z;
                cloud_full_[i].intensity = msg->points[i].reflectivity;
                cloud_full_[i].time = msg->points[i].offset_time / float(1000000); // mid360 时间戳单位为 ns，转换为 ms

                if ((abs(cloud_full_[i].x - cloud_full_[i - 1].x) > 1e-7) ||
                    (abs(cloud_full_[i].y - cloud_full_[i - 1].y) > 1e-7) ||
                    (abs(cloud_full_[i].z - cloud_full_[i - 1].z) > 1e-7)) {
                    is_valid_pt[i] = true;
                }
            }
        }
    });

    for (uint i = 1; i < plsize; i++) {
        if (is_valid_pt[i]) {
            cloud_out_.points.push_back(cloud_full_[i]);
        }
    }
}

3.2 MessageSync 类

MessageSync 类负责将 IMU 数据与点云进行同步。该类接收 ROS 数据包中原始的 IMU 消息与激光雷达消息，通过 Sync 函数将它们组装成一个 MeasureGroup 实例对象，再将它传递给回调函数。我们后续的松耦合、紧耦合算法就只需要考虑如何处理 MeasureGroup 实例对象，而不必再操心数据准备、同步的实现代码了。

这里需要注意：

① bag 包中雷达 msg 的时间戳为一帧雷达数据中首个点的时间戳！

② 每个 MeasureGroup 实例对象中存储的 IMU 消息的时间戳均小于其存储的 LIDAR 消息的 lidar_end_time_。MeasureGroup 实例对象中一般存储 1 帧雷达消息和 10 帧 IMU 消息。

/// IMU 数据与雷达同步
struct MeasureGroup {
    MeasureGroup() { this->lidar_.reset(new FullPointCloudType()); };

    double lidar_begin_time_ = 0;   // 雷达包的起始时间
    double lidar_end_time_ = 0;     // 雷达的终止时间
    FullCloudPtr lidar_ = nullptr;  // 雷达点云
    std::deque<IMUPtr> imu_;        // 上一时时刻到现在的IMU读数
};

bool MessageSync::Sync() {
    if (lidar_buffer_.empty() || imu_buffer_.empty()) {
        return false;
    }

    // MeasureGroup 中是否存在 单帧 LiDAR 数据。若不存在，进入该 if
    if (!lidar_pushed_) {
        measures_.lidar_ = lidar_buffer_.front();
        measures_.lidar_begin_time_ = time_buffer_.front(); // lidar 数据中的时间戳是 lidar_begin_time_

        lidar_end_time_ = measures_.lidar_begin_time_ + measures_.lidar_->points.back().time / double(1000); // 以 s 为单位

        measures_.lidar_end_time_ = lidar_end_time_;
        lidar_pushed_ = true;
    }

    // imu_buffer_ 最后一个 imu 数据的时间戳要大于等于 lidar_end_time_，确保 imu 数据覆盖当前的 lidar 时间范围(lidar_begin_time_ ~ lidar_end_time_)
    if (last_timestamp_imu_ < lidar_end_time_) {
        return false;
    }

    double imu_time = imu_buffer_.front()->timestamp_;
    measures_.imu_.clear();
    while ((!imu_buffer_.empty()) && (imu_time < lidar_end_time_)) {
        imu_time = imu_buffer_.front()->timestamp_;
        if (imu_time > lidar_end_time_) {
            break;
        }
        measures_.imu_.push_back(imu_buffer_.front());
        imu_buffer_.pop_front();
    }

    // 将已处理的 LiDAR 数据从 lidar_buffer_ 和时间缓存 time_buffer_ 中移除，为下一轮同步准备
    lidar_buffer_.pop_front();
    time_buffer_.pop_front();
    lidar_pushed_ = false;

    if (callback_) {
        callback_(measures_);
    }

    return true;
}

4 松耦合 LIO 系统的主要流程

松耦合 LooselyLIO 类持有一个 IncrementalNDTLO（增量 NDT 里程计）对象，一个 ESKF 对象，一个 MessageSync 对象 处理同步之后的点云和 IMU。该类处理流程非常简单：当 MeasureGroup 到达后，在 IMU 未初始化时，使用第 3 章的静止初始化来估计 IMU 零偏。初始化完毕后，先使用 IMU 数据进行预测，再用预测数据对点云去畸变，最后对去畸变的点云做配准。

void LooselyLIO::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();
}

4.1 IMU 静止初始化

IMU 的静止初始化与《自动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航中介绍的大体一致。当 MeasureGroup 到达后，在 IMU 未初始化时，调用 StaticIMUInit::AddIMU() 函数进行 IMU的静止初始化。当 IMU 初始化成功时，在当前 MeasureGroup 中完成 ESKF 中 Q, V, b_g, b_a, g_w, P 的初始化。

void LooselyLIO::TryInitIMU() {
    for (auto imu : measures_.imu_) {
        imu_init_.AddIMU(*imu);
    }

    if (imu_init_.InitSuccess()) {

        // !!! 下面 4 行代码即完成了 Q, V, b_g, b_a, g_w, P 的初始化
        // 读取初始零偏，设置ESKF
        sad::ESKFD::Options options;
        // 噪声由初始化器估计
        options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);
        options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);
        eskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());
        imu_need_init_ = false;

        LOG(INFO) << "IMU初始化成功";
    }
}

IMU 静止初始化结果如下：

I0112 15:59:51.430646 354274 loosely_lio.cc:54] call meas, imu: 10, lidar pts: 3601
I0112 15:59:51.430662 354274 static_imu_init.cc:86] mean acce: -0.00215149 00.00016898 000.0978879
I0112 15:59:51.430694 354274 static_imu_init.cc:109] IMU 初始化成功，初始化时间= 9.99018, bg = -0.00259592 00.00176906 0.000707638, ba = 000.213411 -0.0167615 00-9.70973, gyro sq = 5.96793e-05 4.42613e-05 3.58264e-05, acce sq = 9.71749e-07 1.85436e-06 2.14871e-07, grav = 000.215562 -0.0169305 00-9.80762, norm: 9.81
I0112 15:59:51.430707 354274 static_imu_init.cc:113] mean gyro: -0.00259592 00.00176906 0.000707638 acce: 000.213411 -0.0167615 00-9.70973
imu try init true time:1547714610.30704498
I0112 15:59:51.430723 354274 loosely_lio.cc:100] IMU初始化成功

4.2 ESKF 之运动过程——使用 IMU 预测

IMU 预测部分与先前介绍的 GINS 中预测部分类似。上一个 MeasureGroup 中完成了 IMU 的静止初始化，当前 MeasureGroup 中的 IMU 数据就开始参与 ESKF 的运动过程了。std::vector<NavStated> 类型的 imu_states_ 的大小为 MeasureGroup 中的（IMU 数量 +1），其存储上一 IMU 时刻 ESKF 的名义转态变量和当前 MeasureGroup 中每一个 IMU 数据预测后的 ESKF 的名义转态变量，用来插值进行点云的去畸变。

void LooselyLIO::Predict() {
    imu_states_.clear();
    imu_states_.emplace_back(eskf_.GetNominalState());
    std::cout << "current_time_: " << eskf_.GetTime() << std::endl;
    /// 对IMU状态进行预测
    for (auto &imu : measures_.imu_) {
        eskf_.Predict(*imu);
        imu_states_.emplace_back(eskf_.GetNominalState());
        std::cout << "current_time_: " << eskf_.GetTime() << std::endl;
    }
}

4.3 使用 IMU 预测位姿进行运动补偿

其原理简单来说就是通过固定的世界坐标系，结合每个时刻的插值结果 $T(t)_{WI}$ ，将一帧雷达中所有时刻的点全部转移到雷达扫描结束时刻。

假设比例 $s$ 计算公式如下，其中 $t$ 为待插值的时刻， $t_0$ 为起始时刻， $t_s$ 为结束时刻：

$s_t=\frac{t - t_0}{t_s-t_0}$

旋转部分插值：四元数球面线性插值 (SLERP)，确保旋转路径在旋转空间中的弧长最短。

$q_t = q_{t_0}(q_{t_0}^{-1}q_{t_s})^{s_t}$

平移部分插值：平移向量线性插值 (LERP)

$p_t=(1-s_t)p_{t_0} + s_tp_{t_s}$

注意：这种去畸变的方法前提是滤波器本身有效。如果滤波器失效或位姿发散，去畸变算法也就随之发散了。

SE3 pose_first;
SE3 pose_next;

// 计算旋转插值结果（使用球面线性插值 - SLERP）
Eigen::Quaterniond result_R = pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion());

// 计算平移插值结果（使用线性插值 - LERP）
Eigen::Vector3d result_p = pose_first.translation() * (1 - s) + pose_next.translation() * s;

// 检查结果（仅用于调试）
std::cout << "Interpolated Rotation (Quaternion): " << result_R.coeffs().transpose() << std::endl;
std::cout << "Interpolated Translation: " << result_p.transpose() << std::endl;

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

    if (options_.save_motion_undistortion_pcd_) {
        sad::SaveCloudToFile("/home/wu/slam_in_autonomous_driving/data/ch7/undist/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是该点打到的时间与雷达开始时间之差，单位为毫秒
        // 插值结果为 Ti
        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("/home/wu/slam_in_autonomous_driving/data/ch7/undist/after_undist.pcd", *cloud);
    }
}

/**
 * pose 插值算法
 * @tparam T    数据类型
 * @tparam C 数据容器类型
 * @tparam FT 获取时间函数
 * @tparam FP 获取pose函数
 * @param query_time 查找时间
 * @param data  数据容器
 * @param take_pose_func 从数据中取pose的谓词，接受一个数据，返回一个SE3
 * @param result 查询结果
 * @param best_match_iter 查找到的最近匹配
 *
 * NOTE 要求query_time必须在data最大时间和最小时间之间(容许0.5s内误差)
 * data的map按时间排序
 * @return
 */
template <typename T, typename C, typename FT, typename FP>
inline bool PoseInterp(double query_time, C&& data, FT&& take_time_func, FP&& take_pose_func, SE3& result,
                       T& best_match, float time_th = 0.5) {
    if (data.empty()) {
        LOG(INFO) << "cannot interp because data is empty. ";
        return false;
    }

    // 如果 query_time 超过最大时间，但在容许阈值 time_th 范围内，此时插值的结果直接使用最后一条数据的位姿。
    // rbegin() 返回的是反向迭代器，指向容器的最后一个元素（从后往前遍历的起点）
    double last_time = take_time_func(*data.rbegin());
    if (query_time > last_time) {
        if (query_time < (last_time + time_th)) {
            // 尚可接受
            result = take_pose_func(*data.rbegin());
            best_match = *data.rbegin();
            return true;
        }
        return false;
    }

    auto match_iter = data.begin();
    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        auto next_iter = iter;
        next_iter++;

        if (take_time_func(*iter) < query_time && take_time_func(*next_iter) >= query_time) {
            match_iter = iter;
            break;
        }
    }

    auto match_iter_n = match_iter;
    match_iter_n++;

    double dt = take_time_func(*match_iter_n) - take_time_func(*match_iter);
    double s = (query_time - take_time_func(*match_iter)) / dt;  // s=0 时为第一帧，s=1时为next
    // 出现了 dt为0的bug
    if (fabs(dt) < 1e-6) {
        best_match = *match_iter;
        result = take_pose_func(*match_iter);
        return true;
    }

    SE3 pose_first = take_pose_func(*match_iter);
    SE3 pose_next = take_pose_func(*match_iter_n);
    // 旋转部分使用了四元数的球面线性插值（Slerp）。Slerp（Spherical Linear Interpolation） 是一种在两四元数之间进行插值的方式。与普通的线性插值不同，Slerp 能够保持旋转的路径最短，给出的旋转角度总是通过球面路径最优化。
    //     s 是插值的参数。当 s 在 0 和 1 之间时，结果是 pose_first 和 pose_next 之间的旋转的插值。
    // 平移部分使用线性插值，y = (1-s)*y_0 + s*y_1
    result = {pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion()),
              pose_first.translation() * (1 - s) + pose_next.translation() * s};
    best_match = s < 0.5 ? *match_iter : *match_iter_n;
    return true;
}

4.4 松耦合系统的配准部分

前文已经得到了去畸变的点云，这里只需将其传递给增量 NDT 里程计，并使用滤波器预测得到的先验位姿作为增量 NDT 里程计的初始位姿，经过迭代计算后得到优化后的位姿后再返回给滤波器，滤波器进行观测过程。在这个过程中滤波器部分和点云配准部分是解耦的。

void LooselyLIO::Align() {
    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix()); // 将 lidar 坐标系下的点云转换到 imu 坐标系下
    // scan_undistort_ 为 imu 坐标系下 无畸变的点云
    scan_undistort_ = scan_undistort_trans;

    auto 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);

    /// 处理首帧雷达数据
    if (flg_first_scan_) {
        SE3 pose;
        inc_ndt_lo_->AddCloud(current_scan_filter, pose);
        flg_first_scan_ = false;
        return;
    }

    /// 从EKF中获取预测pose，放入LO，获取LO位姿，最后合入EKF
    SE3 pose_predict = eskf_.GetNominalSE3();
    inc_ndt_lo_->AddCloud(current_scan_filter, pose_predict, true); // 第 3 个参数为 true, 即不使用匀速运动假设推测位姿
    pose_of_lo_ = pose_predict;
    eskf_.ObserveSE3(pose_of_lo_, 1e-2, 1e-2);

    if (options_.with_ui_) {
        // 放入UI
        ui_->UpdateScan(current_scan, eskf_.GetNominalSE3());  // 转成Lidar Pose传给UI
        ui_->UpdateNavState(eskf_.GetNominalState());
    }
    frame_num_++;
}

I0112 21:54:14.812438 383071 ndt_inc.cc:124] aligning with inc ndt, pts: 1532, grids: 819
I0112 21:54:14.812675 383071 ndt_inc.cc:222] iter 0 total res: 2003.41, eff: 960, mean res: 2.08689, dxn: 0.00164213, dx: -0.000169117 00.000230697 00.000262647 00.000125452 0-0.00158076 00.000176706
I0112 21:54:14.812845 383071 ndt_inc.cc:222] iter 1 total res: 1981.36, eff: 954, mean res: 2.0769, dxn: 0.000790319, dx: 07.83699e-06 02.29818e-05 09.93558e-05 -0.000364279 -0.000640747 00.000266245
I0112 21:54:14.812858 383071 ndt_inc.cc:227] converged, dx = 07.83699e-06 02.29818e-05 09.93558e-05 -0.000364279 -0.000640747 00.000266245