# Multi-Robot Active Sensing for Bearing Formations

Nicola de Carli, Paolo Salaris, Paolo Robuffo Giordano

► To cite this version:

Nicola de Carli, Paolo Salaris, Paolo Robuffo Giordano. Multi-Robot Active Sensing for Bearing Formations. MSR 2023 - IEEE International Symposium on Multi-Robot & Multi-Agent Systems, Dec 2023, Boston (MA), United States. pp.1-7. hal-04216640v2

HAL Id: hal-04216640

https://hal.science/hal-04216640v2

Submitted on 7 Nov 2023

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
# Multi-Robot Active Sensing for Bearing Formations

Nicola De Carli, Paolo Salaris, Paolo Robuffo Giordano

**Abstract**— This paper proposes a novel distributed active sensing control strategy for formations of drones measuring relative bearings. To be able to localize their relative positions from bearing measurements, the drone formation must satisfy specific *Persistency of Excitation* (PE) conditions. We propose a solution that can meet these PE conditions by maximizing the information collected from onboard cameras via a distributed gradient-based algorithm. Additionally, we also consider presence of a (concurrent) position-based formation control task using Quadratic Program-based control with Control Lyapunov Functions (CLFs). The results show that the inclusion of active sensing in the formation control law enhances the localization accuracy and, as a consequence, the precision of reaching the desired formation. The improvement is especially important when the underlying graphs are not *Infinitesimally Bearing Rigid* (IBR), as it can be expected.

## I. INTRODUCTION

Cooperative localization from relative sensing is a relevant topic in the multi-robot community [1]–[5], it is especially relevant in GPS-denied environments, such as indoors. One specific area of interest is cooperative localization from bearing measurements [2]–[5], primarily due to the ease of retrieving relative bearings using onboard cameras. Much of the previous work in this domain has been built around the notion of *Infinitesimal Bearing Rigidity* (IBR) [2], [3]. IBR characterizes the conditions, primarily related to graph topology, under which a group of drones measuring constant relative bearings can localize themselves up to a global translation and scaling factor. In case of constant bearings, to resolve the scale ambiguity, a distance measurement among a pair of robots is necessary.

Maintaining formation rigidity can be, however, quite restrictive in terms of motion flexibility, especially when the presence of edges among robots depends on sensing constraints such as limited range, occlusions, and limited field of view (FoV). Recently, in [5], [6] the notion of *Bearing Persistently Exciting* (BPE) formations has been introduced, where at least a subset of the relative bearings is time-varying. By leveraging the concept of PE, this allows resolving the scale ambiguity and to relax the IBR assumption. A distributed observer is also proposed with Uniform Exponential (UE) convergence under the condition of the formation being BPE.

However, there remains the problem of how to ensure the BPE of the formation. Although a coordinated rotation was proposed as a reference trajectory satisfying the BPE condition [5], this strategy may not be desirable in practical

applications where more generic motions are needed. An alternative approach proposed in this work is to instead make use of an *active sensing* strategy that enables the optimization of robot motion for gathering informative measurements about the system state [7]–[9]. This can be exploited for producing a group motion that can (i) actively satisfy the necessary BPE conditions and (ii) be generic enough for fulfilling at the same time other tasks of interest related to the particular mission.

In the past, active sensing has been applied to the cooperative localization problem, e.g., in [10]–[12], where distance measurements together with rigidity and the presence of multiple anchors (i.e. robots knowing their poses) was assumed. An active sensing strategy was then used to steer the formation towards the best geometry from the localization point of view, without considering any PE condition. In [13], the authors considered the active information gathering for ensuring observability of a multi-robot system with two anchors and range measurements. In our previous work [14], we considered a formation with relative distance measurements, and we maximized the observability of the system over a future prediction horizon. The proposed method could be made distributed but at the cost of being suboptimal w.r.t. the centralized one and it was computationally heavy. In [15], an active sensing strategy to estimate the scale of an IBR formation was proposed. Most of the previous works mainly consider relative distance measurements instead of bearings, which are instead quite interesting from an application point of view. Some of these previous works also solely focus on optimizing the geometric arrangement of the group instead of the robots trajectories. In addition, the assumption of rigid formations is also often exploited (which, as explained, can be quite restrictive in terms of group mobility when considering sensing constraints).

In this paper, we propose a gradient-based control law that can address the above shortcomings. We consider maximization of the group observability by optimizing a global information measure: the derivative of the minimum nonzero eigenvalue of the weighted Observability Gramian (OG). This approach is simpler and computationally cheaper than receding horizon strategies and it allows maximizing a global quantity in a distributed way. Additionally, we address the coupling of the active cooperative localization strategy with presence of an independent primary task (formation control) whose performance is however enhanced by the active sensing action. This is obtained by leveraging distributed Quadratic Programming (QP)-based *Control Lyapunov Functions* (CLFs) [16]. The combination of active sensing and CLFs has previously been successfully applied to single robot scenarios employing a receding horizon strategy [9]. We believe that

N. De Carli and P. Robuffo Giordano are with CNRS, Univ Rennes, Inria, IRISA, Rennes, France. {nicola.de-carli, prg}@irisa.fr

P. Salaris is with the Research Center "E. Piaggio" and the Dept. of Information Engineering, University of Pisa, Pisa, Italy. paolo.salaris@unipi.it

This work was supported by the ANR-20-CHIA-0017 project "MULTI-SHARED"
the approach we propose is a promising direction as it allows to achieve cooperative tasks while optimizing the trajectories to enhance the observability of the formation.

The rest of the paper is organized as follows: Section II introduces the main modeling assumptions and the employed observer. Section III presents a gradient-based active sensing controller. Section IV outlines a strategy to integrate active sensing into a position-based formation control task, accommodating not necessarily bearing rigid formations. Section V presents statistical results on various graph topologies, and finally, Sect. VI concludes the paper.

## II. MULTI-ROBOT SYSTEM MODEL

### A. Graph Theory

