PaperlayerView exact revision on arXiv

Source-linked AI summary

DD-GBP: Anytime Multi-Arm Planning and Continuous Execution via Distributed Dynamic Gaussian Belief Propagation

Zhao Jin, Fengyi Dai, Yuetong Yin, Yunan Wang, Wenjie Chen, Chuxiong Hu

arXiv:2610.20460v1cs.RO

TL;DR

Long-horizon multi-arm planning must coordinate robots sharing confined spaces while adapting online to moving obstacles. DD-GBP uses distributed Gaussian belief propagation with a shared GP trajectory representation for planning and execution. It reports the highest success rate among the matched four-arm baselines and strong scalability in larger tests.

  • Problem

    Online long-horizon multi-arm planning must coordinate arms whose motions affect neighbors in confined workspaces while responding to moving obstacles; centralized coupled-state costs and insufficient distributed coordination are challenges.

    Source: §I · p. 1

  • Method

    DD-GBP combines adaptive factor-graph planning, localized asynchronous message passing over active links, and a shared GP representation connecting planning with continuous execution.

    Source: §I · p. 2

  • Results

    DD-GBP has the highest success rate among the evaluated matched four-arm baselines; it records 92.5% collision-free all-target completion in 50 four-arm scenes with four dynamic obstacles.

    Source: §I · p. 2, §VI · p. 13

  • Takeaways & Limitations

    The reported benchmark, scalability studies, and compact-workspace physical demonstrations support DD-GBP's performance and deployability within the tested settings.

    Source: §I · p. 2, §VI · p. 13, Scalability and scenario results · p. 16

    Further work is identified for nonlinear, time-varying GBP convergence theory and coordination across heterogeneous platforms.

    Source: §VI · p. 13

Abstract

from arXiv · show

Safely executing long-horizon multi-arm tasks online in complex dynamic environments remains challenging. We propose a new distributed dynamic Gaussian belief propagation (DD-GBP) method that directly plans joint-space trajectories through parallel local inference. A unified Gaussian-process (GP) parameterization connects adaptive planning horizons, interpolation-time factors, and continuous execution. Each robot maintains a local tree-structured factor graph and exchanges messages asynchronously over active inter-arm links. Dynamic subnet reconstruction and event-triggered updates reduce communication and inference costs while responding to moving obstacles. We also provide theoretical analyses of local inference accuracy, anytime early stopping, triggered-replanning consistency, and GP-consistent execution. In 50 long-horizon, multi-target four-arm scenes with moving obstacles, DD-GBP achieves $92.5\%$ collision-free all-target completion and the highest success rate among six representative baselines. Scenario ablations demonstrate long-horizon safety and robustness to communication impairment and extreme conditions; success remains $86.0\%$ under $90\%$ packet loss. Execution results further show substantially suppressed extreme velocity and acceleration peaks relative to Syn-GBP. On one workstation, obstacle-free tests with 30 coexisting arms retain $99.87\%$ success and a $15.80$ ms mean warm solve, exceeding 60 Hz and outperforming the compared methods in scalability. Dual-arm transport experiments under dynamic interference demonstrate online response and practical deployability. To the best of our knowledge, these results establish a new state of the art for online long-horizon multi-arm planning in dynamic scenes.

I. INTRODUCTION

Multi-arm planning must coordinate motions in shared, confined spaces while updating online around moving obstacles. DD-GBP combines distributed inference and a shared GP trajectory representation to support adaptive, long-horizon planning and continuous execution.

