Cooperative fault-tolerant positioning method based on self-adaptive residual error model interaction

Abstract

The invention discloses a cooperative fault-tolerant positioning method based on self-adaptive residual model interaction, and the method comprises the steps: enabling single fusion information of each sensor to form a sub-model based on a model mixing thought; residual interaction is carried out on each sub-model by using an interactive multi-model, and the model of the fault sensor is switched, so that a relatively large positioning error is prevented from being introduced in a cooperative positioning process; and finally, obtaining optimal state estimation of cooperative positioning according to model state probability fusion. In order to improve the model switching capability, Markov transition probability estimation of adaptive parameter adjustment is introduced into an interactive multi-model framework, so that the situation that the cooperative fault-tolerant performance is reduced due to model switching failure is avoided. The method can be used for cooperative fault-tolerant positioning of the unmanned aerial vehicle cluster in a limited satellite and unknown complex and easily-interfered environment.

Description

Technical Field

[0001] The present invention belongs to the field of navigation positioning and fault diagnosis, and in particular relates to a collaborative fault-tolerant positioning method based on adaptive residual model interaction. Background Art

[0002] In recent years, with the vigorous development of unmanned technology and intelligent autonomous technology, the flight mission of drones has gradually developed from single drone autonomous flight to multi-drone cluster formation autonomous flight. In the military field, the coordinated flight of multi-drone cluster formation can effectively overcome the problems of limited ability to perform tasks and insufficient damage resistance of relying solely on a single drone in a complex battlefield flight environment. At present, drone formations mainly use satellite navigation systems such as the Global Positioning System and the Beidou Satellite Navigation System to provide absolute and relative navigation information for drones, with high positioning accuracy. However, low-altitude drones are extremely susceptible to environmental occlusion and human interference. Incorrect positioning information will cause the entire drone formation configuration to collapse at low altitude. Therefore, it is particularly important to promptly identify, isolate and recover possible faults in sensors such as the global positioning system and data link ranging, so as to ensure the mission execution of multiple unmanned equipment.

[0003] Through the application of fault diagnosis technology, the risk of system operation can be reduced, thereby improving the stability, reliability and safety of the system. The relevant research on fault diagnosis technology has become a hot topic of concern in recent years. By real-time detection of the system status, fault diagnosis technology can detect the time when the fault occurs, confirm the location of the fault, and estimate specific information such as the size of the fault. At the same time, based on the navigation system and a variety of sensor equipment, each drone in the UAV formation can obtain the position and environmental information of the target to follow and complete the formation flight mission. Therefore, if one or more drones in the formation fail, if they cannot be handled in time, the drone with serious faults will still fly together with the drones in the formation, which will directly affect the safety of the entire formation flight. In this case, in order to be able to detect the severity of the fault in time, the fault diagnosis research on the UAV formation flight control system is a top priority. In the existing research results, fault diagnosis technology is mainly used for a single system, and the fault diagnosis technology for the entire UAV formation flight control system needs to be further studied.

[0004] The interactive multi-model method is a "soft switching" method for hybrid systems, which can switch between multiple possible models to solve system uncertainties. Multi-model algorithms are often regarded as an effective method to solve the state estimation problem of systems with uncertain structures and parameters or changing parameters. In fault detection, the interactive multi-model can be extended to the case of Kalman filter combinations with different measurement equations. When the structure of the measurement system changes, the interactive multi-model method can be applied to detect and isolate faults. Therefore, it is of great scientific and application value to study the collaborative fault-tolerant positioning under the framework of interactive multi-models and improve the robustness and accuracy of cooperative navigation of UAV clusters in satellite-constrained and unknown complex and susceptible to interference environments. 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 collaborative navigation methods in satellite denial and unknown complex environments, the present invention proposes a collaborative fault-tolerant positioning method based on adaptive residual model interaction.

[0006] Technical solution: The collaborative fault-tolerant positioning method based on adaptive residual model interaction described in the present invention comprises 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) Estimating the Markov transition probability and initializing the model probability of the adaptive interacting 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 Kalman filter prediction;

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

[0011] (5) Based on the positioning information provided by GNSS, the GNSS data is integrated with the inertial navigation information, and the measurement update is performed by establishing a GNSS measurement model;

[0012] (6) Taking the UWB and GNSS fusion information as sub-models, model interaction is performed to calculate the mixed state estimation and mixed state covariance of all sub-models;

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

[0014] (8) Perform weighted calculation based on the model probability of each sub-model to obtain the final error state estimate and state covariance;

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

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

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

[0018]

