Spatial Optimization in Spatio-temporal Motion Planning

Motion Planning is one of the key modules in autonomous driving systems to generate trajectories for self-driving vehicles. Spatio-temporal motion planners are often used to tackle complicated and dynamic driving scenarios. While effective in dealing with temporal changes in the environment, the existing methods are limited to optimizing a particular family of cost functions defined based on decoupled longitudinal and lateral terms. However, the planning objectives can only be explained using coupled terms in some cases, e.g. closeness to the reference path, lateral acceleration, and heading rate. The limitation arises from expressing such objectives as linear and quadratic terms suitable for optimization. This paper proposes an approach with theoretical proofs to approximate the upper bound of a given couple, nonlinear cost term with a set of uncoupled terms, allowing for converting the planning optimization problem into a linear quadratic optimization. The effectiveness of the proposed approach is shown through a series of simulated scenarios. The proposed approach results in smoother and steadier trajectories in the spatial plane.