I. INTRODUCTION

  • Centralized planning can be costly as the coupled state space grows, while insufficient coordination in distributed planning can still cause collisions.The introduction also notes nonconvex local subproblems for high-DoF manipulators and horizon-length trade-offs in distributed MPC.Source: §I · p. 1
  • Distributed MPC repeatedly replans over finite horizons, while learned planners' performance in new task distributions depends on generalization from training data.The introduction also contrasts these approaches with sampling-based MR-STORM and GP-based factor-graph planning.Source: §I · pp. 1–2
  • DD-GBP extends a preliminary short-horizon dual-arm study with tree-directed local inference, dynamic interaction subnets, adaptive horizons, and a common GP execution model.The framework targets scalable multi-arm coordination and safe long-horizon execution.Source: §I · p. 2
  • The method formulates planning as MAP inference on a changing factor graph and provides bounds for pruning consistency, triggered replanning, and residual-based early stopping.Each robot performs local backward–forward inference and exchanges messages asynchronously only over active inter-arm links.Source: §I · p. 2
  • DD-GBP achieves the highest success rate in the matched four-arm benchmark and the strongest observed scalability among the evaluated baselines.The evaluation spans six representative multi-arm planning baselines and includes mechanism and scenario studies plus dual-arm physical deployment.Source: §I · p. 2
  • A shared GP parameterization connects interpolation-time collision factors, timestamp-aligned inter-arm queries, and execution references.For the adopted prior, the conditional GP mean is the cubic Hermite trajectory sent to the tracking controller.Source: §I · p. 2

A. Inference Problem and Gaussian Belief Propagation

The section formulates inference as finding a posterior distribution represented by local factors. For Gaussian residual factors, GBP performs message computations analytically when the residual functions are affine.

A. Inference Problem and Gaussian Belief Propagation

  • The inference problem is to determine the distribution of X given observations Z, with the posterior expressed as a product of local factors.The factorization encodes conditional-independence structure.Source: A. Inference Problem and Gaussian… · p. 2
  • For Gaussian measurement errors or quadratic penalties, factors have residual form; affine residual functions yield Gaussian factors that GBP can marginalize analytically.Nonlinear functions are locally linearized to obtain the same Gaussian form.Source: A. Inference Problem and Gaussian… · p. 2
  • Maximizing the posterior is equivalent to minimizing its negative logarithm, giving a MAP solution; on a tree, GBP recovers exact marginal means and covariances.For DD-GBP, under the stated convergence conditions, fixed-point means recover the MAP solution of the current linearized Gaussian model.Source: A. Inference Problem and Gaussian… · pp. 2–3, Figure 1 · p. 3

B. Message Passing and Updates on Factor Graphs

A factor graph represents a factorization as a bipartite graph linking variables to the factors in which they participate. Inference sends messages along these edges and updates Gaussian beliefs.

B. Message Passing and Updates on Factor Graphs

  • Variable nodes represent state variables, factor nodes represent factors, and an edge connects them when that variable participates in the factor.The graph is bipartite; the illustrated tree contains unary and pairwise factors.Source: B. Message Passing and Updates on… · p. 3
  • Inference passes messages along graph edges and updates variable beliefs, with local relinearization for nonlinear factors.Beliefs are written in Gaussian information form, with precision Λ=Σ^-1 and η=Λµ.Source: B. Message Passing and Updates on… · p. 3
  • Each variable belief is maintained as a Gaussian in information form rather than by directly writing its mean and covariance.The text defines the information parameters from the covariance and mean.Source: B. Message Passing and Updates on… · p. 3

1) Messages from Variable Nodes to Factor Nodes:

A variable-to-factor message combines information arriving from the variable's other neighboring factors, excluding the destination factor.

1) Messages from Variable Nodes to Factor Nodes:

  • A variable-to-factor message is the product of incoming messages except the one from the destination factor.This sends the variable's other local information toward the receiving factor.Source: 1) Messages from Variable Nodes to… · p. 3
  • The incoming messages are indexed by the neighboring factor set N(vi).Their information parameters are η and Λ for each incoming message.Source: 1) Messages from Variable Nodes to… · p. 3
  • The message is represented through information parameters supplied by neighboring factor-to-variable messages.The text identifies ηwk→vi and Λwk→vi as the incoming messages' information parameters.Source: 1) Messages from Variable Nodes to… · p. 3

2) Messages from Factor Nodes to Variable Nodes:

Factor-to-variable messages marginalize a factor over all variables except the recipient variable. For affine factors, the resulting messages are Gaussian and use information parameters.

