Trajectory Planning for Cooperative Double Unmanned Surface Vehicles Connected with a Floating Rope for Floating Garbage Cleaning

Abstract

Double unmanned surface vehicles (DUSVs) towing a floating rope are more effective at removing large floating garbage on the water’s surface than a single USV. This paper proposes a comprehensive trajectory planner for DUSVs connected with a floating rope for cooperative water-surface garbage collection with dynamic collision avoidance, which takes into account the kinematic constraints and dynamic cooperation constraints of the DUSVs, which reflects the current collection capacity of DUSVs. The optimal travel sequence is determined by solving the TSP problem with an ant colony algorithm. The DUSVs approach the garbage targets based on the guidance of target key points selected by taking into account the dynamic cooperation constraints. An artificial potential field (APF) combined with a leader–follower strategy is adopted so that the each USV passes from different sides of the garbage to ensure garbage capturing. For dynamic obstacle avoidance, an improved APF (IAPF) combined with a leader–follower strategy is proposed, for which a velocity repulsion field is introduced to reduce travel distance. A fuzzy logic algorithm is adopted for adaptive adjustment of the desired velocities of the DUSVs to achieve better cooperation between the DUSVs. The simulation results verify the effectiveness of the algorithm of the proposed planner in that the generated trajectories for the DUSVs successfully realize cooperative garbage collection and dynamic obstacle avoidance while complying with the kinematic constraints and dynamic cooperation constraints of the DUSVs.

1. Introduction

In recent years, water pollution caused by floating garbage has become increasingly serious and patches of garbage often appear on water surfaces, as shown in Figure 1. It is very important to clean up the garbage in good time [1,2,3]. Currently, the main ways for cleaning water-surface garbage include manual salvage, mechanical salvage, and using clean-up boats. However, the safety risk of manual salvage is high, and cleaning that relies heavily on manual operations is inefficient and costly. Therefore, with the development of advanced technologies, surface garbage cleaning by unmanned surface vehicles [4,5,6] has attracted much research interest. In practice, full coverage of the targeted area is generally required for thorough cleaning, and single USVs are often limited in their capability to handle the tasks of garbage cleaning of a large area and large patches of congregated garbage [7,8,9,10]. Double USVs connected by a floating rope, as shown in Figure 2a, provide stronger load capacity and better fault tolerance and hence are more efficient at cleaning areas where the pollution is serious and garbage appears in patches.

Path planning is a critical part of the autonomous navigation of USVs for ensuring the safety and efficiency of the whole process of operation. It generally aims to generate a collision-free path from the starting point to the target position without explicit consideration of the constraints of vehicle dynamics and timing [11,12]. For multiple USVs conducting cooperative operations, the cooperation constraints between the USVs need to be taken into account, and certain pose relationships between the USVs need to be maintained at each moment [13]. In such cases, trajectory planning is necessary, which is concerned with generating the time-parameterized state trajectories for each USV at each moment. For water-surface garbage cleaning with cooperative double USVs connected with a floating rope, the two USVs are required to operate cooperatively to reach the designated positions for each USV at each time instant; therefore, physical, time, and position constraints need to be fulfilled simultaneously.

In trajectory planning for cooperative multi-USVs, the objectives considered include aspects of shortest travel distance, collision avoidance with safety distance, minimum calculation time, trajectory smoothness, and motion-related collaboration constraints such as collaborative behavior [14]. Collaborative behaviors are categorized into time collaborative behavior and time-and-position collaborative behavior [15,16]. The former requires that each USV leaves or reaches the target area at the same time or in sequence, while the latter requires not only time coordination but also the simultaneous position coordination of each USV, so that the trajectories meet the predefined time and spatial relationship to the greatest extent [17]. However, few pieces of research in the literature have worked on collaborative multi-USVs with physical connections while the movement of each USV is relatively free [18]. Formation control or generating cooperative trajectories for physically connected cooperative USVs is a much more challenging problem due to the extra constraints imposed by the physical connection.

Giron-Sierria et al. [19,20] studied the autonomous double USV towing system for recovering floating oil spills at sea. The Dubins path was used to generate the trajectories for the double USVs, which were composed of straight lines and arcs. Zhu et al. [21] proposed a distance-based cooperative control strategy with dual modes of formation and tracking for double USVs cleaning Enteromorpha. Factors such as power, torques in the floating rope, and interference of water flow were all considered in order to achieve intelligent selection of the operation mode of the double USVs. Jiang Wen [22] proposed a formation method based on heading information fusion and a trajectory tracking method based on null-space behavior fusion to deal with the problem of separation and entanglement in cooperative towing by double USVs. However, the studies mentioned above mainly focused on the formation control and coordinative behaviors of double USVs and considered only tasks of collecting a single pollutant target. The problem of planning cooperative trajectories for double USVs connected with a float rope for the whole task of cleaning a water-surface area with many garbage targets is yet to be solved. In particular, the influence of the relative positions of the two USVs on the load capacity of the floating rope need to be addressed.

During the mission of water-surface floating garbage cleaning, the locations of floating garbage and obstacles such as buoys and water plants change due to environmental disturbances such as wind, currents, and waves. Furthermore, dynamic obstacles may also include boats and animals such as ducks. Hence, it is important to take into account the position changes of garbage targets and obstacles in order to achieve the safe and efficient operation of the whole cleaning process. The trade-off between distance cost and computational consumption is one of the main issues in dealing with real-time planning. In the work by Kong et al. [23] for path planning for a single surface autonomous vehicle for floating garbage cleaning, path re-planning is conducted when changes in the positions of garbage areas are detected by the cooperative UAV and the re-planning point is decided based on a neural network to strike a compromise between distance cost and computational burden. The artificial potential field (APF) has been extensively used for local path planning, such as in dynamic obstacle avoidance. It determines the motion of the USV at the next time step according to the attractive fields of the targets and repulsive fields of obstacles in the surrounding environment and the current state of the USV. Its advantages are its simple structure, good real-time performance, and low computational consumption. However, APF-based methods are not apt at achieving the optimal path in the face of dynamic obstacles. To this end, Gao et al. [24] introduced into the traditional repulsion field function the dynamic distance between the robot and the obstacle and the adjustment factor to solve the problem of unreachable targets. Zhang et al. [25] proposed an improved artificial potential field by introducing a water flow field to solve the problem of unreachable targets. However, there is still a problem with dynamic obstacle avoidance, as the robot tends to bypass the obstacle from the front, leading to longer path lengths and failure in obstacle avoidance. Few pieces of research have studied APF for the cooperative operations of DUSVs, while it is found that the garbage collection problem for physically connected DUSV may be viewed as a special dynamic obstacle avoidance problem to be potentially solved by APF.

