Laser radar SLAM method and system based on geometric information and intensity information

Abstract

The present invention provides a laser radar SLAM method and system based on geometric information and intensity information. The laser radar collects object point clouds, calibrates the intensity information of each point in the object point cloud to obtain a calibrated object point cloud, extracts geometric feature points and intensity feature points of each frame of the calibrated object point cloud according to the calibrated object point cloud, and constructs and updates a point cloud intensity map according to the intensity feature points of each frame of the calibrated object point cloud. The point cloud is then registered according to the geometric feature points, the intensity feature points and the point cloud intensity map to obtain the inter-frame pose transformation, and after the point cloud pose is updated to generate a corresponding pose map, trajectory inference is performed according to the pose map to construct a positioning map, and the pose map is optimized by back-end closed-loop detection, and the trajectory inference and positioning map are updated. The technical solution can complete the registration optimization by matching the intensity feature with the intensity map in a degraded environment, effectively solve the laser radar SLAM failure problem in a degraded environment, improve the positioning accuracy and avoid the waste of computing resources.

Description

Technical Field

[0001] The present invention relates to the technical field of synchronous positioning and map construction, and in particular to a laser radar SLAM method, system, computer equipment and storage medium based on geometric information and intensity information. Background Art

[0002] SLAM (Simultaneous localization and Mapping) is a simultaneous positioning and mapping technology widely used in fields such as mobile robots and unmanned driving. LiDAR-based SLAM technology has become a research hotspot in the field and has attracted widespread attention because LiDAR can quickly, accurately and directly obtain surrounding environment information and is not affected by environmental occlusion and light changes.

[0003] The current LiDAR SLAM algorithms mainly focus on the geometric information of point clouds, such as Hector SLAM using the classic ICP algorithm, the LOAM series of algorithms that perform point cloud registration by extracting line and surface features of point clouds, the hdl_graph_slam algorithm that uses the NDT algorithm to achieve point-to-map registration, and the SUMA algorithm that uses facet ICP to match point clouds. These algorithms can achieve good positioning accuracy in general scenarios, but in classic scenarios with degraded geometric structures such as long tunnels and long corridors, as well as in open outdoor scenes that lack geometric feature points, the extracted features are too sparse, which will lead to inaccurate registration results, thus affecting map construction and positioning accuracy.

[0004] Although some scholars have proposed solutions to the degradation problem of lidar SLAM in unstructured scenes, whether it is adding IMU tight coupling through multi-sensor fusion and realizing new coupling methods in degraded scenarios or adding UWB systems to provide position constraints, or using intensity information as an aid, there are corresponding application defects: the multi-sensor fusion method not only increases the application cost, but also is prone to multi-sensor calibration problems. The method of using intensity information as an aid does not fully utilize the intensity information and cannot truly and effectively solve the degradation problem, which in turn leads to a decline in the overall performance of lidar SLAM and affects the actual application effect. Summary of the invention

[0005] The purpose of the present invention is to provide a laser radar SLAM method based on geometric information and intensity information. By constructing a point cloud intensity map based on intensity feature points extracted from object point clouds, the method is used to optimize point cloud registration in a degraded environment where the geometric structure is not obvious, thereby solving the failure problem of laser radar SLAM in the degraded environment of the prior art, effectively improving positioning accuracy, and avoiding the waste of computing resources.

[0006] In order to achieve the above objectives, it is necessary to provide a laser radar slam method, system, computer device and storage medium based on geometric information and intensity information in response to the above technical problems.

[0007] In a first aspect, an embodiment of the present invention provides a laser radar slam method based on geometric information and intensity information, the method comprising the following steps:

[0008] Collecting object point cloud by laser radar; the object point cloud includes three-dimensional coordinate information and corresponding intensity information of each point;

[0009] Calibrate the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud; the calibration object point cloud includes three-dimensional coordinate information of each point and corresponding calibration intensity information;

[0010] Extracting geometric feature points and intensity feature points of each frame of the point cloud according to the calibration object point cloud, and constructing and updating a point cloud intensity map according to the intensity feature points of each frame of the calibration object point cloud;

[0011] Performing point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map to obtain inter-frame pose transformation, updating point cloud pose according to the inter-frame pose transformation, and generating corresponding pose map according to the point cloud pose of each frame;

[0012] Trajectory inference is performed based on the pose graph to construct a positioning map, and the pose graph is optimized through back-end closed-loop detection, and the trajectory inference and positioning map are updated.