2) Messages from Factor Nodes to Variable Nodes:

3) Variable Belief Update:

The variable belief combines incoming factor messages to form a Gaussian marginal estimate for the state variable.

3) Variable Belief Update:

4) Factor Linearization:

The method locally linearizes nonlinear measurement functions at the current belief means, then uses the resulting Gaussian factor for a closed-form update.

4) Factor Linearization:

  • At the current belief means, the nonlinear measurement function is approximated by a local affine function.The approximation uses the function value and its Jacobian at those means.Source: 4) Factor Linearization · p. 3
  • Substituting the Gaussian factor into the message expression gives the closed-form update by the Schur complement.Source: 4) Factor Linearization · p. 3
  • The supplementary material provides a collision-factor Jacobian derivation and a numerical calculation example.Source: 4) Factor Linearization · p. 3

III. DISTRIBUTED DYNAMIC MULTI-ARM MOTION PLANNING METHOD

DD-GBP plans multi-arm motion through distributed inference over Gaussian-process trajectory parameters, adapting local graphs and collision interactions as tasks and geometry change. Its shared interpolation map supports collision evaluation, remote-state queries, warm starts, and continuous GP-consistent execution, while local chain solves provide usable estimates before distributed convergence.

A. GP-Parameterized Planning as MAP Inference

  • DD-GBP minimizes a weighted MAP objective over GP support states, encoding boundary, motion, joint and velocity limits, and collision requirements as finite-weight factors.Factor weights balance endpoint accuracy, motion regularity, and collision avoidance; increasing a weight penalizes departures from its condition more strongly.Source: A. GP-Parameterized Planning as MAP… · p. 4

B. Multi-arm DD-GBP Net

  • Each robot’s local graph retains static self factors while inserting, removing, or relinearizing obstacle and inter-arm factors and remote trajectory nodes as geometry changes.Active inter-arm links define the messages exchanged between robots.Source: B. Multi-arm DD-GBP Net · p. 4, Figure 3 · p. 4

1) Initial and Goal Factor:

3) Obstacle Collision Factor:

5) Motion Transition Factor:

  • A linear Gaussian motion-transition prior links consecutive position–velocity states and defines a continuous GP trajectory, whose interpolation evaluates collision factors between support states.The prior uses a constant-velocity model on each interval; its precision adapts to the time grid and prescribed acceleration scale.Source: 5) Motion Transition Factor · p. 5, 1) Dynamic Reconstruction and… · p. 6

1) Dynamic Reconstruction and Replanning Mechanism:

2) Distributed Message Passing and Updating:

  • A tree-directed backward–forward schedule replaces Syn-GBP’s synchronous updates; robots exchange messages on active links and align remote states by timestamp across different support grids.Each sweep holds factor linearizations and received neighbor messages fixed while recovering local Gaussian means and covariances.Source: 2) Distributed Message Passing and… · p. 7

3) GP-Consistent Interpolation and Online Execution:

  • After each local update, inferred position–velocity means produce a continuous reference through cubic Hermite interpolation, which passes through a velocity-saturation limiter before tracking.For the constant-velocity prior driven by white acceleration, the nominal Hermite reference before saturation is the GP conditional mean.Source: 3) GP-Consistent Interpolation and… · p. 7
  • A shared GP interpolation map supports intermediate-time collision checks, timestamp-aligned remote-state queries, and time-aligned warm starts for continued online refinement.New targets, predicted near-future collisions, and neighborhood changes trigger updates to affected subnets; if an iteration budget expires, the current mean remains executable.Source: 3) GP-Consistent Interpolation and… · p. 7

A. Convergence and Anytime Planning

  • With neighbor information fixed during a local solve, interpolation factors combine with motion transitions on adjacent support pairs, forming a chain whose backward–forward sweep yields exact Gaussian marginals.Each completed robot-wise sweep returns the MAP estimate for that robot’s current fixed Gaussian subproblem.Source: A. Convergence and Anytime Planning · p. 8
  • Boundary anchoring and positive-definite GP process covariances make each local Gaussian information matrix positive definite, yielding a strongly convex objective and a unique MAP solution.The nonlinear early-stopping assessment uses the assembled residual and local linearization mismatch; the anchored GP prior supports local curvature when it dominates residual-weighted second-order terms.Source: A. Convergence and Anytime Planning · p. 8

