Formation Control and Obstacle Avoidance Algorithm of a Multi-USV System Based on Virtual Structure and Artificial Potential Field

Abstract

This paper proposes a formation generation algorithm and formation obstacle avoidance strategy for multiple unmanned surface vehicles (USVs). The proposed formation generation algorithm implements an approach combining a virtual structure and artificial potential field (VSAPF), which provides a high accuracy of formation shape keeping and flexibility of formation shape change. To solve the obstacle avoidance problem of the multi-USV system, an improved dynamic window approach is applied to the formation reference point, which considers the movement ability of the USV. By applying this method, the USV formation can avoid obstacles while maintaining its shape. The combination of the virtual structure and artificial potential field has the advantage of less calculations, so that it can ensure the real-time performance of the algorithm and convenience for deployment on an actual USV. Various simulation results for a group of USVs are provided to demonstrate the effectiveness of the proposed algorithms.

1. Introduction

Compared to a single unmanned surface vehicle (USV), the multi-USV system has numerous advantages, such as the high robustness, fault tolerance, good adaptability, and high efficiency of task execution. Therefore, the multi-USV system has broad application prospects in various areas, such as seabed surveying, rescue, material transportation, and target protection [1,2,3,4].

Several formation control algorithms are popular, including the virtual structure method [5,6], leader–follower method [7,8,9], behavior-based method [10,11], artificial potential field method [12], and graph theory method [13]. Each algorithm has advantages and disadvantages. For example, the virtual structure method can guarantee accuracy of the formation shape but cannot flexibly transform the formation structure. The leader–follower architecture has a clear relationship between robots in formation. In addition, it is simple to deploy the algorithm to the actual robot. However, this architecture largely depends on the leader. An advantage of the behavior-based approach is that the system can rapidly expand, while it is difficult to accurately express the behavior mathematically, which thus does not guarantee the stability of the system.

Although the control methods are different in the existing literature, the research has always been focused on the formation accuracy and obstacle avoidance. To address the problem of formation collision avoidance for USVs, the leader–follower structure and distributed control strategy were adopted in [14]. A leader–follower formation control strategy was proposed for the formation of USVs, which considered partially known control input functions of the USV [15]. An algorithm was presented for distributed maneuvering of multiple autonomous surface vehicles guided by a virtual leader [16]. Combined with an artificial potential field and neural network, an adaptive leader–follower strategy was proposed for USVs to avoid obstacles [17]. A coordinated tracking strategy and distributed controller was designed for multiple USVs under complex marine environments in [18]. A distributed connectivity-preserving and collision-avoiding formation tracking problem of USVs was investigated, which considered the limited communication ranges [19]. A cooperative guidance law to deal with the cooperative path following problem of ring-networked USVs was presented in [20].

The combination of the advantages of different algorithms to obtain better control algorithms has attracted considerable attention. Based on the potential function and behavior rules, a new control algorithm was presented to control the formation of a multiple autonomous underwater vehicle (AUV) system to avoid obstacles [21]. For maintenance and obstacle avoidance of the AUV formation, fuzzy logic was designed to balance different behaviors for the leader and followers [22]. To address the coordination control for multi-AUVs, a three-dimensional coordination control scheme was proposed which combines sliding mode control, backstepping technique, and leader–follower strategy [23]. Considering the constraints of the line of sight (LOS) range and angle tracking error, a leader–follower strategy was proposed to address the formation control of multi-AUV systems [24]. For solving the problem of multi-USV cooperative target protection, a behavior-based fuzzy logic control method was presented [4].

Although extensive studies on the formation control and obstacle avoidance of multi-USV systems [25,26] have been carried out, only a few studies considered the movement ability of actual USVs. Based on the above considerations, a novel strategy is designed by combining a virtual structure and artificial potential field (VSAPF). In this strategy, a virtual structure is used to organize the formation while an artificial potential field is used to achieve collision avoidance between USVs. To solve the obstacle avoidance problem of the formation, an improved dynamic window approach is designed to impact the formation reference point. Compared to the traditional potential field method, the obstacle avoidance strategy proposed in this paper can be used to plan a more executable path for the formation.

