A map construction method based on laser and camera fusion

Abstract

The map construction method of laser and camera fusion includes: obtaining three-dimensional point cloud data P of the vehicle body surrounding environment collected by a laser radar fixed on the top of the vehicle body and image data I of the vehicle surrounding environment collected by a camera fixed in front of the vehicle body; calculating and obtaining the posture E according to the laser point cloud data P; filtering based on the obtained point cloud data P to obtain unobstructed and regularly distributed point cloud data P'; performing semantic segmentation on the image I captured by the camera, removing moving objects and dynamic feature points in the original image, and obtaining an image I' with semantic information; fusing the point cloud P' with the semantic segmentation result I' from the camera to generate a single-frame point cloud C with semantic information; after extracting and matching feature points, eliminating unmatched feature point pairs to obtain a point cloud C' with unmatched pairs eliminated; superimposing the point cloud C' with the existing map M according to the posture E of the car to obtain a real-time updated map M'.

Description

Technical Field

[0001] The present invention relates to the field of computer vision and robot intelligent driving technology, and more specifically, to a map construction method integrating laser and camera. Technical Background

[0002] When entering an unknown environment, positioning and navigation are the main challenges of robot environmental perception tasks. In the study of such problems, accurate map modeling is one of the focuses. Since each sensor has limitations, using a single sensor cannot obtain detailed environmental information. Therefore, it is necessary to study multi-sensor fusion mapping.

[0003] In actual robot application scenarios, dynamic objects will inevitably exist, which will greatly affect the accuracy of map construction. At the same time, in order to enable robots to complete more complex tasks, the robot's ability to understand the scene has received widespread attention from researchers. Semantic maps can well indicate where the robot is, what the robot "sees", and what the displayed point clouds are. The construction of semantic maps in dynamic environments plays an important role in the mobile robot's environmental understanding and subsequent path planning.

[0004] Most of the methods for accurate map modeling in the prior art revolve around a single sensor and the fusion of two sensors. Patent CN201911242680.7 discloses a three-dimensional laser mapping method and system, patent CN202110318776.8 discloses a multi-robot distributed collaborative visual mapping method based on scene recognition, patent CN201810869146.8 discloses a topological map creation method and device based on lidar and GPS, patent CN202110469215.8 discloses a mapping method, device and system based on ultrasonic radar and lidar, etc. These methods have the following two problems: 1. They are greatly affected by the environment: easily affected by weather and dynamic objects 2. Feature point pairs do not match, affecting the map matching accuracy. Summary of the invention

[0005] In order to solve the above technical problems, the present invention provides a map construction method that integrates laser and camera, which uses vision and laser sensors to obtain environmental information, and then performs semantic recognition on two-dimensional scene images based on a semantic segmentation model, and eliminates the existence of moving objects and dynamic feature points in the original image by fusing the semantic image of the data set and the original image. Then, the recognized semantic information and the three-dimensional point cloud are fused, and after feature point extraction and matching, the unmatched feature points are eliminated to perform map optimization. The three-dimensional semantic map can well represent the scene where the mobile robot is located, identify buildings, pedestrians, vehicles, etc. in the scene, and plays an important role in the mobile robot's environmental understanding and subsequent path planning, and can effectively solve the problems of complex dynamic environments for simultaneous positioning and mapping technologies.

[0006] The above technical objectives of the present invention are achieved through the following technical solutions:

[0007] The laser and camera fusion map construction method includes the following steps:

[0008] Step 1: Obtain the three-dimensional point cloud data P of the vehicle body surrounding environment collected by the laser radar fixed on the top of the vehicle body and the image data I of the vehicle surrounding environment collected by the camera fixed in front of the vehicle body;

[0009] Step 2: Calculate and obtain the pose E based on the laser point cloud data P;

[0010] Step 3: At the same time, filtering is performed based on the obtained point cloud data P to obtain unobstructed and regularly distributed point cloud data P';

[0011] Step 4: Perform semantic segmentation on the image I captured by the camera, remove the moving objects and dynamic feature points in the original image, and obtain an image I' with semantic information;

[0012] Step 5: Fuse the point cloud P' and the semantic segmentation result I' from the camera to generate a single-frame point cloud C with semantic information;

[0013] Step 6: After feature point extraction and matching, unmatched feature point pairs are eliminated to obtain a point cloud C' without unmatched pairs.

