Rapid Initialization Method of Unmanned Aerial Vehicle Swarm Based on VIO-UWB in Satellite Denial Environment

Abstract

In environments where satellite signals are blocked, initializing UAV swarms quickly is a technical challenge, especially indoors or in areas with weak satellite signals, making it difficult to establish the relative position of the swarm. Two common methods for initialization are using the camera for joint SLAM initialization, which increases communication burden due to image feature point analysis, and obtaining a rough positional relationship using prior information through a device such as a magnetic compass, which lacks accuracy. In recent years, visual–inertial odometry (VIO) technology has significantly progressed, providing new solutions. With improved computing power and enhanced VIO accuracy, it is now possible to establish the relative position relationship through the movement of drones. This paper proposes a two-stage robust initialization method for swarms of more than four UAVs, suitable for larger-scale satellite denial scenarios. Firstly, the paper analyzes the Cramér–Rao lower bound (CRLB) problem and the moving configuration problem of the cluster to determine the optimal anchor node for the algorithm. Subsequently, a strategy is used to screen anchor nodes that are close to the lower bound of CRLB, and an optimization problem is constructed to solve the position relationship between anchor nodes through the relative motion and ranging relationship between UAVs. This optimization problem includes quadratic constraints as well as linear constraints and is a quadratically constrained quadratic programming problem (QCQP) with high robustness and high precision. After addressing the anchor node problem, this paper simplifies and improves a fast swarm cooperative positioning algorithm, which is faster than the traditional multidimensional scaling (MDS) algorithm. The results of theoretical simulations and actual UAV tests demonstrate that the proposed algorithm is advanced, superior, and effectively solves the UAV swarm initialization problem under the condition of a satellite signal rejection.

1. Introduction

In recent years, the deployment and application of UAV (Unmanned Aerial Vehicle) swarms have increased significantly due to their versatility and effectiveness in a wide range of applications [1,2]. These applications include search and rescue operations [3], environmental monitoring [4], agricultural monitoring [5,6], and military missions [7]. The collective operation of a swarm of drones provides better coverage, greater robustness, and greater efficiency than drones operating alone. Collaborative positioning involves different tasks, including collaborative mapping and positioning [8]. However, accurately and quickly initializing the relative position of a drone swarm, especially in environments where satellite signals are weak or unavailable, presents a fundamental challenge.

Traditionally, the positioning of UAV swarms has relied on global navigation satellite systems (GNSSs) such as GPS (Global Positioning System) and Beidou. However, in many critical scenarios, satellite signals may be unavailable or unreliable due to interference, urban canyons, dense forests, or indoor environments [9]. Therefore, it became necessary to develop alternative methods for initializing and positioning UAV swarms that do not rely on satellite signals. Various approaches have been explored, including the use of airborne sensors such as cameras [10], lidar [11], inertial measurement units (IMUs) [12], WiFi (Wireless Fidelity) [13,14], RFID (radio frequency identification devices) [15], and ultra-wideband (UWB) ranging devices [16,17,18]. The use of VIO (visual–inertial odometry) is a promising method for initializing a UAV swarm. VIO combines visual information from the camera with inertial data from the IMU. In recent years, advances in VIO technology have greatly improved the accuracy and reliability of these systems. Furthermore, the increase in computing power available on drones allows for the implementation of more sophisticated algorithms that can utilize cameras and IMU data for precise positioning. Despite these advancements, there are still significant challenges to achieving a fast and accurate initialization of UAV swarms in GNSS-denial environments.

Many existing approaches focus on small- to medium-sized swarms, often relying on image transmission and processing, which requires large amounts of data bandwidth and computing resources [19]. In a previous study, a vision-based method was introduced in a paper referenced as [20]. This method focused on a large-scale collaborative SLAM (simultaneous localization and mapping) and emphasized the localization function. The paper employed a loop-like approach to perform map fusion on the back end, which helped achieve the localization function. Additionally, similar map-based fusion SLAM methods have been introduced in other papers, referenced as [21,22]. In paper [23], a method for sharing feature points is proposed to reduce the communication burden compared to sending keyframes to the server. Papers [24,25] also chose a flexible approach, selecting keyframes for feature comparison in well-communicated common viewing areas while implementing a low-traffic PGO strategy for more distant locations. Additionally, there is an AROCK algorithm for asynchronous processing, which is very useful in scenarios requiring efficient parallel computing [26]. However, due to the high information load and the complexity of data exchange and processing, these methods are not suitable for initializing large-scale swarms.

In large-scale networks, a significant challenge is the slow updating of moving targets due to high communication loads. The initialization process in such scenarios must be fast and accurate to ensure that the cluster can adapt to the dynamic environment and maintain cohesion and coordination. The high information load hampers the rapid updating of data, and current methods often fall short of providing the necessary update speed and accuracy for large-scale cluster operations. Therefore, we advocate for a light-load communication strategy to accommodate large-scale clusters. With the advancement of SLAM (simultaneous localization and mapping) technology, the accuracy of odometers is improving [27], and odometer information and ranging information are more refined than images or feature points, making them suitable for large-scale communication. Thus, it is possible to achieve accurate relative positioning with ranging information and an odometer. UWB is a signal module that comes with a high price tag but offers superior accuracy compared to others [28]. When compared to WiFi, RFID, Bluetooth, and UWB, UWB stands out for its high-precision positioning, low power consumption, and strong anti-interference capabilities [29]. UWB provides centimeter-level positioning accuracy, making it suitable for establishing a swarm’s spatiotemporal datum in complex environments. Its high bandwidth and low latency ensure real-time data transmission and stable performance even under multipath interference.

