Robot positioning method based on point cloud ground segmentation and icp registration

Abstract

The application relates to the field of autonomous navigation, and discloses a robot positioning method based on point cloud ground segmentation and ICP registration. The robot positioning method based on point cloud ground segmentation and ICP registration first acquires original point cloud data and performs spherical projection to generate a depth image; then the depth image is traversed column by column, an inclination angle is calculated according to the spatial geometric relationship of adjacent pixels, and a ground point set and a non-ground point set are divided according to the inclination angle; then a local smoothness parameter is calculated, edge feature points are extracted from the non-ground point set according to the smoothness parameter, and plane feature points are extracted from the ground point set and the non-ground point set; finally, a line and plane constraint is constructed in a reference target, a pose transformation matrix is solved by using a nonlinear optimization algorithm, and a global pose is updated. Through dimension reduction processing and classification feature extraction, the application reduces the calculation complexity and improves the positioning accuracy of the robot in an unstructured environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology, specifically a robot localization method based on point cloud ground segmentation and ICP registration. Background Technology

[0002] LiDAR SLAM technology is one of the key technologies for mobile robots to achieve autonomous navigation. In unstructured environments, LiDAR odometry usually uses the iterative nearest point algorithm or its variants based on feature points to estimate the robot's pose changes. This type of method usually includes core steps such as point cloud preprocessing, feature extraction, data association, and pose solving.

[0003] However, existing laser point cloud localization methods still have several technical problems in practical applications. Existing laser radar point cloud processing methods usually perform calculations directly on massive unordered point clouds in three-dimensional space, such as constructing spatial index structures like KD-Tree or octree to assist in nearest neighbor search. The construction and maintenance of such three-dimensional spatial indexes require a lot of memory and computing resources, while the computing power of the onboard computing unit of mobile robots is often limited, resulting in long data processing time and difficulty in meeting the real-time requirements of mobile robots in high-speed motion scenarios.

[0004] In the feature extraction stage of unstructured environments, existing technologies typically use a uniform smoothness calculation standard for the entire point cloud without effectively distinguishing between ground and non-ground areas. This can lead to vegetation, gravel, or irregular textures on the ground being misidentified as edge feature points due to their large local curvature. Once these unstable feature points from the ground participate in the subsequent point cloud registration stage, they will introduce incorrect geometric constraints and reduce the robustness of the localization algorithm in complex environments.

[0005] Furthermore, in the pose determination stage, existing registration algorithms mainly rely on minimizing the distance between points or between points and features. However, they lack effective verification of geometric degradation when searching for correspondences. For example, the set of reference points used to construct planar constraints may be in a state of near collinearity, or the set of points used to construct linear constraints may come from the same laser scanning beam. If these invalid constraints directly participate in the nonlinear optimization solution, it can easily lead to the deterioration of the behavior of the pose optimization objective function, causing the algorithm to get stuck in local minima or slow down the convergence speed, thereby affecting the final positioning accuracy of the mobile robot. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a robot localization method based on point cloud ground segmentation and ICP registration, which solves the problems of low localization accuracy and high computational resource consumption of mobile robots in unstructured environments.

[0007] To achieve the above objectives, this invention provides a robot localization method based on point cloud ground segmentation and ICP registration. This method is applied to a mobile robot comprising a mobile chassis, a lidar sensor, and an onboard computing unit. The method is executed by the onboard computing unit and includes the following steps: First, the raw point cloud data collected at the current moment is acquired through the lidar sensor, and spherical projection processing is performed on the raw point cloud data. This step converts the 3D point cloud into a 2D depth image containing distance information. Specifically, this includes: for any lidar point in the raw point cloud data, calculating the Euclidean distance from the lidar point to the center of the lidar sensor; calculating the vertical azimuth angle of the lidar point, and calculating the horizontal azimuth angle using a two-parameter arctangent function; establishing a mapping relationship between the vertical azimuth angle and the depth image row index, and the horizontal azimuth angle and the depth image column index, based on the vertical and horizontal resolutions of the lidar sensor, and assigning the Euclidean distance value to the corresponding pixel position; if multiple lidar points are mapped to the same pixel position, the point with the smallest Euclidean distance value is retained.

