Factor graph fusion navigation method for underwater vehicles with abnormal observation processing capability

Abstract

The present invention belongs to the field of underwater autonomous vehicle navigation technology, and particularly relates to a factor graph fusion navigation method for underwater vehicles with the ability to process abnormal observations. The method solves the problems of continuous system reconstruction and inability to effectively process abnormal observation data in traditional algorithms. The method comprises: step 1: establishing a combined navigation system model based on a factor graph framework; step 2: obtaining the noise covariance matrix of the IMU pre-integration quantity and the IMU pre-integration factor residual vector with zero bias update; step 3: obtaining the DVL and USBL residual vectors and the observation noise covariance matrix; step 4: determining whether the DVL or USBL observation value is normal; if normal, the observation noise covariance matrix obtained in step 3 is the final form; if abnormal, the final form of the observation noise covariance matrix is ​​obtained after calculating the adjustment factor correction; step 5: inserting each residual vector and the final observation noise covariance matrix into the factor graph framework to obtain a navigation solution.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater autonomous vehicle navigation, and in particular relates to an underwater vehicle factor graph fusion navigation method with abnormal observation and processing capability. Background Art

[0002] Autonomous underwater vehicles (AUVs) are emerging tools capable of efficiently completing underwater operations, boasting advantages such as a wide range of movement, compact size, and high flexibility. They play a vital role in applications such as marine resource exploration, seafloor topography surveying, and underwater rescue. In particular, they are used in the military for tactical defense, communications relay, reconnaissance, and guided attack. However, as AUVs become increasingly practical, higher requirements are placed on the accuracy and fault tolerance of navigation systems. Therefore, high-precision navigation and positioning technologies suitable for AUVs will become a research priority for future AUV development.

[0003] Due to the unique characteristics of the underwater environment, radio navigation, typically satellite navigation, suffers from signal attenuation, making it difficult to apply to underwater vehicles. Therefore, today's AUVs primarily rely on underwater sensors such as ultra-short baseline systems (USBLs), Doppler velocimeters (DVLs), or inertial navigation systems (INSs) to obtain vehicle status information. INSs offer the advantage of complete autonomy, requiring no external input and transmitting no information to the outside world. They can provide vehicle status information continuously and around the clock, regardless of the Earth's location. However, INS navigation errors accumulate and even diverge over time, resulting in poor long-term navigation accuracy. DVLs, based on the Doppler effect, transmit and receive ultrasonic signals to the seabed, calculating the current vehicle velocity according to a predefined model. DVLs offer advantages such as responsiveness, stability, and high navigation accuracy. However, in complex underwater environments, DVLs are subject to interference from various unavoidable factors, such as water temperature, depth, and obstacles, which can lead to reduced accuracy and even anomalies. USBLs are a typical acoustic positioning system based on underwater acoustic signal propagation. Its positioning accuracy has reached a remarkably high level and is adaptable to various water environments. However, USBL often suffers from packet loss and poor continuity, and is also susceptible to interference from various unknown obstacles. Given the complementary nature of the three aforementioned navigation strategies, they are often combined to create an INS / DVL / USBL integrated navigation system, which provides more accurate and reliable navigation information than a single sensor alone. Therefore, INS / DVL / USBL integrated navigation will become the preferred navigation method for AUVs.

[0004] Navigation algorithms are the soul of navigation system solutions. For AUV navigation, only high-precision navigation information can ensure the AUV performs its mission properly. Currently, information fusion for INS / DVL / USBL integrated navigation is typically implemented using Kalman filtering. These methods can be roughly divided into two categories: centralized processing and distributed processing. However, centralized processing suffers from poor reliability; a single sensor failure can contaminate the entire system. In contrast, distributed processing offers advantages such as high reliability, low communication overhead, and flexible fault detection and isolation. The Federated Kalman Filter (FKF) is the most commonly used distributed processing method for INS / DVL / USBL integrated navigation. However, when sensor availability changes, the corresponding sub-filters need to be individually isolated, which inevitably leads to frequent system reconfiguration and increases algorithm complexity. Furthermore, the FKF requires all sensors to operate at the same frequency. However, in practical applications, INS, DVL, and USBL often operate at different frequencies and with varying time delays. Although synchronous data processing methods such as interpolation or extrapolation have been developed to combine with FKF by unifying the observation data to the same time, these methods will cause additional errors, resulting in poor estimation accuracy for sensor fusion working at different output frequencies.

[0005] The factor graph optimization (FGO) algorithm is a recently developed information fusion method that offers a novel approach for asynchronous data processing in integrated navigation. In this method, system states and observations are defined as unknown variable nodes and known factor nodes, and the probabilistic relationship between navigation states and observations is encoded. Adding a new sensor observation to the navigation system requires only the embedding of a new node in the factor graph. Therefore, FGO can fully utilize sensor observation information by simply adding or subtracting relevant factor nodes, providing a plug-and-play unified navigation framework with strong flexibility and scalability for asynchronous data fusion processing in integrated navigation systems. However, due to the unique characteristics of underwater acoustic propagation, both DVL and USBL are susceptible to interference from complex underwater environments. Existing FGOs do not fully account for anomalies in navigation sensor observations, resulting in a significant deterioration in the estimated accuracy of the navigation solution when sensor performance varies in complex environments. Therefore, there is an urgent need to research fault detection and error compensation techniques based on the factor graph framework to improve the robustness of FGO for integrated navigation information fusion using INS / DVL / USBL. Summary of the Invention

[0006] The purpose of the present invention is to provide an underwater vehicle factor graph fusion navigation method with abnormal observation processing capability, so as to solve the technical problems existing in the traditional AUV combined navigation algorithm, such as continuous system reconstruction and inability to effectively process abnormal observation data, and ensure the reliability and fault tolerance of the navigation system.

[0007] The technical solution adopted by the present invention is a factor graph fusion navigation method for underwater vehicles with abnormal observation processing capability, which is special in that it includes the following steps:

[0008] Step 1: Establish an INS / DVL / USBL integrated navigation system model based on the factor graph framework and define the state space of the system;

[0009] Step 2: Based on the IMU discrete motion equation and the IMU noise error propagation model, the noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update are calculated;

[0010] Step 3: Establish the observation equation based on the DVL and USBL observation models and obtain the corresponding residual vector and observation noise covariance matrix;

[0011] Step 4: Based on the concept of Mahalanobis distance, determine whether the DVL or USBL observation value is normal; if the observation value is normal, the observation noise covariance matrix obtained in step 3 is its final form; if the observation value is abnormal, design and calculate the adjustment factor to correct the corresponding observation noise covariance matrix in step 3, and obtain the final form of the corresponding observation noise covariance matrix after correction;

[0012] Step 5: The noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update, the residual vector of the DVL and the final observation noise covariance matrix, as well as the residual vector of the USBL and the final observation noise covariance matrix are designed as factor nodes and inserted into the factor graph framework. All nodes in the factor graph are nonlinearly optimized to obtain the navigation solution.

[0013] Furthermore, the INS / DVL / USBL integrated navigation system model based on the factor graph framework is established in step 1, specifically: the information output by the IMU in the INS is processed into factor nodes by pre-integration, the speed information output by the DVL is expressed as a speed observation factor, and the position information output by the USBL is expressed as a position observation factor, and the three are combined in a loosely coupled manner;

[0014] The east-north-sky geographic coordinate system is defined as the g coordinate system, the navigation coordinate system is defined as the n system, and the east-north-sky geographic coordinate system is selected as the navigation coordinate system, the carrier coordinate system is defined as the b system, and the inertial coordinate system is defined as the i system. The state space of the system described in step 1 is shown in the following formula (1):

[0015]

[0016] In formula (1): k is the time node; is the three-dimensional position; is the three-dimensional velocity; is the three-dimensional posture; b gk and b ak are the zero bias of the gyroscope and accelerometer respectively.

[0017] Furthermore, in order to avoid the integral process from time k-1 to time k from being repeatedly calculated when the system state changes at time k-1, the second step includes the following steps:

[0018] Step a1: Pre-integrate the IMU according to the IMU discrete motion equation, and calculate the relative motion increment of the IMU in position, velocity and attitude from time k-1 to time k;

[0019] Step a2: Subtract the calculated relative motion increments of the IMU in position, velocity, and attitude from time k-1 to time k from the estimated relative motion increments to obtain a residual vector of the IMU pre-integration factor with a fixed zero bias;

[0020] Step a3: Calculate the noise covariance matrix and Jacobian matrix of the IMU pre-integration amount according to the IMU noise error propagation model to correct the zero bias of the gyroscope and accelerometer in the residual vector of the IMU pre-integration factor with a fixed zero bias, and obtain the residual vector of the IMU pre-integration factor with a zero bias update.

[0021] Furthermore, the IMU discrete motion equation is shown in the following formula (2):

[0022]

[0023] In formula (2): is the projection of the instantaneous angular velocity of system b relative to system i in system b; f b is the acceleration of the carrier in the b system; n g and n a are the zero-mean Gaussian white noise of the gyroscope and accelerometer, respectively;

[0024] The obtained residual vector of the IMU pre-integration factor with fixed zero bias is shown in the following formula (7):

[0025]

[0026] In formula (7): r Δ～ is the residual of position, velocity and attitude; r b～ Represents the residual of IMU zero bias; Log(·) represents the logarithmic mapping of the vector; and are the relative motion increments of the IMU in attitude, velocity, and position, respectively; is the attitude rotation matrix from b system to n system; g n is the acceleration due to gravity; Δt represents the time interval between two adjacent observations;

[0027] The error state vector of the pre-integration component is defined as follows:

[0028]

[0029] In formula (8): and are the position, velocity and attitude errors of the pre-integrated components respectively; and are the bias errors of the gyroscope and accelerometer respectively;

[0030] The IMU noise error propagation model uses the error perturbation method, and the dynamic model of the error state is shown in the following formula (9):

[0031]

[0032] In formula (9): t is the noise vector; F t and G t are the state transfer matrix and error transfer matrix respectively;

[0033] In formula (7), and The residual vector of the IMU pre-integration factor with zero bias update can be obtained by updating it with the first-order expansion shown in the following formula (12); formula (12) is as follows:

[0034]

[0035] In formula (12): represents the updated zero bias; Exp(·) represents the exponential mapping of the vector; J k-1,k The Jacobian matrix representing the k-1 to k pre-integrated components; It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of

[0036] Furthermore, the DVL and USBL observation equations established in step 3 are shown in the following equation (13):

[0037]

[0038] In formula (13): n DVL 、n USBL are the velocity observation noise of DVL and the position observation noise of USBL respectively; h DVL and h USBL is the observation function, which can be written as a linear function, namely: h DVL (x k )=H DVL x k and h USBL =H USBL x k , where H DVL =[0 3×3 I 3×3 0 3 ×9 ] and H USBL =[I 3×3 0 3×12 ];

[0039] The residual vectors of DVL and USBL are defined as follows:

[0040]

[0041] In formula (14): and Represent the residual vectors of DVL and USBL respectively; are the observation noise covariance matrices of DVL and USBL, respectively.

[0042] Furthermore, in order to simplify the judgment process, the concept of Mahalanobis distance is used in step 4 to judge whether the DVL or USBL observation value is normal, specifically:

[0043] First, determine whether the innovation vector of the DVL or USBL observation value obeys the zero-mean Gaussian distribution; if it obeys, the DVL or USBL observation value is normal; if not, determine whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance.

[0044] Furthermore, the adjustment factor is designed and calculated in step 4 to correct the corresponding observation noise covariance matrix in step 3. When correcting, the adjustment factor is added to the observation noise covariance matrix of the DVL and USBL. to make corrections;

[0045] The regulatory factor The calculation process includes the following steps:

[0046] Step b1: Calculate the adjustment factor The corrected innovation vector covariance is shown in the following equation (21):

[0047]

[0048] In formula (21): l = DVL, USBL; represents the observation innovation vector corresponding to DVL or USBL; P k+1|k is the prior state covariance;

[0049] Step b2: Based on the concept of Mahalanobis distance, a nonlinear equation for the covariance of the corrected innovation vector is established; the established nonlinear equation is shown in the following formula (22):

[0050]

[0051] In formula (22): is the quantile corresponding to the given significance level α, and α is the probability of rejecting the null hypothesis when judging whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance;

[0052] Step b3: Use Newton's method iteration and the derivative formula of the inverse matrix to solve the adjustment factor;

[0053] The expression of the adjustment factor obtained by solving is shown in the following formula (24):

[0054]

[0055] In formula (24): d is the number of iterations;

[0056] The iterative process of formula (24) is achieved by setting Initialize, when the test statistic satisfies Or when the number of iterations is greater than the preset value, the iteration is completed and the adjustment factor can be solved.

[0057] Furthermore, in step 5, the noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update, the residual vector of the DVL and the final observation noise covariance matrix, and the residual vector of the USBL and the final observation noise covariance matrix are designed as factor nodes and inserted into the factor graph framework using the least squares method;

[0058] In step 5, all nodes in the factor graph are nonlinearly optimized to obtain the navigation solution using the Levenberg-Marquardt algorithm.

[0059] Furthermore, it also includes step six: calculating the covariance matrix corresponding to the navigation solution obtained in step five, so as to be used in calculating the navigation solution at the next moment, and solving the covariance of the new information vector when judging whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance.

[0060] Furthermore, in order to reduce computational costs, step seven is also included: determining whether marginalization is required; if so, converting the oldest factor node stored in the sliding window optimizer into prior state information through marginalization, which is used to solve the navigation solution at the next moment, and outputting the navigation solution obtained in step five at the same time; if not, directly outputting the navigation solution obtained in step five.

[0061] The beneficial effects of the present invention are:

[0062] (1) The present invention adopts a factor graph optimization framework to replace the traditional FKF algorithm and designs a new type of combined navigation system structure. The factor graph optimization structure only adds corresponding factors when the sensor is valid, which overcomes the problem that the traditional FKF algorithm needs to repeatedly reconstruct the system and the processing of asynchronous sensor observation data is not ideal, realizes plug-and-play of sensors, and ensures the reliability of the navigation system; at the same time, in the factor graph optimization solution process, a fault detection method based on Mahalanobis distance is designed to deal with the problem of abnormal sensor observation, thereby improving the fault tolerance of the AUV combined navigation system; therefore, the present invention solves the technical problems of continuous system reconstruction and inability to effectively process abnormal observation data in the traditional AUV combined navigation algorithm, ensuring the reliability and fault tolerance of the navigation system.

[0063] (2) In the present invention, a pre-integration calculation method is preferably used to calculate the relative motion increment of the IMU in position, velocity and attitude from time k-1 to time k, and the relative motion increment of the IMU in position, velocity and attitude from time k-1 to time k is calculated by subtracting the estimated value of the relative motion increment from the estimated value of the relative motion increment to calculate the residual vector of the IMU pre-integration factor with a fixed zero bias. This can avoid the integration process from time k-1 to time k from being repeatedly calculated when the system state changes at time k-1, thereby simplifying the calculation process; and in the calculation process, the equivalent factor is calculated according to the actual output frequency of the combined navigation system, thereby reducing the update frequency of the factor node and improving the actual performance.

[0064] (3) The present invention preferably also includes a step of determining whether marginalization is required. Marginalization is combined to limit the computational complexity of the sliding window optimizer. When the number of factor nodes stored in the factor graph exceeds a threshold (sliding window size), the oldest system state is marginalized, and the IMU pre-integrated quantities and DVL and USBL observations corresponding to the marginalized state are converted into prior factors, thereby reducing computational costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 A navigation solution for realizing INS / DVL / USBL information fusion based on a factor graph provided by an embodiment of the present invention;

[0066] Figure 2 It is a specific factor graph framework of the INS / DVL / USBL integrated navigation solution provided by an embodiment of the present invention;

[0067] Figure 3 This is a schematic diagram of the IMU (Inertial Measurement Unit) pre-integration process;

[0068] Figure 4 is a flow chart of an improved factor graph optimization (IFGO) algorithm provided by an embodiment of the present invention;

[0069] Figure 5 is the simulated trajectory of the AUV underwater in the simulation verification of the embodiment of the present invention;

[0070] Figure 6 This is a comparison chart of the position estimation errors of FKF, FGO, and IFGO under simulation environment, where:

[0071] (a) Longitude;

[0072] (b) latitude;

[0073] (c) altitude;

[0074] Figure 7 It is a comparison chart of the mean square error (MSE) of position estimation of FKF, FGO and IFGO in the simulation environment. DETAILED DESCRIPTION

[0075] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0076] The factor graph fusion navigation method for underwater vehicles with abnormal observation processing capability of the present invention comprises the following steps:

[0077] Step 1: Establish an INS / DVL / USBL integrated navigation system model based on the factor graph framework and define the state space of the system;

[0078] Step 2: Based on the IMU discrete motion equation and the IMU noise error propagation model, the noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update are calculated;

[0079] Step 3: Establish the observation equation based on the DVL and USBL observation models and obtain the corresponding residual vector and observation noise covariance matrix;

[0080] Step 4: Based on the concept of Mahalanobis distance, determine whether the DVL or USBL observation value is normal; if the observation value is normal, the observation noise covariance matrix obtained in step 3 is its final form; if the observation value is abnormal, design and calculate the adjustment factor to correct the corresponding observation noise covariance matrix in step 3, and obtain the final form of the corresponding observation noise covariance matrix after correction;