Despite the potential of UWB and other sensor methods [30], there are still significant challenges, especially for large-scale drone swarms. In paper [31], the initialization process is part of the optimization. In other words, there is no specific initialization process, and the optimization is iterated as the cluster moves. For drones with sufficient movement, they conducted several optimization experiments with random initial translation values and selected the result with the lowest cost as the initialization result. In paper [32], an additional anchor node is required to participate in positioning, which means that initialization cannot be completed without an anchor node. Moreover, the position of the anchor node is unknown, so additional initialization is required. The authors only analyzed the case of two members, which means that in the unknown estimation of the cluster, additional initialization of the anchor node must be designed, and the additional error caused by the anchor node estimation problem must be borne. There is also no initialization process in paper [33]. The two drones match feature points through the server. When they are judged to be in the same position, the relative position between the two drones can be restored. Paper [34] obtains global coordinates by deploying anchor nodes with known positions, saving the initialization process. However, deploying anchor nodes will also be a very time-consuming process and cannot be used in the location environment. Paper [35] also combines UWB and vision for initialization in GPS-denied environments, but it uses anchor nodes for guidance so it needs prior information. Paper [36] describes a multi-map fusion system that uses UWB and VIO. The swarm consists of mobile robots and aircraft, but their positions are based on fixed anchor nodes. In fact, as long as there are anchor nodes, the swarm itself does not need to be initialized, or the swarm already has a priori absolute position information at the beginning of the algorithm. For networks with anchor nodes, collaborative positioning can be achieved through MDS. Our laboratory has also performed similar work for this purpose [37]. We believe that it is unwise to deploy an anchor node system for a satellite denial system. Instead, we prefer to find a swarm collaborative positioning method that does not rely on fixed anchor nodes. Of course, selecting nodes as anchor nodes in the initialization part is also a good choice. Paper [38] delves deeper into this topic, providing a detailed introduction to an effective initialization method for two drones. The paper successfully formulates a 4DOF (degree of freedom) problem by leveraging IMU odometers, such as the VINs (visual–inertial odometer system), which can constrain the direction of gravity. This method does not necessitate the deployment of anchor nodes and has no specific requirements for the placement of drones. The algorithm successfully determines the translation and heading angles between drones using both intra-drone motion and inter-drone measurements. Despite the existence of cumulative errors, the algorithm is cleverly designed and robust, requires minimal scene requirements, and responds quickly. Furthermore, this paper builds upon and enhances the aforementioned paper, effectively expanding upon it to create an initialization algorithm suitable for large-scale clusters.

This paper continues the existing relative transformation estimate work. After improvement and optimization, the estimation method of two members is extended to multiple members, and new constraints for multi-member estimation are found. At the same time, we also introduce judgment to try to find a better coordinate recovery time window, which can effectively and quickly estimate the relative transformation relationship of multi-member coordinate systems. The optimization problem has high robustness and precision. The contributions of this article are summarized as follows:

• To ensure the selection of anchor nodes as close to the CRLB as possible, this paper proposes an innovative marginal evaluation method based on distance. By leveraging prior information about mutual distances artfully, the optimal anchor nodes are selected, ensuring a rational spatial distribution. Theoretical analysis is conducted to validate the optimal selection of anchor nodes, providing solid proof for the proposed innovative method.
• It introduces groundbreaking constraints for multi-member estimation and incorporates judgment to identify an optimal coordinate recovery time window, ensuring effective and rapid estimation of the relative transformation relationships among multiple coordinate systems. The addressed optimization problem exhibits high robustness and precision. Our innovative algorithm simplifies and incorporates the state-of-the-art SMACOF (Scaling by Majorizing a Complicated Function) algorithm to help achieve rapid solutions, significantly enhancing the efficiency of swarm initialization.
• A novel two-stage initialization method for more than four UAVs is proposed, verified by theoretical simulation and real-world testing, demonstrating its superior performance in satellite-denied environments. Experimental results confirm that this innovative method effectively addresses the UAV swarm initialization problem, providing a reliable technical guarantee for UAV applications in complex environments.

The rest of this paper is organized as follows: Section 2 presents a theoretical analysis and descriptions of different algorithm modules, including an anchor-node selection strategy named D², improved non-convex optimization, and a simplified SMACOF algorithm. The simulation and real-world experiment results are provided in Section 3, followed by the Discussion in Section 4. We conclude this work in Section 5. Appendix A and Appendix B describe the details of the algorithm.

2. Materials and Methods

2.1. Problem Formulation

Figure 1 provides an overview of the system. In an environment where GPS signals are not available, it is not possible to directly obtain the absolute positions between members. Therefore, in mainstream SLAM systems such as VINs-mono [39], the positions of other members are measured in the local frame of their own VIO system. This paper utilizes VINs-fusion [40] for VIO initialization, and it also supports other visual–inertial or original inertial odometry methods. The plan involves equipping more than 5 UAVs with IMUs, cameras, and UWB sensors to execute a high-precision robust VIO system capable of functioning without satellite signals. In this setup, a drone is randomly selected as the coordinate origin of the drone swarm, and each drone performs its own VIO initialization. At this stage, the UWB is also able to measure the straight-line distance between all members.

After initializing the VINs (visual–inertial odometer system), it aligns its world coordinate system with the direction of gravity. Once UAVs complete their own initialization, their world coordinate system coincides on the Z axis, and each coordinate axis only differs in translation and the yaw angle, and then the 6DOF (degree of freedom) problem is transformed into a 4DOF problem.

