Spatiotemporal Trajectory Planning for Autonomous Vehicle based on Reachable Set and Iterative LQR

Traditional decoupling trajectory planning strategy deals with complex urban road scenes with many shortcomings, such as stereotyped behavior and poor trajectory quality. Aiming at these problems, the paper proposes a combined spatiotemporal trajectory planning and control method based on reachable set and optimization technique. Firstly, to address the shortcomings of existing reachable set methods in terms of unreasonable risk characterization and lack of risk consideration in the spatiotemporal corridor generation process, a risk assessment model that takes into account the uncertainty of the predicted location distribution and a reachable set spatiotemporal corridor generation strategy that integrates the risk field are proposed. Secondly, a trajectory optimization strategy of rolling iterative optimization within the spatiotemporal corridor is designed by using the iterative linear quadratic regulator (ILQR) and taking into account the dynamics of the vehicle's transverse-longitudinal coupling. The optimization strategy ensures the dynamics of the trajectory is feasible and realizes the strong coupling of planning and control. Finally, a safety check module is designed to coordinate the time-consuming corridor generation and the real-time application of the algorithm. To verify the performance of the proposed method, the long-time simulation and real-vehicle comparison tests of complex stochastic interaction scenes are fully carried out. The test results show that the proposed method can effectively realize the coordination between driving efficiency, comfort, and safety in high-density complex traffic flow.