A cooperative fault-tolerant positioning method based on adaptive residual model interaction

Abstract

The application discloses a kind of based on adaptive residual model interaction's cooperative fault-tolerant positioning method, based on the thought of model mixture, the single fusion information of each sensor is formed into submodel;And using interactive multiple models to each submodel is carried out residual interaction, the model of fault sensor is switched off, avoid introducing larger positioning error in the process of cooperative positioning;Finally, according to model state probability fusion obtains the optimal state estimation of cooperative positioning;In order to improve model switching capability, the Markov transition probability estimation of adaptive parameter adjustment is introduced into the framework of interactive multiple models, so as to avoid the situation that model switching fails and causes the decline of cooperative fault-tolerant performance.The application can be used for satellite restricted and unknown complex unmanned aerial vehicle cluster in susceptible to interference environment cooperative fault-tolerant positioning.

Description

Technical Field

[0001] This invention belongs to the field of navigation, positioning and fault diagnosis, and specifically relates to a collaborative fault-tolerant positioning method based on adaptive residual model interaction. Background Technology

[0002] In recent years, with the vigorous development of unmanned and intelligent autonomous technologies, the flight missions of unmanned aerial vehicles (UAVs) have gradually shifted from single-UAV autonomous flight to multi-UAV swarm formation autonomous flight. In the military field, multi-UAV swarm formation coordinated flight can effectively overcome the problems of limited mission capability and insufficient damage resistance when relying on a single UAV in complex battlefield flight environments. Currently, UAV formations mainly use satellite navigation systems such as the Global Positioning System (GPS) and the BeiDou Navigation Satellite System to provide absolute and relative navigation information for UAVs, resulting in high positioning accuracy. However, low-altitude UAVs are highly susceptible to environmental obstruction and human interference. Incorrect positioning information can cause the entire UAV formation to collapse at low altitudes. Therefore, it is crucial to promptly identify, isolate, and recover from potential faults in sensors such as GPS and data link ranging systems to ensure the mission execution capability of multiple unmanned devices.

[0003] The application of fault diagnosis technology can reduce the risks during system operation, thereby improving the stability, reliability, and safety of the system. Research on fault diagnosis technology has become a hot topic in recent years. By monitoring the system status in real time, fault diagnosis technology can detect the time of fault occurrence, confirm the location of the fault, and estimate the magnitude of the fault. Meanwhile, based on navigation systems and various sensor devices, each UAV in a formation can obtain the position and environmental information of the target it is following, thus completing the formation flight mission. Therefore, if one or more UAVs in the formation malfunction and are not dealt with in time, the seriously faulty UAV will continue to fly with the other UAVs in the formation, directly affecting the safety of the entire formation flight. In this situation, in order to detect the severity of the fault in a timely manner, research on fault diagnosis for UAV formation flight control systems is of paramount importance. However, in existing research, fault diagnosis technology is mainly used for single systems; further research is needed on fault diagnosis technology for the entire UAV formation flight control system.

[0004] Interactive multi-model (IMM) is a "soft-switching" approach for hybrid systems, allowing switching between multiple possible models to address system uncertainties. IMM algorithms are often considered an effective method for estimating the state of systems with uncertain structures and parameters or those exhibiting parameter variations. In fault detection, IMM can be extended to combinations of Kalman filters with different measurement equations. When the structure of the measurement system changes, IMM can be applied to detect and isolate faults. Therefore, researching cooperative fault-tolerant positioning within the IMM framework to improve the robustness and accuracy of UAV swarm cooperative navigation in satellite-constrained and unknown, complex, and easily interfered environments has significant scientific and applied value. Summary of the Invention

[0005] Purpose of the invention: In order to solve the problems of poor robustness and inaccurate navigation in existing UAV swarm cooperative navigation methods under satellite rejection and unknown complex environments, this invention proposes a cooperative fault-tolerant positioning method based on adaptive residual model interaction.

