  Multi-modal behaviors exhibited by surrounding vehicles (SVs) can typically
lead to traffic congestion and reduce the travel efficiency of autonomous
vehicles (AVs) in dense traffic. This paper proposes a real-time parallel
trajectory optimization method for the AV to achieve high travel efficiency in
dynamic and congested environments. A spatiotemporal safety module is developed
to facilitate the safe interaction between the AV and SVs in the presence of
trajectory prediction errors resulting from the multi-modal behaviors of the
SVs. By leveraging multiple shooting and constraint transcription, we transform
the trajectory optimization problem into a nonlinear programming problem, which
allows for the use of optimization solvers and parallel computing techniques to
generate multiple feasible trajectories in parallel. Subsequently, these
spatiotemporal trajectories are fed into a multi-objective evaluation module
considering both safety and efficiency objectives, such that the optimal
feasible trajectory corresponding to the optimal target lane can be selected.
The proposed framework is validated through simulations in a dense and
congested driving scenario with multiple uncertain SVs. The results demonstrate
that our method enables the AV to safely navigate through a dense and congested
traffic scenario while achieving high travel efficiency and task accuracy in
real time.
