Inertial baseline integrated navigation method, and product, medium, device and autonomous underwater vehicle

Abstract

Disclosed are an inertial baseline integrated navigation method, and a product, a medium, a device and an autonomous underwater vehicle, relating to the field of navigation. The method comprises: in an initial stage of the starting of an inertial baseline integrated navigation system, an SINS using a USBL to assist with initial alignment and obtaining initial attitude information (1); when the inertial baseline integrated navigation system enters a formal navigation operating state, the SINS performing pure inertial navigation solving on the basis of the initial attitude information, so as to obtain navigation parameter calculation values including errors (2), wherein the navigation parameter calculation values comprise the attitude, velocity and position; on the basis of the navigation parameter calculation values including the errors, constructing a navigation state error model based on a Lie group (3); on the basis of a Huber function, constructing a generalized maximum likelihood estimation model (4); according to the navigation state error model based on a Lie group and the generalized maximum likelihood estimation model, establishing an integrated navigation robust Kalman filtering algorithm (5); on the basis of the integrated navigation robust Kalman filtering algorithm, performing filtering estimation on the inertial baseline integrated navigation system, so as to obtain a state estimation value (6); and on the basis of the state estimation value, performing feedback updating on navigation parameters, and on the basis of the updated navigation parameters, performing navigation. The method can improve the integrated filtering precision and positioning and orientation precision.

Description

Inertial-based integrated navigation method, product, medium, equipment and underwater autonomous underwater vehicle

[0001] The present application claims priority to the Chinese patent application No. 202410535383.6, filed on April 29, 2024, and entitled "Inertial-based integrated navigation method, product, medium, equipment and underwater autonomous underwater vehicle", the content of which is incorporated herein by reference in its entirety. TECHNICAL FIELD

[0002] The present application relates to the field of navigation technology, in particular to an inertial-based integrated navigation method, product, medium, equipment and underwater autonomous underwater vehicle. BACKGROUND

[0003] The underwater autonomous underwater vehicle (AUV) has high autonomy, portability and concealment, and is currently widely used in ocean resource development and underwater target detection. The inertial-based integrated navigation system, such as the combination of strapdown inertial navigation system (SINS) and ultra-short baseline positioning system (USBL), can provide attitude, velocity and position information for AUV underwater work. The filtering algorithm is one of the key technologies of the inertial-based integrated navigation system, and the filtering accuracy directly affects the performance of the AUV. Therefore, in practical engineering applications, it is necessary to improve the integrated filtering accuracy of the inertial-based integrated navigation system as much as possible.

[0004] In order to improve the positioning and orientation accuracy and the robustness to non-Gaussian noise, in recent years, many scholars have carried out a lot of research on the filtering algorithm of the inertial-based integrated navigation system. Due to the extremely complex underwater terrain and underwater sound environment, abnormal phenomena such as signal distortion may occur during the propagation of sound waves in seawater medium, resulting in non-Gaussian noise in the measurement information of the ultra-short baseline. When the traditional standard Kalman filter algorithm is used to estimate the integrated navigation results, the filtering failure problem may occur, which may seriously endanger the safety of underwater vehicle navigation. SUMMARY

[0005] The purpose of the present application is to provide an inertial-based integrated navigation method, product, medium, equipment and underwater autonomous underwater vehicle, which has strong applicability and robustness to Gaussian noise and non-Gaussian noise, and can improve the integrated filtering accuracy and positioning and orientation accuracy of the inertial-based integrated navigation system.

[0006] To achieve the above purpose, the present application provides the following solutions.

[0007] In an exemplary embodiment, the application provides an inertial-based integrated navigation method, comprising: in an initial stage of starting of an inertial-based integrated navigation system, SINS uses USBL to assist initial alignment to obtain initial attitude information of the AUV; when the inertial-based integrated navigation system enters a formal navigation working state, SINS performs pure inertial navigation calculation based on the initial attitude information to obtain a navigation parameter calculation value containing errors; the navigation parameter includes attitude, velocity and position of the AUV; a navigation state error model based on Lie group is constructed according to the navigation parameter calculation value containing errors; a generalized maximum likelihood estimation model is constructed based on Huber function; a robust Kalman filter algorithm for integrated navigation is established according to the navigation state error model based on Lie group and the generalized maximum likelihood estimation model; state estimation values are obtained by filtering and estimating the inertial-based integrated navigation system based on the robust Kalman filter algorithm for integrated navigation; and the navigation parameters are updated by feedback according to the state estimation values, and navigation is performed based on the updated navigation parameters.

[0008] In an exemplary embodiment, the SINS performs pure inertial navigation calculation based on the initial attitude information to obtain a navigation parameter calculation value containing errors, specifically comprising: under the condition that the initial attitude information is obtained after initial alignment and initial position and initial velocity information are obtained by means of other navigation sensors, SINS establishes a navigation differential equation in a geographic coordinate system by relying on angular velocity information output by a gyroscope and acceleration information output by an accelerometer; the navigation differential equation includes attitude matrix differential equation, velocity differential equation and position differential equation; the navigation differential equation of SINS is solved to obtain attitude, velocity and position calculation values containing errors.

