[论文解读] SLAP: Simultaneous Localization and Planning Under Uncertainty for Physical Mobile Robots via Dynamic Replanning in Belief Space: Extended version

Ali‐akbar Agha‐mohammadi, Saurav Agarwal+3 more|arXiv (Cornell University)|Oct 26, 2015

Reinforcement Learning in Robotics被引用 3

一句话总结

本文提出SLAP，一种在信念空间中实现动态重规划的框架，使物理移动机器人能够在不确定性环境下执行同步定位与规划。通过利用离线预计算的FIRM图和在线滚动策略，SLAP实现了跨同伦类别的实时重规划，并通过消除不必要的稳定化步骤，优于FIRM，从而在存在较大定位误差的动态、不确定环境中实现鲁棒导航。

ABSTRACT

Simultaneous localization and Planning (SLAP) is a crucial ability for an autonomous robot operating under uncertainty. In its most general form, SLAP induces a continuous POMDP (partially-observable Markov decision process), which needs to be repeatedly solved online. This paper addresses this problem and proposes a dynamic replanning scheme in belief space. The underlying POMDP, which is continuous in state, action, and observation space, is approximated offline via sampling-based methods, but operates in a replanning loop online to admit local improvements to the coarse offline policy. This construct enables the proposed method to combat changing environments and large localization errors, even when the change alters the homotopy class of the optimal trajectory. It further outperforms the state-of-the-art FIRM (Feedback-based Information RoadMap) method by eliminating unnecessary stabilization steps. Applying belief space planning to physical systems brings with it a plethora of challenges. A key focus of this paper is to implement the proposed planner on a physical robot and show the SLAP solution performance under uncertainty, in changing environments and in the presence of large disturbances, such as a kidnapped robot situation.

研究动机与目标

• 解决在部分可观察、存在不确定性和动态变化的环境中在线同步定位与规划（SLAP）的挑战。
• 克服单次查询POMDP求解器在信念更新后需要完整重新计算的局限，使其在实时SLAP中不可行。
• 实现在不同轨迹同伦类之间的动态重规划，即使在发生如机器人被“劫持”等大范围定位误差后也能实现。
• 在真实世界室内环境中，对物理移动机器人实现并验证信念空间规划器。
• 证明所提方法在避免先前基于稳定化方法的性能权衡的同时，仍能保持高成功率和高性能。

提出的方法

• 使用基于采样的方法离线构建与查询无关的FIRM（基于反馈的信息道路图）图，实现对不同初始信念的复用。
• 将FIRM图用作在线信念空间重规划中的基础策略和代价到目标的近似，通过滚动策略实现。
• 通过重用FIRM图中预计算的转移概率和代价实现动态重规划，实现无需完整重新计算的快速在线更新。
• 在信念空间中集成局部反馈控制器（如悬链面 funnel），以减少信念树搜索中的指数级增长，通过构建信念图实现线性增长。
• 利用高斯信念近似和局部可线性化系统动力学，设计闭环控制器以稳定信念演化。
• 实现一种基于信念空间的滚动策略，根据期望的代价到目标评估候选动作，实现对环境变化和定位漂移的在线适应。

实验结果

研究问题

• RQ1 信念空间规划框架是否能够实现在不确定性下，即使在发生大范围定位误差后，仍支持移动机器人的实时、动态重规划？
• RQ2 如何将多查询、与查询无关的规划器（如FIRM）适配至连续、部分可观察环境中的在线SLAP？
• RQ3 在不确定和动态环境中，消除FIRM中的稳定化阶段在多大程度上提升了性能和成功率？
• RQ4 所提方法是否能够成功引导物理机器人穿越具有障碍物动态变化和位姿偏差的复杂、变化的环境？
• RQ5 与单次查询求解器相比，离线FIRM构建与在线滚动重规划相结合，在计算效率和鲁棒性方面有何提升？

主要发现

• 所提出的SLAP框架成功实现了信念空间中的实时动态重规划，使机器人在环境变化时能够切换至不同同伦类别的轨迹。
• 该方法通过消除对稳定化步骤的需求，优于当前最先进的FIRM框架，从而恢复了此前为换取稳定性而牺牲的性能和成功率。
• 通过重用离线FIRM图中预计算的代价和转移概率，线上重规划过程实现了低计算开销，支持实时运行。
• 大量仿真结果表明，在不确定性条件下，尤其在大范围定位误差场景下，成功率和鲁棒性均有显著提升。
• 该方法在真实世界室内环境中成功验证于物理移动机器人，展示了在动态障碍物地图和位姿偏差下实现可靠导航的能力。
• 基于滚动策略的重规划方法使机器人能够从如“机器人被劫持”等大扰动中恢复，并在持续的传感与执行噪声下保持目标导向行为。