Robust Cooperative Moving Path Following Control for Marine Robotic Vehicles

This paper presents results on recent developments pertaining to the coordinated motion control of a fleet of marine robotic vehicles. Specifically, we address the Cooperative Moving Path Following (CMPF) motion control problem, that consists of steering the robotic vehicles along a priori specified geometric paths that jointly move according to a target frame, while achieving a pre-defined coordination objective. To this end, each vehicle will need to communicate with their neighbors in order to cooperatively solve the CMPF task. Two distinct robust Moving Path Following motion control strategies for achieving robustness on the moving path following tasks are proposed. Experimental results demonstrating the application of CMPF to marine vehicles in the context of source localization and tracking of underwater targets are presented backed with stability and convergence guarantees.


INTRODUCTION
The motion control problem for underactuated robotic vehicles is a relatively mature area of research, with important works addressing trajectory tracking and path following schemes. In the path following (PF) problem, the vehicle is tasked to follow a fixed geometric path without the need of satisfying explicit time constraints, in contrast to trajectory tracking. A series of results addressing the PF motion control problem were published, starting with the pioneering work in Samson (1992), Micaelli and Samson (1993), and Aguiar and Pascoal (2007) for the case of wheeled mobile robots, Encarnação et al. (2000), Belleter et al. (2016) and references therein for marine vehicles and Cichella et al. (2011), Xargay et al. (2013) for UAVs.
A generalization of the path following problem is termed the Moving Path Following (MPF) motion control problem, which consists of steering the robotic vehicle along an a priori specified geometric path expressed with respect to a moving target frame. This problem finds applications in source seeking, convoy protection, target tracking, surveillance and monitoring and also autonomous landing. For example, in target tracking applications in the maritime environment, it is desirable for the vehicles to perform different types of maneuvers. These maneuvers can be framed as specific paths to be followed around the tracked target, and often allow the vehicle to have the necessary flexibility to operate in a highly complex environment, which is constantly inducing disturbances into its body due to the presence of maritime currents, waves and hydrodynamic effects. Further, it is observed that the MPF problem retains the advantages of the classical path following schemes (Aguiar et al., 2005) such as faster convergence of the robot to the moving path, while allowing the target reference frame to move freely.
The works in Oliveira and Encarnação (2013) and Oliveira et al. (2016) introduced the MPF control problem for tracking of ground targets using Unmanned Aerial Vehicles (UAVs) and later on, Oliveira et al. (2017) extended the solution to the 3D case. The proposed approach was suitable for robotic vehicles requiring a minimum positive forward speed, such as certain types of AUVs. In Jain et al. (2018b), a Lyapunov-based MPF control approach was presented for robotic vehicles without this restriction. Other control methods such as vector field method (Kapitanyuk et al., 2017) and nonlinear model predictive control (Jain et al., 2018c) have been proposed to solve the MPF problem. In contrast to the contributions of this paper, the salient features of the above reviewed literature are that they do not consider the external disturbances that depend on the operational environment, such as maritime currents, wind or rough terrain, that can affect the performance of the MPF controller. Further, they assume that the velocity of the target frame is known.
In path following literature, the problem of robustness has been addressed for example, in Dagci et al. (2003), where a cascade sliding mode controller for both kinematics and dynamics of a robotic vehicle was designed. In Aguiar and Pascoal (2002), a disturbance observer for constant unknown ocean currents was designed to solve the problem of dynamic positioning and way-point tracking of an underactuated AUV. Later on, the problem of robustness against parametric uncertainty in trajectory tracking and path following was also addressed in Aguiar and Hespanha (2007). More recently, in Zhang et al. (2014), a sliding mode technique combined with a predictive control strategy was developed to compensate for the impact of the hydrodynamic damping coupling on a 3D path following task for an Autonomous Underwater Vehicle (AUV). In Wang et al. (2016), a H ∞ robust controller for ground vehicles is proposed to achieve path following in the presence of disturbances caused by delays and data packet dropouts. All of the above schemes consider robustness for the path following problem. From the best of the authors knowledge, the only work concerning the problem of robustness in MPF literature is Reis et al. (2019), where sliding mode based controllers and a disturbance observer were designed to compensate external disturbances acting on the robotic vehicle.
A further extension of the MPF framework for multi-robot applications and formation control is the Cooperative Moving Path Following (CMPF) control problem, which consists in steering N vehicles along N paths defined with respect to a moving target while achieving some coordination objective. A special case of CMPF control, where the paths are fixed with respect to a given reference frame is the framework of Cooperative Path Following (CPF). As a recent example, the robustness problem in CPF literature was addressed in Gu et al. (2019), where two cooperative path following controllers using an Extended State Observer to estimate and compensate external disturbances in the kinetic level were proposed and validated experimentally using Autonomous Surface Vehicles (ASVs). In (Jain et al., 2018a), an eventbased controller was explicitly designed to reduce the frequency of communication between the robotic vehicles. The control strategy effectively decomposes the control structure into two distinct layers. The first is responsible for the motion control of each individual vehicle, termed the PF controller. The second, termed the cooperative controller, is responsible for achieving coordination between the robots by using a consensus law. However, (Jain et al., 2018a) does not consider uncertainties and disturbances acting on the robotic vehicles. By decoupling the motion control layer from the cooperative control layer, one could use robust MPF controllers in the first layer to deal with the presence of certain types of disturbances acting on the vehicles, without affecting the formation control.
This paper extends the results obtained for the MPF controllers proposed by Reis et al. (2019) to the Cooperative MPF framework. Two MPF control strategies are proposed for the motion control layer, both using a known target pose and estimates of the target velocities. The first strategy employs a First Order Sliding Mode (FOSM) term to achieve robustness against bounded disturbances. The second strategy seeks to directly compensate the disturbance by computing an estimate of the disturbance using a disturbance observer. The cooperative layer consists of the consensus law proposed by Aguiar (2017). The stability of the proposed control laws is analyzed and it is shown that the origin of the path error is stable and converges to a small neighborhood around zero, even in the presence of bounded estimation errors on the target velocities and environmental disturbances. The design and theoretical results for the two variants of the proposed robust controllers are experimentally validated in a CMPF scenario using Autonomous Underwater Vehicles.

