Underwater Pose Estimation Method Based on Bidirectional Pre-Integration of Visual-Inertial Pressure Fusion

Abstract

The present invention discloses an underwater pose estimation method based on bidirectional pre-integration of vision, inertial and pressure. The optimization problem constructed by using the visual observation constraints, IMU pre-integration observation constraints and pressure gauge pre-integration constraints of each key frame can efficiently and accurately realize the initial values of the scale of the system state parameters, the velocity of the key frame, the zero bias of the acceleration, and the distance between the pressure gauge and the sea level, and realize the initialization of the system state parameters, which is beneficial to subsequent parameter updates. Starting from the initial parameters, the present invention constructs a maximum a posteriori estimation problem through the IMU pre-integration residual, the bidirectional pre-integration residual of the pressure gauge and the visual reprojection residual to update the parameters, and can relatively accurately realize the underwater pose estimation.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater robot navigation control, and particularly relates to an underwater pose estimation method based on bidirectional pre-integration of visual inertial pressure fusion. Background Technique

[0002] With the continuous deepening of marine resource development, especially the construction and operation of underwater infrastructures such as offshore wind power, offshore oil platforms, and submarine pipelines, the health monitoring and defect detection of underwater structures have become increasingly important. Traditional underwater inspection methods mainly rely on manual divers or remotely operated underwater vehicles, but these methods are not only costly and inefficient but also pose certain safety risks. Especially in complex underwater environments, manual inspection faces many challenges, such as low visibility, difficulty in obtaining high-precision positioning information, and high costs caused by long-term operations. Therefore, how to achieve autonomous, intelligent, low-cost, and efficient positioning and navigation of underwater robots has become a research hotspot in current underwater inspection technologies.

[0003] The patent application with the publication number CN116483064A discloses an underwater robot and its dynamic positioning method. First, obtain the current pose and desired pose of the underwater robot, and the desired pose is determined according to the operation target; subtract the current pose of the underwater robot from the desired pose to obtain a pose error; then, input the pose error and environmental disturbances into a neural network controller to obtain the propulsion amounts of the thrusters in the dynamic propulsion system; then, control each thruster to act according to the obtained propulsion amounts to change the pose of the underwater robot so that the underwater robot completes the fixed-point operation task.

[0004] The patent application with the publication number CN116659510 discloses an underwater robot positioning and obstacle avoidance method, device, and storage medium. Among them, the underwater robot positioning and obstacle avoidance method includes the following specific steps: obtain the initial pose data of the underwater robot before diving; during the diving process of the underwater robot, obtain the real-time pose data of the underwater robot according to the initial pose data to obtain the first real-time pose data, and construct a first seabed environment map through sonar; after the underwater robot reaches the working water area, update the first real-time pose data and the first seabed environment map based on SLAM through a multi-category sensor group to obtain the second real-time pose data and the second seabed environment map; according to the second real-time pose data and the second seabed environment map, perform real-time positioning and obstacle avoidance path planning on the underwater robot, thereby realizing a technical solution for improving the positioning and obstacle avoidance accuracy of the underwater robot.

[0005] However, in the underwater positioning systems disclosed in the above patent applications, there are still many difficulties in positioning accuracy and stability.

[0006] Traditional underwater positioning methods, such as ultrasonic or sonar-based systems, are usually limited by the attenuation of signal propagation. The accuracy is affected by factors such as water depth, environmental noise, and terrain complexity, and the cost is relatively high, which is not conducive to the popularization and use of low-cost underwater robots.

[0007] Although vision-based positioning methods can provide relatively accurate positioning information, in the underwater environment, due to factors such as insufficient light, turbid waters, and surface reflections, the extraction of image quality and visual features is often affected, thereby reducing the reliability of positioning.

[0008] In addition, although existing inertial navigation systems can provide relatively accurate positioning in the short term, errors will accumulate after long-term use, and they cannot meet the requirements of long-term and long-distance precise positioning. Summary of the Invention

[0009] The present invention provides a vision-inertial-pressure fusion underwater pose estimation method based on bidirectional pre-integration, which can accurately and reliably realize the pose estimation of an underwater robot.

[0010] The present invention provides a vision-inertial-pressure fusion underwater pose estimation method based on bidirectional pre-integration, including:

[0011] S1. Extract features from the current frame after defogging enhancement and the key frame at the previous moment to obtain feature points and descriptors. At the same time, obtain the current frame pose through IMU recursion, project the landmark points onto the current frame as the tracking initial value, perform matching tracking, and then update the feature points of the current frame through outlier removal and depth filtering to make the feature points converge. Determine whether the current frame is a key frame based on the number of converged feature points or the parallax with the feature points of the historical frame;

[0012] S2. Repeat step S1. When the number of obtained key frames is higher than the set threshold, estimate the gyroscope zero bias based on the positive definite constraint between consecutive frames in the key frames, use the rotation obtained by removing the gyroscope zero bias as a constraint to solve the SFM problem to obtain the initial values of the system state parameters of each key frame, and construct an optimization problem based on the visual observation constraints, IMU pre-integration observation constraints, and pressure gauge pre-integration constraints of each key frame to solve the scale of the system state parameters, the velocity of the key frame, the acceleration zero bias, and the initial value of the distance between the pressure gauge and the sea level, realize the initialization of the system state parameters, and freeze step S2;

[0013] S3. Repeat step S1, construct a sliding window based on multiple key frames closest to the current moment, and update the system state parameters and inverse depth parameters of multiple key frames in the sliding window through the maximum a posteriori estimation problem constructed by the IMU pre-integration residual, the pressure gauge bidirectional pre-integration residual, and the visual reprojection residual to realize underwater pose estimation.

