Vision-inertial-ultra-short baseline integrated navigation method based on factor graph optimization

Abstract

The application discloses a visual inertial and ultra-short baseline combined navigation method based on a factor graph optimization, belongs to the technical field of underwater robot combined navigation, and is used for underwater robot combined navigation. The method comprises the following steps: initializing an underwater robot system; establishing a visual inertial relative motion constraint, adopting a visual inertial state estimator as a front end of a visual inertial navigation system, and outputting robot pose information; acquiring an absolute position observation value of an ultra-short baseline underwater acoustic positioning system, and constructing a weighted residual error of the ultra-short baseline underwater acoustic positioning system; converting a visual inertial trajectory to a world coordinate system by adopting a similarity transformation; establishing a factor graph objective function, adopting an LM algorithm for iterative optimization, and outputting an optimal trajectory. The method effectively suppresses the cumulative drift of the underwater robot visual inertial navigation system by fusing the visual inertial relative motion constraint and the absolute position observation of the ultra-short baseline underwater acoustic positioning system through a factor graph optimization framework, and realizes long-time high-precision combined navigation.

Description

Technical Field

[0001] This invention discloses a visual-inertial and ultra-short baseline integrated navigation method based on factor graph optimization, belonging to the field of underwater robot integrated navigation technology. Background Technology

[0002] In recent years, Factor Graph Optimization (FGO) has gradually become an important optimization framework for visual inertial navigation due to its ability to uniformly represent the constraint relationships between different sensors and to obtain globally consistent state estimation results through nonlinear least squares optimization. Factor Graph uses a graph model to represent the robot state as graph nodes and visual constraints, inertial constraints, and underwater acoustic observation constraints as graph factors. By establishing a unified objective function, it achieves joint optimization of multi-source information, resulting in higher optimization accuracy and better global consistency compared to traditional filtering algorithms.

[0003] However, existing methods for fusing factor maps of visual-inertial navigation systems and ultra-short baseline underwater acoustic positioning systems still have the following shortcomings: (1) The output trajectory of visual-inertial navigation systems is usually established in a local coordinate system, while the positioning results of ultra-short baseline underwater acoustic positioning systems are usually established in an underwater acoustic positioning coordinate system. The two coordinate systems are different, requiring manual coordinate transformation or external parameter calibration, which increases the complexity of system deployment. (2) The observation frequency of ultra-short baseline underwater acoustic positioning systems is much lower than that of visual-inertial navigation systems. The time scales of the two types of data are different, and existing methods are difficult to establish stable multi-frequency joint constraint relationships, resulting in low utilization of ultra-short baseline underwater acoustic positioning system observations. (3) Most existing methods use simple position update methods to utilize ultra-short baseline underwater acoustic positioning system observations, without fully utilizing the complementary characteristics between visual-inertial relative motion constraints and ultra-short baseline underwater acoustic positioning system absolute position constraints, making it difficult to achieve overall trajectory consistency optimization. (4) Ultra-short baseline underwater acoustic positioning system observations are easily affected by environmental noise, multipath propagation, and other factors, resulting in abnormal observation points. If they are directly involved in optimization, it is easy to cause the optimization results to deviate, reducing the overall navigation stability. (5) Existing integrated navigation methods usually process visual information, inertial information and underwater acoustic information separately, lacking a unified nonlinear optimization model, and cannot give full play to the complementary advantages of multi-source sensor information.

[0004] Therefore, there is an urgent need to propose a combined navigation method that can fully utilize visual inertial continuous motion information and absolute position observation information of ultra-short baseline underwater acoustic positioning system. Under the unified factor graph framework, visual inertial motion constraints and ultra-short baseline underwater acoustic positioning system position constraints are constructed to achieve joint optimization of multi-source information, thereby improving the accuracy and robustness of underwater robot autonomous navigation and positioning over long periods of time. Summary of the Invention

[0005] The purpose of this invention is to provide a visual-inertial and ultra-short baseline combined navigation method based on factor graph optimization, in order to solve the problems in the prior art where visual-inertial navigation systems suffer from severe cumulative drift and lack of global constraints during long-duration underwater operations, and where visual-inertial trajectories and USBL observations are difficult to effectively integrate due to coordinate system inconsistencies.