Kinematic Model for an Underactuated Vehicle
Consider an inertial frame of reference {I} and N robotic vehicles, each with its body frame {B i } attached to its center of mass. Define the set of N robotic vehicles as I = {1, 2, ..., N}. The kinematic model of the i-th vehicle moving in R n with n = 2, 3 can be expressed byṗ where p i ∈ R n denotes the position of the i-th robot with respect to the frame {I}, R i ∈ SO(n) denotes the rotation matrix from the frame {B i } to an inertial frame {I}, v i ∈ R n and ω i ∈ R n(n−1)/2 are the linear and angular velocities of the i-th vehicle with respect to its own body frame, S(ω i ) ∈ so(n) is the skew-symmetric matrix associated to the angular velocity ω i . Finally, d v,i ∈ R n and d ω,i ∈ R n(n−1)/2 are kinematic disturbances acting on each robot. Many different factors can be the source of these disturbances, depending on the type of vehicle and the operational environment. Marine vehicles such as AUVs are affected by unknown sea conditions that can induce unwanted external velocities due to maritime currents, waves and wind. In the case of aerial vehicles, wind and internal dynamics can induce some unwanted disturbances in the kinematic model. In this work, we consider the problem of controlling an underactuated vehicle at the kinematic level, with the control signal defined as where the body linear velocity v i is defined as . This is the case for vehicles where only the longitudinal velocity v f ,i ∈ R and the body angular velocity ω i ∈ R n(n−1)/2 can be controlled, such as some types of AUVs. We assume that the vehicle has an innerloop autopilot controller that is responsible to track the linear and angular velocity commands generated by the controller based on the kinematic model of the robotic vehicle. Imperfect tracking by the inner-loop autopilot controller can further contribute to the velocity disturbances acting on the vehicle, that can be lumped into the terms d v,i and d ω,i .