$\mathcal{D}$ represents the drones, and it is assumed that each member $i$ can obtain position information ${}^k\mathcal{D}_i$ output by VIO at moment $t_k$. The distance between the two drones $i$ and $j$ at time $t_k$ is denoted by ${}^{k}d_{ij}$. ${}^k\mathcal{D}_i$ contains the translation and quaternion rotation of the VINs relative to the world coordinate system, and ${}^k\mathcal{D}_i$ is denoted as follows:

$${}^k\mathcal{D}_i={\left[x_i^k,y_i^k{,z}_i^k\right]}^T$$

(1)

Although the VINs can provide a relatively accurate coordinate of their own position, they still cannot sense the position of other members, and there is translation and rotation between each drone’s own body coordinate system. Let us assume that the IMU body frame of each drone is $\left\{{\mathcal{B}}_n\right\}$, and $\left\{{\mathcal{L}}_n\right\}$ is the local frame after the first frame of VIN initialization is gravity-aligned. To locate in 3D space, the number of anchor nodes needs to be at least four, so the primary goal is to find the transformation relationship of the coordinate system between $\left\{{\mathcal{L}}_{\mathrm{i}}\right\}$ and $\left\{{\mathcal{L}}_{\mathrm{j}}\right\}$, and we only need to find $T_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}$, which is represented as (1) due to gravity alignment.

$$T_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}=\left[\begin{array}{cc}{R}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& t_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}\\ 0& 1\end{array}\right]$$

(2)

$$R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}=\left[\begin{array}{ccc}\cos {\theta }_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& -\sin {\theta }_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& 0\\ \sin {\theta }_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& \cos {\theta }_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& 0\\ 0& 0& 1\end{array}\right]$$

(3)

$$t_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}={\left[\begin{array}{ccc}{x}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& y_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& z_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}\end{array}\right]}^T$$

(4)

Here, ${\theta }_{{\mathcal{L}}_1}^{{\mathcal{L}}_{\mathrm{j}}}$ represents the yaw angle of each drone $\mathrm{j}$ with respect to frame $\left\{{\mathcal{L}}_1\right\}$ perpendicular to the Z-axis plane, and $t_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}$ represents the translation of the origin of the other drone $j$ relative to frame $\left\{{\mathcal{L}}_1\right\}$. Let $\left\{\mathcal{W}\right\}$ represent $\left\{{\mathcal{L}}_1\right\}$, which represents the world frame and navigator frame. Here, we introduce the concept of the navigator, who will be selected from the anchor nodes and will not undertake all the computational tasks; instead, all members will be built based on its frame after initialization. The distance between the two UAVs can be expressed as follows:

$${}^{k}d_{ij}=‖t_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}+R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}{}^k\mathcal{D}_i{-}^k{\mathcal{D}}_j‖$$

(5)

where $‖·‖$ means the Euclidean distance, which represents distances in three-dimensional space. In addition, we need to consider the relationship between the UWB installation position and the IMU position. However, since our UAVs share the same architecture, the positions of the UWB and IMU are fixed across different aircraft. Thus, any mechanical errors can be ignored during actual flight tests. There is an error between the measured value and the true value, and this error conforms to the Gaussian distribution, ${\eta }_k~\mathcal{N}\left(0,{\sigma }_{\mathrm{r}}^2\right)$. We denote the measured value with $\widehat{·}$, so we can surmise the following:

$${}^k\tilde{d}_{ij}={}^{k}d_{ij}+{\eta }_k$$

(6)

In addition to this, it is assumed that after the initialization, all members are represented as ${\mathcal{P}}_i^k$ under the $\left\{\mathcal{W}\right\}$ frame.

2.2. Anchor Node Configuration

For the quality of the cluster, there needs to be corresponding evaluation indicators [41]. When there is a large number of UAVs, calculation and communication become significant factors. If all members were to be initialized in the first step, the computational burden would increase exponentially due to the optimization problem. As the number of optimization parameters and constraints increases, the algorithm’s convergence speed will render the initialization algorithm unusable. Furthermore, the complex communication logic will also burden the system. Therefore, in order to reduce the computational burden of initialization, it is better to construct a relatively stable spatiotemporal datum. This paper proposes a fast initialization method for anchor nodes to provide a position initialization datum for other members. Selecting UAVs as anchor nodes for formations raises an important question. For traditional positioning methods like GPS, the configuration of the anchor node significantly impacts positioning accuracy. To analyze the positioning accuracy of UAV swarms, the CRLB is introduced. This section attempts to analyze the impact of the selection of different anchor nodes on the accuracy of the entire swarm from a theoretical level and provide the best suggestions for the selection of anchor nodes. Meanwhile, the D² anchor selection method will be discussed.

The CRLB provides the lower bound of the variance of the parameter estimates, and for arbitrary unbiased estimators, its covariance matrix satisfies the following:

$${CRLB}_{\theta }\ge I^{-1}\left({\theta }_k\right)$$

(7)

where $I\left({\theta }_k\right)$ is the Fisher information matrix, which is defined as

$$I\left({\theta }_k\right)=\mathbb{E}\left[\left({\displaystyle \frac{\partial lnL\left(X;{\theta }_k\right)}{\partial {\theta }_k}}\right){\left({\displaystyle \frac{\partial lnL\left(X;{\theta }_k\right)}{\partial {\theta }_k}}\right)}^T\right]$$

(8)

In our scenario, ${\theta }_k$ represents the position parameter of the drone, and $\mathrm{X}$ is the observation data, assuming that the position of different nodes in a unified coordinate system is known, and the node $i$ position is denoted as $p_i$. Then,

