Integrated navigation system and method based on robust adaptive Kalman filtering

Abstract

The invention belongs to the technical field of underwater positioning navigation, and relates to an integrated navigation system and method based on robust adaptive Kalman filtering, and the system comprises a strapdown inertial navigation system, a Doppler velocimeter, a depth sensor and a data processing module; the method comprises the following steps: constructing a state space model based on an integrated navigation system, and estimating system noise parameters on line in real time by introducing a Sage-Husa adaptive filtering mechanism; performing sequential processing on the multi-dimensional measurement information by adopting a sequential measurement and variance limitation method, and setting upper and lower limits of scalar measurement noise variance; an IGGIII criterion is introduced, and gross errors of measurement information are detected through a residual filter; according to the method, noise and outlier interference can be suppressed in a complex underwater environment, and the positioning precision, robustness and stability of a navigation system are improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater navigation and positioning, and particularly relates to a combined navigation system and method based on a robust adaptive Kalman filter. BACKGROUND

[0002] An autonomous underwater vehicle (AUV) plays an important role in the fields of marine environment monitoring, seabed topography mapping and emergency rescue, and the navigation and positioning accuracy and reliability of the AUV directly affect the quality of task completion. At present, a SINS / DVL combined navigation system is a mainstream scheme for underwater navigation.

[0003] A strapdown inertial navigation system (SINS) can independently provide navigation information as a passive navigation system, but has the problem of error accumulation over time; a Doppler velocity log (DVL) can provide accurate speed measurement values and can assist in suppressing the error accumulation of the SINS. However, the underwater environment is complex, and the DVL measurement is easily disturbed by noise and outliers, and a traditional Kalman filter algorithm cannot effectively process outliers due to inaccurate preset noise statistics, resulting in a decrease in navigation accuracy.

[0004] The Sage-Husa adaptive filter in the prior art can estimate noise parameters in real time, but the measured variance is easily negative when the actual noise is small; the traditional robust filter can process outliers to a certain extent, but has insufficient adaptability. In addition, the traditional loose combination of the SINS / DVL relies on the three-dimensional speed output by the DVL, and when the number of DVL beams is less than 3, the traditional loose combination model cannot work normally and has poor robustness. Therefore, under the condition of a complex underwater environment, there is an urgent need for a combined navigation method that takes into account high accuracy, high robustness and high stability. SUMMARY

[0005] The purpose of the present application is to overcome the deficiencies in the prior art and provide a combined navigation system and method based on a robust adaptive Kalman filter.

[0006] To achieve the purpose of the present application, the following technical solutions are used.

[0007] The combined navigation method based on the robust adaptive Kalman filter comprises the following steps: S1, according to the attitude, speed and position information provided by the strapdown inertial navigation system, the four-dimensional beam direction speed information directly provided by the Doppler velocity log and the depth information provided by the depth gauge, the attitude and speed information are converted to the four-dimensional beam direction of the Doppler velocity log through a state space model, and the four-dimensional beam direction speed measurement difference is calculated by subtracting the four-dimensional beam direction speed information, and the height information in the position information is subtracted from the depth information to calculate the depth measurement difference; S2, the four-dimensional beam direction velocity measurement difference and the depth measurement difference are taken as the measurement values of the robust adaptive Kalman filtering algorithm, filtering is performed through four processes of initializing filtering parameters, filtering time updating, filtering measurement updating and information fusion, and fused information is obtained; wherein: the filtering parameters are initialized, including initializing a time tag, a system state vector, a state covariance matrix, initial values of a set system noise covariance matrix and a measurement noise covariance matrix, and a gross error detection threshold; wherein: the gross error detection threshold is set, that is, the first threshold and the second threshold in the IGG III criterion are set; filtering time updating: according to the error equation of the strapdown inertial navigation system, the state transition matrix at the current time is derived and calculated, the system state is predicted by using the state transition matrix, and the prediction error covariance matrix is calculated; filtering measurement updating: according to the prediction error covariance matrix and the noise parameter of the system estimated online in real time by using the Sage-Husa adaptive mechanism, the measurement values are sequentially processed by the sequential measurement + variance limited method, while the upper and lower limits of the measurement noise are limited, the Kalman filtering gain corresponding to the single scalar measurement is calculated, the measurement residual sequence is calculated, and the residual is standardized, the weight function matrix is generated by the IGG III criterion to update the state estimation, so as to update the state covariance matrix; information fusion: updating the time tag, outputting the optimal state estimation value and the state covariance matrix at the current time, and taking the optimal state estimation value and the state covariance matrix at the current time as the input of the next round of time updating, realizing the cyclic iteration of the filtering process.

[0008] Preferably, the state space model comprises a state equation and a measurement equation, wherein: the state equation is: in the formula: and are the system state vector and the noise vector respectively, assuming is a Gaussian white noise sequence with a mean of 0, and are the system state transition matrix and the system noise matrix respectively; wherein: , wherein, , , are the attitude errors of the carrier in the east-north-sky directions in the strapdown inertial navigation system, , , represent the velocity errors in the east, north and sky directions in the strapdown inertial navigation system, , , are the latitude, longitude and altitude position errors,​ 、 、 is the constant zero bias of the accelerometer in three directions in the carrier coordinate system, 、 、 is the gyroscope constant bias, is the constant error of the velocity value in the beam direction measured by the four-dimensional beam of the Doppler velocimeter, is the constant zero bias of the depth gauge; The measurement equation is: ; Where: is the system observation vector, is the measurement transfer matrix, is the system state vector, is the measurement noise sequence; where: the system observation vector Expressed as: ; Where: , They represent the four-dimensional beam direction velocity measurement value of the Doppler velocimeter and the output of the strapdown inertial navigation system. The velocity value under the system is converted into the velocity value in the four-dimensional beam direction of the Doppler velocimeter; Measurement transfer matrix Expressed as: ; Where: is the posture matrix; is the antisymmetric matrix of the measured velocity information; is a zero matrix, is the unit matrix; M represents the speed conversion matrix, which is expressed as: ; Where: α is the beam tilt angle.

