💻 computer science

A Novel Semi-Coupled Hierarchical Motion Planning Framework for Cooperative Transportation of Multiple Mobile Manipulators

This paper proposes a novel semi-coupled hierarchical framework (SCHF) for cooperative transportation by multiple mobile manipulators that decomposes motion planning into a centralized layer ensuring closed-chain and redundancy constraints, and a decentralized layer for real-time redundancy exploitation, thereby achieving superior success rates and efficiency compared to fully centralized or decoupled approaches in both simulation and real-world cluttered environments.

Original authors: Heng Zhang, Haoyi Song, Wenhang Liu, Xinjun Sheng, Zhenhua Xiong, Xiangyang Zhu

Published 2026-02-25

📖 6 min read🧠 Deep dive

CC BY 4.0

Original authors: Heng Zhang, Haoyi Song, Wenhang Liu, Xinjun Sheng, Zhenhua Xiong, Xiangyang Zhu

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to move a giant, heavy, and awkwardly shaped sofa through a crowded apartment filled with narrow hallways, potted plants, and a cat running around.

If you try to do this alone, you might get stuck, drop the sofa, or hurt your back. But what if you had a team of three or four strong friends? That's the basic idea behind Multiple Mobile Manipulators (MMMs): a group of robots that can both drive around (like a car) and reach out with arms (like a human) to carry big objects together.

However, moving a sofa as a team is incredibly hard for a computer to figure out. Here is why, and how the authors of this paper solved it.

The Three Big Headaches

The "Rigid Chain" Problem:
Imagine your friends are holding the sofa. If one person steps forward, everyone else must move in a specific way to keep the sofa from tilting or breaking. In robot terms, this is called a Closed-Chain Constraint. The robots are physically locked together by the object they are carrying. If the computer plans a path for the sofa, it has to instantly calculate exactly how every single robot arm and wheel must move to match that path. It's like trying to solve a 3D puzzle where every piece is glued to the others.
The "Too Many Choices" Problem (Redundancy):
Each robot has more joints (elbows, knees, wheels) than strictly necessary to hold the sofa. This is good! It means they can wiggle their elbows to avoid a potted plant while still holding the sofa steady. But for a computer, having too many choices is a nightmare. It has to decide which specific way to bend its arms to avoid the plant. If it picks the wrong way, it might get stuck in a corner.
The "Crowded Room" Problem (Obstacles):
The room is full of stuff. The robots need to dodge chairs, people, and walls.

The Old Ways (And Why They Failed)

The researchers looked at how other people tried to solve this:

The "Super-Brain" Approach (Fully Centralized):
Imagine one giant super-computer trying to calculate the exact position of every wheel, every elbow, and every robot's head for every single second of the trip.
- The Problem: It's too slow. As you add more robots, the math gets so complex the computer freezes. It's like trying to solve a Rubik's cube while blindfolded, in the dark, while someone is yelling at you.
The "Do-It-Yourself" Approach (Fully Decoupled):
Imagine the team leader says, "You guys, just follow this path for the sofa," and the robots try to figure out their own arm movements on the fly.
- The Problem: The leader might say, "Walk through that narrow gap," but the robots realize they can't actually fit their arms through there without hitting the wall. The plan fails because the leader didn't check if the robots could actually do it.
The "Bubble" Approach (Virtual Structure):
Imagine drawing a giant invisible box around the sofa and the robots. If any part of that box touches a wall, the whole team stops and goes around.
- The Problem: It's too conservative. If there is a small chair in the middle of the path, the team treats it like a mountain and goes all the way around, even though they could have just stepped over it.

The New Solution: The "Semi-Coupled" Team Leader

The authors propose a new framework called SCHF (Semi-Coupled Hierarchical Framework). Think of this as a smart team leader with a "safety net."

Here is how it works, step-by-step:

Step 1: The Leader Plans the "Big Picture" (Centralized Layer)

The leader looks at the map and plans the path for the sofa (the object).