Motivated by the problems mentioned above, this paper studies the trajectory planning problem for cooperative double USVs connected with a floating rope for multi-floating garbage collection and dynamic obstacle avoidance. The main challenges to be addressed include the following: (1) Different from garbage collection by a single USV, which passes through the garbage, the floating rope connected to the two USVs needs to bypass the garbage closely from different sides of the garbage separately. (2) For dynamic obstacle avoidance, the double USVs need to pass from the same side of obstacles within a safe distance. (3) The distance between the two USVs needs to be dynamically adjusted to allow enough enclosed area within the floating rope for the current garbage target while maintaining between the minimum and maximum distance to avoid internal collision avoidance and garbage escaping.

Therefore, a comprehensive trajectory planner is proposed for multi-floating garbage collection and dynamic obstacle avoidance, taking into consideration kinematic constraints and dynamic cooperation constraints that reflect the time-varying load capacity of the DUSVs system. The planning assumes prior knowledge of the initial positions of all garbage areas and obstacles with the possible assistance of an unmanned aerial vehicle (UAV) and real-time updating of the positions of the encountered garbage and obstacles based on on-board sensors. An optimal sequence for visiting all the garbage areas is obtained based on the initial positions of the garbage areas. Then, the trajectories of the DUSVs for approaching each garbage area and capturing the garbage while avoiding all encountered obstacles are generated. The main contributions of this paper can be summarized as follows:

(1)

Kinematic constraints of the USV and dynamic cooperation constraint between the DUSVs are considered. The cooperation constraint in terms of distances between the two USVs reflects the current garbage collection capacity of the DUSVs.

(2)

For garbage collection, a method based on APF combined with a leader–follower strategy is proposed to ensure each USV bypasses the garbage from different sides in order to realize garbage capture.

(3)

For dynamic obstacle avoidance, a method based on an improved artificial potential field (APF) combined with a leader–follower strategy is proposed. The velocity repulsion field between DUSVs and dynamic obstacle is introduced to deal with the issue of long trajectories.

(4)

For precise cooperation between the DUSVs, a speed control method based on a fuzzy logic algorithm is proposed for adaptive adjustment of the acceleration and deceleration coefficients for the DUSVs.

2. The Problem Description of DUSVs Garbage Cleaning

Consider the scenario of a water area with multiple large areas of floating garbage and obstacles, both static and dynamic, such as aquatic plants, buoys, and ships. A scene model of water-surface garbage collection by floating-rope-connected DUSVs is established. As illustrated in Figure 2, the DUSVs system $(P_1,P_2)$ is cooperative in order to perform the autonomous floating garbage cleaning. It assumes that the initial positions of all garbage areas and obstacles are known with the assistance of an unmanned aerial vehicle (UAV), and positions of the encountered garbage and obstacles are updated based on on-board sensors. It aims to determine the cooperative dynamic trajectories of the underactuated DUSVs connected with a floating rope for cleaning all the floating garbage without any collisions with obstacles.

Assume that the start point is $S(x_s,y_s)$, the end point is $E(x_E,y_E)$, the center of the garbage target is $T_i(x_{T_i},y_{T_i})$, where $i=1,2\cdots p$, p is the number of garbage targets, and the velocity vector is $[v_{T_x},v_{T_y}]$. The center of obstacles is $O_i(x_{O_i},y_{O_i})$, where $i=1,2\cdots k$, k is the number of obstacles and the velocity vector is $[v_{O_x},v_{O_y}]$.

The objective for the trajectory planning problem of the DUSVs can be described as generating the two optimal trajectories, ${\tau }_i^d\left(t\right)=(x_i^d,y_i^d,{\psi }_i^d,t)$, $i=1,2$, of the DUSVs, starting from the launch point, S, capturing each garbage target, $T_i$, by the DUSVs passing from different sides of the garbage targets separately, avoiding all obstacles, $O_i$, by the two USVs passing from the same side of the obstacle, and finally reaching the end point, E, while abiding with the kinematic and cooperation constraints. The model of the obstacles and garbage targets, along with all the constraints, will be discussed in this section.

2.1. The Model of Obstacle and Garbage Target

2.1.1. Obstacle Model

Due to the uncertainty of obstacles on the water’s surface, it is necessary to reserve a sufficient safe distance between the obstacle and the hull. Therefore, the obstacle is modeled as a circle and expanded to ensure a safe distance. The point set of the obstacle area can be represented as:

$${\Omega }_O=\left\{p_O|{∥p_O-O_i∥}^2\le {\left(E_O{R}_O\right)}^2\right\},$$

(1)

where $p_O$ denotes any point in the obstacle area, $R_O$ represents the radius of the obstacle, and $E_O$ is the expansion coefficient of the obstacle.

2.1.2. Garbage Target Model

As shown in Figure 3, due to the uncertain shape of the surface garbage irregular garbage is modeled with a circle or an ellipse, which are expanded to ensure that the DUSVs can safely collect the garbage. The point set of the garbage target area can be expressed as follows:

$${\Omega }_T=\left\{p_T|\frac{{[(x_T-x_{Ti})cos\vartheta +(y_T-y_{Ti})sin\vartheta ]}^2}{{\left(E_T{R}_a\right)}^2}+\right.\left.\frac{{[(y_T-y_{Ti})cos\vartheta +(x_T-x_{Ti})sin\vartheta ]}^2}{{\left(E_T{R}_b\right)}^2}\le 1\right\},$$

(2)

where, $R_a$ and $R_b$ are the long and short half axis of the elliptical garbage target; when $R_a=R_b=R_T$, the model represents a circular garbage target; $\vartheta$ is the angle between the long half axis and the horizontal direction; $E_T$ represents the expansion coefficient of the garbage target.

2.2. The Kinematic Constraints of USV

Assume that the USVs move on a calm water surface, consider that the pose vector of the USV expressed in the world coordinate system is $\eta ={\left[x,y,\psi \right]}^T$, where x and y denote the position coordinate, and $\psi$ is the heading angle. The velocity vector of the USV in the body frame is denoted as $\upsilon ={\left[u,v,r\right]}^T$, where u and v are the surge and sway velocities, and r is the yaw angular velocity. Hence, the three-degree-of-freedom model of the USV in the horizontal plane can be written as:

$$\dot{\eta }=J\left(\psi \right)\upsilon ,$$

(3)

where, $J\left(\psi \right)=\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]$.

On account of the fact that the USVs are underactuated and the sway velocity is relatively small in calm water, the zero sway velocity is assumed, $v=0$. The surge speed, u, surge acceleration, a, and sway angular velocity, r, must satisfy the following constraints:

$$∥u∥\le u_{max},$$

(4)

$$∥a∥\le a_{max},$$

(5)

