Cooperative localisation of UAV swarm based on adaptive SA-PSO algorithm

Abstract

In this paper, to address the cooperative localisation of a heterogeneous UAV swarm in the GNSS-denied environment, an adaptive simulated annealing-particle swarm optimisation (SA-PSO) cooperative localisation algorithm is proposed. Firstly, the forming principle of the communication and measurement framework is investigated in light of a heterogeneous UAV swarm composition. Secondly, a reasonably cooperative localisation function is established based on the proposed forming principle, which can minimise the relative localisation error with limited available information. Then, an adaptive weight principle is incorporated into the particle swarm optimisation (PSO) for better performance. Furthermore, in order to overcome the drawbacks of PSO algorithm easily falling into the local extreme point, an adaptive SA-PSO algorithm is improved to promote the convergence speed of cooperative localisation. Finally, comparative simulations are performed among the adaptive SA-PSO, adaptive PSO, and PSO algorithms to demonstrate the feasibility and superiority of the proposed adaptive SA-PSO algorithm. Simulation results show that the proposed algorithm has better performance in convergence speed, and the cooperative localisation precision can be guaranteed.

Nomenclature

N

the number of UAVs in the swarm

$n_A$

the number of well-equipped UAVs

$n_B$

the number of minimum required poorly equipped UAVs

$n_C$

the number of additional poorly equipped UAVs

$v_k$

the k-th node, each UAV in the swarm is regarded as a node in the digraph

$L_i$

the i-th well-equipped UAV, which can be also expressed as $v_i$

$M_j$

the j-th poorly equipped UAV, which can be also expressed as $v_{n_A+j}$

$\mathcal{G}$

graph of network

$\mathcal{V}$

the set of UAV

$\mathcal{E}$

the set of edges

G

the weighted adjacency matrix

$G_{M}$

the adjacency matrix of ranging principle

$G_{Mb}$

the adjacency matrix of basic-structure-based ranging principle

$G_{Ma}$

the adjacency matrix of additional-structure-based ranging principle

$n_b$

the number of UAVs containing in the basic structure

$a_{kh}$

the adjacency weight, $a_{kh}=a_{hk}(k\neq h)$

$R_{kh}$

the mutual ranging between $v_k$ and $v_h$ , $R_{kh}=R_{hk}(k\neq h)$

$x_k,y_k,z_k$

three dimensional coordinate position of the k-th UAV

$e_r$

the upper bound of IMU drift error

$\hat{x}_k,\hat{y}_k,\hat{z}_k$

inaccurate three dimensional coordinate position measured by IMU of the k-th UAV

1.0 Introduction

Recently, the localisation of UAV swarms in the civil and military domains has become an important area of research [1, 2, 3, 4, 5]. Generally, depending on the combination of information from the inertial measurement unit (IMU) and global navigation satellite system (GNSS), each UAV can offer position information for tracking or guidance [6, 7, 8]. In a UAV swarm, each individual UAV can be regarded as an intermediary node of communication transmission, which can process information and achieve cooperation by a communication network. [9] Moreover, based on the UAV swarm communication interaction, cooperative localisation realizes positioning by comprehensively using the measurement information of each UAV, resulting in a high-precision improvement in the absence of reliable information [10, 11, 12]. As a result, cooperative localisation, which makes full use of relative information within a swarm, has been attracting rising interest.

Indeed, influenced by the actual battlefield or city environment, it is difficult to ensure information acquisition and communication in a GNSS-denied environment. Usually, the information obtained by the UAV swarm will be obscured or jammed due to electromagnetic shielding, spoofing attacks, signal jamming, or other catastrophic GNSS failure events, resulting in divergence, collision or even failure of cooperation. In order to achieve effective work in a GNSS-denied environment, it would be of special interest to investigate how to ensure UAV position acquisition [13, 14, 15], and cooperative localisation shows its potential in the GNSS-denied environment.

Nowadays, UAV networks are implemented as flying wireless sensor networks (WSNs) in most studies. Cooperative localisation algorithms based on WSNs implementation generally formulate the location problem as an optimisation problem. In the past several decades, tradition algorithms such as Levenberg-Marquardt (LM) and quasi-Newton algorithm [16, 17] has been applied to solve the WSNs cooperative localisation problem. Unfortunately, disappointing convergence speed or initial conditions’ limitations prevent the applications of traditional algorithms. Therefore, various intelligent algorithms have been widely utilised in solving optimisation problems to overcome the drawbacks of the aforementioned traditional algorithms.

The particle swarm optimisation (PSO) algorithm is a well-known optimisation method, broadly used over the past two decades, which is effective for the swarm cooperation [18, 19, 20, 21, 22]. The PSO algorithm has been widely applied on WSNs to solve the cooperative localisation problem [23, 24, 25, 26, 27, 28]. Chuang et al. [23] carried out PSO-based localisation strategy based on the model with a sufficient and insufficient number of neighbouring anchors, respectively. With the estimated position of an unknown node and the anchors’ information, the solution of the normalised fitness function was searched by the traditional PSO algorithm.The proposed PSO-based node localisation scheme can reduce error accumulation and increase the success ratio in locating unknown nodes in WSNs. In order to further improve the accuracy and efficiency of PSO algorithm, a distributed two-phase PSO algorithm for solving the flip ambiguity problem was proposed by Li et al. [24]. The initial search space defined by bounding box method helps to accelerate the convergence speed, and the improved PSO algorithm can correct the error caused by the flip ambiguity problem, so as to localise more target nodes. Singh et al. [25] incorporated hop-based evaluation into PSO algorithm to overcome the positioning errors. Mizamir et al. [26] proposed a global best local neighbourhood PSO algorithm to improve the indoor localisation scheme, which contains three anchors used for relative distance measurement for each node to be localised.

