0. 简介

最近随着越来越多的团队开始注重将SLAM应用在机器人和无人驾驶上，最近SLAM的顶刊顶会也开始想着多模态和低成本这两个方向开始发力。而本文讲的这个《Wheel-SLAM: Simultaneous Localization and Terrain Mapping Using One Wheel-mounted IMU》就是讲了如何基于车轮安装低成本IMU的轮式机器人定位算法（Wheel-INS）并同时利用车轮IMU感知地形特征实现回环检测，将Wheel-INS拓展为SLAM方案（Wheel-SLAM）。目前相关的代码已经在Github上开源Wheel-INS和Wheel-SLAM，个人感觉这类结构比较简单的开源方案潜力还是比较大的，因为这类可以更好地嵌入到各个现有的模块中。

1. 文章贡献

移动机器人需要一种对环境干扰具有鲁棒性的可靠位姿估计器。为此，惯性测量单元（IMU）发挥了重要作用，因为它们可以独立感知车辆的全运动状态。然而，由于固有的噪声和偏置不稳定性，它会出现累积误差，尤其是对于低成本传感器。在我们之前对 Wheel-INS的研究中，我们提出通过在机器人的轮子上安装 IMU 以利用旋转调制来限制纯惯性导航系统 (INS) 的误差漂移。但是，由于缺乏外部校正信号，它仍然在很长一段时间内漂移。在这篇文章中，我们建议利用 Wheel-INS 的环境感知能力来实现仅使用一个 IMU 的同时定位和建图 (SLAM)。具体来说，我们使用路堤角度（由 Wheel-INS 估计的机器人滚动角度反映）作为地形特征，以使用 Rao-Blackwellized 粒子滤波器实现闭环。根据粒子维护的网格图中的机器人位置，对路堤角度进行采样和存储。根据当前估计的滚动序列与地形图之间的差异更新粒子的权重。本文的贡献如下：

1. 提出并实现了一种只有一个 Wheel-IMU 使用地形特征（由 Wheel-IMU 测量）的 SLAM 系统。
2. 我们通过广泛的现场实验说明了利用机器人滚动角估计来实现闭环以有效限制Wheel-INS中的误差漂移的可行性。
3. 据我们所知，这是文献中第一个只为轮式机器人使用一个低成本轮式 IMU 的 SLAM 系统。

2. 相关工作

这部分的内容是基于轮速计的，所以我们需要带领大家先了解一下相关工作，从而才可以更加深入的了解作者的思想。

2.1 基于地形匹配的定位

基于地形的车辆定位利用道路信息提供了一种可用的替代GNSS来获取绝对定位结果的方法。地形特征的重复、位置相关性使其可用于机器人的定位和建图[10]。通常，地形特征是通过车内惯性传感器提取的，例如，IMU可以通过车身俯仰角度测量道路坡度，通过车身横摆角度测量道路倾斜角度，通过车辆偏航角速率测量道路曲率[11]。基本假设是惯性传感器信号意味着车辆对地形表面的响应，同样的地形表面会引起类似的车辆运动[12]。现有文献主要采用车辆俯仰角度和俯仰角度差作为基于地形的定位特征[13]，[14]，虽然横滚角度也可以发挥同样的作用[15]，[16]。然而，使用车载惯性信号来确定地形信息会受到车辆操作的影响，例如，制动可能会导致车辆俯仰变化的意外变化[10]，向心加速度可能会引入车辆横滚角度与道路倾斜角度之间的差异[17]。

已经研究了多种方法将地形特征匹配结果集成到定位流程中[10]，[12]，而PF方法在最近的研究中受到了更多关注。在PF方法中，通过评估车内测量值（例如，横滚、俯仰等）与地图响应之间的差异来更新粒子的权重[10]，[15]，[18]。Martini等人[19]使用Pearson积矩相关系数作为距离度量来比较道路坡度测量值与参考地图。然而，在所有这些方法中都需要预先构建地图，这限制了其应用。此外，用于测量地形信息的惯性传感器都安装在车身上。本文提出了一种在没有预先地图的情况下同时定位机器人并测量道路倾斜角度的方法，扩展了我们之前的研究[1]，[2]。由于IMU安装在机器人车轮上，因此地形匹配不会受到车辆操作的影响，这与IMU安装在车身上的情况不同。

