A multi-sensor tight integration navigation method for underwater robots based on robust filtering

Abstract

The present invention discloses a multi-sensor tight integration navigation method for underwater robots based on anti-error filtering. First, a SINS-DVL-USBL tight integration model is constructed to obtain the state equation and measurement equation of the tight integration model. Second, the state update is filtered, and a one-step prediction vector and covariance matrix are calculated based on the state equation to provide a covariance matrix for the measurement update. Then, the measurement noise is estimated based on the Mahalanobis distance to provide a measurement noise matrix for the measurement update. Finally, the measurement update is filtered, and the SINS information is corrected using the DVL and USBL information to obtain the navigation result. The present invention can effectively eliminate outliers and improve the accuracy of robot navigation while reducing the amount of computation. It can also obtain high-precision position information of the underwater robot in complex environments, further improving the operating efficiency of the underwater robot.

Description

Technical Field

[0001] The invention belongs to underwater robot navigation and positioning technology, and in particular relates to an underwater robot multi-sensor tight combination navigation method based on anti-error filtering. Background Art

[0002] In order to enable underwater robots to operate smoothly underwater, it is necessary to provide accurate navigation information for the underwater robots. Currently, the most commonly used navigation method is the Strap-down Inertial Navigation System (SINS). Its advantages are that it can provide accurate position information in a short time, good autonomy and strong concealment. However, SINS also has certain disadvantages, such as the accumulation of positioning errors over time and high cost. For this reason, other navigation methods must be adopted to correct the accumulated errors of SINS and achieve high-precision integrated navigation. The Doppler log velocity (DVL) is a velocity sensor that utilizes the Doppler principle. The combined navigation method based on SINS / DVL can effectively correct the SINS error divergence and significantly improve navigation performance. However, long-duration SINS / DVL integrated navigation systems face two challenges. First, due to the inherent errors in DVL measurements, the long-duration SINS / DVL combination still suffers from error divergence. Second, due to the complex underwater environment, DVL measurements are prone to signal loss. During these periods, the system relies primarily on inertial navigation equipment, which rapidly degrades accuracy. Therefore, alternative navigation devices capable of providing absolute position information are needed. Acoustic navigation systems are a common underwater navigation method. Underwater acoustic positioning systems are typically categorized as long, short, and ultra-short baselines based on baseline length. Ultra-short baselines (USBLs) are increasingly used due to their small distance between acoustic arrays, ease of use, and ease of installation. However, the cumbersome installation and recovery processes of long and short baselines limit their widespread use. Based on the above analysis, the multi-sensor fusion method based on SINS, DVL, and USBL is the primary navigation method for underwater robots.

[0003] The present invention combines SINS, DVL, and USBL to propose an asynchronous sequential robust filtering method based on a tightly combined model. First, a tightly combined navigation model based on SINS / DVL / USBL is proposed based on the original output information of DVL and USBL. The state equation and measurement equation corresponding to the tightly combined model are established, and the state quantity is expanded to 20 dimensions by adding DVL and USBL related errors on the basis of the traditional 15 dimensions; secondly, in order to reduce the dimension of the measurement matrix and improve computational efficiency, an asynchronous sequential robust filtering method is proposed by introducing asynchronous sequential robust filtering technology. This method uses the Mahalanobis distance to construct a robust Kalman filtering algorithm and detects outliers through hypothesis testing based on statistical characteristics. Summary of the Invention

[0004] Purpose of the invention: The present invention provides a multi-sensor tight integration navigation method for underwater robots based on anti-error filtering. To address the problem of outliers in measurement information, the Mahalanobis distance is introduced on the basis of asynchronous sequential filtering to improve the robustness of the filtering.

[0005] Technical solution: The present invention provides a multi-sensor tight integration navigation method for underwater robots based on anti-differential filtering, comprising the following steps:

[0006] (1) Construct a SINS-DVL-USBL tight combination model and obtain the state equation and measurement equation of the tight combination model;

[0007] (2) Filter state update: Calculate the one-step prediction vector and covariance matrix based on the state equation to provide the covariance matrix for measurement update;