[0006] A combined visual-inertial and ultra-short baseline navigation method based on factor graph optimization includes: S1. After the underwater robot system is started, the visual inertial navigation system, the ultra-short baseline underwater acoustic positioning system, and the factor graph optimization system are initialized. S2. Establish visual-inertial relative motion constraints, combine the covariance matrix of the pre-integration constraints of the visual-inertial navigation system to obtain the visual-inertial weighted residuals, use the visual-inertial state estimator as the front end of the visual-inertial navigation system to output the robot pose information, and standardize the trajectory file output by the visual-inertial state estimator into the initial trajectory optimized by the factor graph. S3. Obtain the absolute position observations of the ultra-short baseline underwater acoustic positioning system, establish the position residuals of the ultra-short baseline underwater acoustic positioning system by combining the observation covariance matrix of the ultra-short baseline underwater acoustic positioning system, and construct the weighted residuals of the ultra-short baseline underwater acoustic positioning system based on the position residuals of the ultra-short baseline underwater acoustic positioning system. S4. Based on the time series of the visual inertial navigation system and the observation time series of the ultra-short baseline underwater acoustic positioning system, extract the common time intervals of the visual inertial navigation trajectory and the observation data of the ultra-short baseline underwater acoustic positioning system, and retain the visual inertial trajectory and the observation data of the ultra-short baseline underwater acoustic positioning system located within the common time interval; establish the correspondence between the observation data of the ultra-short baseline underwater acoustic positioning system and the visual inertial pose nodes using the time nearest neighbor principle; and transform the visual inertial trajectory to the world coordinate system using a similarity transformation. S5. Construct Huber weights to screen factors for the ultra-short baseline underwater acoustic positioning system, establish a factor graph objective function, use the LM algorithm for iterative optimization, and output the optimal trajectory.

[0007] S1 includes S1.1, Visual Inertial Navigation System Initialization, which includes initializing IMU bias, initializing camera intrinsic parameters, initializing the visual inertial coordinate system, initializing the first frame pose, and establishing the visual inertial local navigation coordinate system. S1 includes S1.2, initialization of the ultra-short baseline underwater acoustic positioning system, including initializing the coordinate system of the ultra-short baseline underwater acoustic positioning system, reading the measurement data of the ultra-short baseline underwater acoustic positioning system, reading the timestamp of the ultra-short baseline underwater acoustic positioning system, and establishing the observation sequence of the ultra-short baseline underwater acoustic positioning system; The system has a total of A visual inertial pose node One observation node for an ultra-short baseline underwater acoustic positioning system, defined robot status at all times for: ; ; In the formula, Let the robot's position vector be... Let x be the robot's x-axis coordinate. Let be the robot's y-axis coordinate. Let z be the robot's z-axis coordinate. For robot speed; This is the robot's posture rotation matrix; Zero bias for the gyroscope; To achieve zero bias in the accelerometer; S1 includes S1.3, and the initialization of the factor graph optimization system includes setting the robot position state variables of the first frame. The initial value position is set to , .

[0008] S2 includes establishing visual-inertial relative motion constraints: ; In the formula, For visual inertial residuals, for The position output by the visual inertial navigation system at any given time; Define the covariance matrix of the pre-integral constraints for a visual inertial navigation system. Visual inertial weighted residuals were obtained. : ; A visual-inertial state estimator is used as the front end of the visual-inertial navigation system to output robot pose information. : ; In the formula, For the first Time stamp For the robot's pose quaternion; Visual-inertial state estimator output trajectory file : ; In the formula, These are the four components of the attitude quaternion; Will After standardization, the initial trajectory for factor graph optimization is obtained. : ; In the formula, For the first The local coordinate system position of each node.

[0009] S3 includes, S3.1, the ultra-short baseline underwater acoustic positioning system in the... The absolute position observation value output at the next observation time for: ; In the formula, for The component in the x-direction, for The component in the y-direction, for The component in the z-direction, It is the transpose symbol. For the sequencing number index of ultra-short baseline underwater acoustic positioning systems; Constructing observation sequences for an ultra-short baseline underwater acoustic positioning system : ; The measurement model of the ultra-short baseline underwater acoustic positioning system is represented as follows: ; In the formula, For the first The measurement noise of the ultra-short baseline underwater acoustic positioning system at the observation time of the sub-ultra-short baseline underwater acoustic positioning system. In the first The sub-ultra-short baseline underwater acoustic positioning system observes the robot's true position at any given moment.

[0010] S3 includes, in S3.2, constructing the observation covariance matrix of the ultra-short baseline underwater acoustic positioning system. : ; In the formula, The degree of dispersion of noise in the x-direction. The degree of dispersion of noise in the y-direction. The degree of dispersion of noise in the z-direction; Establishing the position residual of an ultra-short baseline underwater acoustic positioning system : ; In the formula, For the back-end factor graph, the first The positional variables to be optimized for each node. Number the visual inertial pose nodes; Establish weighted residuals for ultra-short baseline underwater acoustic positioning system : .

[0011] S4 includes, S4.1, extracting the common time interval of visual inertial navigation trajectory and ultra-short baseline underwater acoustic positioning system observation data, whereby the visual inertial navigation trajectory includes a visual inertial navigation trajectory time series and... The observation data of the ultra-short baseline underwater acoustic positioning system includes the observation time series and... ; Let the time series of the visual inertial navigation system be... for: ; In the formula, For the first The specific timestamp of each visual inertial node. This serves as an index for the time series of a visual-inertial navigation system. Observation time series of ultra-short baseline underwater acoustic positioning system for: ; In the formula, For the first Specific timestamps of observations by the sub-ultra-short baseline underwater acoustic positioning system; S4 includes S4.2, calculating the common time interval: ; ; In the formula, The common start time of the two system data. The common end time of the two system data. To find the maximum value, To find the minimum value; Preserve visual inertial trajectories and ultra-short baseline underwater acoustic positioning system observation data located within a common time interval.

