Exploiting group symmetries to improve precision in kinodynamic and nonholonomic planning

We address the problem of eliminating gaps in paths that are constructed by some nonholonomic and kinodynamic motion planning algorithms. In many of these algorithms, control inputs at each planning step are chosen from a finite set, obtained from discretization of the available control set. While this approach is attractive for computational reasons, it can generate gaps, or discontinuities, either between path segments or between the final state and the desired goal. For the purpose of reducing gaps, the original control set and continuous time interval can be utilized, and perturbations may be applied to incrementally optimize the gap error while respecting collision constraints. By exploiting Lie group symmetries, which emerge in a broad class of robot systems, we are able to avoid costly numerical integrations that usually occur in each step of gradient-based optimization techniques. It is hoped that the approach can ultimately lead to faster planning algorithms by allowing coarser discretizations of time and the available input set, with the understanding that later refinements can be made efficiently.