# Tracking Control of Cooperative Marine Vehicles
Under Hard and Soft Constraints

Esteban Restrepo, Josef Matouš, and Kristin Y. Pettersen Fellow, IEEE

**Abstract**— We solve the tracking-in-formation problem for a group of underactuated autonomous marine vehicles interconnected over a directed topology. The agents are subject to **hard** inter-agent constraints, i.e. connectivity maintenance and collision avoidance, and **soft** constraints, specifically on the non-negativity of the surge velocity, as well as to constant disturbances in the form of unknown ocean currents. The control approach is based on an input-output feedback linearization for marine vehicles and on the edge-based framework for multi-agent consensus under constraints. We establish **almost-everywhere** uniform asymptotic stability of the output dynamics with guaranteed respect of the constraints. High-fidelity simulations are provided to illustrate our results.

**Index Terms**— Formation control, multi-agent systems, marine vehicles, control under constraints.

## I. INTRODUCTION

Applications in marine environments, ranging from transportation to seafloor mapping and infrastructure inspection, often require the use of autonomous systems due to the remoteness or inaccessibility of such environments. Recently, fleets of surface vehicles (ASVs) and underwater vehicles (AUVs) have been increasingly considered for such applications due to the advantages they offer with respect to single vehicles in terms of versatility, resiliency, and reduced cost [1].

In many applications, the mission objective of the multi-agent system can be expressed as tracking a predefined trajectory in a formation [2]. Numerous approaches have been proposed to solve the formation tracking problem; see [3] for an overview. In order to successfully carry out the tracking-in-formation mission in the framework of multi-agent systems of autonomous vehicles, some practical challenges need to be addressed. For instance, in order to guarantee the safety of the system and the success of the mission, the vehicles must guarantee the maintenance of a safe distance between them. Moreover, due to the use of onboard sensors and the challenges posed by the underwater environment, the vehicles need to keep a sufficiently close distance to guarantee the reliability of the communication and the connectivity of the multi-agent system. Furthermore, in special cases when the vehicles use optical sensors or communications, the followers are limited by field-of-view (FOV) constraints.

E. Restrepo is with CNRS-IRISA, Inria Rennes, France, email: esteban.restrepo@inria.fr. J. Matouš and K. Y. Pettersen are with the Centre for Autonomous Marine Operations and Systems, Department of Engineering Cybernetics, Norwegian University of Science and Technology (NTNU), Trondheim, Norway. {Josef.Matous, Kristin.Y.Pettersen}@ntnu.no

Many works in the literature address the coordination problem of multiple marine vehicles under such inter-agent constraints. In [4], [5] planning-based methods are developed to generate trajectories that satisfy the constraints. However, planning algorithms usually require a priori knowledge of the environment, which might be unrealistic in highly dynamical marine environments. Reactive algorithms based on artificial potential fields are proposed in [6], [7] for formation control of AUVs, but the tracking-in-formation problem is not addressed. Leader-follower tracking-in-formation control algorithms are proposed in [8], [9] based on barrier Lyapunov functions.

Under the control designs proposed in the literature, in many instances, in order to guarantee the satisfaction of the inter-agent constraints, the vehicles are forced to move backwards, oftentimes during a prolonged period of time and at relatively high speeds (for backwards motion of a marine vehicle). However, although marine vehicles are able to move backwards, they are not well-suited to do so due to their shapes and their propulsion system. This issue, however, has not been addressed in the literature of multi-ASV/AUV systems.

We propose a distributed control law that solves the tracking-in-formation problem for multiple marine vehicles interacting over a directed communication graph and that guarantees, simultaneously, connectivity preservation and inter-agent collision avoidance. Moreover, we address the issue of backwards motion by imposing a non-negativity constraint on the surge velocity (the forward-motion velocity) of the vehicles. More precisely, on one hand we encode via barrier Lyapunov functions the proximity and safety constraints as **hard constraints** that need to be always satisfied. On the other hand, we encode the non-negativity of the surge velocity as a **soft constraint**, so that it is imposed on the vehicles as long as it does not interfere with the hard constraints, in which case it is dynamically relaxed. The proposed controller is based on the hand-position-based input-output feedback linearization method introduced in [10] for marine vehicles and on the so-called *edge-agreement* representation of multi-agent systems [11], in which the relative states of the connected agents are used instead of the absolute ones, making it well adapted to practical applications where, usually, only relative measurements are available. With regards to the stability analysis, differing from most of the existing works in the literature, where only non-uniform convergence to the formation and to the target vehicle is guaranteed, we establish *almost-everywhere* uniform asymptotic stability of the tracking-in-formation objective and we show that the output error dynamics converge to
the origin exponentially fast, while satisfying the constraints.

In this paper, we extend the preliminary work presented in [12] by considering directed topologies, which better capture the sensor-based interactions in practical scenarios. Motivated by the specific control requirements of marine vehicles, we furthermore extend the control design to deal with the (soft) velocity constraints on top of the (hard) inter-agent constraints. Furthermore, we note that dealing with both soft and hard constraints as we do here for multi-agent systems, to the best of our knowledge, has never been addressed in the context of autonomous vehicles. Moreover, we include here the complete proof of the main Proposition which is not trivial and was partially omitted in the preliminary work. Finally, in this paper we include the results of high-fidelity simulation scenarios taking into account the real physical models of the vehicles as well as dynamical phenomena and disturbances not modeled in the Matlab simulations of the preliminary work [12].

In Section II we present the model of the multi-agent system and the problem formulation. In Section III we present the control design, followed by the stability analysis in Section IV. Finally, simulation results and conclusions are presented in Sections V and VI, respectively.

**Notations:** A continuous function $\alpha: \mathbb{R}_{\ge 0} \to \mathbb{R}_{\ge 0}$ is of class $\mathcal{K}$ ($\alpha \in \mathcal{K}$), if it is strictly increasing and $\alpha(0) = 0$; $\alpha \in \mathcal{K}_{\infty}$ if, in addition, $\alpha(s) \to \infty$ as $s \to \infty$. We use $|x|$ for the absolute value if $x$ is scalar and for Euclidean norm if $x$ is a vector. For a set $S$, the notation $\partial S$ denotes its boundary and $|x|_S$ denotes the set-point distance, i.e. $|x|_S := \inf_{s \in S} |x-s|$. $\otimes$ denotes the Kronecker product, and $I_n$ is the identity matrix of dimension $n$. We use $\mathcal{G} = (\mathcal{V}, \mathcal{E})$ to denote a graph defined by a node set $\mathcal{V} = \{1, 2, \dots, N\}$ which corresponds to the labels of the agents and the set of edges $\mathcal{E} \subseteq \mathcal{V}^2$, of cardinality $M$, represents the communication between a pair of nodes. If agent $j$ has access to information from node $i$, there is an edge $e_k := (i, j) \in \mathcal{E}$, $k \le M$. We use $\mathcal{A} = [a_{ij}]$ to denote the adjacency matrix of the graph where $a_{ii} = 0$, $a_{ij} > 0$ if $e_k \in \mathcal{E}$ and $a_{ij} = 0$ otherwise.

## II. MODEL AND PROBLEM FORMULATION

### A. Model of the marine vehicle

The motion of an ASV or an AUV moving in the horizontal plane can be represented using a three-degrees-of-freedom model, where the degrees of freedom correspond to the surge, sway, and yaw motions. For each vehicle $i$, let $[x_i y_i \psi_i]^{\top}$ denote the pose of each vehicle in the North-East-Down (NED) frame, $[u_{ri} v_{ri} r_i]^{\top}$ denote the relative (with respect to the ocean current) velocities in the body frame, that is, the surge velocity, the sway velocity, and the yaw rate, respectively, see Figure 1. Assuming port-starboard symmetry and linear hydrodynamic damping (see [13] for more details), for each vehicle $i$, the dynamical model is given by

$$ \dot{x}_i = u_{ri} \cos \psi_i - v_{ri} \sin \psi_i + V_x \quad (1a) $$

$$ \dot{y}_i = u_{ri} \sin \psi_i + v_{ri} \cos \psi_i + V_y \quad (1b) $$

$$ \dot{\psi}_i = r_i \quad (1c) $$

$$ \dot{u}_{ri} = F_{ur}(v_{ri}, r_i) + \tau_{ui} \quad (1d) $$

$$ \dot{v}_{ri} = X(u_{ri})r_i + Y(u_{ri})v_{ri} \quad (1e) $$

$$ \dot{r}_i = F_r(u_{ri}, v_{ri}, r_i) + \tau_{ri}, \quad (1f) $$

where $\tau_{ui}$ and $\tau_{ri}$ are the control inputs and $V := [V_x \ V_y]^{\top}$ is the ocean current velocity in the inertial frame. Moreover, $X(u_{ri}) := -X_1 u_{ri} + X_2$, $Y(u_{ri}) := -Y_1 u_{ri} - Y_2$, and $X_1, X_2, Y_1, Y_2, F_{ur}$, and $F_r$ are given in Appendix I. Furthermore, we make the following standard assumptions for marine vehicles and applications, cf. [14]:

**Assumption 1:** The ocean current $V$ is constant, irrotational, and bounded, i.e., $\exists V_{max} > 0$ such that $|V| \le V_{max}$.

**Assumption 2:** The terms $Y_1$ and $Y_2$ are strictly positive.

**Remark 1:** Note that $Y_1, Y_2 > 0$ implies $Y(u_{ri}) < 0$. This is a natural assumption since $Y(u_{ri}) \ge 0$ corresponds to the situation of unstable sway dynamics. That is, a small perturbation applied along the sway direction would cause an undamped motion, which is unfeasible for commercial marine vehicles by design. This is linked to the straight-line stability properties of the AUVs and USVs, cf. [14, Remark 13]. •

### B. Hand-position transformation

The nonlinear dynamic model (1) is the system commonly used in the literature for the control of marine vehicles. For system (1) it is common to choose the origin of the body-fixed frame $p_i := [x_i y_i]^{\top}$ as the control output. However, as can be seen from Eqs. (1d)-(1f), this system is underactuated since there is no way to directly control the sway dynamics (1e). Therefore, by choosing a different output located on the x-axis of the body-fixed frame at a constant distance $l > 0$ from the center of gravity, the underactuated system is transformed into an input-output linearized system with internal dynamics. The latter, introduced in [10] for marine vehicles, is reminiscent of the *hand-position point* transformation commonly used for the control of nonholonomic vehicles, see e.g. [15]. Let the *hand-position point* be denoted by $\xi_{hi} := [\xi_{1i} \ \xi_{2i}]^{\top}$, with

$$ \xi_{1i} := x_i + l \cos \psi_i, \quad \xi_{2i} := y_i + l \sin \psi_i. \quad (2) $$

See Fig. 1 for an illustration in the case of an ASV.

<figure><img src="image_1.png" /><figcaption>Fig. 1: Diagram of an ASV.</figcaption></figure>

Now, we define the following change of coordinates:

$$ \zeta_{1i} = \psi_i \quad (3a) $$

$$ \zeta_{2i} = r_i \quad (3b) $$

$$ \xi_{1i} = x_i + l \cos \psi_i \quad (3c) $$

$$ \xi_{2i} = y_i + l \sin \psi_i \quad (3d) $$

$$ \xi_{3i} = u_{ri} \cos \psi_i - v_{ri} \sin \psi_i - r_i l \sin \psi_i \quad (3e) $$
$$ \xi_{4i} = u_{ri} \sin \psi_i + v_{ri} \cos \psi_i + r_i l \cos \psi_i. \quad (3f) $$

