Point cloud data processing method, robot and storage medium

Abstract

The invention provides a point cloud data processing method, a robot and a storage medium, the method is applied to the field of robots, and the method comprises the following steps: obtaining respective matching scores of a plurality of initial point cloud data, and taking a candidate initial attitude corresponding to the initial point cloud data with the highest matching score as a target initial attitude; controlling the robot to rotate to a target initial attitude and acquiring target point cloud data under the target initial attitude; dividing the target map into a plurality of sub-maps; obtaining matching scores of the sub-maps according to the candidate initial positions and the target point cloud data in the sub-maps, and taking the sub-map with the highest matching score as a target sub-map; when an iteration termination condition is satisfied, determining the candidate initial position in the target sub-map as a target initial position; and obtaining an initial reference pose according to the target initial position and the target initial pose. According to the method, the repositioning success rate and efficiency of the robot away from the original point can be improved.

Description

Technical Field

[0001] This application relates to the field of robotics, and more specifically, to a method for processing point cloud data, a robot, and a storage medium in the field of robotics. Background Technology

[0002] In the field of autonomous robot localization and navigation, especially based on a known prior map, robots typically rely on sensor data (such as LiDAR, cameras, etc.) to redetermine their position and orientation. Currently, before each localization attempt, the robot must ensure it returns to the mapping origin. This is because the initial matching success rate depends on the distance and orientation difference between the robot and the origin. When the robot is far from the origin, or the initial scanning angle differs significantly, the matching between the current frame's point cloud and the prior map is prone to failure, leading to incomplete relocalization. Therefore, how to avoid reliance on the mapping origin during robot relocalization initialization and improve the success rate and efficiency of relocalization when the robot is far from the origin has become an urgent technical problem to be solved. Summary of the Invention

[0003] This application provides a method for processing point cloud data, a robot, and a storage medium. This method can solve the problem of robot relocalization initialization relying on the mapping origin, and improve the success rate and efficiency of robot relocalization when it is far from the origin.

[0004] Firstly, a point cloud data processing method is provided, applied to a robot equipped with a LiDAR for collecting point cloud data. The method includes: acquiring matching scores for multiple initial point cloud data sets; selecting the candidate initial pose corresponding to the initial point cloud data set with the highest matching score as the target initial pose; wherein multiple initial point cloud data sets correspond one-to-one with multiple candidate initial poses of the robot, and the matching score represents the degree of matching between the initial point cloud data set and a pre-constructed prior map; controlling the robot to rotate to the target initial pose and acquiring target point cloud data in the target initial pose; dividing the target map into multiple sub-maps, wherein when the current value of the number of segmentations of the target map is 0, the target map is the prior map; for each sub-map, obtaining the matching score of the sub-map based on the candidate initial positions and target point cloud data in the sub-map, and selecting the sub-map with the highest matching score as the target sub-map; determining the candidate initial positions in the target sub-map as the target initial positions when the iteration termination condition is met; and obtaining the initial reference pose based on the target initial positions and target initial poses.

[0005] The above technical solution effectively improves the success rate and efficiency of relocalization by optimizing the robot's posture and position in stages, combined with multi-candidate posture matching and a sub-map-based iterative matching mechanism, without requiring the robot to return to the mapping origin. Specifically, by acquiring the matching scores of multiple initial point cloud data, and selecting the candidate initial posture corresponding to the initial point cloud data with the highest matching score as the target initial posture, the robot can find the posture direction that best matches the prior map at its current observation position, even when it is far from the mapping origin, thus significantly improving the success rate of subsequent point cloud matching. By determining the target initial posture before searching for the position, the interference of posture errors on the point cloud matching process is avoided, reducing the possibility of matching getting stuck in local optima due to inaccurate posture, and improving the robustness of the overall matching. The prior map is divided into multiple sub-maps, and based on the candidate initial positions and target point cloud data in each sub-map, the matching score of the sub-map is obtained, and the target sub-map with the highest matching score is selected. After the iteration termination condition is met, the candidate initial position in the target sub-map is determined as the target initial position, and combined with the target initial pose, a complete initial reference pose is formed, which provides a stable and reliable starting point for the robot's subsequent real-time localization and navigation tasks, and improves the success rate and efficiency of the robot's relocalization when it is far from the origin.

[0006] In conjunction with the first aspect, in some possible implementations, if the iteration termination condition is not met, the current value of the number of segmentations of the target map is incremented by 1; and the target sub-map is used as the target map.

[0007] The above technical solution determines whether to continue segmenting the sub-map by checking if the iteration termination condition is met. If the termination condition is not met, the current value of the segmentation count of the target map is incremented by 1, the target sub-map is used as the target map, and the process returns to the step of segmenting the target map into multiple sub-maps and continuing to determine the matching scores of the sub-maps. By progressively segmenting the target map, the search range is gradually narrowed, avoiding the high computational complexity of global search, enabling continuous optimization of the matching results, and further improving the final matching accuracy.

[0008] In conjunction with the first aspect, in some possible implementations, the matching scores of multiple initial point cloud data are obtained, including: obtaining the initial point cloud data corresponding to multiple candidate initial poses of the robot; wherein, the initial point cloud data corresponding to each candidate initial pose is: the point cloud data collected by the LiDAR when the robot is in the candidate initial pose; for the initial point cloud data corresponding to each candidate initial pose, the initial point cloud data is matched with the prior map to obtain the matching score of each initial point cloud data.

[0009] The above technical solution avoids the error risk caused by a single initial guess by collecting corresponding initial point cloud data under multiple candidate initial poses and matching them with the prior map. Each candidate initial pose is independently matched and evaluated to ensure that the final selected target initial pose is the globally optimal or near-optimal initial pose, which effectively improves the robustness and accuracy of robot relocalization.

[0010] Combining the first aspect and the above implementation methods, in some possible implementation methods, obtaining the initial point cloud data corresponding to the multiple candidate initial postures of the robot includes: controlling the robot to rotate in place at a preset angular velocity, and obtaining the initial point cloud data corresponding to the multiple candidate initial postures of the robot during the rotation process.

[0011] The above technical solution achieves multi-view perception of the environment by controlling the robot to rotate in place at a preset angular velocity and collecting initial point cloud data corresponding to multiple candidate initial postures during the rotation. This reduces the risk of mismatch due to limited viewpoints and effectively improves the accuracy of point cloud matching and system robustness during relocalization. Furthermore, sampling under multiple postures can be completed simply by controlling the robot to rotate in place, making the operation simple and efficient.

[0012] Combining the first aspect and the above implementation methods, in some possible implementation methods, the initial point cloud data includes multiple first source points. Obtaining the matching scores of each of the multiple initial point cloud data includes: for each initial point cloud data, obtaining a first target transformation matrix between the initial point cloud data and the prior map; transforming each first source point in the initial point cloud data according to the first target transformation matrix to obtain the transformed first source point, and searching for the first target point in the prior map that is closest to the transformed first source point; calculating the first Mahalanobis distance between the transformed first source point and the first target point; and determining the matching score of each initial point cloud data according to the first Mahalanobis distance.

[0013] The above technical solution uses a first target transformation matrix to initially align the point cloud, making the first source point closer to the first target point. It then searches the prior map for the nearest first target point to the first source point, establishing a point-to-point matching relationship and enhancing matching reliability. Mahalanobis distance considers the covariance information of the data distribution and, compared to Euclidean distance, better reflects the true matching degree. It is more robust to outliers and noise, and is suitable for complex or partially occluded scenes. Each candidate initial pose yields a quantified matching score for the initial point cloud data, facilitating the subsequent selection of the optimal initial pose, i.e., the target's initial pose. This provides a reliable basis for high-precision relocalization and avoids getting trapped in local optima.

[0014] Combining the first aspect and the above implementation methods, in some possible implementation methods, obtaining the first target transformation matrix between the initial point cloud data and the prior map includes: for each first source point in the initial point cloud data, searching for the second target point closest to the first source point in the prior map based on the first initial transformation matrix; calculating the local covariance matrix corresponding to each first source point and the local covariance matrix corresponding to each second target point; determining a first candidate transformation matrix based on each first source point, each second target point, the local covariance matrix corresponding to each first source point, the local covariance matrix corresponding to each second target point, and a preset first objective function, wherein the first objective function is used to describe the first Mahalanobis distance between the transformed first source point and the second target point; and using the first candidate transformation matrix as the first target transformation matrix if a preset convergence condition is met.

