Factor graph multi-source information fusion integrated navigation method based on correlation entropy theory

Abstract

The invention discloses a factor graph multi-source information fusion integrated navigation method based on a correlation entropy theory. The method comprises the following steps: constructing a heterogeneous sensor information factor graph model; sensor measurement and prediction state variables are equivalent to two kinds of random variables according to the correlation entropy theory, the random variables are expressed in a correlation entropy kernel function mode, cost function optimization is carried out by introducing an adjustment factor and maximum correlation entropy information, and real-time dynamic adjustment is carried out on the information weight of each sensor; constructing an initial pre-integration object, continuously reading IMU (Inertial Measurement Unit) data, carrying out pre-integration calculation, and storing a current pre-integration result into a sliding window state list; a residual factor is added, the state in the sliding window is set as an optimization variable, iterative optimization is carried out, an optimization objective function is solved, and an iterative optimization combination navigation result is obtained; and evaluating the navigation precision. According to the method, on the premise of ensuring the robustness of the system, the inhibition capability on the abnormal value of the underwater complex environment is remarkably improved, and the pose determination accuracy is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous underwater vehicle navigation operations, in particular to a factor graph multi-source information fusion combined navigation method based on correlation entropy theory. Background Art

[0002] The primary challenge for autonomous underwater vehicles (AUVs) and other marine robots to successfully and efficiently perform their missions is accurately determining their position. Due to the strong attenuation of electromagnetic fields underwater, AUVs cannot use information provided by global navigation satellite systems. Without external reference points, they must rely on proprioceptive measurements obtained from sensors such as ultra-short baselines (USBLs), Doppler velocity loggers (DVLs), and IMUs, performing multi-source data fusion to estimate the vehicle's position.

[0003] Due to the complex underwater operating environment and the limitations of navigation sensor performance parameters, problems such as inaccurate system noise statistics, susceptibility to interference and incomplete reliance, time delay uncertainty, and accuracy variation are inevitable. For example, during underwater propagation, acoustic signals are susceptible to multipath reflections from the water surface and underwater objects, as well as inter-symbol interference, resulting in mixed Gaussian noise and high-intensity outlier contamination in DVL measurements. Furthermore, due to the presence of azimuth information, the measurement equations of USBL systems are highly nonlinear. Multi-source navigation information, under the influence of multiple underwater physical fields such as acoustic, gravity, and electromagnetic fields, will experience uncertain data anomalies and even data failure, resulting in performance degradation of AUV multi-source fusion navigation systems. Therefore, to address the anomalies and failures of heterogeneous and heterogeneous AUV multi-source navigation data, developing an effective adaptive error suppression framework to improve system robustness is essential for further improving the AUV multi-source navigation data integration algorithm framework.

[0004] Existing multi-source fusion navigation methods based on extended Kalman filters rely on complex model construction and processing procedures when processing asynchronous or delayed multi-source navigation information, leading to computational redundancy and suboptimal solutions. This is particularly true when dealing with underwater nonlinear motion models and time-delay uncertainty, requiring increased computational complexity to compensate for errors, which in turn reduces positioning accuracy. Furthermore, existing integrated navigation methods based on factor graph optimization fail to fully consider practical issues such as sensor output anomalies and dynamic navigation performance changes under the influence of multiple underwater environmental physical fields. Adaptive strategies such as the Huber kernel function and switchable constraints offer limited improvements over traditional methods in terms of confidence distance constraints, computational efficiency, and robustness-accuracy trade-offs. These methods also struggle to effectively mitigate the impact of mixed Gaussian noise and high-intensity outlier contamination on the fusion system. Although the maximum correlation entropy criterion demonstrates strong robustness to outlier interference within a filtering framework, it has not yet been effectively integrated into the factor graph optimization framework. The lack of a systematic integration of the correlation entropy theoretical parameter adjustment mechanism with the optimization residual calculation results in a theoretical and practical gap when dealing with complex noise scenarios such as acoustic multipath reflections and intersymbol interference. At the same time, the existing methods lack the adaptive processing capabilities for problems such as failure of heterogeneous sensor data and time delay asynchrony during AUV operations, resulting in the environmental adaptability and real-time solution performance of the multi-source navigation information fusion algorithm being unable to meet the needs of large-scale underwater operations. Summary of the Invention