In these new coordinates the dynamic model becomes

$$ \dot{\zeta}_{1i} = \zeta_{2i} \quad (4a) $$

$$ \dot{\zeta}_{2i} = F_{\zeta_2}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) + \tau_{ri} \quad (4b) $$

$$ \begin{bmatrix} \dot{\zeta}_{1i} \\ \dot{\zeta}_{2i} \end{bmatrix} = \begin{bmatrix} \xi_{3i} + V_x \\ \xi_{4i} + V_y \end{bmatrix} \quad (4c) $$

$$ \begin{bmatrix} \dot{\zeta}_{3i} \\ \dot{\zeta}_{4i} \end{bmatrix} = \begin{bmatrix} F_{\xi_3}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) \\ F_{\xi_4}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) \end{bmatrix} + \mathfrak{R}(\zeta_{1i}) \begin{bmatrix} \tau_{ui} \\ l \tau_{ri} \end{bmatrix} \quad (4d) $$

where

$$ \begin{bmatrix} F_{\xi_3}(\cdot) \\ F_{\xi_4}(\cdot) \end{bmatrix} = \mathfrak{R}(\psi_i) \begin{bmatrix} F_{u_i}(\cdot) - v_{ri}r_i - l r_i^2 \\ u_{ri}r_i + X(\cdot)r_i + Y(\cdot)v_{ri} + F_r(\cdot)l \end{bmatrix}, \quad (5) $$

with $\mathfrak{R}(\cdot) : \mathbb{R} \to \text{SO}(2)$ denoting the rotation matrix and $F_{\zeta_2}(\zeta_{1i}, \xi_{3i}, \xi_{4i})$ is obtained from $F_r(u_{ri}, v_{ri}, r_i)$ substituting $u_{ri} = \xi_{3i} \cos \zeta_{1i} + \xi_{4i} \sin \zeta_{1i}$, $v_{ri} = -\xi_{3i} \sin \zeta_{1i} + \xi_{4i} \cos \zeta_{1i} - \zeta_{2i}l$, $r_i = \zeta_{2i}$. Now, we apply the feedback linearizing inputs

$$ \begin{bmatrix} \tau_{ui} \\ l \tau_{ri} \end{bmatrix} = \mathfrak{R}(\psi_i)^{\top} \begin{bmatrix} -F_{\xi_3}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) + \mu_{1i} \\ -F_{\xi_4}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) + \mu_{2i} \end{bmatrix} \quad (6) $$

where $\mu_{1i}, \mu_{2i}$ are the new inputs to be designed. Substituting (6) into (4), denoting the velocity and acceleration (control input) of the hand-position point $\nu_{hi} := [\xi_{3i} \ \xi_{4i}]^{\top}$ and $\mu_i := [\mu_{1i} \ \mu_{2i}]^{\top}$, respectively, and recalling $\xi_{hi} := [\xi_{1i} \ \xi_{2i}]^{\top}$ and $\mathbf{V} := [V_x \ V_y]^{\top}$, we obtain

$$ \dot{\zeta}_{1i} = \zeta_{2i} \quad (7a) $$

$$ \begin{aligned} \dot{\zeta}_{2i} = & - \left[ \left( Y_1 - \frac{X_1 - 1}{l} \right) U_i \cos(\zeta_{1i} - \phi_i) + Y_2 + \frac{X_2}{l} \right] \zeta_{2i} \\ & - \left[ \frac{Y_1}{l} U_i \cos(\zeta_{1i} - \phi_i) + \frac{Y_2}{l} \right] U_i \sin(\zeta_{1i} - \phi_i) \\ & - \frac{\sin \zeta_{1i}}{l} \mu_{1i} + \frac{\cos \zeta_{1i}}{l} \mu_{2i} \end{aligned} \quad (7b) $$

$$ \dot{\xi}_{hi} = \nu_{hi} + \mathbf{V} \quad (7c) $$

$$ \dot{\nu}_{hi} = \mu_i \quad (7d) $$

where $U_i = |\xi_{hi}|$ and $\phi_i = \arctan 2(\xi_{4i}/\xi_{3i})$.

The main advantage of considering the hand-position point $\xi_{hi}$ instead of the origin of the body-fixed frame $p_i$, is that the nonlinear system (1) is transformed into (7) with the linear external dynamics (7c)-(7d) making it possible to use coordination algorithms normally found in the multi-agent systems literature. Note, however, that for this system the inputs $\mu_i$ affect also the internal dynamics (7a)-(7b). Therefore, contrary to case of linear multi-agent systems, the internal stability of the states $\zeta_{1i}$ and $\zeta_{2i}$ has to be verified.

### C. Problem statement

We consider a multi-agent system composed of $N$ marine vehicles modeled by (7) and with an interaction topology given by a directed graph $\mathcal{G} = (\mathcal{V}, \mathcal{E})$ which is either a spanning tree or a cycle. Moreover, we consider that the multi-agent system is subject to inter-agent distance constraints. For one part, these constraints may come from embedded relative-measurements devices, which are reliable only if used within a limited range.

Hence, the vehicles must remain within a limited distance from their neighbors in order to maintain the connectivity of the graph. Furthermore, to ensure the safety of the system, the agents must avoid collisions among themselves, that is, they must always guarantee a minimal distance with respect to their neighbors. These connectivity and collision-avoidance constraints may be defined as a set of restrictions on the relative (hand) positions. Such constraints may be considered as hard constraints since they are fundamental for ensuring the safety of the system and for reaching the control goal.

More precisely, define the distance (with respect to the hand-position points) between two connected agents as

$$ d_{ij} := |\xi_{hi} - \xi_{hj}| \quad \forall e_k := (i, j) \in \mathcal{E}. \quad (8) $$

For each $e_k = (i, j) \in \mathcal{E}$, let $\delta_{ij}$ and $\Delta_{ij}$ be, respectively, the minimal and maximal distances between agents $i$ and $j$ so that collisions are avoided and that the communication through edge $e_k$ is reliable<fnref n="1" />. Then, the set of inter-agent output constraints is defined as

$$ \mathcal{D}_{ij} := \{d_{ij} \in \mathbb{R} : \delta_{ij} < d_{ij} < \Delta_{ij}\}, \quad \forall e_k = (i, j) \in \mathcal{E}. \quad (9) $$

Note that under the limited range $\Delta_{ij}$ the whole topology may become disconnected if the distances between two arbitrary connected agents go above such range. By imposing the proximity constraints (9) it is guaranteed that any initially existing connection will be preserved. Therefore, if the graph at the initial time is connected and the constraints (9) are satisfied for all $t \ge t_0$, the graph will remain connected for all time. Thus, we assume the following.

**Assumption 3:** The graph topology $\mathcal{G}$ at the initial time $t_0$ is either a directed spanning tree or a directed cycle.

It is important to remark that, in practice, marine vehicles are not optimized for moving backwards. However, backwards motion is not prevented from the dynamical model (1). Therefore, in order to let the vehicles evolve in an optimal way, besides the inter-agent connectivity and collision-avoidance constraints, we formulate the additional constraints

$$ u_{ri}(t) > 0, \quad \forall i \le N, \quad \forall t \ge 0. \quad (10) $$

Nonetheless, in some cases the requirements (10) could enter into conflict with the inter-agent constraints defined by the set (9). Indeed, there might exist situations when the only way to avoid a collision or avoid losing connectivity is to move backwards. Moreover, although not optimized to, marine vehicles can move backwards. The latter motivates us to reformulate the requirements (10) as soft constraints, that is, to impose a positive surge velocity as long as this does not interfere with the satisfaction of the hard constraints (9), but allow negative velocities when required for the satisfaction of (9). We formulate these soft constraints as follows:

$$ u_{ri}(t) + \rho_i(t) > 0, \quad \forall i \le N, \quad \forall t \ge 0, \quad (11) $$

where $\rho_i : \mathbb{R}_{\ge 0} \to \mathbb{R}_{\ge 0}$ will be defined later, such that $\rho_i(t) \approx 0$ when there are no conflicts with the hard constraints and

<footnote n="1">¹Throughout the rest of this paper and with a slight abuse of notation we will use $s_{ij}$ and $s_k$ interchangeably to refer to the same (edge-based) quantity.</footnote>
$\rho_i(t) > 0$ otherwise, allowing $u_{ri}(t)$ to become negative. Akin, to (9) we may define the set of soft constraints as

$$C_i := \{u_{ri} \in \mathbb{R} : u_{ri} > -\rho_i(t)\}, \quad \forall i \le N. \quad (12)$$

Now, let $\xi_{ho} := [\xi_{1o} \ \xi_{2o}]^\top \in \mathbb{R}^2$ and $\nu_{ho} := [\xi_{3o} \ \xi_{4o}]^\top \in \mathbb{R}^2$ define, respectively, the (hand) position and velocity of a (virtual) target, and $\mu_o(t)$ is its acceleration, and let its dynamics be modeled as a second-order integrator

$$\dot{\xi}_{ho} = \nu_{ho}, \quad \dot{\nu}_{ho} = \mu_o(t). \quad (13)$$

Moreover, assume the following.

**Assumption 4:** For all $t$ there exist positive constants $\nu_{ho}$, $\bar{\nu}_{ho}$, and $\bar{\mu}_o$ such that $\nu_{ho} \le \nu_{ho}(t) \le \bar{\nu}_{ho}$, and $|\mu_o(t)| \le \bar{\mu}_o$.

**Assumption 5:** The relative velocity of the target is such that $U_o(t) \cos(\phi_o(t)) > 0$, where $U_o(t) = \sqrt{(\xi_{3o}(t) - V_x)^2 + (\xi_{4o}(t) - V_y)^2}$ and $\phi_o(t) = \arctan((\xi_{4o}(t) - V_y)/(\xi_{3o}(t) - V_x))$.

**Remark 2:** Assumption 5 is required so that the target moves forward in the surge direction despite the ocean current. Furthermore, Assumptions 1 and 4 imply that there exist constants $\bar{U}_o$, $\bar{U}_o^*$, and $\bar{U}_o^*$, such that $\bar{U}_o \le U_o(t) \le \bar{U}_o$ and $\bar{U}_o^* \le \bar{U}_o(t) \le \bar{U}_o$ for all $t \ge 0$.

Then, the control goal is for the $N$ marine vehicles to achieve a desired formation and track the target modeled by (13), all while guaranteeing that the hard constraints given by the set $D_{ij}$ in (9) and the soft constraints (11) are respected.

For the tracking part of the problem, we consider the case that only one agent, labeled $i=1$ without loss of generality, has access to the target's state, $\xi_{ho}$ and $\nu_{ho}$, and knows an upper bound $\bar{\mu}_o$ on the target's acceleration. To address the formation part, let us denote by $z_{1ij}^d \in \mathbb{R}^2$ the desired relative position between a pair of neighboring agents over edge $e_k = (i, j)$. Then, mathematically, the tracking-in-formation problem translates into designing a distributed controllers $\mu_i$ such that, for all $i, j \in \mathcal{V}$,

$$\lim_{t \to \infty} \xi_{hi}(t) - \xi_{ho}(t) = 0, \quad \lim_{t \to \infty} \nu_{hi}(t) - \nu_{ho}(t) = 0 \quad (14a)$$

$$\lim_{t \to \infty} \xi_{hi}(t) - \xi_{hj}(t) - z_{1ij}^d = 0, \quad \lim_{t \to \infty} \nu_{hi}(t) - \nu_{hj}(t) = 0. \quad (14b)$$

