Factor graph underwater integrated navigation method based on adaptive window and factor

Abstract

The invention discloses a factor graph underwater integrated navigation method based on adaptive windows and factors. The factor graph underwater integrated navigation method is suitable for an underwater integrated navigation system consisting of an inertial measurement unit (IMU), a Doppler velocimeter (DVL) and an ultra-short baseline positioning system (USBL). The method comprises the following steps: 1, constructing DVL and USBL information into factor nodes, constructing IMU pre-integration factors through pre-integration, and constructing a factor graph model; 2, improving smooth estimation of the system through a sliding window, dynamically adjusting the factor node weight by using a weighting function, and correcting a navigation error; and 4, clustering the navigation data residual error sequence by adopting a DBSCAN algorithm, analyzing a clustering result, adjusting the size of a window, and performing marginalization operation on historical information to reserve prior. According to the method, through adaptive weight adjustment of sensor factor nodes and dynamic optimization of a sliding window, the interference of sensor abnormal data in an underwater complex environment is effectively inhibited, the balance between positioning precision and real-time performance is realized, and the method is suitable for the high-precision navigation requirement of an autonomous underwater vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater integrated navigation, and specifically discloses a factor graph underwater integrated navigation method based on an adaptive window and factors. Background Art

[0002] The passive navigation of an underwater autonomous vehicle mainly relies on inertial navigation. Usually, a strapdown inertial navigation system is used to provide the pose information of the underwater autonomous vehicle. However, due to the complex underwater environment and the low measurement accuracy of a single sensor, it is necessary to be assisted by other navigation systems to improve its positioning accuracy. The strapdown inertial navigation system (SINS) is based on Newton's mechanics principle, and measures the pose information of the carrier through the internal gyroscope and accelerometer through integration. Because of its advantages of independence, high sampling frequency, high data real-time performance, and low cost, it is widely used. However, as the working time increases, the result gradually diverges, the accuracy drops significantly, and reliable pose information cannot be provided. It must be assisted and corrected by other devices.

[0003] The Doppler velocity log (DVL) can provide high-precision velocity information for the vehicle, and the ultra-short baseline positioning system (USBL) provides the relative position information of the carrier through phased array technology. Currently, the mainstream navigation method for unmanned vehicles is usually to combine SINS / DVL / USBL. However, in a complex underwater environment, there are problems of information loss lock and measurement gross error in DVL and USBL, which seriously affect the pose estimation accuracy of integrated navigation. Therefore, how to solve the problem that the sensor does not affect the positioning accuracy in the process of integrated navigation in the face of noise is an important improvement direction for the SINS / DVL / USBL integrated navigation system.

[0004] In the field of integrated navigation, the Kalman filter is the most widely used fusion algorithm. However, the Kalman filter algorithm focuses on the current moment and the previous moment, and does not make full use of historical information. The factor graph algorithm has a good effect in using historical information and global optimal estimation. Adding a sliding window and a weight function can improve the plug-and-play ability of the factor graph model for sensor data and the adaptability of factor nodes.

[0005] In the sliding window model, the larger the window, the more factor nodes participate in the solution, and the higher the overall positioning accuracy. However, when the window is increased to a certain value, the improvement of the positioning accuracy is not obvious. And when there are abnormal factor nodes in the window, the abnormal nodes will contaminate the effective nodes and reduce the estimation accuracy of the window. At this time, not only the positioning accuracy is reduced, but also the real-time performance drops. In this case, appropriately reducing the window can effectively achieve the balance between positioning accuracy and real-time performance.

[0006] 1. Technical comparison with "Research on Underwater Integrated Navigation Algorithm Based on Improved Unscented Kalman Filter":

[0007] "Research on Underwater Integrated Navigation Algorithm with Improved Unscented Kalman Filter" focuses on suppressing the divergence of positioning accuracy by improving the velocity error model, re - establishing the integrated navigation state equation and measurement equation under the SINS / DVL combination mode.

