Unmanned aerial vehicle formation collaborative navigation method based on open information fusion architecture

Abstract

The invention provides an unmanned aerial vehicle formation collaborative navigation method based on an open information fusion architecture, belongs to the field of unmanned aerial vehicle navigation, and aims to solve the navigation information fusion problem of asynchronous updating of multi-sensor data and available dynamic change of measurement information in a traditional formation collaborative navigation method. In the prior art, when asynchronous data fusion and measurement information availability change are coped, the defects of precision loss, poor flexibility and expansibility and the like exist. According to the method, a state variable and a measurement information set are constructed under an open architecture, a probability model of collaborative navigation state estimation is established based on Bayesian reasoning, and on this basis, a factor graph model is constructed by combining an incidence relation between a measurement model in posterior probability and prior information. By designing an independent measurement factor for each sensor, when sensor data is updated, flexible response to asynchronous data fusion and dynamic change of measurement information is realized by optimizing changed measurement factor nodes in real time and dynamically adding and deleting the nodes. And gradually minimizing an error cost function of each factor node through a nonlinear least square optimization method to realize efficient fusion of multi-sensor data and real-time updating of a navigation state. According to the method, the efficiency, flexibility and expansibility of asynchronous information fusion of the collaborative navigation system are remarkably improved, and reliable technical support is provided for unmanned aerial vehicle formation collaborative navigation.

Description

1. Technical Field

[0001] The present invention relates to a formation cooperative navigation method, in particular to a UAV formation cooperative navigation method based on an open information fusion architecture, belonging to the field of UAV navigation. 2. Prior Art

[0002] In the formation flight of UAVs, high-precision position information is the key to efficiently and reliably executing tasks. In formation cooperative flight, due to the cost and payload of navigation sensors, some formation members have problems with low positioning accuracy, which are prone to formation out-of-control, collision and crashing, restricting the success rate of tasks. In view of the above problems, the concept of UAV formation cooperative navigation is proposed, aiming to solve these challenges through inter-aircraft information sharing and coordination. "UAV Formation Cooperative Navigation Algorithm Based on Improved Particle Filter" (Yue Jingxuan, Wang Hongru, Zhu Dongqin, Acta Aeronautica et Astronautica Sinica, No. 14, 2023) proposed a cooperative navigation method based on improved particle filter, which jointly optimizes the measurement information of multiple sensors through resampling technology, improving the navigation accuracy. However, in cooperative navigation, the update frequencies of the measurement information of multiple sensors are inconsistent, and this method still loses accuracy when processing these asynchronous information. In addition, the dynamic changes of the formation will change the availability of relative measurement information, and the information fusion architecture of this method is difficult to adapt to these dynamic changes, lacking the support of sensor plug-and-play, restricting the scalability and adaptability of the system. 3. Summary of the Invention

[0003] The technical solution adopted by the present invention to solve its technical problems: a UAV formation cooperative navigation method based on an open information fusion architecture, comprising the following steps:

[0004] Step 1: Definition of state variables and measurement information set

[0005] In this method, the core of the UAV navigation system is the inertial measurement unit (IMU), which, as the basic navigation sensor, provides continuous motion state information for each UAV. At the same time, the system supports the plug-and-play of other relative and absolute navigation sensors to meet the requirements of different mission scenarios. On this basis, the state variables of the UAV are defined, and a unified expression mode of measurement information is established to ensure the fusion of different sensor data and the optimization calculation of subsequent state estimation.

[0006] The state of the UAV is defined as follows: The navigation state of UAV i at time t k is defined as where and respectively represent the position, velocity and attitude of UAV i at time t k , and the calibration parameters of the IMU are defined as is the zero bias of the accelerometer, and is the zero bias of the gyroscope. The navigation state and the calibration parameters of the IMU are uniformly defined as the state variables of the UAV:

[0007]