[0015] Combining the first aspect and the above implementation methods, in some possible implementation methods, if the preset convergence condition is not met, the first candidate transformation matrix is ​​used as the first initial transformation matrix; for each first source point in the initial point cloud data, the second target point closest to the first source point is searched in the prior map based on the first initial transformation matrix.

[0016] The above technical solution utilizes the local geometric structure (local covariance matrix) of each point in the point cloud to enhance the ability to describe environmental features. Mahalanobis distance takes into account the local distribution characteristics of the point cloud and has a stronger tolerance for noise and outliers. By introducing the local covariance matrix and the first Mahalanobis distance, a first objective function is constructed, and the optimal transformation matrix is ​​solved by iterative optimization. That is, the transformation matrix between the point cloud data of the current frame and the prior map is dynamically updated during the iteration process to obtain the optimal transformation matrix, which is the first objective transformation matrix. This is beneficial to obtaining more accurate and robust point cloud matching results.

[0017] Combining the first aspect and the above implementation methods, in some possible implementation methods, the target point cloud data includes multiple second source points. For each sub-map, the matching score of the sub-map is obtained based on the candidate initial positions in the sub-map and the target point cloud data. This includes: obtaining the second target transformation matrix between the target point cloud data and the prior map based on the candidate initial positions and the initial pose of the target; transforming each second source point in the target point cloud data according to the second target transformation matrix to obtain the transformed second source point, and searching for the third target point in the prior map that is closest to the transformed second source point; calculating the second Mahalanobis distance between the transformed second source point and the third target point; and determining the matching score of the sub-map based on the second Mahalanobis distance.

[0018] The above technical solution uses a second target transformation matrix to align the target point cloud to the prior map coordinate system, enhancing the geometric correspondence between the second source point and the third target point. Based on the second Mahalanobis distance rather than Euclidean distance, it better reflects the distribution characteristics of the point cloud and improves matching robustness. An independent matching score is generated for each sub-map, facilitating the selection of the optimal matching result from multiple possible candidate initial positions, thereby obtaining the optimal initial position, i.e., the target initial position.

[0019] Combining the first aspect and the above implementation methods, in some possible implementation methods, obtaining the second target transformation matrix between the target point cloud data and the prior map based on the candidate initial position and the target initial pose includes: constructing a second initial transformation matrix between the target point cloud data and the prior map based on the candidate initial position and the target initial pose; for each second source point in the target point cloud data, searching for the fourth target point closest to the second source point in the prior map based on the second initial transformation matrix; calculating the local covariance matrix corresponding to each second source point and the local covariance matrix corresponding to each fourth target point; determining the second candidate transformation matrix based on each second source point, each fourth target point, the local covariance matrix corresponding to each second source point, the local covariance matrix corresponding to each fourth target point, and a preset second objective function, wherein the second objective function is used to describe the second Mahalanobis distance between the transformed second source point and the fourth target point; and using the second candidate transformation matrix as the second target transformation matrix if a preset convergence condition is met.

[0020] Combining the first aspect and the above implementation methods, in some possible implementation methods, if the preset convergence condition is not met, the second candidate transformation matrix is ​​used as the second initial transformation matrix; for each second source point in the target point cloud data, the fourth target point closest to the second source point is searched in the prior map based on the second initial transformation matrix.

[0021] The above technical solution constructs a second objective function by introducing a local covariance matrix and a second Mahalanobis distance, and uses an iterative optimization method to solve for the optimal transformation matrix. That is, the transformation matrix between the target point cloud data and the prior map is dynamically updated during the iteration process to obtain the optimal transformation matrix, which is the second objective transformation matrix. This facilitates obtaining more accurate and robust point cloud matching results. Given the candidate initial positions and the target's initial pose, it can efficiently solve for the optimal transformation matrix between the point cloud data and the prior map, providing reliable technical support for robot relocalization and map matching.

[0022] Combining the first aspect and the above implementation methods, in some possible implementation methods, the candidate initial position in the submap is the center point of the submap.

[0023] In the aforementioned technical solution, the center point is the most representative location on the submap, typically situated at the geometric center of the region. It effectively reflects the overall characteristics of the area. In environments with symmetrical structures or repetitive features, the center point serves as a unified reference point, helping to avoid mismatches. Therefore, using the center point of the submap as a candidate initial location for point cloud matching and pose estimation provides a reasonable and stable initial location assumption, thereby improving matching efficiency and relocalization success rate.

[0024] Combining the first aspect and the above implementation methods, in some possible implementation methods, after obtaining the initial reference pose based on the initial target position and initial target attitude, the method further includes: performing point cloud matching on the point cloud data currently collected by the lidar and the prior map based on the initial reference pose to obtain a third target transformation matrix between the point cloud data and the prior map.

[0025] The above technical solution provides an initial reference pose that is close to the actual pose. After determining the initial reference pose, further fine matching of the point cloud is performed to obtain the third target transformation matrix between the point cloud data and the prior map, thereby achieving high-precision estimation of the robot pose and significantly improving the accuracy and reliability of the relocalization system.

[0026] Combining the first aspect and the above implementation methods, in some possible implementation methods, the iteration termination condition includes: the matching score of the target sub-map is greater than a preset score threshold; or, the current value of the number of segmentations of the target map is greater than a preset number threshold.

[0027] The above technical solution indicates that when the matching score exceeds a preset score threshold, the current candidate initial position is sufficiently accurate, eliminating the need for further iterations and avoiding redundant calculations, thus effectively shortening the relocalization time. Setting a preset threshold for the maximum number of segmentations prevents wasted computational resources due to excessive subdivision of the prior map; it also avoids continuous iteration when the matching score cannot be significantly improved, enhancing the system's stability and robustness. By introducing a dual-condition iteration termination mechanism based on both the matching score and the number of map segmentations, intelligent control of the point cloud matching process is achieved, effectively reducing computational overhead while ensuring matching accuracy, thereby improving the efficiency and practicality of robot relocalization.

[0028] In a second aspect, a robot is provided, including a memory for storing executable program code;

[0029] A processor is configured to call and run the executable program code from the memory, causing the robot to perform the method described in the first aspect or any possible implementation thereof.

[0030] Thirdly, a computer program product is provided, comprising: computer program code, which, when run on a computer, causes the computer to perform the methods described in the first aspect or any possible implementation thereof.

[0031] Fourthly, a computer-readable storage medium is provided that stores computer program code, which, when executed on a computer, causes the computer to perform the methods described in the first aspect or any possible implementation thereof. Attached Figure Description

[0032] Figure 1 This is a schematic flowchart illustrating a point cloud data processing method provided in an embodiment of this application;

[0033] Figure 2 This is a schematic flowchart illustrating another point cloud data processing method provided in the embodiments of this application;

[0034] Figure 3 This is a schematic flowchart illustrating an embodiment of the present application for obtaining a first target transformation matrix;

[0035] Figure 4 This is a schematic flowchart illustrating an embodiment of the present application for obtaining a second target transformation matrix;

[0036] Figure 5 This is a schematic flowchart illustrating another point cloud data processing method provided in the embodiments of this application;

[0037] Figure 6 This is a schematic diagram of the structure of a robot provided in an embodiment of this application. Detailed Implementation

[0038] The technical solutions in this application will be clearly and thoroughly described below with reference to the accompanying drawings. In the description of the embodiments of this application, unless otherwise stated, " / " means "or," for example, A / B can mean A or B. "And / or" in the text is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Furthermore, in the description of the embodiments of this application, "multiple" refers to two or more than two.

[0039] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature.

[0040] In the field of autonomous robot localization and navigation, especially based on known prior maps, robots typically rely on sensor data (such as LiDAR, cameras, etc.) to re-determine their position and orientation. This process is crucial for robots to recover their subsequent localization and navigation capabilities after losing position information, involving several key technical aspects, including but not limited to: sensor data acquisition and processing; prior map construction and maintenance; point cloud matching and pose estimation; and real-time localization and path planning. Currently, mainstream methods mainly fall into two categories: relocalization methods based on traditional geometric matching and relocalization methods based on deep learning.

[0041] Traditional geometric matching methods typically rely on comparing the point cloud data acquired by the current sensors with a pre-constructed prior map, estimating the robot's initial pose by calculating the similarity between the two. Common algorithms include ICP (Iterative Closest Point), FPFH (Fast Point Feature Histograms), and NDT (Normal Distributions Transform). This method requires ensuring the robot returns to the mapping origin before each localization attempt, as the initial matching success rate depends on the distance between the robot and the mapping origin and the difference between the robot's current pose and its initial pose. When the robot is far from the mapping origin, or when the current scanning angle differs significantly from the initial scanning angle, matching between the current frame's point cloud and the prior map is prone to failure, leading to relocalization failure. This method often employs a global search mechanism, searching for the optimal matching position of the current point cloud within the entire prior map. However, this strategy incurs extremely high computational costs, severely impacting the system's real-time response capabilities.