[0008] The invention "A Factor Graph Underwater Integrated Navigation Method Based on Adaptive Sliding Window and Adaptive Factor" focuses on multi - sensor fusion under the factor graph architecture. The increase in measurement data of various different sensors can effectively suppress the divergence problem of positioning accuracy in underwater integrated navigation. The integrated navigation model under the factor graph structure can better achieve the plug - and - play of information. At the same time, the factor graph method makes more full use of historical information compared with the Kalman filter method and has better global optimal estimation ability. The flexibility and scalability of the fusion algorithm under the factor graph framework are demonstrated by adaptive window adjustment.

[0009] 2. Technical comparison with "Tightly - Coupled INS / USBL / DVL Navigation Method Optimized Based on Factor Graph":

[0010] "Tightly - Coupled INS / USBL / DVL Navigation Method Optimized Based on Factor Graph" adopts a factor graph structure in a tightly - coupled state, performs optimal estimation of the error model through residuals, and adjusts the factor graph weights through a gross error detection function based on M - estimation.

[0011] However, in the invention "A Factor Graph Underwater Integrated Navigation Method Based on Adaptive Sliding Window and Adaptive Factor", a sliding window model is added on the basis of the factor graph structure. The weight function of the nodes added to the window is judged and adjusted, which can achieve the adaptability of factor nodes while meeting the function of information plug - and - play. The sliding window can focus the global optimal estimation of the factor graph on the optimal estimation of the nearest window, solving the problem that the traditional factor graph model needs to be updated as a whole when adding new factor nodes, reducing the calculation efficiency, and at the same time solving the problem that the influence of the later - added factor nodes on the overall positioning accuracy is not obvious, improving the positioning accuracy and calculation efficiency.

[0012] 3. Technical comparison with "Research on Inertial / DVL Robust Adaptive Integrated Navigation Method Based on Interacting Multiple Models":

[0013] "Research on Inertial / DVL Robust Adaptive Integrated Navigation Method Based on Interacting Multiple Models" mainly focuses on the modeling of underwater noise. Under the proposed underwater noise model of student t - distribution, the positioning accuracy of SINS / DVL is improved through an integrated navigation method based on interacting multiple models (IMM). This method can adaptively select the noise model according to environmental changes to meet the robustness requirements.

[0014] The adaptability of the present invention "a factor graph underwater integrated navigation method based on adaptive sliding window and adaptive factor" is reflected in the weight adjustment function based on the change of measurement residual. At the same time, the plug-and-play of the factor graph is more convenient than the IMM algorithm in terms of information. When there is a special case of DVL failure, the adaptive factor node can eliminate the measurement node to improve the positioning accuracy. The overall algorithm design is more flexible in terms of adaptability. The marginalization of the sliding window also retains historical information.

[0015] 4. Technical comparison with "Research on inertial-based multi-source fusion positioning technology based on factor graph":

[0016] "Research on Inertial-based Multi-source Fusion Positioning Technology Based on Factor Graph" realizes the positioning of underwater combined navigation through the factor graph of sliding window, uses a fixed-width window to intercept the historical factor nodes and estimated variable nodes involved in the navigation solution, and updates the information factor through window sliding. At the same time, the factor graph is used to realize the collaborative positioning of multiple AUVs.

[0017] The present invention "A factor graph underwater integrated navigation method based on adaptive sliding window and adaptive factor" mainly focuses on the multi-sensor positioning of a single AUV. In the factor graph structure based on the sliding window, compared with the "Research on inertial-based multi-source fusion positioning technology based on factor graph", the newly added factor nodes are judged by weight functions instead of being directly added to the window. At the same time, the historical node information is marginalized and retained as a constraint, rather than directly discarding the marginalized nodes, thereby retaining the historical information outside the window. At the same time, this paper adopts an adjustable sliding window, and further adjusts the window size according to the current working status of the DBSCAN residual clustering analysis. The overall structure is more flexible, and the factor graph model of the adaptive window has better real-time performance. Summary of the invention