[0012] S4 includes, S4.3, establishing the correspondence between observation data of the ultra-short baseline underwater acoustic positioning system and visual-inertial pose nodes using the time nearest neighbor principle, for the first... The observations from the ultra-short baseline underwater acoustic positioning system correspond to the following visual-inertial pose node numbers: ; In the formula, To minimize the objective function ; Set the maximum allowed time difference ,when No correlation is established between observations from the ultra-short baseline underwater acoustic positioning system and visual-inertial pose.

[0013] S4 includes S4.4, where the trajectory of the visual inertial navigation system projected onto the world coordinate system after similarity transformation is... ,Establish and Coordinate transformation relationship between them: ; In the formula, As a scale factor, It is a three-dimensional rotation matrix. It is a three-dimensional translation vector; Construct the optimization objective function: ; In the formula, In order to be with the first The sub-ultra-short baseline underwater acoustic positioning system observes the time-matched visual-inertial navigation system node positions; The objective function is solved using singular value decomposition, and the optimal rotation matrix is ​​output. Optimal scaling factor Optimal translation vector ; Will , , Substituting the coordinate transformation relationship, transform all visual inertial navigation system outputs to the world coordinate system: ; The final output is the aligned visual inertial reference trajectory. : .

[0014] S5 includes, S5.1, constructing Huber weights: ; In the formula, For the factor weights of the ultra-short baseline underwater acoustic positioning system, Let be the residual norm. , For Huber threshold; Set minimum weight threshold ,when Preserve the ultra-short baseline underwater acoustic positioning system factor; if Remove the ultra-short baseline underwater acoustic positioning system factor; Set of factors for ultra-short baseline underwater acoustic positioning system optimization for: ; In the formula, For the first A factor for an ultra-short baseline underwater acoustic positioning system.

[0015] S5 includes S5.2, constructing the objective function of the factor graph. : ; ; In the formula, The set of all state variables to be optimized. ; For a priori residuals; Using the LM algorithm Perform iterative optimization, and calculate the residual Jacobian matrix in each iteration. : ; In the formula, The global residual vector is formed by concatenating the residuals of all factors. This is the vectorized form of all the state variables to be optimized; Construct the normal equation: ; In the formula, The damping factor, For state increments; Update status: ; In the formula, This represents the current iteration number of the LM algorithm. For the first The state vector at the next iteration; The optimization ends when the objective function converges or the maximum number of iterations is reached, and the optimal robot trajectory is output. : ; In the formula, For the first The optimal trajectory for each node.

[0016] Compared with existing technologies, the present invention has the following advantages: The present invention uses a factor graph optimization framework to unify visual-inertial relative motion constraints and USBL absolute position observation constraints into a global optimization problem, making full use of the complementary advantages of VINS high-frequency continuous output and USBL absolute position calibration, effectively suppressing the cumulative drift of the visual-inertial navigation system, and significantly improving the global positioning accuracy of underwater robot long-endurance navigation. By adopting the time nearest neighbor principle to establish a precise time correlation between USBL observation and visual-inertial pose nodes, and introducing Sim(3) similarity transformation to unify the visual-inertial local coordinate system and the USBL world coordinate system, the effective fusion of heterogeneous sensor data is realized. Furthermore, by constructing Huber adaptive weights to robustly filter USBL factors, the inferior observations caused by underwater acoustic signal interruption or abnormal disturbance are effectively eliminated, enhancing the anti-interference ability and robustness of the system in complex underwater environments. Compared with traditional integrated navigation methods, the present invention can achieve globally consistent pose estimation without relying on external loop closure detection, and the factor graph framework has good scalability, which can easily connect more heterogeneous sensors. Attached Figure Description

[0017] Figure 1 This is a flowchart of the method of the present invention; Figure 2 It is a two-dimensional trajectory positioning comparison chart; Figure 3 It is a comparison chart of three-dimensional trajectory positioning; Figure 4 This is a diagram illustrating the location positioning error; Figure 5 It is a bar chart comparing the root mean square error of the location; Figure 6 It is a bar chart comparing absolute trajectory errors; Figure 7 This is a schematic diagram of the eastward error comparison curve; Figure 8 This is a schematic diagram of the northbound error comparison curve; Figure 9 This is a schematic diagram of the overhead error comparison curve; Figure 10 This is a schematic diagram of the eastward error distribution box curve; Figure 11 This is a schematic diagram of the northward error distribution box curve; Figure 12 This is a schematic diagram of the box plot for the axial error distribution. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0019] A combined visual-inertial and ultra-short baseline navigation method based on factor graph optimization includes: S1. After the underwater robot system is started, the visual inertial navigation system, the ultra-short baseline underwater acoustic positioning system, and the factor graph optimization system are initialized. S2. Establish visual-inertial relative motion constraints, combine the covariance matrix of the pre-integration constraints of the visual-inertial navigation system to obtain the visual-inertial weighted residuals, use the visual-inertial state estimator as the front end of the visual-inertial navigation system to output the robot pose information, and standardize the trajectory file output by the visual-inertial state estimator into the initial trajectory optimized by the factor graph. S3. Obtain the absolute position observations of the ultra-short baseline underwater acoustic positioning system, establish the position residuals of the ultra-short baseline underwater acoustic positioning system by combining the observation covariance matrix of the ultra-short baseline underwater acoustic positioning system, and construct the weighted residuals of the ultra-short baseline underwater acoustic positioning system based on the position residuals of the ultra-short baseline underwater acoustic positioning system. S4. Based on the time series of the visual inertial navigation system and the observation time series of the ultra-short baseline underwater acoustic positioning system, extract the common time intervals of the visual inertial navigation trajectory and the observation data of the ultra-short baseline underwater acoustic positioning system, and retain the visual inertial trajectory and the observation data of the ultra-short baseline underwater acoustic positioning system located within the common time interval; establish the correspondence between the observation data of the ultra-short baseline underwater acoustic positioning system and the visual inertial pose nodes using the time nearest neighbor principle; and transform the visual inertial trajectory to the world coordinate system using a similarity transformation. S5. Construct Huber weights to screen factors for the ultra-short baseline underwater acoustic positioning system, establish a factor graph objective function, use the LM algorithm for iterative optimization, and output the optimal trajectory.