[0014] Preferably, a two-way pre-integration residual of the pressure gauge is constructed based on the two-way pre-integration of the pressure gauge and the system state parameters. By comparing the time when the pressure gauge data arrives with the times of the two closest key frames before and after, when the time when the pressure gauge data arrives is closer to the forward key frame, the forward pressure gauge two-way pre-integration constraint is used; when the time when the pressure gauge data arrives is closer to the backward key frame, the backward pressure gauge two-way pre-integration constraint is used;

[0015] The two-way pre-integration residual of the pressure gauge estimated using the forward pressure gauge two-way pre-integration constraint is: , where is the two-way pre-integration residual of the pressure gauge, is the pressure value at time P k to the pressure value at time j P j of the forward pressure gauge two-way pre-integration constraint, are the system state parameters and the inverse depth parameter, , is the gravity vector in the world coordinate system, is the distance between the pressure gauge and the sea level obtained by initialization, , , are respectively the position, velocity and attitude of the k-th key frame in the world coordinate system w , is the k th key frame at time to j the time difference between time is the predicted position component of the k th key frame in the body coordinate system to j time;

[0016] The two-way pre-integration residual of the pressure gauge estimated using the backward pressure gauge two-way pre-integration constraint is: .

[0017] Preferably, the forward pressure gauge two-way pre-integration constraint is the same as the IMU pre-integration observation constraint of the forward key frame;

[0018] The backward pressure gauge two-way pre-integration constraint is the same as the IMU pre-integration observation constraint of the backward key frame. Through the backward pressure gauge two-way pre-integration constraint, the pre-integration quantities of the position, velocity and rotation of the k+1-th key frame at the i th IMU time in the body coordinate system are obtained , , are respectively: , , , where is the accelerometer measurement value at the (i + 1)-th IMU moment, is the accelerometer bias at the i-th IMU moment, is the gyroscope measurement value at the (i + 1)-th IMU moment, is the gyroscope bias at the (i + 1)-th IMU moment, is the logarithmic calculation on Lie algebra.

[0019] Preferably, the specific steps of step S2 include:

[0020] Solving for the gyroscope bias initialization value based on the positive definite constraint between consecutive frames in the key frames to obtain the gyroscope bias initialization value;

[0021] Using the gyroscope bias initialization value to remove the bias of the rotation, taking the rotation with the bias removed as a constraint to solve the SFM problem with prior, and constructing the initial values of the system state parameters based on the solution results, the gyroscope bias initialization value, and the rotation with the bias removed. The system state parameters include scale, and the velocity, attitude, position, accelerometer bias, and gyroscope bias of the key frames, as well as the distance between the pressure gauge and the sea level;

[0022] Updating the scale of the system state parameters, the velocity of the key frames, the accelerometer bias, and the distance between the pressure gauge and the sea level based on the optimization problem constructed by the visual observation constraint and the IMU pre-integration observation constraint of each key frame and the system state parameters, so as to realize the initialization of the system state parameters.

[0023] Preferably, solving for the gyroscope bias initialization value based on the positive definite constraint between consecutive frames in the key frames is: , , , , , , where is the gyroscope bias, is the eigenvalue, k is the index of the landmark co-observed by the i-th key frame and the j-th key frame, n is the number of co-observed landmarks, is the extrinsic parameter of the rotation between the IMU and the camera, is the true value of the rotation pre-integration quantity from the i-th key frame to the j-th key frame in the body coordinate system, , is the unit vector of the k-th landmark co-observed by the i-th key frame and the j-th key frame, is the derivative of the rotation pre-integration quantity from the i-th key frame to the j-th key frame with respect to the gyroscope bias in the body coordinate system, is an anti-symmetric matrix, is the set of all key frames.

[0024] Preferably, based on the descriptors, the feature points of the current frame are successively updated through optical flow tracking, outlier removal, and depth filtering, including:

[0025] Based on the descriptors, the feature points of the current frame are matched with the feature points of the historical frame using the optical flow of consecutive images. After the matching of the feature points is completed, outlier removal is performed on the feature points. Then, a depth filter is used to update the depth and depth uncertainty of the feature points of the current frame until the uncertainty is less than the set threshold and the feature points converge. The converged feature points are triangulated to obtain the landmarks.

[0026] Preferably, before performing outlier removal, when the number of feature points tracked by optical flow is less than the set threshold, a co-visible key frame tracking operation is performed, including:

[0027] When the number of feature points tracked by optical flow is less than the set threshold, multiple co-visible key frames are selected in the local map. Based on the multiple co-visible key frames, the pose of the current frame is obtained through IMU recursion, that is, the visual observation constraint. At the same time, the landmarks in the sliding window constructed by the current key frame of the multiple co-visible key frames are projected onto the current frame, and the feature points of the projected current frame and the historical frame are matched using the descriptors.

[0028] Preferably, if the number of tracked points is less than the given threshold after the co-visible key frame tracking operation, a fast relocalization operation is performed, including:

[0029] In the local map, a bag-of-words search and descriptor matching are performed to obtain the correspondence between the feature points and landmarks of the current frame. Based on the correspondence between the feature points and landmarks of the current frame, PNP calculation is performed to obtain the pose of the current frame.

[0030] Preferably, it is determined whether the current frame is a key frame based on the number of converged feature points or the comparison result with the feature points of the historical frame, including:

[0031] The disparity between the current frame and the previous key frame is calculated based on the correspondence between the feature points of the previous key frame and the current frame. When the disparity is greater than the set disparity threshold, the current frame is set as a key frame;

[0032] Or, when the number of feature points of the current frame is less than the set quantity threshold, the current frame is used as a key frame;

[0033] Or, when the time difference between the current frame and the previous key frame is greater than the set time threshold, the current frame is used as a key frame.

[0034] Preferably, while performing step S3, loop closure detection is also performed using the bag-of-words, including:

[0035] When a new key frame is added to the sliding window, multiple FAST corner points are extracted from the new key frame, and a quadtree is used for homogenization operation. At the same time, corresponding descriptors are extracted for each FAST corner point to form a corresponding dictionary. Based on the formed dictionary, similarity detection is performed on the historical key frames using the bag of words.