$$L_i\left(\mathrm{X};{\theta }_k\right)=\sum _{j=1}^n{\displaystyle \frac{\partial d_k(p_i,p_j)}{\partial p_i}}$$

(9)

A simulation is carried out according to the above formula. The simulation results are shown in Figure 2, where it can be seen that the error caused by the configuration factor of the measured node is as small as possible, and the best position is located in the center of the anchor node; thus, when initializing, the UAV at the edge position should be selected as the anchor node for initialization, and the optimal position distribution of the anchor node can be obtained, minimizing the uncertainty of the overall position estimation.

In order to reduce computational load, we have developed a strategy for screening anchor nodes. These nodes will be chosen to take part in the subsequent optimization of the positioning. When selecting anchor nodes, we will take into account the distribution of different members in 3D space in order to choose the best anchor node that is as close to the CRLB as possible. To achieve this, we will use a method named D², which involves summing distances to assess the significance of each point, wherein the point with the greatest distance will be chosen as the optimal anchor node. When the algorithm begins, the sum of distances from all other members will be calculated for each member $i$, as the marginality evaluation index:

$${\mathcal{F}}_i=\sum _{j=1,j\ne i}^N{d}_{ij}$$

(10)

where ${\mathcal{F}}_i$ represents the evaluation index of member $i$. Next, we select the first four members with the largest distance and ${\mathcal{F}}_i$ as the optimal anchor nodes. It is expressed as follows:

$${\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{a}\mathrm{x}\mathcal{F}}_i$$

(11)

The first four members are chosen as the best anchor nodes because their positions are as close as possible to the CRLB in the entire cluster. This helps to enhance the accuracy of cluster positioning. The method takes into account the distribution of members in 3D space and assesses the sum of their distances from other members to determine the optimal anchor node, thereby maximizing the positioning accuracy of the entire cluster. Through simulation tests, the experimental results are depicted in Figure 3, confirming that the anchor nodes chosen by the method align with the theoretical optimal nodes.

The optimal strategy for siting UAVs was used to select four UAVs as the anchor nodes for subsequent spatial datum establishment, including navigators. Moreover, it is important to consider the impact of anchor node selection on the configuration accuracy, as well as the timing of when the algorithm starts. In practical scenarios, the algorithm should initiate when the motion configuration is optimal. To address this, two criteria are introduced. Firstly, we observe the motion offset of all Z-axis members compared to the other two axes. If the offset is significantly large, this indicates that the members are taking off, and we calculate the singularity of the matrix. For instance, during the few seconds when the plane is in the air, most drones ascend vertically, causing the motion configuration to result in a singular matrix and a non-rigid distance matrix of motion [36].

2.3. Non-Convex Optimization Problems

After choosing the initial anchor node, we need to optimize its position. During the drone initialization process, the IMU constrains the gravity direction, enabling each drone to perceive its pitch and roll angles. However, due to observability limitations, it is not possible to standardize the heading angles between all members using the IMU. The actual ranging is obtained from UWB ranging, and the error is assumed to follow a Gaussian distribution. The true relative distance is obtained from Equation (5), but in practice, the robot-to-robot distance will be affected by noise measurements, causing the calculated distance to be different from the true distance. This process is based on the initialization method for two members as described in the literature [38,42]. Firstly, a weighted least squares problem is established, which is then transformed into a QCQP (quadratically constrained quadratic program) problem. The square distance residuals can be expressed as follows:

$${\mathrm{e}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}≔[{{}^1\hat{d}_{ij}}^T{}^1\tilde{d}_{ij}-{{}^1\hat{d}_{ij}}^2,{{}^2\hat{d}_{ij}}^T{}^2\hat{d}_{ij}-{{}^2\tilde{d}_{ij}}^2,\dots ,{{}^k\hat{d}_{ij}}^T{}^k\hat{d}_{ij}-{{}^k\tilde{d}_{ij}}^2{]}^{\mathrm{T}}\in {\mathbb{R}}^{k\times 1}$$

(12)

where

$${}^k\hat{d}_{ij}=‖t_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}+R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}{}^k\tilde{\mathcal{D}}_i-{}^k\tilde{\mathcal{D}}_j‖$$

(13)

${}^k\tilde{\mathcal{D}}_i$ represents the actual measurement, and we obtain a weighted least squares problem,

$$\underset{\mathsf{\Theta }}{min}{\displaystyle \frac{1}{2}}{{\mathrm{e}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}}^T{R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}}^{-1}{\mathrm{e}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}$$

(14)

where $\mathsf{\Theta }=\left[\begin{array}{cc}\begin{array}{ccc}{x}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& y_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}& z_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}\end{array}& {\theta }_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}\end{array}\right]$, $R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}\in {\mathbb{R}}^{k\times k}$ is a matrix of covariance, and the two measurements are independent of each other, so that ${R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}}_{ab}=0$, where $ab$ refers to the rows and columns in the matrix. The calculation method of the ${R_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}}_{ab}$ is referred to in paper [38], so $R=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}({\sigma }_1^2,\dots ,{\sigma }_N^2)$. Further, we will turn the WLS problem into a QCQP problem, as described in Appendix A for the specific process. We can then obtain a quadratic constraint quadratic optimization problem.