[0009] Preferably, the filtering process of the robust adaptive Kalman filtering algorithm comprises the following steps: S31. Initialize filter parameters: S311, setting an initial time mark k=1, and initializing a system state vector based on the initial navigation state and sensor parameters of the underwater vehicle; wherein the system state vector includes the attitude error, velocity error, position error, sensor zero bias, and Doppler velocimeter beam error of the strapdown inertial navigation system; S312, initializing the state covariance matrix, and setting the initial values ​​of the diagonal elements according to the statistical characteristics of the initial state error; S313, setting initial values ​​of the system noise covariance matrix and the measurement noise covariance matrix; S314, define the rough error detection threshold, i.e. the first threshold in IGG III criterion (1.0-1.5) and the second threshold (2.5-3.0); S32, filter time update: S321, derive and calculate the state transition matrix at the current time using the error equation of the strapdown inertial navigation system; wherein: the state transition matrix contains the coupling relationship of attitude, velocity, and position error; S322, perform system state prediction according to the state transition matrix; S323, update the prediction error covariance matrix; S33, filter measurement update: S331, use the Sage-Husa filtering method to estimate the system noise parameter online in real time; S332, use the sequential measurement + variance restriction method to sequentially process the measurement value while limiting the upper and lower limits of the measurement noise; S333, calculate the Kalman filter gain corresponding to the single scalar measurement; S334, calculate the measurement residual sequence and perform standardization processing on the residual; S335, generate a weight function matrix according to the IGG III criterion; S336, update the state estimation based on the weight function matrix; S337, update the state covariance matrix; S34, information fusion: update the time tag; output the optimal state estimation value and the state covariance matrix at the current time, and simultaneously use the optimal state estimation value and the state covariance matrix at the current time as the input of the next round of time update, to realize the cyclic iteration of the filtering process.

[0010] Preferably, the specific process of using the Sage-Husa filtering method to estimate the system noise parameter online in real time is as follows: ; In the formula: is the one-step state prediction; is the state transition matrix; is the state estimation; is the measurement vector; is the state estimation mean square error matrix; is the prediction mean square error matrix; is the system noise allocation matrix; is the Kalman filter gain; is the measurement matrix; is the measurement noise variance matrix; is the one-step prediction error; is the adaptation coefficient, the initial value is =1, which is used to estimate the weight index in the estimation formula when estimating the measurement noise and process noise; is the fading factor, 0 <1, usually 0.9~0.999.

[0011] Preferably, the specific process of limiting the upper and lower limits of the measurement noise while sequentially processing the measurement value by using the sequential measurement + variance limited method includes the following steps: S51, sequentially processing the measurement value, and performing measurement update in the case of the first scalar, and the measurement update equation is:

[0012] S52, setting the upper and lower limits of the measurement noise, comparing the measurement noise with the upper and lower limits, if the measurement noise exceeds the upper limit, reducing the reliability of the measurement information; if the measurement noise is lower than the lower limit, ensuring that the measurement noise is positive, so as to limit the measurement noise in [R ]; wherein the measurement noise covariance matrix is expressed as: ; In the formula: indicates the measurement update round in the sequential filtering, indicates the first scalar, indicates the first element of the first scalar in the measurement update.

[0013] Preferably, the expression of the weight function matrix is: ; In the formula: is the input measurement prediction error; , are the first threshold value and the second threshold value respectively, both of which are constant values, generally =1.0~1.5, =2.5~3.0; the IGG III criterion divides the weight function into three segments, when the statistic is less than the threshold value , the weight is set to 1, which is equivalent to not weighting the measurement information, generally referred to as the protection zone; when the measurement error is greater than the threshold value , the weight is directly set to 0, this segment is generally referred to as the rejection zone, at this time the filter will not process this measurement information, and the measurement update process will not be performed; when the error amount is between the two threshold values, the measurement information is appropriately weighted, and the weight decreases more as the statistic increases, this segment is referred to as the weight reduction zone. ​​​​​

[0014] Preferably, the weight function matrix is ​​introduced to rewrite the state estimation in the process of online real-time estimation of system noise parameters using the Sage-Husa adaptive mechanism as follows: ; Where: D is the residual weight matrix. When D is the identity matrix, it is equivalent to the standard Kalman filter algorithm process.

[0015] Preferably, the state estimation value includes calibrated attitude, velocity, and position errors, which can be used to correct the navigation output of the strapdown inertial navigation system in real time.

