Synchronous mapping and positioning method and system of three-dimensional laser radar fused with point cloud intensity

Abstract

The application provides a fusion point cloud intensity three-dimensional laser radar synchronous mapping and positioning method and system, and the implementation process comprises the following steps: preprocessing point cloud data collected by a 3D LiDAR to obtain the point cloud data after preprocessing; performing 2D projection on the point cloud data after preprocessing to obtain a point cloud depth map and a point cloud intensity map, and extracting point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map; adaptively weighting and fusing the point cloud geometric feature matching result and the intensity feature matching result in time; constructing a global descriptor point cloud intensity projection shape feature based on point cloud intensity cylindrical projection and spatial density distribution, and improving the effect of loop detection; and optimizing the historical trajectory pose and the point cloud map based on a factor graph to obtain the optimized trajectory pose and the global map. The application constructs a new global descriptor based on point cloud intensity cylindrical projection in the back end, so as to improve the robustness of the back-end loop detection.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of simultaneous localization and mapping, and more particularly to a three-dimensional laser radar simultaneous mapping and positioning method and system fusing point cloud intensity. BACKGROUND

[0002] Simultaneous localization and mapping (SLAM) refers to when a robot first enters a scene that lacks global satellite positioning and is completely unknown, collecting surrounding environment data based on a sensor carried by the robot, constructing a global map and realizing self-positioning. Based on different theoretical bases of SLAM algorithms, SLAM technology can be divided into filter-based SLAM algorithms and graph optimization-based SLAM algorithms. The filter-based SLAM algorithm is based on probability theory, estimates the pose of the robot with maximum probability according to the control input of the robot and the observation value of the sensor, and realizes mapping. This method has a large amount of calculation and is difficult to guarantee real-time performance. The graph optimization-based SLAM algorithm converts the pose of the robot, observation and control input into vertices and edges in graph theory, thereby converting the SLAM problem into an optimization problem. The graph optimization-based SLAM algorithm has better mapping effect and positioning accuracy, and is the focus of current research.

[0003] And based on the different sensors used, SLAM can be divided into visual SLAM (Visual SLAM) and laser SLAM (LiDAR SLAM). Visual SLAM collects image information through a camera as a data source, has the advantages of rich information, low cost, light weight and small size. However, the camera is easily affected by the environment (factors such as light, climate, target material, etc.) and it is not easy to extract three-dimensional information of the target, so a complex and changing or feature-poor visual environment will affect the Visual SLAM system and even make the system fail. LiDAR SLAM collects point cloud information through a laser radar as a data source. Laser radar point cloud data contains rich information such as X, Y, Z coordinate information, number of echoes, intensity information, etc. Among them, 3D laser radar has more abundant spatial information. And laser radar has strong environmental adaptability and can realize mapping and positioning in a more diverse scene.

[0004] Traditional LiDAR SLAM algorithms perform inter-frame matching based on the geometric features of point cloud data, thereby realizing the estimation of the pose of the laser radar. This method has high positioning accuracy in scenes with rich geometric features, but in open environments with poor geometric features, the mapping effect and positioning accuracy of LiDAR SLAM will be affected. At the same time, the traditional LiDAR SLAM algorithm has a loop optimization module at the back end, which often performs loop detection based on the odometer and historical trajectory pose. However, due to the accumulation of drift errors, this loop detection method has low accuracy and poor robustness.

[0005] The point cloud data collected by the laser radar has not only x, y, z three-dimensional coordinate information, but also intensity information. The point cloud intensity information is consistent with the reflectivity of the surrounding target, and the corresponding point cloud intensity is different for different targets. The target extraction, feature matching and other functions can be realized through the point cloud intensity. Therefore, it is feasible to fuse the point cloud intensity on the basis of the traditional LiDAR SLAM framework to improve the mapping effect and positioning accuracy of the existing algorithm. SUMMARY

[0006] The present application aims at the technical problems existing in the prior art, and proposes a three-dimensional laser radar synchronous mapping and positioning method and system fusing point cloud intensity. In a scene with insufficient geometric features, it extracts point cloud geometric features and intensity features from the point cloud data respectively, and uses the intensity features as a supplement to the geometric features to improve the feature richness. Meanwhile, the present application proposes a new global descriptor based on point cloud intensity to improve the performance of the back-end loop optimization.

[0007] According to a first aspect of the present application, a three-dimensional laser radar synchronous mapping and positioning method fusing point cloud intensity is provided, comprising the following steps:

[0008] Step 1: Preprocessing the collected point cloud data to obtain the point cloud data after preprocessing;

[0009] Step 2: Projecting the point cloud data after preprocessing in 2D to obtain a point cloud depth map and a point cloud intensity map;

[0010] Step 3: Segmenting the point cloud data after preprocessing into ground points and non-ground points, and extracting point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map;

[0011] Step 4: Based on the extracted point cloud geometric features and intensity features, adaptively weighting and fusing the point cloud geometric feature matching results and the intensity feature matching results in time to obtain the current pose;

[0012] Step 5: Based on the cylindrical projection and spatial density distribution of the point cloud intensity after preprocessing, constructing a global descriptor point cloud intensity projection shape feature; and performing loop detection based on the global descriptor point cloud intensity projection shape feature, and verifying the results of the loop detection;

[0013] Step 6: Adding the verified loop detection results as constraints into the factor graph, and optimizing the historical trajectory pose and the point cloud map in the factor graph to obtain the optimized trajectory pose and global map.

