Connectivity Maintenance and Recovery for Multi-Robot Motion Planning

This paper proposes a real-time MPC-CLF-CBF motion planner based on Bézier curves that enables multi-robot fleets to maintain connectivity, navigate obstacle-rich environments without deadlocks, and recover from connection losses, as validated by simulations and experiments with eight Crazyflie quadrotors.

The Gist

Imagine you are leading a team of eight tiny, agile drones (like high-tech bees) on a mission to fly through a dense, cluttered forest filled with giant, immovable trees. Your goal is for every drone to reach a specific flower on the other side.

Here is the catch: The drones must stay in touch with each other at all times. If they lose contact, they can't coordinate, and the mission might fail.

This is the core problem the paper solves. It's like trying to walk through a crowded market while holding hands with your friends. If you just try to walk straight to the exit while holding hands, you might get stuck in a corner (a "deadlock") because the crowd is too thick. If you let go to squeeze past a stall, you might lose your friends entirely.

The Problem with Old Methods

Previous ways of controlling these robots were like reactive reflexes.

• The "Stop-and-Go" Problem: If a robot saw an obstacle, it would stop. If it saw a friend getting too far away, it would pull back. In a messy environment, these two commands often fight each other. The robot gets confused, freezes up, and gets stuck in a deadlock.
• The "Broken Chain" Problem: If the chain of friends did break (because an obstacle forced them apart), old systems couldn't fix it. They would just keep flying in separate groups, never reconnecting.

The New Solution: The "Smart Dance Instructor"

The authors propose a new system called MPC–CLF–CBF. Think of this as a Smart Dance Instructor who doesn't just react to the music; they choreograph the entire dance in advance.

Here is how it works, broken down into simple concepts:

1. The "Bezier Curve" (The Smooth Path)

Instead of telling the drones "move forward, then turn," the system draws a smooth, invisible ribbon (a Bézier curve) through the air for each drone.

• Why it's cool: This ribbon is mathematically perfect. It's so smooth that the drones can know exactly how fast to speed up, slow down, or tilt before they even get there. It's like planning a rollercoaster track that is guaranteed to be comfortable and safe, rather than just steering a car randomly.

2. The "Guardian" (HOCBF - Connectivity Maintenance)

This is the rule that says, "Don't let go of the hand."
The system constantly checks the "tension" in the group. If the group starts to stretch too thin, the Guardian gently nudges the drones closer together before they actually break apart. It's like a tightrope walker who feels the rope getting loose and shifts their weight immediately to stay balanced.

3. The "Rescuer" (HOCLF - Connectivity Recovery)

This is the magic trick. Sometimes, despite your best efforts, an obstacle forces the group to split.

• Old systems: "Oh no, we broke! We are stuck."
• This system: "Oh no, we broke! Time to reconnect."
  The Rescuer kicks in. It calculates a new path where the drones can safely loop around the obstacle and meet back up on the other side. It's like a group of friends who get separated by a wall; instead of giving up, they find a door, walk through it, and regroup on the other side.

4. The "Traffic Light" (The Gate Function)

How does the system know when to focus on "staying together" vs. "finding a way to reconnect"?
It uses a Traffic Light (called a Gate Function).

• Green Light (Connected): If everyone is close, the light tells the drones, "Keep holding hands! Don't worry about the distance."
• Red Light (Disconnected): If the group splits, the light instantly switches. It tells the drones, "Forget holding hands for a second! Focus on finding a path to get back together."
  This switch happens smoothly and automatically, so the drones don't panic or freeze.

The Results: A Real-World Test

The researchers didn't just write code; they tested it with 8 real nano-drones (Crazyflies) in a lab.

• They flew through a room with obstacles.
• The drones successfully navigated the mess, avoided crashing into walls or each other, and never lost their connection for long.
• Even when they were forced to split up to get around a blockage, they automatically found their way back to each other and finished the mission together.

Why This Matters

This technology is a big step forward for:

• Search and Rescue: Imagine a swarm of drones searching a collapsed building. If they lose signal, they can't give up; they need to find a way to reconnect to share data.
• Delivery Fleets: Drones delivering packages in a city need to stay in contact to coordinate traffic and avoid collisions.
• Exploration: Robots exploring Mars or the ocean need to stay linked to map the terrain together.

In short: This paper gives a team of robots the ability to be agile (dodge obstacles), social (stay connected), and resilient (fix themselves if they get separated), all while moving smoothly like a well-rehearsed dance troupe.

Technical Summary

Here is a detailed technical summary of the paper "Connectivity Maintenance and Recovery for Multi-Robot Motion Planning" by Wang et al.

1. Problem Statement

The paper addresses the challenge of navigating a fleet of multi-robot systems (specifically quadrotors) through obstacle-rich environments while maintaining network connectivity.

• The Core Conflict: There is a fundamental trade-off between maintaining connectivity (robots staying close enough to communicate) and traversability (robots navigating around obstacles to reach goals).
• Limitations of Existing Methods:
  • Reactive Controllers (CBF-based): While Control Barrier Functions (CBFs) guarantee safety and connectivity, they are reactive and often lead to deadlocks in cluttered environments where connectivity constraints conflict with obstacle avoidance.
  • Lack of Recovery: Most existing methods assume the fleet starts connected and can only preserve connectivity. They lack mechanisms to restore connectivity if the fleet becomes disconnected (either initially or temporarily during navigation).
  • System Dynamics: Many safety constraints have a relative degree greater than one (e.g., position constraints for double-integrator dynamics), which standard first-order CBFs cannot handle directly without approximation.

