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

A Flexible and Scalable SLAM System with Full 3D Motion Estimation

摘要

对于城市搜索救援（USAR）场景中的许多应用，机器人需要学习未知环境的地图。本文提出了一种需要较少计算资源的快速在线学习精确栅格地图的系统，该系统结合了一种基于激光雷达系统的鲁棒扫描匹配方法和一种基于惯性传感器的三维姿态估计系统。通过快速逼近地图梯度和多分辨率栅格，实现了在各种具有挑战性的环境中的可靠的定位和建图能力，并提供了多个数据集，展示了嵌入式手持式建图系统的适用性。我们证明了该系统是足够精确的，在所考虑的场景中不需要显式的闭环矫正技术。该软件是ROS的一个开源软件包。

引言

学习环境模型和进行自我定位的能力是能够在真实环境中操作的真正自主机器人最重要的能力之一。本文提出了一种灵活的、可扩展的系统来解决SLAM问题，该系统已成功应用于无人地面车辆（UGV）、无人表面车辆（USV）和小型室内导航系统。这种方法消耗的计算资源少，因此可以在低重量、低功耗和低成本的处理器上使用，例如在小型自治系统上常用的处理器。我们的方法使用ROS元操作系统作为中间件，并作为开源软件提供。它适配ROS导航功能栈的API，因此可以很容易地与其他ROS生态系统中的SLAM方法进行交互。

本文中介绍的系统的目标是在保持较低计算量的同时，实现足够精确的环境感知和自我定位。它可以用于小规模场景中的SLAM，在小规模场景中，不必进行大的闭环矫正，并且现代激光雷达系统的高更新率是对系统是有益的。这样的场景包括机器人世界杯救援比赛，在比赛中机器人必须在模拟地震场景中寻找灾民，这种场景具有地形崎岖的特点，因此需要对无人车辆进行完整的6自由度运动估计，或者使用比地面机器人移动速度更快的无人飞行器进行室内导航。

我们的方法结合了一个在平面地图上的基于激光扫描（LIDAR）集成的2D SLAM系统和一个基于惯性测量单元（IMU）的集成的3D导航系统，该系统将来自SLAM子系统的2D信息作为一个可能的辅助信息源。SLAM通常是由激光扫描设备的更新触发的软实时运行系统，而完整的三维导航解决方案是硬实时计算的，通常构成了车辆控制系统的一部分。

系统的架构图如下

系统概述

由于所提出的系统必须能够在具有完整6自由度运动的平台上使用，而不是许多其他基于二维栅格的SLAM算法中假设的3自由度运动平台，因此我们的系统必须估计全整的6自由度状态，包括平台的旋转和平移。为此，系统由两个主要部分组成。导航滤波器将来自惯性测量装置和其他可用传感器的信息进行融合，形成一致的3D解决方案，而2D SLAM系统用于提供地平面内的位置和航向信息。这两个估计值都是单独更新的，并且只是松耦合的，因此它们会随着时间的推移保持同步。

我们将导航坐标系定义为一个右手坐标系，其原点位于平台的起点，z轴向上，x轴指向平台在启动时的偏航方向。完整的三维状态表示为$x=({\Omega }^T,p^T,v^T{)}^T$，其中$\Omega =(\varphi ,\theta ,\psi )$为代表横滚、俯仰和偏航的欧拉角，$p=(p_x,p_y,p_z)$和$v=v_x,v_y,v_z)$为导航坐标系中表示的平台的位置和速度。

惯性测量单元构成输入矢量$u=({\omega }^T,a^T)$与机体坐标系下的的角速度$\omega =({\omega }_x,{\omega }_y,{\omega }_z)$和加速度$a=(a_x,a_y,a_z)$。用非线性微分方程组描述任意刚体的运动如下
$$\dot{\Omega }=E_{\Omega }\cdot \omega \text{\hspace{1em}(1)}$$
$$\dot{p}=v\text{\hspace{1em}(2)}$$
$$\dot{v}=R_{\Omega }\cdot a+g\text{\hspace{1em}(3)}$$
其中$R_{\Omega }$是方向余弦矩阵，它将机体坐标系中的向量映射到导航坐标系。$E_{\Omega }$机体坐标系下的角速率映射为欧拉角的导数，并且重力是一个常数向量。当使用低成本传感器时，由于地球自转而产生的伪力的影响通常可以忽略。

