A semantic map construction method based on laser and vision fusion

Abstract

The present invention discloses a semantic map construction method based on laser and visual fusion, which relates to the field of unmanned driving SLAM technology, solves the technical problems of poor robustness of large-scale closed-loop detection in unmanned driving technology and lack of semantic information in constructing point cloud maps. The key points of its technical solution are to use a surround camera module to collect picture data of the surrounding environment, and then transfer the picture to the DeepLabv3+ network for semantic segmentation, and then use the projection relationship between the camera and the laser radar to project the semantic information in the picture onto the point cloud to construct a semantic point cloud map, and use the semantic information to improve the LeGO‑LOAM algorithm to achieve the purpose of optimizing positioning and mapping. It can also use the picture data to use the CALC unsupervised learning network for closed-loop detection to optimize the correction posture and improve the robustness of the SLAM system. The map constructed by this method can achieve large-scale mapping, rich semantic information, and strong robustness.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a semantic map construction method based on laser and vision fusion. Background Art

[0002] SLAM, or simultaneous localization and mapping, is a key technical cornerstone for autonomous driving of driverless cars. It refers to the ability to obtain its own position and gradually build a map through its own sensors in an unknown environment. SLAM is mainly divided into visual SLAM and laser SLAM. The advantage of visual sensors is that they can obtain rich semantic information, and the advantage of laser radar is that they can obtain accurate spatial position information. With the development of deep learning and image recognition technology in recent years, it has become possible to combine semantic information with SLAM. Semantic SLAM technology can build maps with advanced semantic information, enhance the interaction between the car and the surrounding environment and the human-machine collaboration ability, and perform more advanced tasks.

[0003] 3D laser SLAM is represented by LOAM proposed by Zhang J et al. in 2014. It proposed a laser odometer, which uses the matching of line feature points of surface feature points in the laser point cloud between adjacent frames and uses LM optimization to obtain the pose. In 2018, Tixiao Shan et al. proposed LeGO-LOAM based on LOAM, which removed unreliable features by using ground point cloud constraints, calculated poses by using segmented LM optimization, and added a closed loop detection module to improve the speed and accuracy of map construction. The semantic segmentation of an image is to understand the image at the pixel level. Before the popularity of deep learning methods, semantic segmentation methods such as TextonForest and random forest classifiers were more commonly used. With the development of deep learning, the algorithm of image semantic segmentation has become increasingly mature. In 2014, the fully convolutional network (FCN) was born. FCN replaced the fully connected layer of the network with convolution, making it possible to input any image size, from the original network that could only output one category to the current network that can output the categories of all pixels. DeepLabv1 uses dilated convolution to expand the receptive field while maintaining resolution. DeepLabv2 and DeepLabv3 proposed and improved the hole convolution pyramid module to obtain better segmentation results.

[0004] How to establish a suitable and robust SLAM to create a semantic map so that the car no longer relies on GPS and can plan, control and make decisions for the vehicle correctly and timely is an urgent problem to be solved. Summary of the invention

[0005] The present application provides a semantic map construction method based on laser and vision fusion, the technical purpose of which is to improve the accuracy of single-vehicle mapping and construct a large-scale semantic map.

[0006] The above technical objectives of this application are achieved through the following technical solutions:

[0007] A semantic map construction method based on laser and vision fusion, comprising:

[0008] S1: Jointly calibrate the vehicle sensors to obtain the intrinsic parameter matrix and extrinsic parameter matrix of each sensor; the sensors include 4 surround-view cameras, a 16-line laser radar and an IMU;

[0009] S2: After the camera acquires the image data, it crops the overlapping parts of the image data to obtain a cropped image, and then inputs the four cropped images with the same timestamp into the DeepLabv3+ network for semantic segmentation to obtain a semantically segmented image;

[0010] S3: The laser radar collects point cloud data, and projects the semantically segmented image onto the point cloud data with the same timestamp according to the external parameter matrix of the camera and the laser radar for data fusion to obtain a semantic point cloud with semantic information;

[0011] S4: preprocessing the semantic point cloud, and then extracting feature points of each frame of the point cloud, and the laser odometer calculates the position and posture of the camera and the laser radar according to the relationship between the feature points of adjacent frames;

