INS / DVL tight coupling integrated navigation method, device and system of underwater vehicle

Abstract

The invention relates to the technical field of underwater vehicle inertial navigation, and particularly discloses an INS / DVL tight coupling combined navigation method, device and system of an underwater vehicle, and the method comprises the following steps: constructing an INS / DVL tight coupling combined model of an underwater robot; constructing a mahalanobis distance according to the error covariance of the combined model; determining a navigation state filter of the combined model according to a comparison result of the mahalanobis distance and a preset complex environment error judgment threshold value; if the environment is a complex environment, executing a robust state filter based on local consistency measurement; if the environment is a non-complex environment, executing a basic nonlinear filter; and determining navigation parameters of the underwater robot according to the filtering state output result. According to the INS / DVL tight coupling integrated navigation method for the underwater vehicle, the problem that noise parameters are not matched with actual conditions due to environmental changes can be solved on the premise of avoiding parameter excessive compensation in a low-complexity environment and reducing computing power waste.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle inertial navigation, and particularly relates 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] As a core equipment for ocean exploration and development tasks, underwater vehicles have important application values. When underwater vehicles perform tasks such as deployment, operation, and recovery in the vast underwater space, accurate position estimation is required, so more advanced navigation technologies are needed. In an underwater denial environment, electromagnetic waves will undergo severe attenuation, making traditional satellite navigation systems no longer applicable to underwater navigation. The Inertial Navigation System (INS) has the advantages of good autonomy, strong anti-interference ability, high update frequency, and comprehensive navigation information, and has become an essential navigation device underwater. However, its error increases rapidly with time. In order to obtain high-precision navigation information over a long period of time, external measurement information needs to be introduced to suppress the divergence of errors. The Doppler Velocity Log (DVL) has become the mainstream choice for underwater acoustic measurement equipment because of its small size, convenient installation, and accurate measurement of the vehicle speed.

[0003] The INS / DVL loosely coupled navigation algorithm selects the difference between the inertial navigation speed information in the carrier coordinate system and the speed information of the DVL as the observation quantity, selects the inertial navigation system error, the scale factor error of the DVL, and the installation angle error as state variables, and realizes real-time error estimation and feedback correction through Kalman filtering. Finally, high-precision integrated navigation information is obtained. The biggest difference from the loose coupling is that in the tight coupling, the original data of the DVL is no longer subjected to coordinate transformation, but directly input into the navigation state filter. In the navigation state filter, the difference between the corresponding value of the SINS and the DVL beam measurement is directly compared, and then the comparison information is independently integrated into the navigation state filter. The tight coupling can make more full use of the four-dimensional frequency shift information compared with the loose coupling, and the integrated navigation accuracy is higher. In an integrated navigation system, noise parameters are usually set based on prior knowledge to achieve optimal estimation. In a low-complexity environment, these preset noise parameter distributions can well match the actual noise characteristics in the sensor-acquired data. However, when the environmental complexity increases, there will be a significant deviation between the preset noise parameters and the actual working conditions, and even outliers may occur, which will 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 will significantly change the output characteristics of sensors, and thus have an adverse impact on the performance of the INS / DVL integrated navigation system.

[0004] The method for solving the problem of mismatch between INS / DVL combined navigation noise parameters and actual conditions in the existing technology is mainly the Sage-Husa adaptive filtering algorithm. Specifically, the measurement fault detection and adjustment mechanism is introduced to solve the problem that the performance of adaptive filtering estimation of measurement noise parameters is strongly dependent on the fading factor. When there is an abnormality in the observed quantity, the measurement matrix is ​​fault detected and adjusted online to suppress the error. 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 the problems of computational waste and parameter over-compensation, which will affect the 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 waste of computing power 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 INS / DVL integrated navigation noise parameters not matching the actual ones cannot be avoided in the related art.

[0007] As a first aspect of the present invention, an INS / DVL tightly coupled integrated navigation method for 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 according to the error covariance of the INS / DVL tightly coupled combination model;

[0010] 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, wherein the preset complex environment error determination threshold is obtained according to a generalized Pareto distribution;

[0011] If the comparison result indicates that the environment type where 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, where the robust state filter based on local consistency measurement can decompose the noise covariance matrix and can independently adjust the noise components;

[0012] If the environment type where the underwater robot is located is a non-complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combination model is a basic non-linear filter, and obtain a filtered state output result according to the basic non-linear filter;