[0005] In order to overcome the above problems existing in the prior art, the present invention proposes a factor graph multi-source information fusion combined navigation method based on correlation entropy theory.

[0006] The technical solution adopted by the present invention to solve the technical problem is: a factor graph multi-source information fusion combined navigation method based on correlation entropy theory, comprising the following steps: Step 1: Construct a heterogeneous sensor information factor graph model to construct the IMU pre-integration factor and measurement sensor factor; Step 2: Optimize the correlation entropy residual factor. According to the correlation entropy theory, the sensor measurement and predicted state variables are equivalent to two random variables. The random variables are represented by the correlation entropy kernel function. The cost function is optimized by introducing the adjustment factor and the maximum correlation entropy information. The optimization process state is updated and the weight of each sensor information is dynamically adjusted in real time. Step 3, integrated navigation solution: Initialize the configuration file and navigation state, build the initial pre-integration object, continuously read IMU data, perform pre-integration calculation, and store the current pre-integration result in the sliding window state list; add the residual factor, set the state in the sliding window as the optimization variable, use CeresSolver for iterative optimization, solve the optimization objective function, and obtain the iterative optimized integrated navigation result; Step 4: Visual comparison of calculation results: Build a three-dimensional trajectory comparison system to display the actual trajectory and the calculated trajectory in the same coordinate system to evaluate the navigation accuracy.

[0007] The above-mentioned factor graph multi-source information fusion combined navigation method based on correlation entropy theory, the step 1 of constructing a multi-source sensor factor graph optimization framework specifically includes: the system state variable is defined as: in is the position of the carrier, represents the northeast celestial velocity of the carrier in the reference coordinate system, is the carrier's attitude angle, and is the zero bias of IMU gyro and accelerometer; definition To match the sensor measurement values to the carrier, the integrated navigation information fusion problem is converted into a posterior probability distribution The solution: in, is the state variable to be estimated, is the measured value corresponding to the state variable, is the state vector that maximizes the posterior probability among all possible state variable values.

[0008] Through Bayesian inference, the probability problem is transformed into a factor residual distance minimization problem; The maximum a posteriori estimate is obtained by minimizing the sum of the prior and Mahalanobis norms of all residuals; The measurement equations of USBL and DVL are established, which include the position and velocity measurement information provided by USBL and DVL respectively, the measurement function representing the relationship between state variables and measurement values, and the measurement noise term of the sensor.

[0009] In the above-mentioned factor graph multi-source information fusion combined navigation method based on correlation entropy theory, the construction of the IMU pre-integration factor in step 1 specifically includes: establishing the relationship between the measured value and the true value as follows: in and The zero bias of the IMU gyro and accelerometer; and is the noise of the gyroscope and accelerometer; and The angular velocity and acceleration of the carrier are measured by the IMU; and is the real carrier motion angular velocity and acceleration; According to the IMU mechanical arrangement principle, the motion model can be obtained, and then the Time has come Update the posture, speed and position of the moment; The time is used as the initial reference coordinate system to obtain the increments between adjacent pre-integration nodes; the relationship between the pre-integration increment and the zero bias change is established; the zero bias in the update is continuously solved through iterative calculation of the state variables, and then the construction form of the pre-integration residual is obtained.

