A combined navigation method based on density clustering adaptive filtering

Abstract

A combined navigation method based on density clustering adaptive filtering. The combined navigation system consists of an inertial measurement unit (IMU) and a geomagnetic matching module, and includes the following steps: 1. The geomagnetic matching module performs geomagnetic sequence matching based on the collected geomagnetic information to obtain the positioning result of geomagnetic matching, and outputs the velocity and position navigation parameters for standby; 2. The angular rate and acceleration information of the AUV obtained by the IMU are integrated to calculate the carrier position and velocity information; 3. Density clustering is used to discriminate the measurement quality, and then noise estimation is performed. The adaptive filtering algorithm is used to filter the position and velocity navigation parameters, and the result of geomagnetic matching is used to correct the inertial navigation error, so as to achieve the purpose of improving the navigation accuracy. Through the noise adjustment in the adaptive filtering process, the present invention enables the adaptive filtering algorithm to autonomously identify when the quality of geomagnetic measurement data is abnormal, and then adjusts the filtering parameters to ensure the high precision of underwater combined navigation.

Description

Technical Field

[0001] The present invention belongs to the technical field of geomagnetic-aided inertial navigation, and specifically relates to a combined navigation method based on density clustering adaptive filtering. Background Art

[0002] The passive navigation of an underwater autonomous vehicle mainly relies on inertial navigation. The inertial navigation system (INS) is widely used because of its advantages such as independence, high sampling frequency, high data real-time performance, and low cost. However, the working principle of inertial navigation causes its errors to accumulate over time, and it cannot continuously provide reliable position information, and must be assisted and corrected by other devices. Since the geomagnetic field is a vector field, the geomagnetic vector at any point in the near-earth space of the earth is different from that at other points, and there is a one-to-one correspondence with the longitude and latitude of that point. Therefore, in theory, global positioning can be achieved as long as the geomagnetic vector at that point is determined. Therefore, underwater inertial / geomagnetic combined navigation is a hot topic in the research of underwater vehicle navigation.

[0003] Geomagnetic-aided navigation can effectively correct the cumulative error of INS and is an effective method to solve the problems of long endurance and high precision in underwater navigation. However, the principle of geomagnetic matching determines its dependence on the geomagnetic reference map, and the preparation of high-precision geomagnetic reference maps is still under research. At the same time, there are also situations where the accuracy decreases in areas with insignificant changes in geomagnetic characteristics and the matching information is unreliable in geomagnetic anomaly areas. Therefore, how to avoid the divergence of the filtering results of the combined navigation system when the accuracy of the geomagnetic matching result is insufficient is an important improvement direction for inertial / geomagnetic combined navigation.

[0004] In the field of combined navigation, the Kalman filter is the most widely used fusion algorithm, but the traditional Kalman filter is based on the assumption that the noise covariance is completely estimated as Gaussian. However, the geomagnetic / INS combined system may encounter inaccurate noise estimation, and abnormal measurement values caused by geomagnetic anomalies and accuracy degradation may cause system oscillations. Therefore, it is necessary to adjust the R matrix to ensure the overall navigation accuracy.

[0005] 1. Technical comparison with the "RBMCDA underwater multi-target tracking method based on density clustering":

[0006] The "RBMCDA underwater multi-target tracking method based on density clustering" uses the traditional particle swarm algorithm and the density clustering algorithm to obtain the state mean of the particle cluster. In this paper, the density clustering algorithm is used to cluster the state estimation results of all targets of all particles, and the weighted sum is calculated for each sample in each cluster according to the weight to obtain the state mean of each cluster; each particle label vector is respectively matched with the target label matrix to obtain the system target number of each clustering cluster, and the target label matrix is updated to obtain a new target label matrix; according to the density clustering of particle data and the management result of target numbers, all target numbers and state means at the current moment are output.

[0007] However, the present invention is "a combined navigation method based on density clustering adaptive filtering". In the combined navigation method used in the present invention, the key lies in using the density clustering method to identify the outliers of geomagnetic measurements. On the one hand, the present invention focuses on the parameter fusion of inertial navigation and geomagnetism, which is different from the application field of multi-target tracking; on the other hand, the density clustering is used in the present invention to discover similar navigation state parameters to adjust the parameters of the filtering process, rather than to obtain the state mean of the particle cluster in the particle swarm algorithm process to optimize the effect of the particle swarm algorithm.

[0008] 2. Technical comparison with "an underwater autonomous vehicle navigation method based on adaptive filtering":

