Novel SINS-assisted USBL network ranging correction method in underwater complex environment

Abstract

The invention discloses a novel SINS (Strapdown Inertial Navigation System)-assisted USBL (Universal Serial Bus Language) network ranging correction method in an underwater complex environment, which establishes a USBL network containing double transponders, corrects errors caused by a motion effect to USBL ranging information through real-time high-precision information of an SINS, expands a positioning range and improves positioning precision. And then establishing a tightly integrated navigation system based on a slant distance and an azimuth angle with the SINS, and inhibiting an error of the SINS diverging along with time in a filtering fusion mode. Meanwhile, in order to reduce the influence of unknown noise in an underwater environment on a positioning result, a reasonable weight function is designed based on a mixed Kalman / H-infinity filter, and the advantages of the two filters are brought into full play. Besides, the influence of current information and historical data on the positioning result is fully considered, and the multi-information theory is utilized, so that the integrated navigation positioning precision can be improved, and the robustness of the integrated navigation system can be improved.

Description

Technical Field

[0001] The invention belongs to the field of underwater SINS / USBL combined navigation positioning and noise processing, and specifically relates to a novel SINS-assisted USBL network ranging correction method in a complex underwater environment. Background Art

[0002] In the combined navigation system involved in the actual underwater environment, SINS / USBL combined navigation is a common navigation method. Due to the influence of the AUV motion effect, the USBL ranging information will produce errors. In addition, the unknown time-varying noise in the underwater acoustic environment will also affect the final positioning result. With the continuous improvement of mission requirements, the requirements for the underwater navigation positioning range are also constantly increasing. Therefore, a new SINS-assisted USBL network ranging correction method is designed in a complex underwater environment. The designed USBL network is used to expand the range of acoustic positioning and improve the accuracy of underwater combined positioning.

[0003] In recent years, underwater navigation technology has become a research hotspot and is indispensable in both military and civilian fields. Among the currently commonly used underwater positioning technologies, the strapdown inertial navigation system (SINS) is widely used in the field of underwater navigation due to its strong autonomy, good concealment, high short-term positioning accuracy, and good real-time performance. With the increasing requirements for underwater navigation today, a single navigation method no longer meets the actual needs. Therefore, underwater combined navigation and positioning technology has become a current research hotspot. Combining navigation and positioning with ultra-short baseline (USBL) and SINS is one of the important methods of underwater positioning. USBL can provide slant range and azimuth, correct SINS information, and prevent SINS information from diverging over time. However, the accuracy of underwater navigation and positioning still needs to be further improved. On the one hand, under the influence of the AUV motion effect, the slant range information of USBL may be affected, resulting in deviations. In order to effectively correct the errors caused by the motion effect, this scheme proposes a ranging correction method for the SINS-assisted USBL network, which uses the inertial navigation information to timely correct the inaccurate slant range information of the USBL, and uses the dual transponders to establish a SINS / USBL network to expand the range of underwater positioning. On the other hand, the underwater environment contains time-varying noise with unknown statistical characteristics, which makes it difficult for existing algorithms to balance positioning accuracy and system robustness. Therefore, this scheme proposes a hybrid Kalman / H ∞ Filtering algorithm, on the one hand, based on the multi-innovation theory, establishes a multi-innovation hybrid Kalman / H ∞ The filter estimator makes full use of the current new information and historical data to improve the underwater positioning accuracy. On the other hand, by reasonably adjusting the weight function, the advantages of the high early positioning accuracy of the Kalman filter algorithm and the good robustness of the H∞ filter algorithm are fully utilized to effectively improve the positioning accuracy of the SINS / USBL network.

[0004] The present application is different from the prior art as follows:

[0005] Technical comparison with patent CN106483498A "A SINSUSBL tight coupling algorithm"

[0006] Patent CN106483498A establishes a method that can avoid the influence of USBL angle measurement error and USBL to SINS installation angle error on positioning accuracy, and does not need to calibrate the installation angle of USBL to SINS, which is more convenient to use. We considered the influence of motion effect on the ranging accuracy of USBL network and proposed a ranging correction method for USBL network with dual transponders, which improved the accuracy of SINS / USBL combined navigation model.

[0007] Patent CN106483498A uses a USBL network with a single transponder. We use a USBL network with dual transponders that has been corrected by SINS information, which has a wider range of action and a certain improvement in model accuracy.

[0008] Technical comparison with patent CN117804444A "Combined positioning method of underwater robot based on UKF and rolling time domain estimation"

[0009] Patent CN117804444A focuses on solving the problem of reducing the accuracy of the MHE algorithm due to system constraints and external interference on the robot, and introduces an improved adaptive UKF algorithm to provide a solution to the problem that process noise Q is easily affected by external interference, thereby improving the accuracy of the estimation. We provide a USBL network with dual transponders to improve the model accuracy based on the consideration of the influence of motion effects on USBL ranging.

[0010] In patent CN117804444A, an adaptive UKF algorithm is used to solve the impact of uncertain process noise on the estimation results. We combine the advantages of Kalman filtering and H-infinity filtering algorithms to provide a hybrid filtering algorithm that can be applied to underwater SINS / USBL combined navigation systems, and combine it with multi-information theory to fully consider the impact of current information and historical data to further improve the accuracy of positioning results. Summary of the invention