[0013] Determine the navigation parameters of the underwater robot according to the filtered state output result, where the navigation parameters of the underwater robot include attitude, position, and velocity.

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

[0015] Generate a complex environment error determination threshold according to the generalized Pareto distribution;

[0016] Compare 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, determine that the environment type where the underwater robot is located is a complex environment;

[0018] If the Mahalanobis distance is less than the complex environment error determination threshold generated by the generalized Pareto distribution, determine that the environment type where the underwater robot is located is a non-complex environment.

[0019] Furthermore, generating a complex environment error determination threshold according to the generalized Pareto distribution includes:

[0020] Fit the tail data of the Mahalanobis distance according to the generalized Pareto distribution, where the expression of the cumulative distribution function of the generalized Pareto distribution is:

[0021] ,

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

[0023] Determine the complex environment error determination threshold according to the fitting result and in combination with the preset confidence interval, and the expression of the complex environment error determination threshold is:

[0024] ,

[0025] where, represents the quantile corresponding to the preset confidence interval, and when the limit form is adopted.

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

[0027] Construct a robust state filter based on local consistency measurement, and the robust state filter based on local consistency measurement can evaluate the local consistency between the observed data and the predicted state;

[0028] Dynamically adjust the update intensity of the observation noise covariance sub-matrix according to the evaluation result of the local consistency;

[0029] Obtain the updated noise covariance matrix according to the updated observation noise covariance sub-matrix;

[0030] Perform measurement update on the state estimation according to the updated noise covariance matrix to obtain the filtered state output result.

[0031] Furthermore, constructing a robust state filter based on local consistency measurement includes:

[0032] Determine the non-linear expression of the robust state filter based on local consistency measurement:

[0033] ,

[0034] Determine the time update model of the robust state filter based on local consistency measurement, and the time update model includes the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix. The predicted value of the state estimation vector and the predicted value

[0035] of the state estimation covariance matrix

[0036] are respectively expressed as:

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

[0038] Decompose the noise covariance matrix to determine the measurement update model of the robust state filter based on local consistency metrics.

[0039] Further, decomposing the noise covariance matrix to determine the measurement update model of the robust state filter based on local consistency metrics includes:

[0040] Decompose the noise covariance matrix to obtain the noise characteristics of multiple observation components, where the decomposition expression of the noise covariance matrix is:

[0041] ,

[0042] where, , represents the j-th row vector of corresponding to the noise characteristics of the j-th observation component,

[0043] Construct an observation noise covariance sub-matrix according to the noise characteristics of each observation component, where the expression of the observation noise covariance sub-matrix is:

[0044] ,

[0045] where each independently describes the noise variance characteristics of an observation component;

[0046] Calculate the local consistency metric for each observation component, where the local consistency metric of each observation component is expressed as:

[0047] ,

[0048] where, 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] Compare the local consistency metric of each observation component with a preset adjustment threshold;

[0050] If the local consistency metric of the observation component is greater than the preset adjustment threshold, it is determined that the observation component is inconsistent with the predicted state, and the observation noise covariance sub-matrix is adjusted. The adjusted expression of the observation noise covariance sub-matrix is:

[0051] ,

[0052] where, Represents a scaling factor used to adjust the scaling rate;

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

[0054] Perform a decomposition inverse process based on the updated observation noise covariance submatrix to obtain the updated noise covariance matrix, where the expression of the updated noise covariance matrix is:

[0055] ;

[0056] Obtain a measurement update model based on the updated covariance matrix.

[0057] Furthermore, construct a Mahalanobis distance based on the error covariance of the INS / DVL tightly coupled combination model, including:

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

[0059] Obtain cubature points with the same weights according to the spherical radial criterion, and obtain the predicted state vector and the error covariance matrix to achieve the time update of the cubature Kalman filter;

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

[0061] Construct a Mahalanobis distance between the actual observation value and the predicted value according to the innovation and the innovation covariance matrix in the measurement update, and the expression of the Mahalanobis distance is:

[0062] ,

[0063] Where Represents the Mahalanobis distance, Represents the innovation, Represents the innovation covariance matrix.

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

[0065] As another aspect of the present invention, there is provided an INS / DVL tightly coupled integrated navigation device for an underwater vehicle, which is used to implement the INS / DVL tightly coupled integrated navigation method for the underwater vehicle described above, and includes:

[0066] A combined model construction module for constructing an INS / DVL tightly coupled combined model of an underwater robot, where 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 for constructing a Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled combined model;

[0068] A navigation state filter determination module for determining the navigation state filter of the INS / DVL tightly coupled combined model according to the comparison result between the Mahalanobis distance and a preset complex environment error determination threshold, where the preset complex environment error determination threshold is obtained according to the generalized Pareto distribution;

[0069] A local consistency filtering module for, if the comparison result indicates that the environment type where the underwater robot is located is a complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combined model is a robust state filter based on local consistency metrics, and executing the robust state filter based on local consistency metrics to obtain a filtered state output result, where the robust state filter based on local consistency metrics can decompose the noise covariance matrix and can independently adjust the noise components;

[0070] A basic filtering module for, if the environment type where the underwater robot is located is a non-complex environment, determining that the navigation state filter of the INS / DVL tightly coupled combined model is a basic non-linear filter, and obtaining a filtered state output result according to the basic non-linear filter;

[0071] A navigation parameter determination module for determining the navigation parameters of the underwater robot according to the filtered state output result, where the navigation parameters of the underwater robot include attitude, position, and velocity.

[0072] As another embodiment of the present invention, there is provided an INS / DVL tightly coupled integrated navigation system for an underwater vehicle, which includes: an inertial navigation system, a Doppler velocity measurement system, and the INS / DVL tightly coupled integrated navigation device of the underwater vehicle described above, and both the inertial navigation system and the Doppler velocity measurement system are communicatively connected to the INS / DVL tightly coupled integrated navigation device of 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 the error covariance. According to the generalized Pareto distribution, a complex environment determination threshold can be obtained. By comparing the Mahalanobis distance with the preset complex environment determination threshold, real-time judgment of the environment is achieved. When the Mahalanobis distance exceeds this threshold, it is considered that the underwater vehicle is in a highly complex environment, and a multi-adaptive robust state estimator based on local consistency measurement is used to finely adjust the noise covariance corresponding to the outliers, avoiding misadjusting the normal components. When the Mahalanobis distance is less than this threshold, a basic nonlinear filter is executed. Therefore, the INS / DVL tightly coupled integrated navigation method for the underwater vehicle of the present invention can avoid overcompensation of parameters in low-complexity situations, reduce waste of computing power, and greatly solve the problem of accuracy degradation caused by different environments. Description of the Drawings

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

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

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

[0077] Figure 3 It is a flowchart of the method for constructing the Mahalanobis distance provided by the present invention.

[0078] Figure 4 It is a flowchart of the method for determining the navigation state filter of the INS / DVL tightly coupled integrated model provided by the present invention.

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

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

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

[0082] Figure 8 It is a structural block diagram of the INS / DVL tightly coupled integrated navigation system for the underwater vehicle provided by the present invention. Specific Embodiment

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

[0084] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative efforts shall fall within the protection scope of the present invention.

[0085] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and do not have to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so as to implement the embodiments of the present invention described herein. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

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

[0087] S100. Construct an INS / DVL tightly coupled integrated model for the underwater robot, where the INS / DVL tightly coupled integrated model includes an inertial navigation system model and a Doppler velocity measurement system model;

[0088] In the embodiment of the present invention, an INS / DVL tightly coupled integrated model for the underwater robot is constructed according to the inertial navigation system model and the Doppler velocity measurement system model.

[0089] S200. Construct a Mahalanobis distance according to the error covariance of the INS / DVL tightly coupled integrated model;

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

[0091] S300. Determine the navigation state filter of the INS / DVL tightly coupled combined model according to the comparison result between the Mahalanobis distance and a preset complex environment error determination threshold, where the preset complex environment error determination threshold is obtained according to the 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, so that the type of the navigation state filter can be determined.

[0093] It should be understood that since the determination of the preset complex environment error determination threshold can determine the type of the navigation state filter, the preset complex environment error determination threshold can be accurately determined by the generalized Pareto distribution in the embodiment of the present invention, so that an accurate judgment of the Mahalanobis distance can be realized, and thus an accurate determination of the navigation state filter can be realized.

[0094] S400. If the comparison result is that the environment type where the underwater robot is located is a complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combined 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, where the robust state filter based on local consistency measurement can decompose the noise covariance matrix and can independently adjust the noise components;

