A new SINS-assisted USBL network ranging correction method in complex underwater environments

Abstract

A new SINS-assisted USBL network ranging correction method for complex underwater environments. The present invention establishes a USBL network containing dual transponders, and uses the real-time high-precision information of SINS to correct the error caused by the motion effect on the USBL ranging information, thereby expanding the positioning range and improving the positioning accuracy. Then, a tightly integrated navigation system based on slant range and azimuth is established with SINS, and the error diverging over time of SINS is suppressed by filtering and fusion. At the same time, in order to reduce the influence of unknown noise in the underwater environment on the positioning results, a hybrid Kalman / H ∞ Filters, designing reasonable weight functions, give full play to the advantages of the two filters. In addition, fully considering the impact of current innovation and historical data on positioning results, using multi-innovation theory, can not only improve the positioning accuracy of integrated navigation, but also improve the robustness of the integrated navigation system.

Description

Technical Field

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

[0002] SINS / USBL combined navigation is a common navigation method in integrated navigation systems used in real underwater environments. The influence of AUV motion effects can cause errors in USBL ranging information. Furthermore, unknown time-varying noise in the underwater acoustic environment can also affect the final positioning results. As mission requirements continue to increase, the requirements for underwater navigation and positioning range are also increasing. Therefore, a new SINS-assisted USBL network ranging correction method is designed for complex underwater environments. 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, indispensable in both military and civilian applications. Among currently used underwater positioning technologies, the strapdown inertial navigation system (SINS) is widely used in underwater navigation due to its strong autonomy, good concealment, high short-term positioning accuracy, and good real-time performance. With the increasing demand for underwater navigation, a single navigation method no longer meets practical needs. Therefore, underwater integrated navigation and positioning technology has become a current research hotspot. Combining ultra-short baseline (USBL) and SINS navigation and positioning is one of the important methods for underwater positioning. USBL provides slant range and azimuth, correcting SINS information and preventing it from diverging over time. However, the accuracy of underwater navigation and positioning still needs to be further improved. For one thing, the slant range information of the USBL can be affected by the motion of the AUV, resulting in deviations. In order to effectively correct the errors caused by motion effects, this scheme proposes a ranging correction method for the SINS-assisted USBL network, which uses inertial navigation information to timely correct the inaccurate slant range information of the USBL, and uses 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 multi-information 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 fully utilizes current information and historical data to improve underwater positioning accuracy. Furthermore, by properly adjusting the weight function, the advantages of the Kalman filter algorithm's high initial positioning accuracy and the H∞ filter algorithm's robustness are fully utilized to effectively improve the positioning accuracy of the SINS / USBL network.

[0004] The present application differs from the prior art in the following ways:

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

[0006] Patent CN106483498A establishes a method that mitigates the impact of USBL angle measurement errors and USBL-to-SINS installation angle errors on positioning accuracy, eliminating the need for USBL-to-SINS installation angle calibration, making it more convenient to use. We have considered the impact of motion effects on USBL network ranging accuracy and proposed a ranging correction method for USBL networks with dual transponders, improving the accuracy of the SINS / USBL integrated navigation model.

[0007] Patent CN106483498A uses a USBL network with a single transponder. We use a USBL network with dual transponders that has been calibrated with 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 robots based on UKF and rolling time domain estimation"

[0009] Patent CN117804444A focuses on addressing the issue of reduced accuracy of the MHE algorithm due to system constraints and external interference with the robot. The improved adaptive UKF algorithm addresses the vulnerability of process noise Q to external interference, providing a solution and improving estimation accuracy. Our approach considers the impact of motion effects on USBL ranging and proposes a USBL network with dual transponders to improve model accuracy.

[0010] Patent CN117804444A employs an adaptive UKF algorithm to address the impact of uncertain process noise on estimation results. We have combined the strengths of both the Kalman filter and the H-infinity filter to provide a hybrid filtering algorithm suitable for underwater SINS / USBL integrated navigation systems. Incorporating multi-innovation theory, this algorithm fully considers the impact of both current innovations and historical data, further improving positioning accuracy. Summary of the Invention

[0011] In order to solve the problem of increased error due to motion effects and time-varying noise in the SINS / USBL integrated navigation system, the present invention proposes a novel SINS-assisted USBL network ranging correction method in complex underwater environments. The SINS is used to compensate and correct the USBL ranging information, and a dual-transponder SINS / USBL network based on SINS information to correct the USBL ranging information is proposed. In order to effectively deal with the impact of time-varying noise in the underwater environment, a hybrid Kalman / H-based multi-innovation method is proposed. ∞ The filter effectively suppresses 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 by the following specific process:

[0014] Step 1: First, considering 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 from the SINS, thereby suppressing the error caused by 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 equations and observation equations of the SINS / USBL tightly integrated navigation system are established. Based on the tightly integrated navigation algorithm of slant range and azimuth, integrated navigation is performed 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. ∞ 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 a query signal to the transponder through the acoustic array, and the transponder sends a 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 changes in sound speed, 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 The time of this process can be obtained by knowing the position of the transponder A. and The calculation formula is as follows:

[0025]

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

[0027]

[0028] Assuming there are k sampling intervals between the transmitter and 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 get r calculated by 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 time of reception is obtained by using the strapdown inertial navigation system data between the transmitter and the receiver. 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 can 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] A tightly coupled approach using raw range and azimuth information is employed. Range and attitude are calculated using the strapdown inertial navigation system (SINS) and the known transponder position. The difference between the information calculated by the SINS and the information measured by the SINS is used as the system's observation. This method uses the SINS to calculate direction and range given the known transponder position, and uses the difference with the USBL as the observation for the system model.

[0053] φ (n-1)n is the phase difference between the nth element and the (n-1)th element, 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 Northeastern sky geographic coordinate system is used as the navigation coordinate system to establish a SINS / USBL tightly integrated navigation system. 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 system and USBL system 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 Take it 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. The filter combines the advantages of the previous two estimators and can have high precision and good robustness at the same time. In order to realize the weight distribution 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 in the k+1th step. 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 time step interval [k-M+1,k] for J 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;

[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 upper bound 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 multiple innovations ∞ 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 nonlinear mapping principle, 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 respectively 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, b are large, d k+1 The estimated result is more inclined to the extended Kalman filter. In order to ensure the stability of the MI-KFH filter and improve the estimation accuracy, it is necessary to adjust the values ​​of parameters a and b appropriately.

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

[0078]

[0079] in express The prior estimate of express The posterior estimate of , defined as Then e 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 innovation 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), H based on multiple innovations ∞ The formula of the filtering algorithm is:

[0084]

