Laser odometer method, system and device based on maximum likelihood estimation smoothing

Abstract

The present invention belongs to the field of robotics, and specifically relates to a laser odometer method, system, and device based on maximum likelihood estimation smoothing, which aims to solve the problem of poor data association when the existing laser odometer method that directly uses the original point cloud information applies fixed lag smoothing. The method includes: estimating the first pose of the laser radar sensor's current frame coordinate system in the world coordinate system; calculating the pose of the laser radar sensor's current frame coordinate system in the world coordinate system before smoothing as the second pose, if the current frame is an odd frame, using the second pose as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, otherwise, adding the frame to the smoothing window, calculating the smoothed current frame pose as the corresponding output pose; and updating the smoothing window. The present invention ensures effective data association when the laser odometer method that directly uses the original point cloud information applies fixed lag smoothing.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a laser odometer method, system and device based on maximum likelihood estimation smoothing. Background Art

[0002] With the rapid development of perception, control, artificial intelligence and other technologies, robots have entered people's daily production and life. Real-time estimation of the robot's own posture is an important prerequisite for its successful completion of tasks. The odometer method based on the robot's own sensors for active positioning has received widespread attention. Among them, the visual odometer method based on visual sensors and the laser odometer method based on lidar have become the mainstream. Compared with visual odometers, laser odometers are more robust to changes in illumination. The simplest implementation strategy of laser odometers is to estimate the posture of the current frame by matching the point cloud of the current frame of the lidar with the point cloud of the previous frame, which is called frame-by-frame matching. Frame-by-frame matching is fast, but there is a large cumulative error and low accuracy. An improved solution is to optimize the posture of the current frame by matching the point cloud of the current frame with the local map composed of multiple frames of point clouds before the current frame, which is called frame-map matching. Compared with frame-by-frame matching, frame-map matching takes into account the information of multiple frames of point clouds before the current frame, thereby improving the accuracy of data association and the accuracy of laser odometers, but also causing a decrease in efficiency. In order to balance efficiency and accuracy, researchers considered combining frame-to-frame matching with frame-to-image matching to obtain real-time, low-drift laser odometer results. Representative methods include LOAM (LiDAR Odometry and Mapping), A-LOAM (Advanced LiDAR Odometry and Mapping), etc. In the process of pose estimation, nonlinear optimization algorithms are generally required, including Gauss-Newton algorithms, etc. At the same time, for the three-dimensional point cloud data obtained by the lidar sensor, a common way is to directly use the PCL point cloud library to process the point cloud data (including voxel filtering, clustering, etc.), and the matrix calculations involved are usually processed using the Eigen calculation library (including eigenvalue decomposition, exponential mapping, etc.).

[0003] It should be noted that the laser odometry method that combines frame-by-frame matching and frame-image matching only optimizes the pose of the current frame, without considering the consistency of the odometry trajectory in the local area, and is difficult to cope with long-term pose estimation. In order to solve this problem, researchers began to consider introducing fixed-lag smoothing into the laser odometry to improve the local consistency of the estimated trajectory by simultaneously optimizing the poses of multiple frames within the smoothing window. For existing laser odometry methods that directly use raw point cloud information, such as laser odometry methods based on GICP (Generalized Iterative Closest Point) registration, the data association is poor when fixed-lag smoothing is applied, which makes it difficult to construct constraints between multiple frames within the smoothing window, and it is also difficult to achieve the goal of fixed-lag smoothing to improve the local consistency of the estimated trajectory. Therefore, how to propose a solution to the above problem is a problem that technicians in this field currently need to solve. Summary of the invention

[0004] In order to solve the above problems in the prior art, that is, to solve the problem of poor data association when the existing laser odometer method directly using the original point cloud information applies fixed lag smoothing, the present invention proposes a laser odometer method based on maximum likelihood estimation smoothing, which includes the following steps:

[0005] Step S10, the robot senses the environment through the laser radar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, the laser odometer method based on GICP registration is used to estimate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system as the first pose;

[0006] Step S20, according to the first posture, calculate the posture of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing as the second posture; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second posture is used as the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and return to step S10; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and jump to step S30; wherein the smoothing window is composed of l laser radar frames, l≤l max , l max is the maximum number of lidar frames in the smoothing window;

