Overwater combined inertial navigation positioning method based on adaptive interactive multiple models

Abstract

The invention discloses an overwater combined inertial navigation positioning method based on self-adaptive interactive multi-model, which comprises the following steps: S1, establishing SINS / DVL interactive multi-model according to an interactive multi-model algorithm; s2, according to the SINS / DVL interaction multi-model, firstly inputting interaction, then carrying out model filtering, then carrying out model probability updating and finally carrying out hybrid estimation, and taking an estimation result as combined navigation output; s3, feeding back an error term of updating feedback filtering calculation to inertial navigation; through the interactive multi-model algorithm, rapid adaptation can be achieved when the system state changes, the switching probability of each filtering model is adjusted in a self-adaptive mode, the matching precision between the SINS and the DVL is improved, and the challenges of DVL measurement abnormity and SINS noise fluctuation can be effectively coped with; meanwhile, the AIMM algorithm of the adaptive transition probability is adopted, and the optimal filtering model can be recognized in time in a fast changing environment, so that divergence of system errors is effectively inhibited, and the reliability and precision of a navigation result are improved.

Description

Technical Field

[0001] This invention belongs to the field of integrated inertial navigation and positioning technology, specifically relating to a method for integrated inertial navigation and positioning on water based on an adaptive interactive multi-model approach. Background Technology

[0002] Strap-down Inertial Navigation Systems (SINS) provide attitude, velocity, and position information for a vehicle through measurements from integrating gyroscopes and accelerometers; however, their errors tend to diverge over time. Doppler Velocity Logs (DVLs), which measure the velocity of a vehicle relative to the seabed or water body using the Doppler effect, can effectively suppress inertial navigation error divergence when used in conjunction with SINS. In underwater environments, to achieve high-precision, long-endurance navigation for submersible vehicles, it is necessary to suppress and correct inertial navigation errors using a combined SINS / DVL navigation technology.

[0003] Traditional SINS / DVL combined algorithms often employ linear or extended Kalman filters, assuming stable noise statistical characteristics. However, due to the uncertainty of the vehicle's maneuvering state and the inherent tendency for jumps or failures in DVL measurements, conventional SINS / DVL combined navigation algorithms generally have lower reliability and positioning accuracy compared to commonly used ground-based SINS / GNSS combined navigation algorithms. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention proposes a combined inertial navigation and positioning method for waterborne applications based on an adaptive interactive multi-model approach. This invention utilizes an interactive multi-model approach, which allows for the parallel operation of multiple filtering models and their probabilistic weighted fusion. The model contribution is adaptively adjusted based on the current measurement, effectively improving the robustness of the system during model transitions or sudden noise changes. Simultaneously, an adaptive probability switching method is adopted, enabling the algorithm to quickly adapt to highly maneuverable application scenarios and improve positioning accuracy.

[0005] The technical solution adopted in this invention is as follows: a method for combined inertial navigation and positioning on water based on adaptive interactive multi-model is provided, comprising the following steps: S1, establishing a SINS / DVL interactive multi-model according to the interactive multi-model algorithm; S2, based on the SINS / DVL interactive multi-model, first inputting the interaction, then performing model filtering, then performing model probability update, and finally performing hybrid estimation, and using the estimation result as the combined navigation output; S3, feeding back the error term calculated by the update feedback filter to the inertial navigation system.

[0006] Preferably, the S1 step of establishing the SINS / DVL interactive multiple model includes the following steps: a) setting the system state equation and measurement equation as follows:

[0007]

[0008] b. Assuming the models are based on transition probabilities π, their Markov switching process is as follows:

[0009]

[0010] Where π i→i and π j→j π represents the probability that the two models will remain functional in the next time step. i→j and π j→i This represents the probability that two models will switch to the other model at the next time step.

[0011] Preferably, the input interaction in step S2 is based on the flowchart of the multi-model interaction algorithm, and the following equations are proposed first:

[0012]