[0016] The integrated navigation system based on robust adaptive Kalman filtering includes a strapdown inertial navigation system, a Doppler velocimeter, a depth meter, and a data processing module, wherein: The strapdown inertial navigation system includes an inertial measurement unit (IMU) and a strapdown inertial navigation solution unit (SINS). The IMU provides gyroscope information and acceleration information to the SINS solution unit, which then provides the carrier's attitude, velocity, and altitude information to the data processing unit. A Doppler velocimeter, used for providing the data processing unit with four-dimensional beam direction velocity information of the carrier; A depth meter, used for providing depth information of the carrier to the data processing unit; The data processing unit includes a state-space model and a robust adaptive Kalman filter algorithm, wherein: A state space model, including a state equation and a measurement equation, is used to convert attitude and velocity information in a navigation coordinate system calculated by a strapdown inertial navigation solution unit into a four-dimensional beam direction of a Doppler velocimeter, calculate a four-dimensional beam direction velocity measurement difference by subtracting the attitude and velocity information from the four-dimensional beam direction velocity information, and calculate a depth measurement difference by subtracting the altitude information and depth information calculated by the strapdown inertial navigation solution unit, and use the four-dimensional beam direction velocity measurement difference and the depth measurement difference as measurement values ​​for a robust adaptive Kalman filter algorithm; Robust adaptive Kalman filtering algorithm for information fusion, including IGG III criterion, Sage-Husa adaptive filtering algorithm and sequential measurement + variance-constrained method, where: IGGⅢ criterion, used to set the gross error detection threshold and generate the weight function matrix; Sage-Husa adaptive filtering algorithm for online real-time estimation of system noise parameters; The sequential measurement + variance-constrained method is used to sequentially process the measurement values ​​while limiting the upper and lower limits of the measurement noise.

[0017] Compared with the prior art, the present invention has the following beneficial effects: 1.The present application directly utilizes DVL raw beam velocity information for fusion based on a four-dimensional beam-based SINS / DVL tight combination model. When the effective number of DVL beams is less than 3 due to underwater environment (e.g. beams are blocked by marine organisms, poor seabed terrain reflection, etc.), the tight combination model can continuously correct SINS errors through the velocity information of the remaining effective beams. Compared with traditional loose combination models, the present application does not rely on three-dimensional velocity calculated by DVL and avoids the problem of pure inertial navigation caused by beam failure, effectively improving the anti-interference ability and continuous working ability of the system in complex underwater environments.

[0018] 2.The present application provides an improved robust adaptive Kalman filtering algorithm. Compared with traditional Kalman filtering and adaptive filtering, the Sage-Husa adaptive mechanism is used to estimate the noise statistical characteristics in real time, the IGG III robust M estimator is introduced to accurately suppress measurement outliers, and the "sequential measurement + variance restriction" is combined to avoid negative measurement variance, ensuring no divergence in low-noise and beam failure complex scenarios. The multi-dimensional measurement is decomposed into scalar sequential processing, effectively solving the problems of underwater noise priori difficulty, gross error pollution, and filter divergence, and ensuring high precision and high stability of navigation. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 Figure 1 is a schematic diagram of a SINS / DVL tight combination system model. Figure 2 Figure 2 is a flowchart of an improved robust adaptive Kalman filtering algorithm. Figure 3 Figure 3 is an IGG III weight function image. Figure 4 Figure 4 is a motion trajectory curve. Figure 5 Figure 5 is an attitude and velocity parameter curve. Figure 6 Figure 6 is a trajectory comparison between a traditional algorithm and the method proposed in the present application. Figure 7 Figure 7 is an eastward velocity error of combined navigation. Figure 8 Figure 8 is an eastward position error of combined navigation. Figure 9 Figure 9 is a northward velocity error of combined navigation. Figure 10 Figure 10 is a northward position error of combined navigation. Figure 11 Figure 11 is a spatial velocity error of combined navigation. Figure 12 Figure 12 is a spatial position error of combined navigation. Figure 13 Figure 13 is a spatial position error of combined navigation. DETAILED DESCRIPTION

[0020] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention.

[0021] Example 1, as Figure 1 As shown in FIG, the integrated navigation system based on the robust adaptive Kalman filter includes a strapdown inertial navigation system SINS, a Doppler velocimeter DVL, a depth meter PS and a data processing module, wherein: The strapdown inertial navigation system SINS includes an inertial measurement unit (IMU) and a strapdown inertial navigation solver. The IMU is responsible for providing gyroscope information and acceleration information to the strapdown inertial navigation solver. After the strapdown inertial navigation solver solves the information, it provides the carrier's attitude to the data processing unit. ,speed and location information; wherein: location information includes altitude information ; Doppler velocimeter DVL, used to provide the carrier's four-dimensional beam direction velocity information to the data processing unit ; Depth meter PS, used to provide depth information of the carrier to the data processing unit ; The data processing unit includes a state-space model and a robust adaptive Kalman filter algorithm, wherein: The state space model, including the state equation and measurement equation, is used to convert the attitude in the navigation coordinate system calculated by the strapdown inertial navigation solver into the state space model. and speed Information is converted into the four-dimensional beam direction of the Doppler velocimeter and the four-dimensional beam direction velocity information Calculate the difference in four-dimensional beam direction velocity measurement and the height information calculated by the strapdown inertial navigation solver With depth information The depth measurement difference is calculated by subtraction, and the four-dimensional beam direction velocity measurement difference and the depth measurement difference are used as the measurement values ​​of the robust adaptive Kalman filter algorithm; Robust adaptive Kalman filtering algorithm for information fusion, including IGG III criterion, Sage-Husa adaptive filtering algorithm and sequential measurement + variance-constrained method, where: IGGⅢ criterion, used to set the gross error detection threshold and generate the weight function matrix; Sage-Husa adaptive filtering algorithm for online real-time estimation of system noise parameters; The sequential measurement + variance-constrained method is used to sequentially process the measurement values ​​while limiting the upper and lower limits of the measurement noise.

