从零开始理解NDT算法的原理及应用

1. 概述

NDT，全称 Normal Distributions Transform（正态分布变换），是一种广泛使用的点云配准算法，它的核心思想与ICP截然不同：NDT不直接计算点与点之间的对应关系，而是通过概率模型来描述和匹配点云的表面结构。

核心思想 NDT算法的基本流程可以概括为：

• 建模：将目标点云（或参考点云） 所占的空间划分成一系列网格单元，对于每个包含足够数量点的单元格，计算其内部点云的统计特性——一个多维正态分布（高斯分布），这个分布可以很好地描述单元格内点的位置分布情况。
• 匹配：将源点云（待配准的点云） 通过一个初始猜测的变换投射到这些网格单元中，对于变换后的源点云中的每一个点，它落到哪个单元格，就找到该单元格对应的正态分布模型。
• 评分：根据正态分布的概率密度函数，可以计算该点“属于”这个分布的概率，点越靠近分布的均值（即单元格的中心），其概率得分越高。
• 优化：算法的目标是找到一个最优的变换参数（旋转和平移），使得所有源点云的概率得分之和最大化，这是一个非线性优化问题，通常使用牛顿法等优化算法来求解。

2. 算法步骤

步骤一：划分网格并对目标点云进行建模

将目标点云 Q 所在的空间划分为大小固定的网格单元格，对于每个单元格，检查其中包含的点数，如果点数太少（如小于3个），则忽略该单元格，视为无效单元格，对于有效的单元格，计算其内部所有点的均值 q 和协方差矩阵 Σ。

这定义了一个描述该单元格点分布的正态分布 N(q, Σ)。

步骤二：定义评分函数

对于给定的一个变换参数 T（包含旋转和平移），将源点云 P 变换后得到

对于 P’ 中的一个点 x_i’，它落在某个单元格中，该单元格的分布为 N(q, Σ)。该点的概率得分为：

（注：为了简化，省略了正态分布前面的常数项，因为它不影响优化结果）

最终的评分函数是所有点得分的总和：

步骤三：优化变换参数

目标是找到变换 T 使得评分函数 s(T) 最大化，这是一个标准的非线性优化问题。通常采用牛顿法进行迭代优化，需要计算评分函数对变换参数 T 的梯度和海森矩阵 。 在每一步迭代中，根据当前梯度方向和海森矩阵提供的信息，计算一个变换参数的更新量 ΔT 更新变换：

重复迭代，直到收敛（如更新量 ΔT 小于阈值）。

3. NDT算法的优缺点

优点：

• 无需最近邻搜索：这是相对于ICP最大的性能优势，ICP最耗时的步骤是为每个点找最近邻，而NDT只需在初始化时计算一次网格分布，后续优化过程非常高效。
• 对初始值要求相对宽松：虽然也需要初始估计，但其平滑的概率表示使其收敛域通常比ICP更宽，对初始位姿的敏感度略低。
• 天然抗噪：因为是用分布来描述一个区域，所以对离群点和噪声点不敏感，个别错误点不会像在ICP中那样严重破坏匹配过程。
• 产生连续可微的评分函数：这使得可以使用更强大、更快的优化方法（如牛顿法）。

缺点：

• 网格大小是超参数：网格单元的大小需要人为设定，太大会导致分辨率低，配准精度下降；太小会导致单元格内点太少，无法产生有效的分布，且计算量增加。
• 在空旷或结构化程度低的场景中效果差：如果场景非常空旷（如一大片平地），很多单元格点很少，分布模型不具代表性，导致匹配困难。
• 解析导数复杂：虽然可以使用牛顿法，但梯度向量和海森矩阵的推导和计算较为复杂。

4. 应用场景

a. 自动驾驶与高精地图定位

• 实时定位：自动驾驶车辆通过激光雷达扫描周围环境，生成当前帧点云，NDT被用于将当前帧点云与预先制作好的高精地图（HD Map） 进行匹配，由于其计算效率高和对初始位姿相对鲁棒的特性（结合GPS/IMU提供初始位姿），NDT能实现实时、稳定、厘米级的车辆定位。
• 原因：城市道路环境具有丰富的结构特征（如路灯、护栏、建筑墙面），非常适合用NDT的网格分布来建模，同时，对实时性的要求使得NDT“无需最近邻搜索”的优势极大。

b. 大规模室外环境SLAM

• 激光SLAM中的扫描匹配：与ICP类似，NDT可用于激光SLAM中的帧间匹配（里程计计算）和回环检测，特别是在大规模室外场景中，点云数据量大且可能较为稀疏，NDT的效率优势明显。
• 多激光雷达融合：对于装有多个激光雷达的机器人或车辆，NDT可用于将不同视角的点云快速配准到统一坐标系下。

c. 点云地图构建

• 多帧点云融合：将机器人不同时间、不同地点采集的点云数据通过NDT配准后，融合成一个全局一致、更完整、更稠密的点云地图。

5. NDT与ICP对比