Road grid map construction method based on laser SLAM and monocular vision

Abstract

This paper proposes a road grid map construction method based on laser SLAM and monocular vision. This method collects road area information while building the map and automatically completes grid map construction after the map is built. The mapping process consumes few computing resources and achieves high mapping accuracy. This method utilizes the local rematching technology of laser SLAM during multi-frame point cloud acquisition to significantly reduce the cumulative error in relative pose calculations and improve grid map construction accuracy. Using monocular vision for road detection, the detection accuracy is far higher than that of lidar for situations with relatively flat road boundaries or high granularity.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a road grid map construction method based on laser SLAM and monocular vision. Background Art

[0002] Environmental perception, planning and decision-making, and vehicle control are the three most important technologies in the field of autonomous driving. Among them, environmental perception is the basis of planning and decision-making for unmanned vehicles, and it is also the most technically difficult field in urban autonomous driving. Road detection is a relatively basic task in the field of environmental perception. It is also the basis for path planning of unmanned vehicles and the basic guarantee for safe driving. In the field of autonomous driving, in order to achieve global path planning and local path planning in highly dynamic environments, a mature set of autonomous driving solutions often uses a combination of high-precision maps and real-time road detection to complete path planning for urban roads. The maps used by robots can be divided into three categories: size maps, topological maps, and semantic maps. Raster maps are a type of size map commonly used by unmanned vehicles. Compared with topological maps and semantic maps, raster maps describe in detail the outlines of drivable areas and obstacles in the map, providing a unified path planning basis for unmanned vehicles with different geometric shapes and minimum turning radius. The vast majority of path planning algorithms, such as Dijkstra, A * , ant colony algorithm can be used on raster maps.

[0003] The technologies involved in road map construction primarily include road detection, positioning, and mapping. Common road detection methods are primarily categorized as machine vision-based and lidar-based. Machine vision-based road detection solutions can be categorized by sensor type: monocular, binocular, and depth cameras. Monocular cameras offer lower cost and consume fewer computational resources. However, due to the inability to obtain image depth, the dimensions of objects in the image cannot be determined (scale uncertainty). Binocular cameras address this issue, allowing the computer to calculate the depth of each point in the image based on the parallax between the left and right cameras. However, their measurement accuracy is limited by the binocular camera's baseline and resolution, and parallax calculations are computationally expensive. Depth cameras, like binocular cameras, can obtain scale information. Compared to binocular cameras, depth cameras acquire image depth directly through the transmit-receive hardware system, saving software computing resources. However, this active measurement method is susceptible to interference from sunlight when operating outdoors, and has a narrow measurement range. LiDAR-based road detection solutions can be categorized by sensor type: 2D mechanical LiDAR (hereafter referred to as 2D LiDAR), 3D mechanical LiDAR (hereafter referred to as 3D LiDAR), and solid-state LiDAR. 2D LiDAR solutions are relatively low-cost and provide a 360° field of view by measuring the distance of periodically scanned laser points. However, the 2D point cloud information they provide is limited and subject to motion distortion, making them suitable only for simple indoor environments with relatively flat floors. 3D LiDAR operates on a similar principle to 2D LiDAR, measuring the distances of multiple pairs of laser points during a 360° periodic scan to obtain discrete distance information in three dimensions. While 2D LiDAR can provide 3D point cloud information, it is still less informative than image information, offering little differentiation between objects of different colors, and is prone to missing smaller objects. Furthermore, it suffers from motion distortion. Solid-state LiDAR generally provides high-density point cloud information with a limited scanning angle (less than 360°). Compared to the aforementioned two types of LiDAR, it is free of motion distortion, and its higher resolution makes it less likely to miss smaller objects. Furthermore, it is also smaller in size. However, compared with mechanical lidar, it has problems such as limited scanning angle and sidelobe effect, and also has the problem of lack of color distinction.

[0004] SLAM is a common method for solving positioning and mapping problems. Since mapping is done in real time, there is no need to store the data collected for each frame (image or point cloud), which greatly saves storage space, but also places higher requirements on the real-time performance of the algorithm. SLAM is divided into vision-based and laser point cloud-based according to the sensor classification. Visual SLAM algorithms are also divided into three categories according to the sensor classification: monocular camera, binocular camera, and depth camera. ORB-SLAM is the most commonly used SLAM algorithm among the three categories of algorithms. The map it creates consists of a series of visual feature points; commonly used algorithms for laser SLAM include LOAM, cartographer, gmapping, etc., and the map it creates consists of a complete map point cloud. Whether it is visual SLAM or laser SLAM, manually marking the road area information or vector map information directly from the map created by SLAM is a very tedious and low-precision task. Summary of the Invention