$$∥r∥\le r_{max}.$$

(6)

In addition, the surge velocity and yaw angular velocity of the USV are related by $R=|\frac{u}{r}|$, where R is the turning radius. Therefore, the references $u^d$ and $r^d$ need to meet the constraint for R,

$$R\ge R_{min},$$

(7)

where, $R_{min}$ is the minimum turning radius.

Thus, the reference trajectory, ${\tau }^d\left(t\right)$, of the USV can be expressed as:

$$\left(\begin{array}{c}\hfill x_t^d\hfill \\ \hfill y_t^d\hfill \\ \hfill {\psi }_t^d\hfill \end{array}\right)=\left(\begin{array}{c}\hfill x_{t-1}^d\hfill \\ \hfill y_{t-1}^d\hfill \\ \hfill {\psi }_{t-1}^d\hfill \end{array}\right)+\left(\begin{array}{c}\hfill u_{t-1}^{d}cos{\psi }_{t-1}^d\hfill \\ \hfill u_{t-1}^{d}sin{\psi }_{t-1}^d\hfill \\ \hfill n·|r_{t-1}^d|\hfill \end{array}\right)\Delta t,$$

(8)

where $u^d$ and ${\psi }^d$ are the expected linear velocity and angular velocity, satisfying constraints (4)–(6). n is used to explicitly represent the three decisions for steering the USV, i.e., $n=1,0,-1$, corresponding to turning right, heading forward, and turning left.

2.3. The Cooperation Constraints of DUSVs

In this section, the dynamic cooperation constraints in terms of the distance between the two USVs are analyzed. Aspects of internal collision avoidance, garbage escaping, and dynamic load capacity in terms of distance between the two USVs are considered.

First and foremost, there is the lower bound on distance between the DUSVs set to avoid internal collisions, and the upper bound set to avoid garbage escaping:

$$L_{min}<L<L_{max},$$

(9)

where $L=∥P_1-P_2∥$ is the distance between the two USVs.

Figure 4 shows the area, S, enclosed by the DUSVs and the floating rope in two different states. The state shown in Figure 4a is the desired state, with the two USVs in parallel and the enclosed area, S, larger than in Figure 4b, which is the undesired state. In the desired state, the enclosed area, S, is determined by the length of the floating rope, $l_p$, and the distance, L, between the DUSVs.

Define the current area demand, $S_t$, as the sum of the areas of the garbage targets that have already been collected and the current garbage target to be connected. To allow the collection of the current garbage target, it is important to adjust L so that:

$$S>S_t,$$

(10)

where S is the current enclosed area of the floating rope.

Next, we are to find the relationship between S and L for the desired state. The floating rope is under tension on the water surface, and the drag of the water causes it to deform. The shape of the floating rope can be approximated by a catenary suspended in the air [26]. A coordinate system with its origin fixed at the lowest point of the rope is selected, as shown in Figure 4a, and  the equation that describes the curve of the floating rope is given as:

$$y=b(cosh\frac{x}{b}-1)b>0,$$

(11)

where $(x,y)$ denotes any points of the floating rope curve. b is the catenary coefficient and take $b>0$. The length of the floating rope, $l_p$, can be expressed as:

$$l_p=2bsinh\frac{x_d}{b},$$

(12)

where $x_d$ denotes the abscissa of the endpoint of the floating rope curve.

To illustrate the relationship between S and L, suppose $l_p=16$ m. The relationship between $x_d$ and b is depicted in Figure 5a. The endpoint abscissa is considered to be positive, $x_d>0$, for ease of analysis. This shows that $x_d$ and b are positively correlated. By integration, the area surrounded by the floating rope, S, is given as follows:

$$S=2b{x}_{d}cosh\frac{x_d}{b}-2b^{2}sinh\frac{x_d}{b}.$$

(13)

Based on Formulas (12) and (13), the relationship between S and b can be determined and is shown in Figure 5b. It shows that S reaches its maximum of 39.5 ${\mathrm{m}}^2$ when $b=3.8$, which corresponds to $x_d=5.5$ and $L=11$, as indicated by Figure 5a. Therefore, it can be concluded that S continuously decreases as L decreases from 11. Therefore, it is reasonable to select the upper bound of the distance between the DUSVs as $L_{max}=11$, and  L could be adjusted in real-time according to the current area demand, $S_t$.

To sum up, the constraints (9) and (10) are the cooperation constraints for the DUSVs. During mission planning, it is necessary to estimate the total area of all garbage to be collected and then choose a floating rope of appropriate length to ensure that all garbage can be collected in one single mission. In the case when the garbage cannot be handled by one single mission, multiple missions needs to be planned. During the mission, the states S and $S_t$ are checked in real-time, and L is adjusted accordingly to make sure that the cooperation constraints are satisfied.

3. The Structure of the Algorithm

To obtain optimal dynamic trajectories for the DUSVs that can access all floating garbage targets non-repeatedly while avoiding dynamic obstacles, the first step is to determine the optimal access sequence of the multiple garbage targets based on the principle of the shortest travel distance. During the mission, dynamic trajectories are generated by following the optimal access order. For each garbage target, there are the garbage approaching phase and the garbage capturing phase. In the garbage approaching phase, the trajectories are generated based on the guidance of key points around the current target. For garbage capturing, the APF method and a leader–follower strategy are proposed, and the garbage capturing phase start once the DUSVs enter the repulsive field of the current garbage target. When obstacles are encountered, i.e., the DUSVs enter the repulsive field of an obstacle, trajectories for dynamic obstacle avoidance are generated. A method based on IAPF and a leader–follower strategy is adopted, which takes into account the relative speed between the obstacle and the USVs so that the DUSVs bypass the obstacle from the same side of the obstacle with shorter travel distance. The generated trajectories need to meet the kinematic constraints and cooperation constraints. A speed control strategy based on a fuzzy logic algorithm is introduced to keep the DUSVs in good cooperation. A flowchart of the proposed trajectory planning method is depicted in Figure 6.

3.1. The Optimal Access Sequence of Multiple Garbage Targets

There are p garbage areas scattering over the water surface, which are described as $\left\{T_1,T_2,T_3...T_p\right\}$. The aim is to find the optimal access sequence, $\Gamma =\{S,T_{c\left(1\right)},T_{c\left(2\right)}\cdots T_{c\left(p\right)},E\}$, starting from the launch point, S, through each garbage area only once, and finally reaching the end point, E. This problem is established as a traveling salesman problem (TSP) to find a solution that minimizes the total length of the path:

$${\Gamma }^{*}=arg\underset{\Gamma }{min}\sum _{i=1}^{p+1}d(\Gamma (i),\Gamma (i+1\left)\right),$$

(14)