[0020] S1 includes S1.1, Visual Inertial Navigation System Initialization, which includes initializing IMU bias, initializing camera intrinsic parameters, initializing the visual inertial coordinate system, initializing the first frame pose, and establishing the visual inertial local navigation coordinate system. S1 includes S1.2, initialization of the ultra-short baseline underwater acoustic positioning system, including initializing the coordinate system of the ultra-short baseline underwater acoustic positioning system, reading the measurement data of the ultra-short baseline underwater acoustic positioning system, reading the timestamp of the ultra-short baseline underwater acoustic positioning system, and establishing the observation sequence of the ultra-short baseline underwater acoustic positioning system; The system has a total of A visual inertial pose node One observation node for an ultra-short baseline underwater acoustic positioning system, defined robot status at all times for: ; ; In the formula, Let the robot's position vector be... Let x be the robot's x-axis coordinate. Let be the robot's y-axis coordinate. Let z be the robot's z-axis coordinate. For robot speed; This is the robot's posture rotation matrix; Zero bias for the gyroscope; To achieve zero bias in the accelerometer; S1 includes S1.3, and the initialization of the factor graph optimization system includes setting the robot position state variables of the first frame. The initial value position is set to , .

[0021] S2 includes establishing visual-inertial relative motion constraints: ; In the formula, For visual inertial residuals, for The position output by the visual inertial navigation system at any given time; Define the covariance matrix of the pre-integral constraints for a visual inertial navigation system. Visual inertial weighted residuals were obtained. : ; A visual-inertial state estimator is used as the front end of the visual-inertial navigation system to output robot pose information. : ; In the formula, For the first Time stamp For the robot's pose quaternion; Visual-inertial state estimator output trajectory file : ; In the formula, These are the four components of the attitude quaternion; Will After standardization, the initial trajectory for factor graph optimization is obtained. : ; In the formula, For the first The local coordinate system position of each node.

[0022] S3 includes, S3.1, the ultra-short baseline underwater acoustic positioning system in the... The absolute position observation value output at the next observation time for: ; In the formula, for The component in the x-direction, for The component in the y-direction, for The component in the z-direction, It is the transpose symbol. For the sequencing number index of ultra-short baseline underwater acoustic positioning systems; Constructing observation sequences for an ultra-short baseline underwater acoustic positioning system : ; The measurement model of the ultra-short baseline underwater acoustic positioning system is represented as follows: ; In the formula, For the first The measurement noise of the ultra-short baseline underwater acoustic positioning system at the observation time of the sub-ultra-short baseline underwater acoustic positioning system. In the first The sub-ultra-short baseline underwater acoustic positioning system observes the robot's true position at any given moment.

[0023] S3 includes, in S3.2, constructing the observation covariance matrix of the ultra-short baseline underwater acoustic positioning system. : ; In the formula, The degree of dispersion of noise in the x-direction. The degree of dispersion of noise in the y-direction. The degree of dispersion of noise in the z-direction; Establishing the position residual of an ultra-short baseline underwater acoustic positioning system : ; In the formula, For the back-end factor graph, the first The positional variables to be optimized for each node. Number the visual inertial pose nodes; Establish weighted residuals for ultra-short baseline underwater acoustic positioning system : .

[0024] S4 includes, S4.1, extracting the common time interval of visual inertial navigation trajectory and ultra-short baseline underwater acoustic positioning system observation data, whereby the visual inertial navigation trajectory includes a visual inertial navigation trajectory time series and... The observation data of the ultra-short baseline underwater acoustic positioning system includes the observation time series and... ; Let the time series of the visual inertial navigation system be... for: ; In the formula, For the first The specific timestamp of each visual inertial node. This serves as an index for the time series of a visual-inertial navigation system. Observation time series of ultra-short baseline underwater acoustic positioning system for: ; In the formula, For the first Specific timestamps of observations by the sub-ultra-short baseline underwater acoustic positioning system; S4 includes S4.2, calculating the common time interval: ; ; In the formula, The common start time of the two system data. The common end time of the two system data. To find the maximum value, To find the minimum value; Preserve visual inertial trajectories and ultra-short baseline underwater acoustic positioning system observation data located within a common time interval.