We consider a group of $N$ connected drones. The drone sensing and communication interactions are modeled by a constant connected and undirected graph $G := (V, E)$, where $V = \{1, ..., N\}$ is the set of vertices and $E \subseteq V \times V$ is the set of edges. Two drones $i$ and $j$ are called neighbors if $(i, j) \in E$. The set of neighbors of drone $i$ is denoted by $N_i := \{j \in V | (i, j) \in E\}$ and its cardinality is $|N_i|$. With a slight abuse of notation, we may refer to variables related to the edge $e_k := (i, j)$ with a subscript $k$ or $ij$. The incidence matrix for an arbitrary orientation of the graph is denoted by $E$, with $[E]_{ik} = 1$ if vertex $i$ is the head of the edge $k$ and $[E]_{ik} = -1$ if it is the tail, $[E]_{ik} = 0$ otherwise. The Laplacian matrix associated to the graph $G$ can be computed as $L = EE^T$, while a weighted Laplacian is given by $L_W = EWE^T$, with $W \succeq 0$ (i.e. positive semidefinite). The Laplacian matrix is positive semidefinite with $L1_N = 0$, i.e. it has zero row-sum, and it has positive diagonal and non-positive off-diagonal entries. Its eigenvalues are $0 = \mu_1 \le \mu_2 \le ... \le \mu_N$ with eigenvectors $\{1_N/\sqrt{N}, v_2, ..., v_N\}$ and $\mu_2 > 0$ if and only if the graph is connected.

### B. Formation Model

Each drone state is represented by its position $\mathbf{p}_i \in \mathbb{R}^3$ and we refer to the stack of the drones position as $\mathbf{p} = [\mathbf{p}_1^T \quad ... \quad \mathbf{p}_N^T]^T \in \mathbb{R}^{3N}$. The positions and linear velocities of the drones are expressed in a common inertial frame and the velocity of drone $i$ is indicated as $\mathbf{u}_i \in \mathbb{R}^3$ while the stack of the drones velocities as $\mathbf{u} = [\mathbf{u}_1^T \quad ... \quad \mathbf{u}_N^T]^T \in \mathbb{R}^{3N}$. The dynamics of each drone is modeled as a simple single integrator:

$$ \dot{\mathbf{p}}_i = \mathbf{u}_i. \qquad (1) $$

Each drone is able to measure the relative bearing with respect to its neighbors, i.e.:

$$ \beta_{ij} := \frac{\mathbf{p}_{ij}}{d_{ij}} \in \mathcal{S}^2 \qquad (2) $$

where $\mathbf{p}_{ij} = \mathbf{p}_j - \mathbf{p}_i$ and $d_{ij} = \|\mathbf{p}_{ij}\|_2$. We also define the stack of bearing measurements $\beta = [\beta_1^T \quad ... \quad \beta_{|\varepsilon|}^T]^T \in \mathbb{R}^{3|\varepsilon|}$. The inclusion of sensing limitations is left as a future work.

### C. Localization of Bearing Formations

The matrix-valued Laplacian [17] associated to the classical position-based formation control is given as $\mathbf{L}_3 := \mathbf{E}_3\mathbf{E}_3^T$ [18], with $\mathbf{E}_3 = \mathbf{E} \otimes \mathbf{I}_3$ where $\otimes$ is the Kronecker product and $\mathbf{I}_3$ the identity matrix. If the graph is connected, then $\mathrm{rank}(\mathbf{L}_3) = 3N - 3$ and the null-space is $\mathcal{N}(\mathbf{L}_3) = \mathbf{U}$, where $\mathbf{U} = \mathbf{1}_N \otimes \mathbf{I}_3$ represents a basis for a common 3D translation of the formation. For bearing formations the bearing Laplacian matrix is defined as $\mathbf{L}_\beta := \mathbf{E}_3\mathrm{diag}(\mathbf{\Pi}_{\beta_k})\mathbf{E}_3^T$, with $\mathrm{diag}(\mathbf{\Pi}_{\beta_k})$ being the block diagonal matrix with blocks $\mathbf{\Pi}_{\beta_k} := \mathbf{I}_3 - \beta_k\beta_k^T \succeq 0$, which is an orthogonal projector, i.e. $\mathbf{\Pi}_{\beta_k} \mathbf{\Pi}_{\beta_k} = \mathbf{\Pi}_{\beta_k}$ and $\mathbf{\Pi}_{\beta_k} = \mathbf{\Pi}_{\beta_k}^T$. A bearing formation in $\mathbb{R}^3$ is defined *infinitesimally bearing rigid* (IBR) if $\mathrm{rank}(\mathbf{L}_\beta) = 3N - 4$. IBR is a necessary and sufficient condition to localize the relative positions of the robots up to a scale factor from constant bearing measurements. As it was shown in [5], this condition can be relaxed if the bearings are time-varying, implying the IBR condition is not anymore necessary and the scale ambiguity can be resolved. We give some necessary definitions before proceeding.

**Definition II.1.** Given a system with state $\mathbf{x} \in \mathbb{R}^d$, state sensitivity matrix $\mathbf{\Phi}(t, t_0) = \frac{\partial\mathbf{x}(t)}{\partial\mathbf{x}(t_0)}$, output $\mathbf{y}(t)$, and output Jacobian $\mathbf{H}(t) = \frac{\partial\mathbf{y}(t)}{\partial\mathbf{x}(t)}$, the Observability Gramian (OG) is defined as follows [7]:

$$ \mathbf{G}(t) := \mathbf{G}(t_0) + \int_{t_0}^{t} \mathbf{\Phi}(\tau, t_0)^T \mathbf{H}(\tau)^T \mathbf{H}(\tau) \mathbf{\Phi}(\tau, t_0) d\tau \quad (3) $$

Observability of the system linearized along the nominal trajectory is characterized by the invertibility of the OG [19], i.e. the state can be observed from the measured output $\mathbf{y}(t)$ if there exists some $\epsilon > 0$ such that:

$$ \int_{t_0}^{t} \mathbf{\Phi}(\tau, t_0)^T \mathbf{H}(\tau)^T \mathbf{H}(\tau) \mathbf{\Phi}(\tau, t_0) d\tau \succeq \epsilon \mathbf{I}_d. \quad (4) $$

**Definition II.2.** [5] A formation is defined *bearing persistently exciting* (BPE) if the graph $G$ is connected and given $T > 0$ and $\gamma > 0$ the following PE condition holds:

$$ \frac{1}{T} \int_{t}^{t+T} \mathbf{L}_{\beta}(\tau) d\tau \succeq \gamma \mathbf{L}_{3} \qquad (5) $$

It should be noted that this condition is less stringent than the classical PE condition since $\mathrm{dim}(\mathcal{N}(\mathbf{L}_3)) = 3$.

We point out that the previous integral in (5) represents a weighted Observability Gramian (OG) of the system. In fact, the state sensitivity matrix for a single integrator is the identity matrix and the output Jacobian is the so-called *bearing rigidity matrix*, i.e.:

$$ \mathbf{B} = \frac{\partial \beta}{\partial \mathbf{p}} = \mathrm{diag} \left( \frac{\mathbf{\Pi}_{\beta_k}}{d_k} \right) \mathbf{E}_3^T. \qquad (6) $$

From (4) and (6), using the properties of the orthogonal projector, it follows that the OG of the system can be expressed as:

$$ \mathbf{G}(t) = \mathbf{G}(t_0) + \int_{t_0}^{t} \mathbf{E}_3 \mathrm{diag} \left( \frac{\mathbf{\Pi}_{\beta_k}(\tau)}{d_k(\tau)^2} \right) \mathbf{E}_3^T d\tau \quad (7) $$

where we see the similarity to (5), in which the information associated to each edge is weighted by the squared distance. Since the scaling by the positive distances does not affect the
rank of the matrix, it follows that the formation being BPE is a local observability condition of the subspace orthogonal to the common translation.

We now introduce the observer presented in [5], which we use for localizing the drones in a bearing formation. In [5], it is shown that, if the formation is BPE, the bearings are always well-defined (i.e. no collisions occur) and the velocities are bounded, then the following distributed observer achieves UE convergence to the true state up to a common translation:

$$ \dot{\mathbf{p}}(t) = \mathbf{u}(t) - k_e \mathbf{L}_{\beta}(\mathbf{p}(t))\hat{\mathbf{p}}(t) \quad (8) $$

where $\hat{\mathbf{p}}$ is the estimated position vector and $k_e > 0$.

The convergence rate of this observer depends on the BPE parameter $\gamma$ [5]. The aim of the following section is to then provide a control law that aims at indirectly increasing the value of $\gamma$ for ensuring a satisfactory observer performance even for non-rigid bearing formations.

### III. ACTIVE SENSING CONTROL

In this section, we propose a gradient-based control law for actively collecting information about the drone relative positions. To quantify the amount of information, we adopt the minimum nonzero eigenvalue, specifically the fourth eigenvalue denoted as $\lambda_4$, of the weighted OG (5) but with an additional forgetting factor, that is, an OG with dynamics

$$ \dot{\mathbf{G}}_{\beta}(t) = -\rho \mathbf{G}_{\beta}(t) + \mathbf{L}_{\beta}(t). \quad (9) $$

The forgetting factor $\rho > 0$ ensures that the OG remains bounded, since otherwise the term $\mathbf{L}_{\beta} \succeq 0(t)$, $\forall t \ge 0$ would make the OG grow unbounded over time. Notice that if $\mathbf{G}_{\beta}(t_0)$ is a matrix-weighted Laplacian [17] associated to the graph $\mathcal{G}$, then this also holds for $\mathbf{G}_{\beta}(t)$, $\forall t \ge t_0$, that is, (i) the required definiteness pattern of the matrix weights (negative semidefinite off-diagonal terms) is preserved:

$$ \begin{aligned} [\mathbf{G}_{\beta}(t)]_{(ij)} &= e^{-\rho(t-t_0)} [\mathbf{G}_{\beta}(t_0)]_{(ij)} + \int_{t_0}^{t} e^{-\rho(t-\tau)} [\mathbf{L}_{\beta}(\tau)]_{(ij)} d\tau \\ &= e^{-\rho(t-t_0)} [\mathbf{G}_{\beta}(t_0)]_{(ij)} - \int_{t_0}^{t} e^{-\rho(t-\tau)} \mathbf{\Pi}_{\beta_{ij}}(\tau) d\tau \preceq 0 \end{aligned} \quad (10) $$

where $[\mathbf{G}_{\beta}(t_0)]_{(ij)} \preceq 0$ is the $ij$-th block of $\mathbf{G}_{\beta}(t_0)$ and $\mathbf{\Pi}_{\beta_{ij}} \succeq 0$; (ii) $\mathbf{U} \subseteq \mathcal{N}(\mathbf{G}_{\beta}(t))$:

$$ \mathbf{G}_{\beta}(t)\mathbf{U} = e^{-\rho(t-t_0)}\mathbf{G}_{\beta}(t_0)\mathbf{U} + \int_{t_0}^{t} e^{-\rho(t-\tau)}\mathbf{L}_{\beta}(\tau)\mathbf{U} d\tau = \mathbf{0}_{3N \times 3N} \quad (11) $$

and; (iii) if $ij \notin \mathcal{E}$ then $[\mathbf{G}_{\beta}(t)]_{(ij)} = \mathbf{0} \quad \forall t \ge 0$. These properties are used in the next derivations. In particular, the preservation of the sparsity pattern allows for a distributed implementation of the proposed control law.

Consider the state of the full system $\zeta = [\mathbf{p}^T \ \text{vec}(\mathbf{G}_{\beta})^T]^T$, where $\text{vec}(\cdot)$ is the vectorization operator. Then, the system dynamics can be written as:

$$ \dot{\zeta}(t) = \mathbf{f}(\zeta) + \mathbf{g} \mathbf{u} \quad (12) $$

