# Formation Path Following Control of Underactuated AUVs – With Proofs

Josef Matouš Kristin Y. Pettersen Claudio Paliotta Josef Matouš and Kristin 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,
<EMAIL_ADDRESS>Claudio Paliotta is with SINTEF Digital,
Trondheim, Norway<EMAIL_ADDRESS>
###### Abstract

This paper proposes a novel method for formation path following of multiple
underactuated autonomous underwater vehicles. The method combines line-of-
sight guidance with null-space-based behavioral control, allowing the vehicles
to follow curved paths while maintaining the desired formation. We investigate
the dynamics of the path-following error using cascaded systems theory, and
show that the closed-loop system is uniformly semi-globally exponentially
stable. We validate the theoretical results through numerical simulations.

##  I Introduction

Autonomous underwater vehicles (AUVs) are being increasingly used in a number
of applications such as transportation, seafloor mapping, and the ocean energy
industry. Some complex tasks need to be performed by a group of cooperating
AUVs. Consequently, there is a need for control algorithms that can guide a
formation of AUVs along a given path while avoiding collisions with each
other.

A comprehensive overview of various formation path-following methods is
presented in [1]. Most of these methods are based on two concepts: coordinated
path-following [2, 3], and leader-follower [4, 5]. In the _coordinated path-
following_ approach, each vehicle follows a predefined path separately.
Formation is then achieved by coordinating the motion of the vehicles along
these paths. In this approach, the formation-keeping error (_i.e.,_ the
difference between the actual and desired relative position of the vehicles)
may initially grow as the vehicles converge to their predefined paths. In the
_leader-follower_ approach, one leading vehicle follows the given path while
the followers adjust their speed and position to obtain the desired formation
shape. This approach tends to suffer from the lack of formation feedback due
to unidirectional communication (_i.e.,_ the leader does not adjust its
velocity based on the followers).

The null-space-based behavioral (NSB) algorithm has also been proposed to
solve the formation path-following problem [6, 7, 8, 9]. The NSB algorithm is
a centralized control method that allows to combine several hierarchic tasks.
In the NSB framework, it is possible to design the path-following, formation-
keeping, and collision avoidance tasks independently. By combining these
tasks, the vehicles exhibit the desired behavior.

This paper aims to extend the results of [9], where an NSB algorithm is used
to guide two surface vessels moving in the horizontal plane. Specifically, we
propose an algorithm that works with an arbitrary number of AUVs with five
degrees of freedom (DOFs) moving in 3D. Similarly to [9], we solve the path-
following task using line-of-sight (LOS) guidance. Using the cascaded systems
theory results of [10], we prove that the closed-loop system consisting of a
3D LOS guidance law, combined with surge, pitch, and yaw autopilots based on
[11], is uniformly semi-globally exponentially stable (USGES) and uniformly
globally asymptotically stable (UGAS). The theoretical results are verified
through numerical simulations.

The remainder of the paper is organized as follows. Section II introduces the
model of the AUVs. In Section III, we define the formation path-following
problem. In Section IV, we describe the control system. The stability of the
control system is proven in Section V. Section VI contains the results of a
numerical simulation. Finally, Section VII contains some concluding remarks.

##  II Model

In this section, we present the model of the AUV. We start by introducing the
model in a matrix-vector form. Then, we write out the ordinary differential
equations (ODEs) for the individual state variables.

###  II-A Vehicle Model in Vector-Matrix Form

The pose ($\boldsymbol{\eta}$) and velocities ($\boldsymbol{\nu}$) of an AUV
with 5DOFs are defined as

$\displaystyle\boldsymbol{\eta}$
$\displaystyle=\left[x,y,z,\theta,\psi\right]^{\rm T},$
$\displaystyle\boldsymbol{\nu}$ $\displaystyle=\left[u,v,w,q,r\right]^{\rm
T},$ (1)

where $x,y,z$ are the coordinates of the vehicle in North-East-Down (NED)
coordinate frame, and $\theta$ and $\psi$ are the pitch and yaw angles,
respectively. The velocities $u,v,w$ are the linear surge, sway and heave
velocities in a given body-fixed frame, and $q$ and $r$ are the pitch and yaw
rate, respectively. The roll dynamics are disregarded as the roll motion is
assumed to be small and self-stabilizing by the vehicle design.

Let $\mathbf{V}_{c}=\left[V_{x},V_{y},V_{z}\right]^{\rm T}$ be the velocities
of an unknown, constant and irrotational ocean current, given in the inertial
NED frame. Let $\mathbf{J}\left(\boldsymbol{\eta}\right)$ be the
transformation matrix from the body-fixed to the inertial frame.
$\mathbf{J}\left(\boldsymbol{\eta}\right)$ is given by

$\mathbf{J}\left(\boldsymbol{\eta}\right)=\begin{bmatrix}\mathbf{R}\left(\theta,\psi\right)&\mathbf{O}_{3\times
2}\\\ \mathbf{O}_{2\times 3}&\mathbf{T}\left(\theta\right)\end{bmatrix},$ (2)

where $\mathbf{R}\left(\theta,\psi\right)$ is the rotation matrix from the
body-fixed to the inertial coordinate frames, $\mathbf{O}_{n\times m}$ is an
$n\times m$ matrix of zeros, and $\mathbf{T}\left(\theta\right)={\rm
diag}(1,1/\cos\theta)$, which is well-defined if the pitch angle
$\left|\theta\right|<\pi/2$. Note that the mechanical design of torpedo-shaped
rudder-controlled AUVs generally does not allow for pitch angles
$\left|\theta\right|=\pi/2$.

The velocities of the ocean current expressed in the body-fixed coordinate
frame, $\boldsymbol{\nu}_{c}$, are thus

$\boldsymbol{\nu}_{c}=\left[\left(\mathbf{R}\left(\theta,\psi\right)^{\rm
T}\,\mathbf{V}_{c}\right)^{\rm T},0,0\right]^{\rm T}.\vspace{-2mm}$ (3)

We will denote the relative velocities of the vehicle as
$\boldsymbol{\nu}_{r}=\boldsymbol{\nu}-\boldsymbol{\nu}_{c}$. We will also
denote the relative surge, sway and heave velocities as $u_{r}$, $v_{r}$ and
$w_{r}$, respectively.

Let $\mathbf{f}=\left[T_{u},\delta_{e},\delta_{r}\right]$ be the vector of
control inputs, where $T_{u}$ is the surge thrust generated by the propeller,
and $\delta_{e}$ and $\delta_{r}$ are the deflection angles of the elevator
and rudder, respectively. Furthermore, let $\mathbf{M}$ be the mass and
inertia matrix, including added mass effects,
$\mathbf{C}\left(\boldsymbol{\nu}_{r}\right)$ the Coriolis centripetal matrix,
also including added mass effects, and
$\mathbf{D}\left(\boldsymbol{\nu}_{r}\right)$ the hydrodynamic damping matrix.
The dynamics of the vehicle in a matrix-vector form are then [12]

$\displaystyle\dot{\boldsymbol{\eta}}$
$\displaystyle=\mathbf{J}\left(\boldsymbol{\eta}\right)\boldsymbol{\nu},$ (4)
$\displaystyle\mathbf{M}\dot{\boldsymbol{\nu}}_{r}+\mathbf{C}\left(\boldsymbol{\nu}_{r}\right)\boldsymbol{\nu}_{r}+\mathbf{D}\left(\boldsymbol{\nu}_{r}\right)\boldsymbol{\nu}_{r}+\mathbf{g}\left(\boldsymbol{\eta}\right)$
$\displaystyle=\mathbf{B}\mathbf{f},\vspace{-4.5mm}$ (5)

where $\mathbf{g}\left(\boldsymbol{\eta}\right)$ is the gravity and buoyancy
vector, and $\mathbf{B}$ is the actuator configuration matrix that maps the
control inputs to forces and torques.

###  II-B Vehicle Model in Component Form

First, let us present the necessary assumptions for deriving the ODEs for
individual state variables.

###### Assumption 1

The vehicle is slender, torpedo-shaped with port-starboard symmetry.

###### Assumption 2

The hydrodynamic damping is linear.

###### Assumption 3

The vehicle is neutrally buoyant with the center of gravity (CG) and the
center of buoyancy (CB) located along the same vertical axis.

_Remark:_ Assumptions 1 and 3 are valid from the mechanical design of
commercial survey AUVs. Assumption 2 is valid for low-speed missions. Also for
higher-speed missions, this assumption is often made when designing the
controller, as the higher-order damping coefficients are poorly known, and the
forces are dissipative. Attempting to cancel the higher-order damping can thus
introduce destabilizing control efforts.

Under these assumptions, the $\mathbf{M}$ and $\mathbf{B}$ matrices have the
following form

(6)

the corresponding Coriolis matrix is

$\mathbf{C}\left(\boldsymbol{\nu}_{r}\right)=\begin{bmatrix}0&0&0&c_{1}&-c_{2}\\\
0&0&0&0&c_{3}\\\ 0&0&0&-c_{3}&0\\\ -c_{1}&0&c_{3}&0&0\\\
c_{2}&-c_{3}&0&0&0\end{bmatrix},$ (7)

where $c_{1}=m_{34}\,q+m_{33}\,w_{r}$, $c_{2}=m_{25}\,r+m_{22}\,v_{r}$, and
$c_{3}=m_{11}\,u_{r}$. The hydrodynamic damping matrix is

$\mathbf{D}\left(\boldsymbol{\nu}_{r}\right)\approx\mathbf{D}=\begin{bmatrix}d_{11}&0&0&0&0\\\
0&d_{22}&0&0&d_{25}\\\ 0&0&d_{33}&d_{34}&0\\\ 0&0&d_{43}&d_{44}&0\\\
0&d_{52}&0&0&d_{55}\end{bmatrix},$ (8)

and the gravity vector has the following form

$\mathbf{g}\left(\boldsymbol{\eta}\right)=\left[0,0,0,m\,g\,z_{g}\,\sin(\theta)\right]^{\rm
T},$ (9)

where $m$ is the weight of the vessel, $g$ is the gravity acceleration, and
$z_{g}$ is the vertical distance between the CG and CB [12].

Furthermore, we assume that the actuators produce no sway and heave
acceleration. In other words, for every $\mathbf{f}$ there exist $f_{u},t_{q}$
and $t_{r}$ such that

$\mathbf{M}^{-1}\,\mathbf{B}\,\mathbf{f}=\left[f_{u},0,0,t_{q},t_{r}\right]^{\rm
T}.\vspace{-2mm}$ (10)

In [13], it is shown that if a vehicle satisfies Assumptions 1–3, the origin
of the body-fixed coordinate frame can always be chosen such that (10) holds.

Under these assumptions, the model can expressed in the following form

$\dot{x}$

$\displaystyle=\scalebox{0.93}{$u\,\cos\left(\psi\right)\,\cos\left(\theta\right)-v\,\sin\left(\psi\right)+w\,\cos\left(\psi\right)\,\sin\left(\theta\right),$}$
(11a)

$\dot{y}$

$\displaystyle=\scalebox{0.93}{$u\,\cos\left(\theta\right)\,\sin\left(\psi\right)+v\,\cos\left(\psi\right)+w\,\sin\left(\psi\right)\,\sin\left(\theta\right),$}$
(11b)

$\dot{z}$

$\displaystyle=\scalebox{0.93}{$-u\,\sin\left(\theta\right)+w\,\cos\left(\theta\right),$}$
(11c)

$\dot{\theta}$

$\displaystyle=\scalebox{0.93}{$q,$}$ (11d)

$\dot{\psi}$

$\displaystyle=\scalebox{0.93}{$\frac{1}{\cos\left(\theta\right)}\,r,$}$ (11e)

$\dot{u}$

$\displaystyle=\scalebox{0.93}{$f_{u}+F_{u}(u,v,w,q,r)+\boldsymbol{\phi}_{u}(u,v,w,q,r,\theta,\psi)^{\rm
T}\,\mathbf{V}_{c},$}$ (11f)

$\dot{v}$

$\displaystyle=\scalebox{0.93}{$X_{v}(u,u_{c})\,r+Y_{v}(u,u_{c})\,v_{r},$}$
(11g)

$\dot{w}$

$\displaystyle=\scalebox{0.93}{$X_{w}(u,u_{c})\,q+Y_{w}(u,u_{c})\,w_{r}+G(\theta),$}$
(11h)

$\dot{q}$

$\displaystyle=\scalebox{0.93}{$t_{q}+F_{q}(u,w,q,\theta)+\boldsymbol{\phi}_{q}(u,w,q,\theta,\psi)^{\rm
T}\,\boldsymbol{\vartheta}\left(\mathbf{V}_{c}\right),$}$ (11i)

$\dot{r}$

$\displaystyle=\scalebox{0.93}{$t_{r}+F_{r}(u,v,r)+\boldsymbol{\phi}_{r}(u,v,r,\theta,\psi)^{\rm
T}\,\boldsymbol{\vartheta}\left(\mathbf{V}_{c}\right),$}$ (11j)

where
$\boldsymbol{\vartheta}\left(\mathbf{V}_{c}\right)=\left[V_{x},V_{y},V_{z},V_{x}^{2},V_{y}^{2},V_{z}^{2},V_{x}\,V_{y},V_{x}\,V_{z},V_{y}\,V_{z}\right]^{\rm
T}$ , and the expressions for $F_{i}(\cdot)$, $\boldsymbol{\phi}_{i}(\cdot)$,
$i\in\\{u,q,r\\}$, $X_{v}(\cdot)$, $Y_{v}(\cdot)$, $X_{w}(\cdot)$,
$Y_{w}(\cdot)$, and $G(\cdot)$ are given in Appendix A.

##  III Control Objectives

The goal is to control $n$ AUVs so that they move in a prescribed formation
while avoiding collisions, and their barycenter follows a given path.