[0008] Considering that the formation members are equipped with multiple navigation devices, the measurement information set received by UAV i at time t k is defined as:

[0009]

[0010] where, is the measurement information of navigation sensor s of UAV i at time t k , and is the set of navigation measurement sensors carried by UAV members.

[0011] Step 2: Cooperative navigation state estimation probability model

[0012] In the cooperative navigation process of the UAV formation, in order to ensure that the navigation system can accurately estimate the real-time state of the UAV, the present invention adopts the maximum a posteriori probability estimation as the basic method for state update. This method fuses the prior information and the measurement data of multiple sensors, and can still stably obtain high-precision navigation states even in the case of dynamic changes and asynchronous updates of the sensor measurement information. The present invention uses Bayesian inference to model the maximum a posteriori state estimation as a probability density optimization problem, where the likelihood probability density reflects the accuracy of the measurement and describes the dynamic evolution process of the navigation state.

[0013] The problem of estimating the state variables of the formation members can be expressed as the problem of maximizing the posterior probability density estimation of the state variables in the case of obtaining the measurement information set , that is:

[0014]

[0015] where, is the UAV state of the maximum a posteriori probability estimation is the posterior probability density function, which represents the optimal estimation of the UAV state after obtaining all the measurement information.

[0016] According to Bayes' formula, the posterior probability can be decomposed into the prior probability of the state variables and the likelihood probability of the measurement information, that is:

[0017]

[0018] where, is the prior information of the state variable, which describes the dynamic changes of the navigation state. is the likelihood probability density of the measurement information, which describes the relationship between the data measured by the sensor and the state variable, and ∝ means proportional.

[0019] In Equation (4), the prior information of the state variable is determined by the inertial navigation state transition model, while the likelihood probability density of the measurement model is determined by the measurement models of other sensors (where is the set of other navigation sensors except the IMU).

[0020] When processing asynchronous measurement data from multiple sensors, the system uses the dynamic prediction provided by the IMU as the prior information to enable the state to maintain continuous evolution in a short period of time. After the new measurement data of other navigation sensors arrives, the system can asynchronously fuse sensor data with different frequencies by combining the prior information and the likelihood probability density of the measurement information. This process includes: the prediction of the IMU and the calculation of the likelihood probability density of the measurement information of other sensors. The detailed calculation process is as follows:

[0021] In the inertial navigation state transition model, the navigation state variable depends on the navigation state variable at the previous moment the measurement information provided by the IMU and the calibration parameters of the IMU Therefore, the prior information of the state variable can be written as:

[0022]

[0023] where, is the prior probability at the previous moment, is the transition probability of the navigation state variable under the condition of the previous moment, is the evolution model probability of the IMU calibration parameters between consecutive moments.

[0024] Since the measurement values of navigation sensors do not affect each other and the measurement information is independent of each other, the measurement model of each sensor can be separately modeled as a likelihood probability density function To ensure that the system can flexibly handle asynchronous data updates, the likelihood probability density of the measurement model can be expressed as:

[0025]

[0026] where, represents the measurement at time k excluding A set of measurement values of other navigation sensors Represents the measurement model Variables involved in

[0027] Substituting (5) and (6) into (4), we get:

[0028]

[0029] Step 3: Formation cooperative navigation model based on factor graph

[0030] To improve the computational efficiency of UAV formation cooperative navigation and enhance the flexibility of the system in a multi-sensor environment, the present invention uses a factor graph model for state estimation. The factor graph can intuitively represent the constraint relationship between state variables and sensor measurement values, ensuring that the system can efficiently optimize and calculate the available sensor information in real time. In this method, state variables are modeled as variable nodes, and sensor measurement information is mapped to factor nodes. The factor graph model realizes the optimal solution of state estimation by defining the association relationship between factor nodes and variable nodes.

[0031] According to the association relationship between factor nodes and variable nodes, expressing each term on the right side of (7) with factor nodes, the state estimation problem of the UAV can be modeled by a factor graph as follows:

[0032]

[0033] where f i pri is the prior factor node related to the k - 1 moment, f i IMU is the process factor node corresponding to the IMU, f i bias is the IMU bias factor node, f i v represents the measurement factor of other navigation sensors except the IMU, is the set of variable nodes related to the factor node f i v

[0034] In the factor graph model (8), each factor node only performs local calculations with its related state variable nodes. Therefore, when a new sensor is added, the system can automatically register the new measurement factor and optimize it in combination with the existing navigation state; when a certain sensor fails or is removed, its corresponding factor node will be automatically deleted, and other sensors can still maintain navigation calculations, ensuring the continuity and stability of the system. This dynamic factor management mechanism allows the system to achieve plug-and-play of sensors without interruption, ensuring real-time update of the navigation state, while improving the computational efficiency and robustness of the system. ​

[0035] Step 4: Dynamic Sensor Data Fusion and Error Modeling

[0036] To accurately describe the constraint relationship between the navigation state and sensor measurement data, based on the noise characteristics of navigation sensors, this invention constructs the cost function of factor nodes to quantify measurement errors and optimize state estimation. The construction of factor nodes ensures that the system can flexibly adjust and optimize the navigation state according to the data contributions of different sensors.

[0037] The cost function of each factor node is modeled according to its distribution error, and its general expression is as follows:

[0038]

[0039] where is the error function of the factor node (the deviation between the sensor measurement value and the state estimation), is the measurement data of navigation sensor s at time k, represents the squared Mahalanobis distance of vector e, which is used to measure the influence degree of measurement error under the sensor noise covariance matrix, Ξ s is the measurement noise covariance matrix of navigation sensor s, which is used to quantify measurement uncertainty.

[0040] According to the cost function (9) of the factor node, the cost functions corresponding to each factor node can be respectively expressed as follows:

[0041] 1) Prior factor It is used to constrain the smoothness of the current state of the UAV and the state at the previous moment to ensure the continuity of state estimation. Its corresponding cost function is:

[0042]

[0043] where is the state transition model of UAV i, Ξ pri is the process noise covariance matrix.

[0044] 2) IMU factor Based on the inertial measurement information provided by the IMU, it predicts the state of the UAV and is used for short-term state estimation. Its corresponding cost function is:

[0045]

[0046] where is the IMU measurement model of UAV i, Ξ IMU is the noise covariance matrix of the IMU.

[0047] 3) Due to the drift error of the IMU, it is necessary to use the bias factor node to correct the bias of the IMU. The bias factor related to IMU error correction The corresponding cost function is:

[0048]

[0049] where is the time propagation model of the IMU bias, and Ξ bias is the process covariance matrix.

[0050] 4) Similarly, the factors of other sensor measurement models are used to fuse the data of other sensors besides the IMU (such as GNSS, UWB, etc.) to improve the navigation accuracy. The corresponding cost function is:

[0051]

[0052] where is the measurement model of the other navigation sensor v carried by the UAV i, and Ξ v is the measurement noise covariance matrix corresponding to this sensor.

[0053] Step 5: Navigation state solution

[0054] In order to obtain the optimal navigation state estimation, the present invention adopts the nonlinear least squares optimization method to calculate the optimal solution of the cost function of each factor node and achieve the efficient fusion of sensor data.

[0055] Based on the state estimation problem constructed by the factor graph, substituting the factor node model in step 4 into (8) can finally be expressed as a least squares optimization problem:

[0056]

[0057] where is the optimal navigation state variable estimated at time k, which includes the position, velocity, attitude of the UAV i at time k and the bias correction parameters of the IMU.

[0058] Solve (14) through the nonlinear least squares optimization method, and gradually iterate to minimize the cost function of each factor node to update the navigation state Output the optimal navigation state through iterative solution 4. Description of the drawings