[0006] Technical solution: The collaborative fault-tolerant localization method based on adaptive residual model interaction described in this invention includes the following steps:

[0007] (1) Initialize the error state vector and covariance matrix of the Kalman filter to provide initial conditions for the filtering process;

[0008] (2) Perform Markov transition probability estimation and model probability initialization for adaptive interactive multi-model;

[0009] (3) Using the current state and system noise, predict the state vector and covariance matrix of the next time step to complete the prediction of Kalman filtering;

[0010] (4) Establish a relative distance model between the UAV and the cooperating UAV, and update the UWB measurement based on the distance measurement results and the relative position information of the UAV;

[0011] (5) Based on the positioning information provided by GNSS, GNSS data and inertial navigation information are fused, and measurement updates are performed by establishing a GNSS measurement model;

[0012] (6) Use the fused UWB and GNSS information as sub-models, perform model interaction, and calculate the mixed state estimate and mixed state covariance of all sub-models;

[0013] (7) Calculate the measurement residuals and corresponding covariances of all sub-models, and then construct the maximum likelihood function to update the model probability of each sub-model;

[0014] (8) The final error state estimate and state covariance are obtained by weighting the model probabilities of each sub-model.

[0015] (9) Update the Markov transition probability estimate for each sub-model in real time;

[0016] (10) Correct the inertial navigation positioning error, output the cooperative positioning result in the strapdown inertial navigation solution platform, and transmit the corrected positioning information to other UAV nodes through cooperative communication. Finally, return to step (3) to start the next cycle.

[0017] Further, the error state vector mentioned in step (1) is:

[0018]

[0019] Among them, the error state variable X has 18 dimensions; ω and δv represent the 3D inertial navigation platform angle error and 3D inertial navigation platform velocity error in the three directions of northeast, south, and east; δL, δλ, and δh represent the latitude error, longitude error, and altitude error, respectively; ε b and ε r These represent the 3D gyroscope constant drift error and the first-order Markov drift error in three directions, respectively. It's an accelerometer error.

[0020] Furthermore, the Markov transition probability estimate and model probability of the adaptive interactive multi-model described in step (2) are as follows:

[0021]

[0022] ρ=[ρ g ρ a ]

[0023] Where k represents the time of the model, Ψ is the Markov transition probability, and Ψ g→g (k) represents the probability that the GNSS submodel can maintain its own model at time k; Ψ g→a For example, (k) represents the transition probability from the GNSS submodel to the UWB submodel at time k, ρ g and ρ a These are the model probabilities for the GNSS sub-model and the UWB sub-model, respectively.

[0024] Furthermore, the implementation process of step (3) is as follows:

[0025]

[0026] in, Let F(k) be the state-time update equation, k denote time step, X(k) be the 18-dimensional error state variable, F(k) be the state transition matrix, G(k) be the error coefficient matrix, and W(k) be the 9-dimensional noise state variable; INSThe system matrix corresponding to the 9-dimensional basic navigation parameters is obtained through the inertial navigation system error equation; T g T a These are the relevant times for the gyroscope and the clock, respectively.

[0027] Furthermore, the relative distance model described in step (4) is as follows:

[0028]

[0029] Where, ||r ij ||2 represents the vector r representing the true relative position of drones i and j. ij The second norm, For ultra-wideband observation noise; the measurement equation for UWB is defined as:

[0030]

[0031] in, It is the observation vector of UAV i and UWB ranging. It is the observation matrix for UWB ranging, V i uwb This is the measurement noise in UWB ranging; The mathematical expression is:

[0032]

[0033] Among them, e j1 e j2 e j3 It is the direction cosine of drone i to other drone nodes.

[0034] Furthermore, the implementation process of step (5) is as follows:

[0035]

[0036] in, This represents the GNSS observation vector of UAV i. V represents the GNSS observation matrix of UAV i. i GNSS Measurement noise for GNSS positioning; L, λ, and h represent the longitude, latitude, and altitude of the UAV, respectively; the GNSS observation vector and observation matrix of UAV i are defined as:

[0037]

[0038] Among them, R M R N This represents the radius of curvature of the meridional circle and the radius of curvature of the tropotropic circle calculated by inertial navigation.

[0039] Furthermore, the mixed-state estimation and mixed-state covariance described in step (6) are as follows:

[0040]

[0041]

[0042] Wherein, the subscript g represents the GNSS submodel and the subscript a represents the UWB submodel; and P m (k-1|k-1) represent the state estimate and state covariance of sub-model m at time k-1, respectively, and u m→n (k-1|k-1) is the interaction transition probability of submodel m to submodel n at time k-1.

[0043] Furthermore, the implementation process of step (7) is as follows:

[0044]

[0045] S n (k)=H n (k)P n (k|k-1)H n (k) T +R n (k)

[0046] Among them, L n (k) is the maximum likelihood function, Γ n (k) represents the measurement residual of sub-model n, S n (k) represents the covariance corresponding to the measurement residuals of sub-model n;

[0047] The mathematical expression for model probability update is:

[0048]

[0049] Where C(k) is the normalization parameter.

[0050] Furthermore, the final error state estimate and state covariance described in step (8) are:

[0051]

[0052] in, For the final error state estimation, This represents the corresponding state covariance.

[0053] Furthermore, the Markov transition probability estimate described in step (9) is updated as follows:

[0054]

[0055] in, L is the adaptive adjustment factor at time k; m (k) represents the maximum likelihood value of the m sub-model at time k.

[0056] Beneficial Effects: Compared with existing technologies, the beneficial effects of this invention are as follows: Based on the concept of model hybridization, this invention combines the single fused information of each sensor into a sub-model, and uses interactive multi-model to perform residual interaction between the sub-models, switching off the model of the faulty sensor, thus avoiding the introduction of large positioning errors during the cooperative positioning process. Finally, the optimal state estimate for cooperative positioning is obtained by fusing the model state probabilities. To improve the model switching capability, an adaptive parameter adjustment Markov transition probability estimation is introduced into the interactive multi-model framework to avoid the situation where model switching failure leads to a decrease in cooperative fault tolerance performance. This invention can be used for cooperative fault tolerance positioning of UAV swarms in satellite-constrained and unknown, complex, and easily interfered environments. Attached Figure Description

[0057] Figure 1 This is a flowchart of the present invention;

[0058] Figure 2 This is a framework diagram of the interaction of the adaptive residual model;

[0059] Figure 3 It is a map of the movement trajectory of a drone swarm;

[0060] Figure 4 This is a graph showing the variation of GNSS observation fault errors;

[0061] Figure 5 This is a graph showing the variation of fault error observed by UWB.

[0062] Figure 6 This is a graph showing the variation of the root mean square error obtained by the UAV using this invention.

[0063] Figure 7 This is the cumulative error distribution map obtained by the UAV using this invention. Detailed Implementation

[0064] The present invention will now be described in further detail with reference to the accompanying drawings.

[0065] like Figure 1 , Figure 2 As shown, this invention proposes a collaborative fault-tolerant localization method based on adaptive residual model interaction, which specifically includes the following steps:

[0066] Step 1: Initialize the Kalman filter by initializing the error state vector and covariance matrix of the Kalman filter to provide initial conditions for the subsequent filtering process.

[0067] The error state vector is:

[0068]

[0069] Among them, the error state variable X has 18 dimensions, ω and δv are the 3D inertial navigation platform angle error and 3D inertial navigation platform velocity error in the three directions of northeast, zenith, and yaw, respectively, δL, δλ, and δh represent latitude error, longitude error, and altitude error, respectively, and ε b and ε r These represent the 3D gyroscope constant drift error and the first-order Markov drift error in three directions, respectively. It's an accelerometer error.

[0070] Step 2: Perform Markov transition probability estimation and model probability initialization for adaptive interactive multi-model.

