《The-Normal-Distributions-Transform》

计算方法

首先把机器人周围的空间按照固定的大小分成一个个网格，对于点不少于3个的，进行以下计算：

  1. 收集网格中的所有2D点

  2. 计算均值$q=\frac{1}{n}\sum_i \mathbf x_i$

  3. 计算协方差矩阵

    $$ \Sigma=\frac{1}{n}{\textstyle \sum_{i}}(\mathbf x_i-\mathbf q)(\mathbf x_i-\mathbf q)^t $$

在这个单元中包含的二维点$\mathbf x$处测量一个样本的概率现在由正态分布来模拟$N(q,\Sigma)$：

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

和占据网格相同，NDT发布一个平面的常规细分。但是占据网格表示单元被占据的概率，NDT表示在单元内每个位置测量采样的置信度，我们使用单元大小为100cm乘100cm

这个表征有什么用？我们现在有了一个以概率密度的形式对二维平面进行的片状连续和可微调的描述。在我们展示一个例子之前，我们必须注意两个实施细节。

为了尽量减少离散化的影响，我们决定使用四个重叠的网格。也就是说，首先放置一个边长为$l$的单个网格，然后是第二个，水平移动$\frac{l}{2}$，第三个，垂直移动$\frac{l}{2}$，最后是第四个，水平和垂直移动$\frac{l}{2}$。现在每个二维点都落入四个单元。本文其余部分将不明确考虑这一点，我们将描述我们的算法，就好像每个点只有一个单元。因此，如果计算一个点的概率密度，它是在默契的情况下进行的，即所有四个单元的密度都被评估，其结果是相加的。

第二个问题是，对于一个无噪声的测量世界线，协方差矩阵将变得奇异，不能被求逆。在实践中，协方差矩阵有时会接近单值。为了防止这种影响，我们检查$\Sigma$的小特征值是否至少是大特征值的0.001倍。如果不是，则将其设置为这个值。

点云对齐

两个机器人坐标框架之间的空间映射$T$由以下公式给出：

$$ T:\begin{pmatrix} x'\\ y' \end{pmatrix}= \begin{pmatrix} \cos\phi & -\sin\phi\\ \sin\phi & \cos\phi \end{pmatrix}\begin{pmatrix} x\\ y \end{pmatrix}+ \begin{pmatrix} t_x\\ t_y \end{pmatrix} \tag {2} $$

其中$(t_x,t_y)^t$描述了两帧之间的平移和$φ$旋转。 扫描对齐的目标是使用在两个位置进行的激光扫描来恢复这些参数。 给定两次扫描（第一次和第二次），所提出方法的概要如下：

  1. 构建第一次扫描的NDT。
  2. 初始化参数的估计（通过零或使用里程计数据）。
  3. 对于每一个第二次扫描的样本：根据参数将重建的二维点映射到第一次扫描的坐标系中。
  4. 确定每个映射点的相应正态分布。
  5. 通过评估每个映射点的分布并对结果求和来确定参数的分数。
  6. 通过尝试优化分数来计算新的参数估计。 这是通过执行牛顿算法的一步来完成的。
  7. 转到 3，直到满足收敛标准。

前四个步骤很简单： 构建NDT在上一节中进行了描述。 如上所述，里程计数据可用于初始化估计。 使用$T$完成第二次扫描的映射，找到相应的正态分布是在NDT网格中的简单查找。

现在使用以下符号详细描述其余部分：

  • $\mathbf p=(p_i)^t_{1..3}=(t_x,t_y,\phi)^t$：用来估计的参数向量。（二维空间仿射变换）
  • $\mathbf x_i$：在第二次扫描的坐标系中，第二次扫描的激光扫描样本$i$的重建二维点。
  • $\mathbf x'_i$：点$\mathbf x_i$映射到根据参数$\mathbf p$的第一次扫描的坐标帧，$\mathbf x'_i=T(\mathbf x_i,\mathbf p)$。
  • $\mathbf \Sigma_i,\mathbf q_i$：$\mathbf x_i'$的正态分布协方差矩阵和均值，在第一次扫描的NDT中查找。

如果对所有点$\mathbf x_i'$，参数为$\mathbf \Sigma_i$和$\mathbf q_i$的正态分布估计求和是最大值，根据$\mathbf p$的映射能够认为是最佳（optimal）的。我们把这个求和为$\mathbf p$的分数，定义为：

