A laser SLAM method and system integrating visual loop closure detection

Abstract

This invention relates to a laser SLAM method and system that integrates visual loop closure detection. The method includes: S1: acquiring laser point cloud data using a 3D LiDAR and constructing laser point cloud keyframes, and using these keyframes to obtain current positioning information; S2: registering the point cloud data contained in each laser point cloud keyframe to the world coordinate system to obtain a global point cloud map; S3: acquiring visual images using a visual camera and fusing the laser point cloud keyframes to construct point cloud-image fusion keyframes to detect loop closure information. When a loop closure is detected, the corresponding loop closure constraint and loop closure trajectory are obtained; S4: establishing a pose graph model based on all point cloud-image fusion keyframes on the loop closure trajectory, and optimizing the pose graph model using loop closure constraints as constraint edges to obtain optimized point cloud-image fusion keyframes to update the current positioning information and the global point cloud map. The method and system provided by this invention can improve the accuracy of positioning and mapping in laser SLAM systems.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more specifically to a laser SLAM method and system that integrates visual loop closure detection. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology refers to the process by which a mobile robot perceives its surroundings through sensors, estimates its own pose, and builds a map to store environmental information, thus completing the perception of the external environment. It is a key technology and prerequisite for the intelligence of mobile robots. A SLAM system can be divided into four parts: front-end odometry, loop closure detection, back-end optimization, and map building.

[0003] Loop closure detection refers to the process by which a robot, when revisiting the same location, detects the consistency between the current scene and a historical scene, and establishes a correlation between the pose states of the two times. The robot can then use this correlation to eliminate accumulated errors during operation, achieving more accurate localization and mapping results. LiDAR SLAM, a SLAM system that utilizes LiDAR at the front end, faces challenges because the number of historical frames to be matched increases continuously as the system runs. The accuracy and real-time performance of the LiDAR loop closure detection algorithm are difficult to guarantee simultaneously, leading to a gradual accumulation of localization errors and ultimately affecting the accuracy of the system's localization and mapping. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention provides a laser SLAM method and system that integrates visual loop closure detection.

[0005] The technical solution of this invention is: a laser SLAM method integrating visual loop closure detection, comprising:

[0006] Step S1: Laser odometry: Laser point cloud data is collected by 3D LiDAR, the point cloud line features and surface features of the laser point cloud data are extracted, and the laser point cloud key frame is obtained by using the displacement of the 3D LiDAR as a judgment condition. At the same time, the current laser point cloud key frame is matched with the local point cloud map, and the local point cloud map is matched with the global point cloud map according to the point cloud line features and surface features to obtain the current positioning information.

[0007] Step S2: Map building: Register the point cloud contained in each laser point cloud keyframe to the world coordinate system, and obtain the updated global point cloud map after point cloud downsampling;

[0008] Step S3: Visual loop closure detection: Use a visual camera to acquire visual images and fuse the laser point cloud keyframes to construct point cloud-image fusion keyframes to detect loop closure information. When loop closure information is detected, obtain the corresponding loop closure constraints and loop closure trajectories.

[0009] Step S4: Global Graph Optimization: Establish a pose graph model based on all the point cloud-image fusion keyframes on the loop trajectory, add the loop constraint as a constraint edge to the pose graph model, optimize the pose graph model using the GTSAM library to obtain optimized point cloud-image fusion keyframes, and update the current positioning information and the global point cloud map according to the optimized point cloud-image fusion keyframes.

[0010] Compared with existing laser SLAM technology, this invention has the following advantages:

[0011] This invention discloses a laser SLAM method and system that integrates visual loop closure detection. By combining visual images with laser loop closure detection, and using a bag-of-words model of the visual images, image data containing a large amount of information can be transformed into bag-of-words vectors, greatly reducing the computational load of scene matching, improving image matching efficiency, and ensuring the real-time performance of loop closure detection. Finally, the visual loop closure is applied to global graph optimization, reducing accumulated errors and improving the accuracy of localization and mapping in the laser SLAM system. Attached Figure Description

[0012] Figure 1 This is a flowchart of a laser SLAM method that integrates visual loop closure detection in an embodiment of the present invention;

[0013] Figure 2 This is a schematic diagram illustrating the construction process of the visual dictionary in an embodiment of the present invention;

[0014] Figure 3A This is a schematic diagram of the LOAM mapping results of the loop closure detection module in this embodiment of the invention;

[0015] Figure 3B This is a schematic diagram of the mapping result obtained using the method provided by the present invention in an embodiment of the present invention;

[0016] Figure 4 This is a structural block diagram of a laser SLAM system that integrates visual loop closure detection in an embodiment of the present invention. Detailed Implementation