由于传感器噪声的影响，积分出来的速度和位置会出现明显漂移。因此，需要进一步整合传感器信息。在本文中，这些信息是由扫描匹配器提供的，非常适合在室内或场景中使用。其他可能的来源是用于航向信息的磁场传感器或用于高度估计的气压传感器。如果可用，轮式里程计可用于提供速度测量值。在室外场景中，通常采用卫星导航系统作为辅助系统来防止惯性导航解算的漂移。

根据目标平台的不同，可以在系统方程中引入进一步的约束，从而减少状态空间的维度。

2D SLAM

为了能够表示任意环境，使用了占据栅格地图，这是在真实环境中使用激光雷达进行移动机器人定位的一种行之有效的方法。利用激光雷达系统的姿态估计，将扫描转换到一个局部稳定的坐标系。使用估计的平台方向和关节运动值，将扫描转换为由扫描端点构成的点云。取决于使用场景，可以对点云进行预处理，例如进行降采样或去除异常值。在本文提到的方法中，只使用基于端点z轴坐标的滤波，这样在扫描匹配过程中只使用目标扫描平面阈值内的端点。

A.地图查询

占用栅格地图的离散性限制了它可以达到的精度，也不允许直接计算插值或导数。为此，采用了一种插值方法，通过双线性滤波实现子栅格单元的精度，用于估计占用概率和导数。直观地说，占据栅格地图的单元格值可以看作是一个潜在的连续概率分布的采样。

给定一个连续的地图坐标$P_m$，占据概率$M(P_m)$以及梯度$▽M(P_m)=(\frac{\partial M}{\partial x}(P_m),\frac{\partial M}{\partial y}(P_m))$可通过使用下图所示的四个最接近的整数坐标$P_{00..11}$来近似

然后沿x轴和y轴进行线性插值
$$\begin{gathered} M(P_m)\approx =\frac{y-y_0}{y_1-y_0}(\frac{x-x_0}{x_1-x_0}M(P_{11})+\frac{x_1-x}{x_1-x_0}M(P_{01})) \\ +\frac{y_1-y}{y_1-y_0}(\frac{x-x_0}{x_1-x_0}M(P_{10})+\frac{x_1-x}{x_1-x_0}M(P_{00}))\text{\hspace{1em}(4)} \end{gathered}$$
导数可近似为：
$$\frac{\partial M}{\partial x}(P_m)\approx \frac{y-y_0}{y_1-y_0}(M(P_{11})-M(P_{01}))+\frac{y_1-y}{y_1-y_0}(M(P_{10})-M(P_{00})))\text{\hspace{1em}(5)}$$

$$\frac{\partial M}{\partial y}(P_m)\approx \frac{x-x_0}{x_1-x_0}(M(P_{11})-M(P_{10}))+\frac{x_1-x}{x_1-x_0}(M(P_{01})=M(P_{00}))\text{\hspace{1em}(6)}$$

应注意的是，地图的采样点/栅格单元位于彼此之间距离为1（在地图坐标中）的规则网格上，这简化了梯度近似的呈现方程。

B.扫描匹配

扫描匹配是将激光扫描彼此对齐或与现有地图对齐的过程。现代激光扫描仪具有低的距离测量噪声和高的扫描速率。基于这个原因，用于配准扫描的方法可能会产生非常准确的结果。对于许多机器人系统来说，激光扫描仪的准确度和精度比里程计数据要高得多（如果有的话）。

我们的方法基于优化激光束端点与迄今所学地图的对齐。使用高斯-牛顿方法的基本原理是受计算机视觉的启发。使用这种方法，不需要在激光束端点之间进行数据关联搜索或穷举位姿搜索。当扫描与现有地图对齐时，匹配将隐式地执行。