[0019] Among them, the error state quantity X has a total of 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 sky; δL, δλ, δh represent the latitude error, longitude error and height error respectively; ε b and ε r Represent the 3D gyro constant drift error and the first-order Markov drift error in three directions respectively; is the accelerometer error.

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

[0021]

[0022] ρ=[ρ g ρ a ]

[0023] Among them, k represents the time of the model, Ψ is the Markov transition probability, Ψ g→g (k) represents the probability that the GNSS sub-model can remain in its own model at time k; Ψ g→a (k) as an example, it represents the transition probability from the GNSS sub-model to the UWB sub-model at time k, ρ g and ρ a are the model probabilities of the GNSS sub-model and the UWB sub-model respectively.

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

[0025]

[0026] in, is the state time update equation, k represents the time, X(k) is the 18-dimensional error state quantity, F(k) is the state transfer matrix, G(k) is the error coefficient matrix, W(k) is the 9-dimensional noise state quantity; F INSis the system matrix corresponding to the 9-dimensional basic navigation parameters, obtained through the inertial navigation system error equation; T g 、T a They are the relevant times of the gyro and the addition table, respectively.

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

[0028]

[0029] Among them, ||r ij ||2 represents the real relative position vector r of drones i and j ij The second-order norm of is the observation noise of ultra-wideband; the measurement equation of UWB is defined as:

[0030]

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

[0032]

[0033] Among them, e j1 、e j2 、e j3 is the direction cosine from UAV i to other UAV nodes.

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

[0035]

[0036] in, represents the GNSS observation vector of UAV i, represents the GNSS observation matrix of UAV i, V i GNSS is the measurement noise of 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 It represents the meridian curvature radius and the meridian curvature radius calculated by inertial navigation.

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

[0040]

[0041]

[0042] Among them, the subscript g represents the GNSS submodel, and the subscript a represents the UWB submodel; and P m (k-1|k-1) are the state estimation and state covariance of sub-model m at time k-1, u m→n (k-1|k-1) is the interactive transition probability of sub-model m transferring to sub-model 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) is the measurement residual of sub-model n, S n (k) is the covariance corresponding to the measurement residual of sub-model n;

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

[0048]

[0049] Among them, C(k) is the normalization parameter.

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

[0051]

[0052] in, is the final error state estimate, is the corresponding state covariance.

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

[0054]

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

[0056] Beneficial effects: Compared with the prior art, the beneficial effects of the present invention are as follows: Based on the idea of ​​model mixing, the single fusion information of each sensor is combined into a sub-model, and the interactive multi-model is used to perform residual interaction on each sub-model, and the model of the faulty sensor is switched to avoid introducing a large positioning error in the collaborative positioning process, and finally the optimal state estimation of the collaborative positioning is obtained according to the model state probability fusion; in order to improve the model switching capability, the Markov transition probability estimation with adaptive parameter adjustment is introduced into the interactive multi-model framework to avoid the situation where the collaborative fault-tolerant performance is reduced due to the failure of model switching; the present invention can be used for the collaborative fault-tolerant positioning of drone clusters in satellite-restricted and unknown complex and interference-susceptible environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 is a flow chart of the present invention;

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

[0059] Figure 3 It is the trajectory diagram of the drone cluster;

[0060] Figure 4 It is the GNSS observation fault error variation diagram;

[0061] Figure 5 It is the error variation diagram of UWB observation failure;

[0062] Figure 6 is a graph of root mean square error variation obtained by using the present invention to solve the UAV;

[0063] Figure 7 It is a cumulative distribution diagram of errors obtained by solving the UAV using the present invention. DETAILED DESCRIPTION

[0064] The present invention is further described in detail below with reference to the accompanying drawings.

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

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

[0067] The error state vector is:

[0068]

[0069] Among them, the error state quantity X has a total of 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 sky, δL, δλ, δh represent the latitude error, longitude error and height error respectively, ε b and ε r They represent the 3D gyro constant drift error and the first-order Markov drift error in three directions respectively. is the accelerometer error.

[0070] Step 2: Estimate the Markov transition probability of the adaptive interacting multi-model and initialize the model probability.

[0071] The Markov transition probability estimate and model probability of the adaptive interacting multi-model are:

[0072]

[0073] ρ=[ρ g ρ a ]

[0074] Where Ψ is the Markov transition probability, g→a (k) is taken as an example to represent the transition probability from the global navigation satellite system (GNSS) sub-model to the ultra-wideband (UWB) sub-model at time k, Ψ g→g (k) represents the probability that the GNSS sub-model can remain in its own model at time k; ρ g and ρ a are the model probabilities of the GNSS sub-model and the UWB sub-model respectively.

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