The Magic Trick: Before the leader says, "Okay, go through that narrow hallway," they do a quick mental check. They ask: "If I tell the sofa to go here, is there any way for the robots to hold it there without crashing?"
They don't calculate the exact arm movements yet (that takes too long). Instead, they just check if a "safe zone" exists. They also make sure the robots have enough "wiggle room" (redundancy) to avoid obstacles.
The Result: The leader only draws paths that are guaranteed to be possible for the team to execute.

Step 2: The Team Figures Out the Details (Decentralized Layer)

Once the leader says, "The sofa is going to move from Point A to Point B," the individual robots wake up.

Because the leader already checked the "safe zones," the robots know they won't get stuck.
Now, each robot can focus on its own job: "Okay, the sofa is moving left. I need to twist my elbow to avoid that cat."
They do this in real-time, independently, using their own "brains" to find the best, most comfortable way to move their arms.

Why This is a Game Changer

Think of it like a dance troupe:

Old Way: The choreographer tries to memorize every single muscle movement for every dancer for the whole show at once. If the show gets longer, the choreographer's brain explodes.
New Way: The choreographer decides the steps and the formation (the path of the sofa). They make sure the steps are doable. Then, the dancers (the robots) use their own flexibility to adjust their arms and legs to hit the steps perfectly, avoiding the audience members (obstacles) on the fly.

The Results

The paper shows that this new method is:

Faster: It doesn't get bogged down by complex math.
Smarter: It can find paths through tight spaces that other methods think are impossible (like stepping over a small obstacle instead of going around it).
Reliable: It works even in messy, real-world rooms with people walking around, not just in perfect computer simulations.

In short, the authors built a system where a team of robots can carry a heavy object through a messy room without tripping, dropping the load, or getting stuck, by splitting the "big picture" planning from the "fine-tuning" execution.

1. Problem Definition

The paper addresses the motion planning challenge for Multiple Mobile Manipulators (M3) cooperatively transporting a rigid object in cluttered environments. The problem is characterized by three coupled constraints that make traditional planning difficult:

Closed-Chain Constraint: When multiple robots rigidly grasp an object, their motions are constrained to a lower-dimensional manifold. Uniform sampling in the high-dimensional configuration space has a near-zero probability of generating valid states satisfying this constraint.
Redundancy: Each mobile manipulator is kinematically redundant (more degrees of freedom than required for the end-effector task). This redundancy must be optimized to avoid obstacles, maintain dexterity, and ensure system stability, but optimizing it simultaneously with closed-chain constraints is computationally expensive.
Obstacle Avoidance: The system must navigate around static and dynamic obstacles while maintaining the closed-chain formation.

Existing approaches face significant limitations:

Fully Centralized Methods (e.g., Projection-based, Atlas, Tangent Bundle) suffer from high computational complexity ( $O(n^3)$ ) due to the need to invert large constrained Jacobian matrices for the entire system. They often fail in complex, cluttered environments.
Fully Decoupled Hierarchical Methods plan the object's path first and then assign robot configurations. However, they often fail because the object's path planned in the top layer may be inexecutable by the robots in the bottom layer due to reachability or collision constraints (layer conflict).
Virtual Structure Methods treat the robot-object system as a single polygon. These are overly conservative, often forcing the system to bypass obstacles that could be crossed, reducing efficiency.

2. Methodology: Semi-Coupled Hierarchical Framework (SCHF)

The authors propose a Semi-Coupled Hierarchical Framework (SCHF) that decomposes the problem into two layers: a Centralized Layer (Object Motion Planner) and a Decentralized Layer (Redundancy Resolution).

A. Centralized Layer (Object Motion Planner)

