CN-121374648-B - Robot pose optimization method and system based on self-adaptive voxel rule degree

CN121374648BCN 121374648 BCN121374648 BCN 121374648BCN-121374648-B

Abstract

The invention belongs to the technical field of robot pose optimization. The robot pose optimization method and system based on the self-adaptive voxel regularity are provided, firstly, the original laser point cloud and pose prediction result of the current frame are obtained, voxel division is carried out after motion compensation, voxels with the number exceeding the threshold value of the laser points are continuously segmented to meet the condition, then, the regularity of each sub-voxel is judged, high confidence weight is given to the regularity well, sampling is carried out under high resolution, low confidence weight is given to the regularity poorly, sampling is carried out under low resolution, finally, the processed point cloud is registered with a global map, the least square problem is constructed and solved according to the confidence weight, and the pose is corrected after iterative optimization, so that the optimal result is obtained. The self-adaptive downsampling and weighted registration are realized, clutter cloud interference is reduced, the precision and the robustness of the laser odometer are improved, and the self-adaptive downsampling and weighted registration method can adapt to complex scenes.

Inventors

ZHOU JUN
YAO YUBO
MA YONGXIN
ZHOU CHUNLIANG
LIU WENZHE
ZHANG GUANGXING
LI SHOUYE
Chai Kunqing

Assignees

山东大学

Dates

Publication Date: 20260512
Application Date: 20251224

Claims (10)

