[自动驾驶-传感器融合] 激光雷达的运动补偿
引言由于激光雷达成像原理是利用接发器与时间计算来获取光点的位置，以是在传感器的空间运动时，会出现雷达拖影现象(点云畸变)，因此必要采用运动补偿来校准激光雷达的点云，本文及介绍下激光雷达的运动补偿原理及实当代码。
相关原理及代码示例
激光雷达（LiDAR）在运动过程中会产生运动畸变，影响点云的精度。运动补偿的基本原理是通过丈量激光在发射和接收过程中的时间差来计算目标物体与激光雷达之间的距离，然后结合雷达运动过程中的速率信息举行运动补偿，从而得到正确的目标位置信息。
运动补偿一样平常必要通过结合惯性丈量单元（IMU）的数据，校正每个激光点的位置。其目标则是将全部的激光雷达点校正到同一时刻的位置。
IMU
IMU（Inertial Measurement Unit，惯性丈量单元）是一种用于丈量物体运动状态的传感器，通常由加快度计、陀螺仪和磁力计组成。
在本文中IMU提供角速率和线性加快度，用于推算雷达的姿态和位置变化。
运动补偿的基本原理
假设激光雷达在时间 t t t 采集到点 P P P，其坐标为 p t \mathbf{p}_t pt。运动补偿将该点校正到参考时间 t 0 t_0 t0 的坐标 p t 0 \mathbf{p}_{t_0} pt0。基本操作是姿态补偿与位置补偿。
[*]姿态补偿：
IMU提供角速率 ω \boldsymbol{\omega} ω，通过积分得到姿态变化 Δ θ \Delta \boldsymbol{\theta} Δθ。旋转矩阵 R \mathbf{R} R 用于描述姿态变化： R = exp ( Δ θ ∧ ) \mathbf{R} = \exp(\Delta \boldsymbol{\theta}^\wedge) R=exp(Δθ∧)其中 Δ θ ∧ \Delta \boldsymbol{\theta}^\wedge Δθ∧ 是角速率的反对称矩阵。
[*]位置补偿:
IMU提供线性加快度 a \mathbf{a} a，通过积分得到位置变化 Δ p \Delta \mathbf{p} Δp。
将点 p t \mathbf{p}_t pt 校正到 t 0 t_0 t0 时刻的坐标： p t 0 = R − 1 ( p t − Δ p ) \mathbf{p}_{t_0} = \mathbf{R}^{-1} (\mathbf{p}_t - \Delta \mathbf{p}) pt0=R−1(pt−Δp)其中： R − 1 \mathbf{R}^{-1} R−1 是旋转矩阵的逆。- Δ p \Delta \mathbf{p} Δp 是位置变化。末了将雷达的每个点均校正至 t 0 t_0 t0 时刻，得到最终的校正点云，则完成了最基本的激光雷达的运动补偿操作。
代码示例
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;
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.time < now_point_time ||
imu_data_vector.time <=
imu_data_vector.time)
continue;
const double dis_time = now_point_time - imu_data_vector.time;
const double front_scale =
dis_time / (imu_data_vector.time -
imu_data_vector.time);
const Eigen::Quaternionf tran_quater =
imu_data_vector.quater.slerp(
front_scale, imu_data_vector.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;
}
参考文献
Hong S , He J , Zheng X ,et al.LIV-GaussMap: LiDAR-Inertial-Visual Fusion for Real-time 3D Radiance Field Map Rendering.IEEE Robotics and Automation Letters, 2024.DOI:10.1109/LRA.2024.3400149.
王军,赵子豪,李玉莲,等.一种基于IMU的激光雷达三维点云实时运动补偿方法:CN202110559368.1.CN202110559368.1.
weixin_39878698——激光雷达+imu_激光雷达slam-激光点云畸变补偿
…
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。
页:
[1]