The prescribed path in the inertial coordinate frame is given by a smooth
function $\mathbf{p}_{p}:\mathbb{R}\rightarrow\mathbb{R}^{3}$. We assume that
the path function is $\mathcal{C}^{2}$ and regular, _i.e.,_ the function is
continuous up to its second derivative and its first derivative with respect
to the path parameter satisfies

$\left\|\frac{\partial\mathbf{p}_{p}(\xi)}{\partial\xi}\right\|\neq
0.\vspace{-1mm}$ (12)

Therefore, for every point $\mathbf{p}_{p}(\xi)$ on the path, there exist
path-tangential angles, $\theta_{p}(\xi)$ and $\psi_{p}(\xi)$, and a
corresponding path-tangential coordinate frame $(x^{p},y^{p},z^{p})$ (see
Figure 1).

The path-following error $\mathbf{p}_{b}^{p}$ is given by the position of the
barycenter expressed in the path-tangential coordinate frame

$\mathbf{p}_{b}^{p}=\mathbf{R}\left(\theta_{p}(\xi),\psi_{p}(\xi)\right)^{\rm
T}\,\big{(}\mathbf{p}_{b}-\mathbf{p}_{p}(\xi)\big{)},\vspace{-2mm}$ (13)

where

$\displaystyle\mathbf{p}_{b}$
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}\mathbf{p}_{i},$
$\displaystyle\mathbf{p}_{i}$
$\displaystyle=\left[x_{i},y_{i},z_{i}\right]^{\rm T},\vspace{-3mm}$ (14)

where $(x_{i},y_{i},z_{i})$ is the position of the $i^{\rm th}$ vehicle. The
goal of path following is to control the vehicles so that
$\mathbf{p}_{b}^{p}\equiv\mathbf{0}_{3}$, where $\mathbf{0}_{3}$ is a
3-element vector of zeros.

$y$
---
$x$
---
$z$
---
$x^{p}$
---
$y^{p}$
---
$z^{p}$
---
$\mathbf{p}_{p}(\xi)\equiv\mathbf{O}^{p}$
---
$\psi_{p}(\xi)$
---
$\theta_{p}(\xi)$
---
$\mathbf{O}$
---
Figure 1: Definition of the path angles and path-tangential coordinate frame.
$\mathbf{O}$ denotes the origin of the inertial coordinate frame,
$\mathbf{O}^{p}$ denotes the origin of the path-tangential frame, the grey
line represents the projection of the path-tangential vector into the
$xy$-plane. $\mathbf{p}_{p}(\xi)$
---
$\mathbf{p}_{b}^{p}\equiv\mathbf{O}^{f}$
---
$\mathbf{p}_{f,1}^{f}$
---
$\mathbf{p}_{f,2}^{f}$
---
$\mathbf{p}_{f,n}^{f}$
---
$\mathbf{p}_{1}$
---
$\mathbf{p}_{2}$
---
$\mathbf{p}_{n}$
---
Figure 2: Definition of the formation. $\mathbf{O}^{f}$ denotes the origin of
the formation-centered coordinate frame.

To define the formation-keeping problem, we first define the formation-
centered coordinate frame. This coordinate frame is created by translating the
path-tangential frame into the barycenter (see Figure 2). Let
$\mathbf{p}_{f,1}^{f},\ldots,\mathbf{p}_{f,n}^{f}$ be the position vectors
that represent the desired formation. These vectors should be chosen such that
their mean is equal to the barycenter. Since the barycenter is equivalent to
the origin of the formation-centered frame, the vectors must thus satisfy

$\sum_{i=1}^{n}\mathbf{p}_{f,i}^{f}=\mathbf{0}_{3}.\vspace{-0.5mm}$ (15)

The position of vehicle $i$ in the formation-centered frame is

$\mathbf{p}_{i}^{f}=\mathbf{R}^{\rm
T}\left(\theta_{p}(\xi),\psi_{p}(\xi)\right)\left(\mathbf{p}_{i}-\mathbf{p}_{b}\right).$
(16)

The goal of formation keeping is to have
$\mathbf{p}_{i}^{f}\equiv\mathbf{p}_{f,i}^{f}$. This problem can also be
expressed in the inertial coordinate frame as

$\displaystyle\mathbf{p}_{i}$
$\displaystyle\equiv\mathbf{R}\left(\theta_{p}(\xi),\psi_{p}(\xi)\right)\mathbf{p}_{f,i}^{f}+\mathbf{p}_{b},$
$\displaystyle i$ $\displaystyle\in\left\\{1,\ldots,n\right\\}.$ (17)

##  IV Control System

To solve the formation path following problem, we propose a method that
combines collision avoidance (COLAV), formation keeping, and LOS path
following in a hierarchic manner using an NSB algorithm. Since the NSB
algorithm outputs velocity references, we also need a low-level attitude
control system to track these references.

In this section, we first present the attitude control system. Then, in
Section IV-B, we present the NSB algorithm and the associated COLAV and
formation keeping tasks. In Section IV-C, we present the LOS guidance law for
path following. Finally, in Section IV-D, we demonstrate how to use the path
variable update law to cancel unwanted terms in the path following error
dynamics.

###  IV-A Attitude Control System

This system controls the surge velocity, pitch, and yaw via the corresponding
accelerations. The system is based on the autopilots in [11], but extended to
5DOFs.

Let $u_{d}$ be the desired surge velocity and $\dot{u}_{d}$ its derivative.
Let $\hat{\mathbf{V}}_{c}$ be the estimate of the ocean current. Furthermore,
let us define $\tilde{u}=u-u_{d}$ and
$\tilde{\mathbf{V}}_{c}=\hat{\mathbf{V}}_{c}-\mathbf{V}_{c}$. The surge
controller consists of an output-linearizing sliding-mode P-controller and an
ocean current observer

$\displaystyle f_{u}$
$\displaystyle=\dot{u}_{d}-F_{u}(\cdot)-\boldsymbol{\phi}_{u}(\cdot)^{\rm
T}\,\hat{\mathbf{V}}_{c}-k_{u}\,\tilde{u}-k_{c}\,{\rm
sign}\left(\tilde{u}\right),$ (18) $\displaystyle\dot{\hat{\mathbf{V}}}_{c}$
$\displaystyle=c_{u}\,\boldsymbol{\phi}_{u}(\cdot)\,\tilde{u},\vspace{-2mm}$
(19)

where $k_{u}$, $k_{c}$ and $c_{u}$ are positive gains.

Let $\theta_{d}$ be the desired pitch angle and
$\dot{\theta}_{d},\ddot{\theta}_{d}$ its derivatives. Let
$\hat{\boldsymbol{\vartheta}}_{q}$ be the estimate of
$\boldsymbol{\vartheta}(\mathbf{V}_{c})$. Furthermore, let us define
$\tilde{\theta}=\theta-\theta_{d}$, $\tilde{q}=q-\dot{\theta}_{d}$ and
$\tilde{\boldsymbol{\vartheta}}_{q}=\hat{\boldsymbol{\vartheta}}_{q}-\boldsymbol{\vartheta}(\mathbf{V}_{c})$.
Inspired by [14], we introduce the following transformation

$s_{q}=\tilde{q}+\lambda_{q}\,\tilde{\theta},\vspace{-2mm}$ (20)

where $\lambda_{q}$ is a positive constant. The pitch controller consists of
an output-linearizing sliding-mode PD-controller and an ocean current observer

$\displaystyle\begin{split}t_{q}&=\ddot{\theta}_{d}-F_{q}(\cdot)-\boldsymbol{\phi}_{q}(\cdot)^{\rm
T}\,\hat{\boldsymbol{\vartheta}}_{q}-\lambda_{q}\,\tilde{q}\\\ &\quad-
k_{\theta}\,\tilde{\theta}-k_{q}\,s_{q}-k_{d}\,{\rm sign}(s_{q}),\end{split}$
(21) $\displaystyle\dot{\hat{\boldsymbol{\vartheta}}}_{q}$
$\displaystyle=c_{q}\,\boldsymbol{\phi}_{q}(\cdot)\,s_{q},$ (22)

where $k_{\theta}$, $k_{q}$, $k_{d}$ and $c_{q}$ are positive gains.

Let $\psi_{d}$ be the desired yaw angle and $\dot{\psi}_{d},\ddot{\psi}_{d}$
its derivatives. Let $\hat{\boldsymbol{\vartheta}}_{r}$ be the estimate of
$\boldsymbol{\vartheta}(\mathbf{V}_{c})$. Furthermore, let us define
$\tilde{\psi}=\psi-\psi_{d}$ and
$\tilde{\boldsymbol{\vartheta}}_{r}=\hat{\boldsymbol{\vartheta}}_{r}-\boldsymbol{\vartheta}(\mathbf{V}_{c})$.
Similarly to the pitch controller, we introduce the following transformation

$s_{r}=\dot{\tilde{\psi}}+\lambda_{r}\,\tilde{\psi}=\frac{r}{\cos\theta}-\dot{\psi}_{d}+\lambda_{r}\,\tilde{\psi},$
(23)

where $\lambda_{r}$ is a positive constant. The yaw controller is analogous to
the pitch controller introduced in the previous section

$\displaystyle\begin{split}\scalebox{0.95}{$t_{r}$}&\scalebox{0.95}{$=-F_{r}(\cdot)-\boldsymbol{\phi}_{r}(\cdot)^{\rm
T}\,\hat{\boldsymbol{\vartheta}}_{r}-r\,\tan(\theta)\dot{\theta}$}\\\
&\scalebox{0.95}{$+\cos(\theta)\left(\ddot{\psi}_{d}-\lambda_{r}\,\dot{\tilde{\lambda}}-k_{\psi}\,\tilde{\psi}-k_{r}\,s_{r}-k_{d}\,{\rm
sign}(s_{r})\right),$}\end{split}$ (24) (25)

where $k_{\psi}$, $k_{r}$, $k_{d}$ and $c_{r}$ are positive gains.

###  IV-B NSB Tasks

Let us denote the variables associated with the COLAV, formation keeping, and
path following tasks by lower indices $1$, $2$, and $3$, respectively. Each
task produces a vector of desired velocities,
$\mathbf{v}_{d,i}\in\mathbb{R}^{3n},\,i\in\\{1,2,3\\}$.

For the COLAV and formation keeping tasks, the desired velocities are obtained
using task varibles, $\boldsymbol{\sigma}_{1}$ and $\boldsymbol{\sigma}_{2}$,
and their desired values, $\boldsymbol{\sigma}_{d,1}$ and
$\boldsymbol{\sigma}_{d,2}$.

First, let us consider the COLAV task. Let $d_{\rm COLAV}$ be the _activation
distance_ , _i.e.,_ the distance at which the vehicles need to start
performing the evasive maneuvers. The task variable is then given by a vector
of relative distances between the vehicles smaller than $d_{\rm COLAV}$,
_i.e.,_

$\displaystyle\boldsymbol{\sigma}_{1}$
$\displaystyle=\big{[}\left\|\mathbf{p}_{i}-\mathbf{p}_{j}\right\|\big{]}^{\rm
T},$ $\displaystyle\begin{split}\forall&i,j\in\\{1,\ldots,n\\},j>i,\\\
&\left\|\mathbf{p}_{i}-\mathbf{p}_{j}\right\|<d_{\rm COLAV}.\end{split}$ (26)

The desired values of the task are

$\boldsymbol{\sigma}_{1,d}=d_{\rm COLAV}\,\mathbf{1},$ (27)

where $\mathbf{1}$ is a vector of ones. Note that this task does not guarantee
robust collision avoidance. During the transients, the relative distance may
become smaller than $d_{\rm COLAV}$. Therefore, to ensure collision avoidance,
$d_{\rm COLAV}$ shuld be chosen as $d_{\rm min}+d_{\rm sec}$, where $d_{\rm
min}$ is the minimum safe distance between the vehicles, and $d_{\rm sec}$ is
an additional security distance.

Now, let us consider the formation keeping task. The task variable is defined
as

$\displaystyle\boldsymbol{\sigma}_{2}$
$\displaystyle=\left[\boldsymbol{\sigma}_{2,1}^{\rm
T},\ldots,\boldsymbol{\sigma}_{2,n-1}^{\rm T}\right]^{\rm T},$
$\displaystyle\boldsymbol{\sigma}_{2,i}$
$\displaystyle=\mathbf{p}_{i}-\mathbf{p}_{b},$ (28)

and its desired values are

$\boldsymbol{\sigma}_{d,2}=\begin{bmatrix}\mathbf{R}\left(\theta_{p}(\xi),\psi_{p}(\xi)\right)\,\mathbf{p}_{f,1}^{p}\\\
\vdots\\\
\mathbf{R}\left(\theta_{p}(\xi),\psi_{p}(\xi)\right)\,\mathbf{p}_{f,n-1}^{p}\end{bmatrix}.$
(29)

The desired velocities of the COLAV and formation keeping tasks are obtained
using the closed-loop inverse kinematics (CLIK) equation [6]

$\displaystyle\mathbf{v}_{d,i}$
$\displaystyle=\mathbf{J}_{i}^{\dagger}\,\left(\dot{\boldsymbol{\sigma}}_{d,i}-\boldsymbol{\Lambda}_{i}\,\tilde{\boldsymbol{\sigma}}_{i}\right),$
$\displaystyle i$ $\displaystyle\in\\{1,2\\},$ (30)

where
$\tilde{\boldsymbol{\sigma}}_{i}=\boldsymbol{\sigma}_{i}-\boldsymbol{\sigma}_{d,i}$
is the error, $\mathbf{J}^{\dagger}$ is the Moore-Penrose pseudoinverse,
$\mathbf{\Lambda}_{i}$ is a positive definite gain matrix, and
$\mathbf{J}_{i}$ is the task Jacobian given by