[0022] Example 2, asFigure 2 The combination navigation method based on the robust adaptive Kalman filter comprises the following steps: S1, according to the attitude, speed and position information provided by the strapdown inertial navigation system (SINS), the four-dimensional beam direction speed information directly provided by the Doppler velocity log (DVL) and the depth information provided by the depth gauge (PS) , the attitude information and the speed information are converted to the four-dimensional beam direction of the Doppler velocity log through a state space model, and the four-dimensional beam direction speed information is calculated by difference to obtain the four-dimensional beam direction speed measurement difference, and the height information is calculated by difference with the depth information to obtain the depth measurement difference; S2, the four-dimensional beam direction speed measurement difference and the depth measurement difference are taken as the measurement values of the robust adaptive Kalman filter algorithm, and filtering is performed through four processes of initializing filter parameters, filter time updating, filter measurement updating and information fusion to fuse data and output state estimation values and state covariance matrix; wherein: initializing filter parameters: including setting an initial time mark, an initial system state vector, a state covariance matrix, initial values of a system noise covariance matrix and a measurement noise covariance matrix and a gross error detection threshold; wherein: the initial system state vector is initialized based on the initial navigation state of the underwater vehicle and the sensor parameters; wherein: the initial navigation state includes the initial position, speed and attitude; the sensor parameters include the gyroscope zero drift and the accelerometer zero offset; the initial system state vector includes the attitude error, speed error, position error of the strapdown inertial navigation system, the Doppler velocity log wave speed error and the sensor zero offset; the state covariance matrix is initialized, and the diagonal element initial value is set according to the statistical characteristics of the initial state error (such as the sensor factory precision index); the initial values of the system noise covariance matrix Q and the measurement noise covariance matrix R are set; the gross error detection threshold is set, that is, the first threshold and the second threshold in the IGG III criterion are set; filter time updating: according to the error equation of the strapdown inertial navigation system, the state transition matrix at the current time is derived and calculated, the system state is predicted by using the state transition matrix, and the prediction error covariance matrix is calculated; wherein: the state transition matrix includes the coupling relationship of the attitude, speed and position error; filter measurement updating: according to the prediction error covariance matrix and the noise parameter of the system estimated online in real time by using the Sage-Husa adaptive mechanism , the measurement values ​​are sequentially processed by the sequential measurement + variance-constrained method, while limiting the upper and lower limits of the measurement noise to ensure the positive definiteness of the noise covariance matrix, calculating the Kalman filter gain corresponding to the single scalar measurement, and then calculating the measurement residual sequence, and normalizing the residual, and generating the weight function matrix by the IGGⅢ criterion Update the state estimate to update the state covariance matrix; Information fusion: Update the time scale, output the optimal state estimate and state covariance matrix at the current moment, and use the optimal state estimate and state covariance matrix at the current moment as the input for the next round of time update to achieve cyclic iteration of the filtering process.

[0023] Example 3, specific process of online real-time estimation of system noise parameters using the Sage-Husa adaptive filtering mechanism: ; Where: One-step prediction for the state; is the state transfer matrix; is the state estimation; is the measurement vector; is the state estimated mean square error matrix; is the predicted mean square error matrix; Assign a matrix to the system noise; is the Kalman filter gain; is the measurement matrix; is the measurement noise variance matrix; is the one-step prediction error; is the adaptation coefficient, the initial value =1, which serves as the weighting index in the estimation formula when estimating measurement noise and process noise; is the fading factor, 0< <1, usually Take 0.9~0.999.

[0024] Example 4, a specific process of sequentially processing the measurement values ​​using the sequential measurement + variance-constrained method while limiting the upper and lower limits of the measurement noise, includes the following steps: S51, sequentially process the measured values, In the case of sub-scalar measurement, the measurement update equation is: ; S52, set the upper and lower limits of the measurement noise, compare the measurement noise with the upper and lower limits, if it exceeds the upper limit, reduce the credibility of the measurement information; if it is lower than the lower limit, ensure that the measurement noise is positive, thereby reducing the measurement noise. Restricted to [ ]; where: measurement noise covariance matrix The expression is: ; In the formula: represents the measurement update round in the sequential filtering, represents the th scalar, represents the th element in the th measurement update.

[0025] In the embodiment 5, the expression of the weight function matrix is: ; In the formula: is the input measurement prediction error; , are respectively the first threshold value and the second threshold value, both of which are constants, and generally =1.0~1.5, =2.5~3.0; the IGG III criterion divides the weight function into three segments, when the statistic is less than the threshold value , the weight is set to 1, which is equivalent to not weighting the measurement information, and is generally called the protection zone; when the measurement error is greater than the threshold value , the weight is directly set to 0, which is generally called the rejection zone, and at this time the filter will not process this measurement information and will not perform the measurement update process; when the error amount is between the two threshold values, the measurement information is appropriately weighted, and the weight decreases more as the statistic increases, which is called the weight reduction zone.

[0026] In the embodiment 6, the state estimation in the process of online real-time estimation of the system noise parameter by using the Sage-Husa adaptive filtering mechanism is rewritten as:

[0027] In the formula: D is the residual weighted matrix, and when D is the unit matrix, it is equivalent to the standard Kalman filtering algorithm process.

[0028] Preferably, the state estimation value contains the corrected attitude, velocity, and position error, and is used for real-time correction of the navigation output of the strapdown inertial navigation system.

