Multi-navigation-state unmanned system underwater integrated navigation method

Abstract

The invention belongs to the technical field of underwater integrated navigation. The invention provides a multi-navigation-state unmanned system underwater integrated navigation method based on global information fusion. According to the embodiment of the invention, different navigation combination modes under different navigation states are selected, and an SINS / DVL / GPS combined navigation mode is adopted offshore, so that the calculated amount is reduced, and the efficiency is improved; and the deep sea sneaking adopts an SINS / DVL / USBL combined navigation mode. Measurement deviation accumulative divergence caused by navigation state change in maneuvering characteristics of the underwater unmanned system is considered, t distribution is introduced in a federated Kalman filtering architecture for trailing characteristics to replace conventional normal distribution, thick-tail measurement noise is modeled, approximate solution of a posterior probability density function is carried out by using a variational Bayesian theory, and the probability density of the underwater unmanned system is calculated. Accurate estimation of the motion state of the unmanned system is realized, and updating and data fusion of a plurality of sub-filters are realized by using a federal structure.

Description

Technical Field

[0001] The disclosed embodiments relate to the technical field of underwater integrated navigation, and in particular to an underwater integrated navigation method for a multi-state unmanned system. Background Art

[0002] As natural resources become increasingly scarce, the development of marine resources has become an important strategic goal. Autonomous Underwater Vehicle (AUV), as a key device that can operate autonomously underwater, plays an increasingly important role in the fields of ocean exploration, resource exploration, underwater archaeology and military applications. In order to meet the needs of long-term, high-precision, and multi-state underwater navigation, it is particularly important to develop an efficient and reliable navigation system; Traditional underwater navigation systems mostly use a single navigation device, such as the Inertial Navigation System (INS), the Global Positioning System (GPS), etc. However, a single navigation system has certain limitations in practical applications. For example, although the inertial navigation system has high accuracy, the error will accumulate over time and cannot provide long-term effective navigation. The GPS system cannot achieve effective positioning in an underwater environment due to limited signals. Therefore, a single navigation system cannot meet the navigation needs of underwater unmanned systems. In order to solve this limitation, most current studies integrate the information of multiple sensors (such as inertial navigation systems, Doppler speed meters, depth gauges, altimeters, GPS, etc.) through data fusion, and use data fusion algorithms such as Kalman filtering and extended Kalman filtering to improve the accuracy and stability of the navigation system and enhance the fault tolerance and robustness of the system. The traditional combined navigation method of underwater unmanned systems has the problem that the underwater combined navigation accuracy of unmanned system operations will be affected by the navigation state: the underwater combined navigation method is usually composed of a variety of navigation technologies, such as inertial navigation system, geomagnetic auxiliary navigation, etc. The application of these combinations can improve the accuracy and reliability of navigation; however, under different navigation states, the performance of the navigation combination components will be different, thus affecting the navigation accuracy; when sailing at high speed near the coast, the error of the inertial navigation system will accumulate faster, while auxiliary navigation can correct this error to a certain extent and improve navigation accuracy; in deep-sea low-speed diving or relative static state, the role of these auxiliary navigation technologies is more obvious and can provide more accurate positioning information; it can be seen that changes in navigation state will affect the weight and role of each component in the combined navigation method, thereby affecting the overall navigation accuracy; therefore, it is extremely important to ensure that certain technical means are in place to cope with the navigation needs of underwater unmanned systems under different navigation states, so as to better adapt to various navigation states and improve the stability and accuracy of combined navigation.

[0003] Therefore, it is necessary to improve one or more problems existing in the above-mentioned related technical solutions.

[0004] It should be noted that this section is intended to provide background or context for the technical solutions of the present disclosure stated in the claims. The description herein is not admitted to be prior art by virtue of being included in this section. Summary of the invention

[0005] The purpose of the embodiments of the present disclosure is to provide a method for underwater integrated navigation of a multi-state unmanned system, thereby overcoming one or more problems caused by the limitations and defects of related technologies at least to a certain extent.

[0006] According to an embodiment of the present disclosure, a multi-state unmanned system underwater integrated navigation method is provided, the method comprising: Construct the basic inertial navigation kinematic model based on the inertial navigation attitude equation, inertial navigation velocity equation and inertial navigation position equation; Set up the navigation system, combine the basic inertial navigation kinematic model, and obtain the state equation and observation equation of each navigation system; the navigation system includes SINS navigation, DVL navigation, USBL navigation and GPS navigation; Set the unmanned system state and its corresponding combined navigation mode; the unmanned system state includes deep-water stealth state and shallow-water operation state, and the combined navigation mode includes SINS / DVL / USBL combined navigation and SINS / DVL / GPS combined navigation. The SINS / DVL / USBL combined navigation is used in the deep-water stealth state, and the SINS / DVL / GPS combined navigation is used in the shallow-water operation state; Based on the state equations, observation equations, unmanned system navigation states and their corresponding combined navigation methods of each navigation system, SINS / DVL sub-filter, SINS / USBL sub-filter and SINS / GPS sub-filter are constructed; among them, SINS / DVL sub-filter and SINS / USBL sub-filter are used in deep-water submersion state, and SINS / DVL sub-filter and SINS / GPS sub-filter are used in shallow-water operation state; Based on the weight fusion method, the main filter is constructed, combining the SINS / DVL sub-filter, SINS / USBL sub-filter and SINS / GPS sub-filter, and the t-distribution function is introduced to construct the t-distribution federated filter; Obtain system information and process it using a t-distributed federated filter to update the posture, velocity, and position of the unmanned system.