[0014] On the basis of the above technical solution, the present application can also be improved as follows.

[0015] Optionally, in step 1, the pre-processing of the collected point cloud data comprises:

[0016] Step 1.1. Collecting information of the surrounding environment by 3D LiDAR to obtain raw point cloud data;

[0017] Step 1.2. Then, conditionally filtering the raw point cloud data, setting the maximum distance and the maximum height, and filtering out the point cloud data beyond the range;

[0018] Step 1.3. Intensity calibration of the point cloud data after conditional filtering.

[0019] Optionally, in step 2, the 2D projection of the point cloud data after pre-processing comprises: for the pre-processed point cloud data, based on its horizontal direction angle resolution and vertical angle resolution, projecting it into a 2D depth map and a 2D intensity map, wherein the 2D depth map saves the distance of the point cloud to the center of the laser radar, and the 2D intensity map saves the intensity value of the corresponding point cloud.

[0020] Optionally, in step 2, the 2D projection of the point cloud data after pre-processing comprises:

[0021] Step 2.1. Projecting the pre-processed point cloud data into a 2D depth map and a 2D intensity map, wherein the 2D depth map saves the distance of the point cloud to the center of the laser radar, and the 2D intensity map saves the intensity value of the corresponding point cloud after calibration.

[0022] Optionally, in step 3, the extraction of point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map comprises:

[0023] Step 3.1. Segmentation of ground points and non-ground points for the intensity calibrated point cloud data, and assigning labels of ground points, non-ground points and discrete points to the point cloud data;

[0024] Step 3.2. Calculating point cloud geometric smooth features and intensity smooth features based on the 2D depth map and the 2D intensity map of the point cloud data respectively;

[0025] Step 3.3. Based on the results of the geometric smooth features, extracting point cloud geometric plane feature points in the ground point cloud and extracting point cloud geometric edge feature points in the non-ground point;

[0026] Step 3.4. Based on the results of the intensity smooth features, extracting point cloud intensity plane feature points in the ground point cloud and extracting point cloud intensity edge feature points in the non-ground point.

[0027] Optionally, in step 4, the adaptive weight fusion of the point cloud geometric feature matching result and the intensity feature matching result comprises:

[0028] Step 4.1. Determine the richness of geometric features in the environment based on the number of extracted geometric features;

[0029] Step 4.2. If the geometric features are rich, only match and optimize the point clouds of adjacent frames based on geometric features to obtain the relative pose transformation of the laser radar between adjacent frames;

[0030] Step 4.3. If the geometric features are scarce, match and optimize the point clouds of adjacent frames based on intensity features, and weight the matching results obtained and the matching results based on geometric features.

[0031] Optionally, in step 5, the columnar projection of the pre-processed point cloud intensity and the spatial density distribution are used to construct the global descriptor point cloud intensity projection shape feature, which includes:

[0032] Step 5.1. Project the intensity calibrated point cloud onto a columnar surface centered on the laser radar, define a frame of point cloud data obtained by the laser radar and each laser point therein, and project the frame of point cloud data onto a polar coordinate system;

[0033] Step 5.2. Divide the laser point cloud after columnar projection into N s sectors in the horizontal direction and N p blocks in the height direction, and then divide the frame of point cloud data into N s *N p subspaces;

[0034] Step 5.3. For each subspace, extract the maximum value of the point cloud intensity to form a point cloud intensity matrix I;

[0035] Step 5.4. Statistically analyze the density of the point cloud with the same horizontal direction angle interval, and calculate the point cloud density weight D ij ;

[0036] Step 5.5. Multiply the point cloud density weight with the point cloud matrix to construct the global descriptor point cloud intensity projection shape feature based on the point cloud intensity columnar projection.

[0037] Optionally, in step 5, the global descriptor point cloud intensity projection shape feature is used for loop detection, and the result of the loop detection is verified, which includes:

[0038] Step 5.6. After obtaining the global descriptor point cloud intensity projection shape feature, extract the row vector and the column vector of the descriptor; wherein the elements in the row vector and the column vector are the average values of the elements of the corresponding column and the corresponding row of the descriptor matrix;

[0039] Step 5.7. Perform candidate position search based on the column vector, save the column vector as a KD tree in the format of Vector class, index in the KD tree based on the size of the Euclidean distance between the current frame descriptor matrix array vector and the historical frame column vector, and search out n1 points cloud frames corresponding to the column vectors with the smallest Euclidean distance as candidate frames;

[0040] Step 5.8. In the n1 candidate frames, perform preliminary calculation of the relative angle based on the row vector, calculate the cosine distance of the descriptor matrix of the current frame and the candidate frame; take the shift th calculated by the row vector as the initial value, and calculate the cosine distance between the two descriptors within the column range of n th shifts and calculates the cosine distance between the two descriptors;

[0041] Step 5.9. After detecting the loop based on the descriptor, first perform time consistency test, calculate the cosine distance of the corresponding descriptors of the point clouds of the nearby N frames based on the detected orientation of the current frame and the candidate frame;

[0042] Step 5.10. After the time consistency test, take the relative pose calculated based on the descriptor matching as the initial value, perform geometric consistency test by a direct matching algorithm or a feature matching algorithm, and if the geometric consistency test score is less than a threshold value, it is considered that the current loop detection result is accurate.

