Constrained Global Path Optimization for Articulated Steering Vehicles

This paper proposes a new efficient path-planning algorithm for articulated steering vehicles operating in semi-structured environments, in which obstacles are detected online by the vehicle's sensors. The first step of the algorithm is offline and computes a finite set of feasible motions that connect discrete robot states to construct a search space. The motion primitives are parameterized using Bezier curves and optimized as a nonlinear programming problem (NLP) equivalent to the constrained path planning problem. Applying the A* search algorithm to the search space produces the shortest paths as a sequence of these primitives. The sequence is drivable and suboptimal, but it can cause unnatural swerves. Therefore, online path smoothing, which uses a gradient-based method, is applied to solve another NLP. Numerical simulations demonstrate that performance of the proposed algorithm is significantly better than that of existing methods when determining constrained path optimization. Moreover, field experimental results demonstrate the successful generation of fast and safe trajectories for real-time autonomous driving.