SINS / GPS integrated navigation method based on scale factor
By constructing the state equation and measurement equation of the SINS/GPS tightly integrated navigation system, and combining the autonomous integrity extrapolation algorithm and adaptive scaling factor, the problems of nonlinearity of measurement information and inaccuracy of noise statistics in the SINS/GPS loosely integrated navigation system are solved, thereby improving navigation accuracy.
Patent Information
- Application Number
- CN202410991264.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-23
- Publication Date
- 2024-11-08
AI Technical Summary
In SINS/GPS loosely coupled navigation systems, the nonlinearity of measurement information and the inaccuracy of noise statistics make it difficult for traditional snapshot methods to identify slowly changing satellite faults, leading to a decrease in navigation accuracy.
The state equations and measurement equations of the SINS/GPS tightly integrated navigation system are constructed. An autonomous integrity extrapolation algorithm is used for fault detection. The measurement noise variance matrix is adjusted based on an adaptive scaling factor, and the extended Kalman filter equation is updated in real time.
It effectively identifies satellite faults that gradually change, improves the accuracy of measurement information and filtering precision, enhances navigation performance, and improves the accuracy of the SINS/GPS tightly integrated navigation system.
Smart Images

Figure CN118915115A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of navigation, guidance and control, and in particular relates to an adaptive EKF (Extended Kalman Filter) SINS / GPS combined navigation method based on a proportional factor. Background Art
[0002] Among various navigation systems, the advantages of Strap-down Inertial Navigation System (SINS) are mainly as follows:
[0003] (1) Not disturbed by external environmental factors;
[0004] (2) When performing navigation work, the real-time update rate is fast and the navigation accuracy is high in a short time;
[0005] (3) It has strong autonomous capabilities and relies solely on the inertial information of its own inertial devices for navigation.
[0006] However, since the strapdown inertial navigation system is essentially an integral calculation in its operation, its disadvantage is that the navigation positioning error will accumulate over time and it cannot provide accurate navigation information for a long time.
[0007] The advantages of the Global Positioning System (GPS) are: first, it can provide navigation information to user carriers for a long time, and second, its navigation positioning error will not increase with time. However, during operation, GPS is easily affected by the external environment, resulting in low navigation accuracy, poor anti-interference ability, multipath effect and low real-time update rate.
[0008] From the above analysis, it can be seen that the strapdown inertial navigation system and the global satellite navigation and positioning system are complementary in function. If the two are organically combined to form a SINS / GPS combined navigation system, the various advantages of both can be taken into account at the same time. Compared with the two navigation methods working separately, the navigation accuracy of the combined navigation system is more superior.
[0009] However, the SINS / GPS loosely integrated navigation system has a low degree of integration and only uses position information and velocity information as measurement information, resulting in inaccurate nonlinearity and noise statistics. Summary of the invention
[0010] The purpose of the present invention is to provide a SINS / GPS integrated navigation method based on a proportional factor to solve the problems of nonlinearity and inaccurate noise statistics of a SINS / GPS loosely integrated navigation system using position and velocity as measurement information, and the problem that a traditional snapshot method is difficult to identify slowly varying satellite faults.
[0011] The present invention solves the above technical problems through the following technical solutions: a SINS / GPS integrated navigation method based on a proportional factor, comprising:
[0012] According to the state equations of the strapdown inertial navigation system and the global satellite navigation and positioning system, the state equations and measurement equations of the SINS / GPS tightly integrated navigation system are constructed;
[0013] Perform fault detection on the SINS / GPS tightly integrated navigation system based on the measurement information of the SINS / GPS tightly integrated navigation system;
[0014] Based on the fault detection results, an adaptive scaling factor is constructed according to the measurement information of the SINS / GPS tightly integrated navigation system;
[0015] The measurement noise variance matrix of the SINS / GPS tightly integrated navigation system is adjusted in real time according to the adaptive proportional factor, and then the extended Kalman filter equation is updated in real time, and the integrated navigation data is solved according to the updated extended Kalman filter equation.
[0016] Furthermore, the state equations and measurement equations of the SINS / GPS tightly integrated navigation system are constructed, including:
[0017] Construct the state equation of the strapdown inertial navigation system. The specific expression is:
[0018]
[0019] W I (t) = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ];
[0020] Among them, X I (t) represents the state vector of the strapdown inertial navigation system, F I (t) represents the state transfer matrix of the strapdown inertial navigation system, G I (t) represents the noise allocation matrix of the strapdown inertial navigation system, W I (t) represents the process noise vector of the strapdown inertial navigation system, represents the east attitude error angle, represents the north attitude error angle, Indicates the celestial attitude error angle, δv E represents the eastward velocity error, δv N represents the north velocity error, δv Urepresents the celestial velocity error, δL represents the latitude error, δλ represents the longitude error, δh represents the height error, and ε bx Indicates the zero bias of the gyroscope in the x direction, ε by Indicates the y-axis gyroscope zero bias, ε bz Indicates the zero bias of the gyroscope in the z direction, w gx represents the x-axis gyroscope process noise, w gy represents the y-axis gyroscope process noise, w gz represents the z-axis gyroscope process noise, w ax represents the x-axis accelerometer process noise, w ay represents the y-axis accelerometer process noise, w az represents the z-direction accelerometer process noise;
[0021] Construct the state equation of the global satellite navigation and positioning system. The specific expression is:
[0022]
[0023] W G (t) = [w tu w tru ];
[0024] Among them, X G (t) represents the state vector of the global satellite navigation positioning system, F G (t) represents the state transfer matrix of the global satellite navigation and positioning system, G G (t) represents the noise distribution matrix of the global satellite navigation positioning system, W G (t) represents the process noise vector of the global satellite navigation positioning system, represents the satellite clock drift error, represents the satellite clock drift rate error, β tru represents the clock drift rate coefficient, w tu represents the satellite clock drift noise, w tru represents the satellite clock drift noise;
[0025] By combining the state equations of the strapdown inertial navigation system and the global satellite navigation and positioning system, the state equation of the SINS / GPS tightly integrated navigation system is obtained. The specific expression is:
[0026]
[0027] The state vector X is:
[0028] The process noise vector is: W = [w gx ,w gy ,w gz ,w ax ,way ,w az ,w tu ,w tru ] T ;
[0029] The pseudorange measurement equation is constructed according to the state vector X, and the specific expression is:
[0030] Z ρ (t) = δ ρ =H ρ (t)X(t)+V ρ (t);
[0031] H ρ =[0 4×6 H ρ1 0 4×6 H ρ2 ] 4×17 ; H ρ2 =
[10] ;
[0032] Among them, Z ρ (t) represents the system pseudorange measurement, δ ρ represents the system pseudorange error, H ρ (t) represents the system pseudo-range measurement vector, V ρ (t) represents the system pseudorange measurement noise; H ρ1 represents the intermediate variable, H ρ2 represents an intermediate variable; ζ=x,y,z, represents the direction cosine in the ζ direction, represents the direction cosine of the ζ direction of satellite 1, represents the direction cosine of the ζ direction of satellite 2, represents the direction cosine of the ζ direction of satellite 3; Represents the coordinate transformation matrix from the geodetic coordinate system to the Earth-centered Earth-fixed coordinate system;
[0033] The pseudo-range rate measurement equation is constructed according to the state vector X, and the specific expression is:
[0034]
[0035]
[0036] in, represents the system pseudorange rate measurement, represents the system pseudorange rate error, represents the system pseudorange rate measurement vector, represents the system pseudorange rate measurement noise; represents the intermediate variable, represents an intermediate variable;
[0037] The measurement equation of the SINS / GPS tightly integrated navigation system is constructed according to the pseudorange measurement equation and the pseudorange rate measurement equation. The specific expression is:
[0038]
[0039] Among them, Z represents the measurement information, ρ sins represents the pseudorange calculated by strapdown inertial navigation, ρ G represents the satellite output pseudorange, represents the pseudo-range rate of the strapdown inertial navigation solution, represents the satellite output pseudorange rate, H represents the measurement matrix, and V represents the measurement noise matrix.
[0040] Further, the SINS / GPS tightly integrated navigation system is fault-checked according to the measurement information of the SINS / GPS tightly integrated navigation system, including:
[0041] The fault detection statistics are defined based on the measurement information of the SINS / GPS tightly integrated navigation system. The specific formula is:
[0042]
[0043]
[0044] in, represents the fault detection statistic, r avg represents the filter information of SINS / GPS tightly integrated navigation system, V avg represents the intermediate variable, m represents the length of the detection data, r k Indicates the current measurement information of the SINS / GPS integrated navigation system, V k represents the variance of the filter innovation at the current time k, H k represents the measurement matrix at the current moment, P k / k-1 represents the state one-step prediction variance matrix, R k Represents the measurement noise variance matrix at the current moment;
[0045] Whether a fault occurs in the SINS / GPS tightly integrated navigation system is determined according to the fault detection statistic and the threshold value.
[0046] Further, judging whether a fault occurs in the SINS / GPS tightly integrated navigation system according to the fault detection statistic and the threshold value includes:
[0047] Under a given false alarm rate α, when When SINS / GPS tightly integrated navigation system works normally; when At that time, the SINS / GPS integrated navigation system failed.
[0048] Furthermore, the expression of the adaptive scaling factor is:
[0049]
[0050] Among them, α i represents the adaptive scaling factor of the i-th sub-filter; represents the prediction residual of the ith sub-filter, Z i,k represents the quantity measurement of the i-th sub-filter, H i,k represents the measurement matrix component related to the measurement of the i-th subfilter, represents the i-th step prediction state quantity, σ R represents the mean square error of the measurement noise, c0 and c1 represent the prediction residual The constant threshold value.
[0051] Furthermore, the update formula of the extended Kalman filter equation is:
[0052]
[0053]
[0054]
[0055]
[0056]
[0057]
[0058] in, represents the state one-step prediction matrix, represents the state estimation vector at time k-1, P k,k-1 represents the state one-step prediction covariance matrix, Φ k,k-1 represents the state transfer matrix, P k-1 represents the k-1 time state estimation covariance matrix, Γ k-1 represents the system matrix, Q k-1 represents the system noise matrix at time k-1; represents the adjusted measurement noise variance matrix, R i,k represents the measurement noise variance matrix before adjustment, α i represents the adaptive scaling factor of the i-th sub-filter; K k Represents the current filter gain matrix, H k Represents the measurement matrix at the current moment, R kRepresents the measurement noise variance matrix at the current moment; Represents the current state estimation vector, Z k Represents the measurement vector at the current moment; P k Represents the estimated covariance value of the current state, and I represents the identity matrix.
[0059] Beneficial Effects
[0060] Compared with the prior art, the advantages of the present invention are:
[0061] The present invention uses an autonomous integrity extrapolation algorithm to perform preliminary screening of measurement information, effectively identifies satellite slowly varying fault information, improves the accuracy of measurement information, and further improves the filtering accuracy of the SINS / GPS tightly integrated navigation system, thereby improving navigation performance; the present invention adjusts the measurement noise variance matrix in real time through an adaptive proportional factor, thereby improving the nonlinear problem of the SINS / GPS tightly integrated navigation system and the problem of unclear noise model, thereby improving the accuracy of the SINS / GPS tightly integrated navigation system. BRIEF DESCRIPTION OF THE DRAWINGS
[0062] In order to more clearly illustrate the technical solution of the present invention, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only one embodiment of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0063] Figure 1 It is a flow chart of the SINS / GPS integrated navigation method in an embodiment of the present invention;
[0064] Figure 2 is a diagram of the slope of a slowly varying fault in an embodiment of the present invention;
[0065] Figure 3 is a flow chart of an adaptive EKF algorithm based on a scaling factor in an embodiment of the present invention;
[0066] Figure 4 is the motion trajectory of the carrier in the embodiment of the present invention; wherein, the carrier latitude is 34.22027420°, the carrier longitude is 117.14401093°, the carrier altitude is 32.7058m, the eastward speed is -0.0002m / s, the northward speed is 0.0222m / s, the celestial speed is 0.0583m / s, the pitch angle is 1.0363°, the roll angle is -0.5741°, and the heading angle is 298.3382°;
[0067] Figure 5is a position error comparison curve between the adaptive EKF algorithm based on the proportional factor and the Suge-Husa adaptive filtering algorithm in an embodiment of the present invention;
[0068] Figure 6 : is a speed error comparison curve between the adaptive EKF algorithm based on the proportional factor and the Suge-Husa adaptive filtering algorithm in an embodiment of the present invention;
[0069] Figure 7 is a comparison curve of attitude errors between the adaptive EKF algorithm based on the proportional factor and the Suge-Husa adaptive filtering algorithm in an embodiment of the present invention;
[0070] Figure 8 is a position error comparison curve when c0 is 1.4 and c1 is 4.1 and when c0 is 1.1 and c1 is 4.4 in the embodiment of the present invention;
[0071] Fig. 9 is a speed error comparison curve when c0 is 1.4 and c1 is 4.1 and when c0 is 1.1 and c1 is 4.4 in the embodiment of the present invention;
[0072] Fig.10 It is a posture error comparison curve when c0 is 1.4 and c1 is 4.1 and when c0 is 1.1 and c1 is 4.4 in the embodiment of the present invention. DETAILED DESCRIPTION
[0073] The following is a clear and complete description of the technical solutions in the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0074] The technical solution of the present application is described in detail with specific embodiments below. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.
[0075] like Figure 1 As shown, a SINS / GPS integrated navigation method based on a scaling factor provided by an embodiment of the present invention comprises the following steps:
[0076] Step S1: construct the state equation and measurement equation of the SINS / GPS tightly integrated navigation system according to the state equations of the strapdown inertial navigation system and the global satellite navigation and positioning system.
[0077] In a specific embodiment of the present invention, the state equation and measurement equation of the SINS / GPS tightly integrated navigation system are constructed, including:
[0078] Step S1.1: Construct the state equation of the strapdown inertial navigation system. The specific expression is:
[0079]
[0080]
[0081] W I (t) = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ] (3)
[0082] Among them, X I (t) represents the state vector of the strapdown inertial navigation system, F I (t) represents the state transfer matrix of the strapdown inertial navigation system, G I (t) represents the noise allocation matrix of the strapdown inertial navigation system, W I (t) represents the process noise vector of the strapdown inertial navigation system, represents the east attitude error angle, represents the north attitude error angle, Indicates the celestial attitude error angle, δv E represents the eastward velocity error, δv N represents the north velocity error, δv U represents the celestial velocity error, δL represents the latitude error, δλ represents the longitude error, δh represents the height error, and ε bx Indicates the zero bias of the gyroscope in the x direction, ε by Indicates the y-axis gyroscope zero bias, ε bz Indicates the zero bias of the gyroscope in the z direction, w gx represents the x-axis gyroscope process noise, w gy represents the y-axis gyroscope process noise, w gz represents the z-axis gyroscope process noise, w ax represents the x-axis accelerometer process noise, w ay represents the y-axis accelerometer process noise, w az represents the z-direction accelerometer process noise.
[0083] Step S1.2: Construct the state equation of the global satellite navigation and positioning system. The specific expression is:
[0084]
[0085]
[0086] Among them, X G(t) represents the state vector of the global satellite navigation positioning system, F G (t) represents the state transfer matrix of the global satellite navigation and positioning system, G G (t) represents the noise distribution matrix of the global satellite navigation positioning system, W G (t) represents the process noise vector of the global satellite navigation positioning system, represents the satellite clock drift error, represents the satellite clock drift rate error, β tru represents the clock drift rate coefficient, w tu represents the satellite clock drift noise, w tru Represents the satellite clock drift noise.
[0087] Step S1.3: Combine the state equations of the strapdown inertial navigation system and the global satellite navigation and positioning system to obtain the state equation of the SINS / GPS tightly integrated navigation system. The specific expression is:
[0088]
[0089] The state vector X is:
[0090] The process noise vector is: W = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ,w tu ,w tru ] T (8)
[0091] Step S1.4: Construct a pseudorange measurement equation based on the state vector X. The specific expression is:
[0092] Z ρ (t) = δ ρ =H ρ (t)X(t)+V ρ (t) (9)
[0093]
[0094] Among them, Z ρ (t) represents the system pseudorange measurement, δ ρ represents the system pseudorange error, H ρ (t) represents the system pseudo-range measurement vector, V ρ (t) represents the system pseudorange measurement noise; H ρ1 represents the intermediate variable, H ρ2 represents an intermediate variable; ζ=x,y,z, represents the direction cosine in the ζ direction, represents the direction cosine of the ζ direction of satellite 1, represents the direction cosine of the ζ direction of satellite 2, represents the direction cosine of the ζ direction of satellite 3; Represents the coordinate transformation matrix from the geodetic coordinate system to the Earth-centered Earth-fixed coordinate system.
[0095] Step S1.5: Construct the pseudo-range rate measurement equation according to the state vector X. The specific expression is:
[0096]
[0097]
[0098] in, represents the system pseudorange rate measurement, represents the system pseudorange rate error, represents the system pseudorange rate measurement vector, represents the system pseudorange rate measurement noise; represents the intermediate variable, Represents an intermediate variable.
[0099] Step S1.6: Construct the measurement equation of the SINS / GPS tightly integrated navigation system based on the pseudorange measurement equation and the pseudorange rate measurement equation. The specific expression is:
[0100]
[0101] Among them, Z represents the measurement information, ρ sins represents the pseudorange calculated by strapdown inertial navigation, ρ G represents the satellite output pseudorange, represents the pseudo-range rate of the strapdown inertial navigation solution, represents the satellite output pseudorange rate, H represents the measurement matrix, and V represents the measurement noise matrix.
[0102] The measurement equation of the SINS / GPS tightly integrated navigation system consists of the pseudorange measurement equation and the pseudorange rate measurement equation. sins ,y sins ,z sins ) is the coordinate position of a certain observation satellite j in the Earth-Centered-Earth-Fixed (ECEF) coordinate system. Therefore, the pseudorange of satellite j can be calculated by the strapdown inertial navigation system as:
[0103]
[0104] in, represents the pseudorange of satellite j, represents the navigation position coordinates of the strapdown inertial navigation system in the ECEF coordinate system. Since equation (14) is a nonlinear equation, equation (14) is sins ,y sins ,z sins ) and only keep the first two terms, and discard the rest, then the linear pseudorange measurement equation of satellite j can be derived:
[0105]
[0106] in, r j is the real distance between satellite j and GPS receiver. Then:
[0107]
[0108] The direction cosines in the x, y, and z directions are (x, y, z) represents the coordinates of the carrier in the ECEF coordinate system. Substituting equation (16) into equation (15) yields:
[0109]
[0110] in, They represent partial derivatives of x, y, and z respectively.
[0111] The specific expression of the pseudorange measurement value of satellite j is:
[0112]
[0113] in, represents the pseudorange measurement value of satellite j, represents the pseudorange measurement error of satellite j, Represents the satellite clock drift error.
[0114] From this, the pseudo-range difference measurement equation of satellite j can be derived:
[0115]
[0116] Among them, δρ j represents the pseudorange difference of satellite j.
[0117] If the ECEF coordinate system is selected as the navigation coordinate system of the strapdown inertial navigation system, then equation (19) is the measurement equation of the integrated navigation system. Generally, longitude and latitude are used to represent the position information, so the conversion formula between the ECEF coordinate system x, y, z and the geodetic coordinate system L, λ, h needs to be given. The specific conversion formula is as follows:
[0118]
[0119] Where (x, y, z) represents the coordinates of the carrier in the ECEF coordinate system, R N represents the radius of curvature of the y-axis, h represents the altitude, L represents the latitude, λ represents the longitude, and e represents the eccentricity.
[0120]
[0121] Among them, (δx, δy, δz) represents the coordinate error of the carrier in the x, y, and z directions, and (δL, δλ, δh) represents the longitude, latitude, and altitude errors. Then, the system pseudorange measurement equations represented by longitude, latitude, and altitude can be obtained as shown in equations (9) and (10).
[0122] There is a relative motion between the GPS receiver and the satellite in the change of spatial position. The pseudo-range rate (i.e., pseudo-range change rate) of the strapdown inertial navigation relative to the satellite j is calculated by taking the derivative of equation (14):
[0123]
[0124] From this we can conclude that: Substitute equation (14) into equation (22), and Taylor expansion is performed at , retaining only low-order terms, and at the same time, x sins =x,y sins =y,z sins =z, we can get:
[0125]
[0126] in, represents the pseudorange rate measurement value of satellite j, represents the pseudorange rate error of satellite j, represents the rate of change of the true distance between satellite j and the GPS receiver, Represents the satellite clock drift rate error.
[0127] right Taking the derivative we get:
[0128]
[0129] Then we have:
[0130]
[0131] Will and By making a difference, we can get the pseudo-range rate measurement equation of satellite j:
[0132]
[0133] Where j = 1, 2, 3, 4, then the pseudo-range rate measurement equation is:
[0134]
[0135] in, They represent the velocity error components of each axis in the ECEF coordinate system. Since the navigation coordinate system is the northeast sky coordinate system, the velocity error δv in the northeast sky coordinate system needs to be E ,δv N ,δv U Convert to ECEF system. Just add the velocity error in the northeast sky coordinate system to the left coordinate conversion matrix, that is:
[0136]
[0137] The system pseudo-range rate measurement equations can be obtained as shown in formulas (11) and (12). The combined SINS / GPS tight integration navigation system measurement equations are shown in formula (13). The position coordinates of satellite j in the ECEF coordinate system are The pseudorange and pseudorange rate of satellite j are derived from SINS:
[0138]
[0139] The pseudorange and pseudorange rate of satellite j output by the GPS receiver are:
[0140]
[0141] By subtracting equations (29) and (30), we can obtain the system measurement equation:
[0142]
[0143] Let i = 1, 2, 3, 4, we can get the observed quantity Z k and the nonlinear vector h k The expression is:
[0144]
[0145] Step S2: Perform fault detection on the SINS / GPS tightly integrated navigation system according to the measurement information of the SINS / GPS tightly integrated navigation system.
[0146] The original GPS Receiver Autonomous Integrity Monitoring (RAIM) algorithm is a traditional "snapshot method" using epoch observations, among which the representative methods are the parity vector method for constructing a parity vector space and the minimum residual method based on the residual vector. However, the traditional snapshot method is insufficient in identifying and monitoring slowly varying faults, resulting in a reduction in the combined navigation system and altitude. Therefore, the RAIM algorithm was born, which uses filtered new information to construct statistics to achieve fault monitoring, such as the autonomous integrity extrapolation algorithm (AIME algorithm). The present invention uses the AIME algorithm to monitor satellite pseudoranges and pseudorange rates, effectively identify and isolate slowly varying faults, and thus improve filtering accuracy.
[0147] The AIME algorithm uses the filter innovation of the integrated navigation system as the fault detection statistic, where the filter innovation is defined as:
[0148] r k =Z k -Z k,k-1 =Z k -H k X k,k-1 (33)
[0149] Among them, r k , Z k , Z k,k-1 They represent the new information of the current moment (i.e., moment k), the measurement value at the current moment, and the one-step prediction value of the system measurement. k The value of reflects the fault amplitude, so the fault detection statistic is defined as:
[0150]
[0151] in, r avg is the filter innovation, V k is the variance of the filter innovation at time k, and its specific expression is:
[0152]
[0153] From equations (33) to (35), we can see that when the system has no faults, the fault test statistic is It obeys the central chi-square distribution; when a system failure occurs, the failure test statistic It obeys non-central chi-square distribution. The judgment criteria are as follows:
[0154] Under a given false alarm rate α, when the fault test statistic When , the system is considered to be working normally; when the fault test statistic When , the system is considered to have a fault, where Tr is the detection threshold.
[0155] In order to verify the advantages of the autonomous integrity extrapolation algorithm over the parity vector method in identifying satellite slow-varying faults, the embodiment of the present invention simulates and verifies the two methods. In this embodiment, the satellite fault adopts a slow fault (or a slow-varying fault), and a slow fault is added to a certain satellite in the observed satellites between 358400 (seconds per week) and 358600s (seconds per week). The fault slope is as follows: Figure 2 The simulation results show that the fault detection delay time of AIME algorithm and parity vector method is shown in Table 1.
[0156] Delay time comparison table showing 1AIME algorithm and odd-even vector method
[0157] Slow-change fault speed (m / s) 0.1 0.2 0.5 1 2 AIME fault detection delay time (s) 59.7 43.1 27.4 19.1 13 Odd-even vector method fault detection delay time (s) 130 70 28 19.5 12.5
[0158] It can be seen that the AIME algorithm is significantly better than the traditional snapshot method in detecting slow-changing faults, and its fault detection delay is related to the fault speed. The smaller the fault speed, the more obvious the effect of the AIME algorithm. The autonomous integrity extrapolation algorithm is used to preliminarily screen the measurement information, which improves the accurate measurement information for the integrated navigation system filtering, thereby improving the integrated navigation filtering accuracy.
[0159] Step S3: Based on the fault detection result, an adaptive scaling factor is constructed according to the measurement information of the SINS / GPS tightly integrated navigation system.
[0160] Based on the adaptive filtering algorithm of filter innovation, the present invention proposes an adaptive extended Kalman filtering algorithm (AEKF) based on proportional factor. The algorithm constructs the adaptive proportional factor through the prediction residual of the sub-filter, and realizes online correction by using the ratio of the measurement noise variance matrix and the adaptive proportional factor, so as to achieve the purpose of improving the filtering accuracy of the filter and optimizing the information fusion algorithm. The expression of defining the adaptive proportional factor is:
[0161]
[0162] Among them, α i represents the adaptive scaling factor of the i-th sub-filter; represents the prediction residual of the ith sub-filter, Z i,k represents the quantity measurement of the i-th sub-filter, H i,k represents the measurement matrix component related to the measurement of the i-th subfilter, represents the i-th step prediction state quantity, σ R represents the mean square error of the measurement noise, c0 and c1 represent the prediction residual The constant thresholds are 1.1≤c0≤1.5 and 4.1≤c1≤4.5.
[0163] because The value of can reflect the measurement accuracy of each sub-filter to some extent. Therefore, when the measurement error is introduced into the measurement process, When the value of is between c0 and c1, adjust α i , so that it satisfies 0<α i <1, and can be adjusted according to the ratio When a small measurement error is introduced into the measurement process, that is, α i Assign a value of 1 and identify Similarly, when a large measurement error is introduced into the measurement process, that is, α i Assign a value of 0 and identify Totally unbelievable.
[0164] Step S4: adjusting the measurement noise variance matrix of the SINS / GPS tightly integrated navigation system in real time according to the adaptive scale factor, updating the extended Kalman filter equation in real time, and solving the integrated navigation data according to the updated extended Kalman filter equation.
[0165] In a specific implementation of the present invention, the update formula of the extended Kalman filter equation is:
[0166]
[0167]
[0168]
[0169]
[0170]
[0171]
[0172] in, represents the state one-step prediction matrix, represents the state estimation vector at time k-1, P k,k-1 represents the state one-step prediction covariance matrix, Φ k,k-1 represents the state transfer matrix, P k-1 represents the k-1 time state estimation covariance matrix, Γ k-1 represents the system matrix, Q k-1 represents the system noise matrix at time k-1; represents the adjusted measurement noise variance matrix, R i,krepresents the measurement noise variance matrix before adjustment, α i represents the adaptive scaling factor of the i-th sub-filter; K k Represents the current filter gain matrix, H k Represents the measurement matrix at the current moment, R k Represents the measurement noise variance matrix at the current moment; Represents the current state estimation vector, Z k Represents the measurement vector at the current moment; P k represents the current state estimation covariance value, and I represents the unit matrix. The process of the adaptive EKF algorithm based on the scaling factor is as follows Figure 3 shown.
[0173] The simulation parameter conditions of the present invention are set as follows: the measured data set is obtained by collecting data in a suburban vehicle environment using the GLNav toolbox. The data acquisition platform is equipped with a Trimble R10 receiver and an inertial measurement unit IMU IGM-A1, as well as a precise reference solution of the NovAtel SPAN CPT system. The GLNav toolbox has a simple program for SINS and GPS positioning, which facilitates the design of a SINS / GPS tightly integrated navigation system.
[0174] The data update rate of the inertial measurement unit is 100HZ, the gyro performance indicators are: random walk is 0.03° / hr, deviation stability is 6° / hr, and the accelerometer performance indicators are: random walk is 0.029m / s / hr, deviation stability is 0.1mg. Two groups of c0 and c1 values are selected for analysis. The main purposes of the analysis are: first, to analyze the improvement of navigation accuracy by the adaptive EKF filtering algorithm based on the proportional factor of the present invention and the Suge-Husa adaptive filtering algorithm, and to conduct a comparative analysis of the two; second, to analyze the impact of different values of c0 and c1 on the adaptive EKF algorithm based on the proportional factor of the present invention. The actual motion trajectory of the carrier is as follows: Figure 4 shown.
[0175] Figure 5 to Figure 7 The position, velocity and attitude error comparison curves of the adaptive EKF algorithm based on the proportional factor and the Suge-Husa adaptive filtering algorithm are shown in Figure 2. The black curve represents the error curve of the adaptive EKF algorithm based on the proportional factor; the red curve represents the error curve of the Suge-Husa adaptive filtering algorithm. The constant threshold c0 is 1.1 and c1 is 4.4. Figure 5 to Figure 7 It can be seen that the present invention is slightly better than the Suge-Husa adaptive filtering algorithm in terms of position, speed and attitude angle.
[0176] From the expression (36) of the proportional factor, we can see that different values of c0 and c1 will make the proportional factor αi The value changes, thus affecting the measurement noise variance matrix in the entire filter update process The red curve represents the error curve when c0 is 1.4 and c1 is 4.1, and the black curve represents the error curve when c0 is 1.1 and c1 is 4.4. The simulation results are shown in Figure 8 to Figure 10 As shown. Figure 8 to Figure 10 It can be seen that when c0 is 1.4 and c1 is 4.1, the performance of the integrated navigation system is better than that when c0 is 1.1 and c1 is 4.4. Therefore, the selection of constant thresholds c0 and c1 will directly affect the filtering accuracy of the adaptive EKF filtering algorithm, and further affect the navigation accuracy of the integrated navigation system. The adaptive EKF filtering algorithm based on the scale factor of the present invention has better performance than the Suge-Husa adaptive filtering algorithm under a suitable constant threshold.
[0177] What is disclosed above is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of changes or modifications within the technical scope disclosed in the present invention, which should be covered within the protection scope of the present invention.
Claims
1. A SINS / GPS integrated navigation method based on a scale factor, characterized in that: The combined navigation method comprises: According to the state equations of strapdown inertial navigation system and global satellite navigation and positioning system, the state equations and measurement equations of SINS / GPS tightly integrated navigation system are constructed; Perform fault detection on the SINS / GPS tightly integrated navigation system based on the measurement information of the SINS / GPS tightly integrated navigation system; Based on the fault detection results, an adaptive scaling factor is constructed according to the measurement information of the SINS / GPS tightly integrated navigation system; The measurement noise variance matrix of the SINS / GPS tightly integrated navigation system is adjusted in real time according to the adaptive proportional factor, and then the extended Kalman filter equation is updated in real time, and the integrated navigation data is solved according to the updated extended Kalman filter equation.
2. The SINS / GPS integrated navigation method according to claim 1, characterized in that: Construct the state equations and measurement equations of the SINS / GPS tightly integrated navigation system, including: Construct the state equation of the strapdown inertial navigation system. The specific expression is: W I (t)=[w gx ,w gy ,w gz ,w ax ,w ay ,w az ]; Among them, X I (t) represents the state vector of the strapdown inertial navigation system, F I (t) represents the state transfer matrix of the strapdown inertial navigation system, G I (t) represents the noise allocation matrix of the strapdown inertial navigation system, W I (t) represents the process noise vector of the strapdown inertial navigation system, represents the east attitude error angle, represents the north attitude error angle, Indicates the celestial attitude error angle, δv E represents the eastward velocity error, δv N represents the north velocity error, δv U represents the celestial velocity error, δL represents the latitude error, δλ represents the longitude error, δh represents the height error, and ε bx Indicates the zero bias of the gyroscope in the x direction, ε by Indicates the y-axis gyroscope zero bias, ε bz Indicates the zero bias of the gyroscope in the z direction, w gx represents the x-axis gyroscope process noise, w gy represents the y-axis gyroscope process noise, w gz represents the z-axis gyroscope process noise, w ax represents the x-axis accelerometer process noise, w ay represents the y-axis accelerometer process noise, w az represents the z-direction accelerometer process noise; Construct the state equation of the global satellite navigation and positioning system. The specific expression is: W G (t)=[w tu w tru ]; Among them, X G (t) represents the state vector of the global satellite navigation positioning system, F G (t) represents the state transfer matrix of the global satellite navigation and positioning system, G G (t) represents the noise distribution matrix of the global satellite navigation positioning system, W G (t) represents the process noise vector of the global satellite navigation positioning system, δ tu represents the satellite clock drift error, δ tru represents the satellite clock drift rate error, β tru represents the clock drift rate coefficient, w tu represents the satellite clock drift noise, w tru represents the satellite clock drift noise; By combining the state equations of the strapdown inertial navigation system and the global satellite navigation and positioning system, the state equation of the SINS / GPS tightly integrated navigation system is obtained. The specific expression is: The state vector X is: The process noise vector is: W = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ,w tu ,w tru ] T ; The pseudorange measurement equation is constructed according to the state vector X, and the specific expression is: Z ρ (t)=δ ρ =H ρ (t)X(t)+V ρ (t); H ρ =[0 4×6 H ρ1 0 4×6 H ρ2 ] 4×17 ; H ρ2 =[10]; Among them, Z ρ (t) represents the system pseudorange measurement, δ ρ represents the system pseudorange error, H ρ (t) represents the system pseudo-range measurement vector, V ρ (t) represents the system pseudorange measurement noise; H ρ1 represents the intermediate variable, H ρ2 represents an intermediate variable; ζ=x,y,z, represents the direction cosine in the ζ direction, represents the direction cosine of satellite 1 in the ζ direction, represents the direction cosine of the ζ direction of satellite 2, represents the direction cosine of the ζ direction of satellite 3; Represents the coordinate transformation matrix from the geodetic coordinate system to the Earth-centered Earth-fixed coordinate system; The pseudo-range rate measurement equation is constructed according to the state vector X, and the specific expression is: in, represents the system pseudorange rate measurement, represents the system pseudorange rate error, represents the system pseudorange rate measurement vector, represents the system pseudorange rate measurement noise; represents the intermediate variable, represents an intermediate variable; The measurement equation of the SINS / GPS tightly integrated navigation system is constructed according to the pseudorange measurement equation and the pseudorange rate measurement equation. The specific expression is: Among them, Z represents the measurement information, ρ sins represents the pseudorange calculated by strapdown inertial navigation, ρ G represents the satellite output pseudorange, represents the pseudo-range rate of the strapdown inertial navigation solution, represents the satellite output pseudorange rate, H represents the measurement matrix, and V represents the measurement noise matrix.
3. The SINS / GPS integrated navigation method according to claim 1, characterized in that: Perform fault detection on the SINS / GPS tightly integrated navigation system based on the measurement information of the SINS / GPS tightly integrated navigation system, including: The fault detection statistics are defined based on the measurement information of the SINS / GPS tightly integrated navigation system. The specific formula is: in, represents the fault detection statistic, r avg represents the filter information of SINS / GPS tightly integrated navigation system, V avg represents the intermediate variable, m represents the length of the detection data, r k Indicates the current measurement information of the SINS / GPS integrated navigation system, V k represents the variance of the filter innovation at the current time k, H k represents the measurement matrix at the current moment, P k / k-1 represents the state one-step prediction variance matrix, R k Represents the measurement noise variance matrix at the current moment; Whether a fault occurs in the SINS / GPS tightly integrated navigation system is determined according to the fault detection statistic and the threshold value.
4. The SINS / GPS integrated navigation method according to claim 3, characterized in that: Judging whether a SINS / GPS tightly integrated navigation system fails according to the fault detection statistic and the threshold value includes: Under a given false alarm rate α, when the fault detection statistic When the SINS / GPS tight integration navigation system works normally; when the fault detection statistics At that time, the SINS / GPS integrated navigation system failed.
5. The SINS / GPS integrated navigation method according to claim 1, characterized in that: The expression of the adaptive scaling factor is: Among them, α i represents the adaptive scaling factor of the i-th sub-filter; represents the prediction residual of the ith sub-filter, Z i,k represents the quantity measurement of the i-th sub-filter, H i,k represents the measurement matrix component related to the measurement of the i-th subfilter, represents the i-th step prediction state quantity, σ R represents the mean square error of the measurement noise, c0 and c1 represent the prediction residual The constant threshold value.
6. The SINS / GPS integrated navigation method according to any one of claims 1 to 5, characterized in that: The update formula of the extended Kalman filter equation is: in, represents the state one-step prediction matrix, represents the state estimation vector at time k-1, P k,k-1 represents the state one-step prediction covariance matrix, Φ k,k-1 represents the state transfer matrix, P k-1 represents the k-1 time state estimation covariance matrix, Γ k-1 represents the system matrix, Q k-1 represents the system noise matrix at time k-1; represents the adjusted measurement noise variance matrix, R i,k represents the measurement noise variance matrix before adjustment, α i represents the adaptive scaling factor of the i-th sub-filter; K k Represents the current filter gain matrix, H k Represents the measurement matrix at the current moment, R k Represents the measurement noise variance matrix at the current moment; Represents the current state estimation vector, Z k Represents the measurement vector at the current moment; P k Represents the estimated covariance value of the current state, and I represents the identity matrix.