[0011] Aiming at the problem of increased error due to motion effect and time-varying noise in the SINS / USBL integrated navigation system, the present invention provides a novel SINS-assisted USBL network ranging correction method in complex underwater environments. The USBL ranging information is compensated and corrected by SINS, and a dual-transponder SINS / USBL network based on SINS information to correct USBL ranging information is proposed. In order to effectively deal with the influence of time-varying noise in the underwater environment, a hybrid Kalman / Hybrid algorithm based on multiple new information is proposed. ∞ The filter can effectively suppress the influence of time-varying noise on underwater positioning results.

[0012] To achieve the above object, the technical solution adopted by the present invention is:

[0013] A novel SINS-assisted USBL network ranging correction method in a complex underwater environment is characterized in that the specific process is as follows:

[0014] Step 1: First, aiming at the influence of motion effect on USBL network ranging information, the ranging error formula caused by AUV motion effect is derived and analyzed;

[0015] Step 2: Combined with the position information output by the inertial navigation system, a ranging error correction method for the USBL network with dual transponders is proposed. The ranging information output by the USBL network is corrected using the real-time high-precision information of SINS, thereby suppressing the error influence of the motion effect on the USBL network ranging information.

[0016] Step 3: For the USBL network that has been error corrected using SINS information, the state equation and observation equation of the SINS / USBL tightly integrated navigation system are established, and the tightly integrated navigation algorithm based on slant range and azimuth is used to perform integrated navigation using the original information of the ultra-short baseline;

[0017] Step 4: The SINS / USBL integrated navigation system works in a complex underwater environment. Its positioning accuracy and robustness are often affected by noise with unknown statistical characteristics. Combined with the high positioning accuracy of the Kalman filter and the H ∞ The advantage of the robustness of the filtering algorithm is that appropriate weights are selected to achieve joint estimation;

[0018] Step 5: In order to further improve the accuracy of the integrated navigation positioning results, a new hybrid Kalman / H ∞ The filtering algorithm realizes the fusion of SINS information and USBL information;

[0019] Step 6: After completing the above steps, perform SINS / USBL combined navigation and output the navigation results.

[0020] As a further improvement of the present invention, the step (1) is specifically as follows:

[0021] Influence of motion effects on distance measurement In dual transponder mode, USBL sends query signals to the transponder through the acoustic array, and the transponder sends the reply signal to the hydrophone. USBL calculates the distance value based on the time between transmission and reception and the known sound speed information;

[0022] Ignoring the effect of sound speed changes, the calculation formulas for the transmitting distance and receiving distance are as follows;

[0023]

[0024] in, Indicates the transmission distance between USBL and transponder A; Indicates the receiving distance between USBL and transponder A, t A is the time of this process. By knowing the position of transponder A, we can get and The calculation formula is as follows:

[0025]

[0026] In the formula, is the position of transponder A in the n-system, is the position of the AUV in the n system at the time of transmission, is the position of the AUV in the n system at the time of reception, and Satisfy the relationship Taking into account the motion effect between transmitting and receiving, The calculation formula is:

[0027]

[0028] Assuming there are k sampling intervals between the transmitter and the receiver, and the AUV moves at a constant speed between samples, the position update is expressed as:

[0029]

[0030] in is the kth sampling point, nT A s is the sampling period;

[0031] remember Then (5) can be expressed as:

[0032]

[0033] Substituting (7) into (4), the distance relationship between the transmitter and the receiver can be expressed as:

[0034]

[0035] According to formula (8), we can get t A The expression is:

[0036]

[0037] Substituting (9) into (1), we can obtain r calculated by the traditional method: 1(A) and r 2(A) The calculation formula is:

[0038] The distance error caused by the AUV motion effect can be expressed as:

[0039]

[0040] For the convenience of expression, Then formula (12) can be expressed as:

[0041]

[0042] Similarly, for transponder B, we have:

[0043]

[0044] As a further improvement of the present invention, the step (2) is specifically as follows:

[0045] First, the position at the receiving time is obtained by using the strapdown inertial navigation system data between the transmitting and receiving The reception is traced back to the time of transmission. Secondly, the distance without motion influence is calculated by the calculated position at the time of transmission and the calculated position of tracking. Finally, the receiving end distance is calculated based on the time delay and the distance calculated in the second step.

[0046] In static state, the range can be calculated as follows:

[0047]

[0048] According to formula (7), we get:

[0049]

[0050] Similarly, we can obtain r 1(A) ,r 2(A) ,r 3(A) Therefore, the above work completes the correction of USBL network distance information.

[0051] As a further improvement of the present invention, the step (3) is specifically as follows:

[0052] The method of tightly coupling the original information of distance and azimuth is adopted. The distance and attitude are calculated by the strapdown inertial navigation system and the known position of the transponder. The difference between the information calculated by the strapdown inertial navigation system and the information measured by the strapdown inertial navigation system is used as the observation value of the system. This method uses the strapdown inertial navigation system to calculate the direction and distance under the known transponder position, and takes the difference with USBL as the observation value of the system model.