[0017] This invention provides a laser SLAM method that integrates visual loop closure detection, which reduces accumulated error and improves the accuracy of laser SLAM system localization and mapping.

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below through specific implementations and in conjunction with the accompanying drawings.

[0019] Example 1

[0020] like Figure 1As shown in the figure, an embodiment of the present invention provides a laser SLAM method that integrates visual loop closure detection, comprising the following steps:

[0021] Step S1: Laser odometry: Collect laser point cloud data through 3D LiDAR, extract the point cloud line features and surface features of the laser point cloud data, and obtain laser point cloud key frames based on the displacement of 3D LiDAR. At the same time, match the current laser point cloud key frame with the local point cloud map and the local point cloud map with the global point cloud map according to the point cloud line features and surface features to obtain the current positioning information.

[0022] Step S2: Map building: Register the point cloud contained in each laser point cloud keyframe to the world coordinate system, and obtain the updated global point cloud map after point cloud downsampling;

[0023] Step S3: Visual loop closure detection: Use a visual camera to acquire visual images and fuse laser point cloud keyframes to construct point cloud-image fusion keyframes to detect loop closure information. When loop closure information is detected, obtain the corresponding loop closure constraints and loop closure trajectories.

[0024] Step S4: Global Graph Optimization: Establish a pose graph model based on all point cloud-image fusion keyframes on the loop trajectories, add loop constraints as constraint edges to the pose graph model, use the GTSAM library to optimize the pose graph model to obtain optimized point cloud-image fusion keyframes, and update the current localization information and global point cloud map based on the optimized point cloud-image fusion keyframes.

[0025] In one embodiment, step S1 above: Laser odometry: Laser point cloud data is acquired using a 3D LiDAR, and the point cloud line and surface features are extracted. Keyframes of the laser point cloud are obtained using the displacement of the 3D LiDAR as a criterion. Simultaneously, the keyframes are matched with a local point cloud map, and the local point cloud map is matched with a global point cloud map according to the point cloud line and surface features, to obtain the current positioning information. Specifically, this includes:

[0026] First, a 3D LiDAR is installed on the vehicle, which can rotate 360° in one working cycle to collect laser point cloud data and extract the point cloud line and surface features. Simultaneously, the displacement of the 3D LiDAR during operation is calculated. When the displacement exceeds a predetermined threshold, a keyframe of the laser point cloud is created. In this invention, the threshold is set to 0.5m, and within the current working cycle [t]... k ,t k+1 ] of t k+1 The keyframes of the laser point cloud are published continuously.

[0027] Secondly, feature matching is performed between the laser point cloud keyframes and the local point cloud map, and pose estimation is performed at a frequency of 10Hz to achieve coarse positioning of high-frequency odometry; then, the local point cloud map is matched with the global point cloud map at a frequency of 1Hz to achieve fine positioning of low-frequency odometry, thereby obtaining the current positioning information.

[0028] In one embodiment, step S2 above: map construction: the present invention adopts a point cloud map construction method based on key frames, that is, the point cloud contained in each laser point cloud key frame obtained in step S1 is registered in the world coordinate system, and the updated global point cloud map is obtained after point cloud downsampling.

[0029] In one embodiment, step S3 above uses a visual camera to acquire visual images and fuses laser point cloud keyframes to construct point cloud-image fusion keyframes, specifically including:

[0030] Step S301: Obtain distance time t k+1 The latest t p The visual image at time t is used as a keyframe of the visual image. k+1 The keyframes of the laser point cloud published at different times are correlated to form the initial point cloud-image fusion keyframe, where t p During the work cycle [t] k ,t k+1 ]between;

[0031] Since the acquisition devices for lidar point cloud keyframes and visual image keyframes are different, their timestamps are different. Therefore, it is necessary to perform motion compensation on all the lidar point cloud data contained in the initial point cloud-image fusion keyframe using a linear interpolation method to unify their timestamps with those of the visual image keyframes in the lidar coordinate system, so as to achieve synchronization between the timestamps of the lidar point cloud keyframes and the visual image keyframes.

[0032] Step S302: For [t] k ,t k+1 The laser point cloud data contained in the initial point cloud-image fusion keyframe within the time interval is subjected to motion compensation using a linear interpolation method. First, as shown in formula (1), the motion compensation of the initial point cloud-image fusion keyframe within time interval t is calculated. i Moment Cloud X i In [t] k ,t i Pose transformation T between ] k,i Similarly, [t] can be calculated. k ,t p Pose transformation T between ] k,p As in formula (2):

