Synchronous Maneuver Searching and Trajectory Planning for Autonomous Vehicles in Dynamic Traffic Environments

In the real-time decision-making and local planning process of autonomous vehicles in dynamic environments, the autonomous driving system may fail to find a reasonable policy or even gets trapped in some situation due to the complexity of global tasks and the incompatibility between upper level maneuver decisions with the lower level trajectory planning. To solve this problem, this paper presents a synchronous maneuver searching and trajectory planning (SMSTP) algorithm based on the topological concept of homotopy. Firstly, a set of alternative maneuvers with boundary limits are enumerated on a multi-lane road. Instead of sampling numerous paths in the whole spatio-temporal space, we, for the first time, propose using Trajectory Profiles (TPs) to quickly construct the topological maneuvers represented by different routes, and put forward a corridor generation algorithm based on graph-search. The bounded corridor further constrains the maneuver's space in the spatial space. A step-wise heuristic optimization algorithm is then proposed to synchronously generate a feasible trajectory for each maneuver. To achieve real-time performance, we initialize the states to be optimized with the boundary constraints of maneuvers, and we set some heuristic states as terminal targets in the quadratic cost function. The solution of a feasible trajectory is always guaranteed only if a specific maneuver is given. The simulation and realistic driving-test experiments verified that the proposed SMSTP algorithm has a short computation time which is less than 37 ms, and the experimental results showed the validity and effectiveness of the SMSTP algorithm.