[0013] in Let be the probability of the SINS / DVL model at time t-1. As a normalization factor, it ensures that the sum of the probabilities of all models is 1 when calculating the mixture probability; then, the mixture probability is used to weight the means of each model to form a mixture estimate, including the state mean and covariance, as shown in the following equation:

[0014]

[0015] Where m is the SINS / DVL model, at this time and Here are the updated mean and covariance of the SINS / DVL model at time t-1.

[0016] Preferably, in step S2, the model filtering takes the SINS / DVL model as an example, and the filter is selected as Kalman filter. The prediction and update process incorporates the current model's mixture mean and covariance as follows:

[0017]

[0018] Preferably, the model probability algorithm in step S2 is as follows: The measurement likelihood of each filter is calculated using the following equation:

[0019]

[0020] in, Indicates measurement residuals, Model M i Update covariance;

[0021] At this point, the probability formula for the SINS / DVL model at time t is as follows:

[0022]

[0023] Where c is the normalization factor.

[0024] Preferably, the hybrid estimation algorithm in step S2 is as follows: In the final stage of the algorithm at the current time step, the fused estimate of the state mean and covariance is calculated, and the filtering results of each filter are fused according to the updated model probability, as shown in the following formula:

[0025]

[0026] According to Equation 3, the system's transition probability matrix consists of four Markov transition probabilities, as shown in the following formula:

[0027]

[0028] The Markov transition probability π at time t from SINS-assisted measurement to DVL velocimetry. h→r for

[0029]

[0030] in This represents the assumption that the SINS-assisted approach matches the true model at time t. This represents the predicted value of SINS update at time t; according to Bayes' theorem, Formula 23 can be further decomposed into...

[0031]

[0032] Among them, residual Directly from observations Decide, Residual The conditional probability; at this time, A multidimensional Gaussian distribution can be used to approximate this distribution, and the approximation formula is as follows:

[0033]

[0034] When calculating At that time, cross residuals yes A special case representing a shift in the system from relying on SINS information to placing greater trust in DVL prediction information; and the corresponding residual covariance It is through the posterior state estimation at time t. and posterior state covariance Perform calculations

[0035]

[0036]

[0037] Residual at the current moment when SINS is updated Includes cross residuals and self-harm difference The formula for calculating the self-residual is as follows: (Two parts)

[0038]

[0039] Joint probability This indicates that residual information is not considered. The transition probability from relying on SINS information to relying on DVL velocity measurement information at time t-1; at this time Independent of observations The Markov transition probability at time t-1 is equal to:

[0040]

[0041] Joint probability Represents estimated residual information and decision The joint probability, It is directly determined by the model probability at time t, so Decisions involving the selection of SINS predictive information And the decision to select DVL speed measurement information

[0042]

[0043] Substituting formulas 25, 29, and 30 into formula 24, the transition probability can be calculated. for:

[0044]

[0045] At this point, the likelihood function The formulas are obtained through formulas 25 and 27, as follows:

[0046]

[0047] Where n is the cross residual dimensionality The normalization constant can be calculated using the following formula;

[0048]

[0049] Likelihood function With likelihood function The calculation is similar, but the covariance of the relevant residuals needs to be modified accordingly, as shown in the following formula:

[0050]

[0051] Using Equation 31, the Markov transition probabilities of different models can be updated adaptively.

[0052] The beneficial effects of this invention are as follows:

[0053] This invention utilizes an interactive multi-model algorithm to quickly adapt to changes in system state and adaptively adjust the switching probabilities of various filter models. This not only improves the matching accuracy between SINS and DVL, but also effectively addresses the challenges of DVL measurement anomalies and SINS noise fluctuations. Simultaneously, the AIMM algorithm with adaptive transition probabilities can promptly identify the optimal filter model in rapidly changing environments, thereby effectively suppressing the divergence of system errors and improving the reliability and accuracy of navigation results. Detailed Implementation

[0054] Unless otherwise defined, the technical or scientific terms used in this patent document shall have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this patent specification and claims do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Similarly, the terms "an," "a," or "the" do not indicate a quantity limitation, but rather indicate the presence of at least one. The terms "comprising" or "including" indicate that the element or object preceding "comprising" encompasses the element or object listed following "comprising" or its equivalents, and do not exclude other elements or objects. Terms such as "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer" are used only to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly. These terms are only for the convenience of describing the invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention.