[0033]

[0034]

[0035] Among them, T k,k+1 It is the laser point cloud data at the laser point cloud timestamp [t] k ,t k+1 The pose transformations between [ ] can be directly obtained by laser odometry;

[0036] Point cloud X from the initial point cloud-image fusion keyframe i Using pose transformation T k,i Convert to t k At the time coordinates, the point cloud X is obtained. k Then, the point cloud X k Using pose transformation T k,p Convert to visual image timestamp t p At that moment, obtain X, which is synchronized with the timestamp of the visual image. p This achieved time synchronization between laser point clouds and visual images, resulting in motion-compensated point cloud-image fusion keyframes.

[0037] In one embodiment, step S3 above, which involves detecting loop closure information and obtaining the corresponding loop closure constraints and loop closure trajectory, specifically includes:

[0038] Step S311: Select ORB features to extract feature point sets from the existing visual image sample set, use the Kmeans algorithm to cluster the feature point sets, and store the clustering results as words in a tree data structure to build a visual dictionary.

[0039] This invention uses the DBoW3 library to construct a visual dictionary, selects ORB features with high real-time performance to extract features from a large number of visual image samples, and then uses the Kmeans algorithm to cluster the feature point set. The clustering results are used as words and stored in a tree data structure to realize the construction of the visual dictionary.

[0040] Step S312: Perform ORB feature extraction on the current visual image keyframe to obtain feature points. Query the word frequency of the corresponding word in the visual dictionary, calculate the score for each word using the TF-IDF method, and obtain the bag-of-words vector V of the current visual image keyframe. k (v1,v2,···v n );

[0041] like Figure 2 The diagram illustrates the process of constructing a visual dictionary and generating bag-of-words vectors for a single visual image using the visual dictionary.

[0042] Step S313: Traverse all historical point cloud-image fusion keyframes, take the visual image corresponding to the historical point cloud-image fusion keyframe as the historical visual image keyframe, and obtain the bag-of-words vector V of all historical visual image keyframes. h (v1,v2,···v n ), and calculate V according to formulas (3) to (4). k (v1,v2,···v n ) and V h (v1,v2,···v n The Manhattan distance S between the two images is used to obtain the image similarity η. The historical frame with the highest similarity η and greater than the threshold T is selected as the loop closure keyframe.

[0043]

[0044]

[0045] Among them, v i ∈V k v j ∈V h S (k,h) For V k and V h The Manhattan distance between them is used to represent the bag-of-words vector distance; S (k,k-1) ) represents the bag-of-words vector distance between the current keyframe of the visual image and the previous keyframe of the visual image;

[0046] Step S314: Project the point cloud feature points of the point cloud-image fusion key frame of the detected loop closure key frame onto the visual image key frame to obtain the two-dimensional pixel coordinates of the feature points of the visual image key frame. Use the three-point estimation method to assign depth values ​​to the two-dimensional pixel coordinates to obtain the three-dimensional spatial coordinates of the feature points of the current visual image key frame.

[0047] Step S315: Using the three-dimensional spatial coordinates of the feature points, the feature points are matched with the loop closure keyframes. The PnP algorithm is applied to construct the reprojection error model. The least squares problem of equation (x) is solved by optimizing the LM algorithm to obtain the loop closure constraint.

[0048] In one embodiment, step S4 above: Global graph optimization: A pose graph model is established based on all point cloud-image fusion keyframes on the loop trajectory. The loop constraint is added to the pose graph model as a constraint edge. The pose graph model is optimized using the GTSAM library to obtain the optimized point cloud-image fusion keyframes. The current positioning information and global point cloud map are updated according to the optimized point cloud-image fusion keyframes.

[0049] This invention uses sequence_05 of the KITTI dataset to experimentally evaluate the proposed laser SLAM method and system that integrates visual loop closure detection. Figure 3A The image shows the LOAM mapping results without a loop closure detection module. The mapping results at the bounding boxes do not match the road point cloud of the real map. Figure 3B The mapping results obtained using the method provided by this invention show that the road point clouds in the bounding boxes are clear and match well with the road point clouds of the real map.

[0050] Furthermore, this invention employs Absolute Pose Error (APE) to analyze trajectory accuracy. APE is defined as the absolute value of the difference between the estimated experimental pose and the true pose. This metric can intuitively measure the closeness between the experimental trajectory and the true trajectory. As shown in Table 1, using the APE statistical metric on the Sequence_05 dataset, this invention reduces the root mean square error by 65.0%, the average error by 59.4%, and the maximum error by 73.6% compared to the LOAM method without a loop closure detection module. Therefore, the trajectory accuracy obtained using the method of this invention is significantly improved.