[0043] Optionally, in step 6, the loop detection result that passes the verification is added as a constraint into the factor graph, and the optimized trajectory pose and global map are obtained by optimizing the historical trajectory pose and the point cloud map in the factor graph, including:

[0044] Step 6.1. Estimate the pose of the current frame in the global map based on the relative pose transformation of the current frame and the previous frame and the pose of the previous frame in the global map;

[0045] Step 6.2. Extract the point cloud data near the current frame based on the result of the odometry to construct a local point cloud map, match the current frame with the local point cloud map, and add the point cloud data of the current frame into the map, so as to realize point cloud map updating;

[0046] Step 6.3. Take the calculated pose of each frame of the laser radar as a node of the factor graph, take the relative pose transformation relationship between the adjacent frames of the laser radar as an edge of the factor graph, add the result of the loop detection as a new constraint into the factor graph, realize back-end graph optimization, and thus obtain the optimized trajectory pose and point cloud map.

[0047] According to the second aspect of the present application, a three-dimensional laser radar synchronous mapping and positioning system integrating point cloud intensity is provided, comprising:

[0048] The point cloud data preprocessing module is configured to preprocess the collected point cloud data to obtain preprocessed point cloud data.

[0049] The point cloud data calculation module is configured to project the preprocessed point cloud data to obtain a point cloud depth map and a point cloud intensity map, segment the preprocessed point cloud data into ground points and non-ground points, extract point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map, and adaptively weight and fuse the point cloud geometric feature matching result and the intensity feature matching result based on the extracted point cloud geometric features and intensity features to obtain a current pose.

[0050] The loop detection module is configured to construct a global descriptor point cloud intensity projection shape feature based on the preprocessed point cloud intensity cylindrical projection and spatial density distribution, perform loop detection based on the global descriptor point cloud intensity projection shape feature, and verify the loop detection result.

[0051] The point cloud data optimization module is configured to add the verified loop detection result as a constraint into a factor graph, optimize the historical trajectory pose and the point cloud map based on the factor graph, and obtain an optimized trajectory pose and global map.

[0052] Technical effects and advantages of the present application:

[0053] The present application proposes a three-dimensional laser radar synchronous mapping and positioning method and system fusing point cloud intensity, extracts intensity features of point cloud data, and performs inter-frame matching based on the intensity features to improve the accuracy of feature matching. Meanwhile, a global descriptor point cloud intensity projection shape feature is constructed based on point cloud intensity cylindrical projection for loop detection, and a consistency test is set to ensure the accuracy of loop detection, thereby realizing high-precision mapping and positioning in an environment with insufficient geometric features. The present application is simple and convenient to implement, has strong practicability, solves the problems of low practicability and inconvenience in actual application of related technologies, can improve user experience, and has important market value. BRIEF DESCRIPTION OF DRAWINGS

[0054] Figure 1 A three-dimensional laser radar synchronous mapping and positioning method fusing point cloud intensity is proposed for the embodiments of the present application.

[0055] Figure 2 A loop detection framework based on a global descriptor point cloud intensity projection shape feature is proposed for the embodiments of the present application. DETAILED DESCRIPTION

[0056] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0057] In recent years, with advances in computing and sensor technology, intelligent mobile robotics has rapidly developed. More and more industries are experimenting with using them to replace human operators. These robots are tasked with performing tasks such as logistics and distribution, automated cleaning, intelligent food delivery, and security inspections. As the tasks performed by intelligent robots become increasingly complex and diverse, the requirements for their positioning are becoming increasingly stringent. High-precision positioning is a key technology for intelligent robots to accomplish these complex tasks.

[0058] In indoor scenarios, anchor points can be set within the scene and the robot's position and posture can be calculated by measuring the distance from each anchor point. Common indoor positioning methods include ultra-wideband (UWB), Wi-Fi, ZigBee, and Bluetooth. These positioning methods are only suitable for small-scale scenarios such as indoors and rely on external sensors, making them not universally applicable.

[0059] In outdoor scenarios, common positioning methods for intelligent robots include fusion positioning using the Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) (GNSS / INS); positioning technology based on prior maps; and simultaneous localization and mapping (SLAM). GNSS / INS combined navigation and positioning inherits the continuity, high sampling rate, and complete navigation parameters (speed, position, and attitude) of INS. It also combines GNSS position information to achieve long-term, high-precision positioning, resulting in high positioning accuracy. However, in many complex scenarios (such as urban canyons, under the shade of trees, and under overpasses), GNSS positioning accuracy can quickly degrade, rendering the GNSS / INS combined navigation and positioning method ineffective. Prior map-based positioning technology involves obtaining a map of the scene in a certain format before the robot enters it. Sensors equipped with the robot then link the online data with the prior map to determine the robot's current position. However, this method requires the robot to have acquired a map in a certain format beforehand.

[0060] Traditional LiDAR SLAM algorithm is based on the geometric features of point cloud data for inter-frame matching, so as to realize the estimation of laser radar pose. In the scene with rich geometric features, the method has high positioning accuracy, while in the open environment with few geometric features, the mapping effect and positioning accuracy of LiDAR SLAM will be affected. At the same time, in the rear loop optimization module of traditional LiDAR SLAM algorithm, loop detection is usually based on odometer and historical trajectory pose, and due to the accumulation of drift error, the loop detection method has low accuracy and poor robustness.

[0061] In view of the problems that the existing LiDAR SLAM algorithm has low mapping effect and positioning accuracy in the scene with few geometric features, referring to Figure 1 The embodiment of the present application provides a three-dimensional laser radar synchronous mapping and positioning method fusing point cloud intensity, comprising the following steps:

[0062] S1: pre-processing the collected point cloud data to obtain the pre-processed point cloud data;

[0063] Specifically, the pre-processing of the collected point cloud data comprises the following sub-steps:

[0064] Step 1.1: collecting information of the surrounding environment by 3D LiDAR to obtain original point cloud data;

[0065] Step 1.2: conditionally filtering the original point cloud data, setting the farthest distance L max and the maximum height H max , filtering out the point cloud data beyond the range, discrete points and unreliable points;

[0066] Step 1.3: intensity calibration of the point cloud after conditional filtering; the degree value of the point cloud data collected by the laser radar is mainly related to the target surface reflectivity, the incident angle and the measurement distance, and the intensity value I is defined as:

[0067]

[0068] In the formula, R is the distance from the center of the laser radar to the target, p is the target surface reflectivity, a is the incident angle of the laser beam and the target surface, and η all is a constant coefficient, including atmospheric transmission coefficient, system transmission coefficient, etc. Generally, the laser radar has been calibrated between the measurement distance and the point cloud intensity before leaving the factory, so the user only needs to calibrate the point cloud intensity and the incident angle. The relationship between the point cloud intensity and the incident angle is So that the point cloud intensity and the object surface reflectivity are consistent.

[0069] It needs to be explained that the preprocessing of the original point cloud data also includes: point cloud data voxel sampling, which is used to reduce the amount of point cloud data; point cloud intensity reduction, which eliminates the influence of incident angle on point cloud intensity value; and motion distortion compensation, which eliminates the error caused by relative motion on point cloud measurement, so as to obtain the point cloud data after preprocessing.

[0070] S2, 2D projection of the point cloud data after preprocessing is obtained by projecting the point cloud data after preprocessing into 2D depth map and 2D intensity map.

[0071] Specifically, the 2D projection of the point cloud data after preprocessing includes:

[0072] Step 2.1. 2D projection of the point cloud data after preprocessing is obtained by projecting the point cloud data after preprocessing into 2D depth map and 2D intensity map. For the point cloud data after preprocessing, based on its horizontal direction angle resolution and vertical angle resolution, it is projected into 2D depth map and 2D intensity map, wherein 2D depth map saves the distance from point cloud to laser radar center, and 2D intensity map saves the intensity value of corresponding point cloud. Thus, 2D point cloud depth map and point cloud intensity map are obtained, which are used for subsequent point cloud segmentation and feature extraction.

[0073] S3, ground point and non-ground point segmentation of point cloud data is carried out, and point cloud geometric feature and intensity feature are extracted based on point cloud depth map and point cloud intensity map;

[0074] Specifically, the point cloud geometric feature and intensity feature extracted based on the point cloud depth map and the point cloud intensity map include:

[0075] Step 3.1. Ground point and non-ground point segmentation of the point cloud data after intensity calibration is carried out, and the point cloud data is labeled as ground point, non-ground point and discrete point;

[0076] Step 3.2. Based on the 2D depth map and the 2D intensity map of the point cloud data, the point cloud geometric smooth feature and the intensity smooth feature are calculated respectively;

[0077] Step 3.3. Based on the result of the geometric smooth feature, the point cloud geometric plane feature point is extracted in the ground point cloud, and the point cloud geometric edge feature point is extracted in the non-ground point;

[0078] Step 3.4. Based on the result of the intensity smooth feature, the point cloud intensity plane feature point is extracted in the ground point cloud, and the point cloud intensity edge feature point is extracted in the non-ground point.

[0079] Further, in step 3.2, the point cloud smooth feature is extracted by calculating the point cloud geometric smooth feature and the intensity smooth feature, and the point cloud smooth feature is defined as:

[0080]

[0081] Where cR with c I are the depth curvature and intensity curvature of the point cloud, s is the point set used to calculate the smooth feature of the point, R i with I i The depth and intensity values ​​of the point cloud are respectively. According to the size of the curvature value, the point cloud is divided into plane points and edge points. From the point cloud with ground point labels, the points with the minimum c I of intensity plane feature points, and from Extract c I smallest Intensity plane feature points with the largest c are extracted from the point cloud with non-ground point labels. I of intensity edge feature points, and from Extract c I The largest Intensity edge feature points. Similarly, we can extract the point cloud with the minimum c from the ground point label R of geometric plane feature points, and from Extract c R smallest Geometric plane feature points with the largest c are extracted from the point cloud with non-ground point labels. R of geometric edge feature points, and from Extract c R The largest geometric edge feature points, and obtain the number of point cloud geometric edge points, geometric plane points, intensity edge points, and intensity plane points

[0082] S4, based on the extracted point cloud geometric features and intensity features, adaptively weighted fusion of the point cloud geometric feature matching results and the intensity feature matching results is performed in a timely manner;

[0083] The adaptive weighted fusion of the point cloud geometric feature matching results and the intensity feature matching results comprises:

[0084] Step 4.1. Determine the richness of geometric features in the environment based on the number of extracted geometric features;

[0085] Step 4.2. If the geometric features are rich, match and optimize the point clouds of adjacent frames based on the geometric features to obtain the relative pose transformation of the lidar between adjacent frames;

