0. 简介

在本文中，我们关注移动机器人定位的可靠性问题。蒙特卡罗定位（MCL）广泛用于移动机器人的定位。然而，由于缺乏判定MCL估计可靠性的方法，其安全性仍难以保证。本文提出了一种新型定位框架，能够同时实现鲁棒的定位、可靠性估计和快速重新定位。该方法的实现方式与MCL的估计方式相似。通过在定位过程中估计已知和未知障碍，该方法可以增强对环境变化的定位鲁棒性；然而，不可预见的错误仍可能导致定位失败。该方法还包括一个可靠性估计功能，使机器人能够知道定位是否失败。此外，该方法可以无缝集成全局定位方法，通过重要性采样实现。因此，可以在减少全局定位噪声影响的同时，实现从失败状态的快速重新定位。我们使用装备有2D激光雷达的轮式移动机器人进行了三种类型的实验。结果显示，能够实现可靠的MCL，包括鲁棒定位、自我故障检测和快速故障恢复。相关的代码可以在Github中找到

1. 主要贡献

本文的贡献在于实现了上述方法的集成。正如前文所述，所提出的方法基本上是我们之前提案的综合。然而，由于缺乏模型和实现难度，我们之前未能实现这种集成。在这项工作中，我们提出了一种新的图形模型，用于姿态跟踪过程，并利用自由空间特征解决了全局定位问题。因此，可以实现同时达到上述三个目标的可靠定位。我们使用装备有2D激光雷达的轮式移动机器人进行了基于模拟、数据集和我们自己平台的实验。通过模拟实验，我们展示了所提方法在数值上比传统方法表现得更好。通过数据集和我们自己平台的实验，我们证明了该方法也适用于实际机器人。本文的贡献总结如下：

1. 提出了一个新框架，集成了我们之前在[Akai et al.(2018a)Akai, Morales, and Murase, Akai et al.(2018b)Akai, Morales, and Murase, Akai et al.(2020)Akai, Hirayama, and Murase]中提出的方案，并实现了同时进行鲁棒定位、可靠性评估和快速重定位的可靠定位
2. 发布了基于ROS的新框架实现，该实现使用2D激光雷达，并作为开源软件公开

2. 问题设定

在本研究中，我们专注于移动机器人的二维定位问题。目标机器人的姿态由二维位置（x和y）和朝向角度θ组成。我们假设机器人配备了二维激光雷达（LiDAR）和惯性导航系统（INS）。INS的测量数据用u表示，用于预测机器人姿态。LiDAR的测量数据用z表示，用于与表示地图的m进行匹配。在本工作中，我们的目标是利用u和z的序列以及m来估计当前机器人的姿态。此外，我们试图同时实现鲁棒的定位、可靠性估计和快速重新定位。鲁棒定位和可靠性估计通过改进的姿态跟踪算法实现，该算法将在下一小节中描述。快速重新定位通过全局定位与姿态跟踪算法的概率融合实现。

图1：姿态跟踪的图形模型

3. 姿态跟踪的图形模型

3.1 变量定义

图1展示了姿态跟踪过程的图形模型。白色和灰色节点分别表示隐藏变量和可观测变量。机器人姿态x、测量类别c和定位状态s被视为隐藏变量。INS和激光雷达测量值u和z、地图m以及定位状态分类器的输出d则被视为可观测变量。

激光雷达的测量值表示为z = (z^{[1]}, z^{[2]}, …, z^{[K]})，其中z^{[k]}代表第k次测量。测量类别c指示激光雷达测量值的类别，表示为c = (c^{[1]}, c^{[2]}, …, c^{[K]})，其中c^{[k]}对应于z^{[k]}。在这项工作中，我们使用两个类别，表示为c^{[k]} ∈ C = {known, unknown}。这些类别表明测量值是从地图上已存在的障碍物获得的还是未知的。

定位状态分类器用于区分定位是否失败。分类器的实现方法可以是基于阈值和机器学习的方法[Almqvist et al.(2018)Almqvist, Magnusson, Kucner, 和 Lilienthal]。分类器的输出形式会根据其实现方式而变化。在这项工作中，我们使用基于阈值的分类器，该分类器将阈值设定为使用残差误差定义的平均绝对误差（MAE），其中d是一个一维连续值，残差误差代表激光雷达测量点与每个点最近映射障碍物之间的距离集。定位状态s表示为s ∈ S = {success, failure}。这些状态指示定位是否成功。状态s是使用分类器d的输出估计的。由于s = success表示定位已成功，因此可以通过计算其概率，即p(s = success)，来了解可靠性。

3.2 公式化

我们的目标是估计当前时刻t机器人姿态、测量类别和定位状态的联合后验分布，如方程(1)所示。

在公式（1）中，1 : t表示时间序列，即 u_{1:t} = (u_1, u_2, …, u_t)。公式（1）可以通过乘法定理进行分解。

其中，公式（2）、（3）和（4）分别表示机器人姿态、测量类别和定位状态的后验概率。
公式（2）、（3）和（4）无法直接计算。我们分别对它们进行公式化以便于计算。
首先，将贝叶斯定理应用于公式（2）。

为了从第2行转换到第3行，分母被重写为一个归一化常数η，并且应用了D分离 [Bishop(2006)] 到关于z_t 和 d_t 的分布上，以移除非相关的条件变量。然后，将全概率公式应用于每个分布。

为了从第2行转换到第3行，对于 x_t 的分布应用了D分离，同时从 x_{t−1} 的分布中省略了 u_t，因为未来的输入不会影响先前的姿态。因此，得到了关于姿态分布的递归更新方程。
公式（3）可以通过应用贝叶斯定理和D分离来构建。