[0005] In view of this, the present invention proposes a road grid map construction method based on laser SLAM and monocular vision, which can collect road area information while building the map, and automatically complete the construction of the grid map after the map is built. The mapping process occupies less computing resources and has high mapping accuracy.

[0006] To achieve the above object, the technical solution of the present invention is:

[0007] The present invention provides a road grid map construction method based on laser SLAM and monocular vision, which specifically comprises the following steps: installing a 3D laser radar and a monocular camera; jointly calibrating the monocular camera and the 3D laser radar to obtain the camera's internal parameter matrix and the conversion matrix from the laser radar coordinate system to the monocular camera coordinate system; collecting images and laser point clouds of the road section to be mapped, and recording the timestamp of the collection time, and then performing time synchronization on the collected images and point clouds to obtain the final image and the final point cloud data pair; calibrating the road area in the final image as a sample label for deep learning, and using it to train a deep learning model to obtain a semantic segmentation model for the road section to be detected; and calibrating the final point cloud data. According to the aligned laser point cloud, the laser SLAM algorithm is used to obtain the relative pose of the current frame point cloud relative to the first frame, and the point clouds are superimposed according to the relative pose to obtain a global point cloud map; the road area in the image is obtained according to the semantic segmentation model, and each frame of point cloud is projected into the image using the intrinsic parameter matrix and the transformation matrix. The point cloud located on the road is filtered out and downsampled using the image mask predicted by the semantic segmentation model to obtain a single-frame road point cloud; the single-frame road point cloud is transformed into the global map coordinate system using the relative pose, and the road point clouds transformed into the global coordinate system are superimposed frame by frame to obtain a global road point cloud map; the global road point cloud map is converted into a global binary raster map.

[0008] Before converting the global road point cloud map into a global binary raster map, the grid size of the binary raster map and the occupancy threshold of the grid cell are set, and the global road point cloud map is converted into a global binary raster map through the threshold.

[0009] When training the deep learning model, equally spaced annotations are performed in the order of recording time. The annotation method is to select the contour polygon of the road area, and the annotated samples are divided into a training set and a validation set for model training and model selection respectively; the convolutional neural network with ResNet as the skeleton network is used for training samples.

[0010] Among them, when using the laser SLAM algorithm, the laser SLAM algorithm is the LOAM algorithm, and a trained deep convolutional neural network is selected as a monocular vision road detector; the rigid transformation matrix of adjacent frames obtained by coarse matching of the laser odometry is used to correct the point cloud motion distortion of the current frame; and the relative pose matrix is ​​obtained by locally rematching the current frame point cloud with the local point cloud map.

[0011] Among them, the discretized point cloud voting algorithm is used to convert the global road point cloud into a global binary raster map. The specific steps are as follows:

[0012] Establish an integer sparse matrix that meets the maximum size of the global road point cloud;

[0013] The point cloud discrete voting method is used to count the number of global road point clouds falling into each grid;

[0014] A binary road grid map is generated by setting a threshold on the number of point clouds in grid cells, and a Boolean sparse matrix is ​​used to store the map.

[0015] Beneficial effects:

[0016] This invention utilizes the local rematching technology of laser SLAM during multi-frame point cloud acquisition, which greatly reduces the cumulative error of relative pose calculation and improves the mapping accuracy of raster maps; it uses monocular vision for road detection, and the detection accuracy is much higher than that of lidar in cases where the road boundaries are relatively flat or the granularity is high.

[0017] The present invention can realize the automated construction of road grid maps on a smaller scale without the need for GPS and INS positioning. It is still applicable to field road mapping in remote locations with poor satellite signals, and is also applicable to unmanned vehicles without GPS or INS modules installed.

[0018] The semantic segmentation model of the present invention identifies road areas and is therefore still applicable to roads with relatively complex topological relationships. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1Schematic diagram of the installation coordinate system of the camera and 3D laser radar in step 1 of the present invention.

[0020] Figure 2 This is a schematic diagram of the outline polygon of the road area in the manually marked image in step four of the present invention.

[0021] Figure 3 This is a schematic diagram of the road area identified by the semantic segmentation network in step six of the present invention.