[0007] Step S30, traverse each voxel in the voxel map, if the number of voxel points is greater than the set number threshold and the geometry corresponding to the voxel is greater than the set geometry threshold, then the voxel is regarded as a qualified voxel; calculate the logarithmic likelihood of all points in each qualified voxel, and construct a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and use the Gauss-Newton algorithm to solve the pose optimization objective function based on maximum likelihood estimation smoothing to obtain the optimal pose correction amount; based on the optimal pose correction amount, combined with the second pose, calculate the smoothed current frame pose as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing;

[0008] Step S40, determining whether the smoothing window is full max Frame, if full l max If the oldest frame is less than l, the oldest frame is removed from the smoothing window, and its corresponding point cloud is marginalized in the voxel map to implement the smoothing window update, and then the process returns to step S10. max The frame directly returns to step S10.

[0009] In some preferred embodiments, if the current frame is an odd frame and the first frame, after obtaining the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, before returning to step S10, a voxel map is first constructed, and the construction process is:

[0010] use The 3D point cloud data of the first frame Transform to the world coordinate system G to obtain the point cloud Then register to the smoothing window Voxel map of Among them, is the second pose corresponding to the first frame; the voxel map registered The process is: traverse all the For the voxel map Point cloud included A voxel of a point in the point cloud The number of points, mean and covariance matrix of the point set composed of the points are added to the voxel;

[0011] Register to voxel map After that, marginalization is performed; the marginalization process is as follows: traverse all the For the jth voxel, the smoothing window stored in the voxel The number of points corresponding to the first frame Mean and the covariance matrix Assign values ​​to the voxel and Then delete and in, and They respectively represent the number of points, mean and covariance matrix corresponding to the point set composed of marginalized points in the j-th voxel.

[0012] In some preferred embodiments, the geometric degree corresponding to the voxel is calculated as follows:

[0013] Traverse all voxels in the candidate voxel set and calculate the mean μ of each voxel b and the covariance matrix Σ b ; Wherein, b represents a voxel index, and the candidate set is a set constructed based on voxels whose voxel points are greater than a set number threshold;

[0014] The covariance matrix Σ is decomposed using the eigenvalue decomposition algorithm. b Perform eigenvalue decomposition and obtain Σ b The three eigenvalues ​​λ 1 ,λ 2 ,λ 3 ; where λ 1 ≥λ 2 ≥λ 3 ;

[0015] Based on the three eigenvalues, calculate the geometric degree σ b :

[0016] In some preferred embodiments, the optimal posture correction amount is obtained by:

[0017] Traversing all voxels in the candidate voxel set, calculating the geometric degree of each voxel; taking the voxels in the candidate voxel set whose geometric degree is greater than a set geometric degree threshold as qualified voxels;

[0018] Calculate the log-likelihood of all points in each qualified voxel, and construct a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all qualified voxels;

[0019] The posture correction amount ΔT in the posture optimization objective function based on maximum likelihood estimation smoothing is k Add a perturbation δT k , And use the Gauss-Newton algorithm to solve and get the optimal perturbation And then through Update the posture correction value ΔT k ;in, represents six-dimensional Euclidean space, represents generalized addition, δφ k and δt k are the disturbance δT k The amount of rotation and translation of

[0020] Based on the updated posture correction, the geometry of each voxel is recalculated, and the above steps are repeated to update the posture correction until the set number of cycles is reached. The last updated output posture correction is used as the optimal posture correction.

[0021] In some preferred embodiments, the pose optimization objective function based on maximum likelihood estimation smoothing is:

[0022]

[0023]

[0024] in, represents the log-likelihood sum of all points in the jth qualified voxel, N is the number of qualified voxels, and w j is the weight of the log-likelihood of the jth qualified voxel, ΔR k , Δt k They represent the rotation matrix and translation vector of the pose correction of the kth frame in the smoothing window respectively. represents the i-th point in the point set consisting of the points in the k-th frame point cloud in the j-th qualified voxel, μ j and Σ j denote the mean and covariance matrix of qualified voxel j, respectively, (·) T represents the matrix transpose, It represents the number of points in the j-th qualified voxel corresponding to the point set consisting of the points belonging to the k-th frame point cloud.