[0010] In the above-mentioned factor graph multi-source information fusion combined navigation method based on correlation entropy theory, the measurement sensor factor construction process in step 1 specifically includes: the measurement equations of USBL and DVL are respectively expressed as: in Indicates the position and speed measurement information provided by USBL and DVL respectively. is the measurement function between the state variable and the measurement value, represents the measurement noise term of the sensor, and the factor nodes of USBL and DVL are expressed as: in, is the position of the carrier coordinate system relative to the navigation coordinate system, is the velocity of the carrier coordinate system relative to the navigation coordinate system, is the attitude matrix of the carrier coordinate system relative to the navigation coordinate system, is the angular velocity of the carrier relative to the Earth-centered Earth-fixed coordinate system, 、 are the mounting arm matrix error vectors of USBL and DVL sensors, respectively.

[0011] The residual expression of USBL and DVL is: in, is the DVL velocity measurement information vector, is the USBL position measurement information vector, P i is the position state vector to be solved, V i is the velocity state vector to be solved.

[0012] In the above-mentioned factor graph multi-source information fusion combined navigation method based on correlation entropy theory, step 2 specifically includes: Step 2.1, construction of correlation entropy residual: The sensor's measured and predicted state variables are equivalent to two random variables X and Y according to the correlation entropy theory, and the correlation entropy information between them is expressed as: in, represents the kernel function, represents the joint probability density distribution function between state variables, n is the high-order statistic of the error term, The kernel width of the Gaussian kernel directly affects the weight distribution of high-order moments; the cost function is optimized by introducing the adjustment factor λ and the maximum correlation entropy information; the subsequent cost function is reconstructed into the following form based on the adjustable maximum correlation entropy: in, is the measurement noise covariance matrix, is the measurement function that maps state variables to expected measurement values, represents the measurement noise, The meaning is when Then take Otherwise, take ; Step 2.2, solving the optimization problem: transform the probability solution problem into a factor residual distance minimization problem, linearize the measurement state function h, optimize the process state update, and solve it using the least squares equation using the Levenberg-Marquadt method to obtain the state variable increment of each iterative optimization process.

[0013] In the above-mentioned factor graph multi-source information fusion combined navigation method based on correlation entropy theory, step 3 specifically includes: Step 3.1, initialization configuration: read the configuration file, initialize the USBL, DVL, IMU data loader and result save file; calculate the origin of the station center coordinate system and gravity parameters; Step 3.2, data alignment and initialization: Move the IMU data forward to the starting time point, obtain the first USBL and DVL data as the initial position reference; initialize the navigation state and construct the initial pre-integration object; Step 3.3, main processor loop: continuously read IMU data, perform pre-integration calculation, accumulate angle increments and velocity increments, and store the current pre-integration results in the sliding window state list; Step 3.4, optimization problem construction: USBL position factor, select the basic / robust MCE model according to the configuration, and convert the USBL position observation into a state constraint; DVL velocity factor, select different models to constrain the carrier velocity; IMU pre-integration factor, connect adjacent states and provide inertial motion constraints; marginalization factor, maintain sliding window consistency, retain the constraints of historical information on the current state; the state in the sliding window is set as the optimization variable; Step 3.5, nonlinear optimization solution: Use CeresSolver for iterative optimization, configure the LM algorithm, and multi-threaded acceleration; convert the constraints related to the oldest state of the sliding window into prior information, maintain the optimization problem scale, adjust the parameter block pointer, and prepare for the next round of optimization; Step 3.6: Save the current optimal state to a file, remove the oldest state of the sliding window, incorporate the new state, and keep the number of states in the window constant.

[0014] In the above-mentioned factor graph multi-source information fusion combined navigation method based on correlation entropy theory, the time node processing in step 3.3 is specifically as follows: when each integration interval time arrives, align the USBL and DVL data timestamps, process the USBL signal interruption simulation, and perform IMU data interpolation to ensure the time accuracy of the integration node.