Although sufficient research has been carried out on PSO-based cooperative localisation of WSNs, some of the assumptions make the existing schemes unsuitable for a direct migration to the UAV swarms’ applications in the GNSS-denied environment. Therefore, a few scholars have promoted PSO-based research on UAV localisation. Balaji et al. [29] presented a range-based localisation scheme for multi-rotor systems in the GNSS-denied environment. the PSO algorithm was used for tuning the noise of measurements, which were obtained by arranging the Ultra-Wideband sensors at the corners of a rectangle. Liu et al. [30] put forward an adaptive discrete PSO algorithm to find the global optimal solution of the proposed scheme, which was established on the software-defined radio platform with four anchor nodes and a server node. Compared with the traditional PSO algorithm, the improvement can ensure that the execution time is satisfied in real-time. Based on the fast anchor UAV searching technique, Arafat et al. [31] proposed an energy-efficient swarm-intelligence-based clustering algorithm based on the PSO algorithm, in which the particle fitness function was exploited for inter-cluster distance, intra-cluster distance, residual energy, and geographic location.

In the aforementioned anchor-based schemes, the demand for anchors is not conducive to applications in the GNSS-denied environment. Especially for the confrontation mission involving the long-distance flight and unfavourable geographical environment, it is difficult to set the fixed anchors. Even if the anchors can work, they are easily to be broken or even denied like GNSS, not to mention the potential cost burden. Compared with a homogeneous UAV swarm, in view of the cost demands on the military or civilian domain, there is a growing interest in research on the cooperative localisation problem of heterogeneous WSNs considering the characteristics of UAVs. Usually, only a small number of well-equipped UAVs can realize the self-positioning in the GNSS-denied environment. Most low-cost UAVs in a heterogeneous swarm have limited GNSS availability, and the information quality is degraded. Hence, how to combine the limited information, swarm composition and communication network to maximise the performance of each UAV to achieve the heterogeneous UAV swarm positioning is the key issue to be considered in this paper. Moreover, the PSO algorithm for clustering in UAV networks has not yet been extensively studied. Regarding the aformentioned analysis, there exist the following tor disadvantages based on the existing research. (1) The research on the UAV swarm forming principle is insufficient. In some engagements, UAV swarms working in the GNSS-denied environments cannot be simplified to WSNs due to the comprehensive influence of the communication networks, swarm compositions and flight missions. (2) The effectiveness of the PSO algorithm needs to be further improved. When solving a high-dimensional optimisation problem, the PSO algorithm easily falls into a local extreme point. Complex calculation makes the convergence speed limited insufficient and some physical indicators cannot be satisfied.

Therefore, based on the issue of cooperative localisation in the GNSS-denied environment, this paper proposes an adaptive simulated annealing-particle swarm optimisation (SA-PSO) algorithm to find the optimal position of shielded UAVs in the heterogeneous swarm by limited measurements. The main contributions are listed as follows: (1) A two-layer forming principle for cooperative localisation is proposed, in which the performance of each heterogeneous UAV can be brought into full play. (2) The well-determined cooperative localisation function is established based on the forming principle. As a result, it is reasonable to provide the basis for solving the cooperative localisation problem. (3) An adaptive SA-PSO cooperative location algorithm for a UAV swarm is proposed, which can effectively overcome the disadvantages of the PSO algorithm easily falling into the local extreme points, and greatly improve the convergence speed.

The remainder of this paper is organized as follows: Section 2 establishes the two-layer forming principle and the corresponding cooperative localisation functions. Section 3 introduces an adaptive SA-PSO algorithm-based cooperative localisation scheme. Numerical simulations are outlined in Section 4 and conclusions are summarised in section 5.

2.0 Problem formulation

Considering a heterogeneous UAV swarm, which consists of UAVs with different performance levels, serves the mission requirements in the GNSS-denied environment. For modelling, some assumptions are made as follows:

Assumption 1. The UAVs are treated as ideal mass points. UAVs use sensor devices, installed on their fuselage to perform ranging, positioning calculations and data transmission.

Assumption 2. A heterogeneous swarm performs cooperative localisation in GNSS-denied environment. The well-equipped UAVs with sufficient computing capability can achieve self-positioning. Without information from satellites, the cooperative localisation system of poorly equipped UAVs are mainly composed of low-precision inertial measurement unit (IMU) and low-cost mutual ranging equipment, such as inter-flight data links or onboard strap-down finders.

Assumption 3. Inter-flight mutual ranging and communication are ideal. Measurements and localisation can be completed and transferred by the network in real-time, ignoring external disturbances and transmission time. Simultaneously, the relative distance between any two UAVs cannot exceed the coverage of the communication and measurement capabilities. Besides, the same mutual ranging measurements do not exist in this localisation engagement.

2.1 Forming principle

Considering a GNSS-denied scenario, with the inter-flight mutual ranging, poorly equipped UAVs can also realize localisation by cooperation. To address this challenge, a two-layer scheme is investigated based on the composition of the heterogeneous swarm. According to their capabilities, UAVs in the swarm are divided into two types: A-UAV can still realise the localisation in GNSS-denied environment; B-UAV, which can only utilise low-precision IMU for localisation. Correspondingly, the scheme consists of measurement and communication layers. The two-layer schematic diagram for cooperative localisation is shown in Fig. 1.