[0007] Furthermore, select " " As the navigation coordinate system, the expression of the inertial navigation attitude equation in the basic inertial navigation kinematic model is:

[0008] in, is the attitude matrix from the carrier system to the navigation coordinate system, for The differential of is the attitude matrix from the navigation coordinate system to the carrier system, for Relative to Tie The angular velocity of the projection, for The component on the X-axis, for The component on the Y axis, for The component on the Z axis, for The antisymmetric matrix of is the quaternion updated by the rotation vector method when the attitude is updated, is the first plural number, is the second plural number, is the third plural, is the fourth plural number, for The differential of for The differential of for The differential of for The differential of The expression of the inertial navigation velocity equation is:

[0009] in, is the velocity of the carrier in the navigation coordinate system, is the north component of velocity, is the eastward component of velocity, is the vertical component of velocity, for The differential of is the gravitational acceleration vector, is the strapdown matrix, For accelerometer The speed change along the X axis, For accelerometer The speed change along the Y axis, For accelerometer The speed change along the Z axis, is the sampling period, for The constant bias of the accelerometer is: for The component on the X-axis, for The component on the Y axis, for Component in the Z axis; The expression of the inertial navigation position equation is:

[0010] in, is the inertial navigation position information, for The differential of is the longitude in the n system, is the latitude in the n system, is the depth of the n system.

[0011] Furthermore, the state equations of SINS navigation, DVL navigation, USBL navigation and GPS navigation are:

[0012] Among them, the state quantity , is the state quantity at time k, is the constant drift of the gyro's three axes in the carrier coordinate system, for In the X component, for In the Y component, for In the Z component, is the eastward velocity vector, is the north velocity vector, is the vertical velocity vector, is the state transfer matrix at time k, is the system noise distribution matrix at time k, is the process noise at time k; The measurement equation of SINS navigation is:

[0013] in, is the measurement vector at time k, is the observation matrix at time k, To measure noise; The measurement equation for DVL navigation is:

[0014] in, is the measurement matrix for DVL navigation, The measurement noise for DVL navigation; The measurement equations for USBL navigation and GPS navigation are:

[0015] in, The measurement matrix for USBL / GPS navigation, It is the measured noise of USBL / GPS navigation.

[0016] Furthermore, in the SINS / DVL sub-filter, the state equation of SINS / DVL is:

[0017] Among them, the state vector of SINS / DVL , is the state vector of SINS navigation, is the state vector of DVL navigation, is a nonlinear function, is the first noise vector; The measurement equation of SINS / DVL is:

[0018] in, is the first measurement noise, is the eastward component of the first measurement noise in the n system, is the north component of the first measurement noise in the n system, is the vertical component of the first measurement noise in the n system, is the east component of DVL navigation in the n system, is the north component of DVL navigation in the n system, is the vertical component of DVL navigation in the n system, is the eastward velocity error, is the north velocity error, is the vertical velocity error, is the eastward velocity error of the DVL navigation system, is the north velocity error of the DVL navigation system, is the vertical velocity error of the DVL navigation system, It is the first measurement array.

[0019] Furthermore, in the SINS / GPS sub-filter, the state equation of SINS / GPS is:

[0020] in, is the state vector of SINS / GPS, is the second noise vector; The measurement equation of SINS / GPS is:

[0021] in, is the latitude value measured by SINS, is the latitude value measured by GPS, is the longitude value measured by SINS, is the longitude value measured by GPS, is the latitude error of SINS, is the longitude error of SINS, is the latitude error of GPS, is the second measurement noise component, is the second two-sided noise component, is the second measurement array, is the second measurement noise.

[0022] Furthermore, in the SINS / USBL sub-filter, the state equation of SINS / USBL is:

[0023] in, is the state vector of SINS / USBL, is the third noise vector; The measurement equation of SINS / USBL is:

[0024] in, is the height measurement value of SINS, is the height measurement value of USBL, is the height error of SINS, is the height error of USBL, is the third measurement noise, is the third measurement matrix.

[0025] Further, the step of obtaining system information and processing the system information using a t-distributed federated filter to update the posture, speed and position of the unmanned system includes: Acquire system information and distribute the system information according to information distribution coefficients of the main filter, the SINS / DVL sub-filter, the SINS / USBL sub-filter, and the SINS / GPS sub-filter; Determine the navigation state of the unmanned system and its corresponding combined navigation method based on system information; If the navigation state of the unmanned system is a deep-water stealth state, based on the distribution result of the system information, the SINS / DVL sub-filter and the SINS / USBL sub-filter in the t-distributed federated filter are used to perform time update and measurement update to obtain the first state quantity and the first measurement value output by the SINS / DVL sub-filter, and the second state quantity and the second measurement value output by the SINS / USBL sub-filter; the main filter is used to perform information fusion on the first state quantity, the first measurement value, the second state quantity and the second measurement value to obtain the first error estimate and the first navigation parameter of the unmanned system, and the posture, speed and position of the unmanned system in the deep-water stealth state are updated according to the first error estimate and the first navigation parameter; If the navigation state of the unmanned system is a shallow water operation state, based on the distribution result of the system information, the SINS / DVL sub-filter and the SINS / GPS sub-filter in the t-distributed federal filter are used to perform time update and measurement update to obtain the third state quantity and the third measurement value output by the SINS / DVL sub-filter, and the fourth state quantity and the fourth measurement value output by the SINS / GPS sub-filter; the main filter is used to perform information fusion on the third state quantity, the third measurement value, the fourth state quantity and the fourth measurement value to obtain the second error estimate and the second navigation parameter of the unmanned system, and the posture, speed and position of the unmanned system in the shallow water operation state are updated according to the second error estimate and the second navigation parameters.

