Probabilistic Motion Planning in Dynamic Environments

Abstract

Logistics and transportation can greatly benefit from the use of autonomous robots, such as self-driving vehicles. Robots can help to move goods or people without human supervision. One of the main components that enable autonomous navigation among humans is motion planning. Motion planning is responsible for computing a collision-free trajectory that moves the robot to its destination, based on the perceived information. The motion planner should be efficient, robust, and safe. This thesis contributes towards this goal by investigating the design of motion planning algorithms for autonomous navigation of mobile robots near humans.

Traditional motion planners for dynamic environments have two key limitations that this thesis aims to address. First, they assume that their model of dynamic obstacles (e.g., humans) is exactly correct, capturing it with a single deterministic prediction. In practice, the robot cannot observe human intentions and must account for its uncertainty about the human's future behavior. Second, motion planners usually compute a single trajectory around an obstacle as a result of previously taken decisions without exploring alternative options. They react slowly or even fail to find a solution when unpredicted changes make this path undesirable. This results in poor planning performance in dynamic environments.

The goal of this thesis is to develop motion planners that account for the uncertainty of human motion predictions and that are consistent and robust in their decision-making in order to deal with unpredicted changes in dynamic environments. To accomplish this goal, this thesis proposes two motion planning frameworks: scenario-based and topology-driven trajectory optimization.

The first contribution of this thesis is Scenario-based Model Predictive Contouring Control (S-MPCC), a real-time capable probabilistic planning framework that incorporates any uncertainty associated with the motion predictions of dynamic obstacles. Contrary to existing methods that only account for small variations around a single predicted trajectory (unimodal uncertainty), the proposed planner accounts for multiple possible trajectories (multi-modal uncertainty). The planner therefore safely accounts for several outcomes, for instance, to express that a pedestrian may or may not cross in front of the robot.

S-MPCC bounds the probability of collision in each time step with all obstacles through Chance-Constrained Optimization (CCO). The CCO is reformulated as an optimization without uncertainty by sampling trajectories from the predicted distribution, known as scenarios. Each scenario represents a possible position of all obstacles in one time step, and the planner avoids collisions with all scenarios. This Scenario Program (SP), through a tailored linearization, can be solved efficiently online. S-MPCC therefore plans probabilistic safe trajectories independent of the underlying distribution of the uncertainty.

S-MPCC considers the probability of collision separately for each time instance in the planned trajectory. The second contribution of this thesis, Safe Horizon Model Predictive Control (SH-MPC), builds on S-MPCC to constrain the joint probability of collision with all obstacles over the duration of the planned trajectory. Existing methods that separately constrain the probability of collision in each time step (temporal marginal) and with each obstacle (obstacle marginal) lead to overly cautious motion planning when safety constraints are enforced. SH-MPC formulates a single chance constraint to bound the overall probability of collision. This CCO is reformulated as an SP where each scenario represents a possible trajectory for all obstacles. To certify the joint probability of collision with the SP, the number of scenarios that affect the motion plan needs to be identified. SH-MPC estimates this quantity at a negligible computational cost during optimization. Consequently, SH-MPC plans trajectories in real-time under generic uncertainties that are less cautious than existing methods without compromising on safety.

The probabilistic safety of S-MPCC and SH-MPC is linked to the underlying accuracy of the prediction model of the obstacles that provide the scenarios. As a third contribution, a joint prediction and planning framework, Partitioned Scenario Replay (PSR), is proposed that replays past observations of human motion as scenarios for scenario-based planning. PSR does not fit a distribution on observed data but directly uses the data as empirical evidence of the underlying uncertainty and thereby provides a real-world safety guarantee.

A key limitation of the developed scenario-based planners and other optimization-based planners is that they locally refine an initial trajectory. This initial trajectory largely determines the quality of the final trajectory, while it does not consider other options. The fourth contribution of this thesis is Topology-driven Model Predictive Control (T-MPC) that concurrently optimizes trajectories, each attempting a different way to pass the obstacles. T-MPC is composed of a guidance planner and several parallel local planners. The guidance planner identifies guidance trajectories for several distinct maneuvers, relying on results from topology to distinguish trajectories. Each local planner is composed of an existing optimization-based planner (e.g., a scenario-based planner) and an additional set of constraints that are derived from one of the guidance trajectories. The guidance trajectories are optimized by the local planners in parallel, and the results are compared to determine which trajectory gets executed. T-MPC is faster, more consistent, and safer than several state-of-the-art planners. Contrary to similar existing work, it does not rely on an explicit lane structure and therefore enables both urban driving and mobile robotic applications.

The motion planners developed in this thesis are extensively validated in simulation and in experiments with a small-scale mobile robot and a full-scale self-driving vehicle navigating among pedestrians. The robot-agnostic implementation of the proposed planners that were developed for this thesis is available open source.