CN113935904A - Laser odometer error correction method, system, storage medium and computing equipment - Google Patents
Laser odometer error correction method, system, storage medium and computing equipment Download PDFInfo
- Publication number
- CN113935904A CN113935904A CN202110992996.9A CN202110992996A CN113935904A CN 113935904 A CN113935904 A CN 113935904A CN 202110992996 A CN202110992996 A CN 202110992996A CN 113935904 A CN113935904 A CN 113935904A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- ground
- correction
- plane
- ground point
- 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
- 238000012937 correction Methods 0.000 title claims abstract description 101
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000003860 storage Methods 0.000 title claims abstract description 16
- 238000009826 distribution Methods 0.000 claims abstract description 30
- 230000006870 function Effects 0.000 claims description 44
- 239000013598 vector Substances 0.000 claims description 36
- 238000005259 measurement Methods 0.000 claims description 11
- 230000011218 segmentation Effects 0.000 claims description 11
- 230000000694 effects Effects 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 15
- 238000004590 computer program Methods 0.000 description 11
- 238000004422 calculation algorithm Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 9
- 238000004891 communication Methods 0.000 description 8
- 238000012545 processing Methods 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 238000012935 Averaging Methods 0.000 description 1
- 108091034117 Oligonucleotide Proteins 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/80—Geometric correction
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/11—Region-based segmentation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
- G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
- G06T7/85—Stereo camera calibration
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manufacturing & Machinery (AREA)
- Computer Networks & Wireless Communication (AREA)
- Geometry (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention relates to a laser odometer error correction method, a system, a storage medium and a computing device, comprising: acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; establishing a ground point cloud shape model according to ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration; calibrating a correction model according to a plane of ground point cloud distribution and parameters needing correction calibration, and performing online real-time correction on the original point cloud by the calibrated correction model; and inputting the corrected original point cloud into a laser odometer for pose estimation. The method can perform distortion removal correction on the obtained laser point cloud, and reduce the pose estimation error of the laser odometer. The invention can be widely applied to the field of automatic driving.
Description
Technical Field
The invention relates to the field of automatic driving, in particular to a laser odometer error correction method, a laser odometer error correction system, a storage medium and computing equipment.
Background
The laser radar is robust to environmental illumination change, and can directly obtain three-dimensional high-precision point cloud information of an environment, so that the laser radar is widely applied to tasks such as environmental perception, positioning, high-precision map construction and the like, and is an important vehicle-mounted sensor in automatic driving. According to the implementation mode of laser scanning, the method can be divided into two types of mechanical laser radars and solid-state laser radars, and the correction method provided by the invention aims at the mechanical laser radars.
In the principle of operation, the mechanical laser radar is internally provided with laser transmitters with different pitch angles which are vertically arranged, the mechanical laser radar is driven by a motor to horizontally rotate, an azimuth angle measurement omega is obtained through a rotary encoder (rotary encoder), and a distance measurement d is obtained by measuring the Time of Flight (ToF) of laser which is reflected back to the mechanical laser radar from the surface of a target after being emitted. An internal parameter (hereinafter referred to as internal parameter) model is established according to the internal structure and the working principle of the mechanical laser radar to solve the azimuth angle measurement omega and the distance measurement d to obtain a point cloud coordinate PL. A common reference model is taken as an example for explanation, and is shown as formula (1):
wherein alpha is an azimuth correction quantity used for correcting the azimuth measurement omega; beta is a pitch angle and is an included angle of each laser beam relative to the horizontal plane. Meanwhile, there are also internal reference models with more parameter quantities, but the principles thereof are to correct the azimuth angle measurement ω and the distance measurement d and convert the measurement in the spherical coordinate system to the rectangular coordinate system, so the description using the internal reference model in the formula (1) has generality.
The internal parameters are calibrated before the laser radar leaves a factory, and are regarded as known constants in the subsequent use process. However, due to calibration errors, wear of mechanical structures in long-term use, and the like, errors inevitably exist, which may cause distortion of the point cloud obtained by calculation. Selecting a single-frame point cloud collected on a structured planar road for explanation, firstly carrying out ground segmentation on the point cloud to obtain a ground point cloud, as shown in figure 1; then, the distribution of the height of the ground point cloud with respect to the distance in the horizontal direction is counted, and as shown in fig. 2, it can be seen that the ground point cloud has a distortion trend of "low-near and high-far". Although the ground point cloud is taken as an example for description, it is understood from the operation principle of the mechanical laser radar that such a distortion tendency exists in the entire point cloud, and thus the entire point cloud needs to be corrected.
A laser odometer (LiDAR odometer) estimates the pose increment by matching adjacent frame point clouds, is a positioning method based on a track recursion (DR) principle, and is widely applied to the field of automatic driving automobile positioning. In practical application, the vertical pose estimation result of the laser odometer has larger error, and the laser odometer has more serious drift in the height direction after long-time accumulation, so that the use scene and range of the laser odometer are limited. For error correction of the laser odometer, the conventional method mainly models a track into a Gaussian Process (GP), fits a regression model parameter of the Gaussian Process by using a track true value and a point cloud characteristic distribution, and directly corrects a pose estimation result of the laser odometer in application so as to reduce errors. The method is simple to implement and easy to apply in engineering, but model parameters are obtained by a data-driven method, a true pose value is mostly needed in the training process, and the interpretability is poor.
Disclosure of Invention
In view of the above problems, an object of the present invention is to provide a laser odometer error correction method, system, storage medium, and computing device, which can perform distortion removal correction on an obtained laser point cloud and reduce a laser odometer pose estimation error.
In order to achieve the purpose, the invention adopts the following technical scheme: a laser odometer error correction method, comprising: acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; establishing a ground point cloud shape model according to the ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration; calibrating a correction model according to the plane of the ground point cloud distribution and the parameters needing to be corrected and calibrated, and performing online real-time correction on the original point cloud by the calibrated correction model; and inputting the corrected original point cloud into a laser odometer for pose estimation.
Preferably, the establishing of the ground point cloud shape model includes: performing ground segmentation on the original point cloud to obtain a ground point cloud; setting the ground point cloud distribution in unit normal vector as
And determining the distortion trend from the laser radar internal reference modeling calibration error to calibrate the pitch angle correction quantity according to the influence factors of the actual ground point cloud shape.
Preferably, the calibrating and correcting model according to the plane of the ground point cloud distribution and the parameter to be calibrated and corrected includes: the unit normal vector to the ground plane
Carrying out estimation; and constructing a cost function according to the unit normal vector estimation result, and completing the calibration of the pitching angle correction quantity.
Preferably, the unit normal vector to the ground plane
Performing an estimation comprising: setting a laser radar with N wire harnesses, and rotating each wire harness for one circle to obtain M scanning points; wherein, m is in the ith line beamiThe points are divided into ground points, i ═ 1, 2, …, N; estimating the ithM on the wire harnessiUnit normal vector of plane where the ground points are located
And calculating the weight thereof; respectively calculating the unit normal vectors
A pitch angle and an azimuth angle under a spherical coordinate system; respectively carrying out weighted fusion on the pitch angle and the azimuth angle by using the weight to obtain a unit normal vector of the ground plane
Estimating results of a pitch angle and an azimuth angle under a spherical coordinate system; unit normal vector through the ground plane
Calculating the unit normal vector of the ground plane according to the estimation result of the pitch angle and the azimuth angle under the spherical coordinate system
The estimation result of (2).
Preferably, the constructing a cost function according to the unit normal vector estimation result to complete calibration of the tilt angle correction amount includes: fitting the ground point cloud set to obtain the unit normal vector of
And a plane with a distance error threshold value delta, obtaining the plane parameters, an inner point set which accords with the plane model and an outer point set which does not accord with the plane model, and calculating the error from a certain point to the plane; calculating a cost function value corresponding to the point according to the error; calculating a cost function value corresponding to the single-frame point cloud to be used as the measurement of the distortion removal effect of the ground point cloud; traversing all the point clouds, constructing an optimization problem, and completing calibration of the elevation angle correction quantity.
Preferably, the calculating a cost function value corresponding to a single frame point cloud includes: calculating a cost function value by adopting a robust kernel function for the outlier; a Huber kernel function or a Cauchy kernel function is employed.
Preferably, the calculating a cost function value corresponding to a single frame point cloud includes: and performing weighted average calculation on the inner point average cost function value and the outer point average cost function value according to the quantity proportion of the inner point and the outer point to obtain the inner point and outer point cost function value.
A laser odometer error correction system, comprising: the system comprises a data acquisition module, a ground point cloud shape model construction module, a correction module and an estimation module; the data acquisition module is used for acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; the ground point cloud shape model building module is used for building a ground point cloud shape model according to the ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration; the correction module is used for calibrating a correction model according to the plane of the ground point cloud distribution and the parameters needing to be corrected and calibrated, and performing online real-time correction on the original point cloud by the calibrated correction model; and the estimation module is used for inputting the corrected original point cloud into a laser odometer for pose estimation.
A computer readable storage medium storing one or more programs, the one or more programs comprising instructions, which when executed by a computing device, cause the computing device to perform any of the above methods.
A computing device, comprising: one or more processors, memory, and one or more programs stored in the memory and configured to be executed by the one or more processors, the one or more programs including instructions for performing any of the above-described methods.
Due to the adoption of the technical scheme, the invention has the following advantages:
1. the correction model of the invention has less parameters and can effectively reduce the pose estimation error of the laser odometer.
2. The model parameters of the invention have practical physical significance and strong interpretability.
3. The invention does not need artificial markers such as a calibration plate, a calibration cone and the like in the calibration process, and has simple form and easy application.
4. Because the point cloud matching algorithm mainly depends on the geometrical characteristics of the point cloud, the position and pose estimation result of the laser odometer can be influenced by the point cloud coordinate error. The ground point cloud occupies most of one frame of point cloud, and mainly provides pose constraint in the vertical direction. Therefore, the method corrects the original point cloud, is beneficial to improving the overall quality of the point cloud, improves the distortion trend of the ground point cloud of 'low-near, far-far and high', and further reduces the pose estimation error of the laser odometer.
Drawings
FIG. 1 is a diagram illustrating a segmentation result of a ground point cloud in the prior art;
FIG. 2 is a diagram illustrating a statistical result of height distribution of ground point cloud in the prior art;
FIG. 3 is a schematic flow chart of a laser odometer error correction method according to an embodiment of the invention;
FIG. 4 is a schematic diagram of ground point cloud shape modeling in an embodiment of the invention;
FIG. 5 is a flow chart of a pitch angle correction calibration algorithm in an embodiment of the present invention;
FIG. 6 is a schematic diagram illustrating a ground point cloud correction effect according to an embodiment of the present invention;
FIG. 7 is a schematic view of the pose correction effect of the translation portion of the laser odometer in an embodiment of the invention;
FIG. 8 is a schematic view of the pose correction effect of the rotating part of the laser odometer in an embodiment of the invention;
FIG. 9 is a schematic diagram of a computing device in 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 of the embodiments of the present invention will be clearly and completely described below with reference to the drawings of the embodiments of the present invention. It is to be understood that the embodiments described are only a few embodiments of the present invention, and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the described embodiments of the invention, are within the scope of the invention.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments according to the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, and it should be understood that when the terms "comprises" and/or "comprising" are used in this specification, they specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof, unless the context clearly indicates otherwise.
The invention relates to a correction method for carrying out distortion removal correction on obtained laser point cloud by considering the internal reference modeling calibration error of a mechanical laser radar so as to reduce the pose estimation error of a laser odometer. Firstly, modeling the distortion trend of point cloud 'low-near-far-high' as the error of a pitch angle beta; secondly, designing a calibration algorithm, and calibrating the pitch angle correction quantity delta beta by taking point cloud acquired by the laser radar as an information source; then, correcting the original point cloud according to the internal reference model; and finally, sending the corrected point cloud into a pose estimation algorithm to reduce a pose estimation error.
In an embodiment of the present invention, in consideration of a mechanical lidar internal parameter (intrinsic) modeling calibration error, as shown in fig. 3, a method for correcting a laser odometer error is provided, and this embodiment is illustrated by applying the method to a terminal, it is to be understood that the method may also be applied to a server, and may also be applied to a system including a terminal and a server, and is implemented by interaction between the terminal and the server. In this embodiment, the method includes the steps of:
step 1, acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle;
the original point cloud is a frame of point cloud obtained by rotating the laser radar for one circle;
step 3, calibrating a correction model according to a plane of ground point cloud distribution and parameters needing correction calibration, and performing online real-time correction on the original point cloud by the calibrated correction model;
and 4, inputting the corrected original point cloud into a laser odometer for pose estimation.
In the step 2, a ground point cloud shape model is established according to the ground point cloud in the original point cloud, specifically: as can be seen in fig. 2, the actual ground point cloud shape is affected by two factors: one aspect derives from the angle of the lidar relative to the ground, including: influenced by the self gradient of the ground, the pitching of the vehicle body relative to the ground and the installation angle of the laser radar relative to the vehicle body; on the other hand, the actual point cloud is the superposition of the two trends, which are shown in fig. 4, and is derived from the laser radar internal reference modeling calibration error.
It should be noted that the change trend of the angle of the lidar relative to the ground has no influence on the accuracy of the pose estimation, because the lidar and a true value acquisition device (such as a high-precision inertial navigation device) are rigidly connected to the vehicle body in the data acquisition process, and the pose between the lidar and the true value acquisition device is associated through an external parameter between the sensors, so that the trend is not a main cause of vertical drift of the pose estimation result and does not need to be corrected. However, in the calibration process, the influence of the trend needs to be eliminated first, and then the distortion trend derived from the laser radar internal reference modeling calibration error is corrected and calibrated.
Therefore, modeling a ground point cloud shape model includes the steps of:
step 21, performing ground segmentation on the original point cloud to obtain a ground point cloud;
and (4) segmenting a ground part in one frame of point cloud, and only using the ground point cloud for subsequent operation. The method does not limit the specific ground segmentation method, but ensures the segmentation quality of the ground point cloud as much as possible, and the accurate ground point cloud segmentation result is beneficial to improving the effect of the subsequent calibration algorithm. In the embodiment, a ground point cloud segmentation result is shown in fig. 1 by using a ground segmentation algorithm based on a 2.5D depth image proposed by bogossavskyi et al.
Step 22, setting the ground point cloud distribution in unit normal vector as
Determining the distortion trend from the laser radar internal reference modeling calibration error to calibrate the pitch angle correction amount according to the influence factors of the actual ground point cloud shape on the ground plane;
the method specifically comprises the following steps: setting the ground point cloud distribution in unit normal vector as
On the ground plane (for convenience of subsequent operations, n is specified here in a unified wayz>0, namely the plane normal vector points to the upper part of the ground, and the convention is used in the subsequent processing), and the distortion trend of 'low-near-far-high' appears due to the error of the pitch angle beta in the laser radar internal reference model. Therefore, the pitch angle correction quantity delta beta needs to be calibrated by using ground point cloud, and a corrected point cloud coordinate P 'is obtained'LAnd further correcting the whole distortion trend of the point cloud, wherein the correction mode is shown as a formula (2), and the ground point cloud correction effect is shown as a figure 5.
In the step 3, calibrating the correction model according to the plane of the ground point cloud distribution and the parameter to be corrected and calibrated, the method comprises the following steps:
step 31, unit normal vector to ground plane
Carrying out estimation;
and estimating the normal vector direction of the ground plane for constructing and using the cost function. The method specifically comprises the following steps:
311, setting a laser radar with N wire harnesses, and rotating each wire harness for one circle to obtain M scanning points; wherein, m is in the ith line beamiThe points are divided into ground points, i 1, 2，…，N；
It is easy to understand from the modeling approach for the ground point cloud shape that the ground points on the same line bundle are on the same plane, as shown in fig. 5.
Step 312, estimating m on the ith wire harnessiUnit normal vector of plane where individual ground point is located
And calculating the weight thereof;
in the present embodiment, RANdom Sample Consensus (RANSAC) algorithm is used to estimate m on the ith scan lineiUnit normal vector of plane where individual ground point is located
And calculating the weight thereof as shown in equation (3):
step 313 of calculating unit normal vectors
Pitch angle theta in spherical coordinate systemiAnd azimuth angle phii；
As shown in formulas (4) and (5):
step 314, weighting the pitch angle thetaiAnd azimuth angle phiiRespectively carrying out weighted fusion to obtain the ground averageUnit normal vector of face
Pitch angle in spherical coordinate system
And azimuth angle
Estimating a result;
as shown in formulas (6) and (7):
315, passing the unit normal vector of the ground plane
Pitch angle in spherical coordinate system
And azimuth angle
Estimating the result to calculate the unit normal vector of the ground plane
The estimation result of (2);
as shown in formula (8):
step 32, constructing a cost function according to the unit normal vector estimation result, and completing calibration of the pitching angle correction quantity;
as shown in fig. 6, the method specifically includes the following steps:
step 321, fitting the ground point cloud set to obtain the unit normal vector of
And a plane with a distance error threshold value delta, obtaining the plane parameters, an inner point set which accords with the plane model and an outer point set which does not accord with the plane model, and calculating the error from a certain point to the plane;
in the embodiment, the RANSAC algorithm is adopted to collect the ground point cloud PgroundFitting to obtain a unit normal vector of
And a plane with a distance error threshold value delta is obtained, and the plane parameter plane and an inner point set P conforming to the plane model are obtainedinlierAnd a set of outliers P that do not fit the planar modeloutlierAs shown in formula (9):
in the formula, plane ═ a B C D is a plane parameter, and indicates that plane a · x + B · y + C · z + D is 0.
For point Pi＝[xi yi zi]TCalculating the point-to-plane error eiAs shown in formula (10):
step 322, calculating the cost function value cost corresponding to the point according to the errori；
In this embodiment, cost function values cost corresponding to a single point need to be calculatedi. Because the actual ground shape is different from the plane and the ground segmentation algorithm objectively has errors, a robust kernel is adopted for outliersThe cost function value is calculated by the function, which can adopt a Huber kernel function or a Cauchy kernel function, wherein the Huber kernel function is shown as a formula (11), and the Cauchy kernel function is shown as a formula (12):
step 323, calculating a cost function value cost corresponding to the single-frame point cloud as the measurement of the distortion removal effect of the ground point cloud;
in the present embodiment, the inner point set P is comprehensively consideredinlierAnd a set of outliers PoutlierFor the influence of error function value, average cost function value of inner point is calculated according to the quantity ratio of inner point to outer point
And the mean cost function value of the outliers
Performing weighted averaging, as shown in equation (13):
step 324, traversing all the point clouds, constructing an optimization problem, and completing calibration of the elevation angle correction quantity:
in the step 4, inputting the corrected original point cloud into a laser odometer for pose estimation, specifically:
after the pitch angle correction quantity delta beta is calibrated, the original point cloud is corrected on line according to the formula (2), and the corrected point cloud is sent to a laser odometer for pose estimation.
In this embodiment, the laser odometer uses the LeGO-LOAM algorithm proposed by Tixiao shann et al to evaluate the six-degree-of-freedom pose estimation accuracy of the translation part and the rotation part of the laser odometer before and after correction, and the results are shown in fig. 7 and 8. From this, it can be seen that the estimation accuracy of the displacement in the z-axis direction in the translational degree of freedom and the roll angle (roll) and pitch angle (pitch) in the rotational degree of freedom is significantly improved.
In one embodiment of the present invention, there is provided a laser odometer error correction system, including: the system comprises a data acquisition module, a ground point cloud shape model construction module, a correction module and an estimation module;
the data acquisition module is used for acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle;
the ground point cloud shape model building module is used for building a ground point cloud shape model according to the ground point cloud in the original point cloud; calibrating a correction model according to a plane of ground point cloud distribution and parameters needing correction and calibration;
the correction module is used for calibrating a correction model according to a plane of ground point cloud distribution and parameters needing to be corrected and calibrated, and performing online real-time correction on the original point cloud by the calibrated correction model;
and the estimation module is used for inputting the corrected original point cloud into the laser odometer for pose estimation.
The system provided in this embodiment is used for executing the above method embodiments, and for details of the process and the details, reference is made to the above embodiments, which are not described herein again.
As shown in fig. 9, which is a schematic structural diagram of a computing device provided in an embodiment of the present invention, the computing device may be a terminal, and may include: a processor (processor), a communication Interface (communication Interface), a memory (memory), a display screen and an input device. The processor, the communication interface and the memory are communicated with each other through a communication bus. The processor is used to provide computing and control capabilities. The memory includes a non-volatile storage medium, an internal memory, the non-volatile storage medium storing an operating system and a computer program that when executed by the processor implements a laser odometry error correction method; the internal memory provides an environment for the operation of an operating system and computer programs in the non-volatile storage medium. The communication interface is used for carrying out wired or wireless communication with an external terminal, and the wireless communication can be realized through WIFI, a manager network, NFC (near field communication) or other technologies. The display screen can be a liquid crystal display screen or an electronic ink display screen, and the input device can be a touch layer covered on the display screen, a key, a track ball or a touch pad arranged on a shell of the computing equipment, an external keyboard, a touch pad or a mouse and the like. The processor may call logic instructions in memory to perform the following method:
acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; establishing a ground point cloud shape model according to ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration; calibrating a correction model according to a plane of ground point cloud distribution and parameters needing correction calibration, and performing online real-time correction on the original point cloud by the calibrated correction model; and inputting the corrected original point cloud into a laser odometer for pose estimation.
In addition, the logic instructions in the memory may be implemented in the form of software functional units and may be stored in a computer readable storage medium when sold or used as a stand-alone product. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
Those skilled in the art will appreciate that the architecture shown in fig. 9 is merely a block diagram of some of the structures associated with the disclosed aspects and is not intended to limit the computing devices to which the disclosed aspects may be applied, and that a particular computing device may include more or less components than those shown, or may combine certain components, or have a different arrangement of components.
In one embodiment of the invention, a computer program product is provided, the computer program product comprising a computer program stored on a non-transitory computer-readable storage medium, the computer program comprising program instructions that, when executed by a computer, enable the computer to perform the methods provided by the above-described method embodiments, for example, comprising: acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; establishing a ground point cloud shape model according to ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration; calibrating a correction model according to a plane of ground point cloud distribution and parameters needing correction calibration, and performing online real-time correction on the original point cloud by the calibrated correction model; and inputting the corrected original point cloud into a laser odometer for pose estimation.
In one embodiment of the invention, a non-transitory computer-readable storage medium is provided, which stores server instructions that cause a computer to perform the methods provided by the above embodiments, for example, including: acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; establishing a ground point cloud shape model according to ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration; calibrating a correction model according to a plane of ground point cloud distribution and parameters needing correction calibration, and performing online real-time correction on the original point cloud by the calibrated correction model; and inputting the corrected original point cloud into a laser odometer for pose estimation.
The implementation principle and technical effect of the computer-readable storage medium provided by the above embodiments are similar to those of the above method embodiments, and are not described herein again.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A method of laser odometer error correction, comprising:
acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle;
establishing a ground point cloud shape model according to the ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration;
calibrating a correction model according to the plane of the ground point cloud distribution and the parameters needing to be corrected and calibrated, and performing online real-time correction on the original point cloud by the calibrated correction model;
and inputting the corrected original point cloud into a laser odometer for pose estimation.
2. The laser odometer error correction method of claim 1, wherein said establishing a ground point cloud shape model comprises:
performing ground segmentation on the original point cloud to obtain a ground point cloud;
setting the ground point cloud distribution in unit normal vector as
And determining the distortion trend from the laser radar internal reference modeling calibration error to calibrate the pitch angle correction quantity according to the influence factors of the actual ground point cloud shape.
3. The method for correcting the error of the laser odometer according to claim 2, wherein calibrating the correction model according to the plane of the ground point cloud distribution and the parameter to be corrected and calibrated comprises:
the unit normal vector to the ground plane
Carrying out estimation;
and constructing a cost function according to the unit normal vector estimation result, and completing the calibration of the pitching angle correction quantity.
4. The laser odometer error correction method of claim 3, wherein the unit normal vector to the ground plane
Performing an estimation comprising:
setting a laser radar with N wire harnesses, and rotating each wire harness for one circle to obtain M scanning points; wherein, m is in the ith line beamiThe points are divided into ground points, i ═ 1, 2, …, N;
estimating m on the ith wire harnessiUnit normal vector of plane where the ground points are located
And calculating the weight thereof;
respectively calculating the unit normal vectors
A pitch angle and an azimuth angle under a spherical coordinate system;
respectively carrying out weighted fusion on the pitch angle and the azimuth angle by using the weight to obtain a unit normal vector of the ground plane
Estimating results of a pitch angle and an azimuth angle under a spherical coordinate system;
unit normal vector through the ground plane
Calculating the unit normal vector of the ground plane according to the estimation result of the pitch angle and the azimuth angle under the spherical coordinate system
The estimation result of (2).
5. The method for laser odometer error correction according to claim 3, wherein the constructing a cost function based on the unit normal vector estimation result to perform calibration of the pitch angle correction amount comprises:
fitting the ground point cloud set to obtain the unit normal vector of
And a plane with a distance error threshold value delta, obtaining the plane parameters, an inner point set which accords with the plane model and an outer point set which does not accord with the plane model, and calculating the error from a certain point to the plane;
calculating a cost function value corresponding to the point according to the error;
calculating a cost function value corresponding to the single-frame point cloud to be used as the measurement of the distortion removal effect of the ground point cloud;
traversing all the point clouds, constructing an optimization problem, and completing calibration of the elevation angle correction quantity.
6. The laser odometer error correction method of claim 5, wherein said calculating a cost function value for a single frame of point cloud comprises: calculating a cost function value by adopting a robust kernel function for the outlier; a Huber kernel function or a Cauchy kernel function is employed.
7. The laser odometer error correction method of claim 5, wherein said calculating a cost function value for a single frame of point cloud comprises: and performing weighted average calculation on the inner point average cost function value and the outer point average cost function value according to the quantity proportion of the inner point and the outer point to obtain the inner point and outer point cost function value.
8. A laser odometer error correction system, comprising: the system comprises a data acquisition module, a ground point cloud shape model construction module, a correction module and an estimation module;
the data acquisition module is used for acquiring an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle;
the ground point cloud shape model building module is used for building a ground point cloud shape model according to the ground point cloud in the original point cloud, and determining a plane of ground point cloud distribution and parameters needing correction and calibration;
the correction module is used for calibrating a correction model according to the plane of the ground point cloud distribution and the parameters needing to be corrected and calibrated, and performing online real-time correction on the original point cloud by the calibrated correction model;
and the estimation module is used for inputting the corrected original point cloud into a laser odometer for pose estimation.
9. A computer readable storage medium storing one or more programs, the one or more programs comprising instructions, which when executed by a computing device, cause the computing device to perform any of the methods of claims 1-7.
10. A computing device, comprising: one or more processors, memory, and one or more programs stored in the memory and configured to be executed by the one or more processors, the one or more programs including instructions for performing any of the methods of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110992996.9A CN113935904A (en) | 2021-08-27 | 2021-08-27 | Laser odometer error correction method, system, storage medium and computing equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110992996.9A CN113935904A (en) | 2021-08-27 | 2021-08-27 | Laser odometer error correction method, system, storage medium and computing equipment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113935904A true CN113935904A (en) | 2022-01-14 |
Family
ID=79274600
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110992996.9A Pending CN113935904A (en) | 2021-08-27 | 2021-08-27 | Laser odometer error correction method, system, storage medium and computing equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113935904A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115390086A (en) * | 2022-10-31 | 2022-11-25 | 智道网联科技(北京)有限公司 | Fusion positioning method and device for automatic driving, electronic equipment and storage medium |
| CN117589203A (en) * | 2024-01-18 | 2024-02-23 | 陕西太合智能钻探有限公司 | Gyroscope calibration method |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200400821A1 (en) * | 2019-06-21 | 2020-12-24 | Blackmore Sensors & Analytics, Llc | Method and system for vehicle odometry using coherent range doppler optical sensors |
| CN112258600A (en) * | 2020-10-19 | 2021-01-22 | 浙江大学 | Simultaneous positioning and map construction method based on vision and laser radar |
| CN112731357A (en) * | 2020-12-30 | 2021-04-30 | 清华大学 | Real-time correction method and system for positioning error of laser point cloud odometer |
| CN112859051A (en) * | 2021-01-11 | 2021-05-28 | 桂林电子科技大学 | Method for correcting laser radar point cloud motion distortion |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113219440A (en) * | 2021-04-22 | 2021-08-06 | 电子科技大学 | Laser radar point cloud data correction method based on wheel type odometer |
-
2021
- 2021-08-27 CN CN202110992996.9A patent/CN113935904A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200400821A1 (en) * | 2019-06-21 | 2020-12-24 | Blackmore Sensors & Analytics, Llc | Method and system for vehicle odometry using coherent range doppler optical sensors |
| CN112258600A (en) * | 2020-10-19 | 2021-01-22 | 浙江大学 | Simultaneous positioning and map construction method based on vision and laser radar |
| CN112731357A (en) * | 2020-12-30 | 2021-04-30 | 清华大学 | Real-time correction method and system for positioning error of laser point cloud odometer |
| CN112859051A (en) * | 2021-01-11 | 2021-05-28 | 桂林电子科技大学 | Method for correcting laser radar point cloud motion distortion |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113219440A (en) * | 2021-04-22 | 2021-08-06 | 电子科技大学 | Laser radar point cloud data correction method based on wheel type odometer |
Non-Patent Citations (1)
| Title |
|---|
| 柯艳国;王雄奇;魏新;吴贤斌;朱仲贤;董翔宇;黄杰;董二宝;: "一种基于图优化的实时3D激光SLAM算法", 机电一体化, no. 1, 15 April 2020 (2020-04-15) * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115390086A (en) * | 2022-10-31 | 2022-11-25 | 智道网联科技(北京)有限公司 | Fusion positioning method and device for automatic driving, electronic equipment and storage medium |
| CN117589203A (en) * | 2024-01-18 | 2024-02-23 | 陕西太合智能钻探有限公司 | Gyroscope calibration method |
| CN117589203B (en) * | 2024-01-18 | 2024-05-10 | 陕西太合智能钻探有限公司 | Gyroscope calibration method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109001711B (en) | Multi-line laser radar calibration method | |
| CN111208492B (en) | Vehicle-mounted laser radar external parameter calibration method and device, computer equipment and storage medium | |
| JP6966218B2 (en) | Imaging equipment calibration equipment, work machines and calibration methods | |
| CN107632308B (en) | Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm | |
| CN110501712B (en) | Method, device and equipment for determining position attitude data in unmanned driving | |
| CN113935904A (en) | Laser odometer error correction method, system, storage medium and computing equipment | |
| CN112880642B (en) | Ranging system and ranging method | |
| CN110849387B (en) | Sensor parameter calibration method and device | |
| WO2022179094A1 (en) | Vehicle-mounted lidar external parameter joint calibration method and system, medium and device | |
| CN111913169B (en) | Laser radar internal reference and point cloud data correction method, device and storage medium | |
| CN110579754A (en) | Method for determining external parameters of a lidar and other sensors of a vehicle | |
| CN114440928A (en) | Combined calibration method for laser radar and odometer, robot, equipment and medium | |
| CN111932637B (en) | Vehicle body camera external parameter self-adaptive calibration method and device | |
| CN110764110A (en) | Path navigation method, device and computer readable storage medium | |
| CN115079143B (en) | Multi-radar external parameter quick calibration method and device for double-bridge steering mine card | |
| CN114322856B (en) | Mining area pavement evenness detection method, device, storage medium and equipment | |
| WO2021081958A1 (en) | Terrain detection method, movable platform, control device, system, and storage medium | |
| CN112558044A (en) | Automatic correction method for vehicle-mounted laser radar pitch angle | |
| CN110796604A (en) | Image correction method and device | |
| CN116681776B (en) | External parameter calibration method and system for binocular camera | |
| CN115542301B (en) | Method, device and equipment for calibrating external parameters of laser radar and storage medium | |
| EP3699630A1 (en) | System and method for compensating a motion of a vehicle component | |
| CN114004949A (en) | Airborne point cloud assisted mobile measurement system arrangement parameter calibration method and system | |
| CN113405532B (en) | Forward intersection measuring method and system based on structural parameters of vision system | |
| CN115294211A (en) | Vehicle-mounted camera installation external parameter calibration method, system, device and storage medium |
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 |