Cooperative Moving Path Following Problem
In the CMPF control problem, the vehicles must follow a priori specified paths expressed with respect to a moving target whose position can be accurately estimated, while also maintaining some coordination objective. Define the target frame {T} with its origin attached to the target center of mass. Then, the cooperative MPF problem can be divided in the following two sub-problems.

Moving Path Following Problem
Let p t (t) ∈ R n denote the position of the target with respect to the frame {I}, and p t d,i (γ i ) ∈ R n be the desired path for vehicle i, specified with respect to the frame {T} and parameterized by the path variable γ i ∈ R. As illustrated by Figure 1, for a given γ i and time t, p d,i (γ i , t) andṗ d,i (γ i , t) denote the position and velocity of the virtual reference point that must be followed by the i-th vehicle, with respect to the inertial frame {I}: where R t (t) ∈ SO(n) is the rotation matrix of frame {T} with respect to {I}, v t (t) ∈ R n , ω t (t) ∈ R n(n−1)/2 are the linear and angular target velocities and ∇ ≡ ∂/∂γ i is the derivative with respect to γ i .
Note that Assumption 2.1 is already needed in order to compute (4) from (3). Suppose we wish to control the position of the nose of the i-th vehicle, or more generically, a pointp i = p i + R i ǫ placed at a constant position ǫ = [ ǫ 1 ǫ 2 ] T (n = 2) or ǫ = FIGURE 1 | Coordinate frames and vector notation for N = 3 vehicles.
[ ǫ 1 ǫ 2 ǫ 3 ] T (n = 3) from the origin of {B i }. Then, define the MPF error associated to the i-th vehicle as the vector The objective of the MPF control problem is to design a control law u i such that the origin e i ≡ 0 is stable and e i → 0 as t → ∞, ∀i ∈ I. That is, it is desired to steer the vehicles toward their moving geometric paths, such thatp i stabilizes around p d,i (γ i , t), ∀i ∈ I. In order to control the progression of the virtual points p d,i (t, γ i ) along the moving paths, the dynamics of the path variableγ i should be explicitly controlled. This can be achieved by imposing the dynamics for γ i aṡ where the scalar v d is the desired nominal speed of the path variable and ϑ i is a bounded control signal, designed to achieve CMPF objectives such as: (i) consensus over the path variables of the robotic vehicles to achieve a desired formation along the moving path and (ii) faster convergence to the moving path. To move along the geometric paths with the desired velocity, the vehicles must satisfy the desired speed assignments |γ i − v d | → 0 as t → ∞, ∀i ∈ I.

Cooperative Motion Control Problem
Assume that the i-th vehicle communicates with a fixed set N i ⊂ N of neighbor vehicles. Given the path variables γ i (i ∈ I) for the N vehicles and a given undirected, fixed communication topology among them, the objective of the cooperative motion control problem is to design a decentralized control law v r,i (t) such that the positions of the virtual points are synchronized, that is, |γ i − γ j | converges to zero ∀i, j ∈ I as t → ∞.
To simultaneously achieve the speed assignment, coordination objective and also two other secondary objectives, function ϑ i in (6) is decomposed as where v r,i (t) is the cooperative control signal (to be designed) that is responsible for achieving consensus between the vehicles, while g e,i (t) and g ω,i (t) represent secondary objectives, where g e,i (t) is an error correction term, responsible for delaying the evolution of the path variable in case of momentary vehicle failure, and g ω,i (t) is a rotation correction term, responsible for canceling the rotational motion induced on the path by the rotation of the target. These functions are to be properly defined in section 3.3.

