  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 low-level 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 37ms, and the
experimental results showed the validity and effectiveness of the SMSTP
algorithm.
