INS / DVL Tightly Coupled Integrated Navigation Method, Device and System for Underwater Vehicle

Abstract

The present invention relates to the field of underwater vehicle inertial navigation technology, and specifically discloses an INS / DVL tightly coupled integrated navigation method, device, and system for underwater vehicles, comprising: constructing an INS / DVL tightly coupled integrated navigation model for the underwater robot; constructing a Mahalanobis distance based on the error covariance of the integrated model; determining a navigation state filter for the integrated model based on a comparison result of the Mahalanobis distance with a preset complex environment error determination threshold; executing a robust state filter based on a local consistency metric if the environment is complex; executing a basic nonlinear filter if the environment is non-complex; and determining the navigation parameters of the underwater robot based on the output of the filtered state. The INS / DVL tightly coupled integrated navigation method for underwater vehicles provided by the present invention can solve the problem of mismatch between noise parameters and actual conditions caused by environmental changes while avoiding parameter overcompensation in low-complexity environments and reducing computing power waste.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle inertial navigation, and in particular to an INS / DVL tightly coupled integrated navigation method for an underwater vehicle, an INS / DVL tightly coupled integrated navigation device for an underwater vehicle, and an INS / DVL tightly coupled integrated navigation system for an underwater vehicle. Background Art

[0002] Underwater vehicles, as core equipment for ocean exploration and development missions, possess significant application value. Deployment, operation, and recovery within vast underwater spaces require accurate position estimates, necessitating advanced navigation technologies. Electromagnetic waves are severely attenuated in underwater environments, rendering traditional satellite navigation systems unsuitable for underwater navigation. Inertial navigation systems (INS), with their advantages of high autonomy, robust anti-interference capabilities, rapid update rates, and comprehensive navigation information, have become essential underwater navigation equipment. However, their errors increase rapidly over time. To obtain high-precision navigation information over a long period of time, external measurement information is required to suppress error propagation. Acoustic Doppler Velocity Logs (DVLs) have become a mainstream choice for underwater acoustic measurement equipment due to their compact size, ease of installation, and ability to accurately measure vehicle velocity.

[0003] The INS / DVL loose integration navigation algorithm uses the difference between the inertial navigation velocity information and the DVL velocity information in the vehicle coordinate system as the observed quantity. The inertial navigation system error, the DVL scale factor error, and the installation angle error are selected as state variables. Kalman filtering is used to achieve real-time error estimation and feedback correction, ultimately yielding high-precision integrated navigation information. The key difference from loose integration lies in that, in tight integration, the raw DVL data is no longer subjected to coordinate transformation but is directly input into the navigation state filter. The difference between the SINS corresponding value and the DVL beam measurement is directly compared within the navigation state filter, and the comparison information is then independently integrated into the navigation state filter. Compared to loose integration, tight integration can better utilize four-dimensional frequency shift information, resulting in higher integrated navigation accuracy. In integrated navigation systems, noise parameters are typically set based on a priori knowledge to achieve optimal estimation. In low-complexity environments, these preset noise parameter distributions closely match the actual noise characteristics of the sensor data. However, as environmental complexity increases, the preset noise parameters can deviate significantly from actual operating conditions, potentially generating outliers that seriously threaten the reliability of the navigation system. For underwater vehicles, this impact is particularly significant. For example, complex underwater environmental factors such as ocean current disturbances and terrain changes can significantly change the sensor output characteristics, thereby adversely affecting the performance of the INS / DVL integrated navigation system.

[0004] The current method for solving the problem of mismatch between INS / DVL integrated navigation noise parameters and actual conditions is mainly the Sage-Husa adaptive filtering algorithm. Specifically, a measurement fault detection and adjustment mechanism is introduced to solve the problem that the performance of adaptive filtering in estimating measurement noise parameters is highly dependent on the fading factor. When there is an anomaly in the observed quantity, the measurement matrix is fault detected and adjusted online to suppress errors. From a formal point of view, the Sage-Husa adaptive filter can adaptively estimate the mean and variance of the system noise and the mean and variance of the measurement noise. However, for high-dimensional systems, especially for states with relatively weak observability, the estimation effect of the system noise variance is no longer ideal, and in the case of low complexity, this method will have problems of computational waste and parameter over-compensation, which in turn affects performance.

[0005] Therefore, how to solve the mismatch between noise parameters and actual conditions caused by environmental changes while avoiding over-compensation of parameters in low-complexity environments and reducing computing power waste has become a technical problem that needs to be urgently solved by technicians in this field. Summary of the Invention

[0006] The present invention provides an INS / DVL tightly coupled integrated navigation method for an underwater vehicle, an INS / DVL tightly coupled integrated navigation device for an underwater vehicle, and an INS / DVL tightly coupled integrated navigation system for an underwater vehicle, which solve the problems of parameter over-compensation and computing power waste that occur when the problem of mismatch between INS / DVL integrated navigation noise parameters and actual conditions cannot be avoided in the related art.

[0007] As a first aspect of the present invention, a method for tightly coupled INS / DVL integrated navigation of an underwater vehicle is provided, comprising:

[0008] Constructing an INS / DVL tightly coupled combination model of an underwater robot, wherein the INS / DVL tightly coupled combination model includes an inertial navigation system model and a Doppler velocity measurement system model;

[0009] Constructing the Mahalanobis distance based on the error covariance of the INS / DVL tightly coupled combination model;

[0010] determining a navigation state filter of the INS / DVL tightly coupled combination model based on a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution;

[0011] If the comparison result shows that the type of environment in which the underwater robot is located is a complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combination model is a robust state filter based on local consistency measurement, and executing the robust state filter based on local consistency measurement to obtain a filter state output result, wherein the robust state filter based on local consistency measurement can decompose the noise covariance matrix and can independently adjust the noise component;

[0012] If the environment type in which the underwater robot is located is a non-complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combination model is a basic nonlinear filter, and obtaining a filtering state output result according to the basic nonlinear filter;