B. Topology-Switching Consistency under Replanning

The section establishes when switching to local interaction subnets preserves the gated objective, and bounds how replanning changes the trajectory. It also connects GP interpolation to the continuous reference used in execution.

B. Topology-Switching Consistency under Replanning

  • Retaining every active factor, free variable, and fixed boundary term makes the switched energy match the gated full energy, with matching gradients and Hessians.Under the stated vanishing-loss and smoothness conditions, switching adds no discontinuity through order p; pruning also preserves the linearized Gaussian problem when omitted residuals and Jacobians are zero.Source: B. Topology-Switching Consistency… · pp. 8–9
  • With capped subnet selection, the approximation is exactly the contribution of active factors that were omitted, unless they vanish or remain as fixed boundary factors.The equivalence result applies when all contributing active factors are retained.Source: B. Topology-Switching Consistency… · p. 9
  • The previous GP trajectory is transferred to new support-state coordinates by evaluating it at the new timestamps, even when support count or spacing changes.The consistency comparison uses a common time-aligned coordinate vector and relates the updated solution to the transferred trajectory through gradient imbalance and trigger-modified factors.Source: B. Topology-Switching Consistency… · p. 9
  • When warm-start imbalance and changed-factor gradient increments are small, the updated quadratic trajectory stays close to the time-aligned previous plan.The prefix bound measures change in the portion used for near-term execution; a returned iterate’s additional error is bounded by its gradient residual divided by m.Source: B. Topology-Switching Consistency… · p. 9

C. Consistency of GP Interpolation and Continuous Execution

  • Conditioning the second-order GP prior on endpoint position–velocity states yields the same cubic Hermite position and velocity means used for execution.The interpolation map is linear in endpoint states; collision residual Jacobians at interpolated states can adjust both endpoints, allowing denser collision checks without increasing support-state dimension.Source: C. Consistency of GP Interpolation and… · pp. 9–10

V. SIMULATIONS AND EXPERIMENTS

The evaluation compares planning methods in matched long-horizon multi-arm simulations and describes the associated metrics, compute setup, and physical platform.

Compared Method Design

  • The baselines are Syn-GBP, GPMP2, Gafur’s NMPC, DMPC, MR-STORM, and NADMM, with core formulations tuned for shared scenes, targets, and execution limits.Syn-GBP uses synchronous messages and adaptive support count at fixed ∆t = 0.6 s, without interpolation-time collision factors.Source: §V · p. 10

Experimental Settings

  • The method and mechanism comparisons use 50 matched scenes with four 7-DoF Panda arms, three sequential targets per arm, moving obstacles, and a 60-second run budget.The arms face the origin from fixed configurations on a 2 × 2 grid; execution must maintain safety throughout.Source: §V · p. 10
  • Simulation runs in Gazebo/ROS on an Intel Core i9-14900KF workstation; the physical platform uses two Panda arms, 30 Hz obstacle detection, and TCP/IP controller communication.GBP and optimization baselines use CPU execution, while MR-STORM uses an RTX 4080 SUPER GPU.Source: §V · p. 10

Metrics

  • Task success requires reaching all assigned targets within the run budget while maintaining d(q(t)) > 0 throughout execution.The reported metric set also includes clearance, solve times, goal count, path length, time per goal, communication throughput, tracking error, and peak velocity and acceleration.Source: §V · p. 10

1) Method Comparison:

This section presents DD-GBP’s distributed planning and execution framework, including obstacle-residual Jacobians for robot and GP-interpolated states. It also reports assessments of benchmark performance, robustness, scalability, and execution behavior across simulations and physical demonstrations.

3) Scalability:

1) Method Comparison:

  • DD-GBP led the four-arm dynamic benchmark with 92.5% success, 0.0735 m conditional clearance, 3.00 mean goals, and 2.41 mrad measured tracking error.Its path length and time per goal were also near the best, with shorter paths and faster completion than the MPC/MPPI baselines.Source: 1) Method Comparison · p. 10, Table I · p. 11
  • DD-GBP had the smallest execution-peak upper tails and interquartile spreads; its worst velocity and acceleration peaks were 0.900 rad/s and 53.2 rad/s2, versus Syn-GBP’s 7.40 rad/s and 761 rad/s2.Adaptive planning and GP-consistent continuous execution suppress transients; DMPC’s lower mean velocity peak accompanies slow command updates, while substantial extreme excursions remain.Source: 1) Method Comparison · p. 11

2) Failure Analysis:

  • Among 200 DD-GBP arm runs, 14 contacted moving obstacles and one more was blocked from its final target, with no inter-arm collisions; failures were more common under simultaneous interference.Success declined above 0.25 m/s obstacle speed, while DD-GBP still achieved 92.5% collision-free all-target completion at the 0.20 m/s reference speed.Source: 2) Failure Analysis · p. 11

1) Mechanism Ablations:

  • Ablations showed that removing interpolation reduced success to 64.0%, fixing the time step or GP weight to 77.5% or 80.0%, and removing subgraph localization to 83.5%.The no-localization variant used 4.82 MB/s communication versus 2.59 MB/s for DD-GBP; continual replanning without triggering achieved 89.0% success. Tree-directed scheduling also improved optimization loss and inference efficiency, while obstacle prediction achieved 98.0% task success with 17.2 ms warm solves.Source: 1) Mechanism Ablations · pp. 11–12

2) Scenario Sensitivity:

  • DD-GBP achieved 65.0% success at 0.65 m arm spacing, 86.0% under 90% packet loss, and 80.0% of arms completed all five sequential targets without collision.With one to five skipped send cycles, success was 85.0–89.0% and warm solves were 16.0–16.6 ms; at 5 cm obstacle-position noise, success was 77.5%.Source: 2) Scenario Sensitivity · p. 12

C. Exp. III: Real-World Multi-Robot Deployment

  • In two-arm transport tests, DD-GBP planned pick-and-place paths and timing without prescribed avoidance or passage order, and maintained 0.107 m minimum inter-arm clearance during transport.Under sustained hand-driven disturbances, it recovered obstacle clearance while retaining positive minimum distances of 0.032/0.060 m; velocity and acceleration exceedance fractions were 1.44% and 0.24%.Source: 1) Pick-and-Place Tasks in a Compact… · p. 12, 2) Dynamic Obstacle Avoidance in… · p. 13

Parameter sensitivity and selection

  • Sensitivity sweeps identified a broad favorable region: α = 10–50 retained 89.0–92.5% success with 16.3–18.1 ms warm solves, and interaction clearance rc = 0.2–0.4 m retained 90.0–92.5% success.The shared reference was 92.5% success with 16.346 ms warm solve time; collision precision was more sensitive, and the results favored balanced tuning.Source: Figure S1 · p. 16, Parameter sensitivity and selection · p. 16

Tracking and execution quality

  • For inter-arm distances, the relative-point Jacobian stacks the two arm Jacobians with opposite signs, while GP-interpolated obstacle residuals depend on endpoint states and velocities.The endpoint velocity columns are generally nonzero because the GP interpolation depends on both endpoint velocities.Source: S3. COLLISION-FACTOR JACOBIANS AND… · p. 18
  • At q0 = (0, π/2), the example gives d0 = 0.05 m and h = 0.5; a stated perturbation yields linear h = 0.485 versus nonlinear h = 0.484014.The reproducible planar arm uses l1 = 0.4 m and l2 = 0.3 m, with combined collision radius 0.05 m and obstacle center c = (0.5, 0.3) m; its endpoint coordinates use the two-link forward-kinematics equations.Source: A numerical collision-factor… · p. 18
Loading 2610.20460v1…