where the system has a cascade structure with $\mathbf{f}(\zeta) = [\mathbf{0}_{3N}^T \ \text{vec}(\dot{\mathbf{G}}_{\beta})^T]^T$ and $\mathbf{g} = [\mathbf{I}_{3N} \ \mathbf{0}_{3N \times (3N)^2}]^T$. Note that the first derivative of $\lambda_4$ (our 'information metric') depends on the positions of the drones, so that $\lambda_4$ has

relative degree 2 w.r.t. the drone velocities (the available control inputs). This poses a challenge for direct control since the drone positions are solely involved in the dynamics of $\lambda_4$ within the orthogonal projector $\mathbf{\Pi}_{\beta}$. However, the derivative of $\lambda_4$ can be more easily controlled. This has expression

$$ \begin{aligned} \dot{\lambda}_4 &= L_f \lambda_4 = (\mathbf{v}_4 \otimes \mathbf{v}_4)^T \text{vec}(\dot{\mathbf{G}}_{\beta}) \\ &= - \sum_{(i,j) \in \mathcal{E}} (\mathbf{v}_{4i} - \mathbf{v}_{4j})^T [\dot{\mathbf{G}}_{\beta}(t)]_{(ij)} (\mathbf{v}_{4i} - \mathbf{v}_{4j}) \end{aligned} \quad (13) $$

where $L_f \lambda_4 = \frac{\partial \lambda_4}{\partial \zeta} \mathbf{f}(\zeta)$ is the Lie derivative of $\lambda_4$ along $\mathbf{f}$ (analogously we also define $\mathbf{L}_{g_i}$) and $\mathbf{v}_4 = [\mathbf{v}_{41} \ \dots \ \mathbf{v}_{4N}]^T$ is the eigenvector associated to $\lambda_4$. For convenience, let us define $\mathbf{v}_{4ij} := \mathbf{v}_{4i} - \mathbf{v}_{4j}$. The second order derivative of $\lambda_4$ is then

$$ \begin{aligned} \ddot{\lambda}_4 &= \sum_{i=1}^{N} L_{g_i} L_f \lambda_4 \mathbf{u}_i + L_f^2 \lambda_4 \\ &= -\frac{1}{2} \sum_{i=1}^{N} \left( \sum_{j \in \mathcal{N}_i} [\mathbf{v}_{4ij} \otimes \mathbf{v}_{4ji}]^T \frac{\partial \text{vec}([\dot{\mathbf{G}}_{\beta}]_{ij})}{\partial \mathbf{p}_i} \right) \mathbf{u}_i \\ &\quad + \text{vec}(\dot{\mathbf{G}}_{\beta})^T \mathbf{H}_{\lambda} \text{vec}(\dot{\mathbf{G}}_{\beta}) - \rho (\mathbf{v}_4 \otimes \mathbf{v}_4)^T \text{vec}(\dot{\mathbf{G}}_{\beta}) \end{aligned} \quad (14) $$

where $\mathbf{H}_{\lambda} := \frac{\partial^2 \lambda_4}{\partial \text{vec}(\mathbf{G}_{\beta}) \partial \text{vec}(\mathbf{G}_{\beta})^T}$ is the hessian of $\lambda_4$ with respect to the matrix entries. The important point to note here is that (14) has a direct (affine) dependence on the control inputs $\mathbf{u}_i$, which can then be exploited for controlling the quantity $\dot{\lambda}_4$, for instance by applying the gradient-based control law

$$ \mathbf{u}_i := k (L_{g_i} L_f \lambda_4)^T. \quad (15) $$

The control action (15) implements the sought active sensing since it aims at maximizing $\dot{\lambda}_4$ that, indirectly, maximizes the eigenvalue $\lambda_4$ itself (which is the metric we care about, but which is less directly controllable by acting on the control inputs $\mathbf{u}_i$).

We now discuss some properties of the control law (15).

**Proposition III.1.** From an information perspective, it is intuitive that the gradient of $\dot{\lambda}_4$ associated to an edge is orthogonal to the corresponding bearing, i.e.:

$$ [\mathbf{v}_{4ij} \otimes \mathbf{v}_{4ji}]^T \frac{\partial \text{vec}([\dot{\mathbf{G}}_{\beta}]_{ij})}{\partial \mathbf{p}_i} \beta_{ij} = 0 \quad (16) $$

*Proof.* First of all, notice that $[\dot{\mathbf{G}}_{\beta}]_{(ij)} = -\rho[\mathbf{G}_{\beta}]_{(ij)} - \mathbf{\Pi}_{\beta_{ij}}$ and the first term does not depend on $\mathbf{p}_i$. Furthermore, using the properties of the Kronecker product, we have $\text{vec}(\beta_{ij}\beta_{ij}^T) = \beta_{ij} \otimes \beta_{ij}$. It follows that:

$$ \frac{\partial \text{vec}(\mathbf{\Pi}_{\beta_{ij}})}{\partial \mathbf{p}_i} = - \frac{\partial(\beta_{ij} \otimes \beta_{ij})}{\partial \mathbf{p}_i} = \frac{1}{d_{ij}} (\mathbf{\Pi}_{\beta_{ij}} \otimes \beta_{ij} + \beta_{ij} \otimes \mathbf{\Pi}_{\beta_{ij}}) \quad (17) $$

One can then use the Kronecker product properties and show that:

$$ \begin{aligned} & (\mathbf{\Pi}_{\beta_{ij}} \otimes \beta_{ij} + \beta_{ij} \otimes \mathbf{\Pi}_{\beta_{ij}}) \beta_{ij} \\ &= (\mathbf{\Pi}_{\beta_{ij}} \otimes \beta_{ij}) (\beta_{ij} \otimes 1) + (\beta_{ij} \otimes \mathbf{\Pi}_{\beta_{ij}}) (1 \otimes \beta_{ij}) \quad (18) \\ &= (\mathbf{\Pi}_{\beta_{ij}} \beta_{ij} \otimes \beta_{ij}) + (\beta_{ij} \otimes \mathbf{\Pi}_{\beta_{ij}} \beta_{ij}) = \mathbf{0}. \end{aligned} $$