[0013] The navigation parameters of the underwater robot are determined according to the output result of the filtering state, and the navigation parameters of the underwater robot include posture, position and speed.

[0014] Furthermore, determining the navigation state filter of the INS / DVL tightly coupled combination model according to a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold comprises:

[0015] Generate complex environment error judgment threshold based on generalized Pareto distribution;

[0016] Comparing the Mahalanobis distance with the complex environment error determination threshold generated by the generalized Pareto distribution;

[0017] If the Mahalanobis distance is greater than or equal to the complex environment error determination threshold generated by the generalized Pareto distribution, it is determined that the type of environment in which the underwater robot is located is a complex environment;

[0018] If the Mahalanobis distance is less than the complex environment error determination threshold of the generalized Pareto distribution, it is determined that the environment type in which the underwater robot is located is a non-complex environment.

[0019] Furthermore, a complex environment error judgment threshold is generated based on the generalized Pareto distribution, including:

[0020] The Mahalanobis distance tail data is fitted according to the generalized Pareto distribution, wherein the cumulative distribution function of the generalized Pareto distribution is expressed as:

[0021] ,

[0022] in, represents the shape parameter, represents the scale parameter, represents the location parameter, and x represents the observed variable;

[0023] The complex environment error determination threshold is determined based on the fitting result and in combination with the preset confidence interval. The expression of the complex environment error determination threshold is:

[0024] ,

[0025] in, Indicates the quantile corresponding to the preset confidence interval. When using the limit form .

[0026] Furthermore, executing the robust state filter based on local consistency measurement to obtain a filtered state output result includes:

[0027] Constructing a robust state filter based on a local consistency metric, wherein the robust state filter based on the local consistency metric is capable of evaluating the local consistency between the observed data and the predicted state;

[0028] Dynamically adjusting the update intensity of the observation noise covariance submatrix according to the evaluation result of the local consistency;

[0029] Obtaining an updated noise covariance matrix according to the updated observation noise covariance submatrix;

[0030] The state estimation is measured and updated according to the updated noise covariance matrix to obtain the filtering state output result.

[0031] Furthermore, a robust state filter based on local consistency measurement is constructed, including:

[0032] Determine the nonlinear expression of the robust state filter based on local consistency measure:

[0033] ,

[0034] Determine a time update model of the robust state filter based on local consistency measure, wherein the time update model includes a predicted value of a state estimation vector and a predicted value of a state estimation covariance matrix, wherein the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix Respectively expressed as:

[0035] ,

[0036] ,

[0037] in, represents the propagation volume point from time k-1 to time k, represents the noise matrix;

[0038] A noise covariance matrix is decomposed to determine a measurement update model of the robust state filter based on local consistency measure.

[0039] Furthermore, the noise covariance matrix is decomposed to determine a measurement update model of the robust state filter based on local consistency measure, including:

[0040] The noise covariance matrix is decomposed to obtain the noise characteristics of multiple observation components, wherein the decomposition expression of the noise covariance matrix is:

[0041] ,

[0042] in, , express The j-th row vector of corresponds to the noise characteristics of the j-th observation component, represents the observation noise dimension;

[0043] The observation noise covariance submatrix is constructed according to the noise characteristics of each observation component, wherein the expression of the observation noise covariance submatrix is:

[0044] ,

[0045] Among them, each Independently describe the noise variance characteristics of an observation component;

[0046] The local consistency measure is calculated for each observation component, where the local consistency measure of each observation component is The expression is:

[0047] ,

[0048] in, represents the actual observation value of the j-th observation component, represents the predicted value of the j-th observation component, The innovation covariance matrix of the j-th observation component;

[0049] Comparing the local consistency measure of each observation component with a preset adjustment threshold;

[0050] If the local consistency measure of the observation component is greater than the preset adjustment threshold, the observation component is determined to be inconsistent with the predicted state, and the observation noise covariance submatrix is adjusted. The expression of the adjusted observation noise covariance submatrix is:

[0051] ,

[0052] in, Represents the scaling factor, which is used to adjust the scaling rate;

[0053] If the local consistency measure of the observation component is not greater than the preset adjustment threshold, the observation component is determined to be consistent with the predicted state, and the noise covariance matrix is kept unchanged;

[0054] The inverse decomposition process is performed according to the updated observation noise covariance submatrix to obtain an updated noise covariance matrix, wherein the expression of the updated noise covariance matrix is:

[0055] ;

[0056] The measurement update model is obtained according to the updated covariance matrix.

[0057] Furthermore, constructing the Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled combination model includes:

[0058] Determining that the state filter in the INS / DVL tightly coupled combination model is a cubature Kalman filter;

[0059] Obtaining volume points with the same weight according to a spherical radial criterion, and obtaining a predicted state vector and an error covariance matrix to implement a time update of the volume Kalman filter;

[0060] Updating the predicted state vector according to the received external measurement value to achieve measurement update of the cubature Kalman filter;

[0061] According to the innovation in the measurement update and the innovation covariance matrix, the Mahalanobis distance between the actual observation value and the predicted value is constructed. The expression of the Mahalanobis distance is:

[0062] ,

[0063] in, represents the Mahalanobis distance, Indicates new information, represents the innovation covariance matrix.

[0064] Furthermore, the basic nonlinear filter includes a cubic Kalman filter.

[0065] As another aspect of the present invention, an INS / DVL tightly coupled integrated navigation device for an underwater vehicle is provided, for implementing the INS / DVL tightly coupled integrated navigation method for an underwater vehicle as described above, comprising:

[0066] A combined model construction module is used to construct an INS / DVL tightly coupled combined model of the underwater robot, wherein the INS / DVL tightly coupled combined model includes an inertial navigation system model and a Doppler velocity measurement system model;

[0067] A Mahalanobis distance construction module, configured to construct a Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled combination model;