[0012] S5: Correct the camera and lidar poses through the loosely coupled LIO model combined with the IMU pose motion;

[0013] S6: Mapping the semantic point cloud by combining the rectified camera and lidar postures through the LeGO-LOAM algorithm to obtain a semantic point cloud map.

[0014] Furthermore, the method also includes S7: performing closed-loop detection on the current image data and historical image data of the camera based on visual closed-loop detection, thereby correcting the posture of the point cloud data of the lidar, and the corrected posture is fed back to step S6 to correct the semantic point cloud map.

[0015] Furthermore, in step S1, the joint calibration includes:

[0016] First, calibrate the four surround-view cameras to obtain the internal parameters of each camera, including focal length, principal point and distortion coefficient;

[0017] The camera and lidar are calibrated jointly through the ROS function package Lidar_camera_calibration, including: using the lidar coordinate system as the world coordinate system, and calibrating the camera through the external parameter matrix ([R, t]) and the camera's internal parameter matrix (f u ,fv ,u0,v0) converts the point cloud (x w ,y w ,z w ) is converted into camera plane coordinates (u, v), and the conversion formula is expressed as:

[0018]

[0019] The joint calibration of LiDAR and IMU includes: combining the relative coordinates of point cloud data, the motion transformation of IMU and the transformation matrix M, so as to fuse the points scanned by LiDAR into a point cloud; wherein, the calculation process of the transformation matrix M includes: calculating the sum of the distances D between each point and its nearest point cloud, and obtaining the transformation matrix M by minimizing D.

[0020] Furthermore, in step S3, the data fusion includes: projecting the semantically segmented image onto the point cloud data with the same timestamp according to the external parameter matrix of the camera and the lidar, optimizing the mask of the image semantic segmentation result according to the point cloud depth information, and removing the point cloud data that is not projected to complete the segmentation of the point cloud data; wherein, the point cloud data format before projection is PointXYZI, and the point cloud data format after projection is PointXYZRGBI, and the PointXYZRGBI format includes the location information, color information and intensity of the point cloud.

[0021] Further, in the step S4, preprocessing the semantic point cloud includes: denoising the semantic point cloud and removing the ground point cloud;

[0022] Wherein, denoising the semantic point cloud includes: removing invalid points and lost points returned by the point cloud, and then correcting points where semantic information is incorrectly projected;

[0023] Removing the ground point cloud includes: directly removing the ground points, clustering the remaining point clouds, and removing the point cloud clusters whose sizes do not meet the preset threshold in the remaining point clouds.

[0024] Furthermore, in step S4, extracting feature points of each frame of point cloud includes:

[0025] Project the point cloud without ground points into a 1800*16 depth image, and divide the depth image into 6 sub-images according to the horizontal coordinate;

[0026] By improving the LeGO-LOAM algorithm, surface feature candidate points and line feature candidate points are selected in the sub-image according to semantic information, including: assuming p i is a point in the point cloud, S represents and p i A set of consecutive points in the same row, and S is distributed on p iOn both sides; select S as 10, define a threshold M, and compare all points in S with p i If the semantics are the same, then M+1 is added. If M>8, then this point is considered a candidate point for the surface feature and is put into the set F. f If M < 6, this point is considered as a line feature candidate point and is put into the set F l In; then p i It is expressed as:

[0027]

[0028] Calculate F for each sub-image f The smoothness of all points in the set, select the smallest 4 points in each row and save them to Calculate F for each sub-image l The smoothness of all points in the set, select the largest 2 points in each row and save them to in, Represents the set of surface feature points of all sub-images, Represents the set of line feature points of all sub-images;

[0029] The formula for smoothness c is as follows:

[0030]

[0031] Among them, X pi Indicates p i The corresponding coordinates in the laser radar coordinate system;

[0032] Finally, the position and posture between laser radar frames are obtained by improving the segmented LM optimization algorithm in the LeGO-LOAM algorithm. x ,t y ,t z ,θ roll ,θ pitch ,θ yaw ].

[0033] Furthermore, in step S7, visual closed-loop detection is performed using a CALC unsupervised learning network, including:

[0034] Obtain key frames at regular intervals, wherein the key frames include four cropped images, splice the four cropped images to obtain a spliced ​​image, and convert the spliced ​​image into a grayscale image;

[0035] A HOG feature matrix is ​​obtained by directly calculating a histogram of oriented gradients (HOG) of the grayscale image, and the HOG feature matrix is ​​stored in a scene database;

[0036] The grayscale image is distorted and cropped and then input into an anti-noise encoder to obtain a feature matrix with the same dimension as the HOG feature matrix, and the HOG feature matrix is ​​matched with the feature matrix. If the matching degree is greater than a threshold value D, the key frame finds a matching image;

[0037] If two consecutive key frames after the key frame find matching pictures, the key frame finds a closed loop;

[0038] After the key frame finds a closed loop, the point cloud data closest to the timestamp of the key frame is found according to the timestamp of the key frame to perform posture correction.

[0039] The beneficial effects of the present application are as follows: the present application utilizes a surround-view camera to collect image data of the surrounding environment, transmits the image to the DeepLabv3+ network for semantic segmentation, and then utilizes the projection relationship between the camera and the lidar to project the semantic information in the image onto the point cloud to construct a semantic point cloud map. The purpose of positioning and mapping is achieved by improving the LeGO-LOAM algorithm. The image data can also be used to perform closed-loop detection using the CALC unsupervised learning network to optimize the correction posture and improve the robustness of the SLAM system.

[0040] This vehicle SLAM can build a point cloud semantic map for a single vehicle, and can integrate the characteristics of image data for closed-loop detection to improve the accuracy of positioning and mapping, and has good application prospects. Experiments have shown that the map algorithm constructed by the method proposed in this application has a simple structure, rich semantic information, a large mapping range, small errors, and strong robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A flowchart of the method described in this application;

[0042] Figure 2 This is the flow chart of the feature point extraction algorithm;

[0043] Figure 3 This is the flow chart of the closed-loop detection algorithm. DETAILED DESCRIPTION

[0044] The technical solution of the present application will be described in detail below with reference to the accompanying drawings.

[0045] The semantic map construction method based on laser and vision fusion described in this application has the following process: Figure 1 As shown, specifically including:

[0046] S1: Jointly calibrate the vehicle sensors to obtain the intrinsic parameter matrix and extrinsic parameter matrix of each sensor; the sensors include 4 surround-view cameras, a 16-line laser radar and an IMU.

[0047] Specifically, the above cameras are all 110° wide-angle distortion-free cameras; the laser radar is a 16-line Velodyne laser radar. After joint calibration, the intrinsic and extrinsic matrix of each sensor is obtained to facilitate the coordinate conversion between subsequent sensor data.

[0048] The joint calibration of each sensor specifically includes: first calibrating the four surround-view cameras to obtain the internal parameters of each camera, wherein the internal parameters include focal length, principal point, distortion coefficient and other information.

[0049] The camera and lidar are then calibrated jointly through the ROS function package Lidar_camera_calibration, including: using the lidar coordinate system as the world coordinate system, using the external parameter matrix ([R, t]) and the camera's internal parameter matrix (f u ,f v ,u0,v0) converts the point cloud (x w ,y w ,z w ) is converted into camera plane coordinates (u, v), and the conversion formula is expressed as:

[0050]

[0051] Among them, R represents the rotation matrix, t represents the translation matrix, and f u Indicates the length of the focal length in the x-axis direction, f v It represents the length of the focal length on the y-axis, u0 represents the actual position of the pixel on the x-axis, and v0 represents the actual position of the pixel on the y-axis.

[0052] The joint calibration of LiDAR and IMU includes: combining the relative coordinates of point cloud data, the motion transformation of IMU and the transformation matrix M, so as to fuse the points scanned by LiDAR into a point cloud; wherein, the calculation process of transformation matrix M includes: calculating the sum D of the distances between each point and its nearest point cloud, and the optimization goal is to minimize D, so as to obtain the transformation matrix M between LiDAR and IMU.

[0053] S2: After the camera acquires the image data, it crops the overlapping parts of the image data to obtain a cropped image, and then inputs the four cropped images with the same timestamp into the DeepLabv3+ network for semantic segmentation to obtain a semantically segmented image.

