A GP-based Robust Motion Planning Framework for Agile Autonomous Robot Navigation and Recovery in Unknown Environments

TL;DR

This work tackles robust agile navigation for autonomous mobile robots in unknown environments by introducing a GP-based model that proactively predicts back-end motion-planner failures along a receding horizon. When predicted risk exceeds a threshold, the system autonomously halts and executes a recovery strategy that uses the same GP to locate a nearby safe state from which nominal planning can resume. The approach integrates a front-end corridor generation, a back-end Bézier/MINVO trajectory optimization, and a GP regression that estimates the probability of back-end failure $P(Z^b_{m{x}, ext{C}}|oldsymbol{d})$ using features derived from the corridor and robot state $d( ext{C},m{x})=[t_C,| ext{C}|]$, trained in simulation on diverse worlds. Results from simulations and physical experiments with Jackal and Spot demonstrate improved planning success rates and autonomous recovery without real-world retraining, highlighting the method's model- and sensor-agnostic robustness and potential for practical deployment in unknown environments.

Abstract

For autonomous mobile robots, uncertainties in the environment and system model can lead to failure in the motion planning pipeline, resulting in potential collisions. In order to achieve a high level of robust autonomy, these robots should be able to proactively predict and recover from such failures. To this end, we propose a Gaussian Process (GP) based model for proactively detecting the risk of future motion planning failure. When this risk exceeds a certain threshold, a recovery behavior is triggered that leverages the same GP model to find a safe state from which the robot may continue towards the goal. The proposed approach is trained in simulation only and can generalize to real world environments on different robotic platforms. Simulations and physical experiments demonstrate that our framework is capable of both predicting planner failures and recovering the robot to states where planner success is likely, all while producing agile motion.

Figures (7)

• Figure 1: Example in which an unexpected dead end could cause a motion planning failure. The proposed approach predicts the risk of such a failure and recovers before it can occur.
• Figure 2: Block diagram for the proposed approach.
• Figure 3: An example case study investigated in this work visualizing the complete navigation pipeline (a,b), in which the receding horizon, safe corridor motion planner fails (c), prompting the recovery pipeline to take over and recover the system (d). For $\bm{\tau}(t)$, brighter colors denote higher speeds.
• Figure 4: Solver failure trends for (a) $t_C$ and $|\mathcal{C}|$ along with learned distribution vs test distribution for (c) $|\mathcal{C}|=2$ and (d) $|\mathcal{C}|=3$.
• Figure 5: (a) Gazebo world with $1$m doorway and 3 different goals. In (b) an obstacle hides an occluded wall leading to a collision without our framework (c) vs a successful navigation toward $x_g^0$ in (d) with our approach.