[0014] Step 7: Superimpose the point cloud C' with the existing map M according to the car posture E to obtain the real-time updated map M'.

[0015] Finally, a semantic map is generated by the fusion of lidar and camera, which facilitates the subsequent positioning, navigation and obstacle avoidance of unmanned vehicles.

[0016] As a preferred solution, in step 3, a point cloud filtering method is used to pre-process the input point cloud to obtain unobstructed and regularly distributed point cloud data P', which specifically includes the following steps:

[0017] Step 3.1: Remove outliers (caused by occlusion): Perform statistical analysis on the neighborhood of the query point and calculate the distance from it to its neighboring points. The distance distribution characteristics conform to the Gaussian distribution N(u,σ 2 ), u and σ 2 Determine a standard range. Points outside the standard range are outliers and are removed from the data;

[0018] Step 3.2: Simplify the massive point cloud: construct a three-dimensional voxel grid of m×n×l, fill the point cloud data into the corresponding small voxel grid, and replace the other points of each voxel with the centroid of all points in the voxel to reduce the amount of data;

[0019] Step 3.3: Process irregular point cloud distribution: specify the coordinate range, remove distant and sparse parts, and retain dense point clouds that contain most of the features.

[0020] As a preferred solution, in step 4, semantic segmentation is performed on the image I captured by the camera, and moving objects and dynamic feature points in the original image are removed to obtain an image I' with semantic information, which specifically includes the following steps:

[0021] Step 4.1: Generate a semantic segmentation model that can accurately identify dynamic objects such as pedestrians and vehicles by training the network;

[0022] Step 4.2: Use the semantic segmentation model generated by the network to perform semantic segmentation on the image I captured by the camera to obtain an image I with semantic information 1 ;

[0023] Step 4.3: Add a mask to the dynamic object in the point cloud image with semantic information, perform a binary AND operation on each pixel value corresponding to the semantic mask image and the original image, and obtain a processed dynamic object culling image I'.

[0024] As a preferred solution, after extracting and matching feature points in step 6, unmatched feature point pairs are eliminated to obtain a point cloud C' without unmatched pairs, which specifically includes the following steps:

[0025] Step 6.1: Set the maximum number of iterations MAX, and randomly extract Y feature point pairs in each iteration for subsequent calculations. At the same time, calculate the ratio of the best distance to the second best distance for the Y feature point pairs extracted in each iteration:

[0026]

[0027] Among them, X best is the optimal distance, X second is the second best distance;

[0028] Step 6.2: Place the optimal feature point pair and the suboptimal feature point pair in the last two iterations respectively;

[0029] Step 6.3: Dynamically adjust the number of iterations. While ensuring that the number of iterations decreases in an orderly manner, the feature point pairs calculated in the next round will be better and the resulting model will be more accurate.

[0030] As a preferred solution, step 6.3 reduces the calculation time by dynamically adjusting the number of iterations, which specifically includes the following steps:

[0031] Step 6.3.1: Introduce statistic a, a = (the number of feature point pairs detected as local points under this model / the number of overall feature point pairs) 2 , set the statistical threshold to Th.

[0032] Step 6.3.2: When the statistic a is less than Th, introduce formula (1):

[0033]

[0034] When the statistic a is greater than Th, formula (2) is introduced:

[0035] nnum=log((1-a N ) 3 ) (2)

[0036] Step 6.3.3: nnum ≥ 0 or num ≤ nnum*MAX The number of iterations will not change, and the iterations will still proceed in order. Instead, the maximum number of iterations is updated to formula (3):

[0037]

[0038] Where num = log(1-p), p is the learning rate, and if the score of the current iteration exceeds the threshold, or if it has reached the final iteration, the model with the highest score is selected to eliminate the mismatch.

[0039] Preferably, the number of feature points Y in step 6.1 is 10, the statistical threshold Th in step 6.3.1 is 0.8, and the learning rate p in step 6.3.3 is 0.01.

[0040] In summary, the present invention has the following beneficial effects:

[0041] (1) The map construction method of laser and camera fusion provided by the present invention can provide more complete and accurate environmental information, and has little impact on weather changes, and is suitable for a variety of indoor and outdoor places.

