经典文献阅读之--Wheel-SLAM(低成本轮式机器人定位算法)

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

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

上式中，${\tilde{v}}_{wheel}^v$和$v_{wheel}^v$分别表示观测到的和真实的车辆前向速度；${\tilde{\omega }}_x$是Wheel-IMU $x$轴陀螺仪输出；${\omega }_x$是Wheel-IMU $x$轴真实角速度；$\delta {\omega }_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$个粒子可以表示为：

…详情请参照古月居