Nonholonomic Motion Planning Using Diffusion of Workspace Density Functions

This paper introduces a trajectory planning algorithm for nonholonomic mobile robots which operate in an environment with obstacles. An important feature of our approach is that the planning domain is the workspace of the mobile robot rather than its configuration space. The basic idea is to imagine the robot being subjected to Brownian motion forcing, and to generate evolving probability density functions (PDF) that describe all attainable positions and orientations of the robot at a given value of time. By planning a path that optimizes the value of this PDF at each instant in time, we generate a feasible trajectory. The PDF of robot pose can be constructed by solving the corresponding Fokker-Planck equation using the Fourier transform for SE(N). A closed-form approximation of the resulting time-dependent PDF is then used to plan a trajectory based on the observation that the evolution of this “workspace density” is a diffusion process. Examples are provided to illustrate the algorithm.