我们的目的是求刚性变换$\zeta =(p_x,p_y,\psi {)}^T$来最小化
$${\zeta }^{\ast }=\underset{\zeta }{\mathrm{argmin}}\sum _{i=1}^n[1-M(S_i(\zeta )){]}^2\text{\hspace{1em}(7)}$$
也就是说，我们要找到使激光扫描与地图进行最佳对齐的变换。这里，$S_i(\zeta )$是扫描激光束端点$s_i=(s_{i,x},s_{i,y}{)}^T$的世界坐标。它们是$\zeta$的函数，机器人在世界坐标系中的位姿
$$S_i\zeta =\left(\begin{array}{cc}cos(\psi )& -sin(\psi )\\ sin(\psi )& cos(\psi )\end{array}\right)\left(\begin{array}{c}{s}_{i,x}\\ s_{i,y}\end{array}\right)+\left(\begin{array}{c}{p}_x\\ p_y\end{array}\right)\text{\hspace{1em}(8)}$$
函数$M(S_i(\zeta ))$返回给出的坐标处$S_i(\zeta )$的地图值。给定$\zeta$的一些初始估计，我们要估计$\Delta \zeta$，来优化误差测量方程
$$\sum _{i=1}^n[1-M(S_i(\zeta +\Delta \zeta )){]}^2\to 0\text{\hspace{1em}(9)}$$
通过对$M(S_i(\zeta +\Delta \zeta ))$进行一阶泰勒展开，我们可以得到：
$$\sum _{i=1}^n[1-M(S_i(\zeta ))-▽M(S_i(\zeta ))\frac{\partial S_i(\zeta )}{\partial \zeta }\Delta \zeta {]}^2\to 0\text{\hspace{1em}(10)}$$
通过对$\Delta \zeta$求偏导数，使该方程最小化
$$\begin{gathered} 2\sum _{i=1}^n[▽M(S_i(\zeta ))\frac{\partial S_i(\zeta )}{\partial \zeta }{]}^T \\ [1-M(S_i(\zeta ))-▽M(S_i(\zeta ))\frac{\partial S_i(\zeta )}{\partial \zeta }\Delta \zeta ]=0\text{\hspace{1em}(11)} \end{gathered}$$
求解$\Delta \zeta$得出最小化问题的高斯-牛顿方程
$$\Delta \zeta =H^{-1}\sum _{i=1}^n[▽M(S_i(\zeta ))\frac{\partial S_i(\zeta )}{\partial \zeta }{]}^T[1-M(S_i(\zeta ))]\text{\hspace{1em}(12)}$$
其中
$$H=[▽M(S_i(\zeta ))\frac{\partial S_i(\zeta )}{\partial \zeta }{]}^T[▽M(S_i(\zeta ))\frac{\partial S_i(\zeta )}{\partial \zeta }]\text{\hspace{1em}(13)}$$

地图梯度的一种近似方法$▽M(S_i(\zeta ))$我们之前已经提过。利用方程（8）我们得到
$$\frac{\partial S_i(\zeta )}{\partial \zeta }=\left(\begin{array}{ccc}1& 0& -sin(\psi )s_{i,x}-cos(\psi )s_{i,y}\\ 0& 1& cos(\psi )s_{i,x}-sin(\psi )s_{i,y}\end{array}\right)\text{\hspace{1em}(14)}$$
使用$▽M(S_i(\zeta ))$和$\frac{\partial S_i(\zeta )}{\partial \zeta }$、 现在可以计算高斯-牛顿方程（12），得到一个单步的$\Delta \zeta$来接近最小值。值得注意的是，该算法工作于地图梯度的非光滑线性近似$▽M(S_i(\zeta ))$，这意味着不能保证局部二次收敛到最小值。然而，该算法在实际应用中具有足够的精度。

对于许多应用，匹配不确定性的高斯近似是可取的。例如，参数滤波器的更新以及用于图优化 SLAM 后端的位姿约束。一种方法是使用基于采样的协方差矩阵估计，对接近扫描匹配位姿的不同位置估计进行采样，并从中构造协方差。这与无迹卡尔曼滤波器的思想类似。第二种方法是使用近似Hessian矩阵得到协方差矩阵的估计。这里，协方差矩阵近似为
R = V a r { ζ } = σ 2 ⋅ H − 1 (15) R = Var\{\zeta\} = \sigma^2 \cdot H^{-1} \tag{15} R=Var{ζ}=σ2⋅H−1(15)
式中，$\sigma$是取决于激光扫描仪设备特性的比例因子。

C.多分辨率地图表示