$$\begin{array}{ll}& \underset{\mathrm{x}}{\mathrm{m}\mathrm{i}\mathrm{n}}{{\mathcal{x}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}}^{\mathrm{\top }}{{\mathrm{P}}_0}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}·{\mathcal{x}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}\\ & \mathrm{s}.\mathrm{t}.{{\mathcal{x}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}}^{\mathrm{\top }}{{\mathrm{P}}_q}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}·{\mathcal{x}}_{{\mathcal{L}}_{\mathrm{i}}}^{{\mathcal{L}}_{\mathrm{j}}}=r_q,q=1,\dots ,5.\end{array}$$

(15)

Here, $\mathcal{x}$ is a nine-dimensional column vector with five constraints added for the two-member case. Up to this point, they have only considered the scenario where two drones measure each other, as presented in paper [38]. This approach sacrifices accuracy in favor of faster computation speed by transforming the QCQP problem into a semi-definite programming (SDP) problem. However, given that our algorithm runs on the host processor, maintaining the accuracy of the anchor node is crucial for overall precision. Therefore, we opt to retain the QCQP solution method. In Section 2.5, we will delve into how to convert this approach into a convex optimization problem for multi-drone measurements.

2.4. Multi-Dimensional Scale Transformation

In UAV swarms, accurately positioning each node is crucial for collaborative swarm tasks. Although multi-dimensional scaling (MDS) is a commonly used method for node positioning, the traditional MDS algorithm has limitations in terms of large computation and slow convergence speed when dealing with large-scale networks. The traditional MDS algorithm [18] assumes that all nodes only know the ranging values of each other.

$$B_k=-{\displaystyle \frac{1}{2}}J{D_k}^{2}J$$

(16)

We suppose that $n=3$, where $D_k$ refers to the UWB measurements matrix at times $k$,

$$J=\mathbf{E}-n^{-1}\mathbf{I}=\left[\begin{array}{ccc}1-{\displaystyle \frac{1}{n}}& -{\displaystyle \frac{1}{n}}& -{\displaystyle \frac{1}{n}}\\ -{\displaystyle \frac{1}{n}}& 1-{\displaystyle \frac{1}{n}}& -{\displaystyle \frac{1}{n}}\\ -{\displaystyle \frac{1}{n}}& -{\displaystyle \frac{1}{n}}& 1-{\displaystyle \frac{1}{n}}\end{array}\right]$$

(17)

which is the Eigenvalue decomposition of matrix $B_k$. Then, $B_k$ becomes

$$B_k=-J{X}_k^T{X}_{k}J=VU{V}^T=V\sqrt{U}(V\sqrt{U}{)}^T$$

(18)

Then, we have

$$J{X_k}^T=V\sqrt{U}$$

(19)

Traditional MDS algorithms are computationally intensive and time-consuming when processing large-scale data because they rely on singular value decomposition (SVD) for solutions. This paper simplifies the SGD-SMACOF algorithm proposed in paper [37], aiming to improve the computational efficiency and accuracy of node positioning. The SMACOF algorithm realizes the MDS of nodes by iteratively optimizing the stress function. The stress function $\mathcal{S}$ is defined as follows:

$$\mathcal{S}=\sum _{1\le i\le n}\sum _{i<j\le n}{\omega }_{ij}^k{\left({}^k\tilde{d}_{ij}-d_{ij}\left({\mathcal{P}}^k\right)\right)}^2$$

(20)

The proof process is shown in Appendix B, and the final iteration formula is as follows:

$${{\mathcal{P}}^k}_{i+1}=\left(1-\mu \right){{\mathcal{P}}^k}_i+\mu {\mathcal{V}}^{†}\mathcal{L}\left({{\mathcal{P}}^k}_i\right){{\mathcal{P}}^k}_i$$

(21)

where $\mu$ is the learning rate parameter and ${\mathcal{V}}^{†}$ is the generalized inverse matrix. We stop iterating if ${\mathcal{S}}_{i+1}-{\mathcal{S}}_{i+1}<ϵ$ or a certain iteration limit is reached. As shown in Figure 4, it has significant advantages over DC-MDS.

2.5. Fast Swarm Collaborative Positioning Algorithm

This method in [38] works well with a small number of clusters, but the number of computations grows at a rate of $C_n^n$. Based on this, we have developed a more comprehensive four-member initialization unit, focusing on addressing situations with more than five UAVs in a swarm. Drawing inspiration from the base station localization algorithm [43], we believe that anchor node positioning provides a solid solution with a mature research foundation. Thus, our primary focus is on resolving the precise positioning relationship between anchor nodes. The entire algorithm flow is illustrated in Figure 5. It begins with VIO initialization, followed by selecting the appropriate time for initialization, choosing anchor nodes, initializing anchor nodes, and finally achieving global positioning.

If frame $\left\{{\mathcal{L}}_1\right\}$ is the navigator, then the unknown quantities between the anchor nodes are $\left\{T_{{\mathcal{L}}_1}^{{\mathcal{L}}_2},T_{{\mathcal{L}}_1}^{{\mathcal{L}}_3},T_{{\mathcal{L}}_1}^{{\mathcal{L}}_4},\dots ,T_{{\mathcal{L}}_3}^{{\mathcal{L}}_4}\right\}.$ This approach would result in excessive information redundancy and a huge amount of computation. Moreover, the calculation mode requires special consideration, whether one member receives all the data for the settlement, or two members calculate each other and finally spread the calculation results by broadcasting. The former will cause a drone to receive a huge amount of information that cannot be processed, and the latter will reduce the timeliness of information between members. We consider sending and receiving data together through multi-machine communication and UWB communication, processing all data through the host, and then returning the calculation results to all members. This solves the problem of computing power and enables the host to have the ability to control all members in a centralized manner. Moreover, in our method, only limited data are passed during member communication. Thesis [20] requires the host to have stronger computing power and greatly increases the communication burden, which is not suitable for large-scale networks, while our network greatly reduces the communication burden.