Robust MPF Controller Design
In this section, we consider the kinematic controller proposed in Jain et al. (2018a) with a modification designed to ensure robustness against disturbances. For the i-th vehicle, the error dynamics is given bẏ Using model (1) with control signal (2) and MPF error (5), the error dynamics can be rewritten aṡ where is a constant matrix that can take the forms for the planar (n = 2) and 3D (n = 3) cases, respectively. Note that it is always possible to choose ǫ such that is full rank. Vector d i ∈ R n is the total external disturbance acting on the vehicle. In the planar case, it is given by Remark 3.1. Notice that, by the triangle inequality, the total external disturbance d i is bounded by Assumption 3.1. The total external disturbances d i are bounded vector quantities.
Theorem 1 (Robust MPF). Consider an underactuated robotic vehicle described by (1) with control signal given by (2). Let the MPF error kinematics be described by (8), and consider that the pose of the i-th vehicle {p i , R i } ∈ R n × SO(n) and of the target frame {p t , R t } ∈ R n × SO(n) are known. Under Assumptions 2.1 and 3.1, the control law ensures that all trajectories of the MPF error are globally uniformly ultimately bounded and converge to a ball around the origin e i = 0 that can be made arbitrarily small. In (10), the matrix † is the Moore-Penrose pseudo-inverse of , K p ∈ R n×n is a positivedefinite gain matrix and v t ∈ R n , ω t ∈ R n(n−1)/2 are estimates of the target velocities. In (11), ρ i is a scalar such that

are bounded estimation errors on the target velocities.
Proof: Define the Lyapunov candidate V(e i ) = 1 2 e T i e i . Using the error dynamics in (8), its time derivative along the system trajectories iṡ where we have used the fact that e T i S(ω i + d ω,i ) e i = 0, since S(ω i + d ω,i ) is skew-symmetric. Substituting control law (10) in (13) yieldsV where Since K p > 0, the first term is negative definite and bounded by −λ min (K p ) e i 2 . Next, we consider the two cases of (11), when e i ≥ ǫ w or e i < ǫ w .
• For e i ≥ ǫ w in (14), we havė where the Cauchy-Schwarz inequality was employed on term e T i D i . By Assumption 3.1, it is always possible to design ρ i such that (12) is satisfied. Therefore, by Remark 3.1, choosing ρ i ≥ D i renders the second term on the right-hand side negative definite, which estabilishes that the trajectory e i (t) of the closed-loop system reaches the ball B(ǫ w ) : = {e i ∈ R n : e i ≤ ǫ w } in finite time. • When the trajectories are inside B(ǫ w ), we have e i < ǫ w , and (14) getṡ where 0 < θ < 1. Then, using the inequality above, one can write:V Note that µ i ≤ ǫ w for all 0 < θ < 1, which means that the trajectory of the closed-loop system e i (t) again reaches the ball B(µ i ) ⊆ B(ǫ w ) in finite time.
This establishes that the trajectories are globally ultimately uniformly bounded, since V = 1 2 e i 2 is radially unbounded (Khalil, 2002). Moreover, e i (t) converges to the ball B(µ i ) ⊆ B(ǫ w ), which can be made arbitrarily small when ǫ w → 0.