$\displaystyle\mathbf{J}_{i}$
$\displaystyle=\frac{\partial\boldsymbol{\sigma}_{i}}{\partial\mathbf{p}},$
$\displaystyle\mathbf{p}$ $\displaystyle=\left[\mathbf{p}_{1}^{\rm
T},\ldots,\mathbf{p}_{n}^{\rm T}\right]^{\rm T}.$ (31)

The desired velocity of the path-following task is obtained using LOS guidance
that is explained in the next section. These velocities are then combined
using the NSB algorithm

$\mathbf{v}_{\rm
NSB}=\mathbf{v}_{d,1}+\left(\mathbf{I}-\mathbf{J}_{1}^{\dagger}\mathbf{J}_{1}\right)\left(\mathbf{v}_{d,2}+\left(\mathbf{I}-\mathbf{J}_{2}^{\dagger}\mathbf{J}_{2}\right)\mathbf{v}_{d,3}\right),$

(32)

if there are active COLAV tasks, and

$\mathbf{v}_{\rm
NSB}=\mathbf{v}_{d,2}+\left(\mathbf{I}-\mathbf{J}_{2}^{\dagger}\mathbf{J}_{2}\right)\mathbf{v}_{d,3},$
(33)

if there are none ($\mathbf{I}$ is an identity matrix). The NSB velocities
must be decomposed into surge, pitch, and yaw references that can be tracked
by the attitude control system presented in Section IV-A. Similarly to [6], we
propose a method with angle of attack and sideslip compensation

(34) (35) (36)

where $v_{i}$ and $w_{i}$ are the sway and heave velocities, and $\gamma_{i}$
and $\chi_{i}$ are the flight-path and course angles of the $i^{\rm th}$
vehicle, respectively, and $U_{{\rm NSB},i}$, $\gamma_{{\rm NSB},i}$ and
$\chi_{{\rm NSB},i}$ are given by

$\displaystyle U_{{\rm NSB},i}$ $\displaystyle=\left\|\mathbf{v}_{{\rm
NSB},i}\right\|,\quad\mathbf{v}_{{\rm NSB},i}=\begin{bmatrix}\dot{x}_{{\rm
NSB},i}\\\ \dot{y}_{{\rm NSB},i}\\\ \dot{z}_{{\rm NSB},i}\end{bmatrix},$ (37a)
$\displaystyle\gamma_{{\rm NSB},i}$
$\displaystyle=-\arcsin\left(\frac{\dot{y}_{{\rm NSB},i}}{U_{{\rm
NSB},i}}\right),$ (37b) $\displaystyle\chi_{{\rm NSB},i}$
$\displaystyle=\mathrm{arctan}_{2}\left(\dot{y}_{{\rm NSB},i},\dot{x}_{{\rm
NSB},i}\right),$ (37c)

where ${\rm arctan}_{2}(y,x)$ is the four-quadrant inverse tan.

###  IV-C Line-of-Sight Guidance

The desired flight-path angle and course of the path-following task are given
by the following LOS law

(38) (39)

where $\mathbf{p}_{b}^{p}=\left[x_{b}^{p},y_{b}^{p},z_{b}^{p}\right]^{\rm T}$,
and $\Delta\left(\mathbf{p}_{b}^{p}\right)$ is the lookahead distance.
Inspired by [15], we choose the lookahead distance as

$\Delta\left(\mathbf{p}_{b}^{p}\right)=\sqrt{\Delta_{0}^{2}+\left(x_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}},$
(40)

where $\Delta_{0}>0$ is a constant.

The desired velocity of the path-following task is then given by

$\mathbf{v}_{d,3}=\mathbf{1}_{n}\otimes\mathbf{v}_{\rm LOS},$ (41)

where $\cdot\otimes\cdot$ is the Kronecker tensor product, and

$\mathbf{v}_{\rm
LOS}=\begin{bmatrix}\cos\left(\chi_{\mathrm{LOS}}\right)\,\cos\left(\gamma_{\mathrm{LOS}}\right)\\\
\cos\left(\gamma_{\mathrm{LOS}}\right)\,\sin\left(\chi_{\mathrm{LOS}}\right)\\\
-\sin\left(\gamma_{\mathrm{LOS}}\right)\end{bmatrix}\,U_{\rm LOS},$ (42)

where $U_{\rm LOS}$ is the desired path-following speed.

###  IV-D Path Parametrization

Inspired by [15], we use the update law of the path variable $\xi$ to get
desirable behavior of the along-track error ($x_{b}^{p}$).

Note that the kinematics of the $i^{\rm th}$ vehicle can be alternatively
expressed using the total speed ($U_{i}$) and the flight-path ($\gamma_{i}$)
and course ($\chi_{i}$) angles of the vehicle as

$\dot{\mathbf{p}}_{i}=\left[\cos\left(\chi_{i}\right)\,\cos\left(\gamma_{i}\right),\,\cos\left(\gamma_{i}\right)\,\sin\left(\chi_{i}\right),\,-\sin\left(\gamma_{i}\right)\right]^{\rm
T}\,U_{i}.$

(43)

Now, let us investigate the kinematics of the barycenter. Differentiating (13)
with respect to time and substituting (43) yields the following equations

$\displaystyle\begin{split}\dot{x}_{b}^{p}&=\frac{1}{n}\sum_{i=1}^{n}U_{i}\,\Omega_{x}\left(\gamma_{i},\theta_{p},\chi_{i},\psi_{p}\right)\\\
&\quad-\scalebox{1.0}{$\left\|\frac{\partial\mathbf{p}_{p}(\xi)}{\partial\xi}\right\|$}\dot{\xi}+\omega_{z}y_{b}^{p}-\omega_{y}z_{b}^{p},\end{split}$
(44a) $\displaystyle\dot{y}_{b}^{p}$
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}U_{i}\,\Omega_{y}\left(\gamma_{i},\theta_{p},\chi_{i},\psi_{p}\right)+\omega_{x}z_{b}^{p}-\omega_{z}x_{b}^{p},$
(44b) $\displaystyle\dot{z}_{b}^{p}$
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}U_{i}\,\Omega_{z}\left(\gamma_{i},\theta_{p},\chi_{i},\psi_{p}\right)+\omega_{y}x_{b}^{p}-\omega_{x}y_{b}^{p},$
(44c)

where

$\Omega_{x}(\cdot)$

$\displaystyle\mathrlap{\scalebox{0.9}{$=\sin\left(\theta_{p}\right)\sin\left(\gamma_{i}\right)+\cos\left(\theta_{p}\right)\cos\left(\gamma_{i}\right)\cos\left(\psi_{p}-\chi_{i}\right),$}}$
(45a)

$\Omega_{y}(\cdot)$

$\displaystyle\mathrlap{\scalebox{0.9}{$=-\cos\left(\gamma_{i}\right)\sin\left(\psi_{p}-\chi_{i}\right),$}}$
(45b)

$\Omega_{z}(\cdot)$

$\displaystyle\mathrlap{\scalebox{0.9}{$=-\cos\left(\theta_{p}\right)\sin\left(\gamma_{i}\right)+\cos\left(\gamma_{i}\right)\sin(\theta_{p})\cos\left(\psi_{p}-\chi_{i}\right)$}}$
(45c) (45d) (45e)

To stabilize the along-track error dynamics, we choose the following path
variable update

$\begin{split}\dot{\xi}&=\left\|\frac{\partial\mathbf{p}_{p}(\xi)}{\partial\xi}\right\|^{-1}\left(\frac{1}{n}\sum_{i=1}^{n}U_{i}\,\Omega_{x}\left(\gamma_{i},\theta_{p},\chi_{i},\psi_{p}\right)\right.\\\
&\qquad\qquad\qquad\qquad\left.+k_{\xi}\,\frac{x_{b}^{p}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}\right),\end{split}$
(46)

where $k_{\xi}>0$ is a constant.

##  V Closed-Loop Analysis

In this section, we investigate the closed-loop stability of the path
following task. We define two error states, $\tilde{\mathbf{X}}_{1}$ and
$\tilde{\mathbf{X}}_{2}$, as

$\displaystyle\tilde{\mathbf{X}}_{1}$
$\displaystyle=\left[x_{b}^{p},y_{b}^{p},z_{b}^{p}\right]^{\rm
T},\tilde{\mathbf{X}}_{2}=\left[\tilde{\mathbf{X}}_{2,1}^{\rm
T},\ldots,\tilde{\mathbf{X}}_{2,n}^{\rm T}\right]^{\rm T},$ (47)
$\displaystyle\tilde{\mathbf{X}}_{2,i}$
$\displaystyle=\left[\tilde{u}_{i},s_{q,i},\tilde{\theta}_{i},s_{r,i},\tilde{\psi}_{i}\right]^{\rm
T},$ (48)

Now, we can take the barycenter kinematics from (44) and express it in terms
of $\tilde{\mathbf{X}}_{1}$ and $\tilde{\mathbf{X}}_{2}$ as

$\displaystyle\dot{x}_{b}^{p}=-k_{\xi}\frac{x_{b}^{p}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}+\omega_{z}y_{b}^{p}-\omega_{y}z_{b}^{p},$
(49a) $\displaystyle\begin{aligned}
\dot{y}_{b}^{p}&=-\frac{1}{n}\sum_{i=1}^{n}U_{d,i}{\frac{\cos\left(\gamma_{\rm
LOS}\right)y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}}+\omega_{x}z_{b}^{p}-\mathrlap{\omega_{z}x_{b}^{p}}\\\
&+G_{y}\big{(}\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\psi}_{1},\ldots,\tilde{\psi}_{n},\gamma_{1},\ldots,\gamma_{n},\\\
&\quad\quad\mathrlap{u_{d,1},\ldots,u_{d,n},v_{1},\ldots,v_{n},w_{1},\ldots,w_{n},\mathbf{p}_{b}^{p},\psi_{p}\big{)},}\end{aligned}$
(49b) $\displaystyle\begin{aligned}
\dot{z}_{b}^{p}&=\frac{1}{n}\sum_{i=1}^{n}U_{d,i}\frac{z_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}+\omega_{y}x_{b}^{p}-\omega_{x}y_{b}^{p}\\\
&\,\mathrlap{+G_{z}\big{(}\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\theta}_{1},\ldots,\tilde{\theta}_{n},\gamma_{1},\ldots,\gamma_{n},\chi_{1},\ldots,\chi_{n},}\\\
&\mathrlap{\quad\,u_{d,1},\ldots,u_{d,n},v_{1},\ldots,v_{n},w_{1},\ldots,w_{n},\mathbf{p}_{b}^{p},\psi_{p},\theta_{p}\big{)}.}\end{aligned}$
(49c)

The equations for $G_{y}(\cdot)$ and $G_{z}(\cdot)$ are given in Appendix B-A.
Substituting the attitude control system (18)–(25) into vehicle dynamics (11)
yields the following closed-loop behavior of $\tilde{\mathbf{X}}_{2}$

$\dot{\tilde{u}}_{i}$

$\displaystyle=\scalebox{0.97}{$-k_{u}\,\tilde{u}_{i}-k_{c}\,{\rm
sign}\left(\tilde{u}_{i}\right)-\boldsymbol{\phi}_{u}(\cdot)^{\rm
T}\tilde{\mathbf{V}}_{c,i},$}$ (50a)

$\dot{s}_{q,i}$

$\displaystyle=\scalebox{0.97}{$-k_{\theta}\,\tilde{\theta}_{i}-k_{q}\,s_{q,i}-k_{d}\,{\rm
sign}(s_{q,i})-\boldsymbol{\phi}_{q}(\cdot)^{\rm
T}\,\tilde{\boldsymbol{\vartheta}}_{q,i},$}$ (50b)

$\dot{\tilde{\theta}}_{i}$

$\displaystyle=\scalebox{0.97}{$s_{q,i}-\lambda_{q}\,\tilde{\theta}_{i},$}$
(50c)

$\dot{s}_{r,i}$

$\displaystyle=\scalebox{0.97}{$-k_{\theta}\,\tilde{\theta}_{i}-k_{r}\,s_{r,i}-k_{d}\,{\rm
sign}(s_{r,i})-\boldsymbol{\phi}_{r}(\cdot)^{\rm
T}\,\tilde{\boldsymbol{\vartheta}}_{r,i},$}$ (50d)

$\dot{\tilde{\psi}}_{i}$

$\displaystyle=\scalebox{0.97}{$s_{r,i}-\lambda_{r}\,\tilde{\psi}_{i},$}$
(50e)

the ocean current estimate errors

$\displaystyle\dot{\tilde{\mathbf{V}}}_{c,i}$
$\displaystyle=c_{u}\,\boldsymbol{\phi}_{u}(\cdot)\,\tilde{u}_{i},$ (51a)
$\displaystyle\dot{\tilde{\boldsymbol{\vartheta}}}_{q,i}$
$\displaystyle=c_{q}\,\boldsymbol{\phi}_{q}(\cdot)\,s_{q,i},$ (51b)
$\displaystyle\dot{\tilde{\boldsymbol{\vartheta}}}_{r,i}$
$\displaystyle=c_{r}\,\boldsymbol{\phi}_{r}(\cdot)\,s_{r,i},$ (51c)

and the underactuated sway and heave dynamics

$\displaystyle\dot{v}_{i}$
$\displaystyle=X_{v}(u_{i},u_{c})\,r_{i}+Y_{v}(u_{i},u_{c})\,(v_{i}-v_{c}),$
(52) $\displaystyle\dot{w}_{i}$
$\displaystyle=X_{w}(u_{i},u_{c})\,q_{i}+Y_{w}(u_{i},u_{c})\,(w_{i}-w_{c})+G(\theta_{i}).$
(53)

To prove the stability of the closed-loop system, we need the results of the
three following lemmas. The lemmas follow the same structure as the 2D case
for two ASVs in [9], and are extended to handle an arbitrary number of AUVs
moving in 3D.

###### Lemma 1

The trajectories of the closed-loop system (49)–(53) are forward complete.

###### Proof:

The proof is given in Appendix B-C. ∎