[0009] In an exemplary embodiment, the navigation state error model based on Lie group is constructed according to the navigation parameter calculation value containing errors, specifically comprising: a Lie group state variable matrix of SINS is constructed based on the attitude and velocity of the AUV; a Lie group state error model of SINS is constructed according to the right invariant error definition of Lie group theory based on the navigation parameter calculation value containing errors; the Lie group state error model is differentiated to construct a differential equation of the Lie group state error model; and the navigation state error model based on Lie group is obtained by deducing based on the differential equation of the Lie group state error model.

[0010] In an exemplary embodiment, the generalized maximum likelihood estimation model is constructed based on Huber function, specifically comprising: an expression of a robust function ρ in Huber function is selected where ζ i represents the i-th residual error; γ is a key parameter; a cost function of the generalized maximum likelihood estimation model is constructed according to the expression of the robust function ρ where m represents the dimension of the residual error; x represents the state variable; ζ iAn estimated value of the state variable is obtained.

[0011] In an exemplary embodiment, the establishing the robust Kalman filtering algorithm for integrated navigation based on the Lie group-based navigation state error model and the generalized maximum likelihood estimation model comprises: obtaining a measurement model based on the Lie group-based navigation state error model; defining an error state space model of a given linear discrete system based on the measurement model and the Lie group-based navigation state error model; and establishing the robust Kalman filtering algorithm for integrated navigation based on the error state space model and the generalized maximum likelihood estimation model.

[0012] In an exemplary embodiment, the filtering and estimating of the inertial-based integrated navigation system based on the robust Kalman filtering algorithm for integrated navigation comprises: defining a combined system model based on the measurement model and the Lie group-based navigation state error model; defining a decoupling matrix to obtain decoupled measurement values, a measurement matrix and a residual error based on the combined system model; calculating a measurement residual weighted matrix and a state prediction residual weighted matrix based on the decoupled measurement values, the measurement matrix, the residual error and the generalized maximum likelihood estimation model; performing a time update process of the robust Kalman filtering algorithm for integrated navigation to calculate a one-step prediction of the state and a one-step prediction error covariance matrix of the state based on a state transition matrix and a system noise driving matrix; performing a measurement update process of the robust Kalman filtering algorithm for integrated navigation to calculate a filtering gain, wherein the one-step prediction error covariance matrix of the state and a measurement noise covariance matrix are dynamically adjusted in real time based on the measurement residual weighted matrix and the state prediction residual weighted matrix during the calculation of the filtering gain; and further performing a state estimation and a calculation of a mean square error matrix of the state estimation after the calculation of the filtering gain to obtain an estimated value of the state and a calculation result of an error covariance matrix of the state estimation.

[0013] In an exemplary embodiment, the present application further provides a computer program product comprising a computer program which, when executed by a processor, implements the inertial-based integrated navigation method.

[0014] In an exemplary embodiment, the present application further provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the inertial-based integrated navigation method.

[0015] In an example embodiment, the application further provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the inertial-based integrated navigation method.

[0016] In an example embodiment, the application further provides an underwater autonomous underwater vehicle, comprising an inertial-based integrated navigation system configured to implement the inertial-based integrated navigation method.

[0017] According to the specific embodiments provided by the application, the following technical effects are disclosed.

[0018] Currently, most of the inertial-based integrated navigation algorithms used in engineering are based on Kalman Filtering (KF) algorithm for filtering and positioning of the integrated navigation system. This method is simple and easy to implement, but since it can only filter and estimate Gaussian noise, it will bring a large positioning error when non-Gaussian noise exists in the measurement information of the sensor, especially when the proportion of non-Gaussian noise is large. The Huber-IEKF algorithm for integrated navigation robust Kalman filtering proposed in the application can effectively filter and estimate non-Gaussian noise, especially when the noise pollution is serious. Further, the navigation state error model based on Lie group established by the application based on the related theory of Lie group and Lie algebra reduces the dependence of the state transition matrix on the state variable, thereby further improving the estimation accuracy of the system model. In particular, when the alignment time is short or the initial alignment effect is not good due to other reasons, the navigation state error model proposed in the application has obvious advantages. Therefore, the method of the application is suitable for processing Gaussian noise and non-Gaussian noise, has strong applicability and robustness, can improve the integrated filtering accuracy of the inertial-based integrated navigation system, and further improve the positioning and orientation accuracy of the integrated navigation. BRIEF DESCRIPTION OF DRAWINGS

[0019] In order to more clearly illustrate the technical solutions in the embodiments of the application, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor.

[0020] FIG. 1 is a flowchart of the inertial-based integrated navigation method provided by the application;

[0021] FIG. 2 is a schematic diagram of the AUV navigation track in the simulation test;

[0022] FIG. 3 is a schematic diagram of the attitude error comparison in the simulation test;

[0023] Figure 4 is a schematic diagram of a comparison of speed errors in a simulation test;

[0024] Figure 5 is a schematic diagram of a comparison of position errors in a simulation test. DETAILED DESCRIPTION

[0025] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work fall within the scope of protection of the present application.

[0026] The inertial-based integrated navigation method, product, medium, equipment and underwater autonomous underwater vehicle provided by the present application have strong applicability and robustness to Gaussian noise and non-Gaussian noise, and can improve the integrated filtering precision and positioning and orientation precision of the inertial-based integrated navigation system.