[0036] When the set similarity threshold is exceeded, loop preprocessing is performed: calculate the 2D-3D data association between the current key frame and the historical key frames. When the number of inliers is more than the inlier number threshold, the loop detection is successful.

[0037] Calculate the pose of the current key frame in the world system without cumulative drift through PNP, which is used as the observation quantity for pose graph optimization. Use Ceres to solve the pose graph optimization problem to obtain a globally consistent point cloud of the trajectory.

[0038] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0039] The optimization problem constructed by the present invention using the visual observation constraints, IMU pre-integration observation constraints, and pressure gauge pre-integration constraints of each obtained key frame can efficiently and accurately realize the scale of the system state parameters, the speed of the key frame, the zero bias of the acceleration, and the initial value of the distance between the pressure gauge and the sea level, and realize the initialization of the system state parameters, which is beneficial to subsequent parameter updates.

[0040] Starting from the initial parameters, the present invention updates the parameters through the maximum a posteriori estimation problem constructed by the IMU pre-integration residual, the pressure gauge two-way pre-integration residual, and the visual reprojection residual, and can relatively accurately realize the estimation of the underwater pose. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a schematic structural diagram of an underwater binocular vision pressure positioning module provided by a specific embodiment of the present invention;

[0042] Figure 2 It is a system block diagram of a method for estimating underwater pose by visual inertial pressure fusion based on two-way pre-integration provided by a specific embodiment of the present invention;

[0043] Figure 3 It is a schematic diagram of a front-back two-way pressure gauge pre-integration selection strategy provided by a specific embodiment of the present invention;

[0044] Figure 4 It is a schematic diagram of constructing a positive definite constraint for gyroscope zero bias optimization provided by a specific embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0045] In order to make the objectives, technical solutions and advantages of the present invention more clear and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. The specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0046] As Figure 1 The hardware structure involved in the present invention includes a binocular camera, an IMU and a pressure gauge, as well as an on-board processor, all of which are placed in a cabin that can withstand a pressure of 400 meters, constituting the smallest hardware unit that can execute the algorithm of the present invention. By integrating the advantages of various sensors such as vision, inertial navigation, and pressure gauges, the present invention designs a special algorithm module to ensure fast response and real-time positioning in complex underwater environments. Moreover, it is small in size, easy to carry, ready to use immediately after power-on, and simple to operate. The overall hardware cost of the system is low, providing an efficient and low-cost positioning solution for underwater robots, with broad application prospects, especially suitable for underwater operations and inspection tasks with high requirements for cost and real-time performance. Figure 2 Then it is the overall framework and process of the algorithm. Figure 3 、 Figure 4 They are respectively schematic diagrams of different modules of the algorithm. Specifically, the specific embodiment of the present invention provides a visual-inertial-pressure fusion underwater pose estimation method based on bidirectional pre-integration. As Figure 2 shown, it includes:

[0047] S1. Extract features from the current frame after defogging enhancement and the key frame at the previous moment to obtain feature points and descriptors. At the same time, obtain the pose of the current frame through IMU recursion, project the landmark points onto the current frame as the initial value for tracking, perform matching tracking, and then update the feature points of the current frame through outlier removal and depth filtering to make the feature points converge. Determine whether the current frame is a key frame based on the number of converged feature points or the parallax with the feature points of the historical frame.

[0048] In a specific embodiment, dehazing enhancement is performed on the collected underwater images: First, the collected underwater images are grayscaled, denoised, and edge-enhanced. Gaussian filtering is used for denoising to reduce the noise generated by the turbid water quality in the underwater images. Edge enhancement improves the important details in the image, such as the structural contours, through operators, providing richer information for subsequent image analysis. Then, the atmospheric scattering model is utilized. It is assumed that the brightness attenuation of the image is caused by the scattering of suspended particles and molecules in the water body. By analyzing the dark channel or color information in the image, the illumination intensity in the underwater scene is estimated, and the transmission map of the underwater image is estimated to compensate for the illumination attenuation. At the same time, the estimated 3D point depth information is used as a priori. The details and contrast in the image can be restored through a weighted average and depth-guided dehazing algorithm. Farther objects are usually more affected, while closer objects are relatively less affected. Therefore, different image regions at different distances can be enhanced differentially according to the depth information. For color distortion, the color deviation of the underwater image is modeled and adjusted using adaptive white balance and tone mapping methods to effectively restore the color information of the image. Adaptive histogram equalization is used to enhance the contrast of the image, making the important features in the image more obvious and further improving the accuracy of subsequent positioning and visual analysis.

[0049] In a specific embodiment, SuperPoint is used to extract feature points and descriptors from the current frame after dehazing enhancement and the key frame at the previous moment.

[0050] In a specific embodiment, based on the descriptors, the feature points of the current frame are updated through optical flow tracking, outlier removal, and depth filtering in sequence, including:

[0051] Based on the descriptors, the feature points of the current frame are matched with the feature points of the historical frame using the optical flow tracking of consecutive images. After the matching is completed, the outlier removal is performed on the feature points. Then, a depth filter is used to update the depth and the uncertainty of the depth of the feature points of the current frame until the uncertainty is less than the set threshold and the feature points converge. The converged feature points are triangulated to obtain the landmarks.

[0052] Specifically, based on the descriptors, the feature points of the current frame are matched with the feature points of the historical frame using the optical flow tracking of consecutive images, including:

[0053] According to the gray-scale invariance hypothesis, the gray-scale error is minimized : , where represents the gray scale of a certain point on the image (the gray-scale loss value is approximated to the set value to iterate dx and dy. After the iteration is completed, the corresponding relationship between the feature points in the current frame and the historical frame is obtained),,, represents the coordinates of the historical frame, represents the time between the two frames, and x+dx, y+dy are the coordinates of the current frame.