###### Lemma 2

The underactuated sway and heave dynamics are bounded near the manifold
$\left[\tilde{\mathbf{X}}_{1}^{\rm T},\tilde{\mathbf{X}}_{2}^{\rm
T}\right]=\mathbf{0}^{\rm T}$ if $Y_{v}(u,u_{c})<0$, $Y_{w}(u,u_{c})<0$ and
the curvature of the path satisfies

$\displaystyle\left|\kappa(\xi)\right|$
$\displaystyle<\frac{n}{2}\left|\frac{Y_{w}(u,u_{c})}{X_{w}(u,u_{c})}\right|,$
$\displaystyle\left|\iota(\xi)\right|$
$\displaystyle<\frac{n}{2}\left|\frac{Y_{v}(u,u_{c})}{X_{v}(u,u_{c})}\right|,$
(54)

for all $u>0$ and
$u_{c}\in[-\left\|\mathbf{V}_{c}\right\|,\left\|\mathbf{V}_{c}\right\|]$.

###### Proof:

The proof is given in Appendix B-D. ∎

###### Lemma 3

The underactuated sway and heave dynamics are bounded near the manifold
$\tilde{\mathbf{X}}_{2}=\mathbf{0}$, independently of $\tilde{\mathbf{X}}_{1}$
if the assumptions in Lemma 2 are satisfied and the constant term $\Delta_{0}$
in the lookahead distance (40) is chosen so that

$\begin{split}\Delta_{0}>\max&\left\\{\frac{3}{n\left|\frac{Y_{v}\left(u,u_{c}\right)}{X_{v}\left(u,u_{c}\right)}\right|-2\left|\iota(\xi)\right|},\right.\\\
&\quad\left.\frac{3}{n\left|\frac{Y_{w}\left(u,u_{c}\right)}{X_{w}\left(u,u_{c}\right)}\right|-2\left|\kappa(\xi)\right|}\right\\},\end{split}$
(55)

for all $u>0$ and
$u_{c}\in[-\left\|\mathbf{V}_{c}\right\|,\left\|\mathbf{V}_{c}\right\|]$.

###### Proof:

The proof is given in Appendix B-E. ∎

###### Theorem 1

The origin $\left[\tilde{\mathbf{X}}_{1}^{\rm T},\tilde{\mathbf{X}}_{2}^{\rm
T}\right]=\mathbf{0}^{\rm T}$ of the system described by (49),(50) is a USGES
equilibrium point if the conditions of Lemmas 2 and 3 hold and the maximum
pitch angle of the path satisfies

$\theta_{p,{\rm
max}}=\max_{\xi\in\mathbb{R}}\left|\theta_{p}(\xi)\right|<\frac{\pi}{4}.$ (56)

Moreover, the ocean current estimate errors (51) and the underactuated sway
and heave dynamics (52), (53) are bounded.

_Remark:_ Condition (56) is needed to ensure that $\left|\gamma_{\rm
LOS}\right|<\pi/2$. Indeed, from (38), the largest possible LOS reference
angle is

$\begin{split}\gamma_{\rm LOS,max}&=\theta_{p,{\rm
max}}+\lim_{z_{b}^{p}\rightarrow\infty}\arctan\left(\scalebox{1.0}{$\frac{z_{b}^{p}}{\sqrt{\Delta_{0}^{2}+\left(z_{b}^{p}\right)^{2}}}$}\right)\\\
&=\theta_{p,{\rm max}}+\frac{\pi}{4}.\end{split}$ (57)

With (56) satisfied, the cosine of $\gamma_{\rm LOS}$ is always positive. We
will use this fact in the proof.

###### Proof:

The proof follows along the lines of [9], but is extended to an arbitrary
number of 5DOF vehicles. We will also use the results of [10] to prove that
the system is USGES.

In Lemmas 1–3, we have shown that the closed-loop system is forward complete
and the underactuated sway and heave dynamics are bounded near the manifold
$\tilde{\mathbf{X}}_{2}=\mathbf{0}$. Since (50) is UGES [11], we can conclude
that there exists a finite time $T>t_{0}$ such that the solutions of (50) will
be sufficiently close to $\tilde{\mathbf{X}}_{2}=\mathbf{0}$ to guarantee
boundedness of $v_{i}$ and $w_{i}$. Having established that the underactuated
dynamics are bounded, we will now utilize cascaded theory to analyze the
cascade (49), (50), where (50) perturbs the nominal dynamics (49) through the
terms $G_{y}(\cdot)$ and $G_{z}(\cdot)$.

Now, consider the nominal dynamics of $\tilde{\mathbf{X}}_{1}$ (_i.e.,_ (49)
without the perturbing terms $G_{y}$ and $G_{z}$), and a Lyapunov function
candidate

$V(\tilde{\mathbf{X}}_{1})=\frac{1}{2}\tilde{\mathbf{X}}_{1}^{\rm
T}\,\tilde{\mathbf{X}}_{1}=\frac{1}{2}\left((x_{b}^{p})^{2}+(y_{b}^{p})^{2}+(z_{b}^{p})^{2}\right),$
(58)

whose derivative along the trajectories of (49) is

