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.

the trajectory has to be re-planned according to the partially changed planning conditions, e.g. change in goal position during robot motion. Re-planning requires only a partial update in the planned trajectory. These different planning scenarios have been mostly considered as separate planning problems in literature and specific algorithms have been developed for each case. Our study tackles the problem of multi-scenario robot planning and explores the message passing framework for finding Maximum a Posteriori (MAP) estimation to generate optimal trajectories.
Robot motion planning can be formulated as planning-asinference (PAI), which orchestrates the planning problem on a graphical model and then utilizes the already existing and established algorithms for finding MAP [5], [6]. The PAI offers the utility of approximate inference algorithms to compute fast solution as compared to other optimization and sampling-based planning approaches [7]. The computation of the MAP in a factor graph using message passing approaches, such as Belief Propagation (BP) [8], involves local communication among nodes in the graph to determine marginal distributions of variable nodes. This characteristic of message passing algorithms can be harnessed to develop motion planning algorithms that can be employed in different planning scenarios.
In this letter, we explore the potential of GaBP as a message passing framework for solving robot motion planning problems. We also address one of the major limitations of Gaussian Process Motion Planning (GPMP2), its incapability to handle hard constraints. We begin by outlining the theoretical framework for solving batch planning problems. Then, a novel method to enforce hard state constraints in configuration space has been proposed. This is achieved by applying the Difference Map (DM) approach to satisfy constraints via bound projections. Moreover, conditional dependencies in the factor graph nodes have been highlighted, which results in a clear distinction between local and global graph reasoning. Then, the property of local convergence is exploited to formulate local sub-graphs. Based on these sub-graphs, GaBP has been extended to Incremental Gaussian Belief Propagation (iGaBP) to address incremental planning and re-planning problems.
We evaluate the proposed algorithm in an exemplary simulation setting using a 6-DoF COMAU robot manipulator. A comparative analysis with state-of-the-art probabilistic inference and optimization-based motion planning algorithms has been performed. The results show the superior performance of the proposed approach in simulated planning scenarios with respect to success rate and constraint handling. In addition, we showcase a real-world application example for an industrial Waste Electrical and Electronic Equipment (WEEE) disassembly setting and discuss the key findings along with limitations and future work.

II. RELATED WORK
Planning-as-inference has been used to solve planning and sequential decision-making problems [9], [10]. Eventually, the same idea was adopted by the robotics community to solve trajectory optimization and related motion planning problems [5]. In this approach, a multi-variate description of the robot action or state is represented as a factor graph and MAP estimate is computed based on the desired planning objective [6]. Recently, the GPMP2 framework [7] has been proposed which represents the prior trajectory as a Gaussian Process (GP) generated from a linear time variant, stochastic differential equation (LTV-SDE), and finds collision-free trajectories with fast numerical least square optimization. MAP inference of probabilistic graphical models is equivalent to least square optimization solution as per inference-optimization duality [11]. In order to tackle re-planning scenario, GPMP2 proposes Bayes tree formulation to perform incremental trajectory updates. Recently, a stochastic interpretation of GPMP2 has also been proposed [12] that takes into account uncertainties by means of Gaussian variational inference. However, GPMP2 does not fully exploit the underlying structure of the factor graph as it considers the solution of the entire factor graph holistically by fusing all motion objectives. The fusion of motion objectives also limits the ability of this approach to deal with hard constraints.
The application of the message passing approach as a solution method for a motion planning problem has not yet been studied in detail. Early work on robot planning via probabilistic inference [5], [6] adopts a similar message passing approach but it does not provide detailed investigation of its utility. More recently, the GaBP-based inference approach [13] has been used to solve a multi-robot planning problem. The proposed algorithm is very similar to our work as it also employs GaBP but multiple planning scenarios are not investigated. We fill this research gap by complementing [13] and discussing the GaBP approach that exploits factor graph structure to generate trajectories in multiple planning scenarios, which differs from multimodel trajectory optimization [14] that generates multiple solutions based on various cost functions.
We rely on Danny Bickson's GaBP algorithm [15] to solve non-linear factor graph for batch planning. We leverage the insights from our earlier work [16], which suggested that optimizing the local nodes of the factor graph offers greater sensitivity to obstacle avoidance. However, it should be noted that the approach proposed in our previous work [16], which utilized the min-sum message passing algorithm and numerical optimization for local node belief computation, differs from the approach we present in this letter. We also propose the sub-graph formulation and incremental inference via GaBP which leads to a message passing approach capable of planning in various real-world scenarios.
In addition, we address hard constraints handling using GaBP. Typically, constraints can be modeled in soft manner as proposed in GPMP2 or the joint limits can be imposed by clamping such that each optimization update follows the limits as proposed in [17]. While recent work [18], [19] has targeted the constrained optimization solution of factor graphs, they still rely on traditional constrained optimization approaches similar to optimization-based planning algorithms [20]. In context of constraint satisfaction via message passing, Difference-map Belief Propagation (DMPB) [21] has been proposed for low-density parity-check codes in signal processing research based on DM and divide-and-concur methodology [22]. In this letter, we apply DM principles to propose bound projection to deal with jointlimits in robot planning.