[0026] Furthermore, the steps of processing system information by the t-distributed federated filter are specifically as follows: Based on the system information, an approximate posterior probability density function is constructed in the form of multiplication of independent factors; By using the variational Bayesian method, the optimal factor product form of the approximate posterior probability density function is obtained by minimizing the KLD distance between the approximate posterior probability density function and the true posterior probability density function. Based on the approximate posterior probability density function of the t-distributed federated filter, each shape parameter and scale parameter in the t-distributed federated filter is solved in turn, and after obtaining the approximate estimated value of each shape parameter and scale parameter, it is substituted into the approximate equation of the approximate posterior probability density function to obtain the optimal approximate posterior probability density function based on the t-distributed federated filter; The error estimation and navigation parameters are obtained according to the approximate posterior probability density function, and are fed back to the unmanned system. The posture, velocity and position of the unmanned system are corrected through parameter compensation.

[0027] The technical solution provided by the embodiments of the present disclosure may have the following beneficial effects: In the embodiment of the present disclosure, through the above-mentioned multi-state unmanned system underwater combined navigation method, on the one hand, by selecting different navigation combination modes under different states, the SINS / DVL / GPS combined navigation mode is adopted in the near sea to reduce the amount of calculation and improve the efficiency; the SINS / DVL / USBL combined navigation mode is adopted in deep sea diving. On the other hand, the cumulative divergence of the measurement deviation caused by the change of the state in the maneuvering characteristics of the underwater unmanned system is taken into account. Aiming at its tail characteristics, the t distribution is introduced in the federal Kalman filter architecture to replace the conventional normal distribution, the thick-tailed measurement noise is modeled, and the variational Bayesian theory is used to approximate the posterior probability density function to achieve accurate estimation of the motion state of the unmanned system, and the federal structure is used to realize the update of multiple sub-filters and data fusion. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] The accompanying drawings herein are incorporated into the specification and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the specification are used to explain the principles of the present disclosure. Obviously, the accompanying drawings described below are only some embodiments of the present disclosure, and for ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without creative work.

[0029] Figure 1 A step diagram showing a method for underwater integrated navigation of a multi-state unmanned system in an exemplary embodiment of the present disclosure; Figure 2 A schematic diagram showing a specific process of a multi-state unmanned system underwater integrated navigation method in an exemplary embodiment of the present disclosure is shown; Figure 3 A schematic diagram showing the structure of a t-distributed federated filter in an exemplary embodiment of the present disclosure is shown; Figure 4 A schematic diagram showing an algorithm of a sub-filter in a t-distributed federated filter in an exemplary embodiment of the present disclosure. DETAILED DESCRIPTION

[0030] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in a variety of forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that the disclosure will be more comprehensive and complete and to fully convey the concepts of the example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0031] In addition, the accompanying drawings are only schematic illustrations of the embodiments of the present disclosure and are not necessarily drawn to scale. The same reference numerals in the figures represent the same or similar parts, and their repeated descriptions will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities.

[0032] This example implementation provides a multi-state unmanned system underwater integrated navigation method. Figure 1 As shown in , the multi-state unmanned system underwater integrated navigation method may include: steps S101 to S106.

[0033] Step S101: constructing a basic inertial navigation kinematics model based on the inertial navigation attitude equation, the inertial navigation velocity equation and the inertial navigation position equation; Step S102: setting the navigation system, combining the basic inertial navigation kinematic model, and obtaining the state equation and observation equation of each navigation system; wherein the navigation system includes SINS navigation, DVL navigation, USBL navigation and GPS navigation; Step S103: setting the unmanned system state and its corresponding combined navigation mode; wherein the unmanned system state includes a deep-water submerged state and a shallow-water operating state, and the combined navigation mode includes SINS / DVL / USBL combined navigation and SINS / DVL / GPS combined navigation. The SINS / DVL / USBL combined navigation is used in the deep-water submerged state, and the SINS / DVL / GPS combined navigation is used in the shallow-water operating state; Step S104: Based on the state equations, observation equations, unmanned system navigation states and their corresponding combined navigation modes of each navigation system, a SINS / DVL sub-filter, a SINS / USBL sub-filter and a SINS / GPS sub-filter are constructed; wherein the SINS / DVL sub-filter and the SINS / USBL sub-filter are used in the deep-water submersible navigation state, and the SINS / DVL sub-filter and the SINS / GPS sub-filter are used in the shallow-water operation navigation state; Step S105: Based on the weight fusion method, a main filter is constructed, the SINS / DVL sub-filter, the SINS / USBL sub-filter and the SINS / GPS sub-filter are combined, and the t-distribution function is introduced to construct a t-distribution federated filter; Step S106: Obtain system information and process the system information using a t-distributed federated filter to update the posture, speed and position of the unmanned system.

[0034] Through the above-mentioned multi-state unmanned system underwater combined navigation method, on the one hand, by selecting different navigation combination methods under different states, the SINS / DVL / GPS combined navigation method is used in the near sea to reduce the amount of calculation and improve efficiency; the SINS / DVL / USBL combined navigation method is used for deep-sea diving. On the other hand, the cumulative divergence of the measurement deviation caused by the change of the state in the maneuvering characteristics of the underwater unmanned system is considered. According to its tail characteristics, the t distribution is introduced in the federated Kalman filter architecture to replace the conventional normal distribution, and the thick-tailed measurement noise is modeled. The variational Bayesian theory is used to approximate the posterior probability density function to achieve accurate estimation of the motion state of the unmanned system, and the federated structure is used to realize the update of multiple sub-filters and data fusion.