[0081] Step 5: The noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update, the residual vector of the DVL and the final observation noise covariance matrix, as well as the residual vector of the USBL and the final observation noise covariance matrix are designed as factor nodes and inserted into the factor graph framework. All nodes in the factor graph are nonlinearly optimized to obtain the navigation solution.

[0082] Figure 4 This is a flow chart of the Improved Factor Graph Optimization (IFGO) algorithm provided by an embodiment of the present invention.

[0083] The implementation method of the above step 1 is as follows:

[0084] See also Figure 1 In step 1, the INS / DVL / USBL integrated navigation system model based on the factor graph framework is established. Specifically, the INS IMU output is processed into factor nodes through pre-integration, the velocity information output by the DVL is represented as a velocity observation factor, and the position information output by the USBL is represented as a position observation factor. The three are loosely coupled. Then, a nonlinear least squares fusion of the multi-sensor factors is performed using an optimization method to obtain the navigation solution.

[0085] The east-north-sky geographic coordinate system is defined as the g coordinate system, the navigation coordinate system is defined as the n system, and the east-north-sky geographic coordinate system is selected as the navigation coordinate system, the carrier coordinate system is defined as the b system, and the inertial coordinate system is defined as the i system. The state space of the above system in step 1 is shown in the following formula (1):

[0086]

[0087] In formula (1): k is the time node; is the three-dimensional position; is the three-dimensional velocity; is the three-dimensional posture; b gk and b ak are the zero bias of the gyroscope and accelerometer respectively.

[0088] The implementation of the above step 2 is as follows:

[0089] Generally speaking, the output frequency of IMU is much higher than that of DVL and USBL, and also higher than the actual requirements of the integrated navigation system. Therefore, if the state information obtained by IMU integration is directly added to the factor graph as a factor node, the computational load will be seriously increased, reducing the real-time performance of the navigation system. To this end, a pre-integration method is proposed to calculate the equivalent factor according to the actual output frequency of the integrated navigation system, reduce the update frequency of the factor node, and improve the actual performance. In addition, Figure 2 As shown, the zero bias of the inertial sensor is added as a variable node to correct the sensor error of the IMU. The above step 2 preferably includes the following steps:

[0090] Step a1: Pre-integrate the IMU according to the IMU discrete motion equation, and calculate the relative motion increment of the IMU in position, velocity and attitude from time k-1 to time k;

[0091] like Figure 3 As shown, the IMU will recursively integrate according to its own frequency, combining all IMU observations from time k-1 to time k as IMU pre-integration factors and adding them to the factor graph. Considering that angular velocity and acceleration are affected by additive white noise and zero bias, the IMU discrete motion equation, i.e., the IMU observation model, is shown in the following equation (2):

[0092]

[0093] In formula (2): is the projection of the instantaneous angular velocity of system b relative to system i in system b; f b is the acceleration (specific force) of the carrier in the b system; n g and n a are zero-mean Gaussian white noise of the gyroscope and accelerometer, respectively.

[0094] according to Figure 3 In the IMU pre-integration process, assuming that there are K IMU observations with a time interval of Δt in the time period from k-1 to k, the system state at time k can be obtained by K iterative integrations as shown in the following formula (3):

[0095]

[0096] In formula (3), m is the time node of IMU output; is the attitude rotation matrix from b system to n system; g n is the acceleration due to gravity; Exp(·) represents the exponential mapping of the vector; Δt represents the time interval between two adjacent observations.

[0097] For conventional IMU observation factors, given IMU observation Based on this, we can use the IMU observation value and the estimated state x at the previous moment through formula (3) k-1 To predict the state x k . It can be simply described as the following formula (4):

[0098] x k =f IMU (x k-1 ,z k IMU )+n IMU (4);

[0099] In formula (4): n IMU Represents the noise of the IMU (gyroscope and accelerometer).

[0100] Furthermore, the nonlinear optimization residual represented by the IMU pre-integration factor is shown in the following formula (5):

[0101]

[0102] In formula (5): is the observation covariance matrix of the IMU.

[0103] Equation (3) gives an estimate of the carrier motion between time k-1 and time k. However, when the system state changes at time k-1, the integral in Equation (3) will be recalculated. To avoid this defect, a relative motion increment that is independent of position and velocity is used at time k-1:

[0104]

[0105] In formula (6): and are the relative motion increments of the IMU in attitude, velocity, and position, respectively.

[0106] Step a2: Subtract the calculated relative motion increments of the IMU in position, velocity, and attitude from time k-1 to time k from the estimated relative motion increments to obtain a residual vector of the IMU pre-integration factor with a fixed zero bias;

[0107] The residual vector of the IMU pre-integration factor with fixed zero bias is shown in the following equation (7):

[0108]

[0109] In formula (7): r Δ～ is the residual of position, velocity and attitude; r b～ Represents the residual of the IMU zero bias; Log(·) represents the logarithmic mapping of the vector.

[0110] Step a3: Calculate the noise covariance matrix and Jacobian matrix of the IMU pre-integration amount according to the IMU noise error propagation model to correct the zero bias of the gyroscope and accelerometer in the residual vector of the IMU pre-integration factor with a fixed zero bias, and obtain the residual vector of the IMU pre-integration factor with a zero bias update.

[0111] The pre-integration factor is based on the assumption that the IMU bias remains constant throughout the entire integration interval. However, in practical applications, the bias can be affected by observation noise. Failure to update the IMU bias during the navigation solution will severely impact the optimization results of the factor graph. Therefore, it is necessary to update the IMU bias to promptly correct the pre-integration observations.

[0112] The error state vector of the pre-integration component is defined as follows:

[0113]

[0114] In formula (8): and are the position, velocity and attitude errors of the pre-integrated components respectively; and are the bias errors of the gyroscope and accelerometer respectively;

[0115] The above IMU noise error propagation model uses the error perturbation method, and the dynamic model of the error state is shown in the following equation (9):

[0116]