[0054] In a specific embodiment of the present invention, to prevent incorrect matching, the RANSAC (Random Sample Consensus) method is adopted to remove outliers and obtain the corresponding relationship between the feature points of the final historical frame and the current frame. At the same time, the depth filter is updated based on the matching result to update the depth and depth uncertainty of the feature points. When the uncertainty is less than a certain threshold, in a specific embodiment, the threshold is set to 200, then it is considered that the feature point has converged and can be triangulated into a landmark point.

[0055] Before removing outliers in a specific embodiment of the present invention, when the number of feature points tracked by optical flow is less than the set threshold, the co-visible key frame tracking operation is performed, including:

[0056] When the number of feature points tracked by optical flow is less than the set threshold, multiple co-visible key frames are selected in the local map. In one embodiment, the number of co-visible key frames is 5. The co-visible key frames are those with an overlapping area between the historical camera view and the current camera view. The poses of these co-visible key frames are known. Based on multiple co-visible key frames, the pose of the current frame is obtained through IMU recursion, that is, visual observation constraints. At the same time, the landmark points in the sliding window constructed by the current key frame of multiple co-visible key frames are projected onto the current frame, and the feature points of the projected current frame and the historical frame are matched using descriptors to enhance the robustness of front-end tracking.

[0057] If, after the co-visible key frame tracking operation, the number of tracked points is still less than the given threshold, a specific embodiment of the present invention performs a fast relocalization operation, including:

[0058] The corresponding relationship between the feature points of the current frame and the landmark points is obtained by matching in the local map and descriptors. Based on the corresponding relationship between the feature points of the current frame and the landmark points, PNP (Perspective-n-Point) calculation is performed to obtain the pose of the current frame. Since the fast relocalization operation consumes a large amount, it is only triggered when tracking fails due to severe motion or texture loss, and the fast relocalization is started to attempt to recover the pose.

[0059] In a specific embodiment, this embodiment determines whether the current frame is a key frame based on the number of converged feature points or the comparison result with the feature points of the historical frame, including: calculating the parallax between the current frame and the previous key frame based on the correspondence relationship (the position coordinates are subtracted to obtain the parallax) of the feature points of the previous key frame and the current frame. When the parallax is greater than the set parallax threshold, in one embodiment, the parallax threshold is 30 pixels, then the current frame is set as a key frame; or, when the number of feature points in the current frame is less than the set quantity threshold, in one embodiment, the quantity threshold is 150, then the current frame is used as a key frame; or, when the time difference between the current frame and the previous key frame is greater than the set time threshold, the time threshold is 3s, then the current frame is used as a key frame. By the above strategy of selecting key frames, the predicted score constraint stability between two adjacent states in the sliding window is ensured.

[0060] In a specific embodiment, after obtaining the key frame in this embodiment, the feature points of the obtained key frame are extracted again. First, the feature point extraction area is set according to the tracked feature points. Among them, in order to extract evenly, the area where the tracked feature points are excluded is used to prevent repeated extraction, and then the SuperPoint feature extractor is used to perform grid-based corner extraction.

[0061] S2. Repeat step S1. When the number of obtained key frames is higher than the set threshold, estimate the gyroscope zero bias based on the positive definite constraint between consecutive frames in the key frames. Use the rotation obtained by removing the gyroscope zero bias as a constraint to solve the SFM problem to obtain the initial values of the system state parameters of each key frame. Based on the visual observation constraints of each key frame, the optimization problem constructed by the IMU pre-integration observation, the pressure gauge observation constraint, and the system state parameters updates the initial values of the scale of the system state parameters, the velocity of the key frame, the acceleration zero bias, and the distance between the pressure gauge and the sea level. The reason for obtaining the initial values of the above parameters is that the pose obtained by SFM solving has no scale, so the scale needs to be estimated. Also, because the velocity, acceleration zero bias, and the distance from the sea level at the initial moment are all unknown, but we want to perform recursion based on the current moment, we need to know these quantities to initialize the system state parameters and freeze step S2.

[0062] Specifically, the gyroscope bias initialization value is obtained by solving the gyroscope bias based on the positive definite constraint between consecutive frames in the key frames; the gyroscope bias initialization value is used to remove the bias of the rotation, and the rotation with the bias removed is used as a constraint to solve the SFM problem with prior knowledge. Based on the solution results, the gyroscope bias initialization value, and the rotation with the bias removed, the initial values of the system state parameters are constructed. The system state parameters include scale, as well as the velocity, attitude, position, acceleration bias, and gyroscope bias of the key frames, and the distance between the pressure gauge and the sea level; based on the visual observation constraints and IMU pre-integration observation constraints of each key frame and the optimization problem constructed by the system state parameters, the scale of the system state parameters, the velocity of the key frames, the acceleration bias, and the distance between the pressure gauge and the sea level are updated to initialize the system state parameters.

[0063] In a specific embodiment, this embodiment provides IMU pre-integration observation constraints and pressure gauge pre-integration constraints. In this embodiment, the fusion algorithm design is carried out using three sensors with different frequencies and different clocks, and the influence of the time delay needs to be fully considered.

[0064] For two consecutive key frames in the specific embodiment of the present invention For the IMU measurements between them, the IMU pre-integration observation constraints are formed by using the pre-integration method. The pre-integration formula at discrete time is: , , , where , , are the pre-integration quantities of position, velocity, and rotation at the time of the (i + 1)-th IMU data in the body coordinate system, respectively, which can be used for the IMU pre-integration observation constraints of adjacent image frames. represents the time difference between the i-th and (i + 1)-th consecutive IMU times. is the accelerometer measurement value. is the accelerometer bias at the i-th IMU time. is the angular velocity bias at the i-th IMU time. is the gyroscope measurement value. represents the logarithmic calculation on the Lie algebra.