1. The robot pose optimization method based on the self-adaptive voxel rule is characterized by comprising the following steps of: Acquiring an original laser point cloud of a current frame and a pose prediction result of the current frame, and performing motion compensation on the original laser point cloud of the current frame according to the pose prediction result to obtain a laser point cloud after motion compensation; performing preliminary voxel division on the laser point cloud after motion compensation to obtain preliminary voxels, traversing each preliminary voxel, and if the number of laser points in a certain preliminary voxel is greater than a first set threshold, continuing voxel division on the preliminary voxel by using an octree until the number of laser points in the divided sub-voxels is less than a second set threshold, thereby finally obtaining a plurality of divided sub-voxels; If the regularity of a sub-voxel is judged to be poor, a laser point Yun Di of the sub-voxel is given a confidence weight, and a voxel with a second resolution is downsampled, wherein the first confidence weight is greater than the second confidence weight, and the first resolution is greater than the second resolution; and carrying out point cloud registration on the laser point cloud after the voxel downsampling of the current frame and the laser point cloud of the global map, endowing a corresponding first confidence weight or a second confidence weight to a least square problem formed by each laser point, solving the least square problem, carrying out iterative optimization to obtain an optimized variable, and carrying out pose correction on a pose prediction result of the current frame according to the optimized variable to obtain the optimal pose of the current frame.
2. The method for optimizing the pose of a robot based on the degree of rules of adaptive voxels of claim 1, Performing motion compensation of an original laser point cloud of a current frame according to the pose prediction result to obtain a laser point cloud after motion compensation, wherein the method comprises the following steps: When the original laser point cloud of the current frame is acquired from the laser radar Recording the time stamp at this time As the starting time of the current frame, at the starting time of the current frame To the end of the current frame In the process of (2), the integration processing is carried out every time the acceleration and the angular velocity measured by the inertial measurement unit are acquired, and when the current frame end time is reached Integrating the integral to the optimal pose of the previous frame to complete the pose prediction of the inertial measurement unit, thereby obtaining the predicted pose of the current frame; According to the predicted pose of the current frame, the starting moment of the current frame is calculated To the end of the current frame Original laser point cloud in between Counter-propagating all original laser point clouds of the current frame Transitioning to end time of current frame Finish the original laser point cloud of the current frame Obtaining the laser point cloud after the motion compensation of the current frame 。
3. The method for optimizing the pose of a robot based on the degree of rules of adaptive voxels of claim 1, Voxel size by voxel method For the laser point cloud after motion compensation Dividing, and after dividing, obtaining each voxel with the size of Traversing the cube voxels of (2); determining voxel size as Number of laser spots within a voxel of (a) Whether or not it is greater than a first set threshold If the judgment is negative, the voxel is considered to be a regular point, the degree of regularity of the voxel is good, and the laser point cloud in the voxel is marked with good labels, if the judgment is positive, the voxel size is considered to be the same Is a suspicious voxel, continuing with The voxel is further divided into eight voxels of size Continuing to divide the eight voxels into eight voxels of size Is traversed by the voxel of (c).
4. The method for optimizing robot pose based on adaptive voxel regularity according to claim 3, Determining voxel size as Number of laser spots within a voxel of (a) Whether or not it is smaller than a second set threshold value If yes, stopping dividing, if no, continuing to Is divided into the average voxel size, and the voxel size is judged again as Number of laser spots within a voxel of (a) Whether or not it is smaller than a second set threshold value Stopping segmentation until the judgment is yes, otherwise, continuously cycling; To this end, through After the secondary judgment and segmentation, the laser point cloud of the current frame is completed Voxel segmentation of (2), voxel size is Is the number of laser spots within each suspicious voxel Less than a second set threshold The judgment of the regularity of the first voxel is completed by taking the number of laser points in the voxel as a judgment condition.
5. The method for optimizing robot pose based on adaptive voxel regularity according to claim 4, wherein, For voxel size of Each laser point number within the suspicious voxel of (2) is less than the second set threshold Traversing sub-voxels of (2), computing covariance matrix of laser point cloud within each sub-voxel And to covariance matrix Decomposing the characteristic values to obtain characteristics, judging according to the characteristic values, and fitting the laser point cloud in the sub-voxels into a plane or a straight line; Marking a sub-voxel as a line voxel if the laser point cloud within the sub-voxel is fit as a straight line, marking a sub-voxel as a plane if the laser point cloud within the sub-voxel is fit as a plane, and marking a pair of voxels as a size Eight voxel sizes within suspicious voxels of (2) are Is traversed by the voxels of (2), and the voxel size is calculated as The duty cycle of line voxels within a voxel of (a) ; Determining the duty cycle of line voxels Whether or not it is greater than a threshold value If yes, then the voxel size is Each line voxel within a voxel of (1) is traversed, assuming that the first The coordinates of the geometric center of each line voxel are Sequentially calculating vectors between geometric centers of voxels of the nearest line and surrounding voxels Included angle between straight line vectors And the vector between the geometric centers of the two line voxels Included angles between the two line voxels and the linear vector of the two line voxels respectively And , Index identification representing the currently processed voxel level; if the angle between the straight line vectors Less than a threshold value And the vector between the geometric centers of the two line voxels Included angles between the two line voxels and the linear vector of the two line voxels respectively And Are all smaller than The two line voxels are considered to be the same straight line, and the clustering of the straight line is performed, and the line clustering number is calculated Unchanged, otherwise, the two line voxels are not considered to be the same straight line, the line clustering number Adding 1, and obtaining the voxel size as Line cluster number within voxels of (a) 。
6. The method for optimizing robot pose based on adaptive voxel regularity according to claim 5, wherein, If the duty ratio of line voxels is determined Less than or equal to a threshold value When the voxel size is Each face voxel within a voxel of (1) is traversed, assuming that the first The coordinates of the geometric center of each face voxel are Sequentially calculating vectors between geometric centers of nearest surface voxels to the surroundings Included angle between normal vectors And a vector between the geometric centers of the two surface voxels Included angle between normal vector of the two surface voxels And ; If the angle between normal vectors Less than a threshold value And the vector between the geometric centers of the two surface voxels Included angle between normal vector of the two surface voxels And Are all in contact with If the difference between the two surface voxels is smaller than the set threshold, the two surface voxels are considered to be the same plane, and the clustering of the plane is performed, and the number of the surface clusters is calculated Unchanged, otherwise, the two surface voxels are not the same plane, the number of surface clusters Adding 1, and obtaining the voxel size as Number of face clusters within voxels of (b) The judgment of the degree of regularity of the second voxel is completed.
7. The method for optimizing robot pose based on adaptive voxel regularity according to claim 6, wherein, When the voxel size is The duty cycle of line voxels within a voxel of (a) Greater than a threshold value Judging the line cluster number Whether or not it is greater than a threshold value If the determination is yes, the voxel size is The degree of regularity of the voxels of (a) is judged to be poor, and if the judgment is negative, the voxel size is The degree of regularity of the voxels of (1) is judged to be good.
8. The method for optimizing robot pose based on adaptive voxel regularity according to claim 6, wherein, When the voxel size is The duty cycle of line voxels within a voxel of (a) Less than or equal to a threshold value Judging the number of face clusters Whether or not it is greater than a threshold value If the determination is yes, the voxel size is The degree of regularity of the voxels of the image is judged to be poor, the distribution of the laser point cloud within the voxel is considered to be irregular, if the judgment is negative, the voxel size is If the degree of regularity of the voxels is determined to be good, the distribution of the laser point cloud in the voxels is considered to be regular, and the third degree of regularity determination of the voxels is completed.
9. The method for optimizing robot pose based on adaptive voxel regularity according to any one of claims 1 to 8, Assigning a corresponding first confidence weight or a second confidence weight to the least square problem formed by each laser point, solving the least square problem, and obtaining an optimized variable after iterative optimization, wherein the method comprises the following steps: The least squares problem constructed includes: ; Wherein, the Representing the preserved laser point in the current voxel, Representing the nearest neighbor laser spot of the global map, Representing the optimization variables to be estimated, Represents the L 2 norm; Representative seek causes Optimization variable taking minimum value ; If the current laser spot Belonging to voxels with good labels, then the first confidence weight To the left of the least squares problem in the ICP iteration process if the current laser spot Belongs to voxels with worse labels, then the second confidence weight is given Multiplying to the left side of the least square problem, constructing the least square problem for all the reserved laser points, and performing iterative optimization to obtain Represents the first Correcting the pose of the iteration for the second time; after iterative optimization, correcting the pose obtained by each iteration Obtaining optimized variable after continuous multiplication ; Calculation of , wherein, Represents the last frame The optimal pose of the frame is determined, Representing the first and second phases obtained by integrating the acceleration and angular velocity of the inertial measurement unit Frame to the first The predicted pose of the frame is determined, Representative of the current Predicted pose of frame Is used for correcting the pose of the human body, As the current first Outputting the optimal pose of the frame by an odometer, and simultaneously outputting the optimal pose of the current frame For pose prediction of the next frame.
10. A robot pose optimization system based on adaptive voxel regularity, comprising: The motion supplementing unit is configured to acquire an original laser point cloud of the current frame and a pose prediction result of the current frame, and perform motion compensation on the original laser point cloud of the current frame according to the pose prediction result to obtain a laser point cloud after motion compensation; The voxel dividing unit is configured to divide the laser point cloud subjected to motion compensation into preliminary voxels, traverse each preliminary voxel, and continuously divide the preliminary voxels by using the octree until the number of laser points in the divided sub-voxels is smaller than a second set threshold value if the number of laser points in a certain preliminary voxel is larger than the first set threshold value, so as to finally obtain a plurality of divided sub-voxels; A rule judging unit configured to judge the rule of each sub-voxel, if the rule of a certain sub-voxel is judged to be good, giving a first confidence weight to the laser point cloud of the sub-voxel and downsampling the sub-voxel with a first resolution, if the rule of a certain sub-voxel is judged to be bad, giving a second confidence weight to the laser point Yun Di of the sub-voxel and downsampling the sub-voxel with a second resolution, wherein the first confidence weight is larger than the second confidence weight, and the first resolution is larger than the second resolution; the pose optimization unit is configured to perform point cloud registration on the laser point cloud after the voxel downsampling of the current frame is completed and the laser point cloud of the global map, assign a corresponding first confidence weight or a second confidence weight to the least square problem formed by each laser point, solve the least square problem, obtain an optimized variable after iterative optimization, and perform pose correction on a pose prediction result of the current frame according to the optimized variable to serve as an optimal pose of the current frame.