[0071] The Markov transition probability estimates and model probabilities for the adaptive interactive multi-model approach are as follows:

[0072]

[0073] ρ=[ρ g ρ a ]

[0074] Where Ψ is the Markov transition probability, and Ψ g→a For example, (k) represents the transition probability from the Global Navigation Satellite System (GNSS) submodel to the Ultra-Wideband (UWB) submodel at time k, Ψ g→g (k) represents the probability that the GNSS submodel can maintain its own model at time k; ρ g and ρ a These are the model probabilities for the GNSS sub-model and the UWB sub-model, respectively.

[0075] Step 3: Perform state-time updates to complete the prediction step of the Kalman filter.

[0076] Based on the definition of the system equation in the extended Kalman filter, the state vector and covariance matrix of the next time step are predicted using the current state and system noise. Here, the state-time update equation is:

[0077]

[0078] Where X(k) is the 18-dimensional error state variable of the system at time k, F(k) is the state transition matrix at time k, G(k) is the error coefficient matrix at time k, and W(k) is the 9-dimensional noise state variable of the system at time k. The mathematical expression of the state transition matrix is:

[0079]

[0080] Among them, F INS The system matrix corresponding to the 9-dimensional basic navigation parameters can be obtained through the inertial navigation system error equation. sg The mathematical expression is:

[0081]

[0082] F IMU The mathematical expression is:

[0083]

[0084] Among them, T g T a These represent the correlation times of the gyroscope and the adder, respectively. The mathematical expression for the error coefficient matrix G(k) is:

[0085]

[0086] Step 4: Construct an ultra-wideband (UWB) measurement update model.

[0087] Distance measurement was performed using UWB to establish a relative distance model between the UAV and its collaborating UAVs. The UWB measurements were then updated based on the distance measurement results and the relative position information of the UAVs. The UWB relative distance model is as follows:

[0088]

[0089] Where, ||r ij ||2 represents the vector r representing the true relative position of drones i and j. ij The second norm, This refers to the observation noise in UWB. The measurement equation for UWB can be defined as:

[0090]

[0091] in, It is the observation vector of UAV i and UWB ranging. It is the observation matrix for UWB ranging, V i uwb It is the measurement noise of UWB ranging. The mathematical expression is:

[0092]

[0093] Among them, e j1 e j2 e j3 It is the direction cosine of drone i to other drone nodes.

[0094] Step 5: Construct a Global Navigation Satellite System (GNSS) measurement update model.

[0095] Based on the positioning information provided by the GNSS module, GNSS data and inertial navigation information are fused, and measurements are updated by establishing a GNSS measurement model. The GNSS measurement equation is:

[0096]

[0097] in, This represents the GNSS observation vector of UAV i. V represents the GNSS observation matrix of UAV i. i GNSS Measurement noise for GNSS positioning. Here, L, λ, and h are defined as the longitude, latitude, and altitude of the UAV, respectively. Therefore, the GNSS observation vector and observation matrix of UAV i can be defined as:

[0098]

[0099] Where R M R N This represents the radius of curvature of the meridional circle and the radius of curvature of the tropotropic circle calculated by inertial navigation.

[0100] Step 6: Obtain the fused UWB and GNSS information through the measurement updates in Steps 4 and 5. Use the fused UWB and GNSS information as sub-models and perform a model interaction process to calculate the mixture state estimate and mixture state covariance for all sub-models. The mixture state estimate and mixture state covariance are:

[0101]

[0102] Wherein, the subscript g represents the GNSS submodel and the subscript a represents the UWB submodel; and P m (k-1|k-1) represent the state estimate and state covariance of sub-model m at time k-1, respectively, and u m→n (k-1|k-1) is the interactive transition probability (ITP) of submodel m to submodel n at time k-1.

[0103] u m→n The mathematical expression for (k-1|k-1) is:

[0104]

[0105] The mathematical expression is:

[0106]

