Bayesian view on Robot Motion Planning
Planning as Inference
High-dimensional motion planning algorithms are crucial to plan a robot trajectory in complex environment. In addition to collision avoidance and constraints handling capabilities, a key performance criterion is the computation time. Although a range of approaches exist for motion planning, the recent work[1,2] has sparked the interest in a potentially transformative approach, according to which robot motion planning can be accomplished through probabilistic inference[3,4].
The planning as inference (PAI) view originated in the artificial and machine learning research. PAI view has been adopted to solve planning and sequential decision-making problems in artificial agents and robotics. The key idea is that the PAI methods compute the posterior distribution over random variables subject to conditional dependencies in a joint distribution. In other words, in the PAI formulation, the planning objectives are represented as probabilistic models. Probabilistic inference is used to compute the posterior distribution over trajectories, given constraints and goals. All the motion objectives such as motion priors, goals and task constraints are fused together to find a posterior distribution over trajectories in a way similar to Bayesian sensor fusion. This PAI problem formulation enables the utility of the whole approximate inference techniques for a range of planning problems, and it provides certain benefits, such as uncertainty quantification, structured representation and faster convergence.
PAI framework is also closely related to the perception-action generative models originating in cognitive science research. The cognitive generative model provides a unified framework for perception, learning and planning, and it is named as active inference (AIF)[5]. The PAI framework also relates to stochastic optimal control and reinforcement learning.
Min-Sum Message Passing for Planning as Inference
Gaussian Process formulation of continuous-time trajectory[1] offers a fast solution to the motion planning problem via probabilistic inference on factor graph. However, often the solution converges to in-feasible local minima and the planned trajectory is not collision-free. It fuses all the planning problem objectives which are represented as factors and solves non-linear least square optimization problem via numerical (Gauss-Newton or Levenberg-Marquardt) methods. Although this approach of solving factor graph is fast, the batch non-linear least square optimization approach makes it vulnerable to converging to in-feasible local minima. The approach to combine all the factors of the graph makes it faster than state of the art motion planning algorithms, but it comes at the cost of being more prone to getting stuck in local minima. Graph re-optimization could naively help to get out of the in-feasible local minimain expense of additional computation time
TUM has proposed a message passing algorithm[6] that is more sensitive to obstacles with fast convergence time. We leverage the utility of min-sum message passing algorithm that performs local computations at each node to solve the inference problem on factor graph. We first introduce the notion of compound factor node to transform the factor graph to a linearly structured graph. The decentralized approach of solving each compound node increases sensitivity towards avoiding obstacles for complex planning problems.
PAI offers an interesting view on robot motion planning and how message passing algorithms can be adopted to solve the approximate inference problem[4,6]. However, a major drawback of this approach is its inability to handle hard constraints. Fusion of motion objectives only allow to handle soft constraints in the present problem formulation. For WEEE disassembly setup in HR-Recycler, this can cause safety issues as the PAI algorithm can generate trajectories that are not collision-free. In order to handle hard constraints, TUM is working on PAI based risk-aware algorithms that can generate safe planning algorithms for WEEE disassembly setup.
Fig. 1: Trajectory generated by min-sum message passing algorithm.
References
[1] M. Mukadam, J. Dong, X. Yan, F. Dellaert, and B. Boots, “Continuous-time Gaussian process motion planning via probabilistic inference,”Int. J. Robotics Res., vol. 37, no. 11, pp. 1319–1340, 2018.
[2] M. Xie and F. Dellaert, “Batch and incremental kinodynamic motion planning using dynamic factor graphs,”CoRR, vol. abs/2005.12514,2020.
[3] H. Attias, “Planning by probabilistic inference,” in Proceedings of the Ninth International Workshop on Artificial Intelligence and Statistics, AISTATS 2003, Key West, Florida, USA, January 3-6, 2003
[4] M. Toussaint and C. Goerick, “A bayesian view on motor control and planning,” in From Motor Learning to Interaction Learning in Robots, ser. Studies in Computational Intelligence, O. Sigaud and J. Peters,Eds. Springer, 2010, vol. 264, pp. 227–252.
[5] K. J. Friston, J. Mattout, and J. Kilner, “Action understanding and active inference,” Biol. Cybern., vol. 104, no. 1-2, pp. 137–160, 2011.
[6] S. Bari, V. Gabler, D. Wollherr, “MS2MP: A min-sum message passing algorithm for motion planning,” ICRA, 2021 [Accepted]
