A robust and interference-resistant 3D point cloud localization method and system

Abstract

This invention discloses a robust and interference-resistant 3D point cloud localization method and system, belonging to the field of robot navigation technology. The method includes: projecting the original point cloud of a 3D LiDAR onto a virtual horizontal coordinate system using attitude data from an inertial measurement unit (IMU) to eliminate the influence of uneven road surfaces; adaptively selecting and fusing the results of the LiDAR and wheel speed measurement units based on the data quality of the pose estimation units such as wheel speedometers to generate reliable pose priors; and performing high-precision pose optimization using the geometric information contained in the local normal vectors during point cloud matching. This invention also includes a fast relocalization mechanism. This invention can significantly improve the robustness and accuracy of robot localization in complex environments, ensuring continuous and safe operation, and is mainly used in autonomous mobile platforms such as wheeled inspection robots.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation technology, and more specifically, relates to a robust and interference-resistant 3D point cloud positioning method and system. Background Technology

[0002] Wheeled inspection robots are increasingly widely used in fields such as power, chemical, and security, and their autonomous navigation and precise positioning are crucial for task execution. Currently, 3D LiDAR (LiDAR) has become the mainstream localization and mapping sensor due to its ability to acquire rich three-dimensional spatial information of the environment. Compared to 2D LiDAR, 3D LiDAR can construct 3D point cloud maps containing more geometric features, thus theoretically achieving more robust and accurate robot localization.

[0003] However, existing 3D point cloud-based localization algorithms still have shortcomings in terms of stability and anti-interference capabilities. A prominent problem is that when wheeled robots travel on bumpy roads, slopes, or any non-level ground, their vehicle posture undergoes pitch and roll changes. Since 3D LiDAR is usually fixed to the vehicle body, the tilt of the vehicle body will cause the raw point cloud data scanned by the radar to tilt as a whole. If the localization algorithm does not effectively handle this tilt and continues to use matching logic based on the horizontal assumption, the matching degree between the current frame's point cloud and the map will drop sharply, easily leading to matching failures and ultimately resulting in lost localization.

[0004] Once positioning is lost, the robot will be unable to continue its navigation tasks, potentially leading to serious consequences such as overturning on a slope due to loss of pose control, or colliding with surrounding buildings or equipment, causing safety accidents and property damage. Furthermore, existing positioning algorithms often suffer from inefficient relocalization processes after positioning loss, requiring lengthy recovery times, which impacts the robot's operational continuity and efficiency. Therefore, solving the positioning stability problem of robots in non-horizontal scenarios and achieving fast and accurate relocalization is a key technical challenge that urgently needs to be addressed in this field. Summary of the Invention

[0005] Purpose of the invention: The purpose of this invention is to address the shortcomings of existing technologies by providing a robust and interference-resistant 3D point cloud positioning method and system to improve the positioning robustness and reliability of robots in complex scenarios such as non-horizontal and bumpy conditions.

[0006] Technical Solution: The robust and interference-resistant 3D point cloud localization method described in this invention is applied to a mobile robot equipped with a 3D LiDAR, a pose estimation unit, and an attitude measurement unit. The method includes the following steps:

[0007] S1: Acquire the original point cloud data of the 3D LiDAR, the displacement data of the pose estimation unit, and the attitude data of the attitude measurement unit; and use the attitude data to project and transform the original point cloud data to a virtual horizontal coordinate system to generate a horizontal projection point cloud for matching.

[0008] S2: Based on the validity of the displacement data and attitude data, adaptively determine the final pose prior for point cloud matching;

[0009] S3: Based on the selected final pose prior and the horizontal projected point cloud, perform a point cloud matching process to calculate and optimize the robot's current pose, wherein the point cloud matching process utilizes the local normal vector space geometric information of the point cloud for data association and pose calculation.

[0010] To further improve the above technical solution, the pose estimation unit is a wheel speed meter, and the attitude measurement unit is an inertial measurement unit.

[0011] Furthermore, the validity of the wheel speed meter is determined by the following method: the relative displacement is calculated based on the readings of two consecutive timestamps, and the data is determined to be invalid when the linear displacement of the relative displacement is greater than a preset linear displacement threshold or the angular displacement is greater than a preset angular displacement threshold.