Robust MPF Controller Design With Disturbance Observer
In the presence of large amplitude disturbances, it may be difficult to tune the parameters ρ i and ǫ w so as to satisfy (12). In these situations, an observer can be designed to provide an estimate of the disturbance. Furthermore, this estimate can be used in the control law to compensate the real disturbance directly.
Without loss of generality, consider the planar problem. Consider that the vehicle pose {p i , R i } ∈ R 2 × SO(2) is known and that the vehicle orientation is parameterized by the planar Then, the disturbance observer for the translational disturbance is defined as where the estimation errors are defined as p i = p i −p i and d v,i = d v,i −d v,i , and the positions p i , i ∈ I are accurately measured. For positive-definite matrices K 1 , K 2 ∈ R 2×2 , the dynamics of the estimation errors p i , d i can be proven to be Input-to-State Stable (ISS) with respect to the first time-derivative of d v,i (Aguiar and Pascoal, 2002). Similarly, observers for the rotational disturbances d ω,i ∈ R can be designed as: where the estimation errors are defined as ψ i = ψ i −ψ i and d ω,i = d ω,i −d ω,i , and the planar angles ψ i are measured. Again, for positive scalars k ω 1 , k ω 2 ∈ R >0 , the dynamics of the estimation errors ψ i , d ω,i can be proven to be ISS with respect to the first time-derivative of d ω,i (Aguiar and Pascoal, 2002).
Consider an underactuated robotic vehicle described by (1) and control signal given by (2). Let the MPF error kinematics be described by (8), and consider that the pose of the vehicle {p i , R i } ∈ R n × SO(n) and of the target frame {p t , R t } ∈ R n × SO(n) are known. Under Assumptions 2.1 and 3.1, the control law ensures that all trajectories of the MPF error are globally uniformly ultimately bounded and converge to a ball around the origin e i = 0 that can be made arbitrarily small. In (17), matrix † is the Moore-Penrose pseudo-inverse of , K p ∈ R n×n is a positive-definite gain matrix, v t ∈ R n and ω t ∈ R n(n−1)/2 are estimates of the target velocities and d i is the total estimated external disturbance, which is a function of the states of the disturbance observers The term w i is defined by (11), with scalars ρ i satisfying Proof: The proof is very similar to Theorem 1, and can be performed by proposing the same Lyapunov candidate V = 1 2 e T i e i . Differentiating it in time and applying the error dynamics (8) with control law (10) yieldṡ where Note that (20) is similar to (14), but with disturbance D i instead of D i . Therefore, using the same arguments for the proof of Theorem 1 with Assumption 3.1 and condition (19), one can conclude that the trajectories of the MPF error are globally uniformly ultimately bounded and e i (t) converges to the ball B(μ i ) ⊆ B(ǫ w ), which can be made arbitrarily small when ǫ w → 0.
Remark 3.2. Comparing conditions (12) and (19) for the choice of ρ i , in (19) the gain ρ i must overcome only the norm of the disturbance estimation errors instead of the norm of the disturbance. Therefore, if the disturbance observer is properly designed, this method can reduce the necessary amount of control effort when compared to the previous method.
Remark 3.3. Both proposed control laws (10, 17) employ estimates of the target velocities. Since the velocity estimation errors v t and ω t appear as additional disturbances in D i and D i , they can be properly compensated by the proposed controllers as long as ρ i satisfies (12) or (19). In this case, the velocity estimation errors are implicitly assumed to be bounded. Furthermore, notice that in the case where no velocity estimators are employed ( v t = 0 and ω t = 0), the velocity estimation errors are simply v t = v t and ω t = ω t , which are also bounded. These observations imply that velocity estimators are not necessarily required for the implementation of the proposed control laws. However, large velocity estimation errors would increase the lower bounds for the design of ρ i , increasing the amount of control effort, which could lead to loss of performance.

Cooperative Moving Path Following
This section provides a proper design for function ϑ i in (7). First, the design of the error correction term g e,i (t) and of the rotation correction term g ω,i (t) are discussed, and finally we make use of the results from Olfati-Saber et al. (2007) to design a cooperative control law v r,i (t).

Error Correction Term
The term g e,i (t) is a bounded error correction term that acts as an external input to the path dynamics, enabling faster convergence of the robotic vehicle to the moving path. It can be designed to delay or to stop the motion of the virtual point if the vehicle is too far away from the path. This can be done by defining the gradient with respect to the path variable of the MPF error norm squared: and then choosing a gradient descent law g e,i = −k e,i sat(η e,i ) with k e,i > 0. The saturation function guarantees the boundedness for the correction term. Its effect is to effectively delay the evolution of the virtual point along the path by explicitly avoiding the evolution of γ i if the MPF error norm is too large.