[0095] It should be understood that according to the above comparison result between the Mahalanobis distance and the preset complex environment error determination 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 can independently adjust the noise components, so as to effectively reduce the waste of computing power.

[0096] S500. If the environment type where the underwater robot is located is a non-complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combined model is a basic non-linear filter, and obtain a filtered state output result according to the basic non-linear filter;

[0097] In an embodiment of the present invention, when it is determined that the environment type where the underwater robot is located is a non-complex environment, it can be implemented by using a basic non-linear filter, and over-compensation of parameters in the case of a non-complex environment can also be avoided.

[0098] S600. Determine the navigation parameters of the underwater robot according to the filtered state output result, where the navigation parameters of the underwater robot include attitude, position and velocity.

[0099] The INS / DVL tightly coupled integrated navigation method for an underwater vehicle according to an embodiment of the present invention determines a preset complex environment determination threshold through the Pareto distribution. This method can accurately determine the environment where the underwater vehicle is located, avoid the phenomenon of over-compensation of parameters that occurs when a navigation filter originally applicable to a high-complexity environment is applied to a low-complexity environment in the prior art, and at the same time can finely adjust the noise parameters in a high-complexity environment, avoiding waste of computing power.

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

[0101] In the embodiment of the present invention, in combination with Figure 2 the architecture diagram of the INS / DVL tightly coupled integrated model shown, the specific process of constructing the INS / DVL tightly coupled integrated model of the underwater robot includes:

[0102] First, select 15-dimensional inertial navigation system errors as the error state variables of the INS subsystem:

[0103] ,

[0104] Among them, respectively represent the velocity errors in the east, north, and up directions; respectively represent the attitude errors in the east, north, and up directions; respectively represent the latitude error, longitude error, and altitude error of the inertial navigation; respectively represent the zero-bias errors of the three axes of the INS accelerometer in the body coordinate system; respectively represent the zero-drift errors of the three axes of the INS gyroscope in the body coordinate system.

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

[0106] ,

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

[0108] ,

[0109] where:

[0110] ,

[0111] ,

[0112] ,

[0113] ,

[0114] ,

[0115] ,

[0116] ,

[0117] ,

[0118] where, denotes the angular velocity of the Earth's rotation relative to the inertial coordinate system, and respectively denote the radius of the meridian and the radius of the prime vertical at the location of the carrier.

[0119] The error of the DVL is strongly correlated with its scale factor and installation angle error. Therefore, the installation angle error and scale factor error of the DVL are selected as the error state variables of the DVL subsystem:

[0120] ,

[0121] where, denotes the installation error angles of the DVL along the three coordinate axes relative to the INS; denotes the scale factor error.

[0122] Since the installation error angles between the DVL and the INS are constant values, and the scale factor can be approximated as a constant value in the same water area, the state error equation of the DVL subsystem is established as:

[0123] ,

[0124] where, denotes the system matrix of the DVL subsystem, denotes the DVL-related noise.

[0125] Combine the INS subsystem 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 the four-beam Janus-type cross-shaped Doppler sensor. The column vector composed of the Doppler frequency shifts 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 actually output frequency shift information of the DVL can be expressed as:

[0132] .

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

[0134] ,

[0135] Among them, , represents the beam depression angles of the four beams; represents the coordinate transformation matrix from the navigation system to the vehicle system; represents the coordinate transformation matrix from the vehicle system to the Doppler system.

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

[0137] ,

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

[0139] ,

[0140] Construct the measurement model of the INS / DVL tight coupling integrated navigation system:

[0141] ,

[0142] Among them, represents the four-dimensional frequency shift measurement noise of the DVL.

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

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

[0145] The embodiment of the present invention adopts a cubature Kalman filter (CKF). In the time update part, the spherical radial criterion is mainly used to obtain cubature points with the same weights, and then the predicted state vector and the error covariance matrix can be obtained; in the measurement update part, the measured value is mainly used to update the predicted state and improve its estimation error.

[0146] First, establish a non - linear expression of the INS / DVL tightly coupled model:

[0147] .

[0148] S220. Obtain cubature points with the same weights according to the spherical radial criterion, and obtain the predicted state vector and the error covariance matrix to implement the time update of the cubature Kalman filter;

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

[0150] ,