[0035] Next, we will refer to Figures 1 to 4 Each step of the above-mentioned multi-state unmanned system underwater integrated navigation method in this example implementation is described in more detail.

[0036] Figure 2 A specific flow chart of a multi-state unmanned system underwater integrated navigation method of the present invention is shown, and the method comprises: In step S101, a basic inertial navigation kinematics model is established; The inertial navigation attitude is updated as:

[0037] in, is the quaternion updated by the rotation vector method when the attitude is updated, is the b system relative to Tie The angular velocity of the projection, represents its corresponding antisymmetric matrix, , is the attitude matrix from the navigation coordinate system to the carrier system, express Relative to The rotation of the navigation system, including the rotation of the navigation system under the rotation of the earth and the Coriolis centripetal force caused by the unmanned system Rotation :

[0038] in Unmanned systems in The northward and eastward velocities in the system, is the Earth's rotation angular velocity, They are the principal curvature radii of the meridian circle and the meridian circle respectively; Normalized quaternion:

[0039] After standardization Represents the posture transformation, the posture matrix is renew:

[0040] The inertial navigation speed is updated as:

[0041] in The speed of the carrier in the navigation coordinate system, is the local gravity acceleration vector, is the strapdown matrix, Accelerometer The speed changes under the system, is the sampling period, and express The constant zero bias of the accelerometer under the system; The inertial navigation position is updated as:

[0042] In step S102, The navigation coordinate system is defined as the geographic coordinate system of NED, with the origin located at the current position of the carrier. , In the geographical plane, The axis is tangent to the local meridian to the north, The axis is eastward along the tangent of the local latitude, The axis is vertically downward to form a right-handed coordinate system; Said The system refers to the carrier coordinate system, which is the coordinate system of the unmanned system itself, and the origin is located at the center of mass of the carrier. Moving forward along the carrier axis, Along the carrier to the right, Perpendicular to the carrier plane, the three axes satisfy the right-hand rule and rotate with the rotation of the carrier; The motion parameters of the unmanned system in 3D space and 6 degrees of freedom are defined as: : represents the position components of the unmanned system in the three coordinate axes in the navigation coordinate system; : represents the velocity components of the unmanned system in the three coordinate axes in the navigation coordinate system; : is the roll angle, the counterclockwise direction is selected as the positive direction, and its value range is (-180°, 180°); is the pitch angle, the positive direction is the same as the roll angle, and the value range is (-90°, 90°); is the yaw angle, left yaw is positive, and the value range is (-180°, 180°).

[0043] Determine the basic form of the combined navigation mode model: The state equation and observation equation of the combined navigation mode are discretized, and the basic form of the state equation and measurement equation of the multi-state unmanned system underwater combined navigation mode model is:

[0044] Among them, the state quantity is selected , It represents the constant drift of the gyro's three axes in the carrier coordinate system. is the state transfer matrix, is the measurement vector, is the observation matrix, Assign the matrix to the system noise, and represents process noise and measurement noise, is the process noise variance and is non-negative definite; is the measurement noise variance and is positive definite.

[0045] and satisfy:

[0046]

[0047]

[0048] in, is the mean variance, is the covariance, is the noise deviation.

[0049] The discrete Kalman filter equation is as follows: State one-step prediction:

[0050] in, is the optimal state estimate based on the previous (time step number k-1) The current (time step number is k) one-step forecast estimate is obtained, is the current state transfer matrix; State Estimation:

[0051] in, is an estimate based on current forecasts The prior state estimate of is the Kalman gain, is the observed value at time k, is the current measurement matrix; Filter gain:

[0052] represents the prediction covariance matrix between time steps k-1 to k, is the transpose of the current measurement matrix, Observation noise covariance matrix; One-step forecast mean square error:

[0053] represents the one-step prediction mean square error matrix between time steps k-1 to k, Represents the state transfer matrix, which describes the transition relationship of the system state from time k-1 to time k. represents the state estimation error covariance matrix at time k-1, process noise covariance matrix; Estimate the mean square error:

[0054] in, express The moment filter gain, represents the measurement noise covariance, Indicates the use of The estimated quantity obtained by using the time information.

[0055] In step S103, combined navigation combining inertial navigation, acoustic navigation and satellite navigation is performed; combined navigation combining inertial navigation, acoustic navigation and satellite navigation taking into account the change of navigation state in underwater scenes is performed, specifically: (1) Determine the state equation. The state quantities include 16-dimensional state error quantities such as attitude angle error, velocity error, position error, accelerometer bias, and gyroscope bias of inertial navigation. Specifically, they are: Status quantity:

[0056] The state transfer matrix is ​​as follows:

[0057] in:

[0058]

[0059]

[0060]

[0061]

[0062]

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069]

[0070]

[0071]

[0072]

[0073]

[0074] (2) Determine the measurement equation The measurement matrices are:

[0075]

[0076] (3) Estimation of state quantities through state equations and measurement equations .

