Iterative Trajectory Optimization for Real-Time Motion Planner of Autonomous Driving

For autonomous driving, motion planning and motion control are two significant modules. In this paper, an iterative trajectory optimization (ITO) method is proposed to improve the motion controller's tracking performance and increase the physical and operational feasibility of the motion planning trajectory. Based on the proposed scheme, a feedback connection is built from the motion controller to the motion planner. Different from traditional motion controllers, the motion controller in the proposed scheme is divided into two sub-modules, iterative motion simulator, and motion control operator. In the iterative motion simulator, the vehicle trajectory response is simulated while updating a trajectory offset iteratively. This offset will correct the vehicle response closer to the reference trajectory. After the iterative trajectory adjustment finishes, the simulated tracking error and trajectory offset are sent back to motion planner. The motion planner will first evaluate the simulated trajectory in terms of the response effectiveness, and then send the trajectory with the offset to the motion control operator to calculate the vehicle control maneuver. Comparing with tradition motion planning schemes, the proposed ITO approach can guarantee the trajectory's physical and operational feasibility, make the motion controller have better tracking performance, and have a predictive evaluation on the vehicle response. The simulation results demonstrate the effectiveness of the proposed method.