[0008] (3) Measurement noise estimation based on Mahalanobis distance to provide measurement noise matrix for measurement update;

[0009] (4) Filter measurement update: Use the information from DVL and USBL to correct the SINS information and obtain the navigation result.

[0010] Furthermore, the implementation process of step (1) is as follows:

[0011] Construct the state equation of the compact combination model:

[0012]

[0013] Among them, X tight represents the state vector; F tight represents the state transfer matrix of the compact combination model; G tight represents the system noise input matrix; W tight represents the noise vector of the compact combination model; the state vector X tight It is expressed as follows:

[0014]

[0015] in, Represent the velocity errors in the east, north and celestial directions respectively; φ E 、φ N 、φ U Indicates the pitch angle, roll angle and heading angle errors; δL, δλ, δh represent the latitude, longitude and altitude errors respectively; Respectively represent the gyro constant zero bias in three directions; Represents the zero bias of the three directions; δθ x ,δθ y ,δθ z They represent the installation error angles of the three directions of USBL respectively; δD represents the constant error of the slant distance of USBL; δK represents the scale factor error of DVL;

[0016] Tight combination model state transfer matrix F tight It is expressed as follows:

[0017]

[0018] In terms of measurement equation construction, azimuth, slant range, depth, and speed are used as measurement information, and the measurement equation is:

[0019]

[0020] Among them, Z tight represents the measurement matrix; H tight represents the measurement transfer matrix of the compact combination model; V tight represents the measurement noise vector; represents the USBL azimuth; Indicates slant distance; h aU Indicates height; Respectively represent the speed information of the four channel directions output by DVL;

[0021] The measurement transfer matrix H of the tight combination model based on SINS-DVL-USBL tight It consists of two parts: the USBL measurement transfer matrix and the DVL measurement transfer matrix, as follows:

[0022]

[0023] The measurement transfer matrix H involved in the USBL part USBL for:

[0024]

[0025] Wherein, Hh and Hn are expressed as follows:

[0026]

[0027] Where Hr = [0 0 -1] T ;

[0028] Ha, Hp, and Hs are represented as follows:

[0029]

[0030] in, Represents the direction cosine matrix from the carrier system to the acoustic array coordinate system; Represents the direction cosine matrix from the earth system to the navigation system; Indicates the projection of the arm value from the acoustic array coordinate system to the carrier coordinate system on the carrier system; Represents the direction cosine matrix from the carrier system to the navigation coordinate system It represents the relative position of the carrier position and the transponder coordinates obtained by SINS in the acoustic array coordinate system a; Represents the projection of the arm value from the acoustic array coordinate system to the carrier coordinate system in the acoustic array coordinate system;

[0031] DVL-related measurement transfer matrix H DVL :

[0032]

[0033] Wherein, Hu and Hv are represented as follows:

[0034]

[0035] in, represents the direction cosine matrix from the carrier system to the DVL equipment system; δθ represents the installation error angle; Represents the direction cosine matrix from the navigation system to the carrier system; Indicates the speed information in the navigation system obtained by SINS solution;

[0036] According to the sequential filtering idea, the measurement equation at time k is decomposed into the following 8 groups before filtering:

[0037]

[0038] in, represents the sub-measurement transfer matrix at time k; represents the sub-measurement vector at time k; represents the sub-measurement noise vector at time k.

[0039] Furthermore, the implementation process of step (2) is as follows:

[0040] State one-step prediction:

[0041]

[0042] State one-step forecast error covariance matrix:

[0043]

[0044] Among them, F k-1,tight Represents the one-step transfer matrix of the compact combination model state; represents the estimated value of the state vector; P kk-1 represents the state one-step prediction error covariance matrix; Q k-1 represents the system noise covariance matrix.

[0045] Furthermore, the implementation process of step (3) is as follows:

[0046] The information vector obtained by the measurement vector and measurement equation at time k is expressed as follows:

[0047]

[0048] Where N = 1,…,8;

[0049] The probability density function of the measurement information is:

[0050]

[0051] Where m = 8 represents the dimension of the measurement information. The formula for determining whether the measurement information contains outliers is as follows:

[0052]

[0053] Among them, M k Represents the Mahalanobis distance; if there is no outlier in the measurement information, M k The parameters should obey χ with m degrees of freedom 2 Distribution; According to the given significance level α, M k Conduct χ 2 Test, the probability of the event occurring is:

[0054]

[0055] in, Indicates the upper quantile corresponding to the significance level α; the probability of P occurring is very small. If it occurs, it is considered that the measurement information is interfered by outliers, thereby realizing the detection of outliers; when outliers appear in the measurement information, the robustness adjustment factor λ is introduced k , adjust the measurement noise covariance matrix

[0056]

[0057] The robustness factor is defined as follows:

[0058]

[0059] When there are outliers in the measurement information, the measurement noise covariance matrix is ​​adjusted through the robustness factor to improve the robustness of the system. When the measurement information is normal, the robustness factor is 1 and the normal asynchronous sequential Kalman filter update is performed.

[0060] Furthermore, the implementation process of step (4) is as follows:

[0061] Calculate the filter gain of the sub-measurement update at time k:

[0062]

[0063] in, represents the sub-measurement update filter gain at time k; represents the error covariance matrix at time k; represents the measurement noise covariance matrix;

[0064]

[0065] in, Represents the state vector of the sub-state update at time k; represents the sub-measurement vector at time k; the error covariance matrix at time k is updated as follows:

[0066]

[0067] Beneficial effects: Compared with the prior art, the present invention has the following beneficial effects: By processing the original information of DVL and USBL, the present invention designs a SINS-DVL-USBL tightly combined navigation model based on USBL azimuth, slant range and DVL four-channel speed information; in order to reduce the dimension of the measurement matrix and improve the computational efficiency, the asynchronous sequential filtering technology is introduced to propose a method based on asynchronous sequential anti-error filtering; in response to the wild value problem of measurement information, the present invention introduces the Mahalanobis distance on the basis of asynchronous sequential filtering, thereby further improving the robustness of the filtering. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 is a flow chart of the present invention;

[0069] Figure 2 Schematic diagram of asynchronous measurement update of SINS, DVL and USBL data;

[0070] Figure 3 This is the carrier simulation trajectory diagram;

[0071] Figure 4It is the position error curve diagram in the three directions of east, north and sky;

[0072] Figure 5 It is the horizontal position error curve. DETAILED DESCRIPTION

[0073] The present invention will be described in further detail below with reference to the accompanying drawings.

[0074] The present invention provides a multi-sensor tight integration navigation method for underwater robots based on anti-error filtering, such as Figure 1 As shown, the specific implementation process is as follows:

[0075] Step 1: Construct a SINS-DVL-USBL tight combination model.

[0076] The coordinate systems are defined as follows: a: acoustic array coordinate system; b: carrier coordinate system; n: navigation coordinate system; d: DVL equipment coordinate system.

[0077] The Strap-down Inertial Navigation System (SINS) outputs information including speed Position: latitude L, longitude λ, and altitude h.

[0078] The Ultra Short Base Line (USBL) device outputs two azimuth angles: Slope range information: Height information: h aU .

[0079] The Doppler log velocity (DVL) device outputs velocity information in four directions:

[0080] Construct the state equation of the compact combination model:

[0081]

[0082] Among them, X tight represents the state vector; F tight represents the state transfer matrix of the compact combination model; G tight represents the system noise input matrix; W tight represents the system noise vector.

[0083] State vector X tight It is expressed as follows:

[0084]

[0085] in, Represent the velocity errors in the east, north and celestial directions respectively; φ E 、φ N 、φ U Indicates the pitch angle, roll angle and heading angle errors; δL, δλ, δh represent the latitude, longitude and altitude errors respectively; Respectively represent the gyro constant zero bias in three directions; Represents the zero bias of the three directions; δθ x ,δθ y ,δθ z They represent the installation error angles in the three directions of USBL respectively; δD represents the constant error of the slant distance of USBL; δK represents the DVL scale factor error.

