Trajectory Planning with Model Predictive Control for Obstacle Avoidance Considering Prediction Uncertainty

TL;DR

The paper addresses dynamic obstacle avoidance under prediction uncertainty for autonomous robots within the ROS2/Nav2 framework. It proposes a receding-horizon MPC that uses obstacle forecasts modeled as Gaussian distributions $D_{d,k+i} \sim \mathcal{N}(\boldsymbol{\mu}_{d,k+i}, \boldsymbol{\Sigma}_{d,k+i})$ and imposes collision-avoidance constraints via Mahalanobis distance thresholds $d_M^2(\boldsymbol{x};\mathcal{N}(\boldsymbol{\mu},\boldsymbol{\Sigma})) < s^2$ with $s = \sqrt{-2\ln(1-p)}$, using a Vector Auto-Regressive (VAR) process for motion prediction. The approach constructs ellipsoidal forecast regions, inflates them to account for robot and obstacle radii, and integrates the MPC with CasADi and IPOPT without requiring Nav2 stack modifications, demonstrated in Gazebo with scalable handling of multiple dynamic obstacles. The main contributions are the uncertainty-aware MPC constraint design, end-to-end implementation within a mature robotics stack, and open-source dissemination to enable benchmarking and future research in dynamic obstacle avoidance. The work has practical significance for real-time, multi-agent navigation where obstacle dynamics are uncertain and must be anticipated rather than merely reacted to.

Abstract

This paper introduces a novel trajectory planner for autonomous robots, specifically designed to enhance navigation by incorporating dynamic obstacle avoidance within the Robot Operating System 2 (ROS2) and Navigation 2 (Nav2) framework. The proposed method utilizes Model Predictive Control (MPC) with a focus on handling the uncertainties associated with the movement prediction of dynamic obstacles. Unlike existing Nav2 trajectory planners which primarily deal with static obstacles or react to the current position of dynamic obstacles, this planner predicts future obstacle positions using a stochastic Vector Auto-Regressive Model (VAR). The obstacles' future positions are represented by probability distributions, and collision avoidance is achieved through constraints based on the Mahalanobis distance, ensuring the robot avoids regions where obstacles are likely to be. This approach considers the robot's kinodynamic constraints, enabling it to track a reference path while adapting to real-time changes in the environment. The paper details the implementation, including obstacle prediction, tracking, and the construction of feasible sets for MPC. Simulation results in a Gazebo environment demonstrate the effectiveness of this method in scenarios where robots must navigate around each other, showing improved collision avoidance capabilities.

Figures (7)

• Figure 1: Overview: Control Structure
• Figure 2: Visual Representation of Ellipse Construction
• Figure 3: Comparison of different forecast regions for $p=0.90$
• Figure 4: Approximation Error in Ellipse Construction
• Figure 5: Details: Control Structure