[0042] In recent years, with the development of deep learning, some studies have attempted to use neural network models to directly regress robot poses from point cloud or image data. However, these methods typically require a large amount of pose-labeled data for training, resulting in low model generalization.

[0043] As can be seen from the above, existing technologies still suffer from problems such as strong dependence on the mapping origin, high computational complexity, and poor generalization. These problems severely limit the application scenarios of robots, especially in large-scale environments where robots cannot initialize far from the mapping origin, hindering subsequent real-time localization and navigation.

[0044] To at least address the aforementioned technical problems, embodiments of this application provide a point cloud data processing method that improves the initial point cloud matching success rate, thereby supporting robot positioning and navigation in complex 3D scenes. This point cloud data processing method is applied to a robot equipped with a LiDAR system capable of collecting point cloud data.

[0045] Figure 1This is a schematic flowchart illustrating a point cloud data processing method provided in an embodiment of this application.

[0046] For example, such as Figure 1 As shown, the processing method for this point cloud data includes:

[0047] Step 101: Obtain the matching scores of multiple initial point cloud data, and take the candidate initial pose corresponding to the initial point cloud data with the highest matching score as the target initial pose. The multiple initial point cloud data and the robot's multiple candidate initial poses correspond one-to-one. The matching score represents the degree of matching between the initial point cloud data and the pre-built prior map.

[0048] Step 102: Control the robot to rotate to the target initial posture and acquire the target point cloud data in the target initial posture.

[0049] Step 103: Divide the target map into multiple sub-maps. When the current value of the number of divisions of the target map is 0, the target map is the prior map.

[0050] Step 104: For each submap, obtain the matching score of the submap based on the candidate initial positions and target point cloud data in the submap, and take the submap with the highest matching score as the target submap.

[0051] Step 105: If the iteration termination condition is met, determine the candidate initial position in the target sub-map as the target initial position.

[0052] Step 106: Obtain the initial reference pose based on the initial position and initial attitude of the target.

[0053] exist Figure 1 In the illustrated embodiment, by acquiring the matching scores of multiple initial point cloud data and selecting the candidate initial pose corresponding to the initial point cloud data with the highest matching score as the target initial pose, the robot can find the pose direction that best matches the prior map at its current observation position, even when it is far from the mapping origin, thus significantly improving the success rate of subsequent point cloud matching. By determining the target initial pose before performing position search, the interference of pose error on the point cloud matching process is avoided, reducing the possibility of matching getting stuck in local optima due to inaccurate pose and improving the robustness of the overall matching. The prior map is divided into multiple sub-maps. Based on the candidate initial positions and target point cloud data in each sub-map, the matching score of the sub-map is obtained, and the target sub-map with the highest matching score is selected. After the iteration termination condition is met, the candidate initial positions in the target sub-map are determined as the target initial positions, and combined with the target initial pose, a complete initial reference pose is formed, providing a stable and reliable starting point for the robot's subsequent real-time localization and navigation tasks, improving the success rate and efficiency of relocalization when the robot is far from the origin.

[0054] In some embodiments, such as Figure 2 As shown, the point cloud data processing method, in addition to steps 101 to 106 mentioned above, also includes:

[0055] Step 107: If the iteration termination condition is not met, increment the current value of the number of segmentations of the target map by 1, and use the target sub-map as the target map.

[0056] By determining whether the iteration termination condition is met, it decides whether to continue segmenting the submap. If the termination condition is not met, the current value of the target map's segmentation count is incremented by 1, the target submap is used as the target map, and the process returns to dividing the target map into multiple submaps and continuing to determine the matching scores of the submaps. By progressively segmenting the target map, the search range is gradually narrowed, avoiding the high computational complexity of global search, enabling continuous optimization of the matching results, and further improving the final matching accuracy. If the iteration termination condition is met, the candidate initial positions in the target submap are determined as the target initial positions.

[0057] The following is about Figure 1 and Figure 2 The specific implementation methods of each step in the illustrated embodiment are explained below:

[0058] In step 101, multiple candidate initial poses provide a range of choices for the target initial pose, and each candidate initial pose can be understood as a pose of the robot. The robot's pose can be represented by yaw, pitch, and roll. For example, the robot's pose can be represented as (roll, pitch, yaw). Since there is a one-to-one correspondence between multiple initial point cloud data and multiple candidate initial poses of the robot, the matching score of each initial point cloud data can also be understood as the matching score corresponding to the candidate initial pose.

[0059] To determine the target initial pose for robot relocalization, multiple candidate initial poses are introduced. To select the optimal candidate initial pose as the target initial pose, matching scores are obtained for each of the robot's initial point cloud data sets, i.e., matching scores corresponding to the candidate initial poses. The candidate initial pose corresponding to the initial point cloud data with the highest matching score is then selected as the target initial pose. This matching score represents the degree of matching between the initial point cloud data acquired by the robot's configured LiDAR and the pre-constructed prior map.

[0060] A prior map is a 3D environment map pre-built by the robot before it performs a relocalization task. It provides a reference for subsequent initial pose estimation based on point cloud matching. Specifically, the prior map is a point cloud map, which can be constructed using Simultaneous Localization and Mapping (SLAM) technology. During real-time mapping, the prior map is continuously updated and optimized by fusing data from the Inertial Measurement Unit (IMU) and LiDAR, resulting in high accuracy and consistency.

[0061] For example, the robot is equipped with an inertial measurement unit (IMU) and a lidar. The IMU is used to collect the robot's angular velocity and linear acceleration information, while the lidar is used to collect point cloud data of the surrounding environment. Through the coordinated operation of these two sensors, high-frequency estimation of the robot's motion state is achieved, providing high-quality environmental observation data for 3D mapping.

[0062] In the initial mapping phase, the IMU is first initialized, an initial reference coordinate system is established, and IMU integration is initiated to obtain continuous robot motion state predictions, including position, attitude, linear velocity, and sensor bias. The high-frequency motion information provided by the IMU is used for distortion correction and state estimation of subsequent point cloud data. Because the robot is in motion, the point cloud obtained by LiDAR scanning exhibits "mog" or "distortion." Therefore, using the motion trajectory information provided by the IMU, point-by-point timestamp compensation can be performed on the current frame's point cloud to eliminate point cloud distortion caused by robot motion and improve point cloud quality. The IMU data is then fused with the distortion-corrected point cloud data, and an Iterated Error-State Kalman Filter (Iterated ESKF) framework is used to estimate the system's state variables in real time, including pose, velocity, and sensor bias. An ikdtree data structure is used for map maintenance and local map updates, supporting efficient point cloud insertion, deletion, and querying. Based on the latest pose estimation, the point cloud data is fused into the global map to achieve real-time mapping. Finally, the global pose is processed to reduce accumulated errors, and an updated 3D point cloud map is output. This updated 3D point cloud map is a pre-built prior map containing complete environmental geometry information, which can be used for subsequent robot relocalization, path planning, and navigation tasks.

[0063] In step 102, the robot is controlled to rotate to the target initial posture, and target point cloud data in the target initial posture is acquired.

[0064] The initial target attitude can be represented as (0, 0, yaw). In this initial attitude, the roll and pitch angles are both 0, and the yaw angle is the robot's rotation angle around the Z-axis at the point where the matching score is highest during one rotation. For example, if the robot's rotation angle around the Z-axis is 60° at the point of highest matching score, then the initial target attitude can be represented as (0, 0, 60°). The robot is then controlled to rotate 60 degrees around the Z-axis to achieve the initial target attitude, and the point cloud data collected by the LiDAR configured on the robot at this initial attitude is acquired as the target point cloud data. In other words, this target point cloud data is actually the point cloud data collected by the LiDAR when the robot is in the initial target attitude.

[0065] In step 103, when the current value of the number of segmentations of the target map is 0, the target map is the prior map; when the current value of the number of segmentations of the target map is greater than or equal to 1, the target map is the target sub-map among the multiple sub-maps obtained in the previous segmentation.

[0066] The target map is divided into multiple sub-maps according to a preset segmentation rule. For example, the preset segmentation rule is to divide the target map into four sub-regions along the X and Y axes, which are then divided into four sub-maps. It should be noted that this embodiment only uses the division into four sub-maps as an example. In a specific implementation, it can also be divided into n sub-maps, where n is greater than or equal to 2.

