Optimal Acceleration-Bounded Trajectory Planning in Dynamic Environments Along a Specified Path
Jeff Johnson and Kris Hauser. In ICRA 2012
Abstract. Vehicles that cross lanes of traffic encounter the problem of navigating around dynamic obstacles under actuation constraints. This paper presents an optimal, exact, polynomial-time planner for optimal bounded-acceleration trajectories along a fixed, given path with dynamic obstacles. The planner constructs reachable sets in the path-velocity-time (PVT) space by propagating reachable velocity sets between obstacle tangent points in the path-time (PT) space. The terminal velocities attainable by endpoint-constrained trajectories in the same homotopic class are proven to span a convex interval, so the planner merges contributions from individual homotopic classes to find the exact range of reachable velocities and times at the goal. A reachability analysis proves that running time is polynomial given reasonable assumptions, and empirical tests demonstrate that it scales well in practice and can handle hundreds of dynamic obstacles in a fraction of a second on a standard PC.
Download Planner Library (C++, 331KB): PVTPlanner
UPDATE: We have extended the planning system to generalize construction of PT obstacles and to perform velocity propagation with polygonal PT obstacles. These extensions allow the planner to solve more general path conflicts such as merging and vehicle following. Further information and code here: http://www.iu.edu/~motion/iv2013/
Planner-generated Trajectories in Lane Crossing Scenarios
Our planner is used to plan a safe, time-optimal trajectory across a fixed path (yellow) under dynamic constraints and in the presence of dynamic obstacles. Here the agent traverses 4 lanes of a divided roadway.
Path-time Space Representations of Lane Crossing Scenarios
The vehicle-centered path-time space view of the 4-lane scenario. The rectangles represent the computed path-time obstacles. The animation displays the contruction of the visibility graph, and the computation of the time-optimal trajectory.
The vehicle-centered path-time view of the 6-lane scenario. The rectangles represent the computed path-time obstacles. The animation displays the contruction of the visibility graph, and the computation of the time-optimal trajectory.
Assisted Collision Avoidance in Lane Crossing Scenario
In this simple scenario the control policy for the vehicle at each time step is full acceleration. Under this control policy collision occurs.
Because our planner is guaranteed to find a safe trajectory if one exists, it can be used to judge whether a given control will result in a goal-reachable, or safe, state. This is the same scenario as above with the addition of a collision avoidance system that utilizes our planner to evaluate a control before it is applied. If the control will result in an unsafe state, the system utilizes information provided by the planner to adjust the control such that the resulting state is safe.
Our planner is here used to provide collision-avoidance assistance to a human operator. In this scenario, a human operator attempts to navigate the fixed path across the lanes of traffic. At each time step, the user control is passed through a filter that utilizes the planner to determine whether the resulting state is safe. If the control is deemed safe, it is executed as-is. If the control is not deemed safe, it is adjusted such that the resulting state will be safe. When the vehicle is under human control, the control block on the GUI is green. When the vehicle is under planner control, the control block is red.