[0117] In formula (9): t is the noise vector; F t and G t are the state transfer matrix and the error transfer matrix respectively. Therefore, the discretized noise covariance matrix The initial covariance can be Propagation, as shown in the following formula (10):

[0118]

[0119] In formula (10): Q m is the noise vector ω t The discrete covariance matrix of ; the transition matrix F in discrete time form m It can be obtained by first-order Taylor expansion, that is, F m ≈I+F(t m-1 )Δt m-1,m .

[0120] The first-order Jacobian matrix in the integration interval can also be obtained from the initial Jacobian matrix J k-1,k-1=I is obtained by recursive propagation, as shown in the following formula (11):

[0121] J k-1,m =F m J k-1,m-1 (11);

[0122] Using recursive formula (10) and formula (11), we can obtain the noise covariance matrix from k-1 to k pre-integrated components: and the Jacobian matrix J k-1,k .

[0123] In the above formula (7), and The residual vector of the IMU pre-integration factor with zero bias update can be obtained by updating it using the first-order expansion shown in the following equation (12):

[0124]

[0125] In formula (12): represents the updated zero bias; Exp(·) represents the exponential mapping of the vector; J k-1,k The Jacobian matrix representing the k-1 to k pre-integrated components; It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of

[0126] In this way, an accurate IMU pre-integration model with zero bias update is obtained from the classic INS kinematic model.

[0127] The implementation of the above step three is as follows:

[0128] according to Figure 2 As shown in the factor graph, since DVL can provide the speed information of AUV and USBL can provide the position information of AUV, the DVL and USBL observation equations established in the above step 3 are shown in the following formula (13):

[0129]

[0130] In formula (13): n DVL 、n USBLare the velocity observation noise of DVL and the position observation noise of USBL respectively; h DVL and h USBL is the observation function, which can be written as a linear function, namely: h DVL (x k )=H DVL x k and h USBL =H USBL x k , where H DVL =[0 3×3 I 3×3 0 3 ×9 ] and H USBL =[I 3×3 0 3×12 ].

[0131] Then, similar to the establishment of INS factor nodes, the factor nodes of DVL and USBL observations are defined as the residual vectors of DVL and USBL as shown in the following formula (14):

[0132]

[0133] In formula (14): and Represent the residual vectors of DVL and USBL respectively; are the observation noise covariance matrices of DVL and USBL, respectively.

[0134] The implementation of the above step 4 is as follows:

[0135] In step 4, the above-mentioned method judges whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance. Specifically, first judge whether the innovation vector of the DVL or USBL observation value obeys the zero-mean Gaussian distribution; if it obeys, the DVL or USBL observation value is normal; if not, judge whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance.

[0136] According to the DVL and USBL observation equations, the corresponding observation innovation vector can be obtained as shown in the following equation (15):

[0137]

[0138] In formula (15): l = DVL, USBL; is the observed value of DVL or USBL at time k+1; is the prior state estimate at time k+1, which can be obtained by the following equation (28): Substituting into equation (3) yields: The covariance of the innovation vector can be described as follows (16):

[0139]

[0140] In formula (16): P k+1|k is the prior state covariance, which is determined by the initial covariance matrix P k|m-1 =P k By replacing Replace with P k|m-1 Spread obtained.

[0141] If the observation value of DVL or USBL is normal and the observation equation (13) holds, then the new information vector of the observation value obeys the zero-mean Gaussian distribution, that is:

[0142]

[0143] In formula (17), |·| represents the determinant of the matrix.

[0144] When the new information vector of the DVL or USBL observation value does not obey the zero-mean Gaussian distribution, Equation (17) will no longer hold. Therefore, according to the concept of Mahalanobis distance, the new information vector The square of the Mahalanobis distance to the zero vector is chi-squared, which can be used to establish a criterion for sensor fault detection. Based on this criterion, the test statistic of abnormal observation information can be defined as

[0145]

[0146] In formula (18): is the innovation vector Mahalanobis distance to the zero vector.

[0147] For a given significance level α and the corresponding α-quantile There is the following formula (19):

[0148]

[0149] In formula (19), Pr(·) is the probability of a random event. α is usually chosen to be a small value to describe the probability of the null hypothesis being rejected.

[0150] Formula (19) means that if The value is greater than the quantile Then the observed value has a great probability of being abnormal (1-α). Therefore, the hypothesis test criteria for observation abnormality can be established:

[0151] i) Null hypothesis: If the observed values ​​at all times satisfy The observation results are normal;

[0152] ii) Alternative hypothesis: If there exists an observation at time k that satisfies There are abnormal observation results;

[0153] After this, if the null hypothesis is true, IFGO does not perform special treatment; otherwise, an adjustment factor is embedded in the residual term of DVL or USBL in the following formula (28): To adjust the observation noise covariance, that is, to design and calculate the adjustment factor in step 4 above to correct the corresponding observation noise covariance matrix in step 3. When correcting, the adjustment factor is added to the observation noise covariance matrix of the above DVL and USBL. To make corrections, as shown in the following formula (20):

[0154]

[0155] The above regulatory factors The calculation process includes the following steps:

[0156] Step b1: Calculate the adjustment factor The corrected innovation vector covariance is shown in the following equation (21):

[0157]

[0158] In formula (21): represents the observation innovation vector corresponding to DVL or USBL; P k+1|k is the prior state covariance;

[0159] Step b2: Based on the Mahalanobis distance concept, a nonlinear equation is established for the covariance of the corrected innovation vector. The established nonlinear equation is shown in the following equation (22):

[0160]

[0161] In formula (22): is the quantile corresponding to the given significance level α, and α is the probability of rejecting the null hypothesis when judging whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance;

[0162] Step b3: Use Newton's method iteration and the derivative formula of the inverse matrix to solve the adjustment factor;

[0163] Formula (22) is a nonlinear equation, and the adjustment factor It is difficult to solve directly. However, it can be determined iteratively by Newton's method, as shown in the following equation (23):

[0164]