[0009] "An underwater autonomous vehicle navigation method based on adaptive filtering" uses a combined navigation model based on inertial navigation and Doppler velocimeter, and uses adaptive extended Kalman filter (AEKF) to filter the navigation parameters of position and velocity, and directly corrects the navigation parameters output by the system through the output parameter error estimation value to achieve the purpose of improving navigation accuracy. In this paper, by adjusting the constraint conditions, it can be applied to different sensor noise models, and has good adaptability and strong robustness to underwater navigation control.

[0010] The application scenario of the present invention is the combined navigation of inertial navigation and geomagnetism, focusing on solving the problem of insufficient filtering fusion accuracy in the case of abnormal geomagnetic measurement values. The parameter adjustment of the adaptive filtering process of the present invention depends on the result of density clustering. By density clustering, different types of error values between the geomagnetic matching result and inertial navigation are identified, and numerical calculations are performed for different clusters and the corresponding filtering parameters are adjusted.

[0011] 3. Technical comparison with "an improved Sage-Husa adaptive filtering SINS / DVL combined navigation method":

[0012] The "SINS / DVL integrated navigation method based on improved Sage-Husa adaptive filtering" adopts an integrated navigation model based on inertial navigation and Doppler velocimeter, aiming to solve the problem of SINS / DVL-based integrated navigation under the condition of unknown external measurement noise. It uses a forgetting factor as a reference value for adaptive adjustment and focuses on the system model. At the same time, the limiting conditions in its filtering process are preset and will not be adjusted according to the changes of the situation.

[0013] The application scenario of the present invention is inertial navigation and geomagnetic integrated navigation, which focuses on solving the problem of insufficient filtering fusion accuracy under abnormal geomagnetic measurement values. The adaptive filtering process of the present invention does not preset limiting conditions, identifies different navigation states by density clustering method, and focuses on the filtering stability under abnormal geomagnetic measurement.

[0014] 4. Technical comparison with the "Robust Kalman Filter SINS / DVL Integrated Navigation Method Against Outliers":

[0015] The "Robust Kalman Filter SINS / DVL Integrated Navigation Method Against Outliers" adopts an integrated navigation model based on inertial navigation and Doppler velocimeter. In view of the situation that there are outliers in the DVL output, it uses the beta-Bernoulli distribution to model the binary variable for distinguishing outliers and eliminates the measurement outliers.

[0016] The application scenario of the present invention is inertial navigation and geomagnetic integrated navigation, which focuses on solving the problem of insufficient filtering fusion accuracy under abnormal geomagnetic measurement values. The adaptive filtering process of the present invention does not preset outlier judgment conditions, but automatically identifies them by density clustering method. At the same time, in order to ensure the effective utilization of geomagnetic measurement information, the effective utilization of outlier points is realized through the adjustment of filtering parameters, rather than simply eliminating outlier points. Summary of the Invention

[0017] In view of the above problems, the present invention proposes an integrated navigation method based on density clustering adaptive filtering, which overcomes the problem of insufficient geomagnetic matching accuracy in some areas, identifies abnormal measurement values by density clustering, adaptively adjusts filtering parameters, and thus improves the positioning accuracy of the integrated navigation system.

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

[0019] An integrated navigation method based on density clustering adaptive filtering, characterized in that the specific steps are as follows:

[0020] (1) Obtain sensor measurement information. The gyroscope and accelerometer in the inertial measurement unit output the measurement information of the corresponding angular velocity and specific force, and the geomagnetic matching module outputs the measurement information of the corresponding longitude and latitude.

[0021] (2) Establish a geomagnetic / INS integrated navigation system model, determine the multi-state variables composed of velocity, position, attitude, as well as velocity error, longitude and latitude error, angular velocity drift, and accelerometer zero bias, and establish the state equation and measurement equation;

[0022] (3) Use the standard Kalman filter for prediction and correction, and thus output the fusion filtering result;

[0023] (4) During the standard Kalman filtering process, perform density clustering on the innovation term, judge the reliability of the measurement information at the current moment according to the clustering result. In the case where the innovation is in the unreliable classification, perform real-time prediction and correction on the R matrix based on the standard Kalman filter, and feedback to adjust the filtering parameters, so as to suppress the influence of the geomagnetic matching result on the fusion filtering accuracy at low precision;

[0024] The filtering process that introduces density clustering analysis of the innovation term to achieve noise adaptive is as follows:

[0025] Initialize the state estimate value and covariance, through

[0026]

[0027] Calculate the innovation term,

[0028]

[0029] Perform density clustering on the newly calculated innovation, determine the cluster it belongs to, and then judge whether to adjust the filtering parameters;