[0086] The compact combination state vector is based on the traditional 15-dimensional loose combination state vector, with the USBL installation error, slant range constant error, and DVL scale factor error added. Considering that the five added state quantities are all small and can be considered constant, their differential is 0. The compact combination model state transfer matrix F tight It can be expressed as follows:

[0087]

[0088] In terms of measurement equation construction, azimuth, slant range, depth, and speed are used as measurement information. The measurement equation can be expressed as follows:

[0089]

[0090] Among them, Z tight represents the measurement matrix; H tight represents the measurement transfer matrix of the compact combination model; V tight represents the measurement noise vector; represents the USBL azimuth; Indicates slant distance; h aU Indicates height; Respectively represent the speed information of the four channel directions output by DVL.

[0091] The measurement transfer matrix H of the tight combination model based on SINS-DVL-USBL tight It consists of two parts: USBL measurement transfer matrix and DVL measurement transfer matrix, as follows:

[0092]

[0093] The measurement transfer matrix H involved in the USBL part USBL It can be expressed as:

[0094]

[0095] Wherein, Hh and Hn are expressed as follows:

[0096]

[0097] Where Hr = [0 0 -1] T .

[0098] Ha, Hp and Hs can be expressed as follows:

[0099]

[0100] in, Represents the direction cosine matrix from the carrier system to the acoustic array coordinate system; Represents the direction cosine matrix from the earth system to the navigation system. It represents the projection of the arm value from the acoustic array coordinate system to the carrier coordinate system on the carrier system. Represents the direction cosine matrix from the carrier system to the navigation coordinate system. It represents the relative position of the carrier position and the transponder coordinates obtained by SINS in the acoustic array coordinate system a; Represents the projection of the arm value from the acoustic array coordinate system to the carrier coordinate system in the acoustic array coordinate system.

[0101] According to the above formula and state quantity, the DVL related measurement transfer matrix H can be obtained DVL :

[0102]

[0103] Wherein, Hu and Hv are represented as follows:

[0104]

[0105] in, represents the direction cosine matrix from the carrier system to the DVL equipment system; δθ represents the installation error angle; Represents the direction cosine matrix from the navigation system to the carrier system; Indicates the velocity information in the navigation system obtained by SINS solution.

[0106] According to the sequential filtering idea, the measurement equation at time k can be decomposed into the following 8 groups before filtering:

[0107]

[0108] in, represents the sub-measurement transfer matrix at time k; represents the sub-measurement vector at time k.

[0109] represents the sub-measurement noise vector at time k.

[0110] Step 2: Filter state update: Calculate the one-step prediction vector and covariance matrix based on the state equation to provide the covariance matrix for measurement update.

[0111] State one-step prediction:

[0112]

[0113] State one-step forecast error covariance matrix:

[0114]

[0115] Among them, F k-1,tight Represents the one-step transfer matrix of the compact combination model state; Represents the estimated value of the state vector. kk-1 represents the state one-step prediction error covariance matrix; Q k-1 represents the system noise covariance matrix.

[0116] Step 3: Measurement noise estimation based on Mahalanobis distance.

[0117] The innovation vector calculated above can be expressed as follows:

[0118]

[0119] Where N = 1,…,8.

[0120] The probability density function of the measurement information can be expressed as:

[0121]

[0122] Where m = 8 represents the dimension of the measurement information. Based on the above definition, the formula for determining whether the measurement information contains outliers is as follows:

[0123]

[0124] Among them, M k Represents the Mahalanobis distance; if there is no outlier in the measurement information, M k The parameters should obey χ with m degrees of freedom 2 Distribution. According to the given significance level α, M k Conduct χ 2 Test, the probability of the event occurring is:

[0125]

[0126] in Indicates the upper quantile corresponding to the significance level α; the probability of P occurring is very small. If it occurs, it is considered that the measurement information is interfered by outliers, thereby realizing the detection of outliers. When outliers appear in the measurement information, the robustness adjustment factor λ is introduced k , adjust the measurement noise covariance matrix

[0127]

[0128] The robustness factor is defined as follows:

[0129]