[0025] S4 includes, S4.3, establishing the correspondence between observation data of the ultra-short baseline underwater acoustic positioning system and visual-inertial pose nodes using the time nearest neighbor principle, for the first... The observations from the ultra-short baseline underwater acoustic positioning system correspond to the following visual-inertial pose node numbers: ; In the formula, To minimize the objective function ; Set the maximum allowed time difference ,when No correlation is established between observations from the ultra-short baseline underwater acoustic positioning system and visual-inertial pose.

[0026] S4 includes S4.4, where the trajectory of the visual inertial navigation system projected onto the world coordinate system after similarity transformation is... ,Establish and Coordinate transformation relationship between them: ; In the formula, As a scale factor, It is a three-dimensional rotation matrix. It is a three-dimensional translation vector; Construct the optimization objective function: ; In the formula, In order to be with the first The sub-ultra-short baseline underwater acoustic positioning system observes the time-matched visual-inertial navigation system node positions; The objective function is solved using singular value decomposition, and the optimal rotation matrix is ​​output. Optimal scaling factor Optimal translation vector ; Will , , Substituting the coordinate transformation relationship, transform all visual inertial navigation system outputs to the world coordinate system: ; The final output is the aligned visual inertial reference trajectory. : .

[0027] S5 includes, S5.1, constructing Huber weights: ; In the formula, For the factor weights of the ultra-short baseline underwater acoustic positioning system, Let be the residual norm. , For Huber threshold; Set minimum weight threshold ,when Preserve the ultra-short baseline underwater acoustic positioning system factor; if Remove the ultra-short baseline underwater acoustic positioning system factor; Set of factors for ultra-short baseline underwater acoustic positioning system optimization for: ; In the formula, For the first A factor for an ultra-short baseline underwater acoustic positioning system.

[0028] S5 includes S5.2, constructing the objective function of the factor graph. : ; ; In the formula, The set of all state variables to be optimized. ; For a priori residuals; Using the LM algorithm Perform iterative optimization, and calculate the residual Jacobian matrix in each iteration. : ; In the formula, The global residual vector is formed by concatenating the residuals of all factors. This is the vectorized form of all the state variables to be optimized; Construct the normal equation: ; In the formula, The damping factor, For state increments; Update status: ; In the formula, This represents the current iteration number of the LM algorithm. For the first The state vector at the next iteration; The optimization ends when the objective function converges or the maximum number of iterations is reached, and the optimal robot trajectory is output. : ; In the formula, For the first The optimal trajectory for each node.

[0029] This invention Standardization involves VINS-Fusion performing joint visual-inertial estimation based on input camera image data and inertial measurement data, and outputting the robot's continuous motion trajectory. To meet the data input requirements of backend factor graph optimization, the CSV format trajectory output by VINS-Fusion is standardized.

[0030] First, the trajectory file output by VINS-Fusion is read, and each trajectory record is parsed to extract the timestamp, 3D position, and attitude quaternion. For the timestamp data, the original nanosecond-level timestamps are... Convert to seconds The data is then normalized using the timestamp of the first valid frame as the time base to obtain a relative time series starting from zero. For location data, the three-dimensional positions at each time point are constructed into a three-dimensional location matrix in chronological order. Where N is the number of valid VINS states; for attitude data, attitude quaternions of different formats are uniformly converted to... Arrangement format and normalize quaternions. ,in The trajectory data undergoes a validity check, removing records with null values, non-finite values, or abnormal formats. The state data is then rearranged in ascending order of timestamps to ensure that time, position, and attitude information maintain their correspondence within the same state node. Finally, a standardized VINS trajectory data structure containing timestamps, normalized time, 3D position, and attitude quaternions is constructed and saved as a MATLAB format file. This file serves as the initial trajectory input for backend factor graph optimization of the VINS. .

[0031] The process of ending the optimization and outputting the robot's optimal trajectory after the objective function converges or reaches the maximum number of iterations includes: when the first iteration... The iteration and the The relative rate of change of the objective function between iterations satisfies And the relative rate of change of the optimization variable satisfies Meanwhile, the first-order optimality index of the objective function satisfies When the factor graph optimization process converges, the iteration ends; when the number of iterations reaches the preset maximum number of iterations... At this point, even if the above convergence condition is not met, the optimization process is terminated. After optimization, the robot's position state at each time step obtained from the last iteration is... The optimal robot trajectory is obtained through factor graph optimization.

[0032] In another embodiment, the initialization of the factor graph optimization system of the present invention can be performed after the robot position is output by the visual inertial navigation system. Factor graph position state variables are established based on the robot position output by the visual inertial navigation system, and the robot position of the first frame after coordinate unification is used as the initial reference for the position of the factor graph optimization. And establish the first frame position prior constraint. ,in, For the robot's position variable to be optimized in the first frame during factor graph optimization, The initial position of the first frame provided for the visual inertial navigation system.