Figure 1. The schematic diagram of cooperative localisation.

The set of UAV swarm in the whole network is defined as $\mathcal{N}=\{1,2,\dots,N\}$ , where $N=n_A+n_B+n_c$ , $k\in\mathcal{N}$ , $i\in{1,2,\dots,n_A}$ and $j\in{n_A+1,\dots,N}$ . For explanation the forming principle, some preliminaries about graph theory are given. Let $\mathcal{G}=(\mathcal{V},\mathcal{E},G)$ be a weighted graph of order P, with $\mathcal{V}=\{v_1,v_2,\dots,v_N\}$ , $\mathcal{E}\subseteq\mathcal{V}\times\mathcal{V}$ and $G=(G_{pq})_{P\times P}$ . A directed edge $\mathcal{E}_{ij}$ in network $\mathcal{G}$ is denoted by the ordered pair of nodes $(v_p,v_q)$ , where node $v_p$ can receive information from $v_q$ , but not another way around.

Definition 1 [32]. A network is called undirected if there is a connection between two nodes $v_p$ and $v_q$ in $\mathcal{G}$ and $G_{pq}=G_{qp}>0$ , otherwise, $G_{pq}=G_{qp}=0(p\neq q,p,q=1,2,\dots,N)$ . A network is called directed if there is a connection from $v_p$ and $v_q$ in $\mathcal{G}$ and $G_{qp}>0$ ,otherwise, $G_{qp}=0(p\neq q,p,q=1,2,\dots,N)$ .

Definition 2 [32]. An undirected network $\mathcal{G}$ is complete connected if it has a connection between each pair of distinct nodes $v_p$ and $v_q$ in $\mathcal{G}$ .

Figure 2. Ranging principle incorporated with composition. (a) ranging principle of structure 1 and (b) ranging principle based on structure 2.

Definition 3 [32]. An undirected network $\mathcal{G}$ is strongly connected if there exists an undirected path between any pair of distinct nodes $v_p$ and $v_q$ in $\mathcal{G}$ .

According to Assumption 3, following three basic forming structures are discussed

• Structure 1: $n_A=1$ and $n_B=5$ .

• Structure 2: $n_A=2$ and $n_B=3$ .

• Structure 3: $n_A\geq3$ and $n_B=1$ .

According to the composition of a heterogeneous swarm, the forming principle is divided into ranging principle and communication networks. For each proposed composition, the ranging principle is outlined as the principles for the basic structure and its additional structure. The diagrammatic ranging principles for basic structures are shown in Fig. 2. Based on the measurements, the corresponding network is essential to data transmission. A network supporting communication between heterogeneous and homogeneous UAVs is given.

1. 1. Structure 1 : 1 A-UAV and 5 minimum required B-UAVs.

   Basic composition with one A-UAV $L_1$ and the minimum required 5 B-UAVs satisfies the following principle:

   • Ranging principle of measurement layer:

     The ranging principle of basic structure 1 is similar to the complete graph, as shown in Fig. 2(a). Measure the mutual ranging between the A-UAV and each B-UAV. Besides, the mutual ranging between any two B-UAVs needs to be measured.

     The ranging principle of the additional structure requires each additional B-UAV to hold three new ranging links. Simultaneously, the additional structure at least contains a ranging link connected with the UAV in the basic structure.

   • Cooperative network of the communication layer:

     Taking the only A-UAV as the computing centre, the communication network of the $L_1$ -based swarm is undirected and strongly connected.

2. 2. Structure 2 : 2 A-UAVs and 3 minimum required B-UAVs.

   Basic composition with two A-UAVs $L_1,L_2$ and the minimum required 3 B-UAVs satisfies the following principle:

   • Ranging principle of measurement layer:

     As shown in Fig. 2(b), the mutual ranging relationships of L1 and L2 with the other three B-UAVs satisfy undirected and complete connections, respectively.

     The ranging principle of additional structure requires each additional B-UAV to hold three ranging links, at least one of which should be kept with the UAV in the basic structure.

   • Cooperative network of communication layer:

     Choose one of A-UAVs as the computing centre. At the very least, the communication network containing the selected A-UAV and all the B-UAV should be strongly connected. Simultaneously, there exists a directed connection from the other A-UAV to the computing centre.

3. 3. Structure 3 : more than three A-UAVs and 1 B-UAV.

   Basic composition with A-UAVs more than three and 1 B- UAV satisfies the following principle:

   • Ranging principle of measurement layer:

     For any B-UAV in the heterogeneous swarm, its localisation problem can be solved if there exist three ranging links connected to different A-UAVs in the basic structure 3.

     The ranging principle of additional structure first requires that three new ranging links are added correspondingly for each additional B-UAV. For the first additional B-UAV $M_{n_B+1}$ , there exist at least two ranging links connected with different A-UAVs. For the second additional B-UAV $M_{n_B+2}$ , at least one ranging link connected to any A-UAV is needed. When $n_C\geq 3$ , $M_j(j=3,\dots,n_B + n_C)$ only needs to ensure three-ranging-link principle.

   • Cooperative Network of Communication layer:

     Choosing one of A-UAVs as the computing centre, at the very least, the communication network containing the selected A-UAV and all the B-UAVs should be strongly connected. Simultaneously, there exist directed connections from the other A-UAVs to the computing centre, which means that the computing A-UAV can obtain the complete information through transmission.

Remark 1. In the proposed scheme, the cooperative network serves for the data transmission and calculation. In the communication layer, take the selected UAV as centre node, the cooperative network can be simplified as follows:

• There always exists a directed path from any UAV to the centre A-UAV.

• There always exists a directed path from the centre A-UAV to any B-UAV.

Hence, centre A-UAV can receive the data of swarm and send the computed localisation results to the corresponding B-UAV after computing.

2.2 Cooperative localisation functions

Based on the above compositions of the heterogeneous swarm, the forming principle are given for ensuring the data transmission and adequate information for localisation. Accordingly, the cooperative relative localisation functions are established and the fitness function is selected.

To describe the ranging principle as undirected graph $G_M$ , the basic structure based ranging principle is given as follows

(1) \begin{equation}G_{Mb}=[a_{hk}]_{n_b \times n_b}=\left[\begin{array}{c@{\quad}c} \mathit{0}_{n_{A} \times n_{A}} & \mathit{1}_{n_{A} \times n_{B}} \\[4pt] \mathit{1}_{n_{B} \times n_{A}} & \mathit{1}_{n_{B} \times n_{B}}- \mathit{I}_{n_{B} \times n_{B}} \end{array}\right]\end{equation}

and the additional structure based ranging principle of $v_k$ is given as follows

(2) \begin{equation} G_{Ma}(1:(k-1),k)=\left[ a_{hk}\right]_{(k-1) \times 1},(k>n_b,k>h,h = 1,2,\dots,k-1)\end{equation}

where $n_b=n_A +n_B$ , and if the mutual ranging between $v_k$ and $v_j$ needs to be measured, then the adjacency weight $a_{hk}=a_{kh}=1(k\neq h)$ , otherwise, $a_{hk}=a_{kh}=0$ . Hence, $G_M$ is expressed as

(3) \begin{align} G_{M b}=\left[ \begin{array}{c@{\quad}c|c@{\quad}c@{\quad}c} 0_{n_{A} \times n_{A}} & 1_{n_{A} \times n_{B}} & G_{M a}\left(1\,:\, n_{A}, n_{b}+1\right) & \ldots & G_{M a}\left(1\,:\, n_{A}, \textrm{N}\right) \\[4pt] 1_{n_{B} \times n_{A}} & 1_{n_{B} \times n_{B}}-I_{n_{B} \times n_{B}} & G_{M a}\left[\left(n_{A}+1\right)\,:\,\left(n_{b}\right), n_{b}+1\right] & \ldots & G_{M a}\left[\left(n_{A}+1\right)\,:\,\left(n_{b}\right), \textrm{N}\right] \\[4pt] \hline G_{M a}^{\textrm{T}}\left(1\,:\, n_{A}, n_{b}+1\right) & G_{M a}^{\textrm{T}}\left[\left(n_{A}+1\right)\,:\,\left(n_{b}\right), n_{b}+1\right] & 0 & \ldots & a_{n_{b}+1, k} \\[4pt] \vdots & \vdots & \vdots & \ddots & \vdots \\[4pt] G_{M a}^{\textrm{T}}\left(1\,:\, n_{A}, \textrm{N}\right) & G_{M a}^{\textrm{T}}\left[\left(n_{A}+1\right)\,:\,\left(n_{b}\right), \textrm{N}\right] & a_{k, n_{b}+1} & \ldots & 0 \end{array}\right]\nonumber\\[5pt]\end{align}

Thus, let $X=[x_{(n_A+1)},y_{(n_A+1)},z_{(n_A+1)},\dots,x_k,y_k,z_k,\dots,x_N,y_N,z_N]^T \in N^{3(n_B+n_C)}$ ,the cooperative localisation function is described as follows

(4) \begin{equation} F(X) = [f_{hk}]_{3(n_B+n_C)\times 1}=0, k= n_A+1,\dots,N\ {\textrm{where}}\ k>h\ {\textrm{and}}\ a_{hk}=1\end{equation}

with

(5) \begin{equation} f_{hk}=\sqrt{\left(x_{k}-x_{h}\right)^{2}+\left(y_{k}-y_{h}\right)^{2}+\left(z_{k}-z_{h}\right)^{2}}-R_{h k}\end{equation}

Remark 2. Based on Assumption 3, ranging principle of the measurement layer is established, where the number of localisation functions is equal to the number of position coordinates to be solved. When the number of well-equipped A-UAVs is given, the basic structure can ensure that the number of functions is the same as the number of state variables. Furthermore, as the basically additional ranging principle requires, the additional structure with each UAV holding three new ranging links means that the number of swarm localisation functions satisfies the following equation:

(6) \begin{equation} n_A \times n_B +\sum_{p=1}^{n_B-1}p +3 \times n_C=3 \times (n_B+n_C) \end{equation}

Sufficient and non-repetitive measurements can avoid the existence problem of the overdetermined or underdetermined equations. Therefore, the equation set constructed by the localisation functions is well-determined and has a solution.

In the cooperative localisation engagement, with known upper bound of IMU drift error $e_r$ , the measurements transmitted to the centre A-UAV are divided into three types:

• position coordinates of A-UAVs: $[x_k,y_k,z_k], \ k=1,2,\dots,n_A$ ;

• mutual ranging based on the ranging principle: $R_{h k},\ k= n_A+1,\dots,N,\ k>h$ ;

• inaccurate position coordinates measured by IMU $[\hat{x}_k,\hat{y}_k,\hat{z}_k], \ k=n_A+1,\dots,N$ .

Therefore, the localisation problem can be transformed into how to find a solution to minimize the error effectively. we define the fitness function as follows

