A Simultaneous Localization and Mapping Method Based on Multi-Sensors in a Dynamic Environment

Abstract

A simultaneous localization and mapping method based on multi-sensors in a dynamic environment, comprising: First, using YOLOV5 for dynamic target detection and sending detection boxes; using an inertial measurement unit (IMU) to correct point cloud distortion; extracting ground laser point cloud to facilitate subsequent point cloud clustering; extracting laser point cloud within the detection boxes and clustering the laser point cloud of dynamic objects; removing the clustered dynamic object point cloud from the laser point cloud after correcting the distortion for subsequent algorithm calculation; using iterative Kalman filtering to fuse the laser point cloud and IMU information to obtain the current pose information; according to the pose information, superimposing the point cloud after removing distortion and dynamic objects with the point cloud map to obtain a real-time updated point cloud map; using key frames, bag-of-words model and laser point cloud for loop detection.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous mobile robots, and in particular to a method for multi-sensor fusion positioning and mapping in a dynamic environment. Background Art

[0002] The positioning and mapping method based on a single sensor is difficult to cope with a relatively complex environment. For example, the method relying solely on a pure lidar is difficult to well solve the problem of point cloud motion distortion, adapt to environmental changes, and cope with dynamic environments.

[0003] For the solution of positioning and mapping with a lidar as the main sensor, its accuracy mainly depends on the inter-frame matching of point clouds. When there are many dynamic points in the point cloud, its positioning is likely to drift. Even if the positioning is accurate, there will be a large number of "ghosts" in the established point cloud map, thus affecting the reuse of the map.

[0004] In the prior art, regarding the simultaneous localization and mapping method in a dynamic environment, Patent CN112734836A discloses a method based on visual positioning, but this method is difficult to obtain accurate map scale information compared with the method with a lidar as the main sensor, and it is difficult to operate under the condition of lack of light. Patent CN113724387A discloses a method based on semantic segmentation, but this method is difficult to run in real time on a platform with limited computing resources. Summary of the Invention

[0005] In order to make up for the above deficiencies of the prior art, the present invention proposes a multi-sensor based simultaneous localization and mapping method in a dynamic environment. The camera and lidar are used to obtain environmental information, the YOLO V5 based object detection method is used to detect dynamic objects, the inertial measurement unit (IMU) is used to obtain the angular velocity and acceleration information of the robot to correct the point cloud motion distortion and the position of the detection box, and the target point cloud is extracted from the point cloud within the detection box through a conditional clustering method based on the Euclidean distance, and the extracted dynamic point cloud is removed to make the positioning and mapping of the algorithm more accurate and reliable.

[0006] The multi-sensor based simultaneous localization and mapping method in a dynamic environment includes the following steps:

[0007] Step 1: The image processing program runs separately on the GPU, uses the camera to continuously obtain the image information in the current environment, and performs dynamic object detection based on YOLO V5, and outputs the detection box information of the detected dynamic objects.

[0008] Step 2: Correct the motion distortion of the point cloud using the timestamp information of the inertial measurement unit (IMU) and the laser point cloud. By means of linear interpolation and IMU pre-integration, obtain the relative motion information of each laser point at the corresponding timestamp relative to the initial moment of the current frame. According to the relative motion data of the laser points, correct the laser points to the correct positions.

[0009] Step 3: Extract the ground laser point cloud to facilitate subsequent point cloud clustering. It is assumed that the dynamic objects in the environment exist above the feasible ground. Therefore, first extract the ground laser point cloud to greatly reduce the clustering difficulty. After removing the point cloud on the dynamic objects, then merge the ground point cloud with the non-ground point cloud after removing the dynamic object points for subsequent point cloud matching.

[0010] Step 4: Project the laser point cloud onto the image data, extract the laser point cloud within the detection box, and cluster the laser point cloud of the dynamic objects.

[0011] Step 5: Divide the point cloud into dynamic point cloud and non-dynamic point cloud. Retain the laser point cloud of the dynamic objects for use in subsequent dynamic obstacle avoidance algorithms. Remove the clustered dynamic object point cloud from the laser point cloud after correcting the distortion for the positioning and mapping of the algorithm.

[0012] Step 6: Use the iterative Kalman filter to fuse the laser point cloud and IMU information to obtain the relative pose of the robot relative to the previous frame data and the absolute pose in the world coordinate system.

[0013] Step 7: According to the pose information, overlay the point cloud after removing the distortion and dynamic objects with the point cloud map to obtain the real-time updated point cloud map.

[0014] Step 8: Retain the camera key frames, use the key frames, the bag-of-words model, and the laser point cloud for loop closure detection to further reduce the cumulative error.