任何基于爬山/梯度的方法都有陷入局部极小值的固有风险。由于所提出的方法是基于梯度上升的，它也有可能陷入局部极小。这一问题可以通过使用多分辨率地图表示来缓解，类似于计算机视觉中使用的图像金字塔方法。在我们的方法中，我们可以选择使用多个占用栅格地图，其中每个较粗的地图的分辨率为上一层级地图的一半。然而，多层地图并不是通过使用在图像处理中通常采用的高斯滤波和下采样的方法从单个高分辨率地图生成的。相反，不同的地图保存在内存中，并使用对齐过程生成的位姿估计值同时更新。这种生成方法确保了地图在不同尺度上的一致性，同时避免了代价高昂的下采样操作。扫描对齐过程从最粗糙的地图级别开始，得到的估计位姿用作下一级的初始估计。这样做产生的积极的副作用是低精度地图的即时可用性，例如可用于路径规划。

3D 状态估计

本节介绍了对完整三维状态向量的估计以及二维SLAM结果的集成。导航滤波器以100 hz的的频率实时运行，一旦驱动，就会用扫描匹配位姿和其他传感器信息进行异步更新。除扫描匹配位姿外，其他传感器的融合不在本文讨论的范围内。

A.导航滤波器

为了估计平台的6维位姿，我们使用扩展卡尔曼滤波器（EKF）和由方程（1）-（3）定义的通用平台模型。此外，由于陀螺仪和加速度计的零偏随时间变化并对结果产生显著影响，状态向量会被零偏所影响。注意，由于矩阵中的欧拉角项$E_{\Omega }$和$R_{\omega }$，系统方程是非线性的，因此必须使用非线性滤波器。惯性测量被认为是已知的系统输入。

速度和位置更新是测量加速度和角速度的纯积分，如果没有通过测量更新的额外反馈，系统将不稳定。一种防止状态估计在没有测量值的情况下无限增长的常用对策是在方差达到一定阈值时立即进行伪零速度更新，否则无法保证稳定性。在本文中，在平面上的二维位置和方向由扫描匹配更新，而对于完整的三维状态估计额外的高度传感器，如气压计或距离传感器是必要的。

B.SLAM 集成

为了获得最佳性能，必须在两个方向上交换二维SLAM结果和三维EKF估计之间的信息。系统是不同步的，EKF通常以更高的更新率运行。

为了提高扫描匹配过程的性能，将EKF的位姿估计投影到xy平面上，作为扫描匹配优化过程的起始估计。或者，可以对估计的速度和角速率进行积分，以提供扫描匹配的起始估计。

在相反的方向，协方差交（CI）用于融合2D SLAM的结果位姿和完整描述的状态。一个简单的卡尔曼测量更新会导致过度置信的估计结果，因为它假设了测量误差的统计独立。

我们将扫描时的Kalman估计表示为具有协方差的$P$的状态向量$\hat{x}$，而二维SLAM位姿$({\zeta }^{\ast },R)$ 如（7）和（15）所定义。融合结果由下式给出
$$(P^{+}{)}^{-1}=(1-\omega )\cdot P^{-1}+\omega \cdot C^T{R}^{-1}C\text{\hspace{1em}(16)}$$
$${\hat{x}}^{+}=P^{+}=((1-\omega )\cdot P^{-1}\hat{x}+\omega \cdot C^T{R}^{-1}{\zeta }^{\ast }{)}^{-1}\text{\hspace{1em}(17)}$$
观测矩阵$C$将完整状态空间投影到二维SLAM系统的三维子空间。参数$\omega \in (0,1)$用来调整SLAM系统更新的效果。

类似于卡尔曼滤波器和信息滤波器之间的二元论，协方差交也可以写成协方差形式
$$P^{+}=P-(1-\omega {)}^{-1}\cdot KCP\text{\hspace{1em}(18)}$$
$${\hat{x}}^{+}=\hat{x}+K({\zeta }^{\ast }-C\hat{x})\text{\hspace{1em}(19)}$$
其中
$$K=P{C}^T(\frac{1-\omega }{\omega }\cdot R+C^{T}PC{)}^{-1}\text{\hspace{1em}(20)}$$

由于根据（16）和（17）的完整状态协方差的逆在计算上是昂贵的，因此这是这里的首选方法。

结论

本文提出了一种适用于多种场景的灵活、可扩展的SLAM方法。我们演示了各种场景的适用性，如模拟城市的搜索和救援、USV上的海岸测绘以及手持式嵌入式建图系统。结果表明，该系统能够正确地估计和融合使用激光雷达的系统的三维运动，同时消耗较少的计算资源。