[0065] At the same time, through the recurrence formula at discrete time, the covariance matrix of the pre-integration quantity can be obtained, as well as the first-order Jacobian matrix , the pre-integration component is only related to the biases of the accelerometer and gyroscope. Using the first-order Taylor approximation to model the impact of bias variations on the pre-integration measurement can avoid re-integrating all IMU data after the system state variables are updated, greatly saving computational time. The formula is as follows: , , , Among them, , , are the ideal values of the position pre-integration, velocity pre-integration, and rotation pre-integration from the initial time to the current time respectively. After obtaining them, they can be used as the IMU measurement constraints between adjacent image frames for the optimization module. As the IMU measurement constraints between adjacent image frames for the optimization module. is the derivative of the position pre-integration component with respect to the accelerometer bias, is the derivative of the position pre-integration component with respect to the gyroscope bias, is the derivative of the velocity pre-integration component with respect to the accelerometer bias, is the derivative of the velocity pre-integration component with respect to the gyroscope bias, is the derivative of the rotation pre-integration component with respect to the gyroscope bias, is the update amount of the accelerometer bias, is the update amount of the gyroscope bias. When the bias variation is too large, the error of the first-order Taylor approximation is relatively large. In this case, the entire IMU measurement data is re-integrated.

[0066] The observation model of the pressure gauge measurement value provided by the specific embodiment of the present invention is: , where, is the observed value, is the true value, is white noise, following a Gaussian distribution with a mean of 0 and a variance of.

[0067] The specific embodiment of the present invention performs a two-way pre-integration strategy on the observed value of the pressure gauge. Based on the two-way pre-integration of the pressure gauge and the system state parameters, a two-way pre-integration residual of the pressure gauge is constructed. By comparing the time when the pressure gauge data arrives with the times of the two closest key frames before and after, when the time when the pressure gauge data arrives is closer to the forward key frame, the forward pressure gauge two-way pre-integration constraint is used, and when the time when the pressure gauge data arrives is closer to the backward key frame, the backward pressure gauge two-way pre-integration constraint is used.

[0068] As Figure 3 shown, is the key frame data, is the pressure gauge data, and T is the time axis when the corresponding data arrives. Assume the pressure gauge data The arrival time is , located between two key frames and . Then, according to the time difference between the two key frames , since , is assigned to , at this time, the forward pre-integration is directly used, that is, the IMU pre-integration can be used. And since , is assigned to , at this time, the backward pre-integration calculation needs to be used. The original pre-integration formula can no longer meet the requirements. The backward pressure gauge two-way pre-integration constraint is the same as the IMU pre-integration observation constraint of the backward key frame. Through the backward pressure gauge two-way pre-integration constraint, the pre-integration quantities of the position, velocity, and rotation at the i th IMU moment of the (k + 1)th key frame in the body coordinate system are obtained , , respectively as follows: , , wherein, is the accelerometer measurement value at the (i + 1)th IMU moment, is the accelerometer zero bias at the ith IMU moment, is the gyroscope measurement value at the (i + 1)th IMU moment, is the gyroscope zero bias at the (i + 1)th IMU moment, is the logarithmic calculation on the Lie algebra.

[0069] corresponds to Figure 3 . Here, the corresponding moment is Figure 3 in . i is to in a certain IMU moment. In this step, the measurement data of the IMU and the pressure gauge are preprocessed by the forward and backward pre-integration methods proposed by the present invention. In the subsequent steps, the obtained visual observation constraint, IMU observation constraint, and pressure gauge two-way observation constraint will be used for initialization and visual-inertial pressure gauge joint optimization respectively.

[0070] This embodiment initializes the system state using the maximum a posteriori estimation and positive definite constraint, as Figure 2As shown in the initialization part in, the measured values of the three types of sensor data used in the present invention are preprocessed in the above steps. Key frames are obtained from the image data, and bidirectional pre-integration constraints are obtained from the IMU data and the pressure gauge data. In this step, when the number of key frames of the system reaches the threshold, in one embodiment, when the threshold is 10, the system is initialized by combining the processed measurement data. The specific steps are as follows:

[0071] First, use the positive definite constraint between consecutive frames in the key frames to solve for the gyroscope bias. As shown in Figure 4, assume a 3D point can be seen by two frames simultaneously, and are the camera optical centers of the two frames respectively. At this time, the three points can define an epipolar plane. Let and represent to and unit vectors respectively. Then the vertical vector of the epipolar plane can be obtained by the following formula: ; where represents the skew-symmetric matrix, k represents the index of the landmark points that can be seen by both the i-th and j-th key frames. Since all the vertical vectors are perpendicular to the translation vector between the two cameras, these vertical vectors are coplanar. Combine these vertical vectors to obtain the matrix . Its coplanarity is mathematically equivalent to the minimum eigenvalue of the matrix is, and only in the M matrix is the unknown quantity, represents the camera i and the camera j relative rotation between, so solving can define the following optimization problem: , , where represents the optimal solution of this optimization problem, represents the matrix constructed by the co-visible landmark points between camera i and camera j, is the minimum eigenvalue of the matrix .

[0072] When the external parameters and of the rotation and translation between the IMU and the camera are known, the above constraints are transformed into the IMU coordinate system. At the same time, the influence of the gyroscope bias on the system can be calculated by the first-order Taylor approximation. The formula is as follows:

[0073] ; where is the gyroscope bias, is the measured value of the rotational pre-integration component, is the true value of the rotational pre-integration component, is the derivative of the rotational pre-integration component with respect to the gyro bias. In the present invention, since the change rate of the gyro bias with time is slow, it is assumed that the gyro bias is a constant during the initialization phase. Combining the external parameters and substituting Equation (1.14) into (1.13), a new matrix can be obtained , , , , ,, at this time the only unknown in the matrix is the gyro bias. The optimization problem for in Equation (1.12) can be converted into an optimization problem for the gyro bias.