[0012] Further, when the data from the wheel velocity meter is determined to be valid, the step of determining the final pose prior includes: using the relative displacement provided by the wheel velocity meter as the initial prior; calculating the inter-frame matched relative displacement by performing inter-frame point cloud matching on two consecutive frames of the horizontally projected point cloud; evaluating the deviation transformation between the inter-frame matched relative displacement and the relative displacement of the wheel velocity meter, and separating its translational deviation and rotational deviation; if the translational deviation is less than a first preset translation threshold and the rotational deviation is less than a first preset rotation threshold, then the inter-frame matched relative displacement is selected as the final pose prior; otherwise, the wheel velocity meter relative displacement is selected as the final pose prior.

[0013] Furthermore, when the data from the wheel speed meter is determined to be invalid, inter-frame point cloud matching is performed on two consecutive frames of the horizontally projected point cloud to directly calculate the relative displacement without the wheel speed meter data as an initial prior, and the calculated relative displacement is used as the final pose prior.

[0014] Furthermore, it also includes a location loss detection step, which calculates the average nearest neighbor distance between matching pairs of the current frame point cloud and the global point cloud map. When the distance is greater than a preset matching threshold, it is determined that the location has been lost and the relocation mode is activated.

[0015] Furthermore, in the relocalization mode, an initial pose prior is obtained by receiving an externally provided heuristic pose or by laser template matching, and step S3 is executed based on the initial pose prior; wherein, the linear displacement deviation and angular displacement deviation between the initial pose prior and the robot's actual ground pose are both less than their respective preset ranges.

[0016] Furthermore, the point cloud projection transformation in step S1 is based on the extrinsic parameters between the 3D LiDAR and the attitude measurement unit, and is implemented according to the following formula:

[0017] ;

[0018] in, Let be the rotation matrix of the radar coordinate system relative to the world coordinate system. Let be the rotation matrix of the virtual horizontal radar coordinate system relative to the world coordinate system. This is the spatial rotation matrix used to perform the projection transformation.

[0019] Furthermore, the validity of the pose estimation unit data is determined by the following method: the relative displacement is calculated based on the readings of two consecutive timestamps. When the linear displacement of the relative displacement is greater than a preset linear displacement threshold or the angular displacement is greater than a preset angular displacement threshold, the data is determined to be invalid.

[0020] Furthermore, it also includes a pose publishing step, which generates and publishes a smooth pose trajectory with a frequency higher than the calculation frequency of the point cloud matching process by linearly interpolating the robot poses at multiple discrete moments.

[0021] Accordingly, the present invention also provides a system for implementing the above method, comprising a 3D LiDAR, a pose estimation unit, an attitude measurement unit, and at least one processor, wherein the processor is configured to include:

[0022] The point cloud preprocessing module uses the attitude data obtained from the attitude measurement unit to perform coordinate transformation on the raw point cloud data obtained from the 3D LiDAR, so as to generate a horizontal projection point cloud that can compensate for the robot's attitude tilt.

[0023] The motion tracking processing module is configured to perform the following operations: when the data from the pose estimation unit is determined to be valid, the valid displacement data is used as the initial prior; inter-frame point cloud matching is performed on two consecutive frames of the horizontally projected point cloud to calculate the inter-frame matching relative displacement; the deviation between the inter-frame matching relative displacement and the valid displacement data is evaluated; and one of the two is adaptively selected as the final pose prior based on whether the deviation exceeds a preset threshold. When the data from the pose estimation unit is determined to be invalid, inter-frame point cloud matching is performed on two consecutive frames of the horizontally projected point cloud to directly calculate the relative displacement without an initial prior, and the calculated relative displacement is used as the final pose prior.

[0024] The localization calculation module is used to perform matching with the global point cloud map based on the pose prior obtained from the motion tracking processing module and by utilizing the local geometric structure information of the horizontal projection point cloud. This matching process characterizes the geometric structure by calculating the local normal vector of the horizontal projection point cloud and combining it with nearest neighbor search for data association in order to calculate and optimize the robot's final current pose.

