Combining Lattice-Based Planning and Path Optimization in Autonomous Heavy Duty Vehicle Applications

Lattice-based motion planners are an established method to generate feasible motions for car-like vehicles. However, the solution paths can only reach a discretized approximation of the intended goal pose. Moreover, they can be optimal only with respect to the actions available to the planner, which can result in paths with excessive steering. These drawbacks have a negative impact when used in real systems. In this paper we address both drawbacks by integrating a steering method into a state-of-the-art lattice-based motion planner. Un- like previous approaches, in which path optimization happens in an a posteriori step after the planner has found a solution, we propose an interleaved execution of path planning and path optimization. The proposed approach can run in real-time and is implemented in a full-size autonomous truck, and we show experimentally that it is able to greatly improve the quality of the solutions provided by a lattice planner.