Wheel-SLAM：低成本轮式IMU的SLAM系统

摘要

移动机器人需要一种对环境干扰具有鲁棒性的可靠位姿估计器，为此，惯性测量单元（IMU）发挥了重要作用，因为它们可以独立感知车辆的全运动状态。然而，由于固有的噪声和偏置不稳定性，它会出现累积误差，尤其是对于低成本传感器。在我们之前对 Wheel-INS的研究中，本文提出通过在机器人的轮子上安装 IMU 以利用旋转调制来限制纯惯性导航系统 (INS) 的漂移误差。但是，由于缺乏外部校正信号，它仍然在很长一段时间内漂移。

在这篇文章中利用 Wheel-INS 的环境感知能力来实现仅使用一个 IMU 的同时定位和建图 (SLAM)。具体来说，我们使用路堤角度作为地形特征，以使用 Rao-Blackwellized 粒子滤波器实现闭环，根据粒子维护的网格图中的机器人位置，对路堤角度进行采样和存储，根据当前估计的滚动序列与地形图之间的差异更新粒子的权重。实验表明，使用机器人滚动角估计在 WheelINS 中执行 SLAM 的想法是可行的。此外，定位精度比 Wheel-INS 显着提高（超过 30%）

图1. 我们之前的工作[1]中的车辆实验中的车辆横滚估计和测试轨迹。在标记的区域内，车辆一直往返盘旋。可以观察到相应的机器人横滚角估计（表示道路倾斜角）显示出一个可以利用的重复模式，可以用来进行环路闭合检测和修正。

主要贡献

提出并实现了一种Wheel-IMU 使用地形特征（由 Wheel-IMU 测量）的 SLAM 系统。

通过广泛的现场实验说明了利用机器人滚动角估计来实现闭环以有效限制Wheel-INS中的误差漂移的可行性。

据我们所知，这是文献中第一个只为轮式机器人使用一个低成本轮式 IMU 的 SLAM 系统。

相关工作

背景

Wheel-INS [1] 是 Wheel-SLAM 的基础，用于提供机器人里程计和横滚角估计，Wheel-INS 有两个主要优点。首先，轮速可以通过陀螺仪输出和轮半径计算，实现与只使用一个 IMU（无其他传感器）的 ODO/INS 相同的信息融合。其次，它可以利用旋转调制来限制 INS 的误差漂移。由于空间限制，我们在这里仅概述 Wheel-INS 的算法。图2描述了 Wheel-IMU 的安装和相关坐标系的定义。



图2. Wheel-IMU的安装方案以及车辆框架(v-frame)、车轮框架(w-frame)和IMU本体框架(b-frame)的定义

Wheel-INS 的系统概述如图3所示，首先，执行前向 INS 机械化来预测机器人状态，同时，使用 Wheel-IMU x 轴的陀螺仪输出计算轮速，然后，将此车辆速度视为带有非完整约束（NHC）的外部观测，并通过误差状态扩展卡尔曼滤波器（EKF）来更新状态。



图3. Wheel-INS的概述，ω和f分别代表Wheel-IMU测量的角速度和特定力；PVA表示Wheel-IMU的位置、速度和姿态。我们使用Wheel-IMU的输出来进行INS运算以预测机器人状态（PVA）。在Wheel-IMU的x轴上测量的角速度和轮半径被用来计算前进速度。这个速度被作为一个带有非完整约束的三维速度观测值，通过EKF来更新机器人状态以及校正惯性传感器的误差，例如陀螺仪偏差。

动态贝叶斯网络

粒子滤波是一种序列蒙特卡罗方法，其基本思想是使用重要性抽样和离散随机测量近似概率分布的概念，递归计算相关概率分布。在粒子滤波中，机器人状态的后验分布由一组粒子表示，这些粒子随着新信息的融合而递归地演化。基于Rao-Blackwellization技术，Wheel-SLAM将SLAM问题分解为机器人定位问题和地形映射问题，其取决于机器人位姿估计。Wheel-SLAM算法包括四个主要步骤： 1）通过运动模型对机器人状态进行采样； 2）更新地形图； 3）一旦确定了闭环，则更新粒子权重； 4）在必要时对粒子进行重采样。 算法1概述了Wheel-SLAM的流程。



网格地形图的构建

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



图4. 网格地图的构建和回访识别的示意图，不同颜色的曲线表示由不同粒子采样的机器人路径，灰色网格已经被机器人访问过，因此它们具有道路侧倾角度估计，一旦一个粒子检测到机器人已经连续返回访问过的网格（蓝色），则会报告一个潜在的回环，并进行进一步的检查。