III. PRELIMINARIES AND BACKGROUND
Robot trajectory is represented as θ(t) : t → R D , where D is the dimensionality of the state and θ(t) is a continuous time function that maps time t to robot joint states θ in configuration space. Following Mukadam et al. [7], robot trajectory is sampled from a continuous time GP, that is parameterized by N states for a set of times t = t 0 , . . . , t N , where μ(t) is the mean vector and The robot planning problem is characterized as finding the robot trajectory θ * (t) ∈ χ free . The state constraints are represented as θ min and θ max .

A. Trajectory Optimization as Probabilistic Inference
Formally, we are interested in finding the optimal state sequence θ * 0:N that maximizes the conditional posterior p(θ|e). The prior on the trajectory is defined as p(θ) that incorporates initial belief over θ. Whereas, l(θ; e) is the likelihood of states θ with respect to conditional events e on the trajectory such as collision avoidance and constraints. Given the prior and likelihood, the optimal trajectory θ * is computed by obtaining MAP estimation, The robot motion planning problem formulation in (2) consists of two components, prior and likelihood. Adopting the same methodology as in [7] to define the prior as a structured kernel generated from LTV-SDE, the GP prior of θ results in where θ − μ 2 K is the Mahalanobis distance. The kernel K induces smoothness and it can also be used to put soft constraints on start state θ start and goal state θ goal . The likelihood function l(θ; e) which specifies the probability of avoiding obstacles is defined as where h(θ) represents the vector-valued obstacle cost, and Σ obs is a hyperparameter matrix.

B. Factor Graph Formulation
A factor graph G = (Θ, F, E) is used to model a belief over continuous, multivariate random variables θ with respect to conditional probability density functions (PDFs) where f m ∈ F are factors that are attached to the corresponding variable nodes Θ m ∈ Θ via the edges E of the factor graph. The trajectory prior (3) is formulated as prior factors where f p 0 (θ 0 ) and f p N (θ N ) act as soft constraint factors that provide the functionality of θ * 0 = θ start and θ * N = θ goal . The GP prior factor f GP i is defined as where Φ(t i , t i+1 ) is a state transition matrix and Q i,i+1 represents power spectral density matrix (see [23] for details). Similarly, the collision-free likelihood (4) is factorized via unary factors f obs Finding the most probable values for θ from a factor graph is, This factor graph formulation as shown in Fig. 1 is a structured representation of the planning problem, where the system state is represented as number of state variables. An inference algorithm on the factor graph is used to compute the posterior distribution over all trajectories fulfilling the planning objectives imposed through attached factors.

IV. GAUSSIAN BELIEF PROPAGATION FOR ROBOT PLANNING
In this section, we outline the equations for GaBP algorithm to compute MAP in (9). We first present the theoretical formulation of our GaBP-based batch planning algorithm that can handle hard bounded-variable constraints. Then, iGaBP has been proposed to generate trajectories for incremental planning and re-planning scenarios.