[0086] Step 4.3. If geometric features are relatively scarce, match and optimize the point clouds of adjacent frames based on intensity features, and perform weighted fusion of the obtained matching results with the matching results based on geometric features.

[0087] The adaptive fixed-weight fusion of the point cloud geometry feature matching result and the intensity feature matching result comprises the following steps:

[0088] Specifically, in step 4.2, the matching and optimization of the point clouds of adjacent frames based on geometric features are performed to obtain the relative pose transformation of the laser radar between adjacent frames, which comprises:

[0089] The nearest feature point to the current frame point cloud geometric edge feature point is searched from the geometric edge feature points of the previous frame point cloud, the relative transformation thereof is calculated and LM optimization is performed to obtain the transformation of the yaw angle and the displacement in the x and y directions between the two adjacent frames, i.e. Similarly, the nearest feature point to the current frame point cloud geometric plane feature point is searched from the geometric plane feature points of the previous frame point cloud, the transformation of the roll and pitch and the displacement in the z direction between the two adjacent frames are calculated, i.e. Thus, the six-degree-of-freedom transformation between the two adjacent frames is obtained, i.e.

[0090] The threshold value is set The richness of the geometric edge feature points and the number of the geometric plane feature points in the scene around the laser radar are respectively determined. Different intensity feature matching and fusion strategies are performed for different numbers of geometric plane points, and finally the transformation [t x , t y , t z , θ roll , θ pitch , θ yaw ] is obtained.

[0091]

[0092] Among them: is the inter-frame matching result obtained by point cloud intensity feature matching.

[0093] The embodiment step can extract point cloud intensity features in real time based on the richness of the geometric features in the scene, and adaptively fuse the geometric feature matching and the intensity feature matching result to improve the accuracy of feature matching.

[0094] S5, based on the cylindrical projection and spatial density distribution of the pre-processed point cloud intensity, a global descriptor point cloud intensity projection shape feature is constructed; and loop closure detection is performed based on the global descriptor point cloud intensity projection shape feature, and the result of the loop closure detection is verified.

[0095] It needs to be explained that the global descriptor is a method for describing the characteristics of three-dimensional point cloud data, which represents the frame of point cloud data by extracting the global feature information in the point cloud data; the point cloud intensity projection shape feature is full name (Intensity Cylindrical Projection Shape Context, represented as ICPSC), which is a shape feature descriptor constructed by cylindrical projection of point cloud data intensity value.

[0096] In specific implementation, referring to Figure 2 , step S5 includes the following sub-steps:

[0097] Step 5.1. Project the intensity calibrated point cloud to the cylindrical surface centered on the laser radar, define the frame of point cloud data acquired by the laser radar as P = {p1, p2, p3…pN n}, define each laser point in it as p k = [x k , y k , z k , η k ]. Wherein x k , y k , z k are the three-dimensional coordinates of the corresponding laser point in the Cartesian coordinate system centered on the laser radar, η k is the intensity value of the corresponding laser point. First, project a frame of point cloud data to the polar coordinate system:

[0098]

[0099] Wherein, ρ k is the distance of the corresponding laser point from the main optical axis of the laser radar, θ k is the horizontal direction angle of the corresponding laser point.

[0100] Step 5.2. Divide the cylindrical projected laser point cloud into N s sectors in the horizontal direction angle, and divide it into N p blocks in the height, then divide a frame of point cloud data into N s *N p subspaces.

[0101]

[0102] Wherein, Δs is the size of the angle interval, Δp is the size of the vertical height interval, H is the height threshold of the set descriptor extraction range, which can be determined by the maximum vertical direction angle of the laser radar and its structure maximum scanning distance H = H install + L max · tan β. Wherein H install is the installation height of the laser radar, Lmax β is the maximum azimuth angle of the lidar.

[0103] Step 5.3. Define each subspace as s ij where the point cloud distribution in each subspace is:

[0104]

[0105] where i∈[|1, N p |], j∈[|1, N s |].

[0106] For each subspace, extract the maximum value of its point cloud intensity to form the point cloud intensity matrix I. The size of the corresponding element in the intensity matrix I is If then let I ij = 0.

[0107] Step 5.4. Statistically analyze the density of point clouds with the same horizontal direction angle interval, and calculate the point cloud density weight D ij . The point cloud density weight D ij is defined as:

[0108]

[0109] where median is the median of the number of point clouds in the subspace with the same horizontal angle, num ij is the number of point clouds in the corresponding subspace.

[0110] Step 5.5. Multiply the point cloud density weight with the point cloud matrix to construct the global descriptor point cloud intensity projection shape feature based on the point cloud intensity cylindrical projection, defined as The value of each element in the descriptor matrix is:

[0111]

[0112] where is the floor function.

[0113] Step 5.6. After obtaining the global descriptor point cloud intensity projection shape feature, extract the row vector and the column vector where the elements in the row vector and the column vector are the average values of the corresponding column and the corresponding row elements in the descriptor matrix, defined as:

[0114]

[0115] where r i is the value of the corresponding element in the row vector, and c iis a column vector corresponding to the element value.

[0116] Step 5.7. In the loop detection stage, first search the candidate position based on the column vector. Since the descriptor column vector has rotation invariance, the historical column vector is saved as a KD tree in the Vector class format, and the n1 smallest Euclidean distance between the current frame ICPSC descriptor matrix column vector and the historical frame column vector is searched in the KD tree to search out the n1 smallest Euclidean distance corresponding point cloud frame as the candidate frame.