[0085] In formula (31), the definition 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 formula (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 information m k and the filter gain vector ω k The new information vectors M(p,k) and ω(p,k) are formed, where:

[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 we can see 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 as Changes in d k It changes in the interval (0,1), thereby realizing that the weights of the MI-KFH filter can be flexibly adjusted as the 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 multi-innovation method is implemented. ∞ In the filtering process, the SINS information is first used to correct the USBL ranging information affected by the motion effect, thereby improving 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.

[0091] The advantages of the present invention over the prior art are as follows: a novel SINS-assisted USBL network distance correction scheme for underwater complex environments provided by the present application can compensate for the USBL ranging error caused by motion effects, and establish a SINS / USBL tightly integrated navigation system based on dual transponders, thereby improving positioning accuracy and expanding positioning range; in addition, the hybrid Kalman / H-based multi-innovation method proposed by the present invention can effectively improve the positioning accuracy and expand positioning range. ∞ The filter can fully utilize 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 ranging principle of a SINS / USBL network with dual transponders under the influence of motion effects;

[0094] Figure 3 The schematic diagram of the ranging correction principle of the SINS / USBL network with dual transponders is shown below.

[0095] Figure 4 This is a 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 based on this embodiment by other researchers in this field fall 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. Its working mode is as follows: Figure 1 As shown in the figure, a USBL network with dual transponders is established to perform integrated navigation with SINS. The high-precision information of SINS is used to compensate for the USBL ranging information affected by the motion effect, 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-innovation method is proposed. ∞ The filter can not only improve the positioning accuracy of the integrated navigation, but also improve 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 ranging 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 response 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 influence 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 the process. By knowing the position of the transponder A, we can get and The calculation formula is as follows:

[0103]

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

[0105]

[0106] Assuming there are k sampling intervals between the transmitter and 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 get r calculated by 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 paper 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 time of reception is obtained by using the strapdown inertial navigation system data between the transmitter and the receiver. The reception is traced back to the time of transmission. Secondly, the distance without motion influence is calculated by combining 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 can 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 a 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 in Figure 1, the range and attitude are calculated using the strapdown inertial navigation system (SINS) and the known transponder position. The difference between the information calculated by the SINS and the information measured by the SINS is used as the system observation. This method uses the SINS to calculate the direction and range given the known transponder position and uses the difference from the USBL as the observation for the system model.

[0133] Figure 4 In, φ (n-1)n is the phase difference between the nth element and the (n-1)th element, 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 Northeastern sky geographic coordinate system is used as the navigation coordinate system to establish a SINS / USBL tightly integrated navigation system. 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 system and USBL system 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 Take it 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 integrated navigation due to its advantages such as high initial positioning accuracy and the relatively simple algorithm itself. However, complex underwater environments often contain time-varying noise with unknown distribution characteristics, which will cause KF to be unable 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 integrated 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 integrated 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 distribution 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 in the k+1th step. When the system model is accurate, {r k+1} is a white noise innovation sequence, x 2,(k+1 / k) It represents the one-step prediction value of the Kalman filter state based on all the estimators 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 time step interval [k-M+1,k] for J 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 upper bound 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 nonlinear mapping principle, 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 respectively 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, b are large, d k+1 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.

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

[0159]

[0160] in express The prior estimate of express The posterior estimate of . Definition Then e k It is called the new information and is used to feedback and correct the observation bias. 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 equation (29), p ≥ 1 is the length of the new information. During the simulation, the value of p can be selected in the interval [2, 5]. A value of p that is too large will increase the complexity of the algorithm and make it difficult to operate.

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

[0165]

[0166] In formula (31), the definition 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 formula (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 Composed of 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 as Changes in d k It changes in the interval (0,1), thereby realizing that the weights of the MI-KFH filter can be flexibly adjusted as the 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, SINS information is used to correct USBL ranging information affected by motion effects, improving the ranging accuracy of the USBL network containing dual transponders. Then, a tightly integrated SINS / USBL navigation system based on slant range and azimuth is established. Furthermore, to effectively suppress the impact of time-varying noise in underwater acoustic environments on positioning accuracy, a novel hybrid Kalman / H∞ filter is proposed, incorporating multi-innovation theory. This filter maintains high positioning accuracy in complex underwater environments while ensuring good system robustness.

[0172] The above description is merely 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 may be used.

Claims

1. A novel SINS-assisted USBL network ranging correction method in complex underwater environments, characterized by: The specific process is as follows: Step 1: First, considering 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 real-time high-precision information of the SINS is used to correct the ranging information output by the USBL network, thereby suppressing the error caused by 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 equations and observation equations of the SINS / USBL tightly integrated navigation system are established. Based on the tightly integrated navigation algorithm of slant range and azimuth, integrated navigation is performed 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 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. The novel SINS-assisted USBL network ranging correction method in a complex underwater environment according to claim 1 is characterized in that: The step 1 is specifically as follows: Influence of motion effects on distance measurement In dual-transponder mode, the USBL sends a query signal to the transponder through the acoustic array, and the transponder sends a reply signal to the hydrophone. The USBL calculates the distance value based on the time between transmission and reception and the known sound speed information; Ignoring the effect of changes in sound speed, 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 The time of this process can be obtained by knowing the position of the transponder A. and The calculation formula is as follows: Where, is the position of transponder A in the n-system, is the position of the AUV in the n-frame at the time of transmission, is the position of the AUV in the n-frame at the time of reception, and Satisfy the relationship Taking into account the motion effect between the transmitter and receiver, The calculation formula is: Assuming there are k sampling intervals between the transmitter and 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 get r calculated by 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. The novel SINS-assisted USBL network ranging correction method in a complex underwater environment according to claim 2 is characterized in that: The step (2) is specifically as follows: First, the position at the time of reception is obtained by using the strapdown inertial navigation system data between the transmitter and the receiver. 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 can 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. The novel SINS-assisted USBL network ranging correction method in a complex underwater environment according to claim 2 is characterized in that: The step (3) is specifically as follows: A tightly coupled approach using raw range and azimuth information is employed. Range and attitude are calculated using the strapdown inertial navigation system (SINS) and the known transponder position. The difference between the information calculated by the SINS and the information measured by the SINS is used as the system's observation. This method uses the SINS to calculate direction and range given the known transponder position, and uses the difference with the USBL as the observation for the system model. φ (n-1)n is the phase difference between the nth element and the (n-1)th element, 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 Northeastern sky geographic coordinate system is used as the navigation coordinate system to establish a SINS / USBL tightly integrated navigation system. 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 system and USBL system 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 Take it 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. The novel SINS-assisted USBL network ranging correction method in a complex underwater environment according to claim 4 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. The filter combines the advantages of the previous two estimators and can have high precision and good robustness at the same time. In order to realize the weight distribution 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 in the k+1th step. 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 time step interval [k-M+1,k] for J 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; 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 upper bound 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. The novel SINS-assisted USBL network ranging correction method in a complex underwater environment according to claim 5 is characterized by: The step (5) is specifically as follows: A new hybrid Kalman / H based on multiple innovations ∞ filter: in: In formula (25), Represents H 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, Equation (26) converts the Kalman filter performance index Using the nonlinear mapping principle, 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 respectively k+1 Sensitivity to changes in quantitative indicators and stability of MI-KFH filter, when a, b are small, d k+1 is smaller, the estimated result is more inclined to H ∞ The estimated result of the filter; when a, b are large, d k+1 The estimated result is more inclined to the extended Kalman filter. In order to ensure the stability of the MI-KFH filter and improve the estimation accuracy, it is necessary to adjust the values ​​of parameters a and b appropriately. In formula (28), the formula of the Kalman filtering algorithm based on multiple innovations is: in express The prior estimate of express The posterior estimate of , defined as Then e 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 innovation 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 is 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), H based on multiple innovations ∞ The formula of the filtering algorithm is: In formula (31), the definition 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 formula (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 information m k and the filter gain vector ω k The new information vectors M(p,k) and ω(p,k) are formed, where: 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 filter performance 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 we can see 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 as Changes in d k It changes in the interval (0,1), thereby realizing that the weights of the MI-KFH filter can be flexibly adjusted as the 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 multi-innovation method is implemented. ∞ In the filtering process, the SINS information is first used to correct the USBL ranging information affected by the motion effect, thereby improving 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.

Images