59、多传感器移动机器人在不同环境中的定位策略

多传感器移动机器人在不同环境中的定位策略

在移动机器人的定位问题上，传统的分层方法是融合多个传感器独立得出的状态估计。而这里采用的方法是从一组定位算法中选择一个，每个算法独立处理不同的传感器数据。这种选择基于对各个算法在移动机器人部署过程中可能遇到的各种室内场景下的性能评估结果。

多传感器定位

使用三种传感器进行定位，分别是激光测距仪、深度相机和WiFi接收信号强度指示器（RSSI）。每个传感器与机器人里程计独立处理。
- 激光测距仪 ：使用基于纠正梯度细化（CGR）的粒子滤波器。
- 深度相机 ：通过快速采样平面滤波（FSPF）算法处理观测值以检测平面，然后将平面与地图中的墙壁匹配，再使用CGR粒子滤波器定位机器人。
- WiFi ：使用高斯过程学习的WiFi图进行定位，结合了基于图的WiFi定位和基于高斯过程的RSSI定位的优点。

纠正梯度细化（CGR）

对于使用激光测距仪传感器的定位，机器人位置的置信度表示为一组加权样本或“粒子”。CGR算法通过以下步骤迭代更新过去的置信度Bel(xt - 1)：
1. 通过运动模型p(xt|xt - 1, ut - 1)演化Bel(xt - 1)的样本，生成第一阶段提议分布q0。
2. 使用观测模型p(yt|x)的梯度δ/δxp(yt|x)在r次迭代中“细化”q0的样本，产生中间分布qi（i ∈[1, r - 1]）。
3. 使用接受测试对最后一代提议分布qr和第一阶段提议分布q0的样本进行采样，生成最终提议分布q。
4. 用相应的重要性权重wi t对最终提议分布q的样本xi t进行加权，并进行有放回重采样以生成Bel(xt)。

基于深度相机的定位

深度相机为每个像素提供颜色和深度值，结合相机内参可重建3D点云。但由于移动机器人计算资源有限，直接处理完整的3D点云不现实。FSPF算法结合了子采样和特征提取的思想，它在深度图像中随机采样邻域，并在每个邻域内对3D点进行基于RANSAC的平面拟合。
FSPF算法的流程如下：

1: procedure PlaneFiltering(I)
2:     P ← {}  ▷ 平面过滤点
3:     R ← {}  ▷ 平面对应的法线
4:     O ← {}  ▷ 离群点
5:     n ← 0   ▷ 平面过滤点的数量
6:     k ← 0   ▷ 采样邻域的数量
7:     while n < nmax ∧ k < kmax do
8:         k ← k + 1
9:         d0 ← (rand(0, h - 1), rand(0, w - 1))
10:        d1 ← d0 + (rand(-η, η), rand(-η, η))
11:        d2 ← d0 + (rand(-η, η), rand(-η, η))
12:        从d0, d1, d2重建p0, p1, p2
13:        r = (p1 - p0) × (p2 - p0) / ||(p1 - p0) × (p2 - p0)||  ▷ 计算平面法线
14:        ¯z = (p0z + p1z + p2z) / 3
15:        w′ = w * S / (¯z * tan(fh))
16:        h′ = h * S / (¯z * tan(fv))
17:        [numInliers, ˆP, ˆR] ← RANSAC(d0, w′, h′, l, ϵ)
18:        if numInliers > αinl then
19:            将ˆP添加到P
20:            将ˆR添加到R
21:            numPoints ← numPoints + numInliers
22:        else
23:            将ˆP添加到O
24:        end if
25:    end while
26:    return P, R, O
27: end procedure

FSPF的配置参数如下表所示：
| 参数 | 值 | 描述 |
| ---- | ---- | ---- |
| nmax | 2000 | 过滤点的最大总数 |
| kmax | 20000 | 采样邻域的最大数量 |
| l | 80 | 局部样本的数量 |
| η | 60 | 全局样本的邻域（像素） |
| S | 0.5m | 局部样本的世界空间平面大小 |
| ϵ | 0.02m | 内点的最大平面偏移误差 |
| αin | 0.8 | 接受局部样本的最小内点分数 |

基于WiFi的定位

基于图的WiFi定位在受限图中能提供快速准确的机器人定位，但构建WiFi图需要在图的每个顶点位置准确放置机器人以收集训练样本。基于高斯过程的RSSI定位不需要特定位置的训练观测值，但计算复杂度随训练样本数量二次增长。这里的方法结合了两者的优点，先在地图上收集训练样本，然后使用高斯过程离线学习WiFi图节点的均值和方差，运行时通过双三次插值估计位置的WiFi均值和方差。

比较定位算法

为了评估不同定位算法在各种环境中的鲁棒性，在移动机器人自主导航时收集激光扫描仪、深度相机和WiFi传感器数据。导航轨迹多次覆盖每个走廊和开放区域。离线处理收集的数据100次，并评估每个传感器的平均定位误差。
- 走廊环境 ：激光测距仪定位算法最准确，平均误差为几厘米；深度相机定位误差约为十厘米；WiFi定位误差约为一米。
- 开放区域 ：在尺寸超过激光测距仪和深度图像传感器最大范围的开放区域进行额外试验。结果显示，激光测距仪在成功跟踪的轨迹中最小误差最小，但最大误差也最大；WiFi定位算法在每次试验中都能持续跟踪机器人轨迹，中位误差小于一米。不同算法的误差统计如下表：
| 算法 | 最小误差 | 最大误差 | 中位误差 |
| ---- | ---- | ---- | ---- |
| 激光测距仪 | 0.01 | 14.77 | 1.83 |
| 深度相机（Kinect） | 0.17 | 3.47 | 1.08 |
| WiFi | 0.20 | 1.88 | 0.76 |