where $d(\Gamma (i),\Gamma (i+1\left)\right)$ is the Euclidean distance between the center of two garbage targets.

An ant colony algorithm [27] is adopted for solving the established TSP to strike a balance between computational complexity and performance for the problem of water-surface cleaning. Yang et al. [28] employed the discrete grouping teaching optimization algorithm (DGTOA) to solve the multi-point path planning problem with many target points. Jindriska et al. [29] proposed a new improved heuristic method to solve the sufficiently close enough TSP problem (CETSP) in an environment with obstacles and many densely distributed targets. Both of the algorithms mentioned above are complex and computationally costly. In comparison, the TSP problem for water-surface garbage cleaning has relatively fewer target points. Therefore, the ant colony algorithm is selected for its advantages of strong global searching ability and easy implementation with less computational cost.

3.2. DUSV Cooperative Garbage Target Approaching and Capturing

Following the optimal access sequence, the DUSVs approach and capture the garbage targets one by one while bypassing all obstacles whenever encountered. In this section, the algorithm for generating trajectories for garbage target approaching and capturing is presented.

3.2.1. Garbage Approaching Based on the Guidance of Target Key Points

To generate the trajectories for approaching the current garbage target, the headings of the two USVs are guided by the two key points of the garbage target, i.e., the orientations from each USV towards the two key points are respectively chosen as the desired headings for the DUSVs, in order to realize that each USV passes the garbage target from different sides. The two key points of the current garbage target are selected based on the garbage model, and the dynamic cooperation constraints as described in Section 2.3 are considered.

The two key points are initially chosen as the resulting tangent points, $N_i(x_{Ni},y_{Ni})$, $i=1,2$, by making tangents from the current reference position of the two USVs to the current garbage target modeled as a circle or ellipse, as shown in Figure 7. Then, adjustment, $\delta$, is made when necessary so that the distance between the two key points meets cooperation constraints (12) and (13). If $\delta =0$, the key points are just the two tangent points of $N_i$ on the circle or ellipse.

Therefore, the current expected heading, ${\psi }_i^d$, of the DUSVs is given by:

$${\psi }_i^d=\frac{\pi }{2}-{\theta }_i,$$

(15)

where

$${\theta }_i={tan}^{-1}[\left|y_N-y_i\right|/\left|x_N-x_i\right|].$$

(16)

3.2.2. Garbage Capturing Based on APF and Leader–Follower Strategy

When the DUSVs enter within a certain distance of the current garbage target, the system enters into the garbage capturing stage. For garbage capturing, the trajectories for the double USVs need to pass the garbage from different sides without contacting the garbage. However, during the capturing phase and the phase of switching from the current garbage target to the next garbage target, there is a risk of colliding with the current garbage target. To address this issue, APF combined with a leader–follower strategy is proposed to generate the cooperative trajectory for capturing the garbage target without collision.

The artificial potential field consists of an attractive field and a repulsive field [30]. To prevent collisions with the current garbage target and to take the next garbage target in the sequence into consideration, a repulsion field is set up for the current garbage target, $T_i$, and an attractive field for the next garbage target, $T_{i+1}$, as shown in Figure 8.

The function of the repulsion field of the current garbage target, ${U_{rep}}_{T_i}\left(P\right)$, and its gradient are given as follows:

$$U_{{rep}_{T_i}}\left(P\right)=\left\{\begin{array}{cc}\frac{1}{2}\eta {\left(\frac{1}{\rho \left(P,T_i\right)}-\frac{1}{{\rho }_{T_i}}\right)}^2& ,\rho \left(P,T_i\right)\le {\rho }_{T_i}\\ 0& ,\rho \left(P,T_i\right)>{\rho }_{T_i}\end{array}\right.$$

(17)

$$\nabla U_{re{p}_{T_i}}\left(P\right)=\left\{\begin{array}{cc}\eta \left(\frac{1}{\rho \left(P,T_i\right)}-\frac{1}{{\rho }_{T_i}}\right)·\frac{\nabla \rho \left(P,T_i\right)}{{\rho }^2\left(P,T_i\right)}& ,\rho \left(P,T_i\right)\le {\rho }_{T_i}\\ 0& ,\rho \left(P,T_i\right)>{\rho }_{T_i}\end{array}\right.,$$

(18)

where $\eta$ denotes the repulsion gain coefficient, $\rho (P,T_i)=∥P-T_i∥$ is the distance from the USV to the center of the current garbage target, $T_i$, as shown in Figure 7, and the distance of influence of the garbage target, $T_i$, is given by ${\rho }_{T_i}$. The repulsive field is defined as zero outside the distance of influence.

The attractive field function of the next garbage target is given as:

$$U_{{att}_{T_{i+1}}}\left(P\right)=\left\{\begin{array}{cc}\frac{1}{2}\xi {\rho }^2\left(P,T_{i+1}\right)& ,\rho \left(P,T_{i+1}\right)\le {\rho }^t\\ {\rho }^t\xi \rho \left(P,T_{i+1}\right)-\frac{1}{2}\xi {\left({\rho }^t\right)}^2& ,\rho \left(P,T_{i+1}\right)>{\rho }^t\end{array}\right..$$

(19)

As can be seen, ${U_{att}}_{T_{i+1}}\left(P\right)$ is divided into two parts by introducing the interim distance, ${\rho }^t$, to avoid the problem of excessive attractive force due to the long distance to the next garbage target [31]. The corresponding gradient function is given as follows:

$$\nabla U_{at{t}_{T_{i+1}}}\left(P\right)=\left\{\begin{array}{cc}\xi \rho \left(P-T_{i+1}\right)& ,\rho \left(P,T_{i+1}\right)\le {\rho }^t\\ {\rho }^t\xi & ,\rho \left(P,T_{i+1}\right)>{\rho }^t\end{array}\right.,$$

(20)

where $\xi$ denotes the positive proportional coefficient, $\rho (P,T_{i+1})=∥P-T_{i+1}∥$ is the distance between the USV and the next garbage target, $T_{i+1}$, and ${\rho }^t$ is the distance of influence of the next garbage target.

The total potential field and force are the sum of the attractive and repulsive fields and the sum of the attractive and repulsive forces, respectively:

$$U_{T_i}\left(P\right)={U_{att}}_{T_{i+1}}\left(P\right)+{U_{rep}}_{T_i}\left(P\right)$$

(21)

$$F_{T_i}\left(P\right)=F_{{att}_{T_{i+1}}}+F_{{rep}_{T_i}},$$

(22)

where $F_{{att}_{T_{i+1}}}=-\nabla U_{at{t}_{T_{i+1}}}\left(P\right)$ and $F_{{rep}_{T_i}}=-\nabla U_{re{p}_{T_i}}\left(P\right)$.

When either USV enters the distance of influence of the repulsive field of the current garbage target, $T_i$, the system enters the phase for capturing the current garbage target. The USV that first enters is designated as the leader, and the other USV is in turn the follower. The expected pose of the leader at the next time instant, $\tau (t+1)={\left[\begin{array}{ccc}{x}_{t+1}& y_{t+1}& {\psi }_{t+1}\end{array}\right]}^T$, is determined by Equation (8), for which we needs to determine n, with $n=1,0,-1$ corresponding to turning right, heading forward, and turning left, respectively. n is chosen among the three directions and along the greatest descending direction of the potential field according to Equation (22). The follower takes the opposite turning direction to the leader to ensure that the two USVs can pass from different sides of the garbage target at the same time.

Once the leader gets out of the distance of influence of the repulsive field of the current target, the DUSVs complete the capturing of the current target and switch to the next garbage target.

3.3. Cooperative Dynamic Obstacle Avoidance Based on IAPF Method and Leader–Follower Strategy

During the mission, the DUSVs may encounter static as well as dynamic obstacles on the water surface. In this section, the algorithm for generating trajectories for dynamic obstacle avoidance is presented. When the DUSVs travel within a certain distance of the obstacle, the system enters into the phase for obstacle avoidance. The trajectories for DUSVs need to pass the obstacle from same side of the obstacle without getting the floating rope entangled with the obstacle. To address this issue, IAPF combined with a leader–follower strategy is proposed.

To prevent collisions with the encountered obstacle, $O_i$, while considering capturing the current garbage target, $T_i$, the potential field is comprised of the repulsive field of the current obstacle, $O_i$, and the attractive field of the current garbage target, $T_i$, as shown in Figure 9.

By the traditional APF method, the repulsion field function, ${U_{rep}}_{O_i}\left(P\right)$, is obtained by:

$$U_{re{p}_{O_i}}\left(P\right)=\left\{\begin{array}{cc}\frac{1}{2}\eta {\left(\frac{1}{\rho \left(P,O_i\right)}-\frac{1}{{\rho }_{O_i}}\right)}^2& ,\rho \left(P,O_i\right)\le {\rho }_{O_i}\\ 0& ,\rho \left(P,O_i\right)>{\rho }_{O_i}\end{array}\right..$$

(23)

The corresponding repulsive force is given by the gradient function as follows:

$$\nabla U_{re{p}_{O_i}}\left(P\right)=\left\{\begin{array}{cc}\eta \left(\frac{1}{\rho \left(P,O_i\right)}-\frac{1}{{\rho }_{O_i}}\right)·\frac{\nabla \rho \left(P,O_i\right)}{{\rho }^2\left(P,O_i\right)}& ,\rho \left(P,O_i\right)\le {\rho }_{O_i}\\ 0& ,\rho \left(P,O_i\right)>{\rho }_{O_i}\end{array}\right.,$$

(24)

where $\eta$ denotes the repulsion gain coefficient, $\rho (P,O_i)=∥P-O_i∥$ is the distance between the USV to the center of the obstacle, $O_i$, and ${\rho }_{O_i}$ is the distance of influence of the repulsive force field, within which the repulsive force takes effect.

The traditional APF method can quickly plan collision-free trajectories in the environment with only static obstacles. However, when dealing with complex dynamic obstacles, its adaptability decreases, leading to poor performance in obstacle avoidance. To deal with the issue, we propose to introduce into the repulsion field function an extra term called the relative velocity repulsive field, which takes into account the relative velocity between the DUSVs and the obstacles. The improved repulsion field function, $U_{re{p}_{O_i}}^{\prime }\left(P\right)$, is given as:

$$U_{re{p}_{O_i}}^{\prime }\left(P\right)=\left\{\begin{array}{cc}{U}_{re{p}_{O_i}}\left(P\right)+U_{re{p}_v}& ,\rho \left(P,O_i\right)\le {\rho }_{O_i}\wedge \left({\sigma }_1,{\sigma }_2\in (0,\pi )\right)\hfill \\ U_{re{p}_v}& ,\left({\rho }_{O_i}<\rho \left(P,O_i\right)\le {\rho }_{O_i}^{\prime }\right)\wedge \left({\sigma }_1,{\sigma }_2\in (0,\pi )\right)\hfill \\ 0& ,\rho \left(P,O_i\right)>{\rho }_{O_i}^{\prime }\hfill \end{array}\right.,$$

(25)

where $U_{re{p}_v}=\frac{1}{2}\frac{\varsigma {\left|u_{or}\right|}^2}{\rho (P,O_i)}$ is the proposed repulsive potential field with regard to relative velocity between the USV and the obstacle, $u_{o{r}_i}=u_i-u_o$, as shown in Figure 10, $\varsigma$ denotes the coefficient for the relative velocity repulsive field, and ${\sigma }_i$ denotes the orientation of $u_{{or}_i}$ with respect to $P{O}_i$. When ${\sigma }_1,{\sigma }_2\in (0,\pi )$, the extension line of the speed vector of the obstacle and the extension lines of the speed vector of the USV intersect, which implies that the DUSVs are at risk of colliding with the dynamic obstacle in the near future. ${\rho }_{O_i}^{\prime }$ is selected as the distance of influence of the encountered dynamic obstacle on account of the relative velocity, which is larger and thus more conservative than the distance of influence, ${\rho }_{O_i}$, for static obstacles. When ${\sigma }_1$ and ${\sigma }_2$ are beyond $(0,\pi )$, the USV and the dynamic obstacle move away from each other; hence, the velocity repulsion is defined as zero. In this way, the relative velocity repulsion potential field help the USVs avoid the dynamic obstacle in time.

The corresponding gradient function, $\nabla U_{re{p}_v}^{\prime }\left(P\right)$, is obtained by:

$$\nabla U_{re{p}_{O_i}}^{\prime }\left(P\right)=\left\{\begin{array}{cc}\eta \left(\frac{1}{\rho \left(P,O_i\right)}-\frac{1}{{\rho }_{O_i}}\right)·\frac{\nabla \rho \left(P,O_i\right)}{{\rho }^2\left(P,O_i\right)}+\frac{\varsigma \left|u_{or}\right|}{\rho \left(P,O_i\right)}& ,\rho \left(P,O_i\right)\le {\rho }_{O_i}\wedge \left({\sigma }_1,{\sigma }_2\in (0,\pi )\right)\hfill \\ \frac{\varsigma \left|u_{or}\right|}{\rho \left(P,O_i\right)}& ,\left({\rho }_{O_i}<\rho \left(P,O_i\right)\le {\rho }_{O_i}^{\prime }\right)\wedge \left({\sigma }_1,{\sigma }_2\in (0,\pi )\right)\hfill \\ 0& ,\rho \left(P,O_i\right)>{\rho }_{O_i}^{\prime }\hfill \end{array}\right..$$

(26)

The attractive field, ${U_{att}}_{T_i}\left(P\right)$, of the current garbage target is expressed as:

$$U_{{att}_{T_i}}\left(P\right)=\left\{\begin{array}{cc}\frac{1}{2}\xi {\rho }^2\left(P,T_i\right)& ,\rho \left(P,T_i\right)\le {\rho }^o\\ {\rho }^o\xi \rho \left(P,T_i\right)-\frac{1}{2}\xi {\left({\rho }^o\right)}^2& ,\rho \left(P,T_i\right)>{\rho }^o\end{array}\right..$$

(27)

The corresponding gradient function, $\nabla {U_{att}}_{T_i}\left(P\right)$, is obtained by:

$$\nabla U_{at{t}_{T_i}}\left(P\right)=\left\{\begin{array}{cc}\xi \rho \left(P-T_i\right)& ,\rho \left(P,T_i\right)\le {\rho }^o\\ {\rho }^o\xi & ,\rho \left(P,T_i\right)>{\rho }^o\end{array}\right.,$$

(28)

where $\xi$ denotes the positive proportional coefficient, $\rho (P,T_i)=∥P-T_i∥$ is the distance between the USV and the current garbage target, $T_i$, and ${\rho }^o$ is the distance of influence of the current garbage target. In addition, to avoid excessive attractive force caused by a long distance from the garbage target, ${U_{att}}_{T_i}\left(P\right)$ is divided into two parts [31].

Within the distance of influence, ${\rho }_{O_i}^{\prime }$, of the obstacle, the total potential field is the sum of the repulsive and attractive fields:

$$U_{O_i}\left(P\right)=U_{at{t}_{T_i}}\left(P\right)+U_{re{p}_{O_i}}^{\prime }\left(P\right).$$

(29)

The corresponding total force is the sum of repulsive and attractive forces:

$$F_{O_i}\left(P\right)=F_{{rep}_{O_i}}+F_{{att}_{T_i}},$$

(30)

where $F_{{rep}_{O_i}}=-\nabla U_{{rep}_{O_i}}\left(P\right)$ and $F_{{att}_{T_i}}=-\nabla U_{{att}_{T_i}}\left(P\right)$.

The USV that first enters within the distance of influence, ${\rho }_{O_i}^{\prime }$, of the obstacle is designated as the leader, and the other USV in turn becomes the follower, whose heading is determined according to the leader. The expected pose of the leader at the next time instant, $\tau (t+1)={\left[\begin{array}{ccc}{x}_{t+1}& y_{t+1}& {\psi }_{t+1}\end{array}\right]}^T$, is given according to Equation (8), for which n needs to be determined, with $n=1,0,-1$, corresponding to the three turning directions (right, forward, and left), respectively. n is selected among the three directions with the greatest potential descend based on Equation (30). The follower makes the same turning as the leader to ensure that the DUSVs can pass from the same side of the obstacle at the same time.

Once the leader gets out of the obstacle avoidance area, i.e., the distance of influence of the potential field of the obstacle, $\rho (P,O_i)\ge {\rho }_{O_i}^{\prime }$, the trajectories for avoiding the obstacle are complete and the DUSVs continue with the approaching and capturing of the current garbage target, $T_i$, as described in Section 3.2.

3.4. Fuzzy Logic-Based Speed Control Strategy

During the mission, the DUSVs need to keep in good cooperation at a low speed, $u_i<2$ m/s. The two USVS in perfect cooperation should reach the two target key points, $N_1,N_2$, of the garbage target, respectively, at the same time. However, an undesired state, as shown in Figure 4b, may occur when the longitudinal distance between the two USVs increases, which increases the risk of garbage escaping.

A fuzzy logic-based speed controller is proposed to gradually reduce the longitudinal distance between the DUSVs for achieving the desired cooperative performance of the DUSVs. The difference of the distances between the two USVs and their respective target key points is defined as:

$$L_{dif}=\left|∥P_1-N_1∥-∥P_2-N_2∥\right|.$$

(31)

The main principal of the proposed speed control strategy is that when USV $P_1$ is further away from its target key point, then $P_1$ accelerates and USV $P_2$ decelerates; on the contrary, $P_2$ accelerates and $P_1$ decelerates to ensure that $L_{dif}<g$, where g is the upper bound of $L_{dif}$. A tuning coefficient, $\lambda$, is introduced for the speed control, as follows:

$$\left[\begin{array}{c}{u}_1(k+1)\\ u_2(k+1)\end{array}\right]=\left\{\begin{array}{cc}\left[\begin{array}{c}{u}_1\left(k\right)\\ u_2\left(k\right)\end{array}\right]+\left[\begin{array}{c}\lambda \\ -\lambda \end{array}\right]\Delta u,& L_{dif}>0\\ \left[\begin{array}{c}{u}_1\left(k\right)\\ u_2\left(k\right)\end{array}\right]+\left[\begin{array}{c}-\lambda \\ \lambda \end{array}\right]\Delta u,& L_{dif}<0\end{array}\right.,$$

(32)

where $\lambda$ represents the tuning coefficient for acceleration and deceleration of the USVs; $\Delta u$ denotes the speed increment for a sampling period, h, with the prescribed acceleration; a, $u_1\left(k\right)$, and $u_2\left(k\right)$ are the surge velocities of the DUSVs; and $u_1(k+1)$ and $u_2(k+1)$ are the surge velocities at the next time instant. A simple speed compensator with the same $\lambda$ for different $L_{dif}$ has poor performance, resulting in garbage escape and poor coordination between the DUSVs. A fuzzy logic algorithm is seen as an effective method for parameter adaptation [32,33]. Therefore, a fuzzy logic algorithm is adopted for adaptive adjustment of the tuning coefficient, $\lambda$, according to $L_{dif}$.

Hence, the distance difference, $L_{dif}$, is taken as the input of the fuzzy speed controller. Four fuzzy subsets (N, M, F, and VF) are defined over the value interval [0, 10] of $L_{dif}$. The output of the fuzzy speed controller is the coefficient $\lambda$, which takes values in the range of [0, 3], and four fuzzy subsets (S, M, B, and VB) are defined over this range. The Gaussian function is chosen as the membership functions for all variables. The specific description of fuzzy rules is shown in

Table 1. Fuzzy rules for the speed control method.

Number	Input ${\mathit{L}}_{\mathbf{dif}}$	Output $\mathit{\lambda }$
1	N	S
2	M	M
3	F	B
4	VF	VB

.

3.5. Section Summary

In summary, during mission planning, with the information of initial locations of the garbage targets, a floating rope of appropriate length is chosen and the optimal access sequence of the garbage targets is determined. After that, the DUSVs are launched. During the mission, dynamic trajectories are planned for the DUSVs, involving cooperative garbage target approaching, garbage capturing, and dynamic obstacle avoidance. The integration of the APF/IAPF method and multi-strategies including target key point-based guidance, a leader–follower strategy, dynamic cooperation constraint, and fuzzy logic-based speed control strategy help to generate the desired trajectory. To facilitate the understanding of the trajectory generation process, the pseudo-code of the algorithm is presented in Algorithm  1.

Algorithm 1: DUSV cooperative dynamic trajectory planning based on improved artificial potential field fusion multi-strategy

Input:  Locations of obstacles $O_i$ and garbage $T_i$, initial states $\eta$ and $\upsilon$ of DUSVs, constraints $u_{max}$, $a_{max}$, $L_{min}$, and $L_{max}$, and the optimal visiting sequence $\Gamma =\{S,T_{c\left(1\right)},T_{c\left(2\right)}\cdots T_{c\left(p\right)},E\}$; Output:  Optimal trajectories of DUSVs ${\tau }^d\left(t\right)$; 1: Initialization; Choose the current visiting target as $T_{c\left(i\right)},i=1$ 2: while True do 3:     if $L_{dif}>h$ then 4:         Adjust the speed $u_1,u_2$ of DUSVs to improve the coordination of the DUSVs so that $L_{dif}<g$ and constraint (9) are satisified. 5:     end if 6:     if $P_1,P_2$ are outside of the field range ${\rho }_{T_i}$ or ${\rho }_{O_i}^{\prime }$ of of the current garbage or any obstacle, respectively then 7:         Approach $T_{c_i}$, adjust the key points $N_1,N_2$ by constraint (9) and (10). Generate the next moment trajectory points $P_1(t+1),P_2(t+1)$. 8:     else 9:           if Either $P_1$ or $P_2$ enter the range ${\rho }_{T_i}$ of repulsion field of the current target $T_{c\left(i\right)}$ then 10:            Assign the leader and the follower; Generate the trajectories for the leader based on APF method;The follower takes the opposite turning direction; 11:              if the leader gets out of ${\rho }_{T_i}$ then 12:                  Capturing of garbage $T_{c_i}$ completes; Switch to the next visiting target $i=i+1$; 13:              end if 14:           else if Either $P_1$ or $P_2$ enters the range ${\rho }_{O_i}^{\prime }$ of the relative velocity repulsion field of the any obstacle and ${\sigma }_1,{\sigma }_2\in (0,\pi )$ then 15:              Assign the leader and the follower; The local trajectory for the leader is generated by IAPF; the follower takes the same turning direction; 16:              if the leader gets out of obstacle avoidance region ${\rho }_{O_i}^{\prime }$ then 17:                  Obstacle avoidance completes, break; 18:              end if 19:           end if 20:     end if 21:     if the current visiting target is the end point then 22:         Approach the end point; The trajectories ${\tau }^d\left(t\right)$ completes, the task is completed, break; 23:     end if 24: end while 25: Output optimal trajectories ${\tau }^d\left(t\right)$.

4. Simulation and Discussion

In this section, a simulation is conducted to verify the performance of the proposed trajectory planning algorithm, specifically, the effectiveness of garbage collection, dynamic obstacle avoidance, and the satisfaction of the cooperation constraints in terms of speed and distance coordination between the two USVs.

4.1. The Simulation Configurations

An environmental map of 50 m × 50 m is set up with starting and ending points at different positions. Within this map, multiple large areas of garbage with a radius between 1–2 m and obstacles of radius between 2–3 m are randomly generated. Details of the configurations of the simulation are listed in

Table 2. Configurations parameters of the simulation scenarios.

Scence	Centric Position	Long Half Shaft × Short Half Shaft/m	Radius/m	Bevel Angle/°	Time Step	Velocity/m/s
S	(10, 0)
E	(40, 0)
$T_1$	(16, 20)	1.5 × 1		90	t = 0	[−0.1, −0.1]
$T_2$	(25, 30)		1		t = 16	[0.1, −0.2]
$T_3$	(44, 10)		1.5		t = 38	[0.3, 0]
$T_4$	(38, 36)		1		t = 25	[0, −0.1]
$T_5$	(46, 25)		1.5		t = 31	[0.1, 0]

.

To verify the effectiveness of the proposed algorithm for solving dynamic trajectory planning problems, both static obstacles and dynamic obstacles are added to the simulations. The relevant parameters are set as shown in

Table 3. Parameters of static and dynamic obstacles.

	Obstacles	Start Point	Radius/m	Velocity/m/s
Environment	$O_1$	(17, 28)	2	[0.2, −0.2]
	$O_2$	(17, 10)	3
	$O_3$	(38, 18)	2	[0.2, 0]

.

The settings of the parameters of the trajectory planner used in the simulations are shown in

Table 4. Parameter settings for the simulations.

	Parameter Name	Parameter Setting
Problem model	The length of the floating rope, $l_p$	16 m
	The lower bound on distance between DUSVs, $L_{min}$	2 m
	Total garbage area	8$\pi$
	The maximum enclosed area of the floating rope	39.5 ${\mathrm{m}}^2$
Dynamic obstacle avoidance phase	Attractive proportional coefficient, $\xi$	1
	The distance of influence, ${\rho }^o$, of the garbage target, $T_i$	4 m
	Repulsion gain coefficient, $\eta$	400
	The distance of influence, ${\rho }_O$, of the obstacle, $O_i$	4 m
	Relative velocity repulsion field coefficient, $\varsigma$	2
	Relative velocity repulsion field range, ${\rho }_O^{\prime }$	6 m
Garbage capturing phase	Attractive proportional coefficient, $\xi$	1
	The distance of influence, ${\rho }^t$, of the garbage target, $T_{i+1}$	5 m
	Repulsion gain coefficient, $\eta$	400
	The distance of influence, ${\rho }_T$, of the garbage target, $T_i$	0.2 m

.

4.2. The Simulation Results

The simulation results are analyzed from the following two parts. Firstly, the whole generated reference trajectories of the DUSVs are shown in Figure 11 to visually verify effective garbage approaching, capturing, and obstacle avoidance. The effectiveness of the improved method in dynamic obstacle avoidance is stressed with results in Figure 11 and

Table 5. Algorithm data in dynamic environment under different algorithms.

Simulation Environment	Algorithm	Trajectory Length	Navigation Time
Environment	TAPF	177 m	55 s
	IAPF	174.3 m	54 s

. Secondly, the changes of the relevant state variables over time, including the speed of the DUSVs, distances as well as distance differences to the key points between the DUSVs, and the resulting area capacities, are shown in Figure 12, which are to verify the satisfaction of cooperation constraints.

Figure 11 shows the generated reference trajectories of the DUSVs for cooperative surface garbage cleaning in the environment with static and dynamic obstacles. The results obtained by the proposed algorithm using the IAPF method is compared with that using the traditional APF method. The trajectories are generated as discrete reference positions and orientations for the DUSVs with time intervals of 0.1 s. The triangles appearing at time intervals of 1s represent the time-varying poses of the DUSVs. In particular, the coordinates and time of the trajectories are shown when the DUSVs enter the garbage target capture area. The black dotted arrows and red arrows near the obstacles and garbage targets indicate the motion direction of the corresponding obstacles and garbage targets. The gray and pink shaded wide trails indicate the trajectories of the obstacles and the garbage targets.

This shows that the computed optimal access sequence of the garbage targets is $\left\{S,T_1,T_2,T_4,T_5,T_3,E\right\}$. Both algorithms have shown their capability to generate the trajectories for the DUSVs to capture all the garbage while avoiding all the obstacles. In the beginning, the DUSVs travel towards the current garbage target, $T_1$; the distance between the DUSVs is gradually increased to ensure safety as well as to meet the current area demand of $T_1$. When entering the distance of influence of the potential field of $T_1$, the DUSVs pass from different sides of $T_1$, so that the garbage can be captured by the floating rope while being attracted towards the next garbage, $T_2$. Following that, dynamic obstacle $O_1$ is encountered and the DUSVs pass from the same side of the obstacle under the guidance of the repulsive potential field and the attractive potential of the current garbage target, $T_2$. When the DUSVs leave the repulsive potential field of $O_2$, they carry on to approach the current target, $T_2$, and the distance between the two USVs is adjusted to meet the current area demand, i.e., the sum of areas of $T_1$ and $T_2$. When $T_2$ is captured, the DUSVs switch to approach the next target, $T_4$, and so on until they finish with the final target, $T_3$, and travel to the finishing point, E. During the whole process, the speeds of the DUSVs are adjusted by a fuzzy logic-based speed controller to keep the DUSVs in parallel.

In addition, the trajectories around t = 21 s and t = 43 s are plotted separately to compare the performance for dynamic collision avoidance. It is observed that the algorithm with IAPF is effective in reducing the length of trajectories for bypassing the dynamic obstacles compared to the algorithm with traditional APF.

Table 5 presents the total trajectory length and navigation time by the two algorithms. It can be seen that the algorithm with IAPF provides shorter trajectory lengths and costs less navigation time to complete the task, which indicates its efficiency in scenarios with dynamic obstacles.

Figure 12 shows the changes in relevant state variables of the DUSVs over time to verify the effectiveness of the proposed algorithms in meeting the cooperation constraints. Figure 12a,b show the changes in speed of the DUSVs, $u_1$ and $u_2$, and distance difference, $L_{dif}$, over time under the proposed fuzzy logic-based speed controller and simple speed compensator, respectively. For both cases, it shows that, in the beginning, the two USVs travels in parallel at the same speed, which gradually increases to the maximum 2 m/s. In the finishing period, the DUSVs also travel at the same speed, which gradually decreases until the end of the mission. Figure 12a shows that between 32–33 s and 38–42 s, there are obvious variations in $u_2$. The distance difference, $L_{dif}$, surpasses the threshold of 2 m and reaches 3.3 m at 38 s. This is due to the fact that USV1 turns from the outside of $T_4$ to capture $T_4$ and thus lags behind USV1. With the proposed speed controller, USV2 slows down and $L_{dif}$ is effectively reduced within 4 s. Figure 12b shows that between 25–29 s, 32–35 s, and 39–47 s, there are obvious variations in $u_2$ and the distance difference between the DUSVs and the target key points is greater than 2 m. Especially between 39–47 s, it takes 8 s to reduce the distance difference down below 2 m, indicating poor performance of the simple compensator. Therefore, it can be concluded that the proposed speed control strategy effectively improves the cooperation of the DUSVs.

Figure 12c shows the changes in the expected distance, L, between the DUSVs over time. It shows that the initial expected spacing of the DUSVs is 2 m. During the whole process, L is always between the lower bound of 2 m and the upper bound of 11 m. This effectively prevents internal collision between the DUSVs. The values of L are marked at the particular time instants, which correspond to the marked positions of the DUSVs in Figure 11b. It was found that L is gradually increased to prevent any collision with the garbage targets and ensure no escape of garbage.

Figure 12d shows the changes in current area demand $S_t$ and the current enclosed area of the floating rope, S, over time. It can be seen that at current capacity, S can always accommodate the demand, $S_t$. At 46 s, when the DUSVs start to approach the final garbage, $T_3$, the demand, $S_t$, reaches its maximum and S is gradually increased until reaching the demand at 49 s. It should be noticed that constraint (10) is conservative as the estimated garbage area with the circular or elliptical model is larger than the actual garbage area. This implies that the enclosed areas have well satisfied the actual area demands over time.

In summary, the above results indicate that the proposed comprehensive trajectory planner for the floating-rope-connected DUSVs for water surface garbage cleaning is satisfying in generating cooperative trajectories for the two USVS that meet the requirements for garbage collection with good coordination for preventing garbage escaping and internal collision while avoiding dynamic obstacles with a shorter travel distance

5. Conclusions and Future Work

To address the problem that a single USV has difficulty in effectively collecting large floating garbage, a comprehensive trajectory planner for DUSVs connected with a floating rope for surface garbage collection with dynamic obstacle avoidance is proposed, which considers the kinematic constraints of the under-actuated USV and time-varying cooperation constraints of the DUSVs. The DUSVs approach the garbage targets based on the guidance of the target key points, which consider the cooperation constraints. The APF method combined with the leader–follower strategy is proposed for cooperative garbage capturing, and an improved APF with the inclusion of a velocity repulsion field combined with the leader–follower strategy is adopted for dynamic obstacle avoidance with shorter travel distance. Cooperation between the double USVs is ensured through the proposed fuzzy logic-based speed controller. The simulation results show the effectiveness of the proposed algorithm in generating satisfactory trajectories for cooperative garbage collection and dynamic obstacle avoidance while meeting all the constraints. This implies that more efficient and effective cleaning could be achieved in practice with the proposed planner than conventional methods that have not taken the practical issues, i.e., size and shape and the dynamic nature of floating garbage, into account.

The proposed planner is limited by its assumption of full prior knowledge of the initial positions of all garbage areas and obstacles with an UAV and real-time updating of the actual locations of garbage areas and obstacles through on-board sensors. Future work would take into account the uncertainties in positions of the garbage areas and obstacles due to actual communication and sensing.