Keywords

1 Introduction

Unmanned Underwater Vehicle (UUV) plays a huge role in maritime combat. With the improvement of the complexity of the task, single UUV can no longer complete the multi-task work independently, so the formation of multiple UUV can better coordinate to complete all kinds of tasks [1]. And for tasks with a large regional range, multiple UUV formations can be combined into a large formation to improve the efficiency of task execution formation control can generally be divided into two stages: 1) determine a desired formation, and 2) design the corresponding control algorithm for reaching and keeping this formation. At present, there are relatively few research on formation and recombination, but more research on formation control, which mainly includes leader-following method [2, 3] Artificial potential field method [4], virtual structure method [5], consistency control [6] and so on.

The existing formation description methods mainly include three types of algorithms based on displacement, distance and angle. These three types of algorithms respectively use relative displacement, relative distance and relative Angle between UAVs as constraints to define the formation of multi-UUV formation [7,8,9].

In recent years, scholars have proposed some new methods to define formation [10,11,12]. Examples include centroid coordinate method [13], complex Laplacian matrix method [14], and shape theory method [15]. The use of these theories as constraints to define the formation improves the invariance of the formation constraints. Among them, shape theory is a simple method for defining formation proposed by scholars [16]. Because shape theory only cares about the geometric definition of the formation, it can be more flexible to describe the generation and transformation of the UUV formation.

When the multi-UUV formation system encounters an emergency situation, such as an obstacle or communication interruption during the process of moving, it will autonomtively avoid obstacles or interrupt communication, and the initial formation will be disrupted. In the process of solving the emergency situation and reorganizing the formation, reasonable coordination planning should be carried out when selecting the position of each UUV in the formation, so as to reduce the time-consuming of UUV formation recovery process. Therefore, in the optimization selection of position, the formation problem can be transformed into the problem of finding the maximum value of the nearest position between the UUV and the formation. In order to solve this problem, this paper proposes to use the biological- inspired algorithm to optimize and select the optimal route.

2 Preliminaries and Modelling

2.1 Shape Theory

Definition 1: Shape of formation refers to the geometric information retained after removing rotation, contraction and displacement of formation.

Supposing UUVs (the quantity is n) in three-dimensional space needs to form a fixed formation. Defining \(p_i = \left( {p_i^x ,p_i^y ,p_i^z } \right)^T\) as the position of UUV in a fixed coordinate system, then the expected position of each UUV in the target formation can be expressed by the following matrix:

$$ S = \left( {s_1 ,s_2 , \ldots ,s_n } \right) \in {\mathbb{R}}^{3 \times n} $$
(1)

In cases where relative relationships between UUVs are concerned rather than absolute positions, shape theory becomes a more appropriate way to describe UUV formations.

2.2 UUV Kinematic and Kinetic Models

The rectangular coordinate system is used to study the motion of underactuated UUV, and the coordinate system mainly includes the fixed coordinate system and the moving coordinate system, as shown in Fig. 1.

Fig. 1.
figure 1

UUV motion model

Full size image

The UUV formations studied in this paper are located at the same depth. Considering that the UUVs keep a fixed formation moving in the horizontal plane, the kinematic and dynamic models of the UUVs without interference are established [17, 18].

