Improved Technique for Autonomous Vehicle Motion Planning Based on Integral Constraints and Sequential Optimization

The study is dedicated to elaborating and analyzing a technique for autonomous vehicle (AV) motion planning based on sequential trajectory and kinematics optimization. The proposed approach combines the finite element method (FEM) basics and nonlinear optimization with nonlinear constraints. There were five main innovative aspects introduced in the study. First, a 7-degree polynomial was used to improve the continuity of piecewise functions representing the motion curves, providing 4 degrees of freedom (DOF) in a node. This approach allows using the irregular grid for roadway segments, increasing spans where the curvature changes slightly, and reducing steps in the vicinity of the significant inflections of motion boundaries. Therefore, the segment length depends on such factors as static and moving obstacles, average road section curvature, camera sight distance, and road conditions (adhesion). Second, since the method implies splitting the optimization stages, a strategy for bypassing the moving obstacles out of direct time dependency was developed. Thus, the permissible area for maneuvering was determined using criteria of safety distance between vehicles and physical limitation of tire-road adhesion. Third, the nodal inequality constraints were replaced by the nonlinear integral equality constraints. In contrast to the generally distributed approach of restricting the planning parameters in nodes, the technique of integral equality constraints ensures the disposition of motion parameters' curves strictly within the preset boundaries, which is especially important for quite long segments. In this way, the reliability and stability of predicted parameters are improved. Fourth, the seamless continuity of both the sought parameters and their derivatives is ensured in transitional nodes between the planning phases and adjacent global coordinate systems. Finally, the problem of optimization rapidity to match real-time operation requirements was addressed. For this, the quadrature integration approach was implemented to represent and keep all the parameters in numerical form. The study considered cost functions, limitations stipulated by the vehicle kinematics and dynamics, as well as initial and transient conditions between the planning stages. Simulation examples of the predicted trajectories and curves of kinematic parameters are demonstrated. The advantages and limitations of the proposed approach are highlighted.