[0117] In this embodiment, the vector class is a container class in C++STL, which is used to realize functions such as dynamic array; the KD tree is a data structure for high-dimensional space data structure, which can be used for fast nearest neighbor search and range search operations;

[0118] Step 5.8. In the n1 candidate frames, the relative angle is calculated based on the row vector. The value of the ICPSC descriptor row vector is not affected by rotation, but the distribution of the value is affected by the rotation of the laser radar, and the displacement of the value is consistent with the rotation angle of the laser radar. Therefore, the relative angle between the current frame and the candidate frame can be quickly calculated through the row vector.

[0119]

[0120]

[0121] where R c is the row vector of the current frame, R q is the row vector of the candidate frame, is the current frame row vector shifted by th, which can be determined according to the size of th. is the ICPSC descriptor matrix constructed by the current frame point cloud, is the ICPSC descriptor matrix constructed by the candidate frame point cloud, is the result of matching the current frame and the candidate frame point cloud based on the row vector. The threshold value can be set according to the size of to further retain n2 candidate frames.

[0122] And calculate the cosine distance of the ICPSC descriptor matrix of the current frame and the candidate frame to get a more accurate relative angle. The shift th obtained by the row vector calculation is taken as the initial value, and the cosine distance between the two descriptors is calculated within the column range of n th

[0123]

[0124]

[0125] wherein,​ is the i-th column vector of the ICPSC descriptor matrix, is the ICPSC descriptor matrix shifted by d bits in column, where (th-nth) < 0 or (th+nth) > Ns may occur, but the ICPSC descriptor is expanded in the horizontal direction angle, and the first column is adjacent to the last column. is the minimum cosine distance calculated, and a more accurate relative angle is obtained based on the displacement d.

[0126] Step 5.9. After detecting the loop based on the ICPSC descriptor, verify whether the loop detection result is accurate through the consistency verification module. First, perform time consistency verification based on the orientation of the detected current frame and the candidate frame, and calculate the cosine distance of the corresponding descriptor of the N frames of point cloud around them.

[0127]

[0128] where P(Pc, Pq) is the similarity value calculated by time consistency of the current frame Pc and the candidate frame Pq, and if the result of P(Pc, Pq) is greater than the threshold value, the corresponding loop detection result is rejected.

[0129] Step 5.10. After consistency verification, the relative pose calculated based on the descriptor matching is taken as the initial value, and the geometric consistency verification is performed through the direct matching algorithm (such as ICP, NDT, etc.) or the feature matching algorithm (such as SHOT, PFH, FPFH, etc.). If the geometric consistency verification score is less than the threshold value, it is considered that the current loop detection result is accurate.

[0130] In this embodiment, the implementation step is based on the cylindrical projection of point cloud intensity and the spatial density distribution to construct the global descriptor point cloud intensity projection shape feature, and the two-step search matching is performed based on the global descriptor point cloud intensity projection shape feature to realize loop detection, and then the consistency verification module is used to verify the result of loop detection. The intensity calibrated point cloud is projected onto the cylindrical surface centered on the laser radar, and based on the horizontal direction angle and the vertical height, the projected cylindrical point cloud is spatially divided, and the maximum point cloud intensity value in each subspace is saved. At the same time, the spatial density distribution of the point cloud in the same direction angle subspace is considered to construct the global descriptor point cloud intensity projection shape feature. In loop detection, the loop search is performed based on the global descriptor point cloud intensity projection shape feature, and the search result is verified through the consistency verification module. High-precision mapping and positioning can be realized in an environment with insufficient geometric features. The backend retains the loop detection method based on the odometer, and increases the loop detection method based on the point cloud intensity feature descriptor, so as to improve the effect of the backend optimization of the LiDAR SLAM framework.

[0131] S6, the loop detection result passed verification is added as a constraint into the factor graph, and the historical trajectory pose and the point cloud map are optimized by means of the factor graph, so that the optimized trajectory pose and the global map are obtained.

[0132] Specifically, in this step, the following sub-steps are specifically included:

[0133] Step 6.1. The pose of the current frame in the global map is estimated based on the relative pose transformation of the current frame and the previous frame and the pose of the previous frame in the global map.

[0134] R k+1 =R k ·R cur .

[0135] Pk +1 =Pk-R k+1 ·Pc ur

[0136] wherein Rk, R k+1 is the rotation matrix of the previous frame and the current frame under the global map, Pk, Pk + 1 is the position coordinate of the previous frame and the current frame under the global map, R cur , Pcur is the rotation transformation and displacement transformation of the previous frame relative to the current frame.

[0137] Step 6.2. The point cloud data near the current frame is extracted based on the result of the odometer to construct a local point cloud map, the current frame is matched with the local point cloud map, and the point cloud data of the current frame is added into the map, so as to realize point cloud map updating.

[0138] Step 6.3. The pose P = {x, y, z, roll, pitch, yaw} of each frame of the laser radar calculated by the front-end odometer is taken as a node of the factor graph, and the relative pose transformation relationship between adjacent frames of the laser radar is taken as an edge of the factor graph. After the loop is detected in the back-end, the result of the loop detection is added as a new constraint into the factor graph, so as to realize back-end graph optimization, and the optimized trajectory pose and the point cloud map are obtained.