[0015] As a preferred solution, in step 3, when extracting the ground laser point cloud, the specific steps include:

[0016] Step 3.1: Grid the laser point cloud after correcting the motion distortion on the XY plane according to the XY coordinates with a certain side length.

[0017] Step 3.2: For the point cloud in each grid, find the lowest point among them. If the absolute height of the lowest point is higher than a certain threshold, it is considered that there are no ground points in this grid. Otherwise, consider all the laser points within the grid whose height difference from the lowest point does not exceed the preset threshold as possible ground points.

[0018] Step 3.3: Calculate the height mean of all the selected possible ground points, remove the points that exceed the mean by a certain range from the possible ground points, and the remaining points are used as ground points.

[0019] As a preferred solution, in step 4, the laser point cloud within the detection box is extracted, and the laser point cloud of the dynamic object is clustered. The specific steps include:

[0020] Step 4.1: Using the internal parameter data of the camera and the external parameter data calibrated in advance by the camera and the radar, project the lidar point cloud data onto the image data.

[0021] Step 4.2: The camera and lidar data are processed in parallel on the GPU and CPU respectively, and there is no need to synchronize the camera and radar topics. Instead, according to the timestamp information obtained from the camera and radar data, the angular velocity and acceleration information of the IMU are used to compensate the position of the detection box. Integrate the IMU information within the time difference to obtain the relative motion generated by the robot between the moments when the camera and radar data are acquired. Use the calculated relative motion to further align the image and camera data, and then extract the point cloud within the detection box.

[0022] Step 4.3: Since there are only very few points on the dynamic object that is too far away, the impact on the simultaneous localization and mapping algorithm is extremely small, and the point cloud above a certain height generally does not include dynamic objects. Therefore, first perform a pass-through filter on the extracted point cloud according to the distance and height. Then use the clustering method based on the Euclidean distance. When the points around a point are within a certain range, the surrounding points are considered to be of the same class. When no new points can be added, it is considered that a complete class has been clustered. After clustering all the point clouds within the detection box, filter out the point cloud on the target dynamic object according to the information such as the number of point clouds and the point cloud morphology in each class.

[0023] As a preferred solution, in step 8, loop closure detection is performed using key frames, the bag-of-words model, and the laser point cloud. The specific steps include:

[0024] Step 8.1: Generate image key frames, and use the bag-of-words model to describe the key frames by statistically analyzing the feature types, and record the key frames and the current pose in the form of a feature vector.

[0025] Step 8.2: Compare the similarity between the newly generated key frames and the previous key frames, and extract the point clouds in the point cloud map corresponding to the poses of the key frames with higher similarity.

[0026] Step 8.3: Match the extracted several frames of point clouds with the current point cloud, and add the one with the highest matching degree as the loop closure result to the backend optimization.

[0027] The advantages and positive effects of the present invention are:

[0028] 1. The present invention can relatively quickly extract the ground point cloud in a general environment, which is convenient for clustering the point cloud on the ground.

[0029] 2. The method of the present invention combines lidar, camera and IMU, which can eliminate the influence of dynamic environment on map construction and improve the accuracy of positioning and mapping.

[0030] 3. The method of the present invention uses the parallel computing method of camera and lidar, which can greatly shorten the computing time compared with the serial method, has good real-time performance, and can run on platforms with limited computing resources. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 is a schematic flow diagram of the method of the present invention.

[0032] Figure 2 is a schematic flow diagram of the dynamic point cloud extraction of the present invention.

[0033] Figure 3a is a side view of eliminating dynamic points of the present invention, Figure 3b is a side view of non-eliminated dynamic points of the present invention.

[0034] Figure 4a is a top view of eliminating dynamic points of the present invention, Figure 4b is a top view of non-eliminated dynamic points of the present invention. SPECIFIC IMPLEMENTATION METHOD

[0035] The present invention will be further described in detail below with reference to the accompanying drawings:

[0036] The simultaneous localization and mapping method based on multi-sensors in a dynamic environment, the overall process of which is as Figure 1 shown, specifically includes the following steps:

[0037] Step 1: The image processing program runs independently in the GPU, uses the camera to obtain the image information of the current environment in real time, and performs dynamic target detection based on YOLO V5, and outputs the detection box information of the detected dynamic objects. After detecting the dynamic targets in the current environment, the detection boxes of the dynamic objects detected in the current image are recorded in the form of two points ( ), ( ), and sent to the ROS node.

[0038] Step 2: Use the timestamp information of the inertial measurement unit (IMU) and the laser point cloud to correct the motion distortion of the point cloud. Through the method of IMU pre-integration, the motion information from the initial moment to the end moment within the time of one frame of point cloud is obtained. Through the method of linear interpolation, the relative motion information of each laser point at the corresponding timestamp moment relative to the initial moment of the current frame is obtained. According to the relative motion data of the laser points, the laser points are corrected to the correct positions.