[0013] Furthermore, the step of calibrating the intensity information of each point in the object point cloud to obtain a corresponding calibrated object point cloud includes:

[0014] constructing an object plane according to the object point cloud, and performing local normal analysis on the object plane to obtain a laser beam incident angle;

[0015] According to the incident angle of the laser beam and the distance of the object detected by the laser radar, the intensity information of each point is calibrated to obtain the corresponding calibration intensity information; the calibration intensity information is expressed as:

[0016]

[0017] in, is a constant; is the material reflectivity of the object; is the incident angle of the laser beam; is the distance of the object detected by the lidar.

[0018] Furthermore, the step of extracting geometric feature points and intensity feature points of each frame of point cloud according to the calibration object point cloud includes:

[0019] Performing ground segmentation on the calibration object point cloud to obtain ground points;

[0020] Calculating curvature information of each point in each frame of the point cloud, and segmenting each frame of the point cloud according to a first preset number to obtain a segmented point cloud;

[0021] Arrange each segmented point cloud in descending order according to the corresponding curvature information, select a second preset number of non-ground points as edge points from top to bottom, and select a second preset number of plane points from bottom to top;

[0022] According to the preset intensity threshold, the effective intensity points of each segmented point cloud are screened and obtained, and the effective intensity points are statistically analyzed to obtain the corresponding intensity feature points.

[0023] Furthermore, the step of performing statistical analysis on the effective intensity points to obtain corresponding intensity feature points includes:

[0024] Perform statistics on the calibration intensity information of the valid intensity points to obtain the median of the calibration intensity information;

[0025] The points whose calibration intensity information among the valid intensity points is greater than the median of the calibration intensity information are extracted as corresponding intensity feature points.

[0026] Furthermore, the step of constructing and updating the point cloud intensity map according to the intensity feature points includes:

[0027] According to the preset grid size, a grid map is constructed, and each grid unit of the grid map is initialized with the intensity calibration value of the intensity feature point of the first frame point cloud to obtain a point cloud intensity map;

[0028] The current frame point cloud is registered with the point cloud intensity map of the previous frame point cloud, and the point cloud intensity map is updated using the intensity calibration value of the intensity feature point of the current frame point cloud; the update formula of the point cloud intensity map is:

[0029]

[0030] in, is the grid unit; The current frame grid The strength value of The grid for the previous frame The strength value of is the number of frames of the calibration object point cloud, Intensity calibration value for the calibration object point cloud.

[0031] Furthermore, the step of performing point cloud registration according to the geometric feature points, the intensity feature points and the point cloud intensity map to obtain the inter-frame pose transformation includes:

[0032] A minimum point-to-line distance constraint is established based on the edge points of each frame, and a minimum point-to-plane distance constraint is established based on the plane points of each frame;

[0033] According to the minimum constraint of the point-to-line distance and the minimum constraint of the point-to-plane distance, a geometric feature target equation is constructed; the geometric feature target equation is expressed as:

[0034]

[0035] In the formula,

[0036]

[0037]

[0038] in, Indicates k Frame edge point set; Indicates k Frame No. i edge points; Indicated in k -A pair of matching points searched in 1 frame point cloud; Indicates k Frame plane point set; Indicates k Frame No. i plane points; Indicated in k -3 matching points searched in 1 frame point cloud; is the geometric characteristic objective equation; Indicates k Frame and k -1 frame inter-frame pose transformation;

[0039] Point cloud registration is performed according to the geometric feature target equation, and the LM algorithm is used to solve the inter-frame pose transformation.

[0040] Furthermore, the step of performing point cloud registration according to the geometric feature target equation to obtain inter-frame pose transformation includes:

[0041] According to the geometric feature target equation, a degradation detection matrix is ​​constructed, and point cloud registration degradation detection is performed according to the minimum eigenvalue of the degradation detection matrix to obtain a degradation detection result;

[0042] Determine whether the degradation detection result is degradation. If so, construct intensity feature constraints based on the intensity feature points and the point cloud intensity map, establish the corresponding intensity feature target equation, perform point cloud registration based on the intensity feature target equation, and use the LM algorithm to solve and update the inter-frame pose transformation; the intensity feature target equation is expressed as:

[0043]

[0044] In the formula,

[0045]

[0046] in, Represents a set of intensity feature points; Indicates i Intensity feature points; represents the calibration intensity value of the i-th intensity feature point; Represents the point cloud intensity map; For the i Intensity feature points in the point cloud intensity map Point intensity values ​​in ; is the strength characteristic objective equation.

