*3.2. Quadrotor Model*

The kinematic equations that describe the motion of a rigid body in 3-D space can be described by a double integrator model, according to:

$$\ddot{\mathbf{p}} := \underbrace{\mathcal{g}\mathbf{e}\_3 - \frac{1}{m^B}\mathcal{R}(\theta)\,T\mathbf{e}\_3}\_{\mathbf{u}} + \mathbf{d},\tag{10}$$

where **p** := [*x*, *y*, *z*] *<sup>T</sup>* denotes the quadrotor's position expressed in {*U*}, *<sup>θ</sup>* := [*φ*, *<sup>θ</sup>*, *<sup>ψ</sup>*] *T* denotes the orientation vector of {*B*} expressed in {*U*}, and **<sup>u</sup>** <sup>∈</sup> <sup>R</sup><sup>3</sup> can be regarded as the input of the system, comprising both the attitude of the vehicle and the total thrust *T*. The vector **<sup>d</sup>** <sup>∈</sup> <sup>R</sup><sup>3</sup> represents unmeasured external disturbances, such as wind, acting on the vehicle, assumed to be constant and bounded such that **d** ≤ *dmax*. The 3-D rotation matrix adopted for the quadrotor model follows the Z-Y-X convention, and is given by:

$$\prescript{L}{B}{R}(\theta) = \prescript{R}{R}{\tau}(\psi)\prescript{}{R}{\eta}(\theta)\prescript{}{R}{R}\_{\mathcal{X}}(\phi). \tag{11}$$

#### **4. Path Following**

The path following (PF) problem concerns the problem of making a vehicle move along a desired path **p***d*(*γ*) parameterised by a variable *γ* (for example, the arc-length of the curve). The key idea is that each vehicle must approach a virtual target that moves along the path with a desired speed profile *vd*(*γ*), according to Figure 3. Since the end goal is to have more than one vehicle performing path following with a pre-defined inter-vehicle formation, this speed profile is given as the sum of another single-vehicle speed profile and an inter-vehicle coordination term, according to:

$$v\_d(\gamma) := v\_L(\gamma) + v^{\text{coord}}, \text{ with } |v\_L(\gamma)| \le v\_L^{\text{max}}, \tag{12}$$

where *vL*(*γ*) is a desired speed profile defined only as a function of the path, *vmax <sup>L</sup>* is a pre-defined speed upper-bound, and *vcoord* is the speed coordination term that will be used in Section 5 to enable the CPF behaviour. It is important to notice that the desired speed profile *vL*(*γ*) should be the same for all the vehicles, enabling them to follow a given path at the same rate. On the other hand, the speed coordination term *vcoord* will not be the same for all vehicles and will be used to adjust the progression speed of each individual robot based on how aligned they are with each other.

**Remark 1.** *Speed profile vd*(*γ*) *might not correspond directly to an inertial speed, especially if the curve is not parameterised in terms of the arc-length. Nonetheless, a relation between the inertial speed and the desired speed profile is addressed in detail in Section 6.4.*

**Figure 3.** Path following schematic: (**a**) ASV path following. (**b**) Quadrotor path following.

**Problem 1.** *Given a generic vehicle (ASV or quadrotor), consider the geometric path* **p***d*(*γ*) : [0, <sup>∞</sup>] <sup>→</sup> <sup>R</sup>2/R<sup>3</sup> *for the ASV/quadrotor respectively, parameterised by a continuous variable <sup>γ</sup>* <sup>∈</sup> <sup>R</sup> *and vd*(*γ*, *<sup>t</sup>*) <sup>∈</sup> <sup>R</sup> *a desired speed profile for a virtual target moving along the desired path. Furthermore, consider* **p***d*(*γ*) *to be C*<sup>2</sup> *and have its first and second derivatives with respect to γ bounded. Assume the vehicle is equipped with inner-loop controllers allowing it to track a desired control reference* **<sup>u</sup>***<sup>d</sup>* <sup>∈</sup> <sup>R</sup>2/R3*, assumed to be bounded, by recruiting the appropriate forces and torques to apply to the vehicle. Design a feedback control law for the system input* **u***<sup>d</sup> and virtual target γ*¨ *such that:*


#### *4.1. ASV Path Following*

Following the approach proposed by Aguiar et al. [10–12], consider the global diffeomorphic coordinate transformation which expresses the position error defined in the body-frame of the vehicle {*B*} as:

$$\mathbf{e}\_p(t) := {}^B\_{\mathcal{U}} \mathcal{R}(\boldsymbol{\psi}) (\mathbf{p}(t) - \mathbf{p}\_d(\boldsymbol{\gamma})),\tag{13}$$

and let the speed-tracking error be defined as:

$$
\varepsilon\_{\gamma} := \dot{\gamma} - \upsilon\_d(\gamma, t). \tag{14}
$$

With these definitions, the body-fixed position error dynamics are given by:

$$\dot{\mathbf{e}}\_{\mathcal{P}}(t) = {}^{B}\_{\mathcal{U}}\dot{\mathcal{R}}(\boldsymbol{\psi})(\mathbf{p}(t) - \mathbf{p}\_{d}(\boldsymbol{\gamma})) + {}^{B}\_{\mathcal{U}}\mathcal{R}(\boldsymbol{\psi})(\dot{\mathbf{p}}(t) - \dot{\mathbf{p}}\_{d}(\boldsymbol{\gamma})).\tag{15}$$

We recall that the derivative of a rotation matrix can be expressed as the product of a skew-symmetric matrix with the transposed rotation matrix, that is:

$$\prescript{B}{}{\mathcal{R}}(\psi) = -\mathcal{S}(r)\_{ll}^{B}\mathcal{R}(\psi). \tag{16}$$

Replacing (16) in (15) yields the position error dynamics expressed in the body-fixed frame as:

$$\dot{\mathbf{e}}\_{p}(t) = -S(r)\underbrace{{}\_{\downarrow}^{B}R(\boldsymbol{\psi})(\mathbf{p}(t) - \mathbf{p}\_{d}(\boldsymbol{\gamma}))}\_{\mathbf{e}\_{p}(t)} + \mathbf{v} + \underbrace{{}\_{\downarrow}^{B}R(\boldsymbol{\psi})\mathbf{v}\_{\mathbf{c}}}\_{\mathbf{v}\_{\mathbf{c}}} - {}\_{\downarrow}^{B}R(\boldsymbol{\psi})\frac{\partial \mathbf{p}\_{d}(\boldsymbol{\gamma})}{\partial \boldsymbol{\gamma}}\dot{\boldsymbol{\gamma}}.\tag{17}$$

Since there is no direct control in the sway motion, the goal is to generate surge speed and heading rate control references. Therefore, we must make these references appear explicitly in the error expression. By introducing an offset *δ* = [0, *δ*] *<sup>T</sup>* <sup>∈</sup> <sup>R</sup><sup>2</sup> (with *<sup>δ</sup>* <sup>&</sup>lt; 0) in the standard position error, it is possible to re-write (17) as:

$$\dot{\mathbf{e}}\_{p}(t) = -S(r)(\mathbf{e}\_{p} - \boldsymbol{\delta}) + \underbrace{\begin{bmatrix} 1 & 0\\ 0 & -\boldsymbol{\delta} \end{bmatrix}}\_{\boldsymbol{\Delta}} \underbrace{\begin{bmatrix} \boldsymbol{\mu} \\ \boldsymbol{r} \end{bmatrix}}\_{\mathbf{u}} + \begin{bmatrix} 0 \\ \boldsymbol{v} \end{bmatrix} + \mathbf{v}\_{\mathcal{E}} - \prescript{B}{}{\mathcal{U}} \mathcal{R}(\boldsymbol{\psi}) \frac{\partial \mathbf{p}\_{d}(\boldsymbol{\gamma})}{\partial \boldsymbol{\gamma}} \boldsymbol{\gamma}. \tag{18}$$

Consider that each ASV is equipped with a Doppler Velocity Logger (DVL) capable of providing the vehicle's relative velocity with respect to the water **v**, expressed in {*B*}, and a Global Positioning System (GPS) unit which provides measurements of the position of the vehicle **p**, expressed in {*U*}. To estimate the ocean current, Pascoal et al. [18] and Sanches et al. [19] propose the use of a complementary filter. Consider the process model given by (8) and the candidate complementary filter model described by:

$$\begin{aligned} \mathcal{F} := \begin{cases} \dot{\mathbf{p}} = k\_1(\mathbf{p} - \mathbf{\hat{p}}) + \frac{lI}{B}R(\boldsymbol{\psi})\mathbf{v} + \boldsymbol{\Psi}\_{\mathcal{L}} \\ \boldsymbol{\Phi}\_{\mathcal{L}} = k\_2(\mathbf{p} - \mathbf{\hat{p}}), \end{cases} \end{aligned} \tag{19}$$

with *k*<sup>1</sup> and *k*<sup>2</sup> positive constants. The proposed complementary filter is asymptotically stable. For a formal stability analysis of this complementary filter, refer to Pascoal et al. [18].

At this point, it is important to notice that the current velocity *v<sup>c</sup>* and the requested input **u***<sup>d</sup>* that are to be applied to a vehicle's kinematic model cannot be estimated and tracked, respectively, with infinite precision. For this reason, we define the current estimation error and the inner-loop tracking error given by:

$$\begin{aligned} \mathfrak{v}\_{\mathfrak{c}} &:= \mathfrak{v}\_{\mathfrak{c}} - \mathfrak{v}\_{\mathfrak{c}}, \\ \mathfrak{u} &:= \mathfrak{u} - \mathfrak{u}\_{\mathfrak{d}}. \end{aligned} \tag{20}$$

Consider the Proposition 1 introduced below, in which a solution to Problem 1, applied to an ASV, is provided along with convergence guarantees in the presence of bounded estimation and tracking errors.

**Proposition 1.** *Consider the system described by the kinematics in (8), with the outer-loop control laws given by:*

$$\mathbf{u}\_d := \Delta^{-1} \left( -K\_p \sigma(\mathbf{e}\_p - \boldsymbol{\delta}) - \begin{bmatrix} 0 \\ \upsilon \end{bmatrix} - \mathfrak{d}\_c + {}^B\_{\mathrm{II}} \boldsymbol{R}(\boldsymbol{\psi}) \frac{\partial \mathbf{p}\_d(\boldsymbol{\gamma})}{\partial \boldsymbol{\gamma}} \upsilon\_d(\boldsymbol{\gamma}, t) \right), \tag{21}$$

$$\dot{\gamma} := -k\_{\gamma} e\_{\gamma} + \dot{\upsilon}\_{d} (\gamma, t) + (\mathbf{e}\_{p} - \boldsymbol{\delta})^{T\_{B}}\_{\,\,\,\,\Gamma} R(\boldsymbol{\psi}) \frac{\partial \mathbf{p}\_{d}(\gamma)}{\partial \gamma}, \tag{22}$$

*where Kp* 0*, k<sup>γ</sup>* > 0*, and σ*(**e***p*) = *tanh*( # #**e***<sup>p</sup>* # #) **<sup>e</sup>***<sup>p</sup>* **<sup>e</sup>***<sup>p</sup> is a saturation function. The closed-loop system is input-to-state stable (ISS) with respect to* Δ**u˜** + *v***˜c***, and the proposed control law solves Problem 1 for the ASV vehicle.*

#### **Proof.** Appendix A.