[0025] Beneficial effects: Compared with existing technologies, the advantages of this invention are as follows: By preprocessing point cloud data, the tilted original point cloud is projected onto a virtual horizontal plane, effectively eliminating the interference of vehicle posture changes caused by the robot traversing slopes or bumpy roads on point cloud matching, significantly improving the robustness of positioning in non-horizontal scenarios. Positioning accuracy can reach within 3 cm, and the frequency of output pose is improved through linear interpolation. Within a 3-meter linear displacement deviation and a 30-degree angular displacement deviation, the repositioning success rate reaches over 90%.

[0026] By using an adaptive selection mechanism based on pose priors, information from pose estimation units (such as wheel speedometers) and laser odometry is intelligently integrated. This avoids error accumulation when sensor data (such as wheel speedometer slippage) is abnormal, thus improving the overall accuracy and reliability of the positioning results.

[0027] By introducing a rapid repositioning mechanism, the robot can quickly recover its accurate pose using external prior information after positioning is lost, ensuring the continuity of robot operations and reducing system downtime.

[0028] The point cloud matching process makes full use of the geometric information contained in the local normal vectors, which has higher matching accuracy and efficiency compared to simple point-to-point matching. Attached Figure Description

[0029] Figure 1 This is a top-level logic flowchart of a positioning algorithm according to an embodiment of the present invention.

[0030] Figure 2This is a flowchart of point cloud matching according to an embodiment of the present invention, wherein the left side is the relocation mode flowchart and the right side is the motion tracking mode flowchart. Detailed Implementation

[0031] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the embodiments described.

[0032] Example 1: This invention provides a robust and interference-resistant 3D point cloud positioning system, deployed on a mobile robot, such as a wheeled inspection robot. The system hardware includes a 3D LiDAR, a pose estimation unit, an attitude measurement unit, and a processor. In a preferred embodiment, the pose estimation unit is a wheel speedometer used to measure the robot's travel distance; the attitude measurement unit is an inertial measurement unit (IMU) used to measure the robot's three-axis attitude angles (roll, pitch, and yaw). The processor is responsible for running the positioning method provided by this invention.

[0033] Reference Figure 1 and Figure 2 The specific process of the positioning method described in this invention is as follows:

[0034] 1. Data Acquisition and Preprocessing

[0035] The system first acquires real-time data from various sensors, including: raw 3D point cloud data collected by 3D LiDAR, relative displacement data provided by wheel speedometer, and real-time attitude data provided by IMU.

[0036] Before entering the point cloud matching process, a point cloud data preprocessing step is performed to eliminate the influence of non-horizontal road surfaces. The processor utilizes the robot's real-time roll and pitch angles provided by the IMU, as well as pre-calibrated extrinsic parameters between the radar and IMU, to perform a coordinate rotation transformation on each point in the original point cloud, projecting it onto a virtual horizontal coordinate system, thereby generating a "horizontal" horizontally projected point cloud frame. This transformation can be achieved using the following formula:

[0037] ;

[0038] in, The rotation matrix of the actual radar coordinate system relative to the world coordinate system, calculated from IMU data. This is the rotation matrix of the virtual horizontal radar coordinate system relative to the world coordinate system, retaining only the yaw angle. This is the spatial rotation matrix used for the horizontal projection transformation of the original point cloud. The horizontally projected point cloud processed in this way can be directly used for subsequent matching, avoiding matching failures caused by attitude tilt.

[0039] 2. Motion tracking mode

[0040] When the robot's localization is normal, the system operates in motion tracking mode, such as... Figure 2 The flowchart is shown on the right.

[0041] The basic principle is a loosely coupled multi-sensor data fusion method: 3D point cloud matching is the main method, while data from wheel speedometer and inertial measurement unit are used as pose prior inputs to the laser point cloud matching function module. The point cloud matching calculation outputs the optimized robot pose.

[0042] Scenario 1: Data Abnormality. When the input pose prior data is missing (e.g., no wheel velocimeter data or inertial measurement unit data) or abnormal (e.g., wheel velocimeter slippage), the system will directly enter the inter-frame matching relative displacement to calculate the robot pose.