[0067] In step 104, for each sub-map, a matching score is obtained based on the candidate initial positions and target point cloud data in the sub-map, and the sub-map with the highest matching score is selected as the target sub-map.

[0068] Specifically, for each sub-map obtained from the segmentation, a preset point in the sub-map is used as a candidate initial position. The target point cloud data and the prior map are matched to obtain the matching score of the sub-map, and the sub-map with the highest matching score is used as the target sub-map.

[0069] Multiple submaps each have their own candidate initial positions. Therefore, the matching score of each submap can be understood as the matching score of the candidate initial positions within each submap. For example, to determine the target initial position for robot relocalization, multiple candidate initial positions are introduced. To select the best candidate initial position as the target initial position, the matching scores corresponding to each of the robot's multiple candidate initial positions are obtained. Each candidate initial position has a corresponding matching score, and the candidate initial position with the highest matching score is selected as the target initial position. The matching score corresponding to each candidate initial position is the matching score of the submap containing that candidate initial position. A candidate initial position can be represented as (x, y, z), where z is 0, and x and y are the x and y coordinates of a preset point in the submap containing the candidate initial position, respectively.

[0070] In one possible implementation, the preset points in the sub-maps mentioned above are the center points of the sub-maps. Each sub-map typically represents the local structural features of a specific region. Using the center point of the sub-map as the candidate initial position for point cloud matching can fully utilize the unique geometric features of that local region, making the matching process more accurate.

[0071] In one possible implementation, obtaining the matching scores of the multiple initial point cloud data in step 101 above includes the following steps S11 to S12:

[0072] S11: Obtain the initial point cloud data corresponding to the multiple candidate initial poses of the robot.

[0073] The initial point cloud data corresponding to each candidate initial posture is: the point cloud data collected by the LiDAR when the robot is in that candidate initial posture.

[0074] In one possible implementation, acquiring initial point cloud data corresponding to multiple candidate initial poses of the robot includes: controlling the robot to rotate in place at a preset sampling angle, and acquiring initial point cloud data corresponding to each of the multiple candidate initial poses of the robot during the rotation. The preset sampling angle can be pre-set, for example, 60° / s, and the robot is controlled to rotate one full revolution (360°) at a fixed angular velocity of 60° / s, while the LiDAR continuously collects point cloud data during the rotation.

[0075] For example, during the robot's rotation in place, it pauses its rotation every preset sampling angle (e.g., 10°) and acquires point cloud data in the current pose. The robot's rotation can be understood as rotation around the Z-axis, not around the X and Y axes. Under normal circumstances, the robot's chassis will not tilt or roll, so the corresponding roll and pitch are typically 0. That is, during the robot's rotation, the yaw angle is changing, while the pitch and roll angles can both be 0. Based on this, each candidate initial pose can be represented as (0, 0, yaw), where yaw is the degree of rotation around the Z-axis. One rotation around the Z-axis generates 360° / preset sampling angle candidate initial poses. For example, with a preset sampling angle of 10°, one rotation around the Z-axis will generate 36 candidate initial poses.

[0076] S12: For the initial point cloud data corresponding to each candidate initial pose, perform point cloud matching between the initial point cloud data and the prior map to obtain the matching score for each initial point cloud data.

[0077] Specifically, a pre-defined point cloud matching algorithm can be used to perform point cloud matching and obtain a matching score. This pre-defined algorithm can be the Generalized Iterative Closest Point (GICP) matching algorithm. Combining point-to-point and point-to-surface GICP matching algorithms, a probability model (covariance matrix) is introduced to improve the accuracy and robustness of registration. The core idea of ​​GICP is to represent the point cloud as a Gaussian distribution and minimize the Mahalanobis distance between the source and target point clouds during the registration process. The matching score for each initial point cloud data can also be understood as the matching score corresponding to each candidate initial pose, representing the point cloud matching degree between the initial point cloud data acquired by the LiDAR and the prior map under that candidate initial pose. This score can be used to evaluate the point cloud matching quality of the robot under different candidate initial poses. For example, the Iterative Closest Point (ICP) algorithm and the Normal Distributions Transform (NDT) algorithm can also be used for point cloud matching.

[0078] In one possible implementation, the initial point cloud data includes multiple first source points. Step 101 above, which involves obtaining the matching scores of each of the multiple initial point cloud data points, includes steps S21 to S24 as follows. S21 to S24 can also be understood as the specific implementation process of S12 above. The implementation methods of S21 to S24 are described below:

[0079] S21: For each initial point cloud data, obtain the first target transformation matrix between the initial point cloud data and the prior map.

[0080] The first target transformation matrix is ​​the optimal transformation matrix obtained through iterative optimization using the GICP matching algorithm. This step aims to solve for the optimal transformation matrix between the initial point cloud data and the prior map using the GICP matching algorithm, given the candidate initial poses.

[0081] S22: Based on the first target transformation matrix, transform each first source point in the initial point cloud data to obtain the transformed first source point, and search for the first target point in the prior map that is closest to the transformed first source point.

[0082] Specifically, the first target transformation matrix includes a first rotation matrix R1 and a first translation matrix t1. Based on the first target transformation matrix, for each first source point p1 in the initial point cloud data... i After transformation, the first source point can be represented as: R1p1 i +t1. The first source point after the transformation is the first source point after the transformation by the first target transformation matrix. Then, the first source point R1p1 after the transformation can be searched in the prior map Q using the following formula (1). i +t1 is the nearest first target point q1 i :

[0083] q1 i =arg min q1∈Q ‖R1p1 i +t1-q1‖ Formula (1)

[0084] Where q1 i The first source point p1 is searched in the prior map Q based on the first target transformation matrix. i The nearest first target point, q1 represents a point in the prior map Q.

[0085] The first target point can be understood as: among all points on the prior map, the point that is closest to the first source point after transformation by the first target transformation matrix.

[0086] S23: Calculate the first Mahalanobis distance between the transformed first source point and the first target point.

[0087] Mahalanobis distance is a method for measuring the distance between a point and a distribution. It takes into account the covariance structure of the data, thus more accurately reflecting the relative position of data points in multidimensional space. Compared to Euclidean distance, Mahalanobis distance is better suited to the correlation and scale differences between different dimensions.

[0088] For a transformed first source point R1p1 i +t1 and the first target point q1 found in the prior map that is closest to the transformed first source point. i The first source point R1p1 after the transformation i +t1 and the first target point q1 i The first Mahalanobis distance between them can be: in, and p1 i and q1 i The local covariance matrix, where P1 represents the initial point cloud data and Q represents the prior map.

[0089] S24: Determine the matching score for each initial point cloud data based on the first Mahalanobis distance.

[0090] It is understandable that each initial point cloud data includes multiple first source points, and each transformed first source point has a first Mahalanobis distance between it and its corresponding first target point. That is, each transformed first source point corresponds to a first Mahalanobis distance. The average value of the first Mahalanobis distances corresponding to all transformed first source points is used as the matching score of the initial point cloud data.

[0091] For example, the matching score S1 of the initial point cloud data is calculated using the following formula (2):

[0092]

[0093] Where N1 is the total number of the first source points in the initial point cloud data.

[0094] The matching scores of multiple initial point cloud data can be calculated using the above formula (2), and the candidate initial pose corresponding to the initial point cloud data with the highest matching score is taken as the target initial pose.

[0095] For example, to avoid errors caused by calculating the matching score based on a single frame of initial point cloud data at each candidate initial pose, at each candidate initial pose, the matching score can be calculated for multiple frames of initial point cloud data collected by the LiDAR at that candidate initial pose and the prior map. One frame of initial point cloud data corresponds to one matching score, and multiple frames of initial point cloud data will correspond to multiple matching scores. The average of these multiple matching scores is taken as the matching score corresponding to the candidate initial pose.

[0096] In the specific implementation, for each initial point cloud data, the above S21 to S24 are executed to obtain the matching score of each initial point cloud data.

[0097] In one possible implementation, see [reference] Figure 3 Obtain the first target transformation matrix between the initial point cloud data and the prior map, including the following S31 to S35:

[0098] S31: For each first source point in the initial point cloud data, search for the second target point that is closest to the first source point in the prior map based on the first initial transformation matrix.

[0099] In the first iteration, the first initial transformation matrix can be an identity matrix. In subsequent iterations, the first initial transformation matrix is ​​the first candidate transformation matrix obtained after the previous iteration. That is, when the current value of the number of segmentations of the target map is 0, the first initial transformation matrix is ​​an identity matrix; when the current value of the number of segmentations of the target map is not 0, the first initial transformation matrix is ​​the first candidate transformation matrix obtained after the previous segmentation of the target map.

