Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

MR.CAP: Multi-Robot Joint Control and Planning for Object Transport

Hussein Ali Jaafar, Cheng-Hao Kao, Sajad Saeedi

TL;DR

The paper tackles fast, robust multi-robot object transport by unifying planning and control within a factor-graph framework, enabling real-time joint optimization of centroid and formation dynamics. It formulates the problem as MAP inference on a sparse factor graph and solves it with Levenberg–Marquardt, achieving favorable scaling in the number of robots and environment complexity. Across simulations, Gazebo, and real hardware, the approach outperforms MPC and HQP baselines in optimization time and coordination accuracy while remaining robust to disturbances and failures. The work demonstrates clear practical impact for scalable cooperative manipulation and offers extensibility through added factors and motion models, with future directions including smoother continuous-time trajectories via Gaussian processes and higher DOF extensions.

Abstract

With the recent influx in demand for multi-robot systems throughout industry and academia, there is an increasing need for faster, robust, and generalizable path planning algorithms. Similarly, given the inherent connection between control algorithms and multi-robot path planners, there is in turn an increased demand for fast, efficient, and robust controllers. We propose a scalable joint path planning and control algorithm for multi-robot systems with constrained behaviours based on factor graph optimization. We demonstrate our algorithm on a series of hardware and simulated experiments. Our algorithm is consistently able to recover from disturbances and avoid obstacles while outperforming state-of-the-art methods in optimization time, path deviation, and inter-robot errors. See the code and supplementary video for experiments.

MR.CAP: Multi-Robot Joint Control and Planning for Object Transport

TL;DR

The paper tackles fast, robust multi-robot object transport by unifying planning and control within a factor-graph framework, enabling real-time joint optimization of centroid and formation dynamics. It formulates the problem as MAP inference on a sparse factor graph and solves it with Levenberg–Marquardt, achieving favorable scaling in the number of robots and environment complexity. Across simulations, Gazebo, and real hardware, the approach outperforms MPC and HQP baselines in optimization time and coordination accuracy while remaining robust to disturbances and failures. The work demonstrates clear practical impact for scalable cooperative manipulation and offers extensibility through added factors and motion models, with future directions including smoother continuous-time trajectories via Gaussian processes and higher DOF extensions.

Abstract

With the recent influx in demand for multi-robot systems throughout industry and academia, there is an increasing need for faster, robust, and generalizable path planning algorithms. Similarly, given the inherent connection between control algorithms and multi-robot path planners, there is in turn an increased demand for fast, efficient, and robust controllers. We propose a scalable joint path planning and control algorithm for multi-robot systems with constrained behaviours based on factor graph optimization. We demonstrate our algorithm on a series of hardware and simulated experiments. Our algorithm is consistently able to recover from disturbances and avoid obstacles while outperforming state-of-the-art methods in optimization time, path deviation, and inter-robot errors. See the code and supplementary video for experiments.
Paper Structure (10 sections, 7 equations, 5 figures, 3 tables)

This paper contains 10 sections, 7 equations, 5 figures, 3 tables.

Table of Contents

  1. Introduction
  2. Literature Review
  3. Proposed Method
  4. Motion Model
  5. Factor Graphs
  6. Experiments
  7. Experiment 1: Simulation
  8. Experiment 2: Gazebo Simulation
  9. Experiment 3: Real-world Robots
  10. Conclusions

Figures (5)

  • Figure 1: Four robots moving a payload in a complex environment. Our method outperforms model predictive control, MPC (constraint-based) in time and path length.
  • Figure 2: Example of four robots arranged to move the payload placed at an arbitrary location on the platform. The system is capable of operating with more robots, for heavy payloads. The robots can have any starting orientation when connected, as the optimizer accounts for any needed adjustments.
  • Figure 3: Multi-robot control and planning factor graph with centroid pose variables and centroid control variables. Factors and their associated costs are defined in the text. Since estimation is not the emphasis of this work, only the variables at the current state and future states are used in the optimization problem for each iteration. A finite horizon is used in our case, extending to the terminal state to ensure long-term stability.
  • Figure 4: Results of Experiment 1 showing the scalability with respect to the number of (left) robots, and (right) obstacles.
  • Figure 5: Hardware trial results. A payload was pushed by four Turtlebot3 Waffle Pi robots from random start points near the origin (0, 0) to the same goal (3, 0). The three columns correspond to the three types of tasks: (left column) reaching the goal while undergoing large disturbances, (middle column) obstacle avoidance, and (right column) robot failure during obstacle avoidance. The system is consistently able to recover and reach the goal within 1cm accuracy.