[0053] φ (n-1)n is the phase difference between the nth primitive and the (n-1)th primitive, r n is the range of the nth primitive, α and β are the calculation directions, R is the calculation range, h is the calculation height, R u is the relative position, ω and f are the angular velocity and specific force of SINS respectively;

[0054] According to the analysis of the SINS error model, the Northeast Sky Geographic Coordinate System is used as the navigation coordinate system to establish the SINS / USBL tightly integrated navigation system, and its discrete state space model is:

[0055] X k =F k-1 X k-1 +w k-1 (20)

[0056] In the above formula, F k-1 represents the state one-step transfer matrix of the SINS / USBL tightly integrated navigation system; w k-1 is the system noise; and w k-1 ~N(0,Q k ), Q k is the process noise covariance; covariance X k is the state variable of the integrated navigation system, which is a 21-dimensional column vector and its expression is as follows:

[0057]

[0058] Considering that the distance and azimuth measured by SINS and USBL systems are defined separately and

[0059] The measurement equation of the SINS / USBL integrated navigation system can be expressed as:

[0060]

[0061] In the formula, the measurement vector Z k Taken as the difference in position and azimuth between the inertial navigation SINS and USBL; H k is the measurement matrix; v k is the measurement noise, which is approximately white noise, and vk Satisfy v k ~N(0,R k ), R k is the measurement noise covariance matrix.

[0062] As a further improvement of the present invention, the step (4) is specifically as follows:

[0063] Hybrid Kalman / H ∞ The filtering algorithm can flexibly adjust the filter weight between 0 and 1 as the parameters change. This filter combines the advantages of the previous two estimators and can have both high accuracy and good robustness. In order to achieve the weight allocation of hybrid filtering, the indicator J is defined k , whose expression is:

[0064]

[0065] r k+1 =Z k+1 -H k+1 x 2,(k+1 / k) (twenty three)

[0066] In formulas (22) and (23), r k+1 represents the new information of the KF filter at step k+1. When the system model is accurate, {r k+1} is a white noise innovation sequence, x 2,(k+1 / k) Represents the one-step prediction value of the Kalman filter state based on all estimates before time k;

[0067] In order to better deal with the impact of noise on the estimation results, an indicator that can reflect the Kalman filter performance in the hybrid filter is defined

[0068]

[0069] Formula (24) represents the calculation of J in the time step interval [k-M+1,k] i Sampling is performed and the average value is taken. M is an empirical value. If the value of M is too large, the evaluation memory is too long and the purpose of real-time evaluation of the Kalman filter performance index cannot be achieved. If the value of M is too small, the purpose of using time average to approximate the statistical mean cannot be achieved.

[0070] First, define two critical values: J2 and J ∞ If for any time have At this time, the Kalman filter has a very good performance, and J2 is called the supremum of the high-precision operation of the Kalman filter; if for any time have At this time, the Kalman filter performance is poor and may even diverge, so it is called J ∞ is the infimum of the low-precision operation of the Kalman filter.

[0071] As a further improvement of the present invention, the step (5) is specifically as follows:

[0072] A new hybrid Kalman / H based on multi-innovation ∞ filter:

[0073]

[0074] in:

[0075]

[0076] In formula (25), and They represent the Kalman filter based on multiple innovations and the H filter based on multiple innovations. ∞ The estimated value of the filter, represents the hybrid Kalman / H based on multiple innovations ∞ The estimated value of the filter. Formula (26) converts the Kalman filter performance index Using the principle of nonlinear mapping, it is transformed into a weight d with a value in the interval [0,1] k+1 , where parameters a and b correspond to weights d k+1 Sensitivity to changes in quantitative indicators and stability of the MI-KFH filter. When a and b are small, d k+1 is smaller, the estimated result is more inclined to H ∞ The estimated result of the filter; when a and b are large, d k+1 Large, in this case, the estimation result is more inclined to the estimation result of the extended Kalman filter. In order to ensure the stability of the MI-KFH filter and improve the estimation accuracy, it is necessary to appropriately adjust the values ​​of parameters a and b.

[0077] In formula (28), the formula of the Kalman filter algorithm based on multiple innovations is:

[0078]

[0079] in express The prior estimate of express The posterior estimate of Then k It is called the new information, which is used to feedback and correct the observation deviation. In the Kalman filter algorithm, there is only one new information e k, that is, the prediction of the state at time k only uses the state at time k-1 for estimation, so the state innovation before time k-1 will be lost. Based on the multi-innovation theory, the single innovation of the Kalman filter algorithm is expanded to multiple innovations. The innovation in the expansion formula is Generalized to the new information vector E(p,k);

[0080] E(p,k)=(e1,e2,…,e k-p+1 ) T (29)

[0081] Γ(p,k)=(Γ1,Γ2,…,Γ p ) T (30)

[0082] In formula (29), p ≥ 1 is the length of the new information. During the simulation process, the value of p can be selected in the interval [2, 5]. If the value of p is too large, the complexity of the algorithm will increase and it will be difficult to operate.

[0083] In formula (25), based on the multi-innovation H ∞ The formula of the filtering algorithm is:

[0084]