More precisely, we consider the following problem.

**Tracking-in-formation under hard and soft constraints:** Consider a system of $N$ autonomous marine vehicles with dynamics given by (1), interacting over a directed graph which is either a spanning tree or a cycle. Assume, in addition, that the agents are subject to the hard inter-agent output constraints given by the set defined in (9) and the soft constraints given by (12). Under these conditions, find distributed controllers $\mu_i$, $i \le N$, so that the limits (14) are satisfied and that render the sets (9) and (12) forward invariant, i.e., $d_{ij}(t_0) \in D_{ij}$ ($u_{ri}(t_0) \in C_i$) implies that $d_{ij}(t) \in D_{ij}$ ($u_{ri}(t) \in C_i$), $e_k = (i, j) \in \mathcal{E}$ ($\forall i \in \mathcal{V}$) and $\forall t \ge t_0$.

### III. CONTROL DESIGN FOR TRACKING UNDER HARD AND SOFT CONSTRAINTS

In this section we design a distributed control law to solve the tracking-in-formation problem under the previously formulated (hard) inter-agent and (soft) velocity constraints. To do so we design the new inputs $\mu_i$ as

$$\mu_i = \mu_i^h + \mu_i^s, \quad (15)$$

where $\mu_i^h \in \mathbb{R}^2$ will be used to deal with the tracking-in-formation mission and with the hard constraints and $\mu_i^s \in \mathbb{R}^2$ will deal with the soft constraints. For clarity, we present both designs separately in what follows.

#### A. Control design for the hard proximity and safety constraints

The design of $\mu_i^h$ follows a backstepping approach which is well adapted to the normal form of the external dynamics (7c)-(7d). We start by defining a virtual control law for (7c) with $\nu_{hi}$ as input, based on the gradient of a Barrier Lyapunov Function (BLF)—see [16] for more details on BLFs.

First, for each edge $e_k = (i, j) \in \mathcal{E}$, we define a candidate BLF of the form

$$B_{ij}^h(d_{ij}) = \kappa_{1ij} \left[ \ln \left( \frac{\Delta_{ij}^2}{\Delta_{ij}^2 - d_{ij}^2} \right) - \ln \left( \frac{\Delta_{ij}^2}{\Delta_{ij}^2 - |z_{1ij}^d|^2} \right) \right] + \kappa_{2ij} \left[ \ln \left( \frac{d_{ij}^2}{d_{ij}^2 - \delta_{ij}^2} \right) - \ln \left( \frac{|z_{1ij}^d|^2}{|z_{1ij}^d|^2 - \delta_{ij}^2} \right) \right], \quad (16)$$

where

$$\kappa_{1ij} := \frac{\delta_{ij}^2}{|z_{1ij}^d|^2 (|z_{1ij}^d|^2 - \delta_{ij}^2)}, \quad \kappa_{2ij} := \frac{1}{\Delta_{ij}^2 - |z_{1ij}^d|^2}. \quad (17)$$

Note that $B_{ij}^h$ is a non-negative function that satisfies: $B_{ij}^h(|z_{1ij}^d|) = 0$, $\nabla B_{ij}^h(|z_{1ij}^d|) = 0$, and $B_{ij}^h(d_{ij}) \to \infty$ as either $|d_{ij}| \to \Delta_{ij}$ or $|d_{ij}| \to \delta_{ij}$.

Now, for each edge $e_k = (i, j) \in \mathcal{E}$ the virtual control inputs, are given by

$$\nu_{hi}^* := -c_1 \sum_{j \le N} a_{ij} \left[ (\xi_{hi} - \xi_{hj} - z_{1ij}^d) + \frac{\partial B_{ij}^h}{\partial d_{ij}} \frac{\partial d_{ij}}{\partial \xi_{hi}} \right] - c_1 \eta_i (\xi_{hi} - \xi_{ho}) - \hat{V}_i \quad (18)$$

where $c_1$ is a positive gain, $\eta_i = 1$ if $i=1$ and $\eta_i = 0$ otherwise, and $\hat{V}_i$ is agent $i$'s estimate of the ocean current given by the adaptation law

$$\hat{V}_i = c_v (\xi_{hi} - \varphi_i), \quad c_v > 0 \quad (19a)$$

$$\dot{\varphi}_i = \nu_{hi} + \hat{V}_i. \quad (19b)$$

Defining the error coordinates $\tilde{\nu}_{hi} := \nu_{hi} - \nu_{hi}^*$, $\tilde{V}_i := V - \hat{V}_i$, replacing (18) into (7c)-(7d) and using (19) we have

$$\dot{\xi}_{hi} = -c_1 \sum_{j \le N} a_{ij} \left[ (\xi_{hi} - \xi_{hj} - z_{1ij}^d) + \frac{\partial B_{ij}^h}{\partial d_{ij}} \frac{\partial d_{ij}}{\partial \xi_{hi}} \right] - c_1 \eta_i (\xi_{hi} - \xi_{ho}) + \tilde{V}_i + \tilde{\nu}_{hi} \quad (20)$$

$$\dot{\tilde{V}}_i = -c_v \tilde{V}_i \quad (21)$$
$$ \dot{\bar{v}}_{hi} = \mu_i^h - \dot{\nu}_{hi}^* + \mu_i^s. \qquad (22) $$

Then, we design the distributed tracking-in-formation control law as

$$ \mu_i^h = -c_2 \sum_{j \le N} a_{ij}(\bar{\nu}_{hi} - \bar{\nu}_{hj}) - c_2 \eta_i (\nu_{hi} - \nu_{ho}) + \dot{\bar{v}}_{hi}^* \\ - \gamma \text{sign} \left( \sum_{j \le N} a_{ij}(\bar{\nu}_{hi} - \bar{\nu}_{hj}) + \eta_i (\nu_{hi} - \nu_{ho}) \right) \quad (23) $$

where $c_2 > 0$, $\eta_i$ is as in (18), $\bar{\nu}_{hi}^* = \nu_{hi}^* + \hat{V}_i$, and $\gamma$ is a positive control gain to be defined.

## B. Control design for the soft velocity constraints

Since the soft constraints are only imposed on the surge velocity $u_{ri}$, we let the signal $\mu_i^s(t)$ be given by

$$ \mu_i^s(t) = [\cos \zeta_{1i} \sin \zeta_{1i}]^\top \tau_{ui}^s. \qquad (24) $$

Note that by substituting (15) and (24) into (6), the surge-velocity and yaw-rate subsystems yield

$$ \dot{u}_{ri} = F_{u_r}(v_{ri}, r_i) + \tau_{ui}^h + \tau_{ui}^s \qquad (25a) $$

$$ \dot{r}_i = F_r(u_{ri}, v_{ri}, r_i) + \tau_{ri}, \qquad (25b) $$

where

$$ \tau_{ui}^h = \begin{bmatrix} \cos \zeta_{1i} \\ \sin \zeta_{1i} \end{bmatrix}^\top \begin{bmatrix} -F_{\xi_3}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) + \mu_{1i}^h \\ -F_{\xi_4}(\zeta_{1i}, \xi_{3i}, \xi_{4i}) + \mu_{2i}^h \end{bmatrix}. $$

Therefore, from (25), it is clear that the signal $\mu_i^s(t)$, only directly affects the dynamics of the surge velocity $u_{ri}$.

Now, we design the additional input $\tau_{ui}^s$ as the gradient of a barrier function as follows. For each agent $i$, let us define the barrier function

$$ B_i^s(t, u_{ri}) := -\ln \left( \frac{u_{ri} + \rho_i(t)}{u_{ri} + \rho_i(t) + 1} \right). \qquad (26) $$

Note that for $u_{ri} + \rho_i(t) > 0$, $B_i^s(t, u_{ri}) > 0$ for all $t \ge 0$, and $B_i^s(t, u_{ri}) \to \infty$ as $u_{ri} + \rho_i(t) \to 0$. Then, we set the additional control input to

$$ \tau_{ui}^s = -\nabla B_i^s(t, u_{ri}) := -\kappa_u \frac{\partial B_i^s(t, u_{ri})}{\partial u_{ri}}, \quad i \le N, \qquad (27) $$

with $\kappa_u > 0$ and

$$ \dot{\rho}_i = -\kappa_\rho \rho_i + \frac{1}{2} \left[ 1 - \text{sign} \left( \sigma - |F_{ur} + \tau_{ui}^h| \right) \right] |F_{ur} + \tau_{ui}^h|, \qquad (28) $$

where $\kappa_\rho$, $\sigma > 0$ are design constants and we set $\rho_i(t_0) = 0$.

**Remark 3:** Note that under (28) and the initial condition $\rho_i(t_0) = 0$, we have that $\rho_i(t) \ge 0$, for all $t \ge t_0$. To see this, note that second term on the right-hand side of (28) is always positive. Therefore, $\dot{\rho}_i(t) \ge -\kappa_\rho \rho_i(t)$, which means that the set $C_\rho := \{\rho_i \in \mathbb{R}: \rho_i \ge 0\}$ is forward invariant. •

**Remark 4:** The definition of (28) is loosely inspired by the framework developed in [17] to deal with hard and soft constraints in the setting of prescribed-performance control of single-agent systems. The signal $\rho_i(t)$ adjusts the soft constraints whenever the hard constraints and the positive-velocity constraint become conflicting. Note that when $|F_{ur} +

$\tau_{ui}^h| \le \sigma$ for a given $\sigma$, it means that the distances from vehicle $i$ to its neighbors are far from the border of the set $D_{ij}$, since under the barrier-function-based law (51) the controller $\tau_{ui}^h$ grows unbounded as $d_{ij} \to \partial D_{ij}$ for any $e_k = (i, j) \in \mathcal{E}$. In this case, the second term on the right-hand side of (28) is equal to zero. Hence, assuming that in an interval $t \in [t_0, t_0 + T]$, $|F_{ur} + \tau_{ui}^h| \le \sigma$, then (11), with $\rho(t) = 0$, corresponds to a positive-velocity constraint. On the other hand, when $|F_{ur} + \tau_{ui}^h| > \sigma$, the right-hand side of (28) becomes positive and $\rho_i$ grows. Hence, $u_{ri}$ may take negative values, i.e. $u_{ri} > -\rho_i(t)$, avoiding possible conflicts between the constraints. Then, as the vehicles move away from the border of the set $D_{ij}$, $|F_{ur} + \tau_{ui}^h| \le \sigma$ again and $\rho_i(t) \to 0$ exponentially fast, recovering the non-negativity constraint. •

Let us define the following variables:

$$ a(t) = \left( Y_1 - \frac{X_1 - 1}{l} \right) U_o(t), \quad b = Y_2 + \frac{X_2}{l} \qquad (29) $$

$$ c(t) = \frac{Y_1 U_o(t)^2}{l}, \qquad d(t) = \frac{Y_2 U_o(t)}{l}. \qquad (30) $$

**Remark 5:** Note that from Remark 2 we have that $a \le a(t) \le \bar{a}$, $c \le c(t) \le \bar{c}$, and $d \le d(t) \le \bar{d}$, with $a, \bar{a}, c, \bar{c}, d, \bar{d}$ being positive constants. •

Then, the first part of the main result is stated as follows:

**Proposition 1:** Consider N ASVs/AUVs, each described by the model (1), and interconnected over a directed graph which is either a spanning tree or a cycle. Consider the hand-position point $\xi_{hi}^\top := [\xi_{1i} \ \xi_{2i}] = [x_i + l \cos \psi_i \ y_i + l \sin \psi_i]$, where $[x_i \ y_i]^\top$ is the position of the origin of the body-fixed frame of the $i$th agent, $\psi_i$ is its yaw angle, and $l > 0$ is a positive constant. Let $\phi_o(t) = \arctan(\xi_{4o}(t) - V_y/\xi_{3o}(t) - V_x)$ be the crab angle of the target. Then, under Assumptions 1-5 and

$$ 0 < \bar{U}_o < \frac{Y_2}{Y_1}, \quad l > \max \left\{ \frac{m_{22}}{m_{33}}, -\frac{X_2}{Y_2} \right\} \qquad (31) $$

$$ \bar{U}_o^* \le \frac{2 \min\{a(\underline{d} - \underline{c}), b\}}{\frac{Y_1 \bar{U}_o}{l} + 2 \left(Y_1 - \frac{X_1 - 1}{l}\right)}, \qquad (32) $$

with initial conditions such that $d_k(t_0) \in D_k$ for all $e_k \in \mathcal{E}$ and $u_{ri}(t_0) \in C_i$ for all $i \le N$, the controller (6), where the new inputs $\mu_i$ are given by (15) with (23) and (24), achieves the tracking-in-formation objective (14) almost everywhere and renders the constraints sets (9) and (12) forward invariant. □

For clarity of exposition, the different steps of the proof of Proposition 1 are presented in Section IV.

## IV. PROOF OF PROPOSITION 1

The proof is developed in multiple steps presented in the following subsections. We start by rewriting the N systems (7) in a cascaded form, where the external dynamics act as the driving system and the internal dynamics is the driven system. We follow by showing that the soft-constraints set (12) is forward invariant. Next, we show that the driving system under the control laws (23), expressed in edge-based coordinates, is (almost-everywhere) uniformly asymptotically stable with domain of attraction corresponding to the set of constraints, and that its trajectories converge exponentially to the origin. Then, under the property that the trajectories of the external
dynamics converge to the origin exponentially fast, we show that the trajectories of the internal dynamics are uniformly ultimately bounded. Finally, we analyze the stability of the complete cascaded system.

## A. Cascaded system

In order to analyze the stability of the N systems (7) in closed-loop with the controller (23) we rewrite it in a cascaded form as follows. Let us define the error variables $\tilde{\zeta}_{1i} := \zeta_{1i} - \phi_o(t)$, $\tilde{\zeta}_{2i} := \zeta_{2i} - \dot{\phi}_o(t)$, and the velocity errors $\bar{v}_{hi} := \nu_{hi} - (\nu_{ho}(t) - \mathbf{V})$. Denote $\tilde{\zeta}_1^\top := [\tilde{\zeta}_{11} \cdots \tilde{\zeta}_{1N}]$, $\tilde{\zeta}_2^\top := [\tilde{\zeta}_{21} \cdots \tilde{\zeta}_{2N}]$, $\tilde{\zeta}^\top := [\tilde{\zeta}_1^\top \tilde{\zeta}_2^\top]$, $\bar{v}_h^\top := [\bar{v}_{h1}^\top \cdots \bar{v}_{hN}^\top]$, $\text{Sin}(\tilde{\zeta}_1)^\top := [\text{sin} \tilde{\zeta}_{11} \cdots \text{sin} \tilde{\zeta}_{1N}]$. Then, the internal dynamics in compact form yields

$$ \dot{\tilde{\zeta}}_1 = \tilde{\zeta}_2 \quad (33a) $$

$$ \dot{\tilde{\zeta}}_2 = -A(t, \tilde{\zeta})\tilde{\zeta}_2 - B(t, \tilde{\zeta})\text{Sin}(\tilde{\zeta}_1) + \bar{A}(t, \tilde{\zeta}, \bar{v}_h)\bar{v}_h + \bar{B}(\zeta_1)\mu - A(t, \tilde{\zeta})\mathbf{1}_N \dot{\phi}_o(t) - \mathbf{1}_N \ddot{\phi}_o(t) \quad (33b) $$

where

$$ \begin{aligned} A(t, \tilde{\zeta}) &:= \text{diag}\{a(t) \cos \tilde{\zeta}_{1i} + b\} \\ B(t, \tilde{\zeta}) &:= \text{diag}\{c(t) \cos \tilde{\zeta}_{1i} + d(t)\} \\ \bar{A}(t, \tilde{\zeta}, \bar{v}_h) &:= \text{blockdiag}\{[-\alpha(t, \tilde{\zeta}_i, \bar{v}_{hi}) \ \beta(t, \tilde{\zeta}_i, \bar{v}_{hi})]\} \\ \bar{B}(\zeta_1) &:= \text{blockdiag}\{[-\sin \tilde{\zeta}_{1i} \ \cos \tilde{\zeta}_{1i}]\}, \end{aligned} $$

and $\alpha(t, \tilde{\zeta}_i, \bar{v}_{hi})$, $\beta(t, \tilde{\zeta}_i, \bar{v}_{hi})$ are given in Appendix I.

Now, set the tracking errors $\tilde{\xi}_{hi} := \xi_{hi} - \xi_{ho}(t) - \xi_{hio}^d$ with $\xi_{hio}^d$ denoting a desired position in the formation with respect to the leader, and let $\tilde{\xi}_h^\top := [\tilde{\xi}_{h1}^\top \cdots \tilde{\xi}_{hN}^\top]$. Then, the N systems (7) in compact form yields

$$ \dot{\tilde{\xi}} = H(t, \tilde{\zeta})\tilde{\zeta}_s + \Phi(t, \tilde{\zeta}) + G(t, \tilde{\zeta}, \chi) \quad (34a) $$

$$ \dot{\chi} = F(t, \chi), \quad (34b) $$

$$ \text{where } \chi^\top := [\bar{\xi}_h^\top \ \bar{v}_h^\top \ \tilde{V}^\top], \ \tilde{\xi}_s^\top := [\text{Sin}(\tilde{\zeta}_1)^\top \ \tilde{\zeta}_2^\top], \text{ and} $$

$$ \begin{aligned} H(\cdot) &= \begin{bmatrix} 0 & I_N \\ -B(\cdot) & -A(\cdot) \end{bmatrix}, \quad F(\cdot) = \begin{bmatrix} \bar{v}_h \\ \mu - \mathbf{1}_N \mu_o(t) \\ -c_v \tilde{V} \end{bmatrix} \\ G(\cdot) &= \begin{bmatrix} 0 & \bar{A}(\cdot)\bar{v}_h + \bar{B}(\cdot)\mu \end{bmatrix}, \quad \Phi(\cdot) = \begin{bmatrix} 0 \\ -A(\cdot)\dot{\phi}_o(t) - \mathbf{1}_N\ddot{\phi}_o(t) \end{bmatrix}. \end{aligned} $$

## B. Forward invariance of the soft-constraints set

**Lemma 1:** Under the control law (27), with initial conditions such that $u_{ri}(t_0) \in C_i$ for all $i \le N$, the set (12) is forward invariant for the surge-velocity subsystem (25a). □

*Proof:* Consider the barrier function (26), whose derivative along the trajectories of (25a) in $C_i$ yields

$$ \dot{B}_i^s(t, u_{ri}) = \nabla B_i^s(t, u_{ri}) (-\kappa_u \nabla B_i^s(t, u_{ri}) + F_{ur} + \tau_{ui}^h + \dot{\rho}_i(t)) \quad (35) $$

Now, in view of (28), we split the analysis into two cases.

**Case 1** ($|F_{ur} + \tau_{ui}^h| \le \sigma$): in this case (35) becomes

$$ \begin{aligned} \dot{B}_i^s(t, u_{ri}) &\le -\kappa_u |\nabla B_i^s(t, u_{ri})|^2 + |\nabla B_i^s(t, u_{ri})| [\sigma - \kappa_\rho \rho_i(t)] \\ &\le -\kappa'_u |\nabla B_i^s(t, u_{ri})|^2 + \lambda_\sigma \sigma^2 \\ &\quad + \frac{\kappa_\rho \rho_i(t)}{(u_{ri} + \rho_i(t))(u_{ri} + \rho_i(t) + 1)}, \end{aligned} \quad (36) $$

with $\kappa'_u$, $\lambda_\sigma > 0$. Since $\rho_i(t)$ is non-negative for all $t \ge t_0$, cf. Remark 3, in $C_i$ the last term on the right-hand side of (36) is bounded by a constant $\lambda_\rho > 0$. Therefore, we have

$$ \dot{B}_i^s(t, u_{ri}) \le -\kappa_u |\nabla B_i^s(t, u_{ri})|^2 + c \quad (37) $$

where $c := \lambda_\sigma \sigma^2 + \lambda_\rho$.

**Case 2** ($|F_{ur} + \tau_{ui}^h| > \sigma$): in $C_i$ (35) becomes

$$ \begin{aligned} \dot{B}_i^s(t, u_{ri}) &\le -\kappa_u |\nabla B_i^s(t, u_{ri})|^2 + |\nabla B_i^s(t, u_{ri})| [|F_{ur} + \tau_{ui}^h| \\ &\quad - \kappa_\rho \rho_i(t) - |F_{ur} + \tau_{ui}^h|] \\ &\le -\kappa_u |\nabla B_i^s(t, u_{ri})|^2 + c, \end{aligned} \quad (38) $$

where, with a slight abuse of notation, $c = \lambda_\rho$.

From (37)-(38) we conclude that for all $i \le N$ and $u_{ri} \in C_i$, the barrier function $B_i^s(t, u_{ri})$ is bounded along the trajectories. In order to establish forward invariance of the sets $C_i$, for all $i \le N$, we proceed by contradiction. Assume that there exists $t_0 < T < \infty$ such that for all $t \in [t_0, t_0 + T)$, $u_{ri}(t) \in C_i$ and $u_{ri}(t_0 + T) \notin C_i$, for at least one $i \le N$. More precisely, we have $u_{ri}(t) \to \rho_i(t)$ as $t \to t_0 + T$. From the definition of $(t, u_{ri}) \mapsto B_i^s(t, u_{ri})$ in (26), this implies that $B_i^s(t, u_{ri}(t)) \to \infty$ as $t \to t_0 + T$ which is in contradiction with (37)-(38). We conclude that $T = \infty$ and that the sets $C_i$ for all $i \le N$ are forward invariant, i.e., if $u_{ri}(t_0) \in C_i$ then $u_{ri}(t) \in C_i$ for all $t \ge t_0$. ■

## C. External dynamics in edge-based coordinates

To study the stability of the external dynamics (34b), we rely on the edge-agreement framework [11] where instead of considering the states of each individual agent (the nodes of the graph), we consider the relative variables which correspond to the edges in the graph. For that purpose, recalling that only one agent labeled $i = 1$ has access to the target's state, define the tracking-error states with respect to the leader as

$$ z_{1o} := \xi_{h1} - \xi_{ho}, \quad z_{2o} := \nu_{h1} - \nu_{ho}. \quad (39) $$

Similarly, define the formation error and the relative velocity between two connected agents, i.e., for all $e_k \in \mathcal{E}$, by

$$ z_{1k} := \xi_{hi} - \xi_{hj} - z_{1k}^d \quad (40) $$

$$ z_{2k} := \nu_{hi} - \nu_{hj}. \quad (41) $$