A. GaBP as a Solution Method for Batch Planning
GaBP is a message passing framework [8] for computing the marginals of a joint distribution via local communication among nodes in a factor graph. The factor graph in (9) contains non-linear factors restricting the direct applicability of GaBP, which by design solves linearized factor graph. Therefore, we outline an iterative strategy for solving linearized factor graph. Following Bickson's linear GaBP [15] and using similar notations, we start with the assumption that the planning factor graph only contains unary and binary factors.
Assumption 1: A robot planning problem can be fully described by a pair-wise factor graph G, which consists of two kinds of factors. Unary factors (also called self-potentials), φ i : Θ → R ∪ {∞} that are attached to corresponding variable θ i , and binary factors (also called edge potentials) ψ ij : Message passing equations for the pair-wise factor graph can be divided into two types of messages. Messages passed from variables to factor nodes, denoted as m θ →f , and messages passed form factor nodes to variable nodes, denoted as m f →θ . Denoting the adjacent nodes of a factor node as Θ f and the adjacent nodes of a variable node as F θ , the messages from an arbitrary factor f k are iteratively communicated as where \ denotes the set-theoretic difference and θ j is the variable node adjacent to θ i . In case of pair-wise factor graphs θ j = θ i±1 . The messages traversing from variable to factor node is Given the messages from adjacent factor nodes, the marginal belief of a variable node is approximated via Following [15], we use the canonical representation to derive the equations for messages among nodes and marginal means. As all the factors in the graph (9) have the quadratic energy function represented in exponential form, edge potentials ψ ij and self potentials φ i can be written in canonical form as where Λ is the data matrix also called precision matrix and η is the observation matrix. Similarly, the alternate representation of the factor graph is where mean μ and precision Λ are related by η = Λμ.
As the obstacle factor function h(θ) is non-linear, the energy is not quadratic in θ, meaning the factor is not Gaussian distributed. To approximate the Gaussian form of the factor graph, a first order Taylor series expansion is applied for non-linear factors f (θ) to find its approximate values θ close toθ where J is the Jacobian which is a multi-variate partial derivative ∂h ∂θ | θ=θ at the linearized stateθ with δθ Δ = (θ −θ) being the state update vector. Then, the linear form in terms of information and precision matrix is, Finding the MAP estimation in the linearized graph Λδθ − η translates to solving the vector-matrix linear equations [15] as, The structure of the matrix Λ is dictated by the graph topology. As per Bickson's methodology [15], the messages among nodes are proportional to normal distributions with precision as, The (19) represents the messages propagated in the GaBP algorithm. The marginals are computed as follows (cf. [15], Sec.

2.3)
, Similarly, the precision can be calculated as The solution for the non-linear factor graph is then updated according to the linear graph solution δθ as In order to impose hard constraints to the factor graph solution in (22), we introduce a constraint graph as shown in Fig. 2. We leverage the constraint satisfaction methodology from DM, which is an approach based on divide-and-concur projections [22] for constraint satisfaction that has been extended to message passing framework DMPB [21]. In this approach, for each constraint imposed on a variable, a replica of that variable is created in an updated constraint factor graph [21] and then DM-based divide-and-concur approach is used for constraint satisfaction. Instead of divide-and-concur projection as in [21], we propose the bound projection denoted by P B . The messages from constraint factor nodes to variable nodes are updated according to where, the P B represents the bound projection and defined as, Algorithm 1 entails the details of solving batch planning problems including constraint handling via bound projection. Note that the constraint factors are not Gaussian and these are not included in the message passing iterations of belief update. The major benefit of the proposed approach is that the DM methodology step of the GaBP algorithm is only activated in case of constraint violation making the overall approach more efficient. The algorithm proposed in this section solves the complete factor graph, however, an incremental strategy can also be devised (next Sec.) that can solve the factor graph incrementally.

B. Incremental Inference Via GaBP
In this section, we exploit the local convergence characteristic of GaBP for finding an incremental solution of the factor graph. Due to the factorized structure of the planning problem, global inference can be achieved by individually solving many interdependent local sub-problems [16]. We propose the formation of local sub-graphs and solving them incrementally for individual variable nodes that leads to global MAP estimation of the entire factor graph. The sub-graph for θ i is, (25) Fig. 3 shows the proposed sub-graph structure for finding local MAP estimation for θ i . In order to obtain θ * i , the variable nodes Θ sub = {θ i−1 , θ i , θ i+1 } are initialized according to (3). Although the value of θ i+1 is also partially optimized, we consider its value to be initialized from prior p(θ) as it helps in inducing smoothness as per GP prior. The solution for the sub-graph is obtained from GaBP as outlined in Algorithm 1. However, it is an independent node optimization. Solving individual sub-graphs incrementally leads to full graph optimization resulting in θ * (for detailed convergence analysis, cf. [16], Appendix A).
For incremental planning, the key idea is to optimize only the next variable node instead of the entire factor graph. As a result, only one graph node θ i is optimized at a time-step and the full optimized trajectory θ * is generated incrementally. Note that the proposed incremental inference approach iGaBP is different from GaBP; the batch optimization of the complete graph is replaced by an iterative local optimization of each node that has the capability to cater moving obstacles in a dynamic environment. iGaBP can also be used for re-planning scenario.
A naive way to solve the re-planning problem in case of partial modifications in planning conditions is to solve the entire factor graph. However, it is an in-efficient approach for real-world robotic applications. iGaBP offers the functionality to generate trajectory conveniently by simply updating the graph settings to changes and solving only the local sub-graph.