[0039] Step 3: Extract the ground lidar point cloud to facilitate subsequent point cloud clustering. Since the dynamic objects in the default environment exist above the feasible ground, extracting the ground lidar point cloud first greatly reduces the clustering difficulty. After removing the point cloud on the dynamic objects, the ground point cloud is then merged with the non-ground point cloud from which the dynamic object points have been removed for subsequent point cloud matching.

[0040] Step 3.1: Grid the lidar point cloud after correcting the motion distortion on the XY plane according to the XY coordinates with a certain side length.

[0041] Step 3.2: For the point cloud within each grid, find the lowest point. If the absolute height of the lowest point is higher than a certain threshold, it is considered that there are no ground points in this grid. Otherwise, take the lowest point as the ground reference point, and consider all lidar points within the grid whose height difference from the lowest point does not exceed the preset threshold as possible ground points.

[0042] Step 3.3: Calculate the height mean of all the selected possible ground points, and consider the points that exceed the mean by a certain range as noise points and non-ground points, and remove them from the possible ground points. The remaining points are taken as the ground points.

[0043] Step 4: The dynamic point cloud extraction process is as Figure 2 shown. Project the lidar point cloud from which the ground point cloud has been removed onto the image data, extract the lidar point cloud within the detection frame, and cluster the lidar point cloud of the dynamic objects.

[0044] Step 4.1: Use the internal parameter data of the camera calibration and the external parameter data pre-calibrated between the camera and the lidar to project the lidar point cloud data onto the image data.

[0045] Step 4.2: The camera and lidar data are processed in parallel on the GPU and CPU respectively, and there is no need to synchronize the camera and lidar topics. Instead, according to the timestamp information obtained from the camera and lidar data, use the angular velocity and acceleration information of the IMU to compensate the position of the detection frame. Integrate the IMU information within the time difference to obtain the relative motion generated by the robot between the moments when the camera and lidar data are acquired. Use the calculated relative motion to further align the image and camera data, and then extract the point cloud within the detection frame.

[0046] Step 4.3: Since there are only a very small number of points on a dynamic object at a long distance, the impact on the simultaneous localization and mapping algorithm is minimal. And the point cloud above a certain height generally does not include dynamic objects. Therefore, first perform a pass-through filter on the extracted point cloud according to the distance and height. Then use the clustering method based on the Euclidean distance. When the points around a point are within a certain range, the surrounding points are considered to be of the same class. When no new points can be added, it is considered that a complete class has been clustered. Since the objects within the detection box of the object detection are mainly target dynamic objects, the number of laser points on the target dynamic object should occupy the majority of the total number of points in the detection box. Therefore, set the minimum number of laser points requirement. After clustering all the point clouds within the detection box, remove the classes with less than the minimum number of laser points requirement. Arrange the remaining classes in descending order according to the number of point clouds, and then filter out the point clouds on the target dynamic object according to the information such as the shape of the point cloud in each class.

[0047] Step 5: Divide the point cloud into dynamic point cloud and non-dynamic point cloud according to the above steps. Retain the laser point cloud of the dynamic object for use in the subsequent dynamic obstacle avoidance algorithm. Remove the clustered dynamic object point cloud from the rectified distorted laser point cloud for localization and mapping of the subsequent algorithm.

[0048] Step 6: Use the iterative Kalman filter to fuse the laser point cloud and IMU information to obtain the current pose.

[0049] Step 7: According to the pose information, overlay the point cloud after removing distortion and dynamic point cloud with the point cloud map to obtain a real-time updated point cloud map. The comparison of the effects of removing dynamic point cloud and not removing dynamic point cloud is as Figure 3a 、 Figure 3b 、 Figure 4a 、 Figure 4b shown, where Figure 3a 、 Figure 3b are the side view comparison diagrams, Figure 4a 、 Figure 4b are the top view comparison diagrams.

[0050] Step 8: Retain the camera key frames, and use the key frames, bag-of-words model, and laser point cloud for loop closure detection to further reduce the cumulative error.

[0051] Step 8.1: Generate image key frames, and use the bag-of-words model to describe the key frames by statistically analyzing the feature types, and record the key frames and the current pose in the form of a feature vector.

[0052] Step 8.2: Compare the similarity between the newly generated key frames and the previous key frames, and extract the point clouds in the point cloud map corresponding to the poses of the key frames with higher similarity.

[0053] Step 8.3: Match the several frames of point clouds taken out with the current point cloud, and add the one with the highest matching degree as the loop closure result to the backend optimization.