[0051] Table 1 Comparison of APE Statistical Indicators (Unit: m)

[0052] Statistical indicators LOAM method Method of the present invention Root mean square error of trajectory 11.7699 4.1118 average error 9.2648 3.7937 median of error 5.9568 3.9271 Error standard deviation 7.2589 1.5856 Maximum error 33.5559 8.8587

[0053] This invention discloses a laser SLAM method that integrates visual loop closure detection. By incorporating visual images into laser loop closure detection, and using a bag-of-words model of the visual images, image data containing a large amount of information can be transformed into bag-of-words vectors, significantly reducing the computational load of scene matching, improving image matching efficiency, and ensuring real-time loop closure detection. Finally, the visual loop closure is applied to global graph optimization, reducing accumulated errors and improving the accuracy of localization and mapping in the laser SLAM system.

[0054] Example 2

[0055] like Figure 4 As shown, this embodiment of the invention provides a laser SLAM system that integrates visual loop closure detection, including the following modules:

[0056] The laser odometry module 51 is used to collect laser point cloud data through 3D laser radar, extract the point cloud line features and surface features of the laser point cloud data, and obtain laser point cloud key frames based on the displacement of 3D laser radar. At the same time, the laser point cloud key frames are matched with local point cloud maps and local point cloud maps are matched with global point cloud maps according to point cloud line features and surface features to obtain current positioning information.

[0057] The map building module 52 is used to register the point cloud contained in each laser point cloud keyframe to the world coordinate system, and obtain the updated global point cloud map after point cloud downsampling;

[0058] The visual loop closure detection module 53 is used to acquire visual images using a visual camera and fuse laser point cloud keyframes to construct point cloud-image fusion keyframes to detect loop closure information. When loop closure information is detected, the corresponding loop closure constraints and loop closure trajectories are obtained.

[0059] The global graph optimization module 54 is used to build a pose graph model based on all point cloud-image fusion keyframes on the loop trajectory, add loop constraints as constraint edges to the pose graph model, optimize the pose graph model using the GTSAM library to obtain optimized point cloud-image fusion keyframes, and update the current positioning information and global point cloud map based on the optimized point cloud-image fusion keyframes.

[0060] The above embodiments are provided merely for the purpose of describing the present invention and are not intended to limit the scope of the invention. The scope of the invention is defined by the appended claims. Various equivalent substitutions and modifications made without departing from the spirit and principles of the invention should be covered within the scope of the invention.

Claims

1. A laser SLAM method integrating visual loop closure detection, characterized in that, include: Step S1: Laser odometry: Laser point cloud data is collected by 3D LiDAR, the point cloud line features and surface features of the laser point cloud data are extracted, and the laser point cloud key frame is obtained by using the displacement of the 3D LiDAR as a judgment condition. At the same time, the laser point cloud key frame is matched with the local point cloud map and the local point cloud map is matched with the global point cloud map according to the point cloud line features and surface features to obtain the current positioning information. Step S2: Map building: Register the point cloud contained in each laser point cloud keyframe to the world coordinate system, and obtain the updated global point cloud map after point cloud downsampling; Step S3: Visual loop closure detection: Use a visual camera to acquire visual images and fuse the laser point cloud keyframes to construct point cloud-image fusion keyframes to detect loop closure information. When loop closure information is detected, obtain the corresponding loop closure constraints and loop closure trajectories. Step S4: Global Graph Optimization: Establish a pose graph model based on all the point cloud-image fusion keyframes on the loop trajectory, add the loop constraint as a constraint edge to the pose graph model, optimize the pose graph model using the GTSAM library to obtain optimized point cloud-image fusion keyframes, and update the current positioning information and the global point cloud map according to the optimized point cloud-image fusion keyframes.

2. The laser SLAM method with fused visual loop closure detection according to claim 1, characterized in that, Step S1, which uses the displacement of the 3D LiDAR as a criterion to obtain keyframes of the laser point cloud, specifically includes: Calculate the displacement of the 3D LiDAR during operation. When the displacement exceeds a threshold, establish a new keyframe of the laser point cloud and perform the calculation within the current working cycle [t]. k ,t k+1 ] of t k+1 The laser point cloud keyframes are published continuously.

