激光SLAM实战解析：如何高效去除激光雷达运动畸变

1. 激光雷达运动畸变现象解析

第一次用激光雷达做SLAM时，我盯着建图结果里那些扭曲的墙面线条百思不得其解——明明机器人走的是直线，为什么地图上的墙都变成了波浪形？这就是典型的运动畸变现象。简单来说，当激光雷达以10Hz频率工作时，每帧数据的采集需要100ms。在这段时间里，以1m/s速度移动的机器人已经走了10厘米，导致单帧数据内不同时刻的激光点实际处于不同坐标系。

这种现象在三角测距雷达上尤为明显。我测试过某款售价2000元的国产雷达，在机器人旋转时，单帧数据会产生超过20cm的位置偏差。而TOF雷达虽然抗干扰能力强，但在高速运动场景下同样会出现明显畸变。去年给物流AGV做导航时，就遇到过因为畸变导致货架识别位置偏移，最终引发碰撞的案例。

运动畸变的本质是时空不同步问题。就像用手机拍行驶中的汽车，如果快门速度不够快，照片就会模糊。激光雷达的每个点相当于一次独立"拍照"，当这些"照片"的坐标系因机器人运动发生变化时，就形成了我们看到的畸变。

2. 两种主流去畸变方案对比

2.1 纯估计算法的困境

ICP（迭代最近点）是最常用的点云配准方法，我在早期项目中经常直接拿来去畸变。它的核心思想是通过迭代计算旋转矩阵R和平移向量t，使得两帧点云的距离误差最小。具体实现时，先用kd-tree加速最近邻搜索，然后SVD分解求最优变换：

python复制def icp_step(source, target):
    # 去中心化
    src_centroid = np.mean(source, axis=0)
    tgt_centroid = np.mean(target, axis=0)
    H = (source - src_centroid).T @ (target - tgt_centroid)
    U, _, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    t = tgt_centroid - R @ src_centroid
    return R, t

但实际使用时发现三个致命问题：首先，ICP假设两帧点云间存在刚性变换，而运动畸变是非刚性的；其次，当初始位姿误差较大时容易陷入局部最优；最重要的是，处理10Hz的雷达数据时，ICP的迭代计算耗时经常超过1