3.1. System Model
The mathematical model and optimization algorithm for collaborative positioning in this paper can be fused using both IMU–camera and UWB sensor data to improve positioning accuracy and robustness. In this paper, we use two types of sensor data for collaborative localization: IMU–camera, which can provide VIO information on UAVs, and UWB, which can provide information on the distance measurement between UAVs. The relative positional relationships between the UAVs are taken into account, which increases the observability and constraints. This paper considers a co-operative positioning system composed of M drones, where the drones can communicate and exchange information with each other. We assume that each drone is equipped with an IMU and a camera, and that the calibration and synchronization between the IMU and the camera have been completed. This paper also assumes that each drone is equipped with a UWB module, and that the calibration and synchronization between the UWB modules have been completed.
We use an optimization algorithm based on graph optimization (GOP) to solve the above mathematical model of collaborative localization.
Figure 1 is a block diagram of the algorithm flow in this paper. Graph optimization can represent the optimization variables as nodes in the graph, the constraints as edges in the graph, and the loss function as weights or cost functions on the edges [
27]. Graph optimization can utilize techniques such as sparsity, localization, and linearization to reduce computational complexity and memory consumption, and to improve the efficiency and accuracy of a solution.
The loss function for IMU+ camera data represents the consistency constraints between the VIO information of the UAV itself and its position and attitude; i.e., the relative displacements and rotations between two neighboring moments should be equal to the differences in the positions and attitudes under the global co-ordinate system [
28]. The loss function for UWB data represents the constraints between the distance measurements between the UAVs and their positions; i.e., the distance measurements between the UAVs should be equal to the modulus of the difference between the positions in the global co-ordinate system.
A simple, basic model is shown above in
Figure 2, and, in the scenario of this paper, we use the following qualification: Assuming that the scenario occurs in a two-dimensional co-ordinate system, the vehicle is able to obtain its own a priori position information through GPS or anchor nodes and knows the other nodes’ a priori positions. Each member is initialized with itself as the zero point of the environment co-ordinate system. The ranging information of UWB can be shared among all members. The updating frequency of ranging information is synchronized with the key frame rate of the VIO system. There is no time delay or data error in the communication. The environment has zero clutter, so there is no false detection or omission of the vehicle. Moreover, we do not consider the potential effects of restricted communication between robots and assume that robots have full high-bandwidth connectivity when encountering each other.
The model IMU/UWB/camera problems are as follows:
where
is the state transition equation of IMU itself at time
k, and
with
;
is the measurement of the observed landmark point, and
with
;
is the controller input, and
with
; and
is the UWB measurement from node
to node
, and
with
.
f,
h, and
g correspond to their measurement functions. The probability of each node for all parameters and measurements is as follows:
The MAP estimation of
is obtained by minimizing the negative logarithm of the joint probability, which is the linear problem we want to solve:
The nonlinear least-squares problem is as follows:
Linearizing the Equations (5)–(7) yields the following:
where
is the
Jacobian matrix, the expression of which is as follows:
The topological constraints for UWB are as follows:
Similarly, for all parameters
θ, the linearization result becomes the following:
3.2. Factor Graph
A factor graph is a two-way graph used to represent the factorization of a function, which includes variable nodes and factor nodes and represents the relationship between variables and factors through edges. Its advantage is that it can utilize the relative distance information between UAVs to improve positioning accuracy and robustness and reduce the dependence on base stations [
28]. In probability theory and its applications, the factor graph is a model that has been widely used in Bayesian inference. The factor graph can model the collaborative localization problem as a probabilistic graph model, where the variable nodes represent the positional states of the UAVs, the factor nodes represent the distance observations between the UAVs or the positional observations of the base station, and the edges represent the associations between the variables and the factors. By performing Bayesian inference on the factor graph, the optimal position estimation of the UAVs can be solved.
The factor graph can flexibly represent multiple types of observations, such as TOA, TDOA, angle, etc., as well as data sources with different accuracies and reliabilities, such as UWB, GPS, vision, etc., so as to improve the accuracy and robustness of the localization. The factor graph can utilize distributed or centralized inference algorithms, such as belief propagation (BP) [
12] or maximum a posteriori probability (MAP) [
29], to exchange local information among UAVs and achieve globally optimal or suboptimal position estimation. The factor graph can be decomposed or aggregated according to the topology and communication constraints of the UAV swarm, thus reducing the computational complexity and communication overhead and improving the localization efficiency and scalability.
The basic structure of the factor graph is shown in
Figure 3. To describe the factor graph model, we assume that the global function is g(
), where the set {
} refers to the variables present in the model. Due to the dichotomous nature of factor graphs, this global function can be expressed as
where
refers to a single factor or function and consists of a subset of variables in {
}. To further illustrate this, we can represent this graphically, as shown in the image above, where the variable node is represented by
xn and the factor node is represented by
fn. By turning global functions into maximization problems,
is obtained. The optimal solution for the measurement
can be found by looking for the right combination of
Xj, where
Xj represents all known vehicle gestures and landmarks in that factor, maximizing the probability of that particular measurement. Assuming Gaussian noise, linearizing
f(
x) yields
where, for
Taylor series expansion is performed, the first-order linear component is retained, the
x component is derived, and the value after derivation is 0 to obtain the minimum value of
g(
x) as
.
For the incremental equation,
Based on the factor graph theory mentioned above, the factor diagram structure of the proposed system is designed, as shown in the
Figure 4. The topology can effectively suppress error divergence.
3.3. Switching Functions
A switching function is a function that adjusts the weights of the loss function according to variations and anomalies in the data and suppresses or rejects the effects of anomalies and wild values, thus improving the localization robustness. In this paper, two different types of switching functions are used: one for IMU+ camera data and the other for UWB data. The two types of switching functions have different design and judgment criteria, and the following describes these two types of switching functions, respectively.
IMU–camera data refer to the data provided by the inertial measurement unit (IMU) carried by the UAV itself and the camera, which can form a visual–inertial odometry (VIO) system for estimating the UAV’s position and motion status. The advantage of the VIO system is that it can utilize the visual information from the camera and the inertial information from the IMU for autonomous localization without an external reference, and it can provide high-frequency and low-latency outputs. The disadvantage of the VIO system is that it can cause data anomalies and wild values due to noise, failure, and interference of the IMU and the camera itself, thus affecting the positioning accuracy and robustness. To deal with these problems, this paper describes a switching function to adjust the weights of the loss function of the VIO system.
Two different judgment criteria are used to design the switching function of the VIO system: one is based on the moving speed of the UAV, and the other is based on the consistency of the Mahalanobis distance (MD). The judgment criterion based on the UAV’s moving speed refers to the reliability of the VIO system based on the UAV’s movement state. If the UAV’s moving speed is too fast and exceeds the set threshold, this means that there is a problem with the VIO system and there may be a drift or cumulative error, in which case its weight should be reduced, and vice versa:
where
wv is the output value of the switching function,
v is the drone’s moving speed,
v0 is threshold, and
kv is coefficient. It can be seen that, when
, the switching function
wv is close to zero, which indicates that the VIO system data are unreliable and the weight of the loss function is very small; when
v is close to
v0,
wv is close to one, indicating that the VIO system data are reliable and the weight of the loss function is large. In this way, the weight of the VIO system data can be adjusted according to the drone’s moving speed.
The switching function based on MD can judge the reliability of data according to the difference between the data and the prediction value. MD is a distance measure that considers the data distribution and covariance matrix, and it can reflect the correlation and anomaly degree of the data. The larger the MD, the less reliable the data; the smaller the MD, the more reliable the data. This paper uses the following formula to calculate the MD:
where
z is the observation value,
is the prediction value, and
S is the covariance matrix. This paper uses the following formula to calculate the switching function:
where
w is the output value of the switching function, which represents the weight of the loss function. It can be seen that, when the MD is very large, the switching function is close to zero, indicating that the observation value is unreliable and the weight of the loss function is very small. When the MD is very small, the switching function is close to one; this indicates that the observation value is reliable and the weight of the loss function is very large. In this way, it can effectively suppress or eliminate the influence of outliers and wild values.
This paper uses a switching function to deal with IMU–camera data, which do not participate in the IMU pre-integration process, but only participate in the joint solution process of data fusion. In the joint solution process, this paper uses the following formula to calculate the loss function of IMU–camera data:
where
LIC is the loss function of the VIO system,
wIC is the output value of the switching function,
zIC is the observation value of the VIO system, and
h(
x) is the prediction value calculated based on the state variable
x. In this way, this paper effectively deals with the abnormality and wild values of IMU–camera data.
UWB signals have advantages of high accuracy, low power consumption, and anti-interference, which can provide absolute position information between drones. The advantage of UWB data is that it can perform positioning using distance information without visual information, and it can provide low-frequency and high-accuracy output. The disadvantage of UWB data is that, due to various factors in the signal propagation process, such as obstacles, occlusions, reflections, dynamic objects, etc., it causes phenomena such as signal attenuation, reflection, refraction, multipath, etc., which affect the accuracy and reliability of the data. To deal with these problems, this paper describes a switching function to adjust the weight of the loss function of UWB data.
Moreover, we describe a switching function based on the consistency of UWB data, which judges the reliability of UWB data according to its differences. If there is a large difference in the UWB data, this indicates that there is a problem with the data, which may have noise or outliers, and the weight of the UWB data should be reduced; if UWB data are consistent, this indicates that the UWB data are reliable and the weight should be increased. The following equation is used to compute the switching function based on the movement of the UAV:
where
wu is the output value of the switching function,
du is the sum of distances between UWB data,
de is the sum of distances calculated based on position estimation, and
ku is the set parameter. It can be seen that, when
du − de is very large,
wu is close to zero, indicating that the UWB data are inconsistent and the weight of the loss function is very small; when
du − de is very small,
wu is close to one, indicating that UWB data are consistent and the weight of the loss function is large. This way, the weight of UWB data can be adjusted according to consistency. In the joint solution process, we use the following formula to calculate the loss function of UWB data:
where
LUWB is the loss function of UWB data,
ZUWB is the observation value of UWB data, and
h(
x) is the prediction value calculated based on the state variable
x. In this way, this paper can effectively deal with the abnormality and wild values of UWB data. The switching function proposed in this paper can effectively deal with the outliers and wild values of UWB data. The following represents a simulation of the switching function using Python. Assuming that the simulation steps numbered 1000, the switching function had little effect on the simulation results without introducing large errors. Then, random large errors were introduced into the simulation, and the simulation results are shown in
Figure 5.
It can be seen from
Figure 5a,b that, after adding the switching function, the accuracy was significantly improved when facing random large errors. After multiple tests, the accuracy was improved by about 50% in
Figure 5c,d; of course, this was only limited to the introduction of random large errors. If there was always a peaceful and uniform error, the effect improvement was not significant.
3.4. Collaborative Positioning
The first three sections of
Section 3 describe the structure of the algorithm and the proposed methodology. The algorithm flow and pseudocode are given in this section. In addition, we will extend the topology constraints to the verification of the positioning results and the relocation of the failed positioning of single UAV. When the new switching functions proposed in the previous chapter are added in the loss function, it becomes
where
w is the switching function [
30]. The above formula adds three different switch factors, which were introduced in detail in the previous chapter.
and
represent the VIO factor of node
i at time
k based on the drone’s moving speed switching function and on the MD switching function, respectively.
represents the switching function based on the consistency of UWB data between node
i and node
j at time
k. For the VIO factor, the constraint method is used to calculate the following:
where
and
represent the co-ordinates in position obtained by the VIO system
i at time k. To obtain the value of the switching function, further calculation of the square of the sum of distances obtained by UWB is required, and, in the same way,
. In Equation (20),
is replaced by the following formula:
.
It is common for VIO to crash when faced with low feature points and low textures or high-mobility scenes. And, in the experiment, it was found that, when the data of UWB and the data of VIO became unreliable, the positioning results were easy to become confused. In order to further improve the robustness of the algorithm, we will further extend the topological constraints. When the UWB data are unreliable at a certain time, the least two-hop distance between the two nodes or the distance from the previous moment is considered, and the experimental results show that the distance at the previous time has higher accuracy. In addition, when the output result of a single VIO is NAN, we consider the topology of the previous moment and the topology between the drones except the position breakdown one at this moment. According to the improved MDS algorithm, a directionless, centralized co-ordinate after dimension reduction is obtained.
We suppose
n = 3, where
Di refers to UWB measurements matrix at times
i, and
is the Eigenvalue decomposition of matrix
Bi. Then,
Bi becomes
Then, we have
where
and
. Retain the two largest eigenvalues
,
and corresponding eigenvectors
q1,
q2 to calculate the 2D co-ordinates of the node. According to our previous thought proposed in paper [
10], it is possible to convert a problem with 2
M degrees of freedom into a problem with 1 degree of freedom, and this only requires the rotation angle to be solved. Then, the rotation angle is found by the least-squares method, and the location of position breakdown UAV is recovered through the topology. After the position is correct, the position and posture of the drone can be corrected through iteration, so as to improve success rate of positioning. This is the improvement for topological constraints. Algorithm 1 summarizes the algorithm to estimate locations of nodes with noisy measurements.
Algorithm 1 Cooperative Increasing SAM Algorithm |
Input: IMU_camera_data, UWB_data, GPS_priori; |
Output: optimized_nodes_values |
1: graph = create_graph() |
2: for timestep do in parallel |
3: initialize_nodes_values(graph, GPS_priori) |
4: for each drone do |
5: node = create_node(drone, time_step) |
6: graph.add_node(VIO_node) |
7: Use Equation (19) to calculate the VIO system switching function |
8: graph.add_node(switching_node) |
9: end for |
10: get Topology Matrix D by UWB |
11: graph.add_node(UWB_node) |
12: Use Equation (21) to calculate the topological system switching function |
13: graph.add_node(switching_node) |
14: if single_VIO_position = NAN then |
15: Recovered position by MDS (27) |
16: end if |
17: while(times<max_iterations&¬ converged) do |
18: optimize_function(graph) |
19: end while |
20: return optimized_nodes_values |
21:end for in parallel |
22:return |