Efficient Exploration in Crowds by Coupling Navigation Controller and Exploration Planner

Zheng, Zhuoqi, Shengfeng He, and Jia Pan. "Efficient Exploration in Crowds by Coupling Navigation Controller and Exploration Planner." IEEE Robotics and Automation Letters 7.4 (2022): 12126-12133. 新加坡管理大学 PDF

1. 摘要

在这项工作中，我们提出了一种框架，可以通过将强化学习导航控制器和分层探索规划器与恢复规划器紧密耦合来解决这些挑战。

问题：

由于SLAM一般是针对静态场景设计的，未知的动态物体会破坏机器人状态估计的质量，从而导致定位和建图精度降低。在人群密集的地方，动态行人会使得SLAM精度降低，因此传统的探索方法不能应用在社会环境中。
选择局部具有更高信息增益视点的过程中，不能考虑视点所对应位置的行人信息，会频繁出现碰撞情况

贡献点：

提出了和基于强化学习的导航控制器紧密耦合的分层探索规划器。探索规划器允许机器人确定最优的探索策略，不仅可以最大化探索地图的信息增益，而且可以最小化碰撞次数。
SLAM恢复规划器通过从探索地图中特征丰富的位置和历史轨迹中选择一个恢复点来重新定位机器人。
整个探索框架将导航控制器与探索、恢复规划器紧耦合，探索性能较高

2. 方法

探索模式和恢复模式之间相互切换，通过被探索地图的SLAM质量，选择是否进行模式切换

**分层探索模块：**采用分层架构，全局规划器根据下次访问目标确定一个粗略的遍历顺序，而局部规划器在最大化地图信息增益和最小化机器人与人群的交互之间进行权衡，选择一个最优视点。

**恢复规划模块：**在映射区域中选择一个功能丰富的恢复点，可以帮助机器人从 SLAM 故障中恢复。

**导航控制模块：**不仅生成适当的转向指令以避免碰撞，而且还会引导探索模块和恢复规划模块优先选择行人较少的区域，以最大限度地减少机器人与人群的交互。

2.1 基于TSP的全局探索规划器

基于TSP的全局探索规划器优化了所有边界的探索顺序，以确定全局目标 $g_{global}$ 。

先构建地图G
边的权重使用A*算法计算
再用TSP算法计算出规划顺序，选择当前点的下一个边界点作为 $g_{global}$

2.2 Critic引导局部探索规划器

**问题：**之前的方法在考虑信息增益时，不会去考虑对应点的行人密度，可能会导致较大的危险。为了选择一个在低行人密度和高信息增益之间取得适当平衡的视角，该论文呢利用了基于RL的导航控制器的Critic网络提供的信息，评价函数从两个角度进行计算：

信息增益评估： $V_{nbv}(p_{local})= − \frac 1 {|Z |} ∑^k_{i=1} p (z_i | p_{local})logp (z_i | p_{local})$
Critic网络评估： $V_{rl}(p_{local})= V_π(s_{scan}, s_{vel}, p_{local})$

2.3 基于RL的导航控制器

采用基于RL的反应式避障算法作为导航控制器

从局部勘探规划器获取路径 P 后，将该路径上的目标位置 $s_{goal}$ 转换为 $o_{goal}$ 作为基于 RL 的控制器的输入。然后，基于 RL 的控制器中的 Actor 网络输出适当的转向命令，以安全有效地将机器人穿过人群向目标移动。

整个训练过程分为两个阶段：

先在没有静态障碍物的行人场景训练，再在有静态障碍物的行人场景训练
一直在有静态障碍物的行人场景训练

2.4 恢复规划器

为了提高探索系统在密集人群中的鲁棒性，添加了一个恢复规划器，以帮助机器人从SLAM模块的潜在崩溃中恢复过来，例如密集的人群遮挡了对定位很重要的信息地标，或者当机器人与行人碰撞后，SLAM质量下降严重。

触发条件：为了实现高性能的自主探索，当SLAM模块的性能下降时，机器人应切换到主动恢复模式。因此，将触发条件设计为根据定位结果的协方差来衡量 SLAM 输出质量
恢复点提取：通过考虑地图特征的丰富性和历史轨迹中嵌入的信息来计算恢复点候选集。
- 结构性的位置具有高优先级，例如角落
- 在历史地图中具有高精度的点具有高优先级
恢复点选择：在选择最合适的恢复点时，考虑两个因素，包括每个候选点与机器人的距离，以及该点是否可以通过人群轻松到达（类似于探索模块的目标点选取，Critic网络）

3. 实验

在Gazebo中模拟了机器人和拥挤环境，共搭建了八种环境进行实验
使用Turtlebot2平台，并设置了Lidar的扫描角度、范围和分辨率。
对比了五种不同的方法，并使用安全、效率和SLAM质量作为评价指标。
- move_base节点使用动态窗口方法（DWA）进行局部规划，使用Dijkstra算法进行全局规划
- move_base分层探索方法（HEC-MV），它有三个组成部分——基于TSP的全局规划器、基于NBV的本地规划器和来自move_base的DWA导航控制器。
- 人群分层探索方法（HEC），其中其导航模块被DRL网络取代，而不是HEC-MV中的move_base。在 HEC 中，导航规划器与勘探规划器松散耦合。
- 人群中紧密耦合的分层探索（T-HEC），其中局部规划者由基于RL的防撞的Critic网络引导
- 基于恢复的人群中紧密耦合分层探索（RT-HEC），它进一步增加了一个恢复计划器，以及基于 RL 的导航控制器。
实验结果表明，提出的自主探索框架在探索效率、导航安全和SLAM质量方面具有优势。