为了构建公式（4），首先应用贝叶斯定理和D分离。

然后，将全概率定律应用于第二个分布

为了从第2行转换到第3行，对于 s_t 的分布应用了D分离，并且从 s_{t−1} 的分布中省略了 x_t, c_t, u_t, 和 z_t，因为未来的条件不会影响先前的状态。关于定位状态的递归更新方程也因此得到。
最终，目标分布如公式（5）所示。

在公式（5）中，我们有四个重要的模型，分别表示为 p(z_t|x_t, c_t, m)，p(d_t|x_t, s_t, z_t, m)，p(x_t|x_{t−1}, u_t) 和 p(s_t|s_{t−1}, u_t)。这些模型分别对应于类条件测量模型、决策模型、运动模型和可靠性转换模型。这些模型可以参考原文的详细介绍。

在这项工作中，我们使用拉奥-布莱克威尔化粒子滤波器（RBPF）来估计公式（5）。更准确地说，使用粒子滤波器（PF）估计公式（2），而基于PF中采样的姿态，使用分析方法估计公式（3）和（4）。4.

4. 全局定位及其与姿态跟踪的融合

在这项工作中，我们假设全局定位问题是在给定多个INS和LiDAR测量以及地图的条件下，估计机器人姿态的概率分布。因此，我们还试图估计公式（6）所示的分布。

在公式（6）中，N 是用于全局定位的之前步骤的数量。需要注意的是，如果 N = 0，则忽略INS测量 u。

如公式（2）和（6）所示，在所提出的方法中有两个关于机器人姿态的概率分布。理想情况下，只有当定位失败时才需要进行全局定位。然而，由于对LiDAR测量采用独立假设，精确检测定位失败是具有挑战性的 [Thrun et al.(2005)Thrun, Burgard, and Fox, Akai et al.(2019)Akai, Morales, Hirayama, 和 Murase, Akai et al.(2022)Akai, Akagi, Hirayama, Morikawa, 和 Murase]。尽管所提出的方法估计了定位结果的可靠性，但使用估计的可靠性来完美地分类定位是否失败仍然是困难的。换句话说，比起根据估计条件切换它们，无缝融合姿态跟踪和全局定位更为理想。

为了实现无缝融合，我们使用重要性采样。首先，从公式（6）中采样姿态。设 GM 为采样姿态的数量，^Gx^{[i]}_t 为第 i 个采样姿态，我们假设采样姿态近似于公式（6）的概率分布，就像由PF进行的近似一样。

在公式（7）中，δ(·) 是狄拉克δ函数，当括号内的值为0时值为1，否则为0。通过公式（7）所示的近似，我们可以将公式（6）中显示的分布作为提议分布，即用于采样粒子的分布。

在粒子滤波器（PF）中，粒子的似然度由目标分布和提议分布的商来决定。在本方法中使用的目标分布如公式（2）所示。因此，从公式（6）采样的粒子的似然度表示为

其中，( R ) 是预测分布的积分，预测分布指的是\int p(x_t|x_{t-1}, u_t)p(x_{t-1}|u_{1:t-1}, z_{1:t-1}, d_{1:t-1}, m)dx_{t−1}。

在公式（5）中所示的机器人姿态分布中，预测分布用于采样粒子，并使用类条件测量和决策模型计算它们的似然，因为预测分布是建议分布。然而，在从公式（6）中采样的粒子的似然计算中，使用了预测分布。因此，可以实现姿态跟踪和全局定位的无缝融合。

从公式（6）中进行采样的方法有多种。在这项工作中，我们使用了基于自由空间特征的定位方法，该方法在[Millane等人（2019）Millane, Oleynikova, Nieto, Siegwart, 和 Cadena]中提出。

5. 实施

为了实现第4节中描述的功能，本项工作中实现的系统框图如图2所示。主要模块是姿态跟踪器和全局定位器。这两个模块都接收移动速度、扫描和占据网格地图作为输入。全局定位器根据其估计输出采样姿态。姿态跟踪器接收采样姿态，并最终输出估计的姿态。此外，姿态跟踪器还输出定位结果的可靠性和测量类别。

图2：定位系统图

该系统由以下过程组成：

1. 使用给定的初始姿态进行初始化
2. 使用运动模型更新粒子
3. 使用可靠性转换模型更新可靠性
4. 执行全局定位并采样粒子
5. 使用类条件测量和决策模型计算运动模型更新粒子的似然
6. 使用类条件测量和决策模型以及预测分布计算从全局定位采样的粒子的似然
7. 估计机器人姿态、传感器测量类别和可靠性
8. 重新采样粒子

6. 结论

本文提出了一种可靠的蒙特卡罗定位（MCL）方法，它可以（1）在动态环境中稳健地工作，（2）通过估计可靠性立即检测定位失败，并且（3）在估计失败时快速重新定位。为了实现这些功能，我们提出了一种新颖的图形模型，并为同时进行定位、传感器测量类别估计和可靠性估计问题制定了贝叶斯滤波。我们使用了拉奥-布莱克威尔化粒子滤波器来实现这一同时估计系统。

我们进行了三种类型的实验，即模拟实验、数据集实验和我们自己平台的实验。通过模拟实验，我们数值上展示了提出的方法可以实现上述三点。此外，我们还展示了提出的方法优于传统方法。在其余两个实验中，我们将提出的方法与ROS中流行的定位包amcl进行了比较。比较结果显示，提出的方法比amcl实现了更快的重新定位。此外，我们确认提出的方法可以准确地执行姿态跟踪和可靠性估计。

对于未来的工作，我们将尝试提出一种新的图形模型，该模型能够将更准确的定位正确性分类器集成到可靠性估计模型中，以实现实用的可靠性估计器。