[0085] In formula (31), we define Then m k It is called the new information, which is used to feedback and correct the observation deviation. In the HIF algorithm corresponding to equation (25), there is only one new information m k , that is, the prediction of the state at time k only uses the state at time k-1 for estimation, so the state information before time k-1 will be lost, and the new information m k and the filter gain vector ω k The new information vectors M(p,k) and ω(p,k) are composed of:

[0086] M(p,k)=(m1,m2,…,m k-p+1 ) T (32)

[0087] ω(p,k)=(ω1,ω2,…,ω k-p+1 ) T (33)

[0088] Through analysis, we can know that when When the Kalman filter has better filtering performance, the weight d k is 1; when Extended Kalman Filter and H ∞ The filtering performance of the filter is average. At this time, 0<d k <1; when When the extended Kalman filter has poor performance, the weight d k =0, from the above analysis, it can be seen that the extended Kalman filter performance index With weight d k There is a corresponding relationship between them. The mathematical model shown in formula (24) can describe this corresponding relationship, so that Changes in d k It changes in the interval (0,1), thus realizing that the weights of the MI-KFH filter can be flexibly adjusted as external conditions change.

[0089] As a further improvement of the present invention, the step (6) is specifically as follows:

[0090] Using steps 1) to 5), a new hybrid Kalman / H based on multiple innovations is implemented. ∞ In the filtering process, the SINS information is first used to correct the USBL ranging information affected by the motion effect, so as to improve the ranging accuracy of the USBL network containing dual transponders, and then a SINS / USBL tightly integrated navigation system based on slant range and azimuth is established.

[0091] The advantages of the present invention over the prior art are as follows: according to the novel SINS-assisted USBL network distance correction scheme in a complex underwater environment provided by the present application, the USBL ranging error caused by the motion effect can be compensated, and a SINS / USBL tightly combined navigation system based on a dual transponder can be established to improve the positioning accuracy and expand the positioning range; in addition, the hybrid Kalman / H-based multi-information correction scheme proposed by the present invention can be used to correct the USBL distance measurement error caused by the motion effect. ∞ The filter can make full use of the current new information and historical data by flexibly adjusting the weight coefficient, giving full play to the high positioning accuracy of the KF algorithm in the early stage and / H ∞ The algorithm has the advantage of good robustness and can effectively deal with the impact of noise with unknown underwater distribution characteristics on the positioning results of the SINS / USBL integrated navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] Figure 1 This is the working mode diagram of the SINS / USBL integrated navigation system;

[0093] Figure 2 The schematic diagram of the distance measurement of the SINS / USBL network with dual transponders under the influence of motion effects;

[0094] Figure 3 The schematic diagram of the ranging correction of the SINS / USBL network with dual transponders;

[0095] Figure 4 This is the working principle diagram of the SINS / USBL tightly integrated navigation system with dual transponders. DETAILED DESCRIPTION

[0096] The following is a detailed description of the technical solution of the application in conjunction with the accompanying drawings. The described embodiments are only part of the embodiments involved in this patent. All non-innovative embodiments of other researchers in this field based on this embodiment are within the scope of protection of this patent.

[0097] The present invention provides a novel SINS-assisted USBL network ranging correction method in a complex underwater environment, and its working mode is as follows: Figure 1 As shown in the figure, a USBL network with dual transponders is established to perform combined navigation with SINS. The USBL ranging information affected by the motion effect is compensated by the high-precision information of SINS, and the error of SINS diverging over time is suppressed by filtering fusion. At the same time, in order to reduce the unknown noise in the underwater environment, a hybrid Kalman / H-based multi-information method is proposed. ∞ The filter can improve both the positioning accuracy of the integrated navigation and the robustness of the integrated navigation system. The specific steps of the implementation method are as follows:

[0098] 1) Impact of motion effects on USBL network distance measurement information

[0099] Influence of motion effects on distance measurement In dual transponder mode, USBL sends query signals to the transponder through the acoustic array, and the transponder sends the reply signal to the hydrophone. USBL calculates the distance value based on the time between transmission and reception and the known sound speed information, such as Figure 2 shown.

[0100] Ignoring the effect of changes in sound speed, the calculation formulas for the transmitting distance and receiving distance are as follows.

[0101]

[0102] in, Indicates the transmission distance between USBL and transponder A; Indicates the receiving distance between USBL and transponder A. t A is the time of this process. By knowing the position of transponder A, we can get and The calculation formula is as follows:

[0103]

[0104] In the formula, is the position of transponder A in the n-system, is the position of the AUV in the n system at the time of transmission, is the position of the AUV in the n system at the time of reception. and Satisfy the relationship Taking into account the motion effect between transmitting and receiving, The calculation formula is:

[0105]

[0106] Assuming there are k sampling intervals between the transmitter and the receiver, and the AUV moves at a constant speed between samples, the position update can be expressed as:

[0107]

[0108] in is the kth sampling point, nT A s is the sampling period.

[0109] remember Then (5) can be expressed as:

[0110]

[0111] Substituting (7) into (4), the distance relationship between the transmitter and the receiver can be expressed as:

[0112]

[0113] According to formula (8), we can get t A The expression is:

[0114]

[0115] Substituting (9) into (1), we can obtain r calculated by the traditional method: 1(A) and r 2(A) The calculation formula is:

[0116]

[0117]

[0118] The distance error caused by the AUV motion effect can be expressed as:

[0119]

[0120] For the convenience of expression, Then formula (12) can be expressed as:

[0121]

[0122] Similarly, for transponder B, we have:

[0123]

[0124] 2) SINS-assisted USBL ranging correction method

[0125] In order to reduce the influence of motion effect on USBL ranging error, this scheme proposes a USBL ranging information correction method based on the short-term high-precision characteristics of the strapdown inertial navigation system, such as Figure 3 First, the position at the receiving time is obtained by using the strapdown inertial navigation system data between the transmitting and receiving The reception is traced back to the time of transmission. Secondly, the distance without motion influence is calculated by the calculated position at the time of transmission and the calculated position at tracking. Finally, the receiving end distance is calculated based on the time delay and the distance calculated in the second step.

[0126] In static state, the range can be calculated as follows:

[0127]

[0128] According to formula (7), we get:

[0129]

[0130] Similarly, we can obtain r 1(A) ,r 2(A) ,r 3(A) Therefore, the above work completes the correction of USBL network distance information.

[0131] 3) Establishment of SINS / USBL network model with dual transponders;

[0132] This paper adopts a close coupling method of the original information of distance and azimuth, such as Figure 4 As shown. The distance and attitude are calculated by the strapdown inertial navigation system and the known transponder position. The difference between the information calculated by the strapdown inertial navigation system and the information measured by the strapdown inertial navigation system is used as the observation value of the system. This method uses the strapdown inertial navigation system to calculate the direction and distance under the known transponder position, and uses the difference with USBL as the observation value of the system model.

[0133] Figure 4 In, φ (n-1)n is the phase difference between the nth primitive and the (n-1)th primitive, r n is the range of the nth primitive, α and β are the calculation directions, R is the calculation range, h is the calculation height, R u is the relative position, ω and f are the angular velocity and specific force of SINS respectively.

[0134] According to the analysis of the SINS error model, the Northeast Sky Geographic Coordinate System is used as the navigation coordinate system to establish the SINS / USBL tightly integrated navigation system, and its discrete state space model is:

[0135] Xk =F k-1 X k-1 +w k-1 (20)

[0136] In the above formula, F k-1 represents the state one-step transfer matrix of the SINS / USBL tightly integrated navigation system; w k-1 is the system noise; and w k-1 ~N(0,Q k ), Q k is the process noise covariance; covariance X k is the state variable of the integrated navigation system, which is a 21-dimensional column vector, and its expression is as follows:

[0137]

[0138] Considering that the distance and azimuth measured by SINS and USBL systems are defined separately and

[0139] The measurement equation of the SINS / USBL integrated navigation system can be expressed as:

[0140]

[0141] In this scheme, the measurement vector Z k Taken as the difference in position and azimuth between the inertial navigation SINS and USBL; H k is the measurement matrix; v k is the measurement noise, which is approximately white noise, and v k Satisfy v k ~N(0,R k ), R k is the measurement noise covariance matrix.

[0142] 4) Hybrid Kalman / H ∞ (MI-KHF) Filtering Algorithm

[0143] The KF algorithm is widely used in the field of underwater combined navigation due to its advantages such as high initial positioning accuracy and relatively simple algorithm itself. However, complex underwater environments often contain time-varying noise with unknown distribution characteristics, which will cause KF to fail to obtain the optimal positioning results. The HIF algorithm does not need to make any assumptions about noise. In the face of time-varying noise in the underwater acoustic environment, it can still perform state estimation with high accuracy and has good robustness. However, the HIF filter does not make any assumptions about interference, so its estimation results are often too conservative, resulting in the need for further improvement in the positioning results of the SINS / USBL combined navigation system. Therefore, this scheme designs a new hybrid filter that can maintain the high initial estimation accuracy of the KF algorithm and the good robustness of the HIF algorithm, which can adapt to the combined navigation in complex underwater environments.

[0144] Hybrid Kalman / H ∞ The filtering algorithm can flexibly adjust the filter weight between 0 and 1 as the parameters change. This filter combines the advantages of the previous two estimators and can have both high accuracy and good robustness. In order to achieve the weight allocation of hybrid filtering, the indicator J is defined k , whose expression is:

[0145]

[0146] r k+1 =Z k+1 -H k+1 x 2,(k+1 / k) (twenty three)

[0147] In formulas (22) and (23), r k+1 represents the new information of the KF filter at step k+1. When the system model is accurate, {r k+1} is a white noise innovation sequence, x 2,(k+1 / k) Represents the one-step prediction of the Kalman filter state based on all estimates before time k.

[0148] In order to better deal with the impact of noise on the estimation results, an indicator that can reflect the Kalman filter performance in the hybrid filter is defined

[0149]

[0150] Formula (24) represents the calculation of J in the time step interval [k-M+1,k] i Sampling is performed and the average value is taken. M is an empirical value. If the value of M is too large, the evaluation memory is too long and the purpose of real-time evaluation of the Kalman filter performance index cannot be achieved; if the value of M is too small, the purpose of using time averaging to approximate the statistical mean cannot be achieved.