[0008] After generating the depth image, the method performs ground segmentation based on spatial geometric relationships. Specifically, it traverses the depth image column by column, selects a first point and a second point located in the same column with adjacent row indices, and reconstructs them into three-dimensional spatial coordinates based on the intrinsic parameters of the LiDAR sensor; calculates the radial distance difference between the first point and the second point in the horizontal plane, and the vertical height difference in the vertical direction; calculates the arctangent of the ratio of the vertical height difference to the radial distance difference, obtaining the tilt angle of the line connecting the first point and the second point relative to the horizontal plane; compares this tilt angle with a preset slope threshold, and divides the pixels in the depth image into a set of ground points and a set of non-ground points based on the comparison result. Furthermore, the method includes performing a breadth-first search on the pixels marked as ground, clustering adjacent ground pixels into connected components, and removing connected components with fewer than a preset clustering threshold.

[0009] Based on the ground segmentation results, the method performs feature extraction. First, based on the spatial location information of each pixel and its neighboring pixels in the depth image, a local smoothness parameter is calculated for each pixel. The calculation process includes: selecting the left and right neighboring points of the current pixel within the same row of the depth image; calculating the sum of the spatial position deviation vectors of each point in the neighborhood relative to the current pixel; obtaining the Euclidean norm of this vector sum; and normalizing the Euclidean norm using the product of the number of neighboring points and the distance to the current point. This parameter characterizes the curvature of the local surface.

[0010] After calculating the local smoothness parameters, the method performs feature point filtering: extracting the top few points with the largest local smoothness parameters from the non-ground point set as edge feature points; and extracting the top few points with the smallest local smoothness parameters from both the ground point set and the non-ground point set as planar feature points. During the extraction process, feature point stability verification is also performed, removing occluded points at depth steps and parallel points with an angle less than a preset threshold to the laser ray, and marking occupied neighboring pixels using a non-maximum suppression strategy.

[0011] Finally, the method utilizes the extracted feature points to perform pose solving. A reference target is obtained, and linear constraints corresponding to edge feature points and planar constraints corresponding to planar feature points are searched within the reference target. Point-to-line distance residuals are constructed based on linear constraints, and point-to-plane distance residuals are constructed based on planar constraints. The weighted sum of the squared residuals of all valid matching feature points is determined as the pose optimization objective function. The Levenberg-Marquardt algorithm is used to iteratively solve this pose optimization objective function. An incremental equation is constructed to obtain the pose increment and update the pose estimate until the pose increment converges, obtaining the pose transformation matrix at the current time. The onboard computing unit uses this pose transformation matrix to update the global pose of the mobile robot in the global map.

[0012] This invention provides a robot localization method based on point cloud ground segmentation and ICP registration. It has the following beneficial effects: 1. This invention uses spherical projection to convert 3D point cloud data into 2D depth images, and performs ground segmentation and curvature calculation based on the spatial neighborhood relationships of image pixels. Compared to methods that directly construct KD-Tree or octree indexes in 3D space, this technical solution avoids the high computational complexity of large-scale 3D nearest neighbor search through dimensionality reduction processing. At the same time, by pre-removing a large amount of ground point data before feature extraction, it reduces the various computational loads of subsequent feature traversal and solution, lowers the processor load of the onboard computing unit, and meets the real-time positioning requirements of mobile robots under computing power constraints.

[0013] 2. This invention employs a classification feature extraction strategy that combines ground segmentation results with edge feature points, limiting their selection to only non-ground point sets, while planar feature points can be selected from both ground and non-ground point sets. This strategy utilizes the geometric properties of ground points to physically isolate weeds, road textures, or noise from the ground as misidentified edge features during the feature extraction stage, eliminating interference from unstable features in pose calculation. By improving the quality of the feature point cloud used in registration, the stability of the point-to-line geometric constraints is ensured, enhancing the robustness of the localization algorithm in unstructured environments.

[0014] 3. This invention introduces a specific geometric constraint validity verification mechanism in the pose estimation stage. Before constructing the residual equation, the differences in the line bundle indices of the line matching points and the vector outer product magnitude of the plane matching points are verified to effectively identify and eliminate collinear, coplanar, or degenerate invalid matching point pairs. The Levenberg-Marquardt nonlinear optimization algorithm is used to iteratively solve the joint residuals from points to lines and from points to planes, ensuring the convexity and convergence direction of the objective function and preventing the algorithm from getting trapped in local minima, thereby improving the accuracy and convergence speed of mobile robot pose estimation. Attached Figure Description