□
**Proposition III.2.** Assuming that no collisions happen i.e. $d_{ij} \ge D_{\min} > 0$, $\forall i, j \in \{1, 2, ..., N\}$, then the resulting input is always bounded.

*Proof.* w.l.o.g. considering $k = 2$:

$$ \mathbf{u}_i^T = \sum_{j \in \mathcal{N}_i} [\mathbf{v}_{4ij} \otimes \mathbf{v}_{4ij}]^T \frac{1}{d_{ij}} (\mathbf{\Pi}_{\beta_{ij}} \otimes \beta_{ij} + \beta_{ij} \otimes \mathbf{\Pi}_{\beta_{ij}}) \quad (19) $$

and

$$ \begin{aligned} \|\mathbf{u}_i\|_2 &\le \sum_{j \in \mathcal{N}_i} \|\mathbf{v}_{4ij} \otimes \mathbf{v}_{4ij}\|_2 \frac{2}{D_{\min}} \|\mathbf{\Pi}_{\beta_{ij}} \otimes \beta_{ij}\|_2 \\ &\le \frac{8}{D_{\min}} |\mathcal{N}_i| \end{aligned} \quad (20) $$

where we used the subadditivity property of norms, the facts that $\|\mathbf{v}_{4i}\|_2 \le \|\mathbf{v}_4\|_2 = 1$, $\|\mathbf{X} \otimes \mathbf{Y}\|_2 = \|\mathbf{X}\|_2 \cdot \|\mathbf{Y}\|_2$, $\|\mathbf{\Pi}_{\beta}\|_2 = 1$ and $\|\beta_{ij}\|_2 = 1$. □

Finally, the active sensing gradient control (15) requires each robot $i$ to only know quantities which are locally available or communicated by neighboring robots, i.e.,

* $\mathbf{v}_{4i}$: the components of the eigenvector corresponding to the $i$-th robot itself, which can be estimated in a distributed way by suitably modifying the distributed power iteration method (see e.g. [20], [21]) as done e.g. in [22];

* $\mathbf{v}_{4j} \forall j \in \mathcal{N}_i$: which can be communicated by neighboring robots;

* $d_{ij}$: for which an estimate $\hat{d}_{ij}$ can be computed from the estimated positions; $\hat{p}_i$, locally available, and $\hat{p}_j$ which can be communicated by the neighbors

* $\beta_{ij}$ which is measured.

In the next section, we show how to embed the active sensing action (15) within a primary task of formation control.

## IV. FORMATION CONTROL

We establish a hierarchical framework that prioritizes position-based formation control over active sensing by using distributed QP-based Control Lyapunov Functions [16]. We now formally introduce the definition of CLFs which will be instrumental for the next developments.

**Definition IV.1.** A function $\alpha: \mathbb{R}_{\ge 0} \to \mathbb{R}_{\ge 0}$ is of class $\mathcal{K}$ if it is strictly increasing and $\alpha(0) = 0$

**Definition IV.2.** [23] A smooth positive definite function $V: \mathbb{R}^d \to \mathbb{R}$ is a Control Lyapunov Functions (CLF) for a given system $\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}) + \mathbf{g}(\mathbf{x})\mathbf{u}$ if it satisfies:

$$ \inf_{u \in \mathbb{R}^m} \{L_f V(\mathbf{x}) + L_g V(\mathbf{x})\mathbf{u}\} \le -\alpha(V(\mathbf{x})) \quad (21) $$

with $\alpha$ of class $\mathcal{K}$.

The position-based formation control task considered in this work is defined based on the error associated with each edge, denoted as $\mathbf{e}_{ij} = \mathbf{p}_j - \mathbf{p}_i - \mathbf{p}_{ij}^d$. Here, $\mathbf{p}_{ij}^d$ represents the desired relative position between robot $i$ and $j$. We also define the desired position vector as $\mathbf{p}^d = [(\mathbf{p}_i^d)^T \quad \dots \quad (\mathbf{p}_N^d)^T]^T$, which relates to the relative desired position vector $\mathbf{p}_\mathcal{E}^d = \text{stack}(\mathbf{p}_{ij}^d)$ through $\mathbf{p}_\mathcal{E}^d = \mathbf{E}_3^T \mathbf{p}^d$. It is worth noting that $\mathbf{p}^d$ is defined up to a common translation. Then, the position error

is $\mathbf{e} = \mathbf{p} - \mathbf{p}^d = [\mathbf{e}_1^T \quad \dots \quad \mathbf{e}_N^T]^T$ with dynamics $\dot{\mathbf{e}} = \mathbf{u} - \dot{\mathbf{p}}^d$. We consider the following potential function:

$$ V(\mathbf{e}) = \frac{1}{2} \sum_{(i,j) \in \mathcal{E}} \|\mathbf{e}_i - \mathbf{e}_j\|_2^2. \quad (22) $$

We emphasize that, while this function is only positive semidefinite i.e. $V(\mathbf{w}) = 0$ for any vector $\mathbf{w} \in \text{span}(\mathbf{U})$ representing a common translation, it is positive definite with respect to the desired equilibrium set $\{\mathbf{e}_1 = \mathbf{e}_2 = \dots = \mathbf{e}_N\}$. Let us define $\mathbf{c}_i(\zeta) := (L_{g_i} L_{f_i} \lambda_i)^T / \|L_{g_i} L_{f_i} \lambda_i\|_2$ if $\|L_{g_i} L_{f_i} \lambda_i\|_2 \ne 0$ and $\mathbf{c}_i(\zeta) = 0$ otherwise (vector $\mathbf{c}_i(\zeta)$ is thus the unit-norm direction of the active sensing control (15)). The centralized QP, including the active sensing task, is formulated as:

$$ \begin{array}{ll} \underset{\mathbf{u}_i, i=1,\dots,N}{\text{min}} & \frac{1}{2} \sum_{i=1}^{N} \left[ \left( \mathbf{c}_i(\zeta)^T (\mathbf{u}_i - k\mathbf{c}_i(\zeta)) \right)^2 + \eta \|\mathbf{\Pi}_{c_i} \mathbf{u}_i\|_2^2 \right] \\ \text{s.t.} & \sum_{i=1}^{N} \left( \sum_{j \in \mathcal{N}_i} (\mathbf{e}_i - \mathbf{e}_j) \right)^T \mathbf{u}_i + \frac{1}{2} \sum_{(i,j) \in \mathcal{E}} \alpha_i (\|\mathbf{e}_i - \mathbf{e}_j\|_2^2) \le 0 \end{array} \quad (23) $$

where $\mathbf{\Pi}_{c_i} = \mathbf{I}_3 - \mathbf{c}_i \mathbf{c}_i^T$ is the orthogonal projector onto the plane perpendicular to $\mathbf{c}_i$, $\eta > 0$ and $\alpha_i(\cdot)$ is an extended class $\mathcal{K}$.

The first term in the cost function aims at achieving the same information gain as the one obtained by $\mathbf{u}_i = k\mathbf{c}_i(\zeta)$ and, therefore, it represents the active sensing task. The second (regularization) term in the cost function is meant to address two issues, namely $(i)$ avoiding excessive inputs in the direction orthogonal to the active sensing task due to constraint satisfaction, and $(ii)$ obtaining a strongly convex cost function. Indeed, the Hessian of the QP is given by $\mathbf{H}_{QP} := \text{diag}(\mathbf{H}_{QP,i}) = \text{diag}(\mathbf{c}_i \mathbf{c}_i^T + \eta \mathbf{\Pi}_{c_i}) \succ 0$.

Our objective now is to transform this problem into one that is suitable for distributed implementation. Inspired by [24], we compute the analytical expression for the solution of the QP. First, we define $\mathbf{a} = [\mathbf{a}_1^T \quad \dots \quad \mathbf{a}_N^T]^T$, with $\mathbf{a}_i := \sum_{j \in \mathcal{N}_i} (\mathbf{e}_i - \mathbf{e}_j)$, and $b = \sum_{i=1}^N b_i$, with $b_i := \frac{1}{4} \sum_{j \in \mathcal{N}_i} \alpha_i (\|\mathbf{e}_i - \mathbf{e}_j\|_2^2)$, so that the constraint can be written as $\sum_{i=1}^N (\mathbf{a}_i^T \mathbf{u}_i + b_i) \le 0$. We point out that, in absence of input limits, the constraint is always feasible, i.e. $\mathbf{a} = 0$ implies $b = 0$, in which case the constraint is trivially satisfied. The analytical solution to the QP can be obtained by using the Karush-Kuhn-Tucker (KKT) optimality conditions. Let us define the Lagrangian of the problem

$$ \mathcal{L} := \frac{1}{2} \sum_{i=1}^{N} \left[ \left( \mathbf{c}_i(\zeta)^T (\mathbf{u}_i - k\mathbf{c}_i(\zeta)) \right)^2 + \eta \|\mathbf{\Pi}_{c_i} \mathbf{u}_i\|_2^2 \right] + \lambda_{\mathcal{L}} (\mathbf{a}^T \mathbf{u} + b), \quad (24) $$

with $\lambda_{\mathcal{L}}$ being the Lagrange multiplier. The resulting KKT conditions, using * to indicate the optimal solution, are:

$$ \left\{ \begin{array}{l} \frac{\partial \mathcal{L}}{\partial \mathbf{u}_i} = (\mathbf{u}_i^* - k\mathbf{c}_i)^T \mathbf{c}_i \mathbf{c}_i^T + \eta (\mathbf{u}_i^*)^T \mathbf{\Pi}_{c_i} + \lambda_{\mathcal{L}}^* \mathbf{a}_i = \mathbf{0} \quad \forall i \\ \sum_{i=1}^{N} (\mathbf{a}_i^T \mathbf{u}_i^* + b_i) \le 0 \\ \lambda_{\mathcal{L}}^* \ge 0 \\ \lambda_{\mathcal{L}}^* = 0 \quad \text{if} \quad \sum_{i=1}^{N} (\mathbf{a}_i^T \mathbf{u}_i^* + b_i) < 0 \end{array} \right. . \quad (25) $$

From the first equation one obtains:

$$ \mathbf{u}_i^* = \mathbf{H}_{QP,i}^{-1} (k\mathbf{c}_i - \lambda_{\mathcal{L}}^* \mathbf{a}_i) \quad (26) $$
from which two cases are possible: 1) the constraint is not active at the unconstrained solution $k\mathbf{H}_{QP,i}^{-1}\mathbf{c}_i$, hence from the last condition one has $\lambda_{\mathcal{L}}^* = 0$ and $\mathbf{u}_i^* = k\mathbf{H}_{QP,i}^{-1}\mathbf{c}_i$; 2) the constraint is active, hence substituting (26) into the constraint equation with equality yields

$$ \lambda_{\mathcal{L}}^* = \frac{\sum_{i=1}^{N}(\mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1} k \mathbf{c}_i + b_i)}{\sum_{i=1}^{N} \mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1} \mathbf{a}_i}. \quad (27) $$

Substituting back in (26), one obtains the complete solution for the two cases as:

$$ \mathbf{u}_i = k \mathbf{H}_{QP,i}^{-1} \mathbf{c}_i - \max \left( 0, \frac{\sum_{i=1}^{N} (k \mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1} \mathbf{c}_i + b_i)}{\sum_{i=1}^{N} \mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1} \mathbf{a}_i} \right) \mathbf{H}_{QP,i}^{-1} \mathbf{a}_i. \quad (28) $$

