Motion planning with diffusion maps

Abstract

Many robotic applications require repeated, on-demand motion planning in mapped environments. In addition, the presence of other dynamic agents, such as people, often induces frequent, dynamic changes in the environment. Having a potential function that encodes pairwise cost-to-go can be useful for improving the computational speed of finding feasible paths, and for guiding local searches around dynamic obstacles. However, since storing pairwise potential can be impractical given the O(|V|²) memory requirement, existing work often needs to compute a potential function for each query to a new goal, which would require a substantial online computation. This work addresses the problem by using diffusion maps, a machine learning algorithm, to learn the map's geometry and develop a memory-efficient parametrization (O(|V|)) of pairwise potentials. Specially, each state in the map is transformed to a diffusion coordinate, in which pairwise Euclidean distance is shown to be a meaningful similarity metric. We develop diffusion-based motion planning algorithms and, through extensive numerical evaluation, show that the proposed algorithms find feasible paths of similar quality with orders of magnitude improvement in computational speed compared with single-query methods. The proposed algorithms are implemented on hardware to enable real-time autonomous navigation in an indoor environment with frequent interactions with pedestrians.