[0025] In some preferred embodiments, based on the optimal posture correction amount corresponding to the lth frame in the smoothing window, combined with the second posture, the smoothed current frame posture is calculated, and the method is:

[0026]

[0027] in, represents the smoothed current frame pose, represents the second pose corresponding to the Kth frame, It represents the optimal posture correction corresponding to the lth frame in the smoothing window.

[0028] The second aspect of the present invention proposes a laser odometer system based on maximum likelihood estimation smoothing, the system comprising: a first pose estimation module, an odd and even frame processing module, a smoothing optimization module, and a smoothing window updating module;

[0029] The first pose estimation module is configured such that the robot perceives the environment through the laser radar sensor, obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, estimates the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system using the laser odometer method based on GICP registration as the first pose;

[0030] The odd and even frame processing module is configured to calculate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing according to the first pose, as the second pose; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second pose is used as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and the first pose estimation module is returned; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and the smoothing optimization module is jumped; wherein the smoothing window is composed of l frames of laser radar frames, l≤l max , l max is the maximum number of lidar frames in the smoothing window;

[0031] The smoothing optimization module is configured to traverse each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the geometry corresponding to the voxel is greater than a set geometry threshold, the voxel is regarded as a qualified voxel; the logarithmic likelihood of all points in each qualified voxel is calculated, and a posture optimization objective function based on maximum likelihood estimation smoothing is constructed by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and the posture optimization objective function based on maximum likelihood estimation smoothing is solved by using the Gauss-Newton algorithm to obtain an optimal posture correction amount; based on the optimal posture correction amount, combined with the second posture, the smoothed current frame posture is calculated as the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing;

[0032] The smoothing window updating module is configured to determine whether the smoothing window is full. max Frame, if full l max Frame, then remove the oldest frame from the smoothing window, and marginalize its corresponding point cloud in the voxel map to implement the smoothing window update, and then return to the first pose estimation module, which is not enough l max The frame is directly returned to the first pose estimation module.

[0033] According to a third aspect of the present invention, a storage device is provided, in which a plurality of programs are stored. The programs are suitable for being loaded and executed by a processor to implement the above-mentioned laser odometer method based on maximum likelihood estimation smoothing.

[0034] In a fourth aspect of the present invention, a processing arrangement is proposed, comprising a processor and a storage device; the processor is adapted to execute various programs; the storage device is adapted to store multiple programs; the programs are adapted to be loaded and executed by the processor to implement the above-mentioned method.

[0035] Beneficial effects of the present invention:

[0036] The laser odometer method based on maximum likelihood estimation smoothing ensures effective data association when the laser odometer method directly using original point cloud information applies fixed lag smoothing, thereby improving the local consistency of the estimated trajectory and the estimation quality of the odometer. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Other features, objects and advantages of the present application will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings.

[0038] Figure 1 It is a flow chart of a laser odometer method based on maximum likelihood estimation smoothing according to an embodiment of the present invention;

[0039] Figure 2 It is a schematic diagram of the framework of a laser odometer system based on maximum likelihood estimation smoothing according to an embodiment of the present invention. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0041] The present application will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the relevant invention, rather than to limit the invention. It is also necessary to explain that, for ease of description, only the parts related to the relevant invention are shown in the accompanying drawings.

[0042] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application may be combined with each other.

[0043] A laser odometer method based on maximum likelihood estimation smoothing of the present invention is as follows: Figure 1 As shown, the method comprises the following steps:

[0044] Step S10, the robot senses the environment through the laser radar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, the laser odometer method based on GICP registration is used to estimate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system as the first pose;

[0045] Step S20, according to the first posture, calculate the posture of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing as the second posture; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second posture is used as the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and return to step S10; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and jump to step S30; wherein the smoothing window is composed of l laser radar frames, l≤l max , l ma x is the maximum number of lidar frames in the smoothing window;

[0046] Step S30, traverse each voxel in the voxel map, if the number of voxel points is greater than the set number threshold and the geometry corresponding to the voxel is greater than the set geometry threshold, then the voxel is regarded as a qualified voxel; calculate the logarithmic likelihood of all points in each qualified voxel, and construct a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and use the Gauss-Newton algorithm to solve the pose optimization objective function based on maximum likelihood estimation smoothing to obtain the optimal pose correction amount; based on the optimal pose correction amount, combined with the second pose, calculate the smoothed current frame pose as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing;

