Parallelized Probabilistic Motion Planning in Dynamic Environments

project image

People

Oscar de Groot - PhD candidate
Prof. Laura Ferranti - Reliable Control (R2C) Lab TU Delft
Prof. Dariu Gavrila - Intelligent Vehicles (IV) Group TU Delft
Prof. Javier Alonso-Mora - Autonomous Multi-Robot Lab (AMR) TU Delft

Funding

This project is funded in part by NWO-TTW within the SafeVRU project.

More Links
Project Website

Partners

About the Project

This project develops probabilistic motion planners for social robots and automated vehicles. Our main goal is to account for the uncertain future motion of obstacles, such as pedestrians, to plan safe and efficient robot motion. This uncertainty, in practice, stems from the fact that the robot cannot observe the intentions of humans with certainty. It therefore needs to account for plausible outcomes when making a decision.

We developed two model-based planning frameworks in this project:

  • Scenario-Based Trajectory Optimization: To account for uncertainty associated with motion predictions of dynamic obstacles,
  • Topology-Driven Trajectory Optimization: To parallelize existing optimization-based planners, such as scenario-based planning.

Scenario-Based Trajectory Optimization

Safe Horizon MPC

The main idea behind scenario-based planning is to sample from the distribution of the uncertainty. Each sample represents a possible future trajectory of the obstacle. We enforce collision constraints with all samples to account for the uncertainty. By sampling, we can handle any type of distribution, making the approach widely applicable. The more samples are taken, the safer our motion plan becomes. Scenario-based trajectory optimization plans probabilistic safe trajectories in real-time while incorporating generic uncertainties associated with the future motion of dynamic obstacles.

Topology-Driven Trajectory Optimization

Safe Horizon MPC

Many existing planners, in particular optimization-based methods, plan only a single trajectory. They rely on this one plan to remain safe and efficient and tend not to explore other options. Topology-driven trajectory optimization proposes a framework to parallelize existing optimization-based planners, computing several distinct trajectories that each pass the obstacles in a different way. This leads to faster motion plans without compromising on safety.

Related Publications

Probabilistic Risk Assessment for Chance-Constrained Collision Avoidance in Uncertain Dynamic Environments
K. Mustafa, O. de Groot, X. Wang, J. Kober, J. Alonso-Mora. In , in IEEE Int. Conf. on Robotics and Automation (ICRA), 2023.

pdf
Abstract: Balancing safety and efficiency when planning in crowded scenarios with uncertain dynamics is challenging where it is imperative to accomplish the robot’s mission without incurring any safety violations. Typically, chance constraints are incorporated into the planning problem to provide probabilistic safety guarantees by imposing an upper bound on the collision probability of the planned trajectory. Yet, this results in overly conservative behavior on the grounds that the gap between the obtained risk and the specified upper limit is not explicitly restricted. To address this issue, we propose a real-time capable approach to quantify the risk associated with planned trajec- tories obtained from multiple probabilistic planners, running in parallel, with different upper bounds of the acceptable risk level. Based on the evaluated risk, the least conservative plan is selected provided that its associated risk is below a specified threshold. In such a way, the proposed approach provides probabilistic safety guarantees by attaining a closer bound to the specified risk, while being applicable to generic uncertainties of moving obstacles. We demonstrate the efficiency of our proposed approach, by improving the performance of a state- of-the-art probabilistic planner, in simulations and experiments using a mobile robot in an environment shared with humans.

Globally Guided Trajectory Planning in Dynamic Environments
O. de Groot, L. Ferranti, D. Gavrila, J. Alonso-Mora. In IEEE Int. Conf. on Robotics and Automation (ICRA), 2023.

pdf video
Abstract: Navigating mobile robots through environments shared with humans is challenging. From the perspective of the robot, humans are dynamic obstacles that must be avoided. These obstacles make the collision-free space nonconvex, which leads to two distinct passing behaviors per obstacle (passing left or right). For local planners, such as receding-horizon trajectory optimization, each behavior presents a local optimum in which the planner can get stuck. This may result in slow or unsafe motion even when a better plan exists. In this work, we identify trajectories for multiple locally optimal driving behaviors, by considering their topology. This identification is made consistent over successive iterations by propagating the topology information. The most suitable high-level trajectory guides a local optimization-based planner, resulting in fast and safe motion plans. We validate the proposed planner on a mobile robot in simulation and real-world experiments.

Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments
O. de Groot, B. Brito, L. Ferranti, D. Gavrila, J. Alonso-Mora. In , IEEE Robotics and Automation Letters (RA-L), 2021.

web pdf video
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.