Path Rotation Correction Term
The term g ω,i (t) is designed to delay the evolution of the virtual point p d,i in a such a way that minimizes the effect of the target rotational motion, which is evident from the term S(ω t ) p t d,i (γ i ) in (4). This effect is important since, for large target angular velocities ω t , the virtual point could move faster than the i-th vehicle could reach. Therefore, substituting (7) into the error dynamics (8), we seek to design a scalar g ω,i such that If the target angular velocity is known, the minimum can be achieved by the least squares solution with minimum given by Remark 3.4. Note that the minimum is identically null regardless the rotational motion of the target only if and only if: (i) the path is perpendicular to its gradient everywhere, i.e., (∇p t d,i ) T p t d,i = 0 ∀γ i , i ∈ I or (ii) the angular velocity of the target is collinear to the path gradient everywhere, i.e., ω t = c ∇p t d,i = 0 ∀γ i , i ∈ I for some constant c ∈ R. Clearly, condition (ii) never holds in the planar case (n = 2).
From (23) and Assumption 3.2, the error correction term is bounded by.

Cooperative Controller
Consider the distributed consensus law (Aguiar, 2017): where k c,i > 0 are consensus gains and γ i j are estimates of the path variables of the neighbor vehicles (γ j , j ∈ N i ) running inside the i-th vehicle computer. Assuming that the frequency of communication is low, its reasonable to assume that γ i j = γ j , ∀t > 0. Therefore, one can write γ i j = γ j − γ i j , where γ i j is a path variable estimation error. Assumption 3.3. Given a fixed, undirected communication topology between the vehicles, the i-th vehicle updates its path variable γ i to its j ∈ N i neighbors in a fixed frequency. Additionally, assume that no data package is lost during communication. Consequently, the path variable estimation errors γ i j , ∀i, j ∈ I are always bounded.
Theorem 3 (Cooperative Controller). Consider a fleet of N underactuated robotic vehicles with dynamics described by (1) and control signal given by (2). Then, control laws (10) or (17) with robustness term (11) guarantee that the origin of the MPF error e i ≡ 0 is stable under the same conditions and assumptions of Theorems 1 and 2, respectively.
Furthermore, under Assumption 3.3, the cooperative control law given by (24) ensures that |γ i − γ j |, ∀i, j ∈ I are Inputto-State Stable (ISS) 1 with respect to the path variable estimation errors [ γ ] i , error correction terms g e,i and rotation correction terms g ω,i , ∀ i ∈ I.
Proof: The first part of the Theorem was already proved in Theorems 1 and 2. The part related to the cooperative control follows the same core ideas from (Jain et al., 2018a). First, define the disagreement vector (Olfati-Saber et al., 2007) as δ : = γ − α1 N , with α = (1/N)1 T N γ . Note that the consensus condition |γ i − γ j | = 0, ∀i, j ∈ I is achieved if and only if δ = 0. Additionally, the following two properties hold: (i) Lγ = Lδ and (ii) 1 T N δ = 0.
1 Khalil (2002) A nonlinear systemδ = f (t, δ, ǫ) is said to be Input-to-State Stable (ISS) if there exist a class KL function β and a class K function γ such that for any initial state δ(t 0 ) and any bounded input ǫ(t), the solution δ(t) exists for all t ≥ t 0 and satisfies Next, define the ISS Lyapunov function candidate Taking its time-derivative and using (25), yieldṡ with z = Lδ, where we used the properties (i) and (ii) introduced before. Using the Cauchy-Schwartz inequality, yieldṡ V cc ≤ −λ min (K c ) z 2 + λ max (K c ) z γ + z g e + z g ω Applying Young's inequality to the last three terms in (27), we havė with a scalar c ∈ R >0 . Choosing any c > λ max (K c ) 2λ min (K c ) +λ −1 min (K c ) > 0 leaves the first term of the right-hand side strictly negative, which by Assumption 3.3 and by the boundedness of g e,i , g ω,i establishes that the disagreement vector δ is ISS with respect to the bounded disturbances [ γ ] i , g e,i and g ω,i , for all i ∈ I.