Goal: Plan a collision-free 6D trajectory for the object ( $\tau'$ ) in the task space.
Mechanism: Uses a sampling-based planner (e.g., RRTConnect) to explore the object's configuration space.
Key Innovation (Semi-Coupling): Unlike fully decoupled methods, this layer does not just check for object collisions. It performs a validity check for every sampled object pose to ensure it is executable by the robots.
- It calculates the required grasping poses for each robot based on the object's pose.
- It checks the Allowed Sampling Region (ASR) for each robot. This involves verifying if there exists a joint configuration for the robot that satisfies:
  1. The closed-chain constraint (Inverse Kinematics solution exists).
  2. Collision avoidance.
  3. A user-specified lower bound ($thres$) of a redundancy constraint metric ( $f_{RC}$ ).
Result: The object's path is guaranteed to be executable by the decentralized layer, preventing layer conflicts.

B. Decentralized Layer (Redundancy Resolution)

Goal: Optimize the joint configurations of each mobile manipulator in real-time to track the object's trajectory.
Mechanism: Each robot independently solves an optimization problem to maximize a redundancy metric ( $f_{RC}$ ) while satisfying the end-effector pose dictated by the centralized layer.
Redundancy Metric: The metric $f_{RC}$ $f_{R C}$ is a composite function of:
- Dexterity ( $\mu$ ): Manipulability of the arm.
- Formation ( $F$ ): Maintaining the desired relative formation of the system.
- Obstacle Avoidance ( $B$ ): Distance to ground obstacles.
Solver: Uses the Nelder-Mead simplex algorithm for real-time optimization without requiring gradient differentiability.

C. The Coupling Factor ($thres$)

The framework introduces a threshold parameter ($thres$) for the redundancy metric in the centralized layer.

If $thres = 0$, the layers are fully decoupled (prone to conflict).
If $thres = 1$, the layers are tightly coupled (computationally expensive, potentially unsolvable).
Optimal Strategy: A moderate $thres$ (e.g., 0.4) balances computational efficiency with the guarantee that the decentralized layer can successfully execute the plan.

3. Key Contributions

Novel Framework: Introduction of the SCHF, which successfully balances the trade-off between computational efficiency and constraint satisfaction by "softly" coupling the layers via a redundancy metric lower bound.
Efficiency: By decomposing the high-dimensional problem, the framework avoids the $O(n^3)$ complexity of fully centralized projection methods. It reduces the complexity to $O(n)$ for local checks, making it scalable to multiple robots.
Advanced Obstacle Avoidance: Unlike virtual structure methods that only allow "bypassing," SCHF enables "crossing" obstacles (e.g., passing under low barriers) by explicitly checking the feasibility of the object's pose against the robots' workspace.
Real-World Validation: The first demonstration of 6D object transportation by multiple heterogeneous mobile manipulators in cluttered real-world environments.

4. Experimental Results

The framework was evaluated through extensive simulations and real-world experiments.

Simulation Comparison:
- vs. Centralized (PJ, AT, TB): SCHF significantly outperformed fully centralized methods in success rate and planning time, especially as scene complexity and robot count increased. Centralized methods often failed (0% success) in complex scenes (Fig 4c-f), while SCHF maintained high success rates.
- vs. Fully Decoupled: The decoupled approach worked in simple scenes but failed dramatically in cluttered environments due to layer conflicts (planning an object path the robots couldn't reach). SCHF maintained high success rates by ensuring compatibility.
- vs. Virtual Structure: SCHF found shorter, more efficient paths by crossing obstacles, whereas virtual structure methods were forced to take long detours.
Parameter Sensitivity: Experiments showed that a moderate $thres$ (e.g., 0.4) provides the best balance between planning time and success rate.
Real-World Experiments:
- Successfully executed cooperative transportation tasks with 2 and 3 robots in environments containing dynamic obstacles (pedestrians) and terrain changes (slopes).
- The system demonstrated real-time adaptation, where robots adjusted their configurations to avoid pedestrians and adjust the object's orientation to match slope angles while maintaining the closed-chain constraint.

5. Significance

This paper presents a significant advancement in multi-robot cooperative manipulation. By addressing the critical gap between high-level path planning and low-level robot feasibility, the SCHF framework enables robust, efficient, and scalable transportation of bulky objects in complex, unstructured environments. The ability to handle redundancy and closed-chain constraints simultaneously without the prohibitive computational cost of fully centralized methods makes this approach highly practical for future applications in logistics, construction, and search-and-rescue operations.