Chemical Spill Encircling Using a Quadrotor and Autonomous Surface Vehicles: A Distributed Cooperative Approach

Abstract

This article addresses the problem of formation control of a quadrotor and one (or more) marine vehicles operating at the surface of the water with the end goal of encircling the boundary of a chemical spill, enabling such vehicles to carry and release chemical dispersants used during ocean cleanup missions to break up oil molecules. Firstly, the mathematical models of the Medusa class of marine robots and quadrotor aircrafts are introduced, followed by the design of single vehicle motion controllers that allow these vehicles to follow a parameterised path individually using Lyapunov-based techniques. At a second stage, a distributed controller using event-triggered communications is introduced, enabling the vehicles to perform cooperative path following missions according to a pre-defined geometric formation. In the next step, a real-time path planning algorithm is developed that makes use of a camera sensor, installed on-board the quadrotor. This sensor enables the detection in the image of which pixels encode parts of a chemical spill boundary and use them to generate and update, in real time, a set of smooth B-spline-based paths for all the vehicles to follow cooperatively. The performance of the complete system is evaluated by resorting to 3-D simulation software, making it possible to visually simulate a chemical spill. Results from real water trials are also provided for parts of the system, where two Medusa vehicles are required to perform a static lawn-mowing path following mission cooperatively at the surface of the water.

1. Introduction

The problems of perimeter detection, boundary searching, and encircling have been widely researched topics with a variety of practical applications, ranging from the monitoring of wildfire spread in forests [1], to the control and encircling of oil spills [2] and harmful invasive algae blooms [3] at the surface of the ocean. In this paper, we will focus on the problem of chemical spill encircling.

The two main phenomena that contribute to the transportation and spread of hazardous chemicals over water, such as oil, are advection and diffusion. In the first, the chemical is transported due to the flow of water, while the second refers to the motion of the fluid caused by the existence of concentration gradients. One way of modelling the flow field of the incompressible fluid is by solving iteratively the convection–diffusion equations [4]. In the literature, many works address the problem of dynamic boundary tracking at the surface of the ocean by proposing control schemes which require (at least one) surface vessel to measure the concentration gradient of a hazardous contaminant. These measurements of the chemical plume are used by potential field controllers with the end goal of steering the robots to the boundary of the plume [5,6]. A completely different approach, adopted by Saldaña et al. [7], is to consider that a general environmental boundary can be approximated by a closed curve that is slowly varying over time and that can be described by a general parametric equation. In his research, the author proposes a model for the curve described spatially by a truncated Fourier series that changes its shape smoothly over time. To achieve this, it is assumed that a team of Autonomous Surface Vehicles (ASVs) are distributed equally around the chemical spill, and every vehicle is capable of taking local measurements of the boundary as it moves around it. These local measurements are then used to update the shape of the closed curve using recursive least squares. Although this is a very general solution to the problem, it can be argued that the use of a truncated Fourier series to represent a path for underactuated vehicles to follow is a rather poor choice of function, as the resulting curve can self-intersect and exhibit substantial oscillations. Moreover, it does not take into consideration the physical constraints imposed by the vehicles. In order to lift the limitations imposed by this method, more stable parametric curves could be considered, such as Bernstein polynomials or B-splines [8].

In recent years, there has been a massive development of and demand for Autonomous Underwater Vehicles (AUVs), due not only to their agility when it comes to the execution of scientific and comercial missions, but also to their low cost when compared to traditional ships, which require an on-board crew to be operated. Additionally, there has also been an exponential growth in demand for Unmanned Aerial Vehicles (UAVs), with a special emphasis on multirotor systems, which usually offer high-quality camera sensors at low market prices. Aerial vehicles can have a top-down view of the environment, making them the tool par excellence for surveillance and maintenance missions. On the other hand, AUVs and ASVs can be used to carry and release chemical dispersants used in cleanup missions to break oil molecules [9]. Together, these unmanned vehicles have huge potential to automate and reduce the cost of ocean cleanup operations.

In this paper, we address the problem of chemical spill encircling and focus on the development of a set of control and path planning tools that allow a team of robots constituted of a quadrotor (equipped with an onboard camera) and ASVs to detect and encircle the dynamical boundary of a chemical spill closely, as depicted in Figure 1. In our proposal, the quadrotor is responsible for detecting in real time the boundary of a chemical spill in the image stream produced by its onboard camera, and producing a path that itself and one or more ASVs are required to follow cooperatively. To achieve this, we start by proposing a set of single-vehicle motion control laws based on non-linear Lyapunov techniques that allow individual ASVs to follow a pre-defined parametric curve, based on previous works by Aguiar et al. [10,11,12]. These control techniques are then extended to the case of quadrotor vehicles. Borrowing from the work of N. Hung and F. Rego [13], a distributed controller using event-triggered communications is presented, allowing the vehicles to perform Cooperative Path Following (CPF) missions, according to a pre-defined geometric formation. Finally, a new real-time path planning framework that uses growing unclamped (and uniform) cubic B-splines is proposed, which fits a 2-D point cloud generated from the drone’s image stream.

A set of real experiments are performed with the Medusa class of marine vehicles [14] (property of ISR-DSOR) to access the real-life performance of the proposed path following and CPF algorithms. Additionally, the complete path planning solution is evaluated by resorting to the Gazebo 3-D simulator, PX4-SITL [15], and UUVSimulator [16], using a dynamic model of a Medusa vehicle and an Iris quadrotor equipped with a virtual RGB camera.

2. Preliminaries

2.1. Notation

The unit vector ${\mathbf{e}}_3$ is defined as ${\mathbf{e}}_3={[0,0,1]}^T$. For a vector $\mathbf{x}\in {\mathbb{R}}^n$, the symbol $x_i$ denotes the ith element of the vector. We shall use $∥\mathbf{x}∥=\sqrt{{\mathbf{x}}^T\mathbf{x}}$ to denote the Euclidean norm of a vector. The notation $K⪰0$ is used to denote a matrix $K\in {\mathbb{R}}^{n\times n}$ that is positive semi-definite. The symbol I is used to denote the identity matrix and $\mathbf{1}$ is a vector with all elements equal to one. The symbols $\lfloor x\rceil ,x\in \mathbb{R}$ denote the x nearest integer, $\lfloor x\rfloor$ denotes the floor of x, and $\lceil x\rceil$ denotes the ceiling of x. The symbol $R(.)$ is used to denote a rotation matrix with properties $R^T=R^{-1}$ and $det\left(R\right)=1$. The map $S(·):{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n},n=2,3$ yields a skew-symmetric matrix $S\left(\mathbf{x}\right)\mathbf{y}=\mathbf{x}\times \mathbf{y},\forall \mathbf{x},\mathbf{y}\in {\mathbb{R}}^n$. When considering an estimator for an unknown variable x, we use the hat nomenclature $\widehat{x}$ to denote its estimate and $\widehat{x}$ when referring to the estimation error.

2.2. Graph Theory

A weighted digraph $\mathcal{G}=\mathcal{G}(\mathcal{V},\mathcal{E},\mathcal{A})$ consists of a set of N vertices $\mathcal{V}={[V_1,...,V_N]}^T$, a set of directed edges $\mathcal{E}\subseteq \mathcal{V}\times \mathcal{V}$, and a weighted adjacency matrix $\mathcal{A}=\left[a_{ij}\right]\in {\mathbb{R}}^{N\times N}$, such that $a_{ij}>0$ if the edge that connects vertex i to j belongs to the graph, and 0 otherwise. The set of in-neighbours of a vertex i is given by ${\mathcal{N}}_i^{in}=\{j\in \mathcal{V}:(j,i)\in \mathcal{E}\}$, and the set of out-neighbours by ${\mathcal{N}}_i^{out}=\{j\in \mathcal{V}:(i,j)\in \mathcal{E}\}$. The in- and out-degree matrices $D^{in}$ and $D^{out}$ are a set of diagonal matrices defined by:

$$D^{in/out}=diag\left(d_i^{in/out}\right),\mathrm{with}{d}_i^{in}=\sum _{j\in {\mathcal{N}}_i^{in}}{a}_{ij}\mathrm{and}{d}_i^{out}=\sum _{j\in {\mathcal{N}}_i^{out}}{a}_{ji}.$$

(1)

A graph $\mathcal{G}$ is undirected if communication links are unidirectional. If $\mathcal{G}$ is an undirected graph, then $\mathcal{G}$ is also balanced, i.e., $D^{in}=D^{out}:=D$, and its Laplacian matrix L is symmetric, positive semi-definite, and defined according to $L:=(D-\mathcal{A})$. In these conditions, it is well known that L has a simple eigenvalue at zero associated with eigen vector $\mathbf{1}$, with the remaining eigen values being positive. Moreover, $L\mathbf{1}=\mathbf{0}$.

Remark: With the graph definition given above, we adopt the convention that an agent i can receive information from its neighbors in ${\mathcal{N}}_i^{in}$ and send information to its neighbors in ${\mathcal{N}}_i^{out}$.

2.3. Uniform B-Spline Curves

A 2-D B-spline curve of degree k + 1 in ${\mathbb{R}}^2$ is a piecewise polynomial function formed by several components of degree k, defined as:

$$C\left(\gamma \right)=\sum _{i=0}^n{B}_{i,k}\left(\gamma \right)P_i,$$

(2)

