A Hierarchical Motion Planning System for Driving in Changing Environments: Framework, Algorithms, and Verifications

Abstract

In this article, a hierarchical real-time motion planning system is proposed to solve complex navigation problem in realistic dynamic traffic environments. First, a longitudinal safety spacing model is established to describe the possible collision risk in the lane-changing (LC) process, and a piecewise linearization method is proposed to convert the nonlinear constraints into linear constraints. Second, a decision-making strategy is proposed to decide whether the trajectory should be replanned according to the real-time prediction of the surrounding environments, and select the optimal lane. Third, the quintic B-spline curve method and the quadratic programming method are integrated to obtain the optimal LC trajectory, considering factors of safety, comfort, and efficiency. The terminal position objective function based on the artificial potential field is designed to soften the hard security constraints and guide the LC trajectory toward the center of safety zone. Finally, a model predictive control method based on the kinematics vehicle model is utilized to track the planned trajectory. The experimental results of the miniature intelligent vehicle group verify the real-time performance and effectiveness of the proposed motion planning system.