[0022] Figure 4 This is a system block diagram of the LOAM-based grid map construction system described in the present invention.

[0023] Figure 5 This is a schematic diagram of a single-frame road point cloud obtained in step six of the present invention.

[0024] Figure 6 This is the global road point cloud map obtained in step 7 of the manual.

[0025] Figure 7 This is the global binary grid map obtained in step 8 of the manual.

[0026] Among them, 1-3D lidar and its coordinate system; 2-monocular camera and its coordinate system; 3-polygonal outline of the road area in the manually annotated image; 4-road part in the image; 5-road area identified by the semantic segmentation model. DETAILED DESCRIPTION

[0027] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0028] This invention provides an integrated mapping solution combining laser SLAM and monocular vision. This solution collects road area information while building the map and automatically constructs a grid map after the map is completed. This mapping process consumes few computing resources and achieves high accuracy, laying the foundation for subsequent global path planning and assisted global positioning of unmanned vehicles. This invention aims to solve the problem of automated construction of smaller-scale road grid maps and proposes a method for constructing a binary road grid map based on laser SLAM and monocular vision.

[0029] According to the method for constructing a road binary grid map based on laser SLAM and monocular vision according to an embodiment of the present invention, the specific steps are as follows:

[0030] Step 1: Install a forward-looking monocular camera and a 3D laser radar on the vehicle body. The coordinate system of the monocular camera is x c -y c -z c and the laser radar coordinate system x l -y l -z l like Figure 1 shown.

[0031] Step 2: Use the calibration plate to jointly calibrate the monocular camera and 3D lidar to obtain the camera's intrinsic parameter matrix and the lidar coordinate system x l -y l -z l To the monocular camera coordinate system x c -y c -z c The transformation matrix (external parameter matrix).

[0032] Step 3: Collect images and laser point clouds of the road section to be mapped (collect at least once for each branch road section), record the timestamp of the collection time, and then synchronize the collected images and point clouds to obtain the final image and final point cloud data pair.

[0033] Step 4: Manually mark the road area in the final image as a sample label for deep convolutional neural network training to obtain a monocular vision-based semantic segmentation model of the road section to be detected.

[0034] Preferably, step four specifically includes:

[0035] S41: Control the unmanned vehicle to drive on the road to be inspected, and simultaneously collect images from the monocular camera and point clouds from the 3D lidar. It is necessary to ensure that each road on the road network to be inspected is traveled at least once.

[0036] S42: According to the time sequence of the recording, select the manual marking interval N, and manually mark the road area in the image as a sample label every N times. The polygonal outline of the manually marked road area is as follows: Figure 2 As shown in the figure, the binary mask of the road area is calculated based on the polygon outline to obtain the sample labels for neural network learning, and the final samples are randomly divided into 80% training set and 20% validation set, which are used for model training and model selection respectively.

[0037] S43: Select DeepLabV3 neural network (skeleton network uses ResNet18) for training, select the stochastic gradient descent algorithm with momentum factor as the optimizer, and select the polynomial strategy for learning rate adjustment. The formula of the stochastic gradient descent algorithm with momentum factor is:

[0038]

[0039] v←αv-∈g

[0040] θ←θ+v

[0041] Where m is the batch size in the stochastic gradient descent algorithm, θ is a parameter in the network, L is the loss function, and x (i)is the sample, y (i) is the label, ∈ is the learning rate, α is the damping factor, f(x (i) ; θ) means that the neural network has parameters θ and the sample input is x (i) Output when .

[0042] The formula for the polynomial (learning rate adjustment) strategy is as follows:

[0043]

[0044] Where γ is the polynomial exponent, l is the current learning rate, and l min is the minimum learning rate, s i For the current study of algebra, s max is the maximum attenuation algebra.

[0045] S44: Select an appropriate number of training generations based on the complexity of the road, and test the network's performance on the validation set every M generations. The validation set performance indicator is the IoU indicator of the road category, and its calculation formula is as follows:

[0046]

[0047] After the training is completed, the model parameters with the highest IoU index in the validation set are selected as the final parameters. The schematic diagram of the road area predicted by the neural network is as follows: Figure 3 As shown (white - road area, black - non-road area), let its binary image mask be B road (1-road, 0-non-road).

[0048] Step 5: Use the laser SLAM algorithm to obtain the relative pose (positioning) of the laser point cloud in the final point cloud data pair relative to the first frame, and superimpose the point clouds according to the relative pose to obtain a global point cloud map (construction).

