点云配准 -- 2D NDT 算法

1 背景

上一篇文章中介绍了什么是点云配准和经典的 ICP 算法。

点云配准(Point Cloud Registration):输入两幅点云 $P_s$ (source) 和 $P_t$ (target) ,输出一个变换 $T$ 使得 $T(P_s)$ 和 $P_t$ 的重合程度尽可能高。

点云配准的一个常见应用就是用来解决无人车的定位(localization)问题。

什么是无人车的定位呢?就是确定无人车在世界中的什么位置,具体来说,是确定无人车在我们预先构建的地图中的哪个位置。

定位的精度直接影响感知模块和规划控制模块的效果,一般来说对无人车的定位精度要求都在 厘米 级别(平均误差 10 厘米以内)。

如何实现无人车的定位?这取决于你使用什么样的传感器配置。

传统的单点 GPS 定位精度在米级别(3到5米),而我国城市主干道的单一车道宽一般是 3.75 米,因此单独靠 GPS 无法实现可靠的定位系统。

使用差分GPS(differential GPS-DGPS,DGPS)可以有效提高 GPS 的定位精度,常见的 RTK(Real time kinematic) 载波相位差分技术可以达到厘米级精度,基本满足自动驾驶定位的需求。但 RTK 需要架设基站,且较容易受卫星状况、天气状况、数据传输状况的影响。

使用 GPS 属于全局定位的方法,还有一类相对定位的方法,比如使用 IMU 和里程计(Odometry),根据上一时刻的位置和方位推断现在的位置和方位,也叫航迹推算,优点是输出频率高、短时间内精度高,缺点是定位误差会随着时间累积。

其实仔细思考一下人开车的过程,我们的大脑是没有 GPS 和 IMU 这种类似的东西的,我们靠的是我们的眼睛,先对环境进行感知、观察,然后同大脑里已经见过的位置场景进行“匹配”,从而实现了定位。这是一种基于环境特征匹配的定位。

无人车的“眼睛”是什么呢?虽然基于相机的视觉方案潜力巨大,但目前业界使用最普遍的还是 3D 激光雷达(Lidar)。我们可以使用 Lidar 进行基于环境特征的定位,用我们每一帧得到的点云和预先制作的地图进行匹配,从而得到实时的车的位置和姿态,这就可以利用点云配准。

我们可以使用 ICP 算法完成点云配准,但 ICP 算法对位姿初值较敏感,且最近邻点搜索较耗时。

NDT(Normal Distribution Transform,正态分布变换) 算法是一种基于概率分布的点云配准算法,其比 ICP 算法耗时更加稳定,实际定位测试效果也更加鲁棒。本文先只介绍 2D NDT 算法,主要是为了给出 NDT 算法的基本思想和原理,3D 的情况留到下一篇。


2 正态分布变换(Normal Distributions Transform)

The Normal Distributions Transform: A New Approach to Laser Scan Matching 这篇论文提出了用于匹配两幅 2D 激光扫描点云的 NDT 算法。

NDT 配准的基本思想如下:我们不用去比较两个点云里点到点之间的距离,而是先将参考点云/目标点云(reference scan,在无人车定位的场景里就是我们事先构建的点云地图)网格化,按照空间划分为一个个 小格子 ,统计出每个小格子里点云的 正态分布 。如果给定一组变换参数(旋转、平移),对源点云进行变换后,则可以得到其在参考点云网格中每个格子里对应的 概率密度 ,概率密度越大,说明两幅点云匹配程度越高。因此我们可以通过优化的方法求解变换参数,使得变换后的源点云同参考点云匹配尽可能匹配。

NDT 把参考点云表示成了一组局部正态分布的集合,具体做法如下:

  1. 将参考点云所在 2D 空间划分为指定大小的网格(cell),设每个格子里的点为 $ \{\mathbf{x}_i\}_{i=1,…,n} $
  2. 计算每个格子中点云的多元正态分布参数:
    • 均值 $\mathbf{q} = \frac{1}{n}\sum_i \mathbf{x}_i$
    • 协方差矩阵 $\Sigma = \frac{1}{n}\sum_i (\mathbf{x}_i - \mathbf{q}) (\mathbf{x}_i - \mathbf{q})^t$

