A Robot Localization and Mapping Method Integrating Laser Intensity and Point Cloud Geometric Features

Abstract

This invention discloses a method for robot localization and mapping that fuses laser intensity and point cloud geometric features, comprising: Step 1, fusing the point cloud geometric features and laser intensity of the acquired point cloud data from a mobile robot's LiDAR to obtain fused point cloud data, and classifying the fused point cloud data using a point cloud machine learning classification model; Step 2, constructing an intensity-based environment map based on the point cloud geometric features and laser intensity of the classified fused point cloud data, and jointly optimizing the optimal estimated pose of the mobile robot using the geometric residuals and intensity residuals obtained from the map; Step 3, performing real-time loop closure detection on the intensity-based environment map, and updating the intensity-based environment map and the pose of the mobile robot. This method, by fusing laser intensity and point cloud features, can reduce robot drift problems, and the machine learning algorithm, used only for feature classification of the LiDAR point cloud and real-time loop closure detection, reduces computational burden and ensures the real-time localization accuracy of the robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mobile robot navigation, and in particular to a robot positioning and mapping method fusing laser intensity and point cloud geometric features. BACKGROUND

[0002] In the process of navigation, a mobile robot needs to obtain its own pose in real time and perceive the surrounding environment. To this end, first, fixed reference objects need to be recorded in a predefined active area, and then the distance between the robot and multiple pre-recorded reference objects is used to calculate the current pose of the robot. The process of recording reference objects is called mapping, and the process of calculating the pose is called positioning. The processes of mapping and positioning depend on each other. In actual applications, simultaneous localization and mapping (SLAM) is usually used to estimate the pose of the robot. SLAM is a passive positioning scheme that does not rely on external labels (such as GPS, UWB base stations, etc.) deployed in the environment. It only relies on the robot's own sensors (such as lidar or camera, etc.) to calculate the robot's pose in the environment (i.e., positioning) and the environment map needed in subsequent mobile navigation.

[0003] The robot needs to perform simultaneous localization and mapping (SLAM) during autonomous movement, and both mapping and navigation require the robot to know its own position. Therefore, accurate positioning is a key problem that SLAM needs to solve. Current laser SLAM calculates the robot's pose based on the kinematic model and registration of two consecutive frames of lidar scan point clouds, and uses loop closure detection to optimize and adjust the globally estimated pose, thereby obtaining accurate pose and map information. For structured environments with poor scene diversity (such as long corridors, etc.), low-level geometric features will be severely degraded due to the presence of a large number of similarities. Lidar (Lidar) will have a large error by relying only on point cloud registration of geometric features in the environment. At the same time, when the environment becomes larger, the calculation time of loop closure detection will also increase, thereby affecting the real-time performance of positioning. Therefore, to improve the real-time positioning accuracy of the robot in a geometrically simple environment, existing laser SLAM techniques often use multi-sensor fusion and end-to-end deep neural network training methods.

[0004] The most common method of multi-sensor fusion is the fusion of lidar and camera information. Although cameras can provide more accurate feature detection, and high-frequency cameras can also reduce the odometry error of the robot to some extent. However, cameras are easily affected by environmental light and may not work properly in dark or textureless conditions. At the same time, the calibration and information alignment of lidar and camera also bring additional challenges.

[0005] For the fusion scheme of lidar and active sensor UWB, GPS, etc., these active sensors usually rely on the straight-line propagation of electromagnetic waves, and there is a problem of large ranging error in a large range of non-line-of-sight environment, thereby causing large fusion positioning error. Therefore, it is usually only applicable to a small-scale and relatively open environment. Moreover, multiple base stations need to be deployed in deployment.

[0006] The end-to-end based method mainly adopts a neural network to fit the original laser scanning data without any specific intensity analysis and formulaic calculation. Therefore, it is relatively difficult to associate new target objects with objects in the existing map, and a large amount of training data is needed to indirectly complete it. In actual application, it is usually difficult and time-consuming to collect, label and train data, and different environments may produce inconsistent performance problems, and more computing resources are also needed.