2.2 仅使用惯性传感器的SLAM

最近，基于深度学习的惯性定位系统在行人[20]，[21]和车辆导航[22]方面展示了有希望的结果。这些方法从原始惯性测量数据[20]，[21]或动态测量噪声[22]中学习运动信息以数据驱动的方式解决惯性里程计问题，但未能利用环境信号来限制长期位置漂移。
Angermann等人[23]提出了一种仅使用脚踏式IMU（FootSLAM）的行人SLAM系统，利用了人类感知和认知的优势。采用动态贝叶斯网络来表示当行人在受限制的环境中行走时（例如办公楼），他或她主要依赖视觉线索来避开障碍物并确定可访问区域的事实。具体而言，该算法基于PF实现，其中粒子的权重通过行人穿越相邻六边形的规则2D网格的概率来更新。之后，构建了一个概率转移图，隐含地编码了影响行人视觉印象和意图的环境特征。与FootSLAM不同，Wheel-SLAM使用Wheel-IMU来明确提取地形特征以进行回路闭合检测。
总之，Wheel-SLAM借鉴了地形匹配型车辆定位和FootSLAM的思想。与Wheel-INS [1]相比，我们通过使用Wheel-IMU提取道路特征来扩展该方法，从而实现回路闭合，进一步限制误差漂移。在WheelSLAM中，我们实时维护和更新网格地图，并使用Wheel-INS的DR结果检测回路闭合。之后，我们使用当前估计值和地图之间的roll角序列匹配结果来更新粒子的权重，以保证鲁棒性。

3. 方法背景（回顾前面内容，并作为核心）

Wheel-INS [1]是Wheel-SLAM的基础。它被用来提供机器人的里程计和滚动角估计。Wheel-INS有两个主要优点。首先，轮速可以通过陀螺仪输出和轮半径计算，从而实现与ODOM/INS相同的信息融合，仅需一个IMU（无需其他传感器）。
其次，它可以利用旋转调制来限制INS的误差漂移。由于篇幅限制，在这里只概述了Wheel-INS的算法。详细信息可以参考早期的论文[1]，[2]，例如，Wheel-IMU的旋转调制，误差对齐的定义等。
图2展示了Wheel-IMU的安装以及相关坐标系的定义。图3展示了Wheel-INS的系统概述。首先，进行前向INS机械化以预测机器人状态。同时，使用Wheel-IMU x轴上的陀螺仪输出计算轮速。然后，将该车速视为带有非完整约束(NHC)的外部观测值，通过扩展卡尔曼滤波器(EKF) [25]更新状态。由Wheel-IMU陀螺仪数据和轮半径计算得出的前向轮速可写成：

图2. Wheel-IMU的安装方案和车辆坐标系（v-frame）、车轮坐标系（w-frame）和IMU机体坐标系（b-frame）的定义。 [1]

图3. Wheel-INS的概述[1]。其中，ω和f分别是Wheel-IMU测量到的角速率和特定力；PVA代表Wheel-IMU的位置、速度和姿态。我们使用Wheel-IMU的输出来执行INS机械化以预测机器人状态（PVA）。在Wheel-IMU的x轴上测量到的角速度和轮半径被用于计算前向速度。将这个速度作为带有非完整约束的3D速度观测量与INS机械化结果一起，通过EKF来更新机器人状态以及通过纠正惯性传感器误差，例如陀螺仪偏差。

由车轮- imu陀螺仪数据和车轮半径计算出的前轮速度为\

上式中，\tilde{v}^v_{wheel}和v^v_{wheel}分别表示观测到的和真实的车辆前向速度；\tilde{ω}_x是Wheel-IMU x轴陀螺仪输出；ω_x是Wheel-IMU x轴真实角速度；δω_x是陀螺仪测量误差；r是车轮半径；e_v是轮速观测噪声，建模为高斯白噪声。
由于Wheel-IMU随车轮旋转，所以在Wheel-INS中无法确定车辆的俯仰角。换句话说，我们无法确定车辆的上升和下降。因此，我们假设车辆在水平平面上移动。然而，实验结果[1]表明，这个假设不会导致显著的导航误差。

4. 动态贝叶斯网络（略看）