[0100] For example, for each first source point in the initial point cloud data, a first initial transformation matrix is ​​applied to obtain its mapping point in the prior map. Then, the point closest to this mapping point is searched in the prior map, and this searched point is taken as the second target point closest to the first source point found in the prior map based on the first initial transformation matrix. The second target point can be understood as: among all points in the prior map, the point closest to the first source point after transformation by the first initial transformation matrix.

[0101] In some embodiments, to reduce the number of first source points in the initial point cloud data without destroying the point cloud features, the initial point cloud data and the prior map can be first downsampled using voxel filtering to obtain voxel-downsampled initial point cloud data and the prior map. Then, for each first source point in the voxel-downsampled initial point cloud data, a second target point that is closest to the first source point is searched in the voxel-downsampled prior map based on the first initial transformation matrix.

[0102] For example, the initial point cloud data is denoted as P1, the prior map is denoted as Q, and the first initial transformation matrix is ​​denoted as T1.

[0103] For each first source point p1 in P1 i The nearest point q2 is searched in the prior map Q using the following formula (3). i :

[0104] q2 i =arg min q2∈Q ||T1(p1) i )-q2‖ Formula (3)

[0105] Among them, q2 i The first source point p1 is searched in the prior map Q based on the first initial transformation matrix T1.i The nearest second objective point, q2 represents a point in the prior map Q.

[0106] S32: Calculate the local covariance matrix corresponding to each first source point and the local covariance matrix corresponding to each second target point.

[0107] For example, for each first source point, a local point set is extracted within its neighborhood, such as using K-nearest neighbors. This local point set includes the k points closest to the first source point, referred to as the k nearest neighbors. The local surface structure is estimated using a weighted average of the k points in the local point set nearest to the first source point. Principal component analysis is then performed on the local point set of the first source point to obtain its local covariance matrix.

[0108] When calculating the local covariance matrix of the first source point, the k nearest neighbor center of the first source point can be calculated first using the following formula (4), and then the local covariance matrix of the first source point can be calculated using the following formula (5):

[0109]

[0110] in, p1 is the first source point i The k-nearest neighbor center, p1 j p1 i The j-th nearest neighbor among the k nearest neighbors, p1 is the first source point i The local covariance matrix, express The transpose of .

[0111] For example, for each second target point, a local point set is extracted within its neighborhood, such as using K-nearest neighbors. This local point set includes the k points closest to the first target point, which are called the k nearest neighbors of the first target point. The local surface structure is estimated using a weighted average based on the k points in the local point set near the first target point. Principal component analysis is then performed on the local point set of the second target point to obtain its local covariance matrix.

[0112] When calculating the local covariance matrix of the second target point, the k nearest neighbor center point of the second target point can be calculated first using the following formula (6), and then the local covariance matrix of the second target point can be calculated using the following formula (7):

[0113]

[0114] in, For the second target point q2 i The k nearest neighbor center, q2j For q2 i The j-th nearest neighbor among the k nearest neighbors, For the second target point q2 i The local covariance matrix, express The transpose of .

[0115] S33: Determine the first candidate transformation matrix based on each first source point, each second target point, the local covariance matrix corresponding to each first source point, the local covariance matrix corresponding to each second target point, and the preset first objective function; wherein, the first objective function is used to describe the first Mahalanobis distance between the transformed first source point and the second target point.

[0116] The first candidate transformation matrix can be a transformation matrix that minimizes the first objective function.

[0117] For example, the first objective function can be expressed as the following formula (8):

[0118]

[0119] Among them, p1 i Let i be the first source point, and q2 be the source point. i For the search in the prior map, the result is the same as p1. i The nearest second target point, For the second target point q2 i The local covariance matrix, p1 is the first source point i The local covariance matrix, T1(p1) i )=R1p i +t1,T1(p1) i ) is the first source point p1 after the transformation. i T1(p1) i T1 is obtained after transformation by rotation matrix R1 and translation matrix t1. * The first candidate transformation matrix is ​​used to minimize the value of the first objective function.

[0120] For example, the Levenberg-Marquardt (LM) algorithm can be used to solve for the optimal transformation matrix T1. * The LM algorithm is an iterative optimization method for solving nonlinear least squares problems, widely used in data fitting. It combines the advantages of gradient descent and the Gauss-Newton method, exhibiting good convergence and stability when dealing with nonlinear problems.

[0121] S34: If the preset convergence condition is met, the first candidate transformation matrix is ​​used as the first target transformation matrix.

[0122] S35: If the preset convergence condition is not met, use the first candidate transformation matrix as the first initial transformation matrix and return to execute S31.

[0123] The preset convergence conditions can be: the first iteration number reaches the preset iteration number (e.g., 6 times), or the change between the first candidate transformation matrices obtained in at least 2 iterations is less than the first preset change, or the change between the function values ​​of the first objective function calculated in at least 2 iterations is less than the second preset change.

[0124] The first iteration count can be understood as the number of times the first candidate transformation matrix is ​​obtained for each initial point cloud data. For each initial point cloud data, each execution of S31 to S33 yields one first candidate transformation matrix. Therefore, the first iteration count can also be understood as the number of times S31 to S33 are executed. During the accumulation of the first iteration count, each iteration begins with S31 and ends with S33. Therefore, after S33, it can be determined whether the preset convergence condition is met. If the preset convergence condition is not met, S35 is executed to use the first candidate transformation matrix as the first initial transformation matrix, and S31 is returned for execution until the preset convergence condition is met, yielding the first target transformation matrix. The first target transformation matrix includes a first rotation matrix and a first translation matrix. If the preset convergence condition is met, S34 is executed to use the first candidate transformation matrix as the first target transformation matrix.

[0125] In one possible implementation, the target point cloud data includes multiple second source points. For each sub-map, a matching score for the sub-map is obtained based on the candidate initial positions in the sub-map and the target point cloud data, including the following S41 to S44:

[0126] S41: Based on the candidate initial positions and target initial poses in the sub-map, obtain the second target transformation matrix between the target point cloud data and the prior map.

[0127] S42: Based on the second target transformation matrix, transform each second source point in the target point cloud data to obtain the transformed second source point, and search for the third target point in the prior map that is closest to the transformed second source point.

[0128] The second target transformation matrix includes a second rotation matrix R2 and a second translation matrix t2. Based on the second target transformation matrix, for each second source point p2 in the target point cloud data... i After transformation, the transformed second source point can be represented as: R2p2 i +t2. Then, the second source point R2p2 after transformation can be searched in the prior map using the following formula (9). i +t2 is the nearest third target point q3i :

[0129] q3 i =arg min q3∈Q ||R2p2 i +t2-q3‖ Formula (9)

[0130] Among them, q3 i The second source point p2 is searched in the prior map Q based on the second target transformation matrix. i The nearest third target point, q3 represents a point in the prior map Q.

[0131] The third target point can be understood as: among all points on the prior map, the point that is closest to the second source point after transformation by the second target transformation matrix.

[0132] S43: Calculate the second Mahalanobis distance between the transformed second source point and the third target point.

[0133] For a transformed second source point R2p2 i +t2 and the third target point q3, which is closest to the transformed second source point and is found in the prior map. i The transformed second source point R2p2 i +t2 and the third target point q3 i The second Mahalanobis distance between them can be: in, and p2 respectively i and q3 i The local covariance matrix.

[0134] S44: Determine the matching score of the submap based on the second Mahalanobis distance.

[0135] It is understandable that the target point cloud data includes multiple second source points. Each transformed second source point has a second Mahalanobis distance with the third target point. That is, each transformed second source point corresponds to a second Mahalanobis distance. The average of the second Mahalanobis distances corresponding to all transformed second source points is used as the matching score of the sub-map.

[0136] For example, the matching score S2 of the submap is calculated using the following formula (10):

[0137]

[0138] Where N2 is the total number of second source points in the target point cloud data.

[0139] The matching scores of the multiple sub-maps obtained from this segmentation can be calculated using the above formula (10), and the sub-map with the highest matching score is taken as the target sub-map.

[0140] In one possible implementation, see [reference] Figure 4 The second target transformation matrix between the target point cloud data and the prior map is obtained based on the candidate initial positions and the target initial pose in the sub-map, including the following S411 to S416:

[0141] S411: Based on the candidate initial positions and target initial poses in the sub-map, construct the second initial transformation matrix between the target point cloud data and the prior map.