$\displaystyle\dot{V}(\tilde{\mathbf{X}}_{1})$
$\displaystyle=-\tilde{\mathbf{X}}_{1}^{\rm
T}\,\mathbf{Q}\,\tilde{\mathbf{X}}_{1},$ $\displaystyle\mathbf{Q}$
$\displaystyle={\rm diag}(q_{1},q_{2},q_{3}),$ (59a) $\displaystyle q_{1}$
$\displaystyle=\scalebox{1.0}{$\frac{k_{\xi}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}$},$
$\displaystyle q_{2}$
$\displaystyle=\scalebox{1.0}{${\frac{\frac{1}{n}\sum_{i=1}^{n}U_{d,i}\cos\left(\gamma_{\rm
LOS}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}}$},$
(59b) $\displaystyle q_{3}$
$\displaystyle=\scalebox{1.0}{$\frac{\frac{1}{n}\sum_{i=1}^{n}U_{d,i}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}$}.$
(59c)

Note that $\mathbf{Q}$ is positive definite, and the nominal system is thus
UGAS. Furthermore, note that the following inequality

$\displaystyle\dot{V}(\tilde{\mathbf{X}}_{1})$ $\displaystyle\leq-q_{\rm
min}\left\|\tilde{\mathbf{X}}_{1}\right\|^{2},$ (60a) $\displaystyle q_{\rm
min}$
$\displaystyle=\scalebox{1.0}{$\min\left\\{\frac{k_{\xi}}{\sqrt{1+r^{2}}},\frac{\frac{1}{n}\sum_{i=1}^{n}U_{d,i}\cos\left(\gamma_{\rm
LOS}\right)}{\sqrt{\Delta_{0}^{2}+4r^{2}}}\right\\}$},$ (60b)

holds $\forall\tilde{\mathbf{X}}_{1}\in\mathcal{B}_{r}$. Thus, the conditions
of [10, Theorem 5] are fulfilled with $k_{1}=k_{2}=1/2$, $a=2$, and
$k_{3}=q_{\rm min}$, and the nominal system is USGES.

As discussed in the proof of Lemma 1, the perturbing system (50) is UGES,
implying both UGAS and USGES. Furthermore, it is straightforward to show that
the following holds for the Lyapunov function (58)

$\displaystyle\left\|\frac{\partial
V}{\partial\tilde{\mathbf{X}}_{1}}\right\|\,\left\|\tilde{\mathbf{X}}_{1}\right\|$
$\displaystyle=\left\|\tilde{\mathbf{X}}_{1}\right\|^{2}=2\,V\left(\tilde{\mathbf{X}}_{1}\right),\quad\forall\tilde{\mathbf{X}}_{1},$
(61) $\displaystyle\left\|\frac{\partial
V}{\partial\tilde{\mathbf{X}}_{1}}\right\|$
$\displaystyle=\left\|\tilde{\mathbf{X}}_{1}\right\|\leq\mu,\quad\quad\forall\left\|\tilde{\mathbf{X}}_{1}\right\|\leq\mu.$
(62)

Therefore, [10, Assumption 1] is satisfied with $c_{1}=2$ and $c_{2}=\mu$ for
any $\mu>0$.

Finally, [10, Assumption 2] must be investigated. From (82), (92), it can be
shown that for both perturbing terms there exist positive functions
$\zeta_{y,1}(\cdot)$, $\zeta_{y,2}(\cdot)$, $\zeta_{z,1}(\cdot)$,
$\zeta_{z,2}(\cdot)$, such that

$\displaystyle\left|G_{y}(\cdot)\right|$
$\displaystyle\leq\zeta_{y,1}\left(\left\|\tilde{\mathbf{X}}_{2}\right\|\right)+\zeta_{y,2}\left(\left\|\tilde{\mathbf{X}}_{2}\right\|\right)\left\|\tilde{\mathbf{X}}_{1}\right\|,$
(63) $\displaystyle\left|G_{z}(\cdot)\right|$
$\displaystyle\leq\zeta_{z,1}\left(\left\|\tilde{\mathbf{X}}_{2}\right\|\right)+\zeta_{z,2}\left(\left\|\tilde{\mathbf{X}}_{2}\right\|\right)\left\|\tilde{\mathbf{X}}_{1}\right\|.$
(64)

Therefore, all conditions of [10, Proposition 9] are satisfied, and the
closed-loop system is USGES. ∎

##  VI Simulation Results

$0$$50$$100$$150$$-5$$0$$5$$t$ [s]Error [m]Path-following
error$x_{b}^{p}$$y_{b}^{p}$$z_{b}^{p}$$0$$20$$40$$60$$80$$100$$120$$140$$0$$5$$10$$15$$20$$t$
[s]Distance [m]Distance between the vehicles$d_{1,2}$ $d_{1,3}$$d_{2,3}$
$d_{\rm COLAV}$$-4$$-2$$0$$2$$\tilde{\sigma}_{x}$ [m]Formation-keeping
error$-20$$-10$$0$$10$$20$$\tilde{\sigma}_{y}$ [m]Vehicle 1Vehicle 2Vehicle
3$0$$20$$40$$60$$80$$100$$120$$140$$-10$$0$$10$$20$$t$ [s]$\tilde{\sigma}_{z}$
[m] Figure 3: Simulation results. The top-left plot shows the $x$-, $y$\- and
$z$-components of the path-following error $\mathbf{p}_{b}^{p}$, as defined in
(13). The bottom-left plot shows the distance between the vehicles
($d_{i,j}=\|\mathbf{p}_{i}-\mathbf{p}_{j}\|$). The plots on the right show the
$x$-, $y$\- and $z$-components of the formation-keeping error
$\tilde{\boldsymbol{\sigma}}=\boldsymbol{\sigma}_{2}-\boldsymbol{\sigma}_{d,2}$
with $\boldsymbol{\sigma}_{2}$ given by (28) and $\boldsymbol{\sigma}_{d,2}$
given by (29). The grey rectangles mark the intervals when the COLAV task is
active.

In this section, we present the results of a numerical simulation of three
LAUV vehicles [16]. The parameters of the simulation are summarized in Table
I. The barycenter should follow a spiral path given by

$\mathbf{p}_{p}(\xi)=\left[\xi,a\,\cos(\omega\,\xi),b\,\sin(\omega\,\xi)\right]^{\rm
T}.$ (65)

The maximum curvature of this path is

$\displaystyle\max_{\xi\in\mathbb{R}}\left|\kappa(\xi)\right|$
$\displaystyle=\frac{b\,\omega^{2}}{\sqrt{a^{2}\,\omega^{2}+1}},$
$\displaystyle\max_{\xi\in\mathbb{R}}\left|\iota(\xi)\right|$
$\displaystyle=a\,\omega^{2},$ (66)

while the smallest absolute values of $Y_{v}/X_{v}$ and $Y_{w}/X_{w}$ for the
LAUV model are approximately $0.26$. Consequently, the path is chosen such
that the maximum curvature is

$\displaystyle\max_{\xi\in\mathbb{R}}\left|\kappa(\xi)\right|$
$\displaystyle=0.013,$
$\displaystyle\max_{\xi\in\mathbb{R}}\left|\iota(\xi)\right|$
$\displaystyle=0.040,$ (67)

and (54) is satisfied. From (55), the lookahead distance must then satisfy
$\Delta_{0}>4.29$. We choose $\Delta_{0}=5$, since smaller distances guarantee
faster convergence.

The very minimum relative distance to avoid collision is the length of the
LAUV, _i.e._ $2.4$ m. For additional safety, we design the COLAV task with
$d_{\rm min}=5$ m. To add a security zone during transients, $d_{\rm COLAV}$
is chosen to be $10$ m.

The desired formation is an isosceles triangle parallel to the $yz$ plane.
Specifically, the desired positions of the three vehicles are

$\displaystyle\mathbf{p}_{f,1}^{f}$ $\displaystyle=\begin{bmatrix}0\\\ 10\\\
5\end{bmatrix},$ $\displaystyle\mathbf{p}_{f,2}^{f}$
$\displaystyle=\begin{bmatrix}0\\\ -10\\\ 5\end{bmatrix},$
$\displaystyle\mathbf{p}_{f,3}^{f}$ $\displaystyle=\begin{bmatrix}0\\\ 0\\\
-10\end{bmatrix}.$ (68)

Parameter | Value
---|---
$k_{u}$ | $0.05$
$k_{c}$ | $0.1$
$k_{\theta},k_{\psi}$ | $0.0625$
$k_{q},k_{r}$ | $0.25$
$k_{d}$ | $0.1$
$\lambda_{q},\lambda_{r}$ | $0.75$
$c_{u}$ | $5$
$c_{q},c_{r}$ | $1$
$\boldsymbol{\Lambda}_{1}$ | $\mathbf{I}$
$\boldsymbol{\Lambda}_{2}$ | $0.05\,\mathbf{I}$

Parameter | Value
---|---
$\mathbf{V}_{c}$ | $\left[0,0.25,0.05\right]^{\rm T}$
$\Delta_{0}$ | $5$
$d_{\rm COLAV}$ | $10$
$U_{\rm LOS}$ | $1$
$k_{\xi}$ | $1$
$\mathbf{p}_{0}$ | $\mathbf{0}_{3}$
$a$ | $40$
$b$ | $20$
$\omega$ | $\pi/100$

TABLE I: Simulation parameters

The gains of the low-level control systems (18),(21),(24) are chosen such that
the settling time is approximately 10 seconds. The gains of the pitch and yaw
PD controllers are chosen such that the closed-loop system is critically
damped.

The results of the numerical simulation are shown in Figures 3 and 4. The
vehicles start in an inverted triangular formation. The COLAV task is briefly
activated, and the distance between the vehicles drops to approximately 8
meters during the transient. Eventually, the vehicles resolve the situation
and continue to converge to the desired path and formation.

Note that while the COLAV task is active, the formation-keeping error is
diverging. After resolving the situation, the formation-keeping error
converges to zero exponentially. The rate of convergence is given by the
formation-keeping gain $\boldsymbol{\Lambda}_{2}$.

The path-following error seems to converge linearly at first, and then
exponentially as the error gets smaller. This phenomenon is caused by the LOS
guidance law (38), _cf._ [17], and the path parameter update law (46). The
inverse tan in (38) and the last term in (46) act as a saturation, slowing the
convergence for large errors. The rate of convergence of the along-track error
($x_{b}^{p}$) is given by the path parameter update gain $k_{\xi}$, while the
rate of convergence of the cross-track errors ($y_{b}^{p},z_{b}^{p}$) is given
by the lookahead distance $\Delta_{0}$.

Trajectory
---
0
---
20
---
40
---
60
---
80
---
100
---
0
---
$-50$
---
$-100$
---
0
---
$-10$
---
10
---
20
---
$x$ [m]
---
$y$ [m]
---

$z$ [m]
---

Vehicle 1
---
Vehicle 2
---
Vehicle 3
---
Path
---
Figure 4: 3D trajectory of the vehicles. The markers represent the position of
the vehicles at times $t=0,25,50,\ldots,150$ seconds. Markers with
corresponding times are connected by dotted lines to better illustrate the
resulting formation.

##  VII Conclusions and future work

In this paper, we proposed a formation path-following method for an arbitrary
number of AUVs, proved the stability of the path following part, and verified
its effectiveness in simulations.

Because the proposed algorithm is centralized, our method can only be used in
scenarios where all the vehicles can communicate with each other. A
decentralized version of the algorithm is a topic for future work.

In the simulations, the formation-keeping error shows exponential convergence
to zero. However, the stability of the formation-keeping task has not been
theoretically proven. Proving the stability of this task is another potential
topic for future work.

## ACKNOWLEDGMENTS

The authors would like to thank Damiano Varagnolo for the interesting
discussions and inputs.

This work was partly supported by the Research Council of Norway through
project No. 302435 and the Centres of Excellence funding scheme, project No.
223254.

## References

  * [1] 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.
  * [2] E. Borhaug and K. Y. Pettersen, “Formation control of 6-DOF Euler-Lagrange systems with restricted inter-vehicle communication,” in _Proc. 45th IEEE Conf. Decision and Control_ , 2006, pp. 5718–5723.
  * [3] R. Ghabcheloo, A. P. Aguiar, A. Pascoal, C. Silvestre, I. Kaminer, and J. Hespanha, “Coordinated path-following control of multiple underactuated autonomous vehicles in the presence of communication failures,” in _Proc. 45th IEEE Conference on Decision and Control_ , 2006, pp. 4345–4350.
  * [4] R. Cui, S. Sam Ge, B. Voon Ee How, and Y. Sang Choo, “Leader–follower formation control of underactuated autonomous underwater vehicles,” _Ocean Engineering_ , vol. 37, no. 17, pp. 1491–1502, 2010.
  * [5] M. Soorki, H. Talebi, and S. Nikravesh, “A robust dynamic leader-follower formation control with active obstacle avoidance,” in _Proc. 2011 IEEE International Conference on Systems, Man, and Cybernetics_ , 2011, pp. 1932–1937.
  * [6] F. Arrichiello, S. Chiaverini, and T. I. Fossen, “Formation control of underactuated surface vessels using the null-space-based behavioral control,” in _Proc. 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2006, pp. 5942–5947.
  * [7] G. Antonelli, F. Arrichiello, and S. Chiaverini, “Experiments of formation control with multirobot systems using the null-space-based behavioral control,” _IEEE Transactions on Control Systems Technology_ , vol. 17, no. 5, pp. 1173–1182, 2009.
  * [8] S.-K. Pang, Y.-H. Li, and H. Yi, “Joint formation control with obstacle avoidance of towfish and multiple autonomous underwater vehicles based on graph theory and the null-space-based method,” _Sensors_ , vol. 19, no. 11, 2019.
  * [9] Å. Eek, K. Y. Pettersen, E.-L. M. Ruud, and T. R. Krogstad, “Formation Path Following Control of Underactuated USVs,” _European Journal of Control_ , Jun. 2021.
  * [10] K. Y. Pettersen, “Lyapunov sufficient conditions for uniform semiglobal exponential stability,” _Automatica_ , vol. 78, pp. 97–102, Apr. 2017.
  * [11] S. Moe, K. Y. Pettersen, T. I. Fossen, and J. T. Gravdahl, “Line-of-sight curved path following for underactuated USVs and AUVs in the horizontal plane under the influence of ocean currents,” in _Proc. 24th Mediterranean Conf. Control and Automation_ , 2016, pp. 38–45.
  * [12] T. I. Fossen, _Handbook of Marine Craft Hydrodynamics and Motion Control_. John Wiley & Sons, May 2011.
  * [13] E. Borhaug, A. Pavlov, and K. Y. Pettersen, “Straight line path following for formations of underactuated underwater vehicles,” in _Proc. 46th IEEE Conf. Decision and Control_ , 2007, pp. 2905–2912.
  * [14] S. Moe and K. Y. Pettersen, “Set-based line-of-sight (LOS) path following with collision avoidance for underactuated unmanned surface vessels under the influence of ocean currents,” in _Proc. 2017 IEEE Conf. Control Technology and Applications_ , 2017, pp. 241–248.
  * [15] D. Belleter, M. A. Maghenem, C. Paliotta, and K. Y. Pettersen, “Observer based path following for underactuated marine vessels in the presence of ocean currents: A global approach,” _Automatica_ , vol. 100, pp. 123–134, 2019\.
  * [16] A. Sousa, L. Madureira, J. Coelho, J. Pinto, J. Pereira, J. Borges Sousa, and P. Dias, “LAUV: The man-portable autonomous underwater vehicle,” _IFAC Proceedings Volumes_ , vol. 45, no. 5, pp. 268–274, 2012.
  * [17] T. I. Fossen and K. Y. Pettersen, “On uniform semiglobal exponential stability (usges) of proportional line-of-sight guidance laws,” _Automatica_ , vol. 50, no. 11, pp. 2912–2917, 2014.
  * [18] D. Angeli and E. D. Sontag, “Forward completeness, unboundedness observability, and their Lyapunov characterizations,” _Systems & Control Letters_, vol. 38, no. 4-5, pp. 209–217, 1999.

##  Appendix A Components of the dynamical equations

$\displaystyle F_{u}(\cdot)$
$\displaystyle=-\frac{d_{11}\,u+q\,\left(m_{34}\,q+m_{33}\,w\right)-r\,\left(m_{25}\,r+m_{22}\,v\right)}{m_{11}},$
(69a) $\displaystyle\boldsymbol{\phi}_{u}(\cdot)$
$\displaystyle=q\left(\frac{m_{33}}{m_{11}}-1\right)\,\mathbf{r}_{w}+r\left(1-\frac{m_{22}}{m_{11}}\right)\,\mathbf{r}_{v}+\frac{d_{11}}{m_{11}}\,\mathbf{r}_{u},$
(69b) $\displaystyle X_{v}(\cdot)$
$\displaystyle=-u_{c}-\frac{m_{55}\,\left(d_{25}+m_{11}\,u_{r}\right)-m_{25}\,\left(d_{55}+m_{25}\,u_{r}\right)}{m_{22}\,m_{55}-{m_{25}}^{2}},$
(69c) $\displaystyle Y_{v}(\cdot)$
$\displaystyle=-\frac{d_{22}\,m_{55}-m_{25}\,\left(d_{52}-u_{r}\,\left(m_{11}-m_{22}\right)\right)}{m_{22}\,m_{55}-{m_{25}}^{2}},$
(69d) $\displaystyle X_{w}(\cdot)$
$\displaystyle=u_{c}-\frac{m_{44}\,\left(d_{34}-m_{11}\,u_{r}\right)-m_{34}\,\left(d_{44}-m_{34}\,u_{r}\right)}{m_{33}\,m_{44}-{m_{34}}^{2}},$
(69e) $\displaystyle Y_{w}(\cdot)$
$\displaystyle=-\frac{d_{33}\,m_{44}-m_{34}\,\left(d_{43}+u_{r}\,\left(m_{11}-m_{33}\right)\right)}{m_{33}\,m_{44}-{m_{34}}^{2}},$
(69f) $\displaystyle G(\cdot)$
$\displaystyle=\frac{m_{34}\,mgz_{g}\,\sin\left(\theta\right)}{m_{33}\,m_{44}-{m_{34}}^{2}},$
(69g) $\displaystyle F_{q}(\cdot)$
$\displaystyle=\frac{m_{34}\,\left(d_{34}\,q+d_{33}\,w-q\,u\,\left(m_{11}-m_{33}\right)\right)-m_{33}\,\left(d_{44}\,q+d_{43}\,w+mgz_{g}\,\sin\left(\theta\right)+u\,w\,\left(m_{11}-m_{33}\right)\right)}{m_{33}\,m_{44}-{m_{34}}^{2}},$
(69h) $\displaystyle\boldsymbol{\phi}_{q}(\cdot)$
$\displaystyle=\frac{m_{34}\,\left(d_{33}\,\boldsymbol{\varphi}_{w}-\boldsymbol{\varphi}_{u}\,q\,\left(m_{11}-m_{33}\right)\right)-m_{33}\,\left(d_{43}\,\boldsymbol{\varphi}_{w}+\boldsymbol{\varphi}_{{uw}}\,\left(m_{11}-m_{33}\right)\right)}{m_{33}\,m_{44}-{m_{34}}^{2}},$
(69i) $\displaystyle F_{r}(\cdot)$
$\displaystyle=\frac{m_{25}\,\left(d_{25}\,r+d_{22}\,v+r\,u\,\left(m_{11}-m_{22}\right)\right)-m_{22}\,\left(d_{55}\,r+d_{52}\,v-u\,v\,\left(m_{11}-m_{22}\right)\right)}{m_{22}\,m_{55}-{m_{25}}^{2}},$
(69j) $\displaystyle\boldsymbol{\phi}_{r}(\cdot)$
$\displaystyle=\frac{m_{25}\,\left(d_{22}\,\boldsymbol{\varphi}_{v}+\boldsymbol{\varphi}_{u}\,r\,\left(m_{11}-m_{22}\right)\right)-m_{22}\,\left(d_{52}\,\boldsymbol{\varphi}_{v}-\boldsymbol{\varphi}_{uv}\,\left(m_{11}-m_{22}\right)\right)}{m_{22}\,m_{55}-{m_{25}}^{2}},$
(69k)

where
$\left[\mathbf{r}_{u},\mathbf{r}_{v},\mathbf{r}_{w}\right]=\mathbf{R}\left(\theta,\psi\right)$,
and

$\boldsymbol{\varphi}_{ij}(\cdot)=\left[-j\,\mathbf{r}_{i}^{\rm
T}-i\,\mathbf{r}_{j}^{\rm
T},r_{{i1}}\,r_{{j1}},r_{{i2}}\,r_{{j2}},r_{{i3}}\,r_{{j3}},r_{{i1}}\,r_{{j2}}+r_{{i2}}\,r_{{j1}},r_{{i1}}\,r_{{j3}}+r_{{i3}}\,r_{{j1}},r_{{i2}}\,r_{{j3}}+r_{{i3}}\,r_{{j2}}\right]^{\rm
T},\quad i,j\in\\{u,v,w\\}.$ (70)

##  Appendix B Derivations and Lemmas from Section V

###  B-A Derivation of Closed-Loop Barycenter Kinematics

We begin by taking $\dot{y}_{b}^{p}$ from (44b).

$\displaystyle\dot{y}_{b}^{p}$
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}U_{i}\,\cos\left(\gamma_{i}\right)\,\sin\left(\chi_{i}-\psi_{p}\right)-\dot{\xi}\,\iota\,x_{b}^{p}.$
(71)

Now, consider the term $\sin\left(\chi_{i}-\psi_{p}\right)$. The course of the
vessel is given by

$\displaystyle\chi_{i}$ $\displaystyle=\psi_{i}+\beta_{i},$
$\displaystyle\beta_{i}$
$\displaystyle=\arcsin\left(\frac{v_{i}}{U_{i}}\right).$ (72)

After substituting and applying some trigonometric identities, we get

$\displaystyle\sin\left(\chi_{i}-\psi_{p}\right)$
$\displaystyle=\sin\left(\psi_{i}+\beta_{i}-\psi_{p}\right)=\cos\left(\psi_{i}-\psi_{p}\right)\,\sin\left(\beta_{i}\right)+\sin\left(\psi_{i}-\psi_{p}\right)\,\cos\left(\beta_{i}\right)$
(73a)
$\displaystyle=\cos\left(\psi_{i}-\psi_{p}\right)\frac{v_{i}}{U_{i}}+\sin\left(\psi_{i}-\psi_{p}\right)\frac{\sqrt{u_{i}^{2}+w_{i}^{2}}}{U_{i}}.$
(73b)