[0068] a navigation state filter determination module, configured to determine the navigation state filter of the INS / DVL tightly coupled combination model based on a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution;

[0069] A local consistency filtering module is configured to, if the comparison result indicates that the type of environment in which the underwater robot is located is a complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combination model is a robust state filter based on local consistency measurement, and execute the robust state filter based on local consistency measurement to obtain a filtering state output result, wherein the robust state filter based on local consistency measurement is capable of decomposing a noise covariance matrix and independently adjusting noise components;

[0070] a basic filtering module, configured to determine, if the environment in which the underwater robot is located is a non-complex environment, that the navigation state filter of the INS / DVL tightly coupled combination model is a basic nonlinear filter, and obtain a filtering state output result according to the basic nonlinear filter;

[0071] The navigation parameter determination module is used to determine the navigation parameters of the underwater robot according to the output result of the filtering state, and the navigation parameters of the underwater robot include posture, position and speed.

[0072] As another embodiment of the present invention, an INS / DVL tightly coupled integrated navigation system for an underwater vehicle is provided, comprising: an inertial navigation system, a Doppler velocity measurement system, and the aforementioned INS / DVL tightly coupled integrated navigation device for the underwater vehicle, wherein the inertial navigation system and the Doppler velocity measurement system are both communicatively connected to the INS / DVL tightly coupled integrated navigation device for the underwater vehicle.

[0073] The INS / DVL tightly coupled integrated navigation method for an underwater vehicle provided by the present invention constructs a Mahalanobis distance based on error covariance, and according to a generalized Pareto distribution, a complex environment judgment threshold can be obtained. Real-time judgment of the environment is achieved by comparing the Mahalanobis distance with a preset complex environment judgment threshold. When the Mahalanobis distance exceeds the threshold, the underwater vehicle is considered to be in a highly complex environment. A multi-adaptive robust state estimator based on local consistency measurement is used to finely adjust the noise covariance corresponding to the outlier to avoid misadjustment of the normal component. When the Mahalanobis distance is less than the threshold, a basic nonlinear filter is executed. Therefore, the INS / DVL tightly coupled integrated navigation method for an underwater vehicle of the present invention can avoid overcompensation of parameters in low-complexity situations, reduce waste of computing power, and significantly solve the problem of accuracy degradation caused by different environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. Together with the following specific embodiments, they are used to explain the present invention, but do not constitute a limitation of the present invention.

[0075] Figure 1 This is a flow chart of the INS / DVL tightly coupled integrated navigation method for an underwater vehicle provided by the present invention.

[0076] Figure 2 This is the architecture diagram of the INS / DVL tightly coupled combination model provided by the present invention.

[0077] Figure 3 This is a flow chart of the method for constructing Mahalanobis distance provided by the present invention.

[0078] Figure 4 Flowchart of the method for determining the navigation state filter of the INS / DVL tightly coupled combination model provided by the present invention.

[0079] Figure 5 A flow chart of a method for executing a robust state filter based on local consistency measurement provided by the present invention.

[0080] Figure 6 This is a flowchart of the actual specific working process of the INS / DVL tightly coupled integrated navigation method for underwater vehicles provided by the present invention.

[0081] Figure 7 This is a structural block diagram of the INS / DVL tightly coupled integrated navigation device for underwater vehicles provided by the present invention.

[0082] Figure 8 This is a structural block diagram of the INS / DVL tightly coupled integrated navigation system for underwater vehicles provided by the present invention. DETAILED DESCRIPTION

[0083] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0084] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0085] It should be noted that the terms "first," "second," and the like in the specification and claims of the present invention and the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate for the embodiments of the present invention described herein. In addition, the terms "including," "having," and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to these processes, methods, products, or apparatuses.

[0086] In this embodiment, an INS / DVL tightly coupled integrated navigation method for an underwater vehicle is provided. Figure 1 FIG. 1 is a flow chart of an INS / DVL tightly coupled integrated navigation method for an underwater vehicle according to an embodiment of the present invention. Figure 1 Shown, including:

[0087] S100, constructing an INS / DVL tightly coupled combination model of the underwater robot, wherein the INS / DVL tightly coupled combination model includes an inertial navigation system model and a Doppler velocity measurement system model;

[0088] In an embodiment of the present invention, an INS / DVL tightly coupled combination model of an underwater robot is constructed based on an inertial navigation system model and a Doppler velocity measurement system model.

[0089] S200, constructing a Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled combination model;

[0090] It should be understood that the state filter of the INS / DVL tightly coupled combination model may specifically be a cubature Kalman filter, and the error covariance may be obtained according to the time update and measurement update of the filter, and the Mahalanobis distance may be constructed according to the error covariance.

[0091] S300, determining a navigation state filter of the INS / DVL tightly coupled combination model based on a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution;

[0092] In an embodiment of the present invention, a preset complex environment error determination threshold is determined based on the generalized Pareto distribution, and then the Mahalanobis distance can be compared with the preset complex environment error determination threshold to determine the type of the navigation state filter.

[0093] It should be understood that since the determination of the preset complex environment error judgment threshold can determine the type of navigation state filter, the embodiment of the present invention can accurately determine the preset complex environment error judgment threshold through the generalized Pareto distribution, thereby achieving accurate judgment of the Mahalanobis distance and further achieving accurate determination of the navigation state filter.

[0094] S400. If the comparison result shows that the type of environment in which the underwater robot is located is a complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combination model is a robust state filter based on local consistency measurement, and executing the robust state filter based on local consistency measurement to obtain a filter state output result, wherein the robust state filter based on local consistency measurement can decompose the noise covariance matrix and can independently adjust the noise component;

[0095] It should be understood that, based on the comparison result of the above-mentioned Mahalanobis distance and the preset complex environment error judgment threshold, when the comparison result is a complex environment, it is determined that the navigation state filter adopts a robust state filter based on local consistency measurement. The robust state filter based on local consistency measurement can decompose the noise covariance matrix and independently adjust the noise component, thereby effectively reducing the waste of computing power.