[0047] In a second aspect, an embodiment of the present invention provides a laser radar SLAM system based on geometric information and intensity information, the system comprising:

[0048] A point cloud acquisition module is used to acquire object point clouds through a laser radar; the object point clouds include three-dimensional coordinate information of each point and corresponding intensity information;

[0049] An intensity calibration module, used to calibrate the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud; the calibration object point cloud includes three-dimensional coordinate information of each point and corresponding calibration intensity information;

[0050] An intensity map construction module is used to extract geometric feature points and intensity feature points of each frame of the point cloud according to the calibration object point cloud, and to construct and update a point cloud intensity map according to the intensity feature points of each frame of the calibration object point cloud;

[0051] A pose estimation module is used to perform point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map, obtain inter-frame pose transformation, update point cloud pose according to the inter-frame pose transformation, and generate a pose map according to the point cloud pose of each frame;

[0052] The map construction module is used to perform trajectory estimation based on the pose graph, construct a positioning map, optimize the pose graph through back-end closed-loop detection, and update the trajectory estimation and positioning map.

[0053] In a third aspect, an embodiment of the present invention further provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0054] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program implements the steps of the above method when executed by a processor.

[0055] The above-mentioned application provides a laser radar slam method, system, computer equipment and storage medium based on geometric information and intensity information. Through the method, the laser radar collects object point cloud, calibrates the intensity information of each point in the object point cloud to obtain the calibrated object point cloud, extracts the geometric feature points and intensity feature points of each frame of the point cloud according to the calibrated object point cloud, and constructs and updates the point cloud intensity map according to the intensity feature points of each frame of the calibrated object point cloud. Then, the point cloud is aligned according to the geometric feature points, intensity feature points and point cloud intensity map to obtain the inter-frame pose transformation, and after updating the point cloud pose to generate the corresponding pose map, the trajectory is calculated according to the pose map to construct a positioning map, and the pose map is optimized through back-end closed-loop detection, and the trajectory calculation and positioning map are updated. Technical solutions. Compared with the existing technology, the lidar slam method based on geometric information and intensity information can use intensity features to assist geometric features for matching to achieve fast point cloud registration in an environment with obvious geometric features, and use intensity features and intensity map matching to perform registration optimization in a degraded environment, effectively solving the problem of lidar slam failure in a degraded environment, improving positioning accuracy and avoiding waste of computing resources, and has high practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 is a SLAM system architecture diagram of a laser radar SLAM method based on geometric information and intensity information in an embodiment of the present invention;

[0057] Figure 2 is a flow chart of a laser radar slam method based on geometric information and intensity information in an embodiment of the present invention;

[0058] Figure 3 is a structural schematic diagram of a laser radar SLAM system based on geometric information and intensity information in an embodiment of the present invention;

[0059] Figure 4 It is a diagram of the internal structure of a computer device in an embodiment of the present invention. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical scheme and beneficial effects of the present application clearer, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. Obviously, the embodiments described below are part of the embodiments of the present invention and are only used to illustrate the present invention, but are not used to limit the scope of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0061] The present invention considers that the existing laser radar slam method cannot really effectively solve the positioning accuracy problem in degraded environments. Figure 1 The process framework shown in the figure adaptively extracts geometric features and intensity features based on the three-dimensional point cloud data collected by the laser radar, introduces a degradation detection mechanism to perform degradation detection on the point cloud registration, and uses intensity features to optimize the point cloud registration and pose estimation when identifying a degraded scene, thereby improving the accuracy and robustness of the laser radar-based SLAM method in unstructured scenes; at the same time, the method can be applied to the laser radar SLAM front-end point cloud registration and back-end loop detection to realize a complete laser SLAM system including data preprocessing, front-end odometer, back-end closed-loop detection optimization and map construction; the following embodiments will explain in detail the laser radar SLAM method based on geometric information and intensity information of the present invention.

[0062] In one embodiment, Figure 2 As shown, a laser radar slam method based on geometric information and intensity information is provided, comprising the following steps:

[0063] S11. Collecting object point cloud through laser radar; the object point cloud includes three-dimensional coordinate information of each point and corresponding intensity information; wherein, the object point cloud is three-dimensional point cloud data collected by the laser radar sensor, and the three-dimensional point cloud is directly processed subsequently without projecting the point cloud into an image, which can effectively retain the three-dimensional information of the point cloud.

[0064] S12, calibrating the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud; the calibration object point cloud includes three-dimensional coordinate information of each point and corresponding calibration intensity information; wherein the calibration intensity information can be understood as converting the intensity information of each point in the original object point cloud into intensity information only related to the reflectivity of the object; specifically, the step of calibrating the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud includes:

[0065] constructing an object plane according to the object point cloud, and performing local normal analysis on the object plane to obtain a laser beam incident angle;

[0066] According to the incident angle of the laser beam and the distance of the object detected by the laser radar, the intensity information of each point is calibrated to obtain the corresponding calibration intensity information; the calibration intensity information is expressed as:

[0067]

[0068] in, is a constant; is the material reflectivity of the object; is the incident angle of the laser beam; is the distance of the object detected by the lidar.

[0069] S13, extracting geometric feature points and intensity feature points of each frame of point cloud according to the calibration object point cloud, and constructing and updating the point cloud intensity map according to the intensity feature points of each frame of the calibration object point cloud; wherein, the process of extracting geometric feature points and intensity feature points can be understood as extracting geometric feature points by calculating and comparing the curvature information of each non-ground point for each frame of point cloud data after obtaining ground points by performing ground segmentation based on the obtained calibration object point cloud, and extracting intensity feature points by comparing the intensity information of each non-ground point; specifically, the step of extracting geometric feature points and intensity feature points of each frame of point cloud according to the calibration object point cloud includes:

[0070] Perform ground segmentation on the calibration object point cloud to obtain ground points; wherein the ground segmentation is only used to segment the ground points for filtering in subsequent geometric feature point extraction, and the specific ground segmentation method is not limited here;

[0071] Calculate the curvature information of each point in each frame of the point cloud, and segment each frame of the point cloud according to a first preset number to obtain a segmented point cloud; wherein the calculation formula of the curvature information is:

[0072]

[0073] in, Indicates i The curvature of a point; Indicates the target point; Represents a point cloud set within a certain range from the target point (such as five points before and after); is a point in the set of points near the target point.

[0074] In order to ensure that the extracted feature points are evenly distributed, the point cloud of each frame is segmented according to a first preset number, and then feature points are extracted from each segment of the point cloud. The first preset number can be set according to actual application requirements, preferably divided into six segments, and geometric feature points and intensity feature points are extracted according to the following steps.

[0075] Arrange each segmented point cloud in descending order according to the corresponding curvature information, select a second preset number of non-ground points as edge points from top to bottom, and select a second preset number of plane points from bottom to top; wherein the second preset number can be adaptively adjusted according to the number of point clouds obtained by different numbers of beams of the multi-line laser radar. If the number of point clouds in this part is M, the second preset number can be set , no specific restrictions are made here; specifically, the geometric feature point extraction process can be understood as sorting the segmented point cloud according to the curvature size, selecting N points with the largest curvature from non-ground points as edge points, and selecting N points with the smallest curvature including ground points as plane points.

[0076] According to the preset intensity threshold, the valid intensity points of each segmented point cloud are screened and statistically analyzed to obtain the corresponding intensity feature points; wherein the preset intensity threshold is set for the intensity after calibration, and the specific value can be selected according to the actual application requirements. If the preset intensity threshold is 0.5, the points in each segmented point cloud whose calibration intensity information is less than the preset intensity threshold of 0.5 are first filtered out, and then the calibration intensity values ​​of the remaining point clouds are statistically analyzed to further screen out the intensity feature points; specifically, the step of performing statistical analysis on the valid intensity points to obtain the corresponding intensity feature points includes:

[0077] The calibration intensity information of the effective intensity points is counted to obtain the median of the calibration intensity information, and the median is used as the intensity feature extraction threshold;

[0078] The points whose calibration intensity information in the valid intensity points is greater than the median value of the calibration intensity information (intensity feature extraction threshold) are extracted as the corresponding intensity feature points, that is, the calibration intensity value in the point cloud segment is Points greater than the intensity feature extraction threshold are extracted as intensity feature points. Landmarks with obvious intensity features are adaptively extracted, such as license plates, lights, road signs, etc. in real life, which can be extracted as feature points, providing good constraints for point cloud registration in environments where geometric features are not obvious.

[0079] This embodiment realizes adaptive feature extraction through the above method steps. By adjusting the number of segments and the number of feature points extracted in each frame of the point cloud, the uniformity and effectiveness of feature extraction can be effectively guaranteed, which provides a reliable basis for the construction and update of the following point cloud intensity map, and also provides a reliable guarantee for the subsequent balance between alignment accuracy and computational efficiency.

[0080] Specifically, the step of constructing and updating the point cloud intensity map according to the intensity feature points includes:

[0081] According to the preset grid size, a grid map is constructed, and each grid unit of the grid map is initialized with the intensity calibration value of the intensity feature point of the first frame point cloud to obtain a point cloud intensity map;

[0082] The current frame point cloud is registered with the point cloud intensity map of the previous frame point cloud, and the point cloud intensity map is updated using the intensity calibration value of the intensity feature point of the current frame point cloud; wherein, the point cloud intensity map is mainly used for point cloud registration optimization when there is degradation in the following point cloud registration degradation detection. For specific usage methods, please refer to the description of the relevant content of step S14, which will not be repeated here; the update formula of the point cloud intensity map is:

[0083]

[0084] in, is the grid unit; The current frame grid The strength value of The grid for the previous frame The strength value of is the number of frames of the calibration object point cloud, Intensity calibration value for the calibration object point cloud.

[0085] S14, performing point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map to obtain inter-frame pose transformation, and updating point cloud pose according to the inter-frame pose transformation, and generating corresponding pose map according to the point cloud pose of each frame; wherein, the process of point cloud registration can be understood as first using geometric features for registration, and performing degraded scene detection on the registration process, if there is degradation, then performing registration optimization according to the intensity feature points and the point cloud intensity map obtained in step S13, so as to ensure the effectiveness of inter-frame position transformation solution and point cloud pose estimation, thereby improving registration performance and positioning accuracy; specifically, the step of performing point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map to obtain inter-frame pose transformation includes:

[0086] A minimum point-to-line distance constraint is established based on the edge points of each frame, and a minimum point-to-plane distance constraint is established based on the plane points of each frame;

[0087] According to the minimum constraint of the point-to-line distance and the minimum constraint of the point-to-plane distance, a geometric feature target equation is constructed; the geometric feature target equation is expressed as:

[0088]

[0089] In the formula,

[0090]

[0091]

[0092] in, Indicates k Frame edge point set; Indicates k Frame No. i edge points; Indicated in k -A pair of matching points searched in 1 frame point cloud; represents the plane point set of the kth frame; Indicates k Frame No. i plane points; Indicated in k -3 matching points searched in 1 frame point cloud; is the geometric characteristic objective equation; Indicates k Frame and k -1 frame inter-frame pose transformation;

[0093] Point cloud registration is performed according to the geometric feature target equation, and the inter-frame pose transformation is obtained by using the LM algorithm. ; Among them, the process of using the LM algorithm to solve the inter-frame pose transformation can be realized by referring to the prior art, and will not be repeated here;

[0094] The above steps realize that point cloud registration can only guarantee the registration accuracy in an environment with obvious geometric features, and will fail in a degraded environment. Therefore, this embodiment preferably introduces a degradation detection mechanism based on the use of intensity information to assist geometric information registration to effectively solve the above degraded scene problem. Specifically, the step of performing point cloud registration according to the geometric feature target equation to obtain the inter-frame pose transformation includes:

[0095] According to the geometric feature target equation, a degradation detection matrix is ​​constructed, and point cloud registration degradation detection is performed according to the minimum eigenvalue of the degradation detection matrix to obtain a degradation detection result; wherein, the degradation detection matrix A uses the geometric feature target equation The first-order Jacobian matrix of is expressed as follows:

[0096]

[0097] in, Indicates k Frame and k -1 frame inter-frame pose transformation;

[0098] The process of obtaining degradation detection results can be understood as follows: using the singular value decomposition method to obtain The six eigenvalues ​​of The 6 state directions of ; Obtain the degradation factor based on the minimum eigenvalue , and then the degradation factor Compared with the set degradation threshold, if , then the degradation detection result is that there is degradation, otherwise, the degradation detection result is that there is no degradation;

[0099] Determine whether the degradation detection result is degradation. If so, construct intensity feature constraints based on the intensity feature points and the point cloud intensity map, establish the corresponding intensity feature target equation, perform point cloud registration based on the intensity feature target equation, and use the LM algorithm to solve and update the inter-frame pose transformation; the intensity feature target equation is expressed as:

[0100]

[0101] In the formula,

[0102]

[0103] in, Represents a set of intensity feature points; Indicates i Intensity feature points; Indicates i The calibration intensity value of each intensity feature point; Represents the point cloud intensity map; is the i-th intensity feature point in the point cloud intensity map Point intensity values ​​in ; is the strength characteristic objective equation