[0043] The relative displacement is calculated based on the timestamped wheel speedometer readings. Under normal robot operation, this relative displacement is subject to a threshold upper limit constraint. For example, if the interval between two adjacent timestamps is 200 milliseconds, and the vehicle's linear velocity is 1.0 meter per second and angular velocity is 30 degrees per second, then the upper limit for relative linear displacement is approximately 0.2 meters, and the upper limit for relative angular displacement is approximately 6 degrees. If the relative displacement actually calculated from the wheel speedometer readings significantly exceeds this threshold upper limit, it can be determined that wheel slippage is causing the abnormal wheel speedometer readings.

[0044] Scenario 2: Data is normal. When the input pose prior data (i.e., wheel speedometer data or inertial measurement unit data) is normal, the relative displacement provided by the wheel speedometer will be used as the prior for inter-frame relative displacement. Then, the deviation between the inter-frame matched relative displacement and the relative displacement provided by the wheel speedometer will be comprehensively evaluated. If the deviation is small, the inter-frame matched relative displacement will be used; if the deviation is large, the wheel speed odometry result will be used. Finally, the result will be fed into the point cloud matching function to calculate the robot pose.

[0045] Here, mathematical formulas are used to quantify the deviation between the relative displacement of inter-frame matching and the relative displacement obtained by the wheel speedometer:

[0046] ;

[0047] In the formula, The inter-frame relative displacements obtained from the raw data include angular and linear displacements. The inter-frame relative displacement obtained from laser matching also includes displacement information related to relative rotation and relative translation. This refers to the deviation between the two relative displacements mentioned above in terms of rotation and translation.

[0048] From the relative coordinate transformation of the deviation, separate the rotational and translational components.

[0049] ;

[0050] The deviation thresholds for the translation portion (within 10 cm) and the rotation portion (within 15 degrees) are used to classify deviations as small or large. Within a short time interval, the deviation thresholds between the raw data (wheel velocity sensor and inertial navigation system) and the ground truth (approximately laser matching) are considered.

[0051] For point cloud matching, two steps are used: inter-frame point cloud matching and point cloud map matching. Point cloud matching in either step utilizes the local normal vector spatial geometric information of the point cloud. The advantage of spatial normal vectors lies in describing the local geometry of the point cloud; this geometric information can improve matching efficiency and accuracy in laser matching.

[0052] A local normal vector is the normal vector associated with the contour surface formed by a small cluster of spatially close point clouds, describing local geometric information. Specifically, it can be calculated by taking the covariance matrix of this point cloud set: through eigenvalue decomposition, the smaller eigenvalue is found, and its corresponding eigenvector is the normal vector corresponding to this local point cloud cluster, describing local spatial geometric information. This normal vector information is mainly used in laser point cloud matching, where the target deviation function constructed by point cloud matching is projected through the normal vector.

[0053] ;

[0054] In the formula, and They refer to matching pairs from two point cloud sets, The relative rotation matrix between two point cloud sets is represented by a 3x3 matrix. This represents the relative translation vector between two point cloud sets, using a 3x1 matrix. This represents the geometric information of the normal vectors of the point cloud clusters surrounding the matching point in the reference point cloud. The vector transpose operation here is mainly manifested as a dot product, which calculates the projection of the deviation vector between the point after rotation and translation coordinate transformation and the point associated with the data onto the normal vector direction. Inter-frame point cloud matching, also known as laser odometry, involves finding matching pairs between consecutive adjacent frames of point clouds in a time series through data association, and then calculating the relative displacement (linear and angular displacement) through point cloud matching. Since the actual relative displacement between adjacent radar frames is relatively small (typically around 10 Hz for radar frequencies, and the relative displacement of a robot typically occurs within 100 ms between adjacent frames), inter-frame matching generally uses data from the wheel speedometer or inertial navigation system as a priori, assuming no wheel speedometer slippage failure. The relative displacement calculated by matching is generally a small-amplitude fine-tuning, and is generally quite accurate. However, radar failure can occur, which can be obtained from the magnitude of the relative displacement corresponding to the inter-frame matching result or the deviation from the displacement prior corresponding to the original data source. Only when the inter-frame matching is normal will the result calculated by the laser odometry be used as a priori docking to match the point cloud map and update the robot pose.

