An underwater SLAM method integrating forward-looking sonar and ORB-SLAM3

Abstract

The invention discloses an underwater SLAM method integrating forward-looking sonar and ORB-SLAM3, and the steps include: 1. obtaining the measurement of forward-looking sonar, binocular camera and inertial measurement unit (IMU); 2. performing feature tracking on the sonar image, and obtaining the relative pose estimation of the sonar by minimizing the reprojection error; 3. obtaining the pose estimation of the camera by ORB-SLAM3; 4. constructing a factor graph, when visual tracking is effective, taking the pose estimation value of the visual odometer in 3 as a priori factor, pre-integrating the IMU as a pose constraint factor, taking the relative pose estimation of the sonar in 2 as a constraint factor, solving the factor graph, and updating the carrier pose estimation; 5. when visual tracking is successful, the output of the factor graph will follow the visual odometer; if visual tracking is lost, the sonar and IMU factors in the factor graph continue to work, the system initializes a new map immediately after reaching the trackable area, and uses the pose estimation in 4 as the map initial value to connect the two maps, which is conducive to expanding the use range of the system.

Description

Technical Field

[0001] The present invention belongs to the field of robots and autonomous driving, and specifically relates to an underwater SLAM method integrating forward-looking sonar and ORB-SLAM3. Background Art

[0002] In order to ensure that the robot can carry out underwater tasks smoothly, it is necessary to map the seabed environment and locate the robot. Simultaneous Localization and Mapping (SLAM) technology uses sensors (such as cameras, sonar, etc.) installed on the underwater robot to locate itself and build an environmental map. SLAM can correct the accumulated errors through the system's own loopback and relocation functions.

[0003] At present, there are many excellent visual SLAM algorithms on land that can provide high-precision positioning for robots, and have been maturely applied both indoors and outdoors. ORB-SLAM3 provides a binocular visual-inertial SLAM method based on a multi-map system, which has strong robustness and high accuracy. Due to factors such as underwater light and color attenuation, dynamic objects, and underwater dust, cameras are often not always able to detect features. When visual features cannot be detected, the use of IMU dead reckoning often has a large cumulative error. Forward-looking sonar can work in turbid underwater environments, but it is expensive and has low resolution. In some clear waters (such as coral reefs), cameras can capture rich information. Therefore, forward-looking sonar is integrated in ORB-SLAM3, and the advantages of vision and forward-looking sonar are used to complement each other, expanding the scope of use of the SLAM system while ensuring system accuracy.

[0004] The fusion of forward-looking sonar and ORB-SLAM3 is essentially a multi-source sensor fusion navigation problem. The fusion methods are divided into filtering-based methods and nonlinear optimization-based methods. The nonlinear optimization method has high accuracy, but it is difficult to implement. It requires modifying the original backend structure of ORB-SLAM3, and most backends are unavailable after one of the sensors fails, which has poor flexibility. In addition, the sensors often need to be synchronized before fusion. The frequency of sonar and camera is low, and some lower frequency data will be obtained after time synchronization. The factor graph is flexible and can naturally process measurement information of different rates, and supports plug-and-play of sensors. If a sensor fails, it can also be deleted from the factor graph framework in time, which can effectively fuse the forward-looking sonar and ORB-SLAM3.

[0005] 1. Technical comparison with “A method for visual simultaneous positioning and mapping based on point feature sparse strategy”:

[0006] "A visual synchronous positioning and map construction method based on point feature sparse strategy" extracts point features from sonar images using the SIFT algorithm, and then matches SIFT feature points through multiple matching constraints such as polar constraints, parallax constraints, direction constraints, scale constraints, and unique matching constraints. The SLAM backend uses the EKF filter optimization algorithm. The key to this article is to constrain the sparsity of landmarks, control the distribution of the landmark's position space and feature space, limit the landmark's rising speed, reduce the amount of calculation, and shorten the positioning time.

[0007] The present invention is an "underwater SLAM method that integrates forward-looking sonar and ORB-SLAM3", which uses the AKAZE algorithm to extract features from sonar images and eliminates mismatches through a random sampling consistency algorithm. The backend of the SLAM algorithm of the present invention uses nonlinear optimization based on factor graphs, and the amount of calculation will not increase dramatically as the number of landmarks increases.

