Reference path optimization for autonomous ground vehicles driving in structured environments

This paper focuses on the reference path optimization problem for Autonomous Ground Vehicles driving in structured environments. The reference path optimization algorithm is composed of three steps: data acquisition and curvature calculation, initializing straights of the reference path, using path primitives to connect the turn between the straights. In the process of data acquisition, we get the position of trajectory point and the motion data of vehicle through a probe vehicle equipped with a variety of on-board sensors. Then, the curvature information can be found using the relationship between the curvature and the steering angle of the probe vehicle, and the reference path is decomposed into a straight portion and a turn portion. In the initializing straights step, straights are fitted using the least squares method. Finally, we use clothoid curve as a path primitive to fit the remainder of the reference path. And the gradient descent method is used to improve the accuracy of the fitting path. The experimental results show that the proposed algorithm can effectively eliminate the position, heading angle and curvature noise in the reference path while greatly reducing the memory storage for saving road geometry.