[0055] Point cloud map matching: Near the robot's current possible pose corresponding to the localization module, the radar point cloud and point cloud map are correlated using nearest neighbor search to find matching point cloud pairs. Laser matching is then performed on these pairs to optimize and adjust the robot's current pose. The nearest neighbor search uses the traditional KNN nearest neighbor search method.

[0056] 3. Relocation Mode

[0057] The ability to quickly and accurately reposition after location loss refers to obtaining a rough prior pose through user-inspired prior pose or laser template matching when location is lost, and then using point cloud and map matching to more accurately calculate and optimize the robot's pose.

[0058] Determining location loss is primarily based on the matching degree between the current point cloud and the map. A higher mean nearest neighbor (MNN) between matching pairs obtained from the data association between the current point cloud and the map indicates a location failure. Quantitatively, a score is calculated using the following mathematical formula: the greater the distance between matching pairs, the lower the score.

[0059] ;

[0060] In the formula, This represents the average nearest neighbor distance between adjacent point matching pairs after processing using the nearest neighbor matching (ICP) method. The resolution of the point cloud map used. To match the ratio of the average nearest neighbor distance between points to the point cloud map resolution, the score represents the confidence value.

[0061] like Figure 2 As shown in the flowchart on the left, the point cloud matching method is similar to that used in motion tracking. After finding a matching pair through data association search, point cloud matching is performed using the local point cloud geometric information of the normal vector. Although both the relocalization and motion tracking processes use real-time radar point cloud and laser point cloud map matching to calculate and update the robot's pose, the motion tracking process updates based on the robot's more accurate pose from the previous moment, while relocalization is based on the robot's possible pose at the current moment. Therefore, the condition for accurate and rapid relocalization is that the pose prior cannot deviate too much from the ground truth. Specifically, the linear displacement deviation between the input pose prior and the robot's accurate ground truth is generally within 3 meters, and the angular displacement deviation is generally within 30 degrees.

[0062] The robust and interference-resistant 3D point cloud localization algorithm provided by this invention has advantages in two aspects. Firstly, it enhances the robustness and anti-interference capability of wheeled inspection robots' localization function, effectively adapting to uneven surfaces and slopes, thus avoiding collisions, scrapes, and even rollovers. Specifically, the localization accuracy can reach within 3 centimeters, the computation time is approximately 200 milliseconds between adjacent frames, and the pose output frequency is increased through linear interpolation. Within a 3-meter linear displacement deviation and a 30-degree angular displacement deviation, the relocalization success rate reaches over 90%. Secondly, it allows for rapid and accurate relocalization and restoration of normal motion tracking mode when localization loss occurs.

[0063] As described above, although the invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the invention itself. Various changes in form and detail may be made without departing from the spirit and scope of the invention as defined in the appended claims.

Claims

1. A robust and interference-resistant 3D point cloud localization method, applied to a mobile robot equipped with a 3D LiDAR, a pose estimation unit, and an attitude measurement unit, wherein the pose estimation unit is a wheel velocity meter, and the attitude measurement unit is an inertial measurement unit; characterized in that, Includes the following steps: S1: Acquire the original point cloud data of the 3D lidar, the displacement data of the wheel speed meter, and the attitude data of the inertial measurement unit; and use the attitude data to project and transform the original point cloud data to a virtual horizontal coordinate system to generate a horizontal projection point cloud for matching; S2: Based on the validity of the displacement data and attitude data, adaptively determine the final pose prior for point cloud matching; The validity of the wheel speed meter is determined by the following method: the relative displacement is calculated based on the readings of two consecutive timestamps. When the linear displacement of the relative displacement is greater than a preset linear displacement threshold or the angular displacement is greater than a preset angular displacement threshold, the data is determined to be invalid. When the data from the wheel velocity meter is determined to be valid, the step of determining the final pose prior includes: using the relative displacement provided by the wheel velocity meter as the initial prior; calculating the inter-frame matched relative displacement by performing inter-frame point cloud matching on two consecutive frames of the horizontally projected point cloud; evaluating the deviation transformation between the inter-frame matched relative displacement and the relative displacement of the wheel velocity meter, and separating its translational deviation and rotational deviation; if the translational deviation is less than a first preset translation threshold and the rotational deviation is less than a first preset rotation threshold, then the inter-frame matched relative displacement is selected as the final pose prior; otherwise, the wheel velocity meter relative displacement is selected as the final pose prior. When the data from the wheel speed meter is determined to be invalid, inter-frame point cloud matching is performed on two consecutive frames of the horizontal projection point cloud to directly calculate the relative displacement without the wheel speed meter data as an initial prior, and the calculated relative displacement is used as the final pose prior. S3: Based on the selected final pose prior and the horizontal projected point cloud, perform a point cloud matching process to calculate and optimize the robot's current pose, wherein the point cloud matching process utilizes the local normal vector space geometric information of the point cloud for data association and pose calculation.