[0055] Some embodiments of the present invention will be described in detail below. Unless otherwise specified, features in the following embodiments can be combined with each other.

[0056] Specifically, a method for combined inertial navigation and positioning on water based on adaptive interactive multi-model is provided, including the following steps:

[0057] S1, in the SINS / DVL interactive multi-model algorithm, the system is set as a Markov chain, so as to realize the switching between different models in the SINS / DVL interactive multi-model algorithm within different time steps, and re-initialize the filter input in each filtering cycle to realize the interactive fusion between different models.

[0058] Suppose the system state equation and measurement equation are as follows:

[0059]

[0060] The SINS / DVL interactive multi-model algorithm assumes that the models involved in the interaction can describe all states of the system's motion. Taking the implementation of this algorithm with two models as an example, assuming that the models are based on transition probabilities π and a Markov switching process, the implementation formula is as follows:

[0061]

[0062] Where π i→i and π j→j π represents the probability that the two models will remain functional in the next time step. i→j and π j→i This represents the probability that two models will switch to the other model at the next time step.

[0063] In practice, it is generally preferred that the system can still be described using the model from the previous time step in the next time step, hence π i→i and π j→i Generally, the corresponding probability value is relatively large, while π i→j and π j→i Generally, the values ​​are relatively small. After processing the measurement information, the transition probabilities between models are also updated.

[0064] S2, based on the established SINS / DVL interactive multi-model algorithm, perform the following operations:

[0065] (1) Input interaction

[0066] For SINS / DVL interactive multi-model, the mixture probability It can be expressed as the product of the model probability at the previous time step and the model transition probability, as shown in the following formula:

[0067]

[0068]

[0069] in Let SINS / DVL interactive multiple model be the probability at time t-1. This is a normalization factor that ensures that the sum of the probabilities of all models is 1 when calculating the mixture probability.

[0070] A mixture estimate is formed by weighting the means of each model using the mixture probability, which includes the state mean and covariance, as shown in the following formula:

[0071]

[0072] at this time and Model M at time t-1 i The updated mean and covariance.

[0073] (2) Model Filtering

[0074] After each model obtains its own mixed input, each model implements the model filtering process using its corresponding filter. Taking the SINS / DVL interactive multi-model as an example, the filter selected is the Kalman filter, and its prediction and update process substitutes the current model's mixed state mean and covariance into the following formula:

[0075]

[0076] (3) Model probability update

[0077] The measured likelihood of each filter is calculated using the following formula:

[0078]

[0079] in, Indicates measurement residuals, Model M i The updated covariance. Then, the probabilities of each SINS / DVL interactive multi-model at time t are obtained, calculated using the following formula:

[0080]

[0081] Where c is the normalization factor.

[0082] Updated The probability of switching is π. ij The maximum posterior probability of the sum of quantities.

[0083] (4) Hybrid estimation

[0084] In the final stage of the algorithm at the current time step, the fused estimate of the state mean and covariance is calculated, and the filtering results of each filter are fused according to the updated model probability, as shown in the following formula:

[0085]

[0086] Traditional Interacting Multiple Model (IMM) algorithms use a constant transition probability matrix. Since each iteration requires weight calculation of each model using the transition probability matrix, a fixed transition probability matrix simplifies the calculation process. In relatively stable system environments, this fixed transition probability matrix can meet the low-frequency switching requirements between system models, and the benefit of forcibly adjusting the transition probability at each time step is relatively low. However, when dealing with complex and rapidly changing environments, traditional IMM algorithms may result in inflexible model switching. The system also requires more time to identify and switch to the appropriate filter model, thus reducing the overall robustness of the system. With the increasing demand for multi-sensor collaborative systems in complex environments, IMM based on adaptive transition probabilities (AIMM) can better address the robustness and positioning accuracy requirements of systems in complex dynamic environments. Therefore, this paper chooses to use the AIMM algorithm to optimize the algorithm and achieve rapid response in highly maneuverable scenarios.