[0033] The following description, in conjunction with the accompanying drawings and embodiments, provides further details. The method flow of this invention is as follows: Figure 1 As shown, the data is first acquired and preprocessed, then subjected to dual-source front-end computation. The computation results are divided into two branches: the first branch, VINS_Fusion (visual-inertial state estimator), performs feature extraction and tracking sliding window optimization to output the pose sequence; the second branch, the ultra-short baseline underwater acoustic positioning system, outputs the position observation sequence. The results from both branches are dynamically adjusted to optimize the observation noise covariance. The system first removes outlier observations and automatically unifies the spatiotemporal characteristics of the visual inertial navigation system / ultra-short baseline underwater acoustic positioning system. Then, it performs adaptive screening of outlier factors, factor graph construction, and nonlinear (LM) global optimization based on Huber weights for the ultra-short baseline underwater acoustic positioning system. Finally, it outputs the optimal trajectory, absolute trajectory error (ATE), and root mean square error of position (RMSE) accuracy evaluation.

[0034] The following is a further explanation with reference to the embodiments. Embodiment 1 of this invention uses a publicly available underwater dataset as the experimental data source. A combined navigation experimental platform for visual-inertial and ultra-short baseline underwater acoustic positioning systems is constructed using video images, inertial measurement data, and COLMAP-reconstructed trajectories provided in the dataset. A total of 80 observation factors for the ultra-short baseline underwater acoustic positioning system are obtained. Using the method of this invention, residual calculation and weight evaluation are performed on all ultra-short baseline underwater acoustic positioning system observations. 78 ultra-short baseline underwater acoustic positioning system observations meet the weight threshold requirements and are retained for factor graph optimization; 2 ultra-short baseline underwater acoustic positioning system observations are judged as abnormal observations due to their large residuals and are automatically removed. , , .

[0035] like Figure 2 , Figure 3 As shown in the figure, the positioning results of the real trajectory, VINS visual-inertial navigation trajectory, USBL ultra-short baseline observation constraint points, and VINS / USBL integrated navigation trajectory in the horizontal plane and three-dimensional space are presented respectively. As can be seen from the figure, the VINS trajectory can track the real trajectory well in the early stages of motion, but as the motion time increases, its trajectory gradually drifts significantly due to the accumulation of visual feature errors, IMU noise, and inertial integration errors. After introducing USBL observation constraints, the red integrated navigation trajectory maintains good consistency with the real trajectory in most motion areas, effectively reducing the cumulative drift of the VINS.

[0036] like Figure 4As shown in the schematic diagram of the positioning error, the proposed VINS / USBL integrated navigation method based on factor graph optimization not only significantly suppresses the positioning drift of the pure VINS system throughout the entire time period, but also improves the positioning accuracy from meter level to sub-meter level, fully verifying the superiority of this method in integrated navigation and positioning in underwater or complex environments.

[0037] Figure 5 The results comparing the root mean square error (RMSE) of pure VINS and the VINS / USBL integrated navigation method over the entire trajectory are presented. The figure clearly shows that the RMSE of pure VINS is 1.272 m, which is relatively large, while the RMSE of the VINS / USBL integrated navigation method is significantly reduced to 0.310 m, a reduction of approximately 75.6% compared to the pure VINS positioning result. This result demonstrates that the VINS / USBL fusion method based on factor graph optimization, by introducing USBL absolute position observations as constraint factors into the optimization framework, effectively suppresses the drift divergence of the pure VINS positioning system.

[0038] according to Figure 6 The absolute trajectory error comparison histogram shows that the absolute trajectory error (ATE) of pure VINS is as high as 1.272m, which intuitively reflects that even after the rigid body transformation is compensated by Sim(3) alignment, its trajectory still has serious non-uniform distortion. In contrast, the ATE of the method of this invention is only 0.310m, indicating that after introducing USBL absolute observation, the overall geometric structure of the trajectory is perfectly constrained, maintaining a very high consistency with the real path, and the drift is effectively suppressed.

[0039] according to Figure 7 , Figure 8 , Figure 9 The error curves in the three directions illustrate that the proposed VINS / USBL integrated navigation method significantly reduces the positioning error of pure VINS in the east, north, and sky directions. This comprehensive performance improvement strongly demonstrates that the factor graph optimization framework can uniformly apply USBL absolute observation information to all spatial components of the trajectory, achieving consistency constraints in three-dimensional space, rather than only correcting a certain direction.

[0040] Figure 10 , Figure 11 , Figure 12Box plots of the error distributions of pure VINS and the proposed method are presented for the east, north, and celestial directions. It can be seen that pure VINS exhibits a wide box shape, median shift, and numerous outliers in all three directions, with the most severe dispersion in the north direction and the largest extreme error in the celestial direction. In contrast, the proposed method, through factor plot optimization introducing USBL absolute observation constraints and supplemented by Huber robust kernels to suppress outlier interference, results in narrower boxes in all three directions, medians close to zero, no significant outliers, and a reduced overall distribution range. These results, from a statistical distribution perspective, fully validate the high accuracy and reliability of the proposed method in 3D spatial positioning.

