Parallelized Probabilistic Motion Planning in Dynamic Environments

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.

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.

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.

Project Demonstrations

Partners

We present a vehicle system capable of navigating safely and efficiently around Vulnerable Road Users (VRUs), such as pedestrians and cyclists. The system comprises key modules for environment perception, localization and mapping, motion planning, and control, integrated into a prototype vehicle. A key innovation is a motion planner based on Topology-driven Model Predictive Control (T-MPC). The guidance layer generates multiple trajectories in parallel, each representing a distinct strategy for obstacle avoidance or non-passing. The underlying trajectory optimization constrains the joint probability of collision with VRUs under generic uncertainties. To address extraordinary situations (edge cases) that go beyond the autonomous capabilities, such as construction zones or encounters with emergency responders, the system includes an option for remote human operation, supported by visual and haptic guidance. In simulation, our motion planner outperforms three baseline approaches in terms of safety and efficiency. We also demonstrate the full system in prototype vehicle tests on a closed track, both in autonomous and remotely operated modes.

Ground robots navigating in complex, dynamic environments must compute collision-free trajectories to avoid obstacles safely and efficiently. Nonconvex optimization is a popular method to compute a trajectory in real-time. However, these methods often converge to locally optimal solutions and frequently switch between different local minima, leading to inefficient and unsafe robot motion. In this work, we propose a novel topology-driven trajectory optimization strategy for dynamic environments that plans multiple distinct evasive trajectories to enhance the robot's behavior and efficiency. A global planner iteratively generates trajectories in distinct homotopy classes. These trajectories are then optimized by local planners working in parallel. While each planner shares the same navigation objectives, they are locally constrained to a specific homotopy class, meaning each local planner attempts a different evasive maneuver. The robot then executes the feasible trajectory with the lowest cost in a receding horizon manner. We demonstrate, on a mobile robot navigating among pedestrians, that our approach leads to faster trajectories than existing planners.

Autonomous mobile robots require predictions of human motion to plan a safe trajectory that avoids them. Because human motion cannot be predicted exactly, future trajectories are typically inferred from real-world data via learning-based approximations. These approximations provide useful information on the pedestrian's behavior, but may deviate from the data, which can lead to collisions during planning. In this work, we introduce a joint prediction and planning framework, Partitioned Scenario Replay (PSR), that stores and partitions previously observed human trajectories, referred to as scenarios. During planning, scenarios observed in similar situations are reintroduced (or replayed) as motion predictions. By sampling real data and by building on scenario optimization and predictive control, the planner provides probabilistic col- lision avoidance guarantees in the real-world. Relying on this guarantee to remain safe, PSR can incrementally improve its prediction and planning performance online. We demonstrate our approach on a mobile robot navigating around pedestrians.

About the Project

Project Demonstrations

Partners

Related Publications