[0151] In order to better design the weight coefficients of the hybrid filter, the performance of the Kalman filter is discussed below. First, two critical values ​​are defined: J2 and J ∞ If for any time have At this time, the Kalman filter has a very good performance, and J2 is called the supremum of the high-precision operation of the Kalman filter; if for any time have At this time, the Kalman filter performance is poor and may even diverge, so it is called J ∞ is the infimum of the low-precision operation of the Kalman filter.

[0152] 5) A new hybrid Kalman / H based on multiple innovations ∞ (MI-KHF) Filter

[0153] Under the premise of fully considering the new information and historical information, this chapter proposes a new hybrid Kalman / H ∞ Filter (MI-KFH):

[0154]

[0155] in:

[0156]

[0157] In formula (25), and They represent the Kalman filter based on multiple innovations and the H filter based on multiple innovations. ∞ The estimated value of the filter, represents the hybrid Kalman / H based on multiple innovations ∞ The estimated value of the filter. Formula (26) converts the Kalman filter performance index Using the principle of nonlinear mapping, it is transformed into a weight d with a value in the interval [0,1] k+1 , where parameters a and b correspond to weights d k+1 Sensitivity to changes in quantitative indicators and stability of the MI-KFH filter. When a and b are small, d k+1 is smaller, the estimated result is more inclined to H ∞ The estimated result of the filter; when a and b are large, d k+1 The estimated result is more inclined to the estimated result of the extended Kalman filter. In order to ensure the stability of the MI-KFH filter and improve the estimation accuracy, it is necessary to properly adjust the values ​​of parameters a and b.

[0158] In formula (28), the formula of the Kalman filter algorithm based on multiple innovations is:

[0159]

[0160] in express The prior estimate of express The posterior estimate of . Definition Then k It is called the new information, which is used to feedback and correct the observation deviation. In the Kalman filter algorithm, there is only one new information e k , that is, the prediction of the state at time k only uses the state at time k-1 for estimation, so the state innovation before time k-1 will be lost. Based on the multi-innovation theory, the single innovation of the Kalman filter algorithm is expanded to multiple innovations. The innovation in the expanded formula is Generalized to the new information vector E(p,k).

[0161] E(p,k)=(e1,e2,…,e k-p+1 ) T (29)

[0162] Γ(p,k)=(Γ1,Γ2,…,Γ p ) T (30)

[0163] In formula (29), p ≥ 1 is the length of the new information. The value of p can be selected in the interval [2,5] during the simulation. If the value of p is too large, the complexity of the algorithm will increase and it will be difficult to operate.

[0164] In formula (25), based on the multi-innovation H ∞ The formula of the filtering algorithm is:

[0165]

[0166] In formula (31), we define Then m k It is called the new information and is used to feedback and correct the observation deviation. In the HIF algorithm corresponding to equation (25), there is only one new information m k , that is, the prediction of the state at time k only uses the state at time k-1 for estimation, so the state information before time k-1 will be lost. k and the filter gain vector ω k It forms the new information vector M(p,k) and ω(p,k). Among them:

[0167] M(p,k)=(m1,m2,…,m k-p+1 ) T (32)

[0168] ω(p,k)=(ω1,ω2,…,ω k-p+1 )T (33)

[0169] Through analysis, we can know that when When the Kalman filter has better filtering performance, the weight d k is 1; when Extended Kalman Filter and H ∞ The filtering performance of the filter is average. At this time, 0<d k <1; when When the extended Kalman filter has poor performance, the weight d k = 0. From the above analysis, it can be seen that the performance index of the extended Kalman filter is With weight d k There is a corresponding relationship between them. The mathematical model shown in formula (24) can describe this corresponding relationship, so that Changes in d k It changes in the interval (0,1), thus realizing that the weights of the MI-KFH filter can be flexibly adjusted as external conditions change.

[0170] 6) Combined navigation results after processing by the proposed algorithm

[0171] Using steps 1) to 5), a new hybrid Kalman / H-based multi-innovation method can be implemented. ∞ Filtering process. First, the USBL ranging information affected by the motion effect is corrected using SINS information to improve the ranging accuracy of the USBL network containing dual transponders. Then, a SINS / USBL tightly integrated navigation system based on slant range and azimuth is established. In addition, in order to effectively suppress the influence of time-varying noise in the underwater acoustic environment on the positioning accuracy, a new hybrid Kalman / H∞ filter is proposed in combination with the multi-innovation theory. This filter can maintain high-precision positioning accuracy in complex underwater environments and ensure that the system has good robustness.

[0172] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or the like made based on the technical essence of the present invention is not intended to limit the present invention in any other way.

Claims