[0042] (2) The laser and camera fusion map construction method provided by the present invention can eliminate the impact of the dynamic environment on map construction, improve map accuracy, and provide convenience for subsequent positioning, navigation and tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is the overall framework diagram of the map construction method of laser and camera fusion of the present invention.

[0044] Figure 2 It is a schematic diagram of a process for eliminating mismatches between feature points in the map construction method of laser and camera fusion of the present invention. DETAILED DESCRIPTION

[0045] The present invention is further described in detail below in conjunction with the accompanying drawings:

[0046] The laser and camera fusion map construction method includes the following steps:

[0047] Step 1: Obtain the three-dimensional point cloud data P of the vehicle body surrounding environment collected by the laser radar fixed on the top of the vehicle body and the image data I of the vehicle body surrounding environment collected by the camera fixed in front of the vehicle body.

[0048] Step 2: Calculate and obtain the posture E based on the laser point cloud data.

[0049] Step 3: Use the point cloud filtering method to preprocess the point cloud collected by the lidar to obtain the appropriate point cloud data P'. First, perform statistical analysis on the neighborhood of the query point and calculate the distance from it to the neighboring points. The distance distribution characteristics conform to the Gaussian distribution N(u,σ 2 ), u and σ 2 Determine a standard range. Points outside the standard range are outliers and are removed from the data. Then construct a m×n×l 3D voxel grid, fill the point cloud data into the corresponding small voxel grid, and use the centroid of all points in each voxel to replace other points in the voxel to reduce the amount of data. Specify the coordinate range, remove the distant and sparse parts, and retain the dense point cloud containing most of the features. Here, it is set to remove 2 meters from the highest point.

[0050] Step 4: Use the semantic segmentation method to perform semantic segmentation on the image I captured by the camera to remove moving objects and dynamic feature points in the original image. First, train the network to generate a semantic segmentation model that can accurately identify dynamic objects such as pedestrians and vehicles, input the image captured by the camera, and obtain the image I with semantic information. 1 ; Then, a mask is added to the dynamic object in the image with semantic information; Finally, a binary AND operation is performed on each pixel value corresponding to the semantic mask image and the original image to obtain the processed dynamic object removal image I'.

[0051] Step 5: Fuse the point cloud P' with the semantic segmentation result I' from the camera to generate a single-frame point cloud C with semantic information. After the visual semantic segmentation is completed, it needs to be fused with the laser point cloud. The two most important steps are spatial matching and temporal matching. In actual operation, the camera intrinsic parameters need to be calibrated. During the calibration process, a 10×7 black and white grid is used to automatically obtain about 40 groups of 9×6 intersection points through the program to calculate the camera intrinsic parameters. The camera intrinsic parameters are used to perform a joint calibration of the external parameters of the camera and the lidar. During the joint calibration, the joint calibration parameters of the camera and the lidar can be obtained by calculating the 9 groups of data that correspond one to one in the manually annotated image and the point cloud map.

[0052] Step 6: Eliminate unmatched feature point pairs. Figure 2 As shown in the figure, first set the maximum number of iterations MAX, and randomly extract 10 feature point pairs in each iteration for subsequent calculations. At the same time, calculate the ratio of the best distance to the second best distance for the 10 feature point pairs extracted in each iteration, and then place the best feature point pair and the second best feature point pair in the last two iterations respectively, and finally dynamically adjust the number of iterations. Determine the size of the statistic a and the threshold. When the statistic a is less than the set value of 0.8, When the statistic a is greater than the set value 0.8, nnum = log((1-a 10 ) 3 ). When nnum≤0 or nnum≥nnum*MAX, the maximum number of iterations is updated to If the score of the current iteration exceeds the threshold, or if it has reached the final iteration, the model with the highest score is selected to eliminate the unmatched feature point pairs.

[0053] Step 7: Superimpose the point cloud C' with the existing map M according to the car posture E to generate a real-time updated map M'.

[0054] This specific embodiment is merely an explanation of the present invention and is not a limitation of the present invention. After reading this specification, those skilled in the art may make non-creative modifications to the present embodiment as needed. However, as long as they are within the scope of the claims of the present invention, they are protected by the patent law.

Claims