[0165] In formula (23), d is the number of iterations.

[0166] Using the derivative formula of the inverse matrix, that is The expression of the adjustment factor obtained by solving is shown in the following formula (24):

[0167]

[0168] In formula (24):

[0169] The iterative process of formula (24) is achieved by setting Initialize, when the test statistic satisfies Or when the number of iterations is greater than the preset value, the iteration is completed and the adjustment factor can be solved.

[0170] The implementation of the above step 5 is as follows:

[0171] In step 5, the noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update, the residual vector of the DVL and the final observation noise covariance matrix, and the residual vector of the USBL and the final observation noise covariance matrix are designed as factor nodes and inserted into the factor graph framework, preferably using the least squares method.

[0172] A factor graph is a binary graph model of the joint probability distribution of random variables formed by decomposing the global function into the product of local functions. It consists of two types of nodes: factor nodes and state variable nodes. Therefore, the global function f(·) can be decomposed into

[0173]

[0174] In formula (25): y k is the node set of all state variables; f k (·) is a local function.

[0175] for Figure 2 In the INS / DVL / USBL integrated navigation scheme shown in Figure 2, the IMU pre-integration factor, DVL observation factor, and USBL observation factor can be considered as independent zero-mean Gaussian distributions. Therefore, according to equations (7) and (14), they can be expressed as equation (26):

[0176]

[0177] In formula (26), u represents the time when DVL and USBL observations occur.

[0178] During the factor graph optimization process, if the nodes in the factor graph are not constrained, the number of factor nodes will increase infinitely over time, which will continuously increase the optimization time. Therefore, in order to reduce the computational cost, we need to combine marginalization to limit the computational complexity of the sliding window optimizer. When the number of factor nodes saved in the factor graph exceeds the threshold (sliding window size), we marginalize the oldest system state and convert the IMU pre-integrated quantity and DVL and USBL observations corresponding to the marginalized state into prior factors. The reason for marginalizing the system state is that in the factor graph optimization process, prior constraints are required for iterative optimization, so each time a new USBL (or DVL) observation is obtained and the nonlinear optimization is completed, the oldest factor node saved in the sliding window optimizer is converted into prior information by marginalization. This is also the reason why the embodiment of the method of the present invention preferably includes the following step seven.

[0179] X={x k-N+1 ,x k-N+2 ,...,x k} represents the set of state variables of all time nodes in the sliding window of size N. The optimal solution of X can be obtained by the following formula (27):

[0180]

[0181] Then, substitute (26) into (27) to obtain formula (28):

[0182]

[0183] In formula (28): {r ξ ,H ξ} represents the prior state information from marginalization, which represents the summary of past information not included in the window and is used to initialize the probability distribution of the state in the window.

[0184] Solving equation (28) is an optimization problem, and the maximum a posteriori solution of X can be uniquely determined by the Levenberg-Marquardt algorithm. Therefore, in the present invention, in step 5, all nodes in the factor graph are nonlinearly optimized to obtain the navigation solution, and the Levenberg-Marquardt algorithm is used.

[0185] See also Figure 4 The embodiment of the method of the present invention preferably also includes step six: calculating the covariance matrix corresponding to the navigation solution obtained in step five, so as to be used in calculating the navigation solution at the next moment, and solving the covariance of the new information vector when judging whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance.

[0186] This embodiment uses the Levenberg-Marquardt algorithm to solve the navigation solution and the corresponding covariance matrix. According to the Levenberg-Marquardt algorithm, the following formula (29) holds:

[0187]

[0188] In formula (29): is the Jacobian matrix of the global cost function at time k in formula (28); Represents the error of the state within the sliding window; is the residual term in formula (28); is the corresponding covariance matrix; μI is a positive definite diagonal matrix.

[0189] Then, according to the definition of state error covariance, we have formula (30) holds:

[0190]

[0191] In formula (30): Σ represents the relevant covariance. Therefore, the state error covariance P at time k is k Can be obtained from get.

[0192] See also Figure 4 The method embodiment of the present invention preferably further includes step 7: determining whether marginalization is required; if so, converting the oldest factor node stored in the sliding window optimizer into prior state information through marginalization for use in solving the navigation solution at the next moment, and simultaneously outputting the navigation solution obtained in step 5; if not, directly outputting the navigation solution obtained in step 5. The benefits of marginalization have been described in detail above and will not be repeated here.

[0193] Simulation verification:

[0194] The effects of the present invention can be further illustrated by the following simulation.

[0195] The performance of the underwater vehicle factor graph fusion navigation method with abnormal observation processing capability proposed in this invention was evaluated through computer simulation. In the simulation experiment, the initial position of the underwater autonomous vehicle was set to 114.472° east longitude, 30.460° north latitude, and altitude -140m; the initial speed in the east, north, and sky directions was 0m / s; the initial attitude was pitch angle 0°, roll angle 0°, and heading angle 276°. The accelerometer zero bias and white noise were 200ug and 100μg, respectively. The gyroscope constant drift and white noise are 25° / h and The data update rate of the gyroscope and accelerometer is 100Hz. The slant range measurement error of USBL is 8m, and the data update rate is 1Hz; the velocity error of DVL is 0.05m / s, and the data update rate is 1Hz. According to the chi-square distribution table, 11.3456 (α = 0.01) is selected, and the degree of freedom is 3. The sliding window size N is 30. Figure 5 The figure shows a simulated underwater trajectory of an AUV over a period of time, consistent with the actual underwater environment, including static, accelerating, constant speed, and turning states. The total simulation time is 1600 seconds. It is important to note that the operating frequencies of the sensors are asynchronous, and the update frequency of the DVL and USBL is lower than that of the INS. This allows for comprehensive verification of the effectiveness and reliability of the proposed method.

[0196] At the same time, in order to evaluate the performance of the proposed IFGO from the perspective of abnormal observations, various types of observation anomalies are considered, including sudden abnormal observations and observations with external interference presenting a Gaussian mixture distribution. In addition, the mean square error (MSE) is used to quantify the performance of each data fusion algorithm. MSE can be defined as Equation (31):