[0029] In the embodiment 7, the state space model includes a state equation and a measurement equation, wherein: The state equation is: ; In the formula: and are respectively the system state vector and the noise vector, and it is assumed that ​is a Gaussian white noise sequence with mean 0, and are the system state transfer matrix and the system noise matrix respectively; where: ,in, 、 、 is the attitude error of the carrier in the three directions of northeast sky in the strapdown inertial navigation system, 、 、 They represent the velocity errors in the east, north and sky directions of the strapdown inertial navigation system respectively. 、 、 are the latitude, longitude and altitude position errors, 、 、 is the constant zero bias of the accelerometer in three directions in the carrier coordinate system, 、 、 is the gyroscope constant bias, is the constant error of the velocity value in the beam direction measured by the four-dimensional beam of the Doppler velocimeter, is the constant zero bias of the depth gauge; The measurement equation is: ; Where: is the system observation vector, is the measurement transfer matrix, is the system state vector, is the measurement noise sequence; where: the system observation vector Expressed as: ; Where: , They represent the four-dimensional beam direction velocity measurement value of the Doppler velocimeter and the output of the strapdown inertial navigation system. The velocity value under the system is converted into the velocity value in the four-dimensional beam direction of the Doppler velocimeter; Measurement transfer matrix Expressed as: ; Where: is the posture matrix; is the antisymmetric matrix of the measured velocity information; is a zero matrix, is the unit matrix; M represents the speed conversion matrix, which is expressed as: ; Where: α is the beam tilt angle.

[0030] The filtering process of the robust adaptive Kalman filtering algorithm in embodiment 8 comprises the following steps: S31, initializing filtering parameters: S311, based on the initial navigation state of the underwater vehicle and the sensor parameters, setting the initial time index k = 1, and initializing the system state vector; wherein the system state vector includes attitude errors, velocity errors, position errors, sensor zero offsets, and Doppler velocity log beam errors of the strapdown inertial navigation system; S312, initializing the state covariance matrix, and setting the diagonal element initial value according to the statistical characteristics of the initial state error; S313, setting the initial values of the system noise covariance matrix and the measurement noise covariance matrix; S314, limiting the gross error detection threshold, i.e. the first threshold value (1.0-1.5) and the second threshold value (2.5-3.0) in the IGG III criterion; S32, filtering time update: S321, using the error equation of the strapdown inertial navigation system to derive and calculate the state transition matrix at the current time; wherein the state transition matrix includes the coupling relationship of attitude, velocity, and position errors; S322, performing system state prediction according to the state transition matrix; S323, updating the prediction error covariance matrix; S33, filtering measurement update: S331, using the Sage-Husa filtering method to estimate the system noise parameter online in real time; S332, using the sequential measurement + variance restriction method to sequentially process the measurement value while limiting the upper and lower limits of the measurement noise; S333, calculating the Kalman filtering gain corresponding to the single-scalar measurement; S334, calculating the measurement residual sequence and performing standardization processing on the residual; S335, generating a weight function matrix according to the IGG III criterion; S336, updating the state estimation based on the weight function matrix; S337, updating the state covariance matrix; S34, information fusion: updating the time index; outputting the optimal state estimation value and the state covariance matrix at the current time, and simultaneously taking the optimal state estimation value and the state covariance matrix at the current time as the input of the next round of time update, to realize the cyclic iteration of the filtering process.

[0031] Embodiment 9, a SINS / DVL / PS integrated navigation method based on the robust adaptive Kalman filtering, as shown inFigure 2 As shown, the following steps are included: It is known that the gyroscope output angular velocity information collected by SINS , accelerometer outputs three-dimensional relative force information , velocity information of the four-dimensional beam channel collected by DVL , depth information collected by PS .

[0032] Step 1: Establish the equation of state; Equation of state: ; in, and are the system state vector and noise vector respectively, assuming is a Gaussian white noise sequence with mean 0, and are the system state transfer matrix and the system noise matrix respectively. The following error parameters are selected as the system state variables.

[0033] ; in, 、 、 is the attitude error of the carrier in the three directions of northeast and sky in the inertial navigation system, 、 、 They represent the velocity errors in the east, north and sky directions of the inertial navigation system respectively. 、 、 are the latitude, longitude and altitude position errors, 、 、 is the constant zero bias of the accelerometer in three directions in the carrier coordinate system, 、 、 is the gyroscope constant bias, is the constant error of the velocity values ​​in the beam direction measured by the four beams of DVL, is the constant zero bias of the depth gauge. State transfer matrix Specifically expressed as: ; in:

[0034]

[0035]

[0036]

[0037]

[0038]

[0039]

[0040] wherein: is the attitude matrix, is the earth rotation angular velocity, and is the geographic latitude and altitude, and is the meridian radius and prime vertical radius, , , is the east-north-up velocity component in the navigation frame, , , is the accelerometer measured specific force in the navigation frame; Step 2: Establish the measurement equation; Measurement equation: ; wherein, is the system observation vector, is the measurement transition matrix, is the system state vector in step 1, is the measurement noise sequence. The specific observation is expressed as: ; wherein, , respectively represent the velocity measurement values in the four beam directions of the DVL and the velocity values converted from the SINS output n-frame to the velocity values in the four beam directions of the DVL. The conversion of the SINS output velocity from the n-frame to the carrier coordinate system and then to the velocity vector in the four beam directions of the DVL can be completed by the following formula: ; ; wherein, M represents the velocity conversion matrix, and is expressed as: ; wherein, a is the beam inclination angle, and is 45°.

[0041] The measurement matrix H needs to be derived based on the beam velocity measurement difference as follows: ; ; Neglecting the second-order small quantities, the measurement matrix in the system measurement equation is obtained, and is expressed as: ; ; Step 3: Fusion processing of data based on improved robust adaptive Kalman filtering algorithm.

