76、异构传感器融合与置信丰富的3D网格映射在物理机器人中的应用

异构传感器融合与置信丰富的3D网格映射在物理机器人中的应用

1. 引言

智能物理系统的自主导航在很大程度上依赖于系统生成其环境精确地图的能力。生物系统通过整合来自多个来源（如视觉、触觉等）的信息，来创建一个连贯而丰富的环境表征，从而能够在其中可靠地行动。同样，机器人系统也依赖于通过各种机载传感仪器生成的周围环境地图，以在充满障碍物的环境中实现稳健的自主导航。

例如，在洞穴等地下环境中运行的机器人，需要考虑海拔、光照、视觉遮挡等各种物理约束。由于现实世界环境不断变化，使用具有多种传感器模式的机器人系统至关重要，这样可以根据操作要求利用每个传感器系统的内在优势。

2. 多模态映射与异构传感器融合

• 多模态映射 ：在移动机器人领域，占用网格是建模障碍物最常见的框架。与先进的基于网格的映射方法相比，置信丰富的网格映射（CRM）不仅能提供地图的准确表示，还能估计每个体素中地图误差与报告置信值之间的一致性。在融合传感器数据的机器人系统中，这种地图表示形式特别有用，因为它能更准确地捕捉不完美传感器和模型引入的噪声。
• 异构传感器与传感器融合 ：不同类型的传感器具有各自的优缺点。深度相机基于立体匹配或结构光可提供丰富而密集的场景信息，但使用范围通常有限，因为测量误差在较长距离时会显著增加。扫描激光虽然在较大范围内非常可靠和准确，但仅能在水平平面提供距离信息，因此需要收集大量数据才能获得区域的详细地形信息。因此，利用多种互补的传感器可以提高测量精度、鲁棒性以及感知管道的空间和时间覆盖范围。

3. 相关工作

过去，已经使用了多种范围传感器来构建基于网格的地图，包括声纳、深度相机和扫描激光等。在移动机器人的占用网格中，传感器融合的研究也有很多。例如，有人将改进的声纳和立体视觉算法结合，以构建整合两种传感器数据的地图；还有人开发了基于因子图的优化框架，用于高效准确地整合不同采样率的导航传感器；也有提出使用深度学习架构进行传感器融合的方法。

4. 问题陈述

本文的目标是使用安装在具有多模态能力的车辆上的一组异构传感器的测量数据，生成未知环境的置信感知表示。具体而言，采用CRM作为环境的空间表示，以融合来自多个机器人和传感器的传感器数据。本文的贡献包括实时实现分布式CRM，以及测试和分析CRM作为一种用于使用不同测距传感器的异构车辆的一致、准确且方便的映射算法。

5. 技术方法

为了获得更准确和置信感知的地图，CRM放宽了传统映射算法的一些错误假设。
- 置信丰富的表示 ：占用地图 $m = [m_1, \cdots, m_n]$ 被定义为在2D或3D网格的 $n$ 个体素上的一组值。在第 $k$ 个时间步，传感器测量和传感器配置分别由 $z_k$ 和 $x_k$ 给出。地图 $m$ 上的占用概率分布（信念） $b_m^k = p(m|z_{0:k}, x_{0:k})$ 通过贝叶斯框架进行建模。地图信念可以通过线性项 $\alpha_i$ 和常数项 $\beta_i$ 对每个体素 $i$ 进行迭代更新：
[p(m_i|z_{0:k}, x_{0:k}) = (\alpha_i m_i + \beta_i)p(m_i|z_{0:k - 1}, x_{0:k - 1})]
在C++实现中，每个体素 $i$ 的地图信念 $b_{m_i}$ 被实现为一个粒子数组（即32位浮点数），由 $\alpha_i$ 和 $\beta_i$ 系数进行更新。经过实验确定，粒子数量为50是在映射精度、计算和存储效率之间的一个足够的权衡。
- 传感器因果模型 ：与传统的占用网格算法不同，CRM通过传感器因果模型（SCM）维护体素之间的相互依赖关系。SCM是一个概率模型，用于推理体素 $c_k$ 在传感器射线 $x_k$ 下是测量 $z_k$ 的原因，它与传感器前向模型 $p(z_k|c_k, x_k)$ 以及测量射线从体素 $c_k$ 反弹并穿过 $c_k$ 与传感器之间所有体素的概率成正比：
[p(c_k|z_{0:k}, x_{0:k}) = p(c_k|b_m^{k - 1}, z_k, x_k) = \eta’ p(z_k|c_k, x_k) \prod_{j = 1}^{c_l^{k - 1}} (1 - \hat{m} {g(j, x)}^{k - 1})]
其中 $c_l$ 表示体素 $c$ 沿射线 $x$ 的局部索引，即 $c_l = g^{-1}(c, x)$，$\eta’$ 是归一化常数。
- CRM中的传感器融合 ：传感器因果模型允许CRM通过相同的更新算法嵌入各种传感器的前向模型 $p(z|c, x)$，同时考虑传感器的噪声特性。例如，对于具有焦距 $f$ 和基线 $d_b$ 的立体相机，可以得到前向模型 $p(z|c, x) = N(||G_c - x {cam}||^{-1} f d_b, V)$，其中 $G_c$ 是从相机记录的两幅图像之间的视差匹配估计的体素网格上的3D点，$x_{cam}$ 是相机的位置，$V$ 是测量视差上的高斯噪声的方差。前向模型提供了自己的不确定性测量，并嵌入到SCM中，使得地图更新步骤不需要为各种传感器进行手动设计，即它是传感器无关的，与传统的网格映射算法不同。来自异构传感器的测量根据其精度对地图更新做出贡献，因此传感器融合自然地遵循CRM所基于的贝叶斯框架。

