A method for monitoring the autonomous integrity of GNSS / INS integrated navigation

Abstract

This invention relates to an autonomous integrity monitoring method for GNSS / INS integrated navigation, belonging to the field of navigation and positioning technology. The purpose of this invention is to solve the problem that existing technologies struggle to simultaneously address GNSS and INS fault risks while ensuring the integrity of the integrated navigation system. The method involves: first, initializing filtering parameters and integrity-related parameters; acquiring satellite navigation and inertial navigation measurements; performing fault recovery detection; if successful, maintaining the current sensor set; if unsuccessful, removing the faulty sensor from the current sensor set; constructing a full set and subsets based on the input sensor set, and constructing filters for the full set and subsets respectively for filtering and solving to obtain altitude estimates for the full set and subsets; performing fault detection; performing fault elimination; and conducting integrity risk assessment. This invention enables effective detection and elimination of GNSS and INS faults and provides a protection level for integrity risk assessment.

Description

Technical Field

[0001] This invention relates to a method for monitoring the autonomous integrity of GNSS / INS integrated navigation, and more particularly to an autonomous integrity monitoring method based on the separation of multiple hypothesis solutions that takes into account the risk of GNSS and INS failures, belonging to the field of navigation and positioning technology. Background Technology

[0002] Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) have complementary advantages, and their combination can provide users with real-time, high-precision positioning information. However, both GNSS and INS measurement information are susceptible to errors during their generation and application in navigation and positioning. GNSS / INS integrated navigation systems are typically based on Kalman filters. Faults in GNSS or INS at historical and current epochs can affect the positioning results through the recursive nature of the Kalman filter, causing abnormal positioning errors and potentially threatening the safety of users' lives and property.

[0003] In the field of navigation and positioning technology, integrity is typically used to describe a navigation system's ability to handle unknown faults. GNSS ensures its integrity through Receiver Autonomous Integrity Monitoring (RAIM) technology, while INS generally handles faults through hardware redundancy. However, for GNSS / INS integrated navigation systems, RAIM technology cannot monitor INS faults, and hardware redundancy for INS is difficult to implement in many scenarios. Therefore, RAIM is currently an important technical means to ensure the integrity of integrated navigation systems.

[0004] A complete integrity monitoring system comprises two parts: fault detection and elimination, and protection-level calculation. The former detects and eliminates unknown faults, while the latter assesses integrity risks and provides safety boundaries for positioning errors. Advanced Receiver Autonomous Integrity Monitoring (ARAIM) constructs a complete integrity monitoring system for GNSS, but ARAIM cannot be directly applied to integrated navigation systems that combine multiple sensors. In current research on autonomous integrity monitoring methods for GNSS / INS integrated navigation, some methods focus only on fault detection and elimination, neglecting necessary protection-level calculations. For example, invention patents CN202111061571.2 and CN201911225922.1; the former uses an improved residual test method to enhance the detection performance of slowly varying faults, while the latter is based on software redundancy and involves adaptive thresholds for fault detection. However, these methods do not assess integrity risks, making it difficult to guarantee the integrity of the integrated navigation system.

[0005] In summary, achieving autonomous integrity monitoring of GNSS / INS integrated navigation is crucial for ensuring user safety. However, current research has not yet established a complete integrity monitoring system that takes into account the risks of both GNSS and INS failures. The current technical requirements for GNSS / INS integrated navigation integrity monitoring are how to quickly detect and eliminate GNSS or INS failures to protect user positioning errors from their interference, while simultaneously calculating protection levels to conduct integrity risk assessments and providing safety boundaries for user positioning errors. Summary of the Invention

[0006] The purpose of this invention is to address the problem that existing technologies struggle to simultaneously address GNSS and INS failure risks while ensuring the integrity of the integrated navigation system. This invention provides an autonomous integrity monitoring method for GNSS / INS integrated navigation. This method can effectively detect and eliminate GNSS and INS failures and provides a protection level for integrity risk assessment.

[0007] The objective of this invention is achieved through the following technical solution.

[0008] A method for monitoring the autonomous integrity of GNSS / INS integrated navigation includes the following steps:

[0009] Step 1: Initialize the filter parameters and the parameters related to integrity;

[0010] Step 2: Obtain satellite navigation and inertial navigation measurements;

[0011] Step 3: Perform a fault recovery test. If it passes, keep the current sensor set; if it fails, remove the faulty sensor from the current sensor set.

[0012] Step 4: Construct a complete set and a subset based on the input sensor set, and construct filters for the complete set and subset respectively to perform filtering calculations and obtain the height estimates of the complete set and subset.

[0013] Step 5: Perform fault detection: Construct test statistics and detection thresholds based on the height estimates of the entire set and subsets output in Step 4; if a fault is detected, proceed to Step 6; otherwise, proceed to Step 7.

[0014] Step 6: Perform troubleshooting: Calculate the normalized sum of squared residuals of all subsets, find the subset with the smallest sum of squared normalized residuals, mark the sensors excluded from it as faulty sensors, and remove the faulty sensors from the current sensor set; then repeat step 4.