[0197]

[0198] In formula (31), k represents the time node (unit: s), A represents the total duration, represents the state estimate after algorithm optimization, A true value representing the navigation state.

[0199] The position error comparison results of IFGO, FKF and FGO are as follows: Figure 6 As shown in the figure, when the sensors are operating normally, the position and velocity estimation accuracy of FGO is slightly better than that of FKF. This is because in real-time navigation estimation, the distributed FKF needs to wait until all sensor observation data is collected before information fusion can be performed. This requires addressing the asynchrony between information fusion time and sensor observation time in advance, which leads to a decrease in navigation accuracy. However, FGO provides a plug-and-play unified navigation framework with strong flexibility and scalability by simply adding or subtracting relevant factor nodes. Therefore, FGO can effectively overcome the impact of sensor observation asynchrony and promptly add sensor observation information to the factor graph.

[0200] Furthermore, we can see that both FGO and FKF perform poorly when USBL and DVL generate abnormal observations. Based on the previous analysis, it's clear that both algorithms rely on sensor observation accuracy, and the observation error covariance matrix is ​​set during the algorithm initialization phase. Abnormal sensor observations can affect navigation estimation accuracy and, in severe cases, even lead to navigation failure. However, FGO performs relatively well compared to FKF. This is because FGO stores state estimates for a period of time in a sliding window. Each time a state estimate is performed, all state estimates within that period are jointly optimized to minimize the total cost function. This places a constraint on the current state estimate based on the historical state. Therefore, when a DVL or USBL failure occurs, the high accuracy of the historical state will have a certain suppressive effect on the deviation of the current state estimate. In contrast, FKF is essentially a data-weighted fusion of the current estimation results from two sets of subfilters. Therefore, a sensor failure will inevitably affect the navigation performance of the entire system.

[0201] In contrast, the proposed IFGO algorithm significantly improves navigation estimation accuracy when sensor observations are abnormal. This is because the IFGO algorithm, based on the Mahalanobis distance concept, constructs a regulation factor based on the current innovation vector and its covariance matrix. This regulation factor is introduced during the factor graph optimization process to dynamically adjust the weight of each sensor observation, effectively suppressing the impact of abnormal sensor observations on the system state estimation. Figure 7 The mean squared error (MSE) of the position estimates for FKF, FGO, and IFGO is intuitively described. The above results verify that the proposed underwater vehicle factor graph fusion navigation method with anomaly observation processing capabilities demonstrates that when an AUV is operating underwater and the sensor experiences observation anomalies due to external or internal factors, IFGO demonstrates better navigation and positioning accuracy than FKF and FGO.

[0202] This paper, targeting autonomous underwater vehicles (AUVs), proposes a factor graph fusion navigation method for AUVs with the ability to handle abnormal observations. This method, based on an improved factor graph algorithm for observation fault detection and error compensation, aims to enhance the positioning accuracy and robustness of INS / DVL / USBL integrated navigation systems. The present invention first improves upon the traditional integrated navigation algorithm and designs a novel integrated navigation system based on a factor graph framework. This integrated navigation system only adds corresponding factors when sensors are valid, overcoming the problem of the traditional FKF algorithm requiring repeated system reconstruction, enabling plug-and-play of sensors and improving the reliability of the integrated navigation system. Furthermore, during the factor graph optimization process, a fault detection method based on the Mahalanobis distance is designed to address sensor observation anomalies, thereby improving the fault tolerance of the AUV integrated navigation system. This method can meet the positioning accuracy requirements for AUVs traveling along a predetermined trajectory while also improving the efficiency of AUVs during underwater operations.

Claims

1. A factor graph fusion navigation method for underwater vehicles with abnormal observation processing capability, characterized in that: The following steps are involved: Step 1: Establish an INS / DVL / USBL integrated navigation system model based on the factor graph framework and define the state space of the system; In step 1, an INS / DVL / USBL integrated navigation system model based on a factor graph framework is established. Specifically, the information output by the IMU in the INS is processed into factor nodes by pre-integration, the speed information output by the DVL is expressed as a speed observation factor, and the position information output by the USBL is expressed as a position observation factor. The three are loosely coupled. The east-north-sky geographic coordinate system is defined as the g coordinate system, the navigation coordinate system is defined as the n system, and the east-north-sky geographic coordinate system is selected as the navigation coordinate system, the carrier coordinate system is defined as the b system, and the inertial coordinate system is defined as the i system. The state space of the system described in step 1 is shown in the following formula (1): In formula (1): k is the time node; is the three-dimensional position; is the three-dimensional velocity; It is a three-dimensional posture; and are the zero bias of the gyroscope and accelerometer respectively; Step 2: Based on the IMU discrete motion equation and the IMU noise error propagation model, the noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update are calculated; The second step comprises the following steps: Step a1: Pre-integrate the IMU according to the IMU discrete motion equation, and calculate the relative motion increment of the IMU in position, velocity and attitude from time k-1 to time k; Step a2: Subtract the calculated relative motion increments of the IMU in position, velocity, and attitude from time k-1 to time k from the estimated relative motion increments to obtain a residual vector of the IMU pre-integration factor with a fixed zero bias; Step a3: Calculate the noise covariance matrix and Jacobian matrix of the IMU pre-integration quantity according to the IMU noise error propagation model to correct the gyroscope and accelerometer bias in the residual vector of the IMU pre-integration factor with a fixed bias, and obtain the residual vector of the IMU pre-integration factor with a bias update; Step 3: Establish the observation equation based on the DVL and USBL observation models and obtain the corresponding residual vector and observation noise covariance matrix; Step 4: Based on the concept of Mahalanobis distance, determine whether the DVL or USBL observation value is normal; if the observation value is normal, the observation noise covariance matrix obtained in step 3 is its final form; if the observation value is abnormal, design and calculate the adjustment factor to correct the corresponding observation noise covariance matrix in step 3, and obtain the final form of the corresponding observation noise covariance matrix after correction; Step 5: The noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update, the residual vector of the DVL and the final observation noise covariance matrix, as well as the residual vector of the USBL and the final observation noise covariance matrix are designed as factor nodes and inserted into the factor graph framework. All nodes in the factor graph are nonlinearly optimized to obtain the navigation solution.