[0074] Let be the set of all key frames. During the initialization phase, all key frames share a gyro bias. Each pair of key frames with sufficient observations can be used for bias estimation. Combining all key frame observations, the following optimization problem is constructed: ; ; where is the optimal gyro bias obtained by solving, is the smallest eigenvalue. The gyro bias can be obtained by iteratively solving this problem with Ceres.

[0075] (2) After the initialization of the gyro bias is completed, use to obtain the rotation after removing the bias, and use the rotation constraint to solve the SfM problem with prior information to obtain the positions and attitudes of all states in the scale-equivalent sliding window, where has scale equivalence. The difference between the true position and the position calculated by SfM is a scale factor s, that is .

[0076] (3) After the first two steps, the attitudes, gyro biases, and scale-equivalent positions required for initialization have been solved. In the last step, the positions, velocities, attitudes, accelerometer biases, gyro biases, scale s, and pressure gauge initial values () of each state of the key frames will be used as state variables. The variables to be optimized in this problem include the scale s, the velocities of all key frames, the acceleration bias , the distance between the pressure gauge and the sea level ; use the pre-integration constraint and the visual landmark observation constraint provided by the key frames to construct the following optimization problem: , where represents the set of all key frames, K represents the sequence number of the key frame, represents the IMU pre-integration observation constraint, represents the visual landmark observation constraint, are the variables to be optimized in this solution.

[0077] Initialize and solve the entire problem. Combining the results of steps 1 and 2, the position of the initial state can finally be obtained , velocity and attitude , as well as the accelerometer and gyroscope biases , as well as the distance between the pressure gauge and the sea level , will be continuously adjusted in subsequent optimizations.

[0078] S3. Repeat step S1. Based on multiple key frames closest to the current moment, construct a sliding window. Taking the initialized system state parameters as the origin, construct a maximum a posteriori estimation problem through the IMU pre-integration residuals, the pressure gauge two-way pre-integration residuals, and the visual reprojection residuals, and update the system state parameters and inverse depth parameters of multiple key frames in the sliding window to achieve underwater pose estimation.

[0079] In this embodiment, a sliding window is used for real-time pose optimization. The initial position , velocity and attitude , as well as the accelerometer and gyroscope biases , and the distance between the pressure gauge and the sea level obtained through step S2 are used as the origin for the system to continuously run and output the real-time system state. The specific process is as follows:

[0080] Whenever a key frame arrives, the system can enter the non-linear optimization stage based on the sliding window. According to the constructed visual observation constraints, IMU pre-integration observation constraints, and pressure gauge two-way pre-integration observation constraints, non-linear optimization is performed on all state quantities in the entire sliding window. The state quantities of this optimization problem are: , , where represents the position , velocity and attitude of the IMU at the k-th moment in the world coordinate system, as well as the accelerometer and gyroscope biases , is the distance between the pressure gauge and the sea level and will be updated during the sliding window optimization. Denote the inverse depth of the q-th landmark at the frame when it is first observed. The optimization problem of the system is defined as the following formula: , where is a robust kernel function, is the pre-integrated measurement, is the covariance matrix corresponding to the pre-integrated measurement, is the barometer measurement, is the covariance matrix corresponding to the barometer measurement, is the covariance matrix corresponding to the barometer measurement, is the covariance matrix corresponding to the visual measurement, , representing the IMU residual term, the visual residual term and the barometer residual term respectively. The specific calculation methods of the IMU residual term and the visual residual term are prior arts and will not be elaborated here. For details, please refer to the literature ". Qin, P. Li and S. Shen, "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator," in IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, Aug. 2018, doi:10.1109 / TRO.2018.2853729.". This invention will not elaborate in detail. This invention emphasizes the detailed proof of the barometer factor. The above steps have obtained the construction methods of the forward and backward barometer pre-integrated measurements. In this step, the specific definitions of the residuals in the optimization problem, as well as the specific definitions and derivations of the covariance matrices, will be given. First, for the forward barometer pre-integration, the residual is defined as: , where is the barometer two-way pre-integrated residual, is the barometric value at time P k to the barometric value at time j P j the forward barometer two-way pre-integration constraint, are the system state parameters and the inverse depth parameters, , is the gravity vector in the world coordinate system, is the distance between the barometer and the sea level obtained by initialization, , , are respectively w the position, velocity and attitude of the k-th key frame in the world coordinate system is the kThe time difference between the key frame at time j and time is the predicted position component of the k -th key frame in the body system at time j ;

[0081] The barometer two-way pre-integration residual estimated using the backward barometer two-way pre-integration constraint is: (1.21), where is the barometer two-way pre-integration residual, is the barometric pressure value at time P k to the barometric pressure value at time j P j the forward barometer two-way pre-integration constraint, are the system state parameters and the inverse depth parameters, , is the gravity vector in the world system, is the distance between the barometer and the sea level obtained by initialization, , , are respectively the position, velocity and attitude of the w -th key frame in the world system, is the k time difference between the key frame at time j and time is the predicted position component of the k -th key frame in the body system at time j ;

[0082] The covariance matrix corresponding to this residual is shown in Equation (1.22): , where is the barometer measurement white noise, is the upper left 3x3 submatrix, and can be obtained in advance when calculating the prediction.

[0083] The barometer two-way pre-integration residual estimated using the backward barometer two-way pre-integration constraint is: (1.23). The covariance matrix is obtained in the same way as the forward barometer residual. In summary, using the back-end optimization method proposed by the present invention, the maximum a posteriori estimation is performed on all state quantities in the entire sliding window to achieve real-time 6-degree-of-freedom pose output, as well as velocity output in the world system, and at the same time output high-precision sparse point clouds.

