2.1 Distributed cooperative navigation algorithm based on factor graph

A factor graph is a bipartite probabilistic graphical model used to express the joint probability distribution of random variables [Reference Loeliger, Dauwels, Hu, Korl, Ping and Kschischang23–Reference Yan, Luan, Liu and Xing25] ${F}=\left(\mathbf{\textit{U}},\mathbf{\textit{V}},\mathbf{\textit{E}}\right)$ . In the factor graph framework [Reference Wang, Xu, Li, Duan and Zhang26], the factor node ${f}_{i}\in \mathbf{\textit{U}}$ , the variable node ${\mathbf{\textit{x}}}_{j}\in \mathbf{\textit{V}}$ , and the edge ${e}_{ij}\in \mathbf{\textit{E}}$ always exist between the factor node and the variable node. The resulting factor graph can be defined as a decomposition of the function $f\left(X\right)$ : $f\left(\mathbf{\textit{X}}\right)=\prod _{i} {f}_{i}\left({\mathbf{\textit{x}}}_{i}\right)$ , where $\mathbf{\textit{X}}$ is the set of all variable nodes associated with the variable node ${\mathbf{x}}_{i}$ . The factor ${f}_{i}\left({\mathbf{\textit{x}}}_{i}\right)$ is usually defined as an error function ${f}_{i}\left({\mathbf{\textit{x}}}_{i}\right)=d\left(err({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{z}}}_{i})\right)$ , $\mathbf{\textit{d}}\left(\cdot \right)$ is the cost function and ${\mathbf{\textit{z}}}_{i}$ is the actual measured value.

The information fusion problem for UAV formation cooperative navigation is formulated within a factor graph framework, and the variable node ${\mathbf{\textit{x}}}_{i}^{k}$ is used to represent the navigation state of UAV i at the moment of ${t}_{k}$ :

(1) \begin{align}{\mathbf{x}}_{i}^{k}={[\begin{array}{lllll}\delta \mathbf{\textit{p}} {} \quad \delta \mathbf{\textit{v}} {} \quad \psi \quad {} {\mathbf{\textit{d}}}_{a} {} \quad {\mathbf{\textit{d}}}_{g}\end{array}]}^{T} \end{align}

where $\delta \mathbf{\textit{p}}$ is the position error, $\delta \mathbf{\textit{v}}$ is the velocity error, $\psi $ denotes the attitude, ${\mathbf{\textit{d}}}_{a}$ is the acceleration zero bias and ${\mathbf{\textit{d}}}_{g}$ is the gyroscope zero bias. Therefore, the navigation state set of UAV formation C at ${t}_{k}$ is defined as ${\mathbf{\textit{X}}}^{k}=\left\{{\mathbf{\textit{x}}}_{i}^{k}|i\in C\right\}$ .

Let ${\mathbf{\textit{z}}}_{i,k-1}^{IMU}$ represent all measurement data received by the IMU of UAV i from time ${t}_{k-1}$ to ${t}_{k}$ , ${\mathbf{\textit{z}}}_{i,k}^{UWB}$ represent all UWB ranging measurements received by UAV i at time ${t}_{k}$ , and ${\mathbf{z}}_{i,k}^{GNSS}$ represent all GNSS measurements received by UAV i at time ${t}_{k}$ . Therefore, the set of all measured values received by the UAV formation C at ${t}_{k}$ can be defined as: ${\mathbf{\textit{Z}}}^{k}=\left\{{\mathbf{\textit{z}}}_{i,k-1}^{IMU},{\mathbf{\textit{z}}}_{i,k}^{UWB},{\mathbf{\textit{z}}}_{i,k}^{GNSS}|i\in C\right\}$ . The maximum a posteriori estimation of the state variable is expressed as:

(2) \begin{align}{\mathbf{\textit{X}}}_{MAP}^{k}=\mathrm{a}\mathrm{r}\mathrm{g} \, \underset{{\mathbf{\textit{X}}}^{k}}{\mathrm{}\mathrm{m}\mathrm{a}\mathrm{x}}\mathbf{}\mathbf{\textit{P}}\left({\mathbf{\textit{X}}}^{k}|{\mathbf{\textit{Z}}}^{k}\right) \end{align}

The resulting joint probability density of all state variables and measurements can be decomposed as [Reference Wang, Hu and Tao27]:

(3) \begin{align}\mathbf{\textit{P}}({\mathbf{\textit{X}}}^{k}|{\mathbf{\textit{Z}}}^{k})=\mathbf{\textit{P}}\left({\mathbf{\textit{X}}}_{0}\right)\prod _{i=1}^{k} P\left({\mathbf{\textit{x}}}_{i}|{\mathbf{\textit{x}}}_{i-1},{\mathbf{\textit{z}}}_{i-1}^{IMU}\right)\prod _{i=1}^{k} \mathbf{\textit{P}}\left({\mathbf{\textit{z}}}_{i}^{GNSS}|{\mathbf{\textit{x}}}^{i}\right)\prod _{t=1}^{k} \mathbf{\textit{P}}\left({\mathbf{\textit{z}}}_{i}^{UWB}|{\mathbf{\textit{x}}}_{i}^{t},{\mathbf{\textit{x}}}_{j}^{t}\right) \end{align}