[0008] 2. Technical comparison with “Forward-looking imaging sonar underwater positioning and navigation method based on key constraint frames”:

[0009] "Forward-looking imaging sonar underwater positioning and navigation method based on key constraint frames" uses forward-looking sonar, IMU, and depth meter as sensors, and proposes a forward-looking imaging sonar underwater positioning and navigation method based on key constraint frames. The purpose of this paper is to identify and fully utilize the effective constraint information in past sonar images to locate the current posture and underwater features. The main steps are: front-end data processing, including feature extraction and matching of two-dimensional sonar images, inertial measurement data processing; back-end elastic sliding window optimization, including identification and storage of sonar key constraint frames, selection of past frames that can be added to the sliding window optimization from the key constraint frame library, window size control and optimization, and updating of robot posture and underwater feature positions.

[0010] The sensors used in the present invention are optical cameras, forward-looking sonars, and IMUs, focusing on the collaborative positioning of optical and acoustic sensors. The technical core of the present invention is the use of factor graphs for multi-source sensor fusion. Since the recovery of the sonar characteristic inclination angle is taken into account, the present invention can be applied to the positioning and mapping of underwater three-dimensional space motion. The forward-looking sonar SLAM algorithm based on key constraint frames approximates the sonar characteristic inclination angle to zero, which is suitable for positioning and mapping on two-dimensional planes. If applied to three-dimensional scenes, it will lead to large positioning errors.

[0011] 3. Technical comparison with “A method for underwater sonar image matching based on Gaussian distribution clustering”:

[0012] "A method for underwater sonar image matching based on Gaussian distribution clustering" aims to perform accurate three-dimensional reconstruction through two-dimensional sonar images. It provides an underwater sonar image matching method based on Gaussian distribution clustering. Through image registration and optimization of sonar three-dimensional motion parameters, accurate three-dimensional reconstruction of two-dimensional sonar images is performed. The method includes the following steps: Step A: extract features, establish matching relationships between features, and obtain elevation information of the map; Step B: perform pose estimation and update the feature map to generate a three-dimensional space map.

[0013] The present invention aims to use forward-looking sonar to perform synchronous positioning and map construction of underwater carriers, without knowing the surrounding environment and the motion posture of the carrier in advance, and the three-dimensional reconstruction method based on Gaussian distribution clustering is based on a method based on the planarity assumption to calculate the measured value of the elevation angle of the projected shadow point of the scanning azimuth. As the carrier moves, the planarity assumption may not hold. The method of minimizing the reprojection error and associating the acoustic and visual data proposed by the present invention restores the sonar inclination angle without setting a premise assumption, and is more suitable for underwater SLAM systems. Summary of the invention

[0014] In view of the problem that ORB-SLAM3 is easy to lose tracking underwater, resulting in a large cumulative error, the present invention proposes an underwater SLAM method that integrates forward-looking sonar and ORB-SLAM3. The method constructs a factor graph to integrate sonar constraints and the pose constraints of the ORB-SLAM3 visual inertial odometer, takes the visual inertial odometer as the factor graph prior factor, and the sonar odometer and IMU pre-integration as constraint factors. When the object is visually tracked, the output of the factor graph will follow the visual inertial odometer; when the visual tracking is lost, the sonar constraints and IMU pre-integration factors in the factor graph continue to work to limit the cumulative error of the dead reckoning. After the carrier reaches the visually trackable area again, a new submap is initialized immediately, and the pose estimate of the factor graph is used as the initial value of the submap to connect the two adjacent submaps. After the loop is detected, the submaps are merged to further reduce the cumulative error.

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

[0016] An underwater SLAM method integrating forward-looking sonar and ORB-SLAM3, characterized in that the specific steps are as follows:

[0017] (1) Pre-integrate the IMU measurements and use IMU pre-integration to obtain the relative pose between the two camera frames;

[0018] (2) Perform AKAZE feature extraction on the sonar image and calculate the descriptor Hamming distance to perform feature matching between two adjacent sonar image frames;