Since $\mathbf{H}_{QP,i} \succ 0$, a zero denominator implies $\mathbf{a}_i = 0 \ \forall i$, that is, accomplishment of the formation control task. Hence, in this case the input can be set to zero. Otherwise, the first term represents the solution to the unconstrained problem. The term in the numerator $\sum_{i=1}^{N}(k\mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1}\mathbf{c}_i + b_i)$ is negative when the constraint is satisfied by the solution to the unconstrained problem. Since the term at denominator is always greater than zero, then the second term is different from zero only when the constraint is active and it acts to correct the input in order to satisfy the constraint. Also, notice that the proposed controller is piecewise (because of the eigenvalues re-ordering) locally Lipschitz continuous [25].

The two terms $\sum_{i=1}^{N}(k\mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1}\mathbf{c}_i + b_i)$ and $\sum_{i=1}^{N} \mathbf{a}_i^T \mathbf{H}_{QP,i}^{-1} \mathbf{a}_i$ are not locally available but one can estimate their average through dynamic consensus, e.g. [26]. Finally notice that the terms $\mathbf{a}_i$ and $b_i$ depend on the estimated positions of robot $i$ and its neighbors, which must then be estimated as accurately as possible to correctly achieve the formation task. This is achieved by the gradient-based active sensing strategy embedded in the cost function of (23).

## V. SIMULATION RESULTS

In this section, we show the effectiveness of our approach through extensive numerical simulations. We compare the results obtained applying the proposed control law (28), which achieves the desired formation while performing active sensing against a control law which only implements formation control by satisfying the constraint in (23) while minimizing the input norm. For convenience, we refer to the two methods, respectively, as AS and CLF-only. We consider a group of $N = 7$ drones, which can sense the neighboring robots without any sensing constraints and three different graph topologies with different levels of connectivity. We performed a set of 50 simulations starting from random initial positions for each of the following graph topologies (ordered in increasing connectivity level): line graph, cycle graph and 1-redundantly bearing rigid (RBR) graph [4] (Fig. 1). Notice that the first two graphs are not bearing rigid. The drones initial estimated position is drawn from a gaussian distribution centered around the real position with standard deviation $0.8m^2$ along each axis. As desired formation, we chose to have the drones equally spaced along a circle lying on a plane parallel to the x-y plane.

<figure><img src="image_2.png" /><figcaption>Fig. 1: Graphs corresponding to the sets of simulations.</figcaption></figure>

The decrease of the CLF imposed in the constraint is the same for CLF-only and AS, hence, the converge speed of the formation is not of particular interest. We instead focus on the (more relevant) evaluation of the steady state formation error at convergence, and of the estimation error. In the simulations, we used the following parameters: the observer gain is $k_e = 0.1$, the forgetting factor is $\rho = 0.04$, the OG is initialized as a matrix weighted Laplacian with matrix weights $0.5\mathbf{I}_3$, the active sensing gain is $k = 0.2$, the regularization term is $\eta = 0.01$. We also implemented Control Barrier Functions (CBFs), which filters the inputs provided by the proposed controller to ensure collision-free trajectories [27].

We present the results for the line graph in Fig.2, the cycle graph in Fig.3, and the 1-redundant bearing rigid graph in Fig. 4. In each case, the figures are arranged from left to right to display: (a) the trajectory of $\lambda_4(t)$, (b) the estimation error, (c) the formation error, (d) and (e) the violin plot respectively for the estimation and formation error at the end of the trajectory. As a reminder, $\lambda_4(t)$ is the minimum non-zero eigenvalue of the OG and it is a measure of the collected information which affects the worst case convergence rate of the employed localization observer. The plots (a), (b) and (c) show the average trajectories along with the standard deviation of the results across simulations.

The results clearly show that the use of active sensing leads to higher acquired information and, thus, a more accurate localization and, as a consequence, a better performance for the formation control. The use of active sensing provides benefits, particularly in scenarios where the connectivity is lower, as for the line and cycle graphs. In these cases, the active sensing task reduces both the estimation and formation errors. Conversely, in highly connected and rigid graphs like the 1-redundantly rigid graph, the improvement margin is quite small. Indeed, in this case, rigidity of the graph greatly simplifies the localization task since the robot group becomes “instantaneously localizable” (up to a scale factor). This is also evident from Fig.4(c), where the information growth is considerably high even when active sensing is not used (CLF-only case).

## VI. CONCLUSIONS AND FUTURE WORKS

In this paper, we have presented a novel distributed control strategy for bearing formations to maximize the information gathered for cooperative localization. Our active sensing strategy is based on the minimum eigenvalue of the weighted Observability Gramian with a forgetting factor as an information measure. This active sensing strategy can be combined with other additional tasks of interest (a
<figure><img src="image_3.png" /><figcaption>(a) Trajectory of $\lambda_4$</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>(b) Estimation error norm</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>(c) Formation error norm</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>(d) Violin plot of the estimation(e) Violin plot of the formation error norm at the final time error norm at the final time</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>(a) Trajectory of $\lambda_4$</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>(b) Estimation error norm</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>(c) Formation error norm</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>(d) Violin plot of the estimation(e) Violin plot of the formation error norm at the final time error norm at the final time</figcaption></figure>

<figure><img src="image_11.png" /><figcaption>(a) Trajectory of $\lambda_4$</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>(b) Estimation error norm</figcaption></figure>

<figure><img src="image_13.png" /><figcaption>(c) Formation error norm</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>(d) Violin plot of the estimation(e) Violin plot of the formation error norm at the final time error norm at the final time</figcaption></figure>

<caption>Fig. 4: 1-Redundantly Rigid Graph.</caption>

position-based formation control task in our case) by making use of distributed QP-based control with Control Lyapunov Functions. This integration enables the execution of higher-level tasks while concurrently enhancing the minimum level of "localizability" for the robot group, thereby improving the overall task execution.

Numerical simulations over graphs with different levels of connectivity demonstrated the benefits of the approach, in particular for non-rigid graphs. Future works will focus on including sensing constraints such as limited range and field of view, as well as accounting for the drones orientation. By addressing these factors, we can develop a more comprehensive and practical framework that considers the corresponding directed time-varying graph. Such advancements will enhance the applicability and robustness of our proposed strategy in real-world scenarios.