这样每个格子中的一个点 $\mathbf{x}$ 可以用概率分布 $N(\mathbf{q}, \Sigma)$ 来描述:

$$ p(\mathbf{x})\sim exp(- \frac{(\mathbf{x}_i - \mathbf{q})^t \Sigma^{-1} (\mathbf{x}_i - \mathbf{q})}{2})
$$

NDT 这种表示方法的好处是我们得到了 2D 空间的一种分段 连续、可微 的概率密度形式的描述。这对于我们求解优化是十分有帮助的(能求导)。(对比 ICP 算法,其查找对应点关系的时候只能用暴力或者启发式方法)

原论文里提到求解协方差矩阵时有可能会出现奇异(singular)的情况,导致无法求逆。解决方法是检查协方差矩阵较小的特征值是否比较大特征值的 0.001 倍要大,如果过小,将其设为较大特征值的 0.001 倍。(数值计算里的内容,计算协方差矩阵的逆之前,先用奇异值分解来检查矩阵是否接近奇异。如果最大奇异值比最小的大1000倍以上,即条件数大于1000,则将条件数限制在1000,用此时对应的协方差矩阵求逆。)


3 利用牛顿法求解优化问题进行点云配准

我们记 2D 空间的刚性变换为:

$$
T =
\left[ \begin{array}{cccc}
cos(\phi) & -sin(\phi) & t_x \\
sin(\phi) & cos(\phi) & t_y \\
0 & 0 & 1
\end{array} \right]
$$

$(t_x, t_y)$ 是平移, $\phi$ 是旋转角度。我们的目标就是给定源点云 $P_s$ 和参考点云 $P_t$ ,求解出这三个参数使得两幅点云“对齐”。

我们使用 NDT 来进行点云对齐,步骤如下:

  1. 对参考点云 $P_t$ 建立 NDT(网格化,计算每个格子的正态分布参数);
  2. 给定初始化变换 $T_0$ ;
  3. 利用上一次迭代得到的变换 $T_{k-1}$ 将 $P_s$ 进行变换,计算变换后的点在参考点云 NDT 网格中的概率密度;
  4. 定义 NDT 配准分数(NDT score)为每个网格中计算出的概率密度之和,使用牛顿法进行优化,求解变换参数使得 NDT score 最大,得到当前迭代的变换 $T_k$;
  5. 回到第 3 步继续进行迭代,直到满足收敛条件。

下面我们来推导一下如何用牛顿法来进行优化。牛顿法利用目标函数的二阶偏导数,考虑梯度的变化趋势,比梯度下降法在确定搜索方向上考虑了更多信息。由于一般优化问题都是求最小化,我们这里记目标函数 $f = -score$,变换参数为 $\mathbf{p}=[t_x, t_y, \phi]^t=[p_1, p_2, p_3]^t$。牛顿法每次迭代是求解如下方程来得到搜索步长 $\Delta \mathbf{p}$:

$$
H \Delta \mathbf{p}= -\frac{\partial f}{\partial \mathbf{p}}
$$

其中 $H$ 是目标函数的 Hessian 矩阵,即 $H_{ij}=\frac{\partial f}{\partial{p_i}\partial{p_j}}$;$\frac{\partial f}{\partial \mathbf{p}}$ 是梯度。下面我们来推导他们的公式。

对于源点云中的一个点 $\mathbf{x}_i$ ,如果变换后它落在 NDT 参考网格的某个格子中,我们记变换后的点为 $\mathbf{x}_i^{\prime}$,则有 $\mathbf{x}_i^{\prime}=T(\mathbf{x}_i;\mathbf{p})$,即

$$
\left[ \begin{array}{c}
x^{\prime} \\
y^{\prime}
\end{array} \right]=
\left[
\begin{array}{ccc}
cos(\phi) & -sin(\phi) \\
sin(\phi) & cos(\phi) \\
\end{array}
\right ]
\left[ \begin{array}{c}
x^{\prime} \\
y^{\prime}
\end{array} \right] +
\left[ \begin{array}{c}
t_x \\
t_y
\end{array} \right]
$$

变换后的点 $\mathbf{x}_i^{\prime}$ 的概率密度为:

$$
p(\mathbf{x}_i) \sim exp(-\frac{(\mathbf{x}_i - \mathbf{q}_i)^t \Sigma_i^{-1} (\mathbf{x}_i - \mathbf{q}_i)}{2})
$$