[0047] Step S40, determining whether the smoothing window is full max Frame, if full l max If the oldest frame is less than l, the oldest frame is removed from the smoothing window, and its corresponding point cloud is marginalized in the voxel map to implement the smoothing window update, and then the process returns to step S10. max The frame directly returns to step S10.

[0048] In order to more clearly illustrate the laser odometer method based on maximum likelihood estimation smoothing of the present invention, each step in an embodiment of the method of the present invention is described in detail below.

[0049] Step S10, the robot senses the environment through the laser radar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, the laser odometer method based on GICP registration is used to estimate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system as the first pose;

[0050] In this embodiment, the robot obtains the surrounding environment in the current frame coordinate system L of the laser radar sensor through the laser radar sensor carried by the robot. K 3D point cloud data Where K represents the index of the point cloud frame number, and the laser radar sensor current frame coordinate system L K For right-handed system, L K Origin O K Located at the center of the LiDAR sensor, Z K The axis is perpendicular to the bottom surface of the lidar sensor and points upward. K Axis perpendicular to Z K The axis is aligned with the front of the LiDAR sensor. The LiDAR sensor is preferably Velodyne HDL64. Based on 3D point cloud data The laser odometry method based on GICP registration is used to estimate L K The pose in the world coordinate system G As the first position, is a 3×3 rotation matrix, is a 3×1 translation vector, the world coordinate system G and the first frame coordinate system of the lidar sensor, i.e., L 1 coincidence; when K=1, I 4×4 is the unit matrix. The first pose is solved using the laser odometry method based on GICP registration. For details, please refer to the literature: Kenny Chen, Brett T. Lopez, Ali-akbar Agha-mohammadi, Ankur Mehta. Direct LiDAR Odometry: Fast Localization With Dense Point Clouds. IEEE Robotics and Automation Letters, 7(2), pp. 2000-2007, 2022.

[0051] Step S20, according to the first posture, calculate the posture of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing as the second posture; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second posture is used as the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and return to step S10; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and jump to step S30; wherein the smoothing window is composed of l laser radar frames, l≤l max , l max is the maximum number of lidar frames in the smoothing window;

[0052] In this embodiment, according to the first posture Calculate the position and posture of the current frame coordinate system of the lidar sensor in the world coordinate system before smoothing As the second pose. The specific solution process of the second pose is as follows: in, is the position of the laser radar sensor in the (K-1)th frame coordinate system in the world coordinate system, that is, the first position corresponding to the (K-1)th frame, (·) -1 is the matrix inversion operation, is the output pose of the (K-1)th frame of the laser odometer method based on maximum likelihood estimation smoothing of the present invention, Smoothing Window It consists of l frames of LiDAR frames, denoted by the smoothing window The maximum number of lidar frames in a is l max In the present invention, max The preferred setting is 20. When the number of lidar frames in the smoothing window is less than l max , all lidar frames in the smoothing window are retained, and l is the number of all lidar frames in the smoothing window; otherwise, only the latest l are retained in the smoothing window. max Frame lidar frame, at this time l = l max ; Save a voxel map in the smoothing window Voxel Map It is represented in the world coordinate system G, organized in voxels, where the voxel size is 1.3m×1.3m×1.3m. In a single voxel, for different point sets composed of points in the point cloud from the l-frame lidar frame in the voxel, the number of points corresponding to each point set is maintained separately. Mean and the covariance matrix where k∈{1,2,…,l} is the smoothing window The index of the frame in, j is the voxel index, i is the point index, represents the i-th point in the point set consisting of the points in the k-th frame point cloud in the j-th voxel, (·) T Represents the matrix transpose. In addition, the number of points corresponding to the point set composed of marginal points is maintained in a single voxel Mean and the covariance matrix

[0053] If the current Kth frame is an odd frame, output it directly As the output pose of the laser odometer method of the present invention, if the current frame is not the first frame, directly return to step S10, otherwise, the voxel map To initialize, use The 3D point cloud data of the first frame Transform to the world coordinate system G to obtain the point cloud Then register to the smoothing window Voxel map of Then, edge processing is performed, and then the process returns to step S10;