传感器选择策略

配备多个传感器并运行不同定位算法的机器人需要决定何时依赖每个算法。有三种选择传感器和算法的方法：
- 基于计算不确定性的选择 ：每个定位算法都有机器人姿势估计的不确定性度量，但不同算法的不确定性度量由于各自的假设和简化无法直接比较。
- 基于传感不匹配的选择 ：通过计算观测似然函数P(l|s)和置信度Bel(l)之间的不匹配来评估不确定性。这种方法适用于使用相似传感器的算法，但不同类型传感器的算法由于观测似然函数形式不同无法直接比较。
- 数据驱动的选择 ：如果前两种方法不可行，数据驱动的选择可能是唯一的办法。该方法需要在环境的所有区域对每个定位算法进行实验评估，部署时机器人在每个位置选择实验评估中方差最小的算法。但这种方法需要在所有区域进行实验评估，对于大面积部署可能不可行。

综上所述，没有单一传感器在定位方面具有普遍优势，但通过为地图的不同区域选择不同的传感器，可以限制机器人的定位误差。未来的研究方向是学习如何预先预测哪些传感器在地图的不同区域定位最稳健。

多传感器移动机器人在不同环境中的定位策略

选择传感器进行定位的详细分析

基于计算不确定性的选择

在基于计算不确定性的选择方法中，不同的定位算法有着不同的不确定性度量方式。例如，基于粒子滤波器的定位算法，其不确定性由粒子的分布来体现；而基于扩展卡尔曼滤波器（EKF）的定位算法，通过协方差矩阵来捕捉机器人姿态的不确定性。

虽然从理论上来说，机器人可以选择在任何给定时间具有最小不确定性的算法（也就是对应的传感器）。但是，由于不同算法的假设和简化不同，导致无法直接比较它们的不确定性度量。例如，EKF算法假设机器人姿态的分布是单峰高斯分布，而粒子滤波器是基于样本的多峰分布，这两者之间无法直接进行比较。

基于传感不匹配的选择

传感不匹配的选择方法是一种更通用的计算定位算法不确定性的方式。它通过计算观测似然函数 (P(l|s)) 和置信度 (Bel(l)) 之间的不匹配来评估。

像传感器重置定位（SRL）通过直接从观测似然函数 (P(l|s)) 采样，并根据不匹配程度将新生成的样本添加到粒子滤波器中。KLD重采样则通过从里程计模型 (P(l_i|l_{i - 1}, u_i)) 采样并计算与置信度 (Bel(l)) 的重叠来评估不匹配。

有了每个定位算法与其各自置信度分布之间的传感器不匹配估计后，机器人可以选择观测似然函数和置信度之间重叠最好的算法。不过，这种方法适用于使用相似传感器的算法，如深度相机和激光测距仪。对于使用不同类型传感器的算法，如WiFi信号强度测量与测距仪测量，由于不同传感器的观测似然函数形式差异很大，无法直接基于传感器不匹配进行比较。

数据驱动的选择

当基于不确定性和传感不匹配的选择方法都不可行时，数据驱动的选择可能是唯一的解决方案。该方法需要对环境中所有区域的每个定位算法进行实验评估，就像前面比较定位算法时所做的那样。

在部署过程中，机器人在每个位置选择在实验评估中表现出最小方差的算法。这种方法的优点是适用于任何定位算法，并且对单个定位算法没有算法上的限制。然而，它的缺点也很明显，需要在环境的所有区域对所有定位算法进行实验评估，对于大面积的部署来说，这可能是不可行的。

下面通过一个mermaid流程图来总结选择传感器进行定位的三种方法：

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px

    A([开始]):::startend --> B{是否可计算不确定性并比较?}:::decision
    B -->|是| C(基于计算不确定性的选择):::process
    B -->|否| D{是否可计算传感不匹配并比较?}:::decision
    D -->|是| E(基于传感不匹配的选择):::process
    D -->|否| F(数据驱动的选择):::process
    C --> G([结束]):::startend
    E --> G
    F --> G

结论与未来展望

通过对三种不同传感器（激光测距仪、深度相机和WiFi）在整个地图上的定位测试，可以得出以下结论：在激光测距仪和深度相机（Kinect）能够看到特征的区域，这两种传感器的定位比WiFi更准确。然而，在开放区域，激光测距仪和深度相机的定位容易出现较大误差，而WiFi定位的最大误差是有界的，并且其中位误差低于激光测距仪和深度相机。

这表明没有单一的传感器在定位方面具有普遍的优越性。但是，通过为地图的不同区域选择不同的传感器，可以有效地限制机器人的定位误差。

未来的研究方向具有重要的意义。研究人员希望能够学习如何预先预测哪些传感器在地图的不同区域定位最稳健。这将有助于机器人在实际应用中更加智能地选择合适的传感器，提高定位的准确性和可靠性，进一步推动移动机器人在不同环境中的应用和发展。

例如，在一些复杂的室内环境中，如大型商场、仓库等，机器人可以根据预先学习的结果，在不同的区域自动切换合适的传感器进行定位，从而更高效地完成任务。同时，这也为多传感器融合技术的发展提供了新的思路和方向。

总之，多传感器移动机器人的定位问题是一个复杂而具有挑战性的领域，通过不断的研究和探索，有望实现更加智能、准确和可靠的定位系统。