[0054] Specifically, the overlapping areas of the four wide-angle cameras need to be cropped first to make the viewing angle as close to 360° as possible. Then the four cropped images with the same timestamp are passed to the DeepLabv3+ network for semantic segmentation. The segmentation categories include ground, sky, trees, bushes, cars, pedestrians, road signs, etc. Different categories are represented by different color areas on the semantic segmentation image.

[0055] S3: The laser radar collects point cloud data, and projects the semantically segmented image onto the point cloud data with the same timestamp according to the external parameter matrix of the camera and the laser radar for data fusion to obtain a semantic point cloud with semantic information.

[0056] Specifically, the data fusion of the camera and the lidar includes: projecting the semantically segmented image onto the point cloud data with the same timestamp according to the external parameter matrix of the camera and the lidar, optimizing the mask of the image semantic segmentation result according to the point cloud depth information, and removing the point cloud data that is not projected to complete the segmentation of the point cloud data; wherein, the point cloud data format before projection is PointXYZI, and the point cloud data format after projection is PointXYZRGBI, and the PointXYZRGBI format includes the position information, color information and intensity of the point cloud.

[0057] S4: Preprocess the semantic point cloud, and then extract feature points of each frame of the point cloud. The laser odometer calculates the position and posture of the camera and the laser radar according to the relationship between the feature points of adjacent frames.

[0058] The preprocessing of the semantic point cloud includes: performing operations such as denoising and removing ground point cloud on the semantic point cloud.

[0059] Denoising the semantic point cloud first removes the invalid points and lost points returned by the point cloud, and then corrects the points where the semantic information is incorrectly projected. For example, some points on the upper part of the point cloud may be projected into the sky due to calibration errors, but the actual lidar will not return points of the sky. The colors of these points are corrected to the projectable points around them. Because of the errors in the image semantic segmentation results, there will be some isolated points with different colors from their surroundings, and the semantics of these isolated points are corrected. Then, the ground points can be removed according to the results of the semantic point cloud segmentation, the remaining point clouds can be clustered, and the smaller point cloud clusters can be eliminated.

[0060] The extraction of feature points includes the extraction of plane feature points and line feature points. The process is as follows: Figure 2 As shown, it specifically includes: firstly, projecting the point cloud without ground points into a 1800*16 depth image, and dividing the depth image into 6 sub-images according to the horizontal coordinate, and then extracting surface features and line features in each sub-image respectively.

[0061] The difference from the traditional LeGO-LOAM algorithm is that the present application improves the LeGO-LOAM algorithm and uses semantic information to select surface feature candidate points and line feature candidate points before extracting feature points, including: assuming p i is a point in the point cloud, S represents and p i A set of consecutive points in the same row, and S is distributed on p i On both sides; select S as 10, define a threshold M, and compare all points in S with p i If the semantics are the same, then M+1 is added. If M>8, then this point is considered a candidate point for the surface feature and is put into the set F. f If M < 6, this point is considered as a line feature candidate point and is put into the set F l In; then p i It is expressed as:

[0062]

[0063] Calculate F for each sub-image f The smoothness of all points in the set, select the smallest 4 points in each row and save them to Calculate F for each sub-image l The smoothness of all points in the set, select the largest 2 points in each row and save them to in, Represents the set of surface feature points of all sub-images, Represents the set of line feature points of all sub-images.

[0064] The formula for smoothness c is as follows:

[0065]

[0066] Among them, X pi Indicates p i The corresponding coordinates in the laser radar coordinate system;

[0067] Finally, the position and posture between laser radar frames are obtained by improving the segmented LM optimization algorithm in the LeGO-LOAM algorithm. x ,t y ,t z ,θ roll ,θ pitch ,θ yaw ]; where t x ,t y ,t z Respectively represent the translation distance on the x, y, and z axes; θ roll ,θ pitch ,θ yaw Represents the angle of rotation around the x, y, and z axes respectively.

[0068] S5: The loosely coupled LIO (LiDAR-Inertial Odometry) model is combined with the IMU's posture motion to correct the camera and lidar posture to optimize the mapping accuracy.

