CMU-自主探索导航系统（TARE & FAR Planner）学习-All in one

参考引用

• Autonomous Exploration Development Environment
• TARE机器人自主导航系统社区-CSDN社区云
• TARE机器人自主导航系统公开课1
• TARE机器人自主导航系统公开课2
• CMU团队开发的全套开源自主导航算法
• FAR Planner —— IROS2022 最佳学生论文
• ＜论文阅读＞TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments
• 在完全陌生环境中，机器人如何实现自主规划与建图

论文和代码地址

• ICRA 2022：Autonomous Exploration Development Environment and the Planning Algorithms
  Autonomous Exploration Development Environment
• RSS 2021：TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments
  TARE
• IROS 2022：FAR planner: Fast, attemptable route planner using dynamic visibility update
  FAR planner

Part Ⅰ Exploration and Planning Algorithms

1. 引言

1.1 机器人探索（Exploration）背景

• 充满危险或者已经被污染等情况下的未知环境，不适合人类进入
• 复杂的、杂乱的 3D 环境

机器人导航和 SLAM 之间的因果困境问题

• 机器人可通过导航系统自行完成点到点的移动，但其实现高度依赖全局地图。SLAM 可以建立全局地图，但需通过手持、遥控、驾驶等方式，手工遍历整个环境。一旦撤掉操作人员，机器人的应用就会受限于导航与 SLAM 间这种 “鸡与蛋” 的因果困境
• 自主探索（Autonomous Exploration）系统就为解决这类问题而生。借助探索算法，机器人可以自主遍历未知环境，建立完整的地图，全程无需人工的介入。从表现上看，自主探索系统其实是把 SLAM 手工遍历环境的过程自动化了，因此也被归入主动式 SLAM（Active-SLAM）的范畴
• 一些能针对局部环境做出快速反应的规划器（如 FAR Planner）也能在未知环境中实现自主导航，但仍需手动给出目标点 Goalpoints

1.2 机器人探索技术应用示例

• 军用场景下：比较常见
• 民用/工业场景下：以福岛核事故为例
  • 环境背景：发电机进水，反应堆熔毁，氢气爆炸，放射性污染
  • 技术应用：放射性去污、农业恢复、邻里重建

1.3 探索问题和任务描述

• 问题描述

  • 机器人从离散/稀疏的视点（viewpoint）感知环境

    • viewpoint 可以看成是空间中的一个点，系统需要把自己送到一个 viewpoint 上，并在这个点上采集完周围环境的一帧数据后再到下一个 viewpoint 采集下一帧数据
    • 机器人探索问题可以建模为：有一个未知的环境，需要在这个环境中安放一系列的 viewpoint，然后让系统依次的走到这些 viewpoint 上并采集一帧传感器的数据，总和起来这些传感器的数据就构成了对这个未知环境的覆盖

  • 通过 viewpoint 求解一条路径来实现对环境的覆盖（coverage）

• 系统任务

  • 更新环境表述（environment representation）

    • 追踪周围环境中已经覆盖的和未覆盖的区域
    • 分析可通行的空间

      其中无人车相比无人机的探索更复杂，因为还包含对地面的地形可穿越性分析（Terrain Traversability Analysis，TTA），但同时无人机受到负载和电池续航的制约更大

    • 验证 viewpoint 并关联到环境表述

  • 评估 viewpoint 的环境覆盖/信息增益

    • 将系统放在某个 viewpoint 上能够看到环境的哪些部分或区域

  • 寻找一条最优的探索路径

1.4 机器人探索的难点

• 探索是在 “环境表述更新（已覆盖区域）” 和 “可通行路径寻找（覆盖更多区域）” 之间不断循环迭代的过程，因此需要连续不断的计算资源
  

• 在环境尺寸比较大、复杂、杂乱和 3D 地形情况下探索更加困难

1.5 现有探索算法的弊端