Now, let us denote by $E \in \mathbb{R}^{N \times M}$ the incidence matrix of graph $G$, where its $(i, k)$th entry is defined as follows: $[E]_{ik} := -1$ if $i$ is the terminal node of edge $e_k$, $[E]_{ik} := 1$ if $i$ is the initial node of edge $e_k$, and $[E]_{ik} := 0$ otherwise. Let $\xi_h^\top = [\xi_{h1}^\top \cdots \xi_{hN}^\top] \in \mathbb{R}^{2N}$ and $\nu_h^\top = [\nu_{h1}^\top \cdots \nu_{hN}^\top] \in \mathbb{R}^{2N}$ be, respectively, the collection of the hand-position point positions and velocities of all the agents of the system. Then, in compact form we can write

$$ z_1 = [E^\top \otimes I_2] \xi_h - z_1^d \quad (42) $$
$$z_2 = [E^\top \otimes I_2]\nu_h. \quad (43)$$

where $z_1^\top = [z_{11}^\top \cdots z_{1M}^\top]^\top \in \mathbb{R}^{2M}$ and $z_2^\top = [z_{21}^\top \cdots z_{2M}^\top]^\top \in \mathbb{R}^{2M}$.

Now, as observed in [18], using an appropriate labeling of the edges, the incidence matrix can be expressed as $E = [E_t \ E_c]$ where $E_t \in \mathbb{R}^{N \times (N-1)}$ denotes the full column-rank incidence matrix corresponding to an arbitrary spanning tree $\mathcal{G}_t \subset \mathcal{G}$ and $E_c \in \mathbb{R}^{N \times (M-N+1)}$ represents the incidence matrix corresponding to the remaining edges in $\mathcal{G} \setminus \mathcal{G}_t$. Similarly, the edge states may be split as $z_1 = [z_{1t}^\top \ z_{1c}^\top]^\top$, where $z_{1t} \in \mathbb{R}^{2(N-1)}$ are the states corresponding to the edges of $\mathcal{G}_t$ and $z_{1c} \in \mathbb{R}^{n(M-N+1)}$ denote the states of the remaining edges, $\mathcal{G} \setminus \mathcal{G}_t$. The same holds for $z_2$, that is, $z_2 = [z_{2t}^\top \ z_{2c}^\top]^\top$. Moreover, defining

$$R := [I_{N-1} \ T], \quad T := (E_t^\top E_t)^{-1} E_t^\top E_c \quad (44)$$

with $I_{N-1}$ denoting the $N-1$ identity matrix, we have the following identities

$$E = E_t R, \quad z_1 = [R^\top \otimes I_2] z_{1t}, \quad z_2 = [R^\top \otimes I_2] z_{2t}. \quad (45)$$

Then, using (45) and (7c)-(7d) a reduced-order model for the external dynamics in terms of the edges of a spanning tree is given by

$$\dot{z}_{1o} = z_{2o} + \mathbf{V} \quad (46a)$$

$$\dot{z}_{1t} = z_{2t} \quad (46b)$$

$$\dot{z}_{2o} = \mu_1 - \mu_o(t) \quad (46c)$$

$$\dot{z}_{2t} = [E_t^\top \otimes I_2]\mu. \quad (46d)$$

Note that in these coordinates, the control objective as defined in (14) is achieved if the origin of system (46) is asymptotically stabilized.

Noting that in the edge coordinates the distance in (8) may be rewritten as $d_k = |z_{1k} + z_{1k}^d|$, with a slight abuse of notation, the candidate BLFs in (16) may be denoted as $B_k^h(z_{1k})$ replacing $d_k$ by $|z_{1k} + z_{1k}^d|$. Similarly let use redefine the constraints set as

$$\tilde{D}_k := \{z_{1k} \in \mathbb{R}^2 : \delta_k < |z_{1k} + z_{1k}^d| < \Delta_k\}, \quad \forall e_k \in \mathcal{E}. \quad (47)$$

Now, for each edge $e_k \in \mathcal{E}$ let us define the candidate BLF $W_k : \tilde{D}_k \to \mathbb{R}_{\ge 0}$ as

$$W_k(z_{1k}) = \frac{1}{2} [|z_{1k}|^2 + B_k^h(z_{1k})]. \quad (48)$$

Then, for the complete multi-agent system we define

$$W(z_1) = \sum_{k \le M} \varrho_k W_k(z_{1k}), \quad \varrho_k > 0. \quad (49)$$

Let us define the so-called in-incidence matrix $E_\odot \in \mathbb{R}^{N \times M}$, whose elements are defined as follows: $[E_\odot]_{ik} := -1$ if $i$ is the terminal node of edge $e_k$ and $[E_\odot]_{ik} := 0$ otherwise. Then, in the edge-agreement framework, the virtual controllers (18) are given by

$$\nu_h^* = -c_1[E_\odot \otimes I_2]\nabla W(z_{1t}) - c_1[C \otimes I_2]z_{1o} - \hat{V}, \quad (50)$$

where $\nabla W(z_{1t}) := \partial W(z_{1t})/\partial z_{1t}^2$, $\hat{V}$ is a vector of estimates of the ocean current for each agent, and $C^\top := [1 \ 0_{1 \times (N-1)}]$.

Akin to (43), let $\tilde{z}_{2t} := [E^\top \otimes I_2]\tilde{\nu}_h$ and $\tilde{z}_{2o} = \tilde{\nu}_{h1} - \nu_{ho}$, where $\tilde{\nu}_h^\top = [\tilde{\nu}_{h1}^\top \cdots \tilde{\nu}_{hN}^\top] \in \mathbb{R}^{2N}$ and $\tilde{\nu}_{hi}$ is the backstepping error previously defined. Then, the control law (23) in compact form yields

$$\mu^h = -c_2[E_\odot R^\top \otimes I_2]\tilde{z}_{2t} - c_2[C \otimes I_2]\tilde{z}_{2o} + \dot{\tilde{\nu}}_h^* - \gamma \text{sign} ([E_\odot R^\top \otimes I_2]\tilde{z}_{2t} + [C \otimes I_2]\tilde{z}_{2o}). \quad (51)$$

Let $\zeta_1^\top = [z_{1o} \ z_{1t}^\top]$, $\bar{\zeta}_1^\top = [z_{1o} \ \nabla W(z_1)^\top]$, $\zeta_2^\top = [\tilde{z}_{2o} \ \tilde{z}_{2t}^\top]$, and

$$\mathcal{L}_1 = \left[ \begin{bmatrix} 1 & C^\top E_t \\ E_t^\top C & E_t^\top E_\odot \end{bmatrix} \otimes I_2 \right], \quad \mathcal{R}_1 = \left[ \begin{bmatrix} C^\top \\ E_t^\top \end{bmatrix} \otimes I_2 \right]$$

$$\mathcal{L}_2 = \left[ \begin{bmatrix} 1 & C^\top E_t \\ E_t^\top C & E_t^\top E_\odot R^\top \end{bmatrix} \otimes I_2 \right], \quad \mathcal{R}_2 = \left[ \begin{bmatrix} C^\top \\ R E_\odot^\top \end{bmatrix} \otimes I_2 \right].$$

Then, from (46), (21), (50), and (51), the closed-loop external dynamics in the edge-based perspective becomes

$$\dot{\zeta}_1 = -c_1 \mathcal{L}_1 \bar{\zeta}_1 + \zeta_2 + \mathcal{R}_1 \tilde{V} \quad (52a)$$

$$\dot{\zeta}_2 = -c_2 \mathcal{L}_2 \zeta_2 + c_v \mathcal{R}_1 \tilde{V} + \mathcal{R}_1 [\mu^s(t) - \mathbf{1}_{2N}\mu_o(t)] - \gamma \mathcal{R}_1 \text{sign}(\mathcal{R}_2^\top \zeta_2) \quad (52b)$$

$$\dot{\tilde{V}} = -c_v \tilde{V} \quad (52c)$$

**Remark 6:** The function in (48) is reminiscent of scalar potential functions in constrained environments. Hence, the appearance of multiple critical points is inevitable [19]. Indeed, the gradient of the BLF (16), $\nabla W_k(z_{1k})$, vanishes at the origin and at an isolated saddle point separated from the origin. Therefore, when using the gradient of (48), the closed-loop system (52) has multiple equilibria. We address such issues using tools for multi-stable systems [20], [21].

For system (52) we state the following lemma.

**Lemma 2:** The origin of the edge-based closed-loop system (52) is almost-everywhere uniformly asymptotically stable with domain of attraction $\mathcal{D} := \mathbb{R} \times \tilde{\mathcal{D}} \times \mathbb{R} \times \mathbb{R}^{2M} \times \mathbb{R}^{2N}$, with $\tilde{\mathcal{D}} := \bigcap_{k \le M} \tilde{D}_k$, where $\tilde{D}_k$ is defined in (47). Furthermore, for almost all initial conditions, the trajectories of the external dynamics (52) converge to the origin exponentially. □

*Proof:* First define the function

$$W_1(\zeta_1) := \frac{1}{2}|z_{1o}|^2 + W(z_{1t}) \quad (53)$$

where, with a slight abuse of notation, $z_{1t} \mapsto W(z_{1t})$ is defined in (49). In light of Remark 6, let us denote by $z_1^* \in \mathbb{R}^{nM}$ the vector containing the saddle points of the BLF for each edge (48). Moreover, let us define the disjoint set

$$\mathcal{W} := \{0\} \cup \{z_1^*\}, \quad (54)$$

which corresponds to the critical points of $W$ in (49). Then, $W$ satisfies

$$\frac{a_1}{2} |z_1|^2_W \le W(z_1) \le a_2 |\nabla W(z_1)|^2, \quad (55)$$

where $a_1, a_2 > 0$ and $|z_1|_W := \min \{|z_1|, |z_1 - z_1^*|\}$.

<footnote n="2">²To avoid a cumbersome notation we write $\nabla W(z_{1t})$ in place of the more appropriate spelling $\nabla W([R^\top \otimes I_2] z_{1t})$.</footnote>
The derivative of (53) along the trajectories of (52a) yields

$$ \dot{W}_1(\varsigma_1) = -c_1 \bar{\varsigma}_1^\top \mathcal{L}_1 \bar{\varsigma}_1 + \bar{\varsigma}_1^\top \varsigma_2 + \bar{\varsigma}_1^\top \mathcal{R}_1 \tilde{V}. \quad (56) $$

Then, invoking [18, Proposition 1], we have that (56) satisfies

$$ \dot{W}_1(\varsigma_1) \le -c'_1 |\bar{\varsigma}_1|^2 + |\bar{\varsigma}_1||\varsigma_2| + c'_v |\bar{\varsigma}_1||\tilde{V}|, \quad (57) $$

where $c'_1, c'_v$ are positive constants.

Next, define the candidate Lyapunov function

$$ W_2(\varsigma_2) := \frac{1}{2} \varsigma_2^\top \mathcal{P} \varsigma_2 \quad (58) $$

where $\mathcal{P}$ is the positive-definite solution to $^3-\mathcal{L}_2^\top \mathcal{P} - \mathcal{P}\mathcal{L}_2 = -\mathcal{Q}$, for any positive definite $\mathcal{Q}$. Then, the derivative of (58) along the trajectories of (52b), is defined by the differential inclusion $\varsigma_2 \in F_2(t, \varsigma_2)$, where