[0107] Step 7: After step 6, the model probability update process is required. First, the measurement residuals and corresponding covariances of all sub-models are calculated. This process is implemented in steps 4 and 5. Then, the maximum likelihood function is constructed to update the model probability of each sub-model. The maximum likelihood function is:

[0108]

[0109] Among them, Γ n (k) represents the measurement residual of sub-model n, S n (k) represents the covariance corresponding to the measurement residuals of sub-model n.

[0110] Γ n The mathematical expression for (k) is:

[0111]

[0112] S n The mathematical expression for (k) is:

[0113] S n (k)=H n (k)P n (k|k-1)H n (k) T +R n (k)

[0114] The mathematical expression for model probability update is:

[0115]

[0116] Where C(k) is the normalization parameter.

[0117] The mathematical expression for C(k) is:

[0118]

[0119] Step 8: Following the model probability update in Step 7, the real-time model probability at that moment can be obtained. Changes in the model probability can isolate fault information in sensor measurements. First, a model state fusion process is performed, calculating the final error state estimate and state covariance based on the weighted probability of each sub-model. The final error state estimate and state covariance are:

[0120]

[0121] in, For the final error state estimation, This represents the corresponding state covariance.

[0122] Step 9: Update the Markov transition probability estimates of the sub-model in real time.

[0123] The submodel likelihood function is determined by the current submodel measurement residuals and their covariance; therefore, it effectively reflects system performance. Thus, the submodel likelihood function is used here to construct the adaptive adjustment factor, and the Markov transition probability estimate is updated as follows:

[0124]

[0125] in, The adaptive adjustment factor at time k; The mathematical expression is:

[0126]

[0127] Among them, L m (k) represents the maximum likelihood value of the m sub-model at time k.

[0128] Step 10: Correct the inertial navigation positioning error, output the cooperative positioning result in the strapdown inertial navigation solution platform, and transmit the corrected positioning information to other UAV nodes through cooperative communication. Finally, return to step 3 to start the next loop.

[0129] To verify the correctness and effectiveness of the present invention, the above implementation steps were verified using the present invention on the Matlab computing platform. Figure 3 This is a map showing the movement trajectory of a drone swarm. Figure 4 and Figure 5 This simulation illustrates the error changes when random faults occur in GNSS and UWB observations, a type of fault that frequently occurs in real-world observation scenarios. Figure 6 The error map of the three-dimensional position result obtained by the UAV using the present invention is provided by Figure 6 As can be seen, the present invention can accurately calculate the three-dimensional position of the UAV, so that the calculation error of the UAV's three-dimensional position can be kept within 2m. When GNSS or UWB observation fails, the present invention can detect and isolate the fault in time, avoiding large position calculation errors caused by the UAV positioning. Figure 7 The cumulative error distribution map obtained by the UAV using this invention is provided by... Figure 7 The results show that the cumulative error in the UAV position calculation is basically within 2m, indicating that the present invention has high robustness and fault tolerance.

[0130] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A collaborative fault-tolerant localization method based on adaptive residual model interaction, characterized in that, Includes the following steps: (1) Initialize the error state vector and covariance matrix of the Kalman filter to provide initial conditions for the filtering process; (2) Perform Markov transition probability estimation and model probability initialization for adaptive interactive multi-model; (3) Using the current state and system noise, predict the state vector and covariance matrix of the next time step to complete the prediction of Kalman filtering; (4) Establish a relative distance model between the UAV and the cooperating UAV, and update the UWB measurement based on the distance measurement results and the relative position information of the UAV; (5) Based on the positioning information provided by GNSS, GNSS data and inertial navigation information are fused, and measurement updates are performed by establishing a GNSS measurement model; (6) Use the fused UWB and GNSS information as sub-models, perform model interaction, and calculate the mixed state estimate and mixed state covariance of all sub-models; (7) Calculate the measurement residuals and corresponding covariances of all sub-models, and then construct the maximum likelihood function to update the model probability of each sub-model; (8) The final error state estimate and state covariance are obtained by weighting the model probabilities of each sub-model. (9) Update the Markov transition probability estimate for each sub-model in real time; (10) Correct the inertial navigation positioning error, output the cooperative positioning result in the strapdown inertial navigation solution platform, and transmit the corrected positioning information to other UAV nodes through cooperative communication. Finally, return to step (3) to start the next cycle.

2. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The error state vector mentioned in step (1) is: Among them, the error state variable X has 18 dimensions; ω and δv represent the 3D inertial navigation platform angle error and 3D inertial navigation platform velocity error in the three directions of northeast, south, and east; δL, δλ, and δh represent the latitude error, longitude error, and altitude error, respectively; ε b and ε r These represent the 3D gyroscope constant drift error and the first-order Markov drift error in three directions, respectively.

3. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The Markov transition probability estimate and model probability of the adaptive interactive multi-model described in step (2) are as follows: p=[p g r a ] Where k represents the time of the model, Ψ is the Markov transition probability, and Ψ g→g (k) represents the probability that the GNSS submodel can maintain its own model at time k; Ψ g→a For example, (k) represents the transition probability from the GNSS submodel to the UWB submodel at time k, ρ g and ρ a These are the model probabilities for the GNSS sub-model and the UWB sub-model, respectively.

4. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The implementation process of step (3) is as follows: in, Let F(k) be the state-time update equation, k denote time step, X(k) be the 18-dimensional error state variable, F(k) be the state transition matrix, G(k) be the error coefficient matrix, and W(k) be the 9-dimensional noise state variable; INS The system matrix corresponding to the 9-dimensional basic navigation parameters is obtained through the inertial navigation system error equation; T g T a These are the relevant times for the gyroscope and the clock, respectively.

5. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The relative distance model described in step (4) is as follows: Where, ||r ij ||2 represents the vector r representing the true relative positions of drones i and j. ij The second norm, For ultra-wideband observation noise; the measurement equation for UWB is defined as: in, It is the observation vector of UAV i and UWB ranging. It is the observation matrix for UWB ranging, V i uwb This is the measurement noise in UWB ranging; The mathematical expression is: Among them, e j1 e j2 e j3 It is the direction cosine of the distance from drone j to other drone nodes.

6. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The implementation process of step (5) is as follows: in, This represents the GNSS observation vector of UAV i. V represents the GNSS observation matrix of UAV i. i GNSS Measurement noise for GNSS positioning; L, λ, and h represent the longitude, latitude, and altitude of the UAV, respectively; the GNSS observation vector and observation matrix of UAV i are defined as: Among them, R M R N These represent the meridional radius of curvature and the trochanteric radius of curvature calculated by inertial navigation.

7. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The mixed state estimation and mixed state covariance mentioned in step (6) are as follows: Wherein, the subscript g represents the GNSS submodel and the subscript a represents the UWB submodel; and P m (k-1|k-1) represent the state estimate and state covariance of sub-model m at time k-1, respectively, and u m→n (k-1|k-1) is the interaction transition probability of submodel m to submodel n at time k-1.

8. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The implementation process of step (7) is as follows: S n (k)=H n (k)P n (k|k-1)H n (k) T +R n (k) Among them, L n (k) is the maximum likelihood function, Γ n (k) represents the measurement residual of sub-model n, S n (k) represents the covariance corresponding to the measurement residuals of sub-model n; The mathematical expression for model probability update is: Where C(k) is the normalization parameter.

9. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The final error state estimate and state covariance mentioned in step (8) are as follows: in, For the final error state estimation, This represents the corresponding state covariance.

10. The collaborative fault-tolerant localization method based on adaptive residual model interaction according to claim 1, characterized in that, The Markov transition probability estimate described in step (9) is updated as follows: in, L is the adaptive adjustment factor at time k; m (k) represents the maximum likelihood value of the m sub-model at time k.

Images