[0076] According to 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] Among them, X(k) is the 18-dimensional error state quantity of the system at time k, F(k) is the state transfer matrix at time k, G(k) is the error coefficient matrix at time k, and W(k) is the 9-dimensional noise state quantity of the system at time k. The mathematical expression of the state transfer matrix is:

[0079]

[0080] Among them, F INS is the system matrix corresponding to the 9-dimensional basic navigation parameters, which 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 are the relevant time of the gyro and the addition table respectively. The mathematical expression of the error coefficient matrix G(k) is:

[0085]

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

[0087] Use UWB to measure the distance and establish a relative distance model between the UAV and the cooperative UAV. Update the UWB measurement based on the distance measurement results and the relative position information of the UAV. The UWB relative distance model is:

[0088]

[0089] Among them, ||r ij ||2 represents the real relative position vector r of drones i and j ij The second-order norm of is the observation noise of UWB. The measurement equation of UWB can be defined as:

[0090]

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

[0092]

[0093] Among them, e j1 、e j2 、e j3 is the direction cosine from UAV i to other UAV nodes.

[0094] Step 5: Build a global navigation satellite system (GNSS) measurement update model.

[0095] Based on the positioning information provided by the GNSS module, the GNSS data is integrated with the inertial navigation information, and the measurement update is performed by establishing a GNSS measurement model. The GNSS measurement equation is:

[0096]

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

[0098]

[0099] Where R M , R N It represents the meridian curvature radius and the meridian curvature radius calculated by inertial navigation.

[0100] Step 6: Obtain the fusion information of UWB and GNSS through the measurement update of step 4 and step 5, use the fusion information of UWB and GNSS as a sub-model, perform the model interaction process, and calculate the mixed state estimation and mixed state covariance of all sub-models. The mixed state estimation and mixed state covariance are:

[0101]

[0102] Among them, the subscript g represents the GNSS submodel, and the subscript a represents the UWB submodel; and P m (k-1|k-1) are the state estimation and state covariance of sub-model m at time k-1, u m→n (k-1|k-1) is the interaction transition probability (ITP) of sub-model m transferring to sub-model n at time k-1.

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

[0104]

[0105] The mathematical expression is:

[0106]

[0107] Step 7: After step 6, the model probability update process needs to be performed. First, the measurement residuals and corresponding covariances of all sub-models are calculated. This process will be implemented in steps 4 and 5, and 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) is the measurement residual of sub-model n, S n (k) is the covariance corresponding to the measurement residual of sub-model n.

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

[0111]