• 贪心策略

  • 系统在某个地方看一下环境当中哪个地方的分数最高，就在这个地方安放一个 viewpoint，同时把系统送到这个 viewpoint 上去并采集一帧数据；然后再看一下环境当中哪个地方的分数最高，继续在这个地方安放一个 viewpoint，再把系统送到这个 viewpoint 上去继续采集一帧数据，如此往复进行
  • 往往只规划一步或两三步（只优化短期内的目标），没有对环境的一个全局把握（长远的目标），这样会导致系统在环境中来回往复的走，容易在同一个地方重复行走，效率低下

• 数据结构不够高效

  • 系统需要花费大量时间去计算探索路径（探索算法是一个在线算法，系统需要不间断的拿到探索路径，只有拿到最新的探索路径系统才会执行下一步动作），因此计算探索路径所花费的时间长短决定了探索算法的效率
  • 已有的算法需要维护一个全局的地图。这样随着机器人探索范围的扩大，算法的计算速度会因为需要维护更大的地图而变慢，最终导致机器人需要停下来等待计算结束才能继续移动，这样的时走时停更加降低了探索效率

2. TARE（Technologies for Autonomous Robot Exploration）：用于探索复杂三维环境的分层框架

• 先计算出全局探索路径，然后用全局探索路径去引导局部探索路径的计算
• 这样在机器人探索的过程中，它行驶的路线会由于受到全局的引导而更具目的性，比如先探索完这个房间再探索下一个房间，而不是哪里的未知区域比较大就先去哪里

• 全局层面（Global Horizon）

  • 用低分辨率（稀疏）的环境表述，图中绿色方框/体是在全局环境中存放信息的地方（全局环境中没有覆盖完的地方，在未来的某一个时刻，系统需要依次到每一个绿色方框/体的位置上去把局部的环境覆盖完），根据这些绿色方框/体计算一条较为粗略的全局探索路径

• 局部层面（Local Horizon）

  • 用高分辨率（稠密）的环境表述，用相对多的计算量去仔细的算 viewpoint（图一黄色点，图二蓝色点），拿到这些 viewpoint 后再去计算一条精细的局部探索路径，并让系统沿着这条局部探索路径行驶，这样能把主要的计算资源集中在距离机器人较近的空间之内，而不至于浪费在远处不确定性更大的地方

2.1 局部层面规划

更新环境表述并计算出可通行的空间

• 实现方法
  • 采用随机采样 viewpoint 方法，然后求解一个旅行商问题（Traveling Salesman Problem，TSP），对应拿到一个旅行商巡行（Tour）路线（经过每一个 viewpoint，确定机器人探索目的地的先后顺序）；反复迭代的做这件事，最后保留一组最优的 viewpoint 和对应的旅行商巡行路线，这个最优的旅行商巡行路线就是最终要发送给系统执行的局部路径（local path）

    旅行商问题：给定一系列城市和每对城市之间的距离，求解访问每一座城市一次并回到起始城市的最短回路

需要调整传感器覆盖范围以考虑视场重叠问题

• 路径平滑
  • 通过 viewpoint 拟合一条或多条路径段，每个路径段都是动态可行的
  • 其中实心橙色点为采样 viewpoint，空心橙色点为断点（break-point）
  • 系统在经过采样 viewpoint 时不需要停下来而是匀速通过（连续平滑的），但是系统在每个断点处会停下来（连续但不平滑），然后原地调整方向后，继续前进到下一个路径段

2.2 全局层面规划

通过遍历空间构建稀疏路线图

• 实现方法
  • 当有了小的绿色方块/体后，同样求解一个旅行商问题（Traveling Salesman Problem，TSP），拿到一条旅行商巡行路线，这个就是最终的全局路径，然后发到局部层面上去引导局部路径的计算

    把信息存放在小的绿色方块/体中，代表全局环境中没有覆盖完的区域（多数在十字路口的位置）

2.3 实际案例