(7) \begin{equation} F_{\max}=\max(|f_{hk}|),k= n_A+1,\dots,N\ {\textrm{where}}\ k>h\ {\textrm{and}}\ a_{hk}=1\end{equation}

Remark 3. Generally, it is difficult to search for the exact solutions in practical engineering. Hence, the cooperative localisation problem is usually solved within the allowable accuracy range $P_e$ . For relatively accurate integrated navigation relying on the GNSS, the localisation error is usually on the order of ten metres. In order to improve the accuracy of localisation, setting a smaller $P_e$ will be beneficial. Nevertheless, to meet the navigation update requirements, comprehensively considering the balance between localisation accuracy and computing time is needed.

3.0 Cooperative localisation scheme

In this section, subject to the localisation functions and the fitness function, an improved PSO-based swarm intelligence algorithm is proposed to minimise the ranging error. As a preliminary, the PSO algorithm is briefly introduced. Then, an adaptive improvement is made to speed up the PSO algorithm. Furthermore, in order to overcome the drawbacks of falling into local extreme point easily in traditional PSO algorithm, as well as improve the search accuracy and convergence speed, an adaptive SA-PSO algorithm is proposed based on a combination of SA and adaptive PSO algorithms. Finally, the cooperative localisation method based on the proposed SA-PSO algorithm is presented.

3.1 PSO algorithm

Practical swarm optimisation (PSO) algorithm is one of swarm intelligence algorithms. Inspired by the social behaviour of birds, the PSO algorithm was first proposed by James Kennedy and Russell Eherhart [18, 19]. Each single particle represents a bird and the birds flocked around a ‘roost’. Similar to birds looking for attractive food, the particle searches for the global optimal space. To implement the concept, the position of each particle is updated based on the social behaviour of a population of individuals that adapts to its environment by returning to previously discovered promising regions. The process is stochastic in nature and makes use of the memory of each particle, as well as the knowledge gained by the swarm as a whole. Firstly, initialise a set of particles randomly distributed throughout the design space, in which the position of each particle is a feasible solution. Then, the fitness value of the k-th particle can be recorded in the process of iterating, and the optimal fitness value of previous iterations will be memorised, which is called the individual extreme value( $p_{best}$ ). Simultaneously, other particles exchange information with each other to find the global extreme value ( $g_{best}$ ), which represents an optimal value among all particles in the current iteration process. Finally, influenced by the memory of two extreme values, the dynamical update is made for the speed of the next iteration, which drives the particles to move closer to the best point.

The schematic diagram of particle motion is shown in Fig. 3.

Figure 3. Particle motion diagram of the PSO algorithm.

The attributes of each particle include position, speed and fitness function. Suppose a group $\mathcal{P}$ composed of P particles in the $\mathcal{D}$ -dimensional search space, where the position of q-th particle is $p_q=(p_{q1},p_{q2},\dots,p_{qd},\dots,p_{q\mathcal{D}})^T$ , the speed is expressed as ${S}_q=(s_{q1},s_{q2},\dots,s_{qd},\dots,s_{q\mathcal{D}})^T$ , the fitness function is defined as $f_{p_q}$ and the number of iterations is $iter\in[1,2,\dots,\mathcal{I}]$ . In the iterative process of the PSO algorithm, the calculation formulas of the velocity and position are described as follows,