[0054] If the current Kth frame is an even frame, add it to the smoothing window As The latest frame uses the voxel filtering algorithm in the PCL point cloud library to filter the 3D point cloud data. Downsampling is performed, where the filter voxel size is 0.1m×0.1m×0.1m, and then using Transform the downsampled point cloud to the world coordinate system G to obtain the point cloud Then register to the voxel map middle;

[0055] Transform the point cloud to the world coordinate system G Register to voxel map The specific process is as follows: For the voxel map Point cloud included A voxel of a point in the point cloud The number of points, mean and covariance matrix of the point set composed of the points are added to the voxel; according to the above operation, traverse all the points containing The voxels of the points in the point cloud are then To voxel map When K = 1, only the current frame is registered to the smoothing window. Voxel map of , corresponding to the smoothing window The first frame of , at this time, the marginalization process is as follows: traverse all the containing For the jth voxel, the smoothing window stored in the voxel The number of points corresponding to the first frame Mean and the covariance matrix Assign values ​​to the voxel and Then delete and

[0056] Step S30, traverse each voxel in the voxel map, if the number of voxel points is greater than the set number threshold and the geometry corresponding to the voxel is greater than the set geometry threshold, then the voxel is regarded as a qualified voxel; calculate the logarithmic likelihood of all points in each qualified voxel, and construct a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and use the Gauss-Newton algorithm to solve the pose optimization objective function based on maximum likelihood estimation smoothing to obtain the optimal pose correction amount; based on the optimal pose correction amount, combined with the second pose, calculate the smoothed current frame pose as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing;

[0057] In this embodiment, the specific process of calculating the smoothed current frame pose is as follows:

[0058] Step S301, traverse the voxel map For all voxels in voxel a, if the number of voxel points is sufficient, that is If it does not meet the requirement, the voxel is considered as an unqualified voxel, otherwise the voxel is considered as a candidate voxel, and then a candidate voxel set is obtained, where is the number of points in voxel a belonging to the point cloud of the kth frame, ∈ n is the threshold of the number of points in a voxel, when there is only one frame in the smoothing window, ∈ n The preferred setting is 25, otherwise ∈ n The preferred setting is 50;

[0059] Step S302: traverse all voxels in the candidate voxel set, and for voxel b, calculate the mean value of the voxel and the covariance matrix The covariance matrix Σ is decomposed using the eigenvalue decomposition algorithm in the Eigen computing library. b Perform eigenvalue decomposition and obtain Σ b The three eigenvalues ​​λ 1 ,λ 2 ,λ 3 , where λ 1 ≥λ 2 ≥λ 3 , ΔR k and Δt kare the pose correction values ​​of the kth frame in the smoothing window. The rotation matrix (dimension is 3×3) and translation vector (dimension is 3×1), and are the number of points, mean and covariance matrix corresponding to the point set composed of marginalized points in voxel b, and Respectively represent the number of points, mean and covariance matrix corresponding to the point set consisting of the points in voxel b belonging to the k-th frame point cloud; calculate the geometric degree When the voxel geometry is good, that is, σ b >∈ b Satisfies, the voxel is regarded as a qualified voxel, ∈ b is the geometric threshold, which is preferably set to 0.85 in the present invention. For the jth qualified voxel, the log-likelihood sum of all points in the voxel is calculated: On this basis, the pose optimization objective function based on maximum likelihood estimation smoothing is constructed, as shown in formula (1):

[0060]

[0061] Where N is the number of qualified voxels, w j =σ j is the weight of the log-likelihood of the jth qualified voxel;

[0062] Step S303: ΔT in the objective function shown in formula (1) k Adding Perturbations Right now δφ k and δt k are the disturbance δT k The amount of rotation and translation, is a six-dimensional Euclidean space, is generalized addition, Exp(·) is exponential mapping, thus transforming the original objective function into Euclidean space The equivalent problem in : Directly use the Gauss-Newton algorithm to solve the optimal perturbation Use with Update ΔT k , 1≤k≤l;