The rest of this paper is organized as follows. Section 2 introduces the kinematics model of the USV and definition of the formation. In Section 3, the formation algorithm and obstacle avoidance algorithm are introduced. The results of conducted numerical simulations are illustrated in Section 4. Finally, the conclusions are summarized.

2. Model and Problem Statement

In this paper, formation control and obstacle avoidance algorithms for multiple USVs are proposed. This section introduces the kinematic model of the USV and statement of the formation problem.

2.1. USV Kinematic Model

In multi-USV formation, the size of the USV can be ignored compared to the formation scale, so that it can be simplified as a unicycle model.

In Figure 1, $XOY$ represents the geodetic coordinate system, while $x_b{o}_b{y}_b$ represents the body coordinate system of the USV. The state information of the USV includes the location $\left(x,y\right)$ and heading $\theta$, while $P={\left[\begin{array}{ccc}x& y& \theta \end{array}\right]}^T$ is the position vector of the USV. The kinematic equation of the USV is [27]:

$$\dot{P}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$

(1)

where $v$ is the forward speed, while $\omega$ is the angular velocity of the USV.

2.2. Formation Definition

Figure 2 shows a diagram of the virtual structure method in which the thick dashed line denotes the shape outline of the virtual structure, while the USV dashed-line shape denotes the location distribution of each USV in the final formation. The formation implementation can be described by satisfying Equation (2):

$$\underset{t\to \infty }{\lim }\left(p_i^d-p_i^c\right)=0,\forall i\le M$$

(2)

where $p_i^d$ and $p_i^c$ represent the desired position and current position of the $i^{th}$ USV, respectively, and $M$ denotes the number of USVs in the formation.

3. Algorithm Design

In this section, a formation control algorithm is proposed. The virtual structure method has the advantages of a clear geometric position relationship of members in the formation and high accuracy of formation keeping. On the other hand, the artificial potential field method has the advantage of a low computational complexity in collision avoidance research. Therefore, the algorithm of the combination of a virtual structure and artificial potential field is applied for formation control. A schematic diagram of the algorithm is shown in Figure 3. The black circle represents the formation reference point, the virtual structure is built around the formation reference point, the blue circle represents the point on the virtual structure, which is referred to as virtual structure point, and the orange circle represents the virtual leader. During the formation process, the path of the virtual structure depends on the formation reference point. Virtual leaders track their corresponding virtual structure points, while the USV accurately tracks the path of the virtual leader to achieve the desired formation.

3.1. Generation of the Virtual Structure

$p^r\in R^2$ and $u^r\in R^2$ represent the position and velocity vectors of the formation reference point in $XOY$, respectively. The motion model of the formation reference point can be expressed by:

$$\{\begin{array}{l}{\dot{p}}^r=u^r\\ {\dot{u}}^r={\tau }^r\end{array}$$

(3)

where ${\tau }^r$ represents the guidance control law related to formation path planning, according to which the formation reference point moves along the desired formation path.

$p_i^s\in R^2$ denotes the location of the $i^{th}$ virtual structure point in $x_b{o}_b{y}_b$. Its location in $XOY$ can be expressed by:

$$p_i^r=p^r+R\left(\theta \right)L{p}_i^s$$

(4)

where $R\left(\theta \right)$ and $L$ represent the transformation matrix from $x_b{o}_b{y}_b$ to $XOY$ and formation scaling matrix, respectively. They can be expressed as follows:

$$R\left(\theta \right)=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

(5)

$$L=\left[\begin{array}{cc}l& 0\\ 0& l\end{array}\right]$$

(6)

The reasons for the introduction of the virtual leader are as follows. In the initial stage, the USV is randomly distributed in the initialization area, which may be far away from the corresponding virtual structure point $p_i^r$. If the USV directly drives to $p_i^r$, collision between the USVs may occur. During the dynamic process, the effective control of the motion state of the USV will be lost. Similarly, during the process of formation transformation, the shape of the virtual structure changes from $p_i^s$ to $p_i^{s^{\prime }}$. The corresponding expected formation trajectory will also have a step change. The USV changes from tracking the original formation trajectory to tracking the new formation trajectory, with a large position change. In this process, there are also problems of collision between USVs and control of the USV. Using a virtual leader and enabling the USV to track it, the movement state of the USV can be effectively controlled before the generation of the formation. Simultaneously, using the virtual leader as a medium, the introduction of the artificial potential field can avoid collisions between USVs.

The motion model of the virtual leader can be expressed by:

$$\{\begin{array}{l}{\dot{p}}_i=u_i\\ {\dot{u}}_i={\tau }_i\end{array}$$

(7)

where $p_i\in R^2$ and $u_i\in R^2$ are the position and velocity of the $i^{th}$ virtual leader, respectively.

The control input of the $i^{th}$ virtual leader is:

$${\tau }_i=c_1\left(p_i^r-p_i\right)+c_2\left(u_i^r-u_i\right)+{\displaystyle \sum _{j\in N_i}{f}_{i,j}}$$

(8)

where $p_i^r$ and $u_i^r$ are the position and velocity of the $i^{th}$ virtual structure point in $XOY$, respectively. $c_1$ is the feedback coefficient of the position error, $c_2$ is the speed error feedback coefficient, and $f_{i,j}$ is the force term of the artificial potential field for collision avoidance, which is used to achieve collision avoidance between virtual leaders. As for collision avoidance between USVs, because USVs are randomly arranged near their corresponding virtual leader’s position in the initial stage, the USV only needs to track the corresponding virtual leader accurately to avoid collisions with each other. It is considered that the $i^{th}$ virtual leader is in the potential field generated by the neighboring virtual leaders. The total potential energy of the $i^{th}$ virtual leader can be expressed by:

$$V\left(p_i\right)={\displaystyle \sum _{j\ne i}^N\varphi \left(\Vert p_j-p_i\Vert \right)}$$

(9)

where $\varphi (*)$ is the artificial potential field function, and $\Vert *\Vert$ denotes the vector modulus. As the artificial potential field is used to only avoid collision rather than organize the formation, the potential field only has repulsion, while the potential function is designed as:

$${\varphi }_{hb}\left(r\right)=\{\begin{array}{cc}\frac{1}{2}\eta {\left(\frac{1}{r}-\frac{1}{r_0}\right)}^2& 0<r\le r_0\\ 0& r>r_0\end{array}$$

(10)

where $r$ is the distance between virtual leaders and $r_0$ is the influence range of the artificial potential field. When the relative distance between virtual leaders is larger than $r_0$, no interaction of artificial potential field forces exists. $\eta$ is the potential field gain coefficient, which should be selected to consider the actual performance of the USV. A very small value is not conducive to the safe collision avoidance of a USV, while a too large value may lead to the control input exceeding the capacity of the actuator. It is necessary to consider the possible interference to the formation in the design of the artificial potential field and choose the value of the potential field range as follows:

$$\begin{array}{cc}\Vert p_j^r-p_i^r\Vert \ge r_0& \forall i,j\in N_i\end{array}$$

(11)

Thus, in the neighborhood of the final formation, the term of artificial potential field on the right side of Equation (8) can be eliminated. Equations (7) and (8) form a linear second-order system:

$${\ddot{p}}_i+c_2{\dot{p}}_i+c_1{p}_i=c_2{\dot{p}}_i^r+c_1{p}_i^r$$

(12)

According to the Hurwitz theorem, the system is stable when $c_1\ge 0$, $c_2\ge 0$. As the tracking target of the USV, the virtual leaders should avoid violent movement to reduce the frequent maneuver of the USV. The smaller $c_1$ can avoid a too fast movement of the virtual leader to the formation reference point, and thus the speed tends to saturation. The larger $c_2$ can match the speed, which is convenient for a stable generation of the formation. The effect of the artificial potential field during the formation process can be considered as interference to the second-order system. Owing to the existence of a negative gradient term of the artificial potential field, virtual leaders move towards the direction where the artificial potential field can be reduced to achieve collision avoidance.

3.2. Formation Transformation

According to Equation (4), the velocity of the virtual structure point in $XOY$ can be expressed as:

$$u_i^r=u^r+\dot{R}\left(\theta \right)L{p}_i^s+R\left(\theta \right)\dot{L}{p}_i^s+R\left(\theta \right)L{\dot{p}}_i^s$$

(13)

Assuming that the transformation of the formation is achieved by a step change of $p_i^s\to p_i^{s^{\prime }}$, Equation (13) can be simplified to:

$$u_i^r=u^r+\dot{R}\left(\theta \right)L{p}_i^s+R\left(\theta \right)\dot{L}{p}_i^s$$

(14)

where $p_i^s$ represents the shape of the formation. During the formation movement, the formation switching can be achieved by changing the value of $p_i^s$. $L$ represents the size of the virtual structure. The size of the formation can be changed by setting different values of $L$.

3.3. Obstacle Avoidance

The USV formation inevitably needs to pass through the obstacle area during the movement. In some specific application scenarios, such as collaborative object transportation and collaborative area scanning operation, the USV formation is necessary to maintain the complete structure during the process of obstacle avoidance. In this section, we study the conformal obstacle avoidance algorithm for multi-USV formation.

3.3.1. Artificial Potential Field Method

The artificial potential field method is a common algorithm in robot obstacle avoidance research. The robot is in the force field generated by obstacles and target point, the target attracts the robot, the obstacles repel the robot, and the robot moves under the action of joint forces [28,29]. However, the algorithm has a defect of a local minimum value, which will lead to robot falling into a “dead point”. As the actual movement ability is not considered, the robot cannot accurately track the path planned by the algorithm. In practical application, it is often used in combination with other algorithms [30,31,32].

3.3.2. Dynamic window approach

The dynamic window approach is a local path planning algorithm, which considers the dynamic performance of the USV [33,34]. Its implementation consists of two steps. The first step is to generate an effective search space, in which the performance constraints of the USV need to be fully considered. The second step is to select the best scheme in the search space.

(1)

Search spacex

In this step, every group of speed generated in the search space is tested to ensure that they can safely control the USV. The generation of speed instructions is limited by the speed and acceleration of the USV. Simultaneously, for safety, the USV should have enough obstacle avoidance distance during the process of movement. By comprehensively considering the above constraints, a feasible speed space is finally generated.

The USV has the ability to move forward and rotate. Moreover, the moving distance of the USV in the sampling period is limited. Therefore, the trajectory of the USV in the sampling period can be simplified to a straight line. The coordinate of the USV at time $t+1$ can be calculated, so that the trajectory of every set of velocities can be estimated. Therefore, the derivation formulas of the USV are:

$$\{\begin{array}{l}{x}_{t+1}=x_t+v\delta t\cos {\theta }_t\\ y_{t+1}=y_t+v\delta t\sin {\theta }_t\\ {\theta }_{t+1}={\theta }_t+\omega \delta t\end{array}$$

(15)

where $x_t$ and $y_t$ are coordinates of the USV on the x-axis and y-axis of the world coordinate system, $v$ and $\omega$ are the linear and angular velocities of the USV, respectively, and ${\theta }_t$ is the heading of the USV at time $t$.

In the velocity space $\left(v,\omega \right)$, sets of velocity are sampled and every set corresponds to a simulated track, while the velocity sampling range needs to be determined according to the limitations of the property of the USV and environmental constraints.

The limitation of the maximum velocity and minimum velocity is:

$$V_m=\left\{\left(v,\omega \right)|v_{\min }\le v\le v_{\max },{\omega }_{\min }\le \omega \le {\omega }_{\max }\right\}$$

(16)

$$V_m=\left\{\left(v,\omega \right)|\begin{array}{l}v\in \left[v_c-{\dot{v}}_b\delta t,v_c+{\dot{v}}_a\delta t\right],\\ \omega \in \left[{\omega }_c-{\dot{\omega }}_b\delta t,{\omega }_c+{\dot{\omega }}_a\delta t\right]\end{array}\right\}$$

(17)

where $v_c$ and ${\omega }_c$ represent the linear and angular velocities, respectively. ${\dot{v}}_b$ and ${\dot{\omega }}_b$ represent the maximum decelerations of the linear and angular velocities, respectively, while ${\dot{v}}_a$ and ${\dot{\omega }}_a$ represent the maximum accelerations of the linear and angular velocities, respectively.

Based on safe considerations, the USV needs to stop moving before hitting obstacles. The range of the speed of the USV under the maximum deceleration is:

$$V_a=\left\{\left(v,\omega \right)|\begin{array}{c}v\le \sqrt{2\ast dist\left(v,\omega \right)\ast {\dot{v}}_b},\\ \omega \le \sqrt{2\ast dist\left(v,\omega \right)\ast {\dot{\omega }}_b}\end{array}\right\}$$

(18)

where $dist\left(v,\omega \right)$ is the shortest distance from the USV to the obstacle on the corresponding trajectory of the velocity $\left(v,\omega \right)$.

Thus, the actual sampling range of the USV speed is the intersection of the above three speed spaces:

$$V=V_m\cap V_d\cap V_a$$

(19)

(2)

Optimization

The purpose of this step is to obtain the optimal speed set and evaluate the speed set in the search space. In the original dynamic window approach, the definition of the evaluation function is:

$$G\left(v,\omega \right)=\sigma \left(\alpha \cdot heading\left(v,\omega \right)+\beta \cdot dist\left(v,\omega \right)+\gamma \cdot velocity\left(v,\omega \right)\right)$$

(20)

where $\sigma$ is the smoothing function, $\alpha$, $\beta$, $\gamma$ are constants, and $heading\left(v,\omega \right)$ is an index used to evaluate the alignment degree between the current direction and target direction of the USV at the current set sampling speed. $dist\left(v,\omega \right)$ represents the distance between the USV and nearest obstacle on the current track. If there is no obstacle on this track, it is set as a constant. $velocity\left(v,\omega \right)$ is the position vector of the USV. The kinematic equation ofto evaluate the speed of the current track.

The original evaluation function can quickly guide the USV to the target position. However, when the USV approaches the target, the angle between the heading of the USV and target sharply increases. Under the function of the $heading\left(v,\omega \right)$ term in the evaluation function, the USV will then increase its angular velocity $\omega$ to reduce $\theta$, which will make the motion of the USV insufficiently smooth. Therefore, the evaluation function needs to be improved.

(3)

Improved evaluation function

The evaluation function in the dynamic window approach is improved by importing the distance from the USV to the target. The improved evaluation function is as follows:

$$G\left(v,\omega \right)=\sigma \left(\alpha \cdot goal\_dist\left(v,\omega \right)+\beta \cdot dist\left(v,\omega \right)+\gamma \cdot velocity\left(v,\omega \right)\right)$$

(21)

where $goal\_dist\left(v,\omega \right)$ is the distance from the current position of the USV to the target. The physical meaning of the other items is consistent with the evaluation function before the improvement. By the improved algorithm, the heading angle of the USV can converge to the target point more quickly and the obstacle avoidance time can be reduced, while ensuring the safety of obstacle avoidance.

3.4. USV Path Tracking Algorithm

The USV achieves the formation by tracking the corresponding virtual leader. Thus, it is necessary to design the corresponding tracking control algorithm for the USV.

In Figure 4, $P_c=\left[\begin{array}{ccc}{x}_c& y_c& {\theta }_c\end{array}\right]$ represents the current gesture information of the USV, $u={\left[\begin{array}{cc}{v}_{}& {\omega }_{}\end{array}\right]}^T$ represents the control input, and $P_d=\left[\begin{array}{ccc}{x}_d& y_d& {\theta }_d\end{array}\right]$ represents the desired gesture the of USV.

The error formula is:

$$P_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\end{array}\right]=\left[\begin{array}{ccc}\cos {\theta }_e& \sin {\theta }_e& 0\\ -\sin {\theta }_e& \cos {\theta }_e& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_d-x_c\\ y_d-y_c\\ {\theta }_d-{\theta }_c\end{array}\right]$$

(22)

The tracking controller is designed as follows:

(1)

Velocity controller

Figure 4 shows that the distance between the current position and desired position of the USV is:

$$\rho =\sqrt{{\left(x_d-x_c\right)}^2+{\left(y_d-y_c\right)}^2}$$

(23)

The speed controller of the USV is designed as:

$$v=K_{\rho }\cdot \rho$$

(24)

(2)

Heading controller

The angle error of the USV during the tracking is:

$${\theta }_e={\theta }_d-{\theta }_c=\arctan \left(\frac{y_d-y_c}{x_d-x_c}\right)$$

(25)

To accurately track the desired heading by the USV, the Proportional Integral (PI) controller is used to control the angle. The designed heading angle control law is:

$$\omega =K_P{\theta }_e+K_I{\displaystyle \underset{0}{\overset{t}{\int }}{\theta }_{e}dt}$$

(26)

4. Numerical Simulations

Simulations are carried out to evaluate the algorithms proposed in this study.

4.1. Path Tracking Controller

Figure 5a, b shows the simulation results of the line path and circular path tracking, respectively. The arrow denotes the USV, while the black dotted line and red solid line represent the desired path and trajectory of the USV, respectively. The figures show that the USV can accurately track the desired path.

4.2. Coordinated Path Tracking of Formation of Three USVs

Figure 6 shows the virtual structure of the formation, in which the black point and blue point represent the formation reference point and virtual structure point, respectively. The red circle denotes the force range of the potential field.

4.2.1. Line Path Tracking

It is assumed that the initial positions of the virtual leaders and USVs are randomly distributed in the area of $\left\{\left(x,y\right)|x\in \left[-20,20\right],y\in \left[-25,25\right]\right\}$. Figure 7 shows the simulation results of the formation for line path tracking. The blue diamond in Figure 7a indicates the virtual leader, while the red triangle in Figure 7b indicates the USV. As shown in Figure 7, the virtual leaders can stably form the desired formation, while the USV achieves the formation by accurately tracking the corresponding virtual leader. As shown in Figure 8, the distances between the neighboring USVs are maintained at 7.07 and 10 m, consistent with the distance between preset virtual structure points.

4.2.2. Curve Path Tracking

Another typical tracking case is curve path tracking. Assuming that the initial positions of the virtual leaders and USVs are randomly distributed in the area of $\left\{\left(x,y\right)|x\in \left[-10,10\right],y\in \left[-20,20\right]\right\}$, Figure 9 shows the simulation results of the formation for curve path tracking. As shown in Figure 10, the distances between the neighboring USVs are maintained at 7.07 and 10 m. Figure 9 and Figure 10 show that the three USVs achieve the desired formation shape and that the formation can accurately track the curve path.

4.3. Formation Scalability

The scalability of the number of formation members is an important index to evaluate the formation control algorithm. The algorithm proposed in this study can adjust the number of formation members according to the mission requirements. Formation generation simulation of four and six USVs are carried out.

Figure 11 shows the virtual structure of the four-USV formation. Assuming that the shape of the virtual structure is a square, the centroid of the square is selected as the formation reference point, while the distance between each virtual structure point and formation reference point is 5 m. Assuming that the force range of the potential field is set to 5 m, the initial positions of the virtual leaders and USVs are randomly distributed in the area of $\left\{\left(x,y\right)|x\in \left[-20,20\right],y\in \left[-25,25\right]\right\}$.

Figure 12 shows the simulation results of the four-USV formation generation. Figure 13 shows that the distances between each USV and formation reference point are maintained at 5 m, consistent with the distance designed in the virtual structure.

Figure 14 shows the virtual structure of the six-USV formation. The shape of the virtual structure is an isosceles right triangle. Assuming that the virtual structure points are located at the vertex and midpoint of each side of the triangle, the formation reference point coincides with the fifth virtual structure point. The force range of the potential field is set to 5 m, assuming that the initial positions of the virtual leaders and USVs are randomly distributed in the area of $\left\{\left(x,y\right)|x\in \left[-20,20\right],y\in \left[-25,25\right]\right\}$.

Figure 15 shows the simulation results of the four-USV formation generation. Figure 16 shows that the distances between each USV and the formation reference point are maintained at 10, 7.07, and 0 m, consistent with the distance designed in the virtual structure.

4.4. Formation Shape Transformation

4.4.1. Structure Change

The change in the formation structure is achieved by setting different $p_i^s$ values in Equation (14). It is assumed that three USVs are randomly distributed at the initial time, forming a triangle shape first, then switching to the line shape, and finally returning to the triangle shape.

Figure 17 shows that during the period of 0–100 s, the USVs gradually form the triangle formation structure from the random distribution state. The relative distances between USVs are maintained during the movement in the period of 100–200 s movement. After 200 s, the formation structure starts to change. The distances between the USVs gradually stabilize to new values. After moving a certain distance, the formation starts to switch and eventually returns to the original triangle formation.

4.4.2. Structure Scaling

The scaling of the formation configuration is achieved by setting different $\dot{L}$ values in Equation (14). It is assumed that three USVs are randomly distributed at the initial time. The distance between each virtual structure point and formation reference point is expected to be 5 m. The USV gradually forms a triangle formation from the random distribution state. After moving a certain distance, the formation scale gradually reduces to 0.7 times the initial state, and then gradually returns to the initial state value. Figure 18a, b shows the trajectories of the USVs and relative distances between the USVs during the process, respectively.

4.5. Dynamic Window Approach

Assuming that the simulation parameters such as starting point information, target point information, and obstacle information are consistent, the paths generated by the original approach and improved approach are compared.

Figure 19 shows the simulation results of local path planning of the dynamic window approach. As is seen in Figure 19, after the improvement of the evaluation function, the USV can keep a safe distance from the obstacles and move in a shorter path, which saves moving time. After avoiding the obstacles, the improved algorithm can make the USV’s heading angle converge to the target point faster, so as to ensure that the USV can reach the target faster.

4.6. Formation Obstacle Avoidance

Figure 20 shows a diagram of the obstacle avoidance strategy. The USV formation maintains its shape during the obstacle avoidance process. In this study, the strategy of applying the improved dynamic window approach to the formation reference point is used. In this strategy, the whole formation is regarded as a larger-scale USV centered on the formation reference point. Using this method, the formation reference point can plan a favorable path for USVs to avoid obstacles.

Assuming that the distance between each USV and formation reference point is 5 m, the target point is located at (50, 50), the obstacle is located at (20, 20), and the radius of the obstacle is 4 m.

Figure 21 shows the simulation results of the formation obstacle avoidance. Compared to the results in [35], where the artificial potential field for multi-AUV systems is used to achieve obstacle avoidance, the path planned by our algorithm is smoother and more executable for actual vehicles. As shown in Figure 22, all USVs are at a safe distance from the obstacle, which demonstrates that the algorithm is able to generate acceptable safe paths in an obstacle environment. Furthermore, as shown in Figure 22a, the distances between the obstacle and each USV are recorded. Notably, the smallest distance between the USV and obstacle is 5 m. The relative distances between the USVs are shown in Figure 22b. The formation shape can be well maintained as the distance fluctuations remain relatively small.

5. Conclusions

In this study, the formation control algorithm of a multi-USV system and problem of conformal obstacle avoidance are investigated. The algorithm combining the virtual structure and artificial potential field can effectively organize USVs to generate the desired formation and flexibly change the formation shape by structure switching and scaling strategies. Through the improved dynamic window approach, the formation reference point can plan a safe obstacle avoidance path for the formation, and thus the formation can maintain the shape while avoiding the obstacle. Compared to the traditional artificial potential field method, the proposed method is more conducive because it considers the actual execution ability of the USV. Finally, the numerical simulation results validated the effectiveness of the proposed formation control algorithm.