[0096] S500: If the environment type of the underwater robot is a non-complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combination model is a basic nonlinear filter, and obtain a filtering state output result according to the basic nonlinear filter;

[0097] In an embodiment of the present invention, when it is determined that the environment type in which the underwater robot is located is a non-complex environment, a basic nonlinear filter can be used to implement it, and over-compensation of parameters in a non-complex environment can also be avoided.

[0098] S600: Determine navigation parameters of the underwater robot according to the filtering state output result, where the navigation parameters of the underwater robot include posture, position, and speed.

[0099] The INS / DVL tightly coupled integrated navigation method for underwater vehicles in an embodiment of the present invention determines a preset complexity environment determination threshold through Pareto distribution. This method can accurately determine the environment in which the underwater vehicle is located, avoiding the phenomenon of parameter overcompensation that occurs when navigation filters originally suitable for high-complexity environments are applied to low-complexity environments in the existing technology. At the same time, it can also fine-tune noise parameters in high-complexity environments, avoiding waste of computing power.

[0100] In summary, the INS / DVL tightly coupled integrated navigation method for an underwater vehicle in an embodiment of the present invention constructs the Mahalanobis distance based on the error covariance, and according to the generalized Pareto distribution, a complex environment judgment threshold can be obtained. Real-time judgment of the environment is achieved by comparing the Mahalanobis distance with the preset complex environment judgment threshold. When the Mahalanobis distance exceeds the threshold, it is considered that the underwater vehicle is in a highly complex environment. A multi-adaptive robust state estimator based on local consistency measurement is used to finely adjust the noise covariance corresponding to the outlier to avoid misadjustment of the normal component. When the Mahalanobis distance is less than the threshold, the basic nonlinear filter is executed. Therefore, the INS / DVL tightly coupled integrated navigation method for an underwater vehicle of the present invention can avoid over-compensation of parameters in low-complexity situations, reduce waste of computing power, and significantly solve the problem of accuracy degradation caused by different environments.

[0101] In the embodiment of the present invention, combined with Figure 2 The architecture diagram of the INS / DVL tightly coupled combination model is shown in the figure. The specific process of building the INS / DVL tightly coupled combination model of the underwater robot includes:

[0102] First, the 15-dimensional inertial navigation system error is selected as the error state quantity of the INS subsystem:

[0103] ,

[0104] in, Represents the velocity errors in the east, north and sky directions respectively; Represents the attitude errors in the east, north and sky directions respectively; Respectively represent the latitude error, longitude error and altitude error of inertial navigation; Respectively represent the zero bias errors of the three axes of the INS accelerometer in the carrier coordinate system; They represent the zero drift errors of the three axes of the INS gyroscope in the carrier coordinate system.

[0105] The state error equation of the INS subsystem is as follows:

[0106] ,

[0107] in, represents zero-mean Gaussian white noise; Represents the state transfer matrix of the inertial navigation system. According to the characteristics of the INS navigation system itself, its expression is:

[0108] ,

[0109] in:

[0110] ,

[0111] ,

[0112] ,

[0113] ,

[0114] ,

[0115] ,

[0116] ,

[0117] ,

[0118] in, represents the angular velocity of the Earth's rotation relative to the inertial coordinate system, and They respectively represent the meridian radius and the meridian radius of the carrier's location.

[0119] The error of DVL is strongly correlated with its proportional factor and installation angle error, so the installation angle error and proportional factor error of DVL are selected as the error state quantity of DVL subsystem:

[0120] ,

[0121] in, Indicates the installation error angle between DVL and INS in the three coordinate axes; Represents the scale factor error.

[0122] Since the installation error between DVL and INS is constant, the proportional factor can be approximated as a constant in the same water area. The state error equation of the DVL subsystem is established as:

[0123] ,

[0124] in, represents the system matrix of the DVL subsystem, represents DVL-correlated noise.

[0125] The INS subsystem is combined with the DVL subsystem to construct the 19-dimensional state variables of the INS / DVL integrated navigation system:

[0126] ,

[0127] The state equation of the INS / DVL integrated navigation system is:

[0128] .

[0129] In the embodiment of the present invention, the Doppler velocimeter includes single-beam and multi-beam. The present invention is directed to a four-beam Janus cross-type Doppler sensor. The column vector formed by the Doppler frequency shift in the four beam directions of the DVL transducer can be expressed as:

[0130] ,

[0131] Considering the DVL scale factor error and the four-dimensional frequency shift random measurement error, the actual output frequency shift information of the DVL can be expressed as:

[0132] .

[0133] According to the actual output of INS in the navigation coordinate system As shown, the calculated four-dimensional frequency shift information is as follows:

[0134] ,

[0135] in, , represents the beam depression angles of the four beams; Represents the coordinate transfer matrix from the navigation system to the carrier system; Represents the coordinate transfer matrix from the carrier system to the Doppler system.

[0136] The difference between the frequency shift calculated by INS and the actual frequency shift of DVL is used as the measurement value:

[0137] ,

[0138] The measurement matrix of INS / DVL tight coupling is:

[0139] ,

[0140] Constructing the measurement model of INS / DVL tightly coupled integrated navigation system:

[0141] ,

[0142] in, represents the four-dimensional frequency-shifted measurement noise of DVL.

[0143] In an embodiment of the present invention, the Mahalanobis distance is constructed according to the error covariance of the INS / DVL tightly coupled combination model, such as Figure 3 Shown, including:

[0144] S210, determining that the state filter in the INS / DVL tightly coupled combination model is a cubature Kalman filter;

[0145] This embodiment of the present invention employs a cubature Kalman filter (CKF). In the time update phase, the spherical radial criterion is used to obtain volumetric points with identical weights, which are then used to obtain the predicted state vector and error covariance matrix. In the measurement update phase, the measured values are used to update the predicted state, improving its estimated error.