where $P\left({\mathbf{\textit{X}}}_{0}\right)$ denotes the prior information about the initial state of all variables, $P\left({\mathbf{\textit{x}}}_{i}\right|{\mathbf{\textit{x}}}_{i-1},{\mathbf{\textit{z}}}_{i-1}^{IMU})$ denotes the state probability density function of IMU, $P\left({\mathbf{\textit{z}}}_{i}^{GNSS}|{\mathbf{\textit{x}}}^{i}\right)$ and $P\left({\mathbf{\textit{z}}}_{i}^{UWB}|{\mathbf{\textit{x}}}_{i}^{t},{\mathbf{\textit{x}}}_{j}^{t}\right)$ denote the conditional probability density functions of GNSS and UWB, respectively.

In the factorisation of the joint probability density function $P\left({\mathbf{\textit{X}}}^{k}|{\mathbf{\textit{Z}}}^{k}\right)$ , each factor represents an independent term, so the joint probability density function can be decomposed as:

(4) \begin{align}\mathbf{\textit{P}}\left({\mathbf{\textit{X}}}^{k}|{\mathbf{\textit{Z}}}^{k}\right)\propto \prod _{i} {f}_{i}\left({\mathbf{\textit{X}}}_{i}^{k}\right),{\mathbf{\textit{X}}}_{i}^{k}\in {\mathbf{\textit{X}}}^{k} \end{align}

In this paper, it is assumed that the noise obeys a Gaussian distribution [Reference Quan, Chen, Zhou, Zhao, Hu and Zhu28] and the factor nodes can be described as:

(5) \begin{align}{f}_{i}\left({\mathbf{\textit{x}}}_{i}\right)=d\left(err\left({x}_{i}^{k},{z}^{k}\right)\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left\{-\frac{1}{2}{ \Big| \Big| {h}_{i}\left({\mathbf{\textit{x}}}_{i}^{k}\right)-{\mathbf{z}}^{k}\Big| \Big|}_{{\rm{\Sigma }}_{i}}^{2}\right\} \end{align}

where ${||\cdot ||}_{\rm{\Sigma }}^{2}$ represents the squared Mahalanobis distance, ${\rm{\Sigma }}_{i}$ represents the process noise covariance matrix and ${h}_{i}\left(\mathbf{\textit{x}}\right)$ is the nonlinear measure function. The maximum a posteriori estimation problem can then be transformed into a nonlinear least-squares problem [Reference Yang, Ye, Shao, Yang and Huang29].

(6) \begin{align}{\mathbf{\textit{X}}}_{MAP}^{k}=\mathrm{a}\mathrm{r}\mathrm{g} \, \underset{{\mathbf{\textit{X}}}^{k}}{\mathrm{m}\mathrm{i}\mathrm{n}}\sum _{i} { \Big| \Big|{h}_{i}\left({\mathbf{x}}_{i}^{k}\right)-{\mathbf{\textit{z}}}^{k} \Big| \Big|}_{{\rm{\Sigma }}_{i}}^{2} \end{align}

In the distributed UAV formation cooperative navigation process, each UAV only processes its own IMU and GNSS information as well as the UWB ranging information that satisfies the current constraints, and the position optimisation of the UAVs is achieved through message iteration. The factor graph optimisation function can be expressed as:

(7) \begin{equation}\arg \min \sum_k \| \mathbf{r}_{IMU}^k (\mathbf{X}_k, \mathbf{X}_{k+1}) \|_{\Sigma_{IMU}}^2+ \sum_k \| \mathbf{r}_{GNSS}^k (\mathbf{X}_k) \|_{\Sigma_{GNSS}}^2+ \sum_k \left( \| \mathbf{r}_{ij}^{k, UWB} (\mathbf{X}_i^k, \mathbf{X}_j^k) \|_{\Sigma_d}^2 \right)\end{equation}

where r represents the residual. Using Taylor’s formula to linearise the above equation, it is transformed into a least squares problem. The specific simplification process is referred to in reference [Reference Chen, Xiong, Liu, Wang and Xiong30], and the optimal estimation value is solved by the Gauss-Newton method.

Here, taking three UAVs as an example, the distributed UAV formation cooperative navigation factor graph structure is shown in Fig. 1.

Figure 1.

UAV formation cooperative navigation factor graph model.

Figure 1. Long description

A diagram of the cooperative navigation factor graph model for UAV formations. The diagram illustrates the relationships and interactions between various state variables and measurements for UAV navigation. The overall structure includes nodes and edges representing the state variables and measurements, respectively. The nodes are labeled with variables such as position error, velocity error, accelerometer zero bias, gyroscope zero bias, and maximum a posteriori estimates of state variables. The edges represent the measurements received by the UAV formation at different time positions. The diagram shows the flow of information and the dependencies between different state variables and measurements. The nodes are connected by edges labeled with specific measurements, indicating the communication delay values and the joint probability density function. The diagram captures the complex interactions and dependencies in the cooperative navigation of UAV formations, highlighting the importance of accurate state estimation and measurement integration.

In this paper, each UAV is equipped with a suite of sensors, including an IMU, a GNSS receiver and a UWB ranging module. The following is a simple derivation of the factor formula of the above sensor measurement model.

2.1.1 IMU factor node

The INS produces high-rate measurements due to its high output frequency and different sampling frequency from other sensors. In order to achieve plug-and-play for each sensor in the factor graph while reducing the update frequency of the factor graph, this paper uses pre-integration to construct factor nodes.

Let one update cycle of the INS pre-integration be ${t}_{k}\sim {t}_{k+1}$ , and the position, velocity and attitude increments during the cycle be:

(8) \begin{equation}\begin{cases} \Delta \mathbf{p}_{t_k}^{t_{k+1}} = \int_{} \int_{t_k}^{t_{k+1}} \mathbf{C}_{b_{t_k}}^{b_t} \left( \mathbf{d}_t^b - \nabla \mathbf{d}_a \right) dt^2 \\ \Delta \mathbf{v}_{t_k}^{t_{k+1}} = \int_{t_k}^{t_{k+1}} \mathbf{C}_{b_{t_k}}^{b_t} \left( \mathbf{d}_t^b - \nabla \mathbf{d}_a \right) dt \\ \Delta \psi_{t_k}^{t_{k+1}} = \int_{t_k}^{t_{k+1}} \mathbf{E}_{b_{t_k}}^{b_t} \left( \mathbf{g}_t^b - \nabla \mathbf{d}_g \right) dt \end{cases}\end{equation}

where ${\mathbf{\textit{C}}}_{{b}_{{t}_{k}}}^{{b}_{t}}$ and ${\mathbf{\textit{E}}}_{{b}_{{t}_{k}}}^{{b}_{t}}$ denote the rotation matrix and rotation rate matrix of the carrier coordinate system at time t relative to the start moment ${t}_{k}$ , respectively. ${\mathbf{\textit{d}}}_{t}^{b}$ and ${\mathbf{g}}_{t}^{b}$ represent the accelerometer and gyroscope measurements at moment t, while $\nabla {\mathbf{\textit{d}}}_{a}$ and $\nabla {\mathbf{\textit{d}}}_{g}$ denote their corresponding biases. By combining these incremental measurements with the navigation state at ${t}_{k}$ , the state at ${t}_{k+1}$ is updated as follows:

(9) \begin{align}\left\{\begin{array}{l}{\mathbf{\textit{p}}}_{{t}_{k+1}}^{n}={\mathbf{\textit{p}}}_{{t}_{k}}^{n}+({t}_{k+1}-{t}_{k}){\mathbf{\textit{v}}}_{{t}_{k}}^{n}+\frac{1}{2}({t}_{k+1}-{t}_{k}{)}^{2}{\mathbf{g}}^{n}+{\mathbf{\textit{C}}}_{{b}_{{t}_{k}}}^{n}\Delta {\mathbf{\textit{p}}}_{{t}_{k}}^{{t}_{k+1}}\\[3pt]{\mathbf{\textit{v}}}_{{t}_{k+1}}^{n}={\mathbf{\textit{v}}}_{{t}_{k}}^{n}+({t}_{k+1}-{t}_{k}){\mathbf{g}}^{n}+{\mathbf{\textit{C}}}_{{b}_{{t}_{k}}}^{n}\Delta {\mathbf{\textit{v}}}_{{t}_{k}}^{{t}_{k+1}}\\[3pt]{\psi }_{{t}_{k+1}}^{n}={\psi }_{{t}_{k}}^{n}+{\mathbf{\textit{C}}}_{{b}_{{t}_{k}}}^{n}\Delta {\psi }_{{t}_{k}}^{{t}_{k+1}}\end{array}\right.\end{align}

where ${\mathbf{g}}^{n}$ is the gravity vector, ${\mathbf{\textit{C}}}_{{b}_{{t}_{k}}}^{n}$ is the rotation matrix of the navigation coordinate system relative to the carrier coordinate system at the moment of ${t}_{k}$ , from which we get the information of position, velocity and attitude under the navigation coordinate system at the moment of ${t}_{k+1}$ .

Let UAV i receive the measurement information ${\mathbf{\textit{z}}}_{i,{t}_{k}}^{IMU}=\{{\mathbf{\textit{d}}}^{b},{\mathbf{g}}^{b}\}$ , and according to the INS pre-integration process, the estimated value of the navigation state of UAV i at the moment ${t}_{k+1}$ moment status update value:

(10) \begin{align}{\mathbf{\textit{x}}}_{i}^{{t}_{k+1}}={h}\big({\mathbf{\textit{x}}}_{i}^{{t}_{k}},{z}_{i,{t}_{k}}^{IMU},{t}_{k}\big) \end{align}

where ${\mathbf{\textit{x}}}_{i}^{{t}_{k}}$ is the position, velocity and attitude information of the INS carried by UAV i at the ${t}_{k}$ moment. Let ${\alpha }_{i}^{{t}_{k}}$ be the deviation correction value of the accelerometer and gyroscope at ${t}_{k}$ , and the predicted value of INS navigation state at ${t}_{k+1}$ is obtained by $\hat{x}_i^{t_{k+1}} = h(x_i^{t_k}, \alpha_i^{t_k}, z_{i,t_k}^{IMU})$ . Thus, the IMU factor node is defined as:

(11) \begin{align}{\mathbf{\textit{f}}}_{i,{t}_{k}}^{IMU}\!\left({\mathbf{\textit{x}}}_{i}^{{t}_{k}},{\mathbf{\textit{x}}}_{i}^{{t}_{k+1}}\right)=d\left({{\hat{\mathbf{\textit{x}}}}_{i}}^{{t}_{k+1}}-{\mathbf{\textit{x}}}_{i}^{{t}_{k+1}}\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{1}{2}{ \Big| \Big|{{\hat{\mathbf{\textit{x}}}}_{i}}^{{t}_{k+1}}-{\mathbf{\textit{x}}}_{i}^{{t}_{k+1}} \Big| \Big|}_{{\rm{\Sigma }}_{IMU}}^{2}\right) \end{align}

The IMU residual model can be expressed as:

(12) \begin{align}{\mathbf{\textit{r}}}_{IMU}^{k}\left({\mathbf{\textit{X}}}_{k},{\mathbf{\textit{X}}}_{k+1}\right)\propto {\hat{\mathbf{\textit{x}}}}_{i}^{{t}_{k+1}}-{\mathbf{\textit{x}}}_{i}^{{t}_{k+1}}\\[-20pt] \nonumber \end{align}

2.1.2 UWB factor node

The distance between UAV i and UAV j is modelled as:

(13) \begin{align}{\mathbf{\textit{d}}}_{i,j}^{UWB}={\mathbf{\textit{d}}}_{i,j}+{\mathbf{\textit{n}}}_{UWB} \end{align}

where ${\mathbf{\textit{d}}}_{i,j}$ is the true distance between the two UAVs, ${\mathbf{\textit{n}}}_{UWB}$ is the UWB ranging noise, and the distance observation function $h({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{x}}}_{j})$ between UAVs can be expressed as:

(14) \begin{align}h({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{x}}}_{j})=\sqrt{{\mathbf{\textit{x}}}_{i}{\mathbf{\textit{x}}}_{j}^{T}}=\sqrt{\big({\mathbf{\textit{x}}}_{i}^{e}-{\mathbf{\textit{x}}}_{j}^{e}{\big)}^{2}+\big({\mathbf{\textit{x}}}_{i}^{n}-{\mathbf{\textit{x}}}_{j}^{n}{\big)}^{2}+\big({\mathbf{\textit{x}}}_{i}^{u}-{\mathbf{\textit{x}}}_{j}^{u}{\big)}^{2}} \end{align}