[0042] Specifically, the following steps are included: (1) Initialization of filtering parameters: ① Set the initial time index k = 1, and initialize the system state vector based on the initial navigation state of the underwater vehicle (such as initial position, velocity, attitude) and sensor parameters (gyroscope zero drift, accelerometer zero offset, etc.) , which includes attitude error, velocity error, position error, sensor zero offset (accelerometer, gyroscope, depth gauge), and DVL beam error of SINS; ② According to the statistical characteristics of the initial state error, initialize the state covariance matrix ; ③ Set the initial values of the system noise covariance matrix Q and the measurement noise covariance matrix R; ④ Determine the gross error detection threshold, i.e., the first threshold k1 of 1.5 and the second threshold k2 of 3.0 in the IGG III criterion, for subsequent residual judgment.

[0043] (2) Filtering time update: ① According to the error equation of SINS, derive and calculate the state transition matrix at the current time , which includes the coupling relationship of attitude, velocity, and position error; ② Perform system state prediction based on the state transition matrix ; ③ Calculate the prediction error covariance matrix ; (3) Filtering measurement update: ① Use the Sage-Husa filtering method to estimate the noise parameters of the system in real time: ; ② Use the "sequential measurement + variance restriction" method to process the measurement values sequentially, while limiting the upper and lower limits of the measurement noise to ensure the positive definiteness of the measurement noise covariance matrix; ; ③ Calculate the Kalman filtering gain corresponding to the single scalar measurement ; ④ Calculate the measurement residual sequence , and perform standardization processing on the residuals; ⑤ Generate the weight function matrix D according to the IGG III criterion, as shown in Figure 3 ; ; When the statistical quantity is less than the threshold , the weight is set to 1, and the measurement information is not weighted, which is generally called a protection zone; when the measurement error is greater than the threshold , the weight is directly set to 0, and the filter will not process this measurement information, and the measurement updating process will not be performed; when the error quantity is between the two thresholds, the measurement information is appropriately weighted, and the weight decreases more as the statistical quantity increases; ⑥Updating the state estimation based on the weight function matrix ; ⑦Updating the state covariance matrix ; (4) Information fusion: updating the time index k:= k+1; outputting the optimal state estimation and covariance matrix at the current time , , wherein the state estimation value contains the corrected attitude, velocity and position error, and can be used for real-time correction of the navigation output of the SINS; at the same time, the state estimation and covariance matrix at the current time are taken as the input of the next round of time updating, so that the loop iteration of the filtering process is realized.

[0044] In order to verify the navigation performance of the robust adaptive Kalman filter combined navigation system and method in a complex underwater environment, a simulation test is performed. The simulation parameters and simulation results are as follows: (1) Simulation parameter setting In order to verify the effectiveness of the system and method, a simulation platform is built based on MATLAB, and the simulation parameters are as follows: Simulation time: 975s, covering acceleration, constant speed, deceleration and left and right turning motion; Initial state: geographical latitude 34.25°, longitude 108.91°, initial speed 0m / s, initial attitude θ=0°, γ=0°, ψ=0°; Sensor parameters:

[0045] The output frequencies of the IMU, DVL and PS are set to 100Hz, 1Hz and 1Hz respectively. The corresponding motion trajectory and attitude speed parameters are shown in Figure 4 and Figure 5 .

[0046] In order to simulate the AUV in the real underwater complex environment, the DVL measurement heavy tail noise is given between 400s to 600s, the noise is amplified by 50 times with a probability of 10%, so as to simulate the complexity of underwater conditions in a specified time, for example, the AUV encounters ocean turbulence during operation and causes large angle tilt to cause abnormal measurement output, thereby simulating the gross error of DVL output. At the same time, in order to simulate the DVL beam failure, the DVL output is set to 0 every 250s, so as to simulate the intermittent beam abnormality of DVL, for example, the DVL beam is cut off by marine organisms or the seabed geology is silt and the beam cannot be effectively reflected. Under the above environment, the navigation accuracy of the traditional Kalman filter, the traditional Sage-Husa adaptive filter, the improved adaptive filter of "sequential measurement + variance restriction" and the anti-bias adaptive Kalman filter based on M estimation proposed in the application are compared. The gross error detection threshold of M anti-bias estimation is 3 and 8 respectively. 、

[0047] The application is compared with the existing KF, AKF and improved AKF, and the trajectory comparison is shown in Figure 6 The traditional KF and AKF are affected by the heavy tail noise of 400-600s, and the navigation trajectory appears large fluctuation at the second turning, the longitude fluctuation reaches 0.003° (about 276m), and the yaw phenomenon appears, while the improved AKF and the method proposed in the application can better approach the real trajectory. However, the positioning of the improved AKF compared with the application has a larger position error at the third turning. It can be seen that the improved anti-bias adaptive filtering algorithm proposed in the application has the smallest positioning error in the integrated navigation as a whole.

[0048] As shown in Figures 7-12 , the speed error of the four filtering algorithms does not accumulate obviously with time during the whole integrated navigation process. However, compared with the classic KF and AKF, the improved AKF and the M+improved AKF algorithm proposed in the application has better stability. Specifically, when the DVL is affected by the heavy tail noise and produces large measurement gross error, the KF and AKF have large amplitude error in the northeast direction, but the improved AKF and the M+improved AKF have smaller speed error fluctuation, especially the method proposed in the application has the smallest speed error deviation during the whole navigation. By comparing the position error of the four filtering algorithms, the position error of KF and AKF increases significantly after 400s, and the maximum error of the north position can reach 50m and 22m respectively; the position error of the improved AKF and the M+improved AKF algorithm proposed in the application fluctuates smaller, especially the position error of each direction of the M+improved AKF algorithm proposed in the application is kept within 5m, and the positioning accuracy is the highest.