[0015] The beneficial effect of the present invention is that the present invention utilizes the characteristic of the maximum correlation entropy of the state variable that is insensitive to noise outliers, integrates it into the factor graph optimization framework, and constructs a combined navigation framework suitable for AUV underwater operations and multi-source sensor information fusion. Under the premise of ensuring the robustness of the system, it significantly improves the ability to suppress outliers in complex underwater environments and improves the accuracy of posture determination. The factor graph framework adopts a sliding window optimization mode, which effectively speeds up the speed of the overall iterative solution and avoids the problem of the overall optimization calculation amount gradually increasing. In the case of various types of complex noise such as USBL and DVL, this method can have good applicability, proving the robustness of the system. The optimization of residual variables by the correlation entropy theory is adjustable, and the most suitable parameter combination can be effectively determined by offline experimental methods. This method is applicable to various complex marine environments and different types of underwater robot operation scenarios, and has good versatility and scalability. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a schematic diagram of the process of the present invention; Figure 2 It is the AUV multi-source sensor factor graph fusion framework of the present invention; Figure 3 is the calculation result of the combined navigation trajectory of the present invention; Figure 4 This is an example of the attitude error result of the present invention; Figure 5 This is an example of the attitude error result of the present invention; Figure 6 This is an example of the position error result of the present invention. DETAILED DESCRIPTION

[0017] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0018] This embodiment proposes a factor graph optimization multi-source information fusion combined navigation method based on the maximum correlation entropy theory. Figure 1 As shown, it is designed to deal with practical problems such as AUV underwater nonlinear motion, abnormal output of measurement sensors under the action of multi-environment physical fields, and dynamic changes in navigation performance. By reconstructing the essence of the residual terms of the measurement sensors and adjusting the relevant entropy parameters, the influence of abnormal terms on the overall optimization process is effectively reduced. At the same time, the sliding window optimization is adopted to achieve real-time solution of the combined navigation process while ensuring the calculation accuracy. The technical solution of the present invention will be described in detail below, including the construction of a heterogeneous sensor information factor graph model, optimization of the relevant entropy residual factor, offline relevant entropy parameter testing, combined navigation solution, visualization comparison of calculation results and other specific implementation steps, which will be described in detail step by step.

[0019] Step 1: Construction of heterogeneous sensor information factor graph model According to the actual operation characteristics of AUV and the adaptability of underwater sensors, an IMU pre-integration / USBL / DVL combined navigation system based on the factor graph optimization framework is constructed. Figure 2 As shown in Figure 2, the sliding window optimization based on the factor graph is used to handle the calculation range of factor nodes and variable nodes. The system state variables are defined as , where n represents the size of the sliding window, The state variables that need to be solved for the system are expressed as: in is the position of the carrier, represents the northeast celestial velocity of the carrier in the reference coordinate system, is the carrier's attitude angle, and is the IMU gyro and accelerometer zero bias. Accordingly, define Match sensor measurements to the carrier.

[0020] Convert the integrated navigation information fusion problem into a posterior probability distribution The solution: The process of combined navigation dead reckoning is to take the value of the state variable that maximizes the posterior probability density of the equation under the condition of the measured value.

[0021] Based on Bayes’ theorem, there is a proportional relationship between the maximum a posteriori probability and the cost function of the node local error established using sensor measurements: By taking the logarithm of both sides of the formula, the probability problem can be transformed into the problem of minimizing the factor residual distance. The following maximum a posteriori estimate is obtained by minimizing the sum of the prior and Mahalanobis norms of all residuals: in, is the prior mean or reference value of the state variable, Map the state variables to the subspace where the prior information is located, w and m are the number of pre-integration residuals and measurement residual factors in the sliding window, respectively. is the pre-integrated measurement value, is the pre-integrated separation noise covariance matrix, 、 are the USBL and DVL measurement noise covariance matrices, is the USBL position measurement value, is the DVL speed measurement value.

