Self-adaptive integrated navigation method suitable for multiple scenes

Abstract

The invention provides a self-adaptive integrated navigation method suitable for multiple scenes, which comprises the following steps: state estimation: receiving satellite data and inertial navigation data at a moment k, and performing state estimation based on the satellite data and the inertial navigation data; performing measurement updating, constructing a regulatory factor model according to a state estimation result, and judging whether Kalman filtering is divergent or not based on the regulatory factor model, if so, updating a measurement covariance matrix # imgabs0 # through adaptive filtering, and completing measurement updating based on the updated measurement covariance matrix # imgabs1 #; if not, the measurement covariance matrix # imgabs2 # is updated through a multi-window robust strategy, and measurement updating is completed based on the updated measurement covariance matrix # imgabs3 #; and updating positioning, judging whether to continue filtering, if not, ending filtering, and if so, executing the state estimation step when k is equal to k + 1. According to the invention, the noise covariance matrix can be automatically updated and measured according to the motion state of the carrier, and the self-adaptive capability of integrated navigation is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of combined navigation, and in particular to an adaptive combined navigation method applicable to multiple scenarios. Background Art

[0002] Kalman filtering is widely used in the field of GNSS / INS (Global Navigation Satellite System / InertialNavigation system) integrated navigation. To achieve high-precision Kalman filtering, the following conditions must be met: reliable observation information, precise estimation method and accurate dynamic model.

[0003] However, in actual applications, the motion state of objects is changeable, including ultra-high-speed motion, variable acceleration motion, turning, etc. Different motion states correspond to different dynamic models. It is extremely complex to build a high-precision dynamic model based on the motion state of objects in actual motion. Therefore, in complex motion scenes, there is a certain deviation between the dynamic model and the actual motion state of the carrier, which affects the accuracy of the Kalman filter.

[0004] On the other hand, GNSS signals are very susceptible to interference in complex environments, resulting in large GNSS positioning deviations, which causes the Kalman filter to diverge. In order to further weaken the impact of abnormal observation information and dynamic models, adaptive filtering, anti-error filtering, and Sage-Husa adaptive filtering have been developed successively. Among them, Sage-Husa adaptive filtering is the most widely used. It can effectively solve the problem of filtering instability caused by the traditional Kalman filter treating measurement noise and system noise as white noise. However, this filtering method still has the following problems: The value of the forgetting factor will affect the accuracy of the measurement noise estimation. The forgetting factor in the Sage-Husa adaptive filter is pre-set according to the empirical value, which is difficult to adapt to the complex and changing external environment.

[0005] Simultaneously updating the measurement noise covariance matrix and the system noise covariance matrix online can easily cause filter divergence. Compared with traditional Kalman filtering, the calculation of the measurement noise covariance matrix and the system noise covariance matrix is ​​more complicated and places higher requirements on the navigation computer.

[0006] The semi-positive definiteness of the system noise covariance matrix and the positive definiteness of the measurement noise covariance matrix cannot be guaranteed, which easily leads to singular matrix operations. Summary of the invention

[0007] In view of this, the problem to be solved by the present invention is to provide an adaptive integrated navigation method applicable to multiple scenarios, which can automatically update the measurement noise covariance matrix according to the motion state and improve the adaptive ability of the integrated navigation.

[0008] In order to solve the above technical problems, the technical solution adopted by the present invention is: An adaptive integrated navigation method suitable for multiple scenarios, including state estimation, receiving satellite data and inertial navigation data at time k, and performing state estimation based on the satellite data and inertial navigation data; Measurement update, build a regulation factor model based on the state estimation results, and determine whether the Kalman filter diverges based on the regulation factor model. If yes, update the measurement covariance matrix through adaptive filtering , based on the updated measurement covariance matrix Complete measurement update; No, update the measurement covariance matrix through the multi-window anti-error strategy , based on the updated measurement covariance matrix Complete measurement update; Positioning update, determine whether to continue filtering, if not, end filtering, if yes, k=k+1, execute state estimation step.

[0009] Furthermore, the formula of the adjustment factor model is: , in, is the new information sequence at time k; is the measurement coefficient matrix; is the one-step prediction mean square error matrix of the state between time k-1 and time k; is the measurement covariance matrix at time k; is the reserve factor, and ; tr represents the trace of the matrix; The calculation formula is: , in, is the measurement vector at time k; It is a one-step prediction of the state between time k-1 and time k.

[0010] Furthermore, the reserve factor The calculation formula is: , Among them, exp represents the exponential function with the natural constant e as the base, is the horizontal velocity of the carrier, and Auxiliary regulatory factors.

[0011] Furthermore, the satellite data is collected by a receiver, and the calculation formula of the receiver speed is: , Where b represents the forgetting factor, is the velocity vector of satellite j, with a superscript Indicates satellite, Indicates the receiver speed, represents the receiver clock drift, is the pseudorange observation noise change rate of satellite j, with the superscript represents the rate of change of the corresponding variable, represents the wavelength, is the Doppler observation value; According to the receiver speed Determine the horizontal speed of the carrier .

[0012] Furthermore, the updating formula of the forgetting factor is: , in, It is the n-order state one-step transfer matrix between time k-1 and time k; is the system noise covariance matrix at time k-1, represents the measurement noise covariance matrix at time k, is the one-step prediction mean square error matrix of the state at time k-1.

[0013] Furthermore, the multi-window anti-error strategy includes constructing multiple estimates of the new information covariance matrix, and the estimated value of each new information covariance matrix is ​​calculated with the theoretical value of the new information covariance matrix at the current moment to obtain the estimated value of the observation vector covariance matrix, and the estimated value of the observation vector covariance matrix is ​​calculated with the prior observation covariance matrix. Structural robustness factor ,pass Amplify the measurement noise covariance matrix .

[0014] Furthermore, the estimated value of the innovation covariance matrix includes the estimated value of the innovation covariance matrix of the fixed solution, the estimated value of the innovation covariance matrix of the floating-point solution, and the estimated value of the innovation covariance matrix of the single-point solution.

[0015] Furthermore, the robustness factor The formula is: , , in, represents the prior observation covariance matrix, is the new information vector between the one-step predicted value and the observed value at time ki, and N is the number of epochs.

[0016] Furthermore, the noise covariance matrix is ​​estimated based on the Allan variance. , the specific formula is: , Simplifying the recursion, we can get: , In the above formula, Using the gradually disappearing memory algorithm for estimation, we have: , , in, and represents the measurement noise covariance matrix The upper and lower limit conditions, , k and k-1 represent the current moment and the previous moment respectively, represents the forgetting factor at time k, and b represents the initial forgetting factor set in advance.

[0017] The advantages and positive effects of the present invention are: (1) By setting the adjustment factor model, the horizontal speed of the carrier is used to determine whether the filter is divergent, and the scene is divided based on the carrier's moving speed. When it is divergent, the measurement value is updated through adaptive filtering. When it is not divergent, the measurement value is updated through a multi-window anti-error strategy, which can improve the adaptability to various complex environments.

[0018] (2) By calculating the robustness factors of the fixed solution window, floating point solution window, and single point solution window respectively , in order to obtain the optimal robustness factor Dynamic robustness is performed in the form of Carry out scene division to improve the ability to adapt to various complex environments.

[0019] (3) Update the measurement noise covariance matrix Limit the upper and lower limits during the process to avoid amplification Loss of positive definiteness and: avoids calculating robustness factor The denominator is zero, which makes the matrix operation singular and the filtering divergent, improving the long-term reliability of the filtering of the multi-window robustness strategy. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings: Figure 1 It is an overall structural diagram of an adaptive combined navigation method applicable to multiple scenarios of the present invention. DETAILED DESCRIPTION

[0021] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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.

[0022] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which the present invention belongs. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0023] The present invention provides an adaptive integrated navigation method applicable to multiple scenarios, such as Figure 1 As shown, it includes state estimation, receiving satellite data and inertial navigation data at the current moment, and performing state estimation based on the satellite data and inertial navigation data.

[0024] Integrated navigation is GNSS / INS integrated navigation. When GNSS / INS integrated navigation is running, it initializes data, receives satellite data and inertial navigation data, and estimates the position and attitude of the carrier based on the satellite data and inertial navigation data (i.e., state estimation). GNSS / INS integrated navigation continuously receives satellite data and inertial navigation data to continuously estimate the position and attitude of the carrier for long-term positioning.

[0025] In order to accurately estimate the position and attitude of the carrier in the long term, the measurement value at each sampling moment is continuously updated through the Kalman filter principle, which can achieve long-term and accurate positioning.

[0026] Measurement update, build adjustment factor model, determine whether Kalman filter diverges based on the adjustment factor model, if yes, update the measurement covariance matrix through adaptive filtering , based on the updated measurement covariance matrix Complete measurement update; No, update the measurement covariance matrix through the multi-window anti-error strategy , based on the updated measurement covariance matrix Complete measurement update.

[0027] The formula of the adjustment factor model is: , in, is the new information sequence at time k; is the measurement coefficient matrix; is the one-step prediction mean square error matrix of the state between time k-1 and time k; is the measurement covariance matrix at time k; is the reserve factor, and ; tr represents the trace of the matrix.

[0028] The calculation formula of the new information sequence at time k is: , in, is the measurement vector at time k; It is a one-step prediction of the state between time k-1 and time k.

[0029] When the adjustment factor model formula is established, it means that the Kalman filter at time k is in a divergent state. At this time, adaptive filtering is used to update the measurement noise covariance matrix. , based on the updated measurement noise covariance matrix Update the measurement value at time k; when the adjustment factor model formula does not hold, it means that the Kalman filter at time k does not diverge, and the multi-window anti-error strategy is used to update the measurement noise covariance matrix , based on the updated measurement noise covariance matrix Update the measurement value at time k.

[0030] Reserve coefficient in the adjustment factor model The calculation formula is: , Among them, exp represents the exponential function with the natural constant e as the base, is the horizontal velocity of the carrier, and As auxiliary adjustment factors, the values ​​of the two can be set according to the strictness of the actual filtering abnormality judgment, and: The closer it is to 1, the stricter the filter divergence judgment. The time filter divergence judgment is the most stringent.

[0031] The speed information can directly reflect the motion state of the carrier. During the motion of the carrier, the greater the speed, the stronger the time-varying nature of the parameters, and the more susceptible it is to the external environment and motion mode, which leads to filter divergence. The reserve coefficient is determined based on the horizontal speed of the carrier. , and then the reserve coefficient Bringing in the adjustment factor model to determine whether the Kalman filter diverges can improve the filter's adaptive ability. The Kalman filter can effectively distinguish scenarios of carriers at different speeds based on speed information (horizontal speed of the carrier), making the integrated navigation more adaptable to a variety of complex environments.

[0032] To avoid filter divergence causing speed anomalies and thus affecting the reserve coefficient The horizontal velocity can be calculated by GNSS Doppler velocity measurement.

[0033] The GNSS Doppler velocity measurement calculation formula is: , Where b represents the forgetting factor, is the velocity vector of satellite j, with a superscript Indicates satellite, Indicates the receiver speed, represents the receiver clock drift, is the pseudorange observation noise change rate of satellite j, with the superscript represents the rate of change of the corresponding variable, represents the wavelength, is the Doppler observation value.

[0034] The update formula of the forgetting factor b is: , in, It is the n-order state one-step transfer matrix between time k-1 and time k; is the system noise covariance matrix at time k-1, represents the measurement noise covariance matrix at time k-1, is the one-step prediction mean square error matrix of the state at time k-1. After receiving satellite data and inertial navigation data at time k, the integrated navigation can calculate the forgetting factor at time k. By dynamically updating the value of the forgetting factor, the accuracy of subsequent measurement noise estimation is improved, making the combined system adapt to the complex and changing external environment.

[0035] An embodiment of the present application is: Calculate the carrier speed according to the GNSS Doppler speed calculation formula (receiver speed), and then according to the carrier speed Determine the horizontal speed of the carrier , so as to calculate the reserve coefficient later The specific process of GNSS Doppler velocity measurement calculation is: when the receiver observes more than four satellites, the current receiver velocity can be solved , the solution equation is expressed as:

[0036] remember

[0037] Using least squares we get: .

[0038] The multi-window robustness strategy includes constructing multiple innovation covariance windows and calculating the robustness factor for each window separately. , Robustness factor The formula is: , in, represents the prior observation covariance matrix, tr() represents the trace of the matrix (the sum of the diagonal elements of the matrix), N is the number of epochs, is the measurement coefficient matrix at time k, is the one-step prediction mean square error matrix of the state between time k-1 and time k, is the new information vector (new information sequence) between the one-step predicted value and the observed value at time ki.

[0039] The innovation covariance window includes a fixed solution window, a floating point solution window, and a single point solution window. Each window records the estimated value of the innovation covariance matrix of the fixed solution, the estimated value of the innovation covariance matrix of the floating point solution, and the estimated value of the innovation covariance matrix of the single point solution. The three windows respectively store the estimated values ​​of the innovation covariance matrix of the fixed solution, the floating point solution, and the single point solution (the pseudorange difference is equated with the single point solution) of N epochs. The estimated value of the observation vector covariance matrix is ​​calculated based on the estimated value of the innovation covariance matrix in the window and the theoretical value of the innovation covariance matrix at the current k moment, and then the estimated value of the observation vector covariance matrix is ​​compared with the prior observation covariance matrix. Structural robustness factor .

[0040] Calculate the robustness factors of the three windows respectively , through the formula Determine the optimal robustness factor , based on the optimal robustness factor Amplify the measurement noise covariance matrix , the amplification formula is: (Continue to use represents the amplified measurement noise covariance matrix), which can reduce the impact of different distributions of new information in a single window and improve the accuracy of subsequent measurement updates.

[0041] The multi-window robustness strategy includes: based on the measurement noise covariance matrix Calculate the new information vector Vk between the predicted value and the observed value, and update the measurement noise covariance matrix based on the new information vector Vk , based on the updated measurement noise covariance matrix and the measurement covariance matrix Update the measurement value together.

[0042] Calculate the robustness factor During the process, It may be less than zero, resulting in the measurement noise covariance matrix of the subsequent calculation Loss of positive definiteness makes subsequent calculation of robustness factor When the internal denominator of the formula is zero (filter divergence), it affects the update result of the measurement value. To avoid the above problem, the measurement noise covariance matrix is ​​estimated based on the Allan variance , the specific formula is: , Simplifying the recursion, we can get: , In the above formula, If the gradually disappearing memory algorithm is used for estimation, then: , , in, , k and k-1 represent the current moment and the previous moment respectively, represents the forgetting factor at time k, and b represents the initial forgetting factor set in advance.

[0043] By limiting the measurement noise covariance matrix The upper and lower limit conditions and ( ,in is the initial value of the measurement noise covariance matrix), ensuring The non-negative definiteness of the proposed method can reduce the amount of matrix calculations and greatly improve the reliability of filtering.

[0044] Positioning update, determine whether to continue filtering, if not, end filtering, if yes, update the current time and execute the state estimation step.

[0045] After updating the measured value, whether to continue filtering is determined based on whether navigation is ended. If navigation is ended, filtering is ended. If navigation is continued, k=k+1, and the state estimation step is executed to continue receiving satellite data and inertial navigation data at time k.

[0046] The working principle and working process of the present invention are as follows: At time k, receive satellite data or inertial navigation data, and estimate the position data and attitude data of the carrier based on the measurement matrix (the position data and attitude data together constitute the position and attitude data of the carrier); The position data and attitude data at time k are brought into the Kalman filter. The Kalman filter has a regulating factor model to determine whether the filter is divergent. The regulating factor model has a reserve coefficient. , Dynamically adjust according to the carrier speed so that The values ​​of are not necessarily the same. Therefore, the strictness of the judgment of whether the Kalman filter diverges is directly related to the speed. The Kalman filter can automatically adapt to the carrier speed, so that the integrated navigation system can navigate stably at dynamic speeds and improve the adaptive ability of the integrated navigation.

[0047] When the Kalman filter does not diverge, the Sage-Husa adaptive filter is used to update the measurement matrix, and the pose data of the carrier at time k+1 is continuously estimated based on the updated measurement matrix. This solves the problem that the forgetting factor in the Sage-Husa adaptive filter is pre-set according to the empirical value and is difficult to adapt to the complex and changing external environment.

[0048] When the Kalman filter diverges, the multi-window robustness strategy is used to update the measurement matrix, and the pose data of the carrier at time k+1 is continuously estimated based on the updated measurement matrix. The measurement noise covariance matrix can be updated without complex calculations, saving calculation effort.

[0049] The embodiments of the present invention are described in detail above, but the contents described are only preferred embodiments of the present invention and cannot be considered to limit the scope of implementation of the present invention. All equivalent changes and improvements made within the scope of the present invention should still fall within the scope of this patent.

Claims

1. An adaptive integrated navigation method applicable to multiple scenarios, characterized in that: include, State estimation: receiving satellite data and inertial navigation data at time k, and performing state estimation based on the satellite data and inertial navigation data; Measurement update, build a regulation factor model based on the state estimation results, and determine whether the Kalman filter diverges based on the regulation factor model. If yes, update the measurement covariance matrix through adaptive filtering , based on the updated measurement covariance matrix Complete measurement update; No, update the measurement covariance matrix through the multi-window anti-error strategy , based on the updated measurement covariance matrix Complete measurement update; Positioning update, determine whether to continue filtering, if not, end filtering, if yes, k=k+1, execute state estimation step.

2. The adaptive integrated navigation method applicable to multiple scenarios according to claim 1, characterized in that: The formula of the adjustment factor model is: ， in, is the new information sequence at time k; is the measurement coefficient matrix; is the one-step prediction mean square error matrix of the state between time k-1 and time k; is the measurement covariance matrix at time k; is the reserve factor, and ; tr represents the trace of the matrix; The calculation formula is: ， in, is the measurement vector at time k; It is a one-step prediction of the state between time k-1 and time k.

3. The adaptive integrated navigation method applicable to multiple scenarios according to claim 2, characterized in that: The reserve factor The calculation formula is: ， Among them, exp represents the exponential function with the natural constant e as the base, is the horizontal velocity of the carrier, and Auxiliary regulatory factors.

4. The adaptive integrated navigation method applicable to multiple scenarios according to claim 3, characterized in that: The satellite data is collected by a receiver, and the calculation formula of the receiver speed is: ， Where b represents the forgetting factor, is the velocity vector of satellite j, with a superscript Indicates satellite, Indicates the receiver speed, represents the receiver clock drift, is the pseudorange observation noise change rate of satellite j, with the superscript represents the rate of change of the corresponding variable, represents the wavelength, is the Doppler observation value; According to the receiver speed Determine the horizontal speed of the carrier .

5. The adaptive integrated navigation method applicable to multiple scenarios according to claim 4, characterized in that: The updating formula of the forgetting factor is: ， in, It is the n-order state one-step transfer matrix between time k-1 and time k; is the system noise covariance matrix at time k-1, represents the measurement noise covariance matrix at time k-1.

6. The adaptive integrated navigation method applicable to multiple scenarios according to claim 1, characterized in that: The multi-window anti-error strategy includes constructing multiple estimates of the innovation covariance matrix, each of which is calculated with the theoretical value of the innovation covariance matrix at the current moment to obtain an estimate of the observation vector covariance matrix, and the estimate of the observation vector covariance matrix is ​​calculated with the prior observation covariance matrix. Structural robustness factor ,pass Amplify the measurement noise covariance matrix .

7. The adaptive integrated navigation method applicable to multiple scenarios according to claim 6, characterized in that: The estimated value of the innovation covariance matrix includes the estimated value of the innovation covariance matrix of the fixed solution, the estimated value of the innovation covariance matrix of the floating-point solution, and the estimated value of the innovation covariance matrix of the single-point solution.

8. The adaptive integrated navigation method applicable to multiple scenarios according to claim 6, characterized in that: The robustness factor The formula is: ， ， in, represents the prior observation covariance matrix, is the new information vector between the one-step predicted value and the observed value at time ki, and N is the number of epochs.

9. The adaptive integrated navigation method applicable to multiple scenarios according to claim 6, characterized in that: The multi-window robustness strategy includes estimating the measurement noise covariance matrix based on the Allan variance , the specific formula is: ， Simplifying the recursion, we can get: ， In the above formula, Using the gradually disappearing memory algorithm for estimation, we have: ， ， in, and represents the measurement noise covariance matrix The upper and lower limit conditions, , k and k-1 represent the current moment and the previous moment respectively, represents the forgetting factor at time k, and b is set in advance by humans.

Images