Distributed Lloyd-Based algorithm for uncertainty-aware multi-robot under-canopy flocking

1. Problem Statement

The paper addresses the challenge of coordinating a swarm of Unmanned Aerial Vehicles (UAVs) in dense, unstructured, and cluttered environments (specifically forests) under strict constraints:

• No Explicit Communication: Robots cannot exchange data (positions, velocities, trajectories) with each other.
• No Global Localization: The system operates without GNSS or a common reference frame; robots rely solely on on-board sensors.
• Flocking Constraints: Robots must maintain proximity to specific neighbors (proximity maintenance) while navigating to a goal region.
• Uncertainty Handling: The system must account for bounded tracking errors (due to dynamics) and measurement uncertainties (due to sensor noise).
• Safety: Collision avoidance with both static obstacles and other dynamic robots must be guaranteed at all times.

The core problem is to synthesize a distributed control law that ensures the swarm reaches a goal, maintains flocking cohesion, and guarantees safety despite these limitations and uncertainties.

2. Methodology

The proposed solution is a distributed, Lloyd-based algorithm enhanced with adaptive strategies to handle uncertainties.

A. Core Framework: Convex Weighted Voronoi Diagrams (CWVD)

The algorithm is grounded in the Lloyd algorithm, which minimizes a coverage cost function. The authors utilize Convex Weighted Voronoi Diagrams (CWVD) to partition the mission space for each robot based on:

• Robot Neighbors ($V^p_i$): Defined by relative positions of nearby robots.
• Obstacles ($V^o_i$): Defined by the proximity to obstacles.
• Weighting Parameters ($\epsilon_p, \epsilon_o$): These parameters regulate "cautiousness." $\epsilon=1$ implies aggressive behavior, while $\epsilon=2$ (classic Voronoi) implies conservative behavior.

B. Cell Geometry Reshaping for Safety and Flocking

To guarantee safety and flocking, the standard Voronoi cell $V_i$ is reshaped into a final cell $F_i$ through intersection:

1. Safety Reshaping ($A_i$): The cell is modified to ensure the straight path from the robot's current position to the centroid of the cell is collision-free. This involves expanding the effective radius of robots and obstacles by their physical dimensions ($\delta$).
2. Flocking Reshaping ($M_i$): The cell is further constrained by a proximity set $M_i$, ensuring the robot stays within a maximum distance $\Gamma_{ij}$ from its designated neighbors.
3. Final Cell ($F_i = \tilde{V}^p_i \cap \tilde{V}^o_i \cap S_i \cap M_i$): The robot moves toward the centroid of this final convex set.

C. Adaptive Weighting Function ($\phi_i(q)$)

To handle tracking errors and measurement uncertainties, the authors introduce an adaptive weighting function $\phi_i(q)$ centered at a virtual goal $\bar{p}_i$.

• Mechanism: The "spread" of the weighting function is controlled by a parameter $\beta_i$.
• Adaptation Rule: $\beta_i$ is adjusted online. A smaller $\beta_i$ makes the robot more aggressive (centroid closer to the cell boundary), while a larger $\beta_i$ makes it conservative (centroid further from the boundary).
• Uncertainty Compensation: The algorithm calculates a minimum safe distance $d_u$ (sum of tracking error $d^t_u$ and measurement uncertainty $d^m_u$). It enforces a lower bound on $\beta_i$ ($\beta_{min}$) to ensure the centroid $c_{F_i}$ remains at least distance $d_u$ away from the cell boundary $\partial F_i$. This creates a "safety margin" that absorbs potential errors without violating collision or flocking constraints.

D. Control Implementation

• Dynamics: The control law is implemented via Model Predictive Control (MPC) to handle the non-holonomic dynamics of the UAVs.
• Sensors: The system relies on IMU, 2D Lidar, altitude sensors, and UVDAR (UltraViolet Direction And Ranging) for relative localization of neighbors.

3. Key Contributions

1. Novel Distributed Algorithm: A Lloyd-based controller that operates without explicit communication or global localization, suitable for dense forest environments.
2. Theoretical Guarantees: The paper provides sufficient conditions (Theorems 1 and 2) to mathematically guarantee:
   • Collision avoidance with obstacles and other robots.
   • Maintenance of flocking (proximity) constraints.
   • Robustness to bounded tracking errors and measurement uncertainties.
3. Adaptive Uncertainty Handling: A unique strategy using the adaptive parameter $\beta_i$ to dynamically adjust the robot's "aggressiveness" based on real-time error estimates, ensuring safety without needing to reshape the cell geometry dynamically (which could lead to undefined states).
4. Comprehensive Validation: The approach is validated through extensive simulations and real-world experiments in a dense forest with up to 4 UAVs, demonstrating operation without GNSS or external motion capture.

4. Results

Simulation Results

• Scalability: Tested with 9 and 16 robots in cluttered environments.
• Robustness: When the uncertainty margin ($d_u$) was set to 0, the system was faster but suffered collisions. When $d_u$ was set according to theoretical bounds (1.0m), the system achieved a 100% success rate in collision avoidance and flocking maintenance.
• Comparison: Compared against PACNav (a state-of-the-art non-communication algorithm). The proposed method significantly outperformed PACNav in complex scenarios (e.g., 9 robots), where PACNav failed to converge or maintain safety (0% success rate in some dense scenarios), while the proposed method maintained 100% success.

Experimental Results (Real-World Forest)

• Setup: 4 UAVs navigating 40 meters through a dense forest without GNSS.
• Performance: All 4 UAVs successfully reached the goal region.
• Metrics:
  • Average speeds ranged from 0.33 to 0.37 m/s.
  • Total mission time: ~150–175 seconds.
  • Safety & Flocking: The distance between robots remained within the defined safety and flocking constraints throughout the mission, despite high measurement noise and occlusions from trees.
• Handling Errors: Practical strategies (discarding high-uncertainty measurements and re-projecting positions) were successfully employed to handle temporary sensor failures caused by tree occlusions.

5. Significance

This work represents a significant step forward in autonomous multi-robot systems for real-world applications:

• Operational Independence: It proves that complex flocking behaviors can be achieved without relying on fragile communication networks or external infrastructure (like motion capture or GNSS), which is critical for disaster response, search and rescue, and agricultural monitoring in remote areas.
• Safety-Centric Design: Unlike many heuristic approaches, this method provides rigorous mathematical guarantees for safety and proximity, making it suitable for deployment in human-populated or highly cluttered environments.
• Uncertainty Awareness: The explicit integration of tracking and measurement errors into the control law's parameters ($\beta$) offers a robust framework for operating with imperfect sensors, a common reality in field robotics.
• Open Source: The authors provide open-source code and video demonstrations, facilitating reproducibility and further research in distributed robotics.

Limitations: The current implementation is restricted to 2D planar motion (constant altitude). Future work aims to extend the algorithm to 3D and improve agility.