[0142] Taking the i-th sub-map among multiple sub-maps as an example, the center point (x) of the i-th sub-map i ,y i The initial position of the target (x, yaw) has been obtained in the above steps, and the initial position of the target is represented as (0, 0, yaw), (x, yaw) = ... i ,y i (x, 0) and (0, 0, yaw) can form the second initial transformation matrix (x, yaw) between the target point cloud data and the prior map. i ,y i ,0,0,0,yaw).

[0143] S412: For each second source point in the target point cloud data, search for the fourth target point that is closest to the second source point in the prior map based on the second initial transformation matrix.

[0144] In the first iteration, the second initial transformation matrix can be the second initial transformation matrix constructed in S411. In subsequent iterations, the second initial transformation matrix is ​​the second candidate transformation matrix obtained after the previous iteration.

[0145] For example, the target point cloud data is denoted as P2, the prior map is denoted as Q, and the second initial transformation matrix is ​​denoted as T2.

[0146] For each second source point p2 in P2 i The nearest point q4 in Q is searched using the following formula (11). i :

[0147] q4 i =arg min q4∈Q ||T2(p2) i )-q4‖ Formula (11)

[0148] Among them, q4 i The second source point p2 is searched in the prior map Q based on the second initial transformation matrix T2.i The nearest fourth objective point, q4 represents a point in the prior map Q.

[0149] The fourth target point can be understood as: among all points on the prior map, the point that is closest to the second source point after being transformed by the second initial transformation matrix.

[0150] It can be seen that the implementation of S412 is similar to that of S31 above. The difference lies in the fact that the initial transformation matrix and source point cloud may differ, but the prior map used is the same.

[0151] S413: Calculate the local covariance matrix corresponding to each second source point and the local covariance matrix corresponding to each fourth target point.

[0152] For example, the local covariance matrix corresponding to the second source point is calculated using the following formulas (12) and (13):

[0153]

[0154] in, p2 is the second source point i p2 is the k-nearest neighbor center. j p2 i The j-th nearest neighbor among the k nearest neighbors, p2 is the second source point i The local covariance matrix, express The transpose of .

[0155] For example, the local covariance matrix corresponding to the fourth target point is calculated using the following formulas (14) and (15):

[0156]

[0157] in, The fourth target point q4 i The k-nearest neighbor center, q4 j For q4 i The j-th nearest neighbor among the k nearest neighbors, The fourth target point q4 i The local covariance matrix, express The transpose of .

[0158] S414: Determine the second candidate transformation matrix based on each second source point, each fourth target point, the local covariance matrix corresponding to each second source point, the local covariance matrix corresponding to each fourth target point, and the preset second objective function; wherein, the second objective function is used to describe the second Mahalanobis distance between the transformed second source point and the fourth target point.

[0159] For example, the second candidate transformation matrix is ​​the transformation matrix that minimizes the second objective function.

[0160] For example, the second objective function can be expressed as the following formula (16):

[0161]

[0162] Among them, p2 i Let i be the second source point, and q4 be the source point. i For the search in the prior map, the result is the same as p2. i The most recent fourth target point, The fourth target point q4 i The local covariance matrix, p2 is the second source point i The local covariance matrix, T2(p21) i )=R2p i +t2,T2(p2 i ) is the transformed second source point p2 i T2(p2) i T2 is obtained after transformation by rotation matrix R2 and translation matrix t2. * This is the second candidate transformation matrix that minimizes the value of the second objective function. For example, the LM algorithm can be used to solve for the optimal transformation matrix T2. *

[0163] S415: If the preset convergence condition is met, the second candidate transformation matrix is ​​used as the second target transformation matrix.

[0164] S416: If the preset convergence condition is not met, use the second candidate transformation matrix as the second initial transformation matrix and return to execute S412.

[0165] The preset convergence conditions can be: the second iteration number reaches the preset iteration number (e.g., 6 times), or the change between the second candidate transformation matrices obtained in at least 2 iterations is less than the first preset change, or the change between the function values ​​of the second objective function calculated in at least 2 iterations is less than the second preset change.

[0166] The second iteration count can be understood as: for each sub-map obtained from this segmentation, the number of times the second candidate transformation matrix is ​​obtained during the process of determining the matching score of that sub-map. Each execution of S412 to S414 yields one second candidate transformation matrix. Therefore, the second iteration count can also be understood as: the number of times S412 to S414 are executed for each sub-map obtained from this segmentation. In accumulating the second iteration count, each iteration begins at S412 and ends at S414. Therefore, after S414, it can be determined whether the preset convergence condition is met. If the preset convergence condition is not met, S416 is executed to use the second candidate transformation matrix as the second initial transformation matrix, and S412 is returned for execution until the preset convergence condition is met, yielding the second target transformation matrix. The second target transformation matrix includes the second rotation matrix and the second translation matrix. If the preset convergence condition is met, S415 is executed to use the second candidate transformation matrix as the second target transformation matrix.

[0167] In step 105, it is determined whether the iteration termination condition is met. If the iteration termination condition is met, step 106 is executed. The iteration termination condition aims to determine whether to stop further segmentation of the target map to stop iterative matching.

[0168] In one possible implementation, the preset iteration termination conditions include: the matching score of the target sub-map is greater than a preset score threshold; or, the current value of the number of segmentations of the target map is greater than a preset number threshold, that is, the number of segmentations of the prior map is greater than a preset number threshold.

[0169] A preset score threshold can be set in advance. When the matching score of the target submap exceeds this threshold, it indicates that the candidate initial position used in calculating the matching score of the target submap is close enough to the true position to meet the requirements of the initial pose accuracy for subsequent relocalization tasks. Therefore, this candidate initial position can be directly used as the target initial position without further optimization or searching.

[0170] The preset threshold for the number of iterations is also pre-set based on experimental data and actual conditions, for example, 7 times. If the number of times the prior map has been segmented has reached or exceeded this threshold, it indicates that the current sub-map division is already relatively fine, and continuing to perform finer-grained segmentation will hardly significantly improve the matching score. Continuing to iterate at this time will not only increase the computational burden but may also lead to a decrease in efficiency. Therefore, terminating the iteration under this condition helps to avoid invalid calculations and improve the overall speed and efficiency of relocalization. The current value of the number of segmentations can be understood as the number of times the segmentation step has been executed. For example, if step 103 divides the target map into multiple sub-maps, it is a single segmentation, and the number of times step 103 has been executed is the current value of the number of segmentations.

[0171] The number of segmentation attempts can also be understood as the number of iterative matching attempts to match the target point cloud data with the prior map. Each increase in the current value of the segmentation attempt increases the iterative matching attempt by one. In the process of accumulating the iterative matching attempts, each iteration begins at step 103 and ends at step 104. Therefore, as... Figure 2 As shown, after step 104, it can be determined whether the iteration termination condition is met. If the iteration termination condition is met, step 105 is executed; if the iteration termination condition is not met, step 107 is executed.

[0172] Based on the two iteration termination conditions mentioned above, it is possible to dynamically determine whether to continue map segmentation and matching operations while ensuring matching quality, thereby achieving a good balance between matching accuracy and computational efficiency.

[0173] For example, in the first iteration of matching, the current value of the segmentation count of the target map is 0. The target map is the prior map, which is divided into 4 submaps. Submap 'a' has the highest matching score among the 4 submaps, and submap 'a' is the target submap. If the iteration termination condition is not met at this point, the current value of the segmentation count of the target map is incremented by 1, and the current value of the segmentation count is now 1. Submap 'a' is then used as the target map, and the segmentation of submap 'a' continues, further dividing submap 'a' into 4 submaps.

[0174] If submap b has the highest matching score among the four further sub-maps, then submap b is the target submap. If the iteration termination condition is not met at this point, the current value of the target map's segmentation count is incremented by 1, and the current value of the segmentation count is now 2. Submap b is then used as the target map, and the submap b is further segmented into four submaps. The matching scores of the four submaps are then calculated.

[0175] If submap b has the highest matching score among the four further sub-maps, and the iteration termination condition is met, then the sub-maps will not be further subdivided, and the candidate initial position in submap b will be determined as the target initial position.

[0176] In step 106, the initial reference pose is obtained based on the initial position and initial attitude of the target.

[0177] The initial target position is combined with the initial target pose obtained in step 101 to obtain the initial reference pose (x, y, z, roll, pitch, yaw) for relocalization. Here, z = 0, roll and pitch are also 0, x and y are the x and y coordinates of the center point (initial target position) of the target sub-map that satisfies the iteration termination condition, and yaw is the rotation angle of the LiDAR around the Z-axis when the matching score is the highest during one rotation of the LiDAR.

[0178] In the above embodiments, the process of determining the initial reference pose for repositioning includes the following two stages:

[0179] The first stage, coarse orientation matching, aims to reduce attitude angle deviation. The robot rotates 360 degrees in place, and the matching score between the initial point cloud data collected under each candidate initial pose (mainly the yaw angle) and the prior map is recorded. The candidate initial pose corresponding to the initial point cloud data with the highest matching score is selected as the target initial pose (0,0,yaw), reducing attitude angle deviation (mainly yaw angle deviation) and avoiding divergence in the position search in the second stage due to orientation errors, thus reducing the probability of getting trapped in local optima.

[0180] The second stage, position iterative matching, aims to narrow down the search range. The prior map is divided into multiple sub-maps along the X and Y directions. The center point of each sub-map is used as a candidate initial position, and (0,0,yaw) obtained in the first stage is used as the target initial pose. Iterative matching is performed, and the sub-map with the highest score is selected as the target sub-map. By gradually narrowing the search range, the high computational complexity of a global search is avoided.

[0181] In this embodiment, a multi-level search strategy is used to address the dependence on the mapping origin during robot relocalization. First, the robot rotates 360 degrees in place, recording the matching scores in each direction to determine the optimal initial pose angle (primarily the yaw angle). Next, the prior map is divided into four sub-maps along the X and Y directions. The center point of each sub-map is used as a candidate initial position, and matching is performed using the optimal initial pose angle (also referred to as the target initial pose). The sub-map with the highest matching score is recorded. If the iteration termination condition is not met, the sub-map with the highest matching score is further divided into four sub-maps and iteratively matched until the iteration termination condition is met. This scheme improves the relocalization success rate by combining coarse directional matching with iterative position matching, while reducing the dependence on the initial pose.

[0182] In one possible implementation, after the above two stages, an initial reference pose for relocalization can be obtained. Based on this initial reference pose, point cloud matching is performed again to obtain the final transformation matrix from the current frame point cloud to the prior map. Furthermore, a third stage can be included after the above two stages:

[0183] After obtaining the initial reference pose based on the initial target position and initial target attitude, point cloud matching is performed on the current point cloud data collected by the lidar and the prior map based on the initial reference pose to obtain the third target transformation matrix between the current point cloud data and the prior map.

[0184] The current point cloud data is the point cloud data collected by the LiDAR after obtaining the initial reference pose. Point cloud matching can be achieved using the aforementioned GICP matching algorithm, ICP matching algorithm, NDT matching algorithm, etc. The method for determining the third target transformation matrix can refer to the methods for determining the first and second target transformation matrices, with the difference being that, since a relatively accurate initial reference pose has already been obtained, iteration is unnecessary when determining the third target transformation matrix. For example, if the current point cloud data includes multiple third source points, the third target transformation matrix can be determined as follows: Based on the initial reference pose, construct a third initial transformation matrix between the current point cloud data and the prior map; for each third source point in the current point cloud data, search for the fifth target point closest to that third source point in the prior map based on the third initial transformation matrix; calculate the local covariance matrix corresponding to each third source point, and calculate the local covariance matrix corresponding to each fifth target point; determine the third target transformation matrix based on each third source point, each fifth target point, the local covariance matrix corresponding to each third source point, the local covariance matrix corresponding to each fifth target point, and a preset third objective function; where the third objective function describes the third Mahalanobis distance between the transformed third source point and the fifth target point, and the third target transformation matrix is ​​the transformation matrix that minimizes the third objective function.

[0185] The third target transformation matrix can be used as the best estimation transformation matrix between the current point cloud data and the prior map. It is used to transform the currently collected point cloud data from the sensor coordinate system to the coordinate system of the prior map during the robot relocalization process, thereby achieving an accurate estimation of the robot's current pose.

[0186] Figure 5 This is an exemplary flowchart of another point cloud data processing method provided in the embodiments of this application.

[0187] For example, such as Figure 5 As shown, the processing method for this point cloud data includes:

[0188] Step 501: Construct a priori map.

[0189] Step 502: Control the robot to rotate 360° in place, pause the rotation every 10°, perform point cloud matching between the initial point cloud data and the prior map twice, and take the average of the matching scores obtained from the two point cloud matchings as the matching score of the robot in the current direction.

[0190] The final matching score for one direction is the matching score corresponding to one of the candidate initial poses mentioned above. In this example, since the rotation pauses every 10°, after rotating 360°, we can obtain the matching scores corresponding to 36 candidate initial poses, that is, 36 matching scores.

[0191] When performing point cloud matching in each direction, the point cloud data in the LiDAR's buffer is matched with the prior map to calculate the matching score. A preset time can be allowed in each direction to enable the LiDAR to collect multiple frames of point cloud data for multiple point cloud matching operations.

[0192] It should be noted that this example only uses a 10° pause in rotation for point cloud matching. In actual implementation, the rotation can be paused every 5° or every 20°; this embodiment does not impose a specific limitation on this. Furthermore, the number of point cloud matching operations in each direction is only shown as 2 times; in actual implementation, it can be more than 2 times, and this embodiment does not impose a specific limitation on this.

[0193] Step 503: Iterate through the matching scores in all directions, determine the highest matching score, and control the robot to rotate to the direction corresponding to the highest matching score.

[0194] As shown in the example above, the matching score for all directions can be the matching score corresponding to the 36 candidate initial poses. Assuming that the highest matching score among the 36 is achieved when the robot rotates to 60°, then the robot is controlled to rotate to 60°. The pose of the robot when it rotates to 60° is the target initial pose mentioned above, which can be denoted as (0,0,yaw), where yaw = 60° in this example.

[0195] Step 504: Divide the prior map into four sub-maps in the X and Y directions. Use the center point of each sub-map as the initial candidate position and perform two point cloud matching operations. Take the average of the matching scores obtained from the two point cloud matching operations as the final matching score of the sub-map.

[0196] Step 505: Select the sub-map with the highest matching score as the target sub-map, use the target sub-map as the search range for the next step, repeat map segmentation and point cloud matching until the iteration termination condition is met, and obtain the initial reference pose.

[0197] Specifically, the search area can be initialized first. Initializing the search area can be understood as: dividing the prior map into four sub-maps, or four sub-regions, along the X and Y axes; the center point of each sub-region is used as the candidate initial position (x). i ,y i Then, hierarchical matching and filtering are performed on the center point (x, 0) of each sub-region. i ,y iUsing (0,0,yaw) obtained in step 503 as the target initial pose, point cloud matching is performed, and the matching score is calculated. The sub-region with the highest matching score is recorded. This sub-region with the highest matching score is then further divided into four sub-regions along the X and Y directions. The center point of each sub-region is used as a candidate initial position. The matching and filtering process is repeated until the iteration termination condition is met. The center point of the sub-region with the highest matching score obtained when the iteration termination condition is met is used as the target initial position. This target initial position and the target initial pose obtained in step 503 are combined to obtain the initial reference pose.

[0198] Each map segmentation and point cloud matching operation can be understood as a hierarchical matching and filtering process. For example, the first level of matching and filtering refers to: dividing the prior map into four sub-regions along the X and Y axes, and then matching the center point (x, y) of each sub-region. i ,y i Using (0,0,yaw) obtained in step 503 as the initial pose of the target, point cloud matching is performed, and the matching score is calculated. The sub-region with the highest matching score is recorded. The second level of matching and filtering refers to: dividing the sub-region with the highest matching score obtained from the first level of matching and filtering into four sub-regions, and for the center point (x,yaw) of each sub-region... i ,y i Using (0,0,yaw) obtained in step 503 as the initial pose of the target, point cloud matching is performed, and the matching score is calculated. The sub-region with the highest matching score is recorded. The third level of matching and filtering refers to: dividing the sub-region with the highest matching score obtained from the second level of matching and filtering into four sub-regions, and for the center point (x,yaw) of each sub-region... i ,y i Using (0,0,yaw) obtained in step 503 as the initial target pose, point cloud matching is performed, and the matching score is calculated. The sub-region with the highest matching score is recorded. This process is repeated for multiple levels of matching and filtering. The search range is reduced at each level of matching and filtering compared to the previous level, until the iteration termination condition is met.

[0199] It should be noted that step 504 above is only an example of dividing the data into four parts each time. In specific implementations, it can also be divided into two parts, six parts, etc. This embodiment does not make specific limitations on this.

[0200] Step 506: Based on the initial reference pose, perform point cloud matching on the current point cloud data and the prior map to obtain the optimal transformation matrix between the current point cloud data and the prior map, thus completing the pose initialization for relocalization.