[0049] Preferably, step five specifically includes:

[0050] S51: Select LOAM as the laser SLAM algorithm and use the deep convolutional network trained in step 4 as the monocular vision road detector.

[0051] S52: Use the rigid transformation matrix of the adjacent frames (initialized as the unit matrix) to perform motion distortion correction on the point cloud of the current frame. According to the uniform motion assumption of the LOAM algorithm, the motion distortion correction formula is as follows:

[0052]

[0053] P′ i =T (k+1，i) P i

[0054] P i =[x i ,y i , z i , 1] T , P′ i =[x′ i , y′ i , z′ i , 1] T

[0055] where x i ,y i , z i and x′ i , y′ i , z′ i are the three-dimensional coordinates of a single point in the current frame point cloud when it is scanned to the i-th angle before and after correction (since the point correction formula at the same scanning angle is the same, the formula takes a single point as an example), P i , P′ i are their augmented vectors, T k+1 is the transformation matrix of adjacent frames, t k+1 , t k are the time points at which the current frame and the previous frame point cloud complete scanning, t i The moment when the current frame point cloud is scanned to the i-th angle.

[0056] S53: Extract feature points from the corrected point cloud in S52, filter out plane points and edge points respectively, combine the plane points and edge points of the point cloud of the previous frame, and use the laser odometry coarse matching algorithm in LOAM to obtain the rigid transformation matrix of adjacent frames, which is used to update the corresponding matrix in S52.

[0057] S54: The point cloud of the features extracted in S53 is superimposed between frames through relative pose (relative to the first frame, the initial value is set to the unit matrix) to form a local feature point cloud map (local point cloud map), and then sent to the laser mapper to construct a global feature point cloud map (global point cloud map). The laser mapper will delete the points that exceed the local cube area L according to the position of the local point cloud map in the global point cloud map. u ×W i ×H u Local point cloud map.

[0058] S55: Use the feature point cloud extracted in S53 to perform local rematching with the local point cloud map (the algorithm principle of local rematching is the same as that in S53, only the point cloud of the previous frame is replaced by the local point cloud map) to obtain a more accurate relative pose (relative to the first frame) and update the relative pose matrix used in S54.

[0059] Step 6: Use the semantic segmentation model obtained in step 4 to obtain the road area in the image. Use the intrinsic parameter matrix and extrinsic parameter matrix calibrated in step 2 to project each frame of point cloud into the image. Use the image mask predicted by the semantic segmentation model to filter out the point cloud located on the road and downsample it to obtain a single frame of road point cloud.

[0060] Preferably, step six specifically includes:

[0061] The road detector described in S51 is used to perform semantic segmentation of the road area on the image in the synchronous frame, and the point cloud corrected in S52 is projected onto the semantically segmented image (such as Figure 3 ), the projected point cloud numbers in the road area are obtained, and the road point cloud parts in the corrected point cloud are filtered out according to these numbers (such as Figure 5 The specific algorithm is expressed as follows:

[0062] Let a point in the corrected point cloud be P′ i =[x′ i , y′ i , z′ i , 1] T , the image size is H I ×W I , the coordinate system x obtained in step 2 l -y l -z l To coordinate system x c -y c -z c The external parameter matrix is ​​M E , the obtained camera intrinsic parameter matrix is ​​M I ,in

[0063]

[0064] Project the point cloud 3D coordinates to image coordinates The formula is as follows:

[0065]

[0066]

[0067] Where int(·) represents a rounding operation.

[0068] like Does not meet the conditions and Then directly discard the point; if it satisfies, substitute it into the binary image mask B in S43 raod In, if Then retain point P′ i, otherwise discard. Perform the above operation on each point of the corrected point cloud to obtain a single-frame road point cloud (such as Figure 5 ).

[0069] Step 7: Use the relative pose obtained in step 5 to transform the single-frame road point cloud into the global map coordinate system (the pose of the first frame point cloud is the origin), and superimpose the road point clouds transformed into the global coordinate system frame by frame to obtain a global road point cloud map.

[0070] Preferably, step seven specifically includes:

[0071] Select an appropriate leaf node size, perform voxel downsampling on the single-frame road point cloud obtained in step 6, and then superimpose the downsampled single-frame road point cloud onto the global road point cloud map through the relative pose obtained in S55. Since the relative pose used in this step is consistent with that in S55, the local re-matching step in S55 is not required, so the transition step of the local road point cloud can be omitted. The final global road point cloud map is as follows: Figure 6 As shown in the figure, due to the limited field of view of a monocular camera, a single-frame road point cloud exhibits a fan-shaped distribution. However, by stacking multiple frames, a road point cloud approximates the regional distribution of the road. For typical wheeled unmanned vehicles, only the point cloud information in the X and Y axes is retained.

[0072] Step 8: Set the grid size of the binary grid map and the occupancy threshold of the grid cell to convert the global road point cloud map into a global binary grid map.

[0073] Preferably, step eight uses a discretized point cloud voting algorithm to convert the global road point cloud into a global binary raster map, and the specific steps are as follows:

[0074] S81: Calculate the maximum and minimum values ​​of the global road point cloud on the X-axis and Y-axis, respectively denoted as x max , x min ,y max ,y min , let the grid unit size of the grid map be C W ×C H . Create a size of int((x max -x min +1) / C W )×int(y max -y min +1 / C H ) is a sparse matrix S, and the storage type of the sparse matrix is ​​integer.

[0075] S82: For each point of the global road point cloud Perform the following normalization transformation:

[0076]

[0077]

[0078] S83: Add 1 to the position value of the sparse matrix S corresponding to the normalized coordinates of the point cloud, that is, This operation is performed on each point of the global point cloud to obtain the road point cloud voting coefficient matrix S.

[0079] S84: Select the point cloud number threshold N of the grid cell th , let the storage coefficient matrix of the binary raster map be S b , the storage type is Boolean. For each non-zero unit S(i, j) of the sparse matrix S, if S(i, j) ≥ N th , then S b (i, j) = 1. Get the sparse matrix S of the binary grid map b , binary raster map such as Figure 7 As shown (shown in gray in the road grid map, it is actually green).

[0080] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A road grid map construction method based on laser SLAM and monocular vision, characterized in that: The specific steps are as follows: install 3D lidar and monocular camera; perform joint calibration on the monocular camera and 3D lidar to obtain the camera's intrinsic parameter matrix and the conversion matrix from the lidar coordinate system to the monocular camera coordinate system; collect images and laser point clouds of the road section to be mapped, and record the timestamp of the acquisition time, and then synchronize the collected images and point clouds to obtain the final image and the final point cloud data pair; calibrate the road area in the final image as a sample label for deep learning, and use it to train the deep learning model to obtain the semantic segmentation model of the road section to be detected; use laser SLAM to calibrate the laser point cloud in the final point cloud data pair The algorithm obtains the relative pose of the current frame point cloud relative to the first frame, and superimposes the point clouds according to the relative pose to obtain a global point cloud map; obtains the road area in the image according to the semantic segmentation model, projects each frame point cloud into the image using the intrinsic parameter matrix and the transformation matrix, and filters out the point cloud located on the road using the image mask predicted by the semantic segmentation model and performs downsampling to obtain a single-frame road point cloud; transforms the single-frame road point cloud into a global map coordinate system using the relative pose, and superimposes the road point clouds transformed into the global coordinate system frame by frame to obtain a global road point cloud map; and converts the global road point cloud map into a global binary raster map; When training the deep learning model, the recordings were annotated at equal intervals in chronological order. The annotation method was to select the outline polygons of the road area. The annotated samples were divided into a training set and a validation set for model training and model selection, respectively. The convolutional neural network with ResNet as the backbone network was used for training samples. When using a laser SLAM algorithm, the laser SLAM algorithm is a LOAM algorithm, and a trained deep convolutional neural network is selected as a monocular vision road detector; the rigid transformation matrix of adjacent frames obtained by coarse matching of the laser odometry is used to correct the motion distortion of the point cloud of the current frame; and the relative pose matrix is ​​obtained by locally rematching the point cloud of the current frame with the local point cloud map.

2. The method according to claim 1, wherein Before converting the global road point cloud map into a global binary raster map, the grid size of the binary raster map and the occupancy threshold of the grid cell are set, and the global road point cloud map is converted into a global binary raster map through the threshold.

3. The method according to claim 1 or 2, wherein: The discretized point cloud voting algorithm is used to convert the global road point cloud into a global binary raster map. The specific steps are as follows: Establish an integer sparse matrix that meets the maximum size of the global road point cloud; The point cloud discrete voting method is used to count the number of global road point clouds falling into each grid; A binary road grid map is generated by setting a threshold on the number of point clouds in grid cells, and a Boolean sparse matrix is ​​used to store the map.

Images