[0007] Therefore, how to provide a method capable of reducing the cumulative error of real-time positioning of a mobile robot in a structured environment, improving laser point cloud registration, reducing computing overhead in loop detection, providing more accurate real-time positioning, and relying only on a laser radar sensor without external active sensor fusion is a problem to be solved.

[0008] In view of this, the present application is proposed. SUMMARY

[0009] The purpose of the present application is to provide a robot positioning and mapping method fusing laser intensity and point cloud geometric features, which can improve the efficiency of laser point cloud registration in a structured environment with poor scene diversity but rich intensity features, thereby improving the real-time positioning and mapping accuracy of the robot, and thereby solving the above technical problems existing in the prior art.

[0010] The purpose of the present application is achieved by the following technical solutions:

[0011] A robot positioning and mapping method fusing laser intensity and point cloud geometric features, comprising:

[0012] Step 1, obtaining point cloud data of a laser radar of a mobile robot, fusing point cloud geometric features and laser intensity of the point cloud data to obtain fused point cloud data, and classifying the fused point cloud data according to features through a point cloud machine learning classification model;

[0013] Step 2, establishing an intensity environment map according to the point cloud geometric features and the laser intensity of the classified fused point cloud data, and jointly optimizing the best estimated pose of the mobile robot according to geometric residuals and intensity residuals obtained from the constructed intensity environment map;

[0014] Step 3, performing real-time loop detection on the intensity environment map, and updating the intensity environment map and the pose of the mobile robot according to the loop detection result.

[0015] Compared with the prior art, the robot positioning and mapping method provided by the application has the beneficial effects including:

[0016] Since the point cloud geometry features and laser intensity of the point cloud data of the mobile robot laser radar are first fused to obtain fused point cloud data, the fused point cloud data is classified according to features by a point cloud machine learning classification model, a strength environment map is constructed, the geometric residual and intensity residual obtained by using the strength environment map are used to jointly optimize the best estimated pose of the mobile robot, real-time loop detection is performed on the strength environment map, and the strength environment map and the pose of the mobile robot are updated according to the loop detection result, since the intensity of the laser point cloud that can well reflect the features of the environment around the robot is used, the fusion of the laser intensity and the point cloud features can reduce the robot drift problem; real-time loop detection ensures the real-time positioning accuracy of the robot. This method only needs a laser radar sensor and does not need to be fused with other sensors, which reduces the challenge of external calibration of multiple sensors and reduces the computing pressure. Moreover, due to the advantages of the laser radar, it can be well applied to different environments and is also suitable for positioning and mapping of robots in large-scale environments. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0018] Figure 1 The flowchart of the robot positioning and mapping method provided by the embodiment of the present application is provided.

[0019] Figure 2 The technical roadmap of the robot positioning and mapping method provided by the embodiment of the present application is provided.

[0020] Figure 3 The point cloud machine learning classification model in the method provided by the embodiment of the present application is provided.

[0021] Figure 4 The scanning context containing geometry and intensity information in the method provided by the embodiment of the present application is provided. DETAILED DESCRIPTION

[0022] The technical solutions in the embodiments of the present application will be described clearly and completely in combination with the specific contents of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments of the present application, which do not constitute a limitation of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work belong to the protection scope of the present application.

[0023] Firstly, the terms possibly used in the present application are explained as follows:

[0024] The term "and / or" means either of the two or both, for example, X and / or Y means three cases including "X" or "Y" or "X and Y".

[0025] The terms "include", "contain", "have", "possess" or other similar semantic descriptions should be interpreted as non-exclusive inclusion. For example, the inclusion of a technical feature element (such as raw materials, components, ingredients, carriers, dosage forms, materials, sizes, parts, components, mechanisms, devices, steps, processes, methods, reaction conditions, processing conditions, parameters, algorithms, signals, data, products or articles, etc.) should be interpreted as not only including the explicitly listed technical feature element, but also including other technical feature elements not explicitly listed in the art.

[0026] The term "consisting of" means excluding any technical feature element not explicitly listed. If this term is used in the claims, the term will make the claim closed, so that it does not contain technical feature elements other than the explicitly listed technical feature elements, except for conventional impurities related thereto. If the term only appears in a certain clause of the claim, it is only limited to the elements explicitly listed in the clause, and the elements described in other clauses are not excluded from the overall claim.