[0063] Step S304: Repeat steps S302 to S303 for a total of N it times, and obtain the optimal posture correction amount 1≤k≤l, the number of cycles N in the present invention it It is preferably set to 10, where when the smoothing window contains only one frame, the pose correction amount of the frame is initialized to I 4×4Otherwise, the pose correction of the current frame in the smooth window is initialized to the pose correction of the previous frame in the smooth window, and the pose correction of other frames in the smooth window is directly initialized using the result of the most recent smooth optimization of the frame; based on the optimal pose correction corresponding to the lth frame in the smooth window Calculate the smoothed current frame pose This posture is the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing of the present invention.

[0064] Step S40, determining whether the smoothing window is full max Frame, if full l max If the oldest frame is less than l, the oldest frame is removed from the smoothing window, and its corresponding point cloud is marginalized in the voxel map to implement the smoothing window update, and then the process returns to step S10. max The frame directly returns to step S10;

[0065] In this embodiment, if the frames in the smoothing window are full, max Frame, the oldest frame (i.e. the first frame in the smoothing window) is moved out of the smoothing window; and Marginalize the point cloud corresponding to the oldest frame: traverse the voxel map For all voxels in the oldest frame, for voxel j, the mean of the points corresponding to the oldest frame is Covariance matrix and quantity The mean of the points incorporated into the marginalization Covariance matrix and quantity In the first step, we calculate the temporary mean Then use renew Update with μ use renew Last removed and in, and are the optimal pose correction of the oldest frame in the smoothing window. At the same time, the indexes of other frames in the smoothing window are reduced by one in turn to update the smoothing window, and then return to step S10; if the smoothing window is not full l max The frame directly returns to step S10.

[0066] A laser odometer system based on maximum likelihood estimation smoothing according to a second embodiment of the present invention is as follows: Figure 2 As shown, it includes: a first pose estimation module 100, an odd and even frame processing module 200, a smoothing optimization module 300, and a smoothing window updating module 400;

[0067] The first pose estimation module 100 is configured such that the robot senses the environment through a laser radar sensor, obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, estimates the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system using a laser odometer method based on GICP registration as the first pose;

[0068] The odd and even frame processing module 200 is configured to calculate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing according to the first pose, as the second pose; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second pose is used as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and the first pose estimation module 100 is returned; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and the smoothing optimization module 300 is jumped; wherein, the smoothing window is composed of l frames of laser radar frames, l≤l max , l max is the maximum number of lidar frames in the smoothing window;

[0069] The smoothing optimization module 300 is configured to traverse each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the geometry corresponding to the voxel is greater than a set geometry threshold, the voxel is regarded as a qualified voxel; the logarithmic likelihood of all points in each qualified voxel is calculated, and a pose optimization objective function based on maximum likelihood estimation smoothing is constructed by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and the pose optimization objective function based on maximum likelihood estimation smoothing is solved by using the Gauss-Newton algorithm to obtain an optimal pose correction amount; based on the optimal pose correction amount, in combination with the second pose, the smoothed current frame pose is calculated as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing;

[0070] The smoothing window updating module 400 is configured to determine whether the smoothing window is full. max Frame, if full l max Frame, then remove the oldest frame from the smooth window, and marginalize its corresponding point cloud in the voxel map to implement the smooth window update, and then return to the first pose estimation module 100, which is less than l max The frame is directly returned to the first pose estimation module 100.

[0071] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process and related instructions of the system described above can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.

[0072] It should be noted that the laser odometer system based on maximum likelihood estimation smoothing provided in the above embodiment is only illustrated by the division of the above functional modules. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the modules or steps in the embodiments of the present invention can be decomposed or combined. For example, the modules in the above embodiment can be combined into one module, or further divided into multiple sub-modules to complete all or part of the functions described above. The names of the modules and steps involved in the embodiments of the present invention are only for distinguishing the modules or steps, and are not regarded as improper limitations of the present invention.

[0073] A storage device according to a third embodiment of the present invention stores a plurality of programs, wherein the programs are suitable for being loaded by a processor and implementing the above-mentioned laser odometer method based on maximum likelihood estimation smoothing.

[0074] A processing device according to a fourth embodiment of the present invention comprises a processor and a storage device; the processor is suitable for executing various programs; the storage device is suitable for storing multiple programs; the programs are suitable for being loaded and executed by the processor to implement the above-mentioned laser odometer method based on maximum likelihood estimation smoothing.

