**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.

**Figure 12.** Real Medusa vehicles at Doca dos Olivais, Lisbon (Portugal).

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).

**Figure 13.** Simulated world of Doca dos Olivais with red chemical spill in Gazebo.

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.

**Figure 14.** Simulated vehicles in gazebo: (**a**) Medusa ASV. (**b**) Iris quadrotor with a fixed camera.

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◦, pointing downwards, and produced an image with a resolution of 640 × 480 px, according to Figure 15a. Its intrinsic parameters are given by:

$$\begin{cases} (c\_{\mathbf{x}\_{\prime}} c\_{\mathbf{y}}) = (320.5, 240.5) \\ (f\_d s\_{\mathbf{x}\_{\prime}} f\_d s\_{\mathbf{y}}) = (381.4, 381.4) . \end{cases} \tag{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].

**Figure 15.** Simulated camera feed: (**a**) Quadrotor's camera output. (**b**) Binary image.

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.

**Figure 16.** Planning and control architecture.

#### **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 *μ* = [−5, −5, 0] *<sup>T</sup>*. Both vehicles were required to follow the path at *V* = 0.5m/s and communications were bi-directional.

**Figure 17.** Real CPF mission with 2 Medusa vehicles.

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.

**Figure 18.** CPF with 2 real Medusa vehicles: (**a**) X–Y view. (**b**) Communication metrics.

*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 *μ*<sup>1</sup> = [−5, 5, 0] *<sup>T</sup>*m, and for Medusa 2, by *μ*<sup>2</sup> = [−5, −5, 0] *<sup>T</sup>*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.

**Figure 19.** CPF with simulated Iris and Medusa vehicles: (**a**) X–Y view. (**b**). Performance metrics.

#### *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 lawnmowing 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.

**Figure 20.** 3-D view of simulated boundary encircling mission with Iris and Medusa vehicles.

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 *μ* = [−5, 5, 0] *<sup>T</sup>*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.

**Figure 21.** Boundary encircling with simulated Iris and Medusa vehicles: (**a**) X–Y view. (**b**) Performance metrics.

From Figure 21b, it is also evident that the horizontal distance between the real drone's position and the boundary is bounded by 6m. 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.

**Figure 22.** Path generation (**a**) Time = 235 s. (**b**) Time = 558 s.
