Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments

Oscar de Groot, Bruno Brito, Laura Ferranti, Dariu Gavrila, Javier Alonso-Mora

TL;DR

This paper introduces a novel scenario-based trajectory optimization method (S-MPCC) for autonomous robots navigating uncertain dynamic environments. It tackles the challenge of planning safe paths in the presence of unpredictable obstacles by converting probabilistic collision constraints into deterministic ones using sampled "scenarios," enabling real-time, robust motion planning.

What Problem Does This Paper Address?

Autonomous robots, especially those operating in human-centric environments like warehouses or public spaces, must navigate safely despite inherent uncertainties. These uncertainties stem from various sources, including noisy sensor readings and the unpredictable future movements of dynamic obstacles (e.g., pedestrians, other vehicles). Traditional motion planning methods often rely on deterministic obstacle predictions, which fail to guarantee safety when faced with uncertainty. While robust optimization can handle bounded uncertainties, it can be overly conservative. Chance-constrained optimization offers a way to bound the probability of collisions, but directly solving these problems is computationally intractable, especially for unbounded or non-Gaussian uncertainty distributions. This paper aims to bridge this gap by providing a computationally efficient and safe motion planning solution for robots operating under such complex, uncertain conditions.

Methods

The core of the proposed method is a Scenario-based Model Predictive Contouring Control (S-MPCC) framework. The key innovation lies in how it handles chance constraints. Instead of directly solving the probabilistic problem, the authors transform it into a deterministic "scenario program." This is achieved by:

  1. Linearizing Chance Constraints: For each time step in the planning horizon, the probabilistic collision constraint is linearized around the robot's predicted position. This simplifies the geometry of the problem.
  2. Sampling Scenarios: From the linearized chance constraints, a set of "scenarios" is generated by sampling the uncertainty distribution of the dynamic obstacles. Each scenario represents a deterministic collision constraint for a specific realization of the obstacle's position.
  3. Scenario Program: The original chance-constrained optimization problem is reformulated as a deterministic scenario program, where the robot must satisfy all sampled deterministic constraints.
  4. Pruning Scenarios: To manage the potentially large number of scenarios, the authors leverage the geometry of the workspace and the linearized constraints. They identify a "support subsample" – a minimal set of scenarios that are critical for defining the feasible space. This significantly reduces the computational burden.
  5. Model Predictive Contouring Control (MPCC): The scenario program is then solved within an MPCC framework, which allows for online replanning and adaptation to changing environments.

This approach allows for handling arbitrary uncertainty distributions, including non-Gaussian ones, and provides probabilistic safety guarantees.

Datasets

Not discussed in the paper. The paper focuses on a simulation and real-world experimental setup rather than training on large datasets.

Key Insights and Ideas

  • Scenario Generation as a Bridge: The paper elegantly bridges the gap between probabilistic and deterministic optimization by using sampled "scenarios" to represent uncertainty. This is a powerful abstraction for handling complex real-world problems.
  • Geometry-Based Scenario Pruning: The insight that only a subset of sampled scenarios is truly critical for defining the feasible space (the "support subsample") is crucial. Exploiting the geometry of the linearized constraints allows for significant computational savings.
  • Real-time Performance with Probabilistic Guarantees: The method achieves a balance between computational tractability for real-time applications and probabilistic safety guarantees, which is a significant challenge in this domain.
  • Handling Arbitrary Uncertainties: The scenario-based approach is inherently flexible and can handle non-Gaussian and unbounded uncertainty distributions, which are common in real-world robotics.

Key Results

  • Collision Prevention: In simulations with up to 6 pedestrians, the S-MPCC method successfully prevented collisions, outperforming baseline MPCC and a Deep Reinforcement Learning approach (CADRL) in terms of safety.
  • Probabilistic Risk Adherence: The method maintained the probability of collision below the specified risk threshold (e.g., 3σ) in simulations.
  • Smoother Trajectories: S-MPCC generated smoother trajectories, leading to earlier arrival times at the goal compared to baselines.
  • Computational Cost: While achieving better safety and efficiency, the computation time for S-MPCC was higher than the baselines, though still considered real-time capable. The paper notes that further optimization (e.g., considering only nearby pedestrians) can reduce this cost.
  • Real-World Validation: The method was successfully demonstrated on a physical robot platform navigating among pedestrians.

Limitations and Future Work

  • Independence Assumption: The current formulation assumes that the uncertainty models for each time step are independent. The authors suggest that alleviating this assumption could improve the risk bounds.
  • Conservative Risk Bounds: The risk bounds provided by the current method are noted as being relatively conservative. Developing tighter bounds would be beneficial for planning safer long-term motions, especially for robots with slow dynamics.
  • Incorporating Interactions: The authors suggest that incorporating interactions between the robot and pedestrians (e.g., social behaviors) could further improve navigation and reduce uncertainties.
  • Computational Cost Reduction: While real-time capable, further reducing computation time remains a goal, potentially through more aggressive scenario pruning or by considering only relevant obstacles.

Summary updated

Abstract

We present an optimization-based method to plan the motion of an autonomous robot under the uncertainties associated with dynamic obstacles, such as humans. Our method bounds the marginal risk of collisions at each point in time by incorporating chance constraints into the planning problem. This problem is not suitable for online optimization outright for arbitrary probability distributions. Hence, we sample from these chance constraints using an uncertainty model, to generate “scenarios,” which translate the probabilistic constraints into deterministic ones. In practice, each scenario represents the collision constraint for a dynamic obstacle at the location of the sample. The number of theoretically required scenarios can be very large. Nevertheless, by exploiting the geometry of the workspace, we show how to prune most scenarios before optimization and we demonstrate how the reduced scenarios can still provide probabilistic guarantees on the safety of the motion plan. Since our approach is scenario based, we are able to handle arbitrary uncertainty distributions. We apply our method in a Model Predictive Contouring Control framework and demonstrate its benefits in simulations and experiments with a moving robot platform navigating among pedestrians, running in real-time.
BibTeX
@article{groot2021scenario,
  author = {Oscar de Groot and Bruno Brito and Laura Ferranti and Dariu Gavrila and Javier Alonso-Mora},
  title = {{Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments}},
  journal = {IEEE RA-L},
  year = {2021},
  doi = {10.1109/lra.2021.3074866},
}
Figure