[0077] Define the combined navigation mode under different navigation states; take the strapdown inertial navigation system (SINS) as the main fusion center, and use the speed information output by DVL, the position information output by offshore GPS and the position information output by deep-sea USBL as observation quantities for filtering and fusion, and correct the accumulated error of the strapdown inertial navigation system in real time. The navigation states of unmanned systems include deep-water submersible navigation state and shallow-water operation navigation state. The combined navigation modes include SINS / DVL / USBL combined navigation and SINS / DVL / GPS combined navigation. SINS / DVL / USBL combined navigation is used in deep-water submersible navigation state, and SINS / DVL / GPS combined navigation is used in shallow-water operation navigation state.

[0078] In step S104 to step S106, a federated filtering algorithm based on t-distribution is used to perform information fusion to achieve accurate estimation of the motion state of the unmanned system.

[0079] Acoustic navigation refers to a navigation method that provides position information through USBL and speed information through DVL. USBL refers to the ultra-short baseline positioning system, DVL refers to the Doppler speed measurement system, and satellite navigation provides position information through GPS.

[0080] Establish the federated Kalman filter structure of underwater multi-state unmanned system. The specific structure is as follows Figure 3 As shown in the figure, when the unmanned system is operating in shallow water, a combination of inertial navigation, GPS and DVL navigation is used. When the unmanned system is operating in deep water, a combination of inertial navigation, DVL and USBL navigation is used. (1) SINS / DVL sub-filter 1) According to the error model of SINS, the state vector of SINS is selected as:

[0081] in, They are the latitude error, longitude error and altitude error of SINS respectively; are the east, north and vertical velocity errors respectively; is the attitude error; is the constant drift of the gyroscope; is the constant zero bias of acceleration and. The nonlinear state equation of SINS can be established as:

[0082] In the formula, is the nonlinear function obtained according to the nonlinear error equation of inertial navigation, Assign the matrix to the system noise, is the system noise matrix, which is the noise of the gyroscope and accelerometer under the load system, assuming it is white noise with zero mean; there are:

[0083] The random measurement error of DVL is selected as the state vector, that is:

[0084] Then the state equation of DVL can be expressed as:

[0085] The state vector of SINS / DVL is:

[0086] The nonlinear state equation of the SINS / DVL sub-filter is:

[0087] In the formula, is a nonlinear function, which can be obtained from the above formula; is the system noise vector,

[0088] 2) SINS / DVL sub-filter measurement equation The SINS / DVL sub-filter uses velocity matching, so the velocities of SINS and DVL in the navigation coordinate system are taken as observation values, and the linear observation equation of the system is established as follows:

[0089] In the formula, is the measurement noise of the subsystem, To measure the distance matrix:

[0090] in, is the attitude transformation matrix from the DVL instrument coordinate system to the carrier coordinate system, It is the transformation matrix from the carrier coordinate system to the navigation coordinate system.

[0091] (2) SINS / GPS sub-filter 1) SINS / GPS state equation GPS is used to measure the latitude and longitude position values ​​of the unmanned system. The state vector of GPS is selected as:

[0092] Its state equation can be expressed as:

[0093] In the formula, , ; Then the nonlinear state equation of the SINS / GPS sub-filter is:

[0094] 2) SINS / GPS measurement equation SINS / GPS uses position matching, so the difference between the latitude and longitude values ​​of SINS and GPS is used as the observation value:

[0095] in, To measure the noise of the system, The measurement matrix is:

[0096] (3) SINS / USBL sub-filter 1) SINS / USBL state equation USBL is used to measure the underwater depth of the vehicle, and the height error is selected as the state vector of the depth meter:

[0097] Its state equation can be expressed as:

[0098] Then the nonlinear state equation of SINS / GPS is:

[0099] 2) SINS / USBL measurement equation SINS / USBL uses position matching, so the position error between SINS and USBL is used as the observation value:

[0100] in, To measure the noise of the system, The measurement matrix is:

[0101] Introduce t-distribution federated filtering to fuse multi-source data to reduce the impact of measurement errors caused by flight state changes on the accuracy of the combined navigation method; is zero mean and the covariance is Measurement noise with thick-tail characteristics; In a linear state space model, when the statistical characteristics of state noise and measurement noise conform to Gaussian distribution, the result of Kalman filtering is the optimal estimate; When the noise has thick-tail characteristics, the result of Kalman filtering estimation is a suboptimal estimate; The federal filtering based on t distribution is in the framework of federal Kalman filtering, which uses the t distribution function to describe the measurement noise with thick-tail statistical characteristics, and adopts a recursive process similar to Kalman filtering.

[0102] (1) Perform a predictive update of the status.

[0103] (2) Calculate the joint posterior probability density function and use variational Bayesian theory to find an approximate posterior probability density function of each independent form factor.

[0104] 1) Thick-tailed measurement noise modeling The probability density function of the one-step forecast and the likelihood probability density , the thick-tailed measurement noise is modeled as the following t-distribution:

[0105] in, represents the set of all measurements from time step 1 to time step k-1, Represents all observations from time step 1 to time step k-1 given Under the condition of The probability density of Indicates that in a given system state Under the conditions, it was observed The probability density of indicates that the mean is zero, and the nominal covariance matrix is , degrees of freedom parameter The t distribution of ; by measuring the noise, the likelihood probability density function is calculated as ; The t distribution can be viewed as the weighted sum of an infinite number of Gaussian distributions mixed in the proportion of Gamma distribution;

[0106] Indicates that the mean vector is 0, and the equivalent covariance is is an auxiliary random variable, represents a shape parameter and a scale parameter The gamma distribution of The shape parameter of the Gamma probability density function is and the ratio parameter is , the Gamma distribution is expressed as follows:

[0107] Represents the Gamma function, and the likelihood function of the stratified Gaussian form is:

[0108]

[0109] in, Indicates that in a given and Under the conditions, The probability distribution of Gaussian distribution.

[0110] Probability density function for one-step forecast for:

[0111] Among them, given the past observations Under the condition of The probability density distribution of a part of the predictions conforms to the Gaussian distribution.

[0112] 2) Variational approximation of the posterior probability density function In order to jointly estimate the state variables of the model and auxiliary variables , first jointly estimate the posterior probability density function ; In order to obtain an approximate solution for the posterior probability density, the variational Bayes method is used to solve the approximate probability density function in the form of multiplication of independent factors According to the variational Bayes method, the optimal approximate posterior distribution probability density function in these product forms can be obtained by minimizing the approximate posterior probability density function and the true posterior probability density function The KLD distance between them is used to obtain the approximate posterior probability density function and ;

[0113] The optimal solution is:

[0114] in ,here yes The auxiliary elements of Yes means All elements of a series except , It means to seek expectations for data. Represents variables The constant of The variable parameters of are coupled and are decoupled using fixed point iteration. Specifically, only one variable is updated and the other variables are fixed. In the first iteration, The approximate posterior probability density function obtained To calculate the expected value, the obtained posterior probability density function is then updated as , after multiple iterations, it gradually converges to the optimal solution; the joint probability density function can be expressed as:

[0115] State variables of the posterior probability density function , auxiliary random variable Update to a Gaussian distribution and a Gamma distribution respectively, and get:

[0116] It can be expressed as:

[0117] in is the prediction of the state at time k based on the information at time k-1, is the measurement vector at time k, is the measurement matrix, is the covariance matrix of the observation noise; 3) Solution of approximate posterior probability density parameters To approximate the posterior probability density function of the federated filter based on t distribution, it is necessary to solve each parameter in turn, and after obtaining the approximate estimated value of each parameter, substitute it into the approximate equation of the posterior probability density function to obtain the posterior probability density function of the federated filter based on t distribution; let ,get The solution equation is:

[0118] here is the approximate probability density function of Iterations, is a variable The expected i-th iteration, represents the trace operation, It can be expressed as:

[0119] in, is the estimated value of the state at the current moment based on the available information up to the i+1th iteration; Can be updated to a Gamma distribution with shape parameter and scale parameters ,Right now:

[0120] Shape parameters and scale parameters It can be expressed as:

[0121] make ,have to for:

[0122] No. The modified measurement noise covariance matrix is ​​defined as:

[0123] Define the modified likelihood probability density function:

[0124] Substituting the likelihood probability density function into the above formula, we can get:

[0125] Normalization constant The expression is as follows:

[0126] Using the above formula, Update to a mean of And the covariance matrix is Gaussian distribution function, that is:

[0127] The mean vector And the matrix is The expression is as follows:

[0128] After M iterations, the approximate posterior probability density Updated to:

[0129] in, represents the approximate posterior probability density function after M iterations, express The mean vector is , the covariance matrix is Gaussian distribution of Approximate posterior probability density Updated to:

[0130]

[0131] in, Represents the approximate posterior probability density function after M iterations. After M iterations, the approximate posterior probability density Updated to , and is approximated by a parameter and Defined distribution G; In a specific embodiment, the continuous system state equations and measurement equations of each subsystem (ie, each filter) are established above, which are discretized and then time updated and measurement updated in each subfilter.

[0132] See Figure 3 As shown, the calculation method is as follows: Time update: Each sub-filter updates its own state estimate and covariance value. According to the t-distribution federated filtering theory, its time update process can be described as:

[0133]

[0134] Measurement Update: When there is a measurement value, each sub-filter corrects the time update value according to the measurement value. Since the measurement equation of each sub-filter is linear, the measurement update process of the federated filter based on the t distribution can be obtained:

[0135] After completing the sub-filter update in the federated filter structure, the main filter is fused: (1) Information distribution Information allocation is the process of allocating system information between the main filter and the sub-filter. The allocation principles are as follows:

[0136] In the formula, is the process noise variance matrix of the system common state; is the system global covariance matrix; For the The information distribution coefficient of each sub-filter satisfies the principle of information conservation:

[0137] (2) Information Fusion For the federated filter without feedback, the main filter only plays a role in the data fusion process, and the estimated value of the main filter is the global estimate.

[0138]

[0139] Combined navigation mode correction: After obtaining the error estimate and navigation parameters from the above-mentioned federated filter based on t distribution, they are fed back to each navigation device in the system, and the system is corrected through parameter compensation, as shown in the following figure: Figure 4 As shown in the feedback correction.

[0140] The output state of the federated filter is estimated as: Position error: Latitude error , Longitude Error , vertical position error ; Speed ​​error: North speed error , Eastward velocity error , vertical velocity error ; Attitude angle error: Pitch angle component , heading angle component , vertical attitude angle error ; Accelerometer bias: North accelerometer bias , East accelerometer bias , vertical accelerometer bias ; Gyro drift: gyro drift estimate around the north axis , gyro drift estimation around the east axis , Estimation of gyro drift around the vertical axis ; The navigation system parameter correction is as follows: Position correction:

[0141] in , , are the corrected latitude, longitude and vertical position respectively, , , are the corresponding current positions respectively; Speed ​​correction:

[0142] in , , are the corrected latitude, longitude and vertical position respectively, , , are the corresponding current positions respectively; Posture Correction:

[0143]

[0144] in represents the posture matrix before correction, represents the corrected posture matrix, is a small-angle rotation matrix, where The three attitude error angles are , and The antisymmetric matrix formed; Angular velocity correction:

[0145] in , , They respectively represent the angular velocities around the x-axis, y-axis, and z-axis measured by the gyroscope in the carrier coordinate system b before correction; , , They represent the drift errors of the gyroscope around the x-axis, y-axis, and z-axis respectively; , and They respectively represent the angular velocities around the x-axis, y-axis, and z-axis measured by the calibrated gyroscope in the carrier coordinate system; (5) Acceleration correction:

[0146] in,

[0147]

[0148] in, , , They respectively represent the accelerations on the x-axis, y-axis, and z-axis measured by the accelerometer in the carrier coordinate system b before correction, , , They represent the zero bias errors of the accelerometer in the x-axis, y-axis, and z-axis directions respectively. , , They respectively represent the accelerations on the x-axis, y-axis, and z-axis measured by the calibrated accelerometer in the carrier coordinate system; The posture matrix before correction; Through the above-mentioned multi-state unmanned system underwater combined navigation method, on the one hand, by selecting different navigation combination methods under different states, the SINS / DVL / GPS combined navigation method is used in the near sea to reduce the amount of calculation and improve efficiency; the SINS / DVL / USBL combined navigation method is used for deep-sea diving. On the other hand, the cumulative divergence of the measurement deviation caused by the change of the state in the maneuvering characteristics of the underwater unmanned system is considered. According to its tail characteristics, the t distribution is introduced in the federated Kalman filter architecture to replace the conventional normal distribution, and the thick-tailed measurement noise is modeled. The variational Bayesian theory is used to approximate the posterior probability density function to achieve accurate estimation of the motion state of the unmanned system, and the federated structure is used to realize the update of multiple sub-filters and data fusion.

[0149] It should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", "clockwise", "counterclockwise", etc. in the above description indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the embodiments of the present disclosure and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the embodiments of the present disclosure.

[0150] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present disclosure, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0151] In the embodiments of the present disclosure, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present disclosure can be understood according to specific circumstances.

[0152] In the embodiments of the present disclosure, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include the first and second features being in direct contact, or may include the first and second features not being in direct contact but being in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes the first feature being directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes the first feature being directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0153] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example" or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification.

[0154] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the invention disclosed herein. This application is intended to cover any modification, use or adaptation of the present disclosure, which follows the general principles of the present disclosure and includes common knowledge or customary techniques in the art that are not disclosed in the present disclosure. The specification and examples are intended to be exemplary only, and the true scope and spirit of the present disclosure are indicated by the appended claims.

Claims

1. A multi-state unmanned system underwater integrated navigation method, characterized in that: The method includes: Construct the basic inertial navigation kinematic model based on the inertial navigation attitude equation, inertial navigation velocity equation and inertial navigation position equation; Set up the navigation system, combine the basic inertial navigation kinematic model, and obtain the state equation and observation equation of each navigation system; the navigation system includes SINS navigation, DVL navigation, USBL navigation and GPS navigation; Set the unmanned system state and its corresponding combined navigation mode; the unmanned system state includes deep-water stealth state and shallow-water operation state, and the combined navigation mode includes SINS / DVL / USBL combined navigation and SINS / DVL / GPS combined navigation. The SINS / DVL / USBL combined navigation is used in the deep-water stealth state, and the SINS / DVL / GPS combined navigation is used in the shallow-water operation state; Based on the state equations, observation equations, unmanned system navigation states and their corresponding combined navigation methods of each navigation system, SINS / DVL sub-filter, SINS / USBL sub-filter and SINS / GPS sub-filter are constructed; among them, SINS / DVL sub-filter and SINS / USBL sub-filter are used in deep-water submersion state, and SINS / DVL sub-filter and SINS / GPS sub-filter are used in shallow-water operation state; Based on the weight fusion method, the main filter is constructed, combining the SINS / DVL sub-filter, SINS / USBL sub-filter and SINS / GPS sub-filter, and the t-distribution function is introduced to construct the t-distribution federated filter; Obtain system information and process it using a t-distributed federated filter to update the posture, velocity, and position of the unmanned system.

2. The multi-state unmanned system underwater integrated navigation method according to claim 1, characterized in that: Select " As the navigation coordinate system, the expression of the inertial navigation attitude equation in the basic inertial navigation kinematic model is: in, is the attitude matrix from the carrier system to the navigation coordinate system, for The differential of is the attitude matrix from the navigation coordinate system to the carrier system, for Relative to Tie The angular velocity on the projection, for The component on the X-axis, for The component on the Y axis, for The component on the Z axis, for The antisymmetric matrix of is the quaternion updated by the rotation vector method when the attitude is updated, is the first plural number, is the second plural number, is the third plural, is the fourth plural number, for The differential of for The differential of for The differential of for The differential of The expression of the inertial navigation velocity equation is: in, is the velocity of the carrier in the navigation coordinate system, is the north component of velocity, is the eastward component of velocity, is the vertical component of velocity, for The differential of is the gravitational acceleration vector, is the strapdown matrix, For accelerometer The speed change along the X axis, For accelerometer The speed change along the Y axis, For accelerometer The speed change along the Z axis, is the sampling period, for The constant bias of the accelerometer is: for The component on the X-axis, for The component on the Y axis, for Component in the Z axis; The expression of the inertial navigation position equation is: in, is the inertial navigation position information, for The differential of is the longitude in the n system, is the latitude in the n system, is the depth of the n system.