[0019] (3) constructing a factor graph. When visual tracking is effective, the estimated pose value of the carrier in (1) is used as a pose prior factor. By comparing the number of matching feature points with the set threshold, it is determined whether the visual tracking is effective.

[0020] (4) If visual tracking is lost, let the system initialize a new map immediately after reaching the trackable area, using the carrier pose calculated in step (3) as the submap initialization value to link the two submaps. Use DBoW2 for loop closure detection, extract the bag-of-words vectors of each new visual keyframe and match them with the previously extracted descriptors. If a loop closure is detected, merge the new map with the previous submap.

[0021] As a further improvement of the present invention, step (1) is specifically as follows:

[0022] Detect the ORB feature points of the current binocular camera frame, and use the sliding window to build a local map with keyframe feature points. When each new frame arrives, the system searches for old feature points in the local map and matches new feature points through feature descriptors. When the number of tracked feature points is less than the threshold, the front end marks the frame as a keyframe and adds the keyframe to the sliding window, marginalizing the oldest keyframe in the sliding window to maintain a fixed sliding window size.

[0023] The visual-inertial SLAM problem is formulated as a joint optimization of the cost function J(x), which includes the visual reprojection error e r and from IMU e s The time error term is:

[0024]

[0025] Where i is the camera index, k is the camera frame index, and j is the landmark index. The carrier pose estimate and the local map of the visual landmark are updated through BA optimization. and They represent the information matrices of the visual landmark and IMU pre-integration of the kth frame respectively.

[0026] As a further improvement of the present invention, step (2) is specifically as follows:

[0027] Since the inclination of the landmark is lost during sonar imaging, the min ,φ min +Δφ,...,φ max -Δφ,φ max}, where the range Φ is determined by the maximum vertical aperture of the sonar, Δφ is a constant design parameter that determines the resolution of the search space, thereby making a trade-off between performance and accuracy, and the optimal elevation angle of the sonar is:

[0028]

[0029] where π(·) represents the transformation of a point in Cartesian coordinates to polar coordinates, C(·) is the mapping from spherical coordinates to Cartesian coordinates, Indicates the coordinate transformation from the previous frame sonar reference system A to the next frame sonar reference system B, determined by IMU pre-integration The initial value of B z i This process is performed for each landmark to establish a set of optimal elevation angles φ * As an observation model for sonar BA;

[0030] Use data association of visual sonar landmarks to eliminate them, and establish an elliptical association gate centered on the sonar landmark estimate. The elliptical gate is represented by the residual vector and the norm. Let G be the threshold value of the elliptical gate, S be the covariance matrix of the information vector, and if the norm of the information vector satisfies the following formula, the observation is within the association gate:

[0031]

[0032] When the vision detects an object, the sonar and visual landmarks are associated. If no visual landmark is within the association gate, in other words, there is no visual landmark near the sonar landmark, the sonar solution is considered invalid. If both sonar landmarks can be associated with the visual landmark, the landmark point located above is selected, because when the forward-looking sonar is imaging, the landmark point above will cover the landmark point below.

[0033] After determining the position of the landmark, the relative position and pose estimation of the sonar is obtained through BA optimization using the sonar feature reprojection error as the cost function.

[0034] As a further improvement of the present invention, step (3) is specifically as follows:

[0035] In an open environment, the sonar odometer will get an incorrect estimate because the environmental features are not obvious and the constraints are poor. Sonar failure detection can be used to eliminate incorrect pose estimates.

[0036] First, perform SVD singular value decomposition on the sonar measurement Jacobian matrix A:

[0037] A m×n =USV T

[0038] Among them U m×m and V n×n is an orthogonal matrix, S m×n is a singular value (σ1≤...≤σ n ) increasingly larger diagonal matrices, selecting appropriate thresholds σ from Sthresh , and will be less than σ thresh The eigenvalue of is set to 0. When there are enough eigenvalues ​​less than the threshold, the current sonar posture constraint is considered invalid.

[0039] After determining that the sonar is effective, the sonar relative pose estimation value in step (2) is added to the factor graph, and the IMU pre-integration term in step (1) is used as the pose constraint factor. The state estimation problem of the carrier is expressed as follows:

[0040]

[0041]

[0042] Among them, Θ i is related to the factor node f i The set of adjacent variable nodes, f i (Θ i ) is the measurement function, z i is a measurement value. It is solved by jointly optimizing the contributions of the IMU pre-integration constraint, the visual odometry constraint, and the sonar odometry constraint in the factor graph using iSAM2.

[0043] Beneficial effects: The present invention discloses an underwater SLAM method based on forward-looking sonar, vision, and inertia. The method constructs a factor graph to fuse the pose estimation results of the sonar odometer, IMU dead reckoning, and visual inertial odometer. After visual tracking is lost, the sonar and inertial odometer can continue to provide pose estimation, and a new sub-map is initialized immediately after the carrier reaches the trackable area again. After the loop is detected, the sub-maps are merged for global optimization. Among them, the ambiguity problem of sonar landmarks is solved by associating sonar and visual feature data; in addition, the effectiveness of the sonar constraint factor is judged for under-constrained environments. Compared with the prior art, this method has the advantages of a wide range of use, high positioning accuracy, and strong stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 is a system block diagram of the method disclosed in the present invention;

[0045] Figure 2 These are two factor graph structures when visual detection is successful and tracking fails in the method disclosed in the present invention. DETAILED DESCRIPTION

[0046] The present invention is further explained below in conjunction with the accompanying drawings and specific implementation methods.

[0047] Underwater SLAM method that integrates forward-looking sonar and ORB-SLAM3, such as Figure 1 As shown, the following steps are included:

[0048] Step 1: Install the forward-looking sonar, binocular camera, and IMU on the carrier, manipulate the carrier to sail in the water, and obtain the sensor's measurement values. The raw angular velocity and acceleration measurements from the IMU are expressed as:

[0049]

[0050]

[0051] in and is the raw IMU measurement at time t. ω t and Biased b t and additive noise n t To address the impact of , we assume that the additive noise is Gaussian, and the acceleration bias and gyroscope bias are modeled as random walks. is the rotation matrix from the world coordinate system W to the IMU coordinate system B. g is the constant gravity vector in the world coordinate system W.

[0052] Use IMU pre-integration to obtain the relative pose between two camera frames. The IMU pre-integration factor can be defined as:

[0053]

[0054] in It is an IMU pre-integrated measurement consisting of noisy accelerometer and gyroscope measurements between two consecutive camera frames;

[0055] Step 2: Detect the ORB feature points of the current binocular camera frame and triangulate the new features that are successfully matched between the left and right cameras to the real-world location. Use a sliding window to build a local map with keyframe feature points. When each new frame arrives, the system searches for old feature points in the local map and matches the new feature points through their feature descriptors. Project the landmark points corresponding to the matched old features to the current frame. Considering the lth feature observed in the i-th frame, the reprojection error is defined as:

[0056] Φ={φ min ,φ min +Δφ,...,φ max -Δφ,φ max} (4)

[0057] where u j is the coordinate of the feature on the image, K is the camera projection matrix, P j is the position of the point in the world coordinate system W. ξ i ∈SE(3) represents the pose of the camera.

[0058] When the number of tracked feature points is less than the threshold, the front end will mark this frame as a key frame, add the current frame to the sliding window, and remove the oldest key frame in the sliding window to maintain a fixed sliding window size.

[0059] The cost function is constructed using the reprojection error of the visual feature points and the IMU pre-integration factor in step 1 as constraints, and the sliding window is solved to optimize the carrier pose estimate and the local map composed of visual landmark points.

[0060] Step 3: Perform AKAZE feature extraction on the sonar image and perform feature matching between two adjacent frames of sonar images by calculating the descriptor Hamming distance. Since the inclination of the landmark is lost during the sonar imaging process, the inclination of the landmark is calculated in the region Φ = {φ min ,φ min +Δφ,...,φ max -Δφ,φ max}, where the range Φ is determined by the maximum vertical aperture of the sonar and Δφ is a constant design parameter that determines the resolution of the search space, thus making a trade-off between performance and accuracy. Most forward-looking sonars have a small vertical aperture (~12°) that can be calculated quickly. The optimal elevation angle for the sonar is:

[0061]

[0062] where π(·) represents the transformation of a point in Cartesian coordinates to polar coordinates, C(·) is the mapping from spherical coordinates to Cartesian coordinates, Indicates the coordinate transformation from the previous frame sonar reference system A to the next frame sonar reference system B, determined by IMU pre-integration The initial value of B z i This process is performed for each landmark to establish a set of optimal elevation angles φ * As an observation model for sonar BA.

[0063] Since there is ambiguity in the process of searching for sonar landmarks, that is, there may be two solutions that minimize the reprojection error, but in fact only one solution corresponds to the position of the real landmark, the data association of the visual sonar landmark is used to eliminate it. Since noise generally follows a Gaussian distribution, an elliptical association gate is established with the estimated value of the sonar landmark as the center. The elliptical gate is represented by the residual vector and the norm. Let G be the threshold value of the elliptical gate, S be the covariance matrix of the information vector, and if the norm of the information vector satisfies the following formula, it can be said that the observation is within the association gate:

[0064]

[0065] When the vision detects an object, the sonar and visual landmarks are associated with each other. If no visual landmark is within the association gate, in other words, there is no visual landmark near the sonar landmark, the sonar solution is considered invalid. If both sonar landmarks can be associated with the visual landmark, the landmark point located above is taken, because when the forward-looking sonar is imaging, the landmark point above will cover the landmark point below.

[0066] After determining the position of the landmark, the relative position and pose estimation of the sonar is obtained through BA optimization using the sonar feature reprojection error as the cost function.

[0067] Step 4: Construct a factor graph. When visual tracking is effective, use the carrier pose estimation value in step 2 as a pose prior factor, and determine whether visual tracking is effective by comparing the number of matching feature points with the set threshold.

[0068] In an open environment, due to the unclear environmental features and poor constraints, the sonar odometer will get an incorrect estimate. Sonar failure detection can eliminate the wrong pose estimation. First, perform SVD singular value decomposition on the sonar measurement Jacobian matrix A:

[0069] A m×n =USV T (7)

[0070] Among them U m×m and V n×n is an orthogonal matrix, S m×n is a singular value (σ1≤...≤σ n ) increasingly larger diagonal matrices. Selecting an appropriate threshold σ from S thresh , and will be less than σ thresh The eigenvalue of is set to 0. When there are enough eigenvalues ​​less than the threshold, the current sonar posture constraint is considered invalid.

[0071] After determining that the sonar is effective, the sonar relative pose estimation value in step 3 is added to the factor graph, and the IMU pre-integration factor in step 1 is used as the pose constraint factor. The factor graph structure is as follows: Figure 2 As shown, the state estimation problem of the carrier can be expressed as follows:

[0072]

[0073]

[0074] Among them, Θ i is related to the factor node f i The set of adjacent variable nodes, f i (Θ i ) is the measurement function, z iis a measurement value. It is solved by jointly optimizing the contributions of the IMU pre-integration constraint, the visual odometry constraint, and the sonar odometry constraint in the factor graph using iSAM2.

[0075] Step 5: If visual tracking is lost, let the system initialize a new map as soon as it reaches the trackable area, linking the two submaps using the carrier pose calculated in step 4 as the submap initialization value. Use DBoW2 for loop closure detection, extract the bag-of-words vectors for each new visual keyframe and match them with the previously extracted descriptors. If a loop closure is detected, merge the new map with the previous submap.