[0027] Unless otherwise explicitly specified or limited, the terms "mount", "connect", "connect", "fix", and the like should be understood broadly, for example: it can be fixedly connected, or it can be detachably connected, or integrally connected; it can be mechanically connected, or it can be electrically connected; it can be directly connected, or it can be indirectly connected through an intermediate medium, or it can be connected inside two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

[0028] The terms "central", "longitudinal", "lateral", "length", "width", "thickness", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", and the like indicate the orientation or positional relationship shown in the drawings, which are only for the convenience of description and simplification of description, and do not mean that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as limiting herein.

[0029] The fusion of laser intensity and point cloud geometric features of the robot positioning and mapping method provided by the present application is described in detail below. The contents not described in detail in the embodiments of the present application belong to the prior art known to those skilled in the art. If no specific conditions are specified in the embodiments of the present application, the conditions are performed according to the conventional conditions in the art or the conditions recommended by the manufacturer. If no manufacturer is specified for the reagents or instruments used in the embodiments of the present application, they are all conventional products that can be obtained by market purchase.

[0030] As shown in Figure 1 The embodiments of the present application provide a method for robot positioning and mapping by fusing laser intensity and point cloud geometric features, comprising:

[0031] Step 1, obtaining point cloud data of a mobile robot laser radar, fusing point cloud geometric features and laser intensity of the point cloud data to obtain fusion point cloud data, and classifying the fusion point cloud data according to features by a point cloud machine learning classification model;

[0032] Step 2, establishing an intensity environment map according to the point cloud geometric features and laser intensity of the classified fusion point cloud data, and jointly optimizing the best estimated pose of the mobile robot according to the geometric residual and intensity residual obtained from the constructed intensity environment map;

[0033] Step 3, performing real-time loop detection on the intensity environment map, and updating the intensity environment map and the pose of the mobile robot according to the loop detection result.

[0034] In step 1 of the above method, the fusion features are classified according to features by a point cloud machine learning classification model in the following manner, comprising:

[0035] Projecting the fusion point cloud data of the mobile robot laser radar onto a reference plane, dividing the reference plane into two-dimensional grid points of equal size, recording the height of each two-dimensional grid point and the minimum height of the adjacent two-dimensional grid point of the two-dimensional grid point , performing double-threshold filtering on each two-dimensional grid point by the following filtering formula, and dividing each two-dimensional grid point into non-ground points and ground points, the filtering formula is:

[0036] ;

[0037] wherein, represents a two-dimensional grid point; NG represents that the two-dimensional grid point is a non-ground point; G represents that the two-dimensional grid point is a ground point; h k represents the height of the point cloud projected to different layers of the same grid; δh1 represents the height threshold for dividing the point cloud in the same grid; δh2 represents the height threshold for dividing the point cloud in the adjacent grid;

[0038] Calculating the non-ground points and their spherical neighborhoods fused with the point cloud geometric features and laser intensity The information matrix is:

[0039] ;

[0040] wherein, is the gravity center of the neighborhood ; is the laser intensity mean value of the neighborhood , and the points with similar laser intensity to the non-ground point are given a higher weight;

[0041] The point cloud machine learning classification model is processed by the principal component analysis method to obtain the corresponding feature values of each non-ground point ;

[0042] The local linearity, planarity and curvature of each non-ground point are calculated by the corresponding feature values of each non-ground point obtained , the local linearity = (λ1-λ2) / λ1, the planarity = (λ2-λ3) / λ1, and the curvature = λ3 / (λ1+λ2+λ3), and each non-ground point is divided into linear points, different planar points and vertices according to the local linearity, planarity and curvature.

[0043] In step 2 of the above method, the intensity environment map is established according to the classified fusion point cloud data point cloud geometric features and laser intensity in the following manner, and the best estimated pose of the mobile robot is obtained by jointly optimizing the geometric residual and intensity residual obtained from the constructed intensity environment map, including:

[0044] The three-dimensional space is divided into grid cells, and each grid cell is jointly represented by intensity measurement I and geometric occupancy grid probability , the intensity observation value of the grid cell observed at the current time t is obtained by the following formula:

[0045] ;

[0046] wherein, is the intensity observation value of the grid cell the intensity observation value, if the intensity observation value is 0, it means that there is no reflection signal, i.e. the grid cell does not contain an obstacle object; is the total observation number of the grid cell ;

[0047] The geometric occupancy probability value of the grid cell observed at the current time t is obtained by the following formula:

[0048] ;

[0049] wherein, represents the geometric occupancy probability value of the grid cell observed at the current time t; p represents whether the grid cell is occupied; clamp() is an interval limiting function;

[0050] The geometric residual is used to estimate the best pose of the robot by matching the current fused point cloud data and the global map according to different classifications, wherein,

[0051] If the classification of the fused point cloud data is a linear feature point , search for the two nearest linear points and from the global map, the geometric residual of the linear feature point to the linear points and is defined as:

[0052] ; wherein,

[0053] is the current point, is the conversion point of the current point in the intensity environment map; If the classification of the fused point cloud data is a plane feature point

[0054] , search for the three nearest plane points , and from the global map, the geometric residual of the plane feature point to the three plane points , and is defined as: ;

[0055] If the classification of the fused point cloud data is a vertex feature point , the geometric residual of the vertex feature point

[0056] is defined as: ​​

[0057] ;

[0058] By minimizing the intensity residual between the current point and the transition point in the intensity-based environment map The feature is matched with the constructed intensity-based environment map by calculating the intensity residual between the feature and the transition point in the intensity-based environment map , which is:

[0059] ;

[0060] Finally, the geometric residual and the intensity residual are jointly optimized according to the following formula to obtain the optimal estimated pose of the mobile robot, which is:

[0061] .

[0062] In step 3 of the above method, real-time loop detection is performed on the intensity-based environment map in the following manner, which includes:

[0063] Step 1: Key frame election is performed to obtain a key frame, which is selected according to the following criteria: the displacement of the robot exceeds a preset distance of 20 meters, the rotation angle of the robot exceeds a preset angle of 0.67 radians, and the time taken by the displacement of the robot exceeds a preset time of 30 seconds;

[0064] Step 2: For each key frame, the scan point cloud data context is used to extract a global descriptor, a two-dimensional matrix is obtained through the global descriptor, the two-dimensional matrix is divided into sector cells and rings by equally dividing the polar coordinates in the azimuth angle and the radial angle, and the identity of each cell is obtained through geometric and intensity weighting;

[0065] Step 3: The similarity of the loop frame and the candidate frame is determined according to whether the identity difference between the loop frame and the candidate frame cell is less than a given threshold value 51, if it is less than the threshold value, it is determined that the loop is detected, and the constraint between the loop frame and the candidate frame is added to the back-end global optimization to correct the drift of the mobile robot.

[0066] The embodiment of the application further provides a processing device, which comprises:

[0067] at least one memory for storing one or more programs;

[0068] at least one processor capable of executing one or more programs stored in the memory, so that the processor can implement the above method when the one or more programs are executed by the processor.

[0069] The embodiment of the application further provides a readable storage medium storing a computer program, which can implement the above method when the computer program is executed by the processor.

[0070] In summary, the method of the embodiment of the present application can well reflect the characteristics of the environment around the robot for a structured environment with poor scene diversity but rich intensity information, and fusing the laser intensity and the point cloud features can reduce the robot drift problem. Moreover, the point cloud machine learning classification model in the method is only used for feature classification of the lidar point cloud, which can reduce the calculation amount and improve the front-end real-time performance. Meanwhile, after classification, the point cloud registration can be improved, and in the loop detection, the key frame election and scan context matching method are adopted, which reduces the calculation pressure and ensures the real-time positioning accuracy of the robot.

[0071] In order to more clearly show the technical solutions provided by the present application and the technical effects generated, the method of robot positioning and mapping provided by the embodiment of the present application is described in detail below.

[0072] Embodiment 1

[0073] As shown in Figure 1 , the embodiment of the present application provides a method of robot positioning and mapping by fusing laser intensity and point cloud geometric features. In the structured environment with poor scene diversity but rich intensity features, the method improves the point cloud registration, reduces the cumulative error of the robot, reduces the calculation speed of the loop detection, and improves the real-time positioning accuracy of the robot without other external sensors and only relying on the lidar.

[0074] In the embodiment of the present application, the mapping and positioning system receives the point cloud data returned by the lidar, realizes the autonomous pose estimation and environment map construction of the robot. The system as a whole is divided into three parts: 1) fuse the laser intensity information and the point cloud geometric feature information, and perform feature extraction; 2) register the geometric and intensity features of adjacent frames, and construct an intensity environment map; 3) perform loop detection and update the map and pose. The method of the present embodiment needs to use machine learning method to classify the point cloud according to the geometric information and intensity information of the laser point cloud, and the classified point cloud is used for point cloud registration and loop detection.

[0075] As shown in Figure 1 , Figure 2 , the technical roadmap of the method of the present application mainly includes: fusing the point cloud geometric features and the laser intensity features, and using machine learning method to perform feature extraction, robot pose estimation, and construct an intensity map and perform real-time loop detection.

[0076] Figure 3 A machine learning classification framework for point cloud is given, which divides the laser point cloud data into different feature categories through linear transformation. First, for the point cloud data of the lidar, project it onto the reference plane. Then divide the reference plane into a two-dimensional grid of equal size, and record the height of each grid Minimum height of and adjacent grids The data is then divided into non-Ground (NG) points and ground (G) points using a dual threshold filter. The specific division formula is as follows:

[0077] ;

[0078] For non-ground points, geometric and intensity information are fused to calculate the point and its spherical neighborhood. Information matrix:

[0079] ;

[0080] in It is the neighborhood The center of gravity The intensity is the average value of the neighborhood. Points with similar intensities to the feature point are assigned higher weights. Then, principal component analysis is used to obtain the corresponding eigenvalues ​​for each point. Finally, the local linearity, flatness, and curvature of the point are calculated, and the point cloud is divided into different planar points, linear points, and vertices accordingly.

[0081] The intensity map described in this embodiment of the invention is maintained and updated by occupying a grid. The three-dimensional space is divided into grid cells, each grid cell... The intensity measurement I and the geometric occupancy probability are jointly represented. For an observed mesh cell at time t, the intensity measurement is obtained using the following formula:

[0082] ;

[0083] in These are the current intensity observations. For cell mesh The total number of observations. If the grid does not contain an object, the intensity value is 0, indicating that there is no reflected signal.

[0084] For a time-t observation grid cell, its geometric occupancy probability can be obtained by:

[0085] ;

[0086] Where p indicates whether the grid is occupied, and clamp is the interval constraint function.

[0087] To estimate the robot's optimal pose, the geometric residual is calculated by matching the different feature point clouds of the current classification with the global map. This can be done by providing points... and its transformation points The corresponding feature points are searched and calculated from the global map.

[0088] For linear feature points, search for the two nearest linear points. and The residual from this point to the linear feature point is defined as:

[0089] ;

[0090] For a planar feature point, search for its three nearest planar points. , , The residual from this point to the feature point in the plane is defined as:

[0091] ;

[0092] The residual for vertex features is defined as follows:

[0093] ;

[0094] Simultaneously, the intensity residual is calculated by matching the features with the constructed intensity map, which can be achieved by minimizing the current point. Conversion points in the intensity map This is achieved through the strength residual between them:

[0095] ;

[0096] Finally, the optimal pose estimate is obtained by jointly optimizing the geometric residual and the strength residual using the following formula:

[0097] ;

[0098] In this embodiment of the invention, real-time loop closure detection reduces retrieval time by using keyframe election and scanning context. Keyframe selection is primarily based on the following criteria: robot displacement exceeds a preset distance, robot rotation angle exceeds a preset angle, and robot displacement time exceeds a preset time. The global descriptor derived from the scanning point cloud data context is a two-dimensional matrix (see...). Figure 4 The calculation is performed by dividing the polar coordinates of a two-dimensional matrix into sectors and loops in both azimuth and radial angles. The identifier of each cell is obtained through geometric and intensity weighting. Considering the increasing computational burden as time progresses and the map grows, a logical judgment is used to determine the similarity between loop-closed frames and candidate frames based on whether the identifier within a cell is 0. Once a loop is identified, constraints between the two frames can be incorporated into the optimization to correct robot drift and improve positioning accuracy.

[0099] Example 2

[0100] This embodiment is designed for a structured environment with limited scene diversity but rich intensity information. The LiDAR data is acquired by a 16-line LiDAR with an output frequency of 10Hz, a vertical field of view of [+15°, -15°], and a horizontal field of view of 360°; a vertical resolution of 2° and a horizontal resolution of 0.2°; and a minimum and maximum ranging range of 0.2 meters and 100 meters, respectively. It supports outputting the 3D coordinates and intensity information of each point. Based on a 3D simulation environment, a scene of a long, straight tunnel approximately 400 meters long is simulated, with reflective markers spaced 20 meters apart on both sides of the walls. During data acquisition, the robot's movement speed is maintained at 15–20 km / h, and a total of 1000 frames of point cloud data, including geometric and intensity information, are collected.

[0101] The point cloud machine learning classification model described herein uses a principal component analysis algorithm, which integrates laser point cloud intensity and geometric feature information, and classifies its output into ground points, planar points, linear points, and vertices according to features.

[0102] The algorithm used to jointly optimize the geometric and intensity residuals for estimating the optimal pose is the Levenberg-Marquardt optimization algorithm. The scanning context for loop closure detection is as follows: Figure 4 As shown, the determination of loop closure in the current frame is achieved by taking the average cosine value of each sector to obtain its similarity score. If the score exceeds the threshold, it is determined to be a loop closure, which is used to globally optimize and correct the pose and update the map.

[0103] In summary, the method of this invention, for structured environments with poor scene diversity but rich intensity information, allows the intensity of the laser point cloud to effectively reflect the characteristics of the robot's surrounding environment. Fusing laser intensity and point cloud features can reduce robot drift. Furthermore, the machine learning algorithm in this method is only used for feature classification of the LiDAR point cloud, and the loop closure detection employs keyframe election and scanning context matching, reducing computational pressure and ensuring the robot's real-time positioning accuracy. In addition, this method only requires the LiDAR sensor and does not need to be fused with other sensors. This reduces the challenge of external calibration of multiple sensors and lowers computational pressure. Moreover, due to the advantages of LiDAR, it is well-suited for various environments and is also suitable for the localization and mapping of robots in large-scale environments.

[0104] It can be understood that, by applying the method of the present invention, in environments with scarce geometric structures but abundant strength information, statistical methods can be used to extract more geometric spatial features (such as vertices, lines, cylinders, walls, etc.), while a degradation factor algorithm is used to evaluate the pose estimation degradation and determine the degradation direction.

[0105] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0106] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims. The information disclosed in the background section is intended only to enhance the understanding of the overall background technology of the present invention and should not be construed as an admission or implication in any way that such information constitutes prior art known to those skilled in the art.

Claims

1. A method of robot localization and mapping fusing laser intensity and point cloud geometric features, characterized in that, Comprise: Step 1, obtain the point cloud data of the mobile robot laser radar, fuse the point cloud geometry features and laser intensity of the point cloud data to obtain the fused point cloud data, and classify the fused point cloud data according to the features through the point cloud machine learning classification model; Classify the fused features according to the features through the point cloud machine learning classification model in the following way, comprising: Projecting the fused point cloud data of the mobile robot laser radar to a reference plane, dividing the reference plane into equal-sized two-dimensional grid points, recording the height of each two-dimensional grid point and the minimum height of the two-dimensional grid point adjacent to the two-dimensional grid point , and the two-dimensional grid points are divided into non-ground points and ground points by the following filtering formula: ； Wherein, the meanings of each parameter are as follows: represents a two-dimensional grid point; NG represents that the two-dimensional grid point is a non-ground point; G represents that the two-dimensional grid point is a ground point; h k represents the height of different point clouds in the same layer grid; δh1 represents the threshold for dividing the height of point clouds in the same grid; δh2 represents the threshold for dividing the height of point clouds in adjacent grids; Non-ground points of fused point cloud geometry features and laser intensity and their spherical neighborhoods The information matrix for: ； wherein, is the center of gravity of the neighborhood ; is the average laser intensity of the neighborhood , with points having similar laser intensity to the non-ground point being given higher weights; The point cloud machine learning classification model is processed by principal component analysis to obtain feature values of each non-ground point ; By getting the corresponding eigenvalue of each non-ground point The local linearity, planarity and curvature of each non-ground point are calculated, the local linearity = (λ1-λ2) / λ1, the planarity = (λ2-λ3) / λ1, and the curvature = λ3 / (λ1+λ2+λ3), and each non-ground point is classified into a linear point, a different planar point and a vertex according to the local linearity, the planarity and the curvature. Step 2, establish an intensity environment map according to the classified fused point cloud data of the point cloud geometry features and laser intensity, and jointly optimize the best estimated pose of the mobile robot according to the geometric residual and intensity residual obtained from the constructed intensity environment map; In the following way, an intensity environment map is established according to the classified fused point cloud data of the point cloud geometry features and laser intensity, and the best estimated pose of the mobile robot is obtained by jointly optimizing the geometric residual and intensity residual obtained from the constructed intensity environment map, comprising: The three-dimensional space is divided into grid cells, and each grid cell is jointly represented by the intensity measurement I and the geometric occupancy probability of the grid. The grid cell observed at the current time t The intensity observation values ​​are obtained using the following formula: ； wherein, is the intensity observation of the grid cell at the current time instant t, if the intensity observation is 0, it means that there is no reflected signal, i.e. the grid cell does not contain an obstacle object; is the total number of observations of the grid cell at the current time instant t. The geometric occupancy probability value of the grid unit observed at the current time t is obtained by the following formula: ； wherein, represents the geometric occupancy probability value of the grid cell observed at the current time t; p represents whether the grid cell is occupied or not; clamp() is an interval limiting function; The best pose of the robot is estimated by matching the current fused point cloud data classified in different ways with the global map to calculate the geometric residual, wherein, If the classification of the fused point cloud data is a linear feature point searching from the global map for a linear feature point two nearest linear points and the linear feature point to the linear point and the geometric residual is defined as: ； wherein, is the current feature point, is the current point a transition point in the intensity environment map; If the classification of the fused point cloud data is a planar feature point searching from the global map for a planar feature point closest to the planar feature point , and the geometric residual of the planar feature point to the three planar points , and is defined as: ； If the classification of the fused point cloud data is a vertex feature point , the geometric residual of the vertex feature point is defined as: ； by minimizing an intensity residual between the current point and a transition point in the band intensity environment map to match the feature to the constructed band intensity environment map , ； Finally, the best estimated pose of the mobile robot is obtained by jointly optimizing the geometric residual and intensity residual according to the following formula, which is: ； Step 3, real-time loop detection is performed on the intensity environment map, and the estimated pose of the mobile robot and the intensity environment map are updated according to the loop detection result.

2. The method of fusing laser intensity and point cloud geometric features for robot localization and mapping according to claim 1, wherein, In the step 3, the intensity environment map is detected in real time according to the following way, and the estimated pose of the mobile robot and the intensity environment map are updated according to the loop detection result, comprising: Step 1, key frame election is performed to obtain key frames, and the key frames are selected according to the following standards: the robot displacement exceeds the preset distance 20.0 meters, the rotation angle of the robot exceeds the preset angle 0.67 radian, and the time of the robot displacement exceeds the preset time 30 seconds; Step 2, for each key frame, the scanning point cloud data context is used to extract it as a global descriptor, a two-dimensional matrix is obtained through the global descriptor, each cell is calculated by dividing the polar coordinates into sector cells and rings in the azimuth and radial angles, and the identity of each cell is obtained through geometry and intensity weighting; Step 3, determine the similarity of the loop frame and the candidate frame according to whether the identity difference of the loop frame and the candidate frame cell is less than a given threshold value 51, if less than the threshold value, determine the loop, and add the constraint between the loop frame and the candidate frame to the back-end global optimization to correct the drift of the mobile robot.

Images