[0104] Specifically, it can be understood that if the point cloud registration is degraded, the intensity feature points are registered with the previously constructed point cloud intensity map in the registration optimization. In the registration process, based on the intensity feature target equation, the trilinear interpolation method is used to search for the points corresponding to the intensity feature points in the point cloud intensity map, and the LM algorithm is used to iteratively update the pose transformation. .

[0105] After obtaining the pose transformation through the above steps, the point cloud pose is updated through the following formula and the corresponding pose graph is generated:

[0106]

[0107] in, is the point cloud pose of the current frame; is the point cloud pose of the previous frame.

[0108] S15, performing trajectory extrapolation according to the pose graph, constructing a positioning map, optimizing the pose graph through back-end closed-loop detection, and updating the trajectory extrapolation and positioning map;

[0109] The back-end loop detection optimization can be understood as the loop optimization after the front-end odometer is completed. In order to ensure the computational efficiency, the key frame is selected first. If the current frame meets the three requirements of significant displacement, significant rotation angle change and a certain time, the current frame is selected as the key frame and stored in the pose graph maintained by the back-end. Before the intensity loop detection, the geometric features are used to check the similarity of the candidate frames (frames that may form a loop with the currently selected key frame) through ICP registration geometric consistency, and the detection threshold is set to avoid mismatching, that is, the ISC descriptor is extracted from the geometry and intensity information of the key frame, the space is divided into fan-shaped areas according to the azimuth angle, and into annular areas according to the polar coordinates in the radial direction, and multiple units are obtained by intersection. The highest intensity in the unit is taken as the intensity identifier of the unit, and the cosine distance formula is used to calculate the similarity between the extracted descriptor and the candidate descriptor. When the similarity score is less than the set threshold, the candidate frame is identified as the loop frame corresponding to the key frame, and the corresponding loop edge is added to the pose graph for optimization; finally, the offset is corrected by global optimization.

[0110] The construction of the positioning map and the updating of the incremental map after the optimization of the above-mentioned back-end closed-loop detection can be implemented by referring to the existing technology. At this point, a complete lidar SLAM system is constructed.

[0111] The embodiment of the present application introduces a degradation detection registration method under the traditional geometric point cloud registration method. It matches through geometric information in an environment with obvious structural features to avoid the loss of computing resources. At the same time, it uses extracted intensity features and constructed point cloud intensity maps to perform registration optimization in point cloud degradation scenarios, effectively solving the problem of laser radar SLAM failure in scenarios with unclear geometric structures. Compared with the geometric laser SLAM algorithm, this method is more accurate and robust and has higher application value.

[0112] It should be noted that although the steps in the above flowchart are shown in sequence according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders.

[0113] In one embodiment, Figure 3 As shown, a laser radar slam system based on geometric information and intensity information is provided, and the system includes:

[0114] Point cloud acquisition module 1, used to collect object point cloud through laser radar; the object point cloud includes three-dimensional coordinate information and corresponding intensity information of each point;

[0115] Intensity calibration module 2, used to calibrate the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud; the calibration object point cloud includes three-dimensional coordinate information of each point and corresponding calibration intensity information;

[0116] The intensity map construction module 3 is used to extract the geometric feature points and intensity feature points of each frame of the point cloud according to the calibration object point cloud, and to construct and update the point cloud intensity map according to the intensity feature points of each frame of the calibration object point cloud;

[0117] A pose estimation module 4 is used to perform point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map, obtain inter-frame pose transformation, update point cloud pose according to the inter-frame pose transformation, and generate a pose map according to the point cloud pose of each frame;

[0118] The map construction module 5 is used to perform trajectory estimation based on the posture graph, construct a positioning map, optimize the posture graph through back-end closed-loop detection, and update the trajectory estimation and positioning map.

[0119] For the specific definition of a laser radar slam system based on geometric information and intensity information, please refer to the definition of a laser radar slam method based on geometric information and intensity information above, which will not be repeated here. Each module in the above-mentioned laser radar slam system based on geometric information and intensity information can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0120] Figure 4 FIG. 1 shows an internal structure diagram of a computer device in an embodiment, and the computer device may specifically be a terminal or a server. Figure 4As shown, the computer device includes a processor, a memory, a network interface, a display and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a laser radar slam method based on geometric information and intensity information is implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covered on the display screen, or a key, trackball or touchpad set on the computer device housing, or an external keyboard, touchpad or mouse, etc.

[0121] It can be understood by those skilled in the art that Figure 4 The structure shown in the figure is only a block diagram of a part of the structure related to the present application scheme, and does not constitute a limitation on the computer device to which the present application scheme is applied. The specific computing device may include more or fewer components than shown in the figure, or combine certain components, or have the same component arrangement.