Consequently, the term
$U_{i}\,\cos\left(\gamma_{i}\right)\,\sin\left(\chi_{i}-\psi_{p}\right)$ is
equivalent to

$U_{i}\,\cos\left(\gamma_{i}\right)\,\sin\left(\chi_{i}-\psi_{p}\right)=\cos\left(\gamma_{i}\right)\left(\cos\left(\psi_{i}-\psi_{p}\right)v_{i}+\sin\left(\psi_{i}-\psi_{p}\right)\sqrt{u_{i}^{2}+w_{i}^{2}}\right).$
(74)

Now, consider a term $\sin\left(\psi_{i}+\beta_{d,i}-\psi_{p}\right)$. Using a
similar procedure, we get

$\sin\left(\psi_{i}+\beta_{d,i}-\psi_{p}\right)=\cos\left(\psi_{i}-\psi_{p}\right)\frac{v_{i}}{U_{d,i}}+\sin\left(\psi_{i}-\psi_{p}\right)\frac{\sqrt{u_{d,i}^{2}+w_{i}^{2}}}{U_{d,i}}.$
(75)

Combining (74) and (75), we get

$U_{i}\,\cos\left(\gamma_{i}\right)\,\sin\left(\chi_{i}-\psi_{p}\right)=U_{d,i}\,\cos\left(\gamma_{i}\right)\,\sin\left(\psi_{i}+\beta_{d,i}-\psi_{p}\right)+\cos\left(\gamma_{i}\right)\,\sin\left(\psi_{i}-\psi_{p}\right)\left(\sqrt{u_{i}^{2}+w_{i}^{2}}-\sqrt{u_{d,i}^{2}+w_{i}^{2}}\right).$
(76)

Note that the following holds for the angles

$\displaystyle\psi_{i}+\beta_{d,i}-\psi_{p}$
$\displaystyle=\psi_{d,i}+\tilde{\psi}_{i}+\beta_{d,i}-\left(\psi_{d,i}+\beta_{d,i}+\beta_{\rm
LOS}\right)=\tilde{\psi}_{i}-\beta_{\rm LOS},$ $\displaystyle\beta_{\rm LOS}$
$\displaystyle=\scalebox{1.0}{$\arctan\left(\frac{y_{b}^{p}}{\Delta\left(\mathbf{p}_{b}^{p}\right)}\right)$}.$
(77)

Therefore, their sine is given by

$\sin\left(\psi_{i}+\beta_{d,i}-\psi_{p}\right)=\sin\left(\tilde{\psi}_{i}\right)\,\scalebox{1.0}{$\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}-\cos\left(\tilde{\psi}_{i}\right)\scalebox{1.0}{$\frac{y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}.$
(78)

Furthermore, note that the following holds for the flight-path angle

$\gamma_{i}=\theta_{i}-\alpha_{i}=\tilde{\theta}_{i}+\theta_{d,i}-\alpha_{i}=\tilde{\theta}_{i}+\gamma_{\rm
LOS}+\alpha_{d,i}-\alpha_{i}.$ (79)

Consequently, the cosine of the flight-path angle is equal to

$\begin{split}\cos\left(\gamma_{i}\right)&=\cos\left(\gamma_{\rm
LOS}\right)\cos\left(\tilde{\theta}_{i}\right)\cos\left(\alpha_{d,i}-\alpha_{i}\right)-\cos\left(\gamma_{\rm
LOS}\right)\sin\left(\tilde{\theta}_{i}\right)\sin\left(\alpha_{d,i}-\alpha_{i}\right)\\\
&\quad-\sin\left(\gamma_{\rm
LOS}\right)\cos\left(\tilde{\theta}_{i}\right)\sin\left(\alpha_{d,i}-\alpha_{i}\right)-\sin\left(\gamma_{\rm
LOS}\right)\sin\left(\tilde{\theta}_{i}\right)\cos\left(\alpha_{d,i}-\alpha_{i}\right)\end{split}$
(80)

Using the equalities (78), (80), we can rewrite (76) as

$U_{i}\,\cos\left(\gamma_{i}\right)\,\sin\left(\chi_{i}-\psi_{p}\right)=-U_{d,i}\cos\left(\gamma_{\rm
LOS}\right)\scalebox{1.0}{$\frac{y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}+G_{y,i}\left(\tilde{u}_{i},\tilde{\psi}_{i},\gamma_{i},u_{d,i},v_{i},w_{i},\mathbf{p}_{b}^{p},\psi_{p}\right),$
(81)

where

$\begin{split}G_{y,i}(\cdot)&=\cos\left(\gamma_{i}\right)\,\sin\left(\psi_{i}-\psi_{p}\right)\left(\sqrt{u_{i}^{2}+w_{i}^{2}}-\sqrt{u_{d,i}^{2}+w_{i}^{2}}\right)-U_{d,i}\cos\left(\gamma_{i}\right)\,\sin\left(\tilde{\psi}_{i}\right)\scalebox{1.0}{$\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}\\\
&\quad+U_{d,i}\bigg{[}\sin\left(\gamma_{\rm
LOS}\right)\left(\cos\left(\tilde{\theta}_{i}\right)\sin\left(\alpha_{d,i}-\alpha_{i}\right)+\sin\left(\tilde{\theta}_{i}\right)\cos\left(\alpha_{d,i}-\alpha_{i}\right)\right)\\\
&\qquad\qquad-\cos\left(\gamma_{\rm
LOS}\right)\left(\cos\left(\tilde{\theta}_{i}\right)\cos\left(\alpha_{d,i}-\alpha_{i}\right)-1\right)\bigg{]}\scalebox{1.0}{$\frac{y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}\end{split}$
(82)

Substituting (81) into (71), we get the following

$\begin{split}\dot{y}_{b}^{p}&=-\frac{1}{n}\sum_{i=1}^{n}U_{d,i}\cos\left(\gamma_{\rm
LOS}\right)\scalebox{1.0}{$\frac{y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}-\dot{\xi}\,\iota\,x_{b}^{p}\\\
&\quad+{G_{y}\left(\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\psi}_{1},\ldots,\tilde{\psi}_{n},\gamma_{1},\ldots,\gamma_{n},u_{d,1},\ldots,u_{d,n},v_{1},\ldots,v_{n},w_{1},\ldots,w_{n},\mathbf{p}_{b}^{p},\psi_{p}\right)},\end{split}$
(83)

where

$G_{y}(\cdot)=\frac{1}{n}\sum_{i=1}^{n}G_{y,i}\left(\tilde{u}_{i},\tilde{\psi}_{i},\gamma_{i},u_{d,i},v_{i},w_{i},\mathbf{p}_{b}^{p},\psi_{p}\right).$
(84)

Now, we demonstrate a similar procedure for $\dot{z}_{b}^{p}$. From (44b), we
get

$\displaystyle\dot{z}_{b}^{p}$
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}U_{i}\left(-\cos\left(\theta_{p}\right)\sin\left(\gamma_{i}\right)+\cos\left(\gamma_{i}\right)\sin(\theta_{p})\cos\left(\psi_{p}-\chi_{i}\right)\right)+\dot{\xi}\,\kappa\,x_{b}^{p}$
(85a)
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}U_{i}\left(-\sin\left(\gamma_{i}-\theta_{p}\right)-\left(1-\cos\left(\chi_{i}-\psi_{p}\right)\right)\cos\left(\gamma_{i}\right)\sin(\theta_{p})\right)+\dot{\xi}\,\kappa\,x_{b}^{p}.$
(85b)

Once again, we consider the terms

$\sin\left(\gamma_{i}-\theta_{p}\right)=\sin\left(\theta_{i}-\alpha_{i}-\theta_{p}\right)=\sin\left(\theta_{i}-\theta_{p}\right)\frac{u_{i}}{U_{i}}-\cos\left(\theta_{i}-\theta_{p}\right)\frac{w_{i}}{U_{i}},$
(86)

and

$\sin\left(\theta_{i}-\alpha_{d,i}-\theta_{p}\right)=\sin\left(\theta_{i}-\theta_{p}\right)\frac{u_{d,i}}{U_{d,i}}-\cos\left(\theta_{i}-\theta_{p}\right)\frac{w_{i}}{U_{d,i}},$
(87)

which give us the following equality

$U_{i}\,\sin\left(\gamma_{i}-\theta_{p}\right)=U_{d,i}\,\sin\left(\theta_{i}-\alpha_{d,i}-\theta_{p}\right)+\tilde{u}_{i}\,\sin\left(\theta_{i}-\theta_{p}\right).$
(88)

Using a similar trick, we can write the sine as

$\sin\left(\theta_{i}-\alpha_{d,i}-\theta_{p}\right)=\sin\left(\tilde{\theta}_{i}-\alpha_{\rm
LOS}\right)=\sin\left(\tilde{\theta}_{i}\right)\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}-\cos\left(\tilde{\theta}_{i}\right)\frac{\left(z_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}$
(89)

Consequently, we can rewrite (85b) as

$\begin{split}\dot{z}_{b}^{p}&=-\frac{1}{n}\sum_{i=1}^{n}U_{d,i}\frac{z_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}+\dot{\xi}\,\kappa\,x_{b}^{p}\\\
&\quad+{G_{z}\left(\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\theta}_{1},\ldots,\tilde{\theta}_{n},\gamma_{1},\ldots,\gamma_{n},\chi_{1},\ldots,\chi_{n},u_{d,1},\ldots,u_{d,n},v_{1},\ldots,v_{n},w_{1},\ldots,w_{n},\mathbf{p}_{b}^{p},\theta_{p},\psi_{p}\right)},\end{split}$
(90)

where

$\displaystyle G_{z}(\cdot)$
$\displaystyle=\frac{1}{n}\sum_{i=1}^{n}G_{z,i}\left(\tilde{u}_{i},\tilde{\theta}_{i},\gamma_{i},\chi_{i},u_{d,i},v_{i},w_{i},\mathbf{p}_{b}^{p},\theta_{p},\psi_{p}\right),$
(91)
$\displaystyle\begin{split}G_{z,i}(\cdot)&=-U_{i}\left(\left(1-\cos\left(\chi_{i}-\psi_{p}\right)\right)\cos\left(\gamma_{i}\right)\sin(\theta_{p})\right)-\tilde{u}_{i}\,\sin\left(\theta_{i}-\theta_{p}\right)\\\
&\quad-\left(1-\cos\left(\tilde{\theta}_{i}\right)\right)\frac{\left(z_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}-U_{d,i}\sin\left(\tilde{\theta}_{i}\right)\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}.\end{split}$
(92)

###  B-B Desired Pitch and Yaw Rate

For further calculations, we need to evaluate the desired pitch ($q_{d,i}$)
and yaw ($r_{d,i}$) rates of the vessels. From (11d), we get the following
relation between the yaw rate and the derivative of the yaw angle

$q_{d,i}=\dot{\theta}_{d,i}.$ (93)

Now, we consider the desired pitch angle from (35). Since we are investigating
the path following task, we substitute $\gamma_{\rm LOS}$ from (38) for
$\gamma_{{\rm NSB},i}$. Differentiating (35) with respect to time yields

$\displaystyle q_{d,i}$
$\displaystyle=\dot{\theta}_{p}(\xi)+\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)\,\dot{z}_{b}^{p}-z_{b}^{p}\,\dot{\Delta}\left(\mathbf{p}_{b}^{p}\right)}{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}+\frac{u_{d,i}\,\dot{w}}{u_{d,i}^{2}+w_{i}^{2}}$
(94a)
$\displaystyle\begin{split}&=\dot{\xi}\,\kappa(\xi)+\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)\left(\frac{1}{n}\sum_{j=1}^{n}U_{d,j}\frac{\left(z_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}+\dot{\xi}\,\kappa\,x_{b}^{p}+G_{z}(\cdot)\right)}{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}\\\
&\quad+\frac{z_{b}^{p}\left(-k_{\xi}\frac{\left(x_{b}^{p}\right)^{2}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}-\frac{1}{n}\sum_{j=1}^{n}U_{d,j}\left(\scalebox{1.0}{$\frac{\cos\left(\gamma_{{\rm
LOS},j}\right)^{2}\left(y_{b}^{p}\right)^{2}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}+\frac{\left(z_{b}^{p}\right)^{2}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}\right)+y_{b}^{p}\,G_{y}(\cdot)+z_{b}^{p}\,G_{z}(\cdot)\right)}{\Delta\left(\mathbf{p}_{b}^{p}\right)\left(\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}\right)}\\\
&\quad+u_{d,i}\frac{X_{w}\left(u_{d,i}+\tilde{u}_{i},u_{c}\right)\,q+Y_{w}\left(u_{d,i}+\tilde{u}_{i},u_{c}\right)\left(w_{i}-w_{c}\right)}{u_{d,i}^{2}+w_{i}^{2}}.\end{split}$
(94b)

From (11e), we get the following relation between the yaw rate and the
derivative of the yaw angle

$r_{d,i}=\dot{\psi}_{d,i}\,\cos\left(\theta_{d,i}\right).$ (95)

Substituting the time-derivative of (36), we get

