CN115047486A - Laser odometer method, system and device based on maximum likelihood estimation smoothing - Google Patents
Laser odometer method, system and device based on maximum likelihood estimation smoothing Download PDFInfo
- Publication number
- CN115047486A CN115047486A CN202210673366.XA CN202210673366A CN115047486A CN 115047486 A CN115047486 A CN 115047486A CN 202210673366 A CN202210673366 A CN 202210673366A CN 115047486 A CN115047486 A CN 115047486A
- Authority
- CN
- China
- Prior art keywords
- pose
- voxel
- frame
- smoothing
- laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000009499 grossing Methods 0.000 title claims abstract description 124
- 238000000034 method Methods 0.000 title claims abstract description 89
- 238000007476 Maximum Likelihood Methods 0.000 title claims abstract description 65
- 238000012937 correction Methods 0.000 claims description 43
- 238000005457 optimization Methods 0.000 claims description 33
- 239000011159 matrix material Substances 0.000 claims description 28
- 230000008569 process Effects 0.000 claims description 19
- 238000012545 processing Methods 0.000 claims description 11
- 238000003860 storage Methods 0.000 claims description 11
- 239000000126 substance Substances 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 8
- 238000000354 decomposition reaction Methods 0.000 claims description 7
- 238000006386 neutralization reaction Methods 0.000 claims description 2
- 230000006870 function Effects 0.000 description 20
- 238000013507 mapping Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000005305 interferometry Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 241000486463 Eugraphe sigma Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000008520 organization Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Abstract
The invention belongs to the technical field of robots, and particularly relates to a laser odometry method, a system and a device based on maximum likelihood estimation smoothing, which aim to solve the problem of poor data association when the existing laser odometry method directly utilizing original point cloud information is applied to fixed lag smoothing. The method comprises the following steps: estimating a first position of a current frame coordinate system of the laser radar sensor under a world coordinate system; calculating the pose of the current frame coordinate system of the laser radar sensor before smoothing in the world coordinate system, and taking the pose as a second pose, if the current frame is an odd frame, taking the second pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing, and if not, adding the frame into a smoothing window, and calculating the pose of the current frame after smoothing as the corresponding output pose; and updating the smooth window. The invention ensures that the laser odometer method directly utilizing the original point cloud information is effectively associated with data when fixed lag smoothing is applied.
Description
Technical Field
The invention relates to the technical field of robots, in particular to a laser odometer method, a system and a device based on maximum likelihood estimation smoothing.
Background
With the rapid development of technologies such as perception, control, artificial intelligence and the like, robots have entered the daily production and life of people. The real-time estimation of the pose of the robot is an important premise for the robot to successfully complete tasks. An odometry method for performing active positioning according to a sensor carried by a robot itself is receiving general attention, and among them, a visual odometry method based on a visual sensor and a laser odometry method based on a laser radar are the mainstream. Laser odometers are more robust to illumination variations than visual odometers. The simplest implementation strategy of the laser odometer is to estimate the pose of a current frame by matching the point cloud of the current frame with the point cloud of a previous frame of the laser radar, which is called frame matching. The frame matching speed is high, but large accumulated errors exist, and the precision is low. One improvement scheme is to match the point cloud of the current frame with a local map formed by multiple frame point clouds before the current frame to optimize the pose of the current frame, which is called frame map matching. Compared with frame matching, the frame image matching considers the information of multi-frame point clouds before the current frame, so that the accuracy of data association is improved, the precision of the laser odometer is improved, and meanwhile, the efficiency is reduced. In order to balance efficiency and accuracy, researchers consider combining frame-to-frame matching with frame-to-map matching to obtain real-time, low-drift laser Odometry results, and representative methods include loam (LiDAR interferometry and mapping), a-loam (advanced LiDAR interferometry and mapping), and the like. In the attitude estimation process, a nonlinear optimization algorithm is generally required, including a Gauss-Newton algorithm and the like; meanwhile, for three-dimensional point cloud data acquired by a laser radar sensor, a common way is to directly use a PCL point cloud library to process the point cloud data (including voxel filtering, clustering and the like), and for related matrix calculation, use an Eigen calculation library to process the point cloud data (including eigenvalue decomposition, exponential mapping and the like).
It should be noted that the laser odometer method combining frame-frame matching and frame-image matching only optimizes the pose of the current frame, does not consider the consistency of the odometer track in the local area, and is difficult to cope with long-time pose estimation. To address this problem, researchers began considering the introduction of Fixed-Lag Smoothing (Fixed-Lag Smoothing) into laser odometers to improve local consistency of estimated trajectories by simultaneously optimizing multi-frame poses within the Smoothing window. For the existing laser odometry method directly using original point cloud information, such as the laser odometry method based on gicp (generalized Iterative closed point) registration, the data association is poor when fixed lag smoothing is applied, so that it is difficult to construct constraints among multiple frames in a smoothing window, and it is also difficult to achieve the goal of improving the local consistency of the estimated track by fixed lag smoothing. Therefore, how to propose a solution to the above problems is a problem that needs to be solved by those skilled in the art.
Disclosure of Invention
In order to solve the above problems in the prior art, that is, to solve the problem that the data association is poor when the existing laser odometer method directly using the original point cloud information applies the fixed lag smoothing, the invention provides a laser odometer method based on the maximum likelihood estimation smoothing, which comprises the following steps:
step S10, the robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment under the current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data to serve as a first pose;
step S20, calculating the pose of the current frame coordinate system of the laser radar sensor before smoothing in the world coordinate system according to the first pose to serve as a second pose; if the current frame is an odd frame, the methodThe frame is not added with a smoothing window, the second pose is used as the output pose corresponding to the current frame of the laser odometry method based on the maximum likelihood estimation smoothing, and the step S10 is returned; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, then registering the three-dimensional point cloud data into a voxel map, and turning to the step S30; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l max Is the maximum number of lidar frames in the smoothing window;
step S30, traversing each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the corresponding geometry of the voxel is greater than a set geometry threshold, taking the voxel as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
step S40, judging whether the smooth window is full max Frame, if full l max Removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, and returning to the step S10 until the frame is less than l max The frame returns directly to step S10.
In some preferred embodiments, if the current frame is an odd frame and is a first frame, after obtaining the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing, before returning to step S10, a voxel map is constructed, and the construction process is as follows:
use of
Three-dimensional point cloud data of a first frame
Transforming to world coordinate system G to obtain point cloud
And then registers with the smooth window
Voxel map of
Performing the following steps; wherein the content of the first and second substances,
a second pose corresponding to the first frame; registered voxel map
The process comprises the following steps: traverse all containers
For a voxel map
Including the point cloud
A voxel of the point in (1), belonging to the point cloud in the voxel
The number of points, the mean and the covariance matrix of the point set formed by the points of (a) are added to the voxel;
registration to voxel map
After the neutralization, performing marginalization treatment; the marginalization process is as follows: traverse all containers
For the jth voxel, a smoothing window stored in that voxel is used
Number of points corresponding to inner first frame
Mean value
Sum covariance matrix
Respectively assigned to the voxels
And
then delete
And
wherein the content of the first and second substances,
and
respectively representing the point number, the mean value and the covariance matrix corresponding to the point set formed by the marginalized points in the jth voxel.
In some preferred embodiments, the voxel-corresponding geometry is calculated by:
traversing all voxels in the candidate voxel set, and calculating the mean value mu of each voxel b Sum covariance matrix Σ b (ii) a B represents a voxel index, and the candidate set is a set constructed on the basis of voxels of which the number of voxel points is greater than a set number threshold;
using eigenvalue decomposition algorithm to covariance matrix sigma b Carrying out characteristic value decomposition to obtain sigma b Three characteristic values of (a) 1 ,λ 2 ,λ 3 (ii) a Wherein λ is 1 ≥λ 2 ≥λ 3 ；
Calculating the geometric degree sigma based on the three characteristic values b ：
In some preferred embodiments, the optimal pose correction amount is obtained by:
traversing all voxels in the candidate voxel set, and calculating the geometric degree of each voxel; taking the voxels with the geometry larger than a set geometry threshold value in the candidate voxel set as qualified voxels;
calculating the log-likelihood of all points in each qualified voxel, and constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels;
optimizing a pose correction Δ T in the pose optimization objective function for maximum likelihood estimate smoothing k Adding perturbation delta T k ，
And solving by using Gauss-Newton algorithm to obtain optimal disturbance
And then pass through
Updating pose correction Δ T k (ii) a Wherein the content of the first and second substances,
a six-dimensional euclidean space is represented,
denotes the generalized addition, δ φ k And δ t k Respectively, disturbance δ T k The amount of rotation and translation of;
and recalculating the geometric degree of each voxel based on the updated pose correction amount, circularly updating the pose correction amount until the set cycle number is reached, and taking the finally updated pose correction amount as the optimal pose correction amount.
In some preferred embodiments, the pose optimization objective function based on maximum likelihood estimation smoothing is:
wherein, the first and the second end of the pipe are connected with each other,
representing the sum of log-likelihood of all points in the jth qualifying voxel, N being the number of qualifying voxels, w j Weight of log likelihood, Δ R, for the jth qualifying voxel k 、Δt k A rotation matrix and a translation vector respectively representing the pose correction amount of the k-th frame within the smoothing window,
represents the ith point, mu, in a point set composed of points belonging to the kth frame point cloud in the jth qualified voxel j Sum-sigma j Means and covariance matrices for qualifying voxels j, respectively, (.) T The transpose of the matrix is represented,
and representing the point number corresponding to a point set formed by the points belonging to the point cloud of the kth frame in the jth qualified voxel.
In some preferred embodiments, the pose correction amount of the current frame after smoothing is calculated based on the optimal pose correction amount corresponding to the ith frame in the smoothing window and in combination with the second pose, and the method includes:
wherein the content of the first and second substances,
representing the pose of the current frame after smoothing,
showing the corresponding second pose of the Kth frame,
and representing the optimal pose correction value corresponding to the frame I in the smooth window.
In a second aspect of the present invention, a laser odometer system based on maximum likelihood estimation smoothing is proposed, the system comprising: the device comprises a first bit position estimation module, a parity frame processing module, a smooth optimization module and a smooth window updating module;
the first pose estimation module is configured to sense the environment through a laser radar sensor by the robot and acquire three-dimensional point cloud data of the surrounding environment under a current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data to serve as a first pose;
the parity frame respectively processing module is configured to calculate a pose of the current frame coordinate system of the laser radar sensor before smoothing in a world coordinate system according to the first pose to serve as a second pose; if the current frame is an odd frame, the frame is not added with a smoothing window, the second pose is used as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing, and the output pose is returned to the first pose estimation module; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, and registering the three-dimensional point cloud data into a voxel mapSkipping to a smooth optimization module; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l max Is the maximum number of lidar frames in the smoothing window;
the smooth 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 corresponding geometry of the voxel is greater than a set geometry threshold, the voxel is taken as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
the smooth window updating module is configured to judge whether the smooth window is full of l max Frame, if full l max Frame, removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, returning to the first pose estimation module, and keeping the first pose estimation module until the frame is less than l max The frame is directly returned to the first pose estimation module.
In a third aspect of the invention, a storage device is provided, in which a plurality of programs are stored, the programs being adapted to be loaded and executed by a processor to implement the above-described maximum likelihood estimation based smoothing laser odometry method.
In a fourth aspect of the invention, a processing arrangement is provided, comprising a processor, a storage device; a processor adapted to execute various programs; a storage device adapted to store a plurality of programs; the program is adapted to be loaded and executed by a processor to implement the method as described above.
The invention has the beneficial effects that:
the laser odometer method based on the maximum likelihood estimation smoothing ensures that the laser odometer method directly utilizing the original point cloud information is effectively associated with data when fixed lag smoothing is applied, thereby improving the local consistency of the estimated track and improving the estimation quality of the odometer.
Drawings
Other features, objects and advantages of the present application will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, made with reference to the accompanying drawings.
FIG. 1 is a schematic flow diagram of a laser odometry method based on maximum likelihood estimation smoothing in accordance with an embodiment of the invention;
FIG. 2 is a block diagram of a laser odometer system with maximum likelihood estimation based smoothing according to an embodiment of the invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The present application will be described in further detail with reference to the following drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the relevant invention and not restrictive of the invention. It should be noted that, for convenience of description, only the portions related to the related invention are shown in the drawings.
It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict.
The invention relates to a laser odometer method based on maximum likelihood estimation smoothing, which comprises the following steps as shown in figure 1:
step S10, the robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment under the current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data to serve as a first pose;
step S20, calculating the pose of the current frame coordinate system of the laser radar sensor before smoothing in the world coordinate system according to the first pose to serve as a second pose; if the current frame is an odd frame, the frame is not added with a smoothing window, the second pose is used as the output pose corresponding to the current frame of the laser odometry method based on the maximum likelihood estimation smoothing, and the step S10 is returned; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, then registering the three-dimensional point cloud data into a voxel map, and turning to the step S30; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l ma x is the maximum number of lidar frames in the smoothing window;
step S30, traversing each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the corresponding geometry of the voxel is greater than a set geometry threshold, taking the voxel as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
step S40, judging whether the smooth window is full max Frame, if full l max Removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, and returning to the step S10 until the frame is less than l max The frame returns directly to step S10.
In order to more clearly illustrate the maximum likelihood estimation smoothing-based laser odometry method of the present invention, the following is a detailed description of the steps in one embodiment of the method of the present invention.
Step S10, the robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment under the current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data to serve as a first pose;
in this embodiment, the robot obtains the current frame coordinate system L of the surrounding environment of the laser radar sensor through the carried laser radar sensor K Underlying three-dimensional point cloud data
Wherein K represents the index of the point cloud frame number, and the current frame coordinate system L of the laser radar sensor K Is a right-handed system, L K Origin O of K Located in the center of the lidar sensor, Z K The axis is perpendicular to the bottom surface of the laser radar sensor and is upward in direction, X K Axis perpendicular to Z K The axis is coincident with the front of the lidar sensor, which is preferably Velodyne HDL 64. Based on three-dimensional point cloud data
L is estimated by utilizing a laser odometer method based on GICP registration K Pose under world coordinate system G
As a first posture, wherein
Is a 3 x 3 rotation matrix and,
is a translation vector of 3 × 1, a world coordinate system G and a first frame coordinate system L of the laser radar sensor 1 Overlapping; when the K is equal to 1,
I 4×4 is a unit array. The first attitude is solved by using a laser odometer method based on GICP registration, and the specific process is detailed in documents: kenny Chen, Brett T.Lopez, Ali-akbar Agha-mohammadi, Ankur Mehta.direct LiDAR Odometer, Fast Localization With depth detection points, IEEE Robotics and Automation Letters,7(2), pp.2000-2007,2022.
Step S20, calculating the pose of the current frame coordinate system of the laser radar sensor before smoothing in the world coordinate system according to the first pose to serve as a second pose; if the current frame is an odd frame, the frame is not added with a smoothing window, the second pose is used as the output pose corresponding to the current frame of the laser odometry method based on the maximum likelihood estimation smoothing, and the step S10 is returned; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, then registering the three-dimensional point cloud data into a voxel map, and turning to the step S30; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l max Is the maximum number of lidar frames in the smoothing window;
in this embodiment, according to the first position
Calculating to obtain the pose of the laser radar sensor before smoothing under the world coordinate system
As a second pose. The second attitude concrete solving process is as follows:
wherein, the first and the second end of the pipe are connected with each other,
is the pose of the (K-1) th frame coordinate system of the laser radar sensor under the world coordinate system, namely the first pose corresponding to the (K-1) th frame, (-) -1 Is a matrixThe operation of the inversion is carried out,
to estimate the output pose of the smoothed laser odometry method frame (K-1) of the present invention based on maximum likelihood,
smooth window
Consisting of l frames of lidar frames, recording a smooth window
The maximum number of lidar frames in (1) is l max In the present invention max Preferably 20, when the number of lidar frames in the smoothing window is less than l max All the laser radar frames in the smoothing window are reserved, and at the moment, l is the number of all the laser radar frames in the smoothing window; otherwise, only the latest l is kept in the smoothing window max Frame lidar frame, when l is l max (ii) a Saving a voxel map in a smoothing window
Voxel map
Representing a organization in voxels with a voxel size of 1.3m x 1.3m under the world coordinate system G. In a single voxel, respectively maintaining the point number corresponding to each point set for different point sets formed by the points of the point cloud in the voxel from the laser radar frame of the frame I
Mean value
Sum covariance matrix
Where k ∈ {1,2, …, l } is a smooth window
The index of the frame within, j is the voxel index, i is the point index,
represents the ith point in a point set formed by the points belonging to the point cloud of the kth frame in the jth voxel, (. DEG) T The matrix transposition is represented, and the number of points corresponding to the point set formed by the marginalized points is maintained in a single voxel
Mean value
Sum covariance matrix
If the current Kth frame is an odd frame, directly outputting
As the output pose of the laser odometry method of the invention, if the current frame is not the first frame, the process directly returns to the step S10, otherwise, the voxel map is subjected to
Carry out initialization, i.e. use
Three-dimensional point cloud data of a first frame
Transforming to world coordinate system G to obtain point cloud
And then registers with the smooth window
Voxel map of
Then performing marginalization processing, and then returning to step S10;
if the current Kth frame is an even frame, adding the current Kth frame into a smooth window
As
The three-dimensional point cloud data is processed by using a voxel filtering algorithm in a PCL point cloud base
Downsampling with a filtered voxel size of 0.1m × 0.1m × 0.1m, and then using
Transforming the down-sampled point cloud to a world coordinate system G to obtain the point cloud
And then registered to a voxel map
Performing the following steps;
point cloud transformed to world coordinate system G
Registration to voxel map
The process is as follows: for voxel maps
Including the point cloud
A voxel of the points in (1), belonging to the point cloud in the voxel
The number of points, the mean and the covariance matrix of the point set formed by the points of (a) are added to the voxel; according to the above operation, traverse all the containers
The voxels of the point in (1), thereby completing the point cloud
To voxel map
And (4) registering. When K is 1, only the current frame is registered to the smoothing window
Voxel map of
In, corresponding to a smooth window
The marginalization process is performed at this time for the first frame of (1), the process is as follows: traverse all containers
For the jth voxel, a smoothing window stored in that voxel is used
Number of points corresponding to inner first frame
Mean value
Sum covariance matrix
Respectively assigned to the voxels
And
then delete
And
step S30, traversing each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the corresponding geometry of the voxel is greater than a set geometry threshold, taking the voxel as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose of the smoothed current frame as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
in this embodiment, a specific process of calculating the pose of the current frame after smoothing is as follows:
step S301, traversing the voxel map
If the number of voxel points is sufficient, for the voxel a
If not, the voxel is regarded as unqualified voxel, otherwise, the voxel is regarded as candidate voxel, and a candidate voxel set is obtained, wherein
The quantity of points belonging to the k frame point cloud in the voxel a belongs to n Threshold number of points in voxel, when there is only one frame in the smoothing window, e n Preferably set to 25, else ∈ n Preferably 50;
step S302, traversing all voxels in the candidate voxel set, and calculating the mean value of the voxel for the voxel b
Sum covariance matrix
Using eigenvalue decomposition algorithm in Eigen calculation library to covariance matrix sigma b Carrying out characteristic value decomposition to obtain sigma b Three eigenvalues of (a) 1 ,λ 2 ,λ 3 Wherein λ is 1 ≥λ 2 ≥λ 3 ，ΔR k And Δ t k Pose corrections for the kth frame within the smooth window, respectively
A rotation matrix (dimension 3 x 3) and a translation vector (dimension 3 x 1),
and
respectively the point number, the mean value and the covariance matrix corresponding to the point set formed by the marginalized points in the voxel b,
and
respectively representing the point number, the mean value and the covariance matrix corresponding to a point set formed by points belonging to the kth frame point cloud in the voxel b; calculating the geometry
When the voxel geometry is better, i.e. sigma b >∈ b Satisfy, the voxel is taken as a qualified voxel, e b For the geometry threshold, the present invention is preferably set to 0.85. For the jth qualifying voxel, the sum of the log-likelihoods of all the points in that voxel is calculated:
on the basis, a pose optimization objective function based on maximum likelihood estimation smoothing is constructed, and the pose optimization objective function is shown as a formula (1):
wherein N is the number of qualified voxels, w j ＝σ j A weight that is the log likelihood of the jth qualifying voxel;
step S303: for Δ T in the objective function shown in equation (1) k Adding perturbations
Namely, it is
δφ k And δ t k Respectively, disturbance δ T k The amount of rotation and the amount of translation,
is a six-dimensional European-style space,
for generalized addition, Exp (-) is exponential mapping, so as to convert the original objective function into Euclidean space
The equivalence problem in (1):
directly solving by using Gauss-Newton algorithm to obtain optimal disturbance
In combination with
Updating Δ T k ，1≤k≤l；
Step S304: repeating the steps S302-S303 for N it Then, the optimal pose correction is obtained
K is more than or equal to 1 and less than or equal to l, and the cycle number N in the invention it It is preferably set to 10 in which, when only one frame is contained in the smoothing window, the pose correction amount of the frame is initialized to I 4×4 Otherwise, initializing the pose correction of the current frame in the smooth window to the pose correction of the previous frame in the smooth window, and directly initializing the pose correction of other frames in the smooth window by using the nearest smooth optimization result of the frame; optimal pose correction value corresponding to frame I in smooth window
Calculating the pose of the smoothed current frame
The pose is used as the output pose corresponding to the current frame of the laser odometer method based on the maximum likelihood estimation smoothing.
Step S40, determine instituteWhether the smooth window is full max Frame, if full l max Removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, and returning to the step S10 until the frame is less than l max The frame is directly returned to step S10;
in this embodiment, if the frame in the smoothing window is full l max Frames, moving the oldest frame (i.e., the first frame within the smoothing window) out of the smoothing window; and in the voxel map
And (3) point cloud corresponding to the middle-edged oldest frame: traversing voxel maps
For voxel j, the mean of the points corresponding to the oldest frame
Covariance matrix
And amount of
Mean of points blended into marginalization
Covariance matrix
And amount of
In (1), first, calculate a provisional mean value
Then use
Updating
Update with mu
By using
Updating
Finally removing
And
wherein the content of the first and second substances,
and
respectively, optimal pose corrections for the oldest frame in the smooth window
The rotation matrix and translation vector of (a); meanwhile, the indexes of other frames in the smoothing window are sequentially reduced by one to realize the updating of the smoothing window, and then the step S10 is returned; if the smoothing window is not full max The frame returns directly to step S10.
A laser odometer system based on maximum likelihood estimation smoothing according to a second embodiment of the present invention, as shown in fig. 2, includes: a first bit position estimation module 100, a parity frame processing module 200, a smooth optimization module 300, and a smooth window update module 400;
the first pose estimation module 100 is configured to sense an environment through a laser radar sensor, and acquire three-dimensional point cloud data of a surrounding environment under a current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data to serve as a first pose;
the parity frame respectively processing module 200 is configured to calculate a pose of the current frame coordinate system of the laser radar sensor before smoothing in a world coordinate system according to the first pose, and use the pose as a second pose; if the current frame is an odd frame, the frame is not added with a smoothing window, the second pose is used as the output pose corresponding to the current frame based on the laser odometry method of maximum likelihood estimation smoothing, and the output pose is returned to the first pose estimation module 100; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, registering the three-dimensional point cloud data into a voxel map, and skipping to the smooth optimization module 300; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l max Is the maximum number of lidar frames in the smoothing window;
the smooth 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 corresponding geometry of the voxel is greater than a set geometry threshold, the voxel is taken as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
the smooth window update module 400 is configured to determine whether the smooth window is full max Frame, if full l max Removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, returning to the first pose estimation module 100, and keeping the first pose estimation module less than l max The frame is returned directly to the first bit position estimation module 100.
It can be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working process and related description of the system described above may refer to the corresponding process in the foregoing method embodiment, and details are not described herein again.
It should be noted that, the laser odometer system based on maximum likelihood estimation smoothing provided in the foregoing embodiment is only illustrated by the division of the above functional modules, and in practical applications, the above functions may be allocated to different functional modules according to needs, that is, the modules or steps in the embodiment of the present invention are further decomposed or combined, for example, the modules in the foregoing embodiment may be combined into one module, or may be further split into multiple sub-modules, so as to complete all or part of the above described functions. 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 to be construed as unduly limiting the present invention.
A storage device of a third embodiment of the present invention stores therein a plurality of programs adapted to be loaded by a processor and to implement the above-described maximum likelihood estimation smoothing-based laser odometry method.
A processing apparatus according to a fourth embodiment of the present invention includes a processor, a storage device; a processor adapted to execute various programs; a storage device adapted to store a plurality of programs; the program is adapted to be loaded and executed by a processor to implement the laser odometry method for maximum likelihood estimation based smoothing described above.
It can be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working processes and related descriptions of the storage device and the processing device described above may refer to the corresponding processes in the foregoing method examples, and are not described herein again.
Those of skill in the art would appreciate that the various illustrative modules, method steps, and modules described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that programs corresponding to the software modules, method steps may be located in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. To clearly illustrate this interchangeability of electronic hardware and software, various illustrative components and steps have been described above generally in terms of their functionality. Whether these functions are performed in electronic hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The terms "first," "second," "third," and the like are used for distinguishing between similar elements and not necessarily for describing or implying a particular order or sequence.
So far, the technical solutions of the present invention have been described in connection with the preferred embodiments shown in the drawings, but it is easily understood by those skilled in the art that the scope of the present invention is obviously not limited to these specific embodiments. Equivalent changes or substitutions of related technical features can be made by those skilled in the art without departing from the principle of the invention, and the technical scheme after the changes or substitutions can fall into the protection scope of the invention.
Claims (9)
1. A laser odometry method based on maximum likelihood estimation smoothing, characterized in that the method comprises the steps of:
step S10, the robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment under the current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data to serve as a first pose;
step S20, calculating the pose of the current frame coordinate system of the laser radar sensor before smoothing in the world coordinate system according to the first pose to serve as a second pose; if when it is usedThe previous frame is an odd frame, the smoothing window is not added in the previous frame, the second pose is used as the output pose corresponding to the current frame of the laser odometry method based on the maximum likelihood estimation smoothing, and the step S10 is returned; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, then registering the three-dimensional point cloud data into a voxel map, and turning to the step S30; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l max Is the maximum number of lidar frames in the smoothing window;
step S30, traversing each voxel in the voxel map, and if the number of voxel points is greater than a set number threshold and the corresponding geometry of the voxel is greater than a set geometry threshold, taking the voxel as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
step S40, judging whether the smooth window is full max Frame, if full l max Removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, and returning to the step S10 until the frame is less than l max The frame returns directly to step S10.
2. The maximum likelihood estimation smoothing-based laser odometry method according to claim 1, wherein if the current frame is an odd frame and is a first frame, after obtaining the output pose corresponding to the current frame of the maximum likelihood estimation smoothing-based laser odometry method, before returning to step S10, a voxel map is constructed by the following steps:
use of
Three-dimensional point cloud data of a first frame
Transforming to world coordinate system G to obtain point cloud
And then registers with the smooth window
Voxel map of
Performing the following steps; wherein the content of the first and second substances,
a second pose corresponding to the first frame; registered voxel map
The process comprises the following steps: traverse all containers
For a voxel map
Including the point cloud
A voxel of the point in (1), belonging to the point cloud in the voxel
The number of points, the mean and the covariance matrix of the point set formed by the points of (a) are added to the voxel;
registration to voxel map
After the neutralization, performing marginalization treatment; the marginalization process is as follows: traverse all of
For the jth voxel, a smoothing window stored in that voxel is used
Number of points corresponding to inner first frame
Mean value
Sum covariance matrix
Respectively assigned to the voxels
And
then delete
And
wherein the content of the first and second substances,
and
respectively representing points corresponding to a point set composed of marginalized points in the jth voxelNumber, mean, and covariance matrices.
3. The laser odometry method based on maximum likelihood estimation smoothing of claim 1, characterized in that the geometrical degree corresponding to the voxel is calculated by:
traversing all the voxels in the candidate voxel set, and calculating the mean value mu of each voxel b Sum covariance matrix Σ b (ii) a B represents a voxel index, and the candidate set is a set constructed on the basis of voxels of which the number of voxel points is greater than a set number threshold;
using eigenvalue decomposition algorithm to covariance matrix sigma b Carrying out characteristic value decomposition to obtain sigma b Three characteristic values of (a) 1 ,λ 2 ,λ 3 (ii) a Wherein λ 1 ≥λ 2 ≥λ 3 ；
Calculating the geometric degree sigma based on three characteristic values b ：
4. The laser odometry method based on maximum likelihood estimation smoothing of claim 1, wherein the optimal pose correction amount is obtained by:
traversing all voxels in the candidate voxel set, and calculating the geometry of each voxel; taking the voxels with the geometry larger than a set geometry threshold value in the candidate voxel set as qualified voxels;
calculating the log-likelihood of all points in each qualified voxel, and constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels;
optimizing pose correction Δ T in the pose optimization objective function based on maximum likelihood estimation smoothing k Adding perturbation delta T k ，
And use Gauss-Newton algorithm is solved to obtain the optimal disturbance
And then pass through
Updating pose correction Δ T k (ii) a Wherein the content of the first and second substances,
a six-dimensional euclidean space is represented,
denotes the generalized addition, δ φ k And δ t k Respectively, disturbance δ T k The amount of rotation and translation of;
and recalculating the geometric degree of each voxel based on the updated pose correction amount, circularly updating the pose correction amount until the set cycle number is reached, and taking the finally updated pose correction amount as the optimal pose correction amount.
5. The maximum likelihood estimation smoothing-based laser odometry method of claim 4, wherein the pose optimization objective function based on maximum likelihood estimation smoothing is:
wherein the content of the first and second substances,
representing the sum of log-likelihoods of all points in the jth qualifying voxel, N being the number of qualifying voxels, w j Is as followsWeight of log-likelihood, Δ R, of j qualifying voxels k 、Δt k A rotation matrix and a translation vector respectively representing the pose correction amount of the k-th frame within the smoothing window,
represents the ith point, mu, in a point set composed of points belonging to the kth frame point cloud in the jth qualified voxel j Sum-sigma j Respectively, the mean and covariance matrices of the qualifying voxels j, (.) T The transpose of the matrix is represented,
and representing the point number corresponding to a point set formed by the points belonging to the point cloud of the kth frame in the jth qualified voxel.
6. The laser odometer method based on maximum likelihood estimation smoothing of claim 5, wherein based on the optimal pose correction amount corresponding to the ith frame in the smoothing window, in combination with the second pose, the pose of the current frame after smoothing is calculated, and the method is as follows:
wherein the content of the first and second substances,
representing the pose of the current frame after smoothing,
showing the corresponding second pose of the Kth frame,
and representing the optimal pose correction value corresponding to the frame I in the smooth window.
7. A laser odometer system for maximum likelihood estimation based smoothing, the system comprising: the device comprises a first bit position estimation module, a parity frame processing module, a smooth optimization module and a smooth window updating module;
the first pose estimation module is configured to sense the environment through the laser radar sensor by the robot and acquire three-dimensional point cloud data of the surrounding environment under the current frame coordinate system of the laser radar sensor; estimating the pose of the current frame coordinate system of the laser radar sensor in a world coordinate system by using a laser odometer method based on GICP registration based on the three-dimensional point cloud data as a first pose;
the parity frame respectively processing module is configured to calculate a pose of the current frame coordinate system of the laser radar sensor before smoothing in a world coordinate system according to the first pose to serve as a second pose; if the current frame is an odd frame, the frame is not added with a smooth window, the second pose is used as the output pose corresponding to the current frame of the laser odometry method based on the maximum likelihood estimation smoothing, and the output pose is returned to the first pose estimation module; if the current frame is an even frame, adding the frame into a smooth window, converting the corresponding three-dimensional point cloud data into a world coordinate system, registering the three-dimensional point cloud data into a voxel map, and skipping to a smooth optimization module; wherein the smooth window is composed of l frames of laser radar frames, and l is not more than l max ，l max Is the maximum number of lidar frames in the smoothing window;
the smooth 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 corresponding geometry of the voxel is greater than a set geometry threshold, the voxel is taken as a qualified voxel; calculating the log-likelihood of all points in each qualified voxel, constructing a pose optimization objective function based on maximum likelihood estimation smoothing by maximizing the sum of the log-likelihood of all points in all the qualified voxels, and solving the pose optimization objective function based on maximum likelihood estimation smoothing by using a Gauss-Newton algorithm to obtain the optimal pose correction amount; calculating the pose of the smoothed current frame based on the optimal pose correction value and the second pose, and taking the pose as the output pose corresponding to the current frame of the laser odometry method based on maximum likelihood estimation smoothing;
the smooth window updating module is configured to judge whether the smooth window is full of l max Frame, if full l max Frame, removing the oldest frame from the smooth window, marginalizing the point cloud corresponding to the oldest frame in the voxel map to update the smooth window, returning to the first pose estimation module, and keeping the first pose estimation module until the frame is less than l max The frame is directly returned to the first pose estimation module.
8. A storage device having stored thereon a plurality of programs, wherein the programs are adapted to be loaded and executed by a processor to implement the maximum likelihood estimation based smoothing laser odometry method of any one of claims 1-6.
9. A processing device comprising a processor, a storage device; a processor adapted to execute various programs; a storage device adapted to store a plurality of programs; wherein the program is adapted to be loaded and executed by a processor to implement the maximum likelihood estimation based smoothing laser odometry method of any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210673366.XA CN115047486A (en) | 2022-06-14 | 2022-06-14 | Laser odometer method, system and device based on maximum likelihood estimation smoothing |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210673366.XA CN115047486A (en) | 2022-06-14 | 2022-06-14 | Laser odometer method, system and device based on maximum likelihood estimation smoothing |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115047486A true CN115047486A (en) | 2022-09-13 |
Family
ID=83162373
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210673366.XA Pending CN115047486A (en) | 2022-06-14 | 2022-06-14 | Laser odometer method, system and device based on maximum likelihood estimation smoothing |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115047486A (en) |
-
2022
- 2022-06-14 CN CN202210673366.XA patent/CN115047486A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109682382B (en) | Global fusion positioning method based on self-adaptive Monte Carlo and feature matching | |
| CN108759833B (en) | Intelligent vehicle positioning method based on prior map | |
| CN109345574B (en) | Laser radar three-dimensional mapping method based on semantic point cloud registration | |
| CN111486845B (en) | AUV multi-strategy navigation method based on submarine topography matching | |
| CN110930495A (en) | Multi-unmanned aerial vehicle cooperation-based ICP point cloud map fusion method, system, device and storage medium | |
| CN111780763A (en) | Visual positioning method and device based on visual map | |
| CN111667506B (en) | Motion estimation method based on ORB feature points | |
| CN112001955A (en) | Point cloud registration method and system based on two-dimensional projection plane matching constraint | |
| CN112444246B (en) | Laser fusion positioning method in high-precision digital twin scene | |
| CN115372989A (en) | Laser radar-based long-distance real-time positioning system and method for cross-country automatic trolley | |
| CN112183171A (en) | Method and device for establishing beacon map based on visual beacon | |
| CN111553985B (en) | O-graph pairing European three-dimensional reconstruction method and device | |
| CN114677418A (en) | Registration method based on point cloud feature point extraction | |
| CN110986956A (en) | Autonomous learning global positioning method based on improved Monte Carlo algorithm | |
| CN110706253B (en) | Target tracking method, system and device based on apparent feature and depth feature | |
| CN111739071A (en) | Rapid iterative registration method, medium, terminal and device based on initial value | |
| CN113781525B (en) | Three-dimensional target tracking method based on original CAD model | |
| CN114898041A (en) | Improved ICP method based on luminosity error | |
| CN117392268A (en) | Laser scanning mapping method and system based on self-adaption combined CPD and ICP algorithm | |
| CN116907509A (en) | AUV underwater auxiliary navigation method, system, equipment and medium based on image matching | |
| CN109886988A (en) | A kind of measure, system, device and the medium of Microwave Imager position error | |
| CN115047486A (en) | Laser odometer method, system and device based on maximum likelihood estimation smoothing | |
| CN111275748A (en) | Point cloud registration method based on laser radar in dynamic environment | |
| CN114777775A (en) | Multi-sensor fusion positioning method and system | |
| Sabatta et al. | Vision-based path following using the 1D trifocal tensor |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |