Robust alignment method, system and terminal for information filtering of strapdown inertial-based navigation system

Abstract

The application belongs to the technical field of SINS initial alignment, and discloses a strapdown inertial base navigation system information filtering robust alignment method, a system and a terminal, constructs a strapdown inertial base navigation system initial alignment model, carries out time updating and measurement updating of an information Kalman filtering algorithm, calculates Mahalanobis distance between an observation and observation innovation, calculates a corresponding weight of the observation innovation, introduces an inflation factor λ based on a Mahalanobis distance algorithm, corrects an estimated R matrix, and uses the corrected R matrix to carry out IKF algorithm measurement updating, that is, measurement updating of the robust IKF. k The RIKF method provided by the application is not only suitable for initial alignment of a waterborne system velocity aided inertial base navigation system, but also suitable for initial alignment of a global satellite navigation system signal availability poor or shielding condition, a waterborne system velocity aided inertial base navigation system, and can be further applied to the field of integrated navigation.

Description

Technical Field

[0001] The invention belongs to the technical field of initial alignment of a strapdown inertial navigation system (SINS), and in particular relates to a strapdown inertial navigation system information filtering robust alignment method, system and terminal. Background Art

[0002] At present, the Strapdown Inertial Navigation System (SINS) has become the main means of long-duration, long-range, high-precision navigation for autonomous underwater vehicles (AUVs) due to its advantages such as strong autonomy, high concealment, simple structure, small size and low cost. SINS does not emit electromagnetic waves or other signals to the outside world during navigation and is not easily disturbed by the outside world. It has the unique advantages of high concealment and strong anti-interference ability. Therefore, SINS has become the mainstream device for AUV autonomous navigation. At present, AUVs do not yet have autonomous long-distance navigation capabilities in the true sense, and the main reason is the accuracy problem of navigation, that is, the drift error accumulation problem of the autonomous inertial navigation system (INS) (abbreviated as "inertial navigation system" or "inertial navigation"). At present and for a long time to come, the entire voyage still requires the space-based positioning and navigation system to regularly calibrate the INS. However, high-frequency radio signals attenuate extremely rapidly with water depth. During the calibration process, the AUV needs to surface or near the water surface, which increases the probability of its own exposure and limits its ability to sail stealthily for a long time.

[0003] Initial alignment is a necessary stage before a SINS (Synthetic Inertial Navigation System) performs navigation calculations. Its accuracy is crucial to the SINS's solution accuracy, and the vehicle's rapid response capability is largely dependent on the alignment time. Underwater SINS initial alignment requires external navigation information. Currently, commonly used underwater navigation aids include acoustic positioning and oceanographic field matching. Oceanographic field matching is still in the theoretical research stage, and the development of oceanographic fields is not yet sufficient for practical applications. The deployment cost of underwater acoustic positioning systems is high, especially in open seas and deep waters. A Doppler velocity log (DVL) uses the Doppler frequency shift between the acoustic pulse transmitted signal and the scattered echo signal to determine the vehicle's velocity relative to the water layer or bottom. This velocity measurement error does not accumulate over time. DVLs can provide reliable external velocity information to the SINS during AUV navigation at great depths and in wide waters. The inertial navigation system composed of a SINS and DVL enables all-weather, fully autonomous navigation. During the initial alignment of an underwater dynamic base, the external environment is complex and changeable. DVL velocity measurement is easily contaminated by non-Gaussian noise, which causes the performance of the initial alignment method based on the standard Kalman filter (KF) framework to deteriorate or even diverge.

[0004] The Information Kalman Filter (IKF) is a variant of the standard Kalman Filter (KF). While the standard KF uses the state error covariance matrix to represent the Gaussian distribution, the IKF uses the information matrix (the inverse of the state error covariance matrix) to represent the Gaussian distribution. These two representations are dual to each other. Compared to the KF, the IKF has the advantage of not requiring accurate initial state information, resulting in more efficient measurement updates. Since the IKF is also a filtering method based on the KF framework, practical applications also require the Gaussian distribution assumption for measurement noise. Therefore, when the observed quantity is contaminated by non-Gaussian noise such as outliers, the IKF filtering performance will degrade or even diverge.

[0005] Through the above analysis, the problems and defects of the existing technology are as follows:

[0006] (1) Among the existing underwater assisted navigation technologies, ocean physical field matching is still in the theoretical research stage, and the construction of ocean physical fields cannot meet the needs of practical applications.

[0007] (2) Among the existing underwater assisted navigation technologies, the deployment cost of the underwater acoustic positioning system is high, especially in the open sea and deep sea, where the deployment is more difficult.

[0008] (3) In the initial alignment of the underwater dynamic base, the prior information of the state quantity is not always accurately known, and the external environment is complex and changeable. The DVL velocity measurement is easily contaminated by non-Gaussian noise, causing the performance of the initial alignment method based on the KF framework to deteriorate or even diverge. Summary of the Invention

[0009] In response to the problems existing in the prior art, the present invention provides a method, system and terminal for robust alignment of information filtering of a strapdown inertial navigation system, and in particular relates to a method, system, medium, device and terminal for robust alignment of information filtering of a strapdown inertial navigation system assisted by vehicle velocity.

[0010] The present invention is achieved in that a strapdown inertial based navigation system information filtering (IKF) robust alignment method is provided, wherein the strapdown inertial based navigation system information filtering robust alignment method comprises:

[0011] Based on the Mahalanobis distance algorithm, the expansion factor λ is introduced. When the observation is contaminated by non-Gaussian noise, the measurement noise covariance matrix R is expanded to obtain and use Substitute R for IKF measurement update to obtain the robust IKF.

[0012] Furthermore, the strapdown inertial navigation system information filtering robust alignment method comprises the following steps:

[0013] Step 1: construct the initial alignment model of the strapdown inertial navigation system;

[0014] Step 2: Update the time of the IKF algorithm;

[0015] Step 3: Calculate the Mahalanobis distance between the observed value and the observed new information, and calculate the corresponding weight of the observed new information;

[0016] Step 4: Based on the Mahalanobis distance algorithm, introduce the expansion factor λ k , correct the measurement noise covariance matrix R;

[0017] Step 5: Perform robust IKF (RIKF) measurement update: Use the corrected R matrix to perform IKF algorithm measurement update;

[0018] Step 6: Use RIKF to estimate the state error and correct the SINS navigation solution error.

[0019] Further, the construction of the initial alignment model of the strapdown inertial navigation system in step one includes:

[0020] (1) Initial alignment state equation

[0021] The selected state quantity does not consider the speed and position information of the altitude channel, so the selected state quantity is:

[0022]

[0023] Where, δL and δζ are latitude error and longitude error respectively; δv E ,δv N are the eastward velocity error and the northward velocity error respectively; α=[α x ; α y ; α z ] is the error angle of the Euler platform; is the gyroscope constant drift; is the accelerometer bias.

[0024] The state equation corresponding to the initial alignment of the strapdown inertial navigation system based on the SINS / DVL combination is:

[0025]

[0026] Where w SINS ~N(0,Q SINS ) is the system noise, Q SINS is the system noise covariance matrix. The state transfer matrix F established by the state equation SINS for:

[0027]

[0028] Where F is a 7×7 matrix, and the non-zero elements in F are as follows:

[0029] F 1,4 =1 / R e , F 2,1 =(v E / R e )tanLsecL,F 2,3 =secL / R e , F 3,3 =(v N / R e )tanL;

[0030] F 3,4 =2ω ie sinL+(v E / R e )tanL,F 3,6 =-f U , F 3,7 =f N ;

[0031] F 4,3 =-[2ω iesinL+(v E / R e )tanL],F 4,5 =f U ;

[0032] F 4,7 =-f E , F 5,4 =-1 / R e , F 5,6 =ω ie sinL+(v E / R e )tanL,F 5,7 =-[ω ie cosL+(v E / R e )];

[0033] F 6,1 =-ω ie sinL,F 6,3 =1 / R e , F 6,5 =-[ω ie sinL+(v E / R e )tanL],F 6,7 =-v N / R e ;

[0034] F 7,1 =ω ie cosL+(v E / R e )(secL) 2 , F 7,3 =tanL / R e , F 7,5 =ω ie cosL+v E / R e ;

[0035] F 7,6 =v N / R e ;

[0036] Where, G is a 7×6 matrix and is expressed as follows:

[0037]

[0038] Where, Represents the posture matrix The first two lines of .

[0039] (2) Initial alignment measurement equation

[0040] DVL is combined with SINS, and the velocity error is selected as the observation quantity, and the navigation speed measured by SINS is selected. The velocity v under load measured by DVL b The difference in projection on the n-frame is taken as the observed quantity, and the measurement equation corresponding to the initial alignment of the strapdown inertial navigation system based on the SINS / DVL combination is:

[0041]

[0042] Where H v is the measurement matrix, the measurement noise V v ~N(0,R v ), R v is the measurement noise array; under the condition of linear small misalignment angle, If the eastward velocity error δv is selected E and north velocity error δv N As an observation, we have:

[0043]

[0044] In practical applications, replace Get the measurement matrix H v for:

[0045]

[0046] Where, Representation matrix The first two lines of .

[0047] Furthermore, the time update of the IKF algorithm in step 2 includes:

[0048]

[0049]

[0050] Where, F k-1 is the state transfer matrix at time k-1, H k is the noise matrix measured at time k, is the prior estimate of the state quantity, P kk-1 is the prior estimate of the state error covariance matrix, IP=P -1 is the information matrix.

[0051] Furthermore, the calculation of the Mahalanobis distance between the observation value and the observation innovation in step 3 and the calculation of the corresponding weight of the observation innovation include:

[0052] Select the observation quantity at time k Prior estimates of the observed quantity As the evaluation index, the evaluation index θ at time k is k The definition of is as follows:

[0053]

[0054] Where, is the Mahalanobis distance; is the prior estimate of the measurement error covariance matrix; μ k is the observation innovation vector; for the real observation quantity If the evaluation index θ k satisfy but will be marked as a normal observation; otherwise, if the evaluation index θ k satisfy but will be marked as abnormal observations.

[0055] Furthermore, in step 4, the Mahalanobis distance algorithm is used to introduce the expansion factor λ k , the corrections to the R matrix include:

[0056] By introducing the expansion factor λ k Used to expand the measurement noise covariance matrix R k :

[0057]

[0058] Substitute the noise covariance matrix expression into the k-time evaluation index θ k The expression of , we get:

[0059]

[0060] The new k-time evaluation index θ k The expression is transformed into solving λ k For nonlinear problems, then:

[0061]

[0062] Where λ k Solved by Newton's iteration method, λ k (i+1) and λ k The relationship (i) is expressed as:

[0063]

[0064] Where, And λ k (i) The initial value is λ k(0)=1; when the evaluation index meets When , the iteration terminates.

[0065] The IKF algorithm measurement update using the modified R matrix in step 5 includes:

[0066] When the probability parameter α is set to 0.99, the efficiency of the robust filtering method is 99%, and the chi-square distribution value of 2 degrees of freedom is use Instead of R k The RIKF algorithm is obtained by performing standard IKF filtering. The RIKF algorithm measurement update equation at time k is as follows:

[0067]

[0068]

[0069]

[0070] Wherein, k=1, 2, 3, ...

[0071] Another object of the present invention is to provide a strapdown inertial navigation system information filtering robust alignment system using the strapdown inertial navigation system information filtering robust alignment method, wherein the strapdown inertial navigation system information filtering robust alignment system comprises:

[0072] An initial alignment model building module is used to build an initial alignment model for the strapdown inertial navigation system;

[0073] Time measurement update module, used to update the time of the IKF algorithm;

[0074] The Mahalanobis distance calculation module is used to calculate the Mahalanobis distance between the observation quantity and the observation innovation, and calculate the corresponding weight of the observation innovation;

[0075] R matrix correction module, used to introduce the expansion factor λ based on the Mahalanobis distance algorithm k , correct the R matrix;

[0076] RIKF measurement update module, used to use the corrected R matrix to perform IKF algorithm measurement update;

[0077] The error correction module is used to use the RIKF to estimate the state error and correct the SINS navigation solution error.

[0078] Another object of the present invention is to provide a computer device, comprising a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor performs the following steps:

[0079] Based on the Mahalanobis distance algorithm, the expansion factor λ is introduced. When the observation is contaminated by non-Gaussian noise, the measurement noise covariance matrix R is expanded to obtain and use Substituting R for IKF measurement update, we get the Robust IKF (RIKF).

[0080] Another object of the present invention is to provide a computer-readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the processor performs the following steps:

[0081] Based on the Mahalanobis distance algorithm, the expansion factor λ is introduced. When the observation is contaminated by non-Gaussian noise, the measurement noise covariance matrix R is expanded to obtain and use Substituting R for IKF measurement update, we obtain the robust IKF (RIKF).

[0082] Another object of the present invention is to provide an information data processing terminal, which is used to implement the strapdown inertial navigation system information filtering robust alignment system.

[0083] In combination with the above technical solutions and the technical problems solved, please analyze the advantages and positive effects of the technical solutions to be protected by the present invention from the following aspects:

[0084] First, in view of the technical problems existing in the above-mentioned prior art and the difficulty of solving these problems, this paper closely combines the technical solutions to be protected by the present invention and the results and data during the research and development process, and analyzes in detail and in depth how the technical solutions of the present invention solve the technical problems and some creative technical effects brought about by solving the problems. The specific description is as follows:

[0085] In response to the problems of the existing physical field matching auxiliary navigation means being immature and the acoustic navigation means being relatively complex to deploy and having high costs, the present invention utilizes a Doppler odometer (DVL) to provide speed information to assist the SINS in performing initial alignment or combined navigation. The strapdown inertial navigation system composed of the SINS and the DVL can realize the initial alignment or combined navigation of the autonomous underwater vehicle (AUV) in all weather conditions and in all water areas. However, due to the complexity of the underwater environment (factors such as water flow rate and changes in bottom terrain), the DVL speed information is easily contaminated by non-Gaussian noise such as wild values, and the prior information of the state quantity is not always accurately known. The present invention introduces an expansion factor λ based on the Mahalanobis Distance (MD) algorithm. When the observed quantity is contaminated by non-Gaussian noise, the measurement noise covariance matrix R is expanded to obtain and use Replacing R to perform IKF measurement update results in the robust IKF (RIKF).

[0086] The RIKF method proposed in this paper is not only applicable to the initial alignment of an underwater vehicle-based velocity-assisted inertial-based navigation system, but also to the initial alignment of an underwater vehicle-based velocity-assisted inertial-based navigation system under conditions of poor availability or shielding of the Global Satellite Navigation System (GNSS) signal. It can also be further extended to the field of integrated navigation.

[0087] Second, considering the technical solution as a whole or from the perspective of the product, the technical effects and advantages of the technical solution to be protected by the present invention are described in detail as follows:

[0088] According to the experimental results, in an underwater non-Gaussian environment, compared with KF and IKF, the RIKF provided by the present invention can effectively improve the alignment accuracy while ensuring the robustness of underwater dynamic alignment. It can also effectively overcome the problems of unknowable prior information of state quantities during the initial alignment process and the degradation of filtering performance when the observation quantities are contaminated by different types of non-Gaussian noise.

[0089] Third, as auxiliary evidence for the inventiveness of the claims of the present invention, it is also reflected in the following important aspects:

[0090] (1) The technical solution of the present invention solves a technical problem that people have been eager to solve but have never been able to solve successfully: for the initial alignment above the water surface with the assistance of GNSS signals, the research is relatively mature because the position and velocity in the navigation coordinate system (n system) are used as observation information. In the dynamic initial alignment of underwater AUV, the external environment is complex and changeable. For example, the complex changes in the bottom terrain and the changes in water flow rate will affect the DVL's measurement of velocity, making the measurement noise easily contaminated by non-Gaussian noise, so that the statistical characteristics of the measurement noise have non-Gaussian characteristics. This will cause the IKF filtering performance based on the standard Kalman filter framework to deteriorate or even diverge. In order to further accurately obtain the attitude and heading information of SINS in an underwater non-Gaussian environment, the present invention proposes an underwater dynamic alignment method based on robust IKF without GNSS assistance. The invented method can not only effectively overcome the non-Gaussian colored noise caused by the complex external environment, but also achieve high-precision initial alignment when the prior information of the state quantity is unknown or inaccurate, effectively overcoming the technical difficulties in engineering applications in which the use of Kalman filtering to achieve initial alignment requires assuming that the noise characteristics conform to the Gaussian distribution characteristics and obtaining a relatively accurate initial value P0 of the state error covariance matrix P.

[0091] (2) The technical solution of the present invention overcomes technical bias: Information Kalman Filter (IKF) is widely used in target tracking, collaborative navigation, SLAM, precision measurement and other fields due to its unique advantage of simpler representation of global uncertainty. The inventor searched through China National Knowledge Infrastructure (CNKI) and did not find any application of IKF in the initial alignment of inertial navigation system. The reason is that, on the one hand, the initial alignment has high speed requirements, that is, the initial alignment time is required to be as short as possible (fine alignment is generally required to be completed within 600 to 900 seconds). When IKF performs filter update, it is necessary to invert the state error covariance matrix P, which reduces the prediction efficiency during the filter time update process; on the other hand, in the initial alignment process, the initial value of P is generally selected based on empirical values. However, in practice, the prior information of the state quantity is not always accurately known. For example, the initial attitude angle obtained by coarse alignment may not necessarily meet the small misalignment angle requirement based on KF filtering. At this time, if the empirical value is selected, the filtering performance will be reduced or even diverge. IKF does not require the initial value of P to be known accurately. IKF allows the initial value of P to be set to infinity, so that the initial alignment will no longer be restricted by the accurate knowledge of the initial value of P. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0093] Figure 1 Flowchart of a method for robust alignment of information filtering of a strapdown inertial navigation system provided by an embodiment of the present invention;

[0094] FIG2 is a schematic diagram of a DVL output according to an embodiment of the present invention; wherein:

[0095] FIG2( a ) is a schematic diagram of a DVL output (Gaussian noise contamination) provided by an embodiment of the present invention;

[0096] FIG2( b ) is a schematic diagram of DVL output (outlier contamination) provided by an embodiment of the present invention;

[0097] FIG3 is a schematic diagram of the RMSE (50 Monte Carlo tests) of attitude angle estimation errors of different methods provided by an embodiment of the present invention; wherein:

[0098] FIG3( a ) is a schematic diagram of the RMSE of the pitch angle estimation error of different methods provided in an embodiment of the present invention;

[0099] FIG3( b ) is a schematic diagram of the RMSE of the roll angle estimation error of different methods provided by an embodiment of the present invention;

[0100] FIG3( c ) is a schematic diagram of the RMSE of the heading angle estimation error of different methods provided by an embodiment of the present invention;

[0101] FIG4 is a schematic diagram of the TRMSE (last 100 seconds) of attitude angle estimation errors of different methods provided by an embodiment of the present invention; wherein:

[0102] FIG4( a ) is a schematic diagram of TRMSE of pitch angle estimation errors of different methods provided by an embodiment of the present invention;

[0103] FIG4( b ) is a schematic diagram of TRMSE of pitch angle estimation errors of different methods provided by an embodiment of the present invention;

[0104] FIG4( c ) is a schematic diagram of TRMSE of heading angle estimation errors of different methods provided by an embodiment of the present invention;

[0105] FIG5 is a schematic diagram of the TRMSE (last 100 seconds) of attitude angle estimation errors of different methods provided by an embodiment of the present invention; wherein:

[0106] FIG5( a ) is a schematic diagram of TRMSE of pitch angle estimation errors of different methods provided by an embodiment of the present invention;

[0107] FIG5( b ) is a schematic diagram of TRMSE of roll angle estimation errors of different methods provided by an embodiment of the present invention;

[0108] FIG5( c ) is a schematic diagram of TRMSE of heading angle estimation errors of different methods provided by an embodiment of the present invention;

[0109] FIG6 is a schematic diagram of attitude angle estimation errors using different methods provided by an embodiment of the present invention; wherein:

[0110] FIG6( a ) is a schematic diagram of pitch angle alignment errors of different methods provided by an embodiment of the present invention;

[0111] FIG6( b ) is a schematic diagram of roll angle alignment errors of different methods provided by an embodiment of the present invention;

[0112] FIG6( c ) is a schematic diagram of heading angle alignment errors of different methods provided by an embodiment of the present invention;

[0113] Figure 7 This is a structural block diagram of an information filtering robust alignment system for a strapdown inertial navigation system provided by an embodiment of the present invention;

[0114] In the figure: 1. Initial alignment model construction module; 2. Time measurement update module; 3. Mahalanobis distance calculation module; 4. R matrix correction module; 5. RIKF measurement update module; 6. Error correction module. DETAILED DESCRIPTION

[0115] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0116] In view of the problems existing in the prior art, the present invention provides a method, system and terminal for robust alignment of information filtering of a strapdown inertial navigation system. The present invention is described in detail below with reference to the accompanying drawings.

[0117] 1. Explanatory Examples In order to enable those skilled in the art to fully understand how to implement the present invention, this section provides an illustrative example that expands upon the technical solutions of the claims.

[0118] The present invention introduces the expansion factor λ based on the Mahalanobis distance (MD) algorithm. When the observed quantity is contaminated by non-Gaussian noise, the measurement noise covariance matrix R is expanded to obtain and use Substitute R for IKF measurement update to obtain RIKF. The initial alignment process using RIKF is as follows: Figure 1 shown.

[0119] like Figure 1 As shown, the strapdown inertial navigation system information filtering robust alignment method provided by the embodiment of the present invention includes the following steps:

[0120] S101, constructing the initial alignment model of the strapdown inertial navigation system;

[0121] S102, performing time update of the IKF algorithm;

[0122] S103, calculating the Mahalanobis distance between the observed value and the observed new information, and calculating the corresponding weight of the observed new information;

[0123] S104, based on the Mahalanobis distance algorithm, introduces the expansion factor λ k , correct the estimated R matrix;

[0124] S105, perform RIKF measurement update: use the corrected R matrix to perform IKF algorithm measurement update;

[0125] S106, using RIKF to estimate the state error and correct the SINS navigation solution error.

[0126] The strapdown inertial navigation system information filtering robust alignment method provided by the embodiment of the present invention specifically includes:

[0127] 1. Initial Alignment Model of Strapdown Inertial Navigation System

[0128] 1.1 Initial alignment equation of state

[0129] Since the altitude channel of SINS is independent and divergent, its altitude channel information can be accurately obtained with the help of external sensors such as barometers or water pressure gauges. Therefore, the selected state variables do not consider the velocity and position information of the altitude channel. The selected state variables are:

[0130]

[0131] Where, δL and δζ are latitude error and longitude error respectively; δv E ,δv N are the eastward velocity error and the northward velocity error respectively; α=[α x ; α y ; α z ] is the error angle of the Euler platform; is the gyroscope constant drift; is the accelerometer bias.

[0132] The state equation corresponding to the initial alignment of the strapdown inertial navigation system based on the SINS / DVL combination is shown in Equation (2).

[0133]

[0134] Where w SINS ~N(0,Q SINS ) is the system noise, Q SINS is the system noise covariance matrix. The state transfer matrix F established by formula (2) SINS for:

[0135]

[0136] Where F is a 7×7 matrix, and the non-zero elements in F are as follows:

[0137] F 1,4 =1 / R e , F 2,1 =(v E / R e )tanLsecL,F 2,3 =secL / R e , F 3,3 =(v N / R e )tanL

[0138] F 3,4 =2ω ie sinL+(v E / R e )tanL,F 3,6 =-fU , F 3,7 =f N

[0139] F 4,3 =-[2ω ie sinL+(v E / R e )tanL],F 4,5 =f U

[0140] F 4,7 =-f E , F 5,4 =-1 / R e , F 5,6 =ω ie sinL+(v E / R e )tanL,F 5,7 =-[ω ie cosL+(v E / R e )]

[0141] F 6,1 =-ω ie sinL,F 6,3 =1 / R e , F 6,5 =-[ω ie sinL+(v E / R e )tanL],F 6,7 =-v N / R e

[0142] F 7,1 =ω ie cosL+(v E / R e )(secL) 2 , F 7,3 =tanL / R e , F 7,5 =ω ie cosL+v E / R e

[0143] F 7,6 =v N / R e (4)

[0144] In formula (4), G is a 7×6 matrix, and its specific form is as follows:

[0145]

[0146] Where, Represents the posture matrix The first two lines of .

[0147] 1.2 Initial alignment measurement equation

[0148] When DVL is combined with SINS, the velocity error is generally selected as the observation quantity, that is, the velocity in the navigation system (n system) measured by SINS is selected. The velocity v under the load system (system b) measured by DVL b The difference in projection on the n-frame is taken as the observation quantity. The measurement equation corresponding to the initial alignment of the strapdown inertial navigation system based on the SINS / DVL combination is shown in Equation (6).

[0149]

[0150] Where H v is the measurement matrix, the measurement noise V v ~N(0,R v ), R v is the measurement noise array. Under the condition of linear small misalignment angle, If the eastward velocity error δv is selected E and north velocity error δv N As an observation, we have:

[0151]

[0152] In practical applications, replace The measurement matrix H can be obtained v for:

[0153]

[0154] Where, Representation matrix The first two lines of .

[0155] 2. Basic Theory of Robust Information Filtering (RIKF) Algorithm

[0156] Figure 1 This is the initial alignment process of the strapdown inertial navigation system based on the RIKF algorithm.

[0157] The IKF time update and measurement update equations are as follows.

[0158] (a) Time update equation

[0159]

[0160]

[0161] (b) Measurement update equation

[0162]

[0163]

[0164]

[0165] Where, F k-1 is the state transfer matrix at time k-1, and its expression is shown in formula (4). k is the measurement noise matrix at time k, and its expression is shown in formula (7). is the prior estimate of the state quantity, P kk -1 is the prior estimate of the state error covariance matrix, IP = P -1 is the information matrix.

[0166] In order to achieve the robustness of the standard IKF, the observation quantity at time k is selected Prior estimates of the observed quantity As the evaluation index, the evaluation index θ at time k is k The definition of is shown in formula (14).

[0167]

[0168] Where, is the Mahalanobis distance; is the prior estimate of the measurement error covariance matrix; μ k is the observation innovation vector. For the real observation If its evaluation index θ k satisfy but will be marked as a normal observation; otherwise, if its evaluation index θ k satisfy but It will be marked as an abnormal observation, and the expansion factor λ is introduced k Used to expand the measurement noise covariance matrix R k ,Right now:

[0169]

[0170] Substituting formula (15) into formula (14) yields:

[0171]

[0172] Formula (16) can be transformed into solving λ kThe nonlinear problem is shown in Equation (17).

[0173]

[0174] Where λ k It can be solved by Newton's iterative method. Therefore, λ k (i+1) and λ k The relationship (i) can be expressed as:

[0175]

[0176] Where, And λ k (i) The initial value is λ k (0)=1. When the evaluation index satisfies The iteration ends when . The present invention sets the probability parameter α to 0.99, that is, the efficiency of the robust filtering method is 99%, and the 2-degree-of-freedom chi-square distribution value is use Instead of R k The RIKF algorithm is obtained by performing standard IKF filtering. The RIKF algorithm measurement update equation at time k (k = 1, 2, 3...) is as follows:

[0177]

[0178]

[0179]

[0180] like Figure 7 As shown, the strapdown inertial navigation system information filtering robust alignment system provided by the embodiment of the present invention includes:

[0181] Initial alignment model construction module 1, used for constructing the initial alignment model of the strapdown inertial navigation system;

[0182] Time measurement update module 2, used for time update of IKF algorithm;

[0183] Mahalanobis distance calculation module 3, used to calculate the Mahalanobis distance between the observation value and the observation innovation, and calculate the corresponding weight of the observation innovation;

[0184] R matrix correction module 4 is used to introduce the expansion factor λ based on the Mahalanobis distance algorithm k , correct the estimated R matrix;

[0185] RIKF measurement update module 5, used to perform IKF algorithm measurement update using the corrected R matrix;

[0186] The error correction module 6 is used to use the RIKF to estimate the state error and correct the SINS navigation solution error.

[0187] 2. Application Examples: In order to demonstrate the creativity and technical value of the technical solution of the present invention, this section provides application examples of the claimed technical solution on specific products or related technologies.

[0188] The technical solution of this invention is primarily applicable to underwater AUVs. Domestic research on AUV technology began in the 1980s. Since the proposal of the underwater vehicle research plan in 1979, a growing number of domestic institutions have been conducting research on underwater vehicles. Over these years, underwater vehicles have made significant progress and achieved a series of results.

[0189] In 1994, a certain automation institute successfully developed China's first AUV, the "Explorer," marking a technological leap from tethered to untethered. This AUV successfully underwent multiple sea trials, reaching a depth of 1,000 meters and reaching the internationally advanced level at the time. In 1992, the Shenyang Institute of Automation of the Chinese Academy of Sciences, in collaboration with the 702nd Research Institute of China Shipbuilding Industry Corporation, the Institute of Acoustics of the Chinese Academy of Sciences, and the Russian Institute of Marine Technology, initiated a project to develop the 6,000-meter-class AUV, the "CR-01." This model was successfully developed and sea-tested in 1995, making China one of the few countries in the world capable of developing a 6,000-meter-class AUV. Building on the CR-01, the CR-02 was successfully developed in 1997. This model addressed key navigation and control technologies in complex terrain. Its navigation system utilizes an integrated INS / DVL / LBL navigation system, keeping positioning errors to within ten meters. In recent years, a certain automation institute has achieved remarkable results in the field of AUVs. The institute has developed two AUV technology systems: the "Qianlong" series and the "Exploration" series. The "Qianlong" series AUVs are primarily developed for deep-sea resource exploration, while the "Exploration" series is primarily used for marine scientific research. The "Qianlong" series includes four main models, "Qianlong No. 1" through "Qianlong No. 4." "Qianlong No. 1" weighs 1,500 kilograms and has a maximum diving depth of 6,000 meters, covering 97% of the world's oceans, with an endurance of up to 30 hours. "Qianlong No. 2" and "Qianlong No. 3" have a maximum diving depth of 4,500 meters. The navigation system of the Qianlong series AUVs includes inertial navigation, DVL, and underwater acoustic positioning equipment. "Qianlong No. 1" supports both ultra-short baseline and long baseline acoustic positioning and inertial navigation integrated navigation schemes. Using INS / LBL integrated navigation, "Qianlong No. 1" achieves an absolute positioning accuracy of better than 2.63 meters. In 2016, the Qianlong-2 conducted autonomous navigation operations in complex seabed environments, achieving a single dive time of 32 hours and 13 minutes, with a heading accuracy of ±1° and a navigation accuracy of 0.5%. The "Exploration" series includes the "Exploration 100," "Exploration 1000," and "Exploration 4500." The "Exploration 100," developed with support from the National 863 Program, is a small autonomous underwater vehicle (AUV) weighing only 47 kilograms and capable of a maximum diving depth of 100 meters. The "Exploration 1000," with a maximum diving depth of 800 meters, is designed to obtain hydrological data in the sensitive Kuroshio Current area.

[0190] In summary, AUVs are important tools for ocean exploration, marine scientific research, marine environmental protection, marine life protection, and marine military reconnaissance. They are also an important component of the user end of underwater positioning, navigation and timing (PNT) system construction. The technology of AUVs is also relatively mature. The inertial-based navigation system composed of an inertial navigation system (INS) and a Doppler velocimeter (DVL) has become the mainstream navigation method for AUVs. The method of the present invention can improve the adaptability of existing AUV products to the environment and their rapid startup capabilities during the initial alignment process.

[0191] 3. Evidence of the effects of the embodiments: The embodiments of the present invention have achieved some positive effects during the development or use process, and indeed have great advantages over the existing technology. The following content describes them with reference to the data, charts, etc. of the experimental process.

[0192] The RIKF was validated based on shipborne measured data. The experimental data was collected from a shipborne experimental system. The main performance indicators of the inertial measurement unit (IMU) and Doppler velocity log (DVL) used in the experimental system are shown in Tables 1 and 2, respectively. As can be seen from Tables 1 and 2, the IMU data update rate (200 Hz) is significantly higher than the DVL data update rate (1 Hz).

[0193] Table 1 Main performance indicators of IMU

[0194]

[0195]

[0196] Table 2 DVL performance indicators

[0197]

[0198] A single-antenna GPS receiver was also installed on the test vessel, outputting velocity and position information with a data update rate of 1Hz. GPS output data was combined with IMU output data for navigation, generating reference attitude, velocity, and position information, which served as the attitude, velocity, and position references in the experiments, respectively. The onboard experiments were conducted within the Yangtze River. The experimental process was designed as follows: when the experimental system was turned on, the test vessel remained moored for approximately 15 minutes; the vessel then sailed outward, moving for approximately 6.4 hours. Throughout the entire motion process, raw data from the IMU and DVL outputs, as well as GPS velocity and position data, were recorded. Two sets of 900-second dynamic data (one contaminated by Gaussian noise, the other contaminated by outliers) were selected from the measured data for initial alignment performance testing of the Kalman filter (KF), information Kalman filter (IKF), and robust information Kalman filter (RIKF). The two 900-second data sets included raw data from the gyroscope and accelerometer, velocity data from the DVL, and the corresponding attitude, velocity, and position references. The output of DVL is shown in Figure 2. As can be seen from Figure 2, in practical applications, the DVL output will be contaminated by high-intensity outliers.

[0199] Before conducting the experiments, we analyzed the shipborne measured data and discovered that in real-world applications, DVL velocity measurement information does contain outliers and non-Gaussian noise. While this outlier or non-Gaussian noise does not occur in every application, if it does occur, it can have serious consequences in military applications if not effectively addressed. For this reason, when validating the algorithm using the measured data (Figure 2(a)), the data was artificially degraded to ensure the algorithm's effectiveness in harsh environments.

[0200] In the experiment, the initial misalignment angle of SINS is set to [0.5°; 0.5°; 1°], and the initial measurement noise covariance matrix R0 = diag([0.1 2 ,0.1 2 ])m 2 / s 2 , the initial state error covariance matrix is:

[0201] Where pi is the circumference of a circle.

[0202] 1. Mixed Gaussian distribution pollution

[0203] Based on the Wiener approximation theorem, any non-Gaussian noise distribution can be represented or adequately approximated by a finite sum of Gaussian noise distributions with known probability densities. Assume that the actual probability distribution of the observation noise is as shown in Equation (22).

[0204] ρactual =(1-α)N(0,R c )+αN(0,R p ) (twenty two)

[0205] In the formula, the interference factor α satisfies 0≤α≤0.1; R c = R0 is the measurement noise covariance matrix of the DVL output velocity data, R p =κR0 (κ is the proportional coefficient) is the interference noise covariance matrix with a large standard deviation. When the interference factor α deviates from 0, the distribution of formula (22) is also called a fat-tailed distribution.

[0206] In the experiment, Equation (22) was artificially introduced into Figure 2(a). To fully verify the feasibility and effectiveness of applying RIKF to SINS initial alignment, the following two types of mixed Gaussian distribution noise pollution are considered in combination with practical applications: 1) the interference noise amplitude is fixed and the ratio varies; 2) the interference noise ratio is fixed and the amplitude varies.

[0207] (1) The interference noise amplitude is fixed, that is, R is set p =100R0, α is set to 0, 0.02, 0.04, 0.06, 0.08, and 0.1 respectively, and 50 Monte Carlo simulation tests are performed using KF, IKF, and RIKF respectively.

[0208] To evaluate the filtering performance, the root mean square error (RMSE) and the time-averaged RMSE (TRMSE) of the attitude error are selected as the performance evaluation indicators. The RMSE and TRMSE of the attitude error are defined as shown in Equations (23) and (24), respectively.

[0209]

[0210]

[0211] Where "attitude" represents the attitude angle, and M represents the total number of Monte Carlo simulation runs, with M = 50 in this experiment. In the initial alignment simulation, the TRMSE is calculated for times between 800s and 900s, i.e., T1 = 800s and T2 = 900s. The RMSE (α = 0.1) of the attitude estimation error obtained using different initial alignment methods is shown in Figure 3. The TRMSE of the attitude estimation error obtained using different initial alignment methods is shown in Figure 4.

[0212] Figure 3 shows that the KF and IKF are not robust to non-Gaussian noise. During initial alignment using the KF and IKF, when the DVL output is contaminated by non-Gaussian noise, the IKF alignment error curve and the KF heading angle alignment error curve diverge. The experimental results show that the KF's RMSE for horizontal attitude angle alignment error converges to within 0.05°, and the RMSE for heading angle alignment error converges to within 1°. The IKF's RMSE for horizontal attitude angle alignment error converges to within 0.2° and 0.5°, respectively, and the RMSE for heading angle alignment error converges to within 1°. The RIKF's RMSE for horizontal attitude angle and heading angle alignment error converges to within 0.05° and 0.5°, respectively. The initial alignment performance of RIKF is significantly better than that of KF and IKF. This is because when interference noise appears, RIKF uses the MD algorithm to expand the measurement noise array, thereby suppressing abnormal observation information, making RIKF more robust to alignment when the DVL output is contaminated by non-Gaussian noise.

[0213] Figure 4 shows that when the interference factor α is 0 (i.e., when the measurement noise is uncontaminated by interference noise), the TRMSE of the attitude estimation errors of the KF and IKF are lower than those of the other cases (α = 0.02, 0.04, 0.06, 0.08, and 0.1). This also shows that the KF and IKF have better filtering performance when α is 0 (i.e., no interference noise). However, when α increases (i.e., when the measurement noise is contaminated by interference noise), the attitude angle estimation accuracy of the KF and IKF decreases significantly. Compared with the KF and IKF, the RIKF has higher initial alignment accuracy and stability. Experimental results show that when the DVL output is contaminated by non-Gaussian noise, the RIKF can effectively suppress the impact of mixed Gaussian noise on the initial alignment results. The application of the RIKF in the initial alignment of strapdown inertial navigation systems is feasible and effective.

[0214] (b) Set α = 0.1 and κ to 50, 100, 150, 200, 250, and 300, and perform 50 Monte Carlo simulations using KF, IKF, and RIKF, respectively. The TRMSE of the pose estimation errors obtained using different initial alignment methods is shown in Figure 5.

[0215] Figure 5 shows that the RIKF achieves higher initial alignment accuracy than the KF and IKF. This also demonstrates that the RIKF is more robust when the proportion of interfering noise increases. Experimental results demonstrate that the RIKF can effectively suppress the impact of interfering noise of varying amplitudes and proportions on the initial alignment results. Under complex non-Gaussian conditions, the application of the RIKF to SINS initial alignment is feasible and effective.

[0216] 2. Wild value pollution

[0217] As shown in Figure 2(b), in practice, the DVL output is indeed contaminated by high-intensity outliers. The algorithm was validated using measured data (Figure 2(b)). Initial alignment tests were performed using KF, IKF, and RIKF, with the results shown in Figure 6.

[0218] As shown in Figure 6, when the DVL velocity information is contaminated by outliers, the alignment accuracy of KF and IKF decreases significantly, which also shows that KF and IKF are not robust to velocity outliers. Under the condition that the external auxiliary information is contaminated by outliers, the alignment performance of RIKF is significantly better than that of KF and IKF. This is because: RIKF first uses the MD algorithm to accurately identify velocity outliers, and then eliminates the influence of velocity outliers on the initial alignment through the measurement update process such as Equations (19) to (21), thereby achieving improved alignment performance compared to KF and IKF. From the alignment test and Figure 6, it can be seen that the heading angle alignment errors of KF and IKF at 900s are 0.9434° and 0.7179°, respectively, and the heading angle error curves do not converge; the heading angle alignment error of RIKF at 900s is 0.1242°, and the heading angle error curve converges stably to within 0.25° after 400s. Compared with KF and IKF, RIKF not only has higher alignment accuracy, but also its alignment error curve is more stable and has better convergence.

[0219] The experimental results show that, in underwater non-Gaussian environments, the RIKF effectively improves alignment accuracy while maintaining robustness for underwater dynamic alignment compared to the KF and IKF. The RIKF can effectively overcome the degradation of filtering performance during the initial alignment process when the observations are contaminated by various types of non-Gaussian noise.

[0220] The RIKF method proposed in this paper is not only applicable to the initial alignment of an underwater vehicle-based velocity-assisted inertial-based navigation system, but also to the initial alignment of an underwater vehicle-based velocity-assisted inertial-based navigation system under conditions of poor availability or shielding of the Global Satellite Navigation System (GNSS) signal. It can also be further extended to the field of integrated navigation.

[0221] It should be noted that the embodiments of the present invention can be implemented by hardware, software, or a combination of software and hardware. The hardware portion can be implemented using dedicated logic; the software portion can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated design hardware. Those skilled in the art will appreciate that the above-mentioned devices and methods can be implemented using computer-executable instructions and / or contained in processor control code, for example, such as a carrier medium such as a disk, CD or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. The devices and modules of the present invention can be implemented by hardware circuits such as very large-scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, or can be implemented by a combination of the above-mentioned hardware circuits and software, such as firmware.

[0222] The above description is only a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with this technical field within the technical scope disclosed by the present invention and within the spirit and principles of the present invention should be covered by the scope of protection of the present invention.

Claims

1. A strapdown inertial navigation system information filtering robust alignment method, characterized in that: The strapdown inertial navigation system information filtering robust alignment method comprises: Based on the Mahalanobis distance algorithm, the expansion factor is introduced When the observation is contaminated by non-Gaussian noise, the measurement noise covariance matrix Expand to get , and use Alternative Perform IKF measurement update to obtain robust RIKF; The strapdown inertial navigation system information filtering robust alignment method comprises the following steps: Step 1: construct the initial alignment model of the strapdown inertial navigation system; Step 2: Update the time of the IKF algorithm; Step 3: Calculate the Mahalanobis distance between the observed value and the observed new information, and calculate the corresponding weight of the observed new information; Step 4: Based on the Mahalanobis distance algorithm, introduce the expansion factor λ k , correct the estimated R matrix; Step 5: Perform RIKF measurement update: Use the corrected R matrix to perform IKF algorithm measurement update; Step 6: Use RIKF to estimate the state error and correct the SINS navigation solution error; Calculating the Mahalanobis distance between the observed value and the observed innovation in step 3 and calculating the corresponding weight of the observed innovation include: choose Observable quantity at a moment Prior estimates of the observed quantity As the evaluation index, the Mahalanobis distance between Moment Evaluation Indicators The definition of is as follows: ； Where, is the Mahalanobis distance; is the prior estimate of the measurement error covariance matrix; is the observation innovation vector; for the real observation quantity If the evaluation index satisfy ,but Will be marked as a normal observation; on the contrary, if the evaluation index satisfy ,but will be marked as abnormal observations; In step 4, the Mahalanobis distance algorithm is used to introduce the expansion factor λ k , the correction of the estimated R matrix includes: By introducing the expansion factor Used to expand the measurement noise covariance matrix : ； Substitute the noise covariance matrix expression into Moment Evaluation Indicators The expression of , we get: ； The new Moment Evaluation Indicators The expression is transformed into the solution For nonlinear problems, then: ； Where, Solved by Newton's iteration method, and The relationship is expressed as: ； Where, ,and The initial value is When the evaluation index meets When , the iteration terminates; The IKF algorithm measurement update using the modified R matrix in step 5 includes: The probability parameter Set to 0.99, the efficiency of the robust filtering method is 99%, and the chi-square distribution value of 2 degrees of freedom is ;use replace Perform standard IKF filtering to obtain the RIKF algorithm. The RIKF algorithm measurement update equation at a certain moment is as follows: ； ； ； Where, .

2. The strapdown inertial navigation system information filtering robust alignment method as claimed in claim 1, wherein: The construction of the initial alignment model of the strapdown inertial navigation system in step 1 includes: (1) Initial alignment state equation The selected state quantity does not consider the speed and position information of the altitude channel, so the selected state quantity is: ； Where, 、 are latitude error and longitude error respectively; 、 They are the eastward velocity error and the northward velocity error respectively; is the Euler platform error angle; is the gyroscope constant drift; is the accelerometer bias; The state equation corresponding to the initial alignment of the strapdown inertial navigation system based on the SINS / DVL combination is: ； Where, is the system noise, is the system noise covariance matrix; the state transfer matrix established by the state equation for: ； Where, is a 7×7 matrix, The non-zero elements in are as follows: ， ， ， ； ， ， ； ， ， ； ， ， ， ； ， ， ， ； ， ， ； ； Where, ; It is a 7×6 matrix and is expressed as follows: ； Where, , Represents the posture matrix The first two lines of (2) Initial alignment measurement equation DVL is combined with SINS, and the velocity error is selected as the observation quantity, and the navigation speed measured by SINS is selected. The speed under load measured by DVL exist The difference in projection on the system is taken as the observed quantity, and the measurement equation corresponding to the initial alignment of the strapdown inertial navigation system based on the SINS / DVL combination is: ； Where, is the measurement matrix, measurement noise , is the measurement noise array; under the condition of linear small misalignment angle, ; If the eastward velocity error is selected and north velocity error As an observation, we have: ； In practical applications, replace , and the measurement matrix for: ； Where, Representation matrix The first two lines of .

3. The strapdown inertial navigation system information filtering robust alignment method as claimed in claim 1, wherein: The time update of the IKF algorithm in step 2 includes: ； ； Where, for The state transfer matrix at the moment, for The noise array is measured at all times. is the prior estimate of the state quantity, is the prior estimate of the state error covariance matrix, is the information matrix.

4. A strapdown inertial navigation system information filtering robust alignment system using the strapdown inertial navigation system information filtering robust alignment method according to any one of claims 1 to 3, characterized in that: The strapdown inertial navigation system information filtering robust alignment system includes: An initial alignment model building module is used to build an initial alignment model for the strapdown inertial navigation system; Time measurement update module, used to perform time update and measurement update of IKF algorithm; The Mahalanobis distance calculation module is used to calculate the Mahalanobis distance between the observation quantity and the observation innovation, and calculate the corresponding weight of the observation innovation; R matrix correction module, used to introduce the expansion factor λ based on the Mahalanobis distance algorithm k , correct the estimated R matrix; RIKF measurement update module, used to use the corrected R matrix to perform IKF algorithm measurement update; The error correction module is used to use the RIKF to estimate the state error and correct the SINS navigation solution error.

5. An information data processing terminal, characterized in that: The information data processing terminal is used to implement the strapdown inertial navigation system information filtering robust alignment system as described in claim 4.

Images