V. IMPLEMENTATION DETAILS
The GaBP-based message passing framework has been implemented on top of Georgia Tech Smoothing and Mapping (GTSAM) library similar to GPMP2. The implemented algorithm has two layers: the top layer is programmed in MATLAB and incorporates robot kinematic model, obstacles and trajectory characteristics, the lower-level GaBP solver is implemented in C++, augmenting the GTSAM library.

A. Prior and Likelihood
We use a constant-velocity prior similar to [7] with a Markovian state consisting of position and velocity in configuration space. The trajectory is generated from LTV-SDE [23]. Similar to recent state-of-the-art motion planning algorithms [2], [7], the robot collision body is represented by a set of spheres. The obstacle cost function is obtained by computing hinge loss for the spheres. d represents the signed distance from the sphere to the closest obstacle surface in the workspace, and ε is the safety distance. For likelihood in (4), the Σ obs is defined as The trajectory prior is initialized as a constant velocity straight line with N = 11 states from the start state to the goal state. The term σ obs puts weight on observing the obstacles. We set σ obs = 0.0015 and ε = 0.2 for our manipulator planning tasks.

B. Motion Constraints
Motion constraint handling is very crucial in real-world motion planning tasks. There are two kinds of constraints that we encounter in real-world implementation of motion planning algorithms. We handle the equality constraints in a soft manner similar to [7] by imposing them directly into the trajectory prior. To prevent joint limit violations, we use the GaBP as outlined in Section IV-A.

VI. EVALUATION
We benchmark the proposed algorithms on an exemplary motion planning task: finding collision-free trajectories in a simulated WEEE disassembly cell. WEEE disassembly is a process in the recycling procedure in which the components of the WEEE devices such as PC-towers and Microwave ovens are dismantled. For that purpose, the robot is mounted with end-effector tools to perform the disassembly of the electronic devices. A COMAU 6-DOF racer robot arm has been used for benchmarking the   proposed algorithms. All the benchmarks have been run on an x86_64 of 12th Gen Intel 24-Core i9-12900 K system.
Comparing different motion planning algorithms is a difficult task mainly due to the inconsistent mechanisms used for planning problem formulations, solution methodologies, and planning environments. In order to keep the comparison fair, we select the planning algorithms that use similar methodologies as the proposed approach and benchmark on two fundamental criteria: success rate and computation time. Comparative analysis of the proposed algorithms has been performed against GPMP2, GPMP2-inter and TrajOpt [24]. Planning conditions and parameter values have been kept identical for all cases. In the case of GPMP2-inter, GP interpolation has been performed by adding factors and the resulting output of the algorithm is an interpolated trajectory with the total states N = 51. Whereas for GaBP, trajectory interpolation is not part of the algorithm and it has been performed as a separate step before trajectory execution for real-world experiments.