3. The multi-state unmanned system underwater integrated navigation method according to claim 2, characterized in that: The state equations for SINS navigation, DVL navigation, USBL navigation and GPS navigation are: Among them, the state quantity , is the state quantity at time k, is the constant drift of the gyro's three axes in the carrier coordinate system, for In the X component, for In the Y component, for In the Z component, is the eastward velocity vector, is the north velocity vector, is the vertical velocity vector, is the state transfer matrix at time k, is the system noise distribution matrix at time k, is the process noise at time k; The measurement equation of SINS navigation is: in, is the measurement vector at time k, is the observation matrix at time k, To measure noise; The measurement equation for DVL navigation is: in, is the measurement matrix for DVL navigation, The measurement noise for DVL navigation; The measurement equations for USBL navigation and GPS navigation are: in, The measurement matrix for USBL / GPS navigation, It is the measured noise of USBL / GPS navigation.

4. The underwater integrated navigation method of a multi-state unmanned system according to claim 3 is characterized in that: In the SINS / DVL sub-filter, the state equation of SINS / DVL is: Among them, the state vector of SINS / DVL , is the state vector of SINS navigation, is the state vector of DVL navigation, is a nonlinear function, is the first noise vector; The measurement equation of SINS / DVL is: in, is the first measurement noise, is the eastward component of the first measurement noise in the n system, is the north component of the first measurement noise in the n system, is the vertical component of the first measurement noise in the n system, is the east component of DVL navigation in the n system, is the north component of DVL navigation in the n system, is the vertical component of DVL navigation in the n system, is the eastward velocity error, is the north velocity error, is the vertical velocity error, is the eastward velocity error of the DVL navigation system, is the north velocity error of the DVL navigation system, is the vertical velocity error of the DVL navigation system, It is the first measurement array.

5. The underwater integrated navigation method of a multi-state unmanned system according to claim 4, characterized in that: In the SINS / GPS sub-filter, the state equation of SINS / GPS is: in, is the state vector of SINS / GPS, is the second noise vector; The measurement equation of SINS / GPS is: in, is the latitude value measured by SINS, is the latitude value measured by GPS, is the longitude value measured by SINS, is the longitude value measured by GPS, is the latitude error of SINS, is the longitude error of SINS, is the latitude error of GPS, is the second measurement noise component, is the second two-sided noise component, is the second measurement array, is the second measurement noise.

6. The underwater integrated navigation method of a multi-state unmanned system according to claim 5, characterized in that: In the SINS / USBL sub-filter, the state equation of SINS / USBL is: in, is the state vector of SINS / USBL, is the third noise vector; The measurement equation of SINS / USBL is: in, is the height measurement value of SINS, is the height measurement value of USBL, is the height error of SINS, is the height error of USBL, is the third measurement noise, is the third measurement matrix.

7. The underwater integrated navigation method of a multi-state unmanned system according to claim 6, characterized in that: The steps of obtaining system information and processing the system information using a t-distributed federated filter to update the posture, velocity, and position of the unmanned system include: Acquire system information and distribute the system information according to information distribution coefficients of the main filter, the SINS / DVL sub-filter, the SINS / USBL sub-filter, and the SINS / GPS sub-filter; Determine the navigation state of the unmanned system and its corresponding combined navigation method based on system information; If the navigation state of the unmanned system is a deep-water stealth state, based on the distribution result of the system information, the SINS / DVL sub-filter and the SINS / USBL sub-filter in the t-distributed federated filter are used to perform time update and measurement update to obtain the first state quantity and the first measurement value output by the SINS / DVL sub-filter, and the second state quantity and the second measurement value output by the SINS / USBL sub-filter; the main filter is used to perform information fusion on the first state quantity, the first measurement value, the second state quantity and the second measurement value to obtain the first error estimate and the first navigation parameter of the unmanned system, and the posture, speed and position of the unmanned system in the deep-water stealth state are updated according to the first error estimate and the first navigation parameter; If the navigation state of the unmanned system is a shallow water operation state, based on the distribution result of the system information, the SINS / DVL sub-filter and the SINS / GPS sub-filter in the t-distributed federal filter are used to perform time update and measurement update to obtain the third state quantity and the third measurement value output by the SINS / DVL sub-filter, and the fourth state quantity and the fourth measurement value output by the SINS / GPS sub-filter; the main filter is used to perform information fusion on the third state quantity, the third measurement value, the fourth state quantity and the fourth measurement value to obtain the second error estimate and the second navigation parameter of the unmanned system, and the posture, speed and position of the unmanned system in the shallow water operation state are updated according to the second error estimate and the second navigation parameters.

8. The underwater integrated navigation method of a multi-state unmanned system according to claim 7, characterized in that: The specific steps of the t-distributed federated filter to process system information are: Based on the system information, an approximate posterior probability density function is constructed in the form of multiplication of independent factors; By using the variational Bayesian method, the optimal factor product form of the approximate posterior probability density function is obtained by minimizing the KLD distance between the approximate posterior probability density function and the true posterior probability density function. Based on the approximate posterior probability density function of the t-distributed federated filter, each shape parameter and scale parameter in the t-distributed federated filter is solved in turn, and after obtaining the approximate estimated value of each shape parameter and scale parameter, it is substituted into the approximate equation of the approximate posterior probability density function to obtain the optimal approximate posterior probability density function based on the t-distributed federated filter; The error estimation and navigation parameters are obtained according to the approximate posterior probability density function, and are fed back to the unmanned system. The posture, velocity and position of the unmanned system are corrected through parameter compensation.

Images