[0059] The present invention discloses a UAV formation cooperative navigation method based on an open information fusion architecture, aiming to solve the deficiencies of the existing formation cooperative navigation methods in terms of asynchronous information processing efficiency, dynamic adaptability, and system scalability. Appendix Figure 1 This is the flow chart of formation cooperative navigation based on the open information fusion architecture proposed by the present invention. The attached figure shows that starting from the definition of state variables and measurement information sets, the IMU prediction information and the likelihood ratios of other sensors are successively received, a cooperative navigation probability estimation model is constructed, and the flexible processing of multi-sensor measurement information is realized. Under the factor graph framework, a navigation graph structure composed of navigation state variable nodes and measurement factor nodes is established, and an independent measurement factor node model is designed to support the dynamic adjustment of sensor information. Finally, the optimal navigation state is obtained through non-linear least squares optimization. The information fusion architecture in this figure supports the plug-and-play of multi-source sensors, asynchronous information processing, and dynamic adjustment of members, improving the navigation accuracy and real-time performance of the formation system, and having good scalability and dynamic adaptability.

[0060] 5. Implementation Examples

[0061] For the UAV formation cooperative navigation method based on the open information fusion architecture of the present invention, the specific steps of the SINS / GPS / BAR / UWB formation cooperative navigation scheme based on the open information fusion architecture are designed as follows:

[0062] Step 1: Definition of State Variables and Measurement Information Sets

[0063] In the parallel formation configuration, the number of formation members is 15, but the number of formation members is dynamically adjusted according to mission requirements (such as UAV mission failure exit, new member addition, or formation reorganization). Each formation member is equipped with corresponding navigation equipment, including IMU (Inertial Measurement Unit): update frequency 100Hz, which can provide high-frequency acceleration and angular velocity information but accumulates errors over time. GNSS (Global Navigation Satellite System): update frequency 1Hz, which provides low-frequency but high-precision absolute position measurement and is vulnerable to occlusion. UWB (Ultra-Wideband Ranging): update frequency 10Hz, which can provide high-precision relative ranging information for relative navigation between formation members. BAR (Barometer): update frequency 5Hz, which can provide altitude information but is greatly affected by environmental air pressure changes.

[0064] The state of the UAV is defined as follows: The navigation state of UAV i at time t k is defined as where and respectively represent the position, velocity, and attitude of UAV i at time t k The calibration parameters of the IMU are defined as is the zero bias of the accelerometer, is the zero bias of the gyroscope. Therefore, the state variables of UAV i at time t k can be defined as:

[0065]

[0066] The measurement information set received by the unmanned aerial vehicle i at time t k is defined as:

[0067]

[0068] where, is the measurement information of the navigation sensor s of the unmanned aerial vehicle i at time t k , and is the set of navigation measurement sensors carried by the members of the unmanned aerial vehicle

[0069] Step 2: Cooperative navigation state estimation probability model

[0070] In the process of cooperative navigation of the unmanned aerial vehicle formation, in order to ensure that the navigation system can accurately estimate the real-time state of the unmanned aerial vehicle, the present invention adopts the maximum a posteriori probability estimation as the basic method for state update. The problem of estimating the state variables of the formation members is expressed as the problem of maximizing the posterior probability density estimation of the state variable under the condition of obtaining the measurement information set , that is:

[0071]

[0072] where, is the state of the unmanned aerial vehicle estimated by the maximum a posteriori probability estimation is the posterior probability density function, indicating the optimal estimation of the state of the unmanned aerial vehicle after obtaining all measurement information.

[0073] According to Bayes' formula, the posterior probability distribution function can be decomposed into a likelihood probability density of a measurement model and the prior information of the state variable , that is:

[0074]

[0075] where, is the prior information of the state variable, describing the dynamic change of the navigation state, is the likelihood probability density of the measurement information, describing the relationship between the data measured by the sensor and the state variable, and ∝ means proportional.