A. Batch Planning Benchmark
We evaluated the proposed algorithm for 24 unique batch planning problems for three different scenarios of PC-Tower disassembly that include, the trajectory going around it and  also going in and outside of the PC-tower. For each scenario, different start and end configurations have been tested. In order to make the planning problem more challenging, the start and end configurations have been kept very close to the body of PC-tower.
The results for batch planning are summarized in Table I. Overall, GPMP2 offers fast solutions but it comes at the behest of lower success rate. GPMP2-inter takes more time to converge but the success rate is better than GPMP2 as the obstacle factors have also been attached to the interpolated states. As the number of states considered increases, TrajOpt [24] demonstrates improved performance, but this improvement comes at the cost of increased computation time. GaBP offers better collision avoidance capabilities due to the local message communication among graph nodes. The role of unary obstacle factors and their associated cost is more critical than the binary factors, making GaBP more sensitive to obstacles. For a larger number of states, the scalability of GaBP is constrained by the increased communication among graph nodes, leading to longer computation times. Nevertheless, this limitation can be overcome by implementing parallel processing of messages.  Table II indicates the computing efficiency of the proposed approach. We observed that the trajectory smoothness does get affected in case of individual node optimization via iGaBP. However, this is not a major concern as, even in real-world implementation, trajectory interpolation takes place after the planning phase which handles the overall smoothness.

C. Re-Planning Benchmark
We benchmark the re-planning results against Incremental Gaussian Process Motion Planning (iGPMP2). Fig. 4(d) shows the trajectory produced in the case when goal pose is changed during motion. The resulting re-planned trajectory is the updated trajectory given the new goal pose. iGaBP computes only the portion of the factor graph in which the planning conditions have been changed, making the re-planning faster as presented in Table III.

D. Performance Evaluation for WEEE Disassembly
The performance of the proposed algorithm has also been tested for WEEE disassembly in a real-world pilot setup. The software stack for WEEE disassembly motion planner is shown in Fig. 5. The robot is mounted on one edge of the disassembly table. We implemented the planner as a ROS Action Server. The client (a high-level task planner) can send the request to the Planner action server to plan and execute the trajectory. The trajectory is interpolated via piece-wise cubic B-splines before executing it on the robot. Fig. 6 shows the robot trajectory produced for the pick-and-place task of PC-Tower disassembly procedure.
The batch planning results in Table IV for the WEEE disassembly cell indicate the necessity of handling hard constraints in real-world implementations as the 6-DoF COMAU robot in combination with the end-effector toolchain is prone to run into joint limits. In GaBP, no joint limit violations were observed, while GPMP2 had 4 out of 24 planning tests with violated joint limits. The activation of constraint violation check in Algorithm 1 causes the computation time for GaBP to increase, as more iterations are required for convergence. Putting the constraint factors on interpolated states can further improve the performance of GPMP2-inter. However, it is not expected to perform better than GPMP2.

VII. LIMITATIONS AND FUTURE WORK
The proposed algorithm is limited in a sense that it can only handle hard inequality constraints. However, we did not find it limiting in our real-world experiments as joint limits are more crucial that are tackled via proposed GaBP algorithm. Furthermore, The graph structure matrix Λ ∈ R m × n is not square (m = n) in (17). We transform the matrices to Λ T n×m Λ m×n → Λ n×n and Λ T n×m η m×1 → η n×1 which needs to be computed prior to running messages which add extra computational load. The methodology proposed in (cf. [15], Sec. 7.1) can be adopted to overcome this in future work. Also, incremental planning and re-planning have been only tested in simulation for the WEEE disassembly cell due to the COMAU robot missing a real-time joint control interface for incremental execution.
For future work, we plan to investigate the distributed properties of GaBP to address its scalability limitations. The local sub-graph formulation and solution proposed in Section IV-B allows for parallel processing and different messaging schedules that can further increase the computing efficiency, possibly bringing it at par with GPMP2. Furthermore, GaBP can run continually in the background and compute the inference as new data arrives. We plan to explore further the similar kind of capabilities e.g. just-in-time computation of GaBP for robot planning.

VIII. CONCLUSION
The problem targeted by this work is a standard kinematic motion planning for a robot manipulator in the presence of obstacles and joint limit constraints. We argue the case of GaBP as a solution method for MAP estimation of probabilistic inference-based robot motion planning. We highlight that the GaBP framework offers the utility to cater batch planning, incremental planning and re-planning scenarios. Further, our investigation emphasizes the necessity of handling hard constraints. It has been observed that handling the joint limit constraints in soft manner reduces the success rate of planning algorithms in real-world robot settings. The investigation and experiments performed in this work also indicated that the GaBP has the right structural and computational properties to act as a generic framework to produce motion trajectories in various real-world planning scenarios. Its ability to handle hard constraints further strengthens its suitability for such applications. For future work, an exciting research direction is the examination of distributive properties of GaBP that could further improve its computing efficiency.