[0087] According to Equation 3, the system's transition probability matrix consists of four Markov transition probabilities, as shown in the following formula:

[0088]

[0089] The Markov transition probability π at time t from SINS-assisted measurement to DVL velocimetry. h→r for

[0090]

[0091] in This represents the assumption that the SINS-assisted approach matches the true model at time t. Let represent the predicted value of SINS update at time t. According to Bayes' theorem, equation (4.23) can be further decomposed into:

[0092]

[0093] residual Directly from observations Decide.

[0094] Residual The conditional probability, if a biased estimate was successfully eliminated after filtering by DVL speed measurement information during the combination process, and combined with the short-term update prediction of SINS, then... It can be approximated by a multidimensional Gaussian distribution:

[0095]

[0096] When calculating At that time, cross residuals yes A special case represents a shift in the system from relying on SINS information to placing greater trust in DVL prediction information. and the corresponding residual covariance It is through the posterior state estimation at time t. and posterior state covariance Perform calculations

[0097]

[0098] Residual at the current moment when SINS is updated Includes cross residuals and self-harm difference The two parts, the formula for calculating the self-residual is shown in equation (4.28).

[0099]

[0100] Cross residuals The probability that the system shifts from relying on SINS information to relying more on DVL prediction information. Self-harm The corresponding system maintains reliance on SINS update information and does not switch to other models. The residuals of the model probability update step in the interactive multi-model process are innovation vectors calculated based on the prior state of the current model itself, while the ones mentioned here... Updated via SINS (corresponding residual) ) and DVL velocimetry (corresponding residual) The proposed Markov transition probability prediction is calculated from the posterior information of the sub-filter and is based on the interaction between the post-processing of the sub-filter and multiple models.

[0101] Joint probability This indicates that residual information is not considered. The transition probability from relying on SINS information to relying on DVL velocities at time t-1. Independent of observations Equal to the Markov transition probability at time t-1

[0102]

[0103] Joint probability Represents estimated residual information and decision The joint probability, It is directly determined by the model probability at time t, so Decisions involving the selection of SINS predictive information And the decision to select DVL speed measurement information

[0104]

[0105] Substituting formulas 25, 29, and 30 into formula 24, the transition probability can be calculated. for

[0106]

[0107] At this point, the likelihood function The formulas are obtained through formulas 25 and 27, as follows:

[0108]

[0109] Where n is the cross residual dimensionality The normalization constant can be calculated using formula 33.

[0110]

[0111] Likelihood function With likelihood function The calculation is similar, but the covariance of the relevant residuals needs to be modified accordingly. The modified formula is as follows:

[0112]

[0113] Using Equation 31, the Markov transition probabilities of different models can be updated adaptively. Discussion of the likelihood function. If, for time t, the DVL velocimetry model is closer to the actual motion of the system than the SINS prediction, then the cross residual determined by Equation 26... The covariance corresponding to the cross residuals calculated by Formula 27 The determined Gaussian distribution is closer to the standard multidimensional Gaussian distribution than the SINS prediction model, thus making... The probability of transitioning to DVL prediction at time t along with As the value increases, the interactive multi-model can effectively switch from relying on SINS prediction information to relying on DVL information, thereby achieving estimation of the current state.

[0114] S3. After obtaining the current positioning information, the system continues to provide feedback on the error term to suppress the divergence of SINS error.

[0115] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or basic characteristics. Therefore, the embodiments should be considered exemplary and non-limiting in all respects. The scope of the invention is defined by the appended claims rather than the foregoing and description, and thus all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention.

Claims

1. A method for combined inertial navigation and positioning on water based on adaptive interactive multi-model, characterized in that: Includes the following steps: S1. Based on the interactive multi-model algorithm, establish the SINS / DVL interactive multi-model; S2, based on the SINS / DVL interactive multi-model, first inputs the interaction, then performs model filtering, then performs model probability update, and finally performs hybrid estimation, and uses the estimation result as the integrated navigation output; S3 updates the error term calculated by the feedback filter and feeds it back into the inertial navigation system.