[0022] Construction of IMU pre-integration factor The IMU can be used to measure the angular velocity of the carrier and acceleration (comparative force) , IMU measurement is affected by various errors, including gyro and accelerometer zero bias, scale factor error, non-orthogonality error and noise, among which noise and zero bias are the main factors. Therefore, the relationship between the measured value and the true value is established as: in and is the IMU gyro and accelerometer zero bias, and is the noise of the gyroscope and accelerometer. According to the IMU mechanical arrangement principle, the motion model can be obtained as: in and are the projections of the earth's rotational angular velocity and gravitational acceleration in the navigation coordinate system n, is the quaternion representation of the carrier's attitude relative to the navigation coordinate system, represents the rate of change of position, is the time derivative of velocity, reflecting the combined effect of acceleration, including sensor measurements and correction for the motion of the Earth, is the time derivative of the quaternion, updated by the angular velocity, is the rotational angular velocity of the carrier coordinate system relative to the Earth-centered Earth-fixed coordinate system. Based on this, we can obtain Time has come The attitude, speed and position at each moment are updated as follows: in and are the velocity increments caused by Coriolis force and gravity respectively. Based on the above calculations, when When time is used as the initial reference coordinate system, the increment between adjacent pre-integration nodes can be expressed as: in is the position increment caused by the Coriolis force.

[0023] Furthermore, the relationship between the pre-integration increment and the zero bias change is established as follows: in, 、 is the Jacobian matrix of two adjacent state variables, describing the influence of gyroscope and accelerometer zero bias on pre-integrated measurement value At this point, the calculation and update method of the pre-integration increment has been established. The zero bias in the update needs to be continuously solved through iterative calculation of the state variables. Then, the construction form of the pre-integration residual can be obtained as follows: .

[0024] Construction of measurement sensor factors The measurement equations of USBL and DVL can be expressed as in Indicates the position and speed measurement information provided by USBL and DVL respectively. is the measurement function between the state variable and the measurement value, represents the measurement noise term of the sensor, then the factor nodes of USBL and DVL can be expressed as So the residual of USBL and DVL is expressed as At the same time, due to the inconsistency of spatial distribution between the carrier and the measurement sensor, the existence of the lever arm effect occurs. In order to reduce the adverse impact of this part on the solution accuracy, the sensor lever arm error is compensated, such as.

[0025] Step 2: Optimization of the relevant entropy residual factor 2.1 Correlation entropy residual construction First, the sensor's measured and predicted state variables are equivalent to two random variables X and Y according to the correlation entropy theory, and the correlation entropy information between them is expressed as: in, represents the kernel function, represents the joint probability density distribution function between state variables. The correlation entropy information contains all even-order moments of the random variable errors. Therefore, the correlation entropy of random variables X and Y in the kernel width control region can obtain higher-order information from the measurement, which is more suitable for random systems contaminated by nonlinear noise. The cost function is optimized by introducing the adjustment factor λ and the above maximum correlation entropy information. Based on the adjustable maximum correlation entropy reconstruction, the subsequent cost function is reconstructed into the following form: in, is the measurement noise covariance matrix, is the measurement function that maps state variables to expected measurement values, represents the measurement noise, The meaning is when Then take Otherwise, take The purpose here is to control the size of the overall Mahalanobis distance according to the positive and negative values of the residual itself.

[0026] 2.2 Optimization Problem Solving As mentioned above, the probability solving problem is transformed into the factor residual distance minimization problem. Since the measurement state function h is a nonlinear function, it must be linearized first in order to solve it. The process is as follows: The measurement Jacobian matrix Hi is given at the linearization point The (multivariate) partial derivative at , expressed as: The increment of the solution in the iterative process can be expressed as: The optimization process state update is essentially the solution of the least squares equation, which is solved using the Levenberg-Marquadt method. The Levenberg-Marquadt algorithm allows multiple iterations to converge while controlling the region in which the Gauss-Newton quadratic approximation with higher confidence can be used. This is called the trust region method. The following equivalent transformation is performed: Among them, A i is the whitened Jacobian matrix, where is the measurement function h of the state variable at the linearization point The partial derivative at , is the measurement noise covariance matrix. By whitening, adjust The scale of b is used to take into account the uncertainty on both sides. i is the prediction error after whitening, z i is the actual measured value.