[0146] First, the nonlinear expression of the INS / DVL tightly coupled model is established:

[0147] .

[0148] S220, obtaining volume points with the same weights according to a spherical radial criterion, and obtaining a predicted state vector and an error covariance matrix to implement a time update of the volumetric Kalman filter;

[0149] The predicted value of the state estimation vector in CKF and the predicted value of the state estimation covariance matrix It can be expressed as:

[0150] ,

[0151] ,

[0152] in, represents the propagation volume point from time k-1 to time k, represents the system noise matrix.

[0153] S230, updating the predicted state vector according to the received external measurement value to implement measurement update of the cubic Kalman filter;

[0154] After receiving external measurement information, the system performs measurement update. At this time, the predicted value of the state observation vector is and the innovation covariance matrix It can be expressed as:

[0155] ,

[0156] ,

[0157] in, represents the propagation volume point, represents the measurement noise matrix.

[0158] Structural measurement innovation :

[0159] .

[0160] S240: Construct a Mahalanobis distance between the actual observation value and the predicted value based on the innovation in the measurement update and the innovation covariance matrix. The expression of the Mahalanobis distance is:

[0161] ,

[0162] in, represents the Mahalanobis distance, Indicates new information, represents the innovation covariance matrix.

[0163] It should be understood that according to the measured new information and the innovation covariance matrix , we can get the Mahalanobis distance between the actual observation value and the predicted value .

[0164] It should be noted that after each measurement update, the corresponding epoch is calculated. .

[0165] In an embodiment of the present invention, the navigation state filter of the INS / DVL tightly coupled combination model is determined according to the comparison result of the Mahalanobis distance and the preset complex environment error judgment threshold, such as Figure 4 Shown, including:

[0166] S310, generating a complex environment error determination threshold according to a generalized Pareto distribution;

[0167] In the embodiment of the present invention, the Mahalanobis distance sequence is constructed using the Mahalanobis distance of each epoch. , where N represents the total number of measurement epochs. Sort in ascending order and select the tail m maximum Mahalanobis distance values, where , construct extreme sample values .

[0168] Specifically, the error judgment threshold for complex environments is generated based on the generalized Pareto distribution, including:

[0169] The Mahalanobis distance tail data is fitted according to the generalized Pareto distribution, wherein the cumulative distribution function of the generalized Pareto distribution is expressed as:

[0170] ,

[0171] in, represents the shape parameter, represents the scale parameter, represents the location parameter, and x represents the observed variable;

[0172] The complex environment error determination threshold is determined based on the fitting result and in combination with the preset confidence interval. The expression of the complex environment error determination threshold is:

[0173] ,

[0174] in, Indicates the quantile corresponding to the preset confidence interval. When using the limit form

[0175] In the embodiment of the present invention, the preset confidence interval can be specifically set to 90%.

[0176] S320, comparing the Mahalanobis distance with the complex environment error determination threshold generated by the generalized Pareto distribution;

[0177] S330: If the Mahalanobis distance is greater than or equal to the complex environment error determination threshold generated by the generalized Pareto distribution, determine that the type of environment in which the underwater robot is located is a complex environment;

[0178] Specifically, when It is believed that we are currently in a highly complex underwater environment, and the Doppler velocity measurement beam is subject to the risk of high noise and frequent gross errors. At this time, the deviation between the predicted measurement value and the actual measurement value is large. The system adopts a multi-adaptive robust state estimator based on local consistency measurement.

[0179] S340: If the Mahalanobis distance is less than the complex environment error determination threshold of the generalized Pareto distribution, determine that the environment type in which the underwater robot is located is a non-complex environment.

[0180] Specifically, if At this time, the deviation between the predicted measurement value and the actual measurement value is small, so it is considered that the current environment is underwater with low complexity, and the system adopts CKF.

[0181] In an embodiment of the present invention, the robust state filter based on local consistency measurement is executed to obtain a filtering state output result, such as Figure 5 Shown, including:

[0182] S410: Constructing a robust state filter based on local consistency measurement, wherein the robust state filter based on local consistency measurement can evaluate the local consistency between the observed data and the predicted state;

[0183] In an embodiment of the present invention, a robust state filter based on local consistency measurement is constructed, and the time update model can specifically refer to the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix of the INS / DVL tightly coupled model mentioned above.

[0184] Specifically, a robust state filter based on local consistency measurement is constructed, including:

[0185] 1) Determine the nonlinear expression of the robust state filter based on local consistency measure:

[0186] ,

[0187] 2) Determine a time update model of the robust state filter based on local consistency measure, wherein the time update model includes a predicted value of the state estimation vector and a predicted value of the state estimation covariance matrix, wherein the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix Respectively expressed as:

[0188] ,

[0189] ,

[0190] in, represents the propagation volume point from time k-1 to time k, represents the noise matrix;

[0191] 3) Decomposing the noise covariance matrix to determine the measurement update model of the robust state filter based on local consistency measure.

[0192] In the embodiment of the present invention, it should be understood that the main improvement of the robust state filter based on local consistency metric is that the measurement update model can be specifically decomposed to achieve fine adjustment of the noise covariance.

[0193] Specifically, decomposing the noise covariance matrix to determine the measurement update model of the robust state filter based on local consistency measure includes:

[0194] 31) Decompose the noise covariance matrix to obtain the noise characteristics of multiple observation components;

[0195] Specifically, the observation noise covariance matrix Perform Cholesy decomposition as follows:

[0196] ,

[0197] in, , express The j-th row vector of corresponds to the noise characteristics of the j-th observation component, represents the observation noise dimension.

[0198] 32) Construct the observation noise covariance submatrix according to the noise characteristics of each observation component;

[0199] Specifically, the observation noise covariance submatrix is constructed, and the expression is:

[0200] ,