$\displaystyle r_{d,i}$
$\displaystyle=\left(\dot{\psi}_{p}(\xi)-\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)\,\dot{y}_{b}^{p}-y_{b}^{p}\,\dot{\Delta}\left(\mathbf{p}_{b}^{p}\right)}{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}-\frac{\dot{v}}{\sqrt{U_{d,i}^{2}-v_{i}^{2}}}\right)\,\cos\left(\theta_{d,i}\right)$
(96a)
$\displaystyle\begin{split}&=\left(\dot{\xi}\,\iota(\xi)-\frac{\Delta\left(\mathbf{p}_{b}^{p}\right)\left(\frac{1}{n}\sum_{j=1}^{n}U_{d,i}\frac{\cos\left(\gamma_{\rm
LOS}\right)\left(y_{b}^{p}\right)}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}-\dot{\xi}\,\iota\,x_{b}^{p}+G_{y}(\cdot)\right)}{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}\right.\\\
&\qquad+\frac{y_{b}^{p}\left(-k_{\xi}\frac{\left(x_{b}^{p}\right)^{2}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}-\frac{1}{n}\sum_{j=1}^{n}U_{d,i}\left(\scalebox{1.0}{$\frac{\cos\left(\gamma_{\rm
LOS}\right)^{2}\left(y_{b}^{p}\right)^{2}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}$}+\frac{\left(z_{b}^{p}\right)^{2}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}\right)+y_{b}^{p}\,G_{y}(\cdot)+z_{b}^{p}\,G_{z}(\cdot)\right)}{\Delta\left(\mathbf{p}_{b}^{p}\right)\left(\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}\right)}\\\
&\qquad-\frac{X\left(u_{d,i}+\tilde{u}_{i},u_{c}\right)\,r+Y\left(u_{d,i}+\tilde{u}_{i},u_{c}\right)\left(v_{i}-v_{c}\right)}{\sqrt{u_{d,i}^{2}+w_{i}^{2}}}\Bigg{)}\,\cos\left(\theta_{d,i}\right).\end{split}$
(96b)

###  B-C Proof of Lemma 1

In [11], it is shown that the error states (50a)–(50e) are UGES and the ocean
current estimate errors (51a)–(51c) are bounded, which implies that
(50a)–(51c) are forward complete. Therefore, we only need to prove that the
underactuated sway and heave dynamics (52), (53) and the barycenter dynamics
(49a)–(49c) are forward complete.

First, let us consider the underactuated sway dynamics. From (52), we get

$\dot{v}_{i}=X_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(\tilde{r}_{i}+r_{d,i}\right)+Y_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(v_{i}-v_{c}\right),$
(97)

where $\tilde{r}_{i}=r_{i}-r_{d,i}$. Now, let us consider a Lyapunov function
candidate

$V_{v}(v_{i})=\frac{1}{2}v_{i}^{2}.$ (98)

Its derivative along the trajectories of $v_{i}$ is

$\dot{V}_{v}(v_{i})=X_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(\tilde{r}_{i}+r_{d,i}\right)\,v_{i}+Y_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(v_{i}-v_{c}\right)\,v_{i}.$
(99)

From the boudedness of $\tilde{\mathbf{X}}_{2,i}$, $\kappa(\xi)$,
$\iota(\xi)$, $u_{d,i}$, $u_{c}$ and $v_{c}$, we can conclude that there
exists some scalar $\beta_{v,0}>0$ such that
$\left\|\left[\tilde{\mathbf{X}}_{2,i}^{\rm
T},\kappa(\xi),\iota(\xi),u_{d,i},u_{c},v_{c}\right]^{\rm
T}\right\|\leq\beta_{0}$. Moreover, from (96), we can conclude that there
exist some positive functions $a_{r}(\beta_{v,0})$ and $b_{r}(\beta_{v,0})$
such that

$\left|r_{d,i}\right|\leq
a_{r}(\beta_{v,0})\,\left|v_{i}\right|+b_{r}(\beta_{v,0}).$ (100)

Consequently, we can upper bound $\dot{V}_{v}(v_{i})$ using the following
expression

$\dot{V}_{v}(v_{i})\leq
X_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(\tilde{r}_{i}\,v_{i}+a_{r}(\cdot)v_{i}^{2}+b_{r}(\cdot)v_{i}\right)+Y_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(v_{i}^{2}-v_{c}\,v_{i}\right).$
(101)

Using Young’s inequality, we get

$\displaystyle\begin{split}\dot{V}_{v}(v_{i})&\leq\left(X_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(2+a_{r}(\cdot)\right)+2\,Y_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\right)\,v_{i}^{2}\\\
&\quad+X_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(\tilde{r}_{i}^{2}+b_{r}(\cdot)^{2}\right)+Y_{v}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,v_{c}^{2}\end{split}$
(102a) $\displaystyle\leq\alpha_{v}\,V_{v}(v_{i})+\beta_{v}.$ (102b)

Using the comparison lemma, we get

$V_{v}\left(v_{i}(t)\right)\leq\left(V_{v}\left(v_{i}(t_{0})\right)+\frac{\beta_{v}}{\alpha_{v}}\right)\,{\rm
exp}\left(\alpha_{v}(t-t_{0})\right)-\frac{\beta_{v}}{\alpha_{v}}.$ (103)

As $V_{v}(v_{i})$ is defined for all $t>t_{0}$, it follows that $v_{i}$ is
also defined for all $t>t_{0}$. The solutions of (52) thus fulfill the
definition of forward completeness, as defined in [18].

Now, let us consider the underactuated heave dynamics. From (53), we get

$\dot{w}_{i}=X_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(\tilde{q}_{i}+q_{d,i}\right)+Y_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(w_{i}-w_{c}\right)+G(\theta_{i}),$
(104)

where $\tilde{q}_{i}=q_{i}-q_{d,i}$. Similar to the previous paragraph, we
consider a Lyapunov function candidate

$V_{w}(w_{i})=\frac{1}{2}w_{i}^{2},$ (105)

whose derivative is

$\dot{V}_{w}(w_{i})=X_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(\tilde{q}_{i}+q_{d,i}\right)\,w_{i}+Y_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,\left(w_{i}-w_{c}\right)\,w_{i}+G(\theta)\,w_{i}.$
(106)

From the boudedness of $\tilde{\mathbf{X}}_{2,i}$, $\kappa(\xi)$,
$\iota(\xi)$, $u_{d,i}$, $u_{c}$ and $w_{c}$, we can conclude that there
exists some scalar $\beta_{0}>0$ such that
$\left\|\left[\tilde{\mathbf{X}}_{2,i}^{\rm
T},\kappa(\xi),\iota(\xi),u_{d,i},u_{c},w_{c}\right]^{\rm
T}\right\|\leq\beta_{w,0}$. Moreover, from (94), we can conclude that there
exist some positive functions $a_{q}(\beta_{w,0})$ and $b_{q}(\beta_{w,0})$
such that

$\left|q_{d,i}\right|\leq
a_{q}(\beta_{w,0})\,\left|w_{i}\right|+b_{q}(\beta_{w,0}).$ (107)

Consequently, we can upper bound $\dot{V}_{w}(w_{i})$ using the following
expression

$\dot{V}_{w}(w_{i})\leq
X_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(\tilde{q}_{i}\,w_{i}+a_{q}(\cdot)w_{i}^{2}+b_{q}(\cdot)w_{i}\right)+Y_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(w_{i}^{2}-w_{c}\,w_{i}\right)+G(\theta_{i})\,w_{i}.$
(108)

Using Young’s inequality, we get

$\displaystyle\begin{split}\dot{V}_{w}(w_{i})&\leq\left(X_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(2+a_{q}(\cdot)\right)+2\,Y_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)+1\right)\,w_{i}^{2}\\\
&\quad+X_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\left(\tilde{q}_{i}^{2}+b_{q}(\cdot)^{2}\right)+Y_{w}\left(\tilde{u}_{i}+u_{d,i},u_{c}\right)\,w_{c}^{2}+G(\theta)^{2}\end{split}$
(109a) $\displaystyle\leq\alpha_{w}\,V_{w}(w_{i})+\beta_{w}.$ (109b)

Using the comparison lemma, we get

$V_{w}\left(w_{i}(t)\right)\leq\left(V_{w}\left(w_{i}(t_{0})\right)+\frac{\beta_{w}}{\alpha_{w}}\right)\,{\rm
exp}\left(\alpha_{w}(t-t_{0})\right)-\frac{\beta_{w}}{\alpha_{w}}.$ (110)

Using the same arguments as in the previous paragraph, we conclude that the
solutions of (53) are forward complete.

Finally, let us consider the barycenter dynamics. We use a Lyapunov function
candidate

$V_{b}(\mathbf{p}_{b}^{p})=\frac{1}{2}\left(\left(x_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}\right),$
(111)

whose derivative along the solutions of (49a)–(49c) is

$\displaystyle\dot{V}_{b}\left(\mathbf{p}_{b}^{p}\right)$
$\displaystyle=-k_{\xi}\frac{\left(x_{b}^{p}\right)^{2}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}-\frac{1}{n}\sum_{i=1}^{n}U_{d,i}\left(\frac{\cos\left(\gamma_{\rm
LOS}\right)^{2}\left(y_{b}^{p}\right)^{2}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}+\frac{\left(z_{b}^{p}\right)^{2}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}\right)+G_{y}(\cdot)\,y_{b}^{p}+G_{z}(\cdot)\,z_{b}^{p}$
(112a) $\displaystyle\leq
G_{y}(\cdot)\,y_{b}^{p}+G_{z}(\cdot)\,z_{b}^{p}+\frac{1}{2}\left(x_{b}^{p}\right)^{2}.$
(112b)

Using Young’s inequality, we get

$\dot{V}_{b}\left(\mathbf{p}_{b}^{p}\right)\leq\frac{1}{2}\left(\left(x_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}\right)+\frac{1}{2}\left(G_{y}(\cdot)^{2}+G_{z}(\cdot)^{2}\right)=V_{b}\left(\mathbf{p}_{b}^{p}\right)+\frac{1}{2}\left(G_{y}(\cdot)^{2}+G_{z}(\cdot)^{2}\right).$
(113)

Note that from (82) and (92), we can conclude that there exist some positive
function $\zeta_{y}(U_{d,1},\ldots,U_{d,n})$ and
$\zeta_{z}(U_{d,1},\ldots,U_{d,n})$ such that

$\displaystyle\left|G_{y}(\cdot)\right|\leq\zeta_{y}(\cdot)\left\|\left[\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\psi}_{1},\ldots,\tilde{\psi}_{n}\right]^{\rm
T}\right\|,$ (114)
$\displaystyle\left|G_{z}(\cdot)\right|\leq\zeta_{z}(\cdot)\left\|\left[\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\theta}_{1},\ldots,\tilde{\theta}_{n}\right]^{\rm
T}\right\|.$ (115)

Consequently, there exists a class-$\mathcal{K}_{\infty}$ function
$\zeta_{p}(\cdot)$ such that

$\dot{V}_{p}\left(\mathbf{p}_{b}^{p}\right)\leq
V_{p}\left(\mathbf{p}_{b}^{p}\right)+\zeta_{p}\left(v_{1},\ldots,v_{n},w_{1},\ldots,w_{n},\tilde{u}_{1},\ldots,\tilde{u}_{n},\tilde{\psi}_{1},\ldots,\tilde{\psi}_{n},\tilde{\theta}_{1},\ldots,\tilde{\theta}_{n}\right).$
(117)

Since all the arguments of $\zeta_{p}(\cdot)$ are forward complete, Corollary
2.11 of [18] is satisfied and the barycenter dynamics is forward complete,
thus concluding the proof of Lemma 1.

###  B-D Proof of Lemma 2

First, we consider the sway dynamics. We take the Lyapunov function candidate
$V_{v}$ from (98) and simplify its derivative by setting
$\left[\tilde{\mathbf{X}}_{1}^{\rm T},\tilde{\mathbf{X}}_{2}^{\rm
T}\right]=\mathbf{0}^{\rm T}$.

$\dot{V}_{v}(v_{i})=X_{v}\left(u_{d,i},u_{c}\right)\,r_{d,i}\,v_{i}+Y_{v}\left(u_{d,i},u_{c}\right)\,\left(v_{i}-v_{c}\right)\,v_{i}.$
(118)

Next, we find an upper bound on $r_{d,i}\,v_{i}$. We substitute from (96), set
$\left[\tilde{\mathbf{X}}_{1}^{\rm T},\tilde{\mathbf{X}}_{2}^{\rm
T}\right]=\mathbf{0}^{\rm T}$ and collect all terms that grow linearly with
$v_{i}$ to obtain the following expression

(119) (120)

We can bound this expression as

$\displaystyle\left|r_{d,i}\,v_{i}\right|$
$\displaystyle\leq\frac{2}{n}\left|v_{i}\right|\,\left|\iota(\xi)\right|\sum_{j=1}^{n}\left(\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\right)+\left|F_{v}(\cdot)\right|$
(121a)
$\displaystyle\leq\frac{2}{n}\left|\iota(\xi)\right|\,v_{i}^{2}+\frac{2}{n}\left|v_{i}\right|\,\left|\iota(\xi)\right|\left(\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)+\left|F_{v}(\cdot)\right|,$
(121b)

which we can substitute to (118) to obtain

$\begin{split}\dot{V}_{v}(v_{i})&\leq\left(X_{v}\left(u_{d,i},u_{c}\right)\frac{2}{n}\left|\iota(\xi)\right|+Y_{v}\left(u_{d,i},u_{c}\right)\right)v_{i}^{2}+\left(\frac{2}{n}\left|v_{i}\right|\,\left|\iota(\xi)\right|\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)\\\
&\quad+\left(\left|F_{v}(\cdot)\right|-Y_{v}\left(u_{d,i},u_{c}\right)\left|v_{c}\right|\right)\left|v_{i}\right|.\end{split}$
(122)

For a sufficiently large $v_{i}$, the quadratic term will dominate the linear
term. Therefore, we can conclude that $v_{i}$ is bounded if

$X_{v}\left(u_{d,i},u_{c}\right)\frac{2}{n}\left|\iota(\xi)\right|+Y_{v}\left(u_{d,i},u_{c}\right)<0.$
(123)

Since $Y_{v}$ is assumed to be always negative, the inequality is satisfied if

$\left|\iota(\xi)\right|<\frac{n}{2}\left|\frac{Y_{v}\left(u_{d,i},u_{c}\right)}{X_{v}\left(u_{d,i},u_{c}\right)}\right|.$
(124)

