[自动驾驶-传感器融合] 激光雷达的运动补偿

引言

由于激光雷达成像原理是利用接发器与时间计算来获取光点的位置，所以在传感器的空间运动时，会出现雷达拖影现象(点云畸变)，因此需要采用运动补偿来校准激光雷达的点云，本文及介绍下激光雷达的运动补偿原理及实现代码。

相关原理及代码示例

激光雷达（LiDAR）在运动过程中会产生运动畸变，影响点云的精度。运动补偿的基本原理是通过测量激光在发射和接收过程中的时间差来计算目标物体与激光雷达之间的距离，然后结合雷达运动过程中的速度信息进行运动补偿，从而得到准确的目标位置信息。

运动补偿一般需要通过结合惯性测量单元（IMU）的数据，校正每个激光点的位置。其目标则是将所有的激光雷达点校正到同一时刻的位置。

IMU

IMU（Inertial Measurement Unit，惯性测量单元）是一种用于测量物体运动状态的传感器，通常由加速度计、陀螺仪和磁力计组成。

在本文中IMU提供角速度和线性加速度，用于推算雷达的姿态和位置变化。

运动补偿的基本原理

假设激光雷达在时间 $t$ 采集到点 $P$，其坐标为 ${\mathbf{p}}_t$。运动补偿将该点校正到参考时间 $t_0$ 的坐标 ${\mathbf{p}}_{t_0}$。基本操作是姿态补偿与位置补偿。

• 姿态补偿：
  IMU提供角速度 $\mathbf{\omega }$，通过积分得到姿态变化 $\Delta \mathbf{\theta }$。旋转矩阵 $\mathbf{R}$ 用于描述姿态变化：$$\mathbf{R}=\exp (\Delta {\mathbf{\theta }}^{\wedge })$$其中 $\Delta {\mathbf{\theta }}^{\wedge }$ 是角速度的反对称矩阵。
• 位置补偿:
  IMU提供线性加速度 $\mathbf{a}$，通过积分得到位置变化 $\Delta \mathbf{p}$。

将点 ${\mathbf{p}}_t$ 校正到 $t_0$ 时刻的坐标：$${\mathbf{p}}_{t_0}={\mathbf{R}}^{-1}({\mathbf{p}}_t-\Delta \mathbf{p})$$其中：${\mathbf{R}}^{-1}$ 是旋转矩阵的逆。- $\Delta \mathbf{p}$ 是位置变化。最后将雷达的每个点均校正至 $t_0$ 时刻，得到最终的校正点云，则完成了最基本的激光雷达的运动补偿操作。

代码示例

void MotionCompensation(const std::vector<IMUData>& imu_data_vector,
                        const pcl::PointCloud<PointXYZIRT>& input_cloud,
                        pcl::PointCloud<PointXYZIRT>& output_cloud) {
  if (input_cloud.points.empty()) return;

  if (imu_data_vector.size() < 5) {
    pcl::copyPointCloud(input_cloud, output_cloud);
    return;
  }
  int points_size = input_cloud.points.size();
  output_cloud.reserve(points_size);

  int imu_index = 0;
  int imu_index_size = imu_data_vector.size() - 1;

  for (int i = 0; i < points_size; i++) {
    PointXYZIRT now_point = input_cloud.points[i];
    if (!std::isfinite(now_point.x) || !std::isfinite(now_point.y) ||
        !std::isfinite(now_point.z))
      continue;

    Eigen::Vector3f this_point(now_point.x, now_point.y, now_point.z);
    const double now_point_time = now_point.timestamp;

    // 寻找与当前点时间最接近的IMU数据
    for (; imu_index < imu_index_size; ++imu_index) {
      if (imu_data_vector[imu_index + 1].time < now_point_time ||
          imu_data_vector[imu_index + 1].time <=
              imu_data_vector[imu_index].time)
        continue;

      const double dis_time = now_point_time - imu_data_vector[imu_index].time;
      const double front_scale =
          dis_time / (imu_data_vector[imu_index + 1].time -
                      imu_data_vector[imu_index].time);
      const Eigen::Quaternionf tran_quater =
          imu_data_vector[imu_index].quater.slerp(
              front_scale, imu_data_vector[imu_index + 1].quater);
      // lidar_to_imu_ 表示的是IMU坐标系到激光坐标系标定位置
      this_point = tran_quater * (this_point + lidar_to_imu_) - lidar_to_imu_;
      break;
    }

    now_point.x = this_point.x();
    now_point.y = this_point.y();
    now_point.z = this_point.z();
    output_cloud.points.emplace_back(std::move(now_point));
  }

  output_cloud.width = output_cloud.points.size();
  output_cloud.height = 1;
  output_cloud.is_dense = true;
  return;
}

参考文献

[1] Hong S , He J , Zheng X ,et al.LIV-GaussMap: LiDAR-Inertial-Visual Fusion for Real-time 3D Radiance Field Map Rendering[J].IEEE Robotics and Automation Letters, 2024.DOI:10.1109/LRA.2024.3400149.

[2] 王军,赵子豪,李玉莲,等.一种基于IMU的激光雷达三维点云实时运动补偿方法:CN202110559368.1[P].CN202110559368.1[2025-02-15].

[3] weixin_39878698——激光雷达+imu_激光雷达slam-激光点云畸变补偿

…