Based on paper [38], we improved the non-convex QCQP problem, converted the unknown quantity of the four members, and added an unknown quantity from $\left\{T_{{\mathcal{L}}_1}^{{\mathcal{L}}_2},T_{{\mathcal{L}}_1}^{{\mathcal{L}}_3},T_{{\mathcal{L}}_1}^{{\mathcal{L}}_4}\right\}$, as shown in Figure 6, so that a stable geometric relationship is formed, as shown in Figure 6, so that we have an additional constraint relation. The simulation results show that adding a relative ranging is helpful for improving the overall accuracy. We make the appropriate transformation so that the order of the solutions becomes $\left\{T_{{\mathcal{L}}_1}^{{\mathcal{L}}_2},T_{{\mathcal{L}}_2}^{{\mathcal{L}}_3},T_{{\mathcal{L}}_3}^{{\mathcal{L}}_4},T_{{\mathcal{L}}_4}^{{\mathcal{L}}_1}\right\}$.

In order to represent the algorithm more clearly, we simplify the symbol in Equation (15). Here, $i$ does not represent members, but rather the number of columns; ${\mathrm{x}}_i\in {\mathbb{R}}^{1\times 9}$ denotes the nine-dimensional unknown in column $i$, such as $x_1{=\mathcal{x}}_{{\mathcal{L}}_1}^{{\mathcal{L}}_2}$; $P_i^0$ denotes the corresponding weight matrix; $\mathcal{X}$ denotes the set of all anchor node unknowns; and the optimization goal is to minimize the quadratic sum of all unknown vectors weighted by their respective matrices.

$${min}_{x\in \mathcal{X}}\sum _{i=1}^4{x}_i^H{P}_i^0{x}_i$$

(22)

For such a geometrically closed-loop vector shown in Figure 6, there are the following properties:

$$T_{{\mathcal{L}}_1}^{{\mathcal{L}}_2}{T}_{{\mathcal{L}}_2}^{{\mathcal{L}}_3}{T}_{{\mathcal{L}}_3}^{{\mathcal{L}}_4}{T}_{{\mathcal{L}}_4}^{{\mathcal{L}}_1}=1$$

(23)

Therefore, in order to ensure the physical and geometric constraints of the optimization problem, we introduce the following new constraints under the original constraints:

$${\sum }_{i=1}^4{x}_i\left[3\right]=0$$

(24)

and

$$x_i^H{P}_i^j{x}_i=r_i^j$$

(25)

where $j$ corresponds to the number of constraints, $j\in \left[\mathrm{1,5}\right]$. The optimization variable is a matrix of position vectors for all drones:

$$\mathcal{X}=\left[x_1,x_2,...,x_N\right]$$

(26)

Combining the above objective functions and constraints, the complete optimization problem can be formulated as follows:

$$\begin{array}{ll}& \underset{\mathrm{X}\in \mathcal{X}}{\mathrm{m}\mathrm{i}\mathrm{n}}{\displaystyle \sum _{i=1}^N}{x}_i^H{P}_i^0{x}_i\\ & \mathrm{s}\mathrm{u}\mathrm{b}\mathrm{j}\mathrm{e}\mathrm{c}\mathrm{t}\hspace{0.25em}\mathrm{t}\mathrm{o} {\displaystyle \sum _{i=1}^N}{\mathrm{x}}_i\left[3\right]=0\\ & {\mathrm{x}}_i^H{\mathrm{P}}_i^j{\mathrm{x}}_i=r_i^j, \forall i\in \left[1,N\right],\hspace{0.25em}j\in \left[1,5\right],N=4\end{array}$$

(27)

This is a quadratic non-convex optimization problem with linear and quadratic constraints, so it can be solved by a standard convex optimization algorithms library, such as the interior point method and gradient descent method; this paper uses the CVX of MATLAB and the CVXPY library in Python to simulate them, respectively. The simulation results are similar in accuracy, and the results will be given in the next section.

During the initial value stage, to achieve faster convergence, we will use the data from the gyroscope and magnetic compass in the UAV flight controller as the initial values. This allows the other three anchor nodes and the navigator to establish the translation and rotation relationship of the coordinate system between each other. Essentially, the four anchor nodes can restore the transformation relationship of the coordinate systems of other members. Additionally, the distance matrix is available and the algorithm in Section 2.4 can be used to obtain the relative translation relationship between all coordinate systems. The positional relationship between the members is restored through the anchor node. Assuming that Y is the recovery coordinate with the possibility of arbitrary rotation translation and $Y_{trans}$ is the result of the initialization of the anchor node, all member positions can be obtained by a simple transformation.

$$\underset{Y}{\mathit{argmin}}(Y_{trans}-sYR+t)$$

(28)

Now, only the heading angle has not been restored. At this time, the original 4DOF problem becomes a 1DOF problem. After performing another step of least squares on the remaining members, the rotation of the coordinate system is restored for all members. At this stage, we are able to determine the translation and rotation relationships between all coordinate systems and accurately understand the relative positions of all members. Our primary focus in this paper is not on large-scale swarm attitude information but rather on obtaining precise position information for each point within the swarms. The pseudocode for the algorithm flow is presented in Algorithm 1.

Algorithm 1. Cooperative Increasing SAM Algorithm