[0076] In equation (4), the prior information of the state variable is determined by the IMU high-frequency state transition model, while the likelihood probability density of the measurement model is determined by other low-frequency sensors (where, ) is determined by the measurement model. Since the update frequencies of different sensors are different, asynchronous processing is required when the system fuses data. The IMU provides state prediction at a frequency of 100Hz, and can provide short-term prediction when GNSS (1Hz), UWB (10Hz), and BAR (5Hz) data have not arrived, so that the state can maintain continuous evolution at a high time resolution. When GNSS, UWB, or BAR data arrives, the system is based on Bayesian inference, uses the IMU prediction as a prior, and combines the new measurement data to calculate the posterior probability density to achieve real-time state correction. This process includes: calculating the likelihood probability density of the IMU prediction and the measurement information of other low-frequency sensors. The detailed calculation process is as follows:

[0077] In the inertial navigation state transition model, the navigation state variable depends on the navigation state variable at the previous moment the measurement information provided by the IMU and the calibration parameters of the IMU Therefore, the prior information of the state variable can be written as:

[0078]

[0079] where is the prior probability at the previous moment, is the transition probability of the navigation state variable under the condition of the previous moment, is the evolution model probability of the IMU calibration parameters between consecutive moments.

[0080] Since the measurement values of the navigation sensors do not affect each other and the measurement information is independent of each other, the measurement model of each sensor can be separately modeled as a likelihood probability density function Then the likelihood probability density of the measurement model can be expressed as:

[0081]

[0082] where represents the measurement at time k excluding the measurement value sets of GNSS, UWB, and BAR other than , represents the variables involved in the measurement model .

[0083] Substituting (5) and (6) into (4), we get:

[0084]

[0085] Step 3: Formation cooperative navigation model based on factor graph

[0086] The state variables (including position, velocity, attitude, and IMU bias parameters) are modeled as variable nodes. The sensor measurement information (such as IMU, GNSS, UWB, BAR, etc.) is mapped to factor nodes, and each factor node establishes a constraint relationship with the corresponding measurement information. According to the association relationship between the factor nodes and the variable nodes, the terms on the right side in (7) are expressed in terms of factor nodes and can be further written as:

[0087]

[0088] where, f i pri is the prior factor node related to the k - 1 moment, f i IMU is the process factor node corresponding to the IMU, f i bias is the IMU bias factor node, f i GNSS , f i UWB , f i BRO are the measurement factors of the GNSS, UWB, and BRO sensors, is the set of variable nodes related to the factor nodes of GNSS, UWB, and BRO.

[0089] In the factor graph model (8), the factor graph performs local calculations for each factor node and its related variable nodes. When a new UAV joins the formation, the system can automatically add its GNSS, IMU, UWB, and BAR sensors and dynamically add the corresponding measurement factor nodes in the factor graph, enabling the new member to seamlessly access the navigation system. When a UAV exits the formation, the system will automatically delete the UWB relative ranging factor and the GNSS absolute positioning factor of this member and adjust the factor graph structure to avoid redundant calculations. When the UWB device fails, the system will remove the invalid relative ranging factor and rely on GNSS and IMU for navigation estimation to improve the robustness of the system. By dynamically adjusting the factor graph structure, under the conditions of changes in formation size, addition or deletion of sensors, or fault recovery, the navigation state estimation can still be updated in real time and calculated efficiently, thereby enhancing the flexibility of the system.

[0090] Step 4: Construction of factor nodes

[0091] The cost function of each factor node is modeled based on its distribution error, and its general expression is as follows:

[0092]

[0093] where, Denote the squared Mahalanobis distance of vector e, which is used to measure the influence degree of measurement error under the sensor noise covariance matrix, Ξ s is the measurement noise covariance matrix of navigation sensor s, which is used to quantify measurement uncertainty.

[0094] According to the cost function (9) of the factor node, the cost functions corresponding to each factor node can be expressed as follows:

[0095] 1) Prior factor The corresponding cost function is:

[0096]

[0097] Among them, is the state transition model of UAV i, Ξ pri The process noise covariance matrix, and the specific expression forms are as follows respectively

[0098]

[0099] Among them, represents the position, velocity, and attitude quaternion of UAV i at the previous moment, Δt is the time step of the system, The rotation matrix from the body frame to the inertial frame, and are the measured acceleration and angular velocity of the IMU respectively, is the zero bias of the accelerometer, is the zero bias of the gyroscope, exp(·) represents the exponential mapping of the quaternion, which is used to update the attitude, I 3×3 is the identity matrix, and the noise standard deviations are taken as σ p = 0.1m, σ v = 0.01m / s, σ q = 0.001rad,

[0100] 2) IMU factor The corresponding cost function is:

[0101]

[0102] Among them, is the IMU measurement model of UAV i, Ξ IMU is the noise covariance matrix of the IMU, and its specific expression form is as follows:

[0103]

[0104] Among them, the gravitational acceleration g = 9.807m / s 2 , the accelerometer noise standard deviation Gyroscope noise standard deviation

[0105] 3) Bias factors related to IMU error correction The corresponding cost function is:

[0106]

[0107] where is the time propagation model of the IMU carried by UAV i, and its specific form can be represented by its own non - linear model, Ξ bias is the process covariance matrix. Here, we assume that the drifts of the gyroscope and accelerometer are both constant drifts.

[0108] 4) The measurement functions and measurement noise covariance matrices of sensors GNSS, BRO, and UWB are taken as the following values respectively:

[0109] GNSS provides absolute position measurement values, and the measurement function and measurement noise covariance matrix are expressed as:

[0110]

[0111] where is the GNSS position information received by UAV i at time k, and the position noise standard deviation is taken as σ x = σ y = σ z = 0.5m.

[0112] The barometric altimeter calculates altitude by measuring air pressure, then the measurement function and measurement noise covariance matrix can be expressed as:

[0113]

[0114] where is the altitude information received by the barometric altimeter of UAV i at time k, and the UWB ranging noise standard deviation σ BRO = 0.5m.

[0115] UWB provides the relative distance between two nodes, and the measurement function and measurement noise covariance matrix can be expressed as:

[0116]

[0117] where is the relative distance information received by UAV i from UAV j at time k, and the UWB ranging noise standard deviation σ UWB = 0.2m

[0118] Step 5: Navigation state solution

[0119] The present invention uses non - linear least squares to solve the above collaborative navigation factor graph model, so as to achieve accurate estimation of the navigation state. Substituting the factor node model in step 4 into (8), the optimal estimation of the navigation state can be expressed as:

[0120]

[0121] where, is the navigation state variable calculated at time k, which includes the position, velocity, attitude of the UAV i at time k and the bias correction parameters of the IMU.

[0122] Solve (14) through the non - linear least squares optimization method, and gradually iterate to minimize the cost function of each factor node to update the navigation state Output the optimal navigation state through iterative solution

[0123] The parts not described in detail in the present invention belong to the common general knowledge of those skilled in the art.

Claims

1. A formation cooperative navigation method for aircraft based on an open information fusion architecture, characterized in that The described formation cooperative navigation method includes: 1). Define the state variables of the aircraft under an open architecture. The state variables include position, velocity, attitude, and calibration parameters of the inertial measurement unit, and define a measurement information set to support the plug-and-play of multiple types of sensors; 2). Based on the cooperative navigation states of the formation members, construct a probability model for state estimation, and decompose the joint posterior probability density of the state variables and the measurement information into the prior information of the state variables and the likelihood probability density of the measurement model to achieve asynchronous fusion and dynamic adaptation of multi-sensor data; 3). Convert the probability model into a factor graph model, map the navigation state variables to variable nodes, and map the measurement information to factor nodes, and realize the plug-and-play of navigation information through the real-time update of the factor graph; 4). In view of the noise characteristics of the navigation sensor measurement information, construct a cost function for the factor nodes, and the cost function is calculated based on the error function and the measurement noise covariance matrix; 5). Based on the factor graph model, use the nonlinear least squares optimization method to recursively solve the navigation state variables, and gradually minimize the error cost functions of each factor node to achieve real-time update of the navigation state.