[0201] Through the above implementation, the robot can determine its initial reference pose without returning to the mapping origin, improving its flexibility and robustness in complex environments. Separating pose and position processing, the robot's matching scores in each direction (multiple candidate initial poses) are first recorded to determine the optimal initial pose angle (target initial pose). This helps reduce the probability of matching getting stuck in local optima due to pose errors. Then, position search is performed based on the optimal initial pose. In the position search, to avoid incomplete searches caused by random searches across the entire prior map or efficiency issues caused by globally dense searches, a hierarchical search iterative matching method is used. This improves computational complexity while simultaneously filtering the entire prior map, increasing the relocalization success rate.

[0202] Figure 6 This is a schematic diagram of the structure of a robot provided in an embodiment of this application.

[0203] For example, such as Figure 6 As shown, the robot 600 includes a memory 601 and a processor 602. The memory 601 stores executable program code 6011, and the processor 602 is used to call and run the executable program code 6011, so that the robot 600 performs a point cloud data processing method.

[0204] Furthermore, this application also protects an apparatus that may include a memory and a processor, wherein the memory stores executable program code, and the processor is used to call and execute the executable program code to perform a point cloud data processing method provided in this application.

[0205] This embodiment can divide the device into functional modules based on the above method example. For example, each module can correspond to a separate function, or two or more functions can be integrated into one processing module. The integrated module can be implemented in hardware. It should be noted that the module division in this embodiment is illustrative and only represents one logical functional division. In actual implementation, there may be other division methods.

[0206] It should be understood that the apparatus provided in this embodiment is used to perform the above-described point cloud data processing method, and therefore can achieve the same effect as the above-described method.

[0207] When using integrated units, the device may include a processing module and a storage module. When applied to a robot, the processing module can be used to control and manage the robot's movements, while the storage module can support the robot in executing relevant program code.

[0208] The processing module may be a processor or a controller, which can implement or execute various exemplary logic blocks, modules, and circuits shown in conjunction with the disclosure of this application. The processor may also be a combination of functions that implement computing capabilities, such as a combination of one or more microprocessors, a combination of digital signal processing (DSP) and a microprocessor, etc., and the storage module may be a memory.

[0209] In addition, the device provided in the embodiments of this application may specifically be a chip, component or module. The chip may include a connected processor and a memory. The memory is used to store instructions. When the processor calls and executes the instructions, the chip can execute a point cloud data processing method provided in the above embodiments.

[0210] This embodiment also provides a computer-readable storage medium storing computer program code. When the computer program code is run on a computer, the computer executes the above-described related method steps to implement a point cloud data processing method provided in the above embodiment.

[0211] This embodiment also provides a computer program product. When the computer program product is run on a computer, it causes the computer to execute the above-described related method steps to realize the point cloud data processing method provided in the above embodiment.

[0212] In this embodiment, the device, computer-readable storage medium, computer program product, or chip are all used to execute the corresponding methods provided above. Therefore, the beneficial effects they can achieve can be referred to the beneficial effects in the corresponding methods provided above, and will not be repeated here.

[0213] Through the above description of the embodiments, those skilled in the art will understand that, for the sake of convenience and brevity, only the division of the above functional modules is used as an example. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above.

[0214] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0215] The above description is merely a specific embodiment of this application, but the scope of protection of this application 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 this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A point cloud data processing method, characterized in that, Applied to a robot equipped with a lidar system for collecting point cloud data, the method includes: The matching scores of multiple initial point cloud data are obtained, and the candidate initial pose corresponding to the initial point cloud data with the highest matching score is taken as the target initial pose. The multiple initial point cloud data and the robot's multiple candidate initial poses are in one-to-one correspondence, and the matching score represents the degree of matching between the initial point cloud data and the pre-built prior map. Control the robot to rotate to the target's initial posture and acquire the target point cloud data under the target's initial posture; The target map is divided into multiple sub-maps, wherein when the current value of the number of divisions of the target map is 0, the target map is the prior map; For each sub-map, a matching score is obtained based on the candidate initial positions in the sub-map and the target point cloud data, and the sub-map with the highest matching score is taken as the target sub-map; If the iteration termination condition is met, the candidate initial position in the target sub-map is determined as the target initial position; Based on the initial position and initial attitude of the target, the initial reference pose is obtained.

2. The method according to claim 1, characterized in that, The method further includes: If the iteration termination condition is not met, increment the current value of the number of segmentations of the target map by 1; The target sub-map is used as the target map.

3. The method according to claim 1, characterized in that, The initial point cloud data includes multiple first source points, and obtaining the matching scores of each of the multiple initial point cloud data includes: For each of the initial point cloud data, obtain the first target transformation matrix between the initial point cloud data and the prior map; Based on the first target transformation matrix, each first source point in the initial point cloud data is transformed to obtain the transformed first source point, and the first target point closest to the transformed first source point is searched in the prior map. Calculate the first Mahalanobis distance between the transformed first source point and the first target point; Based on the first Mahalanobis distance, determine the matching score for each initial point cloud data.

4. The method according to claim 3, characterized in that, The step of obtaining the first target transformation matrix between the initial point cloud data and the prior map includes: For each first source point in the initial point cloud data, a second target point that is closest to the first source point is searched in the prior map based on the first initial transformation matrix; Calculate the local covariance matrix corresponding to each of the first source points and the local covariance matrix corresponding to each of the second target points; A first candidate transformation matrix is ​​determined based on each first source point, each second target point, the local covariance matrix corresponding to each first source point, the local covariance matrix corresponding to each second target point, and a preset first objective function. The first objective function is used to describe the first Mahalanobis distance between the transformed first source point and the second target point. If the preset convergence condition is met, the first candidate transformation matrix is ​​used as the first target transformation matrix.

5. The method according to claim 4, characterized in that, The method further includes: If the preset convergence condition is not met, the first candidate transformation matrix is ​​used as the first initial transformation matrix; For each first source point in the initial point cloud data, a second target point that is closest to the first source point is searched in the prior map based on the first initial transformation matrix.

6. The method according to claim 1, characterized in that, The target point cloud data includes multiple second source points. For each sub-map, a matching score is obtained based on the candidate initial positions in the sub-map and the target point cloud data, including: Based on the candidate initial positions in the sub-map and the initial pose of the target, obtain the second target transformation matrix between the target point cloud data and the prior map; Based on the second target transformation matrix, each second source point in the target point cloud data is transformed to obtain the transformed second source point, and a third target point that is closest to the transformed second source point is searched in the prior map. Calculate the second Mahalanobis distance between the transformed second source point and the third target point; The matching score of the sub-map is determined based on the second Mahalanobis distance.

7. The method according to claim 6, characterized in that, The step of obtaining the second target transformation matrix between the target point cloud data and the prior map based on the candidate initial positions in the sub-map and the target initial pose includes: Based on the candidate initial positions in the sub-map and the initial pose of the target, a second initial transformation matrix is ​​constructed between the target point cloud data and the prior map; For each second source point in the target point cloud data, a fourth target point that is closest to the second source point is searched in the prior map based on the second initial transformation matrix; Calculate the local covariance matrix corresponding to each of the second source points and the local covariance matrix corresponding to each of the fourth target points; A second candidate transformation matrix is ​​determined based on each second source point, each fourth target point, the local covariance matrix corresponding to each second source point, the local covariance matrix corresponding to each fourth target point, and a preset second objective function. The second objective function is used to describe the second Mahalanobis distance between the transformed second source point and the fourth target point. If the preset convergence condition is met, the second candidate transformation matrix is ​​used as the second target transformation matrix.

8. The method according to claim 7, characterized in that, The method further includes: If the preset convergence condition is not met, the second candidate transformation matrix is ​​used as the second initial transformation matrix; For each second source point in the target point cloud data, a fourth target point that is closest to the second source point is searched in the prior map based on the second initial transformation matrix.

9. The method according to claim 1, characterized in that, After obtaining the initial reference pose based on the initial target position and the initial target attitude, the method further includes: Based on the initial reference pose, point cloud matching is performed on the current point cloud data collected by the lidar and the prior map to obtain a third target transformation matrix between the current point cloud data and the prior map.

10. The method according to claim 1, characterized in that, The iteration termination conditions include: The matching score of the target sub-map is greater than a preset score threshold; or, The current value of the number of segmentations of the target map is greater than the preset number threshold.

11. A robot, characterized in that, The robot includes: Memory, used to store executable program code; A processor for calling and running the executable program code from the memory, causing the robot to perform the method as described in any one of claims 1 to 10.

12. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed, implements the method as described in any one of claims 1 to 10.

Images