$$ F_2(t, \varsigma_2) := \begin{cases} (52b), & \mathcal{R}_2^\top \varsigma_2 \neq 0 \\ -c_2 \mathcal{L}_2 \varsigma_2 + c_v \mathcal{R}_1 \tilde{V} & \\ -\gamma \lambda + \mathcal{R}_1 [\mu^s(t) - \mathbf{1}_{2N} \mu_o(t)], & \mathcal{R}_2^\top \varsigma_2 = 0 \end{cases} $$

and $\lambda \in [-1, 1]$. From the proof of Proposition 1 and boundedness of $B_i^s(t, u_{ri}(t))$ for all $t \ge t_0$ and all $i \le N$ there exists a constant $\bar{\mu}^s$ such that $|\mu_i^s(t)| \le \bar{\mu}^s$ for all $t \ge t_0$. Thus, setting $\gamma \ge \sqrt{2\bar{N}}(\bar{\mu}^s + \bar{\mu}_o)$ in (51) and using $||s||_1 = s^\top \text{sign}(s)$, the derivative of $W_2$ yields

$$ \begin{aligned} \dot{W}_2(\varsigma_2) = & -c_2 \varsigma_2^\top \mathcal{P} \mathcal{L}_2 \varsigma_2 + c_v \varsigma_2^\top \mathcal{P} \mathcal{R}_1 \tilde{V} \\ & - \gamma \varsigma_2^\top \mathcal{P} \mathcal{R}_1 \text{sign}(\mathcal{R}_2^\top \varsigma_2) + \varsigma_2^\top \mathcal{P} \mathcal{R}_1 [\mu^s(t) - \mathbf{1}_{2N} \mu_o(t)] \\ & \le -c'_2 |\varsigma_2|^2 + c''_v |\varsigma_2||\tilde{V}|, \end{aligned} \quad (59) $$

where $c'_2$ and $c''_v$ are positive constants.

Next, let $\varsigma^\top := [\varsigma_1^\top \ \varsigma_2^\top \ \tilde{V}^\top]$ and define the candidate Lyapunov function

$$ W_\varsigma(\varsigma) := W_1(\varsigma_1) + \kappa_1 W_2(\varsigma_2) + \frac{\kappa_2}{2} |\tilde{V}|^2. \quad (60) $$

From (57), (59), and (52c), we have

$$ \begin{aligned} \dot{W}_\varsigma(\varsigma) \le & -c'_1 |\bar{\varsigma}_1|^2 - \kappa_1 c'_2 |\varsigma_2|^2 - \kappa_2 c_v |\tilde{V}|^2 \\ & + |\bar{\varsigma}_1||\varsigma_2| + c'_v |\varsigma_1||\tilde{V}| + \kappa_1 c''_v |\varsigma_2||\tilde{V}|. \end{aligned} \quad (61) $$

Setting $\kappa_1, \kappa_2$ large enough, we obtain

$$ \begin{aligned} \dot{W}_\varsigma(\varsigma) \le & -\bar{c}_1 |\bar{\varsigma}_1|^2 - \bar{c}_2 |\varsigma_2|^2 - \bar{c}_v |\tilde{V}|^2 \\ & \le -\bar{c} W_\varsigma(\varsigma). \end{aligned} \quad (62) $$

Now, recalling Remark 6, let

$$ W_\varsigma := \{0\} \times W \times \{0\} \times \{0\}^{2(N-1)} \times \{0\}^{2N} \quad (63) $$

where $W$ is defined in (54), be the set containing the equilibria of the closed-loop system (52). Then, from (55) we have

$$ \dot{W}_\varsigma(\varsigma) \le -\bar{c}' |\varsigma|^{\bar{W}_\varsigma}. \quad (64) $$

Thus, the closed-loop system (52) is uniformly asymptotically multi-stable at $W_\varsigma$, cf. [20]. Furthermore, since the critical point $z_1^*$ of the barrier Lyapunov function is a saddle point, after [21, Proposition 1], it follows that the region of attraction of the unstable equilibrium $z_1^*$ has zero Lebesgue measure.

Therefore, we conclude that the origin of (52) is almost-everywhere uniformly asymptotically stable in $\mathcal{D}$, except for a zero-measure set of initial conditions.

To establish forward invariance of the set $\tilde{\mathcal{D}}$ we proceed by contradiction. Assume that there exists $0 < T < \infty$ such that for all $t \in [t_0, t_0+T)$, $\tilde{z}_{1k}(t) \in \tilde{\mathcal{D}}_k$ and $z_{1k}(t_0+T) \notin \tilde{\mathcal{D}}_k$, for at least one $k \le M$. More precisely, we have $|z_{1k}(t)| \to \Delta_k$ or $|z_{1k}(t)| \to \delta_k$ as $t \to t_0+T$ for at least one $k \le M$. From the definition of $\tilde{z}_{1t} \mapsto W(\tilde{z}_{1t})$ in (49) and $\tilde{z}_{1k} \mapsto W_k(\tilde{z}_{1k})$ in (48), this implies that $W_\varsigma(\varsigma(t)) \to \infty$ as $t \to t_0+T$ which is in contradiction with (62). We conclude that $T = \infty$ and that for all initial conditions such that $\tilde{z}_1(t_0) \in \tilde{\mathcal{D}}$, $\tilde{z}_1(t) \in \tilde{\mathcal{D}}$ for all $t \ge t_0$. Satisfaction of the inter-agent constraints follows from the forward invariance of $\tilde{\mathcal{D}}$.

Since (52) is asymptotically stable at the origin, with domain of attraction $\mathcal{D}$, it follows that for (almost) all initial conditions $\varsigma(t_0) \in \mathcal{D}$ there exist small positive constants $\varepsilon(\varsigma(t_0))$ and $\bar{\varepsilon}(\varsigma(t_0))$ such that $z_{1k}(t) \in \tilde{\mathcal{D}}_{\varepsilon k}$, where

$$ \tilde{\mathcal{D}}_{\varepsilon k} := \{z_{1k} \in \mathbb{R}^2 : \delta_k + \varepsilon \le |z_{1k} + z_{1k}^d| \le \Delta_k - \bar{\varepsilon}\}, \quad \forall k \le M. $$

Moreover, for any $z_1 \in \tilde{\mathcal{D}}_\varepsilon$, with $\tilde{\mathcal{D}}_\varepsilon := \bigcap_{k \le M} \tilde{\mathcal{D}}_{\varepsilon k}$, we have that the BLF $W$ in (49) satisfies