(8) \begin{equation} \left\{\begin{array}{l} s_{q d}( { iter }+1)=w s_{q d}( { iter })+c_{1} r_{1}\left(p_{ {best }, d}-p_{q d}({ iter })\right)+c_{2} r_{2}\left(g_{ {best }, d}-p_{q d}( { iter })\right) \\[4pt] p_{q d}( { iter }+1)=p_{q d}( { iter })+s_{q d}( { iter }) \end{array}\right. \end{equation}

where w represents the weight of inertia, $c_1$ and $c_2$ are non-negative constants, $r_1$ and $r_2$ are random numbers and obey the uniform distribution on [0,1]. Denote $s_{\max}$ as the bounds of particle search range and the speed $s_{qd}\in[{-}s_{\max},s_{\max}]$ .

The weight of inertia w in the PSO algorithm adjusts with the number of iteration as follows

(9) \begin{equation} w(iter)=w_{\max}-\frac{iter}{\mathcal{I}}(w_{\max}-w_{\min}) \end{equation}

where $w_{\max}$ and $w_{\min}$ are upper and lower bounds of the weight on [0,1].

The outline of a basic PSO algorithm is given in the Fig. 4.

Figure 4. Diagram of the PSO algorithm.

3.2 Adaptive weight for the PSO algorithm

In view of the weak convergence speed of the PSO algorithm, an adaptive weight is introduced as a further improvement. An adjustable parameter $\beta$ is added into it to adjust the inertia weight factor w and the acceleration factors $c_1$ and $c_2$ in real-time. The adaptive principle is described as follows,

(10) \begin{equation} \left\{\begin{array}{l} \alpha( { iter })=\dfrac{\sum_{{q}=1}^{P} \sqrt{\sum_{{d}=1}^\mathcal{D}\left[p_{{qd}}( { iter })-g_{best, {d}}( { iter })\right]^{2}}}{N} \\[8pt] \beta( { iter })=\dfrac{\alpha( { iter })}{\max (\alpha)} \\[9pt] w( { iter })=\beta( { iter }) \\[4pt] c_{1}( { iter })=2 \times \beta({ iter }) \\[4pt] c_{2}( { iter })=2-c_{1}( { iter }) \end{array}\right.\end{equation}

Remark 4. Based onthe adaptive principle, the w, $c_1$ and $c_2$ are simultaneously controlled by $\beta$ . The adaptive improvement can promote the convergence speed. At the beginning, $c_1$ is bigger than $c_2$ , which means that the errors with individual optimum play a more important role. With iter increasing and w and $\beta$ decreasing, $c_1$ decreases and $c_2$ increases, correspondingly.

3.3 Adaptive SA-PSO algorithm-based cooperative localisation

In order to further improve the performance of the adaptive PSO algorithm, a simulated annealing(SA) is introduced to it.

Although the convergence speed is improved by the adaptive-improved PSO algorithm, limited by the inner characteristic of the PSO algorithm, falling into a local extreme point is an inevitable problem in the calculation process. Particles may converge to the minimum value prematurely during the search process, and falling into the local extreme point easily will lead to an obvious reduction of global search ability.

The SA algorithm faces the drawbacks of relying too much on the process of cooling. On the other hand, if global convergence is achieved, harsher temperature-limiting conditions are required, which leads to a reduction of optimisation efficiency.

On the other hand, the SA algorithm is a heuristic algorithm that allows a solution worse than the current one in a certain probable situation. Thus, it is possible for the SA algorithm to break away from the local optimal solution to obtain the global optimal solution, which can effectively overcome the aforementioned disadvantage of the PSO algorithm.

Therefore, combined with the adaptive weight, the SA and the PSO, the proposed algorithm will not only improve the local optimisation of the cooperative localisation problem but also realize the requirement for high efficiency. The diagram of cooperative localisation based on adaptive SA-PSO algorithm is given in Fig. 5.

Figure 5. The diagram of cooperative localisation based on adaptive SA-PSO algorithm.

The adaptive SA-PSO algorithm for cooperative localisation is proposed as follows:

1. 1. Data collection. Transmit the necessary data to the centre A-UAV by forming principle organised in Section 2.1. The data includes the positions of A-UAVs, the mutual ranging, inaccurate positions of B-UAVs measured by IMU and the upper bound of IMU drift error.

2. 2. Parameter initialisation. Initialise the following parameters: population size P, maximum number of iterations $\mathcal{I}$ , speed limitation $s_{\max}$ , initial weight w and initial acceleration factors $c_1$ and $c_2$ .

3. 3. Population initialisation. Initialise the states on the basis of inaccurate localisation measurements $[\hat{x},\hat{y},\hat{z}]$ and the upper bound of IMU drift error $e_r$ .

4. 4. Fitness calculation. Evaluate the fitness values of each particle and swarm. The current particle’s position and fitness value are stored in $\gamma_q$ . Store the individual optimum $p_{best}$ and global optimum $g_{best}$ .

5. 5. Temperature initialisation. Initialise the annealing temperature t(0) as

   (11) \begin{equation} t(0)=\frac{F_{\max}(p_{best})}{ln5} \end{equation}

6. 6. Temperature calculation.

   (12) \begin{equation} t_{F}\left(\gamma_{q}\right)=\dfrac{\textrm{e}^{-\left(F_{\max}\left(\gamma_{q}\right)-F_{\max}\left(p_{best}\right)\right) / t( { iter }-1)}}{\sum_{{q}=1}^{{P}} \textrm{e}^{-\left(F_{\max}\left(\gamma_{{q}}\right)-F_{\max}\left(p_{best}\right)\right) / t({ iter }-1)}} \end{equation}

7. 7. Cumulative probability calculation. Calculate the cumulative probability of each individual

   (13) \begin{equation} T_F(q)=\sum_{l=1}^{q}{t_F\left(\gamma_l\right)} \end{equation}

8. 8. Roulette judgement. Apply the roulette rule to determine the global optimal, substitute value $p^{*}_{best}$ from all the $p_{best}$ . Generate a random number $\epsilon\in[0,1]$ , if $\epsilon<T_F(1)$ , the first particle is chosen; else if $T_F(q-1)<r<T_F(q)$ , the q-th particle is chosen.

9. 9. States updating. Update the states of each particle according to Equations (8) and (10).

10. 10. Temperature reduction. Apply the attenuation coefficient method to realise the temperature-reducing process as

    (14) \begin{equation} t(iter+1)=0.4t(iter) \end{equation}

11. 11. Judgement. Stop the optimisation process and output the localisation results when the termination conditions are satisfied; otherwise, return to Step 6 and continue the optimisation search.

4.0 Simulation

In this section, the effectiveness of the proposed adaptive SA-PSO cooperative relative localisation algorithm was demonstrated. Subject to various engagements, two aspects of the proposed cooperative localisation algorithm are verified: the expandability of the forming principle and the superiority of the adaptive SA-PSO algorithm. For simplification, the expressions of three PSO algorithms are abbreviated in the legend as ‘PSO’, ‘APSO’ and ‘SAPSO’, respectively.

4.1 Simulation of basic forming structure

In the case of solving the localisation problem by forming principle of three basic structures, comparative experiments were conducted with the PSO algorithm to verify the feasibility and superiority of the proposed adaptive SA-PSO cooperative relative localisation algorithm.

4.1.1 Simulation of basic forming structure 1

In this GNSS-degraded engagement, 5 poorly equipped UAVs’ integrated navigation lost the GNSS information for a period of time. According to the performance of the equipped IMU, the upper bound of the drift errors in this interrupted time period is $e_r=60$ m.

Generate randomly the IMU measurements within a reasonable range, the initial conditions are listed in Table 1.

Table 1. Initial conditions of Case 4.1.1

Hence, according to Equation (1) the symmetric matrix of mutual ranging can be measured as follows,

(15) \begin{equation} R_{ideal}=\left[ \begin{array}{l@{\quad}|@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l} 0&6,473.634&8,594.780&6,547.223&9,045.309&9,646.087\\[4pt] \hline \dots&0&5,081.110&5,624.563&9,178.430&7,455.768\\[4pt] \dots&&0&2,891.439&6,338.085&4,816.879\\[4pt] \dots&&&0&4,646.996&4,689.856\\[4pt] \dots&&&&0&3,407.461\\[4pt] \dots&&&&&0\\[4pt] \end{array} \right] \end{equation}

Choose the common parameters as $P=120$ , $\mathcal{I}=800$ , $V_{\max}=60$ and $P_e=1$ m. Since all randomly generated particles are located in a spherical space, so we took IMU measurements $[\hat{x},\hat{y},\hat{z}]$ as centre and $e_r$ as radius. The initial $p_{best}$ started from the IMU measurements. The constant parameters of PSO algorithm were designed as $w_{\max}=0.8$ , $w_{\min}=0.6$ and $c_1=c_2=1.49445$ .

The simulation results are shown in Fig. 6.

Figure 6. Optimisation results of basic structure 1. (a) Fitness value, and (b) adaptive acceleration factors.

The cooperative localisation results of basic structure 1 are shown in Table 2.

Table 2. Localisation results of Case 4.1.1

4.1.2 Simulation of basic forming structure 2

Similar to the Section 4.1.1, when $e_r=80$ m, A-UAVs $L_1$ and $L_2$ locate in (3,000m, 6,000m, 3,500m) and (4,000m, 4,500m, 3,000m), the initial conditions of B-UAVs were listed in Table 3.

Table 3. Initial conditions of Case 4.1.2

According to Equation (1), the symmetric matrix of the measured mutual ranging was given as follows,

(16) \begin{equation} R_{ideal}= \left[ \begin{array}{l@{\quad}l@{\quad}|@{\quad}l@{\quad}l@{\quad}l} 0&0&2,348.583&5,025.350&3,790.795\\[4pt] 0&0&2,989.455&3,586.941&3,239.310\\[4pt] \hline \dots&&0&4,391.319&2,146.067\\[4pt] \dots&&&0&2,755.054\\[4pt] \dots&&&&0\\[4pt] \end{array} \right] \end{equation}

Some parameters were selected as $P=100$ and $\mathcal{I}=500$ . The simulation results are shown in the Fig. 7 and Table 4.

Table 4. Localisation results of Case 4.1.2

Figure 7. Optimisation results of basic structure 2. (a) Fitness value, and (b) adaptive acceleration factors.

4.1.3 Simulation of basic forming structure 3

In this case, the only B-UAV $M_4$ is located at (1,200m, 1,200m, 2,221m). The known IMU drift error $e_r$ is less than 70m and the measured location is (1,203.759m, 1,262.639m, 2,226.235m). The A-UAVs $L_1$ , $L_2$ and $L_3$ are located at (2,000m, 4,000m, 3,500m), (3,000m, 4,500m, 3,000m) and (4,000m, 3,400m, 4,000m), respectively. The number of particles and iterations were set as 30 and 100. $P_e$ was chosen as $0.001$ m.

According to Equation (1), the symmetric matrix of the measured mutual ranging was given as follows,

(17) \begin{equation} R_{ideal}= \left[ \begin{array}{l@{\quad}l@{\quad}l@{\quad}|@{\quad}l} 0&0&0&3,180.541\\[4pt] 0&0&0&3,838.859\\[4pt] 0&0&0&3,980.558\\[4pt] \hline 3,180.541&3,838.859&3,980.558&0\\[4pt] \end{array} \right] \end{equation}

The simulation results are shown in the Fig. 8. The calculation time of three algorithms are $0.0476$ , $0.0174$ and $0.0096$ s, respectively.

Figure 8. Optimisation results of basic structure 3. (a) Fitness value, and (b) adaptive acceleration factors.

4.1.4 Simulation analysis of basic forming principle

Based on the above simulations, the effectiveness of the proposed cooperative relative localisation scheme for basic structure is demonstrated. In the GNSS-degraded environment, the heterogeneous UAV swarm can realise cooperative localisation with the forming principle established on the composition. With the ranging principle and communication network, the cooperative localisation problem can be solved through limited information. Obviously, when termination condition $F_{\max}<P_e$ is satisfied, the maximum localisation error is less than 11m which satisfies the individual accuracy within 15m.

The SA-PSO cooperative localisation algorithm shows a performance advantage in terms of computing speed. The computing speed of the proposed adaptive SA-PSO algorithm increases significantly. In the first and second case, the convergence speed of the proposed adaptive SA-PSO algorithm increases by over 10-fold compared with traditional PSO algorithm and over 1.5-fold compared with APSO algorithm. For the reduction of variables to be solved, the acceleration of computing speed is significant. In the third case, with a higher termination condition, SA-PSO algorithm realises the convergence within 10ms. The convergence speed of SA-PSO algorithm is more than 1.5 times higher than that of APSO algorithm and nearly 3.5 times higher than that of the PSO algorithm.

4.2 Simulation of additional forming structure

Based on the simulation of basic structure, the expandability of proposed ranging principle for additional structure is demonstrated in this section.

4.2.1 Simulation of the general additional ranging principle

For the additional ranging principle, the general basic condition requires three unrepeated ranging links.

Take Structure 2 for example, the applicability of additional principle is verified in this simulation.

The common parameters were given as $P=100$ , $\mathcal{I}=800$ and $V_{\max}=60$ .

The real position conditions of basic structure are same as that in Section 4.1.2. One additional B-UAV located in (2,000m, 2,500m, 4,828m) was added. The additional-structure-based ranging connection is $G_{Ma}=[0;0;1;1;1;0]$ .With $e_r=70$ m, generate randomly the IMU measurements at (2,017.054m, 2,474.198m, 4,851.443m) and the corresponding mutual ranging matrix is measured as follows,

(18) \begin{equation} R_{ideal}= \left[ \begin{array}{l@{\quad}l@{\quad}|@{\quad}l@{\quad}l@{\quad}l@{\quad}|@{\quad}l} 0&0&2,348.583&5,025.350&3,790.795&0\\[4pt] 0&0&2,989.455&3,586.941&3,239.310&0\\[4pt] \hline \dots&&0&4,391.319&2,146.067&3,837.506\\[4pt] \dots&&&0&2,755.054&3,258.812\\[4pt] \dots&&&&0&3,833.670\\[4pt] \hline \dots&&&&&0\\[4pt] \end{array}\right]\end{equation}

The simulation results are shown in the Fig. 9 and Table 5.

Table 5. Localisation results of Case 4.2.1

Figure 9. Optimisation results of additional structure based on structure 2. (a) Fitness value, and (b) adaptive acceleration factors.

4.2.2 Simulation of the additional ranging principle for Structure 3

According to the ranging principle of structure 3, the first two additional B-UAVs are required to satisfy the corresponding ranging connection. With the conditions in Section 4.1.3, the cases were given as follows,

• Case 1 : one additional UAV

  Parameter selection: $N=100$ , $\mathcal{I}=800$ , $V_{\max}=60$ , $P_e=0.5$ m, $e_r=70$ m.

  $M_5$ is located at (2,000m, 1,500m, 1,828m) and $G_{Ma}^T=[1 \quad 0 \quad 1 \quad 1 \quad 0]$ .

• Case 2 : two additional UAVs

  Parameter selection: $N=100$ , $\mathcal{I}=800$ , $V_{\max}=60$ , $P_e=0.5$ m, $e_r=70$ m.

  $M_6$ is located at (1,700m, 1,600m, 1,320m) and $G_{Ma}^T=\left[\begin{array}{l@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l}1 &0& 1& 1& 0& 0\\0& 1& 0& 1& 1& 0\end{array}\right]$ .

• Case 3 : three additional UAVs

  Parameter selection: $N=120$ , $\mathcal{I}=800$ , $V_{\max}=80, $ , $P_e=1$ m, $e_r=60$ m.

  $M_7$ is located at (2,600m, 1,123m, 3,400m) and $G_{Ma}^T=\left[\begin{array}{l@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l}0 &1& 1& 1& 0& 0&0\\[4pt]0& 1& 0& 1& 1& 0&0\\[4pt]0& 0& 0& 1&1&1&0 \end{array}\right]$ .

The simulation results are shown in the Tables 6–8.

Table 6. Localisation results of additional Case 1

Table 7. Localisation results of additional Case 2

Table 8. Localisation results of additional Case 3

4.2.3 Simulation analysis of additional ranging principle

In these simulations, the additional ranging principle was verified. Theoretically, the well-determined cooperative localisation function ensures the existence of the solution. Through simulation verification, the expandability of the proposed cooperative relative localisation scheme is ensured by the additional ranging principle. Even though the scale of the heterogeneous swarm is enlarged, the localisation problem can be solved with limited measurements.

The simulation results also show the superiority of adaptive SA-PSO cooperative localisation algorithm. With the scale expansion, the complexity of the localisation problem grows rapidly, which challenges the performance of the computer. As the case for the additional principle shown in this section, the new additional UAV extends the calculation time of the PSO algorithm twice times than that of basic structure. According to the three cases of expanded Structure 3, the calculation time of the PSO algorithm exceeds the update capability of the UAV and more time is consumed with the quantity of additional B-UAV increasing. The convergence speed of the proposed adaptive SA-PSO algorithm is promoted over 10-fold compared with traditional PSO algorithm and over 1.3-fold compared with APSO algorithm.

Hence, combined with the composition design of heterogeneous swarm, the cooperative localisation problem can be solved with the proposed ranging principle. Simultaneously, the better convergence capability of SA-PSO cooperative localisation algorithm benefits to the applications.

5.0 Conclusion

In this paper, we have first established a composition-based forming principle for the cooperative localisation problem. Then, according to the proposed forming principle, an adaptive SA-PSO algorithm has been proposed to improve the convergence speed of cooperative localisation. The main contributions can be drawn as:

1. 1. A two-layer forming principle is investigated. Comprehensively considering the communication and measurement, the principle helps to make full use of the performance of a heterogeneous UAV swarm.

2. 2. Based on the proposed forming principle, the cooperative localisation problem is transformed into an optimisation problem. The expandability of the proposed forming principle can be guaranteed by the correspondingly well-determined equations.

3. 3. The proposed adaptive SA-PSO algorithm can solve the cooperative localisation problem effectively and quickly. Simulation results demonstrate that the localisation precision can be ensured with a maximum single error less than 8m. Simultaneously, the convergence speed of the proposed algorithm can be promoted more than 10 times compared with the PSO algorithm.

In the future, we will study the cooperative localisation problem with more practical factors, such as the time-delay and disturbances on transmission, communication restrictions and UAV dynamics.