Sampling-based nonholonomic and kinodynamic planning iteratively constructs solutions with sampled controls. A constructed trajectory is returned as an acceptable solution if its "gaps", including discontinuities within the trajectory and mismatches between the terminal and goal states, are within a given gap tolerance. For a given coarseness in the sampling of the control space, finding a trajectory with a small gap tolerance might be either impossible or extremely expensive. In this paper, we propose an efficient trajectory perturbation method, which complements existing steering and perturbation methods, enabling these sampling-based algorithms to quickly obtain solutions by reducing large gaps in constructed trajectories. Our method uses system symmetry, e.g., invariance of dynamics with respect to certain state transformations, to achieve efficient gap reduction by evaluating trajectory final state with a constant-time operation and naturally generating the admissible pe...
Peng Cheng, Emilio Frazzoli, Steven M. LaValle