Thus, the UWB ranging factor is defined as:

(15) \begin{align}{\mathbf{\textit{f}}}_{i,j}^{UWB}({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{x}}}_{j})=\mathbf{\textit{d}}\left({\mathbf{\textit{d}}}_{i,j}^{UWB}-h({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{x}}}_{j})\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{1}{2}{\Big| \Big|{\mathbf{\textit{d}}}_{i,j}^{UWB}-h({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{x}}}_{j}) \Big| \Big|}_{{\rm{\Sigma }}_{d}}^{2}\right) \end{align}

The UWB ranging factor residual model is expressed as:

(16) \begin{align}{\mathbf{\textit{r}}}_{ij}^{k,WB}\propto {\mathbf{\textit{d}}}_{i,j}^{UWB}-h({\mathbf{\textit{x}}}_{i},{\mathbf{\textit{x}}}_{j})\\[-20pt] \nonumber \end{align}

2.1.3 GNSS factor node

The GNSS measurement equation can generally be modelled as:

(17) \begin{align}{\mathbf{\textit{z}}}_{i}^{GNSS}={h}^{GNSS}\left({\mathbf{\textit{x}}}_{i}\right)+{\eta }_{GNSS} \end{align}

where ${h}^{GNSS}\left( \bullet \right)$ denotes the relationship between the carrier’s current navigation state and the GNSS measurements ${\mathbf{\textit{z}}}_{i}^{GNSS}$ , and ${\eta }_{GNSS}$ is the GNSS measurement noise. The GNSS factor of UAV i at the moment ${t}_{k}$ is defined as:

(18) \begin{align}{\mathbf{\textit{f}}}_{i,{t}_{k}}^{GNSS}\left({\mathbf{\textit{x}}}_{i}^{{t}_{k}}\right)=d\left({\mathbf{\textit{z}}}_{i,{t}_{k}}^{GNSS}-{h}^{GNSS}\left({\mathbf{\textit{x}}}_{i}^{{t}_{k}}\right)\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{1}{2}{ \Big| \Big|{\mathbf{\textit{z}}}_{i,{t}_{k}}^{GNSS}-{h}^{GNSS}\left({\mathbf{\textit{x}}}_{i}^{{t}_{k}}\right) \Big| \Big|}_{{\rm{\Sigma }}_{GNSS}}^{2}\right) \end{align}

The GNSS factor residual model can be expressed as:

(19) \begin{align}{\mathbf{\textit{r}}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}^{k}\left({\mathbf{\textit{X}}}_{k}\right)\propto {\mathbf{\textit{z}}}_{i,{t}_{k}}^{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}-{h}^{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}\left({\mathbf{\textit{x}}}_{i}^{{t}_{k}}\right) \\[-20pt] \nonumber \end{align}

2.2 Delay factor graph model

The transmission delay of UWB communication technology is mainly composed of fixed delay, propagation delay and random delay. Fixed delay mainly includes system processing delay and wireless network queuing delay in a multi-user environment. Propagation delay is usually proportional to the distance. Among them, the random delay is caused by multiple factors, such as the multipath effect and non-line-of-sight (NLOS) propagation in complex environments. Thus, the transmission delay model of UWB communication is established:

(20) \begin{align}\delta t=\rm{\Delta }T+\frac{{\mathbf{\textit{z}}}_{k}^{i}}{c}+{t}_{\eta } \\[-50pt] \nonumber \end{align}