[0122] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the steps of the above method are implemented when the processor executes the computer program.

[0123] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the above method are implemented.

[0124] In summary, the embodiments of the present invention provide a laser radar slam method, system, computer device and storage medium based on geometric information and intensity information. The laser radar slam method based on geometric information and intensity information realizes the laser radar to collect object point cloud, calibrates the intensity information of each point in the object point cloud to obtain the calibrated object point cloud, extracts the geometric feature points and intensity feature points of each frame of the point cloud according to the calibrated object point cloud, and constructs and updates the point cloud intensity map according to the intensity feature points of each frame of the calibrated object point cloud, and then performs point cloud registration according to the geometric feature points, intensity feature points and point cloud intensity map to obtain the inter-frame pose transformation. After updating the point cloud pose to generate the corresponding pose graph, the trajectory is calculated according to the pose graph to build a positioning map, and the pose graph is optimized through back-end closed-loop detection, and the technical solution of updating the trajectory calculation and positioning map is presented. The laser radar slam method based on geometric information and intensity information can use intensity features to assist geometric features in matching to achieve fast point cloud registration in an environment with obvious geometric features, and use intensity features and intensity map matching to perform registration optimization in a degraded environment, effectively solving the laser radar slam failure problem in a degraded environment, improving positioning accuracy and avoiding waste of computing resources, and has high practical value.

[0125] Each embodiment in this specification is described in a progressive manner, and the same or similar parts of each embodiment can be directly referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. It should be noted that the technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above-mentioned embodiments are not described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0126] The above-mentioned embodiments only express several preferred implementation modes of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in the technical field, several improvements and substitutions can be made without departing from the technical principles of the present invention, and these improvements and substitutions should also be regarded as the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be based on the protection scope of the claims.

Claims

1. A laser radar slam method based on geometric information and intensity information, characterized in that: The method comprises the following steps: Collecting object point cloud by laser radar; the object point cloud includes three-dimensional coordinate information and corresponding intensity information of each point; Calibrate the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud; the calibration object point cloud includes three-dimensional coordinate information of each point and corresponding calibration intensity information; Extracting geometric feature points and intensity feature points of each frame of the point cloud according to the calibration object point cloud, and constructing and updating a point cloud intensity map according to the intensity feature points of each frame of the calibration object point cloud; Performing point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map to obtain inter-frame pose transformation, updating point cloud pose according to the inter-frame pose transformation, and generating corresponding pose map according to the point cloud pose of each frame; Perform trajectory extrapolation according to the pose graph, construct a positioning map, optimize the pose graph through back-end closed-loop detection, and update the trajectory extrapolation and positioning map; The steps of constructing and updating the point cloud intensity map according to the intensity feature points include: According to the preset grid size, a grid map is constructed, and each grid unit of the grid map is initialized with the intensity calibration value of the intensity feature point of the first frame point cloud to obtain a point cloud intensity map; The current frame point cloud is aligned with the point cloud intensity map of the previous frame point cloud, and the point cloud intensity map is updated using the intensity calibration value of the intensity feature point of the current frame point cloud; the update formula of the point cloud intensity map is: in, is the grid unit; Grid for the current frame The strength value of The grid for the previous frame The strength value of is the number of frames of the calibration object point cloud, Intensity calibration value for the calibration object point cloud.

2. The laser radar slam method based on geometric information and intensity information as claimed in claim 1, characterized in that: The step of calibrating the intensity information of each point in the object point cloud to obtain a corresponding calibrated object point cloud comprises: constructing an object plane according to the object point cloud, and performing local normal analysis on the object plane to obtain a laser beam incident angle; According to the incident angle of the laser beam and the distance of the object detected by the laser radar, the intensity information of each point is calibrated to obtain the corresponding calibration intensity information; the calibration intensity information is expressed as: in, is a constant; is the material reflectivity of the object; is the incident angle of the laser beam; is the distance of the object detected by the lidar.

3. The laser radar slam method based on geometric information and intensity information as claimed in claim 1, characterized in that: The step of extracting geometric feature points and intensity feature points of each frame of point cloud according to the calibration object point cloud comprises: Performing ground segmentation on the calibration object point cloud to obtain ground points; Calculating curvature information of each point in each frame of the point cloud, and segmenting each frame of the point cloud according to a first preset number to obtain a segmented point cloud; Arrange each segmented point cloud in descending order according to the corresponding curvature information, select a second preset number of non-ground points as edge points from top to bottom, and select a second preset number of plane points from bottom to top; According to the preset intensity threshold, the effective intensity points of each segmented point cloud are screened and obtained, and the effective intensity points are statistically analyzed to obtain the corresponding intensity feature points.

