Improving the performance of sampling-based motion planning with symmetry-based gap reduction

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 perturbed trajectories. Simulation results demonstrate dramatic performance improvement for unidirectional, bidirectional, and PRM-based sampling-based algorithms with the proposed enhancement with respect to their basic counterparts on different systems: one with the second-order dynamics, one with nonholonomic constraints, and one with two different modes.