[0041] To verify the effectiveness of the method of the present invention, an experimental verification was conducted using a publicly available underwater dataset. The three-dimensional reconstructed reference trajectory was used as the true value, and the positioning accuracy of the original visual inertial navigation trajectory and the fused trajectory were evaluated respectively.

[0042] The experiment used root mean square error (RMSE) and absolute trajectory error (ATE) as evaluation metrics.

[0043] The experimental results are shown in Table 1:

[0044] Table 1 Comparative Experimental Results

[0045] ;

[0046] The results show that the present invention can effectively reduce the cumulative drift of visual inertial navigation, make the fused trajectory closer to the real trajectory, and significantly improve the positioning accuracy of underwater robots.

[0047] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A visual-inertial and ultra-short baseline integrated navigation method based on factor graph optimization, characterized in that, include: S1. After the underwater robot system is started, the visual inertial navigation system, the ultra-short baseline underwater acoustic positioning system, and the factor graph optimization system are initialized. S2. Establish visual-inertial relative motion constraints, combine the covariance matrix of the pre-integration constraints of the visual-inertial navigation system to obtain the visual-inertial weighted residuals, use the visual-inertial state estimator as the front end of the visual-inertial navigation system to output the robot pose information, and standardize the trajectory file output by the visual-inertial state estimator into the initial trajectory optimized by the factor graph. S3. Obtain the absolute position observations of the ultra-short baseline underwater acoustic positioning system, establish the position residuals of the ultra-short baseline underwater acoustic positioning system by combining the observation covariance matrix of the ultra-short baseline underwater acoustic positioning system, and construct the weighted residuals of the ultra-short baseline underwater acoustic positioning system based on the position residuals of the ultra-short baseline underwater acoustic positioning system. S4. Based on the time series of the visual inertial navigation system and the observation time series of the ultra-short baseline underwater acoustic positioning system, extract the common time intervals of the visual inertial navigation trajectory and the observation data of the ultra-short baseline underwater acoustic positioning system, and retain the visual inertial trajectory and the observation data of the ultra-short baseline underwater acoustic positioning system located within the common time interval; establish the correspondence between the observation data of the ultra-short baseline underwater acoustic positioning system and the visual inertial pose nodes using the time nearest neighbor principle; and transform the visual inertial trajectory to the world coordinate system using a similarity transformation. S5. Construct Huber weights to screen factors for the ultra-short baseline underwater acoustic positioning system, establish a factor graph objective function, use the LM algorithm for iterative optimization, and output the optimal trajectory.

2. The visual-inertial and ultra-short baseline integrated navigation method based on factor graph optimization according to claim 1, characterized in that, S1 includes S1.1, Visual Inertial Navigation System Initialization, which includes initializing IMU bias, initializing camera intrinsic parameters, initializing the visual inertial coordinate system, initializing the first frame pose, and establishing the visual inertial local navigation coordinate system. S1 includes S1.2, initialization of the ultra-short baseline underwater acoustic positioning system, including initializing the coordinate system of the ultra-short baseline underwater acoustic positioning system, reading the measurement data of the ultra-short baseline underwater acoustic positioning system, reading the timestamp of the ultra-short baseline underwater acoustic positioning system, and establishing the observation sequence of the ultra-short baseline underwater acoustic positioning system; The system has a total of A visual inertial pose node One observation node for an ultra-short baseline underwater acoustic positioning system, defined robot status at all times for: ； ； In the formula, Let the robot's position vector be... Let x be the robot's x-axis coordinate. Let be the robot's y-axis coordinate. Let z be the robot's z-axis coordinate. For robot speed; This is the robot's posture rotation matrix; Zero bias for the gyroscope; To achieve zero bias in the accelerometer; S1 includes S1.3, and the initialization of the factor graph optimization system includes setting the robot position state variables of the first frame. The initial value position is set to , .

3. The visual-inertial and ultra-short baseline combined navigation method based on factor graph optimization according to claim 2, characterized in that, S2 includes establishing visual-inertial relative motion constraints: ； In the formula, For visual inertial residuals, for The position output by the visual inertial navigation system at any given time; Define the covariance matrix of the pre-integral constraints for a visual inertial navigation system. Visual inertial weighted residuals were obtained. : ； A visual-inertial state estimator is used as the front end of the visual-inertial navigation system to output robot pose information. : ； In the formula, For the first Time stamp For the robot's pose quaternion; Visual-inertial state estimator output trajectory file : ； In the formula, These are the four components of the attitude quaternion; Will After standardization, the initial trajectory for factor graph optimization is obtained. : ； In the formula, For the first The local coordinate system position of each node.

4. The visual-inertial and ultra-short baseline combined navigation method based on factor graph optimization according to claim 3, characterized in that, S3 includes, S3.1, the ultra-short baseline underwater acoustic positioning system in the... The absolute position observation value output at the next observation time for: ； In the formula, for The component in the x-direction, for The component in the y-direction, for The component in the z-direction, It is the transpose symbol. For the sequencing number index of ultra-short baseline underwater acoustic positioning systems; Constructing observation sequences for an ultra-short baseline underwater acoustic positioning system : ； The measurement model of the ultra-short baseline underwater acoustic positioning system is represented as follows: ； In the formula, For the first The measurement noise of the ultra-short baseline underwater acoustic positioning system at the observation time of the sub-ultra-short baseline underwater acoustic positioning system. In the first The sub-ultra-short baseline underwater acoustic positioning system observes the robot's true position at any given moment.

