Research on two stage obstacle-avoidance trajectory planning and trajectory tracking control in curves

Abstract

The existing obstacle-avoidance trajectory planning and trajectory tracking control algorithms have limitations such as long-time consumption, high failure rate in dynamic traffic environments, and insufficient trajectory tracking accuracy in curved roads. Based on the above problems, this paper designs a two stage obstacle-avoidance trajectory planner based on nonlinear optimization theory. In first stage Part-NLP, only considering the safety obstacle avoidance, a point mass model and linearization constraints are established to quickly solve the initial trajectory. In the second stage Full-NLP, considering smooth soft constraints comprehensively, the initial trajectory is optimized by establishing driving corridors and a lightweight iterative framework. In control module, this paper selects a linear quadratic form lateral trajectory tracking controller, and the parameters were optimized through the carnivorous plant algorithm. The joint simulation results show that in dynamic traffic environment of curved roads, the two stage planner proposed can accurately plan safe and smooth obstacle avoidance trajectories, and there is a significant reduction in time consumption compared to traditional NLP algorithms. The control strategy can accurately track the planned trajectories, with lateral error controlled within plus or minus 0.1 m, heading error controlled within plus or minus 0.15 rad, speed tracking error controlled within plus or minus 0.15 m/s, and vehicle yaw angle error controlled within plus or minus 0.04 rad; the hardware-in-loop test results indicate that the controller can achieve real-time and accurate trajectory tracking.