• 下图二涉及重定位（relocation）问题：系统把自己转移到另一个地方去，这个转移的过程就叫重定位（relocation）
  • 为什么要做重定位？
    • 当环境比较复杂比较绕的时候，经常会出现这样的情况：在系统附近的局部区域已经全部覆盖好了，此时系统就需要高效的把自己引导到另一个地方去
    • 下图二当系统到达左上角的死路（dead end）时，此时局部环境已经完全覆盖好，再将自己引导出来到达一个十字路口（intersection），然后选择一条支路继续完成覆盖

• 完成局部覆盖
  • 局部路径简化为到相邻块的最短路径
  • 隐式地在 “探索” 和 “重定位” 之间转换
  • 当局部都覆盖完之后问题就退化了，此时拿到的局部路径就是一条最短路径（下图二），然后引导自己到达下一个绿色方块/体，继续探索未覆盖的区域

2.4 算法实现方面的细节

• 广义曲面（Generalized Surface）
  • 定义为自由空间和非自由空间之间的边界
  • 包括物体表面和 “边界（frontiers，即下图中两条斜边）”
    • 传统的探索算法会在环境中提取 frontiers，然后系统会朝着 frontiers 的方向去探索

• 最大化表面覆盖（Surface Coverage）
  • 如果传感器在一定距离和角度范围内感知到一个表面被覆盖
    • 系统会使传感器最大化覆盖物体的表面（Surface Points）：在局部区域内通过循环采取视点（Viewpoint）的方式寻找能让传感器 “看全，看好” 的最短路径，下图蓝色曲线代表着这样一条轨迹。橙色圆圈表示采样得到的视点。红色曲线代表物体表面需要被传感器 “覆盖“ 的点

• 实际情况中，距离比角度的问题更重要

2.5 Takeaways（重要结论）

• 探索需要在 “局部覆盖” 和 “全局重定位以覆盖更多” 之间进行平衡
• 需要对环境进行高效的表述，以跟踪计算 覆盖/未覆盖区域 并搜索路径
• 局部数据密度很重要，可以权衡远离机器人的数据密度以换取计算效率
  • 在系统附近局部区域保持高分辨率的局部探索路径是很有必要的
  • 在全局范围内花费较少的计算得到一条粗略的全局探索路径
• 优化全局探索路径优于贪婪策略，因为后者存在进行冗余重访的问题

3. FAR Planner：采用动态可见性更新的快速、试探性路径规划算法

前面讲的探索问题是一个环境覆盖问题：把一个系统放到一个未知环境中，系统会自主把环境覆盖完。而 FAR Planner 是要把系统送到某一个目标点上去，这个目标点（Goal point）是我们人为给定的，系统通过某种途径算一系列路径（path）

3.1 常见路径规划算法

• 基于离散化搜索：A*、D*
• 基于采样：RRT

  以上两种方法在应付已知环境时性能还是不错的，但是在应付未知环境时需要一路摸索过去才能到达目标点，此时算法性能会大幅下降：因为需要长时间计算规划路径，导致系统在环境中来回走（冗余重访）

• 基于环境特征（Feature-Based）：如 Visibility Graph，其基于对环境特征的提取与几何关系分析生成路径图（Roadmap），然后在生成的 Roadmap 上进行规划，这种方式可生成理论最短路径

  • 相较与 RRT 的随机性而使得生成的路径图高度重叠（Redundence）与稠密（Dense），难以动态维护，基于环境特征所生成的 Visibility Graph 拥有更少的节点，可在环境变化发生时，或在未知环境中，随着机器人移动进行快速动态更新，也最大限度降低路径规划的计算复杂度
  • 然而基于环境特征的方式因为对环境输入的要求较高，导致很难被应用到实际中去限制了该方法的发展

3.2 FAR(Fast, Attemptable Route) Planner