[0130] From the above analysis, it can be seen that when there are outliers in the measurement information, the measurement noise covariance matrix can be adjusted through the robustness factor to improve the robustness of the system; when the measurement information is normal, the robustness factor is 1 and a normal asynchronous sequential Kalman filter update is performed.

[0131] Step 4: Filter measurement update: Use the information from DVL and USBL to correct the SINS information and obtain the navigation result.

[0132] Calculate the filter gain of the sub-measurement update at time k:

[0133]

[0134] in, represents the sub-measurement update filter gain at time k. represents the error covariance matrix at time k. represents the measurement noise covariance matrix.

[0135]

[0136] in, Represents the state vector of the sub-state update at time k; represents the sub-measurement vector at time k. The error covariance matrix at time k is updated as follows:

[0137]

[0138] According to the above calculation process, the asynchronous measurement update steps of SINS, DVL and USBL data are as follows: Figure 2 As shown:

[0139] 1) First, IMU information (200Hz) is received, and the filter state is updated (state update 1);

[0140] 2) Upon receiving DVL information (2 Hz), a sequential measurement update (sequential measurement update 1) is performed.

[0141] 3) Synchronize the updated state quantity after sequential measurement in step 2 to the new state update (state update 2);

[0142] 4) USBL information is received (cycle 6s), and sequential measurement update (sequential measurement update 2) is performed at this time;

[0143] 5) Synchronize the updated state quantity of the sequential measurement in step 4) into the new state update (state update 3);

[0144] 6) Execute status update 3 and wait for new DVL data to arrive.

[0145] The simulation parameters based on the SINS-DVL-USBL integrated navigation are as follows: gyro bias: 0.01° / h; table bias: 50μg; SINS update frequency: 200Hz. USBL installation error: θ ab =[0.2 ° 0.2 ° 0.5 ° ] T ;USBL lateral error: 0.5 ° ; Timing error: 0.5ms; Data update cycle: 3s; Depth meter error: 0.5m. DVL installation error: [0.1 ° 0.1 ° 0.2 ° ] T ; DVL scale factor coefficient: 0.003; output frequency: 2Hz. According to the above simulation conditions, the carrier simulation trajectory can be obtained as follows Figure 3 As shown. Figure 3 Based on the traditional sequential Kalman filter algorithm and the method of the present invention, the SINS-DVL-USBL integrated navigation simulation verification is carried out. The results are as follows Figure 4 and Figure 5 shown. Figure 4 It is the position error curve in the three directions of east, north and celestial. Compared with the traditional sequential Kalman filter algorithm, the position error of the method of the present invention is smaller. Figure 5 is Figure 4 The horizontal position error curve calculated based on this can further illustrate that the position error of the method of the present invention is small.

Claims

1. A multi-sensor tight integration navigation method for underwater robots based on anti-error filtering, characterized in that: The following steps are involved: (1) Construct a SINS-DVL-USBL tight combination model and obtain the state equation and measurement equation of the tight combination model; (2) Filter state update: Calculate the one-step prediction vector and covariance matrix based on the state equation to provide the covariance matrix for measurement update; (3) Measurement noise estimation based on Mahalanobis distance to provide measurement noise matrix for measurement update; (4) Filter measurement update: Use the information from DVL and USBL to correct the SINS information and obtain the navigation result; The implementation process of step (4) is as follows: Calculate the filter gain of the sub-measurement update at time k: in, represents the sub-measurement update filter gain at time k; represents the error covariance matrix at time k; represents the measurement noise covariance matrix; in, Represents the state vector of the sub-state update at time k; represents the sub-measurement vector at time k; the error covariance matrix at time k is updated as follows:

2. The method for multi-sensor tight integration navigation of underwater robots based on robust filtering according to claim 1, characterized in that: The implementation process of step (1) is as follows: Construct the state equation of the compact combination model: Among them, X tight represents the state vector; F tight represents the state transfer matrix of the compact combination model; G tight represents the system noise input matrix; W tight represents the noise vector of the compact combination model; the state vector X tight It is expressed as follows: in, Represent the velocity errors in the east, north and celestial directions respectively; φ E 、φ N 、φ U Indicates the pitch angle, roll angle and heading angle errors; δL, δλ, δh represent the latitude, longitude and altitude errors respectively; Respectively represent the gyro constant zero bias in three directions; Represents the zero bias of the three directions; δθ x ,δθ y ,δθ z They represent the installation error angles of the three directions of USBL respectively; δD represents the constant error of the slant distance of USBL; δK represents the scale factor error of DVL; Tight combination model state transfer matrix F tight It is expressed as follows: In terms of measurement equation construction, azimuth, slant range, depth, and speed are used as measurement information, and the measurement equation is: Among them, Z tight represents the measurement matrix; H tight represents the measurement transfer matrix of the compact combination model; V tight represents the measurement noise vector; represents the USBL azimuth; Indicates slant distance; h aU Indicates height; Respectively represent the speed information of the four channel directions output by DVL; The measurement transfer matrix H of the tight combination model based on SINS-DVL-USBL tight It consists of two parts: the USBL measurement transfer matrix and the DVL measurement transfer matrix, as follows: The measurement transfer matrix H involved in the USBL part USBL for: Wherein, Hh and Hn are expressed as follows: Where Hr = [0 0 -1] T ; Ha, Hp, and Hs are represented as follows: in, Represents the direction cosine matrix from the carrier system to the acoustic array coordinate system; Represents the direction cosine matrix from the earth system to the navigation system; Indicates the projection of the arm value from the acoustic array coordinate system to the carrier coordinate system on the carrier system; Represents the direction cosine matrix from the carrier system to the navigation coordinate system It represents the relative position of the carrier position and the transponder coordinates obtained by SINS in the acoustic array coordinate system a; Represents the projection of the arm value from the acoustic array coordinate system to the carrier coordinate system in the acoustic array coordinate system; DVL-related measurement transfer matrix H DVL : Wherein, Hu and Hv are represented as follows: in, represents the direction cosine matrix from the carrier system to the DVL equipment system; δθ represents the installation error angle; Represents the direction cosine matrix from the navigation system to the carrier system; Indicates the speed information in the navigation system obtained by SINS solution; According to the sequential filtering idea, the measurement equation at time k is decomposed into the following 8 groups before filtering: in, represents the sub-measurement transfer matrix at time k; represents the sub-measurement vector at time k; represents the sub-measurement noise vector at time k.

3. The method for multi-sensor tight integration navigation of underwater robots based on robust filtering according to claim 1, characterized in that: The implementation process of step (2) is as follows: State one-step prediction: State one-step forecast error covariance matrix: Among them, F k-1,tight Represents the one-step transfer matrix of the compact combination model state; represents the estimated value of the state vector; P k,k-1 represents the state one-step prediction error covariance matrix; Q k-1 represents the system noise covariance matrix.

4. The method for multi-sensor tight integration navigation of underwater robots based on robust filtering according to claim 2, characterized in that: The implementation process of step (3) is as follows: The innovation vector obtained by the measurement vector and measurement equation at time k is expressed as follows: Where N = 1,…,8; The probability density function of the measurement information is: Among them, P k,k-1 represents the state one-step prediction error covariance matrix; m = 8 represents the dimension of the measurement information; the formula for determining whether the measurement information contains outliers is as follows: Among them, M k Represents the Mahalanobis distance; if there is no outlier in the measurement information, M k The parameters should obey χ with m degrees of freedom 2 Distribution; according to the given significance level α, M k Conduct χ 2 Test, the probability of the event occurring is: in, Indicates the upper quantile corresponding to the significance level α; the probability of P occurring is very small. If it occurs, it is considered that the measurement information is interfered by outliers, thereby realizing the detection of outliers; when outliers appear in the measurement information, the robustness adjustment factor λ is introduced k , adjust the measurement noise covariance matrix The robustness factor is defined as follows: When there are outliers in the measurement information, the measurement noise covariance matrix is ​​adjusted through the robustness factor to improve the robustness of the system. When the measurement information is normal, the robustness factor is 1 and the normal asynchronous sequential Kalman filter update is performed.

Images