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

Automatic driving has received great attention from a broad of domains such as academia, industry, and government nowadays, while the subsystem of the path-planning for obstacle avoidance is crucial for the high-level automatic driving vehicle. This paper intends to present a novel optimal path planning method for obstacle avoidance on highways. At first, 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 to 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. Then a path selecting mechanism is built which considers a normalized weighted sum of the path length, curvature, heading error to the road centerline, the 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 corresponding 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 velocity profile along with the optimal path which is also based on polynomials respecting to the traveled distance is determined by the optimization technique, which incorporates the driving comfort and safety as the objective. Finally, several daily encountering scenarios for obstacle avoidance on different shapes of the highway are simulated to verify the effectiveness of the proposed framework.