• 定义与特点

  • 一种可在现实环境中实时提取环境（障碍物）几何特征并动态建立 Visibility Graph 进行路径规划及导航的算法，该算法支持在没有已知地图的环境下根据实时传感器输入对路径图以及导航路径进行实时动态调整，可在 1-2ms 实现对 300m 环境内的全局路径规划及动态调整

• 实现方法

  • 障碍物的多边形提取和配准

    • 首先，输入传感器点云数据
    • 然后，根据输入的点云创建二进制图像，并应用平均滤波方法生成模糊图像
    • 接着从这些具有稠密顶点的模糊图像中提取多边形轮廓（下图 a：黑色像素是可通行的，白色像素是障碍物，其他不同颜色表示不同的多边形）
    • 最后，将提取的多边形（下图 b 中红色）与传感器点云数据（下图 b 中白色）进行配准
      

  • 两层可视图动态更新

    • 对于未知环境，算法对每一帧传感器输入进行处理：提取障碍物多边形特征，并通过检测角点之间的相互可视性建立一个局部 V-Graph 用于全局 V-Graph 更新
    • 这样，将分散建立大范围全局 V-Graph 的计算量，并实现未知环境下的动态更新，使得规划器可在未知环境下探索式路径规划
      
      

  • 在 V-Graph 上规划

    • 动态障碍物检测与路径更新：通过对传感器多帧输入的比较，检测环境中的动态障碍物（如：行人，车辆），算法会对受到动态障碍物遮挡的 V-Graph 进行更新，并在动态障碍物移开后恢复之前 V-Graph 内相对应角点之间的连接
    • 在未知环境中的尝试性（Attemptable）路径规划：当导航/目标点（Goal Point）处在当前没有先验地图（Prior Map）的未知环境中时，算法会根据当前已知信息规划出多个可行路径进行尝试，并根据之后的最新环境信息进行更新

  • 扩展到三维多层 V-Graph

    • 适配于无人机规划方法的扩展 3D 版本将环境建模为多个水平切片并提取多层多边形。如下图所示：可见性边（青色）跨越多层多边形（红色）。进一步，在车辆（坐标系）和目标（红点）之间的 3D 多层可视图法上搜索一条路径（蓝色）
      

• 应用场景

  • 已知环境下的快速路径规划以及导航

    • 在已知环境中快速建立 V-Graph并进行路径规划，如：在已知环境信息的工厂，配送中心的自主机器人路径规划及导航，同时系统可根据实时输入的环境以及出现的动态障碍物对于已知地图进行更新，并实时更新导航路径。用户可根据不同任务需求，在此规划算法基础上设置更高层的任务规划算法，生成结合任务的时序导航点，实现并处理不同场景的任务需求，并快速部署到现实机器人平台

  • 未知环境中的定向探索及导航

    • 在某些搜救任务中，需要对未知环境中的某一特定区域进行搜索，该算法可根据当前已知环境障碍物信息规划出多个可行性路径进行尝试，并在探索前往目标位置过程不断更新优化路径，以探索区域建立全局路径地图，为后续机器人导航提供环境信息，并且可直接为后续的其他机器人或人员提够到达目标区域的导航路径

  • 机器学习以及仿真训练

    • 该算法可生成基于环境特征的导航路径，可用于生成大量导航及动态环境数据用于网络训练。结合 Matterport3D 模型及 AI Habitat Engine 渲染引擎，提供多种传感器输出，例如，RGB 图、深度图、语义图以及 LiDAR 点云, 可以为例如，基于激光雷达或者视觉感知的状态估计，局部避障，全局路径规划，探索等Sim-to-Real网络训练提供学习数据源。例如基于，训练基于网络的视觉导航及避障策略（Visual Planning Policy），训练端到端的定位及状态估计网络（End-to-End Localization network）

Part Ⅱ System, Applications, and Extensions

1. 引言

1.1 仿真环境

• CARLA Simulator 2017
  

• AirSim Simulator 2017
  

1.2 真实数据集（如 KITTI）和仿真环境对比