PF是一种顺序蒙特卡洛方法，其基本思想是使用重要性采样和离散随机测度逼近概率分布的概念，递归地计算相关概率分布[26]。在PF中，机器人状态的后验分布由一组粒子表示，这些粒子随着新信息的整合而递归地演化。基于Rao-Blackwellization技术[27]–[29]，WheelSLAM将SLAM问题分解为机器人定位问题和地形映射问题，后者是基于机器人姿态估计的。

在Wheel-SLAM中，我们尝试估计后验分布。

这是一个条件概率分布，表示基于控制输入u_{1:t}和道路倾斜角度观测z_{1:t}的机器人状态x_{1:t}和地形图M的后验分布。WheelSLAM问题的条件独立性质意味着(2)中的后验分布可以分解如下：

其中m_i是第i个地形特征，N_f是特征的总数。在Wheel-SLAM中，进行机器人状态估计和道路俯仰角感知时使用Wheel-INS。Wheel-SLAM使用PF来估计机器人轨迹分布。对于每个粒子，单独的轨迹基础的地形图是相互独立的。因此，每个粒子由机器人姿态和地形图组成；因此，在时间t时，第i个粒子可以表示为：

在此，i=1,2,3，…，N_p是粒子的索引，而N_p是所使用的粒子的总数。x^t_i是第i个粒子在时间t估计的机器人姿态，而M_i是第i个粒子维护的地形图。在这里，机器人姿态由2D平移p^t_i∈\mathbb{R}^2和朝向R^t_i∈SO(2)表示，因为Wheel-INS无法估计俯仰角，即机器人在垂直方向上的运动[1]。
Wheel-SLAM算法包括四个主要步骤：1）通过运动模型对新的机器人状态进行采样；2）更新地形地图；3）一旦有确信的循环闭合，更新粒子权重；4）必要时重新对粒子进行重采样。算法1张红展示了Wheel-SLAM的大致算法。

第一步是从机器人概率运动模型中采样，为每个粒子生成一个新的姿态:

这意味着机器人的位姿x_t是机器人控制量u_t和上一个位姿x_{t−1}的概率函数。在这里，我们采用Wheel-INS来预测机器人状态。

5. 网格地形图构建（略看）

与FootSLAM中使用的六边形网格地图相比，由于相对简单的机器人运动模式，我们将网格简化为正方形。由于我们假设车辆在水平平面上移动，因此我们构建一个2D网格地图。每个网格都包含WheelINS在该位置估计的相应道路倾角。图4说明了沿着机器人姿势演变构建的网格地图。

图4. 网格地图构建和回访识别的示意图。不同颜色的曲线表示由不同粒子采样的机器人路径。灰色网格已被机器人访问，因此具有路面倾角估计值。一旦某个粒子检测到机器人连续返回到已访问的网格（蓝色），则会报告潜在的回环检测以供进一步检查（详见第III-D节）。

6. 粒子重量更新（略看）

在开始时，所有的粒子都被赋予相同的权重。当机器人移动时，每个粒子都有不同的轨迹和地形地图。为了确保回环检测的可靠性并减少异常值的影响，我们设置了三个标准。首先，回环需要在长度为N_r的窗口内由机器人位置连续检测到。其次，我们使用Pearson相关系数[19]计算N_r个滚动序列匹配分数，并将它们与阈值Cthr进行比较。在这个N_r窗口中，至少需要有N_{thr}（N_{thr}<=Nr）个系数大于C_{thr}。第三，当前位置的相关系数需要大于阈值。如果满足所有三个要求，我们认为它是一个真正的回环，并随后按以下方式更新粒子权重：

该公式中，k表示时间步；i表示粒子编号；N_c(N_c≥N_{thr})表示在长度为Nr的窗口中大于阈值的相关系数的数量；RMSE表示均方根误差；V_{coeff}是窗口中大于阈值的相关系数的集合。之后进行归一化以使权重之和等于1。需要注意的是，虽然Wheel-SLAM需要精确的回访，但是车辆不需要总是在同一条路上行驶。当机器人探索未知环境时，位置误差会积累，但一旦机器人回到以前访问过的路上，就可以纠正。请参考第IV节的实验结果和讨论。此外，由于我们的算法需要一系列的道路坡度角进行匹配，因此它无法有效地在十字路口检测到环路闭合，因为机器人可能会垂直于其上次进入的方向进入该地点。