Optimal longitudinal control planning with moving obstacles

At intersections and in merging traffic, intelligent road vehicles must solve challenging optimal control problems in real-time to navigate reliably around moving obstacles. We present a complete planner that computes collision-free, optimal longitudinal control sequences (acceleration and braking) using a novel visibility graph approach that analytically computes the reachable subset of path-velocity-time space. We demonstrate that our method plans over an order of magnitude faster than previous approaches, making it scalable and fast enough (tenths of a second on a PC) to be called repeatedly on-line. We demonstrate applications to autonomous driving and vehicle collision warning systems with many moving obstacles.