$$ \left\{ {\begin{array}{*{20}l} {\dot{x} = u\cos \psi - v\sin \psi } \hfill \\ {\dot{y} = u\sin \psi - v\cos \psi } \hfill \\ {\dot{\psi } = r} \hfill \\ {\dot{u} = \frac{m_2 vr - X_u u + \tau_u }{{m_1 }}} \hfill \\ {\dot{v} = \frac{ - m_1 ur - Y_v v}{{m_2 }}} \hfill \\ {\dot{r} = \frac{(m_1 - m_2 )uv - N_r r + \tau_r }{{m_3 }}} \hfill \\ \end{array} } \right. $$
(2)

Among them, \(x\), \(y\) respectively represent the coordinates of UUV in the inertial coordinate system, \(\psi\) represents the heading Angle, \(u\), \(v\), \(r\) respectively represent the vertical velocity, horizontal velocity and yaw angular velocity of UUV, \(X_u\), \(Y_u\), \(N_r\) represent the hydrodynamic coefficient, \(m_1\), \(m_2\), \(m_3\) represent the mass inertia coefficient of UUV, \(\tau_u\), \(\tau_r\) respectively represent the thrust and steering torque.

3 Design of UUV Formation Controller with Time Delay Based on Consistency

The premise of formation transformation is the control of formation keeping. In a multi-UUV system, one UUV is selected as the primary UUV, and the other UUV is the secondary UUV. The master/slave path tracking controller is designed so that these UUV tracks the primary UUV to keep a fixed formation forward. Since the propagation speed of underwater sonar signal is about 1480 m/s, the underwater communication capability of UUV is very limited, and underwater acoustic communication has delay. Therefore, this paper considers the delay of multiple UUV groups to design the controller.

It is assumed that UUV groups keep a fixed depth motion of \(z_i\), and when there is a delay in communication between UUVs, the controller is designed as follows:

$$ \begin{array}{*{20}l} {u_{x_i } = \dot{f}^x (t) - \gamma (v_{x_i } (t - \tau_{ij} (t)) - f^x (t - \tau_{ij} (t)))} \hfill \\ { - \sum_{j = 1}^m {a_{ij} \left[ {(x_{f_i } (t) - x_{f_j } (t - \tau_{ij} (t)) - \left( {\delta_i^x - \delta_j^x } \right) + \gamma (v_{x_i } (t - \tau_{ij} (t)) - v_{x_j } (t - \tau_{ij} (t)))} \right]} } \hfill \\ \end{array} $$
(3)
$$ \begin{array}{*{20}l} {u_{y_i } = \dot{f}^y (t) - \gamma (v_{y_i } (t - \tau_{ij} (t)) - f^y (t - \tau_{ij} (t)))} \hfill \\ { - \sum_{j = 1}^m {a_{ij} \left[ {(y_{f_i } (t) - y_{f_j } (t - \tau_{ij} (t)) - \left( {\delta_i^y - \delta_j^y } \right) + \gamma (v_{y_i } (t - \tau_{ij} (t)) - v_{y_j } (t - \tau_{ij} (t)))} \right]} } \hfill \\ \end{array} $$
(4)

Among them, the control gain \(\gamma > 0\), \(\tau_{ij} \left( t \right)\) represents the UUV communication delay between the ith and jth UUV. \(f^x \left( t \right)\) and \(f^y \left( t \right)\) are continuously differentiable functions, representing the motion velocity characteristics of UUV. \(\delta_i^x\), \(\delta_i^y\) indicates the desired location. If the upper limit of time delay is \(\tau_0\), there is \(0 < \tau_{ij} \left( t \right) < \tau_0\), and it must meet:

$$ 0 \le \tau_{ij} (t) \le \frac{1}{\omega_0 }\arctan \frac{(1 + \lambda_i )\omega_0 \gamma }{{\lambda_i }} $$
(5)

In the above equation,

$$ \omega_0 = \sqrt {\frac{{(1 + \lambda_i )^2 \gamma^2 \pm \sqrt {(1 + \lambda_i )^4 \gamma^4 + 4\lambda_i^2 } }}{2}} $$
(6)

where \(\lambda_i\) is the eigen root of the Laplacian matrix L of figure G. When the above conditions are met, the UUV group maintains a stable formation movement.

When Eq. (5) established, the second order UUV group system with time delay can achieve consistent stability and maintain stable formation navigation.

4 UUV Formation Recombination Based on Bio-inspired Algorithm

4.1 Description of the Problem

As shown in Fig. 2, the UUV formation system composed of two sub-formations maintains a longitudinal prismatic formation, UUV1 and UUV4 are the leading aircraft, and the rest are tracking slaves. After the obstacle avoidance process is completed, the UUV formation system needs to re-formation according to the formation controller, and the path will be significantly longer. Therefore, it is necessary to recalculate the optimized path, so that the multi-UUV system can re-plan the path according to the optimized route, and finally complete the formation with the shortest path.

Fig. 2.
figure 2

Recombination process of multiple UUV formations

Full size image

Supposing there are UUV formation teams (the quantity is m) and UUVs (the quantity is n), then the desired formation (and target formation) of the jth formation can be represented by a vector (\(s_{j.1} ,s_{j.2} , \ldots ,s_{j.n_j }\)). Among them, \(n_j\) represents the number of UUVs in the jth formation, representing the two-dimensional coordinates of the desired formation, then the desired formation is:

$$ S_j = \left( {s_{j.1}^T ,s_{j.2}^T , \ldots ,s_{j \cdot n_j }^T } \right)^T \in {\mathbb{R}}^{3 \times n_j } \quad (j = 1, \cdots m) $$
(7)

The desired formation of multiple formations can be expressed as:

$$ S = \left( {S_1^T ,S_2^T , \ldots ,S_m^T } \right)^T \in {\mathbb{R}}^{3 \times \left( {n_1 + n_2 + \ldots + n_m } \right)} $$
(8)

The initial position of the multi-formation can be expressed as:

$$ P = \left( {P_1^T ,P_2^T , \ldots ,P_m^T } \right) \in {\mathbb{R}}^{3 \times \left( {n_1 + n_2 + \ldots + n_m } \right)} $$
(9)

The target position of the multi- formation can be expressed as

$$ Q = \left( {Q_1^T ,Q_2^T , \ldots ,Q_m^T } \right) \in {\mathbb{R}}^{3 \times \left( {n_1 + n_2 + \ldots + n_m } \right)} $$
(10)

For a UUV formation, it must be possible to find a target formation Q, which minimizes the total distance between the starting position and the target position \(P - Q\) of all UUVs. Therefore, the multi-UUV formation control problem is transformed into an optimization problem with constraints through these transformations:

$$ \min_{Q \in {\mathbb{R}}^{3 \times n} } ||P_j - Q_j ||,s.t.Q_j \in [S_j ]\quad (or\quad Q_j \ S_j ,j = 1,2, \ldots ,m). $$
(11)

Among them, \(\left[ {S_j } \right]\) represents a set of feasible formations with the same shape and different sizes and directions; m represents all the number of sub-formations.

According to the definition of shape theory, if \(Q_j \ S_j\), then there are corresponding rotation R, translation d and scaling operation a, under the corresponding operation,

$$ Q_j = \alpha_j R_j S_j + d_j $$
(12)

Since the formation needs to ensure the safe distance between UUVs, and the spacing distance should not be too far, the zoom ratio value \(a_j\) should be valued within a fixed range \(a_{{\text{min}}} \le a_j \le a_{{\text{max}}}\). In order to avoid the overlap of the initial formation between formations, it is necessary to increase the allowable range \({\Omega }_j \in \rm{\mathbb{R}}^3\) for each expected formation, and there is a safe distance between each formation’s \({\Omega }_j\) to avoid collision. Therefore, considering the rotation, scaling and scope constraints of the target formation, the formation generation problem of the formation can be written in the following form:

$$ \begin{array}{*{20}l} {\mathop {\min }\limits_{Q \in {\mathbb{R}}^{3 \times n} } ||P_j - Q_j ||} \hfill \\ {s.t.\quad A_j Q_j = a_j (I_{n_j } \otimes R_j )S_j \;\;\;(j = 1,2, \ldots ,m).} \hfill \\ {\quad \quad a_{\min } \le a_j \le a_{\max } } \hfill \\ {\quad \quad Q_j \in \Omega_j } \hfill \\ \end{array} $$
(13)

The formation of the desired one, rotation angle of the target formation, scaling ratio and the restricted area are taken as constraint conditions, and the problem of the form of the formation is transformed into an optimization issue.

In the case that the initial position of UUV and the target position of formation are known, the target position of UUV can be assigned to each UUV by local adjustment, and the total path can be the shortest, then it will be converted to another optimization problem:

$$ \begin{array}{*{20}l} {\min \sum_{i = 1}^n {\sum_j^n {c_{ij} } } x_{ij} } \hfill \\ {s.t.\sum_{j = 1}^n {x_{ij} } = 1} \hfill \\ {\sum_{i = 1}^n {x_{ij} } = 1} \hfill \\ {x_{ij} = \{ 0,1\} \quad i,j = 1,2, \ldots ,N} \hfill \\ \end{array} $$
(14)

Among them, \(n\) represents the sum of UUV; cij represents the expected distance from the ith UUV to jth; \(x_{ij} = 1\) represents ith UUV’s targeted coordinate is the coordinate of the j.

4.2 Standard Ant Colony Algorithm

The idea of the standard ant colony algorithm is to use the characteristics of the pheromone released by the ant colony in the process of reaching the target point, with the iteration of time, all ants concentrate on the shortest path to obtain the optimal solution.

Supposing the number of ants in the ant colony is m, the number of ants is n, the distance between waypoints i and waypoints j is \(d_{ij} \left( {i,j = 1,2, \ldots ,n} \right)\), at the time of t, the pheromone concentration on the connecting path between ant i and ant j is \(\tau_{ij} \left( t \right)\). At the initial moment, the pheromone concentration among \(k\left( {k = 1,2, \ldots ,m} \right)\) all ants are the same, setting as \(\tau_{ij} \left( 0 \right) = \tau_0\). Setting \(P_{ij}^k \left( t \right)\) as a demonstration of probability for the ant k to travel from waypoint i to waypoint j at the time of t, then:

$$ P_{ij}^k (t) = \left\{ {\begin{array}{*{20}l} {\frac{{[\tau_{ij} (t)]^\alpha \times [\gamma_{ij} (t)]^\beta }}{{\sum_{s \in allow_k } {[\tau_{is} (t)]^\alpha \times [\gamma_{is} (t)]^\beta } }},} \hfill & {s \in allow_k } \hfill \\ {0,} \hfill & {s \notin allow_k } \hfill \\ \end{array} } \right. $$
(15)

In Formula (15), \(\gamma_{ij} \left( t \right) = 1/d_{ij}\) represents the heuristic function, that is, the expectation degree of the ant from the waypoint i to the waypoint j; \(\alpha\) represents the factor of pheromone importance; \(\beta\) represents the important degree factor of heuristic function; \(allow_k \left( {k = 1,2, \ldots ,m} \right)\) represents the set of waypoints to be accessed by the ant k, with the increase of time, the elements in \(allow_k\) decrease continuously until the access is completed and it becomes an empty set. Ant k releases the pheromone, the pheromone on the connecting path of each waypoint gradually decreases. Supposing \(\rho \left( {0 < \rho < 1} \right)\) represent the degree of pheromone evaporation. When the ant completes an iteration, the pheromone concentration between each waypoint needs to be updated according to the following formula:

$$ \left\{ {\begin{array}{*{20}l} {\tau_{ij} (t + 1) = (1 - \rho )\tau_{ij} (t) + \rho \Delta \tau_{ij} } \hfill \\ {\Delta \tau_{ij} = \sum_{k = 1}^n {\Delta \tau_{ij}^k } } \hfill \\ \end{array} } \right. $$
(16)

In the above formula, \(\Delta \tau_{ij}^k\) denotes the pheromone concentration released by the kth ant at the conjunction waypoint of path i and path j, \(\Delta \tau_{ij}\) denotes the sum of the pheromone concentration released by all ants at the conjunction waypoint of path i and path j. Generally speaking, the ant cycle system model is used to express \(\Delta \tau_{ij}\):

$$ \Delta \tau_{ij}^k = \left\{ {\begin{array}{*{20}l} {Q/L_k ,} \hfill & {{\text{Ant}}\,{\text{k}}\,{\text{goes}}\,{\text{from}}\,{\text{i}}\,{\text{to}}\,{\text{j}}} \hfill \\ 0 \hfill & {{\text{else}}} \hfill \\ \end{array} } \right. $$
(17)

Among them, Q is a constant parameter, representing the total amount of pheromone released by the ant in one iteration; \(L_k\) represents the length of the path taken by the kth ant.

4.3 Improved Ant Colony Algorithm

In this paper, the standard ant colony algorithm is being improved by the introduction of local pheromone update, to accelerate the convergence speed and avoid falling into local optimal. The following formula (18) is used for local update:

$$ \tau_{ij} (t + 1) = (1 - \theta )\tau_{ij} (t) + \theta \tau_0 $$
(18)

In Formula (18), \(\tau_0\) represents the initial pheromone value, \(\theta\) represents the local pheromone volatile factor. In the present position i of UUV, selecting the next position j. By reducing the pheromone value on the path \(\left( {i,j} \right)\) through the volatilization factor, the probability of the rest of the UUV selecting the path \(\left( {i,j} \right)\) will be reduced, hence, avoiding the repetition of the search path, the probability of searching the rest of the path can be increased, and the search speed of the algorithm can be improved. The global update still adopts the standard ant colony algorithm

$$ \tau_{ij} (t + 1) = (1 - \rho )\tau_{ij} (t) + \Delta \tau_{ij} . $$
(19)

The steps of the improved ant colony algorithm to solve the UUV recombination formation are described as follows:

  • Step 1: Initializing parameters.

    Initializing related parameters, including the number n of UUV in UUV formation, pheromone importance factor \(\alpha\) important factor of inspiration function \(\beta\), local pheromone volatile factor \(\theta\), global pheromone volatile factor \(\rho\), total pheromone release Q, maximum number of iterations iter_max, initial value of iteration number iter, Obtaining the initial position coordinates of each UUV and the position coordinates of the target point.

  • Step 2: Calculating the distance between UUVs.

    According to the initial position coordinates of each UUV, the mutual distance is calculated.

  • Step 3: Constructing the solution space.

    Each UUV is randomly placed at different starting points, and the next target position is calculated according to the following formula until all UUV reach the end of the formation.

  • Step 4: Updating the path.

    Calculating the path length \(L_k \left( {k = 1,2, \ldots ,n} \right)\) of each UUV and recording the shortest length \(L_{\min }\) of the formation path in the current number of iterations.

  • Step 5: Updating pheromones locally.

    When each UUV selects the target position once, pheromone update is carried out on the last path information \(\left( {i,j} \right)\) will be updated according to the formula

    $$ \tau_{ij} (t + 1) = (1 - \theta )\tau_{ij} (t) + \varepsilon \tau_0 $$
    (20)

    until the path construction is completed. Otherwise, returning to the Step 4 to continue updating the path.

  • Step 6: Updating pheromones globally.

    Updating the global pheromone concentration according to the formula, until the number of iterations is reached.

  • Step 7: The algorithm terminates.

If the algorithm does not reach the maximum number of iterations iter_max, setting iter = iter + 1 and returning to step 3; Otherwise, the algorithm terminates, and the results are outputted.

The block diagram of the algorithm is shown as follows (Fig. 3):

Fig. 3.
figure 3

Block diagram of improved ant colony algorithm

Full size image

5 Simulation Experiment

In order to verify the effectiveness of the algorithm proposed above, this paper conducts simulation experiments based on python. The experiment was conducted on a PC with a 2.7 GHz Intel Core i7-5700HQ CPU (4 CPU cores) on a Windows 10 64-bit operating system. Parameters of the improved ant colony algorithm are shown in Table 1, formation configuration is shown in Fig. 4.

Table 1. Parameters of the improved ant colony algorithm.
Full size table
Fig. 4.
figure 4

Formation diagram.

Full size image

9 UUVs are placed in the starting position, and the UUVs are divided into two sub-formations, among which, 1, 4 and 9 are the UUVs, which are the leaders. And the rest are the followers. After the simulation starts, the formation is formed at first, and the formation is converted into triangle and longitudinal shape respectively, then the formation is kept while sailing. If the unmanned ships encounter an obstacle while sailing, they will trigger an obstacle avoidance algorithm to pass between two relatively distant obstacles. After clearing the obstacle, they continue to perform the formation algorithm, in time of changing back into formation. The whole formation controls the trajectory of the whole process, as is shown in Fig. 5.

Fig. 5.
figure 5

Sailing chart of formation

Full size image

As can be seen from the trajectory of the whole formation control process in Fig. 6, the formation description and control algorithm in this paper can quickly transform multiple UUV unmanned ships into the specified formation. Able to restore a broken formation to a set formation after passing an obstacle. About 35 s after the obstacle avoidance is over, the formation returns to a triangle.

After obstacle avoidance, the position of UUV is shown in the figure below. The improved ant colony algorithm is used to optimize the path decision of the reformation. The new formation generation process is shown in the figure, and the generated optimal path is shown in Fig. 7.

Fig. 6.
figure 6

Initial and target points

Full size image
Fig. 7.
figure 7

Optimal path diagram

Full size image

The standard particle swarm optimization algorithm (PSO), the standard ant colony algorithm (ACO) and the improved ant colony algorithm (IACO) in this paper are respectively used for simulation experiments. The convergence rate of the algorithm is shown in Fig. 8. It can be seen from the convergence curve that IACO has faster convergence speed and higher search accuracy. When the algorithm is carried out for about 9 times, the optimal solution can be searched. Similarly, PSO and ACO require about 17 and 11 iterations respectively. The UUV formation is in a scattered position after autonomous obstacle avoidance. After IACO calculation, the shortest average distance of the optimal path generated after the 9th iteration is 72.4 m.

Fig. 8.
figure 8

Iterative process

Full size image

The algorithm was tested for 10 times, and the distance of the optimization path and the number of iterations to reach the final result were recorded. The results are shown in Table 2. It can be seen from the analysis of the data in the table that the path optimization based on IACO is significantly better than that of ACO and PSO in terms of path length and number of iterations. It can find the path with good quality in a short time, with the highest search accuracy and the least number of iterations. The path searched by PSO fluctuates greatly, with the average path length remaining at about 81 m, that by ACO is about 74 m, and that by IACO is about 72 m. The search accuracy is the highest and the number of iterations is the least.

Table 2. Comparison of the effects of three algorithms
Full size table

6 Conclusion

In this paper, firstly, the UUV formation, based on shape theory, is described. Then the UUV horizontal plane kinematics and dynamics models are established. Based on the consistency controller, the formation and maintenance of multi-UUV formation control under the delay condition are realized. Secondly, the ant colony algorithm in the bio-inspired algorithm was improved, and the local pheromone updating method was proposed to accelerate the convergence speed of the ant colony algorithm and avoid falling into the local optimal. The improved ant colony algorithm was used to find the optimal path in the process of multi-UUV formation recombination. Finally, the simulation platform is used to verify the effectiveness of the proposed method. The proposed method can be applied to a variety of task scenarios. In the face of external interference and obstacles, the time of reformation is effectively reduced, the endurance of the multi-UUV formation system is improved, and the redundancy is enhanced.