$$ \frac{a_1}{2} |z_{1t}|^2_W \le W(z_{1t}) \le \frac{a'_1}{2} |z_{1t}|^2_W. \quad (65) $$

Therefore, from (65) and (62), we conclude that for almost all initial conditions $\varsigma(t_0) \in \mathcal{D}$, the trajectories $\varsigma(t)$ of the external dynamics converge to the origin exponentially. ■

### D. Internal dynamics

The $i$th component of the nominal part of (34a) yields

$$ \dot{\tilde{\zeta}}_{1i} = \tilde{\zeta}_{2i} \quad (66a) $$

$$ \begin{aligned} \dot{\tilde{\zeta}}_{2i} = & -(a(t) \cos \tilde{\zeta}_{1i} + b) \tilde{\zeta}_{2i} - (c(t) \cos \tilde{\zeta}_{1i} + d(t)) \sin \tilde{\zeta}_{1i} \\ & + \vartheta(t, \tilde{\zeta}_i), \end{aligned} \quad (66b) $$

where $\vartheta(t, \tilde{\zeta}_i) := -(a(t) \cos \tilde{\zeta}_{1i} + b) \dot{\phi}_o(t) - \ddot{\phi}_o(t)$. Due to $\vartheta(\cdot)$, (66) does not have an equilibrium at the origin. Thus, we analyze the ultimate boundedness of $\tilde{\zeta}_i(t)$.

For this purpose, define the candidate Lyapunov function

$$ W_{\tilde{\zeta},i}(t, \tilde{\zeta}_i) = \frac{1}{2} \tilde{\zeta}_{si}^\top \begin{bmatrix} a^2(t) + c(t) & a(t) \\ a(t) & 1 \end{bmatrix} \tilde{\zeta}_{si} \\ + (a(t)b + d(t))(1 - \cos \tilde{\zeta}_{1i}). \quad (67) $$

Note that, $W_{\tilde{\zeta},i} > 0$ for all $[\cos \tilde{\zeta}_{1i}, \sin \tilde{\zeta}_{1i}, \tilde{\zeta}_{2i}] \ne [1, 0, 0]$. The derivative of (67) yields

$$ \begin{aligned} \dot{W}_{\tilde{\zeta},i}(t, \tilde{\zeta}_i) = & - \tilde{\zeta}_{si}^\top \begin{bmatrix} a(t)(d(t) - c(t) \cos \tilde{\zeta}_{1i}) & 0 \\ 0 & b \end{bmatrix} \tilde{\zeta}_{si} \\ & + \frac{\partial W_{\tilde{\zeta},i}}{\partial \tilde{\zeta}_i} \begin{bmatrix} 0 \\ \vartheta(t, \tilde{\zeta}_i) \end{bmatrix} + \tilde{\zeta}_{si}^\top \begin{bmatrix} 2\dot{a}(t)a(t) + \dot{c}(t) & \dot{a}(t) \\ \dot{a}(t) & 0 \end{bmatrix} \tilde{\zeta}_{si} \\ & + (\dot{a}(t)b + \dot{d}(t))(1 - \cos \tilde{\zeta}_{1i}). \end{aligned} $$

Note that, $\dot{a}, \dot{c}$, and $\dot{d}$ are only function of $\dot{U}_o$, cf. (29)-(30). Therefore, from Remark 2 there exist constants $\bar{a}^*, \bar{c}^*, \bar{d}^*$ such

<footnote>$^3$This holds since for any directed graph containing a spanning tree, $-E_t^\top E_\odot R^\top$ is Hurwitz, cf. [22]</footnote>
that $|\dot{a}(t)| \le \bar{a}^*$, $|\dot{c}(t)| \le \bar{c}^*$, and $|\dot{d}(t)| \le \bar{d}^*$. Moreover, from Remark 5 and the definition of $\vartheta$, we have that

$$ \left| \frac{\partial W_{\tilde{\zeta},i}}{\partial \tilde{\zeta}_i} \right| \left| \begin{bmatrix} 0 \\ \vartheta(t, \tilde{\zeta}_i) \end{bmatrix} \right| \le \alpha_2 \bar{\vartheta} |\tilde{\zeta}_{si}| \quad (68) $$

where $\alpha_2 > 0$ and the bound $\bar{\vartheta} > \vartheta(t, \tilde{\zeta}_i)$ exists since $\vartheta$ is a function of bounded signals. Hence, there exist positive constants $\alpha_1$ and $\alpha_3$ such that

$$ \dot{W}_{\tilde{\zeta},i}(t, \tilde{\zeta}_i) \le -\alpha_1 |\tilde{\zeta}_{si}|^2 + \alpha_2 \bar{\vartheta} |\tilde{\zeta}_{si}| + \alpha_3, \quad (69) $$

From (69) and [23, Corollary 5.1] we conclude that for every $i \le N$ the nominal subsystem (66) is uniformly bounded.

### E. Complete cascaded system

Consider now the system (34). From Lemma 2 we have that the driving system (52) in edge coordinates is uniformly asymptotically stable almost everywhere in $D$, which implies that so is the subsystem (34b). Moreover, in $D$ the trajectories of (34b) converge to the origin exponentially. Therefore, from converse Lyapunov theorems there exists a function $W_\chi$ that, in $D$, satisfies

$$ \alpha'_4 |\chi|^2 \le W_\chi(t, \chi) \le \alpha'_5 |\chi|^2 \quad (70a) $$

$$ \dot{W}_\chi(t, \chi) \le -\alpha'_6 |\chi|^2 \quad (70b) $$

$$ \left| \frac{\partial W_\chi}{\partial \chi} \right| \le \alpha'_7 |\chi|, \quad (70c) $$

for some positive constants $\alpha'_4$, $\alpha'_5$, $\alpha'_6$, and $\alpha'_7$.

Now, note that the interconnection term $G$ on the right-hand side of (34a) satisfies the bound

$$ |G(\tilde{\zeta}, \chi)| \le G_1(|\chi|) |\tilde{\zeta}_s| + G_2(|\chi|), \quad (71) $$

where $G_1, G_2 \in K_\infty$. Moreover, from the forward invariance of $\tilde{D}$ established in the proof of Lemma 2 and the asymptotic stability of (34b), the bound (71) may be simplified to

$$ |G(\tilde{\zeta}, \chi)| \le \bar{G}_1 |\chi| |\tilde{\zeta}_s| + \bar{G}_2 |\chi|, \quad (72) $$

for some positive constants $\bar{G}_1$ and $\bar{G}_2$.

Define the candidate Lyapunov function

$$ \tilde{W}(t, \tilde{\zeta}, \chi) := kW_\chi(t, \chi) + \sum_{i \le N} W_{\tilde{\zeta},i}(t, \tilde{\zeta}_i), \quad (73) $$

where $W_{\tilde{\zeta},i}$ is defined in (67). From, (68), (69), (70), and (72), in $D$ the derivative of $\tilde{W}$ along the trajectories of (34) satisfies

$$ \begin{aligned} \dot{\tilde{W}}(t, \tilde{\zeta}, \chi) &\le -\alpha_1 |\tilde{\zeta}_s|^2 + \alpha_2 \bar{\vartheta} |\tilde{\zeta}_s| - k\alpha'_6 |\chi|^2 + \alpha_3 \\ &\quad + \alpha_2 |\tilde{\zeta}_s| \left[ \bar{G}_1 |\chi| |\tilde{\zeta}_s| + \bar{G}_2 |\chi| \right] \\ &\le -\alpha'_1 |\tilde{\zeta}_s|^2 + \alpha_2 \bar{G}_2 |\chi| |\tilde{\zeta}_s| - k\alpha'_6 |\chi|^2 \\ &\quad + \alpha_2 \bar{G}_1 |\chi| |\tilde{\zeta}_s|^2 + \alpha'_3 \end{aligned} \quad (74) $$

where $\alpha'_1$ and $\alpha'_3$ are positive constants.

Now, since the trajectories of (34b) converge to the origin exponentially fast, the exists a time $T > t_0$ such that for all

$t \ge t_0 + T$, $|\chi(t)| \le \alpha'_1/(2\alpha_2\bar{G}_1)$. Hence, for all $t \ge t_0 + T$ and for a sufficiently large $k$ we have

$$ \begin{aligned} \dot{\tilde{W}}(t, \tilde{\zeta}, \chi) &\le -\frac{\alpha'_1}{2} |\tilde{\zeta}_s|^2 + \alpha_2 \bar{G}_2 |\chi| |\tilde{\zeta}_s| - k\alpha'_6 |\chi|^2 \\ &\quad + \alpha_2 \bar{G}_1 |\chi| |\tilde{\zeta}_s|^2 + \alpha'_3 \\ &\le -\alpha'_1 |\tilde{\zeta}_s|^2 - \alpha'_6 |\chi|^2 + \alpha'_3, \end{aligned} \quad (75) $$

for some positive constants $\alpha''_1$ and $\alpha''_6$. On the other hand, for all $t < t_0 + T$ we have

$$ \dot{\tilde{W}}(t, \tilde{\zeta}, \chi) \le k' \tilde{W} + \alpha'_3, \quad k' > 0, \quad (76) $$

implying that there is no finite escape time. Thus, from (75) and (76) the trajectories of the closed-loop system (34) are uniformly ultimately bounded, the limits in (14) are achieved exponentially and the hard and soft constraints sets, respectively (9) and (12) are forward invariant.

### V. SIMULATION RESULTS

In this section we illustrate the performance of the controller (6), (51), through high-fidelity simulations in DUNE [24]. DUNE (Unified Navigation Environment) is a software platform designed to run on autonomous underwater vehicles. DUNE also contains a high-fidelity AUV simulator, allowing us to validate the proposed control algorithm, reproducing as closely as possible a physical experiment. We conduct two simulation cases of the tracking-in-formation problem for six ASVs subject to hard (distance) and soft (positive surge velocity) constraints. It is only assumed that the vehicles are interconnected at the initial time, so the controller must preserve such connectivity. An animation of the simulation results is accessible at https://youtu.be/1yJm3_0jYfI.

In the first scenario, we consider that the agents interact over a directed spanning tree illustrated in Fig. 2 and follow a virtual leader describing a circular trajectory (solid black line in Fig. 4) determined by (13) with the input given by $\mu_o(t) = [-0.022 \cos(0.021t) - 0.022 \sin(0.021t)]^\top$, with initial conditions $h_o(0) = [200 \ 0]^\top \text{m}$ and $\nu_{ho}(0) = [0 \ 1.05]^\top \text{m/s}$. In the second scenario, the agents communicate over a cycle graph illustrated in Fig. 3 and the leader describes a figure-eight trajectory (solid black line in Fig. 10) given by (13) and the input $\mu_o(t) = [-0.024 \cos(0.022t + 1.57) - 0.048 \sin(0.044t + 3.14)]^\top$, with initial conditions $h_o(0) = [150 \ 0]^\top \text{m}$ and $\nu_{ho}(0) = [-1.1 \ -1.1]^\top \text{m/s}$. In both cases, only the agent labeled “1” knows the state of the target (labeled “0”).

The initial conditions for both scenarios are presented in Table I. The desired formation is a regular hexagon described by the desired relative position vectors $z_{1k}^d$ given by (15, 7.5), (-15, 22.5), (-15, 7.5), (0, 15), (-15, 22.5), for the spanning tree topology and (15, 7.5), (0, 15), (-15, 7.5), (-15, -7.5), (0, -15), (15, -7.5), for the cycle topology. The maximal and minimal distance parameters are $\Delta_k = 75 \text{ m}$ and $\delta_k = 5 \text{ m}$. The hand position point is chosen at $l = 1 \text{ m}$, and the control gains are set to $c_1 = 1$, $c_2 = 0.2$, $\gamma = 0.25$, $c_v = 0.2$, $\kappa_u = 0.05$, $\kappa_\rho = 4$, and $\sigma = 0.3$. Furthermore, in order to avoid discontinuities in the control, the non-smooth sign(s) function
in (51) and (28) is replaced by the smooth approximation
tanh($c_a s$), $c_a \gg 1$.

<figure><img src="image_2.png" /><figcaption>Fig. 2: Interaction topology: directed spanning tree.</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>Fig. 3: Interaction topology: directed cycle.</figcaption></figure>

<table><caption>TABLE I: Initial conditions</caption><thead><tr><th>Index</th><th>x<sub>i</sub></th><th>y<sub>i</sub></th><th>ψ<sub>i</sub></th><th>u<sub>ri</sub></th><th>v<sub>ri</sub></th><th>r<sub>i</sub></th></tr></thead><tbody><tr><td>1</td><td>103</td><td>35</td><td>6.25</td><td>0</td><td>0</td><td>0</td></tr><tr><td>2</td><td>75</td><td>34</td><td>4.56</td><td>0</td><td>0</td><td>0</td></tr><tr><td>3</td><td>112</td><td>17</td><td>1.61</td><td>0</td><td>0</td><td>0</td></tr><tr><td>4</td><td>90</td><td>-12</td><td>4.64</td><td>0</td><td>0</td><td>0</td></tr><tr><td>5</td><td>108</td><td>-6</td><td>4.65</td><td>0</td><td>0</td><td>0</td></tr><tr><td>6</td><td>62</td><td>4</td><td>3.32</td><td>0</td><td>0</td><td>0</td></tr></tbody></table>

<figure><img src="image_4.png" /><figcaption>Fig. 4: Paths followed by the agents. The initial conditions and final configuration are marked by blue and red arrows, respectively. The solid black line represents the trajectory of the virtual leader.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Fig. 5: Inter-agent distances. The dashed lines represent the connectivity and collision-avoidance constraints.</figcaption></figure>

Figs. 4-8 present the results of the simulation scenario with the spanning tree topology. As shown in Fig. 4, the LAUVs manage to reach the desired formation (red arrows) as is also evidenced from the formation and tracking errors in Fig. 5. Furthermore, the formation and velocity errors are presented in Figs. 6-7. Note that the errors do not converge to zero but

<figure><img src="image_6.png" /><figcaption>Fig. 6: Norms of the formation/tracking errors.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>Fig. 7: Norms of the velocity errors.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 8: Surge velocities under the soft-constraint requirement.</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>Fig. 9: Surge velocities without the soft-constraint requirement.</figcaption></figure>

rather to a small region around zero. These nonzero steady-state errors are caused by two factors: the uncertainty of the navigation system, and the delay in the actuators. The vehicles still manage to respect the hard connectivity and collision-avoidance constraints, shown as dashed red lines in Fig. 5. The surge velocities in Fig. 8 are non-negative. For comparison, the surge velocities shown in Fig. 9 result from a simulation without the soft-constraint requirement, and it is clear that in contrast to Fig. 8, in this case the velocities of vehicles 3, 4, 5, and 6 also become negative between 90s and 150s.

Figs. 10-14 present the results of the second scenario, with the cycle graph topology. It can be seen from Figs. 10 and 11 that also here the LAUVs manage to reach the desired formation (red arrows), with a small steady-state error in the formation and velocity errors, presented in Figs. 12 and 13, respectively. As before, a second simulation without the soft-constraint requirement was performed for comparison. In contrast to Fig. 8 where all the surge velocities are positive,
<figure><img src="image_10.png" /><figcaption>Fig. 10: Paths followed by the agents. The initial conditions and final configuration are marked by blue and red arrows, respectively. The solid black line represents the trajectory of the virtual leader.</figcaption></figure>

<figure><img src="image_11.png" /><figcaption>Fig. 11: Inter-agent distances. The dashed lines represent the connectivity and collision-avoidance constraints.</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>Fig. 12: Norms of the formation/tracking errors.</figcaption></figure>

it can be seen in Fig. 9 that without the soft-constraint requirement, the velocities of vehicles 5 and 6 become negative at around 70s and 110s. Note that in this case the collision-avoidance constraint is slightly violated for one pair of vehicles around 50s. This is because in the DUNE simulations, contrary to the theoretical results presented above, both the velocity and the acceleration of the LAUVs are bounded due to the physical limits of the actuators. Therefore, although in theory the control input should increase unboundedly to prevent that the constraints are violated, in practice this is not the case and the control inputs saturate. The consideration of saturation in the control together with hard and soft constraints is, however, still an open problem and out of the scope of this paper. Lastly, we note that, in the included video of the simulations, it may appear as if there are collisions. Please note that this is only a visualization issue. Specifically, the sizes of the arrows representing the LAUVs are chosen larger than their real sizes.

## VI. CONCLUSIONS AND FUTURE WORK

In this paper we addressed the tracking-in-formation control problem of cooperative autonomous marine vehicles interacting over directed graphs and under hard inter-agent

<figure><img src="image_13.png" /><figcaption>Fig. 13: Norms of the velocity errors.</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>Fig. 14: Surge velocities under the soft-constraint requirement.</figcaption></figure>

<figure><img src="image_15.png" /><figcaption>Fig. 15: Surge velocities without the soft-constraint requirement.</figcaption></figure>

constraints (proximity and collision avoidance) and soft constraints (positive surge velocity). We proposed a distributed control law that solves this problem and that guarantees, simultaneously, connectivity preservation and inter-agent collision avoidance. With respect to the stability analysis, it is important to emphasize that, beyond mere convergence properties as usually established in the literature of multi-agent systems, we establish almost-everywhere uniform asymptotic stability with exponential convergence of the tracking errors. Current research focuses on validating the results experimentally and extending them to consider input saturation and AUVs in 3D.

## APPENDIX I

Let $m_{ij}$ and $d_{ij}$ be, respectively, the $ij$-th entries of the mass and damping matrices. Then, we have

$$X_1 = \frac{m_{11}m_{33} - m_{23}^2}{m_{22}m_{33} - m_{23}^2}; \quad X_2 = \frac{d_{33}m_{23} - d_{23}m_{33}}{m_{22}m_{33} - m_{23}^2} \quad (77)$$

$$Y_1 = \frac{(m_{11} - m_{22})m_{23}}{m_{22}m_{33} - m_{23}^2}; \quad Y_2 = \frac{d_{22}m_{33} - d_{32}m_{23}}{m_{22}m_{33} - m_{23}^2} \quad (78)$$

$$F_{u_r}(v_r, r) = \frac{1}{m_{11}}(m_{22}v_r + m_{23}r) - \frac{d_{11}}{m_{11}}u_r \quad (79)$$

$$F_r(u_r, v_r, r) = \frac{m_{23}d_{22} - m_{22}(d_{32} + (m_{22} - m_{11})u_r)}{m_{22}m_{33} - m_{23}^2}v_r$$
$$ + \frac{m_{23}(d_{23} + m_{11}u_r) - m_{22}(d_{33} + m_{23}u_r)}{m_{22}m_{33} - m_{23}^2} r. \quad (80) $$

$$ \begin{bmatrix} \alpha(\tilde{\zeta}_i, \tilde{\xi}_{3i}, \tilde{\xi}_{4i}) \\ \beta(\tilde{\zeta}_i, \tilde{\xi}_{3i}, \tilde{\xi}_{4i}) \end{bmatrix} = \mathfrak{R}^{\top}(\tilde{\zeta}_{1i} + \phi_o) \begin{bmatrix} v_1(\tilde{\zeta}_i, \tilde{\xi}_{3i}, \tilde{\xi}_{4i}) \\ v_2(\tilde{\zeta}_i, \tilde{\xi}_{3i}, \tilde{\xi}_{4i}) + \frac{Y_2}{l} \end{bmatrix} \quad (81) $$

where $\mathfrak{R}(\cdot) : \mathbb{R} \to \text{SO}(2)$ is the rotation matrix and

$$ v_1(\cdot) = \frac{Y_1}{l} U_o \sin \tilde{\zeta}_{1i} + \left( Y_1 - \frac{X_1 - 1}{l} \right) \zeta_{2i} $$

$$ v_2(\cdot) = \frac{Y_1}{l} \left[ (\bar{\xi}_{3i} \cos(\tilde{\zeta}_{1i} + \phi_o) + \bar{\xi}_{4i} \sin(\tilde{\zeta}_{1i} + \phi_o)) + U_o \cos \tilde{\zeta}_{1i} \right]. $$

REFERENCES

[1] J. Nicholson and A. Healey, “The present state of autonomous underwater vehicle (AUV) applications and technologies,” *Marine Technology Society Journal*, vol. 42, no. 1, pp. 44–51, 2008.

[2] X. Xiang, B. Jouvencel, and O. Parodi, “Coordinated formation control of multiple autonomous underwater vehicles for pipeline inspection,” *International Journal of Advanced Robotic Systems*, vol. 7, no. 1, p. 3, 2010.

[3] B. Das, B. Subudhi, and B. B. Pati, “Cooperative formation control of autonomous underwater vehicles: An overview,” *International Journal of Automation and Computing*, vol. 13, no. 3, pp. 199–225, Jun. 2016.

[4] S. K. Gan, R. Fitch, and S. Sukkarieh, “Online decentralized information gathering with spatial-temporal constraints,” *Autonomous Robots*, vol. 37, pp. 1–25, 2014.

[5] R. Gomes and F. L. Pereira, “A model predictive control scheme for autonomous underwater vehicle formation control,” in *Proc. 13th APCAA International Conference on Automatic Control and Soft Computing*, 2018, pp. 195–200.

[6] Q. Jia and G. Li, “Formation control and obstacle avoidance algorithm of multiple autonomous underwater vehicles (AUVs) based on potential function and behavior rules,” in *Proc. 2007 IEEE International Conference on Automation and Logistics*, 2007, pp. 569–573.

[7] L. Briñón Arranz, A. Seuret, and C. Canudas de Wit, “Contraction control of a fleet circular formation of AUVs under limited communication range,” in *Proc. 2010 American Control Conference*, 2010, pp. 5991–5996.

[8] Z. Gao and G. Guo, “Velocity free leader-follower formation control for autonomous underwater vehicles with line-of-sight range and angle constraints,” *Information Sciences*, pp. 359–378, 2019.

[9] A. Naderolasli, K. Shojaei, and A. Chatraei, “Platoon formation control of autonomous underwater vehicles under LOS range and orientation angles constraints,” *Ocean Engineering*, 2023.

[10] C. Paliotta, E. Lefeber, K. Y. Pettersen, J. Pinto, M. Costa, and J. T. de Figueiredo Borges de Sousa, “Trajectory tracking and path following for underactuated marine vehicles,” *IEEE Transactions on Control Systems Technology*, vol. 27, no. 4, pp. 1423–1437, 2019.

[11] M. Mesbah and M. Egerstedt, *Graph theoretic methods in multiagent networks*, ser. Princeton series in applied mathematics. Princeton: Princeton University Press, 2010, oCLC: ocn466341412.

[12] E. Restrepo, J. Matouš, and K. Y. Pettersen, “Tracking-in-formation of multiple autonomous marine vehicles under proximity and collision-avoidance constraints,” in *Proc. 2022 European Control Conference*, 2022, pp. 930–937.

[13] T. I. Fossen, *Handbook of marine craft hydrodynamics and motion control*. John Wiley & Sons, 2011.

[14] W. Caharija, K. Y. Pettersen, M. Bibuli, P. Calado, E. Zereik, J. Braga, J. T. Gravdahl, A. J. Sørensen, M. Milovanović, and G. Bruzzone, “Integral line-of-sight guidance and control of underactuated marine vehicles: Theory, simulations, and experiments,” *IEEE Transactions on Control Systems Technology*, vol. 24, no. 5, pp. 1623–1642, 2016.

[15] J. Lawton, R. Beard, and B. Young, “A decentralized approach to formation maneuvers,” *IEEE Transactions on Robotics and Automation*, vol. 19, no. 6, pp. 933–941, 2003.

[16] K. P. Tee, S. S. Ge, and E. H. Tay, “Barrier Lyapunov functions for the control of output-constrained nonlinear systems,” *Automatica*, vol. 45, no. 4, pp. 918–927, 2009.

[17] F. Mehdifar, C. P. Bechlioulis, and D. V. Dimarogonas, “Funnel control under hard and soft output constraints,” in *Proc. 61st Conference on Decision and Control*, 2022, pp. 4473–4478.

[18] E. Restrepo, A. Loría, I. Sarras, and J. Marzat, “Edge-based strict Lyapunov functions for consensus with connectivity preservation over directed graphs,” *Automatica*, vol. 132, p. 109812, 2021.

[19] E. Rimon and D. E. Koditschek, “Exact robot navigation using artificial potential functions,” *IEEE Transactions on Robotics and Automation*, vol. 8, no. 5, pp. 501–518, Oct. 1992.

[20] P. Forni and D. Angeli, “Input-to-state stability for cascade systems with multiple invariant sets,” *Systems & Control Letters*, vol. 98, pp. 97–110, 2016.

[21] P. Monzón and R. Potrie, “Local and global aspects of almost global stability,” in *Proc. 45th IEEE Conference on Decision and Control*, San Diego, CA, USA, 2006, pp. 5120–5125.

[22] D. Mukherjee and D. Zelazo, “Robustness of consensus over weighted digraphs,” *IEEE Transactions on Network Science and Engineering*, vol. 6, no. 4, pp. 657–670, 2019.

[23] H. K. Khalil, *Nonlinear systems; 2nd ed.* Upper Saddle River, NJ: Prentice-Hall, 1996.

[24] “DUNE: Unified navigation environment,” https://github.com/LSTS/dune/, accessed 2023-03-14.

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

Esteban Restrepo received the B.Sc. degree in Mechatronics Engineering from Universidad EIA, Envigado, Colombia and from Arts et Métiers, Paris, France, in 2017, and the M.Sc. degree in Robotic Systems Engineering from Arts et Métiers, Paris, France, in 2018. He obtained the Ph.D. degree in Automatic Control from the University Paris-Saclay, France, in 2021. From January 2022 to January 2023 he was a Postdoctoral Researcher with the Division of Decision and Control Systems, KTH Royal

Institute of Technology, Stockholm, Sweden. He is currently a Postdoctoral Researcher with CNRS, and Inria Rennes, France. His research interests include control of multi-agent systems with application to autonomous robots and aerospace systems.

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

Josef Matouš received the BSc degree in Systems and Control and the MSc degree in Cybernetics and Robotics from the Czech Technical University in Prague, Czechia, in 2018 and 2020, respectively, and the PhD degree in Engineering Cybernetics at the Norwegian University of Science and Technology in Trondheim (NTNU), Norway, in 2023. Currently, he is a postdoctoral researcher at the Department of Engineering Cybernetics at NTNU. His research interests include distributed cooperative control of under-

water vehicles, and applications of advanced nonlinear control schemes to the marine domain.

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

Kristin Y. Pettersen (S’93-M’98-SM’04-F’17) received the MSc and PhD degrees in engineering cybernetics from the Norwegian University of Science and Technology (NTNU), Trondheim, Norway, in 1992 and 1996, respectively. She is a Professor in the Department of Engineering Cybernetics, NTNU, where she has been a faculty member since 1996. She was Head of Department 2011-2013, Vice-Head of Department 2009-2011, and Director of the NTNU ICT Program of Robotics 2010-2013. She is Adjunct

Professor at the Norwegian Defence Research Establishment (FFI) and Key Scientist at the CoE for Autonomous Marine Operations and Systems and the VISTA Centre for Autonomous Robotic Operations Subsea. She is a co-founder of the NTNU spin-off company Eelume AS, where she was CEO 2015-2016. She has published four books and more than 300 papers in international journals and conference proceedings. Her research interests focus on nonlinear control of mechanical systems with applications to robotics, with a special emphasis on marine robotics and snake robotics. She was awarded the IEEE Transactions on Control Systems Technology Outstanding Paper Award in 2006 and in 2017. She was awarded an ERC-AdG-2020 Advanced Grant from the European Research Council, and received the IEEE CSS Hendrik W. Bode Lecture Prize in 2020.