Trajectory Planning with Shadow Trolleys for an Autonomous Vehicle on Bending Roads and Switchbacks

In automated driving, the road geometry information such as waypoints is usually available from previously stored maps. In this paper, we present a scenario based Model Predictive Control (MPC) trajectory planning algorithm that consists of spatial planning with embedded temporal optimization that leverages waypoints information of the road. Our trajectory planning algorithm is structured such that the spatial and temporal planning is integrated so that both the longitudinal and lateral aspects, reflected in the shape and length of the planned trajectory, are dynamically changing to best negotiate the constraints from road curvature and surrounding vehicles. The concept of a vehicle connected to shadow trolleys traveling along the rails on the road is introduced. Given waypoints, a reference cubic spline can be constructed to define the rail for trolleys and form a curvilinear coordinate. This concept facilitates the description of vehicle motion, trajectories, and surroundings with respect to the trolley, which is especially convenient for a vehicle traveling on high curvature road and switchbacks. A temporal optimization of the trolley instances is first conducted, which in turn allows us to make a proper approximation and reduce an originally complex nonlinear spatial-temporal optimization problem into one that requires only Quadratic Programming (QP). We present simulation results of various challenging scenarios on complex road geometry with multiple surrounding vehicles of varying behavior to demonstrate the effectiveness and efficiency of the proposed algorithm. Simulation results show reasonable, flexible, and safe maneuvers.