1. A novel SINS-assisted USBL network ranging correction method in complex underwater environment, characterized by: The specific process is as follows: Step 1: First, aiming at the influence of motion effect on USBL network ranging information, the ranging error formula caused by AUV motion effect is derived and analyzed; Step 2: Combined with the position information output by the inertial navigation system, a ranging error correction method for the USBL network with dual transponders is proposed. The ranging information output by the USBL network is corrected using the real-time high-precision information of SINS, thereby suppressing the error influence of the motion effect on the USBL network ranging information. Step 3: For the USBL network that has been error corrected using SINS information, the state equation and observation equation of the SINS / USBL tightly integrated navigation system are established, and the tightly integrated navigation algorithm based on slant range and azimuth is used to perform integrated navigation using the original information of the ultra-short baseline; Step 4: The SINS / USBL integrated navigation system works in a complex underwater environment. Its positioning accuracy and robustness are often affected by noise with unknown statistical characteristics. Combined with the high positioning accuracy of the Kalman filter and the H ∞ The advantage of the robustness of the filtering algorithm is that appropriate weights are selected to achieve joint estimation; Step 5: In order to further improve the accuracy of the integrated navigation positioning results, a new hybrid Kalman / H ∞ The filtering algorithm realizes the fusion of SINS information and USBL information; Step 6: After completing the above steps, perform SINS / USBL combined navigation and output the navigation results.

2. According to claim 1, a novel SINS-assisted USBL network ranging correction method in a complex underwater environment is characterized in that: The step (1) is specifically as follows: Influence of motion effects on distance measurement In dual transponder mode, USBL sends query signals to the transponder through the acoustic array, and the transponder sends the reply signal to the hydrophone. USBL calculates the distance value based on the time between transmission and reception and the known sound speed information; Ignoring the effect of sound speed changes, the calculation formulas for the transmitting distance and receiving distance are as follows; in, Indicates the transmission distance between USBL and transponder A; Indicates the receiving distance between USBL and transponder A, t A is the time of this process. By knowing the position of transponder A, we can get and The calculation formula is as follows: In the formula, is the position of transponder A in the n-system, is the position of the AUV in the n system at the time of transmission, is the position of the AUV in the n system at the time of reception, and Satisfy the relationship Taking into account the motion effect between transmitting and receiving, The calculation formula is: Assuming there are k sampling intervals between the transmitter and the receiver, and the AUV moves at a constant speed between samples, the position update is expressed as: in is the kth sampling point, nT A s is the sampling period; remember Then (5) can be expressed as: Substituting (7) into (4), the distance relationship between the transmitter and the receiver can be expressed as: According to formula (8), we can get t A The expression is: Substituting (9) into (1), we can obtain r calculated by the traditional method: 1(A) and r 2(A) The calculation formula is: The distance error caused by the AUV motion effect can be expressed as: For the convenience of expression, Then formula (12) can be expressed as: Similarly, for transponder B, we have:

3. According to a novel SINS-assisted USBL network ranging correction method in a complex underwater environment as described in claim 2, it is characterized in that: The step (2) is specifically as follows: First, the position at the receiving time is obtained by using the strapdown inertial navigation system data between the transmitting and receiving The reception is traced back to the time of transmission. Secondly, the distance without motion influence is calculated by the calculated position at the time of transmission and the calculated position of tracking. Finally, the receiving end distance is calculated based on the time delay and the distance calculated in the second step. In static state, the range can be calculated as follows: According to formula (7), we get: Similarly, we can obtain r 1(A) ,r 2(A) ,r 3(A) Therefore, the above work completes the correction of USBL network distance information.

4. According to a novel SINS-assisted USBL network ranging correction method in a complex underwater environment as described in claim 2, it is characterized in that: The step (3) is specifically as follows: The method of tightly coupling the original information of distance and azimuth is adopted. The distance and attitude are calculated by the strapdown inertial navigation system and the known position of the transponder. The difference between the information calculated by the strapdown inertial navigation system and the information measured by the strapdown inertial navigation system is used as the observation value of the system. This method uses the strapdown inertial navigation system to calculate the direction and distance under the known transponder position, and takes the difference with USBL as the observation value of the system model. φ (n-1)n is the phase difference between the nth primitive and the (n-1)th primitive, r n is the range of the nth primitive, α and β are the calculation directions, R is the calculation range, h is the calculation height, R u is the relative position, ω and f are the angular velocity and specific force of SINS respectively; According to the analysis of the SINS error model, the Northeast Sky Geographic Coordinate System is used as the navigation coordinate system to establish the SINS / USBL tightly integrated navigation system, and its discrete state space model is: X k =F k-1 X k-1 +w k-1 (20) In the above formula, F k-1 represents the state one-step transfer matrix of the SINS / USBL tightly integrated navigation system; w k-1 is the system noise; and w k-1 ~N(0,Q k ), Q k is the process noise covariance; covariance X k is the state variable of the integrated navigation system, which is a 21-dimensional column vector and its expression is as follows: Considering that the distance and azimuth measured by SINS and USBL systems are defined separately and The measurement equation of the SINS / USBL integrated navigation system can be expressed as: In the formula, the measurement vector Z k Taken as the difference in position and azimuth between the inertial navigation SINS and USBL; H k is the measurement matrix; v k is the measurement noise, which is approximately white noise, and v k Satisfy v k ~N(0,R k ), R k is the measurement noise covariance matrix.

