Motion Planning for Autonomous Vehicles in Urban Scenarios: A Sequential Optimization Approach

Motion planning is essential for an autonomous vehicle to perform safe and humanlike driving behaviors, especially in highly dynamic scenarios such as dense urban and highway environments. The motion planning problem is challenging in that it needs to handle static and dynamic obstacles and obey kinematic and dynamic constraints as well as traffic rules. In this work, we propose an efficient hierarchical

motion planning approach based on path-speed decomposition. We find plan a path to avoid static obstacles, then generate a speed profile along the path to interact

with dynamic obstacles. The first sub-problem is path planning with static obstacles. It can be viewed as non-convex constrained nonlinear optimization, which requires a good enough initial guess to start and is often sensitive to algorithm parameters. We formulate it as convex spline optimization. The convexity of the formulated problem makes it able to be solved fast and reliably, while guaranteeing a global optimum. We then reorganize the constrained spline optimization into a recurrent formulation, which further reduces the computational time to be linear in the optimization horizon size. The second sub-problem is the minimum-time speed planning problem over a fixed path with dynamic obstacle constraints and point-wise speed and acceleration constraints. The contributions of our speed planning method are three-fold. First, we formulate the speed planning as an iterative convex optimization problem

based on space discretization. Our formulation allows imposing dynamic obstacle constraints and point-wise speed and acceleration constraints simultaneously. Second,

we propose a modified vertical cell decomposition method to handle dynamic obstacles. It divides the freespace into channels, where each channel represents a homotopy of free paths and defines convex constraints for dynamic obstacles. Third, we demonstrate significant improvement over previous work on speed planning for typical driving scenarios such as following, merging, and crossing.