Now, we perform a similar procedure for the heave dynamics. We take the
Lyapunov function candidate $V_{w}$ from (105) and simplify its derivative by
setting $\left[\tilde{\mathbf{X}}_{1}^{\rm T},\tilde{\mathbf{X}}_{2}^{\rm
T}\right]=\mathbf{0}^{\rm T}$.

$\dot{V}_{w}(w_{i})=X_{w}\left(u_{d,i},u_{c}\right)\,q_{d,i}\,w_{i}+Y_{w}\left(u_{d,i},u_{c}\right)\,\left(w_{i}-w_{c}\right)\,w_{i}+G(\theta_{i})\,w_{i}.$
(125)

Next, we find an upper bound on $q_{d,i}\,w_{i}$. We substitute from (94), set
$\left[\tilde{\mathbf{X}}_{1}^{\rm T},\tilde{\mathbf{X}}_{2}^{\rm
T}\right]=\mathbf{0}^{\rm T}$ and collect all terms that grow linearly with
$w_{i}$ to obtain the following expression

(126) (127)

We can bound this expression as

$\displaystyle\left|q_{d,i}\,w_{i}\right|$
$\displaystyle\leq\frac{2}{n}\left|\kappa(\xi)\right|\,w_{i}^{2}+\frac{2}{n}\left|w_{i}\right|\,\left|\kappa(\xi)\right|\left(\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|v_{i}\right|\right)+\left|F(\cdot)\right|,$
(128a)

which we can substitute to (125) to obtain

$\begin{split}\dot{V}_{w}(w_{i})&\leq\left(X_{w}\left(u_{d,i},u_{c}\right)\frac{2}{n}\left|\kappa(\xi)\right|+Y_{w}\left(u_{d,i},u_{c}\right)\right)w_{i}^{2}+\left(\frac{2}{n}\left|w_{i}\right|\,\left|\kappa(\xi)\right|\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)\\\
&\quad+\left(\left|F(\cdot)\right|-Y_{w}\left(u_{d,i},u_{c}\right)\left|v_{c}\right|+\left|G(\theta_{i})\right|\right)\left|w_{i}\right|+G(\theta_{i})\,w_{i}.\end{split}$
(129)

For a sufficiently large $w_{i}$, the quadratic term will dominate the linear
term. Therefore, we can conclude that $w_{i}$ is bounded if

$X_{w}\left(u_{d,i},u_{c}\right)\frac{2}{n}\left|\kappa(\xi)\right|+Y_{w}\left(u_{d,i},u_{c}\right)<0.$
(130)

Since $Y_{w}$ is assumed to be always negative, the inequality is satisfied if

$\left|\kappa(\xi)\right|<\frac{n}{2}\left|\frac{Y_{w}\left(u_{d,i},u_{c}\right)}{X_{w}\left(u_{d,i},u_{c}\right)}\right|,$
(131)

which concludes the proof of Lemma 2.

###  B-E Proof of Lemma 3

First, we consider the sway dynamics. We take the Lyapunov function candidate
$V_{v}$ from (98) and simplify its derivative by setting
$\tilde{\mathbf{X}}_{2}=\mathbf{0}$.

$\dot{V}_{v}(v_{i})=X_{v}\left(u_{d,i},u_{c}\right)\,r_{d,i}\,v_{i}+Y_{v}\left(u_{d,i},u_{c}\right)\,\left(v_{i}-v_{c}\right)\,v_{i}.$
(132)

Next, we find an upper bound on $r_{d,i}\,v_{i}$. We substitute from (96), set
$\tilde{\mathbf{X}}_{2}=\mathbf{0}$ and collect all terms that grow linearly
with $v_{i}$ to obtain the following expression

$\displaystyle\begin{split}{r_{d,i}\,v_{i}}&=\scalebox{1.0}{$\left(v_{i}\left(1+\frac{\Delta(\mathbf{p}_{b}^{p})\,x_{b}^{p}}{\Delta(\mathbf{p}_{b}^{p})^{2}+\left(x_{b}^{p}\right)^{2}}\right)\iota(\xi)\frac{1}{n}\sum_{j=1}^{n}U_{j}\,\Omega_{x}(\gamma_{j},\theta_{p},\chi_{j},\psi_{p})-\frac{y_{b}^{p}\,v_{i}\,\sum_{j=1}^{n}\left(\frac{\cos\left(\gamma_{\rm
LOS}\right)y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}+\frac{z_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}\right)}{n\,\Delta(\mathbf{p}_{b}^{p})\left(\Delta(\mathbf{p}_{b}^{p})^{2}+\left(y_{b}^{p}\right)^{2}\right)}\right.$}\\\
&\qquad\left.+\frac{v_{i}\,\Delta(\mathbf{p}_{b}^{p})\sum_{j=1}^{n}\frac{\cos\left(\gamma_{\rm
LOS}\right)y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}}{n\,\left(\Delta(\mathbf{p}_{b}^{p})^{2}+(y_{b}^{p})^{2}\right)}+\frac{Y_{v}(u_{d,i},u_{c})}{\sqrt{u_{d,i}^{2}+w_{i}^{2}}}v_{i}^{2}\right)\cos(\theta_{d,i})+H_{v}(u_{d,i},\theta_{d,i},u_{c},v_{c},v_{i},w_{i},r_{i},\mathbf{p}_{b}^{p},\xi),\end{split}$
(133)
$\displaystyle\begin{split}H_{v}(\cdot)&=\left(\left(1+\frac{\Delta(\mathbf{p}_{b}^{p})\,x_{b}^{p}}{\Delta(\mathbf{p}_{b}^{p})^{2}+\left(x_{b}^{p}\right)^{2}}\right)k_{\xi}\,\iota(\xi)\frac{x_{b}^{p}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}-\frac{y_{b}^{p}\,k_{\xi}\,x_{b}^{p}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}\Delta(\mathbf{p}_{b}^{p})\left(\Delta(\mathbf{p}_{b}^{p})^{2}+\left(y_{b}^{p}\right)^{2}\right)}\right.\\\
&\qquad\left.+\frac{X_{v}(u_{d,i},u_{c})\,r_{i}-Y_{v}(u_{d,i},u_{c})\,v_{c}}{\sqrt{u_{d,i}^{2}+w_{i}^{2}}}\right)v_{i}\,\cos(\theta_{d,i}).\end{split}$
(134)

We can bound this expression as

$\displaystyle\left|r_{d,i}\,v_{i}\right|$
$\displaystyle\leq\left(\frac{2}{n}\left|\iota(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\left|v_{i}\right|\,\sum_{j=1}^{n}\left(\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\right)+\left|H_{v}(\cdot)\right|$
(135a)
$\displaystyle\leq\left(\frac{2}{n}\left|\iota(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\,v_{i}^{2}+\left(\frac{2}{n}\left|\iota(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\left(\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)$
(135b) $\displaystyle\quad+\left|H_{v}(\cdot)\right|,$ (135c)

which we can substitute to (132) to obtain

$\begin{split}\dot{V}_{v}(v_{i})&\leq\left(X_{v}\left(u_{d,i},u_{c}\right)\left(\frac{2}{n}\left|\iota(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)+Y_{v}\left(u_{d,i},u_{c}\right)\right)v_{i}^{2}\\\
&\quad+\left(\frac{2}{n}\left|\iota(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\left(\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)\\\
&\quad+\left(\left|H_{v}(\cdot)\right|-Y_{v}\left(u_{d,i},u_{c}\right)\left|v_{c}\right|\right)\left|v_{i}\right|.\end{split}$
(136)

For a sufficiently large $v_{i}$, the quadratic term will dominate the linear
term. Therefore, we can conclude that $v_{i}$ is bounded if

$X_{v}\left(u_{d,i},u_{c}\right)\left(\frac{2}{n}\left|\iota(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)+Y_{v}\left(u_{d,i},u_{c}\right)<0.$
(137)

From the definition of the lookahead distance (40), this condition is
satisfied if

$\Delta_{0}>\frac{3}{n\left|\frac{Y_{v}\left(u_{d,i},u_{c}\right)}{X_{v}\left(u_{d,i},u_{c}\right)}\right|-2\left|\iota(\xi)\right|}.$
(138)

Now, we perform a similar procedure for the heave dynamics. We take the
Lyapunov function candidate $V_{w}$ from (105) and simplify its derivative by
setting $\tilde{\mathbf{X}}_{2}=\mathbf{0}$.

$\dot{V}_{w}(w_{i})=X_{w}\left(u_{d,i},u_{c}\right)\,q_{d,i}\,w_{i}+Y_{w}\left(u_{d,i},u_{c}\right)\,\left(w_{i}-w_{c}\right)\,w_{i}+G(\theta_{i})\,w_{i}.$
(139)

Next, we find an upper bound on $q_{d,i}\,w_{i}$. We substitute from (94), set
$\tilde{\mathbf{X}}_{2}=\mathbf{0}$ and collect all terms that grow linearly
with $w_{i}$ to obtain the following expression

$\displaystyle\begin{split}{q_{d,i}\,w_{i}}&=\scalebox{1.0}{$w_{i}\left(1+\frac{\Delta(\mathbf{p}_{b}^{p})\,x_{b}^{p}}{\Delta(\mathbf{p}_{b}^{p})^{2}+\left(x_{b}^{p}\right)^{2}}\right)\kappa(\xi)\frac{1}{n}\sum_{j=1}^{n}U_{j}\,\Omega_{x}(\gamma_{j},\theta_{p},\chi_{j},\psi_{p})-\frac{z_{b}^{p}\,w_{i}\,\sum_{j=1}^{n}\left(\frac{\cos\left(\gamma_{\rm
LOS}\right)y_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(y_{b}^{p}\right)^{2}}}+\frac{z_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}\right)}{n\,\Delta(\mathbf{p}_{b}^{p})\left(\Delta(\mathbf{p}_{b}^{p})^{2}+\left(z_{b}^{p}\right)^{2}\right)}$}\\\
&\quad+\frac{w_{i}\,\Delta(\mathbf{p}_{b}^{p})\sum_{j=1}^{n}\frac{z_{b}^{p}}{\sqrt{\Delta\left(\mathbf{p}_{b}^{p}\right)^{2}+\left(z_{b}^{p}\right)^{2}}}}{n\,\left(\Delta(\mathbf{p}_{b}^{p})^{2}+(z_{b}^{p})^{2}\right)}+u_{d,i}\frac{Y_{w}(u_{d,i},u_{c})}{{u_{d,i}^{2}+w_{i}^{2}}}w_{i}^{2}+H_{w}(u_{d,i},u_{c},v_{c},w_{i},v_{i},q_{i},\mathbf{p}_{b}^{p},\xi),\end{split}$
(140)
$\displaystyle\begin{split}H_{w}(\cdot)&=\left(\left(1+\frac{\Delta(\mathbf{p}_{b}^{p})\,x_{b}^{p}}{\Delta(\mathbf{p}_{b}^{p})^{2}+\left(x_{b}^{p}\right)^{2}}\right)k_{\xi}\,\kappa(\xi)\frac{x_{b}^{p}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}}-\frac{y_{b}^{p}\,k_{\xi}\,x_{b}^{p}}{\sqrt{1+\left(x_{b}^{p}\right)^{2}}\Delta(\mathbf{p}_{b}^{p})\left(\Delta(\mathbf{p}_{b}^{p})^{2}+\left(y_{b}^{p}\right)^{2}\right)}\right.\\\
&\qquad\left.+u_{d,i}\frac{X_{w}(u_{d,i},u_{c})\,r_{i}-Y_{w}(u_{d,i},u_{c})\,v_{c}}{{u_{d,i}^{2}+w_{i}^{2}}}\right)w_{i}.\end{split}$
(141)

We can bound this expression as

$\displaystyle\left|q_{d,i}\,w_{i}\right|$
$\displaystyle\leq\left(\frac{2}{n}\left|\kappa(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\left|w_{i}\right|\,\sum_{j=1}^{n}\left(\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\right)+\left|H_{w}(\cdot)\right|$
(142a)
$\displaystyle\begin{split}&\leq\left(\frac{2}{n}\left|\kappa(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\,w_{i}^{2}+\left(\frac{2}{n}\left|\kappa(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\left(\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)\\\
&\quad+\left|H_{w}(\cdot)\right|,\end{split}$ (142b)

which we can substitute to (139) to obtain

$\begin{split}\dot{V}_{w}(w_{i})&\leq\left(X_{w}\left(u_{d,i},u_{c}\right)\left(\frac{2}{n}\left|\kappa(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)+Y_{w}\left(u_{d,i},u_{c}\right)\right)w_{i}^{2}\\\
&\quad+\left(\frac{2}{n}\left|\kappa(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)\left(\sum_{j\in\\{1,\ldots,n\\}\setminus\\{i\\}}\bigl{(}\left|u_{j}\right|+\left|v_{j}\right|+\left|w_{j}\right|\bigr{)}+\left|u_{i}\right|+\left|w_{i}\right|\right)\\\
&\quad+\left(\left|H_{w}(\cdot)\right|-Y_{w}\left(u_{d,i},u_{c}\right)\left|v_{c}\right|\right)\left|w_{i}\right|.\end{split}$
(143)

For a sufficiently large $w_{i}$, the quadratic term will dominate the linear
term. Therefore, we can conclude that $w_{i}$ is bounded if

$X_{w}\left(u_{d,i},u_{c}\right)\left(\frac{2}{n}\left|\kappa(\xi)\right|+\frac{3}{n\,\Delta(\mathbf{p}_{b}^{p})}\right)+Y_{w}\left(u_{d,i},u_{c}\right)<0.$
(144)

From the definition of the lookahead distance (40), this condition is
satisfied if

$\Delta_{0}>\frac{3}{n\left|\frac{Y_{w}\left(u_{d,i},u_{c}\right)}{X_{w}\left(u_{d,i},u_{c}\right)}\right|-2\left|\kappa(\xi)\right|}.$
(145)