• 仿真环境优势
  • 可以轻松获取所有地面真值（ground truth），例如：车辆姿态、语义信息等
  • 方便模拟环境变化，如照明、天气、动态障碍物、真实事故等
  • 基于闭环控制回路的自治栈（全套系统）测试（数据集往往只能测试系统的单个模块等）

1.3 自治栈（Autonomy Stack）

• 通用的导航算法堆栈
  • 允许用户在闭环控制回路中开发高级算法进行仿真和测试
  • 允许用户轻松地将自治堆栈移植到车载计算机
  • 支持机器人和计算机视觉的联合研究
  • 采用地面平台进行扩展应用

2. 机器人智能系统平台

2.1 机器人智能系统平台架构

• Top-Layer：上层规划器
  • TARE 探索算法和 FAR Planner 路径规划器
• Mid-Layer：开发环境 & 实用工具
  • 开发环境（Development Environment）
    • 仿真环境：室内环境、校园、停车场、隧道与森林
    • 自主导航算法：Collision Avoidance、Terrain Traversability Analysis、waypoint Following
  • 实用工具：Visualization Tools 和 Joystick-based Debugging

  • 与上层算法的通用接口：waypoint，上层只需发送 waypoint 给仿真环境即可实现通信
  • 与底层算法的通用接口：cmd_vel，发出的指令包含机器人的线速度和角速度

• Bottom-Layer：状态估计和运动控制
  • 提供了与 SOTA SLAM（如 A-LOAM、LeGO-LOAM、LIO-SAM、FAST-LIO2等）的接口

2.2 上层算法（Top-Layer）

2.2.1 TARE 探索规划器

• 用于高效探索大型复杂环境的两层分层框架
  • 局部层：使用稠密的地图表示，规划出一条精细的局部探索轨迹，以覆盖机器人周围的环境
  • 全局层：使用稀疏的地图表示，规划出一条粗糙的覆盖整个环境的全局探索轨迹，然后利用这条全局探索轨迹引导局部探索轨迹
    

2.2.2 FAR Planner 全局路径规划器

• 特点
  • 基于可视图（Visibility graph）的路径规划器，可以在已知或未知的环境中工作，并可以在大尺度环境中改变路径（re-route）
  • 一个线程在机器人移动过程中不断动态维护一个可视图（Visibility graph）（不断在后台运行并更新地图），另一个线程可以在这个可视图上 3ms 内搜索一条可以到达目标点的可行路径

2.3 中间层算法（Mid-Layer）

• 中间层：自主导航系统框图

2.3.1 仿真环境模型

• 室内环境、校园、停车场、隧道与森林

2.3.2 避障（Collision Avoidance）

• 局部避障（Local Planner 的主要内容）
  对应论文：Falco: Fast Likelihood-based Collision Avoidance with Extension to Human-guided Navigation

  • 算法的主要思想在于尽可能地让计算在线下完成

  • 作用

    • 大部分上层算法输出的轨迹都是无碰撞的轨迹，但实际情况下上层算法输出的轨迹都是基于比较粗糙的地图（忽略了一些细节），因此无法保证实际运行过程中百分百无碰撞，所以需要局部规划器根据精细的地图输出轨迹
    • 将上层算法与其他部分分割开来，无需输出指令给底层机器人，而只需发送一个 waypoint，剩下的交给局部规划器去输出指令给机器人执行

  • 实现方法

    • 首先用 Matlab 生成离线轨迹库来模拟机器人在未来一段时间内可能走过的轨迹
    • 然后对所有轨迹覆盖的空间，计算其内部所有点（在一定解析度下）与所有轨迹发生碰撞的可能性
    • 经过这样的离线计算，可以得到一个空间内 3D 点到轨迹的对应关系
    • 在实时运行的过程中，一旦空间内的某个点上有障碍物，可以马上知道哪些轨迹将会受到影响，并且会降低选择这些轨迹作为最终路径的可能性

  • 将路径建模为蒙特卡洛样本，并选择到达目标可能性最大的一组路径
  • 因为大量的计算都在线下进行，线上运行的时候只需要实时选择无碰撞的轨迹，算法可以在几毫秒内规划出一条无碰撞而最接近目标点的路径

  • 示例
    • 所有的黄线都是当前时刻机器人周围的局部无碰撞路径（通过 Matlab 离线生成），会导致碰撞的路径因为白色障碍物的影响已经被剔除
    • 在实际的自主导航过程中，避障模块接收上层的算法提供的 waypoint来引导其行驶