[0015] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0016] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] Example: Please see the appendix Figure 1 This invention provides a robot localization method based on point cloud ground segmentation and ICP registration, which is executed on a mobile robot. The mobile robot mainly includes a mobile chassis, a lidar sensor rigidly connected to the top of the mobile chassis, and an onboard computing unit communicatively connected to the mobile chassis and the lidar sensor. The onboard computing unit is used to execute the localization method, which specifically includes the following steps: Step S1: The raw point cloud data collected at the current moment is acquired through the lidar sensor. The raw point cloud data consists of the coordinates of laser points distributed in a disordered manner in three-dimensional space. The airborne computing unit performs spherical projection processing on the raw point cloud data to map the three-dimensional data into a two-dimensional depth image. The pixel value in the depth image represents the Euclidean distance from the corresponding laser point to the center of the lidar sensor.

[0018] Step S2: The depth image is traversed column by column. The airborne computing unit calculates the geometric positional relationship between two adjacent pixels in the same column of the depth image, determines the tilt angle of the line connecting the adjacent points relative to the horizontal plane, compares the calculated tilt angle with a preset slope threshold, and marks the points in the depth image with attributes based on the comparison results, thereby dividing the point cloud into a set of ground points and a set of non-ground points.

[0019] Step S3: Based on the neighborhood distribution information of the depth image, calculate the local curvature value for each point in the ground point set and the non-ground point set, and perform feature classification and filtering on the points according to the magnitude of the local curvature value; specifically, this includes: extracting points with local curvature values ​​greater than a first preset value from the non-ground point set as edge feature points, and extracting points with local curvature values ​​less than a second preset value from the ground point set and the non-ground point set as planar feature points.

[0020] Step S4: Obtain the point cloud data or local map of the previous frame as a reference target. Search the reference target for the nearest neighboring point set to the edge feature point in terms of spatial distance to construct a straight line constraint, and search the reference target for the nearest neighboring point set to the plane feature point in terms of spatial distance to construct a plane constraint. Based on the straight line constraint, construct the point-to-line distance residual, and based on the plane constraint, construct the point-to-plane distance residual. Establish a pose optimization objective function that includes the point-to-line distance residual and the point-to-plane distance residual, and use a nonlinear optimization algorithm to iteratively solve the pose optimization objective function to obtain the pose transformation matrix of the mobile robot relative to the reference target at the current moment.

[0021] The process of performing spherical projection processing on the original point cloud data as described in step S1 is specifically implemented by the airborne computing unit by executing the following sub-steps S101 to S103: Step S101: Establish data model and initialize depth image: The airborne computing unit defines the coordinate system of the LiDAR sensor, for example, with the sensor center as the origin, the robot's direction of travel as the positive X-axis, and the vertical upward direction as the positive Z-axis. Define the current timestamp. The raw point cloud data below is a collection ,in Meanwhile, based on the number of vertical beams of the lidar sensor Number of sampling points per horizontal loop , construct a resolution of The two-dimensional matrix is ​​used as a depth image. Before projection begins, all elements in the matrix are initialized to preset invalid values ​​to identify spatial regions where no echo signal was detected.

[0022] Step S102: Calculate the spherical projection parameters of the laser point: For the set any laser point in The onboard computing unit calculates the Euclidean distance from the point to the origin. Vertical azimuth and horizontal azimuth To ensure accurate angle mapping across the entire 360-degree range, the horizontal azimuth angle... The calculation is performed using the two-parameter arctangent function (atan2). The specific calculation model is as follows:

[0023]

[0024]

[0025] Step S103: Perform discretization mapping and pixel assignment: The airborne computing unit performs discretization mapping and pixel assignment based on the vertical field of view parameters of the lidar sensor. and the horizontal field of view range, the calculated continuous angle values Quantization mapping to discrete row and column indices of depth images .

[0026] row index Corresponding to the distribution of the vertical scanning beam, assuming the laser beam is uniformly distributed in the vertical direction, the calculation formula is:

[0027] Column Index The calculation formula corresponding to the distribution of horizontal rotation angles is:

[0028] in, This indicates the rounding operation. and To ensure that the index value falls within the matrix Within the valid index range The onboard computing unit will calculate the Euclidean distance. Assigning values ​​to a matrix The median coordinate is The pixel element, i.e. Mapping multiple laser points to the same pixel coordinate In this case, the airborne computing unit compares the Euclidean distances of each point and retains the one with the smallest value. This is to ensure that the depth image can represent the geometric contours of the foremost obstacles in the environment.

[0029] The process of performing column-by-column traversal processing on the depth image and dividing it into ground point sets and non-ground point sets, as described in step S2, is specifically implemented by the airborne computing unit by executing the following sub-steps S201 to S204: Step S201, Traversal and Validation of Depth Image Data: The onboard computing unit performs column-by-column scanning of the depth image; for any column in the depth image... (in (Column index), row index Select two adjacent pixels as the difference object, and define the current traversal point as... ,coordinate Define the neighboring point with the smaller row index as ,coordinate Airborne computing unit reading point With point pixel values and If any pixel value is an invalid value set in step S1, the current point pair is determined to be unable to form a valid constraint, and the process jumps directly to the next loop; if both points are valid measurement points, the subsequent geometric parameter calculation is performed.

[0030] Step S202: Calculate the spatial geometric difference components of adjacent points: The airborne computing unit, based on the installation height and intrinsic parameters of the lidar sensor, calculates the spatial geometric difference components of adjacent points. With point Data in depth images can be reconstructed into three-dimensional coordinates in the sensor coordinate system. and Given the point With point Located in the same column of the depth image, and having the same or approximately the same horizontal azimuth angle, the airborne computing unit calculates the radial distance difference between the two points on the XY horizontal plane. and the vertical height difference in the Z-axis direction .

[0031] Radial distance difference This represents the change in the projected distance between two points on the horizontal plane, and the calculation formula is as follows:

[0032] Vertical height difference The formula for representing the change in vertical height between two points is as follows:

[0033] Step S203: Calculate the neighborhood tilt angle and generate initial markers: The airborne computing unit 130 constructs the tilt angle of the line connecting adjacent points relative to the horizontal plane based on the radial distance difference and the vertical height difference. The tilt angle The formula used to quantify the flatness of the current laser scanning area is as follows:

[0034] The onboard computing unit 130 reads the preset ground slope threshold. The calculated tilt angle and Perform numerical comparison: -like Judgment point With point The surfaces between them are smooth and nearly horizontal, making the points With point Mark as potential ground points; -like Judgment point There are high mutations between them, which will be the point Mark as a non-ground point.

[0035] Step S204: Ground point filtering based on connected component clustering: To eliminate isolated mislabeled points caused by measurement noise, the airborne computing unit performs a breadth-first search on all pixels marked as potential ground points. The airborne computing unit groups adjacent pixels in the depth image that are all marked as potential ground points into a connected component and counts the number of pixels contained in each connected component. If the number of pixels in a connected component is less than a preset clustering threshold, the connected component is determined to be a noise point set and reset as a non-ground point; if the number of pixels is greater than or equal to the clustering threshold, the ground attribute of the connected component is retained. After filtering, all points with retained ground attributes constitute the ground point set, and the remaining valid points constitute the non-ground point set.

[0036] In this invention, the process of calculating local curvature values ​​and performing feature classification and filtering in step S3 is specifically implemented by the airborne computing unit by executing the following sub-steps S301 to S303: Step S301: Constructing a neighborhood and calculating local smoothness parameters: The airborne computing unit traverses every valid pixel in the depth image, defining the current point to be processed as... Its spatial coordinates are The airborne computing unit selects points within the same row of the depth image. Left and right sides A series of valid points constitute a set of points. The neighborhood set of each point Airborne computing units are based on neighborhood sets. Points within and points Spatial position deviation, calculation point Smoothness parameter This parameter The following formula is used to characterize the curvature features of a local surface:

[0037] in, This represents the number of points within the neighborhood set. For the neighboring region The coordinate vector of a point, This represents the Euclidean norm.

[0038] Step S302: Perform stability verification and invalid point removal for feature points: To ensure the robustness of feature points, the airborne computing unit 130 removes two types of unstable points based on geometric relationships before screening: The first category is obstructed points: 130 computing points of the onboard computing unit. and adjacent points Euclidean distance and points and adjacent points Euclidean distance ,like The value is greater than the preset fracture threshold, and the point Distance value Distance value Then the decision point For occluded edge points, mark them as unselectable; conversely, mark unselectable points as unselectable.

[0039] The second category is parallel points: 130 calculation points of the airborne computing unit. If the vector connecting the points in the neighborhood makes an angle less than a preset angle threshold with the laser beam, it indicates that the laser beam is approximately parallel to the object surface, resulting in significant ranging noise. The airborne computing unit 130 will then assign this point to the specified location. Marked as unselectable.

[0040] Step S303: Non-maximum suppression and feature extraction based on sub-region partitioning: The airborne computing unit divides the depth image into equal parts in the horizontal direction. The system is divided into sub-regions, and filtering is performed independently within each sub-region. The onboard computing unit uses smoothness parameters... The available points within the sub-region are sorted and extracted based on the classification results output in step S2: For edge feature points: the airborne computing unit searches only within the set of non-ground points and selects the smoothness parameter. Greater than the edge threshold And the largest value in the front These points constitute the set of edge feature points for the current frame.

[0041] For planar feature points: the airborne computing unit searches only within the ground point set and the non-ground point set, selecting the smoothness parameter. Less than the plane threshold And the smallest value is the first These points constitute the set of planar feature points for the current frame.

[0042] During the selection process, the onboard computing unit executes a nonmaximum suppression strategy: when a point is selected as a feature point, its neighborhood set is... The remaining points within the area are marked as occupied, preventing them from being selected again, to ensure the spatial discreteness of the feature points. The finally extracted edge and planar feature points are used as input data for constructing pose constraints.

[0043] The process of constructing the distance residual and solving the pose using a nonlinear optimization algorithm, as described in step S4, is specifically implemented by the airborne computing unit by executing the following sub-steps S401 to S404: Step S401: Perform pose prediction and feature point coordinate transformation: The airborne computing unit generates the current time step based on the kinematic model of the mobile robot or the integral data of the inertial measurement unit. Compared to the previous moment The pose prediction value. The pose transformation matrix to be solved is defined as... The airborne computing unit uses this pose prediction value to extract the current frame edge feature points (denoted as set) from step S3. ) and planar feature points (denoted as set) Transform from the current radar coordinate system to the reference coordinate system of the reference target. For any point in the feature point set... Its transformed reference coordinates The calculation formula is:

[0044] Step S402, Nearest Neighbor Search and Geometric Constraint Verification of the Reference Point Set: The airborne computing unit performs the transformation of the feature points in the KD-Tree structure constructed from the reference target. Search for geometric corresponding points: For edge feature points The onboard computing unit searches for the two points in the reference target that are closest in Euclidean distance. and Onboard computing unit verification and The harness index, only when and When the laser scanning beams originate from different laser beams and the distance between the two points is less than a preset correlation threshold, the following judgment is made. This constitutes a valid linear constraint.

[0045] For planar feature points The onboard computing unit searches for the three points in the reference target that are closest in Euclidean distance. , and The airborne computing unit calculates vectors. with vector The outer product modulus; if the modulus is greater than the preset area threshold (e.g., 0.2), it indicates that the three points are not collinear and can form a stable plane, and it is determined that they constitute a valid plane constraint; otherwise, it is considered an invalid match and is rejected.

[0046] Step S403: Constructing the geometric distance residual equation: The airborne computing unit constructs residual constraints based on the effective matching results. For edge feature points, construct the distance residual $d_E$ from the point to the line, calculated using the following formula:

[0047] For planar feature points, construct the distance residual from the point to the plane. The onboard computing unit first calculates the normal vector of the reference plane through the outer product of vectors. Then, the residuals are calculated:

[0048]

[0049] Step S404: Construct a nonlinear least squares objective function and solve iteratively: The airborne computing unit uses the sum of squared residuals of all valid matching feature points as the objective function for pose optimization.