[0069] S6: Mapping the semantic point cloud by combining the rectified camera and lidar postures through the LeGO-LOAM algorithm to obtain a semantic point cloud map.

[0070] The specific mapping method is the same as that in the LeGO-LOAM algorithm, and the post-processing operation includes removing all road target point clouds in the map.

[0071] As a specific embodiment, the method also includes step S7: performing closed-loop detection on the current image data and historical image data of the camera based on visual closed-loop detection, thereby correcting the posture of the point cloud data of the lidar, and the corrected posture is fed back to step S6 to correct the semantic point cloud map.

[0072] The image has obvious advantages over the point cloud for closed loop detection. The CALC unsupervised learning network first converts the original image into a grayscale image, and then divides it into two threads. One thread calculates the oriented gradient histogram HOG of the grayscale image to obtain the descriptor feature, and the other thread distorts and crops the grayscale image again and passes it into the anti-noise encoder to generate a matrix of the same dimension as HOG. The loss function of the anti-noise encoder is the Euclidean distance between the two HOGs. The network structure is simple and can achieve real-time detection. The network is trained to obtain weights.

[0073] The steps of closed-loop detection are to take a key frame (containing four original images) at regular intervals, stitch the four images into one image, obtain its HOG feature matrix and store it in the scene database. Closed-loop detection is performed once every 10 key frames, and the image is passed into the trained anti-noise encoder network to obtain its feature matrix of the same dimension as HOG, and the most similar HOG feature matrix is ​​matched using Euclidean distance in the scene database. If the degree of matching is greater than a certain threshold D, it is considered that the key frame has found a matching image. In order to avoid erroneous closed-loop detection, it is set that only when matching images can be found in 3 consecutive key frames, the frame is considered to have found a closed loop. After the closed loop of the image is found, the laser point cloud closest to the timestamp of the image can be found according to the timestamp of the image for posture correction.

[0074] The above are exemplary embodiments of the present application, and the protection scope of the present application is defined by the claims and their equivalents.

Claims

1. A semantic map construction method based on laser and vision fusion, characterized in that: include: S1: Jointly calibrate the vehicle sensors to obtain the intrinsic parameter matrix and extrinsic parameter matrix of each sensor; the sensors include 4 surround-view cameras, a 16-line laser radar and an IMU; S2: After the camera acquires the image data, it crops the overlapping parts of the image data to obtain a cropped image, and then inputs the four cropped images with the same timestamp into the DeepLabv3+ network for semantic segmentation to obtain a semantically segmented image; S3: The laser radar collects point cloud data, and projects the semantically segmented image onto the point cloud data with the same timestamp according to the external parameter matrix of the camera and the laser radar for data fusion to obtain a semantic point cloud with semantic information; S4: preprocessing the semantic point cloud, and then extracting feature points of each frame of the point cloud, and the laser odometer calculates the position and posture of the camera and the laser radar according to the relationship between the feature points of adjacent frames; S5: Correct the camera and lidar poses through the loosely coupled LIO model combined with the IMU pose motion; S6: Mapping the semantic point cloud by combining the rectified camera and lidar postures through the LeGO-LOAM algorithm to obtain a semantic point cloud map.

2. The semantic map construction method according to claim 1, characterized in that: The method also includes S7: performing closed-loop detection on the current image data and historical image data of the camera based on visual closed-loop detection, thereby correcting the posture of the point cloud data of the lidar, and the corrected posture is fed back to step S6 to correct the semantic point cloud map.

3. The semantic map construction method according to claim 1, characterized in that: In step S1, the joint calibration includes: First, calibrate the four surround-view cameras to obtain the internal parameters of each camera, including focal length, principal point and distortion coefficient; The camera and lidar are calibrated jointly through the ROS function package Lidar_camera_calibration, including: using the lidar coordinate system as the world coordinate system, and calibrating the camera through the external parameter matrix ([R, t]) and the camera's internal parameter matrix (f u ,f v ,u0,v0) converts the point cloud (x w ,y w ,z w ) is converted into camera plane coordinates (u, v), and the conversion formula is expressed as: The joint calibration of the laser radar and the IMU includes: combining the relative coordinates of the point cloud data, the motion transformation of the IMU and the transformation matrix M, so as to fuse the points scanned by the laser radar into a point cloud; wherein, the calculation process of the transformation matrix M includes: calculating the sum of the distances D between each point and its nearest point cloud, and obtaining the transformation matrix M by minimizing D; Among them, R represents the rotation matrix, t represents the translation matrix, and f u Indicates the length of the focal length in the x-axis direction, f v It represents the length of the focal length on the y-axis, u0 represents the actual position of the pixel on the x-axis, and v0 represents the actual position of the pixel on the y-axis.