[0076] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. An underwater SLAM method integrating forward-looking sonar and ORB-SLAM3, characterized in that: The specific steps are as follows: (1) Pre-integrate the IMU measurements and use IMU pre-integration to obtain the relative pose between the two camera frames; (2) Perform AKAZE feature extraction on the sonar image and calculate the descriptor Hamming distance to perform feature matching between two adjacent sonar image frames; Step (2) is as follows: Since the inclination of the landmark is lost during sonar imaging, the min ,φ min +Δφ,...,φ max -Δφ,φ max }, where the range Φ is determined by the maximum vertical aperture of the sonar, Δφ is a constant design parameter that determines the resolution of the search space, thereby making a trade-off between performance and accuracy, and the optimal elevation angle of the sonar is: where π(·) represents the transformation of a point in Cartesian coordinates to polar coordinates, C(·) is the mapping from spherical coordinates to Cartesian coordinates, Indicates the coordinate transformation from the previous frame sonar reference system A to the next frame sonar reference system B, determined by IMU pre-integration The initial value of B z i represents the sonar, and this process is performed for each landmark to establish a set of optimal elevation angles φ * As an observation model for sonar BA; Use data association of visual sonar landmarks to eliminate them, and establish an elliptical association gate centered on the sonar landmark estimate. The elliptical gate is represented by the residual vector and the norm. Let G be the threshold value of the elliptical gate, S be the covariance matrix of the information vector, and if the norm of the information vector satisfies the following formula, the observation is within the association gate: When the vision detects an object, the sonar and visual landmarks are associated. If no visual landmark is within the association gate, in other words, there is no visual landmark near the sonar landmark, the sonar solution is considered invalid. If both sonar landmarks can be associated with the visual landmark, the landmark point located above is selected, because when the forward-looking sonar is imaging, the landmark point above will cover the landmark point below. After determining the location of the landmark, the relative position and pose estimation of the sonar is obtained through BA optimization using the sonar feature reprojection error as the cost function. (3) constructing a factor graph. When visual tracking is effective, the estimated pose value of the carrier in (1) is used as a pose prior factor. By comparing the number of matching feature points with the set threshold, it is determined whether the visual tracking is effective. (4) If visual tracking is lost, let the system initialize a new map immediately after reaching the trackable area, use the carrier pose calculated in step (3) as the submap initialization value to link the two submaps, use DBoW2 for loop closure detection, extract the bag-of-words vectors of each new visual keyframe and match them with the previously extracted descriptors, and merge the new map with the previous submap if a loop closure is detected.

2. The underwater SLAM method integrating forward-looking sonar and ORB-SLAM3 according to claim 1, characterized in that: Step (1) is as follows: Detect the ORB feature points of the current binocular camera frame, and use the sliding window to build a local map with keyframe feature points. When each new frame arrives, the system searches for old feature points in the local map and matches new feature points through feature descriptors. When the number of tracked feature points is less than the threshold, the front end marks the frame as a keyframe and adds the keyframe to the sliding window, marginalizing the oldest keyframe in the sliding window to maintain a fixed sliding window size. The visual-inertial SLAM problem is formulated as a joint optimization of the cost function J(x), which includes the visual reprojection error e r and from IMU e s The time error term is: Where i is the camera index, k is the camera frame index, and j is the landmark index. The carrier pose estimate and the local map of the visual landmark are updated through BA optimization. and They represent the information matrices of the visual landmark and IMU pre-integration of the kth frame respectively.

3. The underwater SLAM method according to claim 1, characterized in that: Step (3) is as follows: In an open environment, the sonar odometer will get an incorrect estimate because the environmental features are not obvious and the constraints are poor. Sonar failure detection can be used to eliminate incorrect pose estimates. First, perform SVD singular value decomposition on the sonar measurement Jacobian matrix A: A m×n =USV T Among them U m×m and V n×n is an orthogonal matrix, S m×n is a singular value (σ1≤...≤σ n ) increasingly larger diagonal matrices, selecting appropriate thresholds σ from S thresh , and will be less than σ thresh The eigenvalue of is set to 0. When there are enough eigenvalues ​​less than the threshold, the current sonar posture constraint is considered invalid. After determining that the sonar is effective, the sonar relative pose estimation value in step (2) is added to the factor graph, and the IMU pre-integration term in step (1) is used as the pose constraint factor. The state estimation problem of the carrier is expressed as follows: Among them, Θ i is related to the factor node f i The set of adjacent variable nodes, f i (Θ i ) is the measurement function, z i is a measurement value solved by jointly optimizing the contributions of the IMU pre-integration constraint, the visual odometry constraint, and the sonar odometry constraint in the factor graph using iSAM2.

Images