$\mathbf{Input}:n{,}^k{D}_i\in R^{1\times 3}{,}^k{d}_{ij}\in R,ij\in \left[1,...,n\right]$; $\mathbf{Output}:Y_{trans}\in R^{3\times n}$ 1: FIM Detection $2:\mathbf{w}\mathbf{h}\mathbf{i}\mathbf{l}\mathbf{e}graph\_is\_rigid\left(R_{\mathcal{G}}\right(z^{UWB}\left)\right)\mathbf{d}\mathbf{o}$ 3: Anchor Selection 4:     Select anchor nodes based on the FIM detection results by using (11). 5: Improved QCQP 6:     Determine the translation and rotation between all anchor nodes by using (28) 7: Quick SMACOF $8:[Y$$,\mathrm{stress}]\leftarrow$$\mathrm{SMACOF}\left(D,3,\mu ,ϵ\right)$ 9: Relative to Absolute Transformation $10:\left[Y_{trans}\right]\leftarrow \mathrm{p}\mathrm{r}\mathrm{o}\mathrm{c}\mathrm{r}\mathrm{u}\mathrm{s}\mathrm{t}\mathrm{e}\mathrm{s}\left(X_{\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{r}}^{\mathrm{\top }},Y\right)$ $11:\mathbf{return}{Y}_{trans}$

3. Results

The experimental section of this paper is split into two parts: swarm simulation and actual flight experiments. These experiments aim to demonstrate the effectiveness and superiority of the proposed method in various environments. The focus is on the accuracy of UAV swarm initialization (RMSE), trajectory overview, and the analysis of time complexity and errors related to different algorithms.

3.1. Simulations

This study aims to test the proposed fast initialization method’s performance across different numbers of UAV swarms through detailed swarm simulation experiments. The simulation experiments are conducted on a computer equipped with an Intel i5-9300H CPU, 16GB RAM, RTX 1650 GPU, and Windows 11 system. The simulation experiments were carried out in MATLAB. Two sets of experiments are performed: one with 10 sorties and one with 50 sorties, representing small-scale and large-scale UAV swarm initialization processes in a satellite signal rejection environment, respectively. This study compares four different initialization algorithms: nonlinear least squares (NLS) [44], semidefinite programming (SDP), the SOTA quadratic convex optimization problem (QCQP) proposed in paper [38], and the method proposed in this paper.

Within the simulation, each drone utilizes a visual–inertial odometer and UWB sensors to mimic real-world sensor data acquisition. The visual–inertial odometer provides relative displacement trajectories over time, while the UWB sensors measure the distance between drones. To replicate sensor noise in a real environment, Gaussian noise is incorporated into the VIO data, with the standard deviation of the noise set based on the performance parameters of the actual sensor. The simulation utilizes ${\sigma }_{vio}=0.01\mathrm{m}$. Gaussian noise is also introduced to the UWB ranging data to simulate ranging errors in practical applications: ${\sigma }_{UWB}=0.05\mathrm{m}$.

The initial conditions and iterations of each optimization algorithm were fine-tuned through multiple experiments to ensure a fair comparison of different algorithms. For the NLS algorithm, the initial position value is in the positive direction of the x-axis, the distance corresponds to the UWB measurement, and the number of iterations is set to 200. For the SDP algorithm, the maximum number of iterations is also set to 200. Finally, for the QCQP algorithm, the initial position value is in the positive direction of the x-axis, the distance corresponds to the UWB measurement, and the maximum number of iterations is set to 200.

In a simulation environment, 10 and 50 UAVs are randomly distributed in a three-dimensional space. They follow preset trajectory patterns such as circular, rectangular, elliptical, straight, triangular, and a crossed Figure 7. Each trajectory can be adjusted for different parameters including height, size, speed, and opening/closing angles. During the simulation, the UAVs move according to these different shapes and at various altitudes. The trajectory of each UAV is created using VIO data, and the distance matrix between UAVs is obtained using UWB ranging. To ensure a fair comparison, algorithms using NLS, SDP, QCQP, and our proposed algorithm are run independently on the same dataset. Each algorithm is tested for accuracy, convergence time, and performance with varying numbers of drones. Additionally, the true value of the drones is represented in the same coordinate system, with each drone’s VIO measurements starting from the (0,0,0) point. The analysis involves evaluating the RMSE between true and estimated values 60 s after takeoff, with flight speeds ranging from 1 to 5 m/s, and the time taken for each algorithm to converge.

The experiment results are shown in

Table 1. The results of the simulation experiment.

Swarm Number	Method	RMSE (m)	Average Time Cost (s)
5	NLS	1.72	7.23
5	SDP	0.55	3.52
5	QCQP	0.48	5.02
5	Ours	0.52	3.48
10	NLS	1.93	9.30
10	SDP	0.58	6.89
10	QCQP	0.49	10.95
10	Ours	0.52	3.55

The data in bold represent the best results in the same group of experiments.

and Figure 8. The algorithm has a slightly slower performance than the SDP algorithm when the number of UAVs is small, but it has similar accuracy to QCQP. The algorithm’s advantages become more evident when the number of UAVs is large, especially after exceeding 15 UAVs. Thanks to an optimization step, the algorithm maintains consistent computational efficiency regardless of the number of drones. In addition, due to the high accuracy of the QCQP algorithm, the proposed algorithm consistently delivers precise results. Two important metrics to note are the convergence time, which is the time required for the convergence of non-convex optimizations, and time cost, representing the total time needed from the start of the algorithm until the recovery of the coordinate axis. The total time may vary due to the re-computation of the optimization problem or non-convergence issues.

3.2. Real-World Experiments