[0027] Then the increment expression can be transformed into: Adding non-negative constants to the diagonal of a matrix using Levenberg-Marquardt To modify the equation: By solving this system of equations, we can obtain the increments of the state variables at each step of the iterative optimization process. In this information fusion framework, the information weight of each sensor is dynamically adjusted based on the actual situation. By adjusting the information weight of each sensor, the influence of outliers is suppressed, effectively fusing navigation information and enhancing the robustness of the integrated navigation algorithm.

[0028] Step 3: Combined navigation solution 3.1 Initialization Configuration Read the configuration file to obtain the processing time window, number of iterations, initial state (speed, attitude, IMU deviation), data file path, sensor installation parameters, noise parameters and other configurations; initialize the USBL, DVL, IMU data loader and result save file; calculate the origin of the station center coordinate system and gravity parameters.

[0029] 3.2 Data Alignment and Initialization Move the IMU data forward to the starting time point and obtain the first USBL and DVL data as the initial position reference; initialize the navigation state (position, attitude, velocity, IMU zero bias, etc.) and construct the initial pre-integration object.

[0030] 3.3 Main processing loop IMU pre-integration: continuously read IMU data, perform pre-integration calculations, and accumulate angle increments and velocity increments; time node processing: when each integration interval arrives, align the USBL and DVL data timestamps, handle USBL signal interruption simulation, perform IMU data interpolation when necessary, ensure the integration node time accuracy, and store the current pre-integration results in the sliding window state list.

[0031] 3.4 Optimization Problem Construction Residual factors added: USBL position factor, selects the basic / anti-error MCE model according to the configuration, and converts USBL position observations into state constraints; DVL velocity factor, similar to USBL, selects different models to constrain the carrier velocity; IMU pre-integration factor, connects adjacent states and provides inertial motion constraints; marginalization factor, maintains sliding window consistency, and retains the constraints of historical information on the current state.

[0032] Parameter block setting: Set the state (position, attitude, speed, deviation, etc.) in the sliding window as the optimization variable.

[0033] 3.5 Nonlinear Optimization Solution Use CeresSolver for iterative optimization, configuring the LM algorithm and multi-threaded acceleration. Perform two optimizations: the first for rapid convergence, and the second for fine-tuning. Convert the constraints related to the oldest state of the sliding window into prior information, maintain the optimization problem size, adjust the parameter block pointer, and prepare for the next round of optimization. 3.6 Result Output and Sliding Window Update Save the current optimal state (position, velocity, attitude, IMU bias) to a file. Remove the oldest state in the sliding window and include the new state, keeping the number of states in the window constant.

[0034] Step 4: Visual comparison of calculation results 4.1 Trajectory and Error Visualization Build a 3D trajectory comparison system to display the following trajectories in the same coordinate system: True trajectory: A reference path recorded by a high-precision inertial / acoustic integrated navigation system Results of the present invention: Solution trajectory based on correlation entropy factor graph optimization Synchronously draw the attitude, velocity and position error curves at each moment, and define the error as: in, 、 、 are the calculated values of the three-axis positions, 、 、 are the standard true values of the three-axis positions respectively.

[0035] The horizontal axis is the timestamp, and the vertical axis is the error value. The degree of fluctuation of the curve intuitively reflects the stability of the algorithm.

[0036] 4.2 Quantitative calculation of key indicators Navigation accuracy is evaluated from the following two dimensions: N is the total number of sampling points: Root mean square error (RMSE) Mean Absolute Error (MAE) in, is the predicted value of the state vector of the kth sample, is the true value of the state vector of the kth sample.

