A Dynamic Programming Approach to Safe Path Planning

This paper considers finding an optimal path for a robot from a given initial position to a final position. The dynamics of the robot is assumed to be a discrete LTI system with Gaussian process noise (to account for the uncertainties). The obstacles in the robot's configuration space are taken to be convex polytopes. The inputs that can be applied to the robot at each instant belongs to a known finite set. As the dynamics of the robot includes a Gaussian process noise, its motion is a Gaussian process with a mean dependent on the input applied at each instant. The probability of collision with the obstacles corresponding to each input is then computed for its feasibility. An algorithm resembling the dynamic programming is presented for growing a tree of state distributions. It is also shown that this tree contains the optimal path. Numerical simulations and hardware experiments (with a Kobuki robot) are performed for validating the algorithm.