[0139] It should be noted that the graph optimization is an optimization algorithm for solving a nonlinear least squares problem, and is mainly used for state estimation and mapping problems in the fields of robotics and computer vision.

[0140] In specific implementation, the method proposed in the technical scheme of the present application can be automatically run by a person skilled in the art by using computer software technology, and the system device of the method, such as a computer readable storage medium storing the corresponding computer program of the technical scheme of the present application and a computer device including the running of the corresponding computer program, should also be within the protection scope of the present application.

[0141] The embodiment of the present application also provides a three-dimensional laser radar synchronous mapping and positioning system fusing point cloud intensity, comprising the following modules:

[0142] A point cloud data preprocessing module is configured to preprocess the collected point cloud data to obtain preprocessed point cloud data.

[0143] A point cloud data calculation module is configured to project the preprocessed point cloud data in 2D to obtain a point cloud depth map and a point cloud intensity map; segment the preprocessed point cloud data into ground points and non-ground points, and extract point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map; and adaptively weight and fuse the point cloud geometric feature matching result and the intensity feature matching result in time based on the extracted point cloud geometric features and intensity features to obtain a current pose.

[0144] A loop detection module is configured to construct a global descriptor point cloud intensity projection shape feature based on the preprocessed point cloud intensity cylindrical projection and spatial density distribution; and perform loop detection based on the global descriptor point cloud intensity projection shape feature, and verify the loop detection result.

[0145] A point cloud data optimization module is configured to add the verified loop detection result as a constraint into a factor graph, and optimize the historical trajectory pose and the point cloud map in the factor graph to obtain an optimized trajectory pose and global map.

[0146] In some possible embodiments, a three-dimensional laser radar synchronous mapping and positioning system fusing point cloud intensity is provided, comprising a readable storage medium, wherein the readable storage medium stores a computer program, and the computer program is configured to implement the three-dimensional laser radar synchronous mapping and positioning method fusing point cloud intensity.

[0147] Finally, it should be noted that: the above only describes the preferred embodiments of the present application and is not used to limit the present application, although the present application has been described in detail with reference to the foregoing embodiments, for those skilled in the art, the technical solutions recorded in the foregoing embodiments can be modified, or some technical features can be replaced, any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application should be included in the protection scope of the present application.

Claims

1. A method for 3D lidar simultaneous mapping and localization fusing point cloud intensity, characterized in that, The method comprises the following steps: Step 1, preprocessing the collected point cloud data to obtain the point cloud data after preprocessing; Step 2, projecting the point cloud data after preprocessing into a 2D depth map and a 2D intensity map; Step 3, segmenting the ground points and non-ground points based on the point cloud depth map and the point cloud intensity map, and extracting the point cloud geometric features and intensity features; Step 4, based on the extracted point cloud geometric features and intensity features, adaptively weighting and fusing the point cloud geometric feature matching results and the intensity feature matching results to obtain the current pose; Step 5, based on the cylindrical projection of the point cloud intensity after preprocessing and the spatial density distribution, constructing a global descriptor point cloud intensity projection shape feature; and based on the global descriptor point cloud intensity projection shape feature, performing loop closure detection and verifying the loop closure detection results; Step 6, adding the verified loop closure detection results as constraints into the factor graph, and optimizing the historical trajectory pose and the point cloud map in the factor graph to obtain the optimized trajectory pose and global map.

2. The method of claim 1, wherein, In step 1, the preprocessing of the collected point cloud data comprises: Step 1.1, collecting information about the surrounding environment by a 3D LiDAR to obtain original point cloud data; Step 1.2, then performing conditional filtering on the original point cloud data, setting the maximum distance and the maximum height, and filtering out the point cloud data beyond the range; Step 1.3, intensity calibration of the point cloud data after conditional filtering.

3. The fusion of point cloud intensity 3D lidar simultaneous mapping and localization method according to claim 1, characterized in that, In step 2, the 2D projection of the point cloud data after preprocessing comprises: Step 2.1, projecting the point cloud data after preprocessing into a 2D depth map and a 2D intensity map, wherein the 2D depth map saves the distance from the point cloud to the center of the laser radar, and the 2D intensity map saves the intensity value of the corresponding point cloud after calibration.

4. The fusion of point cloud intensity 3D lidar simultaneous mapping and localization method according to claim 1, characterized in that, In step 3, the extraction of the point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map comprises: Step 3.1, segmenting the ground points and non-ground points of the point cloud data after intensity calibration, and assigning labels to the point cloud data of the ground points, non-ground points and discrete points; Step 3.2, calculating the point cloud geometric smooth features and intensity smooth features based on the 2D depth map and the 2D intensity map of the point cloud data; Step 3.3, based on the results of the geometric smooth features, extracting point cloud geometric plane feature points in the ground point cloud and point cloud geometric edge feature points in the non-ground point cloud; Step 3.4, based on the results of the intensity smooth features, extracting point cloud intensity plane feature points in the ground point cloud and point cloud intensity edge feature points in the non-ground point cloud.

5. The method for simultaneous 3D laser radar mapping and positioning based on point cloud intensity fusion according to claim 1, characterized in that: In step 4, the adaptive weighting and fusion of the point cloud geometric feature matching results and the intensity feature matching results comprises: Step 4.1, judging the richness of the geometric features in the environment based on the number of extracted geometric features; Step 4.2, if the geometric features are rich, only based on the geometric features to match and optimize the adjacent frame point clouds to obtain the relative pose transformation of the laser radar between adjacent frames; Step 4.