where $\Delta T$ is the communication fixed delay, ${t}_{\eta }$ is the random communication delay obeying a Gaussian distribution with mean 0 and variance ${\sigma }^{2}$ . ${\mathbf{\textit{z}}}_{k}^{i}$ expresses the inter-UAV communication distance and c is the speed of light.

The transmission distance of UWB communication is within 1km, and the communication delay is generally less than 1s. Thus, the relevant propagation delay ${\mathbf{\textit{z}}}_{k}^{i}/c$ is on the order of microseconds, and the communication transmission delay model can be simplified as:

(21) \begin{align}\delta t \approx \Delta T + t_\eta,\quad \delta t \sim \mathrm{N}(\Delta T, \sigma^2)\end{align}

The short-term flight trajectory of UAV formation can be approximated as linear motion, so the position and ranging information of UAV i and UAV j can be expressed as:

(22) \begin{align}\left\{\begin{array}{ll}{\mathbf{\textit{p}}}_{i}^{k}\approx {\mathbf{\textit{p}}}_{i}^{k-\delta t}+{\mathbf{\textit{v}}}_{i}^{k-\delta t}\delta t+\frac{1}{2}{{a}}_{i}^{k-\delta t}\delta {t}^{2} {} \\[5pt] {\mathbf{\textit{p}}}_{j}^{k}\approx {\mathbf{\textit{p}}}_{j}^{k-\delta t}+{\mathbf{\textit{v}}}_{j}^{k-\delta t}\delta t+\frac{1}{2}{{a}}_{j}^{k-\delta t}\delta {t}^{2} {} \\[5pt] {\mathbf{\textit{d}}}_{ij}= \left | \left|{\mathbf{\textit{p}}}_{i}^{k}-{\mathbf{\textit{p}}}_{j}^{k} \right| \right| {} \end{array}\right. \end{align}

Let UAV i receive the measurement information of UAV j at k moments, but the information corresponds to the measurement information of UAV j at $k-\delta t$ moments. Let the state of UAV j at the moment of $k-\delta t$ be ${\mathbf{\textit{X}}}_{j}^{k-\delta t}$ , and use the state transfer model to predict the state of UAV j at the current moment of k to be:

(23) \begin{align}{\hat{\mathbf{\textit{X}}}}_{j}^{k}=h\big({\mathbf{\textit{X}}}_{j}^{k-\delta t},\delta t\big) \end{align}

where $h\left( \bullet \right)$ is the state propagation function from ${\mathbf{\textit{X}}}_{j}^{k-\delta t}$ to ${\mathbf{\textit{X}}}_{j}^{k}$ .

Because $\delta t$ is unknown, it needs to be obtained after factor graph optimisation, the factor requires delayed prior information as a starting term. From Equation (21), the residual squared term of the delayed a priori factor can be expressed as:

(24) \begin{align}{||{\mathbf{\textit{r}}}_{t}^{k}\left(\delta t\right)||}_{{\rm{\Sigma }}_{t}}^{2}={(\delta {t}^{k}-\rm{\Delta }T)}^{T}{\boldsymbol{\Sigma }}_{t}^{-1}(\delta {t}^{k}-\rm{\Delta }T) \end{align}

For the UWB ranging module, let the position information of UAV i at k moments be ${p}_{i}^{k}$ , and the predicted value of the position of UAV j at k moments be ${\hat{\mathbf{\textit{p}}}}_{j}^{k}$ , which is obtained from Equation (22). The UWB ranging residual including communication delay is expressed as:

(25) \begin{align}{\mathbf{\textit{r}}}_{d}=\parallel {\mathbf{\textit{p}}}_{i}^{k}-{\hat{\mathbf{\textit{p}}}}_{j}^{k}\parallel -{\mathbf{\textit{d}}}_{ij,k}^{UWB} \end{align}

where ${\mathbf{\textit{d}}}_{ij,k}^{UWB}$ is the true distance between UAV i and UAV j at k moments.

The UWB factor considering communication delay is modified to:

(26) \begin{align}{\mathbf{\textit{f}}}^{UWB}\big({\mathbf{\textit{X}}}_{ij}^{k},\delta t\big)=\mathrm{e}\mathrm{x}\mathrm{p}\left\{-\frac{1}{2}\parallel {\mathbf{\textit{r}}}_{d}{\parallel }_{{\rm{\Sigma }}_{d}}^{2}\right\} \end{align}

where ${\mathbf{\textit{X}}}_{ij}^{k}$ denotes the state information of UAV i and UAV j at k moments, and the ranging covariance ${\rm{\Sigma }}_{d}$ with delay is expressed as:

(27) \begin{align}{\rm{\Sigma }}_{d}={\rm{\Sigma }}_{uwb}+{\rm{\Sigma }}_{t}={\rm{\Sigma }}_{uwb}+H{\sigma }^{2}{H}^{T} \end{align}

where ${\boldsymbol{\Sigma }}_{uwb}$ is the UWB’s own measurement covariance, and H is the delayed Jacobian matrix:

(28) \begin{align}\mathbf{\textit{H}}=\frac{\partial h\big({\mathbf{\textit{X}}}_{j}^{k-\tau },\delta t\big)}{\partial \delta t} \end{align}

Therefore, the optimisation function of UAV formation cooperative navigation factor graph considering communication delay is expressed as:

(29) \begin{align}\begin{array}{ll} {} \mathrm{a}\mathrm{r}\mathrm{g}\, \mathrm{m}\mathrm{i}\mathrm{n}\sum\limits_{\mathbf{\textit{X}},\delta t} \parallel {\mathbf{\textit{r}}}_{IMU}^{k}({\mathbf{\textit{X}}}_{k},{\mathbf{\textit{X}}}_{k+1}){\parallel }_{{\rm{\Sigma }}_{IMU}}^{2}+\sum\limits _{k} \parallel {\mathbf{\textit{r}}}_{GNSS}^{k}\left({\mathbf{\textit{X}}}_{k}\right){\parallel }_{{\rm{\Sigma }}_{GNSS}}^{2}\\[10pt] {} +\sum\limits _{k} \left(\parallel {\mathbf{\textit{r}}}_{ij}^{k,UWB}\big({\mathbf{\textit{X}}}_{i}^{k},{\mathbf{\textit{X}}}_{j}^{k-\delta t},\delta t\big){\parallel }_{{\rm{\Sigma }}_{d}}^{2}+\parallel {\mathbf{\textit{r}}}_{t}^{k}\left(\delta t\right){\parallel }_{{\rm{\Sigma }}_{t}}^{2}\right)\end{array} \end{align}

2.3 Sliding window design

In the process of iterative optimisation, each time the system uploads the measurement value of a sensor, the factor graph will generate a new node. As the number of objective function items increases, the number of rows of the Jacobian matrix will increase, which will seriously increase the computational burden. The sliding window method is used to dynamically retain the recent critical state measurements to limit the scope of optimisation [Reference Zhao and Ma31], which effectively balances the state estimation accuracy with the computational efficiency and improves the system real-time performance. A sliding window with a width of $N(1\le N\le l)$ is set, and the UAV solves the factor graph optimisation by combining the previous N sets of sensor observations and state information for the current moment.

Let the set of states and measures in the sliding window at moment k be $\text{Q}=\{{\mathbf{\textit{X}}}_{1}^{k},{\mathbf{\textit{X}}}_{2}^{k}, \cdots,{\mathbf{\textit{X}}}_{n}^{k}, {\mathbf{\textit{Z}}}_{1}^{k},{\mathbf{\textit{Z}}}_{2}^{k},\cdots ,{\mathbf{\textit{Z}}}_{n}^{k}|n\in \text{N}\}$ , the joint probability density distribution of the factor graph can be rewritten as:

(30) \begin{align}\begin{array}{ll} {} \mathbf{\textit{p}}({\mathbf{\textit{X}}}^{k}|{Z}^{k})=\mathbf{\textit{p}}({\mathbf{\textit{X}}}^{0})\cdot \prod\limits _{k\in N} \mathbf{\textit{p}}({\mathbf{\textit{x}}}^{k}|{\mathbf{\textit{z}}}^{k-1,IMU},{\mathbf{\textit{x}}}^{k-1})\cdot \\[8pt] {} \prod\limits _{k\in N} \mathbf{\textit{p}}({\mathbf{\textit{z}}}^{k,GNSS}|{\mathbf{\textit{x}}}^{k})\cdot \prod\limits _{k\in N} \mathbf{\textit{p}}\big({\mathbf{\textit{z}}}^{k,UWB}|{\mathbf{\textit{x}}}_{i}^{k},{\mathbf{\textit{x}}}_{j}^{k-\delta t}\big)\cdot \prod\limits _{k\in N} \mathbf{\textit{p}}\left(\delta {t}_{ij}^{k}\right|\delta {t}_{ij}^{k-1})\end{array} \end{align}

Simply using the sliding window to filter out the state and measurement values at distant times in the past may cause the loss of epoch information, resulting in the non-global optimal solution for the estimated value. The old nodes are removed without loss of calendar element information by the marginalisation operation. Let $x$ be the variable nodes to be filtered, $y$ be the new variable node, the joint probability density can be expressed as $\mathbf{\textit{p}}(\mathbf{\textit{x}},\mathbf{\textit{y}})$ in the sliding window range, and the probability density $\mathbf{\textit{p}}\left(\mathbf{\textit{y}}\right)$ of the node y can be obtained by integrating the marginalised node x.

Let the joint probability density $\mathbf{\textit{p}}(\mathbf{\textit{x}},\mathbf{\textit{y}})$ conform to a Gaussian distribution, chunked in the form of an information vector and an information matrix:

(31) \begin{align}\mathbf{\textit{p}}(\mathbf{\textit{x}},\mathbf{\textit{y}})=\text{N}\Bigg({\boldsymbol{\Lambda }}^{-1}\left[\begin{array}{l}{\eta }_{x}\\[3pt] {\eta }_{y}\end{array}\right],{\left[\begin{array}{ll}{\boldsymbol{\Lambda }}_{xx} {} \quad {\boldsymbol{\Lambda }}_{xy}\\[3pt] {\boldsymbol{\Lambda }}_{yx} {} \quad {\boldsymbol{\Lambda }}_{yy}\end{array}\right]}^{-1}\Bigg) \end{align}

Based on the factor graph linearisation process and the Gauss-Newton method, there are:

(32) \begin{align}\rm{\Lambda }\rm{\Delta }\mathbf{\textit{X}}=\mathbf{\textit{b}} \end{align}

where $\rm{\Delta }\boldsymbol{X}$ is the state node increment, $\rm{\Lambda }$ is the information matrix, $\rm{\Lambda }={\mathbf{\textit{J}}}^{T}\mathbf{\textit{J}}$ , $\mathbf{\textit{J}}$ is the Jacobi matrix, $\mathbf{\textit{b}}={\mathbf{\textit{J}}}^{{T}}\mathbf{\textit{r}}$ , $\mathbf{\textit{r}}$ is the residual term corresponding to factor nodes associated with variable nodes. The matrix is divided into blocks, there are:

(33) \begin{align}\left[\begin{array}{ll}{\rm{\Lambda }}_{xx} {} \quad {\rm{\Lambda }}_{xy}\\[3pt] {\rm{\Lambda }}_{yx} \quad {\rm{\Lambda }}_{yy}\end{array}\right]\left[\begin{array}{l}\rm{\Delta }\mathbf{\textit{x}}\\[3pt] \rm{\Delta }\mathbf{\textit{y}}\end{array}\right]=\left[\begin{array}{l}{\mathbf{\textit{b}}}_{x}\\[3pt] {\mathbf{\textit{b}}}_{y}\end{array}\right] \end{align}

The variable x is eliminated by matrix Schur complement operation, and the following is obtained:

(34) \begin{align}\big({\rm{\Lambda }}_{yy}-{\rm{\Lambda }}_{yx}{\rm{\Lambda }}_{xx}^{-1}{\rm{\Lambda }}_{xy}\big)\rm{\Delta }\mathbf{\textit{y}}={\mathbf{\textit{b}}}_{y}-{\rm{\Lambda }}_{yx}{\rm{\Lambda }}_{xx}^{-1}{\rm{\Lambda }}_{xy} \end{align}

In summary, even if there is no old variable x, the remaining variable y can be solved. And the constraint information of the old variables is retained in the information matrix, set:

(35) \begin{align}\left\{ \begin{array}{ll}{\boldsymbol{\Lambda }}_{y}^{0}={\boldsymbol{\Lambda }}_{yy}-{\boldsymbol{\Lambda }}_{yx}{\boldsymbol{\Lambda }}_{xx}^{-1}{\boldsymbol{\Lambda }}_{xy} {} \\[3pt] {\mathbf{\textit{b}}}_{y}^{0}={\mathbf{\textit{b}}}_{y}-{\boldsymbol{\Lambda }}_{yx}{\boldsymbol{\Lambda }}_{xx}^{-1}{\boldsymbol{\Lambda }}_{xy} {} \end{array}\right. \end{align}

Then the priori information of the remaining variable $y$ can be expressed as:

(36) \begin{align}\mathbf{\textit{p}}\left({\mathbf{\textit{y}}}^{0}\right)\propto \mathrm{e}\mathrm{x}\mathrm{p}\left({\left({\mathbf{\textit{b}}}_{y}^{0}\right)}^{T}\mathbf{\textit{y}}-{\mathbf{\textit{y}}}^{T}{{A}}_{y}^{0}\mathbf{\textit{y}}\right) \end{align}

Thus, the final optimised objective function for the factor graph within the sliding window can be obtained:

(37) \begin{equation}\begin{aligned}X_{MAP}^k ={}& \arg\min_{X, \delta t} \left\| (b^0)^T X_k - X_k^T \Lambda_y^0 X_k \right\|_{\Sigma_0}^2 \\&+ \sum_{k \in N} \left\| r_{IMU}^k (X_k, X_{k+1}) \right\|_{\Sigma_{IMU}}^2 \\&+ \sum_{k \in N} \left\| r_{GNSS}^k (X_k) \right\|_{\Sigma_{GNSS}}^2 \\&+ \sum_{k \in N} \left( \left\| r_{ij}^{k, UWB} (X_i^k, X_j^{k-\delta t}, \delta t) \right\|_{\Sigma_d}^2 + \left\| r_t^k (\delta t) \right\|_{\Sigma_t}^2 \right)\end{aligned}\end{equation}

2.4 Delayed estimation of iterative step size optimisation

Considering the real-time performance of the system, the number of message iterations of the factor graph algorithm is limited to 5 times. In order to achieve fast convergence of delay estimates during the limited number of iterations, the AMAGD algorithm is designed by combining historical delay estimates with real-time observation data. By dynamically adjusting the learning rate and introducing the momentum term, the historical gradient information is used to accelerate the convergence. At the same time, combined with the joint determination of ranging error and positioning error, the error is minimised within a limited number of iterations.

According to the cooperative navigation algorithm mentioned in this paper, the communication delay will cause the ranging time to lag behind reality, resulting in ranging error. When the delayed ranging data is used for positioning, it will further lead to the deviation of position estimation, so the ranging and position errors are correlated with the communication delay. The ranging error and position error after each iteration are calculated respectively:

(38) \begin{align}\left\{ \begin{array}{ll}{\mathbf{\textit{e}}}_{uwb}=\parallel {\mathbf{\textit{p}}}_{i}-{\hat{\mathbf{\textit{p}}}}_{j}\left(\delta \hat{t}\right)\parallel -{\mathbf{\textit{d}}}_{ij} {} \\[3pt] {\mathbf{\textit{e}}}_{p}=\parallel {\hat{\mathbf{\textit{p}}}}_{i}-{\mathbf{\textit{p}}}_{i}\parallel {} \end{array}\right. \end{align}

where, ${\hat{\mathbf{\textit{p}}}}_{j}\left(\delta \hat{t}\right)$ is the position delay compensation calculated by Equation (22) for this iteration delayed solution $\delta \hat{t}$ , ${\hat{\mathbf{\textit{p}}}}_{i}$ is the estimated position of this iteration, ${\mathbf{\textit{d}}}_{ij}$ and ${\mathbf{\textit{p}}}_{i}$ are the real distance and the real position, respectively. According to ${\mathbf{\textit{e}}}_{uwb}$ and ${\mathbf{\textit{e}}}_{p}$ , the joint loss function can be established as:

(39) \begin{align}\mathbf{\textit{L}}(\delta \hat{t})=\alpha {\mathbf{\textit{e}}}_{uwb}^{2}+\beta {\mathbf{\textit{e}}}_{p}^{2}+\gamma (\delta \hat{t}-\mu {)}^{2} \end{align}

where $\alpha $ and $\beta $ are the weights of distance measurement error and positioning error, respectively, and $0\lt \beta \lt \alpha \le 1$ . $\gamma $ is the delay prior weight ( $\gamma =1/{\sigma }^{2}$ for Gaussian distribution).

Calculate the gradient of the loss function with respect to the delay time:

(40) \begin{align} \boldsymbol{\nabla} \mathbf{\textit{L}}=2\alpha {\mathbf{\textit{e}}}_{uvb}\frac{\partial {\mathbf{\textit{e}}}_{uvb}}{\partial \delta \hat{t}}+2\beta {\mathbf{\textit{e}}}_{p}\frac{\partial {\mathbf{\textit{e}}}_{p}}{\partial \delta \hat{t}}+2\gamma (\delta \hat{t}-\mu ) \end{align}

Among them:

(41) \begin{align}\left\{\begin{array}{ll}\frac{\partial {\mathbf{\textit{e}}}_{uwb}}{\partial \delta \hat{t}}=\frac{({\mathbf{\textit{p}}}_{i}-{\hat{\mathbf{\textit{p}}}}_{j}{)}^{T}}{\parallel {\mathbf{\textit{p}}}_{i}-{\hat{\mathbf{\textit{p}}}}_{j}\parallel }\cdot ({\mathbf{v}}_{j}+{\mathbf{a}}_{j}\delta \hat{t}) {} \\[6pt] \frac{\partial {\mathbf{e}}_{p}}{\partial \delta \hat{t}}=\frac{\partial {\mathbf{\textit{p}}}_{i}}{\partial \delta \hat{t}}\cdot \frac{{\hat{\mathbf{\textit{p}}}}_{i}-{\mathbf{\textit{p}}}_{i}}{\parallel {\hat{\mathbf{\textit{p}}}}_{i}-{\mathbf{\textit{p}}}_{i}\parallel } {} \end{array}\right. \end{align}

For a more stable accelerated convergence of the delay estimates, the momentum term ${\mathbf{\textit{m}}}_{k}$ is set to denote an exponentially weighted moving average of the historical gradients:

(42) \begin{align}{\mathbf{\textit{m}}}_{k}=\eta \cdot {\mathbf{\textit{m}}}_{k-1}+(1-\eta )\cdot \boldsymbol{\nabla} \mathbf{\textit{L}} \end{align}

where $\eta $ is the momentum decay rate, usually $0\lt \eta \lt 1$ .

The learning rate is set to adaptively adjust the update step size, and the initial learning rate is set to:

(43) \begin{align}{\lambda }_{0}={\lambda }_{b}\cdot \frac{|\delta \hat{t}-\mu |}{\sigma } \end{align}

where ${\lambda }_{b}$ is the base learning rate, set to 0.1.

The learning rate parameter is adjusted according to the update trend of the loss function. If the loss decreases in two consecutive iterations, then: ${\lambda }_{k}=\mathrm{m}\mathrm{i}\mathrm{n}(1.1{\lambda }_{k-1},{\lambda }_{max})$ . If the loss value increases, then: ${\lambda }_{k}=\mathrm{m}\mathrm{a}\mathrm{x}(0.5{\lambda }_{k-1},{\lambda }_{min})$ , otherwise it remains unchanged.

In summary, the delay estimation update value is obtained:

(44) \begin{align}\delta {t}_{k+1}=\delta {t}_{k}-{\lambda }_{k}{m}_{k} \\[-25pt] \nonumber \end{align}

Table 1.

Parameters for each sensor

Table 1. Long description

A table with three rows and three columns, detailing parameters for each sensor. The columns are labeled Sensor, Parameter, and Value. The table includes data for three types of sensors: Gyroscope, Accelerometer, and UWB. For the Gyroscope, the parameters listed are Random constant/(°)·h⁻sup1; with a value of 0.02, White noise/(°)·h⁻sup1; with a value of 0.02, 1st-order Markov noise/(°)·h⁻sup1; with a value of 0.02, and Update frequency/Hz with a value of 50. For the Accelerometer, the parameters listed are 1st-order Markov bias error/g with a value of 0.0001 and Update frequency/Hz with a value of 50. For the UWB, the parameters listed are Ranging error/m with a value of 0.5 and Update frequency/Hz with a value of 5.

Figure 2.

UAV communication delay diagram.