[0050]

[0051] in, The weighting coefficients are used. The airborne computing unit employs the Levenberg-Marquardt algorithm to solve this nonlinear problem. The airborne computing unit performs a linearized Taylor expansion of the objective function with respect to the pose Lie algebra increment and calculates the Jacobian matrix. And construct incremental equations:

[0052] in, This is the damping factor used to adjust the weights of gradient descent and Gauss-Newton method. This is the current residual vector. The airborne computing unit solves this equation to obtain the pose increment. Update the current pose using Lie algebra exponential mapping. This iterative process continues until...

[0053] After obtaining the converged pose transformation matrix 308, the onboard computing unit performs a global localization state update. Specifically, the onboard computing unit stores the mobile robot's state at the previous moment. Global pose in global map coordinate system The airborne computing unit operates based on the following formula:

[0054] Perform pose accumulation calculations to determine the current time step. global pose This global pose The final output of this localization method includes the mobile robot's three-dimensional coordinates and orientation angle on the global map. The onboard computing unit processes this global pose... The data is transmitted to the navigation control module as real-time feedback for path planning and motion closed-loop control, thereby enabling the mobile robot to autonomously locate itself in unstructured environments.

[0055] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A robot localization method based on point cloud ground segmentation and ICP registration, characterized in that, The method is applied to a mobile robot, which includes a mobile chassis, a lidar sensor rigidly connected to the top of the mobile chassis, and an onboard computing unit communicatively connected to the mobile chassis and the lidar sensor. The method is executed by the onboard computing unit and includes the following steps: Step S1: Acquire the raw point cloud data collected at the current moment through the lidar sensor, and perform spherical projection processing on the raw point cloud data to generate a depth image containing distance information; Step S2: Traverse the depth image column by column, calculate the tilt angle based on the spatial geometric relationship between two adjacent pixels in the same column, compare the tilt angle with a preset slope threshold, and divide the pixels in the depth image into a ground point set and a non-ground point set based on the comparison result. Step S3: Based on the spatial location information of each pixel and its neighboring pixels in the depth image, calculate the local smoothness parameter of each pixel; filter the pixels according to the local smoothness parameter, extract edge feature points from the non-ground point set, and extract planar feature points from the ground point set and the non-ground point set; Step S4: Obtain a reference target; search for the set of points in the reference target that are geometrically closest to the edge feature points to construct a linear constraint; and search for the set of points that are geometrically closest to the planar feature points to construct a planar constraint; construct a pose optimization objective function based on the linear constraint and the planar constraint; iteratively solve the pose optimization objective function using a nonlinear optimization algorithm to obtain the pose transformation matrix of the mobile robot at the current time; and update the global pose of the mobile robot in the global map using the pose transformation matrix.

2. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, Step S1 involves performing spherical projection processing on the original point cloud data to generate a depth image containing distance information, specifically including: For any laser point in the original point cloud data, calculate the Euclidean distance from the laser point to the center of the lidar sensor; Calculate the vertical azimuth angle of the laser point, and calculate the horizontal azimuth angle of the laser point using the two-parameter arctangent function; The row index of the depth image is calculated based on the vertical azimuth angle and the vertical resolution of the lidar sensor, and the column index of the depth image is calculated based on the horizontal azimuth angle and the horizontal resolution of the lidar sensor. The Euclidean distance is assigned to the pixel position in the depth image determined by the row index and the column index; if multiple laser points are mapped to the same pixel position, the point with the smallest Euclidean distance value is retained.

3. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, Step S2, which calculates the tilt angle based on the spatial geometric relationship between two adjacent pixels in the same column, specifically includes: Select a first point and a second point in the depth image that are located in the same column and have adjacent row indices, and restore the first point and the second point to three-dimensional spatial coordinates based on the intrinsic parameters of the lidar sensor; Taking advantage of the fact that the first point and the second point have approximately the same horizontal azimuth angle, the radial distance difference between the first point and the second point on the horizontal plane and the vertical height difference in the vertical direction are calculated. Calculate the arctangent of the ratio of the vertical height difference to the radial distance difference to obtain the tilt angle of the line connecting the first point and the second point relative to the horizontal plane.

4. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, After step S2, which divides the pixels in the depth image into a set of ground points and a set of non-ground points based on the comparison results, the method further includes: Perform a breadth-first search on the pixels marked as ground, and cluster adjacent pixels that are both marked as ground into connected components; Count the number of pixels contained in each of the connected components; If the number of pixels is less than a preset clustering threshold, the connected component is determined to be a noise set and reset to a non-ground point; if the number of pixels is greater than or equal to the preset clustering threshold, the ground attribute of the connected component is retained.

5. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, The calculation of the local smoothness parameters for each pixel in step S3 specifically includes: Within the same row of the depth image, a predetermined number of neighboring points to the left and right of the current pixel are selected to form a neighborhood set; Calculate the Euclidean norm of the sum of the spatial position deviation vectors of each neighboring point in the neighborhood set relative to the current pixel; The local smoothness parameter is obtained by dividing the Euclidean norm by the product of the number of points in the neighborhood set and the distance value of the current pixel.

6. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, Before step S3, which extracts edge feature points and planar feature points, the method further includes performing feature point stability verification: Calculate the Euclidean distance between the current pixel and its left and right adjacent pixels. If the Euclidean distance is greater than a preset breakage threshold and there is a depth step, then the current pixel is determined to be an occluded point and is removed. Calculate the vector connecting the current pixel to its neighboring pixels. If the angle between this vector and the laser ray is less than a preset angle threshold, then the current pixel is determined to be a parallel point and is discarded.

7. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, Step S3, which involves extracting edge feature points from the non-ground point set and extracting planar feature points from both the ground point set and the non-ground point set, specifically includes: The depth image is divided into multiple sub-regions in the horizontal direction; Within each sub-region, only the top N points from the set of non-ground points whose local smoothness parameter is greater than the edge threshold and whose value is the largest are selected as the edge feature points. Within each sub-region, the top M points with the smallest local smoothness parameter (which is less than the plane threshold) are selected from the set of ground points and the set of non-ground points as the plane feature points. During the selection process, once a pixel is selected as a feature point, the remaining pixels in its neighborhood set are marked as occupied, preventing it from being selected again.

8. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, Step S4, which involves searching the set of points in the reference target that are geometrically closest to the edge feature points to construct a straight-line constraint, and searching the set of points that are geometrically closest to the planar feature points to construct a planar constraint, specifically includes: For the edge feature points, search for the two points with the closest Euclidean distance in the reference target, verify the line bundle index of the two points, and determine that the line constraint is valid only when the two points come from different laser scanning line bundles and the distance between them is less than a preset association threshold. For the planar feature points, search for the three points with the closest Euclidean distance in the reference target, calculate the magnitude of the cross product of the vectors formed by these three points, and determine that the planar constraint is valid only when the magnitude of the cross product is greater than a preset area threshold.

9. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, Step S4, which involves constructing a pose optimization objective function based on the linear and planar constraints, specifically includes: For the edge feature points, construct the point-to-line distance residual, which represents the perpendicular distance from the edge feature point to the line determined by the corresponding two points; For the planar feature point, construct the point-to-plane distance residual, which represents the normal distance from the planar feature point to the plane determined by the corresponding three points; The weighted sum of the squared differences between the point-to-line distance residuals and the point-to-plane distance residuals of all valid matching feature points is determined as the pose optimization objective function.

10. The robot localization method based on point cloud ground segmentation and ICP registration according to claim 1, characterized in that, In step S4, a nonlinear optimization algorithm is used to iteratively solve the pose optimization objective function to obtain the pose transformation matrix of the mobile robot at the current moment, and the global pose of the mobile robot in the global map is updated using the pose transformation matrix. Specifically, this includes: A linearized Taylor expansion of the pose optimization objective function with respect to the pose Lie algebra increment is performed, the Jacobian matrix is ​​calculated, and an incremental equation containing the damping factor is constructed. The incremental equation is solved using the Levenberg-Marquardt algorithm to obtain the pose increment, and the pose estimate is updated using the exponential mapping until the pose increment converges, and the pose transformation matrix is ​​output. The global pose of the mobile robot at the previous moment is read, and the global pose at the previous moment is accumulated with the pose transformation matrix to calculate the global pose at the current moment.

Images