2. The factor graph fusion navigation method for underwater vehicles with abnormal observation and processing capabilities according to claim 1 is characterized by: The IMU discrete motion equation is shown in the following formula (2): In formula (2): is the projection of the instantaneous angular velocity of system b relative to system i in system b; f b is the acceleration of the carrier in the b system; n g and n a are the zero-mean Gaussian white noise of the gyroscope and accelerometer, respectively; The obtained residual vector of the IMU pre-integration factor with fixed zero bias is shown in the following formula (7): In formula (7): r Δ～ is the residual of position, velocity and attitude; r b～ Represents the residual of IMU zero bias; Log(·) represents the logarithmic mapping of the vector; and are the relative motion increments of the IMU in attitude, velocity, and position, respectively; is the attitude rotation matrix from b system to n system; g n is the acceleration due to gravity; Δt represents the time interval between two adjacent observations; The error state vector of the pre-integration component is defined as follows: In formula (8): and are the position, velocity and attitude errors of the pre-integrated components respectively; and are the bias errors of the gyroscope and accelerometer respectively; The IMU noise error propagation model uses the error perturbation method, and the dynamic model of the error state is shown in the following formula (9): In formula (9): t is the noise vector; F t and G t are the state transfer matrix and error transfer matrix respectively; In formula (7), and The residual vector of the IMU pre-integration factor with zero bias update can be obtained by updating it with the first-order expansion shown in the following formula (12); formula (12) is as follows: In formula (12): represents the updated zero bias; Εxp(·) represents the exponential mapping of the vector; J k-1,k The Jacobian matrix representing the k-1 to k pre-integrated components; It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of It's J k-1,k It is equivalent to the submatrix of 3. The underwater vehicle factor graph fusion navigation method with abnormal observation and processing capability according to claim 2 is characterized by: The DVL and USBL observation equations established in step 3 are shown in equation (13): In formula (13): n DVL 、n USBL are the velocity observation noise of DVL and the position observation noise of USBL respectively; h DVL and h USBL is the observation function, which can be written as a linear function, namely: h DVL (x k )=H DVL x k and h USBL (x k )=H USBL x k , where H DVL =[0 3×3 I 3×3 0 3×9 ] and H USBL =[I 3×3 0 3×12 ]; The residual vectors of DVL and USBL are defined as follows: In formula (14): and Represent the residual vectors of DVL and USBL respectively; are the observation noise covariance matrices of DVL and USBL, respectively.

4. The underwater vehicle factor graph fusion navigation method with abnormal observation processing capability according to claim 1 is characterized in that: In step 4, based on the concept of Mahalanobis distance, it is determined whether the DVL or USBL observation value is normal, specifically: First, determine whether the innovation vector of the DVL or USBL observation value obeys the zero-mean Gaussian distribution; if it obeys, the DVL or USBL observation value is normal; if not, determine whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance.

5. The underwater vehicle factor graph fusion navigation method with abnormal observation and processing capability according to claim 3 is characterized by: The adjustment factor is designed and calculated in step 4 to modify the corresponding observation noise covariance matrix in step 3. When modifying, the adjustment factor is added to the observation noise covariance matrix of the DVL and USBL. to make corrections; The regulatory factor The calculation process includes the following steps: Step b1: Calculate the adjustment factor The corrected innovation vector covariance is shown in the following equation (21): In formula (21): l = DVL, USBL; represents the observation innovation vector corresponding to DVL or USBL; P k+1|k is the prior state covariance; Step b2: Based on the Mahalanobis distance concept, a nonlinear equation is established regarding the covariance of the corrected innovation vector; the established nonlinear equation is shown in the following equation (22): In formula (22): is the quantile corresponding to the given significance level α, and α is the probability of rejecting the null hypothesis when judging whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance; Step b3: Use Newton's method iteration and the derivative formula of the inverse matrix to solve the adjustment factor; The expression of the adjustment factor obtained by solving is shown in the following formula (24): In formula (24): d is the number of iterations; The iterative process of formula (24) is achieved by setting Initialize, when the test statistic satisfies Or when the number of iterations is greater than the preset value, the iteration is completed and the adjustment factor can be solved.

6. The underwater vehicle factor graph fusion navigation method with abnormal observation and processing capability according to claim 1 is characterized by: In step 5, the noise covariance matrix of the IMU pre-integration quantity and the residual vector of the IMU pre-integration factor with zero bias update, the residual vector of the DVL and the final observation noise covariance matrix, and the residual vector of the USBL and the final observation noise covariance matrix are designed as factor nodes and inserted into the factor graph framework using the least squares method; In step 5, all nodes in the factor graph are nonlinearly optimized to obtain the navigation solution using the Levenberg-Marquardt algorithm.

7. The underwater vehicle factor graph fusion navigation method with abnormal observation and processing capability according to claim 1 is characterized by: It also includes step six: calculating the covariance matrix corresponding to the navigation solution obtained in step five, so as to be used in calculating the navigation solution at the next moment. When judging whether the DVL or USBL observation value is normal based on the concept of Mahalanobis distance, the covariance of the new information vector is solved.

8. The underwater vehicle factor graph fusion navigation method with abnormal observation and processing capability according to claim 7 is characterized by: It also includes step seven: determining whether marginalization is required; if so, converting the oldest factor node stored in the sliding window optimizer into prior state information through marginalization, which is used to solve the navigation solution at the next moment, and outputting the navigation solution obtained in step five at the same time; if not, directly outputting the navigation solution obtained in step five.

Images