A Dynamic Programming Approach to Optimal Planning for Vehicles with Trailers

In this paper we deal with the optimal feedback synthesis problem for robotic vehicles with trailers which can be modeled by differential equations in chained-form. With respect to classical methods for numerical evolution of optimal feedback synthesis via dynamic programming which are based on both input and state discretization, our method exploits the lattice structure naturally imposed on the reachable set by input quantization. A generalized Dijkstra algorithm can be used to obtain sub-optimal (optimal up to the lattice resolution) feedback laws, for chained-form vehicles with n-trailers, in an effective way.