Toward High Automatic Driving by a Dynamic Optimal Trajectory Planning Method Based on High-Order Polynomials

This paper intends to present a novel optimal trajectory planning method for obstacle avoidance on highways. Firstly, a mapping from the road Cartesian coordinate system to the road Frenet-based coordinate system is built, and the path lateral offset in the road Frenet-based coordinate system is represented by a function of quintic polynomial respecting the traveled distance along the road centerline. With different terminal conditions regarding its position, heading and curvature of the endpoint, and together with initial conditions of the starting point, the path planner generates a bunch of candidate paths via solving nonlinear equation sets numerically. A path selecting mechanism is further built which considers a normalized weighted sum of the path length, curvature, consistency with the previous path, as well as the road hazard risk. The road hazard is composed of Gaussian-like functions both for the obstacle and road boundaries, which means, if one path is near the obstacle or road boundaries, the driving risk would become large and the path would not be preferred chosen. Then the optimal collision-free path would be transformed back to the road Cartesian coordinate system and used for tracking by the path following module. Moreover, the speed profile along with the optimal path which is also based on polynomials respecting the traveled distance is determined by the multi-object optimization technique, which incorporates the driving comfort and safety simultaneously. Finally, several scenarios for obstacle avoidance on different shapes of the highway are simulated to verify the effectiveness of the proposed framework.