[0084] This embodiment also provides loop detection using the bag of words and post - processes the output pose and sparse point cloud. In this step, the system is also executing the fifth step. The fifth step and the sixth step are two parallel threads. This step is mainly responsible for eliminating the cumulative error during the operation of the fifth step. First is the calculation of the bag - of - words similarity: when a new key frame arrives, 500 FAST corner points are extracted from this frame, and uniformization operation is performed using a quadtree. At the same time, corresponding descriptors are extracted for each corner point to form a dictionary, and the bag of words is used to perform similarity detection on all historical key frames. When the similarity threshold exceeds 0.1, it enters the next step. Execute loop pre - processing: calculate the 2D - 3D data association between the current frame and the historical frame. When the number of inliers is more than 50, the loop detection is considered successful. After the loop detection is successful, calculate the cumulative error. Through PNP, obtain the pose of the current frame in the world system without cumulative drift, which is used as the observation for pose graph optimization. This invention uses IMU measurement data and is a four - degree - of - freedom non - objective system. Therefore, only four - degree - of - freedom pose graph optimization is performed. The entire pose graph optimization problem can be defined as: , where is the robust kernel function, are the positions and yaw angles of multiple key frames, is the set of optimization variables, is the set of observations of all non - loop edges, is the set of observations of all loop edges, and is the relative measurement between key frame i and key frame j. Solve this problem using Ceres (a solver from Google) to obtain a globally consistent trajectory. At the same time, update all points in the map using the trajectory after eliminating the cumulative error to obtain a globally consistent point cloud.

[0085] In summary, the present invention provides a low-cost and lightweight underwater pose estimation method based on bidirectional pre-integration of visual inertial pressure fusion, aiming to solve the problems of high cost, complex use, insufficient accuracy, and great difficulty in manual operation of traditional underwater robot positioning solutions, which has great significance for the popularization and use of low-cost underwater robots. The system integrates binocular vision technology, inertial measurement unit, and pressure sensor, and through innovative algorithm design, realizes robust real-time positioning in complex and changeable underwater environments. In particular, the system performs defogging and enhancement processing for underwater image characteristics, and develops visual feature points and descriptors suitable for underwater environments, effectively improving the utilization rate of visual information. Combining key technologies such as the derivation of forward and backward barometer pre-integration factors, the initialization of gyroscope zero bias by maximum a posteriori estimation, and the online initialization of barometers, the system can achieve real-time state estimation of position, attitude, and velocity through a sliding window algorithm without high costs. In addition, loop detection and four-degree-of-freedom pose graph optimization implemented by the bag-of-words method, as well as the design of a robust visual tracking strategy and a fast re-localization module, further enhance the positioning accuracy and stability of the system. At the same time, the system can also establish and reuse a sparse visual point cloud map, providing a reliable map reference for the navigation and control of underwater robots. The present invention not only achieves significant improvements in the autonomous navigation and positioning accuracy of underwater robots, but also realizes the goals of low cost and lightweight through the optimization of hardware design, providing an efficient and economical solution for the health monitoring and preventive maintenance tasks of underwater infrastructure such as bridge pier detection, submarine pipeline maintenance, offshore wind power platform monitoring, offshore oil platform inspection, inspection of the underwater part of ships, and submarine cable maintenance, and providing a cheap alternative to traditional underwater positioning solutions.

Claims

1. A visual-inertial pressure fusion underwater pose estimation method based on bidirectional pre-integration, characterized in that, Including: S1. Extract features from the current frame after haze removal enhancement and the key frame at the previous moment to obtain feature points and descriptors. At the same time, obtain the current frame pose through IMU recursion, project the landmark points onto the current frame as the tracking initial value, perform matching tracking, and then update the feature points of the current frame through outlier removal and depth filter to make the feature points converge. Determine whether the current frame is a key frame based on the number of converged feature points or the parallax with the feature points of the historical frame; S2. Repeat step S1. When the number of obtained key frames is higher than the set threshold, estimate the gyroscope zero bias based on the positive definite constraint between consecutive frames in the key frames, use the rotation obtained by removing the gyroscope zero bias as a constraint to solve the SFM problem to obtain the initial values of the system state parameters of each key frame. Based on the visual observation constraints, IMU pre-integration observation constraints, and pressure gauge pre-integration constraints of each key frame, construct an optimization problem to solve the initial values of the scale of the system state parameters, the speed of the key frame, the acceleration zero bias, and the distance between the pressure gauge and the sea level, and realize the initialization of the system state parameters. Freeze step S2; S3. Repeat step S1. Based on multiple key frames closest to the current moment, construct a sliding window. Taking the initialized system state parameters as the origin, update the system state parameters and inverse depth parameters of multiple key frames in the sliding window through the maximum a posteriori estimation problem constructed by the IMU pre-integration residual, the pressure gauge two-way pre-integration residual, and the visual reprojection residual to realize underwater pose estimation.

2. The visual-inertial pressure fusion underwater pose estimation method based on bidirectional pre-integration according to claim 1, wherein Construct a pressure gauge two-way pre-integration residual based on the pressure gauge two-way pre-integration and the system state parameters. Compare the time when the pressure gauge data arrives with the times of the two closest key frames before and after. When the time when the pressure gauge data arrives is closer to the forward key frame, use the forward pressure gauge two-way pre-integration constraint. When the time when the pressure gauge data arrives is closer to the backward key frame, use the backward pressure gauge two-way pre-integration constraint; The pressure gauge two-way pre-integration residual for estimating using the forward pressure gauge two-way pre-integration constraint before use is: , where is the pressure gauge two-way pre-integration residual, is the pressure value at time P k to the pressure value at time j P j the forward pressure gauge two-way pre-integration constraint of are the system state parameters and the inverse depth parameters, , is the gravity vector in the world coordinate system, is the distance between the pressure gauge and the sea level obtained by initialization, , , are respectively the position, velocity and attitude of the k-th key frame in the world coordinate system w ; is the k th key frame at time to j the time difference between time is the position prediction component of the k th key frame in the body coordinate system to j time; The pressure gauge two-way pre-integration residual for the estimation using the back pressure gauge two-way pre-integration constraint after use is as follows: .

