Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants

Abstract

The paper presents a technique of motion planning for autonomous vehicles (AV) based on simultaneous trajectory and speed optimization. The method includes representing the trajectory by a finite element (FE), determining trajectory parameters in Frenet coordinates, composing a model of vehicle kinematics, defining optimization criteria and a cost function, forming a set of constraints, and adapting the Gaussian N-point scheme for quadrature numerical integration. The study also defines a set of minimum optimization parameters sufficient for making motion predictions with smooth functions of the trajectory and speed. For this, piecewise functions with three degrees of freedom (DOF) in FE’s nodes are implemented. Therefore, the high differentiability of the trajectory and speed functions is ensured to obtain motion criteria such as linear and angular speeds, acceleration, and jerks used in the cost function and constraints. To form the AV roadway position, the Frenet coordinate system and two variable parameters are used: the reference path length and the lateral displacement perpendicular to reference line’s tangent. The trajectory shape, then, depends only on the final position of the AV’s mass center and the final reference’s curvature. The method uses geometric, kinematic, dynamic, and physical constraints, some of which are related to hard restrictions and some to soft restrictions. The planning technique involves parallel forecasting for several variants of the AV maneuver followed by selecting the one corresponding to a specified criterion. The sequential quadratic programming (SQP) technique is used to find the optimal solution. Graphs of trajectories, speeds, accelerations, jerks, and other parameters are presented based on the simulation results. Finally, the efficiency, rapidity, and prognosis quality are evaluated.