2.3.3 地形可穿越性分析（Terrain Traversability Analysis）

• 方法

  • 将机器人周围的地图叠加起来并栅格化，然后计算每一个格子对应的地面的高度，再计算这些格子里所有包含的点相对于地面的高度并最终输出，根据这些高度来区分地面和障碍物（下图中红色的点代表不可通行的区域，绿色点覆盖的区域则代表可通行区域）

• 输出：两个地形图

  • 10m x 10m 的局部地形图（高分辨率）

    • 提供给局部避障（路径规划）使用

  • 40m x 40m 的地形图（低分辨率）

    • 提供给最上层的规划算法使用

• 负障碍物（negative obstacle）检测

  • 通常来说 LiDAR 看不见这些负障碍物（坑、洞、下坡等）
  • 直接当成机器人看不见的地方，设置为不可通行
  • 该功能在系统中是一个参数进行调控的，可选择打开或关闭

2.3.4 航点跟随（waypoint Following）

• 所有 waypoint 都是使用局部规划器规划轨迹以到达这个点，所以这些 waypoint 离机器人都比较近，而且是在一个可到达的区域
• 如果实际操作过程中需要机器人到达一个比较远的地方，这个时候就需要一个上层规划器将 waypoint 拆分成很多小的目标点，一步一步引导机器人过去

2.4 底层集成（Bottom-Layer）

• 兼容激光 SLAM 算法：LOAM、A-LOAM、LeGO-LOAM、LIO-SAM、LIO-mapping、FAST-LIO2、Faster-LIO

• 集成配置：以 LIO-SAM 为例（下面有完整配置文件网盘链接）

  • stateEstimationTopic = /lio_sam/mapping/odometry
  • registeredScanTopic = /lio_sam/mapping/cloud_registered
  • flipStateEstimation = false
  • flipRegisteredScan = false

  <!-- 文件路径： \autonomous_exploration_development_environment\src\loam_interface\launch\loam_interface.launch -->
  <launch>
    <node pkg="loam_interface" type="loamInterface" name="loamInterface" output="screen" required="true">
      <param name="stateEstimationTopic" type="string" value="/lio_sam/mapping/odometry" />
      <param name="registeredScanTopic" type="string" value="/lio_sam/mapping/cloud_registered" />
      <!-- 不同的slam算法发布的odometry以及registered cloud坐标系是不一样的，比如loam发布的点云坐标系是map_rot
          与我们使用loam_interface里发布出来的registered_cloud的坐标系map是差了一个旋转变换的
          这时候就需要flipRegiateredCloud设置为true，但是也有别的slam算法发布出来的点云和loaminterface发布的
          点云坐标系一致，那就不需要转换，参数就是false -->
      <param name="flipStateEstimation" type="bool" value="false" />
      <param name="flipRegisteredScan" type="bool" value="false" />
      <param name="sendTF" type="bool" value="true" />
      <param name="reverseTF" type="bool" value="false" />
    </node>
  </launch>
  

  state_estimation_setup_notes

  • 链接：https://pan.baidu.com/s/1Z3c6xwHN-Xwzom-_PQ5bTg?pwd=25xc
  • 提取码：25xc

• 注意事项

  • LiDAR 可水平或一定倾斜的安装
  • 但不建议旋转驱动 LiDAR，因为当障碍物突然出现时，可能会由于更新频率比较慢导致反应时间变长，从而发生碰撞等现象

