雷达系列论文翻译（十二）：Cartographer系列（一）

Efficient Sparse Pose Adjustment for 2D Mapping

摘要

位姿图是解决SLAM问题的一种常用表示方法。位姿图是一组机器人位姿的几何，这些位姿由特征观测得到的非线性约束连接到附近的位姿。大型位姿图的优化一直是移动机器人的一个瓶颈，因为直接非线性优化的计算时间会随着图的大小呈立方增长。本文提出了一种构造和求解线性子问题的有效方法，这是这些直接方法的瓶颈。我们比较了稀疏位姿调整(SPA)方法和其他间接方法，结果展示我们的方法的收敛速度和精度都大大超过了其他方法。我们在一组大的室内真实地图和一个非常大的仿真数据集上展示了它的有效性。C++中的开源实现和数据集是公开可用的。

引言

最近有关机器人建图的文献表明，对基于图的SLAM方法的兴趣越来越大。在最常见的形式中，图中有表示机器人位姿态和世界特征的节点，测量值作为约束将它们连接起来。所有方法的目标都是联合优化节点的姿态，以最小化约束带来的误差。这个问题的一个经典变体来自计算机视觉，被称为BA，它通常用Levenberg-Marquardt（LM）非线性优化器的一个特殊变体来求解。

由于特征的数量往往超过机器人的位姿的数鲁昂，可以通过将特征观测转换为机器人位姿之间的直接约束（通过边缘化或者直接匹配）、来创建更多的紧凑系统。在典型的机器人建图应用中，位姿约束系统表现出了稀疏的连接结构，因为传感器的范围通常限制在机器人附近。

有效地求解位姿图（即寻找节点的最优位置）是这些方法的关键问题，特别是在在线建图问题中。一个$100m\times 100m$办公空间的典型二维激光地图可能有几千个节点和更多的约束。此外，向该地图添加闭环约束可以影响系统中几乎所有的位姿。

LM方法的核心在于解决一个大的稀疏线性问题。本文提出了一种从约束图中高效计算稀疏矩阵的方法，并用直接稀疏线性方法求解。类似于计算机视觉文献中提到的稀疏BA，我们称这种方法为稀疏位姿调整(SPA)，因为它处理的是位姿和位姿之间的约束。将SBA/GraphSLAM优化器与求解线性子问题的有效方法相结合具有以下优点

• 它考虑了约束中的协方差信息，从而得到更精确的解。
• SPA对初始化具有鲁棒性和容错性，对于增量和批处理都具有很低的故障率（陷入局部极小值）。
• 收敛速度非常快，因为它只需要对LM方法进行几次迭代。
• 与EKF和信息滤波器不同，SPA是完全非线性的：每次迭代时，它都会将当前位姿周围的所有约束线性化。
• SPA在批处理和增量模式下都是有效的。

我们在“实验结果”部分中记录了该方法的这些和其他特性，并将我们的方法与其他LM和非LM最先进的优化器进行了比较。

SPA的一个好处是建图系统可以连续不断地优化它，提供所有节点的最佳全局估计，而且只需很少的计算开销。在增量模式下，在添加每个节点后对图进行优化，添加任何节点后进行优化所需的时间都不超过15毫秒。

虽然SPA可以用于三维位姿参数化，但本文将其局限于二维建图，这是一个发展良好的领域，具有多种具有竞争性的优化技术。我们的目的是表明，如果使用SPA作为其优化引擎，即使在带有大尺度闭环的大规模环境中，基于二维位姿的建图系统也可以在线运行。而不需要使用子图来进行储存或使用复杂的切分方案

系统形成

解决SLAM问题的流行方法是所谓的“基于图的”或“基于网络的”方法。这种思想是用一个图来表示机器人的历史测量，图中的每个节点都表示一个传感器测量值或者一个局部地图，并在节点上标记测量被获取的位置。两个节点之间的边表示由连接测量的对齐产生的空间信息，可以视为两个节点之间的空间约束。