[0015] Step 7, Integrity Risk Assessment: Calculate the protection level using the full set and subset positioning results and the detection threshold. If the protection level is lower than the alarm limit, output the current full set positioning result to the user; otherwise, issue an alarm to the user, indicating that the current solution result cannot be used for positioning.

[0016] The parameters that need to be initialized in step 1 include:

[0017] (1) Filtering parameters: initial state variable estimate X0 and initial mean square error matrix P0;

[0018] (2) Parameters related to integrity: Continuity risk requirement C req Integrity risk requirement I req Vertical alarm limit VAL, navigation satellite prior fault probability P sat The prior fault probability P of the inertial navigation system ins and probability threshold P f .

[0019] The measurements required in step 2 include:

[0020] (1) Satellite navigation measurement: The position of the i-th navigation satellite in the geocentric coordinate system at time k. speed pseudo-distance ρ i,k and pseudorange rate

[0021] (2) Inertial navigation measurement values: Specific force and angular velocity in the body coordinate system measured by the inertial navigation system at time k.

[0022] Step 3 includes the following steps:

[0023] Step 3-1: Determine if the fault sensor set is empty at the current moment: If the fault sensor set is empty, the fault recovery detection passes and proceed to step 4; otherwise, proceed to step 3-2.

[0024] Step 3-2: Calculate the fault recovery test statistic t. FRT

[0025] Let S be the set of all sensors. all The set of fault sensors is denoted as S. fault The set of fault-free sensors is denoted as S. ff Then S ff =S all -S fault According to S all and S ff Construct filters separately and solve for the heights, and denote the resulting height solutions as follows: and Then the fault recovery test statistic t FRT The calculation formula is as follows:

[0026]

[0027] Step 3-3: Calculate the fault recovery detection threshold T FRT

[0028] Fault recovery detection threshold T FRT The expression is:

[0029] T FRT =Q -1 (P f / 2)σ Δh

[0030] In the formula Q -1 (·) denotes the inverse function of the tail probability density function of the standard normal distribution, σ Δh For t FRT The standard deviation.

[0031] Steps 3-4: Determine whether the marked faulty sensor has been restored.

[0032] If t FRT <T FRT If the faulty sensor has recovered, the fault recovery detection has passed, and the current sensor set should be maintained; otherwise, the faulty sensor has failed the fault recovery detection and should be removed from the current sensor set.

[0033] Step 4 includes the following steps:

[0034] Step 4-1, Set Partitioning

[0035] The inertial navigation system is considered as a sensor at the same level as navigation satellites, and the current sensor set input to step 4 is denoted as the global set S0. The number of sensors in S0 is m, and the j-th sensor is denoted as sensor. jThen a total of m subsets need to be constructed, and the j-th subset is denoted as S. j The corresponding sensor set is as follows:

[0036] S j =S0-{sensor j} j=1,…,m

[0037] Step 4-2, Filtering Solution

[0038] After the set partitioning is completed, the universal set S0 or the subset S j There are two possibilities for the sensor combination: one, it includes both navigation satellites and inertial navigation systems; two, it includes only navigation satellites.

[0039] When S0 or S j When both navigation satellites and inertial navigation systems are included, the filter corresponding to this set is a compact combination structure, and its state equation is:

[0040]

[0041] In the formula δX inte F is a 23-dimensional error state vector. inte G is a 23×23 dimensional state transition matrix. inte The noise driving matrix is ​​23×14 dimensional, ω inte It is a 14-dimensional noise vector.

[0042] Based on the current position and velocity calculations of the inertial navigation system and the estimated satellite pseudorange and pseudorange rate, the filter measurement value is the difference between the estimated pseudorange and pseudorange rate and the measured value. At this point, the measurement equation for the GNSS / INS compact combination is:

[0043] δZ inte =H inte δX inte +μ inte

[0044] In the formula, δZ inte H is the measured value of the filter. inte Let μ be the Jacobian matrix after linearization. inte To observe noise.

[0045] When S0 or S j When only navigation satellites are included, the filter corresponding to this set is a GNSS filter structure, and its state equation is:

[0046] In the formula, δX gnss F is an 11-dimensional error state vector. gnss G is an 11×11 dimensional state transition matrix. gnssThe noise driving matrix is ​​11×5 dimensional, ω gnss It is a 5-dimensional noise vector.

[0047] Based on the position and velocity solutions from the reference motion model and the estimated satellite position and velocity, the pseudorange and pseudorange rate are calculated. The filter measurement is the difference between the estimated pseudorange and pseudorange rate and the measured value. The measurement equation for GNSS filtering is then:

[0048] Z gnss =H gnss δX gnss +μ gnss

[0049] In the formula, δZ gnss H is the measured value of the filter. gnss Let μ be the Jacobian matrix after linearization. gnss To observe noise.

[0050] After discretizing the state equations and measurement equations of each set using a discretization algorithm, the error state vector of each set is estimated based on the Kalman filter algorithm. The estimated error state vector is then used to correct the predicted state variables of each set, ultimately yielding the position and velocity estimates for each set. The height estimate of the entire set S0 is denoted as... Subset S j The height estimate is denoted as

[0051] Step 5 includes the following steps:

[0052] Step 5-1: Calculate the fault detection test statistic.

