Inverse Dynamics-Based Motion Planning for Autonomous Vehicles: Simultaneous Trajectory and Speed Optimization with Kinematic Continuity

This article presents an alternative variant of motion planning techniques for autonomous vehicles (AVs) centered on an inverse approach that concurrently optimizes both trajectory and speed. This method emphasizes searching for a trajectory and distributing its speed within a single road segment, regarded as a final element. The references for the road lanes are represented by splines that interpolate the path length, derivative, and curvature using Cartesian coordinates. This approach enables the determination of parameters at the final node of the road segment while varying the reference length. Instead of directly modeling the trajectory and velocity, the second derivatives of curvature and speed are modeled to ensure the continuity of all kinematic parameters, including jerk, at the nodes. A specialized inverse numerical integration procedure based on Gaussian quadrature has been adapted to reproduce the trajectory, speed, and other key parameters, which can be referenced during the motion tracking phase. The method emphasizes incorporating kinematic, dynamic, and physical restrictions into a set of nonlinear constraints that are part of the optimization procedure based on sequential quadratic optimization. The objective function allows for variation in multiple parameters, such as speed, longitudinal and lateral jerks, final time, final angular position, final lateral offset, and distances to obstacles. Additionally, several motion planning variants are calculated simultaneously based on the current vehicle position and the number of lanes available. Graphs depicting trajectories, speeds, accelerations, jerks, and other relevant parameters are presented based on the simulation results. Finally, this article evaluates the efficiency, speed, and quality of the predictions generated by the proposed method. The main quantitative assessment of the results may be associated with computing performance, which corresponds to time costs of 0.5–2.4 s for an average power notebook, depending on optimization settings, desired accuracy, and initial conditions.