2. The robust and interference-resistant 3D point cloud localization method according to claim 1, characterized in that, It also includes a location loss detection step, which calculates the average nearest neighbor distance between matching pairs of the current frame point cloud and the global point cloud map. When the distance is greater than a preset matching threshold, it is determined that the location has been lost and the relocation mode is activated.

3. The robust and interference-resistant 3D point cloud localization method according to claim 2, characterized in that, In the relocation mode, the robot receives an externally provided heuristic pose or an initial pose prior obtained through laser template matching, and performs step S3 based on the initial pose prior; wherein the linear displacement deviation and angular displacement deviation between the initial pose prior and the robot's actual ground pose are both less than their respective preset ranges.

4. The robust and interference-resistant 3D point cloud localization method according to claim 1, characterized in that, The projection transformation in step S1 is based on the extrinsic parameters between the 3D LiDAR and the attitude measurement unit, and is implemented according to the following formula: in, Radar coordinate system Relative to the world coordinate system The rotation matrix, Inertial measurement unit coordinate system Transform to world coordinate system Required rotation matrix, From the radar coordinate system Transform to the inertial measurement unit coordinate system Required rotation matrix; Let be the rotation matrix of the virtual horizontal radar coordinate system relative to the world coordinate system. Let Z be the rotation matrix about the Z-axis. Yaw angle This is the spatial rotation matrix used to perform the projection transformation.

5. The robust and interference-resistant 3D point cloud localization method according to claim 1, characterized in that, It also includes a pose publishing step, which generates and publishes a smooth pose trajectory with a frequency higher than the calculation frequency of the point cloud matching process by linearly interpolating the robot poses at multiple discrete moments.

6. A robust and interference-resistant 3D point cloud positioning system, comprising a 3D lidar, a pose estimation unit, an attitude measurement unit, and a processor, characterized in that, The processor is configured to include: The point cloud preprocessing module uses the attitude data obtained from the attitude measurement unit to perform coordinate transformation on the raw point cloud data obtained from the 3D LiDAR, so as to generate a horizontal projection point cloud that can compensate for the robot's attitude tilt. The motion tracking processing module is configured to perform the following operations: when the data from the pose estimation unit is determined to be valid, the valid displacement data is used as the initial prior; inter-frame point cloud matching is performed on two consecutive frames of the horizontally projected point cloud to calculate the inter-frame matching relative displacement; the deviation between the inter-frame matching relative displacement and the valid displacement data is evaluated; and one of the two is adaptively selected as the final pose prior based on whether the deviation exceeds a preset threshold. When the data from the pose estimation unit is determined to be invalid, inter-frame point cloud matching is performed on two consecutive frames of the horizontally projected point cloud to directly calculate the relative displacement without an initial prior, and the calculated relative displacement is used as the final pose prior. The localization calculation module is used to perform matching with the global point cloud map based on the pose prior obtained from the motion tracking processing module and by utilizing the local geometric structure information of the horizontal projection point cloud. This matching process characterizes the geometric structure by calculating the local normal vector of the horizontal projection point cloud and combining it with nearest neighbor search for data association in order to calculate and optimize the robot's final current pose.

Images