4. The semantic map construction method according to claim 1, characterized in that: In the step S3, the data fusion includes: projecting the semantically segmented image onto the point cloud data with the same timestamp according to the external parameter matrix of the camera and the lidar, optimizing the mask of the image semantic segmentation result according to the point cloud depth information, and removing the point cloud data that is not projected to complete the segmentation of the point cloud data; wherein the point cloud data format before projection is PointXYZI, and the point cloud data format after projection is PointXYZRGBI, and the PointXYZRGBI format includes the location information, color information and intensity of the point cloud.

5. The semantic map construction method according to claim 1, characterized in that: In the step S4, preprocessing the semantic point cloud includes: denoising the semantic point cloud and removing the ground point cloud; Wherein, denoising the semantic point cloud includes: removing invalid points and lost points returned by the point cloud, and then correcting points where semantic information is incorrectly projected; Removing the ground point cloud includes: directly removing the ground points, clustering the remaining point clouds, and removing the point cloud clusters whose sizes do not meet the preset threshold in the remaining point clouds.

6. The method for constructing a semantic map according to claim 5, characterized in that: In step S4, extracting feature points of each frame of point cloud includes: Project the point cloud without ground points into a 1800*16 depth image, and divide the depth image into 6 sub-images according to the horizontal coordinate; By improving the LeGO-LOAM algorithm, surface feature candidate points and line feature candidate points are selected in the sub-image according to semantic information, including: assuming p i is a point in the point cloud, S represents and p i A set of consecutive points in the same row, and S is distributed on p i On both sides; select S as 10, define a threshold M, and compare all points in S with p i If the semantics are the same, then M+1 is added. If M>8, then this point is considered a candidate point for the surface feature and is put into the set F. f If M < 6, this point is considered as a line feature candidate point and is put into the set F l In; then p i It is expressed as: Calculate F for each sub-image f The smoothness of all points in the set, select the smallest 4 points in each row and save them to Calculate F for each sub-image l The smoothness of all points in the set, select the largest 2 points in each row and save them to in, Represents the set of surface feature points of all sub-images, Represents the set of line feature points of all sub-images; The formula for smoothness c is as follows: Among them, X pi Indicates p i The corresponding coordinates in the laser radar coordinate system; Finally, the position and posture between laser radar frames are obtained by improving the segmented LM optimization algorithm in the LeGO-LOAM algorithm. x ,t y ,t z ,θ roll ,θ pitch ,θ yaw ]; Among them, t x ,t y ,t z Respectively represent the translation distance on the x, y, and z axes; θ roll ,θ pitch ,θ yaw Represents the angle of rotation around the x, y, and z axes respectively.

7. The semantic map construction method according to claim 2, characterized in that: In step S7, visual closed-loop detection is performed using a CALC unsupervised learning network, including: Obtain key frames at regular intervals, wherein the key frames include four cropped images, splice the four cropped images to obtain a spliced ​​image, and convert the spliced ​​image into a grayscale image; A HOG feature matrix is ​​obtained by directly calculating a histogram of oriented gradients (HOG) of the grayscale image, and the HOG feature matrix is ​​stored in a scene database; The grayscale image is distorted and cropped and then input into an anti-noise encoder to obtain a feature matrix with the same dimension as the HOG feature matrix, and the HOG feature matrix is ​​matched with the feature matrix. If the matching degree is greater than a threshold value D, the key frame finds a matching image; If two consecutive key frames after the key frame find matching pictures, the key frame finds a closed loop; After the key frame finds a closed loop, the point cloud data closest to the timestamp of the key frame is found according to the timestamp of the key frame to perform posture correction.

Images