2.5 实用工具

2.5.1 Visualization Tools

• 在 rviz 中有选择的显示：完整点云地图(浅白色点)、已探测区域点云地图(蓝色点)、机器人轨迹(彩色曲线)、探测区域体积、行驶距离、算法运行时间，这些数据会自动存放到一个文件中

2.5.2 Joystick-based Debugging

• 目的：将上层规划器与其他部分分割开来，用人的指令取代上层规划器，同时也保留了中间层和底层的功能，因此机器人此时仍然能实现避障功能
• 作用
  • 可以方便的对中间层和底层进行 debug 和调试，而不用考虑上层的干扰

• 允许用户在手动、智能操纵杆和自主模式之间方便切换
• 适配 PS3/4、Xbox 以及虚拟操纵杆

2.6 自主移动平台集成

• 完全支持差速移动平台
• 部分支持全向移动平台，但侧向移动能力会被忽略，只是当做差速移动平台来使用
• 不支持基于阿克曼转向的汽车类移动平台（主要是狭窄环境下无法支持）

3. 自主导航系统应用

3.1 DAPPA 地下挑战赛

• 涉及隧道、地下城市空间、洞穴 3 类环境
• 在自主导航系统框架下扩展了行为执行和多机器人协同（下图黄色框图）
• 因为系统中有高层级的/全局的路径规划/探索算法可以处理回环问题，因此底层集成时不需要提供带有回环的 SLAM / 状态估计算法（因为局部避障模块和 TTA 模块只考虑机器人周围临近区域的信息）

• 比赛中的自主导航系统使用了 TARE 探索算法和 FAR 路径规划算法
  • 蓝色点为出发点（start），end 代表机器人剩余待探索区域的终点
  • 示例：机器人首先通过 TARE 探索算法到达 A 点，然后操作手发送一个目标点在 B 点，于是机器人切换为 FAR 路径规划算法摸索着到达 B 点，到达 B 点后再次切换为 TARE 探索算法完成剩余的探索任务

3.2 Mattport3D 和 AI Habitat

• 自主导航系统支持仿真环境下的开发和应用
• Mattport3D 是一系列仿真模型的集合，包含了 90 个从真实房间里建立的模型，外观十分逼真，并且这些模型还包含语义信息，可通过 AI Habitat 仿真引擎实时渲染出 RGB、深度和语义图像（这三类图像在自主导航系统中不会用到）

4. 系统扩展应用示例

4.1 用于视觉/激光雷达导航的自监督学习

• 在 Mattport3D 和 AI Habitat 仿真环境中跑这套自主导航算法（提供 RGB、深度和语义三种图像），结合导航系统输出和这三种图像和来训练神经网络，理论上训练好之后，这个神经网络可以只根据图像输出来模拟导航算法的输出，这样的好处就是可以摆脱对 LiDAR 的依赖（因为导航算法是需要 LiDAR 做精确定位的，但是训练好神经网络之后理论上来说就只需要一个相机来给机器人达到类似效果的导航）
• 可以支持模拟到真实（simulation to real）的适应
  

4.2 与人类交互的混合主动导航（Mixed initiative navigation）

• 利用具有语义真值的 Matterport 3D 房屋模型
• 使用自主导航系统进行导航以生成训练数据

4.3 拥挤环境中的社交导航（Social navigation）

• 需要修改 Gazebo 配置以插入行人模型
• 行人的真值可以简化仿真中的部分问题

4.4 农业机器人应用

FAR Planner在农业机器人中的应用

• 在仿真环境中搭建了果园场景，先在仿真环境中模拟，然后再在实际场景中使用

5. 仿真 Demo 与界面介绍