Description

Robot pose optimization method and system based on self-adaptive voxel rule degree Technical Field The invention relates to the technical field of robot pose optimization, in particular to a robot pose optimization method and system based on self-adaptive voxel rule degree. Background The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art. Meanwhile, the technology of positioning and map construction (SLAM, simultaneous Localization AND MAPPING) is a core foundation for realizing autonomous navigation and environment perception by intelligent systems such as automatic driving, mobile robots, unmanned aerial vehicles and the like. The three-dimensional laser radar-based SLAM system (3D LiDAR SLAM) can directly acquire environment three-dimensional geometric information which is high in precision and is not influenced by illumination change, and is widely applied to indoor and outdoor complex scenes. The laser inertial odometer is used as the front end of the whole SLAM system and is responsible for estimating the motion pose of the carrier in real time through continuous scanning matching, and the precision and the robustness of the laser inertial odometer directly determine the mapping and positioning performance of the whole SLAM system. However, in practical applications, both outdoor urban roads and indoor complex environments, the smart carrier inevitably encounters a large number of scenes composed of finely divided, cluttered, irregular objects, such as thick leaves, densely parked bicycle/electric vehicle clusters, etc. After the three-dimensional laser radar scanning is carried out on the objects, the generated laser point cloud is generally characterized by high density, disorder, unstructured and extremely irregular spatial distribution. These laser point clouds pose a serious challenge to conventional geometry-based matching laser odometers, often resulting in errors in the registration of the odometers during point cloud registration, thereby causing significant drift in the position and pose estimation of the odometers, and even in failure of the operation of the entire three-dimensional laser SLAM system. In order to solve the problems and improve the precision and the robustness of the laser odometer, the prior solution is generally two methods, namely a point cloud preprocessing method adopting voxelized downsampling. In the running process of the mobile robot, dividing an original and dense current frame laser point cloud acquired by a laser radar into a series of regular three-dimensional space grids (namely voxels), then carrying out uniform resolution downsampling on the point cloud in each voxel to reduce the number of laser points in the voxel, carrying out feature extraction on the point cloud in each non-empty voxel to obtain feature points in the voxel, carrying out point cloud registration on the feature points and the point cloud in a map, and finally estimating the optimal pose of the current frame. Although the method reduces the number of irregular laser points which possibly affect the robustness of the laser odometer in a disordered object scene by voxelizing and downsampling the current frame point cloud, the method also reduces the number of regular laser points which can improve the accuracy of the laser odometer, and the method adopts an equal downsampling processing strategy of 'one-cut' for all voxels, lacks self-adaptive perceptibility of an environment structure, and makes a SLAM system extremely fragile when encountering irregular disordered point cloud. The other method is based on deep learning, firstly, the point cloud of the current frame is segmented, and object types of each segmented point cloud are marked, so that irregular and disordered point clouds are identified, and the weight of the irregular and disordered point clouds in the follow-up point cloud registration and optimization is reduced to reduce the influence of the irregular and disordered point clouds on the accuracy and the robustness of the laser odometer. Although the method has good effect, the method relies on a large amount of model training data, so that the calculation time consumption is increased, and the real-time performance of the laser odometer is reduced. Disclosure of Invention In order to solve the problems of insufficient self-adaptive sensing capability and high cost of a deep learning method in the prior art, the invention provides a robot pose optimization method and system based on self-adaptive voxel regularity, which can judge the regularity of point clouds in voxels, perform self-adaptive voxel downsampling according to different regularity, and finally endow different confidence weights during point cloud registration according to different regularity so as to reduce the negative influence of irregular disordered point clouds on a laser odometer. In order to achieve the above purpo