[0112] S n The mathematical expression of (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 of model probability update is:

[0115]

[0116] Among them, C(k) is the normalization parameter.

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

[0118]

[0119] Step 8: By updating the model probability in step 7, the real-time model probability at this moment can be obtained. The fault information in the sensor measurement can be isolated by the change of the model probability. First, the model state fusion process is performed, and the weighted calculation is performed according to the model probability of each sub-model to obtain the final error state estimate and state covariance. The final error state estimate and state covariance are:

[0120]

[0121] in, is the final error state estimate, is the corresponding state covariance.

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

[0123] The sub-model likelihood function is determined by the sub-model measurement residual and measurement residual covariance at the current moment, so the sub-model likelihood function can well reflect the system performance. Therefore, the sub-model likelihood function is used here to construct the adaptive adjustment factor, and the Markov transition probability estimate is updated as:

[0124]

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

[0126]

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

[0128] Step 10, correct the inertial navigation positioning error, output the collaborative positioning result in the strapdown inertial navigation solution platform, and transmit the corrected positioning information to other UAV nodes through collaborative communication, and finally return to step 3 to start the next cycle.

[0129] In order to verify the correctness and effectiveness of the present invention, the present invention is used to verify the above implementation steps on a Matlab computing platform. Figure 3 is the trajectory diagram of the drone cluster. Figure 4 and Figure 5 The error changes when random faults occur in GNSS and UWB observations are simulated. This kind of fault situation often occurs in actual scene observations. Figure 6 This is the error diagram of the three-dimensional position result obtained by the UAV using the present invention. Figure 6 It can be concluded that the present invention can accurately calculate the three-dimensional position of the UAV, so that the settlement error of the three-dimensional position of the UAV can be kept within 2m. When a GNSS or UWB observation fails, the present invention can detect and isolate the fault immediately to avoid causing a large position solution error in the UAV positioning. Figure 7 This is the cumulative distribution diagram of the error obtained by using the present invention to solve the UAV. Figure 7 It can be seen that the error of the drone position solution is basically accumulated within 2m, which shows that the present invention has high robust fault tolerance.

[0130] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A collaborative fault-tolerant positioning method based on adaptive residual model interaction, characterized in that: The following steps are involved: (1) Initialize the error state vector and covariance matrix of the Kalman filter to provide initial conditions for the filtering process; (2) Estimating the Markov transition probability and initializing the model probability of the adaptive interacting 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 Kalman filter prediction; (4) Establish a relative distance model between the UAV and the cooperative UAV, and perform UWB measurement updates based on the distance measurement results and the relative position information of the UAVs; (5) Based on the positioning information provided by GNSS, the GNSS data is integrated with the inertial navigation information, and the measurement update is performed by establishing a GNSS measurement model; (6) Taking the UWB and GNSS fusion information as sub-models, model interaction is performed to calculate the mixed state estimation and mixed state covariance of all sub-models; (7) Calculate the measurement residuals and corresponding covariances of all sub-models, and then construct a maximum likelihood function to update the model probability of each sub-model; (8) Perform weighted calculation based on the model probability of each sub-model to obtain the final error state estimate and state covariance; (9) Update the Markov transition probability estimate of each sub-model in real time; (10) Correct the inertial navigation positioning error, output the collaborative positioning result in the strapdown inertial navigation solution platform, and transmit the corrected positioning information to other UAV nodes through collaborative communication, and finally return to step (3) to start the next cycle.

2. According to claim 1, a collaborative fault-tolerant positioning method based on adaptive residual model interaction is characterized in that: The error state vector described in step (1) is: Among them, the error state quantity X has a total of 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 sky; δL, δλ, δh represent the latitude error, longitude error and height error respectively; ε b and ε r Represent the 3D gyro constant drift error and the first-order Markov drift error in three directions respectively; is the accelerometer error.

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

4. According to claim 1, a collaborative fault-tolerant positioning method based on adaptive residual model interaction is characterized in that: The implementation process of step (3) is as follows: in, is the state time update equation, k represents the time, X(k) is the 18-dimensional error state quantity, F(k) is the state transfer matrix, G(k) is the error coefficient matrix, W(k) is the 9-dimensional noise state quantity; F INS is the system matrix corresponding to the 9-dimensional basic navigation parameters, obtained through the inertial navigation system error equation; T g , T a They are the relevant times of the gyro and the addition table, respectively.

5. According to claim 1, a collaborative fault-tolerant positioning method based on adaptive residual model interaction is characterized in that: The relative distance model described in step (4) is: Among them, ||r ij ||2 represents the real relative position vector r of drones i and j ij The second-order norm of is the observation noise of ultra-wideband; the measurement equation of UWB is defined as: in, is the observation vector of UAV i and UWB ranging, is the measurement matrix of UWB ranging, V i uwb is the measurement noise of UWB ranging; The mathematical expression is: Among them, e j1 、e j2 、e j3 is the direction cosine from UAV i to other UAV nodes.

6. According to claim 1, a collaborative fault-tolerant positioning method based on adaptive residual model interaction is characterized in that: The implementation process of step (5) is as follows: in, represents the GNSS observation vector of UAV i, represents the GNSS observation matrix of UAV i, V i GNSS is the measurement noise of 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 It represents the meridian curvature radius and the meridian curvature radius calculated by inertial navigation.

7. According to claim 1, a collaborative fault-tolerant positioning method based on adaptive residual model interaction is characterized in that: The mixed state estimate and mixed state covariance described in step (6) are: Among them, the subscript g represents the GNSS submodel, and the subscript a represents the UWB submodel; and P m (k-1|k-1) are the state estimation and state covariance of sub-model m at time k-1, u m→n (k-1|k-1) is the interactive transition probability of sub-model m transferring to sub-model n at time k-1.

8. According to claim 1, a collaborative fault-tolerant positioning method based on adaptive residual model interaction is 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) is the measurement residual of sub-model n, S n (k) is the covariance corresponding to the measurement residual of sub-model n; The mathematical expression of model probability update is: Among them, C(k) is the normalization parameter.

9. The method for collaborative fault-tolerant positioning based on adaptive residual model interaction according to claim 1, characterized in that: The final error state estimate and state covariance described in step (8) are: in, is the final error state estimate, is the corresponding state covariance.

10. The method for collaborative fault-tolerant positioning 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: in, is the adaptive adjustment factor at time k; L m (k) is the maximum likelihood value corresponding to the m sub-model at time k.

Images