[0027] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the drawings and specific embodiments.

[0028] As shown in Figure 1, the inertial-based integrated navigation method provided by the present application comprises the following steps 1 to step 7.

[0029] Step 1: In the initial stage of starting the inertial-based integrated navigation system, the strapdown inertial navigation system uses the ultra-short baseline positioning system to assist initial alignment to obtain initial attitude information of the underwater autonomous underwater vehicle.

[0030] In some embodiments, the inertial-based integrated navigation system of the present application preferably adopts a navigation system combined with a strapdown inertial navigation system (SINS) and an ultra-short baseline positioning system (USBL) to provide navigation information of the underwater autonomous underwater vehicle (AUV) when working underwater, which usually includes attitude, speed and position information.

[0031] According to the matrix chain operation rule, the attitude matrix at any time can be decomposed into the following form:

[0032] Wherein, b represents the carrier coordinate system, also known as the navigation system, and the carrier in the present application mainly refers to the AUV. n represents the "east-north-sky" geographic coordinate system. ​C represents the attitude transformation matrix between the body coordinate system (b-system) and the geographic coordinate system (n-system), which contains the pitch, roll and heading information. t represents the alignment time, and 0 represents the initial time. C represents the attitude transformation matrix between the body coordinate system (b-system) and the geographic coordinate system (n-system), which contains the pitch, roll and heading information. t represents the alignment time, and 0 represents the initial time. C represents the attitude transformation matrix between the body coordinate system (b-system) and the geographic coordinate system (n-system), which contains the pitch, roll and heading information. t represents the alignment time, and 0 represents the initial time. C represents the attitude transformation matrix between the body coordinate system (b-system) and the geographic coordinate system (n-system), which contains the pitch, roll and heading information. t represents the alignment time, and 0 represents the initial time. C represents the attitude transformation matrix between the body coordinate system (b-system) and the geographic coordinate system (n-system), which contains the pitch, roll and heading information. t represents the alignment time, and 0 represents the initial time. C represents the attitude transformation matrix between the body coordinate system (b-system) and the geographic coordinate system (n-system), which contains the pitch, roll and heading information. t represents the alignment time, and 0 represents the initial time. The rotation angular velocity of the geographic coordinates (i.e., navigation coordinates) calculated by the navigation system with respect to the inertial coordinate system (inertial space) and the angular velocity output by the gyroscope is updated in real time, and the differential equation is as follows:

[0033] According to formulas (1)-(3), once the initial attitude matrix is determined, the real-time updated and can be used to obtain the attitude matrix at any time t. is the skew-symmetric matrix of , and its specific expansion is The other parameters with the symbol × have similar meanings. are the components in the X, Y, and Z axis directions, respectively.

[0034] The specific force equation of the strapdown inertial navigation system (SINS) is rewritten as:

[0035] where v n and v b represent the motion velocities of the carrier in the geographic coordinate system n and the body coordinate system b, respectively; f b represents the specific force output by the accelerometer, i.e., the acceleration information; and are the projections of the angular velocity of the earth rotation in the geographic coordinate system n and the body coordinate system b, respectively; i.e., the rotation angular velocity of the earth with respect to the inertial system; represents the rotation angular velocity of the carrier with respect to the earth's surface, i.e., the rotation angular velocity of the navigation system with respect to the earth system; and g n represents the local gravity acceleration.

[0036] As can be seen from the above formula (4), the initial attitude matrix exists in the specific force equation. Double integration of both sides of the above formula (4) gives: ​

[0037] where,

[0038] β p (t) is calculated directly by numerical integration algorithm. dσ and dτ represent the infinitesimal change of integration variable. p In (t), v b(0) The initial time Doppler velocity log measurement information can be directly obtained, The relative position information measured by USBL can be obtained, and the remaining integral terms are calculated by numerical integration algorithm.

[0039] where, represents the initial time USBL measured relative position vector. represents the t time USBL measured relative position vector. and represents the installation deviation matrix between the strapdown inertial navigation system and the USBL acoustic array, which is a known quantity, corresponding to the initial time and t time respectively.

[0040] After calculating the results of α p (t) and β p (t), the initial attitude matrix can be obtained by vector attitude determination algorithm, and the current time attitude matrix

[0041] In step 1 of the present application, an initial alignment method assisted by USBL relative position information is given, which is different from the traditional underwater DVL assisted initial alignment method, and provides a new solution for the start of underwater strapdown inertial navigation system.

[0042] Step 2: when the inertial integrated navigation system enters the formal navigation working state, the strapdown inertial navigation system performs pure inertial navigation calculation based on the initial attitude information, and obtains the navigation parameter calculation value containing error.

[0043] Under the condition of obtaining initial attitude information through initial alignment and obtaining initial position and initial velocity by means of other auxiliary sensors, the strapdown inertial navigation system relies on the angular velocity information b output by the gyroscope and the acceleration information f output by the accelerometer to calculate the navigation parameters in real time, including the attitude, velocity and position of the AUV.

[0044] The present application adopts the navigation differential equation of the strapdown inertial navigation system in geographic coordinate system (n system) to solve the attitude, velocity and position in real time:

[0045] where, represents the attitude transformation matrix between the body coordinate system (b-frame) and the geographic coordinate system (n-frame). L represents the latitude, λ represents the longitude, and h represents the geographic height; L, λ, and h collectively constitute the position information navigation quantity p n The "." above the parameter represents its first derivative. represents the eastward velocity, represents the northward velocity, represents the skyward velocity. R M is the meridian curvature radius of the earth, R N is the prime vertical curvature radius of the earth.

[0046] In the navigation differential equations (9)-(11), the attitude matrix differential equation (9) can be converted into a quaternion differential equation and solved based on the fourth-order Runge-Kutta method. The velocity differential equation (10) and the position differential equation (11) can be continuously solved using a numerical integration method. By solving the navigation differential equations (9)-(11) of the strapdown inertial navigation system, complete attitude, velocity, and position information navigation quantities, i.e. v n and p n , can be obtained. Due to the existence of gyro and accelerometer measurement errors, there are navigation errors in the calculated values of the navigation parameters, and the calculated values of the navigation parameters containing errors are denoted as and δp n . Subsequently, the navigation errors will be estimated by combining with other navigation devices and using filtering estimation, and the strapdown inertial navigation will be feedback corrected.

[0047] Initial alignment acts on the startup phase of the integrated navigation system, and provides initial navigation information, especially initial attitude information, for the subsequent integrated navigation running phase. Pure inertial navigation calculation is one of the core contents of the integrated navigation system, and the purpose of integrated navigation is to eliminate the errors of pure inertial navigation calculation and ensure the positioning and orientation accuracy of the strapdown inertial navigation system.

[0048] Step 3: constructing a Lie group-based navigation state error model according to the calculated values of the navigation parameters containing errors.

[0049] The step 3 of constructing a Lie group-based navigation state error model according to the calculated values of the navigation parameters containing errors specifically includes the following steps 3.1 to 3.4.

[0050] Step 3.1: constructing a Lie group state variable matrix of the SINS based on the attitude and velocity of the AUV.

[0051] In this application, the Lie group state variable is defined as an SE(3) Lie group matrix, which is composed of an attitude matrix velocity , i.e.:

[0052] Taking inverse of formula (3) gives:

[0053] Step 3.2: Based on the error-containing navigation parameter calculation value, the Lie group state error model of SINS is constructed according to the right-invariant error definition of Lie group theory.

[0054] In the present application, the system state variable of the Invariant Extended Kalman Filter (IEKF) is a group composed of matrices, so the state error is defined on the group, and there are two forms of left-invariant error and right-invariant error.

[0055] Left-invariant error definition:

[0056] Right-invariant error definition:

[0057] According to the definition of the right-invariant error model, we have:

[0058] where is the inverse of the Lie group state variable matrix of SINS; and are error-containing navigation parameter calculation values; represents the Lie group matrix composed of error-containing navigation parameter calculation values calculated by inertial navigation is the transpose of . The parameter above with "hat" indicates that the parameter contains errors.

[0059] where the Lie group state error model is:

[0060] η φ and η v are the attitude error and velocity error in the right-invariant error model, respectively. Simplifying and rewriting formula (16) gives:

[0061] Step 3.3: Derive the Lie group state error model to construct the differential equation of the Lie group state error model.

[0062] According to the mapping relationship between Lie group and Lie algebra, when the misalignment angle error φ is a small angle, we have:

[0063] where, I represents the identity matrix. Deriving formulas (19) and (20) gives the differential equation of the Lie group state error model:

[0064] where denotes the derivative of δv n denotes the derivative of v n η φ and η v denote the attitude error vector and the velocity error vector in the right-invariant error model, respectively. and are the derivatives of the attitude error vector and the velocity error vector in the right-invariant error model, respectively. denotes the rotation angular velocity of the navigation frame with respect to the inertial frame, denotes the rotation angular velocity of the earth with respect to the inertial frame, denotes the rotation angular velocity of the navigation frame with respect to the earth frame, where:

[0065] ω ie denotes the earth rotation angular velocity, L denotes the local latitude, v E and v N denote the eastward and northward velocities, respectively, R M denotes the meridian radius of curvature, R N denotes the prime vertical radius of curvature, and h denotes the altitude; denotes the error of calculated by the inertial navigation system; denotes the projection of the gyro drift in the navigation frame; g n denotes the earth gravity vector. δp n denotes the position error; ε b denotes the gyro drift, denotes the accelerometer bias.

[0066] In the differential equations (21) and (22) of the Lie group state error model, M1, M2 and M av denote the following, respectively:

[0067] The differential equation of the position error is:

[0068] where M pv and M pp are coefficient matrices.

[0069] Step 3.4: Based on the differential equations of the Lie group state error model, the Lie group-based navigation state error model is derived.

[0070] The right error model state error is defined as:

[0071] ​According to the derivation of the above formula (21), (22), (28), the gyro drift ε b and the accelerometer zero offset Modeling as a random constant plus Gaussian noise, the error equation about the state quantity is obtained as:

[0072] Equation (30) is the navigation state error model based on Lie group, wherein G(t) and W(t) represent noise distribution matrix and process noise vector, respectively. w g and w a represent three-axis gyroscope measurement noise and three-axis accelerometer measurement noise, respectively. F(t) is a state transition matrix. X is a simplified notation for X(t) at time t.

[0073] The state transition matrix can be determined from the error equations (21), (22), (28) based on Lie group derivation as:

[0074] In equation (31): M ap =M1+M2 (33);

[0075] Briefly: R Mh =R M +h, R Nh =R N +h (38)。

[0076] Step 4: Construct a generalized maximum likelihood estimation model based on the Huber function.

[0077] The present application defines a more general cost function, and selects the expression of a function ρ with high robustness; based on the cost function, a generalized maximum likelihood estimation model is obtained.

[0078] A more general cost function used in the present application is:

[0079] In the formula, m represents the dimension of the residual; ζ i represents the i-th residual. Compared with the ordinary maximum likelihood estimation method, ρ in the generalized maximum likelihood estimation cost function is arbitrary, and in particular when ρ = -ln(f(ζ iWhen the cost function J(x) reaches its maximum value, the generalized maximum likelihood estimation is the maximum likelihood estimation. The solution process of the state variable x is the same as the maximum likelihood estimation, and the specific expression of the function ρ plays a key role in the estimation of the state variable x. In order to make the state estimation value have higher estimation accuracy and robustness when the mixed Gaussian distribution noise or measurement anomaly occurs, Huber proposed a high-robustness function ρ expression as follows:

[0080] The influence function φ and the weighting function W are obtained as follows:

[0081] The size of the parameter γ has a key influence on the state estimation, and if the parameter γ is set too large, the estimation accuracy of the filter will be improved, but the robustness of the filter will be reduced, and vice versa, if the parameter γ is set too small, the robustness of the filter can be improved but the accuracy will be reduced.

[0082] The formula (39) is the cost function of the generalized maximum likelihood estimation model constructed in the application, and the state estimation value x is obtained by seeking the ζ that makes J(x) maximum. i The formula (40) is the robust function ρ in the formula (39), and the maximum likelihood estimation will have different effects by using different robust functions. The formulas (41) and (42) are two functions for describing the properties of the robust function ρ.

[0083] Step 5: According to the navigation state error model based on the Lie group and the generalized maximum likelihood estimation model, a robust Kalman filter algorithm for integrated navigation is established.

[0084] The step 5 establishes the robust Kalman filter algorithm for integrated navigation according to the navigation state error model based on the Lie group and the generalized maximum likelihood estimation model, and specifically includes the following steps 5.1 to 5.3.

[0085] Step 5.1: Obtain the measurement model according to the navigation state error model based on the Lie group.

[0086] Based on the above navigation state error model (30) based on the Lie group, the measurement model can be easily obtained according to the principle of the auxiliary navigation sensor: Z(t) = H(t)X(t) + V(t) (43);

[0087] In the formula, Z(t) represents the measurement vector, H(t) represents the measurement matrix, and V(t) represents the measurement noise vector.

[0088] Step 5.2: Give the error state space model of the linear discrete system according to the measurement model and the navigation state error model based on the Lie group.

[0089] ​According to the measurement model (43) and the Lie group-based navigation state error model (30), the error state space model of a given linear discrete system is shown as follows:

[0090] wherein Φ k / k-1 represents a discretized state transition matrix, Γ k / k-1 represents a discretized noise distribution matrix, W k-1 represents a discretized process noise vector. Z k , H k , X k , V k correspond to Z(t), H(t), X(t), V(t) discretized respectively.

[0091] Step 5.3: Establish a combined navigation robust Kalman filtering algorithm based on the error state space model and the generalized maximum likelihood estimation model; the combined navigation robust Kalman filtering algorithm includes a time update process and a measurement update process; the time update process includes one-step prediction of state and one-step prediction of state error covariance matrix estimation; the measurement update process includes filter gain calculation, state estimation, and calculation of state estimation error covariance matrix.

[0092] Based on the error state space model (44) and the generalized maximum likelihood estimation model, the combined navigation robust Kalman filtering algorithm (referred to as Huber-IEKF in this application) is as follows.

[0093] Time update:

[0094] One-step prediction of state:

[0095] One-step prediction of state error covariance matrix:

[0096] wherein Φ k / k-1 is determined by integrating the state transition matrix determined by the error equation based on the Lie group.

[0097] Measurement update:

[0098] Filter gain:

[0099] State estimation:

[0100] State estimation error covariance matrix:

[0101] wherein X represents state, P represents estimated error covariance matrix, and the subscript represents time. represents the state estimation value at k-1 time, P k-1denotes the state estimation error covariance matrix at time k-1. denotes the state prediction value at time k-1, P k / k-1 denotes the state prediction error covariance matrix at time k-1, also called the one-step prediction mean square error matrix of state. Γ k-1 denotes the noise assignment matrix at time k-1, Q k-1 denotes the process noise covariance matrix at time k-1, K k denotes the filtering gain at time k, R k denotes the measurement noise covariance matrix at time k, ψ x denotes the adjustment matrix of the state prediction error covariance matrix, ψ y denotes the adjustment matrix of the measurement noise covariance matrix, denotes the state estimation value at time k, P k denotes the state estimation error covariance matrix at time k.

[0102] In the combined navigation robust Kalman filtering algorithm established in the application, the Huber function is a robust estimation function, and the invariant extended Kalman filter (IEKF) + Huber function can effectively reduce the influence of outliers on the estimation result.

[0103] Step 6: filtering estimation of the inertial-based combined navigation system based on the combined navigation robust Kalman filtering algorithm to obtain a state estimation value.

[0104] The step 6 filtering estimation of the inertial-based combined navigation system based on the combined navigation robust Kalman filtering algorithm to obtain a state estimation value specifically includes the following steps 6.1 to 6.6.

[0105] Step 6.1: defining a combined system model according to a measurement model and a Lie group-based navigation state error model.

[0106] Let be the one-step prediction of state, be the difference between the true value of the state variable and the predicted value, then according to the measurement model (43) and the Lie group-based navigation state error model (30), the combined system model can be written as:

[0107] Step 6.2: defining a decoupling matrix to obtain decoupled measurement values, a measurement matrix and a residual based on the combined system model.

[0108] The decoupling matrix is defined as:

[0109] The decoupled measurement values Z k , the measurement matrix M k and the residual ξ k are obtained based on the combined system model (50).

[0110] The new measurement equation based on Huber's estimation is: Z k =M k X k +ξ k (55)

[0111] Step 6.3: Calculate the weighted matrix of measurement residuals and the weighted matrix of state prediction residuals based on the decoupled measurement values, measurement matrix, residuals, and generalized maximum likelihood estimation model.

[0112] The residual between the estimated value and the measured value is defined as: ζ=MX-Z(56).

[0113] Formula (56) is the static model of formula (55), which omits the time step subscript k.

[0114] Based on the definitions of the cost function (39) and robust function (40) of the generalized maximum likelihood estimation model, the diagonal matrix can be derived. and Where ρ′(ζ i ) is ρ(ζ i The first derivative of ) for the robust function ρ(ζ) i Find the first derivative to obtain the weighting function.

[0115] The diagonal matrix ψ is the weighted matrix ψ of the measurement residuals. y The weighted matrix ψ of the state prediction residuals x Composition, namely:

[0116] Step 6.4: Execute the time update process of the robust Kalman filter algorithm for integrated navigation, and calculate the one-step prediction of the state and the one-step prediction error covariance matrix of the state based on the state transition matrix and the system noise driving matrix.

[0117] Equations (45) to (49) represent the filtering steps of the robust Kalman filter algorithm (Huber-IEKF) for integrated navigation in this application. First, a one-step prediction of the state is calculated based on the state transition matrix and the system noise driving matrix. The one-step prediction error covariance matrix P of the state k / k-1 Then adjust the filter gain K k The calculation is performed based on ψ. x and ψ y Real-time one-step prediction error covariance matrix P k / k-1 and the measurement noise covariance matrix R k Dynamically adjust the filter gain K kAfter the calculation, state estimation and the calculation of the mean square error matrix of state estimation are further performed to obtain the calculation results of the state estimation value and the error covariance matrix P of state estimation. k

[0118] Step 6.5: Perform the measurement update process of the integrated navigation robust Kalman filtering algorithm to calculate the filtering gain K k In the process of calculating the filtering gain, the measurement residual weight matrix ψ y and the state prediction residual weight matrix ψ x are dynamically adjusted in real time according to the one-step prediction error covariance matrix P k / k-1 of the state and the measurement noise covariance matrix R k .

[0119] Step 6.6: After the calculation of the filtering gain K k , further perform the state estimation and the calculation of the mean square error matrix of state estimation to obtain the calculation results of the state estimation value and the error covariance matrix P of state estimation. k

[0120] Step 7: Perform feedback update on the navigation parameters according to the state estimation value, and perform navigation based on the updated navigation parameters.

[0121] In actual applications, feedback correction is also a very important means, which can effectively improve the accuracy and stability of navigation solution. After the measurement update of the integrated navigation robust Kalman filtering algorithm (Huber-IEKF) each time, the navigation parameters are updated according to the obtained state estimation value . The specific update formula is as follows:

[0122] ​​In actual navigation, the AUV needs to obtain its own attitude, speed and position information in real time, compare it with the expected attitude, speed and position information, and then perform the next motion control. High-precision navigation parameter information helps the stable control system. In the initial stage of the navigation system startup, the initial alignment of the SINS is performed to obtain the initial attitude information of itself, and the initial speed and initial position information are obtained by using other navigation sensors. When the navigation system enters the formal navigation working state, the SINS first performs pure inertial navigation calculation according to the differential equations of attitude, speed and position. Since the pure inertial calculation alone will have cumulative errors, an error state space model (44) is constructed, and a combined navigation robust Kalman filtering algorithm (45)-(49) is used to estimate the error state. After obtaining the state estimation result, the SINS is corrected by feedback using formula (58) to eliminate the cumulative error, thereby improving the positioning and orientation accuracy of the inertial-based integrated navigation system and the robustness of dealing with non-Gaussian noise, and providing necessary conditions for the subsequent safe work of the AUV underwater.

[0123] In the simulation test of the present application, the Huber-IEKF algorithm is used to filter and estimate the SINS / USBL inertial-based integrated navigation system. The application of the Huber-IEKF algorithm in the SINS / USBL inertial-based integrated navigation system under non-Gaussian noise conditions is simulated and verified. The USBL measurement error is set to 0.1% slant range, the data output frequency is 1HZ, and the integrated navigation simulation parameter settings are shown in Table 1.

[0124] Table 1 SINS / USBL inertial-based integrated navigation simulation parameters

[0125] Figure 2 shows the underwater search motion trajectory of an AUV. First, it dives to a specified depth, then performs continuous reciprocating motion in a certain area, the motion trajectory covers a certain specific area, and then it floats to the end point. According to the characteristics of the measurement outliers, the measurement noise of the ultra-short baseline positioning system is set as mixed Gaussian noise. In the case of 10% outlier occurrence probability, the simulation verification comparison is performed. The running trajectory of the underwater vehicle is shown in Figure 2, and the total running time of the underwater vehicle is 3800s. At the same time, the integrated navigation test of the robust Kalman filtering algorithm (Huber-IEKF algorithm) and the conventional Kalman filtering algorithm (KF algorithm) is performed. The error comparison test results of the two algorithms are shown in Figures 3, 4 and 5.

[0126] As can be seen from the navigation parameter error comparison curves in Figures 3 to 5, the robust Kalman filter algorithm (Huber-IEKF algorithm) of this application has higher filtering accuracy than the conventional Kalman filter algorithm (KF algorithm). This is because the Huber-IEKF algorithm dynamically adjusts the filter gain and the mean square error matrix of the state estimation with each measurement update, thereby improving the robustness of the filter. Moreover, the system matrix of the Huber-IEKF algorithm based on Lie group theory has a lower dependence on the system state variables, thus resulting in higher filtering accuracy. The RMS statistics of navigation parameter errors calculated by the SINS / USBL inertial basis integrated navigation system under the two filtering algorithms are shown in Table 2.

[0127] Table 2. RMS statistics of attitude, velocity, and position errors for two filtering algorithms.

[0128] Parameters in Table 2 δν represents the attitude error in the X, Y, and Z directions, respectively; x ,δν y ,δν z These represent the velocity errors in the X, Y, and Z directions, respectively; δp x δp y δp z The values ​​represent the position errors in the X, Y, and Z directions, respectively. Table 2 shows the RMS statistics of the attitude, velocity, and position errors for the two filtering algorithms. It can be seen that the RMS values ​​for the attitude error of the SINS / USBL inertial basis integrated navigation system based on the conventional Kalman filter algorithm are (0.0079°, 0.0033°, 0.2987°), the RMS values ​​for the three-dimensional velocity error are (0.0739 m / s, 0.0809 m / s, 0.0556 m / s), and the RMS values ​​for the three-dimensional position error are (12.7894 m, 4.6792 m, 3.7884 m). The inertial basis integrated navigation attitude error RMS values ​​of the Hur-IEKF algorithm in this application are (0.0017°, 0.0011°, 0.1081°), the three-dimensional velocity error RMS values ​​are (0.0190m / s, 0.0145m / s, 0.0093m / s), and the three-dimensional position error RMS values ​​are (3.3941m, 1.8189m, 0.5198m). It is evident that the RMS values ​​of all navigation errors of the Hur-IEKF algorithm in this application are lower than the filtered estimates of the conventional KF algorithm, indicating that the Hur-IEKF algorithm in this application has higher stability and robustness. This application can effectively address the impact of non-Gaussian noise on underwater integrated navigation systems, improving the filtering estimation accuracy and robustness of underwater navigation systems, especially when noise pollution is severe.

[0129] In some embodiments, the present application also provides a computer program product comprising a computer program which, when executed by a processor, implements the inertial-based integrated navigation method.

[0130] In some embodiments, the present application also provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the inertial-based integrated navigation method.

[0131] In some embodiments, the present application also provides a computer device comprising a processor, a memory, an input / output interface (I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device comprises a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for running the operating system and the computer program in the non-volatile storage medium. The database of the computer device is configured to store to-be-processed transactions. The input / output interface of the computer device is configured to exchange information between the processor and external devices. The communication interface of the computer device is configured to communicate with external terminals through network connection. The computer program, when executed by the processor, can implement the inertial-based integrated navigation method.

[0132] In some embodiments, the present application also provides an underwater autonomous underwater vehicle comprising an inertial-based integrated navigation system configured to implement the inertial-based integrated navigation method.

[0133] The principles and implementation modes of the present application are described herein by using specific examples, and the above examples are only used to help understand the method of the present application and its core idea; meanwhile, for those skilled in the art, according to the idea of the present application, the specific implementation modes and application ranges will be changed. In conclusion, the content of the present description should not be understood as a limitation of the present application.

Claims

1. An inertial-based integrated navigation method, characterized by, The application relates to an inertial-based integrated navigation system and a navigation method thereof. In an initial stage of starting the inertial-based integrated navigation system, a strapdown inertial navigation system (SINS) utilizes an ultra-short baseline positioning system (USBL) to assist initial alignment, so as to obtain initial attitude information of an autonomous underwater vehicle (AUV); When the inertial-based integrated navigation system enters a formal navigation working state, the SINS performs pure inertial navigation calculation based on the initial attitude information, so as to obtain navigation parameter calculation values containing errors; the navigation parameters include the attitude, the speed and the position of the AUV; A navigation state error model based on a Lie group is constructed according to the navigation parameter calculation values containing errors; A generalized maximum likelihood estimation model is constructed based on a Huber function; According to the navigation state error model based on the Lie group and the generalized maximum likelihood estimation model, a robust Kalman filtering algorithm for integrated navigation is established; The inertial-based integrated navigation system is filtered and estimated based on the robust Kalman filtering algorithm for integrated navigation, so as to obtain state estimation values; The navigation parameters are updated based on the state estimation values, and navigation is performed based on the updated navigation parameters.

2. The inertial-based integrated navigation method of claim 1, wherein, The SINS performs pure inertial navigation calculation based on the initial attitude information, so as to obtain the navigation parameter calculation values containing errors, and the specific process includes the following steps: Under the condition that the initial attitude information is obtained through initial alignment and initial position and initial speed information are obtained through other navigation sensors, the SINS establishes a navigation differential equation in a geographic coordinate system by means of angular velocity information output by a gyroscope and acceleration information output by an accelerometer; the navigation differential equation includes an attitude matrix differential equation, a speed differential equation and a position differential equation; The navigation differential equation of the SINS is solved, so as to obtain the attitude, the speed and the position calculation values containing errors.

3. The inertial-based integrated navigation method of claim 1, wherein, The navigation state error model based on the Lie group is constructed according to the navigation parameter calculation values containing errors, and the specific process includes the following steps: A Lie group state variable matrix of the SINS is constructed based on the attitude and the speed of the AUV; A Lie group state error model of the SINS is constructed according to a right invariant error definition of the Lie group theory based on the navigation parameter calculation values containing errors; The Lie group state error model is differentiated, so as to construct a differential equation of the Lie group state error model; The navigation state error model based on the Lie group is obtained by deducing the differential equation of the Lie group state error model.

4. The inertial-based integrated navigation method of claim 1, wherein, The generalized maximum likelihood estimation model is constructed based on the Huber function, and the specific process includes the following steps: The expression of the robust function ρ in Huber function is selected where ζ i represents the ith residual; γ is a key parameter; Constructing a cost function for a generalized maximum likelihood estimation model according to the expression for the robust function p Wherein m represents the dimension of a residual error; and x represents a state variable. By seeking ζ that maximizes J(x) i The state variable estimate is obtained.

5. The inertial-based integrated navigation method of claim 1, wherein, The robust Kalman filtering algorithm for integrated navigation is established according to the navigation state error model based on the Lie group and the generalized maximum likelihood estimation model, and the specific process includes the following steps: A measurement model is obtained according to the navigation state error model based on the Lie group; An error state space model of a linear discrete system is given according to the measurement model and the navigation state error model based on the Lie group; The robust Kalman filtering algorithm for integrated navigation is established based on the error state space model and the generalized maximum likelihood estimation model; the robust Kalman filtering algorithm for integrated navigation includes a time updating process and a measurement updating process; the time updating process includes one-step prediction of a state and one-step prediction of a mean square error matrix estimation of the state; the measurement updating process includes calculation of a filtering gain, state estimation and calculation of a mean square error matrix of the state estimation.

6. The inertial-based integrated navigation method of claim 5, wherein, The combined navigation robust Kalman filtering algorithm is used for filtering estimation of the inertial-based combined navigation system to obtain a state estimation value, and specifically includes the following steps. A combined system model is defined according to a measurement model and a navigation state error model based on a Lie group; A decoupling matrix is defined, and a decoupled measurement value, a measurement matrix and a residual error are obtained based on the combined system model; A measurement residual error weighted matrix and a state prediction residual error weighted matrix are calculated according to the decoupled measurement value, the measurement matrix, the residual error and a generalized maximum likelihood estimation model; A time update process of the combined navigation robust Kalman filtering algorithm is performed to calculate one-step prediction of a state and one-step prediction error covariance matrix of the state according to a state transition matrix and a system noise driving matrix; A measurement update process of the combined navigation robust Kalman filtering algorithm is performed to calculate a filtering gain, and the one-step prediction error covariance matrix of the state and a measurement noise covariance matrix are dynamically adjusted in real time during the calculation of the filtering gain according to the measurement residual error weighted matrix and the state prediction residual error weighted matrix; After the filtering gain is calculated, state estimation and calculation of a mean square error matrix of the state estimation are further performed to obtain a state estimation value and a calculation result of an error covariance matrix of the state estimation.

7. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to implement the inertial-based combined navigation method of any one of claims 1-6.

8. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the inertial-based combined navigation method of any one of claims 1-6.

9. A computer device comprising: The memory, the processor and the computer program stored on the memory and executable on the processor are characterized in that the processor executes the computer program to implement the inertial-based combined navigation method of any one of claims 1-6.

10. An underwater autonomous underwater vehicle comprising: The inertial-based combined navigation system is characterized in that the inertial-based combined navigation system is used to implement the inertial-based combined navigation method of any one of claims 1-6.

Images