[0049] ​In order to further compare the position error, the spatial position error formula is introduced, and the formula is as follows: ; The spatial position error curves of the four algorithms can be calculated by using the above formula, as shown in Figure 13 . Specifically, the maximum spatial position error is 54.3 m, 26.4 m, 13.9 m and 4.83 m, respectively. Compared with the traditional KF, the positioning accuracy of the improved AKF is improved by 74.4%, and the accuracy of the proposed robust Kalman filter based on M estimation is improved by 91.1%, which has the highest positioning accuracy. The simulation test results show that the SINS / DVL / PS integrated navigation with the robust adaptive Kalman filter of the application has good positioning accuracy and robustness in the case of processing underwater complex conditions leading to system noise presenting thick tail distribution and intermittent failure of DVL.

[0050] The preferred embodiments of the application are described above with reference to the accompanying drawings, and the scope of the application is not limited by this. Any modifications, equivalent replacements and improvements made by those skilled in the art without departing from the scope and essence of the application should be within the scope of the application.

Claims

1. An integrated navigation method based on robust adaptive Kalman filtering is characterized by: The steps include: S1. Based on the attitude, velocity, and position information provided by the strapdown inertial navigation system, the four-dimensional beam direction velocity information directly provided by the Doppler velocimeter, and the depth information provided by the depth meter, the attitude and velocity information are converted into the four-dimensional beam direction of the Doppler velocimeter through a state-space model, and the four-dimensional beam direction velocity information is subtracted to calculate the four-dimensional beam direction velocity measurement difference, and the height information in the position information is subtracted from the depth information to calculate the depth measurement difference; S2. Using the four-dimensional beam direction velocity measurement difference and the depth measurement difference as the measurement value of the robust adaptive Kalman filter algorithm, filtering is performed through four processes: initializing filter parameters, updating filter time, updating filter measurement, and information fusion to obtain fused information; wherein: Initialize the filter parameters: including setting the initialization time scale, initializing the system state vector, the state covariance matrix, setting the initial values ​​of the system noise covariance matrix and the measurement noise covariance matrix, and the gross error detection threshold; among which: setting the gross error detection threshold, that is, setting the first threshold and the second threshold in the IGG III criterion; Filter time update: Based on the error equation of the strapdown inertial navigation system, the state transfer matrix at the current moment is derived and calculated. The state transfer matrix is ​​used to predict the system state and calculate the prediction error covariance matrix. Filtered measurement update: Based on the prediction error covariance matrix and the Sage-Husa adaptive filtering mechanism, the system noise parameters are estimated online in real time. The measurement values ​​are sequentially processed using the sequential measurement + variance-constrained method, while limiting the upper and lower limits of the measurement noise. The Kalman filter gain corresponding to the single scalar measurement is calculated, and the measurement residual sequence is calculated and normalized. The weight function matrix is ​​generated using the IGG III criterion to update the state estimate and the state covariance matrix. Information fusion: Update the time scale, output the optimal state estimate and state covariance matrix at the current moment, and use the optimal state estimate and state covariance matrix at the current moment as the input for the next round of time update to achieve cyclic iteration of the filtering process.

2. The integrated navigation method based on robust adaptive Kalman filtering according to claim 1, characterized in that: The state space model includes state equations and measurement equations, where: The state equation is: ; Where: and are the system state vector and noise vector respectively, assuming is a Gaussian white noise sequence with mean 0, and are the system state transfer matrix and the system noise matrix respectively; where: ,in, 、 、 is the attitude error of the carrier in the three directions of northeast sky in the strapdown inertial navigation system, 、 、 They represent the velocity errors in the east, north and sky directions of the strapdown inertial navigation system respectively. 、 、 are the latitude, longitude and altitude position errors, 、 、 is the constant zero bias of the accelerometer in three directions in the carrier coordinate system, 、 、 is the gyroscope constant bias, is the constant error of the velocity value in the beam direction measured by the four-dimensional beam of the Doppler velocimeter, is the constant zero bias of the depth gauge; The measurement equation is: ; Where: is the system observation vector, is the measurement transfer matrix, is the system state vector, is the measurement noise sequence; where: the system observation vector Expressed as: ； Where: , They represent the four-dimensional beam direction velocity measurement value of the Doppler velocimeter and the output of the strapdown inertial navigation system. The velocity value under the system is converted into the velocity value in the four-dimensional beam direction of the Doppler velocimeter; Measurement transfer matrix Expressed as: ； Where: is the posture matrix; is the antisymmetric matrix of the measured velocity information; is a zero matrix, is the unit matrix; M represents the speed conversion matrix, which is expressed as: ； Where: α is the beam tilt angle.

3. The integrated navigation method based on robust adaptive Kalman filtering according to claim 1, characterized in that: The filtering process of the robust adaptive Kalman filtering algorithm includes the following steps: S31. Initialize filter parameters: S311, setting an initial time mark k=1, and initializing a system state vector based on the initial navigation state and sensor parameters of the underwater vehicle; wherein the system state vector includes the attitude error, velocity error, position error, sensor zero bias, and Doppler velocimeter beam error of the strapdown inertial navigation system; S312, initializing the state covariance matrix, and setting the initial values ​​of the diagonal elements according to the statistical characteristics of the initial state error; S313, setting initial values ​​of the system noise covariance matrix and the measurement noise covariance matrix; S314, define the gross error detection threshold, that is, the first threshold in the IGG III criterion (1.0~1.5) and the second threshold (2.5~3.0); S32, filter time update: S321. Using the error equation of the strapdown inertial navigation system, derive and calculate the state transfer matrix at the current moment; wherein the state transfer matrix includes the coupling relationship between attitude, velocity, and position errors; S322. Predicting the system state according to the state transfer matrix; S323, updating the prediction error covariance matrix; S33, filter measurement update: S331, using Sage-Husa filtering method to estimate system noise parameters online in real time; S332, using the sequential measurement + variance-constrained method to sequentially process the measurement values ​​while limiting the upper and lower limits of the measurement noise; S333, calculating the Kalman filter gain corresponding to the single scalar measurement; S334, calculating the measurement residual sequence and performing standardization processing on the residual; S335, generating a weight function matrix according to the IGG III criterion; S336, updating the state estimate based on the weight function matrix; S337, updating the state covariance matrix; S34, information fusion: update the time scale; output the optimal state estimation value and state covariance matrix at the current moment, and use the optimal state estimation value and state covariance matrix at the current moment as the input of the next round of time update to realize the cyclic iteration of the filtering process.