[0201] Among them, each Independently describes the noise variance characteristics of an observation component.

[0202] 33) Calculate the local consistency measure for each observation component;

[0203] In the embodiment of the present invention, the local consistency metric dynamically adjusts the update strength of the noise covariance submatrix by evaluating the local consistency between the observed data and the predicted state. For each observation component j, its local consistency metric is calculated :

[0204] ,

[0205] in, represents the actual observation value of the j-th observation component, represents the predicted value of the j-th observation component, The innovation covariance matrix of the j-th observation component.

[0206] 34) comparing the local consistency measure of each observation component with a preset adjustment threshold;

[0207] 35) If the local consistency measure of the observation component is greater than the preset adjustment threshold, the observation component is determined to be inconsistent with the predicted state, and the observation noise covariance submatrix is adjusted;

[0208] In the embodiment of the present invention, when When it is large, it indicates that the observed component is inconsistent with the predicted state and may be an outlier. Therefore, it is necessary to increase its noise covariance to reduce its impact on state estimation.

[0209] According to the local consistency measure , dynamically adjust the noise covariance submatrix :

[0210] ,

[0211] in, Represents the scaling factor, which is used to adjust the scaling rate.

[0212] 36) If the local consistency measure of the observation component is not greater than the preset adjustment threshold, the observation component is determined to be consistent with the predicted state, and the noise covariance matrix is kept unchanged;

[0213] In the embodiment of the present invention, when When it is small, it indicates that the observed component is highly consistent with the predicted state, keeping its noise covariance unchanged.

[0214] 37) Perform the inverse decomposition process based on the updated observation noise covariance submatrix to obtain the updated noise covariance matrix;

[0215] Specifically, after completing the adjustment of the noise covariance submatrix of all observation components, the updated submatrix Reorganized into :

[0216] ,

[0217] in, Represents the updated submatrix The corresponding row vector.

[0218] right Perform the inverse operation to obtain the new :

[0219] .

[0220] 38) Obtain the measurement update model based on the updated covariance matrix.

[0221] Through the inverse process of Cholesky decomposition, the new Merge into a new observation noise covariance matrix :

[0222] .

[0223] After obtaining the new observation noise covariance matrix Then, it is used in the measurement update process of state estimation.

[0224] Calculate the Kalman gain:

[0225] ,

[0226] Update the state estimate:

[0227] ,

[0228] Update the state covariance matrix:

[0229] .

[0230] S420, dynamically adjusting the update intensity of the observation noise covariance submatrix according to the evaluation result of the local consistency;

[0231] In an embodiment of the present invention, the evaluation result of local consistency is the local consistency measure, and the update strength of the noise covariance submatrix is dynamically adjusted according to the local consistency measure. When the local consistency measure is large, the noise covariance submatrix is increased, otherwise the noise covariance is kept unchanged.

[0232] S430, obtaining an updated noise covariance matrix according to the updated observation noise covariance submatrix;

[0233] The noise covariance matrix is obtained by performing an inverse operation on the observation noise covariance submatrix.

[0234] S440: Update the state estimation according to the updated noise covariance matrix to obtain a filtering state output result.

[0235] It should be understood that when the embodiment of the present invention adopts a robust filter with local consistency measurement, it can independently adjust the decomposed noise covariance submatrix, thereby achieving fine-grained adjustment and effectively suppressing outlier interference, thereby achieving high-precision and high-robustness navigation in complex environments.

[0236] It should be noted that when the underwater robot is in a non-complex environment, a non-basic linear filter is used as the navigation state filter of the INS / DVL tightly coupled combination model. Preferably, the basic nonlinear filter includes a cubature Kalman filter.

[0237] The following combination Figure 6 The actual specific working process of the INS / DVL tightly coupled integrated navigation method for underwater vehicles provided by the present invention is described.

[0238] First, a tightly coupled INS / DVL model for an underwater vehicle is established, including an inertial navigation system and a Doppler velocity measurement system. Second, during the pre-cruise phase, a Mahalanobis distance is constructed based on the error covariance, and a threshold for complex environment judgment is derived using a generalized Pareto distribution. Third, the noise covariance matrix is decomposed, and a consistency adaptation mechanism is constructed based on a local consistency metric to independently adjust different observation noise components. Finally, the complex environment judgment threshold is applied during the cruise phase. When the Mahalanobis distance generated by a single measurement exceeds the judgment threshold, a multi-adaptive robust state estimator based on the local consistency metric is executed; otherwise, a basic nonlinear filter is used.

[0239] In summary, the INS / DVL tightly coupled integrated navigation method for underwater vehicles provided by the present invention introduces a complex environment judgment threshold in the pre-cruise stage and a dynamic filtering algorithm selection mechanism in the cruise stage. In a low-complexity environment, a basic nonlinear filter is used to avoid parameter overcompensation and waste of computing power. In a high-complexity environment, a robust filter that performs local consistency measurement is used. By decomposing the noise covariance matrix and independently adjusting the noise components, outliers are processed in a refined manner, the robustness and accuracy in a complex environment are improved, and the interference of outliers is effectively suppressed, thereby achieving high-precision and high-robustness navigation in a complex environment. At the same time, the system resource utilization efficiency is optimized, and the performance and robustness of the INS / DVL tightly coupled integrated navigation system are significantly improved.

[0240] As another embodiment of the present invention, an INS / DVL tightly coupled integrated navigation device 100 for an underwater vehicle is provided, which is used to implement the INS / DVL tightly coupled integrated navigation method for an underwater vehicle as described above, wherein: Figure 7 Shown, including:

[0241] A combined model construction module 110 is used to construct an INS / DVL tightly coupled combined model of the underwater robot, wherein the INS / DVL tightly coupled combined model includes an inertial navigation system model and a Doppler velocity measurement system model;

[0242] A Mahalanobis distance construction module 120, configured to construct a Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled combination model;