$$ \text{score}(\mathbf{p})=\sum_i \exp\left( \frac{-(\mathbf x_i'-\mathbf q_i)^t\mathbf \Sigma_i^{-1}(\mathbf x_i'-\mathbf q_i)}{2} \right) \tag {3} $$

使用牛顿算法优化

牛顿法

一元函数

泰勒展开：

$$ f(x)=f(x_{0})+f'(x_{0})(x-x_{0})+\frac{1}{2} f''(x_{0})(x-x_{0})^{2}+\ldots+\frac{1}{n !} f^{(n)}(x_{0})(x-x_{0})^{n} \ldots $$

忽略2次以上的项：

$$ f(x)=f\left(x_{0}\right)+f^{\prime}\left(x_{0}\right)\left(x-x_{0}\right)+\frac{1}{2} f^{\prime \prime}\left(x_{0}\right)\left(x-x_{0}\right)^{2} $$

两边求导：

$$ f'(x)=f^{\prime}\left(x_{0}\right)+ f^{\prime \prime}\left(x_{0}\right)\left(x-x_{0}\right)=0 $$

可以解得：

$$ x=x_0-\frac{f'(x_0)}{f''(x_0)} $$

给定初值，反复带入式子，直到达到导数为0的点或最大迭代次数

多元函数

多元函数在$x_0$处泰勒展开

$$ f(x)=f(x_0)+\nabla f(x_0)^T(x-x_0)+\frac{1}{2}(x-x_0)^T \nabla^2f(x_0)(x-x_0)+o((x-x_0)^2) $$

忽略二次及以上的项，上式两边同时求梯度，得到函数的导数（梯度向量）：

$$ \nabla(x)=\nabla f(x_0)+\nabla^2f(x_0)(x-x_0) $$

其中$\nabla ^2f(x_0)$为Hessian矩阵，写为$\mathbf H$。令梯度函数为$0$

$$ \begin{align} \nabla f(x_0)+\nabla^2f(x_0)(x-x_0)=0 \\ x=x_0-\left(\nabla^2f(x)\right)^{-1}\nabla f(x_0) \end{align} $$

解线性方程组，简写为向量形式，梯度向量写为$\mathbf g$

$$ \mathbf x=\mathbf x_0 - \mathbf H^{-1}\mathbf g $$

从初始位置$x_0$开始，反复计算函数在处的Hessian矩阵和梯度向量，然后进行迭代

$$ \mathbf x_{k+1}=\mathbf x_k - \mathbf H ^{-1} \mathbf g $$

最终达到函数常驻点。$-\mathbf H^{-1}\mathbf g$称为牛顿方向。迭代终止的条件是梯度的模接近于0，或者函数数值下降小于指定阈值

优化方法

由于优化问题通常被描述为最小化问题，我们将采用我们的符号来表示这个约定。因此，本节中要最小化的函数是$-\text score$。 牛顿算法迭代地找到参数$\mathbf p = (p_i )^ t $，使函数 $f$ 最小化。 每次迭代求解以下方程：

$$ \mathbf{H\Delta p}=-\mathbf g\tag {4} $$

其中 $\mathbf g$ 是 $f$ 与条目的转置梯度

$$ g_i=\frac{\partial f}{\partial p_i}\tag {5} $$

$\mathbf H$为 $f$ 的Hessian

$$ H_{ij}=\frac{\partial f}{\partial p_i \partial p_j}\tag {6} $$

该线性系统的解为一个添加到当前估计的增量$\mathbf{\Delta p}$

$$ \mathbf p \leftarrow \mathbf p + \mathbf {\Delta p}\tag {7} $$

如果$\mathbf H$为正定，$f(\mathbf p)$最初将沿着$\Delta\mathbf{p}$的方向减少。如果不是这种情况，$\mathbf{H}$将被$\mathbf H'=\mathbf H + \lambda \mathbf I$，$\lambda$的选择，使得$\mathbf H'$是安全正定的。关于最小化算法本身的实际细节可以在以下文章中找到，例如[4]。

正定矩阵

（1）广义定义：设$\boldsymbol M$是$n$阶方阵，如果对任何非零向量$\boldsymbol z$，都有$\boldsymbol{z}^T\boldsymbol{Mz}>0$，其中$\boldsymbol{z}^T$ 表示$\boldsymbol z$的转置，就称$\boldsymbol M$为正定矩阵 （2）狭义定义：一个$n$阶的实对称矩阵M是正定的的条件是当且仅当对于所有的非零实系数向量$\boldsymbol z$，都有$\boldsymbol{z}^T\boldsymbol{Mz}>0$。其中*$\boldsymbol{z}^T$表示z*的转置。

这一算法现在被应用于函数$-\text{score}$。梯度和Hessian是通过收集方程3的所有和的偏导数建立的。为了缩短符号，避免混淆参数号$i$和激光扫描样本的索引$i$，样本号的索引$i$被删除。此外，我们写道

$$ \mathbf q = \mathbf x'_i - \mathbf q_i \tag{8} $$

可以很容易地验证，$\mathbf q$相对于$\mathbf p$的偏导数等于$x'_i$的偏导数。那么，$-\text{score}$的一个总和$s$是由以下公式给出的

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

对于这样的总和，梯度的条目是（使用连锁规则）：

$$ \begin{align} \tilde{g}_i&=-\frac{\partial s}{\partial p_i}=-\frac{\partial s}{\partial q}\frac{\partial q}{\partial p_i} \tag{10}\\ &=\mathbf{q}^t\mathbf{\Sigma}^{-1}\frac{\partial q}{\partial p_i}\exp\frac{-\mathbf q^t\Sigma^{-1}\mathbf q}{2} \end{align} $$

$\mathbf q$ 关于 $p_i$ 的偏导数由 $T$ 的 Jacobi 矩阵 $\mathbf{J_T}$ 给出（见等式 2）：

$$ \mathbf{J_T}=\begin{pmatrix} 1&0&-x\sin\phi-y\cos\phi\\ 0&1&x\cos\phi-y\sin\phi \end{pmatrix}\tag{11} $$

Hessian $\mathbf H$ 中的总和条目由下式给出：

$$ \begin{align} \tilde{H}_{ij}=-\frac{\partial s}{\partial p_i \partial p_j}=&-\exp\frac{-\mathbf q^t \mathbf \Sigma^{-1}\mathbf q}{2}((-\mathbf q^t\mathbf \Sigma^{-1}\frac{\partial \mathbf q}{\partial p_i})(-\mathbf q^t\mathbf \Sigma^{-1}\frac{\partial \mathbf q}{\partial p_j})\\ &+ (-\mathbf q^t\mathbf \Sigma^{-1}\frac{\partial^2 \mathbf q}{\partial p_i \partial p_j}) + (-\frac{\partial \mathbf q^t}{\partial p_j}\mathbf \Sigma^{-1}\frac{\partial \mathbf q}{\partial p_i}) ) \end{align} \tag{12} $$

$\mathbf q$ 的二阶导数是（见方程11）：

$$ \frac{\partial^2 \mathbf q}{\partial p_i\partial p_j}=\left\{\begin{matrix} \left( \begin{matrix} -x\cos\phi+y\sin\phi\\ -x\sin\phi-y\cos\phi \end{matrix} \right) & i=j=3 \\ \left( \begin{matrix} 0\\0 \end{matrix} \right) & \text{othervise} \end{matrix}\right.\tag{13} $$

从这些方程可以看出，构建梯度和Hessian的计算成本很低。 每个点只有一次调用指数函数和少量乘法。 三角函数仅取决于$\phi$（角度的当前估计），因此每次迭代只能调用一次。 接下来的两节将使用该算法进行位置跟踪和 SLAM。

位置跟踪

本节介绍如何应用扫描匹配算法从给定时间 $t = t_{start}$ 跟踪当前位置。 下一节然后将这种方法扩展到 SLAM。

此时全局参考坐标系由局部机器人坐标系定义。 下面将相应的激光扫描称为关键帧。 针对该关键帧执行跟踪。 在时间 $t_k$，算法执行以下步骤：

  1. 令 $\delta$ 为时间 $t_{k-1}$ 和 $t_k$ 之间运动的估计值（例如来自里程计）。
  2. 根据 $\delta$ 映射时间 $t_{k-1}$ 的位置估计。
  3. 使用当前扫描、关键帧的NDT和新的位置估计执行优化算法。
  4. 检查关键帧是否足够“接近”当前扫描。 如果是，迭代。 否则将最后一次成功匹配的扫描作为新的关键帧。

扫描是否仍然足够接近的决定是基于一个简单的经验标准，包括关键帧与当前帧之间的平移和角距离以及结果得分。 为了对位置跟踪有用，算法必须实时执行：在 1.4 GHz 机器上构建扫描的 NDT 需要大约 10 ms。 对于扫描之间的小幅移动，优化算法通常需要大约 1-5 次迭代（很少超过 10 次）。 一次迭代需要大约 2 毫秒，所以实时性没有问题。

SLAM应用

我们将地图定义为关键帧及其全局姿势的集合。 本节介绍当机器人到达未知区域时，如何针对该地图进行定位，以及如何扩展和优化该地图。

A 针对多次扫描进行定位

对于每次地图中扫描$i$和关联的角度$\phi_i$（或者旋转矩阵$\mathbf R_i$）和平移向量$(t_x,t_y)_i^t=\mathbf T_i$。这些描述了全局坐标系中扫描 $i$ 的位姿。 当前机器人位姿由旋转矩阵 $\mathbf R$ 和平移向量 $\mathbf T$ 表示。从机器人坐标系到扫描 $i$ 的坐标系的映射 $T'$ 由下式给出：

$$ T':\begin{pmatrix} x'\\y' \end{pmatrix} = \mathbf R_i^t\left(\mathbf R\begin{pmatrix} x\\y \end{pmatrix}+\mathbf T - \mathbf T_i \right)\tag{14} $$

只需要很小的改动就可以使算法适应这种情况。 现在通过应用$T'$计算二维扫描点 $i$ 的映射。 此外，$T'$的雅可比和二阶偏导数现在变得稍微复杂一些。 映射的雅可比现在由下式给出：

$$ \mathbf J_{T'}=\mathbf R_i^t\mathbf J_T \tag{15} $$

$T'$ 的二阶偏导数现在由下式给出：

$$ \frac{\partial^2 \mathbf x'}{\partial p_i\partial p_j}=\left\{\begin{matrix} \mathbf R_i^t\left( \begin{matrix} -x\cos\phi+y\sin\phi\\ -x\sin\phi-y\cos\phi \end{matrix} \right) & i=j=3 \\ \left( \begin{matrix} 0\\0 \end{matrix} \right) & \text{othervise} \end{matrix}\right.\tag{16} $$

优化算法的梯度和 Hessian 可以通过对所有重叠扫描求和来构建。 但我们找到了一个替代方案，它更快并且产生同样好的结果：对于在机器人位置拍摄的每个扫描样本，确定扫描，其中评估概率密度的结果是最大的。 仅此扫描用于此样本和当前迭代。 这样，为优化算法构建梯度和 Hessian 所需的操作与重叠关键帧的数量无关，除了找到提到的最大值。

B 添加新的关键帧和优化地图

在每个时间步长，地图由一组关键帧及其在全局坐标帧中的位置组成。如果当前扫描与地图的重叠部分太小，地图就会被最后一个成功匹配的扫描所扩展。然后，每一个重叠的扫描都要与新的关键帧分别匹配，产生两个扫描之间的相对姿势。一个图被维护着，它保存着成对匹配结果的信息。

在这个图中，每个关键帧都由一个节点表示。一个节点持有该关键帧在全局坐标框架中的估计姿态。两个节点之间的边表示相应的扫描已被配对，并持有两个扫描之间的相对姿态。

添加新的关键帧后，通过优化定义在所有关键帧参数上的误差函数来优化地图。成对配准的结果用于为每个匹配对定义二次误差模型，如下所示：两次扫描的全局参数还定义了两次扫描之间的相对位姿。令 $\Delta p$ 为全局参数定义的相对位姿与成对匹配结果定义的相对位姿之间的差。然后我们使用二次模型将这两次扫描的分数建模为 $\Delta p$ 的函数

$$ \text {score}'(\Delta p)=\text{score} + \frac{1}{2}(\Delta p)^t\mathbf H(\Delta p) \tag{17} $$

因此得分是最终得分，当成对匹配已经收敛并且$\mathbf H$是由此获得的Hessian。 该模型是通过将 $\Delta p = 0$ 附近的分数进行泰勒展开式推导出来的，直至二次项。 请注意，线性项丢失了，因为我们扩展了一个极值点。 这个分数现在在所有边缘上求和并优化。

如果关键帧的数量变大，这种最小化就不能在实时条件下进行（自由参数的数量是 $3N-3$，其中 $N$ 是关键帧的数量）。因此，我们仅在地图的子图上进行优化。这个子图是通过收集所有关键帧来构建的，可以通过不超过三个边从新关键帧的节点到达。我们现在仅针对属于该子图中包含的关键帧的参数优化上面的误差函数。 当然，如果必须关闭一个循环，我们就必须优化所有关键帧。

参考

  1. 理解牛顿法
  2. 《The-Normal-Distributions-Transform》

扩展

  • 可信域牛顿法
  • 拟牛顿法
    • DFP
    • BFGS
    • L-BFGS
    • Broyden
    • SR1