[0053] There are currently m subsets, therefore there are m corresponding fault detection test statistics. Among them, the j-th fault detection test statistic t... FD,j The expression is:

[0054]

[0055] Step 5-2: Calculate the fault detection threshold

[0056] The fault detection threshold T corresponding to the j-th fault detection test statistic FD,j The calculation formula is:

[0057]

[0058] Where P(H0) is the prior probability of the fault-free mode, and its calculation formula is:

[0059] P(H0)=(1-P sat ) m-1 (1-Pins )

[0060] σ Δh,j Let be the standard deviation corresponding to the j-th test statistic, and its calculation formula is as follows:

[0061]

[0062] In the formula, and They need to be matrices P respectively. 0,k and P j,k The element corresponding to the height, and Let P be a matrix 0j,k The element corresponding to the height, P 0j,k The calculation formula is:

[0063]

[0064] In the formula, I represents the identity matrix, Φ i,k,k-1 Φ represents the state transition matrix of each filter at time k. i,k,k-1 K i,k P represents the filter gain matrix. i,k H represents the posterior mean square error matrix. i,k Represents the observation matrix (i = 0 or j); Q 0j,k Let S0 represent the universal set and S0 represent the subset S0. j The process noise covariance matrix, R 0j,k Let S0 represent the universal set and S0 represent the subset S0. j The measurement noise covariance matrix.

[0065] Step 5-3: Determine if a fault has been detected.

[0066] If at the current time for any t FD,j and T FD,j All satisfy t FD,j <T FD,j If the result is 0, it means the fault detection passed, i.e., no fault was detected, and then proceed to step 7; otherwise, it means the fault detection failed, which means that some sensors in the entire set S0 have failed, and then proceed to step 6.

[0067] Step 6 includes the following steps:

[0068] Step 6-1: Calculate the normalized sum of squared residuals for each subset.

[0069] Troubleshooting requires calculating the normalized sum of squared residuals (SSE) for each subset. j Its expression is:

[0070] j = 1, 2, ..., m

[0071] In the formula, γ ρ,j,k Represents residual γ j,k The component related to pseudorange, P ρ,j,k P represents the residual mean square error matrix. γ,j,k The submatrix related to the pseudorange. γ j,k and P γ,j,k The calculation formula is:

[0072]

[0073] Step 6-2: Determine the set of faulty sensors

[0074] SSE j Characterize the degree of impact of faulty sensors on each subset. SSE j The smallest subset is most likely to not contain any faulty sensors, therefore SSE j The sensors removed from the smallest subset are the faulty sensors. The normalized sum of squared residuals (SSE) is then used. j The smallest subset is denoted by S. min Then the set of fault sensors S fault for:

[0075] S fault =S0-S min

[0076] Step 6-3: Complete Reconstruction

[0077] After troubleshooting, the entire set needs to be reconstructed and the fault detection performed again. Let the reconstructed entire set be denoted as S. 0,new ,but:

[0078] S 0,new =S0-S fault

[0079] S 0,new Step 4 is re-executed as a new complete set.

[0080] Step 7 includes the following steps:

[0081] Step 7-1: Construct the vertical protection level equations

[0082] The equations for the vertical protection stage are constructed as follows:

[0083]

[0084] In the formula, σ h,0 and σ h,j Let S0 represent the universal set and S0 represent the subsets. j The vertical positioning error variance, P(H) j ) is a subset S jThe prior probability of the corresponding failure mode, Q(·), represents the tail probability density function of the standard normal distribution.

[0085] If subset S j If it does not include an inertial navigation system, then:

[0086] P(H j ) = P sat (1-P sat ) m-2 (1-P ins )

[0087] If subset S j If an inertial navigation system is included, then:

[0088] P(H j ) = P ins (1-P sat ) m-1

[0089] Step 7-2: Solve the equations for the vertical protection stage.

[0090] Step 7-2-1: Initialize the upper limit VPL of the vertical protection stage. up Vertical protection level lower limit VPL down And the accuracy indicator EPS;

[0091] Step 7-2-2: Let VPL = (VPL) up +VPL down ) / 2;

[0092] Step 7-2-3, if |VPL up -VPL down If | < eps, then output the current VPL; otherwise, proceed to step 7-2-4.

[0093] Step 7-2-4: Calculate the following probability function.

[0094]

[0095] If f(VPL) > I req VPL down =VPL; otherwise, set VPL up =VPL; Execute step 7-2-2.

[0096] Step 7-3: Determine whether the integrity risk requirement has been met.

[0097] If VPL≥VAL, it means that the global positioning solution does not meet the integrity risk requirements and an alarm should be issued to the user in a timely manner; if VPL<VAL, the global positioning solution at the current time is output to the user, and VPL is used as the safety boundary of the user's vertical positioning error at the current time.

[0098] Beneficial effects:

[0099] 1. This invention provides a complete autonomous integrity monitoring framework for GNSS / INS integrated navigation. It incorporates different types of sensors into a multi-hypothesis solution separation integrity monitoring framework, constructs test statistics based on consistency tests to perform fault detection and elimination and protection level calculation, and this framework is easily extended to other types of integrated navigation systems.

[0100] 2. The fault detection and elimination algorithm provided by this invention can effectively detect and eliminate GNSS satellite faults and INS faults, ensuring that the integrated navigation positioning results are not affected by GNSS satellite faults and INS faults;

[0101] 3. The protection level algorithm provided by this invention can take into account the failure risks of GNSS and INS, effectively encapsulate the positioning error of the integrated navigation system, provide a safe boundary for the user's positioning error, and reduce the vertical protection level to about 10m, thereby improving the availability of the integrity monitoring algorithm.

[0102] 4. The fault recovery detection algorithm designed in this invention can avoid the multi-layer filtering structure after the traditional multi-hypothesis decomposition algorithm detects a fault. With m sensors, only m+1 sub-filters need to be constructed, which significantly reduces the computational complexity and can ensure that at least one sub-filter is not affected by any fault when a sensor fails. Attached Figure Description

[0103] Figure 1 The flowchart of the autonomous integrity monitoring algorithm for GNSS / INS integrated navigation in a preferred embodiment of the present invention is shown.

[0104] Figure 2 The filter structure is shown in a preferred embodiment of the present invention when the sensor set includes an INS;

[0105] Figure 3 The filter structure is shown in a preferred embodiment of the present invention when the sensor set does not include an INS;

[0106] Figure 4 The fault detection curve for GNSS failure in a preferred embodiment of the present invention is shown.

[0107] Figure 5 The fault detection curve for INS failure in a preferred embodiment of the present invention is shown;

[0108] Figure 6 This illustrates a flowchart of calculating the vertical protection level in a preferred embodiment of the present invention;

[0109] Figure 7The vertical protection level and vertical positioning error curves in a preferred embodiment of the present invention are shown. Detailed Implementation

[0110] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments, through which the features and advantages of the invention will become clearer. However, the present invention is not limited to the exemplary embodiments disclosed below; it can be implemented in various forms. The purpose of this specification is merely to help those skilled in the art to comprehensively understand the specific details of the invention.

[0111] This embodiment focuses on the autonomous integrity monitoring of a UAV using a GNSS / INS integrated navigation system for navigation and positioning. The UAV's trajectory and navigation information are generated through simulation. During the simulation, the UAV's flight time is 900 seconds, the GNSS constellation is GPS, the INS output frequency is 50 Hz, and the GPS output frequency is 2 Hz. The sensor error parameters during the simulation are shown in Table 1.

[0112] Table 1 Sensor Error Parameters

[0113]

[0114] like Figure 1 As shown, this invention provides a method for monitoring the autonomous integrity of GNSS / INS integrated navigation. It includes the following steps:

[0115] Step 1: Parameter Initialization

[0116] The parameters that need to be initialized include:

[0117] (1) Filtering parameters: initial state variable estimate X0 and initial mean square error matrix P0.

[0118] (2) Parameters related to integrity: Continuity risk requirement C req Integrity risk requirement I req Vertical alarm limit VAL, navigation satellite prior fault probability P sat The prior fault probability P of the inertial navigation system ins and probability threshold P f .

[0119] Step 2: Acquisition of GNSS / INS Integrated Navigation Measurement Information

[0120] The measurement information that needs to be obtained includes:

[0121] (1) Satellite measurement information: the position, velocity, pseudorange and pseudorange rate of the navigation satellite in the geocentric coordinate system at the current moment.

[0122] (2) Inertial navigation measurement information: the specific force and angular velocity in the body coordinate system measured by the inertial navigation system within the current integrated navigation solution cycle.

[0123] Step 3: Fault Recovery Detection

[0124] Step 3-1: Determine if the fault sensor set is empty at the current moment.

[0125] If the set of faulty sensors is empty at the current moment, the fault recovery detection passes and proceeds directly to step 4; otherwise, proceed to step 3-2.

[0126] Step 3-2: Calculate the fault recovery test statistic t. FRT

[0127] Fault recovery test statistic t FRT The calculation formula is as follows:

[0128]

[0129] In the formula, and S represents the set of all sensors. all and the set of fault-free sensors S ff The height estimate is obtained through filtering. The set of faulty sensors is denoted as S. fault Then S ff =S all -S fault .

[0130] Step 3-3: Calculate the fault recovery detection threshold T FRT

[0131] Fault recovery detection threshold T FRT The expression is:

[0132] T FRT =Q -1 (P f / 2)σ Δh

[0133] In the formula Q -1 (·) denotes the inverse function of the tail probability density function of the standard normal distribution, σ Δh For t FRT The standard deviation.

[0134] Steps 3-4: Determine if the fault of the marked sensor has been resolved.

[0135] If t FRT <T FRTIf the fault recovery detection is successful, the current sensor measurement should be maintained; otherwise, the fault recovery detection has failed and the faulty sensor should be removed from the current sensor set.

[0136] Step 4: Set Partitioning and Filtering Solution

[0137] Step 4-1, Set Partitioning

[0138] The inertial navigation system is considered as a sensor at the same level as navigation satellites, and the current sensor set input to step 4 is denoted as the global set S0. Assume that the number of sensors in S0 is m, and the j-th sensor is denoted as sensor. j Then a total of m subsets need to be constructed, and the j-th subset is denoted as S. j The corresponding sensor set is as follows:

[0139] S j =S0-{sensor j} j=1,…,m

[0140] Step 4-2, Filtering Solution

[0141] After the set partitioning is completed, the universal set S0 or the subset S j There are two possibilities for the sensor combination: one, it includes both navigation satellites and inertial navigation systems; two, it includes only navigation satellites.

[0142] When S0 or S j When both navigation satellites and inertial navigation systems are included, the filter corresponding to this set is a compact combination structure, and its state equation is:

[0143]

[0144] In the formula, δX gnss F is an 11-dimensional error state vector. gnss G is an 11×11 dimensional state transition matrix. gnss The noise driving matrix is ​​11×5 dimensional, ω gnss It is a 5-dimensional noise vector.

[0145] Based on the current position and velocity calculated by the inertial navigation system and the satellite position and velocity, the estimated satellite pseudorange and pseudorange rate can be calculated. The filter measurement value is the difference between the estimated pseudorange and pseudorange rate and the measured value. At this time, the measurement equation of the GNSS / INS compact combination is:

[0146] δZ inte =H inte δX inte +μ inte

[0147] In the formula, δZ inteH is the measured value of the filter. inte Let μ be the Jacobian matrix after linearization. inte To observe noise.

[0148] Figure 2 The filtering structure is shown when the sensor set includes INS. In a preferred embodiment of the present invention, indirect filtering is used for filtering calculation, that is, the state error is estimated by combining the measurement information of GNSS and INS through an extended Kalman filter, and the estimated state error is used to correct each state variable, finally obtaining the estimated value of the state variable.

[0149] When S0 or S j When only navigation satellites are included, the filter corresponding to this set is a GNSS filter structure, and its state equation is:

[0150] In the formula, δX gnss F is an 11-dimensional error state vector. gnss G is an 11×11 dimensional state transition matrix. gnss The noise driving matrix is ​​11×5 dimensional, ω gnss It is a 5-dimensional noise vector.

[0151] The measurement equations for GNSS filtering can be constructed by referring to the compact combination:

[0152] δZ gnss =H gnss δX gnss +μ gnss

[0153] In the formula, δZ gnss H is the measured value of the filter. gnss Let μ be the Jacobian matrix after linearization. gnss To observe noise.

[0154] Figure 3 The filtering structure is shown when the sensor set does not include an INS. In a preferred embodiment of the invention, the predicted values ​​of the state variables when the sensor set does not include an INS are obtained by a uniformly accelerated motion model (CA model). With this filtering structure, even without INS measurement information, the filter can obtain highly accurate estimates of the user's position and velocity.

[0155] After discretizing the state equations and measurement equations of each set using a discretization algorithm, the error state vector of each set is estimated based on the Kalman filter algorithm. The estimated error state vector is then used to correct the predicted state variables of each set, ultimately yielding the position and velocity estimates for each set. The height estimate of the entire set S0 is denoted as... Subset S j The height estimate is denoted as

[0156] Step 5, Fault Detection

[0157] Step 5-1: Calculate the fault detection test statistic.

[0158] There are currently m subsets, therefore there are m corresponding fault detection test statistics. Among them, the j-th fault detection test statistic t... FD,j The expression is:

[0159]

[0160] Step 5-2: Calculate the fault detection threshold

[0161] The detection threshold T corresponding to the j-th test statistic FD,j The calculation formula is:

[0162]

[0163] Where P(H0) is the prior probability of the fault-free mode, and its calculation formula is:

[0164] P(H0)=(1-P sat ) m-1 (1-P ins )

[0165] σ Δh,j Let be the standard deviation corresponding to the j-th test statistic, and its calculation formula is as follows:

[0166]

[0167] In the formula, and They are matrices P 0,k and P j,k The element corresponding to the height, and Then it is matrix P 0j,k The element corresponding to the height, P 0j,k The calculation formula is:

[0168]

[0169] In the formula, I represents the identity matrix, Φ i,k,k-1 Φ represents the state transition matrix of each filter at time k. i,k,k-1 K i,k P represents the filter gain matrix. i,k H represents the posterior mean square error matrix. i,k Represents the observation matrix (i = 0 or j); Q 0j,k Let S0 represent the universal set and S0 represent the subset S0. jThe process noise covariance matrix, R 0j,k Let S0 represent the universal set and S0 represent the subset S0. j The measurement noise covariance matrix.

[0170] Step 5-3: Determine if a fault has been detected.

[0171] If at the current time for any t FD,j and T FD,j All satisfy t FD,j <T FD,j A 0 indicates that the fault detection passed, meaning no fault was detected; conversely, a 0 indicates that the fault detection failed, meaning that some sensors in the entire set S0 have failed.

[0172] Figure 4 The figure shows the fault recovery and fault detection curves for a GNSS fault in a preferred embodiment of the present invention. As can be seen from the figure, after satellite PRN19 experiences a pseudorange fault of 35m at 200s, the fault detection test statistic of the subset S4, which does not include PRN19, responds rapidly and exceeds the corresponding threshold at 201.5s, at which point the GNSS satellite fault is successfully detected.

[0173] Figure 5 The figure shows the fault recovery and fault detection curves when the INS fails in a preferred embodiment of the present invention. As can be seen from the figure, the z-axis accelerometer of the INS experiences a 0.1 m / s² speed fluctuation at 200 s. 2 After a fault of a certain size, the fault detection test statistic of the subset S8 that does not contain INS responds quickly and exceeds the corresponding threshold at 214s, at which point the INS fault is successfully detected.

[0174] Figure 4 and Figure 5 This demonstrates that the method described in this invention can effectively detect GNSS satellite faults and INS faults.

[0175] Step 6: Troubleshooting

[0176] Step 6-1: Calculate the normalized sum of squared residuals for each subset.

[0177] Troubleshooting requires calculating the normalized sum of squared residuals (SSE) for each subset. j Its expression is:

[0178] j = 1, 2, ..., m

[0179] In the formula, γ ρ,j,k Represents residual γ j,k The component related to pseudorange, P ρ,j,k P represents the residual mean square error matrix. γ,j,kThe submatrix related to the pseudorange. γ j,k and P γ,j,k The calculation formula is:

[0180]

[0181] Step 6-2: Screening the set of faulty sensors

[0182] SSE j Characterize the degree of impact of faulty sensors on each subset. SSE j The smallest subset is most likely to not contain any faulty sensors, therefore SSE j The sensors removed from the smallest subset are the faulty sensors. The normalized residuals and SSE are then calculated. j The smallest subset is denoted by S. min Fault sensor set S fault for:

[0183] S fault =S0-S min

[0184] In a preferred embodiment of the present invention, under PRN15 fault conditions, Figure 4 The subsets SSE corresponding to 201.5s j As shown in Table 2:

[0185] Table 2. Normalized residual sum of squares for each subset during satellite PRN15 failure.

[0186]

[0187] Table 2 shows that SSE j The smallest subset is S5, and the faulty sensor can be identified as satellite PRN15 based on the troubleshooting method.

[0188] In a preferred embodiment of the present invention, under INS failure conditions, Figure 5 The subsets SSE corresponding to time 214s j As shown in Table 3:

[0189] Table 3 Normalized residual sum of squares for each subset during INS failure

[0190]

[0191]

[0192] Table 3 shows that SSE j The smallest subset is S8, and the faulty sensor can be identified as INS based on the troubleshooting method.

[0193] Tables 2 and 3 illustrate that the method described in this invention can effectively eliminate GNSS satellite faults and INS faults.

[0194] Step 6-3: Complete Reconstruction

[0195] After troubleshooting, the entire set needs to be reconstructed and the fault detection performed again. Let the reconstructed entire set be denoted as S. 0,new ,but:

[0196] S 0,new =S0-S fault

[0197] S 0,new Step 4 is re-executed as a new complete set.

[0198] Step 7: Integrity Risk Assessment

[0199] Step 7-1: Construct the vertical protection level equations

[0200] The equations for the vertical protection stage are constructed as follows:

[0201]

[0202] In the formula, σ h,0 and σ h,j Let S0 represent the universal set and S0 represent the subsets. j The vertical positioning error variance, P(H) j ) represents the prior probability of fault mode j.

[0203] If subset S j If an inertial navigation system is included, then:

[0204] P(H j ) = P sat (1-P sat ) m-2 (1-P ins )

[0205] If subset S j If an inertial navigation system is included, then:

[0206] P(H j ) = P ins (1-P sat ) m-1

[0207] Step 7-2: Solve the equations for the vertical protection stage.

[0208] like Figure 6 As shown, the process for calculating the vertical protection level in a preferred embodiment of the present invention is as follows:

[0209] Step 7-2-1: Initialize the upper limit VPL of the vertical protection stage.up Vertical protection level next VPL down and accuracy index eps

[0210] Step 7-2-2: Let VPL = (VPL) up +VPL down ) / 2

[0211] Step 7-2-3, if |VPL up -VPL down If | < eps, output the current VPL; otherwise, proceed to step 7-2-4.

[0212] Step 7-2-4: Calculate the following probability function.

[0213]

[0214] If f(VPL) > I req VPL down =VPL; otherwise, set VPL up =VPL; Execute step 7-2-2.

[0215] Step 7-3: Determine whether the integrity risk requirement has been met.

[0216] If VPL≥VAL, it means that the global positioning solution does not meet the integrity risk requirements and an alarm should be issued to the user in a timely manner; if VPL<VAL, the global positioning solution at the current time is output to the user, and VPL is used as the safety boundary of the user's vertical positioning error at the current time.

[0217] Figure 7 The vertical protection level and vertical positioning error curves in a preferred embodiment of the present invention are shown. Figure 7 This indicates that the vertical protection level calculated by the method described in this invention is approximately 10m, which is lower than the vertical alarm threshold of 35m, and the vertical protection level can effectively cover the vertical positioning error.

[0218] Based on the fault detection, troubleshooting, and integrity risk assessment steps in the above preferred embodiments, the GNSS / INS integrated navigation autonomous integrity monitoring method provided by the present invention can effectively detect and eliminate GNSS satellite and INS faults, and the calculated protection level is lower than the alarm threshold, which can effectively envelop the positioning error and thus ensure the integrity of the GNSS / INS integrated navigation system.

[0219] The above detailed description further illustrates the purpose, technical solution, and beneficial effects of the invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A GNSS / INS integrated navigation autonomous integrity monitoring method, characterized in that: The method comprises the following steps: Step 1, initializing filter parameters and integrity-related parameters; The parameters to be initialized in step 1 include: (1) filter parameters: initial state variable estimation value X0 and initial mean square error matrix P0; (2) Parameters related to integrity: continuity risk requirement C req , integrity risk requirement I req , vertical alert limit VAL, navigation satellite a priori failure probability P sat , inertial navigation system a priori failure probability P ins and probability threshold P f ; Step 2, obtaining satellite navigation and inertial navigation measurement values; The measurement values to be obtained in step 2 include: (1) Satellite navigation measurement value: position of the i-th navigation satellite at time k in the Earth-Centered Earth-Fixed coordinate system velocity Pseudo-range p i,k and pseudo-range rate (2) inertial navigation measurement values: specific force and angular velocity in the body coordinate system measured by the inertial navigation system at time k; Step 3, performing fault recovery detection, if passed, keeping the current sensor set; if not passed, removing the faulty sensor from the current sensor set; Step 4, constructing the full set and the subset according to the input sensor set, and constructing filters for the full set and the subset respectively to perform filtering calculation to obtain the height estimation values of the full set and the subset; Step 5, performing fault detection: constructing fault detection test statistics and fault detection threshold according to the full set and subset height estimation values output in step 4; if a fault is detected, performing step 6; otherwise, performing step 7; Step 5 includes the following steps: Step 5-1, calculating fault detection test statistics At present, there are m subsets, so there are m fault detection test statistics corresponding thereto; wherein, the jth fault detection test statistic t FD,j The expression is: Step 5-2, calculating fault detection threshold The jth fault detection test statistic corresponds to a fault detection threshold T FD,j The calculation formula is: Wherein, P(H0) is the prior probability of the fault-free mode, and its calculation formula is: P(H0) = (1 - P sat ) m-1 (1 - P ins ) σ Δh,j The standard deviation σj corresponding to the jth test statistic is calculated as follows: wherein and are the elements of matrix P 0,k and P j,k corresponding to the height, respectively, and is the element of matrix P 0j,k corresponding to the height, P 0j,k is calculated as follows: In the formula, I represents the identity matrix, Φ i,k,k-1 K represents the state transition matrix of each filter at time k. i,k P represents the filter gain matrix. i,k H represents the posterior mean square error matrix. i,k Represents the observation matrix, i = 0 or j; Q 0j,k Let S0 represent the universal set and S0 represent the subset S0. j The process noise covariance matrix, R 0j,k Let S0 represent the universal set and S0 represent the subset S0. j The measurement noise covariance matrix; Step 5-3, judging whether a fault occurs If the current time t FD,j and T FD,j satisfy t FD,j <T FD,j , it means that the fault detection is passed, i.e. no fault is detected, then step 7 is performed; otherwise, it means that the fault detection is failed, which implies that some sensors in the set S0 have failed, then step 6 is performed. Step 6, performing fault elimination: calculating the normalized residual sum of squares of all subsets, finding the subset with the smallest normalized residual sum of squares, marking the excluded sensor as a faulty sensor, and removing the faulty sensor from the current sensor set; then re-executing step 4; Step 6 includes the following steps: Step 6-1, calculating the normalized residual sum of squares of each subset Troubleshooting requires computing the normalized sum of squared errors SSE for each subset j whose expression is where γ ρ,j,k represents the residual error γ j,k component related to the pseudo-range, P ρ,j,k represents the residual error mean square error matrix P γ,j,k sub-matrix related to the pseudo-range; γ j,k and P γ,j,k The calculation formula is: Step 6-2, determining the faulty sensor set SSE j characterize the degree of influence of the faulty sensors on each subset; SSE j The smallest subset is most likely to contain no faulty sensors, so SSE j The sensors removed by the smallest subset are faulty sensors; the normalized sum of squared residuals, SSE j The smallest subset is denoted S min Then the set of faulty sensors S fault is: S fault = S0- S min Step 6-3, full set reconstruction After troubleshooting, the full set needs to be reconstructed and fault detection is performed again; the reconstructed full set is denoted as S 0,new Then: S 0,new = S0- S fault S 0,new re-execute step 4 as a new full set; Step 7, integrity risk assessment: calculating the protection level using the full set and subset positioning results and the fault detection threshold, if the protection level is lower than the alarm limit, outputting the current full set positioning result to the user; otherwise, alarming the user and prompting that the current calculation result cannot be used for positioning.

