Optimal Robot Formations: Balancing Range-Based Observability and User-Defined Configurations

TL;DR

The paper tackles balancing range based observability with task driven formation goals in multi robot systems using UWB ranging and EKF SLAM.It proposes a geometry based cost function framework that combines an adjacent formation term, a camera overlap term, and existing estimation and collision terms into a final offline objective $J_{cov}(x)$ to realize user defined formations.Formation optimization is performed with momentum based gradient descent and includes Hungarian matching to minimize travel when reordering robot IDs, all in a 2D planning context.In both simulation and real experiments, the high coverage formation achieved by minimizing $J_{cov}$ reduces coverage time substantially with only modest degradation in landmark and inter robot pose estimation, demonstrating practical benefits for inspection and coverage tasks.

Abstract

This paper introduces a set of customizable and novel cost functions that enable the user to easily specify desirable robot formations, such as a ``high-coverage'' infrastructure-inspection formation, while maintaining high relative pose estimation accuracy. The overall cost function balances the need for the robots to be close together for good ranging-based relative localization accuracy and the need for the robots to achieve specific tasks, such as minimizing the time taken to inspect a given area. The formations found by minimizing the aggregated cost function are evaluated in a coverage path planning task in simulation and experiment, where the robots localize themselves and unknown landmarks using a simultaneous localization and mapping algorithm based on the extended Kalman filter. Compared to an optimal formation that maximizes ranging-based relative localization accuracy, these formations significantly reduce the time to cover a given area with minimal impact on relative pose estimation accuracy.

Figures (9)

• Figure 1: Comparing the coverage span of two formations. The circles represent the camera's field-of-view of each robot, and the red dots denote the location of the ranging tags. (a) The robots are clustered together to ensure high relative pose estimation accuracy, as shown in Charles2022OptimalMF. (b) The robots are spread apart in a horizontal line to cover a larger area, which minimizes coverage time.
• Figure 2: Problem setup for a two-tag multi-robot system, where Robot $p$ is equipped with tags $\tau_i$ and $\tau_j$, and a camera with a circular view of radius $r_p$ in the up or down direction. Without loss of generality, the pink robot, defined as Robot $1$, is considered to be the reference robot.
• Figure 3: Formations obtained by minimizing $J_{\text{adj}}(\mbf{x})$. The contours represent the heatmap of the cost function $J_{\text{adj}}(\mbf{x})$, by varying the position vector, $\mbf{r}^{mn}_{n}$, between all the robots.
• Figure 4: The formation with adjacent camera overlap after minimizing $J_{\text{overlap}}$, with $\lambda = 0.25$. The left plot shows the effects of the heatmap of $J_{\text{overlap}}(\mbf{x})$ from the perspective of only Robot $1$, and the right plot shows the effects of the heatmap from the perspective of all the robots. Only position $\mbf{r}^{mn}_{n}$ is varied between all the robots to generate the heatmaps.
• Figure 5: Final formation acquisition with coverage in the $x$-direction without (top) and with (bottom) the camera overlap cost function, $J_\text{overlap}(\mbf{x})$.