在基于图的SLAM的上下文中，通常会考虑两个不同的问题。第一种是基于传感器数据的约束识别。由于环境中潜在的模糊性或对称性，这种所谓的数据关联问题通常很难解决。这个问题的解决方案通常被称为SLAM前端，它直接处理传感器数据。第二个问题是修正机器人的姿态，以获得给定约束条件下环境的一致地图。这部分方法被称为优化器或SLAM后端。它的任务是寻找一个节点的配置，使约束中编码的测量的似然最大化。物理学中的弹簧-质量模型给出了这个问题的另一种观点。在这个视图中，节点被视为质量，约束被视为与质量相连的弹簧，弹簧和质量的最小能量配置描述了建图问题的解决方案。

如下图所示，在其操作过程中，基于图形的SLAM系统交错执行前端和后端。这是必需的，因为前端需要在部分优化的地图上操作，以限制对潜在约束的搜索。当前的估计越精确，前端产生的约束就越鲁棒，运算速度也就越快。因此，以估计精度和执行时间衡量的优化算法的性能对整个建图系统有很大影响。

本文详细描述了一种高效、紧凑的二维图优化方法。我们的算法可以与处理不同类型传感器的任意前端耦合。为了表达的清晰，我们简要地描述了激光数据的前端。然而，提到的概念可以直接应用于不同的传感器。

稀疏位姿调整

为了优化一组姿势和约束，我们使用著名的Levenberg-Marquardt（LM）方法作为一个框架，通过特定的实现，可以有效地处理二维地图构建中遇到的稀疏系统。类似于计算机视觉中的稀疏BA，它是一个类似于LM的有效实现，用于相机和特征，我们称之为稀疏位姿调整(SPA)

A.误差公式