4. The laser radar slam method based on geometric information and intensity information as claimed in claim 3, characterized in that: The step of performing statistical analysis on the effective intensity points to obtain corresponding intensity feature points comprises: Perform statistics on the calibration intensity information of the valid intensity points to obtain the median of the calibration intensity information; The points whose calibration intensity information among the valid intensity points is greater than the median of the calibration intensity information are extracted as corresponding intensity feature points.

5. The laser radar slam method based on geometric information and intensity information as claimed in claim 1, characterized in that: The step of performing point cloud registration according to the geometric feature points, the intensity feature points and the point cloud intensity map to obtain the inter-frame pose transformation comprises: A minimum point-to-line distance constraint is established based on the edge points of each frame, and a minimum point-to-plane distance constraint is established based on the plane points of each frame; According to the minimum constraint of the point-to-line distance and the minimum constraint of the point-to-plane distance, a geometric feature target equation is constructed; the geometric feature target equation is expressed as: In the formula, in, Indicates k Frame edge point set; Indicates k Frame No. i edge points; Indicated in k -A pair of matching points searched in 1 frame point cloud; Indicates k Frame plane point set; Indicates k Frame No. i plane points; Indicated in k -3 matching points searched in 1 frame point cloud; is the geometric characteristic objective equation; Indicates k Frame and k -1 frame inter-frame pose transformation; Point cloud registration is performed according to the geometric feature target equation, and the LM algorithm is used to solve the inter-frame pose transformation.

6. The laser radar slam method based on geometric information and intensity information as claimed in claim 5, characterized in that: The step of performing point cloud registration according to the geometric feature target equation to obtain inter-frame pose transformation comprises: According to the geometric feature target equation, a degradation detection matrix is ​​constructed, and point cloud registration degradation detection is performed according to the minimum eigenvalue of the degradation detection matrix to obtain a degradation detection result; Determine whether the degradation detection result is degradation. If so, construct intensity feature constraints based on the intensity feature points and the point cloud intensity map, establish the corresponding intensity feature target equation, perform point cloud registration based on the intensity feature target equation, and use the LM algorithm to solve and update the inter-frame pose transformation; the intensity feature target equation is expressed as: In the formula, in, Represents a set of intensity feature points; Indicates i Intensity feature points; Indicates i The calibration intensity value of each intensity feature point; Represents the point cloud intensity map; For the i Intensity feature points in the point cloud intensity map Point intensity values ​​in ; is the strength characteristic objective equation.

7. A laser radar slam system based on geometric information and intensity information, characterized in that: The system comprises: A point cloud acquisition module is used to acquire object point clouds through a laser radar; the object point clouds include three-dimensional coordinate information of each point and corresponding intensity information; An intensity calibration module, used to calibrate the intensity information of each point in the object point cloud to obtain a corresponding calibration object point cloud; the calibration object point cloud includes three-dimensional coordinate information of each point and corresponding calibration intensity information; An intensity map construction module is used to extract geometric feature points and intensity feature points of each frame of the point cloud according to the calibration object point cloud, and to construct and update a point cloud intensity map according to the intensity feature points of each frame of the calibration object point cloud; A pose estimation module is used to perform point cloud registration according to geometric feature points, intensity feature points and point cloud intensity map, obtain inter-frame pose transformation, update point cloud pose according to the inter-frame pose transformation, and generate a pose map according to the point cloud pose of each frame; A map construction module, used to perform trajectory calculation based on the pose graph, construct a positioning map, optimize the pose graph through back-end closed-loop detection, and update the trajectory calculation and positioning map; Among them, according to the intensity feature points, the point cloud intensity map is constructed and updated, including: According to the preset grid size, a grid map is constructed, and each grid unit of the grid map is initialized with the intensity calibration value of the intensity feature point of the first frame point cloud to obtain a point cloud intensity map; The current frame point cloud is aligned with the point cloud intensity map of the previous frame point cloud, and the point cloud intensity map is updated using the intensity calibration value of the intensity feature point of the current frame point cloud; the update formula of the point cloud intensity map is: in, is the grid unit; Grid for the current frame The strength value of The grid for the previous frame The strength value of is the number of frames of the calibration object point cloud, Intensity calibration value for the calibration object point cloud.

8. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images