2. Methodology: MPC–CLF–CBF Framework

The authors propose a real-time, optimization-based motion planner named MPC–CLF–CBF. This framework integrates Model Predictive Control (MPC) with High-Order Control Barrier Functions (HOCBFs) and High-Order Control Lyapunov Functions (HOCLFs) using Bézier curves for trajectory generation.

A. Mathematical Formulation

• System Model: The robots are modeled as double-integrator systems (position and velocity states, acceleration control inputs).
• Trajectory Representation: Trajectories are parameterized as piecewise Bézier curves. This allows for smooth, continuous-time trajectories where arbitrary orders of derivatives (velocity, acceleration, jerk) can be easily computed, making it suitable for agile systems like quadrotors.
• Constraints:
  1. Connectivity Maintenance (HOCBF): Uses the algebraic connectivity (Fiedler value, $\lambda_2$) of the communication graph as a barrier function. If $\lambda_2 > 0$, the graph is connected. The HOCBF ensures $\lambda_2$ remains above a threshold $\epsilon$.
  2. Connectivity Recovery (HOCLF): When connectivity is lost ($\lambda_2 \approx 0$), the system switches to a Control Lyapunov Function (CLF) approach. This minimizes the distance between disconnected robot pairs, actively driving them back into communication range.
  3. Collision Avoidance (HOCBF): Hard constraints ensure minimum safety distances between robots and obstacles.
  4. Dynamics & Physical Limits: Constraints on velocity, acceleration, and continuity of derivatives between Bézier segments.

B. The "Gate" Mechanism (Dynamic Weighting)

A key innovation is the smooth gate function ($\eta(\lambda_2)$) that dynamically adjusts the penalty weights for the slack variables in the optimization problem:

• When Connected ($\lambda_2 > \epsilon$): The gate prioritizes the HOCBF (maintenance) constraints, penalizing deviations that might break connectivity.
• When Disconnected ($\lambda_2 \le \epsilon$): The gate shifts weight to the HOCLF (recovery) constraints, penalizing large inter-robot distances to force the fleet to reconnect.
• This smooth transition prevents the optimization from becoming infeasible due to competing objectives.

C. Optimization Solver

• The problem is formulated as a Quadratic Program (QP) with a finite horizon.
• Since the connectivity constraints are nonlinear, the authors use Sequential Quadratic Programming (SQP). They linearize the constraints around the predicted trajectory from the previous iteration, solving a series of convex QPs to converge to a solution in real-time.
• Constraints are sampled at discrete time steps within the continuous horizon to ensure computational efficiency.

3. Key Contributions

1. Real-Time Motion Planner: A novel algorithm (MPC–CLF–CBF) that concurrently generates trajectories and control inputs for multi-robot fleets in cluttered environments.
2. Connectivity Recovery: Unlike prior works that only preserve connectivity, this method can recover connectivity from an initially disconnected state or after a temporary loss during navigation.
3. High-Order Constraints: The framework generalizes to HOCBFs and HOCLFs, handling systems with relative degrees greater than one (essential for mechanical systems like quadrotors).
4. Bézier-Based Smoothness: The use of Bézier curves ensures dynamically feasible, smooth trajectories with continuous derivatives, ideal for agile control.
5. Dynamic Weighting: The introduction of the gate function effectively balances the trade-off between maintaining connectivity and recovering from disconnection without manual tuning.

4. Experimental Results

The authors validated the algorithm through extensive simulations and physical experiments.

• Baselines: Compared against:
  • CLF–CBF: A reactive controller with recovery terms (prone to deadlocks).
  • MPC–CBF: A predictive planner without recovery terms (cannot fix disconnections).
• Simulation Metrics:
  • Success Rate: MPC–CLF–CBF achieved significantly higher success rates in obstacle-rich environments (up to 20% obstacle density) compared to baselines. For example, with 12 robots and 20% obstacles, MPC–CLF–CBF maintained ~88% success, while baselines dropped significantly or failed to maintain connectivity.
  • Connectivity Preservation: The proposed method maintained a higher "Percentage Connected" time, especially in dense obstacle scenarios where reactive controllers got stuck in deadlocks.
  • Ablation Study: The "gate" parameter ($w_\eta$) was shown to be critical; without it, the planner struggled to switch between maintenance and recovery modes efficiently.
• Physical Experiment:
  • Setup: 8 Crazyflie nano-quadrotors navigating a 10m x 6m workspace with obstacles.
  • Result: The fleet successfully navigated through clutter, maintained connectivity when possible, and recovered connectivity when necessary, validating the algorithm's real-world applicability.

5. Significance

This work bridges a critical gap in multi-robot systems by solving the connectivity vs. traversability dilemma in a predictive, rather than reactive, manner.

• Robustness: It prevents the "deadlock" problem common in reactive controllers, allowing fleets to navigate complex, real-world environments (e.g., search and rescue, warehouse logistics) without losing communication.
• Resilience: The ability to recover from disconnection makes the system robust to initial configuration errors or unexpected environmental changes.
• Scalability: The use of Bézier curves and SQP allows the system to scale to larger fleets (tested up to 12 robots in simulation) while maintaining real-time performance (100 Hz control frequency).
• Generalizability: The framework is applicable to any differentially flat system (like quadrotors) and can be extended to 3D environments and heterogeneous teams in future work.