将所有点 $\{\mathbf{x}_i\}$ 对应的概率密度相加,得到总的 NDT 配准分数:

$$
score(\mathbf{p})=\sum_i exp(-\frac{(\mathbf{x}_i - \mathbf{q}_i)^t \Sigma_i^{-1} (\mathbf{x}_i - \mathbf{q}_i)}{2})
$$

这个 $score(\mathbf{p})$ 就是我们要优化的目标函数。使用牛顿法求解此优化的关键是确定一阶和二阶导数信息,也就是梯度向量和 Hessian 矩阵。

因为这个目标函数是由每个格子里点的概率密度累加而来的,我们只需要分析每个点对应的分数 $s$ 的导数信息。

令 $\mathbf{q}=\mathbf{x}_i - \mathbf{q}_i = T(\mathbf{x}_i;\mathbf{p}) - \mathbf{q}_i$,则

$$
s = -exp(-\frac{\mathbf{q}^t\Sigma^{-1}\mathbf{q}}{2})
$$

根据链式法则以及向量、矩阵求导公式,$s$ 梯度为:

$$
\frac{\partial{s}}{\partial{p_i}} = \frac{\partial{s}}{\partial{q}} \frac{\partial{q}}{\partial{p_i}} = \mathbf{q}^t \Sigma^{-1} \frac{\partial{T(\mathbf{x};\mathbf{p})}}{\partial{p_i}} exp(-\frac{\mathbf{q}^t\Sigma^{-1}\mathbf{q}}{2}), i=1,2,3
$$

这里我们需要求变换 $T$ 的 Jacobian 矩阵:

$$
\frac{\partial{T(\mathbf{x};\mathbf{p})}}{\partial{\mathbf{p}}}=J_{T}=
\left[
\begin{array}{cccc}
1 & 0 & -x \sin(\phi) - y \cos(\phi)\\
0 & 1 & x \cos(\phi) - y \sin(\phi)
\end{array}
\right]
$$

下面我们继续求 $s$ 关于 $p_i$、$p_j$ 的二阶偏导数:

$$
\begin{aligned}
H_{ij} &= \frac{\partial^2 s}{\partial p_i \partial p_j} = \partial(\frac{\partial s}{\partial p_i}) / \partial p_j =\partial(\mathbf{q}^t \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial{p_i}} exp(-\frac{\mathbf{q}^t\Sigma^{-1}\mathbf{q}}{2})) / \partial p_j\\
&= -exp(\frac{-\mathbf{q}^t\Sigma^{-1}\mathbf{q}}{2})\bigg[(\mathbf{q}^t\Sigma^{-1}\frac{\partial \mathbf{q}}{\partial p_i})(\mathbf{q}^t\Sigma^{-1}\frac{\partial \mathbf{q}}{\partial p_j}) - (\frac{\partial \mathbf{q}}{\partial p_j}^t \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial p_i}) - (\mathbf{q}^t\Sigma^{-1}\frac{\partial^2 \mathbf{q}}{\partial p_i \partial p_j})\bigg]
\end{aligned}
$$

其中,向量 $\mathbf{q}$ 对变换参数 $p$ 的二阶导数的向量为:

$$
\frac{\partial^2 \mathbf{q}}{\partial p_i \partial p_j} =
\begin{cases}
[-x \cos(\phi) + y \sin(\phi), x \cos(\phi) - y \sin(\phi)]^t, i=j=3 \\
[0, 0]^t, otherwise
\end{cases}
$$

有了一阶、二阶导数信息,我们就可以利用牛顿法迭代进行优化。

再次总结一下 NDT 点云配准的步骤:

  1. 将参考点云网格化;
  2. 计算参考点云网格中每一个格子的正态分布统计信息;
  3. 将源点云中每个点用上一次得到的变换参数变换到参考点云网格中,计算 NDT score;
  4. 利用牛顿法求解最优搜索步长 $\Delta \mathbf{p}$,得到新的变换参数;
  5. 判断是否满足收敛条件,如果否则返回第 3 步继续迭代。

参考资料

The Normal Distributions Transform: A New Approach to Laser Scan Matching.

两种常见的点云配准方法ICP&NDT

NDT(Normal Distributions Transform)算法原理与公式推导