[0018] In order to solve the above technical problems, the present invention proposes a factor graph underwater integrated navigation method based on adaptive windows and factors. This method overcomes the problem of insufficient use of historical information in traditional Kalman filtering, realizes plug-and-play of factor information through sliding windows, and adaptively adjusts the weights of sensor measurement parameters, thereby improving the positioning accuracy of the integrated navigation system. And the working conditions of the sensor are analyzed by DBSCAN residual clustering to achieve adaptive adjustment of the window size. While retaining the positioning accuracy, the real-time performance and flexibility of the system are improved.

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

[0020] The factor graph underwater integrated navigation method based on adaptive window and factor comprises the following steps:

[0021] (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, the four-channel velocity information obtained by the Doppler velocity meter, and the direction angle and slant range obtained by the ultra-short baseline, and then obtain the relative position information;

[0022] (2) Establish a factor graph model of SINS / DVL / USBL. Perform pre-integration processing on the SINS measurement information, construct the factor nodes of each sensor composed of error functions, establish the residual models of each factor node according to the working principle, and solve the minimum non-linear least squares problem to obtain the optimal estimation model of the system error variables;

[0023] (3) In the process of global optimal estimation of the factor graph, add a sliding window model to focus the global optimal estimation on the latest window nodes. For the newly added nodes, adjust the weights of the residual adjustment factor nodes through the weight function and then add them to the window. For the factor nodes with the earliest historical information in the window, perform marginalization processing, and use the prior distribution obtained from the marginalized nodes as a constraint to be added to the graph, so as to achieve a better global optimal estimation;

[0024] (4) Perform adaptive adjustment on the sliding window based on the DBSCAN residual clustering results of the residual sequence. When new noise points appear, it is regarded as a sensor anomaly. At this time, shrink the window and perform marginalization processing on the historical information; when the residual sequence forms clusters, increase the window to maintain the stability of the smooth estimation.

[0025] As a further improvement of the present invention, the maximum a posteriori estimation of the residual models of each factor node established in step (2) is:

[0026]

[0027] Among them, X represents the set of all variable nodes. If a prior factor node needs to be added to the factor graph structure, the prior factor node is:

[0028]

[0029] The variable of the factor node is:

[0030]

[0031] Among them, is the attitude misalignment angle; V E V N V U are the three-axis velocity errors of the carrier in the ENU coordinate system; Lλh are the longitude, latitude, and horizontal position positioning errors; ε x ε y ε z is the angular velocity drift of the gyroscope; For adding zero bias;

[0032] The solution of the global optimal estimation in the factor graph model is as follows:

[0033]

[0034] Where r USBL and r DvL represent the DVL and USBL residuals, and l is the weight.

[0035] As a further improvement of the present invention, for the residual of each sensor factor node calculated in step (2), the residual of the pre-integration factor node of the SINS navigation module is:

[0036]

[0037] The residual of the factor node obtained by the DVL navigation module is:

[0038]

[0039] Where V beam = [V1 V2 V3 V4] T represents the four-channel velocity. The residual of the factor node obtained by the USBL navigation module is:

[0040]

[0041] Where represents the calculation of the azimuth angle, P uT represents the position measured by the USBL in the transponder coordinate system, represents the calculation of the slant range, and respectively represent the coordinate transformation matrix from the n system to the e system and the attitude transfer matrix, P real represents the actual position of the carrier, is the position of the underwater acoustic transponder.

[0042] As a further improvement of the present invention, the steps of performing the sliding window in step (3) are as follows:

[0043] (1-1) Determine whether the number of windows is saturated. If the number of windows is not saturated, add the factor nodes after weight adjustment to the window.

[0044] (1-2) When the number of windows is already saturated, move the entire window one factor node to the right and perform adaptive adjustment on the next factor node.

[0045] (1-3) Take the first node before the sliding window as the node with the earliest historical information and marginalize it.

[0046] As a further improvement of the present invention, the steps for adjusting the factor nodes in step (3) through a weight function are as follows:

[0047] After the factor nodes newly added to the sliding window calculate the residuals, they are

[0048]

[0049] weight-adjusted, where r1 and r2 are constant thresholds, and θ is a constant. When the residual is greater than the threshold, the information of this factor node is locked, so the window skips this node to determine the next node. The factor nodes with residuals within the threshold range are adjusted through the weight function, thereby realizing the adaptive adjustment of information by the factor graph;

[0050] When the number of factor nodes in the window is less than the number of windows, the factor nodes are added to the window after weight adjustment; when the number of factor nodes in the window is greater than the number of windows, the earliest factor node in the window is marginalized, the overall window is shifted to the right, and the prior distribution obtained from the marginalized factor node is added as a constraint to the overall factor graph structure.

[0051] As a further improvement of the present invention, the steps for marginalizing the node information of window movement and factor nodes in the model of the sliding window in step (4) are as follows:

[0052] (2-1) The overall window is shifted to the right, and the information of the earliest node is marginalized:

[0053]

[0054] (2-2) The marginalized node is x k , and the next node, i.e., x k+1 is updated. After the update, the prior distribution of x k+1 is

[0055] (2-3) Calculate the cost function and add the prior distribution and the cost function as constraints to the factor graph model based on the sliding window.

[0056] As a further improvement of the present invention, the steps for DBSCAN residual clustering in step (4) are as follows:

[0057] (3-1) Initialize the sensor residual data point set: Label all points as unvisited, initialize the cluster number to 0, specify the neighborhood radius ε and the minimum density threshold MinPts. The neighborhood radius limits the radius range of a single point, and the minimum density threshold specifies the minimum number of points within the neighborhood of a single data point to become a core point;

[0058] (3-2) Traverse all residual data points: Traverse the residual data points in sequence. If a residual data point has not been traversed, mark it; skip the points that have already been marked.

[0059] (3-3) Determine the core points: Find the number of all points contained in the neighborhood of a point. If the number of points contained in the neighborhood of this point is greater than or equal to the minimum density threshold MinPts, then this point is a core point, and this point, as a core point, forms a cluster with all the points contained in this neighborhood, and all the points in the neighborhood of this point are density-reachable points of this point. For the points that are not density-reachable, temporarily mark them as noise points.

[0060] (3-4) Expand the cluster: Starting from the core point, visit all the points contained in this cluster, mark these points as visited, and recursively execute steps 3 and 4 until all density-reachable points have been visited.

[0061] (3-5) Iterate steps 2 to 4 until all points have been visited: That is, repeatedly execute steps 2 to 4 until all points have been visited.

[0062] (3-6) Final classification: According to the above steps, different data points form different clusters. When there is a density-reachable relationship between two data core points, then these two clusters are grouped into one cluster. If the data points temporarily marked as noise points in step 3 belong to a cluster, then remove them from the noise list. For the data points that do not belong to any cluster, they are noise points.

[0063] As a further improvement of the present invention, the adaptive window adjustment step based on the clustering result in step (4) is as follows:

[0064] (4-1) Analyze the clustering result of DBSCAN on the residual data sequence. When a new residual data point forms a cluster with the previous residual data points, it is considered that this data point and the previous data points are working under normal conditions. By increasing the sliding window, more effective factors participating in the estimation are included in the window, improving the smoothness of the navigation estimation.

[0065] (4-2) When a new residual data point does not form a cluster with the previous point set, it is regarded as a noise point appearing. At this time, reduce the window to reduce the influence of abnormal factors on the estimation of effective factors, and make the effective factors marginalized into prior constraints for the abnormal factors within the window.

[0066] (4-3) When multiple noise points gradually become a cluster, it is considered that the formation of the previous noise points is due to a sudden change in the motion state of the carrier, and the sensor is still working normally, so gradually increase the window.

[0067] (4-4) When the overall cluster is stable, the window does not need to change. When adjusting the window, marginalize the old factor nodes in sequence.

[0068] Beneficial effects:

[0069] (1) By combining the navigation model with a tightly coupled factor graph and an adaptive weight function, the adaptive adjustment of factor nodes is achieved, reducing the impact of abnormal factor nodes on the global optimal estimation.

[0070] (2) When the window size of the traditional fixed window size model is too large, the improvement of positioning accuracy is not obvious, and the calculation time increases significantly. The global positioning accuracy of abnormal factor nodes decreases under the large window factor graph model. In response to this situation, this patent proposes a method for adaptive window size based on DBSCAN residual clustering. The working state of the sensor is analyzed according to the clustering results of DBSCAN, and then the window size is adaptively adjusted according to the working state. This patent reduces the impact of abnormal factor nodes on the positioning estimation of the large window factor graph model and the impact of the reduction of system real-time performance under the large window, improves the flexibility and plug-and-play ability of the factor graph model, and achieves a balance between positioning accuracy and calculation real-time performance. Description of the drawings

[0071] Figure 1 is the overall schematic diagram in the disclosed method of the present invention;

[0072] Figure 2 is the schematic diagram of the factor node sliding window in the disclosed method of the present invention;

[0073] Figure 3 is the DBSCAN clustering schematic diagram in the disclosed method of the present invention;

[0074] Figure 4 is the flow of the adaptive window adjustment method in the disclosed method of the present invention. Detailed implementation manners

[0075] The present invention will be further described in detail below in conjunction with the drawings and specific implementation manners:

[0076] The present invention discloses an underwater autonomous vehicle integrated navigation method based on a sliding window and an adaptive factor graph. The overall schematic diagram is as Figure 1 shown, the schematic diagram of the factor node sliding window is as Figure 2 shown, the DBSCAN clustering schematic diagram is as Figure 3 shown, and the flowchart of the adaptive window adjustment method is as Figure 4 shown, including the following steps:

[0077] Step 1: Obtain sensor measurement information. The gyroscope and accelerometer in the inertial measurement unit output the corresponding angular velocity and specific force measurement information, the four-channel velocity information obtained by the Doppler velocimeter, and the direction angle and slant range obtained by the ultra-short baseline to further obtain the relative position information;

[0078] Step 2: Establish a SINS / DVL / USBL factor graph model, perform pre-integration processing on the SINS measurement information, construct each sensor factor node composed of error functions and calculate the residuals, and then obtain the optimal estimation model of the system error variables by solving the minimum nonlinear least squares problem. The maximum a posteriori estimation of the factor graph is:

[0079]

[0080] where X represents the set of all variable nodes. If a prior factor node needs to be added to the factor graph structure, the prior factor node is:

[0081]

[0082] The factor node variables are:

[0083]

[0084] where, is the attitude misalignment angle; V E V N V U are the three-axis velocity errors of the carrier in the ENU coordinate system; Lλh are the longitude, latitude, and horizontal position positioning errors; ε x ε y ε z is the gyroscope angular velocity drift; is the accelerometer zero bias.

[0085] The solution of the global optimal estimation in the factor graph model is:

[0086]

[0087] where r USBL and r DvL represent the DVL and USBL residuals, and l is the weight.

[0088] The residual of the pre-integration factor node of the SINS navigation module is:

[0089]

[0090] The residual of the factor node obtained by the DVL navigation module is:

[0091]

[0092] The residual of the factor node obtained by the USBL navigation module is:

[0093]

[0094] where represents the azimuth angle, PuT represents the position measured by USBL in the transponder coordinate system, represents the calculation of the slant range, and respectively represent the coordinate transformation matrix and the attitude transfer matrix from the n - system to the e - system, P real represents the actual position of the carrier, and then is the position of the underwater acoustic transponder.

[0095] Step 3: In the process of global optimal estimation in the factor graph, add the sliding window model, and focus the global optimal estimation on the latest window nodes. For the newly added nodes, after adjusting the weights of the residual adjustment factor nodes through the weight function, add them to the window, while perform marginalization processing on the factor node with the earliest historical information in the window, and add the prior distribution obtained from the marginalized nodes as a constraint to the graph, so as to achieve a better global optimal estimation.

[0096] The sliding window in the factor graph includes the following steps:

[0097] (1 - 1) Judge whether the number of windows is saturated. If the number of windows is not saturated, add the factor nodes after weight adjustment to the window.

[0098] (1 - 2) When the number of windows is already saturated, move the whole window one factor node to the right, and at the same time perform adaptive adjustment on the next factor node.

[0099] (1 - 3) Take the first node before the sliding window as the node with the earliest historical information and marginalize it.

[0100] The factor nodes added to the window are adjusted through the weight formula:

[0101]

[0102] where r1 and r2 are constant thresholds, and θ is a constant. When the residual is greater than the threshold, the information of this factor node is unlocked, so the window skips this node to judge the next node. The factor nodes with residuals within the threshold range are adjusted through the weight function, so as to realize the adaptive adjustment of information by the factor graph.

[0103] The marginalization steps of the factor node with the earliest historical information in the sliding window process are as follows:

[0104] (2 - 1) Move the whole window to the right and marginalize the information of the earliest node:

[0105]

[0106] (2 - 2) The marginalized node is x k , update the next node, that is, x k+1 and after the update, xk+1 The prior distribution of

[0107] (2 - 3) Calculate the cost function And add the prior distribution and the cost function as constraints to the factor graph model based on the sliding window.

[0108] Step 4: Adaptively adjust the sliding window based on the DBSCAN residual clustering results of the residual sequence. When a new noise point appears, it is regarded as a sensor anomaly. At this time, shrink the window and perform marginalization processing on the historical information; when the residual sequence forms a cluster, increase the window to keep the smooth estimation stable.

[0109] The steps of DBSCAN residual clustering are as follows:

[0110] (3 - 1) Initialize the set of sensor residual data points: Mark all points as unvisited and initialize the cluster number to 0. Specify the neighborhood radius ε and the minimum density threshold MinPts. The neighborhood radius limits the radius range of a single point, and the minimum density threshold specifies the minimum number of points in the neighborhood of a single data point to become a core point.

[0111] (3 - 2) Traverse all residual data points: Traverse the residual data points in turn. If a residual data point has not been traversed, it is marked; points that have already been marked are skipped.

[0112] (3 - 3) Determine the core points: Find the number of all points contained in the neighborhood of a point. If the number of points contained in the neighborhood of this point is greater than or equal to the minimum density threshold MinPts, then this point is a core point, and this point forms a cluster with all the points contained in this neighborhood, and all the points in the neighborhood of this point are density - reachable points of this point. For points that are not density - reachable, they are temporarily marked as noise points.

[0113] (3 - 4) Expand the cluster: Starting from the core point, visit all the points contained in this cluster, mark these points as visited, and recursively execute steps 3 and 4 until all density - reachable points have been visited.

[0114] (3 - 5) Iterate steps 2 to 4 until all points have been visited: That is, repeatedly execute steps 2 to 4 until all points have been visited.

[0115] (3 - 6) Final classification: According to the above steps, different data points form different clusters. When there is a density - reachable relationship between two data core points, these two clusters are grouped into one cluster. If the data points temporarily marked as noise points in step 3 belong to a cluster, they are removed from the noise list. For data points that do not belong to any cluster, they are noise points.

[0116] The steps of adaptively adjusting the window according to DBSCAN clustering are as follows:

[0117] (4-1) Analyze the clustering result of DBSCAN on the residual data sequence. When a new residual data point forms a cluster with the previous residual data points, it is considered that this data point and the previous data points work under normal conditions. By increasing the sliding window, more effective factors participating in the estimation within the window can be included, improving the smoothness of navigation estimation.

[0118] (4-2) When the new residual data point does not form a cluster with the previous point set, it is regarded as a noise point. At this time, the window is reduced to reduce the influence of abnormal factors on the estimation of effective factors, making the effective factors marginalized into prior constraints for the abnormal factors within the window.

[0119] (4-3) When multiple noise points gradually form a cluster, it is considered that the formation of the previous noise points is due to a sudden change in the motion state of the carrier, and the sensor is still working normally, so the window is gradually increased.

[0120] (4-4) When the overall cluster is stable, the window does not need to change. When the window is adjusted, the old factor nodes are marginalized in sequence.

[0121] The above is only a preferred embodiment of the present invention, and it is not intended to limit the present invention in any other form. Any modification or equivalent change made according to the technical essence of the present invention still falls within the scope claimed by the present invention.

Claims

1. An underwater integrated navigation method based on an adaptive window and factors for factor graphs, characterized in that It includes the following steps: (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. The four-channel velocity information obtained by the Doppler velocimeter, and the direction angle and slant range obtained by the ultra-short baseline are used to obtain the relative position information; (2) Establish the SINS / DVL / USBL factor graph model. Pre-integrate the SINS measurement information, construct each sensor factor node composed of error functions, establish the residual model of each factor node according to the working principle, and solve the minimum non-linear least squares problem to obtain the optimal estimation model of the system error variable; (3) During the process of global optimal estimation in the factor graph, add the sliding window model to focus the global optimal estimation on the latest window nodes. For the newly added nodes, after adjusting the weight of the residual adjustment factor node through the weight function, add them to the window. For the factor node with the earliest historical information in the window, perform marginalization processing, and add the prior distribution obtained from the marginalized node as a constraint to the graph to achieve a better global optimal estimation; (4) Based on the DBSCAN residual clustering result of the residual sequence, adaptively adjust the sliding window. When a new noise point appears, it is regarded as a sensor anomaly. At this time, shrink the window and perform marginalization processing on the historical information; when the residual sequence forms a cluster, increase the window to maintain the stability of the smooth estimation.

2. The factor graph underwater integrated navigation method based on an adaptive window and factors according to claim 1, wherein The maximum a posteriori estimation of the residual model of each factor node established in step (2) is: where X represents the set of all variable nodes. A prior factor node needs to be added to the factor graph structure, and the prior factor node is: The factor node variable is: Among them, is the attitude misalignment angle; V E V N V U are the three-axis velocity errors of the vehicle in the ENU coordinate system; Lλh are the longitude, latitude, and horizontal position positioning errors; ε x ε y ε z is the gyroscope angular velocity drift; is the accelerometer zero bias; The solution of the global optimal estimation in the factor graph model is: where r USBL and r DvL represent the DVL and USBL residuals, and l is the weight.

3. The factor graph underwater integrated navigation method based on an adaptive window and factors according to claim 1, wherein The residuals of each sensor factor node calculated in step (2), where the residual of the pre-integration factor node of the SINS navigation module is: The residual of the factor node obtained by the DVL navigation module is: where V beam = [V1 V2 V3 V4] T represents the four-channel speed. The factor node residuals obtained by the USBL navigation module are: Among them represents the calculation of the azimuth angle, P uT represents the position measured by the USBL in the transponder coordinate system, represents the calculation of the slant range, and respectively represent the coordinate transformation matrix and the attitude transfer matrix from the n-system to the e-system, P real represents the actual position of the vehicle, is the position of the underwater acoustic transponder.

4. The factor graph underwater integrated navigation method based on an adaptive window and factors according to claim 1, wherein The steps for performing the sliding window in step (3) are as follows: (1-1) Determine whether the number of windows is saturated. If the number of windows is not saturated, add the factor node after weight adjustment to the window. (1-2) When the number of windows is saturated, the entire window moves one factor node to the right, and at the same time, perform adaptive adjustment on the next factor node. (1-3) Take the first node before the sliding window as the node with the earliest historical information and marginalize it.

5. The factor graph underwater integrated navigation method based on an adaptive window and a factor according to claim 1, wherein The steps for adjusting the weight of the factor node in step (3) are as follows: After calculating the residual of the factor node newly added to the sliding window, pass through Adjust the weight. Among them, r1 and r2 are constant thresholds, and θ is a constant. When the residual is greater than the threshold, the information of this factor node is locked, so the window skips this node to judge the next node. The factor nodes with residuals within the threshold range are adjusted through the weight function, so as to realize the adaptive adjustment of the factor graph to the information; When the number of factor nodes in the window is less than the number of windows, the factor nodes are added to the window after weight adjustment; when the number of factor nodes in the window is greater than the number of windows, the earliest factor node in the window is marginalized, the entire window moves to the right, and the prior distribution obtained from the marginalized factor node is added as a constraint to the overall factor graph structure.

6. The factor graph underwater integrated navigation method based on an adaptive window and factors according to claim 1, wherein The node information marginalization steps for window movement of the model in step (4) and marginalization of factor nodes are as follows: (2-1) The overall window moves to the right, and the earliest node information is marginalized: (2-2) The marginalized node is x k , for the next node, namely x k+1 is updated. After the update, the prior distribution of x k+1 is (2-3) Calculate the cost function And add the prior distribution and the cost function as constraints to the sliding window-based factor graph model.

7. The factor graph underwater integrated navigation method based on an adaptive window and factors according to claim 1, wherein The steps of DBSCAN residual clustering in step (4) are as follows: (3-1) Initialize the sensor residual data point set: Mark all points as unvisited, initialize the cluster number to 0, specify the neighborhood radius ε and the minimum density threshold MinPts. The neighborhood radius limits the radius range of a single point, and the minimum density threshold specifies the minimum number of points within the neighborhood of a single data point to become a core point; (3-2) Traverse all residual data points: Traverse the residual data points in sequence. If a residual data point has not been traversed, mark it; skip the points that have already been marked; (3-3) Determine the core point: Find the number of all points contained within the neighborhood of a point. If the number of points contained within the neighborhood of this point is greater than or equal to the minimum density threshold MinPts, then this point is a core point, and this point forms a cluster with all the points contained within this neighborhood. All points within the neighborhood of this point are density-reachable points of this point. For points that are not density-reachable, temporarily mark them as noise points; (3-4) Expand the cluster: Starting from the core point, visit all the points contained within this cluster, mark these points as visited, and recursively execute steps 3 and 4 until all density-reachable points have been visited; (3-5) Iterate steps 2 to 4 until all points have been visited: That is, repeatedly execute steps 2 to 4 until all points have been visited; (3-6) Final classification: According to the above steps, different data points form different clusters. When there is a density-reachable relationship between two data core points, then these two clusters are grouped into one cluster. If the data points temporarily marked as noise points in step 3 belong to a cluster, then remove them from the noise list. For data points that do not belong to any cluster, they are noise points.

8. The factor graph underwater integrated navigation method based on an adaptive window and a factor according to claim 1, wherein The adaptive window adjustment steps based on the clustering results in step (4) are as follows: (4-1) Analyze the clustering results of DBSCAN on the residual data sequence. When a new residual data point forms a cluster with the previous residual data points, it is considered that this data point and the previous data points are working under normal conditions. Increase the sliding window to have more effective factors participating in the estimation within the window and improve the smoothness of the navigation estimation; (4-2) When the new residual data point does not form a cluster with the previous point set, it is regarded as a noise point. At this time, reduce the window to reduce the influence of abnormal factors on the estimation of effective factors and make the effective factors marginalized into prior constraints for abnormal factors within the window; (4-3) When multiple noise points gradually become a cluster, it is considered that the formation of the previous noise points is due to a sudden change in the motion state of the carrier, and the sensor is still working normally, so gradually increase the window; (4-4) When the overall cluster is stable, the window does not need to change. When adjusting the window, marginalize the old factor nodes in sequence.

Images