2. The GNSS / INS integrated navigation autonomous integrity monitoring method of claim 1, wherein: Step 3 includes the following steps: Step 3-1, judging whether the current time fault sensor set is empty: if the fault sensor set is empty, the fault recovery detection is passed, and step 4 is executed; otherwise, step 3-2 is executed; Step 3-2, calculating the failure recovery check statistic t FRT Let S be the set of all sensors. all The set of fault sensors is denoted as S. fault The set of fault-free sensors is denoted as S. ff Then S ff =S all -S fault According to S all and S ff Construct filters separately and solve for the heights, and denote the resulting height solutions as follows: and Then the fault recovery test statistic t FRT The calculation formula is as follows: Step 3-3, calculating a failure recovery detection threshold T FRT Failure recovery detection threshold T FRT The expression is: T FRT = Q -1 (P f / 2)σ Δh where Q -1 (·) denotes the inverse function of the tail probability density function of the standard normal distribution, σ Δh is the standard deviation of t FRT . Step 3-4, judging whether the marked faulty sensor has been recovered if t FRT <T FRT indicates that the marked faulty sensor has recovered, i.e. the fault recovery detection has passed and the current sensor set has to be maintained; otherwise, the fault recovery detection has not passed and the faulty sensor has to be excluded from the current sensor set.

3. The GNSS / INS integrated navigation autonomous integrity monitoring method of claim 1, wherein: Step 4 includes the following steps: Step 4-1, set partitioning The inertial navigation system is regarded as a sensor at the same level as the navigation satellite, and the current sensor set input to step 4 is denoted as a full set S0; the number of sensors in S0 is m, and the jth sensor is denoted as sensor j Therefore, m subsets need to be constructed, and the jth subset is denoted as S j The corresponding sensor set is: S j = S0- {sensor j}j = 1,..., m Step 4-2, filtering calculation After the set partitioning is completed, the full set S0 or the subset S j There are two possibilities for the sensor combination in the full set S0 or the subset S : one is to contain both navigation satellites and inertial navigation systems; the other is to contain only navigation satellites. When S0 or S j When both navigation satellites and inertial navigation system are included, the filter corresponding to the set is a tight coupling structure, and its state equation is: where δX inte is a 23-dimensional error state vector, F inte is a 23 x 23-dimensional state transition matrix, G inte is a 23 x 14-dimensional noise driving matrix, ω inte is a 14-dimensional noise vector; According to the inertial navigation system position and velocity calculation value at the current time and the satellite position and velocity, the satellite pseudo-range and pseudo-range rate estimation value is calculated, and the filter measurement value is the difference between the pseudo-range and pseudo-range rate estimation value and the measurement value, at this time, the measurement equation of the GNSS / INS tight combination is: δZ inte = H inte δX inte + μ inte where δZ inte is the measured value of the filter, H inte is the Jacobian matrix after linearization, μ inte is the observation noise; When S0 or S j When only navigation satellites are included, the filter corresponding to this set is a GNSS filter structure with state equations: where δX gnss is an 11 -dimensional error state vector, F gnss is an 11 x 11 -dimensional state transition matrix, G gnss is an 11 x 5 -dimensional noise driving matrix, ω gnss is a 5 -dimensional noise vector; According to the position and velocity calculation value of the reference motion model and the satellite position and velocity, the satellite pseudo-range and pseudo-range rate estimation value is calculated, and the filter measurement value is the difference between the pseudo-range and pseudo-range rate estimation value and the measurement value, at this time, the measurement equation of the GNSS filter is: Z gnss = H gnss δX gnss + μ gnss where δZ gnss is the measured value of the filter, H gnss is the Jacobian matrix after linearization, μ gnss is the observation noise; The state equation and the measurement equation of each set are discretized by a discretization algorithm, error state vectors of each set are estimated based on a Kalman filtering algorithm, the estimated error state vectors are used to correct the predicted state variables of each set, and finally the position and velocity estimation values corresponding to each set are obtained; the height estimation value of the full set S0 is denoted as The height estimation value of the subset S j is denoted as 4. The GNSS / INS integrated navigation autonomous integrity monitoring method of claim 1, wherein: Step 7 includes the following steps: Step 7-1, constructing the vertical protection level equation The vertical protection level equation is constructed as follows: where σ h,0 and σ h,j denote the vertical positioning error variances of the universal set S0and the subset S j , respectively, P(H j ) is the prior probability of the failure mode corresponding to the subset S j , and Q(·) denotes the tail probability density function of the standard normal distribution; If the subset S j does not contain an inertial navigation system, then: P(H j ) = P sat (H sat | H m-2 ) P ins (H sat | H m-2 ) If the subset S j comprises an inertial navigation system, then: P(H j ) = P ins (1-P sat ) m-1 Step 7-2, solving the vertical protection level equation Step 7-2-1, initialize vertical protection level upper bound VPL up , vertical protection level lower bound VPL down and precision indicator eps; Step 7 - VPL = (VPL up + VPL down ) / 2; Step 7-2-3, if |VPL up -VPL down |<eps, output the current VPL; otherwise, perform Step 7-2-4. Step 7-2-4, calculating the following probability function If f(VPL) > I req , let VPL down = VPL; else let VPL up = VPL; perform step 7-2-2; Step 7-3, judging whether the integrity risk requirement is satisfied If VPL≥VAL, it indicates that the integrity risk requirement is not satisfied, and the user should be warned in time; if VPL<VAL, the user is outputted the current time set positioning solution, and VPL is used as the safety boundary of the user's vertical positioning error at the current time.

Images