where $\mathcal{P}=\{P_i\in {\mathbb{R}}^2,i=0,...,n\}$ are a set of control points and $B_{i,k}\left(\gamma \right)$ are the B-spline basis functions. It follows from the Cox–De Boor’s recursive algorithm, according to L. Piegl and W. Tiller ([8], Chapter 2.2), that:

$$B_{i,0}\left(\gamma \right)=\left\{\begin{array}{c}1,\mathrm{if}{\gamma }_i\le \gamma \le {\gamma }_{i+1}\hfill \\ 0,\mathrm{otherwise}\hfill \end{array}\right.,$$

(3)

$$B_{i,j}\left(\gamma \right)=\frac{\gamma -{\gamma }_i}{{\gamma }_{i+j}-{\gamma }_i}{B}_{i,j-1}\left(\gamma \right)+\frac{{\gamma }_{i+j+1}-\gamma }{{\gamma }_{i+j+1}-{\gamma }_{i+1}}{B}_{i+1,j-1}\left(\gamma \right),$$

(4)

where the index $j=0,...,k$ and the values ${\gamma }_i$ belong to the m-dimensional knot vector $\mathbf{U}={\left\{{\gamma }_i\right\}}_{i=0}^m$, with the number of knots related to the degree of the curve and the number of control points by $m=k+1+n$.

For the particular case of 2-D, uniform, non-clamped cubic B-splines with $n-k+1$ segments, each segment’s x- and y-coordinates of the parametric curve can be described according to the vectorial notation [17] as follows:

$$C_i\left(\gamma \right):={\left[\begin{array}{cc}{C}_i^x\left(\gamma \right)& C_i^y\left(\gamma \right)\end{array}\right]}^T,$$

(5)

with $C_i^x\left(\gamma \right)$ and $C_i^y\left(\gamma \right)$ computed according to:

$$C_i^{x/y}\left(\gamma \right):=\underset{\left[\begin{array}{cccc}{B}_{i,3}\left(\gamma \right)& B_{i+1,3}\left(\gamma \right)& B_{i+2,3}\left(\gamma \right)& B_{i+3,3}\left(\gamma \right)\end{array}\right]}{\underbrace{\frac{1}{6}\left[\begin{array}{cccc}{(\gamma -i)}^3& {(\gamma -i)}^2& (\gamma -i)& 1\end{array}\right]\left[\begin{array}{cccc}-1& 3& -3& 1\\ 3& -6& 3& 0\\ -3& 0& 3& 0\\ 1& 4& 1& 0\end{array}\right]}}\left[\begin{array}{c}{P}_i^{x/y}\\ P_{i+1}^{x/y}\\ P_{i+2}^{x/y}\\ P_{i+3}^{x/y}\end{array}\right],$$

(6)

where $\gamma \in [0,n-k+1]$ and $i:=\lfloor \gamma \rfloor$, such that $\gamma -i\in [0,1]$ and each curve segment is only defined by four distinct control points. Defining a unidimensional vector with all control points $\mathbf{P}={[P_0^x,...,P_n^x,P_0^y,...,P_n^y]}^T\in {\mathbb{R}}^{2(n+1)}$, where both x- and y-coordinates are concatenated, and a vector of distinct curve parameters $\gamma =[{\gamma }_0,...,{\gamma }_q]\in {\mathbb{R}}^{q+1}$ at which we wish to evaluate our curve, $\mathbf{C}\left(\gamma \right)\in {\mathbb{R}}^{2(q+1)}$ is given by:

$$\mathbf{C}\left(\gamma \right)=B\left(\gamma \right)·\mathbf{P},$$

(7)

where $B\left(\gamma \right)\in {\mathbb{R}}^{2(q+1)\times 2(n+1)}$ is a diagonal by blocks matrix, and for each line of B, only four basis functions are different then zero and computed according to (6).

3. Vehicle Modelling

Let $\left\{U\right\}$ denote an inertial reference frame and $\left\{B\right\}$ a body-fixed reference frame attached to the geometric center of mass of each vehicle, according to Figure 2.

3.1. ASV Model

The ASV vehicle is modeled as a rigid body whose motion is restricted to a 2-D plane at the surface of the water, such that the roll and pitch angles are zero, i.e., $\varphi =\theta =0$. Let the kinematic equations of the vehicle be given by:

$$\underset{\dot{\mathbf{p}}}{\underbrace{\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]}}=\underset{{}_B^{U}R\left(\psi \right)}{\underbrace{\left[\begin{array}{cc}cos\left(\psi \right)& -sin\left(\psi \right)\\ sin\left(\psi \right)& cos\left(\psi \right)\end{array}\right]}}\underset{\mathbf{v}}{\underbrace{\left[\begin{array}{c}u\\ v\end{array}\right]}}+\underset{{\mathbf{v}}_c}{\underbrace{\left[\begin{array}{c}{v}_{cx}\\ v_{cy}\end{array}\right]}},$$

(8)

$$\dot{\psi }=r,$$

(9)

where $\mathbf{p}:={[x,y]}^T$ denotes the ASV position expressed in $\left\{U\right\}$, $\mathbf{v}:={[u,v]}^T$ denotes the body-velocity vector, ${}_B^{U}R\left(\psi \right)\in {\mathbb{R}}^{2\times 2}$ denotes the rotation matrix, and $\mathbf{v}:={[v_{cx},v_{cy}]}^T$ denotes the ocean current, expressed in $\left\{U\right\}$ and assumed to be constant, irrotational, and bounded. The ASV model is considered to be underactuated, with the input of the system being given by $\mathbf{u}={[u,r]}^T\in {\mathbb{R}}^2$.

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}}:=\underset{\mathbf{u}}{\underbrace{g{\mathbf{e}}_3-\frac{1}{m}{}_B^{U}R\left(\theta \right)T{\mathbf{e}}_3}}+\mathbf{d},$$

(10)

where $\mathbf{p}:={[x,y,z]}^T$ denotes the quadrotor’s position expressed in $\left\{U\right\}$, $\theta :={[\varphi ,\theta ,\psi ]}^T$ denotes the orientation vector of $\left\{B\right\}$ expressed in $\left\{U\right\}$, and $\mathbf{u}\in {\mathbb{R}}^3$ can be regarded as the input of the system, comprising both the attitude of the vehicle and the total thrust T. The vector $\mathbf{d}\in {\mathbb{R}}^3$ represents unmeasured external disturbances, such as wind, acting on the vehicle, assumed to be constant and bounded such that $∥\mathbf{d}∥\le d_{max}$. The 3-D rotation matrix adopted for the quadrotor model follows the Z-Y-X convention, and is given by:

$${}_B^{U}R(\theta )=R_z\left(\psi \right)R_y\left(\theta \right)R_x\left(\varphi \right).$$

(11)

4. Path Following

