Robot pose optimization method and system based on adaptive voxel regularity
By using an adaptive voxel regularity method to determine the regularity of point clouds and perform adaptive voxel downsampling, the problem of insufficient accuracy and robustness of laser odometry in complex scenarios is solved, achieving higher accuracy and stability.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SHANDONG UNIV
- Filing Date
- 2025-12-24
- Publication Date
- 2026-05-12
AI Technical Summary
Existing technologies lack the accuracy and robustness of laser odometry in complex scenarios, especially when dealing with dense, cluttered, and irregular objects, leading to pose estimation drift and SLAM system failure.
An adaptive voxel regularity method is adopted. By judging the regularity of the point cloud within a voxel, adaptive voxel downsampling is performed, and different confidence weights are assigned during point cloud registration to reduce the influence of irregular point clouds.
This technology improves the accuracy and stability of laser inertial odometry in complex scenarios, solves the problems of insufficient adaptive sensing capability and high cost of deep learning methods in existing technologies, and achieves higher accuracy and robustness.
Smart Images
Figure CN121374648B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot pose optimization technology, specifically to a robot pose optimization method and system based on adaptive voxel regularity. Background Technology
[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.
[0003] Simultaneous Localization and Mapping (SLAM) technology is the core foundation for autonomous navigation and environmental perception in intelligent systems such as autonomous driving, mobile robots, and drones. Among these, 3D LiDAR SLAM systems are widely used in complex indoor and outdoor scenes due to their ability to directly acquire high-precision, unaffected 3D geometric information about the environment, particularly in light conditions. Laser inertial odometry, as the front end of the entire SLAM system, is responsible for estimating the vehicle's pose in real time through continuous scanning and matching; its accuracy and robustness directly determine the mapping and localization performance of the entire SLAM system. However, in practical applications, whether on outdoor urban roads or in complex indoor environments, intelligent vehicles inevitably encounter scenes composed of numerous fragmented, chaotic, and irregular objects, such as dense foliage or densely parked bicycles / electric vehicles. After being scanned by 3D LiDAR, these objects generate laser point clouds that are typically highly dense, disordered, unstructured, and spatially irregular. These laser point clouds pose a serious challenge to traditional laser odometry based on geometric feature matching, often causing errors in point cloud registration, which in turn leads to significant drift in pose estimation and even causes the entire 3D laser SLAM system to fail.
[0004] To address the aforementioned issues and improve the accuracy and robustness of laser odometry, current solutions typically employ two approaches: one is a voxel-based downsampling point cloud preprocessing method. During the operation of the mobile robot, the raw, dense laser point cloud acquired by the LiDAR in the current frame is divided into a series of regular 3D spatial grids (i.e., voxels). Then, the point cloud in each voxel is downsampled at a uniform resolution to reduce the number of laser points within the voxel. Next, feature extraction is performed on the point cloud within each non-empty voxel to obtain feature points within that voxel. These feature points are then registered with the point cloud in the map, and finally, the optimal pose for the current frame is estimated. While this method reduces the number of irregular laser points in cluttered scenes that might affect the robustness of laser odometry by voxelizing and downsampling the current frame point cloud, it also reduces the number of regular laser points that could improve the accuracy of laser odometry. This "one-size-fits-all" downsampling strategy for all voxels lacks adaptive perception of environmental structure, making the SLAM system exceptionally vulnerable to irregular and cluttered point clouds. Another approach is based on deep learning. This method first segments the point cloud of the current frame, then labels each segmented point cloud with its object type. This identifies irregular and cluttered point clouds and reduces their weight in subsequent point cloud registration and optimization, thereby mitigating their impact on the accuracy and robustness of the laser odometry. While this method is effective, it relies heavily on large amounts of model training data, increasing computation time and reducing the real-time performance of the laser odometry. Summary of the Invention
[0005] To address the shortcomings of existing technologies, such as insufficient adaptive perception capabilities and high costs of deep learning methods, this invention provides a robot pose optimization method and system based on adaptive voxel regularity. This method can determine the regularity of point clouds within voxels, perform adaptive voxel downsampling based on different regularity levels, and finally assign different confidence weights during point cloud registration based on different regularity levels, thereby reducing the negative impact of irregular and cluttered point clouds on laser odometry.
[0006] To achieve the above objectives, the present invention adopts the following technical solution:
[0007] In a first aspect, the present invention provides a robot pose optimization method based on adaptive voxel regularity.
[0008] A robot pose optimization method based on adaptive voxel regularity includes the following process:
[0009] Obtain the original laser point cloud of the current frame and the pose prediction result of the current frame. Perform motion compensation on the original laser point cloud of the current frame based on the pose prediction result to obtain the motion-compensated laser point cloud.
[0010] The motion-compensated laser point cloud is initially divided into voxels to obtain initial voxels. Each initial voxel is traversed. If the number of laser points in a certain initial voxel is greater than a first set threshold, the initial voxel is further divided into voxels using an octree until the number of laser points in the divided sub-voxels is less than a second set threshold, and finally multiple divided sub-voxels are obtained.
[0011] For each sub-voxel, a regularity judgment is performed. If the regularity of a sub-voxel is judged to be good, a first confidence weight is assigned to the laser point cloud of that sub-voxel, and voxel downsampling at a first resolution is performed on that sub-voxel. If the regularity of a sub-voxel is judged to be poor, a second confidence weight is assigned to the laser point cloud of that sub-voxel, and voxel downsampling at a second resolution is performed on that sub-voxel. The first confidence weight is greater than the second confidence weight, and the first resolution is greater than the second resolution.
[0012] After voxel downsampling of the current frame, the laser point cloud is registered with the laser point cloud of the global map. The least squares problem formed by each laser point is assigned a corresponding first confidence weight or second confidence weight. The least squares problem is solved and optimized through iteration to obtain the optimization variables. The pose prediction result of the current frame is corrected according to the optimization variables and taken as the optimal pose of the current frame.
[0013] Secondly, the present invention provides a robot pose optimization system based on adaptive voxel regularity.
[0014] A robot pose optimization system based on adaptive voxel regularity includes:
[0015] The motion compensation unit is configured to: acquire the original laser point cloud of the current frame and the pose prediction result of the current frame, perform motion compensation on the original laser point cloud of the current frame based on the pose prediction result, and obtain the motion-compensated laser point cloud.
[0016] The voxel division unit is configured to: perform preliminary voxel division on the motion-compensated laser point cloud to obtain preliminary voxels; traverse each preliminary voxel; if the number of laser points in a preliminary voxel is greater than a first set threshold, then continue to divide the preliminary voxel using an octree until the number of laser points in the divided sub-voxels is less than a second set threshold, and finally obtain multiple divided sub-voxels.
[0017] The regularity judgment unit is configured to: perform regularity judgment on each sub-voxel; if the regularity of a sub-voxel is judged to be good, then assign a first confidence weight to the laser point cloud of that sub-voxel and perform voxel downsampling at a first resolution on that sub-voxel; if the regularity of a sub-voxel is judged to be poor, then assign a second confidence weight to the laser point cloud of that sub-voxel and perform voxel downsampling at a second resolution on that sub-voxel, wherein the first confidence weight is greater than the second confidence weight, and the first resolution is greater than the second resolution;
[0018] The pose optimization unit is configured to: perform point cloud registration between the laser point cloud after voxel downsampling of the current frame and the laser point cloud of the global map; assign a corresponding first confidence weight or second confidence weight to the least squares problem formed by each laser point; solve the least squares problem and obtain the optimization variables after iterative optimization; and correct the pose prediction result of the current frame based on the optimization variables to obtain the optimal pose of the current frame.
[0019] Compared with the prior art, the beneficial effects of the present invention are:
[0020] This invention innovatively proposes a robot pose optimization method based on adaptive voxel regularity. According to the definition of voxel regularity, through step-by-step judgment, it can accurately identify and label the parts of the laser point cloud with poor regularity. These laser point clouds generally represent dense foliage, dense groups of bicycles / electric vehicles, and other messy point clouds, which have a negative impact on subsequent point cloud registration and iterative optimization. At the same time, it can also identify and label laser point clouds with good voxel regularity. These laser point clouds with more regular geometric distribution have a positive impact on subsequent point cloud registration and iterative optimization.
[0021] This invention enables adaptive voxel downsampling based on the regularity of voxels. In the point cloud preprocessing stage, voxels labeled "good" are downsampled at a higher resolution to preserve the laser point clouds within them, thus retaining to a greater extent the laser point clouds that are beneficial to subsequent point cloud registration and iterative optimization. Voxels labeled "poor" are downsampled at a lower resolution to preserve the laser point clouds within them, thus retaining to a lesser extent the laser point clouds that are harmful to subsequent point cloud registration and iterative optimization.
[0022] This invention improves the accuracy and robustness of laser inertial odometry in point cloud registration. During point cloud registration, based on the label of the voxel to which the laser point belongs, if the label is "good," a higher confidence weight is assigned when constructing the least squares problem to increase the influence of well-regularized laser points in iterative optimization; if the label is "poor," a lower confidence weight is assigned when constructing the least squares problem to reduce the influence of poorly regularized laser points in iterative optimization. Through the above methods, the accuracy and stability of laser inertial odometry can be effectively improved, enabling it to adapt to complex and chaotic scenes without easily collapsing.
[0023] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description
[0024] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.
[0025] Figure 1 A flowchart illustrating a robot pose optimization method based on adaptive voxel regularity, provided as an exemplary embodiment of the present invention;
[0026] Figure 2 A schematic diagram of a voxel segmentation method based on the number of laser points provided as an exemplary embodiment of the present invention;
[0027] Figure 3 A schematic diagram illustrating the calculation of the number of linear clusters and the number of surface clusters, provided as an exemplary embodiment of the present invention;
[0028] Figure 4 A schematic diagram illustrating an adaptive voxel downsampling method based on voxel regularity, provided as an exemplary embodiment of the present invention;
[0029] Figure 5 This is a schematic diagram of a robot pose optimization system based on adaptive voxel regularity, provided as an exemplary embodiment of the present invention. Detailed Implementation
[0030] The present invention will be further described below with reference to the accompanying drawings and embodiments.
[0031] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.
[0032] This implementation proposes a robot pose optimization method based on adaptive voxel regularity, such as... Figure 1 As shown, when receiving the raw laser point cloud of the current frame begins, the timestamp of the laser point cloud at this moment is recorded, and the acceleration and angular velocity of the IMU from this moment onwards are integrated. After receiving the raw laser point cloud of the current frame, the optimal pose corrected in the previous frame is added to the integral of the acceleration and angular velocity of the IMU within the current frame time period to complete the pose prediction of the current frame, and motion compensation for the raw laser point cloud of the current frame is also completed. Subsequently, the laser point cloud is initially divided into voxels. Then, each voxel is traversed. If the number of laser points in a voxel is greater than the first preset threshold set by the user, the voxel is further divided into voxels using an octree until the number of laser points in the voxel after division is less than the second preset threshold set by the user. This completes the voxel segmentation based on the number of laser points. Then, the regularity of each voxel is judged. If the regularity of the voxel is judged to be good, the distribution of the laser point cloud within the voxel is considered to be regular, and a high confidence weight (i.e., the first confidence weight) is assigned to the laser point cloud of the voxel. In the subsequent adaptive voxel downsampling process, voxel downsampling with a higher resolution (i.e., the first resolution) is performed to retain more of these laser points with good geometric features. If the regularity of the voxel is judged to be poor, the distribution of the laser point cloud within the voxel is considered to be irregular, and a lower confidence weight (i.e., the second confidence weight) is assigned to the laser point cloud of the voxel. In the subsequent adaptive voxel downsampling process, voxel downsampling with a lower resolution (i.e., the second resolution) is performed to retain fewer of these laser points without good geometric features. After adaptive voxel downsampling is completed, when the laser point cloud of the current frame is registered with the laser point cloud of the global map using the ICP method, a confidence weight is assigned to the least squares problem formed by each laser point. Then, the least squares problem is solved using the LM method. The optimized variable obtained after iterative optimization is the correction of the predicted pose of the current frame. The predicted pose of the current frame after pose correction is used as the optimal pose of the current frame for odometry output. At the same time, the optimal pose of the current frame after correction is used for pose prediction of the next frame.
[0033] In this implementation, since irregular and chaotic point clouds typically exhibit highly dense, disordered, and unstructured characteristics in local space, the number of laser points, the proportion of feature voxels, and the number of feature clusters within a voxel can be progressively determined based on these characteristics. Using these as definitions, the regularity of the voxel is determined, and this regularity is used as a label for the laser point cloud of that voxel. This provides a reliable basis for subsequent adaptive voxel downsampling and assigning confidence weights to point cloud registration. The specific steps are as follows:
[0034] Step 1: When acquiring the current frame from the LiDAR (let's call it the first frame) The original laser point cloud (frame) Record the timestamp at that moment. This serves as the start time of the current frame. At the start time of the current frame... Until the end of the current frame During the process, whenever high-frequency acceleration measured by the IMU is acquired... and angular velocity Integrate all of them until the end of the current frame is reached. At that time, the integral is accumulated to the optimal pose of the previous frame. Above, the IMU's pose prediction (i.e., forward propagation) is completed to obtain the predicted pose of the current frame. .
[0035] Step 2: Based on the predicted pose of the current frame Set the start time of the current frame Until the end of the current frame The original laser point cloud between Perform backpropagation to obtain all the original laser point clouds of the current frame. Transform to the end of the current frame Complete the processing of the original laser point cloud in the current frame. Motion compensation is performed to obtain the laser point cloud of the current frame after motion compensation. .
[0036] Step 3: As Figure 2 As shown, firstly, the voxel method is used to measure voxel size. Laser point cloud of the current frame after motion compensation Perform segmentation. After segmentation, for each voxel, the size is... The cube voxels are traversed.
[0037] Determine the voxel size as The number of laser points within a voxel Is it greater than the first set threshold? :
[0038] If the judgment is negative, the voxel is considered to contain regular points, the regularity of the voxel is considered good, and the laser point cloud within the voxel is labeled "good"; if the judgment is positive, the voxel size is considered to be... The voxel is a suspicious voxel, continue with The voxel size is further divided into eight voxels of average size. Cube voxels.
[0039] Continuing with the division of these eight voxels, the dimensions are... Traverse the voxels and determine the voxel size. The number of laser points within a voxel Is it less than the second set threshold? If the judgment is yes, then stop the segmentation; if the judgment is no, then continue with... The voxel size is divided into average segments, and the voxel size is determined again. The number of laser points within a voxel Is it less than the second set threshold? The process continues until a positive result is found, at which point the segmentation stops; otherwise, it continues indefinitely.
[0040] Thus, after After several judgments and segmentations, the laser point cloud of the current frame is completed. voxel segmentation, voxel size is The suspected voxels were all composed of a number of laser dots. Less than the second set threshold The voxels were composed of sub-voxels, and the number of laser points within each voxel was used as the criterion to complete the first determination of the regularity of the voxels.
[0041] Step 4: For voxel sizes of The number of laser points within each suspicious voxel is less than a second set threshold. The sub-voxels are traversed. The covariance matrix of the laser point cloud within each sub-voxel is calculated. And for the covariance matrix Perform eigenvalue decomposition to obtain eigenvalues The sorting order is The three characteristic values are judged by formula (1), where Set to 5, this fits the laser point cloud within a child voxel to a plane or a straight line. If the laser point cloud within a child voxel is fitted to a straight line, the child voxel is labeled as a "line voxel," and the eigenvalue is set to 5. The corresponding vector As its linear vector; if the laser point cloud within a sub-voxel is fitted to a plane, the sub-voxel is labeled as a "face voxel", and the eigenvalues are... The corresponding vector As its normal vector.
[0042] (1);
[0043] Step 5: For voxel dimensions of The dimensions of the eight voxels within the suspected voxel are The voxels are traversed, and the voxel size is calculated using formula (2). The percentage of filamentous voxels within voxels ,in, Indicates voxel size as The number of filaments within a voxel. Indicates voxel size as The total number of voxels within a voxel.
[0044] (2);
[0045] Step 6: As Figure 3 As shown, determine the proportion of line elements. Is it greater than the threshold? :
[0046] If the determination is yes, then the voxel size is The process iterates through each line voxel within the voxel. Assume that the first voxel... The coordinates of the geometric center of each line element are Calculate the coordinates of its closest surrounding voxels in turn (assuming the coordinates of the geometric center of one of the voxels are...). The vector between the geometric centers of ) The angle between (Formula (3)) and the linear vector And the vector between the geometric centers of these two line voxels. The angles between the line vectors of the two line elements respectively and If the angle between line vectors Less than the threshold And the vector between the geometric centers of these two line voxels The angles between the line vectors of the two line elements respectively and All less than As shown in formula (4), the two line voxels are considered to be the same straight line, and the line is clustered. The number of line clusters is... If the line remains unchanged, then the two line voxels are considered not to be the same line, and the line cluster number is determined. Increase by 1. After the traversal is complete, the voxel size is obtained. Number of linear clusters within voxels ,in, An index identifier representing the voxel level currently being processed.
[0047] If we determine the proportion of line elements Less than or equal to the threshold When the voxel size is Iterate through each facet voxel within the voxel. Assume that the first voxel... The coordinates of the geometric center of each line element are Calculate the coordinates of its closest facets in turn (assuming the geometric center of one of the linets is...). The vector between the geometric centers of ) The angle between (Formula (3)) and the normal vector And the vector between the geometric centers of the two facet elements. The angle between the normal vectors of these two face elements and If the angle between the normal vectors Less than the threshold And the vector between the geometric centers of these two facet elements The angle between the normal vectors of these two face elements and All close to As shown in formula (5), these two face elements are considered to be on the same plane, and clustering is performed on this plane. The number of face clusters is... If the number of facets remains unchanged, then the two facets are considered not to be on the same plane, and the number of facet clusters is determined. Increase by 1. After the traversal is complete, the voxel size is obtained. Number of face clusters within voxels This completes the second determination of the regularity of the voxels.
[0048] (3);
[0049] and and (4);
[0050] and and (5);
[0051] Step 7: As shown in formula (6), when the voxel size is The percentage of filamentous voxels within voxels Greater than the threshold When determining the number of linear clusters Is it greater than the threshold? If the determination is yes, then the voxel size is The regularity of a voxel is judged as "poor," meaning the distribution of the laser point cloud within that voxel is considered irregular; if judged as "no," the voxel size is... If the regularity of a voxel is judged as "good," then the distribution of the laser point cloud within that voxel is considered regular. When the voxel size is... The percentage of filamentous voxels within voxels Less than or equal to the threshold When determining the number of face clusters Is it greater than the threshold? If the determination is yes, then the voxel size is The regularity of a voxel is judged as "poor," meaning the distribution of the laser point cloud within that voxel is considered irregular; if judged as "no," the voxel size is... If the regularity of a voxel is judged as "good," then the distribution of the laser point cloud within that voxel is considered regular. This completes the third voxel regularity assessment. All voxel regularity assessments are now complete; the laser point cloud in the current frame, measured at voxel size... Each voxel was labeled with a regularity rating of "good" or "poor".
[0052] (6);
[0053] In this implementation, after determining the regularity of the voxels, the dimensions of all voxels are... All voxels were labeled either "good" or "poor". When performing voxel downsampling on the current frame's laser point cloud, instead of using the traditional method to downsample all voxels to the same size, different voxel sizes were used based on the "voxel regularity" label, such as... Figure 4 As shown, specifically: the size of each voxel of the laser point cloud in the current frame is... During voxel traversal, if the voxel regularity label is "good," it is considered a point cloud with rules beneficial to subsequent odometry state estimation, and voxel downsampling is performed at a higher resolution (i.e., the first resolution) to achieve the effect of preserving the regular point cloud to a greater extent; if the voxel regularity label is "poor," it is considered a cluttered point cloud that is harmful to subsequent odometry state estimation, and voxel downsampling is performed at a lower resolution (i.e., the second resolution) to achieve the effect of preserving the cluttered point cloud to a lesser extent; thus, the adaptive voxel downsampling based on voxel regularity is completed.
[0054] After determining the regularity of the voxels, the dimensions of all voxels are: All voxels were labeled either "good" or "poor". Voxels labeled "good" had their laser point clouds assigned a higher confidence weight. Voxels labeled "poor" have their laser point clouds assigned lower confidence weights. .
[0055] After the previous step of adaptive voxel downsampling based on voxel regularity, the laser point cloud of the current frame, once processed, will have its nearest neighbor laser point found in the global map's laser point cloud using the ICP method, and residuals will be constructed. When constructing the least squares problem... This represents the laser points currently retained in the voxel. The nearest neighbor laser point represents the global map. This represents the optimization variable to be estimated. If the current laser point... If a voxel is labeled "good", then a higher confidence weight will be applied. (i.e., the first confidence weight) is multiplied to the left side of the least squares problem in the ICP iteration process; if the current laser point If a voxel is labeled "poor", then a lower confidence weight will be applied. (i.e., the second confidence weight) is multiplied to the left side of the least squares problem in the ICP iteration process. After constructing the least squares problem using formula (7) and performing iterative optimization on all retained laser points, the resulting... Representing the Pose correction in the next iteration.
[0056] After iterative optimization using the LM method, as shown in formula (8), the pose correction obtained in each iteration is... The optimization variable is obtained after multiplication. That is, the current number Frame pose correction.
[0057] As shown in formula (9), where Represents the previous frame The optimal pose of the frame. This represents the acceleration and angular velocity obtained by integrating the IMU from the first... Frame to the The predicted pose of the frame. Representative of the current number Predicted pose of a frame Pose correction. Optimal pose of the frame Left multiplication from the first Frame to the Predicted pose of a frame Get the current number Predicted pose of a frame The current number Predicted pose of a frame Left multiplication After pose correction, the position is obtained , As the current number The optimal pose of the frame is used for odometry output, and the optimal pose of the current frame is also output. It prepares for pose prediction in the next frame.
[0058] (7);
[0059] (8);
[0060] (9);
[0061] in, Represents the optimization variable to be estimated. Represents the L2 norm; Representatives search for Optimization variable that takes the minimum value .
[0062] In summary, the robot pose optimization method based on adaptive voxel regularity proposed in this invention can determine the regularity of point clouds within voxels, perform adaptive voxel downsampling according to different regularity, and finally assign different confidence weights during point cloud registration according to different regularity, so as to reduce the negative impact of irregular and messy point clouds on laser odometry.
[0063] Figure 5 A robot pose optimization system based on adaptive voxel regularity is shown, including:
[0064] The motion compensation unit 501 is configured to: acquire the original laser point cloud of the current frame and the pose prediction result of the current frame, perform motion compensation on the original laser point cloud of the current frame based on the pose prediction result, and obtain the motion-compensated laser point cloud.
[0065] The voxel division unit 502 is configured to: perform preliminary voxel division on the motion-compensated laser point cloud to obtain preliminary voxels, traverse each preliminary voxel, and if the number of laser points in a preliminary voxel is greater than a first set threshold, then continue to divide the preliminary voxel using an octree until the number of laser points in the divided sub-voxels is less than a second set threshold, and finally obtain multiple divided sub-voxels.
[0066] The regularity judgment unit 503 is configured to: perform regularity judgment on each sub-voxel; if the regularity of a sub-voxel is judged to be good, then assign a first confidence weight to the laser point cloud of the sub-voxel and perform voxel downsampling at a first resolution on the sub-voxel; if the regularity of a sub-voxel is judged to be poor, then assign a second confidence weight to the laser point cloud of the sub-voxel and perform voxel downsampling at a second resolution on the sub-voxel, wherein the first confidence weight is greater than the second confidence weight, and the first resolution is greater than the second resolution;
[0067] The pose optimization unit 504 is configured to: perform point cloud registration between the laser point cloud after voxel downsampling of the current frame and the laser point cloud of the global map; assign a corresponding first confidence weight or second confidence weight to the least squares problem formed by each laser point; solve the least squares problem and obtain the optimization variables after iterative optimization; and correct the pose prediction result of the current frame according to the optimization variables to obtain the optimal pose of the current frame.
[0068] It is understood that the aforementioned units can be individually or entirely merged into one or more other units, or some of the units can be further divided into multiple functionally smaller units. This achieves the same operation without affecting the technical effects of the embodiments of this application. The aforementioned units are based on logical functional division. In practical applications, the function of one unit can be implemented by multiple units, or the function of multiple units can be implemented by one unit. In other embodiments of this application, the system may also include other units. In practical applications, these functions can also be implemented with the assistance of other units, and can be implemented collaboratively by multiple units.
[0069] According to another embodiment of this application, the system of this embodiment can be constructed by running a computer program (including program code) capable of performing the steps involved in the corresponding method of the present invention on a general-purpose computing device, such as a computer, which includes processing elements and storage elements such as a central processing unit (CPU), random access memory (RAM), and read-only memory (ROM). The computer program can be recorded on, for example, a computer-readable recording medium, loaded into the aforementioned computing device through the computer-readable recording medium, and run therein.
[0070] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A robot pose optimization method based on adaptive voxel regularity, characterized in that, Includes the following processes: Obtain the original laser point cloud of the current frame and the pose prediction result of the current frame. Perform motion compensation on the original laser point cloud of the current frame based on the pose prediction result to obtain the motion-compensated laser point cloud. The motion-compensated laser point cloud is initially divided into voxels to obtain initial voxels. Each initial voxel is traversed. If the number of laser points in a certain initial voxel is greater than a first set threshold, the initial voxel is further divided into voxels using an octree until the number of laser points in the divided sub-voxels is less than a second set threshold, and finally multiple divided sub-voxels are obtained. For each sub-voxel, a regularity judgment is performed. If the regularity of a sub-voxel is judged to be good, a first confidence weight is assigned to the laser point cloud of that sub-voxel, and voxel downsampling at a first resolution is performed on that sub-voxel. If the regularity of a sub-voxel is judged to be poor, a second confidence weight is assigned to the laser point cloud of that sub-voxel, and voxel downsampling at a second resolution is performed on that sub-voxel. The first confidence weight is greater than the second confidence weight, and the first resolution is greater than the second resolution. After voxel downsampling of the current frame, the laser point cloud is registered with the laser point cloud of the global map. The least squares problem formed by each laser point is assigned a corresponding first confidence weight or second confidence weight. The least squares problem is solved and optimized through iteration to obtain the optimization variables. The pose prediction result of the current frame is corrected according to the optimization variables and taken as the optimal pose of the current frame.
2. The robot pose optimization method based on adaptive voxel regularity as described in claim 1, characterized in that, Motion compensation is performed on the original laser point cloud of the current frame based on the pose prediction result to obtain the motion-compensated laser point cloud, including: When acquiring the original laser point cloud of the current frame from the lidar... Record the timestamp at that moment. , as the start time of the current frame, at the start time of the current frame Until the end of the current frame During the process, each time the acceleration and angular velocity measured by the inertial measurement unit are acquired, integration is performed, and the process continues until the end of the current frame. When the time comes, the integral is accumulated to the optimal pose of the previous frame to complete the pose prediction of the inertial measurement unit and obtain the predicted pose of the current frame. Based on the predicted pose of the current frame, the start time of the current frame is... Until the end of the current frame The original laser point cloud between Perform backpropagation to obtain all the original laser point clouds of the current frame. Transform to the end of the current frame Complete the processing of the original laser point cloud in the current frame. Motion compensation is performed to obtain the motion-compensated laser point cloud for the current frame. .
3. The robot pose optimization method based on adaptive voxel regularity as described in claim 1, characterized in that, Using voxel method with voxel size Laser point cloud after motion compensation Perform segmentation, and after segmentation, assign a size of 1 to each voxel. Traverse the cube voxels; Determine the voxel size as The number of laser points within a voxel Is it greater than the first set threshold? If the judgment is negative, then the voxel is considered to be a regular point, the regularity of the voxel is good, and the laser point cloud in the voxel is labeled as good. If the determination is yes, then the voxel size is considered to be The voxel is a suspicious voxel, continue with The voxel size is further divided into eight voxels of average size. The cube voxel is further divided into eight voxels with dimensions of [missing information]. The voxels are traversed.
4. The robot pose optimization method based on adaptive voxel regularity as described in claim 3, characterized in that, Determine the voxel size as The number of laser points within a voxel Is it less than the second set threshold? If the judgment is yes, then stop the segmentation; if the judgment is no, then continue with... The voxel size is divided into average segments, and the voxel size is determined again. The number of laser points within a voxel Is it less than the second set threshold? The process continues until a positive result is found, at which point the segmentation stops; otherwise, the loop continues indefinitely. Thus, after After several judgments and segmentations, the laser point cloud of the current frame is completed. voxel segmentation, voxel size is The suspected voxels were all composed of a number of laser dots. Less than the second set threshold The voxel composition is determined by the number of laser points within each voxel, which is used as the criterion to complete the first determination of the regularity of the voxel.
5. The robot pose optimization method based on adaptive voxel regularity as described in claim 4, characterized in that, For voxel size The number of laser points within each suspicious voxel is less than a second set threshold. The sub-voxels are traversed, and the covariance matrix of the laser point cloud within each sub-voxel is calculated. And for the covariance matrix Eigenvalue decomposition is performed to obtain features. Based on the eigenvalues, the laser point cloud within the sub-voxel is fitted to a plane or a straight line. If the laser point cloud within a child voxel is fitted to a straight line, the child voxel is labeled as a line voxel; if the laser point cloud within a child voxel is fitted to a plane, the child voxel is labeled as a plane voxel. For voxel sizes of... The dimensions of the eight voxels within the suspected voxel are Traverse the voxels and calculate the voxel size. The percentage of filamentous voxels within voxels ; Determine the proportion of line elements Is it greater than the threshold? If the determination is yes, then the voxel size is Traverse each line voxel within the voxel, assuming that the first voxel is... The coordinates of the geometric center of each line element are Calculate the vector between the geometric center of the nearest surrounding line voxel in turn. Angle between straight vectors and the vector between the geometric centers of these two line elements. The angles between the line vectors of the two line elements respectively and , An index identifier representing the voxel level currently being processed; If the angle between the straight vectors Less than the threshold And the vector between the geometric centers of these two line voxels The angles between the line vectors of the two line elements respectively and All less than If the two line elements are considered to be from the same straight line, then clustering is performed on that line, and the number of line clusters is calculated. If the line remains unchanged, then the two line voxels are considered not to be the same line, and the line cluster number is determined. Increment by 1, and after the traversal is complete, the voxel size is obtained. Number of linear clusters within voxels .
6. The robot pose optimization method based on adaptive voxel regularity as described in claim 5, characterized in that, If we determine the proportion of line elements Less than or equal to the threshold When the voxel size is Traverse each facet voxel within the voxel, assuming that the first voxel is... The coordinates of the geometric center of each facet element are Calculate the vector between the geometric center of the nearest surrounding facet element in turn. Angle between normal vectors and the vector between the geometric centers of the two facet elements. The angle between the normal vectors of these two face elements and ; If the angle between the normal vectors Less than the threshold And the vector between the geometric centers of these two facet elements The angle between the normal vectors of these two face elements and All with If the difference between the two facet elements is less than a set threshold, then the two facet elements are considered to be on the same plane, and clustering is performed on that plane. The number of facet clusters is calculated. constant; Otherwise, it is assumed that the two face elements are not on the same plane, and the number of face clusters is [not specified]. Increment by 1, and after the traversal is complete, the voxel size is obtained. Number of face clusters within voxels The second determination of the regularity of voxels was completed.
7. The robot pose optimization method based on adaptive voxel regularity as described in claim 6, characterized in that, When the voxel size is The percentage of filamentous voxels within voxels Greater than the threshold When determining the number of linear clusters Is it greater than the threshold? If the determination is yes, then the voxel size is The regularity of the voxels was judged to be poor; If the determination is negative, then the voxel size is... The regularity of the voxels was judged to be good.
8. The robot pose optimization method based on adaptive voxel regularity as described in claim 6, characterized in that, When the voxel size is The percentage of filamentous voxels within voxels Less than or equal to the threshold When determining the number of face clusters Is it greater than the threshold? If the determination is yes, then the voxel size is The regularity of a voxel is judged as poor, meaning the distribution of the laser point cloud within that voxel is considered irregular; if the judgment is negative, the voxel size is... If the regularity of a voxel is judged to be good, then the distribution of the laser point cloud within that voxel is considered regular, thus completing the third judgment of the regularity of the voxel.
9. The robot pose optimization method based on adaptive voxel regularity as described in any one of claims 1-8, characterized in that, Assign a corresponding first confidence weight or second confidence weight to the least squares problem formed by each laser point, solve the least squares problem, and obtain the optimization variables after iterative optimization, including: The constructed least squares problems include: ； in, This represents the laser points currently retained in the voxel. The nearest neighbor laser point represents the global map. Represents the optimization variable to be estimated. Represents the L2 norm; Representatives search for Optimization variable that takes the minimum value ; If the current laser point If a voxel is labeled as "good", then the first confidence weight is... Multiply to the left side of the least squares problem in the ICP iteration process; if the current laser point If a voxel is labeled as "poor," then the second confidence weight will be... Multiplying to the left side of the least squares problem, constructing a least squares problem for all retained laser points and iteratively optimizing, the result is... Representing the Pose correction in the next iteration; After iterative optimization, the pose correction obtained in each iteration is... The optimization variable is obtained after multiplication. ; calculate ,in, Represents the previous frame The optimal pose of the frame. Represents the acceleration and angular velocity obtained from the first inertial measurement unit. Frame to the The predicted pose of the frame. Representative of the current number Predicted pose of a frame pose correction, As the current number The optimal pose of the frame is used for odometry output, and the optimal pose of the current frame is also output. Used for pose prediction in the next frame.
10. A robot pose optimization system based on adaptive voxel regularity, characterized in that, include: The motion compensation unit is configured to: acquire the original laser point cloud of the current frame and the pose prediction result of the current frame, perform motion compensation on the original laser point cloud of the current frame based on the pose prediction result, and obtain the motion-compensated laser point cloud. The voxel division unit is configured to: perform preliminary voxel division on the motion-compensated laser point cloud to obtain preliminary voxels; traverse each preliminary voxel; if the number of laser points in a preliminary voxel is greater than a first set threshold, then continue to divide the preliminary voxel using an octree until the number of laser points in the divided sub-voxels is less than a second set threshold, and finally obtain multiple divided sub-voxels. The regularity judgment unit is configured to: perform regularity judgment on each sub-voxel; if the regularity of a sub-voxel is judged to be good, then assign a first confidence weight to the laser point cloud of that sub-voxel and perform voxel downsampling at a first resolution on that sub-voxel; if the regularity of a sub-voxel is judged to be poor, then assign a second confidence weight to the laser point cloud of that sub-voxel and perform voxel downsampling at a second resolution on that sub-voxel, wherein the first confidence weight is greater than the second confidence weight, and the first resolution is greater than the second resolution; The pose optimization unit is configured to: perform point cloud registration between the laser point cloud after voxel downsampling of the current frame and the laser point cloud of the global map; assign a corresponding first confidence weight or second confidence weight to the least squares problem formed by each laser point; solve the least squares problem and obtain the optimization variables after iterative optimization; and correct the pose prediction result of the current frame according to the optimization variables to obtain the optimal pose of the current frame.