• Development Environment 仿真界面
  • RegScan
    • 表示机器人静止时传感器 LiDAR 所能看到的范围（registered scan）
  • overallMap
    • 为提供给人看的全局地图，即下图一中白色轮廓背景（机器人/传感器是不知道这张全局地图的）
  • ExploredArea
    • 表示已经探索过和正在探索的区域（下图一中蓝色区域）
  • Trajectory
    • 表示机器人的轨迹（下图一中彩色曲线）
  • TerrainMap
    • 表示 10×10m 的局部地形图（高分辨率），提供给局部避障（路径规划）使用，如下图二所示
  • TerrainMapExt
    • 表示 40x40m 的地形图（低分辨率），提供给最上层的规划算法使用，如下图三所示（通过设定一个相对于地面的高度阈值来判断是否可以通行，其中红色为障碍物，绿色为可通行地面）

• FAR Planner 仿真界面
  • 人为给定一个 Goalpoint，可以在未知环境中不断尝试（Attemptable）和试错，当把自己引导到一个错误的路径时，需要快速的 replan，基于现有已知的信息得出一个最优路径并执行
  • save read
    • 保存已经探索完生成的 V-Graph，读取之前保存的 V-Graph
  • Planning Attemptable
    • 若取消勾选，则系统默认只会规划已知环境的路径（不会去尝试未知区域），当把 Goalpoint 放在未知区域时才会自动切换为 Attemptable 模式下的规划
  • Update Visibility Graph
    • 算法中会提供各个环境中已经探索完的全局 V-Graph（data 文件夹中的 .vgh文件），只需 read 进来然后快速的进行规划和导航操作。此时便可把 Update Visibility Graph 取消勾选（如果环境是动态的则需要勾选以实现更新）

Part Ⅲ Algorithms Installation and Operation

前置信息

• 支持 Ubuntu 18.04 with ROS Melodic & Ubuntu 20.04 with ROS Noetic
• 目前仅支持 AMD64 架构，不支持 ARM 架构

1. Autonomous Exploration Development Environment

# 1. 安装依赖
$ sudo apt update
$ sudo apt install libusb-dev

# 2. 克隆源码
$ git clone https://github.com/HongbiaoZ/autonomous_exploration_development_environment.git

# 3. 分支检查与编译安装
$ cd autonomous_exploration_development_environment
$ git checkout melodic # Ubuntu 20.04 则将 melodic 替换为 noetic
$ catkin_make

# 4. 通过脚本安装仿真环境
$ ./src/vehicle_simulator/mesh/download_environments.sh

# 5. 启动开发环境
$ source devel/setup.sh
$ roslaunch vehicle_simulator system_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

# 案例：发送一系列 waypoint，同时发送导航边界和速度，车辆在跟踪 waypoint 的同时在边界内行驶（另开一个终端）
$ cd autonomous_exploration_development_environment
$ source devel/setup.sh
$ roslaunch waypoint_example waypoint_example_garage.launch

2. TARE（Technologies for Autonomous Robot Exploration）

# 1. 克隆源码
$ git clone https://github.com/caochao39/tare_planner.git

# 2. 编译安装
$ cd tare_planner
$ catkin_make

# 3. 启动开发环境
$ cd ~/autonomous_exploration_development_environment
$ source devel/setup.sh
$ roslaunch vehicle_simulator system_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

# 4. 启动 TARE（另开一个终端）
$ cd ~/tare_planner
$ source devel/setup.sh
$ roslaunch tare_planner explore_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

3. FAR Planner

# 1. 克隆源码
$ git clone https://github.com/MichaelFYang/far_planner

# 2. 编译安装
$ cd far_planner
$ catkin_make

# 3. 启动开发环境
$ cd ~/autonomous_exploration_development_environment
$ source devel/setup.sh
$ roslaunch vehicle_simulator system_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

# 4. 启动 FAR Planner（另开一个终端）
$ cd ~/far_planner
$ source devel/setup.sh
$ roslaunch far_planner far_planner.launch

Part Ⅳ Integrating System on Real Robot (to be continued…)

Part Ⅴ Algorithms Annotation (to be continued…)