[论文解读] Non-Gaussian SLAP: Simultaneous Localization and Planning Under Non-Gaussian Uncertainty in Static and Dynamic Environments

Mohammadhussein Rafieisakhaei, Suman Chakravorty+1 more|arXiv (Cornell University)|May 5, 2016

Robotic Path Planning Algorithms参考文献 54被引用 6

一句话总结

该论文提出了一种基于凸优化和有限时域控制的非高斯SLAP（同时定位与规划）框架，用于在静态和动态环境中生成低不确定性、无碰撞的轨迹，且信念分布为非高斯分布。通过集成障碍函数与同伦类探索，该方法实现了局部最优、平滑的路径，最小化了感知不确定性与控制代价，从而在过程与测量不确定性下实现了实时在线规划。

ABSTRACT

Simultaneous Localization and Planning (SLAP) under process and measurement uncertainties is a challenge. It involves solving a stochastic control problem modeled as a Partially Observed Markov Decision Process (POMDP) in a general framework. For a convex environment, we propose an optimization-based open-loop optimal control problem coupled with receding horizon control strategy to plan for high quality trajectories along which the uncertainty of the state localization is reduced while the system reaches to a goal state with minimum control effort. In a static environment with non-convex state constraints, the optimization is modified by defining barrier functions to obtain collision-free paths while maintaining the previous goals. By initializing the optimization with trajectories in different homotopy classes and comparing the resultant costs, we improve the quality of the solution in the presence of action and measurement uncertainties. In dynamic environments with time-varying constraints such as moving obstacles or banned areas, the approach is extended to find collision-free trajectories. In this paper, the underlying spaces are continuous, and beliefs are non-Gaussian. Without obstacles, the optimization is a globally convex problem, while in the presence of obstacles it becomes locally convex. We demonstrate the performance of the method on different scenarios.

研究动机与目标

• 解决机器人系统在非高斯过程与测量不确定性下进行同时定位与规划的挑战。
• 开发一种计算高效的基于优化的方法，避免POMDP求解器固有的维度灾难与历史依赖问题。
• 在具有移动障碍物和时变约束的动态环境中，实现实时在线规划。
• 通过探索多个同伦类并选择综合考虑不确定性与控制代价的最低成本路径，提升轨迹质量。
• 通过凸优化与有限时域控制保持低计算成本，实现快速重规划。

提出的方法

• 将SLAP建模为一个非高斯信念状态的凸开环最优控制问题，避免使用高斯近似。
• 采用有限时域控制（RHC）策略，实现在环境变化下的在线规划与快速重规划。
• 引入障碍函数以在非凸环境中实现碰撞避免，同时保持优化问题的凸性。
• 使用内点法结合梯度下降求解优化问题，确保收敛至局部最优轨迹。
• 采用两个球体覆盖机器人本体，使用膨胀多边形的最小体积椭球（MVEE）定义障碍物约束。
• 应用基于距离与方位的观测模型，设计代价函数以减少信念不确定性，如引理1所示。

实验结果

研究问题

• RQ1 如何在不依赖高斯近似的情况下，高效地实现非高斯信念分布下的SLAP？
• RQ2 使用障碍函数的凸优化能否在非凸静态环境中生成无碰撞、低不确定性的轨迹？
• RQ3 与单路径优化相比，同伦类探索在不确定性下如何提升轨迹质量？
• RQ4 该方法能否支持具有时变障碍物的动态环境，并实现在线重规划？
• RQ5 在非高斯信念空间规划中，不确定性降低与控制代价之间存在何种权衡？

主要发现

• 在无障碍物情况下，优化问题为全局凸；在有障碍物时为局部凸，可通过内点法高效求解。
• 该方法成功避开了障碍物与禁入区域，如图6所示，机器人的轨迹始终位于椭球形障碍物之外。
• 在动态环境（图7与图8）中，机器人能够在线重规划以避开移动障碍物，调整路径绕过物体，同时保持靠近信息源。
• 对不同同伦类中的轨迹进行评估，选择综合平衡不确定性与控制代价的最低成本路径，从而提升解的质量。
• 有限时域策略实现了快速重规划，使系统能够适应执行过程中的新障碍物或检测到的变化。
• 该方法生成了平滑、安全且短路径，在非高斯不确定性下有效降低了信念不确定性，同时最小化了能量消耗。