6. 实验设置

所有实验均在加州理工学院的自主系统与技术中心（CAST）进行。该中心有一个20×12×13米的飞行场地，配备了由48台高清摄像机组成的OptiTrack运动捕捉系统。在实验中，手动控制机器人，并使用OptiTrack进行姿态估计。

实验中使用了两种机器人移动系统，并且在其上运行了基于C++和ROS集成的CRM：
- Rollocopter ：由NASA喷气推进实验室（JPL）开发的六自由度自主混合机器人，适用于空中和地面模式。它配备了NVIDIA Jetson TX2微型计算机和两个Hokuyo URG - 04LX - UG01 LiDAR，可在240°的视野内以0.36°的角分辨率和10Hz的频率测量距离。每个Hokuyo传感器刚性安装在车轮上，随着车辆移动而旋转，使平面深度传感器能够覆盖更多的3D空间。该平台结合了在平坦地形上高效滚动和在更具挑战性场景中飞行的能力。
- Intel Aero Drone ：由Intel Aero计算板和Intel Atom x7 - Z8750 CPU供电的四轴飞行器。它配备了一个以20Hz记录深度图像的Intel RealSense R200深度感应相机，此外还安装了一个朝前的Hokuyo URG - 04 - UG01 LiDAR。

实验流程如下：

graph LR
    A[实验准备] --> B[实验1：单传感器性能测试]
    B --> C[实验2：双传感器融合测试]
    C --> D[实验3：多机器人多传感器融合测试]

7. 实验内容

• 实验1 ：在Rollocopter上使用两个LiDAR，比较CRM与对数几率映射的性能。在地板上放置盒子作为障碍物进行地图注册，这是首次在该新型机器人平台上实现任何映射算法。
• 实验2 ：控制Intel Aero无人机使用RealSense相机和Hokuyo LiDAR进行测量。两个传感器在实验中朝前对齐，以比较CRM在利用两种测量源组合方面与对数几率映射的效果。深度测量由映射算法实时处理，RealSense和LiDAR的处理频率分别为20Hz和10Hz。
• 实验3 ：将Rollocopter和Intel Aero无人机组合，使它们相互交换地图更新并融合各自传感器套件的测量数据。手动控制机器人并行移动，以覆盖障碍物课程。Intel无人机上的RealSense相机朝下安装，以获取与下方Rollocopter测量数据有大量重叠的地板深度图像，从而使映射算法能够融合两个机器人的更新，以获得更准确的可通行空间模型。使用Velodyne VLP - 16 3D LiDAR捕获环境的地面真值。

在这个实验中，还实现了一种允许在不同传感代理之间交换地图更新的程序：通过广播每个体素 $i$ 的 $\alpha_i$ 和 $\beta_i$ 系数，可以有效地传达CRM地图的变化，而无需发送每个体素的50个粒子值。每进行10次深度测量，每个机器人上运行的CRM实例就以约2Hz的频率以ROS消息的形式广播这些地图更新。接收的CRM实例在收到更新消息后，会根据贝叶斯更新公式进行应用。

通过这些实验，可以全面评估CRM在不同场景下的性能，以及其在异构传感器融合和多机器人协作中的应用潜力。

异构传感器融合与置信丰富的3D网格映射在物理机器人中的应用

8. 实验结果分析

为了更直观地展示实验结果，我们将从几个关键指标对不同实验进行分析。以下是实验的主要评估指标：
|评估指标|描述|
| ---- | ---- |
|地图准确性|地图与实际环境的匹配程度，通过与地面真值对比评估|
|置信度一致性|地图中报告的置信值与实际误差的相符程度|
|计算效率|算法处理传感器数据和更新地图所需的时间|