[0037] Based on the above method, the calculation results of the combined navigation trajectory sample are compared. Figure 3 As shown, the attitude, speed, and position obtained by the navigation method are compared with the benchmark. The comparison results are shown in Figure 4-6 As shown, Figure 4 The blue, red, and yellow lines represent the attitude errors of the easting, northing, and celestial axes, respectively. The unit is minute ('). After about 100 seconds of attitude alignment, the average attitude error is less than 5 minutes. Figure 5 The blue, red, and yellow lines in the middle represent the velocity errors in the east, north, and celestial directions, respectively, in meters per second (m / s). The mean error is less than 0.02 m / s. Figure 6 In the figure, the blue, red, and yellow lines represent latitude, accuracy, and altitude errors, respectively, in degrees (°), degrees (°), and meters (m). The mean error is less than 5 meters. The combined navigation solution results in this example demonstrate that the multi-source information fusion method proposed in this paper effectively suppresses interference from complex underwater environments, achieves high-precision combined navigation, and strikes a good balance between convergence speed and accuracy.

[0038] The above embodiments are merely exemplary embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art may make various modifications or equivalent substitutions to the present invention within the spirit and scope of protection of the present invention, and such modifications or equivalent substitutions shall also be deemed to fall within the scope of protection of the present invention.

Claims

1. A factor graph multi-source information fusion combined navigation method based on correlation entropy theory is characterized by: The steps include: Step 1: Construct a heterogeneous sensor information factor graph model to construct the IMU pre-integration factor and measurement sensor factor; Step 2: Optimize the correlation entropy residual factor. According to the correlation entropy theory, the sensor measurement and predicted state variables are equivalent to two random variables. The random variables are represented by the correlation entropy kernel function. The cost function is optimized by introducing the adjustment factor and the maximum correlation entropy information. The optimization process state is updated and the weight of each sensor information is dynamically adjusted in real time. Step 3, integrated navigation solution: Initialize the configuration file and navigation state, build the initial pre-integration object, continuously read IMU data, perform pre-integration calculation, and store the current pre-integration result in the sliding window state list; add the residual factor, set the state in the sliding window as the optimization variable, use CeresSolver for iterative optimization, solve the optimization objective function, and obtain the iterative optimized integrated navigation result; Step 4: Visual comparison of calculation results: Build a three-dimensional trajectory comparison system to display the actual trajectory and the calculated trajectory in the same coordinate system to evaluate the navigation accuracy.

2. The factor graph multi-source information fusion combined navigation method based on correlation entropy theory according to claim 1 is characterized in that: The step 1 of constructing a multi-source sensor factor graph optimization framework specifically includes: the system state variable is defined as: in is the position of the carrier, represents the northeast celestial velocity of the carrier in the reference coordinate system, is the carrier's attitude angle, and is the IMU gyro and accelerometer zero bias; definition To match the sensor measurement values to the carrier, the integrated navigation information fusion problem is converted into a posterior probability distribution The solution: in, is the state variable to be estimated, is the measured value corresponding to the state variable, The state vector that maximizes the posterior probability among all possible state variable values; Through Bayesian inference, the probability problem is transformed into a factor residual distance minimization problem; The maximum a posteriori estimate is obtained by minimizing the sum of the prior and Mahalanobis norms of all residuals; The measurement equations of USBL and DVL are established, which include the position and velocity measurement information provided by USBL and DVL respectively, the measurement function representing the relationship between state variables and measurement values, and the measurement noise term of the sensor.

3. The factor graph multi-source information fusion combined navigation method based on correlation entropy theory according to claim 1 is characterized in that: The construction of the IMU pre-integration factor in step 1 specifically includes: establishing the relationship between the measured value and the true value as follows: in and The zero bias of the IMU gyro and accelerometer; and is the noise of the gyroscope and accelerometer; and The angular velocity and acceleration of the carrier are measured by the IMU; and is the real carrier motion angular velocity and acceleration; According to the IMU mechanical arrangement principle, the motion model can be obtained, and then the Time has come Update the posture, speed and position of the moment; The time is used as the initial reference coordinate system to obtain the increments between adjacent pre-integration nodes; the relationship between the pre-integration increment and the zero bias change is established; the zero bias in the update is continuously solved through iterative calculation of the state variables, and then the construction form of the pre-integration residual is obtained.

4. The factor graph multi-source information fusion combined navigation method based on correlation entropy theory according to claim 1 is characterized in that: The measurement sensor factor construction process in step 1 specifically includes: the measurement equations of USBL and DVL are respectively expressed as: in Indicates the position and speed measurement information provided by USBL and DVL respectively. is the measurement function between the state variable and the measurement value, represents the measurement noise term of the sensor, and the factor nodes of USBL and DVL are expressed as: in, is the position of the carrier coordinate system relative to the navigation coordinate system, is the velocity of the carrier coordinate system relative to the navigation coordinate system, is the attitude matrix of the carrier coordinate system relative to the navigation coordinate system, is the angular velocity of the carrier coordinate system relative to the Earth-centered Earth-fixed coordinate system, 、 are the mounting lever arm error vectors of the USBL and DVL sensors, respectively; The residual expression of USBL and DVL is: in, is the DVL velocity measurement information vector, is the USBL position measurement information vector, P i is the position state vector to be solved, V i is the velocity state vector to be solved.

5. The factor graph multi-source information fusion combined navigation method based on correlation entropy theory according to claim 1 is characterized in that: The step 2 specifically includes: Step 2.1, construction of correlation entropy residual: The sensor's measured and predicted state variables are equivalent to two random variables X and Y according to the correlation entropy theory, and the correlation entropy information between them is expressed as: in, represents the kernel function, represents the joint probability density distribution function between state variables, n is the high-order statistic of the error term, The kernel width of the Gaussian kernel directly affects the weight distribution of high-order moments; the cost function is optimized by introducing the adjustment factor λ and the maximum correlation entropy information; the subsequent cost function is reconstructed into the following form based on the adjustable maximum correlation entropy: in, is the measurement noise covariance matrix, is the measurement function that maps state variables to expected measurements, represents the measurement noise, The meaning is when Then take Otherwise, take ; Step 2.2, solving the optimization problem: transform the probability solution problem into a factor residual distance minimization problem, linearize the measurement state function h, optimize the process state update, and solve it using the least squares equation using the Levenberg-Marquadt method to obtain the state variable increment of each iterative optimization process.

6. The factor graph multi-source information fusion combined navigation method based on correlation entropy theory according to claim 1 is characterized in that: The step 3 specifically includes: Step 3.1, initialization configuration: read the configuration file, initialize the USBL, DVL, IMU data loader and result save file; calculate the origin of the station center coordinate system and gravity parameters; Step 3.2, data alignment and initialization: Move the IMU data forward to the starting time point, obtain the first USBL and DVL data as the initial position reference; initialize the navigation state and construct the initial pre-integration object; Step 3.3, main processor loop: continuously read IMU data, perform pre-integration calculation, accumulate angle increments and velocity increments, and store the current pre-integration results in the sliding window state list; Step 3.4, optimization problem construction: USBL position factor, select the basic / robust MCE model according to the configuration, and convert the USBL position observation into a state constraint; DVL velocity factor, select different models to constrain the carrier velocity; IMU pre-integration factor, connect adjacent states and provide inertial motion constraints; marginalization factor, maintain sliding window consistency, retain the constraints of historical information on the current state; the state in the sliding window is set as the optimization variable; Step 3.5, nonlinear optimization solution: Use CeresSolver for iterative optimization, configure the LM algorithm, and multi-threaded acceleration; convert the constraints related to the oldest state of the sliding window into prior information, maintain the optimization problem scale, adjust the parameter block pointer, and prepare for the next round of optimization; Step 3.6: Save the current optimal state to a file, remove the oldest state in the sliding window, incorporate the new state, and keep the number of states in the window constant.

7. The factor graph multi-source information fusion combined navigation method based on correlation entropy theory according to claim 6 is characterized in that: The time node processing in step 3.3 is specifically as follows: when each integration time interval arrives, align the USBL and DVL data timestamps, process the USBL signal interruption simulation, and perform IMU data interpolation to ensure the integration node time accuracy.

Images