[0054] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes, but is not limited to, the embodiments described in the specific implementation schemes. Any other similar implementation manners obtained by those skilled in the art according to the technical solutions of the present invention also fall within the protection scope of the present invention.

Claims

1. A method for simultaneous localization and mapping based on multi-sensors in a dynamic environment, comprising the following steps: Step 1: The image processing program runs independently on the GPU, uses the camera to obtain the image information in the current environment in real time, and performs dynamic object detection based on YOLOV5, outputting the detection box information of the detected dynamic objects; Step 2: Use the timestamp information of the inertial measurement unit (IMU) and the laser point cloud to correct the motion distortion of the point cloud; through the methods of linear interpolation and IMU pre-integration, obtain the relative motion information of the laser point at the corresponding timestamp relative to the initial moment of the current frame; according to the relative motion data of the laser point, correct the laser point to the correct position; Step 3: Extract the ground laser point cloud to facilitate subsequent point cloud clustering; by default, dynamic objects in the environment exist above the feasible ground, so extracting the ground laser point cloud first greatly reduces the clustering difficulty. After removing the point cloud on the dynamic objects, then merge the ground point cloud with the non-ground point cloud after removing the dynamic object points for subsequent point cloud matching; Step 4: Project the laser point cloud onto the image data, extract the laser point cloud within the detection box, and cluster the laser point cloud of the dynamic object; Step 5: Divide the point cloud into dynamic point cloud and non-dynamic point cloud; retain the laser point cloud of the dynamic object for use in subsequent dynamic obstacle avoidance algorithms; remove the clustered dynamic object point cloud from the laser point cloud after correcting the distortion for algorithm positioning and mapping; Step 6: Use iterative Kalman filtering to fuse the laser point cloud and IMU information to obtain the relative pose of the robot relative to the previous frame data and the absolute pose in the world coordinate system; Step 7: According to the pose information, superimpose the point cloud after removing the distortion and dynamic objects with the point cloud map to obtain the real-time updated point cloud map; Step 8: Retain the camera key frames, use the key frames, bag-of-words model, and laser point cloud for loop closure detection to further reduce the cumulative error; In step 3, the specific steps for extracting the ground laser point cloud include: Step 3.1: Grid the laser point cloud after correcting the motion distortion on the XY plane according to the XY coordinates with a certain side length; Step 3.2: For the point cloud in each grid, find the lowest point among them. If the absolute height of the lowest point is higher than a certain threshold, it is considered that there are no ground points in this grid. Otherwise, all laser points in the grid with a height difference not exceeding the preset threshold from the lowest point are considered possible ground points; Step 3.3: Calculate the height mean of all the selected possible ground points, remove the points that exceed the mean by a certain range from the possible ground points, and the remaining points are used as ground points.

2. The method for simultaneous localization and mapping based on multi-sensors in a dynamic environment according to claim 1, characterized in that: In step 4, the specific steps for extracting the laser point cloud within the detection box and clustering the laser point cloud of the dynamic object include: Step 4.1: Use the internal parameter data of the camera and the external parameter data pre-calibrated between the camera and the lidar to project the lidar point cloud data onto the image data; Step 4.2: The camera and lidar data are processed in parallel on the GPU and CPU respectively, and the camera and lidar topics do not need to be synchronized. Instead, the detection frame position is compensated using the angular velocity and acceleration information of the IMU based on the timestamp information obtained from the camera and lidar data. The IMU information is integrated within the time difference to obtain the relative motion of the robot between the camera and lidar data acquisition moments. The calculated relative motion is used to further align the image and camera data, and then the point cloud within the detection frame is extracted. Step 4.3: First, perform straight-through filtering on the extracted point cloud according to distance and height; then use the clustering method based on Euclidean distance. When the points around a point are within a certain range, the surrounding points are considered to be points of the same class. When no new points can be added, it is considered that a complete class is clustered; after clustering all the point clouds in the detection box, the point clouds on the target dynamic object are filtered out according to the number of point clouds in each class, the point cloud morphology and other information.

3. The method for simultaneous localization and mapping based on multi-sensors in a dynamic environment according to claim 1, characterized in that: In step 8, the key frame, bag-of-words model, and laser point cloud are used for loop detection. The specific steps include: Step 8.1: Generate an image keyframe, use the bag-of-words model to describe the keyframe by statistical feature types, and record the keyframe and current pose in the form of a representation vector; Step 8.2: Compare the similarity between the newly generated keyframe and the previous keyframe, and extract the point cloud in the point cloud map under the corresponding posture of the keyframe with higher similarity; Step 8.3: Match the extracted point clouds with the current point cloud, and add the one with the highest matching degree as the loop result to the backend optimization.

Images