Experimental Setup
The experiments were performed on Porto de Leixões (Porto, Portugal) using three Light Autonomous Underwater Vehicles (LAUVs) from the Underwater Systems and Technology Laboratory (LSTS) at the Faculty of Engineering of the University of Porto (FEUP) (Figure 2A). LAUVs are lightweight, portable vehicles that can be easily launched, operated and recovered with a minimal operational setup.
The vehicles operate under the DUNE/Neptus environments, which are part of a software toolchain (Pinto et al., 2013) developed and maintained by LSTS. DUNE is the on-board software running on the vehicles, comprising all the software needed for communications, navigation, control, maneuvering, plan execution and supervision of multiple types of robotic vehicles. The control algorithms were implemented on C++, using the available DUNE libraries. Neptus is a software used for command, control and monitoring, comprising many typical functions needed for a typical mission, such as planning, execution and post-mission analysis ( Figure 2B).
A target vehicle was simulated and continuously sends its position and orientation (computed from GPS/IMU measurements using an extended Kalman filter (Braga et al., 2012) to the three follower vehicles through static UDP connections with a maximum frequency of 1 Hz. The control algorithm for the target vehicle is a vector field method (Nelson et al., 2007) that is responsible to steer the vehicle along a circumference with radius equal to 60 m in the clockwise direction at 0.5 m/s. The desired moving paths for the follower AUVs are planar circumference centered at the target vehicle with phase difference of 2π/3 between them: where R = 25 m, φ 1 = 0 rad, φ 2 = 2π/3 rad and φ 3 = −2π/3 rad. Each vehicle sends its path variable to the neighbor vehicles with a frequency of 1 Hz to maintain coordination, according to the consensus law (24) and Assumption 3.3. The consensus gains are k c,i = 0.1 , ∀i ∈ I. For the construction of the MPF errors e i , the value ǫ = 1 0 T was used. The controller gain matrices and error correction gains were chosen as K p,i = diag(0.2, 0.2) and k e,i = 2 , ∀i ∈ I. The reference for the path variable velocity is v d = 1 m/s.
Remark 4.1. We point out the fact that this particular kind of vehicles cannot generate reliable negative forward velocities due to its propeller design. Given the fact that control laws (10), (17) can generate negative forward velocities if the virtual point is behind the line-of-sight of the vehicle, a substitute controller was designed to override the original controller in case this happens. Therefore, while the forward velocity generated by (10) or (17) is negative (v f ,i < 0), the applied control signal will be (10), (17) generate a positive v f ,i again. Constants v C , ω C ∈ R are strictly positive. That means that the vehicle performs a "turning" maneuver with constant velocities until the virtual point is once again inside its line-of-sight. The direction of the turn is clockwise if the virtual point is to the right of the vehicle and counterclockwise if the virtual point is to the left of the vehicle. This strategy allows arbitrary initial configurations of the vehicles with respect to the initial position of the virtual point, and also allows the vehicles to recover from practical dead lock situations where their line-of-sight is kept facing away from the virtual point, which could happen, for example, in case of communication losses. In this case, v C = 1.7 m/s and ω C = 1 rad/s, approximately the upper saturation limits for the actuators.

CMPF With Velocity Compensation
The first experiment shows the results of the CMPF controller with velocity compensation, ρ i = 0 and no disturbance compensation (d v,i = 0 andd ω,i = 0). Figure 3 shows the trajectories of the vehicles. The trajectory of the target is represented as the dashed black circle, in the clockwise direction. The small colored circles represent the beginning of the trajectory, while the colored asterisks represent its end. Noticeably, the three vehicles try to follow their respective paths (shown in dashed lines) around the rotating target, while maintaining their phase difference. Figure 4 shows the obtained results. The initial position of the vehicles was distant from the network router (located closer to the northeast part of Figure 3), which affected the wireless communications for a while. However, the initially large path variable errors rapidly decrease and remain bounded to less than 4 m ( Figure 4B). Because of the communication losses and possibly the presence of ocean currents, the secondary controller  described in Remark 4.1 had to recover some vehicles during the transient, resulting in some of the turning maneuvers we see in the beginning of the trajectories (Figure 3). After that transient, the norm of the MPF errors converge to a small region of less than 3 m while the control signal remains inside its linear region (Figures 4C,D). Note how the consensus law acts precisely when the path variable errors are high (Figure 4E), how the error correction terms acts when the MPF error norm is high (to prevent the evolution of the path variables), and how the rotation correction terms is fixed to a small value (≈ 0.18 m/s) during the whole experiment. This is due to the fact that the target moves with constant angular velocity and the paths are circles to all three vehicles (see 23).

Robust CMPF With Sliding Mode Term
The second experiment shows the results of the robust CMPF controller with velocity compensation and Sliding Mode term, with MPF control law given by (10) with ρ i = 0.2 for the three vehicles and ǫ w = 0.5 m. The consensus law for cooperation among the vehicles is given by (24), as before. Figure 5 shows the vehicle trajectories around the target, starting and ending in the southwest and southeast corners, respectively. Once more, due to communication losses and the presence of ocean currents in the southwest location, the secondary controller described in Remark 4.1 was activated for some of vehicles during the transient. However, the proposed controller was able to stabilize the error faster than the nominal controller. Besides, from Figure 6A, it is possible to notice the practical sliding mode phenomena around the origin e i ≡ 0. That means that the controller is able to achieve better performance than the previous one, given that ǫ w can be designed to be arbitrarily small. However, from (11), small values of ǫ w can result in higher gains for w i , which can potentially saturate the control inputs. In fact, sometimes the control saturation limits are reached after the transient, as shown in Figures 6C,D, and practical sliding mode is momentarily lost. The reason is the limited velocity range allowed by the actuators, combined with our particular value choice for ǫ w , and moments of occasional increase in the target velocity. Even so, performance is slightly better than in the previous case, and the amount of control chattering is acceptable.  The consensus law, error correction signals and rotation correction signals are omitted, but are similar to those observed in Figure 4.

Robust CMPF With Sliding Mode Term and Disturbance Compensation
The third and last experiment shows the results of the robust CMPF controller with velocity compensation, Sliding Mode term and direct disturbance compensation using a linear observer. The control law is given by (17) with ρ i = 0.2 and ǫ w = 0.5 m, as before. Again, the consensus law for cooperation among the vehicles is given by (24). As seen from Figure 7, only the vehicles Noptilus 1 and 3 were used on this experiment, since the battery on Noptilus 2 was depleted. However, the results obtained by Noptilus 1 and 3 can still be compared to the previous results obtained for the same two vehicles. The chosen paths are the same circles defined in (28), but this time with φ 1 = 0 and φ 3 = π rad. This modification was used to guarantee that the two vehicles stay as far as possible from each another. Once again, in Figure 8A, notice the practical sliding mode phenomena around the origin e i ≡ 0, except during the instants where the control inputs are saturated (Figures 8C,D). However, in this case, the control chattering is significantly smaller than the one observed in Figures 6C,D, under the same experimental conditions. We explain this fact by the presence of the disturbance estimator. Since part of the disturbance is compensated, the sliding mode term can spend less effort compensating the remaining total disturbance, a result compatible with the theoretical insight of Remark 3.2. The path variable errors remain bounded by 4 m, as shown in Figure 8B. The estimated disturbances are shown in Figure 9. The linear velocity disturbances remained bounded by < 0.3m s after the transient, while the angular velocity disturbances showed higher variation, but remained bounded to < 0.5rad s after the transient.

CONCLUSIONS
This work addressed the robust cooperative MPF problem for marine vehicles. We demonstrated that the origin of the MPF errors associated to the vehicles are stable with the two proposed robust CMPF control schemes in the presence of bounded disturbances acting on the vehicles. Furthermore, it was theoretically demonstrated that the cooperative control scheme is ISS with respect to the path variable estimation errors and to two other bounded, auxiliary input variables, named error correction term and rotation correction terms. The proposed cooperative controller. He also helped by suggesting important changes on the code and with the organization of the manuscript. AA contributed with the proposition of the disturbance compensation method for improving the performance of the first controller (Theorem 2), and also strongly contributed to the stability proofs and overall organization of the paper. JS made the experiments possible by setting up the mission at Porto de Leixões and has contributed by suggesting some changes on the code.