Probabilistic Inference-Based Robot Motion Planning via Gaussian Belief Propagation

Robot motion planning via probabilistic inference renders a unique viewpoint on the trajectory optimization problem, in which the joint distribution of motion objectives is represented as a factor graph. Thus, the objectives are solved by obtaining the Maximum a Posteriori of the factor graph. While this distinctly improves the computational efficiency by applying least square optimization, the approach is incapable of handling hard constraints directly. In this work, we put forth an alternate perspective and argue that a message passing framework, such as Belief Propagation, offers greater utility as a solution method for robot planning problems. We present the theoretical formulation of Gaussian Belief Propagation (GaBP) as a generic message passing framework that exploits the structure of the factor graph to solve multiple planning scenarios such as batch planning, incremental planning and re-planning. In addition, the GaBP algorithm has been extended to handle hard state constraints by adopting the Difference Map strategy. We benchmark our framework in a simulation environment. The results show that our algorithms outperform the state-of-the-art with respect to collision avoidance and constraint handling ability within our benchmark. We close this article with the outline of a real-world robotic application within industrial disassembly.