[0151] ,

[0152] where represents the propagated cubature points from time k - 1 to time k, represents the system noise matrix.

[0153] S230. Update the predicted state vector according to the received external measurement value to implement the measurement update of the cubature Kalman filter;

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

[0155] ,

[0156] ,

[0157] where Indicates the propagation volume point, Indicates the measurement noise matrix.

[0158] Construct the innovation of the measurement :

[0159] .

[0160] S240. Construct the Mahalanobis distance between the actual observation value and the predicted value according to the innovation and the innovation covariance matrix in the measurement update, and the expression of the Mahalanobis distance is:

[0161] ,

[0162] where, Indicates the Mahalanobis distance, Indicates the innovation, Indicates the innovation covariance matrix.

[0163] It should be understood that according to the innovation of the measurement and the innovation covariance matrix , the Mahalanobis distance between the actual observation value and the predicted value can be obtained

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

[0165] In the embodiment of the present invention, the navigation state filter of the INS / DVL tight coupling combined model is determined according to the comparison result between the Mahalanobis distance and the preset complex environment error determination threshold, as Figure 4 shown, including:

[0166] S310. Generate a complex environment error determination threshold according to the generalized Pareto distribution;

[0167] In the embodiment of the present invention, a Mahalanobis distance sequence is constructed using the Mahalanobis distance of each epoch , where N represents the total number of measurement epochs, and is sorted in ascending order, and the m largest Mahalanobis distance values at the tail are selected, where , and an extreme value sample value is constructed.

[0168] Specifically, generating a complex environment error determination threshold according to the generalized Pareto distribution includes:

[0169] Fitting the tail data of the Mahalanobis distance according to the generalized Pareto distribution, and the expression of the cumulative distribution function of the generalized Pareto distribution is:

[0170] ,

[0171] wherein, represents the shape parameter, represents the scale parameter, represents the position parameter, and x represents the observed variable;

[0172] Determine the complex environment error judgment threshold according to the fitting result and in combination with the preset confidence interval, and the expression of the complex environment error judgment threshold is:

[0173] ,

[0174] wherein, represents the quantile corresponding to the preset confidence interval. When , the limit form

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

[0176] S320. Compare the Mahalanobis distance with the complex environment error judgment threshold generated by the generalized Pareto distribution;

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

[0178] Specifically, when , it is considered that the current situation is an underwater high-complexity environment, and there is a risk of large noise and frequent gross errors in the Doppler velocity measurement beam. At this time, the deviation between the predicted measurement value and the actual measurement value is large, and 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 judgment threshold generated by the generalized Pareto distribution, determine that the environment type where the underwater robot is located is a non-complex environment.

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

[0181] In the embodiment of the present invention, execute the robust state filter based on local consistency measurement to obtain the filtered state output result, as Figure 5 shown, including:

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

[0183] In the embodiments 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 in the INS / DVL tightly coupled model described above.

[0184] Specifically, constructing a robust state filter based on local consistency measurement includes:

[0185] 1) Determining the non-linear expression of the robust state filter based on local consistency measurement:

[0186] ,

[0187] 2) Determining the time update model of the robust state filter based on local consistency measurement, where the time update model includes the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix, and the predicted value of the state estimation vector and the predicted value of the state estimation covariance matrix are respectively expressed as:

[0188] ,

[0189] ,

[0190] where represents the propagated cubature points 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 measurement.

[0192] In the embodiments of the present invention, it should be understood that the main improvement of the robust state filter based on local consistency measurement lies in that the measurement update model can be specifically decomposed to achieve fine-tuning 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 measurement includes:

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

[0195] Specifically, decomposing the observation noise covariance matrix by Cholesy decomposition into:

[0196] ,

[0197] where , denote the j-th row vector of , corresponding to the noise characteristics of the j-th observation component, denote the dimension of the observation noise.

[0198] 32) Construct an observation noise covariance sub-matrix according to the noise characteristics of each observation component;

[0199] Specifically, construct the observation noise covariance sub-matrix, and the expression is:

[0200] ,

[0201] where each independently describes the noise variance characteristics of an observation component.

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

[0203] In the embodiments of the present invention, the local consistency metric dynamically adjusts the update intensity of the noise covariance sub-matrix by evaluating the local consistency between the observation data and the predicted state. For each observation component j, calculate its local consistency metric :