The path following (PF) problem concerns the problem of making a vehicle move along a desired path ${\mathbf{p}}_d\left(\gamma \right)$ parameterised by a variable $\gamma$ (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 $v_d\left(\gamma \right)$, 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\left(\gamma \right):=v_L\left(\gamma \right)+v^{coord},\mathrm{with}\left|v_L\left(\gamma \right)\right|\le v_L^{max},$$

(12)

where $v_L\left(\gamma \right)$ is a desired speed profile defined only as a function of the path, $v_L^{max}$ is a pre-defined speed upper-bound, and $v^{coord}$ 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 $v_L\left(\gamma \right)$ 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 $v^{coord}$ 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 $v_d\left(\gamma \right)$ 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.

Problem 1.

Given a generic vehicle (ASV or quadrotor), consider the geometric path ${\mathbf{p}}_d\left(\gamma \right):[0,\infty ]\to {\mathbb{R}}^2/{\mathbb{R}}^3$ for the ASV/quadrotor respectively, parameterised by a continuous variable $\gamma \in \mathbb{R}$ and $v_d(\gamma ,t)\in \mathbb{R}$ a desired speed profile for a virtual target moving along the desired path. Furthermore, consider ${\mathbf{p}}_d\left(\gamma \right)$ to be $C^2$ 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 ${\mathbf{u}}_d\in {\mathbb{R}}^2/{\mathbb{R}}^3$, 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 ${\mathbf{u}}_d$ and virtual target $\ddot{\gamma }$ such that:

• the vehicle’s position converges to a tube around the desired position that can be made arbitrarily small, i.e., $∥\mathbf{p}\left(t\right)-{\mathbf{p}}_d\left(\gamma \right)∥$ converges to a neighbourhood of the origin;
• the speed of the virtual target moving along the path converges to the desired speed profile, i.e., $|\dot{\gamma }-v_d(\gamma ,t)|\to 0$ as $t\to \infty$.

4.1. ASV Path Following

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

$${\mathbf{e}}_p\left(t\right):={}_U^{B}R\left(\psi \right)(\mathbf{p}\left(t\right)-{\mathbf{p}}_d\left(\gamma \right)),$$

(13)

and let the speed-tracking error be defined as:

$$e_{\gamma }:=\dot{\gamma }-v_d(\gamma ,t).$$

(14)

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

$${\dot{\mathbf{e}}}_p\left(t\right)={}_U^B\dot{R}\left(\psi \right)(\mathbf{p}\left(t\right)-{\mathbf{p}}_d\left(\gamma \right))+{}_U^{B}R\left(\psi \right)(\dot{\mathbf{p}}\left(t\right)-{\dot{\mathbf{p}}}_d\left(\gamma \right)).$$

(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:

$${}_U^B\dot{R}\left(\psi \right)=-S\left(r\right){}_U^{B}R\left(\psi \right).$$

(16)

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

$${\dot{\mathbf{e}}}_p\left(t\right)=-S\left(r\right)\underset{{\mathbf{e}}_p\left(t\right)}{\underbrace{{}_U^{B}R\left(\psi \right)(\mathbf{p}\left(t\right)-{\mathbf{p}}_d\left(\gamma \right))}}+\mathbf{v}+\underset{{\mathbf{v}}_c}{\underbrace{{}_U^{B}R\left(\psi \right){\mathbf{v}}_c}}-{}_U^{B}R\left(\psi \right)\frac{\partial {\mathbf{p}}_d\left(\gamma \right)}{\partial \gamma }\dot{\gamma }.$$

(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 $\delta ={[0,\delta ]}^T\in {\mathbb{R}}^2$ (with $\delta <0$) in the standard position error, it is possible to re-write (17) as:

$${\dot{\mathbf{e}}}_p\left(t\right)=-S\left(r\right)({\mathbf{e}}_p-\delta )+\underset{\Delta }{\underbrace{\left[\begin{array}{cc}1& 0\\ 0& -\delta \end{array}\right]}}\underset{\mathbf{u}}{\underbrace{\left[\begin{array}{c}u\\ r\end{array}\right]}}+\left[\begin{array}{c}0\\ v\end{array}\right]+{\mathbf{v}}_{\mathbf{c}}-{}_U^{B}R\left(\psi \right)\frac{\partial {\mathbf{p}}_d\left(\gamma \right)}{\partial \gamma }\dot{\gamma }.$$

(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 $\mathbf{v}$, expressed in $\left\{B\right\}$, and a Global Positioning System (GPS) unit which provides measurements of the position of the vehicle $\mathbf{p}$, expressed in $\left\{U\right\}$. 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:

$$\mathcal{F}:=\left\{\begin{array}{c}\dot{\widehat{\mathbf{p}}}=k_1(\mathbf{p}-\widehat{\mathbf{p}})+{}_B^{U}R\left(\psi \right)\mathbf{v}+{\widehat{\mathbf{v}}}_c\hfill \\ {\dot{\widehat{\mathbf{v}}}}_c=k_2(\mathbf{p}-\widehat{\mathbf{p}}),\hfill \end{array}\right.$$

(19)

with $k_1$ and $k_2$ 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 ${\mathbf{v}}_c$ and the requested input ${\mathbf{u}}_d$ 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{array}{cc}\hfill {\tilde{v}}_c& :=v_c-{\widehat{v}}_{\mathbf{c}},\hfill \\ \hfill \tilde{\mathbf{u}}& :=\mathbf{u}-{\mathbf{u}}_d.\hfill \end{array}$$

(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-\delta )-\left[\begin{array}{c}0\\ v\end{array}\right]-{\widehat{v}}_c+{}_U^{B}R\left(\psi \right)\frac{\partial {\mathbf{p}}_d\left(\gamma \right)}{\partial \gamma }{v}_d(\gamma ,t)\right),$$

(21)

$$\ddot{\gamma }:=-k_{\gamma }{e}_{\gamma }+{\dot{v}}_d(\gamma ,t)+{({\mathbf{e}}_p-\delta )}^T{}_U^{B}R\left(\psi \right)\frac{\partial {\mathbf{p}}_d\left(\gamma \right)}{\partial \gamma },$$

(22)

where $K_p⪰0$, $k_{\gamma }>0$, and $\sigma \left({\mathbf{e}}_p\right)=tanh\left(∥{\mathbf{e}}_p∥\right)\frac{{\mathbf{e}}_p}{∥{\mathbf{e}}_p∥}$ is a saturation function. The closed-loop system is input-to-state stable (ISS) with respect to $\Delta \tilde{\mathbf{u}}+{\tilde{v}}_{\mathbf{c}}$, and the proposed control law solves Problem 1 for the ASV vehicle.

Proof. 

Appendix A.    □

4.2. Quadrotor Path Following

Given that the quadrotor system is modelled by a double integrator in the inertial frame $\left\{U\right\}$, as stated in (10), consider the position and velocity errors defined in $\left\{U\right\}$ as:

$${\mathbf{e}}_p:=\mathbf{p}\left(t\right)-{\mathbf{p}}_d\left(\gamma \right),$$

(23)

$${\mathbf{e}}_v:=\dot{\mathbf{p}}-\frac{\partial {\mathbf{p}}_d}{\partial \gamma }{v}_d(\gamma ,t),$$

(24)

and a virtual target speed tracking error defined by (14). Consider also a new auxiliary error $\mathbf{z}$, defined as:

$$\mathbf{z}:={\mathbf{e}}_v+K_1{\mathbf{e}}_p,$$

(25)

where $K_1⪰0$ is a gain matrix. The position and velocity error dynamics can be written as:

$${\dot{\mathbf{e}}}_p=\dot{\mathbf{p}}-\frac{\partial {\mathbf{p}}_d}{\partial \gamma }\dot{\gamma },$$

(26)

$${\dot{\mathbf{e}}}_v=\ddot{\mathbf{p}}-\frac{d}{dt}\left(\frac{\partial {\mathbf{p}}_d}{\partial \gamma }{v}_d(\gamma ,t)\right).$$

(27)

Furthermore, consider the time derivative introduced in (27), the desired virtual target speed function (12), and the virtual target speed tracking error function (14). Then, the time derivative term introduced in (27) can be expanded as:

$$\begin{array}{cc}\hfill \frac{d}{dt}\left(\frac{\partial {\mathbf{p}}_d}{\partial \gamma }{v}_d(\gamma ,t)\right)& =\left[\underset{\mathbf{h}\left(\gamma \right)}{\underbrace{\frac{{\partial }^2{\mathbf{p}}_d}{\partial {\gamma }^2}{v}_d(\gamma ,t)+\frac{\partial {\mathbf{p}}_d}{\partial \gamma }\frac{\partial v_L\left(\gamma \right)}{\partial \gamma }}}\right](e_{\gamma }+v_d(\gamma ,t))+\frac{\partial {\mathbf{p}}_d}{\partial \gamma }{\dot{v}}^{coord}\left(t\right).\hfill \end{array}$$

(28)

Replacing (10) and (28) in (27) yields:

$${\dot{\mathbf{e}}}_v=\mathbf{u}+\mathbf{d}-\mathbf{h}\left(\gamma \right)(e_{\gamma }+v_d(\gamma ,t))-\frac{\partial {\mathbf{p}}_d}{\partial \gamma }{\dot{v}}^{coord}\left(t\right).$$

(29)

Unlike the case of the ASVs where current estimates are given by a complementary filter, in the case of a quadrotor, a different direction is taken towards estimating disturbances such as wind. According to Xie and Cabecinhas et al. [20,21], straightforward implementations of estimators can lead to windup and result in unbounded growth of an external disturbance estimate. To avoid such problems, Xie and Cabecinhas propose the use of a sufficiently smooth projection operator in the estimator design. Consider the disturbance observer given by:

$$\dot{\widehat{\mathbf{d}}}:=K_d\mathrm{Proj}(\mathbf{z},\widehat{\mathbf{d}})=\mathbf{z}-\frac{{\eta }_1{\eta }_2}{2{({\beta }^2+2\beta d_{max})}^{n+1}{d}_{max}^2}\widehat{\mathbf{d}},$$

(30)

where $K_d$ denotes a diagonal gain matrix and:

$${\eta }_1=\left\{\begin{array}{c}{({\widehat{\mathbf{d}}}^T\widehat{\mathbf{d}}-d_{max}^2)}^{n+1},\mathrm{if}({\widehat{\mathbf{d}}}^T\widehat{\mathbf{d}}-d_{max}^2)>0\hfill \\ 0,\mathrm{otherwise},\hfill \end{array}\right.$$

(31)

and:

$${\eta }_2={\widehat{\mathbf{d}}}^T\mathbf{z}+\sqrt{{\left({\widehat{\mathbf{d}}}^T\mathbf{z}\right)}^2+{\varsigma }^2},$$

(32)

where $\varsigma ,\beta >0$ are arbitrary constants. This projection operator, first proposed in Cai et al. [22], enjoys the useful properties:

$${\tilde{\mathbf{d}}}^{T}Proj(\mathbf{z},\widehat{\mathbf{d}})\ge {\tilde{\mathbf{d}}}^T\mathbf{z},$$

(33)

and:

$$∥\widehat{\mathbf{d}}∥\le d_{max}+\beta ,\forall t\ge 0.$$

(34)

Once again, consider the inner-loop tracking error and disturbance estimation error given by:

$$\tilde{\mathbf{u}}:=\mathbf{u}-{\mathbf{u}}_d,$$

(35)

$$\tilde{\mathbf{d}}:=\mathbf{d}-\widehat{\mathbf{d}}.$$

(36)

Proposition 2.

Consider the system described by (10), the disturbance estimator dynamics given by (30), and the inner-loop tracking error given by (35). Furthermore, consider the control law given by:

$${\mathbf{u}}_d:=-\widehat{\mathbf{d}}+\mathbf{h}\left(\gamma \right)v_d(\gamma ,t)+\frac{\partial {\mathbf{p}}_d}{\partial \gamma }{\dot{v}}^{coord}\left(t\right)-{\mathbf{e}}_v{K}_v-{\mathbf{e}}_p{K}_p,$$

(37)

$$\ddot{\gamma }:=-k_{\gamma }{e}_{\gamma }+{\dot{v}}_d(\gamma ,t)+{\mathbf{e}}_p^T\frac{\partial {\mathbf{p}}_d}{\partial \gamma }+{\mathbf{z}}^T\left(\mathbf{h}\left(\gamma \right)+K_1\frac{\partial {\mathbf{p}}_d}{\partial \gamma }\right),$$

(38)

where $K_p,K_v⪰0$, and $k_{\gamma }$ is a positive gain. For sufficiently small initial position and velocity errors (${\mathbf{e}}_p$, ${\mathbf{e}}_v$), and a sufficiently large separation between the time-scales of the inner and outer loop systems, it can be guaranteed that the system error converges to a neighbourhood of zero. The proposed control law solves Problem 1 for the quadrotor vehicle.

Remark 2.

In-depth and quantitative overall stability analysis can be conducted for the inner–outer loop control system, but this will be dependent directly on the type of inner loop adopted. This results from the fact that the desired accelerations ${\mathbf{u}}_{\mathbf{d}}$ must be decoupled in a set of desired thrust and attitude for the quadrotor to track. Given that this analysis is out of the scope of this work, we assume that the quadrotor is equipped with a generic inner loop that is capable of keeping the tracking error $\tilde{\mathbf{u}}$ small and bounded.

Proof. 

Appendix B.    □

5. Cooperative Path Following

In this section, the problem of CPF is addressed. The end goal is to have an algorithm that allows one quadrotor and multiple ASVs to perform a path following mission cooperatively, using a distributed architecture. The vehicles are required to execute their mission according to a fixed geometric configuration. To cope with limitations imposed by real environments where inter-vehicle communications are discrete, an Event-Triggered Communications (ETC) mechanism is adopted, based on previous work developed by A. Aguiar and A. Pascoal [23] and N. Hung and F. Rego [13].

Synchronisation Problem with Event-Triggered Communications

Consider a group of $N\in {\mathbb{R}}^{+}\backslash \left\{1\right\}$ autonomous vehicles/agents in a network that can be described mathematically by a digraph $\mathcal{G}(\mathcal{V},\mathcal{E},\mathcal{A})$, consisting of N vertices, a set of directed edges $\mathcal{E}\subseteq \mathcal{V}\times \mathcal{V}$, where the edge ${\epsilon }_{ij}$ represents the flow of information from agent i to agent j, and a weighted adjacency matrix $\mathcal{A}=\left[a_{ij}\right]\in {\mathbb{R}}^{N\times N}$. Furthermore, each vehicle i is able to receive information from its neighbours in ${\mathcal{N}}_i^{in}$ and send information to its neighbours in ${\mathcal{N}}_i^{out}$, i.e., $\mathcal{G}$ is undirected. Moreover, consider that the communication topology of the vehicles is fixed; hence, the Laplacian L associated to $\mathcal{G}$ is constant. Let the state vector of the system be composed by the path parameter of each individual vehicle $\gamma ={[{\gamma }_1,...,{\gamma }_N]}^T$. In addition, each vehicle is equipped with the PF controllers proposed in Section 4, and has an assigned path to follow, appropriately parameterised in order to ensure that a given desired formation between the vehicles is met. The CPF problem consists in designing a distributed control scheme that adjusts the speed of the vehicles such that all path parameters $\gamma$ reach a consensus. Consider the problem formulation below.

Problem 2.

For each agent i, with $i=1,...,N$, derive a consensus protocol for the speed correction term ${\mathbf{v}}^{\mathbf{coord}}={[v_1^{coord},...,v_N^{coord}]}^T$, such that ${lim}_{t\to \infty }|{\gamma }_i-{\gamma }_j|=0$, $\forall j\in N_i^{in}$, and the formation of vehicles achieves the desired speed assignment ${\mathbf{v}}_{\mathbf{L}}\left(\gamma \right)={[v_{L1},...,v_{LN}]}^T$ as $t\to \infty$.

Note that, according to the previously developed (PF) controllers, for each vehicle i, $|{\dot{\gamma }}_i-v_d(\gamma ,t)|=0$ is only guaranteed as $t\to \infty$, as the controlled variable is ${\ddot{\gamma }}_i$ and not ${\dot{\gamma }}_i$. Having this fact in mind, and assuming that the vehicles have already converged to their desired paths, i.e., $e_p\approx 0$ (and $e_v\approx 0$ in the case of the quadrotor), then the following simplifying assumption can be made:

Assumption 1.

The speed progression of all the virtual targets along the desired path is always assumed to be modelled by a single integrator system, which can be expressed in vectorial form as:

$$\dot{\gamma }={\mathbf{v}}_{\mathbf{d}}(\gamma ,t)={\mathbf{v}}_{\mathbf{L}}\left(\gamma \right)+{\mathbf{v}}^{\mathbf{coord}}.$$

(39)

Let the synchronisation error vector be defined as $\epsilon ={[{\epsilon }_1,...,{\epsilon }_N]}^T$ where, for each i:

$${\epsilon }_i:=\sum _{j\in {\mathcal{N}}_i^{in}}{a}_{ij}({\gamma }_i-{\gamma }_j),$$

(40)

with $a_{ij}$ elements of the weighted adjacency matrix that describes the vehicle network. This error can also be expressed in vectorial form as:

$$\epsilon :=L\gamma ,$$

(41)

where ${\epsilon }_i$ denotes the coordination error between vehicle i and its neighbours. With the above notation, the coordination error dynamics of the multi-vehicle system are given by:

$$\dot{\epsilon }:=L\dot{\gamma }.$$

(42)

In the work of N. Hung and F. Rego [13], the authors propose a scheme where each agent i has a set of estimators ${\widehat{\gamma }}_j,j\in {\mathcal{N}}_i^{in}$ for the true state of each in-neighbour virtual target ${\gamma }_j$. In addition, each agent i has an estimator for its own state ${\widehat{\gamma }}_i$, which is reset whenever vehicle i broadcasts its true state ${\gamma }_i$. The other estimators are reset whenever agent i receives the true state from its in-neighbours $j\in {\mathcal{N}}_i^{in}$. In this work, a time-dependent broadcast condition is adopted.

Proposition 3.

Consider the distributed control law given by:

$$v_i^{coord}:=-k_{\epsilon }\sum _{j\in {\mathcal{N}}_i^{in}}{a}_{ij}({\gamma }_i-{\widehat{\gamma }}_j),$$

(43)

where $k_{\epsilon }>0$ and ${\widehat{\gamma }}_j$ is vehicle i’s estimate of vehicle j’s real virtual target value. Consider also that the bank of estimators that each vehicle i is running is described by the dynamics equation:

$${\dot{\widehat{\gamma }}}_i:=v_L\left({\widehat{\gamma }}_i\right).$$

(44)

At any time instant t, under negligible transmission delays, the vehicle j’s self-state estimate ${\widehat{\gamma }}_j$ is equal to vehicle i’s estimate of ${\widehat{\gamma }}_j$, which allows us to express the estimator dynamics using vectorial notation as:

$$\dot{\widehat{\gamma }}:={\mathbf{v}}_{\mathbf{L}}\left(\widehat{\gamma }\right),$$

(45)

where $\widehat{\gamma }={[{\widehat{\gamma }}_1,...,{\widehat{\gamma }}_N]}^T$ is the self-estimate of the virtual target of each vehicle. Let $\tilde{\gamma }={[{\tilde{\gamma }}_1,...,{\tilde{\gamma }}_N]}^T$ denote the local estimation errors of each vehicle, such that $\tilde{\gamma }=\gamma -\widehat{\gamma }$. Then, ${\mathbf{v}}^{\mathbf{coord}}$ can also be given in vectorial notation, according to:

$${\mathbf{v}}^{\mathbf{coord}}:=-k_{\epsilon }[D\gamma -\mathcal{A}\widehat{\gamma }]=-k_{\epsilon }(\epsilon +\mathcal{A}\tilde{\gamma }),$$

(46)

where D is a diagonal matrix and A the graph adjacency matrix. Consider also a triggering function used to define when to broadcast the along-path position of the virtual target of each vehicle, defined as:

$$\left\{\begin{array}{c}{\delta }_i\left(t\right):=\left|{\tilde{\gamma }}_i\left(t\right)\right|-g_i\left(t\right)\hfill \\ {\tilde{\gamma }}_i\left(t\right)={\widehat{\gamma }}_i\left(t\right)-{\gamma }_i\left(t\right),\hfill \end{array}\right.$$

(47)

where ${\tilde{\gamma }}_i\left(t\right)$ is the local estimation error of agent i and $g_i\left(t\right)$ is a time-dependent threshold function, such that if the estimation error exceeds this threshold, i.e., ${\delta }_i\left(t\right)\ge 0$, vehicle i broadcasts its state to the out-neighbours ${\mathcal{N}}_i^{out}$ and resets its local estimator. Furthermore, consider $g_i\left(t\right)$ to belong to a class of non-negative functions, given by:

$$g_i\left(t\right)=c_i+b_i{e}^{-{\alpha }_{i}t},$$

(48)

with $c_i$, $b_i$ and ${\alpha }_i$ being positive constants and $\mathbf{g}\left(t\right)={[g_1,...,g_N]}^T$ being the collection of functions $g_i$ for each individual vehicle i. Consider also that ${\mathbf{v}}_{\mathbf{L}}\left(\gamma \right)=v_L\mathbf{1}+{\tilde{\mathbf{v}}}_{\mathbf{L}}$, where ${\tilde{\mathbf{v}}}_{\mathbf{L}}$ is a bounded and arbitrarily small term that accounts for a transient period in which the vehicles are on different sections of the path, with slightly different desired speed profiles. Then, under Assumption 1, the system is ISS with respect to the error vector ε and the inputs $\tilde{\gamma }$ and ${\tilde{\mathbf{v}}}_{\mathbf{L}}$.

Proof. 

Appendix C.    □

The proposed control scheme used for achieving CPF using ETC is summarised in Algorithm 1.

Algorithm 1 Event-Triggered Communication for vehicle i (adapted from [24]).

1: At every time instant t, each vehicle i follows the procedure: 2: procedure Coordination and Communication 3:     if ${\delta }_i\left(t\right)\ge 0$ where ${\delta }_i$ is computed using (47) and (48) then 4:         Broadcast ${\gamma }_i\left(t\right)$; 5:         Reset the estimator ${\widehat{\gamma }}_{\mathbf{i}}$; 6:     if Receive a new message from agent $j\in {\mathcal{N}}_i^{in}$ then 7:         Reset ${\widehat{\gamma }}_j\left(t\right)$; 8:     Run the estimators according to (44); 9:     Update the first order control protocol ${\mathit{u}}_i$ using (43).

Given the general distributed control scheme, we now elaborate and address a specific formation, in the context of this work, in the sections that follow.

6. Path Planning

This section addresses the problem of generating a set of smooth and planar reference paths for each individual vehicle to follow, with the end goal of encircling the boundary of a chemical spill. In order to make the vehicles follow the dynamic boundary according to a pre-defined formation (such as a triangle) multiple paths should be generated from one reference path that encodes the boundary. Borrowing from the work of Saldaña et al. [7], we start by presenting a rigorous mathematical definition of a dynamic boundary below.

Definition 1.

A dynamic boundary is a set of planar points ${\Omega }_t$, such that $\forall z\in {\Omega }_t$, and for any $\xi >0$, the open disk centered at point z with radius ξ contains points of ${\Omega }_t$ and its complement set ${\Omega }_t^C$. Moreover, the dynamic boundary can be approximated by a parametric closed curve (Jordan curve) $\mathbf{C}(\gamma ,t):[0,\infty ]\times [0,\infty ]\to {\mathbb{R}}^2$, mapped by a parameter $\gamma \in {\mathbb{R}}_0^{+}$ and time $t\in {\mathbb{R}}_0^{+}$. The curve is continuous with no self-intersecting points, and changes smoothly with respect to both time t and parameter γ, as depicted in Figure 4a.

Since the chemical spill boundary is assumed to be dynamic, a path planning problem can be formulated in which a quadrotor is actively re-planning the path that the ASVs should follow at the water surface, as the group of vehicles moves along it and more up-to-date data is acquired by the quadrotor’s vision system. Consider, therefore, Problem 3.

Problem 3.

Consider a quadrotor flying over a body of water at a pre-defined fixed altitude, equipped with a camera sensor pointing downwards with a fixed pitch angle relative to the vehicle’s body reference frame $\left\{B\right\}$. Consider also that the vehicle is capable of detecting the boundary of a chemical spill in the 2-D image provided by the camera sensor. Furthermore, one or more ASVs at the surface of the water are required to follow a path dictated by the quadrotor, according to a pre-defined vehicle formation. As the quadrotor detects the dynamic boundary in the image:

1

use the data provided by its navigation system to convert the pixels to a 2-D point cloud expressed in the inertial frame $\left\{U\right\}$;

2

remove outliers and perform pre-processing on the 2-D point cloud;

3

generate a smooth and planar reference path by formulating an online optimisation problem that fits the data with open uniform B-splines;

4

send the updated path to the vehicle network;

5

make each vehicle generate an unique path for itself, capturing the pre-defined vehicle formation;

6

repeat the process.

In order to solve Problem 3, a few simplifying assumptions are made:

Assumption 2.

The dynamic boundary is located at the ocean’s surface, assumed to be a 2-D plane at $Z_U=0$ in the inertial frame of reference $\left\{U\right\}$.

Assumption 3.

The quadrotor has a navigation system that can track the vehicle’s pose with good accuracy.

Assumption 4.

The quadrotor has a limited field of view of the environment, i.e, the camera sensor might not be able to capture the entire chemical spill boundary, but rather sections of it, according to Figure 4b.

Assumption 5.

The detection of the pixels that encode the boundary in the image frame is a sub-system that is assumed to be already available, such as the one proposed in [25].

6.1. Planar Point Cloud Generation

The camera model adopted is characterised by: (i) a set of extrinsic parameters, which model the conversion between coordinates expressed in the world/inertial reference frame $\left\{U\right\}$ and the camera reference frame $\left\{C\right\}$; (ii) intrinsic parameters which describe how a set of points in $\left\{C\right\}$ are represented in the image frame, according to Figure 5.

The intrinsic parameters consist of the focal distance $f_d$, the scale factors ($s_x$, $s_y$) in the X- and Y-axis, respectively, and ($c_x$, $c_y$), which corresponds to the offset of the focal point in the image plane. These parameters can be obtained a priori by resorting to a camera calibration process, described in detail in [26]. Combining the matrices of intrinsic parameters K, also known as the full-rank calibration matrix, and the matrix of external parameters ${}_U^C\left[R\right|T]$, and expressing the inertial frame coordinates as homogeneous coordinates, the transformation between inertial frame and camera plane is described by:

$$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\underset{K}{\underbrace{\left[\begin{array}{ccc}{f}_d{s}_x& 0& c_x\\ 0& f_d{s}_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]}}\underset{{}_U^C\left[R\right|T]}{\underbrace{\left[\begin{array}{cc}{}_U^{C}R& {}_U^C\mathbf{T}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]}}\left[\begin{array}{c}{X}_U\\ Y_U\\ Z_U\\ 1\end{array}\right],$$

(49)

where x and y denote the coordinates in the image frame and $\lambda$ is a scale factor. It is important to mention that ${}_U^C\left[R\right|T]$ results from a series of successive rigid-body transformations (rotations and translations) given by:

$${}_U^C\left[R\right|T]={}_B^C\left[R\right|T]{}_U^B\left[R\right|T],$$

(50)

where ${}_U^B\left[R\right|T]$ denotes the conversion of coordinates expressed in the inertial frame $\left\{U\right\}$ to the quadrotor’s body frame $\left\{B\right\}$, provided by its navigation system, and ${}_B^C\left[R\right|T]$ is a matrix known a priori, as the camera attached to the vehicle is assumed to be fixed. The intrinsic and extrinsic parameters can be aggregated in a matrix $\Omega$ according to:

$$\Omega =K·{}_U^C\left[R\right|T].$$

(51)

In order to convert a given set of pixels $(x,y)$ that encode the chemical spill boundary in the image frame to a point cloud expressed in the inertial frame, depth information about the scene is required. Taking into consideration Assumption 2, all the points in the inertial frame will lie on the plane described by $Z_U=0$, which solves the depth requirement. Moreover, from Assumption 3, it can be concluded that the linear system of Equation (49) is well defined and can be inverted such that for each pixel representing the boundary of the chemical spill, $X_U$ and $Y_U$ are extracted from:

$$\frac{1}{\lambda }\left[\begin{array}{c}{X}_U\\ Y_U\\ 1\end{array}\right]={\left[\begin{array}{ccc}{\Omega }_1& {\Omega }_2& {\Omega }_4\\ {\Omega }_5& {\Omega }_6& {\Omega }_8\\ {\Omega }_9& {\Omega }_{10}& {\Omega }_{12}\end{array}\right]}^{-1}\left[\begin{array}{c}x\\ y\\ 1\end{array}\right].$$

(52)

Remark 3.

This methodology relies heavily on the assumption that the quadrotor has a good navigation system, since small estimation errors in the altitude of the vehicle can lead to errors of several meters in the generated point cloud.

6.2. Pre-Processing the Planar Point Cloud

Before using the 2-D point cloud to generate a path, it is important to pre-process the information provided in it. Consider, for instance, the example in Figure 6, where the quadrotor produces a 2-D point cloud, representing the boundary, at an arbitrary time-step $t_k$. In the point cloud, some points represent the chemical spill boundary in a region that is close to the vehicle—the region of interest, i.e., where the main cluster of points is expected to be located (in region B). The separation between regions A and B is defined by drawing a normal to the path at the point where the re-planning starts (defined formally in Section 6.3.1). Some points are outliers as a result of either noisy measurements or regions of the boundary that are not entirely captured by the field of view of the camera. The latter can be seen as disconnected from the main cluster and should be disregarded in the path planning process. According to Figure 6, the original path (in purple) obtained at time $t_{k-1}$ should be re-planned in order to obtain a new one (in red) that better fits the main cluster of points.

Unlike conventional motion planning problems, the main cluster of points does not have an explicit ordering, yielding a sequence of waypoints that the vehicle should visit sequentially in time—this information must be inferred. On the other hand, it is possible to define explicitly where the path re-planning process starts—at a point ${\mathbf{p}}_s:=C\left({\gamma }_s\right)$ arbitrarily further ahead of the drone’s position on the current curve $C\left(\gamma \right)$, such that ${\gamma }_{drone}\le {\gamma }_s$. Motivated by this example, and inspired by the work of Liu Y. et al. [27], the following pre-processing steps are introduced:

• Remove unused points that are behind the point ${\mathbf{p}}_s$, i.e., points in region A;
• Order the remaining set of points and remove outliers in region B.

6.2.1. Removing Unused Points

Consider ${\mathbf{p}}_s\in {\mathbb{R}}^2$ to be the point at which the path re-planning starts. In order to remove the points that are in region A, consider that ${\psi }_s$ is the tangent angle to the current path at ${\mathbf{p}}_s$. A coordinate transformation can be applied to the 2-D point cloud $\mathit{X}:={\left\{{\mathit{X}}_l\right\}}_{l=1}^L\in {\mathbb{R}}^2$, such that in a new reference frame, points that are behind ${\mathbf{p}}_s$ (in region A) have a negative X-coordinate. This coordinate transformation is given by:

$${\mathit{X}}_l^{\circ }=R\left({\psi }_s\right)·({\mathit{X}}_l-{\mathbf{p}}_s),\forall l=1,...,L,$$

(53)

where ${\mathit{X}}_l^{\circ }={[{\mathit{X}}_l^{\circ x},{\mathit{X}}_l^{\circ y}]}^T$. Each point ${\mathit{X}}_l$ is discarded if ${\mathit{X}}_l^{\circ x}<0$. The points that belong to set $\mathit{X}$ and are not discarded, and should be saved in a new set ${\mathit{X}}^{★}:={\left\{{\mathit{X}}_j\right\}}_{j=1}^J\in {\mathbb{R}}^2$ with $J\le L$. The pseudo-code is shown in Algorithm 2.

Algorithm 2 Remove points “behind” the re-planning point.

1: Obtain a new 2-D point cloud $\mathit{X}:={\left\{{\mathit{X}}_l\right\}}_{l=1}^L\in {\mathbb{R}}^2$; 2: Define ${\mathbf{p}}_s$ as the desired initial point for the re-planning to start; 3: Define ${\psi }_s$ as the tangent angle to the current path at $t_k$ at ${\mathbf{p}}_s$; 4: Follow the procedure: 5: procedureRemove unused points($\mathit{X}$, ${\mathbf{p}}_s$, ${\psi }_s$) 6:     for $l=1,...,L$ do 7:         Compute ${\mathit{X}}_l^{\circ }$ according to (53); 8:         if ${\mathit{X}}_l^{\circ x}<0$ then 9:            Discard ${\mathit{X}}_l$; 10:     return the new set ${\mathit{X}}^{★}:={\left\{{\mathit{X}}_j\right\}}_{j=1}^J\in {\mathbb{R}}^2$ with $J\le L$.

6.2.2. Ordering a Set of Points and Removing Outliers

In order to avoid clustering outliers, reduce the point cloud to a curve-like shape, and extract some implicit ordering from the data, Lee I. [28] proposes an algorithm that seeks to extract a structure “as simple as possible” from the data, by resorting to an Euclidean Minimum Spanning Tree (EMST). Consider the unordered set of points ${\mathit{X}}^{★}$ obtained previously and a graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$, such that $\mathcal{V}=\{{\mathit{X}}_j=(x_j,y_j)|j=1,...,J\}$ and $\mathcal{E}=\left\{({\mathit{X}}_i,{\mathit{X}}_j)\right|i,j=1,...,J,i\ne j\}$. The EMST is a tree that connects all points in $\mathcal{G}$ with the weight of its edges corresponding to the Euclidean distance between each pair of points, that can be computed according to the very popular Kruskal’s algorithm. In order to reduce the time complexity of this process, a threshold distance $N_J$ can be used to define whether each pair of points is connected and a KDTree [29] can be used to compute a sparse graph $\mathcal{G}$ where each point has a limited set of neighbours, as shown in Figure 7.

To remove outliers and define a coarse path to follow, Breadth First Search (BFS) can be applied to the EMST, starting from ${\mathbf{p}}_s$. This removal of outliers from the point cloud is crucial to avoid smaller clusters of points being considered later in the curve fitting problem. The resulting ordered list of points that forms the path with the highest number of points should be saved in a new ordered set ${\mathit{X}}^{†}:={\left\{X_k\right\}}_{k=1}^K\in {\mathbb{R}}^2$. The proposed steps are summarised in Algorithm 3.

Algorithm 3 Order a set of 2-D points.

1: Add the desired initial point for the path ${\mathbf{p}}_s$ to ${\mathit{X}}^{★}$; 2: Define a threshold distance for the neighbours $N_J$; 3: Follow the procedure: 4: procedureOrder Points(${\mathit{X}}^{★}$, $N_J$) 5:     Construct a KDTree from ${\mathit{X}}^{★}$ and use $N_J$ as a distance threshold; 6:     Create a graph $\mathcal{G}$ with J vertices and no edges; 7:     for ${\mathit{X}}_j,j=1,...,J$ do 8:         Query the KDTree for the neighbours of ${\mathit{X}}_j$ and their euclidean distances; 9:         Add the corresponding edges to the graph $\mathcal{G}$; 10:     Compute the MST of the graph $\mathcal{G}$ starting from vertex corresponding to ${\mathbf{p}}_s$; 11:     Compute the path with the highest number of points, starting at ${\mathbf{p}}_s$ using BFS; 12:     return the new ordered set of points ${\mathbf{X}}^{†}:={\left\{{\mathbf{X}}_k\right\}}_{k=1}^K\in {\mathbb{R}}^2$.

6.3. Path Generation—Approximating the Point Cloud with a Parametric Curve

In order to have a suitable representation of a path that the proposed controllers can follow, it is a requirement to generate a curve that is smooth and at least ${\mathcal{C}}^2$. In order to fulfil this requirement, the ordered set of points produced previously can be approximated by non-clamped uniform cubic B-splines, composed of multiple spline segments, where each segment is paramaterised by $\gamma \in [0,1)$.

6.3.1. Define the Number of Segments to Use

Consider now the ordered sequence of K points obtained via the application of Algorithms 2 and 3 to the original 2-D point cloud. In order to fit the points with a parametric curve, we are required to attribute to each point ${\mathit{X}}_k\in {\mathbb{R}}^2$ a corresponding ${\gamma }_k$ in the target parametric curve. This problem could be formulated as a nonlinear optimisation problem—which is computationally demanding to solve for real-time applications. A non-optimal, but more efficient solution, proposed by Liu M. et al. [30] for Simultaneous Localisation and Mapping (SLAM) applications, is to consider $D_X$ to be the total distance between the points, given by:

$$D_X:=\sum _{k=2}^K∥{\mathit{X}}_k-{\mathit{X}}_{k-1}∥,$$

(54)

and the corresponding vector of parametric values $\gamma ={[{\gamma }_1,...,{\gamma }_k]}^T$ to be given by:

$$\left\{\begin{array}{c}{\gamma }_1=0,\hfill \\ {\gamma }_k={\gamma }_{k-1}+\frac{∥{\mathit{X}}_k-{\mathit{X}}_{k-1}∥}{D_X}{\gamma }_{max},k=2,...,K,\hfill \end{array}\right.$$

(55)

where ${\gamma }_{max}$ is the maximum parameter value of the parametric curve. For cubic B-splines, this number depends directly on the number of control points $N_C$ that the target curve will have, such that ${\gamma }_{max}=N_C-3$. The number of control points also dictates how many spline segments are used for the fitting problem. The optimal number of control points can be obtained by solving yet another nonlinear optimisation problem, but due to the real-time nature of the problem, this option is disregarded. Given that a uniform cubic B-spline must have at least four control points to define one segment, and that a low number of sections can under-fit a long set of points whilst a high number leads to over-fitting issues, this number should not be a static constant either. A non-optimal yet dynamic way of defining the number of control points $N_C$ is by taking:

$$N_C:=max\left\{⌊\frac{D_X}{\rho }⌉,4\right\},$$

(56)

with $(1/\rho )>0$ being a control point’s density (tunning parameter defined a priori). A smaller $\rho$ leads to a higher $N_C$. Applying this method to the previous example, and considering $N_C=7$, ${\gamma }_{max}={\gamma }_{11}=4$, the result in Figure 8 is obtained.

6.3.2. Fitting the Points with a Uniform Cubic B-Spline

For fitting the ordered set of points ${\mathit{X}}^{†}$ with a non-clamped uniform cubic B-spline $C(\gamma ,\mathbf{P})$, an optimisation problem is formulated. Consider the objective function given by:

$$f\left(\mathbf{P}\right):=\underset{goal}{\underbrace{\sum _{k=1}^K{∥\mathbf{C}({\gamma }_k,\mathbf{P})-{\mathbf{X}}_k∥}^2}}+F_r,$$

(57)

with:

$$F_r=\underset{\mathrm{regularisation}\mathrm{term}}{\underbrace{\lambda {\int }_0^{{\gamma }_{max}}{∥\frac{\partial \mathbf{C}(\gamma ,\mathbf{P})}{\partial \gamma }∥}^{2}d\gamma +\beta {\int }_0^{{\gamma }_{max}}{∥\frac{{\partial }^2\mathbf{C}(\gamma ,\mathbf{P})}{\partial {\gamma }^2}∥}^{2}d\gamma }},$$

(58)

where $\mathbf{P}={[P_0^x,...,P_{N_c-1}^x,P_0^y,...,P_{N_c-1}^y]}^T\in {\mathbb{R}}^{2N_c}$ is the vector of control points that defines the target curve. The first term minimises the distance between the target B-spline curve and the set of points, whilst $F_r$ is a regularisation term and $\alpha ,\gamma \ge 0$ are the regularisation variables. The integral of the $L_2^2$ norm of the first derivative penalises the total length of the curve, while the integral of the $L_2^2$ norm of the second derivative penalises bends in the path. This objective function can also be expressed using vector notation, according to:

$$f\left(\mathbf{P}\right)=\underset{goal}{\underbrace{{∥B\left(\gamma \right)\mathbf{P}-\mathbf{X}∥}^2}}+\underset{\mathrm{regularisation}\mathrm{term}}{\underbrace{\lambda {\mathbf{P}}^T{R}_1\mathbf{P}+\beta {\mathbf{P}}^T{R}_2\mathbf{P}}},$$

(59)

where $\mathbf{X}=[X_1^x,...,X_K^x,X_1^y,...,X_K^y]$ denotes the points to fit, and $R_1$, $R_2$ are constant matrices that can be computed numerically (see Appendix D).

In order to define the new path, it would not suffice to discard the previously planned curve defined after ${\gamma }_s$ and minimise the objective function with respect to the control points. To guarantee ${\mathcal{C}}^2$ continuity between the previous path and the newly planned one, linear equality constraints should be imposed on the values of ${\mathbf{C}}^{new}\left(0\right)$, ${\mathbf{C}}^{ne{w}^{\prime }}\left(0\right)$, and ${\mathbf{C}}^{ne{w}^{″}}\left(0\right)$ of the new curve. Moreover, it is a requirement to save the old curve up to ${\gamma }_s$, as it may still be in use by other vehicles in the network.

Consider the re-planning point ${\mathbf{p}}_s$ introduced previously, chosen such that it corresponds to the transition between the spline segment that the virtual target of the drone is “sitting on”, and the next segment, according to:

$${\mathbf{p}}_s={\mathbf{C}}^{old}\left({\gamma }_s\right)\mathrm{with}{\gamma }_s=\lceil {\gamma }_{drone}\rceil ,$$

(60)

where ${\gamma }_{drone}$ corresponds to the quadrotor’s virtual target at time instant $t_k$. With this choice of ${\gamma }_s$, it is possible to take advantage of the local support property of B-splines and simplify the equality constraints of the problem, while at the same time simplifying the storage of the curves in memory.

Considering that ${\mathbf{p}}_s$ is dictated by (60), the old curve segments that are described by parametric values such as $\gamma \ge {\gamma }_s$ should be discarded and replaced by a newer curve. Since each curve segment is defined by only four control points, discarding those segments is equivalent to removing control points with indexes $i\ge {\gamma }_s+3$ from the old control points vector. This operation results in a vector given by:

$${\mathbf{P}}^{old}={[P_0^x,P_1^x,...,P_{{\gamma }_s}^x,P_{{\gamma }_s+1}^x,P_{{\gamma }_s+2}^x,P_0^y,P_1^y,...,P_{{\gamma }_s}^y,P_{{\gamma }_s+1}^y,P_{{\gamma }_s+2}^y]}^T.$$

(61)

For the particular example in Figure 9, spline 1 (in green) should be discarded given that ${\gamma }_{drone}\in [0,1)$; hence, ${\gamma }_s=1$ and spline 0 are kept. To achieve this, all the control points with indexes $i\ge 1+3$ should be removed from the control points vector ${\mathbf{P}}^{old}$, i.e., $P_4=(P_4^x,P_4^y)$.

Making use of the local support property once more, it is known that ${\mathcal{C}}^2$ continuity between two consecutive cubic spline segments is guaranteed, as long as the last three control points of the first segment coincide with the first three control points of the second segment. A trivial way of generating a new B-spline with guarantees of ${\mathcal{C}}^2$ continuity in the transition with the old curve, without explicitly defining equality constraints on the derivatives of the function, is to solve the following optimisation problem:

$$\begin{array}{cc}\hfill {\mathbf{P}}^{new}& =\underset{{\mathbf{P}}^{new}}{\mathrm{argmin}}f\left({\mathbf{P}}^{new}\right)\hfill \\ \hfill \mathrm{subject}\mathrm{to}\\ \hfill & \left[\begin{array}{c}{P}_0^{xnew}\\ P_1^{xnew}\\ P_2^{xnew}\\ P_0^{ynew}\\ P_1^{ynew}\\ P_2^{ynew}\end{array}\right]=\left[\begin{array}{c}{P}_{{\gamma }_s}^x\\ P_{{\gamma }_s+1}^x\\ P_{{\gamma }_s+2}^x\\ P_{{\gamma }_s}^y\\ P_{{\gamma }_s+1}^y\\ P_{{\gamma }_s+2}^y\end{array}\right],\hfill \end{array}$$

(62)

where ${\mathbf{P}}^{new}={[P_0^{xnew},...,P_{N_C-1}^{xnew},P_0^{ynew},...,P_{N_C-1}^{ynew}]}^T$ is a new control points vector.

To keep track of old and new curves, it is possible to concatenate only the new control points vector ${\mathbf{P}}^{new}$ with the old control points vector ${\mathbf{P}}^{old}$, ignoring the first three control points, i.e., $P_0^{new}$, $P_1^{new}$, and $P_2^{new}$, which are repeated as a result of the equality constraints imposed by (62). Applying this methodology to the previous example, the final control points vector is given according to Figure 10.

These series of procedures are summarised in Algorithm 4. For the sake of simplicity, the separation between the X- and Y-coordinates of the control points was omitted.

Algorithm 4 Fitting the points—growing a uniform cubic B-spline

1: Compute $D_X$, $\gamma$ and $N_C$ according to Equations (54), (55), and (56), respectively; 2: Consider ${\gamma }_s=\lceil {\gamma }_{t_k}\rceil$ and the original control points vector: $$\mathbf{P}={\left[\begin{array}{c}{P}_0,P_1,...,P_{{\gamma }_s},P_{{\gamma }_s+1},P_{{\gamma }_s+2},P_{{\gamma }_s+3},P_{{\gamma }_s+4}...,P_n\end{array}\right]}^T;$$ (63) 3: Remove control points (corresponding to splines to be re-planned) from the original control points vector, such that: $${\mathbf{P}}^{old}={\left[\begin{array}{c}{P}_0,P_1,...,P_{{\gamma }_s},P_{{\gamma }_s+1},P_{{\gamma }_s+2}\end{array}\right]}^T;$$ (64) 4: Solve the optimisation problem in (62) and obtain a new vector with $N_C$ control points: $$\begin{array}{cc}& {\mathbf{P}}^{new}={\left[\begin{array}{c}{P}_0^{new},P_1^{new},P_2^{new},...,P_{N_C-1}^{new}\end{array}\right]}^T,\hfill \\ \mathrm{with}& P_0^{new}=P_{{\gamma }_s},P_1^{new}=P_{{\gamma }_s+1},P_2^{new}=P_{{\gamma }_s+2};\hfill \end{array}$$ (65) 5: Concatenate the new vector with the old vector (ignoring the first three control points, which are repeated): $${\mathbf{P}}^{final}={\left[\begin{array}{c}{P}_0,P_1,...,P_{{\gamma }_s},P_{{\gamma }_s+1},P_{{\gamma }_s+2},P_3^{new},...,P_{N_C-1}^{new}\end{array}\right]}^T.$$ (66)

6.4. From 2-D Path to Vehicle Formation

To generate individual paths for each vehicle to follow, we can consider a reference path (obtained via the application of the previous algorithms) and offset each point according to an expression that captures a desired vehicle formation. Start by considering a formation vector denominated ${\mathbf{\mu }}_i\in {\mathbb{R}}^3$ for each vehicle i, with each distance defined in the tangential reference frame $\left\{T\right\}$ to the virtual target’s position in the original curve, according to Figure 11. According to Xie et al. [31], it is possible to define a desired path for each vehicle given by:

$${\mathbf{p}}_{di}\left({\gamma }_i\right)=\mathbf{C}\left({\gamma }_i\right)+{}_T^{U}R\left({\gamma }_i\right){\mathbf{\mu }}_i,$$

(67)

where ${\mathbf{p}}_{di}$ is the desired path for the vehicle i, $\mathbf{C}\left({\gamma }_i\right)$ is the planned curve, and ${}_T^{U}R\left({\gamma }_i\right)$ is a rotation matrix computed according to:

$${}_T^{U}R\left({\gamma }_i\right)=[{\mathbf{r}}_1\left({\gamma }_i\right),{\mathbf{r}}_3\left({\gamma }_i\right)\times {\mathbf{r}}_1\left({\gamma }_i\right),{\mathbf{r}}_3\left({\gamma }_i\right)],$$

(68)

with:

$$\begin{array}{cccc}\hfill {\mathbf{r}}_1\left({\gamma }_i\right)& =\frac{\partial {\mathbf{p}}_d/\partial \gamma }{∥\partial {\mathbf{p}}_d/\partial \gamma ∥},\mathrm{with}∥\partial {\mathbf{p}}_d/\partial \gamma ∥\ne 0\hfill & \hfill {\mathbf{r}}_3\left({\gamma }_i\right)& =\frac{{\mathbf{r}}_d-({\mathbf{r}}_d·{\mathbf{r}}_1\left({\gamma }_i\right)){\mathbf{r}}_1\left({\gamma }_i\right)}{∥{\mathbf{r}}_d-({\mathbf{r}}_d·{\mathbf{r}}_1\left({\gamma }_i\right)){\mathbf{r}}_1\left({\gamma }_i\right)∥},\hfill \end{array}$$

(69)

such that ${\mathbf{r}}_1$ is the tangent to the curve. Moreover, since all vehicles will only be required to operate in a 2-D plane, a trivial definition for one of the axis of the tangential frame $\left\{T\right\}$ is ${\mathbf{r}}_d={[0,0,1]}^T$.

A path might not be not parameterised according to the arc length and, for B-splines in particular, each spline segment is such that $\gamma \in [0,1]$. Therefore, it is commonplace to define a constant required speed $V\le V_{max}$ for the vehicle and let the desired speed profile for the virtual targets be given by:

$$v_L\left(\gamma \right)=\frac{V}{∥{\mathbf{p}}_d^{\prime }\left(\gamma \right)∥}.$$

(70)

7. Implementation Details

To evaluate the performance of the proposed PF and CPF algorithms applied to marine ASVs, real water trials were conducted at Doca dos Olivais (Lisbon, Portugal) using the Medusa class of underactuated marine vehicles [14], shown in Figure 12. The vehicles used in the real trials were equipped with a GPS Astech MB100, a NavQuest600 Micro DVL, and a Vectornav VN-100T Attitude and Heading Reference System (AHRS). The operating system used during development was Ubuntu 18.04LTS along with ROS Melodic.

To analyse the performance of the proposed online path planning algorithm, a realistic simulation environment that closely resembles the Doca dos Olivais site was developed and incorporated into the Gazebo simulator. Given the main goal of having a fleet of vehicles encircling a chemical spill, is was necessary to overlay a red stain mesh on top of the ocean’s surface (Figure 13).

For simulating the Medusa ASVs, a CAD model of the vehicles was incorporated into the simulator (Figure 14a). The virtual vehicle was also equipped with DVL, AHRS, and GPS sensors provided by the UUVSimulator plugin [16]. To simulate the quadrotor, the Iris vehicle provided by the PX4 SITL Gazebo plugin [15] was used; see Figure 14b.

The simulated quadrotor was equipped with a virtual camera mounted 21 mm below the vehicle’s center of mass and with a pitch angle of $-{45}^{\circ }$, pointing downwards, and produced an image with a resolution of $640\times 480$ px, according to Figure 15a. Its intrinsic parameters are given by:

$$\left\{\begin{array}{c}(c_x,c_y)=(320.5,240.5)\hfill \\ (f_d{s}_x,f_d{s}_y)=(381.4,381.4).\hfill \end{array}\right.$$

(71)

Given assumption 5, the detection of the boundary region between the spill and the ocean surface was out of the scope of this work. Therefore, we resorted to OpenCV library [32] to mask and threshold the red colours in the image feed. After this step, the Canny edge detection algorithm was applied to the binary image to retrieve the pixels corresponding to the boundary, according to Figure 15b. To solve the optimisation problem proposed in Section 6.3.2, we resorted to Scipy’s SQP solver [33].

The entire system architecture is shown in Figure 16. The inner-loop controls adopted for the quadrotor were the ones already provided by PX4, while for the ASV, we resorted to PID inner-loop controllers to steer the vehicles.

8. Experimental and Simulation Results

In this section, we present some real experimental results regarding the PF and CPF controllers applied to two Medusa ASV vehicles. In addition, realistic 3-D simulation results are also presented for the case study where two Medusa vehicles were required to perform a CPF mission on a pre-defined path with a quadrotor, in a leader–follower formation. Finally, a third case study is presented, where a simulated quadrotor had to detect the boundary of a chemical spill, and plan, in real-time, a path for both itself and a Medusa ASV to follow cooperatively. The control gains adopted are available in Appendix E.

8.1. Cpf with ETC between 2 Medusa Vehicles (Real)

For the real trial, performed at Doca dos Olivais (Lisbon, Portugal), two Medusa vehicles were required to perform a lawn-mowing mission cooperatively at the surface of the water, according to Figure 17. The black vehicle (Medusa 1) was required to follow the leader (Medusa 2) according to the formation vector $\mathbf{\mu }={[-5,-5,0]}^T$. Both vehicles were required to follow the path at $V=0.5$ m/s and communications were bi-directional.

According to the results in Figure 18a, the along-track error of Medusa 1 increases quickly as the virtual target tries not only to minimise the distance to the vehicle but also its distance to its neighbour’s (Medusa 2) virtual target. As the vehicles start to move, this error starts to decrease, and according to Figure 18b, after approximately 50 s, the vehicles align themselves according to the desired formation, approach the desired speed profile and, as a consequence, the rate of information exchange decreases. This decrease in the rate of communication is due to the bank of estimators for the virtual targets running in each vehicle being able to better predict the evolution of the virtual targets.

8.2. Cpf with ETC between a Quadrotor and Medusa Vehicles (Simulation)

For this case study, a CPF mission was performed such that a simulated quadrotor and two Medusa vehicles were required to perform a lawn-mowing mission, according to Figure 19a. In this experiment, the aircraft was required to fly at a fixed altitude of 30 m; the formation vector for Medusa 1 was given by ${\mathbf{\mu }}_1={[-5,5,0]}^T$ m, and for Medusa 2, by ${\mathbf{\mu }}_2={[-5,-5,0]}^T$ m, leading to a triangular formation with 2 ASVs side by side, behind the quadrotor. In this experiment, there was bi-directional communication between the pairs of vehicles: (quadrotor, Medusa 1) and (quadrotor, Medusa 2). From the results in Figure 19b, it is observable that the vehicles converge to their desired formation at around 25 s. After this period of time, the position error converges to a neighbourhood of zero and the virtual target speeds converge to their desired value. As a consequence, the number of communication events between the vehicles drops as the bank of observers in each vehicle can more accurately track the state of the virtual target of their peers.

8.3. Boundary Encircling with a Quadrotor and a Medusa Vehicle (Simulation)

For the last simulated experiment, the quadrotor was required to start the same lawn-mowing that was adopted for the mission with one Medusa ASV. As soon as a chemical spill boundary was detected in the drone’s image stream, the quadrotor was required to start the path planning algorithm at a rate of 1 Hz and send the most up-to-date path to the ASV, according to Figure 20. The drone was required to follow the path at 30 m of altitude with a desired constant speed of $0.5$ m/s. Since the quadrotor was equipped with a fixed-mounted camera, it was also required to align its yaw angle with the tangent to the path in order not to lose sight of the boundary being followed.

In order to guarantee that the path further ahead could be generated for the ASV to follow, it was desirable for the marine vehicle to follow the the quadrotor from behind, i.e., with a formation vector $\mathbf{\mu }={[-5,5,0]}^T$ m. In Figure 21a, a top-down view of the executed mission is shown. In Figure 21b, plots of the PF errors are provided along with the norm of the horizontal distance of each vehicle to the real boundary being followed. It is observable that the tracking error only increased in zones where the chemical spill had a crease. This is justified by the fact that the Medusa vehicle, when performing tight turns, was not able to cope with its virtual target speed and slowed down, leading to sudden spikes in the along-track error. These tracking errors were instantly compensated by the adaptive virtual target dynamics, which attempted to minimise the distance between itself and the vehicle. It is also observable that the norm of the distance between the marine vehicle and the chemical spill is much lower than its aerial counterpart, with the Medusa always following the boundary from its outskirts, due to the formation vector adopted.

From Figure 21b, it is also evident that the horizontal distance between the real drone’s position and the boundary is bounded by $6\mathrm{m}$. This result is to be expected, as the altitude estimates are mainly provided by the simulated GPS system and small errors in the estimated attitude, especially yaw angle, will lead to errors of several meters in the generated 2-D point cloud. Due to the type of application at hand, and given that it is typical to have errors of several meters in underwater scenarios, these errors are considered within an acceptable range. In addition, the small oscillations in the boundary distance plot result from the simulated chemical spill boundary mesh being a composition of discrete lines which are picked up by the drone’s camera.

In Figure 22, a plot of the point cloud generated by the algorithm is shown at two different time instants (in green), as well as the corresponding planned B-spline paths (in blue). Note that in Figure 22a, some of the green dots further away from the vehicle were discarded by the planning algorithm, as they were too far away from the main cluster of points.

9. Conclusions and Future Work

This paper addressed the problem of encircling an environmental boundary caused by a chemical spill using a team of robots composed of an aerial quadrotor and Medusa marine vehicles. The path following problem was introduced, and a non-linear control law derived for the ASV, exploiting the technique described in P. Aguiar and F. Vanni [10,11,12]. Inspired by this control law, a new one was derived for a quadrotor following the same methodology, with some key differences due to the nature of the aircraft. For the section that followed, the CPF problem was formulated and a proposal to solve the problem was presented, such that the synchronisation controller was distributed and the same for all vehicles (aerial and marine) using event-triggered communications based on previous work by N. Hung and F. Rego [13]. In addition, a new real-time path planning algorithm was developed that made use of the camera sensor onboard of the quadrotor to have a local view of the boundary and generate a point cloud expressed in the inertial frame. This data was then used to solve an optimisation problem which generates a B-spline-based path that grows dynamically as the drone moves along the boundary and acquires more data. The path is then shared with all ASV vehicles in the network in real time. The proposed algorithms were implemented in ROS, and a 3-D virtual scenario was generated, allowing for a mixture of real and simulated results. Future work includes making the height at which the quadrotor operates dynamic and introducing curvature limits as inequality constraints to the path planning problem, as well as obstacle avoidance before carrying out integrated experiments with real vehicles.