粒子权重更新

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

实验描述 

为了证明所提出的Wheel-SLAM系统的可行性和有效性，我们在武汉大学校园内使用一辆汽车进行了五组现场测试。该汽车装有一个Wheel-IMU和参考系统，以提供车辆姿态的真实值，如图5所示。测试中车辆运动的特性如表I所示。



两个MEMS IMU和高端IMU的主要技术参数列在表II中，其中ARW表示角随机游走；Acc.表示加速度计；VRW表示速度随机游走。



图6展示了五个实验轨迹。汽车开始移动之前的静态IMU数据用于获得Wheel-IMU的初始横滚和俯仰角以及陀螺仪偏差的初始值，其他惯性传感器误差设为零。



图6. 实验轨迹。Seq. 1、Seq. 2 和 Seq. 3 是环形轨迹，车辆在一个方向上移动了多次，而 Seq. 4 和 Seq. 5 是更为复杂的轨迹，在大规模环境中车辆不仅沿着同一车道同一方向行驶，还沿着同一条道路相反的方向行驶。

实验中设置的Wheel-SLAM的关键参数列在表III中，标准差在表III中表示为STD。



性能比较与分析

1）性能比较：图7分别比较了五个实验中Wheel-SLAM和Wheel-INS的定位误差和航向误差。



图7. 在所有五个实验中，Wheel-INS和Wheel-SLAM的估计轨迹及相应的水平位置误差和航向误差的比较。

图8展示了Wheel-SLAM在Seq. 1，Seq. 2和Seq. 4中构建的地形图，由于车辆轮胎直接接触地面，所以制图不受车辆悬架系统的影响，特别是当车辆机动性大时，这种影响尤为明显，这也是在车体上安装IMU时的情况。此外，这些地图可以用于提供有价值的信息，以监测道路的变形和劣化情况。



图8. Wheel-SLAM在Seq. 1、Seq. 2和Seq. 4中估计的地形地图。颜色表示道路倾角的值。道路倾角越大，颜色越浅。

2）Wheel-SLAM特性分析：为了进一步评估Wheel-SLAM的性能和稳定性，我们设置不同的粒子数来比较其定位性能。该算法每个配置运行100次。图9显示了结果。



图9. 在Seq. 1中，使用不同粒子数的Wheel-SLAM的定位RMSE。

讨论

Wheel-SLAM的核心原则可以总结如下：

1）粒子扩散以对机器人的可能状态进行采样，并通过每个粒子维护的轨迹检测环路闭合；

2）道路坡度角序列匹配结果用于更新粒子权重，以挑选出最可信的粒子。

在Wheel-SLAM中发挥核心作用的是滚动序列匹配策略。它必须足够稳健，以保留突出的粒子同时过滤掉虚警。因此，我们采用了相当严格的环路闭合检测标准，使环路闭合检测具有鲁棒性。

然而，可以意识到，在应用Wheel-SLAM时存在两个主要限制。首先，机器人必须严格重新访问以前的地方，并具有一定的长度，这不像基于视觉的SLAM，其中车辆通过使用外部感知传感器（例如相机和LiDAR）具有远程感知能力，在Wheel-SLAM中，Wheel-IMU用于提取地形特征，这些特征只能通过机器人的精确到达来获取，其次，环路闭合的成功取决于道路坡度角序列的匹配，如果机器人在极其平滑的道路上行驶，没有任何坡度角的波动，那么很难检测到环路闭合。

总结

在本研究中，我们提出了一种利用单个轮式惯性测量单元（Wheel-IMU）执行同时定位与地图构建（SLAM）的方法，通过利用Wheel-IMU的环境感知能力，具体而言，我们通过从机器人的横滚角估计中提取地形特征来启用环路闭合检测，从而将我们之前的Wheel-INS方法扩展为Wheel-SLAM。系统采用了Rao-Blackwellized粒子滤波器，每个粒子维护其自己的机器人状态和栅格地图。实验结果表明，所提出的方法能够有效抑制Wheel-INS的误差漂移，相对于Wheel-INS，定位和方向精度平均提高了52.6％和53.2％。

然而，Wheel-SLAM有两个主要限制，首先，需要一定程度的道路沿角变化。其次，机器人必须准确回到同一地点，Wheel-SLAM适用于那些在给定区域内重复移动的机器人，例如清扫机器人和在受限区域内巡逻的机器人， 未来研究方向包括将Wheel-SLAM与其他外部感知传感器（例如相机和LiDAR）集成，以提高机器人导航系统的稳健性和适用性。

审核编辑：刘清