系统变量是机器人的全局位姿的集合$c$，由平移和角度参数化：$c_i=[t_i,{\theta }_i]=[x_i,y_i,{\theta }_i{]}^T$. 约束是从其他节点$c_j$的位置对节点$c_i$的测量。在$c_i$的坐标系下，$c_i$和$c_j$之间的测量偏移为̄${\bar{z}}_{ij}$，精度矩阵${\wedge }_{ij}$（协方差矩阵的逆）。对于任何$c_i$和$c_j$的实际姿态，其偏移量可计算为
h ( c i , c j ) = { R i T ( t j − t i ) θ i − θ i (1) h(c_i,c_j) = \{ \begin{matrix} R_i^T(t_j - t_i) \\ \theta_i - \theta_i\end{matrix} \tag{1} h(ci​,cj​)={RiT​(tj​−ti​)θi​−θi​​(1)
其中$R_i$是代表${\theta }_i$的$2\times 2$的旋转矩阵，$h(c_i,c_j)$被叫做测量方程。

与约束关联的误差函数和总误差是
$$\begin{gathered} e_{ij}={\bar{z}}_{ij}-h(c_i,c_j) \\ {\chi }^2(c,p)=\sum _{ij}{e}_{ij}^T{\wedge }_{ij}{e}_{ij}\text{\hspace{1em}(2)} \end{gathered}$$
注意，角度参数在$h(c_i,c_j)$中不是唯一的，因为加上或减去$2\pi$会得到相同的结果。角度差总是被归一化到$(-\pi ,\pi ]$中。

B.线性系统

通过最小化方程2中的总误差，可以找到最优的位置。解决这个问题的标准方法是Levenberg-Marquardt（LM），围绕$c$的当前值迭代线性化求解。线性系统是把变量$c$叠加成一个向量$x$，把误差函数叠加成一个向量$e$存储。然后我们定义：
$$\begin{array}{c}\Lambda =\left[\begin{array}{ccc}{\Lambda }_{ab}& & \\ & \ddots & \\ & & {\Lambda }_{mn}\end{array}\right]\\ \\ J=\frac{\partial e}{\partial x}\\ \\ H=J^T\Lambda J\end{array}\text{\hspace{1em}(3)}$$
LM系统是：
$$(H+\lambda diagH)\Delta x=J^T\Lambda e\text{\hspace{1em}(4)}$$
这里$\lambda$是一个小的正乘数，它在提督下降法和牛顿-欧拉法之间转换。梯度下降法鲁棒性强，不易陷入局部极小，但收敛速度慢；牛顿-欧拉法有相反的行为。

矩阵$H$由每个测量值$h(c_i,c_j)$的四个分量相加而成：
$$\begin{array}{ccccc}\ddots & & & & \\ & J_i^T{\Lambda }_{ij}{J}_i& \ddots & J_i^T{\Lambda }_{ij}{J}_j& \\ & ⋮& \ddots & ⋮& \\ & J_j^T{\Lambda }_{ij}{J}_i& ⋮& J_j^T{\Lambda }_{ij}{J}_j& \\ & & & & \ddots \end{array}\text{\hspace{1em}(5)}$$
在这里，我们稍微滥用了$J$的符号，$J_i$是误差$e_{ij}$关于变量$c_i$的雅各比，分量都是$3\times 3$矩阵块。右侧通过为每个约束添加$3\times 1$矩阵块$J_{c_i}{\Lambda }_{ij}{e}_{ij}$和$J_{c_j}{\Lambda }_{ij}{e}_{ij}$形成。

解线性方程得到一个增量$\Delta x$可以添加回$x$的当前值，如下所示：
$$\begin{array}{c}{t}_i=t_i+\Delta t_i\\ {\theta }_i={\theta }_i+\Delta {\theta }_i\end{array}\text{\hspace{1em}(6)}$$

E.误差的雅各比

出现在方程（4）中的测量函数的雅可比矩阵如下。
$$\begin{array}{cc}\frac{\partial e_{ij}}{\partial t_i}=\left[\begin{array}{c}-R_i^T\\ 00\end{array}\right]& \frac{\partial e_{ij}}{\partial {\theta }_i}=\left[\begin{array}{c}-\partial R_i^T/\partial {\theta }_i(t_j-t_i)\\ -1\end{array}\right]\\ & \\ \frac{\partial e_{ij}}{\partial t_i}=\left[\begin{array}{c}{R}_i^T\\ 00\end{array}\right]& \frac{\partial e_{ij}}{\partial {\theta }_i}={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T\end{array}$$

D.稀疏性

我们对大型系统感兴趣，在这些系统中，位姿$||c||$的数量可以是10k或更多（我们能找到的最大的真实世界室内数据集大约是3k个位姿，但我们可以生成任意顺序的合成数据集）。系统变量的数目是 $3||c||$，$H$矩阵是$||c|{|}^2$，或超过$10^8$个元素。操作如此大的矩阵是昂贵的。幸运的是，对于典型的场景，约束的数量只随着位姿的数量线性增长，因此$H$矩阵非常稀疏。我们可以利用稀疏性来更有效地求解线性问题。

对于稀疏格式的(4)，我们使用CSparse软件包。这个软件包有一个高度优化的适用于稀疏线性系统的Cholesky分解求解器。它采用了几种有效的分解策略来分解矩阵$H$，包括逻辑排序和一种近似最小度（AMD）算法重新排序变量当$H$矩阵非常大时。

一般来说，矩阵分解的复杂度是$O(n^3)$。对于稀疏矩阵来说，复杂度将取决于Cholesky因子的密度，而Cholesky因子又取决于$H$的结构及其变量的顺序。Mahon等人分析了作为SLAM系统中矫正闭环函数的Cholesky分解的行为。如果闭环的数目是常数，那么Cholesky因子密度是$O(n)$并且分解复杂度是$O(n)$。如果闭环的个数与变量的个数成线性增长，则Cholesky因子密度增长速度为$O(n^2)$并且分解复杂度是$O(n^3)$。

E.压缩列储存

LM算法的每一次迭代都有三个步骤：建立线性系统、分解矩阵$H$和求解$\Delta x$。建立系统对于约束的数量是线性的（因此在大多数基于图的SLAM系统中对于变量数量上也是线性的）。在许多情况下，它是线性解算器更耗时的部分。在这里，我们概述了一种根据等式（5）生成的约束建立矩阵$H$的稀疏矩阵形式的有效方法。

CSparse使用压缩列存储（CCS）格式来储存稀疏矩阵。下图显示了其基本思想。

数组中的每个非零项都放在val向量中。项首先按列排序，然后按列中的行排序。col_ptr对于每一列都有一个元素，指明了前面所有列一共有多少个非0元素，而row_ind指明了每一列中非0元素的行索引。

CCS格式的存储效率很高，但很难以增量方式创建，因为向列中添加的每个新的非零元素都会导致所有后续条目发生偏移。最有效的方法是按列顺序创建稀疏矩阵，这将需要循环遍历约束$||c||$次。相反，我们只遍历一次约束，并存储每个$3\times 3$ 矩阵块$J_i^T{\Lambda }_{ij}{J}_j$以一种特殊的块定向数据结构，与CCS格式并行。算法见下表。

在这个算法中，我们通过一个约束将$3\times 3$块矩阵存储到C++ std::map数据结构，每列一个。map是基于其键（即行索引）的有序插入的有效方法。一旦创建了此数据结构，我们就使用map的有序性来创建稀疏的CCS格式，通过按键的顺序在每个map上循环，首先创建列和行索引，然后输入这些值。将列/行创建与值插入分离的原因是，前者只需在LM的任何一组迭代中进行一次。

注意，只存储上三角元素，因为CSparse中的Cholesky解算器只查看这一部分，并且假设矩阵是对称的。

F.连续LM系统

LM系统算法详见下表。

对于一组具有相关测量的节点，它在LM算法中只执行一个步骤。运行单个迭代允许LM的增量操作，因此可以在迭代之间添加更多节点。该算法是连续的，因为λ在两次迭代之间被保存，所以连续的迭代可以根据它们的结果改变λ。其思想是，添加少量节点和测量值不会对系统造成太大的改变，因此λ的值具有梯度下降与欧拉-牛顿法的状态信息。当闭环矫正发生时，系统很难找到一个好的极小值，并且λ会在接下来的几次迭代中上升，从而使系统沿着一条好的路径运行；
有许多不同的方式用来调整$\lambda$。我们选择一个简单的。系统从一个小的$\lambda =10^{-4}$开始.如果更新后的系统的误差比原来的系统小，则$\lambda$减半。如果误差相同或更大，则$\lambda$加倍。这在增量优化的情况下非常有效。只要增加节点时误差减小，$\lambda$就减小，系统就保持在牛顿-欧拉区域。当添加一个连接关系导致系统发生无法校正的大失真时，$\lambda$会上升，系统返回到更鲁棒的梯度下降。

扫描匹配

SPA需要从激光扫描（或其他传感器）的匹配中得到精确（逆协方差）估计。一些扫描匹配算法可以提供这一点，例如，Gutmannet al.使用点和参考扫描中提取的线进行匹配，并返回高斯误差估计。最近，由Olson扩展的Konolige和Chou的相关方法提供了一种在给定范围内寻找全局最佳匹配的有效方法，同时返回精确的协方差。该方法允许单个扫描或一组对齐的扫描与另一个单个扫描或一组对齐的扫描相匹配。在SRI的建图系统Karto中，这种方法用于序列扫描的局部匹配，以及中扫描集合之间的闭环匹配。

结论

在本文中，我们提出并实验验证了一个应用于二维位姿图的称为稀疏位姿调整（SPA）的非线性优化系统。SPA依赖于有效的线性矩阵构造和稀疏的非迭代Cholesky分解进行高效地表示和求解大型稀疏位姿图，我们所能找到的所有真实数据集都不具有挑战性，即使是在批处理模式下。最大的地图需要亚秒的时间得到了全局最优。在线计算最差的结果为10ms/节点；与EKF滤波器或其他计算性能较差的方法不同，我们不必将地图拆分为子图以获得全局最小误差。

与最先进的方法相比，SPA更快，收敛更好。唯一的例外是在初始化效果不好的地图中，只有TORO的随机梯度下降技术才能收敛；但是通过应用生成树初始化，SPA甚至可以比TORO更好地解决复杂的合成示例。当与扫描匹配前端相结合时，SPA将实现在线探索和地图构建。由于SPA是一种位姿图方法，因此它允许对地图进行增量添加和删除，从而有利于长期建图。