3. The method for underwater pose estimation based on bidirectional pre-integration visual-inertial pressure fusion according to claim 2, wherein The forward pressure gauge two-way pre-integration constraint is the same as the IMU pre-integration observation constraint of the forward key frame; The two-way pre-integration constraint of the backward pressure gauge is the same as the IMU pre-integration observation constraint of the backward key frame. The pre-integration quantities of the position, velocity, and rotation at the k+1-th key frame in the body coordinate system are obtained through the two-way pre-integration constraint of the backward pressure gauge i at the -th IMU time are , , respectively, where is the accelerometer measurement value at the (i+1)-th IMU time is the accelerometer bias at the i-th IMU time is the gyroscope measurement value at the (i+1)-th IMU time is the logarithmic calculation on the Lie algebra 4. The visual-inertial pressure fusion underwater pose estimation method based on bidirectional pre-integration according to claim 1, wherein The specific steps of step S2 include: Solve the gyroscope zero bias based on the positive definite constraint between consecutive frames in the key frames to obtain the initial value of the gyroscope zero bias initialization; Use the initial value of the gyroscope zero bias to remove the zero bias of the rotation, use the rotation after removing the zero bias as a constraint to solve the SFM problem with prior knowledge, and construct the initial values of the system state parameters based on the solution results, the initial value of the gyroscope zero bias, and the rotation after removing the zero bias. The system state parameters include the scale, the speed, attitude, position, acceleration zero bias, and gyroscope zero bias of the key frame, and the distance between the pressure gauge and the sea level; Update the scale of the system state parameters, the speed of the key frame, the acceleration zero bias, and the distance between the pressure gauge and the sea level based on the optimization problem constructed by the visual observation constraints, IMU pre-integration observation constraints of each key frame, and the system state parameters to realize the initialization of the system state parameters.

5. The underwater pose estimation method based on bidirectional pre-integration visual-inertial pressure fusion according to claim 4, wherein The gyroscope bias initialization value is obtained by solving the gyroscope bias based on the positive definite constraint between consecutive frames in the key frames as follows: , , , , , , where is the gyroscope bias, is the eigenvalue, k is the index of the landmark jointly observed by the i-th key frame and the j-th key frame, n is the number of jointly observed landmarks, is the external parameter of the rotation between the IMU and the camera, is the true value of the rotation pre-integration component from the i-th key frame to the j-th key frame in the body coordinate system, , is the unit vector of the k-th landmark jointly observed by the i-th key frame and the j-th key frame, is the derivative of the rotation pre-integration component from the i-th key frame to the j-th key frame with respect to the gyroscope bias in the body coordinate system, is the skew-symmetric matrix, is the set of all key frames 6. The method for underwater pose estimation based on bidirectional pre-integration visual-inertial pressure fusion according to claim 1, wherein Update the feature points of the current frame through optical flow tracking, outlier removal, and depth filter in sequence based on the descriptors, including: Using the optical flow tracking of consecutive images based on descriptors to match the feature points of the current frame with those of the historical frame, removing outliers from the matched feature points, and then using a depth filter to update the depth and depth uncertainty of the feature points in the current frame until the uncertainty is less than the set threshold and the feature points converge. Triangulating the converged feature points to obtain the landmarks.

7. The visual-inertial pressure fusion underwater pose estimation method based on bidirectional pre-integration according to claim 1 or 6, characterized in that, Before outlier removal, when the number of feature points tracked by optical flow is less than the set threshold, perform co-visible key frame tracking operations, including: When the number of feature points tracked by optical flow is less than the set threshold, select multiple co-visible key frames in the local map, and obtain the pose of the current frame through IMU recursion based on the multiple co-visible key frames, that is, the visual observation constraint. At the same time, project the landmarks in the sliding window constructed by the current key frame of the multiple co-visible key frames onto the current frame, and use the descriptors to match the feature points of the projected current frame with those of the historical frame through optical flow tracking.

8. The method for underwater pose estimation based on bidirectional pre-integration visual-inertial pressure fusion according to claim 7, characterized in that, If after the co-visible key frame tracking operation, the number of tracked points is less than the given threshold, perform fast relocalization operations, including: Perform bag-of-words search and descriptor matching in the local map to obtain the correspondence between the feature points and landmarks of the current frame, and perform PNP calculation based on the correspondence between the feature points and landmarks of the current frame to obtain the pose of the current frame.

9. The visual-inertial pressure fusion underwater pose estimation method based on bidirectional pre-integration according to claim 1, wherein Judge whether the current frame is a key frame based on the number of converged feature points or the comparison result with the feature points of the historical frame, including: Calculate the disparity between the current frame and the previous key frame based on the correspondence between the feature points of the previous key frame and the current frame. When the disparity is greater than the set disparity threshold, set the current frame as a key frame; Or, when the number of feature points in the current frame is less than the set number threshold, set the current frame as a key frame; Or, when the time difference between the current frame and the previous key frame is greater than the set time threshold, set the current frame as a key frame.

10. The visual-inertial pressure fusion underwater pose estimation method based on bidirectional pre-integration according to claim 1, characterized in that While performing step S3, also use the bag-of-words for loop closure detection, including: When a new key frame is added to the sliding window, extract multiple FAST corner points from the new key frame, perform homogenization operation using a quadtree, and at the same time extract the corresponding descriptors for each FAST corner point to form a corresponding dictionary, and perform similarity detection on the historical key frames of the bag-of-words based on the formed dictionary; When the similarity threshold is exceeded, perform loop closure preprocessing: calculate the 2D-3D data association between the current key frame and the historical key frame. When the number of inliers is more than the inlier number threshold, the loop closure detection is successful; Calculate the pose of the current key frame in the world system without cumulative drift through PNP as the observation quantity for pose graph optimization, and use Ceres to solve the pose graph optimization problem to obtain the point cloud of the globally consistent trajectory.

Images