实验1结果

在Rollocopter上使用两个LiDAR进行实验1，对比CRM与对数几率映射的性能。通过对生成的地图与Velodyne VLP - 16 3D LiDAR捕获的地面真值进行对比，发现CRM在地图准确性上表现更优。CRM能够更准确地识别障碍物的位置和形状，而对数几率映射在一些边缘区域存在一定的模糊性。在置信度一致性方面，CRM能够更准确地反映地图误差，而对数几率映射的置信度估计相对较为粗糙。在计算效率上，由于CRM需要维护每个体素的置信度信息，计算量相对较大，但仍能满足实时性要求。

实验2结果

实验2中，控制Intel Aero无人机使用RealSense相机和Hokuyo LiDAR进行测量。CRM在融合两种传感器数据方面表现出色，能够充分利用深度相机的丰富细节和LiDAR的高精度距离信息。与对数几率映射相比，CRM生成的地图在准确性和置信度一致性上都有显著提升。在计算效率方面，由于传感器数据量增加，两种算法的计算时间都有所增加，但CRM仍然能够保持较好的实时性能。

实验3结果

实验3将Rollocopter和Intel Aero无人机组合，进行多机器人多传感器融合实验。通过交换地图更新和融合测量数据，CRM能够生成更全面、准确的环境地图。在与地面真值对比时，发现融合后的地图在障碍物检测和可通行空间建模方面都有很大的改进。在置信度一致性方面，CRM能够有效地整合不同传感器的不确定性，提供更可靠的置信度估计。在计算效率方面，由于涉及多机器人之间的通信和数据融合，计算复杂度进一步增加，但通过优化通信协议和算法实现，仍然能够满足实时性要求。

9. 技术优势总结

CRM作为一种新兴的映射算法，在异构传感器融合和物理机器人应用中具有以下显著优势：
- 准确的地图表示 ：CRM不仅能够提供地图的准确表示，还能估计每个体素中地图误差与报告置信值之间的一致性，从而更准确地捕捉不完美传感器和模型引入的噪声。
- 传感器无关性 ：通过传感器因果模型，CRM能够嵌入各种传感器的前向模型，使得地图更新步骤不需要为各种传感器进行手动设计，具有良好的通用性和扩展性。
- 自然的传感器融合 ：基于贝叶斯框架，CRM能够根据传感器的准确性自动融合来自异构传感器的测量数据，实现更高效、准确的地图更新。
- 实时性支持 ：尽管CRM需要处理更多的信息，但通过优化算法实现和合理的计算资源分配，仍然能够满足实时性要求，适用于实际的机器人应用场景。

10. 应用前景展望

随着机器人技术的不断发展，异构传感器融合和置信丰富的3D网格映射将在更多领域得到广泛应用。以下是一些可能的应用场景：
- 智能物流 ：在仓库和物流中心，机器人需要准确地感知周围环境，规划最优路径，以实现高效的货物搬运和存储。CRM能够融合多种传感器数据，提供更准确的环境地图，帮助机器人更好地完成任务。
- 灾难救援 ：在灾难现场，如地震、火灾等，环境复杂多变，救援机器人需要快速、准确地了解灾区情况，寻找幸存者。CRM的置信度信息能够帮助机器人评估地图的可靠性，提高救援效率和安全性。
- 太空探索 ：在太空探索任务中，机器人需要在未知的环境中进行自主导航和探测。CRM的传感器无关性和自然的传感器融合能力，能够适应不同类型的传感器，为太空机器人提供更可靠的环境感知。

11. 总结与结论

本文介绍了一种基于置信丰富的网格映射（CRM）的异构传感器融合方法，并通过一系列实验验证了其在物理机器人中的有效性和优势。通过放宽传统映射算法的错误假设，CRM能够提供更准确、置信感知的地图，同时实现自然的传感器融合。实验结果表明，CRM在地图准确性、置信度一致性和计算效率方面都表现出色，适用于多种机器人应用场景。未来，随着传感器技术和算法的不断发展，CRM有望在更多领域发挥重要作用，推动机器人技术的进一步发展。

整个实验过程的流程图如下：

graph LR
    A[实验准备] --> B[实验1：单传感器性能测试]
    B --> C[实验2：双传感器融合测试]
    C --> D[实验3：多机器人多传感器融合测试]
    D --> E[结果分析]
    E --> F[技术优势总结]
    F --> G[应用前景展望]
    G --> H[总结与结论]

总之，异构传感器融合与置信丰富的3D网格映射为物理机器人的环境感知和导航提供了一种强大的解决方案，具有广阔的应用前景和发展潜力。