1. A map construction method using laser and camera fusion, comprising the following steps: Step 1: Obtain the three-dimensional point cloud data P of the vehicle body surrounding environment collected by the laser radar fixed on the top of the vehicle body and the image data I of the vehicle body surrounding environment collected by the camera fixed in front of the vehicle body; Step 2: Calculate and obtain the pose E based on the laser point cloud data P; Step 3: At the same time, filtering is performed based on the obtained point cloud data P to obtain unobstructed and regularly distributed point cloud data P'; Step 4: Perform semantic segmentation on the image I captured by the camera, remove the moving objects and dynamic feature points in the original image, and obtain an image I' with semantic information; Step 5: Fuse the point cloud P' and the semantic segmentation result I' from the camera to generate a single-frame point cloud C with semantic information; Step 6: After feature point extraction and matching, unmatched feature point pairs are eliminated to obtain a point cloud C' without unmatched pairs. Step 7: According to the car posture E, the point cloud C' is superimposed on the existing map M to obtain the real-time updated map M', and finally a semantic map fused by the lidar and the camera is generated, which facilitates the subsequent positioning, navigation and obstacle avoidance of the unmanned vehicle; In step 3, the input point cloud is preprocessed using a point cloud filtering method to obtain unobstructed and regularly distributed point cloud data P', which specifically includes the following steps: Step 3.1: Remove outliers caused by occlusion: Perform statistical analysis on the neighborhood of the query point and calculate the distance from it to the nearest neighbor. The distance distribution characteristics conform to the Gaussian distribution. Points with distances outside the standard range are outliers and are removed from the data. Step 3.2: Simplify the massive point cloud: construct a three-dimensional voxel grid of m×n×l, fill the point cloud data into the corresponding small voxel grid, and replace the other points of each voxel with the centroid of all points in the voxel to reduce the amount of data; Step 3.3: Process irregularly distributed point clouds: specify the coordinate range, remove distant and sparse parts, and retain dense point clouds that contain most features; In step 4, semantic segmentation is performed on the image I captured by the camera to remove moving objects and dynamic feature points in the original image to obtain an image I' with semantic information, which specifically includes the following steps: Step 4.1: Generate a semantic segmentation model that can accurately identify dynamic objects such as pedestrians and vehicles by training the network; Step 4.2: Perform semantic segmentation on the image I captured by the camera through the semantic segmentation model generated by the network to obtain an image I1 with semantic information; Step 4.3: Add a mask to the dynamic object in the point cloud image with semantic information, perform a binary AND operation on each pixel value corresponding to the semantic mask image and the original image, and obtain a processed dynamic object removal image I'; Step 6 After the feature points are extracted and matched, unmatched feature point pairs are eliminated to obtain a point cloud C' without unmatched pairs. Specifically, the following steps are included: Step 6.1: Set the maximum number of iterations MAX, randomly extract Y feature point pairs in each iteration for subsequent calculations; at the same time, calculate the ratio of the best distance to the second best distance for the Y feature point pairs extracted in each iteration: Among them, X best is the optimal distance, X second is the second best distance; Step 6.2: Place the optimal feature point pair and the suboptimal feature point pair in the last two iterations respectively; Step 6.3: Dynamically adjust the number of iterations; while ensuring that the number of iterations is reduced in an orderly manner, the feature point pairs calculated in the next round will be better and the obtained model will be more accurate; Step 6.3 reduces the calculation time by dynamically adjusting the number of iterations, which specifically includes the following steps: Step 6.3.1: Introduce statistic a, a = (the number of feature point pairs detected as local points under this model / the number of overall feature point pairs) 2 , set the statistical threshold to Th; Step 6.3.2: When the statistic a is less than Th, introduce formula (1): When the statistic a is greater than Th, formula (2) is introduced: nnum=log((1-a N ) 3 ) (2) Step 6.3.3: If nnum ≥ 0 or num ≤ nnum*MAX, the number of iterations will not change and the iterations will still be performed in sequence; instead, the maximum number of iterations is updated to formula (3): Where num=log(1-p), p is the learning rate, and if the score of the current iteration exceeds the threshold, or if it has reached the final iteration, the model with the highest score is selected to eliminate the mismatch.

2. The laser and camera fusion map construction method according to claim 1, characterized in that: The number of feature points Y in step 6.1 is 10, the statistical threshold Th in step 6.3.1 is 0.8, and the learning rate p in step 6.3.3 is 0.01.

Images