5. According to a novel SINS-assisted USBL network ranging correction method in a complex underwater environment as described in claim 2, it is characterized in that: The step (4) is specifically as follows: Hybrid Kalman / H ∞ The filtering algorithm can flexibly adjust the filter weight between 0 and 1 as the parameters change. This filter combines the advantages of the previous two estimators and can have both high accuracy and good robustness. In order to achieve the weight allocation of hybrid filtering, the indicator J is defined k , whose expression is: r k+1 =Z k+1 -H k+1 x 2, ( k+1 / k ) (23) In formulas (22) and (23), r k+1 represents the new information of the KF filter at step k+1. When the system model is accurate, {r k+1 } is a white noise innovation sequence, x 2,(k+1 / k) Represents the one-step prediction value of the Kalman filter state based on all estimates before time k; In order to better deal with the impact of noise on the estimation results, an indicator that can reflect the Kalman filter performance in the hybrid filter is defined Formula (24) represents the calculation of J in the time step interval [k-M+1,k] i Sampling is performed and the average value is taken. M is an empirical value. If the value of M is too large, the evaluation memory is too long and the purpose of real-time evaluation of the Kalman filter performance index cannot be achieved. If the value of M is too small, the purpose of using time average to approximate the statistical mean cannot be achieved. First, define two critical values: J2 and J ∞ If for any time have At this time, the Kalman filter has a very good performance, and J2 is called the supremum of the high-precision operation of the Kalman filter; if for any time have At this time, the Kalman filter performance is poor and may even diverge, so it is called J ∞ is the infimum of the low-precision operation of the Kalman filter.

6. According to a novel SINS-assisted USBL network ranging correction method in a complex underwater environment as described in claim 2, it is characterized by: The step (5) is specifically as follows: A new hybrid Kalman / H based on multi-innovation ∞ filter: in: In formula (25), and They represent the Kalman filter based on multiple innovations and the H filter based on multiple innovations. ∞ The estimated value of the filter, represents the hybrid Kalman / H based on multiple innovations ∞ The estimated value of the filter. Formula (26) converts the Kalman filter performance index Using the principle of nonlinear mapping, it is transformed into a weight d with a value in the interval [0,1] k+1 , where parameters a and b correspond to weights d k+1 Sensitivity to changes in quantitative indicators and stability of the MI-KFH filter. When a and b are small, d k+1 is smaller, the estimated result is more inclined to H ∞ The estimated result of the filter; when a and b are large, d k+1 Large, in this case, the estimation result is more inclined to the estimation result of the extended Kalman filter. In order to ensure the stability of the MI-KFH filter and improve the estimation accuracy, it is necessary to appropriately adjust the values ​​of parameters a and b. In formula (28), the formula of the Kalman filter algorithm based on multiple innovations is: in express The prior estimate of express The posterior estimate of Then k It is called the new information, which is used to feedback and correct the observation deviation. In the Kalman filter algorithm, there is only one new information e k , that is, the prediction of the state at time k only uses the state at time k-1 for estimation, so the state innovation before time k-1 will be lost. Based on the multi-innovation theory, the single innovation of the Kalman filter algorithm is expanded to multiple innovations. The innovation in the expansion formula is Generalized to the new information vector E(p,k); E(p,k)=(e1,e2,…,e k-p+1 ) T (29) Γ(p,k)=(Γ1,Γ2,…,Γ p ) T (30) In formula (29), p ≥ 1 is the length of the new information. During the simulation process, the value of p can be selected in the interval [2, 5]. If the value of p is too large, the complexity of the algorithm will increase and it will be difficult to operate. In formula (25), based on the multi-innovation H ∞ The formula of the filtering algorithm is: In formula (31), we define Then m k It is called the new information, which is used to feedback and correct the observation deviation. In the HIF algorithm corresponding to equation (25), there is only one new information m k , that is, the prediction of the state at time k only uses the state at time k-1 for estimation, so the state information before time k-1 will be lost, and the new information m k and the filter gain vector ω k The new information vectors M(p,k) and ω(p,k) are composed of: M(p,k)=(m1,m2,…,m k-p+1 ) T (32) ω(p,k)=(ω1,ω2,…,ω k-p+1 ) T (33) Through analysis, we can know that when When the Kalman filter has better filtering performance, the weight d k is 1; when Extended Kalman Filter and H ∞ The filtering performance of the filter is average. At this time, 0<d k <1; when When the extended Kalman filter has poor performance, the weight d k =0, from the above analysis, it can be seen that the extended Kalman filter performance index With weight d k There is a corresponding relationship between them. The mathematical model shown in formula (24) can describe this corresponding relationship, so that Changes in d k It changes in the interval (0,1), thus realizing that the weights of the MI-KFH filter can be flexibly adjusted as external conditions change.

7. The novel SINS-assisted USBL network ranging correction method in a complex underwater environment according to claim 2 is characterized in that: The step (6) is specifically as follows: Using steps 1) to 5), a new hybrid Kalman / H based on multiple innovations is implemented. ∞ In the filtering process, the SINS information is first used to correct the USBL ranging information affected by the motion effect, so as to improve the ranging accuracy of the USBL network containing dual transponders, and then a SINS / USBL tightly integrated navigation system based on slant range and azimuth is established.

Images