[0030] When the innovation is in the main cluster, that is, the cluster with the most data points, the measurement noise matrix R does not need to change. When the innovation clustering result is in other clusters, denoted as the abnormal cluster, that is, the current innovation belongs to the outlier, at this time, an adjustment factor α needs to be introduced to correct the measurement noise. The final adaptive filtering process is:

[0031]

[0032] Among them, is the predicted value of the state variable at time k, Φ k is the one-step transition matrix from time t k-1 to t k P k∣k-1 is the covariance matrix of the predicted value, is the Kalman filter gain, H k is the observation matrix, R k is the observation noise covariance matrix.

[0033] As a further improvement of the present invention, the state equation established in step (2) is:

[0034]

[0035] Where X(t) is the state vector; A(t) is the state transition matrix; W(t) is the process noise covariance matrix.

[0036] The 13-dimensional state vector is used as

[0037] X(t) = [φ x φ y φ z δV E δV N δLδλε x ε y ε z ▽ x ▽ y ▽ z

[0038] Where φ x φ y φ z is the attitude misalignment angle; δV E δV N are the eastward and northward velocity errors of the vehicle relative to the navigation system; δLδλ is the latitude and longitude positioning error; ε x ε y ε z is the gyroscope angular velocity drift; ▽ x ▽ y ▽ z is the accelerometer zero bias.

[0039] As a further improvement of the present invention, the measurement equation established in step (2) is:

[0040] Z(t) = H(t)X(t) + V(t)

[0041] Where Z(t) is the measurement vector; H(t) is the measurement matrix; V(t) is the measurement noise covariance matrix.

[0042] The measurement vector Z(t) is a two-dimensional vector, expressed as

[0043]

[0044] Where L I and λ I are the latitude and longitude of inertial navigation; L M and λ M are the latitude and longitude output by the geomagnetic matching module.

[0045] As a further improvement of the present invention, the density clustering of the innovation term used in step (4) includes the following steps:

[0046] (2-1) Input η at time k and before​i , for i = 0...k, determine the neighborhood parameter ∈, MinPts

[0047] Among them, ∈ indicates the radius of the cluster, and MinPts represents the minimum number of samples in a cluster family;

[0048] (2 - 2) Determine the neighborhood N j of the sample η ∈ (η j ). If the number of samples in its neighborhood exceeds MinPts, then mark η j as a core object, establish a new cluster, and add all points in the neighborhood to the new cluster. Otherwise, mark η j as a border point or a noise point;

[0049] (2 - 3) Repeat the above steps for each sample point until complete classification.

[0050] As a further improvement of the present invention, the setting of the neighborhood parameter used in step (2 - 1) should be based on the principle of completely detecting the anomaly of the geomagnetic matching measurement information, that is, ∈ takes a smaller value within a reasonable range, generally set as the mean square error output by the geomagnetic matching module, and MinPts takes a larger value within a reasonable range, generally more than 50% of the total number of current points.

[0051] As a further improvement of the present invention, the calculation of the adjustment factor α in step (4) is as follows:

[0052] (3 - 1) Calculate the number of points in different clusters and sort them. Mark the cluster with the largest number of members as the main cluster, and mark the median of the data points as the cluster center, denoted as η s ;

[0053] (3 - 2) Select the parameter values of the points η j in the outlier cluster, including H j , P j∣j-1 , H j T ;

[0054] (3 - 3) Calculate the adjustment factor according to the formula

[0055] ​Beneficial effects: The present invention discloses a combined navigation method for an underwater autonomous vehicle based on density clustering and adaptive filtering. This method utilizes the output position information of a geomagnetic / inertial combined navigation system, uses the information of the geomagnetic matching module as measurement information, and improves the navigation accuracy by fusing the results of geomagnetism and inertia. The information categories provided by the geomagnetic matching module are detected through density clustering, and further, an adaptive filtering method is used to adjust the filtering parameters when the accuracy of the geomagnetic matching result is insufficient. By adjusting the measurement noise when the geomagnetic matching accuracy is insufficient, the problem of filtering divergence caused by inaccurate measurement noise is compensated, the robustness of the combined navigation can be improved within a certain range, and the problem of the accuracy decline of the filtering result under the condition of low prior geomagnetic map accuracy or external magnetic field interference can be reduced. Description of the Drawings

[0056] Figure 1 is the flowchart of the method disclosed by the present invention;

[0057] Figure 2 is the flowchart of density clustering in the method disclosed by the present invention. Detailed Embodiments

[0058] The present invention will be further described in detail below in conjunction with the drawings and specific embodiments:

[0059] The present invention discloses a combined navigation method for an underwater autonomous vehicle based on density clustering and adaptive filtering. The flowchart of the method is as Figure 1 shown. The inertial navigation data and the position output of the geomagnetic matching module are fused and estimated. Among them, the innovation term of the filtering module is subjected to density clustering and the adjusted parameter α is returned. The flowchart of density clustering is as Figure 2 shown. The innovation data is classified, and the specific steps are as follows:

[0060] Step 1: Obtain sensor measurement information. The gyroscope and accelerometer in the inertial measurement unit output the corresponding measurement information of angular velocity and specific force, and the geomagnetic matching module outputs the corresponding measurement information of longitude and latitude.

[0061] Step 2: Establish a geomagnetic / INS combined navigation system model, determine the multi-bit state variables composed of velocity, position, attitude, and velocity error, longitude and latitude error, angular velocity drift, and accelerometer zero bias, and establish a state equation and a measurement equation.

[0062] The established state equation is:

[0063]

[0064] where X(t) is the state vector; A(t) is the state transition matrix; W(t) is the process noise covariance matrix.

[0065] The 13-dimensional state vector is adopted as

[0066] X(t) = [φ x φ y φ z δV E δV N δLδλε x ε y ε z ▽ x ▽ y ▽ z

[0067] where φ x φ y φ z is the attitude misalignment angle; δV E δV N is the eastward and northward velocity error of the vehicle relative to the navigation system; δLδλ is the latitude and longitude positioning error; ε x ε y ε z is the gyroscope angular velocity drift; ▽ x ▽ y ▽ z is the accelerometer zero bias.

[0068] The established measurement equation is:

[0069] Z(t) = H(t)X(t) + V(t)

[0070] where Z(t) is the measurement vector; H(t) is the measurement matrix; V(t) is the measurement noise covariance matrix.

[0071] The measurement vector Z(t) is a two-dimensional vector, expressed as

[0072]

[0073] where L I and λ I are the latitude and longitude of inertial navigation; L M and λ M are the latitude and longitude output by the geomagnetic matching module.

[0074] Step 3: Perform prediction and correction using the standard Kalman filter to output the fused filter result;

[0075] Step 4: During the standard Kalman filter process, perform density clustering on the innovation term, judge the reliability of the measurement information at the current moment according to the clustering result, and in the case where the innovation is in the unreliable classification, perform real-time prediction and correction on the R matrix based on the standard Kalman filter, and feedback and adjust the filter parameters to suppress the influence of the geomagnetic matching result on the fused filter accuracy at low precision.

[0076] ​The filtering process that realizes noise adaption by introducing the density clustering innovation item is as follows:

[0077] Initialize the state estimation value and covariance, and then

[0078]

[0079] Calculate the innovation item.

[0080]

[0081] Perform density clustering on the newly calculated innovation, determine the cluster it belongs to, and then judge whether to adjust the filtering parameters.

[0082] The density clustering of the used innovation item includes the following steps:

[0083] (2-1) Input η at time k and before, i , i = 0...k, and determine the neighborhood parameters (∈, MinPts).

[0084] Among them, ∈ indicates the radius of the cluster, and MinPts represents the minimum number of samples in a cluster.

[0085] The setting of the used neighborhood parameters should be based on the principle of completely detecting abnormal geomagnetic matching measurement information, that is, ∈ takes a smaller value within a reasonable range, generally set as the mean square error output by the geomagnetic matching module, and MinPts takes a larger value within a reasonable range, generally more than 50% of the total number of current points.

[0086] (2-2) Determine the neighborhood N j of the sample η ∈ (η j ). If the neighborhood contains more samples than MinPts, then mark η j as a core object, establish a new cluster, and add all points in the neighborhood to the new cluster. Otherwise, mark η j as a boundary point or a noise point.

[0087] (2-3) Repeat the above steps for each sample point until complete classification.

[0088] When the innovation is in the main cluster (i.e., the cluster with the most data points), the measurement noise matrix R does not need to change. When the innovation clustering result is in other clusters (denoted as abnormal clusters), that is, the current innovation belongs to outliers. At this time, an adjustment factor α needs to be introduced to correct the measurement noise.

[0089] The calculation of the adjustment factor α is as follows:

[0090] (3-1) Calculate the number of points in different clusters and sort them, mark the cluster with the most members as the main cluster, and mark the median of the data points as the cluster center, denoted as ηs ;

[0091] (3 - 2) Select the parameter values of the outlier cluster center η j , including H j , P j∣j-1 , H j T ;

[0092] (3 - 3) Calculate the adjustment factor according to the formula Calculate the adjustment factor.

[0093] The final adaptive filtering process is as follows:

[0094]

[0095] As described above, it is only a preferred embodiment of the present invention, and it is not a limitation of the present invention in any other form. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.

Claims

1. A combined navigation method based on density clustering adaptive filtering, characterized in that, the specific steps are as follows: (1) Obtain sensor measurement information. The gyroscope and accelerometer in the inertial measurement unit output the corresponding measurement information of angular velocity and specific force, and the geomagnetic matching module outputs the corresponding measurement information of longitude and latitude; (2) Establish a geomagnetic / INS combined navigation system model, determine the multi-bit state variables composed of velocity, position, attitude, velocity error, longitude and latitude error, angular velocity drift, and accelerometer zero bias, and establish a state equation and a measurement equation; (3) Use the standard Kalman filter for prediction and correction to output the fusion filtering result; (4) During the standard Kalman filtering process, perform density clustering on the innovation term, judge the reliability of the measurement information at the current moment according to the clustering result. In the case where the innovation is in the unreliable classification, perform real-time prediction and correction on the R matrix based on the standard Kalman filter, and feedback-adjust the filtering parameters to suppress the influence of the geomagnetic matching result on the fusion filtering accuracy under low precision; The filtering process of introducing density clustering analysis of the innovation term to achieve noise adaptation is as follows: Initialize the state estimate value and covariance, and pass through Calculate the innovation term, Perform density clustering on the newly calculated innovation to determine the cluster it belongs to, and then judge whether to adjust the filtering parameters; When the innovation is in the main cluster, that is, the cluster with the most data points, the measurement noise matrix R does not need to change. When the innovation clustering result is in other clusters, denoted as the abnormal cluster, that is, the current innovation belongs to the outlier, at this time, an adjustment factor α needs to be introduced to correct the measurement noise. The final adaptive filtering process is: Among them, is the predicted value of the state quantity at time k, Φ k is the one-step transition matrix from time t k-1 to t k , P k∣k-1 is the covariance matrix of the predicted value, is the Kalman filter gain, H k is the observation matrix, R k is the observation noise covariance matrix.

2. A combined navigation method based on density clustering adaptive filtering according to claim 1, characterized in that, the state equation established in step (2) is: where X(t) is the state vector; A(t) is the state transition matrix; W(t) is the process noise covariance matrix; The 13-dimensional state vector is used as Among them, φ x φ y φ z is the attitude misalignment angle; δV E δV N are the eastward and northward velocity errors of the vehicle relative to the navigation system; δL and δλ are the latitude and longitude positioning errors; ε x ε y ε z is the gyroscope angular velocity drift; is the accelerometer zero bias.

3. A combined navigation method based on density clustering adaptive filtering according to claim 1, characterized in that, the measurement equation established in step (2) is: Z(t) = H(t)X(t) + V(t) where Z(t) is the measurement vector; H(t) is the measurement matrix; V(t) is the measurement noise covariance matrix; The measurement vector Z(t) is a two-dimensional vector, expressed as Among them, L I and λ I are the latitude and longitude of inertial navigation; L M and λ M are the latitude and longitude output by the geomagnetic matching module.

4. A combined navigation method based on density clustering adaptive filtering according to claim 1, characterized in that, the density clustering of the innovation term used in step (4) includes the following steps: (2-1) Input η at time k and before i , i = 0...k, to establish neighborhood parameters ∈, MinPts; where ∈ indicates the radius of the cluster, and MinPts represents the minimum number of samples in a cluster; (2-2) Determine the sample η j 's neighborhood N ∈ (η j ), if its neighborhood contains more than MinPts samples, then mark η j as a core object, create a new cluster, and add all points in the neighborhood to the new cluster. Otherwise, mark η j as a border point or a noise point; (2-3) Repeat the above steps for each sample point until the classification is complete.

5. A combined navigation method based on density clustering adaptive filtering according to claim 4, characterized in that, the setting of the neighborhood parameter used in step (2-1) should be based on the principle of completely detecting the abnormality of the geomagnetic matching measurement information, that is, ∈ takes a smaller value within a reasonable range, set as the mean square error output by the geomagnetic matching module, and MinPts takes a larger value within a reasonable range, which is more than 50% of the total current number of points.

6. A combined navigation method based on density clustering adaptive filtering according to claim 1, characterized in that, the calculation of the adjustment factor α in step (4) is as follows: (3-1) Sort by calculating the number of points in different clusters, mark the cluster with the most members as the main cluster, and mark the median of the data points as the cluster center, denoted as η s ; (3-2) Select the parameter values of the midpoint η of the abnormal cluster, including H j , P j , H j∣j-1 ; j T ; (3-3) Calculate the adjustment factor according to the formula ​

Images