[0204] ,

[0205] where, denotes the actual observed value of the j-th observation component, denotes the predicted value of the j-th observation component, the innovation covariance matrix of the j-th observation component.

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

[0207] 35) If the local consistency metric of the observation component is greater than the preset adjustment threshold, it is determined that the observation component is inconsistent with the predicted state, and the observation noise covariance sub-matrix is adjusted;

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

[0209] According to the local consistency metric , dynamically adjust the noise covariance sub-matrix :

[0210] ,

[0211] where, denotes the scaling factor, which is used to adjust the scaling rate.

[0212] 36) If the local consistency metric of the observation component is not greater than the preset adjustment threshold, it is determined that the observation component is consistent with the predicted state, and the noise covariance matrix remains unchanged;

[0213] In the embodiment of the present invention, when is small, it indicates that the observation component has a high consistency with the predicted state, and its noise covariance remains unchanged.

[0214] 37) Perform the decomposition inverse process according to the updated observation noise covariance sub - matrix to obtain the updated noise covariance matrix;

[0215] Specifically, after completing the adjustment of the noise covariance sub - matrices of all observation components, the updated sub - matrices are re - combined into :

[0216] ,

[0217] where, represents the row vector corresponding to the updated sub - matrix .

[0218] Perform an inverse operation on to obtain a new :

[0219] .

[0220] 38) Obtain the measurement update model according to the updated covariance matrix.

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

[0222] .

[0223] After obtaining the new observation noise covariance matrix , 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 adjust the update intensity of the observation noise covariance sub - matrix according to the evaluation result of the local consistency;

[0231] In the embodiment of the present invention, the evaluation result of the local consistency is the local consistency metric. The update intensity of the noise covariance sub - matrix is dynamically adjusted according to the local consistency metric. When the local consistency metric is large, the noise covariance sub - matrix is increased; otherwise, the noise covariance remains unchanged.

[0232] S430. Obtain the updated noise covariance matrix according to the updated observation noise covariance sub - matrix;

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

[0234] S440. Perform measurement update on the state estimate according to the updated noise covariance matrix to obtain the filtered state output result.

[0235] It should be understood that when the robust filter using the local consistency metric is adopted in the embodiment of the present invention, the decomposed noise covariance sub - matrix can be independently adjusted, so as to achieve fine - tuning, effectively suppress the outlier interference, and thus achieve high - precision and high - robustness navigation in a complex environment.

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

[0237] The following combines Figure 6 Describe the actual specific working process of the INS / DVL tightly - coupled combined navigation method for the underwater vehicle provided by the present invention.

[0238] First, establish an INS / DVL tightly - coupled model carried on the underwater vehicle, including an inertial navigation system and a Doppler velocity measurement system; secondly, construct the Mahalanobis distance based on the error covariance in the pre - cruise stage, and obtain the determination threshold for the complex environment according to the generalized Pareto distribution. Thirdly, decompose the noise covariance matrix, construct a consistency adaptive mechanism based on the local consistency metric, and independently adjust different observation noise components. Finally, apply the determination threshold for the complex environment to the cruise stage. When the Mahalanobis distance generated by a single measurement is greater than the determination threshold, execute the multi - adaptive robust state estimator based on the local consistency metric; otherwise, execute the basic non - linear filter.

[0239] In summary, the INS / DVL tightly coupled integrated navigation method for an underwater vehicle provided by the present invention introduces a complex environment determination threshold in the pre-cruise stage and a dynamic filtering algorithm selection mechanism in the cruise stage. In a low-complexity environment, a basic non-linear filter is used to avoid parameter over-compensation and computing power waste, while in a high-complexity environment, a robust filter for local consistency measurement is executed. By decomposing the noise covariance matrix and independently adjusting the noise components, outliers are refined, and the robustness and accuracy in complex environments are improved, effectively suppressing outlier interference. Thus, high-precision and high-robustness navigation are achieved in complex environments, while the system resource utilization efficiency is optimized, significantly enhancing the performance and robustness of the INS / DVL tightly coupled integrated navigation system.

[0240] As another embodiment of the present invention, there is provided an INS / DVL tightly coupled integrated navigation device 100 for an underwater vehicle, which is used to implement the INS / DVL tightly coupled integrated navigation method for an underwater vehicle described above. Among them, as Figure 7 shown, it includes:

[0241] A combined model construction module 110, configured to construct an INS / DVL tightly coupled combined model of an underwater robot, where 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 combined model;

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

[0244] A local consistency filtering module 140, configured to, if the comparison result indicates that the environment type where the underwater robot is located is a complex environment, determine that the navigation state filter of the INS / DVL tightly coupled combined 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, where the robust state filter based on local consistency measurement can decompose the noise covariance matrix and can independently adjust the noise components;

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

[0246] The navigation parameter determination module 160 is used to determine the navigation parameters of the underwater robot according to the filtering state output result, 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 an error covariance, and can obtain 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, it is considered that the underwater vehicle is in a highly complex environment. A multi-adaptive robust state estimator based on a local consistency metric is used to finely adjust the noise covariance corresponding to the abnormal value to avoid misadjusting 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 device for an underwater vehicle of the present invention can avoid over-compensation of parameters in a low-complexity situation, reduce the waste of computing power, and greatly solve the problem of reduced accuracy due to different environments.

[0248] The specific working principle of the INS / DVL tightly coupled integrated navigation device for an underwater vehicle provided by the present invention can be referred to the description of the INS / DVL tightly coupled integrated navigation method for an underwater vehicle in the foregoing text, 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 with 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 mentioned above, constructs the Mahalanobis distance based on the error covariance, and obtains the complex environment judgment threshold according to the generalized Pareto distribution. The real-time judgment of the environment is realized 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 adopted to finely adjust the noise covariance corresponding to the abnormal value to avoid misadjusting 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 system for an underwater vehicle of the present invention can avoid over-compensation of parameters in low-complexity situations, reduce the waste of computing power, and greatly solve the problem of reduced accuracy caused by different environments.

[0251] Regarding the specific working principle of the INS / DVL tightly coupled integrated navigation system of the underwater vehicle provided by the present invention, reference can be made to the description of the INS / DVL tightly coupled integrated navigation method of the underwater vehicle in the previous text, and details are not described herein again.

[0252] It can be understood that the above embodiments are merely exemplary embodiments adopted to illustrate the principle of the present invention. However, the present invention is not limited thereto. For those of ordinary skill in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also regarded as the protection scope 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 according to the error covariance of the INS / DVL tightly coupled combination model; 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, 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 combined 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 filtering 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 type of environment in which the underwater robot is located is a non-complex environment, determining the navigation state filter of the INS / DVL tightly coupled combination model as 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 filtering state output result, 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 the comparison result of the Mahalanobis distance and the preset complex environment error determination threshold includes: Generate complex environment error judgment threshold according to 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 type of environment 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, generating a complex environment error determination threshold according to a generalized Pareto distribution, comprising: The tail data of the Mahalanobis distance are fitted according to the generalized Pareto distribution, wherein the expression of the cumulative distribution function of the generalized Pareto distribution is: ， 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 according to 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. In the limiting 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 local consistency measure, wherein the robust state filter based on local consistency measure can evaluate 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 measurement, 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 the 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; For each observation component, its local consistency measure is calculated, where the local consistency measure of each observation component is The expression is: ， in, represents the actual observed value of the jth observation component, represents the predicted value of the jth observed component, The innovation covariance matrix of the jth observation component; comparing the local consistency measure of each observed component to 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 observation noise covariance submatrix after adjustment is: ， in, Represents the scaling factor, which is used to adjust the scaling rate; If the local consistency measure of the observed component is not greater than the preset adjustment threshold, the observed 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 according to 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 volumetric Kalman filter; 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 achieve 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 new information and the new information covariance matrix in the measurement update, 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 comprises 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 building module, used to build 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, used for constructing the 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 according to a comparison result between 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 used for determining that the navigation state filter of the INS / DVL tightly coupled combined model is a robust state filter based on local consistency measurement if the comparison result shows that the type of environment in which the underwater robot is located is a complex environment, and executing 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 can decompose the noise covariance matrix and can independently adjust the noise component; A basic filtering module, for determining the navigation state filter of the INS / DVL tightly coupled combination model as a basic nonlinear filter if the type of environment in which the underwater robot is located is a non-complex environment, and obtaining 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 an underwater vehicle, characterized in that: include: An inertial navigation system, a Doppler velocity measurement system and the INS / DVL tightly coupled integrated navigation device of the underwater vehicle as claimed in 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