3. The laser SLAM method with fused visual loop closure detection according to claim 1, characterized in that, Step S3 involves acquiring visual images using a visual camera and fusing the laser point cloud keyframes to construct point cloud-image fusion keyframes, specifically including: Step S301: Obtain distance time t k+1 The latest t p The visual image at time t is used as a keyframe of the visual image. k+1 The laser point cloud keyframes published at specific times are correlated to form an initial point cloud-image fusion keyframe, where t p During the work cycle [t] k ,t k+1 ]between; Step S302: For [t] k ,t k+1 The laser point cloud data contained in the initial point cloud-image fusion keyframe within the specified time period is subjected to motion compensation using a linear interpolation method. First, as shown in formula (1), the motion compensation of the initial point cloud-image fusion keyframe within time period t is calculated. i Momentary point cloud X i In [t] k ,t i Pose transformation T between ] k,i Similarly, [t] can be calculated. k ,t p Pose transformation T between ] k,p As in formula (2): Among them, T k,k+1 The laser point cloud data is at the laser point cloud timestamp [t] k ,t k+1 The pose transformations between [ ] can be directly obtained by laser odometry; All point clouds X in the initial point cloud-image fusion keyframe i Using pose transformation T k,i Transform to t k At the time coordinates, the point cloud X is obtained. k Then, the point cloud X k Using pose transformation T k,p Convert to visual image timestamp t p At that moment, obtain X, which is synchronized with the timestamp of the visual image. p This achieves time synchronization between point cloud and visual image, and obtains motion-compensated point cloud-image fusion keyframes.

4. The laser SLAM method with fused visual loop closure detection according to claim 1, characterized in that, The step S3, which involves detecting loop closure information and obtaining the corresponding loop closure constraints and loop closure trajectories, specifically includes: Step S311: Select ORB features to extract feature point sets from the existing visual image sample set, use the Kmeans algorithm to cluster the feature point sets, and store the clustering results as words in a tree data structure to construct a visual dictionary. Step S312: Perform ORB feature extraction on the current keyframe of the visual image to obtain feature points. Query the word frequency of the word corresponding to the feature point in the visual dictionary. Calculate the score of each word using the TF-IDF method to obtain the bag-of-words vector V of the current keyframe of the visual image. k (v1,v2,···v n ); Step S313: Traverse all historical point cloud-image fusion keyframes, take the visual image corresponding to the historical point cloud-image fusion keyframe as the historical visual image keyframe, and obtain the bag-of-words vector V of all the historical visual image keyframes. h (v1,v2,···v n ), and calculate V according to formulas (3) to (4). k (v1,v2,···v n ) and V h (v1,v2,···v n The Manhattan distance S between the two images is used to obtain the image similarity η. The historical frame with the highest similarity η and greater than the threshold T is selected as the loop closure keyframe. Among them, v i ∈V k v j ∈V h S (k,h) For V k and V h The Manhattan distance between them is used to represent the bag-of-words vector distance; S (k,k-1) ) represents the bag-of-words vector distance between the current keyframe of the visual image and the previous keyframe of the visual image; Step S314: Project the point cloud feature points of the point cloud-image fusion key frame of the detected loop closure key frame onto the visual image key frame to obtain the two-dimensional pixel coordinates of the feature points of the visual image key frame. Use the three-point estimation method to assign depth values ​​to the two-dimensional pixel coordinates to obtain the three-dimensional spatial coordinates of the feature points of the current visual image key frame. Step S315: Using the three-dimensional spatial coordinates of the feature points, the feature points are matched with the loop closure keyframes. The PnP algorithm is applied to construct the reprojection error model. The least squares problem of equation (x) is solved by optimizing the LM algorithm to obtain the loop closure constraint.

5. A laser SLAM system integrating visual loop closure detection, characterized in that, Includes the following modules: The laser odometry module is used to collect laser point cloud data through 3D LiDAR, extract the point cloud line features and surface features of the laser point cloud data, and obtain laser point cloud key frames based on the displacement of the 3D LiDAR. At the same time, the laser point cloud key frames are matched with local point cloud maps and local point cloud maps are matched with global point cloud maps according to the point cloud line features and surface features to obtain current positioning information. The map building module is used to register the point cloud contained in each laser point cloud keyframe to the world coordinate system, and obtain the updated global point cloud map after point cloud downsampling; The visual loop closure detection module is used to acquire visual images using a visual camera and fuse the laser point cloud keyframes to construct point cloud-image fusion keyframes to detect loop closure information. When loop closure information is detected, the corresponding loop closure constraints and loop closure trajectories are obtained. The global graph optimization module is used to establish a pose graph model based on all the point cloud-image fusion keyframes on the loop trajectory, add the loop constraint as a constraint edge to the pose graph model, optimize the pose graph model using the GTSAM library to obtain optimized point cloud-image fusion keyframes, and update the current positioning information and the global point cloud map according to the optimized point cloud-image fusion keyframes.

Images