[0075] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process and related instructions of the storage device and processing device described above can refer to the corresponding process in the aforementioned method example and will not be repeated here.

[0076] Those skilled in the art should be able to appreciate that the modules and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented with electronic hardware, computer software or a combination of the two, and the programs corresponding to the software modules and method steps can be placed in random access memory (RAM), internal memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disks, removable disks, CD-ROMs, or any other form of storage medium known in the technical field. In order to clearly illustrate the interchangeability of electronic hardware and software, the composition and steps of each example have been generally described in the above description according to the function. Whether these functions are performed in electronic hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but this implementation should not be considered to exceed the scope of the present invention.

[0077] The terms "first", "second", "third", etc. are used to distinguish similar objects rather than to describe or indicate a particular order or sequence.

[0078] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims

1. A laser odometer method based on maximum likelihood estimation smoothing, characterized in that: The method comprises the following steps: Step S10, the robot senses the environment through the laser radar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, the laser odometer method based on GICP registration is used to estimate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system as the first pose; Step S20, according to the first posture, calculate the posture of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing as the second posture; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second posture is used as the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and return to step S10; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and jump to step S30; wherein the smoothing window is composed of l laser radar frames, l≤l max , l max is the maximum number of lidar frames in the smoothing window; Step S30, traverse each voxel in the voxel map, if the number of voxel points is greater than the set number threshold and the geometry corresponding to the voxel is greater than the set geometry threshold, then the voxel is regarded as a qualified voxel; calculate the logarithmic likelihood of all points in each qualified voxel, and construct a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and use the Gauss-Newton algorithm to solve the pose optimization objective function based on maximum likelihood estimation smoothing to obtain the optimal pose correction amount; based on the optimal pose correction amount, combined with the second pose, calculate the smoothed current frame pose as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing; Step S40, determining whether the smoothing window is full max Frame, if full l max If the oldest frame is less than l, the oldest frame is removed from the smoothing window, and its corresponding point cloud is marginalized in the voxel map to implement the smoothing window update, and then the process returns to step S10. max The frame directly returns to step S10.

2. The laser odometer method based on maximum likelihood estimation smoothing according to claim 1, characterized in that: If the current frame is an odd frame and the first frame, after obtaining the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, before returning to step S10, a voxel map is first constructed, and the construction process is: use The 3D point cloud data of the first frame Transform to the world coordinate system G to obtain the point cloud Then register to the smoothing window Voxel map of Among them, is the second pose corresponding to the first frame; the voxel map registered The process is: traverse all the For the voxel map Point cloud included A voxel of a point in the point cloud The number of points, mean and covariance matrix of the point set composed of the points are added to the voxel; Register to voxel map After that, marginalization is performed; the marginalization process is as follows: traverse all the For the jth voxel, the smoothing window stored in the voxel The number of points corresponding to the first frame Mean and the covariance matrix Assign values ​​to the voxel and Then delete and in, and They respectively represent the number of points, mean and covariance matrix corresponding to the point set composed of marginalized points in the j-th voxel.

3. The laser odometer method based on maximum likelihood estimation smoothing according to claim 1, characterized in that: The geometric degree corresponding to the voxel is calculated as: Traverse all voxels in the candidate voxel set and calculate the mean μ of each voxel b and the covariance matrix Σ b ; Wherein, b represents a voxel index, and the candidate set is a set constructed based on voxels whose voxel points are greater than a set number threshold; The covariance matrix Σ is decomposed using the eigenvalue decomposition algorithm. b Perform eigenvalue decomposition and obtain Σ b The three eigenvalues ​​λ1,λ2,λ3; where λ1≥λ2≥λ3; Based on the three eigenvalues, calculate the geometric degree σ b :

