4、传感器数据异常排查（下）：LiDAR点云畸变与运动补偿
上一节我们聊了IMU和GPS的坑，这一节轮到LiDAR了。说实话，LiDAR点云畸变这个问题，是我入行自动驾驶头两年踩得最深的坑之一。你想想看，一个静止的障碍物，因为车辆运动被扫成了歪的，这要是直接送进定位模块，不飘才怪。
4.1 LiDAR点云畸变是怎么来的？
先搞清楚一个基本事实：LiDAR不是瞬间完成扫描的。机械式LiDAR靠旋转镜面逐点发射激光，一帧点云需要几十毫秒才能拼完。这几十毫秒里，车可能已经移动了好几米。
举个例子，我调试过一台64线LiDAR，转速10Hz，一帧耗时100ms。100ms内，如果车速是72km/h（20m/s），车已经往前走了2米。你想想，一个原本在正前方的路沿，被扫到最后一根线时，已经跑到侧面去了。这就是畸变。
核心结论：点云畸变的本质是「时间差」导致的「空间错位」。每一束激光的发射时刻不同，对应的车辆位姿也不同。
4.2 运动补偿：把点云「掰直」
运动补偿说白了就是：知道每个点是在什么时刻、什么位姿下被扫到的，然后统一变换到同一个参考时刻（通常是帧起始或帧结束）。
我个人习惯的做法是：
- 获取每个点的精确时间戳——从LiDAR驱动里读出来，别自己猜。
- 利用IMU/轮速计做位姿插值——在两次位姿更新之间，用匀速或匀加速模型推算。
- 将每个点变换到帧起始坐标系——用插值得到的位姿做SE(3)变换。
代码实现大致长这样：
// 伪代码：点云运动补偿
for (const auto& point : raw_cloud.points) {
double t_point = point.timestamp; // 每个点的时间戳
double t_start = raw_cloud.start_time;
double ratio = (t_point - t_start) / (raw_cloud.end_time - t_start);
// 插值得到该时刻的位姿
Eigen::Isometry3d T_interp = interpolatePose(ratio);
// 将点变换到帧起始坐标系
Eigen::Vector3d p_compensated = T_interp.inverse() * point.position;
compensated_cloud.push_back(p_compensated);
}避坑指南：我曾经遇到过一个问题——IMU和LiDAR的时间基准不一致，导致插值出来的位姿全是错的。后来我强制要求所有传感器统一使用GPS时间作为主时钟，才彻底解决。
4.3 多传感器时间戳不同步问题
这个问题比点云畸变更隐蔽。你想想，LiDAR用的是自己的时钟，IMU用的是板载时钟，GPS用的是卫星时钟。三个时钟各走各的，漂移速度还不一样。时间戳不同步，你做的所有融合都是白搭。
我总结了一套排查流程：
| 步骤 | 操作 | 常见问题 |
|---|---|---|
| 1 | 检查各传感器时间戳格式 | 有的用秒，有的用纳秒，有的用ROS时间 |
| 2 | 计算时间戳差值序列 | 看差值是否稳定，有没有跳变 |
| 3 | 做时间对齐 | 用PTP或NTP同步，或者软件插值对齐 |
| 4 | 验证对齐效果 | 用静止场景看点云和IMU是否一致 |
嗯，这里要注意：ROS2里有个time_source参数，默认是系统时间。如果你用的是仿真环境，记得改成sim_time，否则时间戳会乱套。
4.4 数据丢帧与异常值处理
数据丢帧这事，说白了就是传感器没按预期输出数据。原因很多：线缆松动、带宽不足、驱动崩溃、CPU过载……我见过最离谱的一次，是因为LiDAR的网线被老鼠咬了一口，时不时丢几个包。
丢帧的排查方法：
- 统计帧间隔——正常LiDAR 10Hz，帧间隔应该是100ms±5ms。如果出现200ms甚至更大的间隔，说明丢帧了。
- 检查ROS2话题频率——用
ros2 topic hz /lidar_points看实际频率。 - 看时间戳连续性——相邻帧的时间戳差值如果突然变大，基本可以断定丢帧。
警告：丢帧不能简单丢弃了事。如果你直接跳过丢掉的帧，定位模块会以为时间没变，导致速度估计出错。我建议的做法是：检测到丢帧时，用IMU做纯惯性递推，填补这段时间的位姿。
异常值处理相对简单一些。LiDAR的异常值通常表现为：距离为0、距离无穷大、或者反射强度异常。我个人习惯的做法是：
- 过滤掉距离为0或NaN的点
- 根据传感器量程过滤（比如超过200m的点直接扔掉）
- 用统计滤波去除孤立点（每个点周围少于3个邻居的，视为噪点）
你想想看，一个飞鸟扫出来的点，如果不处理，可能会被当成障碍物，导致定位匹配出错。这种坑我踩过不止一次。
4.5 本章知识体系
下面这张图总结了LiDAR数据异常排查的核心逻辑，从问题诊断到解决方案，一目了然：
说实话，LiDAR数据异常排查这件事，没有捷径。你只能一个一个环节去查，从硬件到驱动到算法，层层剥开。我当年为了查一个间歇性丢帧的问题，在测试车上蹲了三天，最后发现是电源线接触不良。嗯，这种经历多了，你自然就会形成自己的排查套路。
个人经验：我建议你在系统里加一个「数据健康度监控」模块，实时统计每个传感器的帧率、时间戳间隔、异常点比例。一旦指标超标，立刻报警。这比事后翻日志高效得多。
好了，LiDAR这块的坑就聊到这儿。下一节我们继续聊定位漂移的另一个常见原因——地图与匹配的问题。