4. The integrated navigation method based on robust adaptive Kalman filtering according to claim 3, characterized in that: The specific process of online real-time estimation of system noise parameters using the Sage-Husa filtering method is as follows: ； Where: One-step prediction for the state; is the state transfer matrix; is the state estimation; is the measurement vector; is the state estimated mean square error matrix; is the predicted mean square error matrix; Assign a matrix to the system noise; is the Kalman filter gain; is the measurement matrix; is the measurement noise variance matrix; is the one-step prediction error; is the adaptation coefficient, the initial value =1, which serves as the weighting index in the estimation formula when estimating measurement noise and process noise; is the fading factor, 0< <1, Take 0.9~0.

999.

5. The integrated navigation method based on robust adaptive Kalman filtering according to claim 3, characterized in that: The specific process of sequentially processing the measurement values ​​and limiting the upper and lower limits of the measurement noise using the sequential measurement + variance-constrained method includes the following steps: S51, sequentially process the measured values, In the case of sub-scalar measurement, the measurement update equation is: ； S52, set the upper and lower limits of the measurement noise, compare the measurement noise with the upper and lower limits, if it exceeds the upper limit, reduce the credibility of the measurement information; if it is lower than the lower limit, ensure that the measurement noise is positive, thereby reducing the measurement noise. Restricted to [ ]; where: measurement noise covariance matrix The expression is: ； Where: represents the measurement update round in sequential filtering, Indicates the scalar, Indicates the Measurement update No. elements.

6. The integrated navigation method based on robust adaptive Kalman filtering according to claim 3, characterized in that: The expression of the weight function matrix is: ； Where: is the measurement prediction error of the input; 、 are the first and second thresholds, both are constant values. =1.0~1.5, =2.5~3.0; IGGⅢ criterion divides the weight function into three segments. When the statistic is less than the threshold When the measurement error is greater than the threshold, the weight is set to 1, which is equivalent to not weighting the measurement information, and is called the protection zone. When the error is between the two thresholds, the weight is directly set to 0. This section is called the rejection zone. At this time, the filter will not process the measurement information and the measurement update process will not be performed. When the error is between the two thresholds, the measurement information is appropriately downgraded. As the statistic increases, the weight decreases more. This section is called the downgraded zone.

7. The integrated navigation method based on robust adaptive Kalman filtering according to claim 6, characterized in that: The weight function matrix is ​​introduced to rewrite the state estimation in the process of online real-time estimation of system noise parameters using the Sage-Husa adaptive filtering mechanism as follows: ； Where: D is the residual weight matrix. When D is the identity matrix, it is equivalent to the standard Kalman filter algorithm process.

8. The integrated navigation method based on robust adaptive Kalman filtering according to claim 3, characterized in that: The state estimate includes the corrected attitude, velocity, and position errors, and can be used to correct the navigation output of the strapdown inertial navigation system in real time.

9. An integrated navigation system based on robust adaptive Kalman filtering, including a strapdown inertial navigation system, a Doppler velocimeter, a depth gauge, and a data processing module, wherein: The strapdown inertial navigation system includes an inertial measurement unit (IMU) and a strapdown inertial navigation solution unit (SINS). The IMU provides gyroscope information and acceleration information to the SINS solution unit, which then provides the carrier's attitude, velocity, and altitude information to the data processing unit. A Doppler velocimeter, used for providing the data processing unit with four-dimensional beam direction velocity information of the carrier; A depth meter, used for providing depth information of the carrier to the data processing unit; Its characteristics are: The data processing unit includes a state-space model and a robust adaptive Kalman filter algorithm, wherein: A state space model, including a state equation and a measurement equation, is used to convert attitude and velocity information in a navigation coordinate system calculated by a strapdown inertial navigation solution unit into a four-dimensional beam direction of a Doppler velocimeter, calculate a four-dimensional beam direction velocity measurement difference by subtracting the attitude and velocity information from the four-dimensional beam direction velocity information, and calculate a depth measurement difference by subtracting the altitude information and depth information calculated by the strapdown inertial navigation solution unit, and use the four-dimensional beam direction velocity measurement difference and the depth measurement difference as measurement values ​​for a robust adaptive Kalman filter algorithm; Robust adaptive Kalman filtering algorithm for information fusion, including IGG III criterion, Sage-Husa adaptive filtering algorithm and sequential measurement + variance-constrained method, where: IGGⅢ criterion, used to set the gross error detection threshold and generate the weight function matrix; Sage-Husa adaptive filtering algorithm for online real-time estimation of system noise parameters; The sequential measurement + variance-constrained method is used to sequentially process the measurement values ​​while limiting the upper and lower limits of the measurement noise.

Images