## REFERENCES

[1] L. Krick, M. E. Broucke, and B. A. Francis, "Stabilisation of infinitesimally rigid formations of multi-robot networks," *International Journal of control*, vol. 82, no. 3, pp. 423–439, 2009.

[2] F. Schiano, A. Franchi, D. Zelazo, and P. Robuffo Giordano, "A rigidity-based decentralized bearing formation controller for groups of quadrotor uavs," in *2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2016, pp. 5099–5106.

[3] S. Zhao, Z. Li, and Z. Ding, "Bearing-only formation tracking control of multiagent systems," *IEEE Transactions on Automatic Control*, vol. 64, no. 11, pp. 4541–4554, 2019.

[4] M. H. Trinh, Q. Van Tran, and H.-S. Ahn, "Minimal and redundant bearing rigidity: Conditions and applications," *IEEE Transactions on Automatic Control*, vol. 65, no. 10, pp. 4186–4200, 2019.

[5] Z. Tang, R. Cunha, T. Hamel, and C. Silvestre, "Relaxed bearing rigidity and bearing formation control under persistence of excitation," *Automatica*, vol. 141, p. 110289, 2022.

[6] Z. Tang and A. Loría, "Localization and tracking control of autonomous vehicles in time-varying bearing formation," *IEEE Control Systems Letters*, 2022.

[7] A. J. Krener and K. Ide, "Measures of unobservability," in *Proceedings of the 48th IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference*. IEEE, 2009, pp. 6401–6406.

[8] P. Salaris, M. Cognetti, R. Spica, and P. Robuffo Giordano, "Online optimal perception-aware trajectory generation," *IEEE Transactions on Robotics*, vol. 35, no. 6, pp. 1307–1322, 2019.

[9] O. Napolitano, D. Fontanelli, L. Pallottino, and P. Salaris, "Information-aware lyapunov-based mpc in a feedback-feedforward control strategy for autonomous robots," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 4765–4772, 2022.

[10] J. Le Ny and S. Chauvière, "Localizability-constrained deployment of mobile robotic networks with noisy range measurements," in *2018 Annual American Control Conference (ACC)*. IEEE, 2018, pp. 2788–2793.

[11] C. C. Cossette, M. A. Shalaby, D. Saussié, J. Le Ny, and J. R. Forbes, "Optimal multi-robot formations for relative pose estimation using range measurements," in *2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2022, pp. 2431–2437.

[12] J. Cano and J. L. Ny, "Ranging-based localizability-constrained deployment of mobile robotic networks," *arXiv preprint arXiv:2202.00756*, 2022.

[13] F. Riz, L. Palopoli, and D. Fontanelli, "Perception-aware trajectory planning for a pair of unicycle-like robots with absolute and relative ranging measurements," *IEEE Control Systems Letters*, 2023.
[14] N. De Carli, P. Salaris, and P. Robuffo Giordano, "Online decentralized perception-aware path planning for multi-robot systems," in *2021 International Symposium on Multi-Robot and Multi-Agent Systems (MRS)*. IEEE, 2021, pp. 128-136.

[15] R. Spica and P. Robuffo Giordano, "Active decentralized scale estimation for bearing-based localization," in *2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2016, pp. 5084-5091.

[16] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, "Control barrier functions: Theory and applications," in *2019 18th European control conference (ECC)*. IEEE, 2019, pp. 3420-3431.

[17] L. Pan, H. Shao, M. Mesbahi, Y. Xi, and D. Li, "Consensus on matrix-weighted time-varying networks," *arXiv preprint arXiv:2001.11179*, 2020.

[18] J. Cortés and M. Egerstedt, "Coordinated control of multi-robot systems: A survey," *SICE Journal of Control, Measurement, and System Integration*, vol. 10, no. 6, pp. 495-503, 2017.

[19] P. Bernard, N. Mimmo, and L. Marconi, "On the semi-global stability of an ek-like filter," *IEEE Control Systems Letters*, vol. 5, no. 5, pp. 1771-1776, 2020.

[20] P. Yang, R. A. Freeman, G. J. Gordon, K. M. Lynch, S. S. Srinivasa, and R. Sukthankar, "Decentralized estimation and control of graph connectivity for mobile sensor networks," *Automatica*, vol. 46, no. 2, pp. 390-396, 2010.

[21] I. Malli, C. P. Bechlioulis, and K. J. Kyriakopoulos, "Robust distributed estimation of the algebraic connectivity for networked multi-robot systems," in *2021 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2021, pp. 9155-9160.

[22] D. Zelazo, A. Franchi, H. H. Bülthoff, and P. Robuffo Giordano, "Decentralized rigidity maintenance control with range measurements for multi-robot systems," *The International Journal of Robotics Research*, vol. 34, no. 1, pp. 105-128, 2015.

[23] H. K. Khalil, *Nonlinear control*. Pearson New York, 2015, vol. 406.

[24] X. Tan and D. V. Dimarogonas, "Distributed control barrier function-based control scheme for multi-agent systems under a collective constraint," in *the 25th International Symposium on Mathematical Theory of Networks and Systems Bayreuth, Germany, 12-16 September 2022*. Elsevier, 2022, pp. 571-574.

[25] X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames, "Robustness of control barrier functions for safety critical control," *IFAC-PapersOnLine*, vol. 48, no. 27, pp. 54-61, 2015.

[26] R. A. Freeman, P. Yang, and K. M. Lynch, "Stability and convergence properties of dynamic average consensus estimators," in *Proceedings of the 45th IEEE Conference on Decision and Control*. IEEE, 2006, pp. 338-343.

[27] L. Wang, A. D. Ames, and M. Egerstedt, "Safety barrier certificates for collisions-free multirobot systems," *IEEE Transactions on Robotics*, vol. 33, no. 3, pp. 661-674, 2017.