An Optimized Trajectory Planner and Motion Controller Framework for Autonomous Driving in Unstructured Environments

This paper proposes an optimized trajectory planner and motion planner framework, which aim to deal with obstacle avoidance along a reference road for autonomous driving in unstructured environments. The trajectory planning problem is decomposed into lateral and longitudinal planning sub-tasks along the reference road. First, a vehicle kinematic model with road coordinates is established to describe the lateral movement of the vehicle. Then, nonlinear optimization based on a vehicle kinematic model in the space domain is employed to smooth the reference road. Second, a multilayered search algorithm is applied in the lateral-space domain to deal with obstacles and find a suitable path boundary. Then, the optimized path planner calculates the optimal path by considering the distance to the reference road and the curvature constraints. Furthermore, the optimized speed planner takes into account the speed boundary in the space domain and the constraints on vehicle acceleration. The optimal speed profile is obtained by using a numerical optimization method. Furthermore, a motion controller based on a kinematic error model is proposed to follow the desired trajectory. Finally, the experimental results show the effectiveness of the proposed trajectory planner and motion controller framework in handling typical scenarios and avoiding obstacles safely and smoothly on the reference road and in unstructured environments.