4. The laser odometer method based on maximum likelihood estimation smoothing according to claim 1, characterized in that: The optimal posture correction amount is obtained by: Traversing all voxels in the candidate voxel set, calculating the geometric degree of each voxel; taking the voxels in the candidate voxel set whose geometric degree is greater than a set geometric degree threshold as qualified voxels; Calculate the log-likelihood of all points in each qualified voxel, and construct a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all qualified voxels; The posture correction amount ΔT in the posture optimization objective function based on maximum likelihood estimation smoothing is k Add a perturbation δT k , And use the Gauss-Newton algorithm to solve and get the optimal perturbation And then through Update the posture correction value ΔT k ;in, represents six-dimensional Euclidean space, represents generalized addition, δφ k and δt k are the disturbance δT k The amount of rotation and translation of Based on the updated posture correction, the geometry of each voxel is recalculated, and the above steps are repeated to update the posture correction until the set number of cycles is reached. The last updated output posture correction is used as the optimal posture correction.

5. The laser odometer method based on maximum likelihood estimation smoothing according to claim 4, characterized in that: The pose optimization objective function based on maximum likelihood estimation smoothing is: in, represents the log-likelihood sum of all points in the jth qualified voxel, N is the number of qualified voxels, and w j is the weight of the log-likelihood of the jth qualified voxel, ΔR k , Δt k They represent the rotation matrix and translation vector of the pose correction of the kth frame in the smoothing window respectively. represents the i-th point in the point set consisting of the points in the k-th frame point cloud in the j-th qualified voxel, μ j and Σ j denote the mean and covariance matrix of qualified voxel j, respectively, (·) T represents the matrix transpose, It represents the number of points in the j-th qualified voxel corresponding to the point set consisting of the points belonging to the k-th frame point cloud.

6. The laser odometer method based on maximum likelihood estimation smoothing according to claim 5, characterized in that: Based on the optimal posture correction amount corresponding to the lth frame in the smoothing window, combined with the second posture, the smoothed current frame posture is calculated, and the method is: in, represents the smoothed current frame pose, represents the second pose corresponding to the Kth frame, It represents the optimal posture correction corresponding to the lth frame in the smoothing window.

7. A laser odometer system based on maximum likelihood estimation smoothing, characterized in that: The system comprises: a first pose estimation module, an odd and even frame processing module, a smoothing optimization module, and a smoothing window updating module; The first pose estimation module is configured such that the robot perceives the environment through the laser radar sensor, obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, estimates the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system using the laser odometer method based on GICP registration as the first pose; The odd and even frame processing module is configured to calculate the pose of the current frame coordinate system of the laser radar sensor in the world coordinate system before smoothing according to the first pose, as the second pose; if the current frame is an odd frame, the frame is not added to the smoothing window, and the second pose is used as the output pose corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing, and the first pose estimation module is returned; if the current frame is an even frame, the frame is added to the smoothing window, and the corresponding three-dimensional point cloud data is transformed into the world coordinate system and registered in the voxel map, and the smoothing optimization module is jumped; wherein the smoothing window is composed of l frames of laser radar frames, l≤l max , l max is the maximum number of lidar frames in the smoothing window; The smoothing optimization module is configured to traverse each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the geometry corresponding to the voxel is greater than a set geometry threshold, the voxel is regarded as a qualified voxel; the logarithmic likelihood of all points in each qualified voxel is calculated, and a posture optimization objective function based on maximum likelihood estimation smoothing is constructed by maximizing the sum of the logarithmic likelihoods of all points in all qualified voxels, and the posture optimization objective function based on maximum likelihood estimation smoothing is solved by using the Gauss-Newton algorithm to obtain an optimal posture correction amount; based on the optimal posture correction amount, combined with the second posture, the smoothed current frame posture is calculated as the output posture corresponding to the current frame of the laser odometer method based on maximum likelihood estimation smoothing; The smoothing window updating module is configured to determine whether the smoothing window is full. max Frame, if full l max Frame, then remove the oldest frame from the smoothing window, and marginalize its corresponding point cloud in the voxel map to implement the smoothing window update, and then return to the first pose estimation module, which is not enough l max The frame is directly returned to the first pose estimation module.

8. A storage device storing a plurality of programs, characterized in that: The program is suitable for being loaded and executed by a processor to implement the laser odometer method based on maximum likelihood estimation smoothing as described in any one of claims 1 to 6.

9. A processing device, comprising a processor and a storage device; the processor is adapted to execute each program; the storage device is adapted to store a plurality of programs; characterized in that: The program is suitable for being loaded and executed by a processor to implement the laser odometer method based on maximum likelihood estimation smoothing as described in any one of claims 1 to 6.

Images