2. The method for combined inertial navigation and positioning on water based on adaptive interactive multi-model as described in claim 1, characterized in that: The S1 step of establishing the SINS / DVL interactive multi-model includes the following steps: a. The system state equation and measurement equation are defined as follows: b. Assuming the models are based on transition probabilities π, their Markov switching process is as follows: Where π i→i and π j→j π represents the probability that the two models will remain functional in the next time step. i→j and π j→i This represents the probability that two models will switch to the other model at the next time step.

3. The method for combined inertial navigation and positioning on water based on adaptive interactive multi-model as described in claim 1, characterized in that: The input interaction in step S2 is based on the flowchart of the multi-model interaction algorithm. The following equations are proposed first: in Let be the probability of the SINS / DVL model at time t-1. As a normalization factor, it ensures that when calculating the mixture probability, the sum of the probabilities of all models is 1; The mixture probability is then used to weight the means of each model to form a mixture estimate, which includes the state mean and covariance, as shown in the following equation: Where m is the SINS / DVL model, at this time and Here are the updated mean and covariance of the SINS / DVL model at time t-1.

4. The method for combined inertial navigation and positioning on water based on adaptive interactive multi-model as described in claim 1, characterized in that: In step S2, the model filtering takes the SINS / DVL model as an example, and the filter selected is the Kalman filter. The prediction and update process incorporates the current model's mixture mean and covariance as follows:

5. The method for combined inertial navigation and positioning on water based on adaptive interactive multi-model as described in claim 1, characterized in that: The model probability algorithm in step S2 is as follows: The update calculates the measured likelihood for each filter using the following equation: in, Indicates measurement residuals, Model M i Update covariance; At this point, the probability formula for the SINS / DVL model at time t is as follows: Where c is the normalization factor.

6. The method for combined inertial navigation and positioning on water based on adaptive interactive multi-model as described in claim 1, characterized in that: The hybrid estimation algorithm in step S2 is as follows: In the final stage of the algorithm at the current time step, the fused estimate of the state mean and covariance is calculated, and the filtering results of each filter are fused according to the updated model probability, as shown in the following formula: According to Equation 3, the system's transition probability matrix consists of four Markov transition probabilities, as shown in the following formula: The Markov transition probability π at time t from SINS-assisted measurement to DVL velocimetry. h→r for in This represents the assumption that the SINS-assisted approach matches the true model at time t. This represents the predicted value of SINS update at time t; According to Bayes' theorem, formula 23 can be further decomposed into: Among them, residual Directly from observations Decide, Residual The conditional probability; at this time, A multidimensional Gaussian distribution can be used to approximate this distribution, and the approximation formula is as follows: When calculating At that time, cross residuals yes A special case representing a shift in the system from relying on SINS information to placing greater trust in DVL prediction information; and the corresponding residual covariance It is through the posterior state estimation at time t. and posterior state covariance Perform calculations Residual at the current moment when SINS is updated Includes cross residuals and self-harm difference The formula for calculating the self-residual is as follows: (Two parts) Joint probability This indicates that residual information is not considered. The transition probability of the system shifting from relying on SINS information to relying on DVL velocity measurement information at time t-1; at this time Independent of observations The Markov transition probability at time t-1 is equal to: Joint probability Represents estimated residual information and decision The joint probability, It is directly determined by the model probability at time t, so Decisions involving the selection of SINS predictive information And the decision to select DVL speed measurement information Substituting formulas 25, 29, and 30 into formula 24, the transition probability can be calculated. for: At this point, the likelihood function The formulas are obtained through formulas 25 and 27, as follows: Where n is the cross residual dimensionality The normalization constant can be calculated using the following formula; Likelihood function With likelihood function The calculation is similar, but the covariance of the relevant residuals needs to be modified accordingly, as shown in the following formula: Using Equation 31, the Markov transition probabilities of different models can be updated adaptively.

Images