Each drone is equipped with an IMU, a stereo camera, and a UWB sensor. The IMU captures the drone’s motion information, the camera captures visual data, and the UWB measures the distance between drones. To speed up the initial setup process, initial values use data from calibrated gyroscopes and magnetic compasses. The drone used in the experiment was a modified Amu drone, shown in Figure 9a, with a 250 mm frame. It is equipped with a Jetson TX2 processor, Pixhawk 6c mini flight controller, zed mini camera, and NoopLoop’s LinkTrack S series UWB. The flight-data-processing computer is the same as the emulation computer.

In the experiment, the drone’s connection with the computer was established through ROS1 multi-machine communication, and the frequency of VIO data output was set to 100 Hz. A FZMotion Mocap System with eight cameras running at 120 Hz was used. According to the technical specifications and the reported accuracy from the FZMotion software, the positioning accuracy can reach 0.05 mm/m. Since this accuracy is two orders of magnitude smaller than the expected accuracy of the VIO-UWB system, it was deemed sufficient for use as the ground truth measurement. The UWB ranging can achieve a refresh rate of 50 Hz for 10 members in practical applications. The sensor data of each UAV are locally recorded through ROSBAG, and all the bags are centrally processed by the computer to simulate multi-drone communication. Before each experiment, the drones are randomly distributed in the field, and they follow a fixed flight trajectory at an approximate altitude for approximately 100 s. The first 60 s of flight data are selected for later analysis. The flight trajectories recorded by VIO and motion capture are aligned, as shown in Figure 10 and Figure 11. The yaw angles of the four anchor nodes are transformed and time-aligned, as shown in Figure 10. Figure 11 shows the VIO and motion capture trajectories of the seven members after time alignment.

In the analysis, we used the NLS, SDP, and QCQP algorithms for initialization calculations based on the collected data. Each algorithm independently ran on the same dataset to ensure a fair comparison. We recorded and compared the calculation time, RMSE, and trajectory error of each algorithm for analysis.

In our analysis, we have compared the effectiveness of our anchor node algorithm with that of the algorithm presented in paper [38]. The results are displayed in

Table 2. The experimental results for anchor node initialization.

Anchor Method	RMSE (m)	Average Time Cost (s)	Success Rate
QCQP	0.11	3.99	86.3
Ours	0.12	2.89	98.5

The data in bold represent the best results in the same group of experiments.

. Our algorithm demonstrates considerable improvement in the success rate, attributed to the assistance of the gyroscope and the initial value of the magnetic compass. Our algorithm ensures a fundamentally guaranteed success rate, with similar absolute errors in translation and significantly reduced time consumption.

Please see the full experimental results in Figure 12 and Figure 13 and

Table 3. Experimental results for different algorithms.

Method	RMSE (m)	Average Time Cost (s)
NLS	0.359	8.23
SDP	0.183	4.12
QCQP	0.115	5.57
Ours	0.141	3.98

The data in bold represent the best results in the same group of experiments.

. The operation time and average time here refer to the time after takeoff, and the takeoff process is not included in these times. The trend of the experiment results was similar to that of the simulation. In Figure 12, we analyze the time required to solve different algorithms, and the results are consistent with those in Figure 8 from the simulation. It is important to note that the time analyzed is for the algorithm’s convergence, not the complete algorithm. Figure 13 illustrates the comparison between estimated and true values during flight. Our algorithm demonstrates significantly faster performance than the QCQP algorithm, with accuracy comparable to that of the QCQP. In summary, we have achieved a solution accuracy close to that of the current state-of-the-art algorithm [38], but in less time.

4. Discussion

Combined with simulation and real-world experiments, the accuracy of the original QCQP algorithm is higher than that of the algorithm we proposed. Still, this advantage will be greatly weakened by the time consumption when the number of clusters increases. Comparing the simulation results, we can see that the time cost of the QCQP method is positively correlated with the number of members, while the time consumption of our method is not directly related to the number of members, so the increase in time consumption will offset the advantage of the accuracy of the QCQP method. In addition, we consider that the UAV swarm should be as fast and accurate as possible; in fact, in the experiment, we can find that the accuracy of our QCQP method is comparable, which is significantly better than other SDP and NLS methods, and combined with the lower time consumption, our method is still very competitive, especially in the initialization stage of large-scale swarms. We used a priori information based on gyroscopes and magnetic compasses in this work. This means that for all members of the cluster, we can obtain a very precise initial value of the attitude without using GPS. By ensuring the accuracy of the initial value, we can achieve a very high success rate of convergence. This indicates that the algorithm is very robust.

5. Conclusions

In this paper, we propose a two-stage initialization method based on VIO and ranging to effectively solve the problem of the rapid initialization of UAV swarms in satellite signal rejection environments. Both theoretical simulations and real-world tests show that the proposed method has high precision and robustness, providing crucial support for the application of UAV swarms in complex environments. The method also has shortcomings. Our experiments do not meet the flight test of large-scale UAVs, so for more than 10 UAVs, it is still in the stage of simulation experiments. In addition, the algorithm has high requirements for time synchronization, this algorithm adopts a hard synchronization strategy and is not optimized for the time difference between different devices, and we are currently carrying out research and experiments on time synchronization problems. You can also try an asynchronous approach. In short, the algorithm still has limitations, but it is still competitive in the current initialization method due to its speed, robustness, and high precision. In future work, our focus should be on improving the algorithm’s solution speed without sacrificing accuracy. Additionally, this algorithm should be used in conjunction with mature path-planning algorithms to maximize its utility.