2. The formation cooperative navigation method according to claim 1, characterized in that, Definition of UAV state variables and measurement information set: The navigation state of UAV i at time t k is defined as where and represent the position, velocity and attitude of UAV i at time t k respectively. The calibration parameters of the IMU are defined as is the zero bias of the accelerometer, is the zero bias of the gyroscope. Therefore, the state variables of UAV i at time t k can be defined as: Considering that formation members are equipped with a variety of navigation measurement devices, the measurement information set received by UAV i at time t k is defined as: Among them, is the measurement information of the navigation sensor s of the UAV i at time t k and is the set of navigation measurement sensors carried by the UAV members.

3. The formation cooperative navigation method according to claim 1, wherein The established cooperative navigation state estimation probability model, based on Bayesian inference, decomposes the posterior probability of the navigation state into the likelihood probability density of the measurement model and the prior information of the state variables, and then constructs a probability estimation model for the navigation system. This model can flexibly handle the asynchronous update of multi-sensor data, ensure the consistency of the global state, and adapt to the change of measurement information by dynamically adjusting the measurement. The problem of estimating the navigation state variables of the formation members can be expressed as: wherein, is the maximum a posteriori estimate of the state variable , and is the joint posterior probability density function. The posterior probability distribution function according to Bayes' formula can be decomposed into the likelihood probability density of a measurement model and the prior information of the state variable Therefore it can be further expressed as: wherein, is the prior probability at the previous moment, is the transition probability of the navigation state variable under the condition of the previous moment, is the evolution model probability of the IMU calibration parameter between consecutive moments, is the measurement likelihood probability of other sensors except the IMU, represents the measurement at time k excluding from the set of measurement values of other navigation sensors in represents the variables involved in 4. The formation cooperative navigation method according to claim 1, wherein Convert the navigation state probability estimation model into a factor graph model. According to the association relationship between the factor nodes and the variable nodes, convert the posterior probability density function into the form of a factor graph. This model ensures the rapid integration of asynchronous update data and updates the navigation state in real time according to the availability of each sensor. The obtained factor graph model is as follows: Among them, f i pri is the prior factor node related to the (k - 1)th moment, f i IMU is the process factor node corresponding to the IMU, f i bias is the IMU bias factor node, f i v represents other measurement model factors except the IMU, is the factor node f i v associated variable node set.

5. The formation cooperative navigation method according to claim 1, wherein In view of the noise characteristics of the navigation sensor measurement information, construct a cost function for the factor nodes, and use this as the objective function for optimizing the navigation state estimation. The cost function is defined as a weighted square form of the error function by describing the error relationship between the measurement value and the state variables and combining the measurement noise covariance matrix of the navigation sensor. The specific form is as follows: The present invention combines the noise characteristics of the navigation sensor and adopts the following cost function form for the factor nodes: Among them, represents the squared Mahalanobis distance of vector e, and Ξ s is the measurement noise covariance matrix of navigation sensor s.

6. The formation cooperative navigation method according to claim 1, wherein Based on the factor graph model, use the nonlinear least squares optimization method to recursively solve the navigation state variables, and gradually minimize the cost functions of each factor node. The estimated navigation state variables can be expressed as: Among them, is the navigation state variable calculated at time k, which includes the position, velocity, attitude of UAV i at time k, and the bias correction parameters of the IMU. Solve (7) through the nonlinear least squares optimization method, gradually minimize the error cost functions of each factor node, achieve efficient fusion of multi-sensor asynchronous data, and update the navigation state in real time.

Images