[0243] a navigation state filter determination module 130, configured to determine a navigation state filter for the INS / DVL tightly coupled combination model based on a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution;

[0244] A local consistency filtering module 140 is configured to, if the comparison result indicates that the environment in which the underwater robot is located is a complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combination model is a robust state filter based on local consistency measurement, and execute the robust state filter based on local consistency measurement to obtain a filtered state output result, wherein the robust state filter based on local consistency measurement is capable of decomposing a noise covariance matrix and independently adjusting noise components;

[0245] A basic filtering module 150 is configured to determine, if the environment in which the underwater robot is located is a non-complex environment, that the navigation state filter of the INS / DVL tightly coupled combination model is a basic nonlinear filter, and obtain a filtering state output result according to the basic nonlinear filter;

[0246] The navigation parameter determination module 160 is used to determine the navigation parameters of the underwater robot according to the output result of the filtering state, and the navigation parameters of the underwater robot include posture, position and speed.

[0247] The INS / DVL tightly coupled integrated navigation device for an underwater vehicle provided by the present invention constructs a Mahalanobis distance based on error covariance, and according to a generalized Pareto distribution, a complex environment determination threshold can be obtained. Real-time judgment of the environment is achieved by comparing the Mahalanobis distance with a preset complex environment determination threshold. When the Mahalanobis distance exceeds the threshold, the underwater vehicle is considered to be in a highly complex environment. A multi-adaptive robust state estimator based on local consistency measurement is used to finely adjust the noise covariance corresponding to the outliers to avoid misadjustment of normal components. When the Mahalanobis distance is less than the threshold, a basic nonlinear filter is executed. Therefore, the INS / DVL tightly coupled integrated navigation device for an underwater vehicle of the present invention can avoid overcompensation of parameters in low-complexity situations, reduce waste of computing power, and significantly solve the problem of accuracy degradation caused by different environments.

[0248] The specific working principle of the INS / DVL tightly coupled integrated navigation device for underwater vehicles provided by the present invention can be referred to the description of the INS / DVL tightly coupled integrated navigation method for underwater vehicles mentioned above, which will not be repeated here.

[0249] As another embodiment of the present invention, an INS / DVL tightly coupled integrated navigation system 10 for an underwater vehicle is provided, wherein Figure 8 As shown, it includes: an inertial navigation system 200, a Doppler velocity measurement system 300 and the INS / DVL tightly coupled integrated navigation device 100 of the underwater vehicle mentioned above, and the inertial navigation system 200 and the Doppler velocity measurement system 300 are both communicatively connected to the INS / DVL tightly coupled integrated navigation device 100 of the underwater vehicle.

[0250] The INS / DVL tightly coupled integrated navigation system for an underwater vehicle provided by the present invention adopts the INS / DVL tightly coupled integrated navigation device for the underwater vehicle described above, constructs a Mahalanobis distance based on error covariance, and obtains a complex environment judgment threshold according to a generalized Pareto distribution. Real-time judgment of the environment is achieved by comparing the Mahalanobis distance with a preset complex environment judgment threshold. When the Mahalanobis distance exceeds the threshold, the underwater vehicle is considered to be in a highly complex environment. A multi-adaptive robust state estimator based on local consistency measurement is adopted to finely adjust the noise covariance corresponding to the outlier to avoid misadjustment of the normal component. When the Mahalanobis distance is less than the threshold, a basic nonlinear filter is executed. Therefore, the INS / DVL tightly coupled integrated navigation system for an underwater vehicle of the present invention can avoid overcompensation of parameters in low-complexity situations, reduce waste of computing power, and significantly solve the problem of accuracy degradation caused by different environments.

[0251] The specific working principle of the INS / DVL tightly coupled integrated navigation system for underwater vehicles provided by the present invention can be referred to the description of the INS / DVL tightly coupled integrated navigation method for underwater vehicles mentioned above, which will not be repeated here.

[0252] It will be understood that the above embodiments are merely exemplary embodiments for illustrating the principles of the present invention, and the present invention is not limited thereto. Those skilled in the art will appreciate that various modifications and improvements can be made without departing from the spirit and substance of the present invention, and such modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. An INS / DVL tightly coupled integrated navigation method for an underwater vehicle, characterized in that: include: Constructing an INS / DVL tightly coupled combination model of an underwater robot, wherein the INS / DVL tightly coupled combination model includes an inertial navigation system model and a Doppler velocity measurement system model; Constructing the Mahalanobis distance based on the error covariance of the INS / DVL tightly coupled combination model; determining a navigation state filter of the INS / DVL tightly coupled combination model based on a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution; If the comparison result shows that the type of environment in which the underwater robot is located is a complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combination model is a robust state filter based on local consistency measurement, and executing the robust state filter based on local consistency measurement to obtain a filter state output result, wherein the robust state filter based on local consistency measurement can decompose the noise covariance matrix and can independently adjust the noise component; If the environment type in which the underwater robot is located is a non-complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combination model is a basic nonlinear filter, and obtaining a filtering state output result according to the basic nonlinear filter; The navigation parameters of the underwater robot are determined according to the output result of the filtering state, and the navigation parameters of the underwater robot include posture, position and speed.

2. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 1, characterized in that: Determining the navigation state filter of the INS / DVL tightly coupled combination model according to a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold includes: Generate complex environment error judgment threshold based on generalized Pareto distribution; Comparing the Mahalanobis distance with the complex environment error determination threshold generated by the generalized Pareto distribution; If the Mahalanobis distance is greater than or equal to the complex environment error determination threshold generated by the generalized Pareto distribution, it is determined that the type of environment in which the underwater robot is located is a complex environment; If the Mahalanobis distance is less than the complex environment error determination threshold of the generalized Pareto distribution, it is determined that the environment type in which the underwater robot is located is a non-complex environment.

3. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 2, wherein the complex environment error determination threshold is generated according to the generalized Pareto distribution, comprising: The Mahalanobis distance tail data is fitted according to the generalized Pareto distribution, wherein the cumulative distribution function of the generalized Pareto distribution is expressed as: ， in, represents the shape parameter, represents the scale parameter, represents the location parameter, and x represents the observed variable; The complex environment error determination threshold is determined based on the fitting result and in combination with the preset confidence interval. The expression of the complex environment error determination threshold is: ， in, Indicates the quantile corresponding to the preset confidence interval. When using the limit form .

4. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 1, characterized in that: Executing the robust state filter based on local consistency measurement to obtain a filtering state output result includes: Constructing a robust state filter based on a local consistency metric, wherein the robust state filter based on the local consistency metric is capable of evaluating the local consistency between the observed data and the predicted state; Dynamically adjusting the update intensity of the observation noise covariance submatrix according to the evaluation result of the local consistency; Obtaining an updated noise covariance matrix according to the updated observation noise covariance submatrix; The state estimation is measured and updated according to the updated noise covariance matrix to obtain the filtering state output result.

5. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 4, characterized in that: Construct a robust state filter based on local consistency measure, including: Determine the nonlinear expression of the robust state filter based on local consistency measure: ， Determine a time update model of the robust state filter based on local consistency measure, wherein the time update model includes a predicted value of a state estimation vector and a predicted value of a state estimation covariance matrix, wherein the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix Respectively expressed as: ， ， in, represents the propagation volume point from time k-1 to time k, represents the noise matrix; A noise covariance matrix is decomposed to determine a measurement update model of the robust state filter based on local consistency measure.

6. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 5, characterized in that: Decomposing the noise covariance matrix to determine a measurement update model of the robust state filter based on the local consistency measure includes: The noise covariance matrix is decomposed to obtain the noise characteristics of multiple observation components, wherein the decomposition expression of the noise covariance matrix is: ， in, , express The j-th row vector of corresponds to the noise characteristics of the j-th observation component, represents the observation noise dimension; The observation noise covariance submatrix is constructed according to the noise characteristics of each observation component, wherein the expression of the observation noise covariance submatrix is: ， Among them, each Independently describe the noise variance characteristics of an observation component; The local consistency measure is calculated for each observation component, where the local consistency measure of each observation component is The expression is: ， in, represents the actual observation value of the j-th observation component, represents the predicted value of the j-th observation component, The innovation covariance matrix of the j-th observation component; Comparing the local consistency measure of each observation component with a preset adjustment threshold; If the local consistency measure of the observation component is greater than the preset adjustment threshold, the observation component is determined to be inconsistent with the predicted state, and the observation noise covariance submatrix is adjusted. The expression of the adjusted observation noise covariance submatrix is: ， in, Represents the scaling factor, which is used to adjust the scaling rate; If the local consistency measure of the observation component is not greater than the preset adjustment threshold, the observation component is determined to be consistent with the predicted state, and the noise covariance matrix is kept unchanged; The inverse decomposition process is performed according to the updated observation noise covariance submatrix to obtain an updated noise covariance matrix, wherein the expression of the updated noise covariance matrix is: ； The measurement update model is obtained according to the updated covariance matrix.

7. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 1, characterized in that: The Mahalanobis distance is constructed based on the error covariance of the INS / DVL tightly coupled combination model, including: Determining that the state filter in the INS / DVL tightly coupled combination model is a cubature Kalman filter; Obtaining volume points with the same weight according to a spherical radial criterion, and obtaining a predicted state vector and an error covariance matrix to implement a time update of the volume Kalman filter; Updating the predicted state vector according to the received external measurement value to achieve measurement update of the cubature Kalman filter; According to the innovation in the measurement update and the innovation covariance matrix, the Mahalanobis distance between the actual observation value and the predicted value is constructed. The expression of the Mahalanobis distance is: ， in, represents the Mahalanobis distance, Indicates new information, represents the innovation covariance matrix.

8. The INS / DVL tightly coupled integrated navigation method for underwater vehicles according to claim 1, characterized in that: The basic nonlinear filter includes a cubic Kalman filter.

9. An INS / DVL tightly coupled integrated navigation device for an underwater vehicle, used to implement the INS / DVL tightly coupled integrated navigation method for an underwater vehicle according to any one of claims 1 to 8, characterized in that: include: A combined model construction module is used to construct an INS / DVL tightly coupled combined model of the underwater robot, wherein the INS / DVL tightly coupled combined model includes an inertial navigation system model and a Doppler velocity measurement system model; A Mahalanobis distance construction module, configured to construct a Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled combination model; a navigation state filter determination module, configured to determine the navigation state filter of the INS / DVL tightly coupled combination model based on a comparison result of the Mahalanobis distance and a preset complex environment error determination threshold, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution; A local consistency filtering module is configured to, if the comparison result indicates that the type of environment in which the underwater robot is located is a complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combination model is a robust state filter based on local consistency measurement, and execute the robust state filter based on local consistency measurement to obtain a filtering state output result, wherein the robust state filter based on local consistency measurement is capable of decomposing a noise covariance matrix and independently adjusting noise components; a basic filtering module, configured to determine, if the environment in which the underwater robot is located is a non-complex environment, that the navigation state filter of the INS / DVL tightly coupled combination model is a basic nonlinear filter, and obtain a filtering state output result according to the basic nonlinear filter; The navigation parameter determination module is used to determine the navigation parameters of the underwater robot according to the output result of the filtering state, and the navigation parameters of the underwater robot include posture, position and speed.

10. An INS / DVL tightly coupled integrated navigation system for underwater vehicles, characterized in that: include: The inertial navigation system, the Doppler velocity measurement system and the INS / DVL tightly coupled integrated navigation device of the underwater vehicle according to claim 9, wherein the inertial navigation system and the Doppler velocity measurement system are both communicatively connected to the INS / DVL tightly coupled integrated navigation device of the underwater vehicle.

Images