Trajectory Optimization for An Autonomous Vehicle Driving across Stochastic Traffic Flows based on Direct Collocation

Trajectory optimization is widely adopted in the control of autonomous vehicles, allowing them to drive on a road without collisions with obstacles and other vehicles on the road. However, different from the scenario of driving along a road, manipulating an autonomous vehicle to cross a road shows more challenges to solving the optimization problem. In this research, we adopt a method called direction collocation for solving this nonconvex trajectory optimization problem, where the goal of the autonomous vehicle is to drive across traffic flows from one side of the road to the other side, with randomly localized vehicles in four lanes coming in both directions. We present the dynamics of the autonomous vehicle as well as other vehicles on the road. Then we add constraints including no collisions, speed limit, and torque limit, adopting the fuel consumption as the cost. At last, we use direct collocation to compute the optimized trajectory with various traffic flow rates. It shows great robustness for the scheme to find the optimized solution with stochastic traffic flows.