5. The visual-inertial and ultra-short baseline combined navigation method based on factor graph optimization according to claim 4, characterized in that, S3 includes, in S3.2, constructing the observation covariance matrix of the ultra-short baseline underwater acoustic positioning system. : ； In the formula, The degree of dispersion of noise in the x-direction. The degree of dispersion of noise in the y-direction. The degree of dispersion of noise in the z-direction; Establishing the position residual of an ultra-short baseline underwater acoustic positioning system : ； In the formula, For the back-end factor graph, the first The positional variables to be optimized for each node. Number the visual inertial pose nodes; Establish weighted residuals for ultra-short baseline underwater acoustic positioning system : 。 6. The visual-inertial and ultra-short baseline combined navigation method based on factor graph optimization according to claim 5, characterized in that, S4 includes, S4.1, extracting the common time interval of visual inertial navigation trajectory and ultra-short baseline underwater acoustic positioning system observation data, whereby the visual inertial navigation trajectory includes a visual inertial navigation trajectory time series and... The observation data of the ultra-short baseline underwater acoustic positioning system includes the observation time series and... ; Let the time series of the visual inertial navigation system be... for: ； In the formula, For the first The specific timestamp of each visual inertial node. This serves as an index for the time series of a visual-inertial navigation system. Observation time series of ultra-short baseline underwater acoustic positioning system for: ； In the formula, For the first Specific timestamps of observations by the sub-ultra-short baseline underwater acoustic positioning system; S4 includes S4.2, calculating the common time interval: ； ； In the formula, The common start time of the two system data. The common end time of the two system data. To find the maximum value, To find the minimum value; Preserve visual inertial trajectories and ultra-short baseline underwater acoustic positioning system observation data located within a common time interval.

7. The visual-inertial and ultra-short baseline integrated navigation method based on factor graph optimization according to claim 6, characterized in that, S4 includes, S4.3, establishing the correspondence between observation data of the ultra-short baseline underwater acoustic positioning system and visual-inertial pose nodes using the time nearest neighbor principle, for the first... The observations from the ultra-short baseline underwater acoustic positioning system correspond to the following visual-inertial pose node numbers: ； In the formula, To minimize the objective function ; Set the maximum allowed time difference ,when No correlation is established between observations from the ultra-short baseline underwater acoustic positioning system and visual-inertial pose.

8. The visual-inertial and ultra-short baseline combined navigation method based on factor graph optimization according to claim 7, characterized in that, S4 includes S4.4, where the trajectory of the visual inertial navigation system projected onto the world coordinate system after similarity transformation is... ,Establish and Coordinate transformation relationship between them: ； In the formula, As a scale factor, It is a three-dimensional rotation matrix. It is a three-dimensional translation vector; Construct the optimization objective function: ； In the formula, In order to be with the first The sub-ultra-short baseline underwater acoustic positioning system observes the time-matched visual-inertial navigation system node positions; The objective function is solved using singular value decomposition, and the optimal rotation matrix is ​​output. Optimal scaling factor Optimal translation vector ; Will , , Substituting the coordinate transformation relationship, transform all visual inertial navigation system outputs to the world coordinate system: ； The final output is the aligned visual inertial reference trajectory. : 。 9. The visual-inertial and ultra-short baseline integrated navigation method based on factor graph optimization according to claim 8, characterized in that, S5 includes, S5.1, constructing Huber weights: ； In the formula, For the factor weights of the ultra-short baseline underwater acoustic positioning system, Let be the residual norm. , For Huber threshold; Set minimum weight threshold ,when Preserve the ultra-short baseline underwater acoustic positioning system factor; if Remove the ultra-short baseline underwater acoustic positioning system factor; Set of factors for ultra-short baseline underwater acoustic positioning system optimization for: ； In the formula, For the first A factor for an ultra-short baseline underwater acoustic positioning system.

10. The visual-inertial and ultra-short baseline integrated navigation method based on factor graph optimization according to claim 9, characterized in that, S5 includes S5.2, constructing the objective function of the factor graph. : ； ； In the formula, The set of all state variables to be optimized. ; For a priori residuals; Using the LM algorithm Perform iterative optimization, and calculate the residual Jacobian matrix in each iteration. : ； In the formula, The global residual vector is formed by concatenating the residuals of all factors. This is the vectorized form of all the state variables to be optimized; Construct the normal equation: ； In the formula, The damping factor, For state increments; Update status: ； In the formula, This represents the current iteration number of the LM algorithm. For the first The state vector at the next iteration; The optimization ends when the objective function converges or the maximum number of iterations is reached, and the optimal robot trajectory is output. : ； In the formula, For the first The optimal trajectory for each node.

Images