4. 点云帧间匹配与里程计：ICP算法原理与变体、NDT算法、基于IMU辅助的帧间匹配、回环检测初探

各位同学，欢迎来到第四章。这一章，我们正式进入高精地图构建的核心环节——帧间匹配与里程计。

说白了，就是解决一个最基础的问题：“我”的激光雷达，在连续两帧点云之间，到底移动了多少？ 这个问题的答案，直接决定了我们能否把散落的点云拼接成一张完整的地图。

我个人习惯把这一章看作是整个建图系统的“脊梁骨”。脊梁骨要是歪了，后面再怎么修修补补，地图也是歪的。今天我们就来掰扯掰扯这根脊梁骨上的几个关键关节：ICP、NDT、IMU辅助，以及回环检测的初探。

4.1 ICP算法：最朴素的“点对点”匹配

ICP，全称Iterative Closest Point，迭代最近点。这名字起得挺直白，就是反复找最近点，然后算变换。

它的核心思想其实很简单：假设两帧点云中，距离最近的两个点，就是物理世界中的同一个点。 然后基于这个假设，去求解一个旋转矩阵R和平移向量t，让这些“配对”的点之间的距离之和最小。

嗯，这里要注意。这个假设在现实中并不总是成立。比如你扫描一个墙角，前一帧看到的是墙的正面，后一帧看到的是墙的侧面，最近的点可能根本不是同一个物理点。这就是ICP的“原罪”——对初值极其敏感。

我在项目中遇到过最典型的情况：车辆在高速上行驶，点云帧间位移很大。直接用ICP，结果直接发散，里程计飞到了马路牙子外面。后来怎么解决的？加了一个匀速运动模型做初值猜测，才把ICP拉回来。

ICP的数学形式，大家看看就好：

目标函数：min(R,t) Σ || R * p_i + t - q_i ||²

其中：
  p_i 是源点云中的点
  q_i 是目标点云中与p_i最近的点
  R 是旋转矩阵
  t 是平移向量

求解这个目标函数，通常用SVD分解。流程大致是：

1. 计算两组点的质心
2. 去质心化，得到协方差矩阵
3. 对协方差矩阵做SVD分解，得到R
4. 由R反算t

你想想看，这个流程每迭代一次，就要重新找一次最近点。如果点云有10万个点，找最近点就是O(n²)的复杂度，非常慢。所以后来有了各种加速方法，比如KD-Tree。

4.2 ICP的变体们：各有各的绝活

经典的ICP太“脆”了，所以业界搞出了很多变体。我挑几个常用的说说：

变体名称	核心改进	适用场景
Point-to-Plane ICP	用点到平面的距离代替点到点的距离	结构化场景（墙面、地面多）
Generalized ICP (GICP)	融合了点对面和面对面的思想，用协方差建模	通用场景，鲁棒性较好
Sparse ICP	引入稀疏范数，对离群点更鲁棒	有大量动态物体干扰的场景

我个人最常用的是Point-to-Plane ICP。为什么？因为城市道路场景中，地面和墙面占了点云的很大比例。点到平面的距离，本质上是在惩罚点沿着法线方向的误差，这比简单的欧氏距离要合理得多。

我曾经在一个地下车库的项目里，用Point-to-Plane ICP配合地面约束，把里程计的漂移从5%降到了1%以内。地下车库没有GPS，全靠这个撑着。

4.3 NDT算法：换个思路，用概率说话

ICP是“点对点”的硬匹配，那NDT（Normal Distributions Transform）就是“分布对分布”的软匹配。

NDT的思路很巧妙：它不直接匹配点，而是先把空间划分成网格，每个网格里统计点云的分布（均值+协方差），然后用这些分布来做匹配。

这样做的好处是什么？

• 速度快：不需要反复找最近点，一次建好网格，后面就是查表
• 对初值不那么敏感：因为用的是概率分布，有一定的“容错”空间
• 天然支持多模态：一个网格里可以有多个分布，适合处理复杂场景

NDT的匹配过程，本质上是一个优化问题：

目标函数：min(T) Σ exp( -0.5 * (x_i - μ_j)ᵀ Σ_j⁻¹ (x_i - μ_j) )

其中：
  x_i 是源点云中的点，经过变换T
  μ_j, Σ_j 是目标点云中对应网格的均值和协方差

嗯，这里要注意。NDT的网格大小是个超参数。网格太大，丢失细节；网格太小，退化成ICP。我一般建议在城市道路场景用1米到2米的网格，高速场景可以放大到4米。

我记得有一次做高速公路的高精地图，用ICP死活匹配不好，因为路面太平坦，缺乏特征。换成NDT之后，虽然单帧匹配精度不如ICP，但胜在稳定，长距离跑下来漂移反而更小。

4.4 基于IMU辅助的帧间匹配：给点云装上“惯性导航”

激光雷达的帧率一般是10Hz，也就是100毫秒一帧。在这100毫秒里，如果车辆急转弯或者颠簸，两帧点云之间的运动可能非常大。光靠点云匹配，很容易跟丢。

这时候，IMU就派上用场了。

IMU（惯性测量单元）提供的是高频的角速度和加速度，频率可以到200Hz甚至更高。它的优势是短时间内非常准，但长时间会漂移。激光雷达正好相反，长时间稳定，但短时间可能跟丢。

你想想看，这俩是不是天生一对？

我常用的做法是松耦合：

1. 用IMU的角速度积分，得到帧间的旋转初值
2. 用IMU的加速度积分（去除重力），得到帧间的平移初值
3. 把这个初值喂给ICP或NDT，作为初始猜测

这样做的好处是，ICP的迭代次数可以从几十次降到3-5次，而且几乎不会发散。

更高级的做法是紧耦合，把IMU的测量方程和点云的匹配方程放在一起优化。这个我们后面讲到多传感器融合的时候再细聊。

4.5 回环检测初探：给里程计“上把锁”

里程计有个天生的毛病——漂移。不管你的ICP做得有多好，IMU辅助有多强，误差总会一点点累积。跑个几百米可能还行，跑个几公里，地图就歪到姥姥家了。

回环检测，就是用来解决这个问题的。

它的核心思想很简单：当车辆再次经过同一个地方时，识别出这个“回环”，然后用这个约束去修正之前累积的误差。

回环检测怎么做？常见的方法有：

• 基于位置的回环：用GPS或里程计的位置信息，当位置接近时，触发回环检测
• 基于外观的回环：用点云的局部特征（比如Scan Context、M2DP）做检索，找到相似的历史帧
• 基于学习的回环：用深度学习提取点云的特征向量，做最近邻搜索

我个人最推荐的是Scan Context。它把一帧点云投影到一个鸟瞰图的环形网格里，然后用环形匹配的方式做检索。这个方法对视角变化不敏感，而且计算量小，非常适合实时系统。

嗯，这里要注意。回环检测不是万能的。如果场景高度对称（比如长直隧道），或者场景变化太大（比如白天和晚上），回环检测很容易产生误匹配。所以，一般会加一个几何验证的步骤，比如用ICP再精匹配一次，确认回环是否成立。

我曾经在一个地下矿山的项目里，因为场景太相似，回环检测疯狂报假阳性。后来加了一个时间窗口约束——只有距离上次经过超过一定时间的回环才接受，才把误检率降下来。

可可精品资料库