3. If the geometric features are insufficient, then the matching and optimization of the point clouds of adjacent frames are performed based on the intensity features, and the matching results obtained are fused with the matching results based on the geometric features.

6. The fused point cloud intensity 3D lidar simultaneous mapping and localization method according to claim 1, wherein, In step 5, the global descriptor point cloud intensity projection shape feature is constructed based on the cylindrical projection and the spatial density distribution of the point cloud intensity after preprocessing, including: Step 5.

1. Project the point cloud after intensity calibration onto a cylindrical surface centered on the laser radar, define a frame of point cloud data obtained by the laser radar and each laser point therein, and project the frame of point cloud data onto a polar coordinate system; Step 5.

2. Divide the laser point cloud after cylindrical projection into N s sectors in horizontal direction angle and N p blocks in height, then divide the point cloud data of the frame into N s *N p subspaces; Step 5.

3. For each subspace, extract the maximum value of the point cloud intensity to form a point cloud intensity matrix I; Step 5.

4. Statistically binning the density of point clouds with the same horizontal directional angle interval, calculating the point cloud density weight D ij ; Step 5.

5. Multiply the point cloud density weight with the point cloud matrix to construct the global descriptor point cloud intensity projection shape feature based on the cylindrical projection of the point cloud intensity.

7. The fusion of point cloud intensity 3D lidar simultaneous mapping and localization method according to claim 6, characterized in that, In step 5, the loop closure detection is performed based on the global descriptor point cloud intensity projection shape feature, and the result of the loop closure detection is verified, including: Step 5.

6. After obtaining the global descriptor point cloud intensity projection shape feature, extract the row vector and the column vector of the descriptor; wherein the elements in the row vector and the column vector are the average values of the elements of the corresponding column and the corresponding row of the descriptor matrix; Step 5.

7. Based on the column vector, search for a candidate position, save the column vector as a KD tree in the form of a Vector class, based on the size of the Euclidean distance between the column vector of the current frame descriptor matrix and the column vector of the historical frame, index in the KD tree, and search for n1 point cloud frames corresponding to the column vectors with the smallest Euclidean distance as candidate frames; Step 5.

8. In the n1 candidate frames, based on the row vector, the cosine distance between the current frame and the candidate frame is calculated; the shift th obtained by the row vector calculation is taken as the initial value, and the cosine distance between the two descriptors is calculated within the column range of n th columns around the initial value. Step 5.

9. After detecting the loop closure based on the descriptor, first perform a time consistency test, based on the orientations of the detected current frame and the candidate frame, calculate the cosine distance of the descriptors of the N frames of point cloud data around them respectively; Step 5.

10. After the time consistency test, take the relative pose calculated based on the descriptor matching as the initial value, perform geometric consistency test through direct matching algorithm or feature matching algorithm, and if the geometric consistency test score is less than the threshold value, it is considered that the current loop closure detection result is accurate.

8. The fusion of point cloud intensity 3D lidar simultaneous mapping and localization method according to claim 1, characterized in that, In step 6, the loop closure detection result that passes the verification is added as a constraint into the factor graph, and the optimized trajectory pose and global map are obtained by optimizing the historical trajectory pose and the point cloud map in the factor graph, including: Step 6.

1. Estimate the pose of the current frame in the global map based on the relative pose transformation of the current frame and the previous frame and the pose of the previous frame in the global map; Step 6.

2. Extract the point cloud data around the current frame based on the result of the odometry to construct a local point cloud map, match the current frame with the local point cloud map, and add the point cloud data of the current frame into the map, thereby realizing the update of the point cloud map; Step 6.

3. Take the calculated pose of each frame of laser radar as a node of the factor graph, take the relative pose transformation relationship between adjacent frames of laser radar as an edge of the factor graph, add the result of the loop closure detection as a new constraint into the factor graph, realize the optimization of the back-end graph, and thereby obtain the optimized trajectory pose and point cloud map.

9. A fused point cloud intensity 3D lidar simultaneous mapping and localization system for implementing a fused point cloud intensity 3D lidar simultaneous mapping and localization method according to any one of claims 1-8, characterized in that, The following modules are included: The point cloud data preprocessing module is configured to preprocess the collected point cloud data to obtain preprocessed point cloud data. The point cloud data calculation module is configured to perform 2D projection on the preprocessed point cloud data to obtain a point cloud depth map and a point cloud intensity map; perform ground point and non-ground point segmentation on the preprocessed point cloud data, and extract point cloud geometric features and intensity features based on the point cloud depth map and the point cloud intensity map. Based on the extracted point cloud geometric features and intensity features, the point cloud geometric feature matching result and the intensity feature matching result are adaptively weighted and fused in real time to obtain a current pose. The loop closure detection module is configured to construct a global descriptor point cloud intensity projection shape feature based on the preprocessed point cloud intensity cylindrical projection and spatial density distribution; perform loop closure detection based on the global descriptor point cloud intensity projection shape feature, and verify the result of the loop closure detection. The point cloud data optimization module is configured to add the verified loop closure detection result as a constraint into a factor graph, and optimize the historical trajectory pose and the point cloud map in the factor graph to obtain an optimized trajectory pose and a global map.

Images