Laser odometer error correction method, system, storage medium and computing device

By establishing a ground point cloud shape model and using cost function to correct the pitch angle, the laser odometer error problem caused by point cloud distortion of mechanical lidar is solved, and the laser odometer position estimation error is reduced and the point cloud quality is realized. promote.

CN113935904BActive Publication Date: 2024-11-01TSINGHUA UNIVERSITY
View PDF 6 Cites 0 Cited by

Patent Information

Application Number
CN202110992996.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-08-27
Publication Date
2024-11-01
Estimated Expiration
2041-08-27

AI Technical Summary

Technical Problem

During long-term use of mechanical lidar, point cloud distortion caused by calibration errors and mechanical structure wear, resulting in large errors in the estimation of the vertical position of the laser odometer, limiting its use scenario and range.

Method used

By obtaining the original point cloud, establishing a ground point cloud shape model, determining the plane of the ground point cloud distribution and parameters that need to be corrected, using the cost function to calibrate the pitch angle correction amount, the correction model performs online real-time correction of the original point cloud, and reducing laser light Odometer position estimation error.

Benefits of technology

It effectively reduces the laser odometer position estimation error, improves the overall quality of point clouds, improves the "near low far high" distortion trend of ground point clouds, and enhances the interpretability and simplicity of the model.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN113935904B_ABST
    Figure CN113935904B_ABST
Patent Text Reader

Abstract

The present invention relates to a laser odometer error correction method, system, storage medium and computing device, which include: obtaining an original point cloud; the original point cloud includes a ground point cloud and points of obstacles around a vehicle; according to the ground point cloud in the original point cloud, establishing a ground point cloud shape model, determining the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated; calibrating the correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and performing online real-time correction on the original point cloud by the calibrated correction model; inputting the corrected original point cloud into the laser odometer for posture estimation. The present invention can perform de-distortion correction on the obtained laser point cloud and reduce the error of the laser odometer posture estimation. The present invention can be widely used in the field of autonomous driving.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving, and in particular to a laser odometer error correction method, system, storage medium and computing device. Background Art

[0002] LiDAR is relatively robust to changes in ambient lighting and can directly obtain three-dimensional high-precision point cloud information of the environment. Therefore, it is widely used in tasks such as environmental perception, positioning, and high-precision map construction. It is an important on-board sensor in autonomous driving. According to the implementation method of laser scanning, it can be divided into two categories: mechanical LiDAR and solid-state LiDAR. The correction method proposed in this invention is for mechanical LiDAR.

[0003] In terms of working principle, the mechanical laser radar is equipped with vertically arranged laser transmitters with different pitch angles, which are driven by a motor to rotate horizontally, and the azimuth angle measurement ω is obtained through a rotary encoder. The distance measurement d is obtained by measuring the time of flight (ToF) of the laser reflected from the target surface after being emitted and returned to the laser radar. According to the internal structure and working principle of the mechanical laser radar, an internal parameter (hereinafter referred to as internal parameter) model is established to solve the azimuth angle measurement ω and distance measurement d, and obtain the point cloud coordinates P L . Take a common internal reference model as an example, as shown in formula (1):

[0004]

[0005] Among them, α is the azimuth correction, which is used to correct the azimuth measurement ω; β is the pitch angle, which is the angle of each laser beam relative to the horizontal plane. It should also be noted that there are also internal reference models with more parameters, but their principles are all to correct the azimuth measurement ω and the distance measurement d, and convert the measurement in the spherical coordinate system to the rectangular coordinate system. Therefore, the internal reference model in formula (1) is used for explanation with generality.

[0006] The above internal parameters are calibrated before the LiDAR leaves the factory and are considered as known constants in the subsequent use process. However, due to calibration errors, wear of mechanical structures during long-term use, etc., errors are inevitable, which will cause distortion of the point cloud obtained by solution. A single-frame point cloud collected on a structured plane road is selected for illustration. First, the point cloud is segmented to obtain the ground point cloud, such as Figure 1 As shown; then the distribution of the ground point cloud height with respect to the horizontal distance is statistically analyzed, as shown Figure 2As shown in the figure, it can be seen that the ground point cloud has a distortion trend of "near low and far high". It should be pointed out that although the explanation is based on the ground point cloud as an example, from the working principle of mechanical laser radar, it can be seen that this distortion trend exists in the overall point cloud, so the overall point cloud needs to be corrected.

[0007] Laser odometry estimates the pose increment by matching adjacent frame point clouds. It is a positioning method based on the dead reckoning (DR) principle and is widely used in the field of autonomous vehicle positioning. In practical applications, it is found that the vertical pose estimation results of the laser odometry have large errors, and there is a serious drift in the height direction after a long period of accumulation, which limits the use scenarios and scope of the laser odometry. For the error correction of the laser odometry, the existing methods mainly model the trajectory as a Gaussian process (GP), use the trajectory truth value and point cloud feature distribution to fit the Gaussian process regression model parameters, and directly correct the pose estimation results of the laser odometry in the application to reduce the error. This method is simple to implement and easy to apply in engineering, but the model parameters are obtained through a data-driven method, and most of the pose truth values ​​are required during the training process, which has poor interpretability. Summary of the invention

[0008] In view of the above problems, the purpose of the present invention is to provide a laser odometer error correction method, system, storage medium and computing device, which can perform dedistortion correction on the obtained laser point cloud and reduce the laser odometer pose estimation error.

[0009] To achieve the above-mentioned purpose, the present invention adopts the following technical scheme: a laser odometer error correction method, which comprises: obtaining an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around a vehicle; according to the ground point cloud in the original point cloud, establishing a ground point cloud shape model, determining the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated; calibrating a correction model according to the plane of the ground point cloud distribution and the parameters that need 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 the laser odometer for posture estimation.

[0010] Preferably, the establishment of the ground point cloud shape model includes: performing ground segmentation on the original point cloud to obtain the ground point cloud; setting the ground point cloud to be distributed in a unit normal vector The pitch angle correction amount is calibrated on the ground plane and based on the influencing factors of the actual ground point cloud shape, the distortion trend derived from the calibration error of the lidar intrinsic parameter modeling is determined.

[0011] Preferably, the calibration correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated includes: Estimation is performed; and a cost function is constructed according to the estimation result of the unit normal vector to complete the calibration of the pitch angle correction amount.

[0012] Preferably, the unit normal vector to the ground plane is The estimation includes: assuming that the laser radar has N beams, each beam rotates one circle to obtain M scanning points; among them, there are m in the i-th beam i points are divided into ground points, i = 1, 2, ..., N; the m points on the i-th line are estimated i The unit normal vector of the plane where the ground point lies And calculate its weight; calculate the unit normal vector respectively The pitch angle and azimuth angle in the spherical coordinate system; the pitch angle and the azimuth angle are weightedly fused using the weights to obtain a unit normal vector of the ground plane The pitch angle and azimuth estimation results in the spherical coordinate system; the unit normal vector of the ground plane The pitch angle and azimuth angle estimation results in the spherical coordinate system are used to calculate the unit normal vector of the ground plane The estimated results.

[0013] Preferably, the cost function is constructed according to the estimation result of the unit normal vector to complete the calibration of the pitch angle correction amount, including: fitting the ground point cloud set to obtain the unit normal vector as and a plane with a distance error threshold of δ, and obtain the plane parameters, the set of interior points that conform to the plane model, and the set of exterior points that do not conform to the plane model, and calculate the error from a point to the plane; calculate the cost function value corresponding to the point according to the error; calculate the cost function value corresponding to a single-frame point cloud as a measure of the dedistortion effect of the ground point cloud; traverse all point clouds, construct an optimization problem, and complete the calibration of the pitch angle correction amount.

[0014] Preferably, the calculating of the cost function value corresponding to the single-frame point cloud includes: using a robust kernel function to calculate the cost function value for outliers; using a Huber kernel function or a Cauchy kernel function.

[0015] Preferably, the calculating of the cost function value corresponding to the single-frame point cloud includes: performing weighted average calculation on the average cost function value of the inner points and the average cost function value of the outer points according to the ratio of the number of the inner and outer points.

[0016] A laser odometer error correction 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 to acquire an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around a vehicle; the ground point cloud shape model construction module establishes a ground point cloud shape model according to the ground point cloud in the original point cloud, determines the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated; the correction module calibrates the correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and performs online real-time correction on the original point cloud by the calibrated correction model; the estimation module inputs the corrected original point cloud into the laser odometer for posture estimation.

[0017] A computer-readable storage medium storing one or more programs, wherein the one or more programs include instructions, which, when executed by a computing device, cause the computing device to perform any of the above methods.

[0018] A computing device comprises: one or more processors, a memory and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for executing any of the above methods.

[0019] The present invention adopts the above technical solution, which has the following advantages:

[0020] 1. The correction model of the present invention has few parameters and can effectively reduce the error of the laser odometer posture estimation.

[0021] 2. The model parameters of the present invention have actual physical meaning and are highly interpretable.

[0022] 3. The present invention does not require artificial markers such as calibration plates and calibration cones during the calibration process, and is simple in form and easy to use.

[0023] 4. Since the point cloud matching algorithm mainly relies on the geometric features of the point cloud, the point cloud coordinate error will affect the pose estimation result of the laser odometer. The ground point cloud occupies the vast majority of a frame of point cloud, and mainly provides pose constraints in the vertical direction. Therefore, the present invention corrects the original point cloud, which helps to improve the overall quality of the point cloud, improve the distortion trend of the ground point cloud of "near low and far high", and thus reduce the pose estimation error of the laser odometer. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 It is a schematic diagram of ground point cloud segmentation results in the prior art;

[0025] Figure 2 It is a schematic diagram of the statistical results of the height distribution of ground point clouds in the prior art;

[0026] Figure 3 It is a schematic flow chart of a laser odometer error correction method in one embodiment of the present invention;

[0027] Figure 4 is a schematic diagram of ground point cloud shape modeling in one embodiment of the present invention;

[0028] Figure 5 is a flow chart of a pitch angle correction value calibration algorithm in one embodiment of the present invention;

[0029] Figure 6 is a schematic diagram of the ground point cloud correction effect in one embodiment of the present invention;

[0030] Figure 7 Schematic diagram of the pose correction effect of the translation part of the laser odometer in one embodiment of the present invention;

[0031] Figure 8 It is a schematic diagram of the posture correction effect of the rotating part of the laser odometer in one embodiment of the present invention;

[0032] Fig. 9 It is a schematic diagram of the structure of a computing device in one embodiment of the present invention. DETAILED DESCRIPTION

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

[0034] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0035] The present invention considers the calibration error of the internal parameter modeling of the mechanical laser radar, performs distortion correction on the obtained laser point cloud, and then reduces the correction method of the laser odometer posture estimation error. First, the distortion trend of the point cloud "near low and far high" is modeled as the error of the pitch angle β; secondly, a calibration algorithm is designed, and the point cloud collected by the laser radar is used as the information source to calibrate the pitch angle correction value Δβ; then, the original point cloud is corrected according to the internal parameter model; finally, the corrected point cloud is sent to the posture estimation algorithm to reduce the posture estimation error.

[0036] In one embodiment of the present invention, the calibration error of the mechanical laser radar is modeled by considering the intrinsic parameters, such as Figure 3 As shown, a laser odometer error correction method is provided. This embodiment uses the method applied to a terminal as an example for illustration. It can be understood that the method can also be applied to a server, and can also be applied to a system including a terminal and a server, and is implemented through the interaction between the terminal and the server. In this embodiment, the method includes the following steps:

[0037] Step 1: Obtain the original point cloud; the original point cloud includes the ground point cloud and the points of obstacles around the vehicle;

[0038] The original point cloud is a frame of point cloud obtained by rotating the laser radar one circle;

[0039] Step 2: According to the ground point cloud in the original point cloud, a ground point cloud shape model is established to determine the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated;

[0040] Step 3: calibrate the correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and use the calibrated correction model to perform online real-time correction on the original point cloud;

[0041] Step 4: Input the corrected original point cloud into the laser odometer for pose estimation.

[0042] In the above step 2, the ground point cloud shape model is established according to the ground point cloud in the original point cloud, specifically: Figure 2 It can be seen that the shape of the actual ground point cloud is affected by two factors: on the one hand, it comes from the angle of the laser radar relative to the ground, including: the slope of the ground itself, the pitch 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, it comes from the calibration error of the laser radar internal parameter modeling. The actual point cloud is the superposition of the above two trends, such as Figure 4 shown.

[0043] It should be noted that the changing trend of the angle of the laser radar relative to the ground has no effect on the accuracy of the pose estimation, because during the data acquisition process, the laser radar and the true value acquisition device (such as high-precision inertial navigation) are rigidly fixed to the vehicle body, and the pose between the two is associated through the external parameters between the sensors. Therefore, this trend is not the main cause of the vertical drift of the pose estimation result and does not need to be corrected. However, during the calibration process, the influence of this part of the trend needs to be eliminated first, and then the correction amount of the distortion trend derived from the calibration error of the laser radar internal parameter modeling is calibrated.

[0044] Therefore, modeling the ground point cloud shape model includes the following steps:

[0045] Step 21: perform ground segmentation on the original point cloud to obtain a ground point cloud;

[0046] Segment the ground part of a frame of point cloud, and only use the ground point cloud for subsequent operations. The present invention does not limit the specific ground segmentation method, but the segmentation quality of the ground point cloud should be guaranteed as much as possible. Accurate ground point cloud segmentation results help improve the effect of subsequent calibration algorithms. In the embodiment, the ground segmentation algorithm based on 2.5D depth image proposed by Bogoslavskyi et al. is used. The ground point cloud segmentation result is as follows: Figure 1 shown.

[0047] Step 22: Set the ground point cloud distribution to the unit normal vector On the ground plane, the pitch angle correction amount is calibrated based on the distortion trend of the laser radar internal parameter modeling calibration error according to the influencing factors of the actual ground point cloud shape;

[0048] Specifically: Set the ground point cloud distribution to the unit normal vector On the ground plane (to facilitate subsequent operations, n is uniformly specified here z >0, that is, the plane normal vector points to the top of the ground, and this convention is used in subsequent processing). Due to the error in the pitch angle β in the lidar internal parameter model, a distortion trend of "near low and far high" appears. Therefore, it is necessary to use the ground point cloud to calibrate the pitch angle correction Δβ to obtain the corrected point cloud coordinates P′ L , and then correct the overall distortion trend of the point cloud. The correction method is shown in formula (2). The correction effect of the ground point cloud is as follows: Figure 5 shown.

[0049]

[0050] In the above step 3, the correction model is calibrated according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, including the following steps:

[0051] Step 31: Unit normal vector to the ground plane Make estimates;

[0052] Estimate the normal vector direction of the ground plane for use in cost function construction. The specific steps include:

[0053] Step 311: Assume that the laser radar has N beams, and each beam rotates once to obtain M scanning points; wherein, there are m scanning points in the i-th beam. i points are segmented into ground points, i = 1, 2, ..., N;

[0054] From the modeling method of the ground point cloud shape, it is easy to know that the ground points on the same line are on the same plane, such as Figure 5 shown.

[0055] Step 312: Estimate the m on the i-th line harness. i The unit normal vector of the plane where the ground point lies And calculate its weight;

[0056] In this embodiment, the random sampling consensus (RANSAC) algorithm is used to estimate the m on the i-th scan line. i The unit normal vector of the plane where the ground point lies And calculate its weight, as shown in formula (3):

[0057]

[0058] Step 313: Calculate the unit normal vectors respectively The pitch angle θ in the spherical coordinate system i and azimuth φ i ;

[0059] As shown in formula (4) and formula (5):

[0060]

[0061]

[0062] Step 314: Use weights to convert the pitch angle θ i and azimuth φ i Perform weighted fusion respectively to obtain the unit normal vector of the ground plane Pitch angle in spherical coordinates and azimuth Estimation results;

[0063] As shown in formula (6) and formula (7):

[0064]

[0065]

[0066] Step 315: pass through the unit normal vector of the ground plane Pitch angle in spherical coordinates and azimuth Estimated result, calculate the unit normal vector of the ground plane The estimated results of

[0067] As shown in formula (8):

[0068]

[0069] Step 32: construct a cost function according to the unit normal vector estimation result to complete the calibration of the pitch angle correction value;

[0070] like Figure 6 As shown, the specific steps include:

[0071] Step 321: Fit the ground point cloud set to obtain the unit normal vector: and a plane with a distance error threshold of δ, and obtain the plane parameters, the set of interior points that conform to the plane model, and the set of exterior points that do not conform to the plane model, and calculate the error from a certain point to the plane;

[0072] In this embodiment, the RANSAC algorithm is used to analyze the ground point cloud set P ground Fitting is performed to obtain the unit normal vector: and the plane with a distance error threshold of δ, and obtain the plane parameter plane and the set of interior points P that conform to the plane model inlier And the set of external points P that do not conform to the plane model outlier , as shown in formula (9):

[0073]

[0074] In the formula, plane = [ABC d] is the plane parameter, indicating that the plane A·x+B·y+C·z+D = 0.

[0075] For point P i =[x i y i z i ] T , calculate the error e from the point to the plane i , as shown in formula (10):

[0076]

[0077] Step 322: Calculate the cost function value corresponding to the point according to the error i ;

[0078] In this embodiment, it is necessary to calculate the cost function value cost corresponding to a single point i Since the actual ground shape is different from the plane and there are objective errors in the ground segmentation algorithm, a robust kernel function is used to calculate the cost function value for outliers. The Huber kernel function or the Cauchy kernel function can be used. The Huber kernel function is shown in formula (11) and the Cauchy kernel function is shown in formula (12):

[0079]

[0080]

[0081] Step 323, calculating the cost function value cost corresponding to the single frame point cloud as a measure of the ground point cloud dedistortion effect;

[0082] In this embodiment, the interior point set P is comprehensively considered. inlier And the set of external points P outlier The impact on the error function value is based on the ratio of the number of internal and external points to the average cost function value of the internal points. And the average cost function value of the external point Perform weighted averaging, as shown in formula (13):

[0083]

[0084] Step 324: traverse all point clouds, construct an optimization problem, and complete the calibration of the pitch angle correction value:

[0085]

[0086] In the above step 4, the corrected original point cloud is input into the laser odometer for pose estimation, specifically:

[0087] After calibrating the pitch angle correction Δβ, the original point cloud is corrected online according to formula (2), and the corrected point cloud is sent to the laser odometer for pose estimation.

[0088] In this embodiment, the laser odometer uses the LeGO-LOAM algorithm proposed by Tixiao Shan 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. The results are shown in Figures 7 and 8. Figure 8 As shown in FIG. 1 , it can be seen that the estimation accuracy of the displacement along the z-axis in the translational degree of freedom and the roll angle and pitch angle in the rotational degree of freedom is significantly improved.

[0089] In one embodiment of the present invention, a laser odometer error correction system is provided, which includes: a data acquisition module, a ground point cloud shape model construction module, a correction module and an estimation module;

[0090] The data acquisition module is used to obtain the original point cloud; the original point cloud includes the ground point cloud and the points of obstacles around the vehicle;

[0091] The ground point cloud shape model building module builds the ground point cloud shape model according to the ground point cloud in the original point cloud; the model is calibrated and corrected according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated;

[0092] The correction module calibrates the correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and the calibrated correction model performs online real-time correction on the original point cloud;

[0093] The estimation module inputs the corrected original point cloud into the laser odometry for pose estimation.

[0094] The system provided in this embodiment is used to execute the above-mentioned method embodiments. Please refer to the above-mentioned embodiments for specific processes and detailed contents, which will not be repeated here.

[0095] like Fig. 9 As shown, it is a schematic diagram of the structure of a computing device provided in an embodiment of the present invention. The computing device can be a terminal, which can include: a processor, a communication interface, a memory, a display screen and an input device. Among them, the processor, the communication interface and the memory complete mutual communication through a communication bus. The processor is used to provide computing and control capabilities. The memory includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. When the computer program is executed by the processor, a laser odometer error correction method is implemented; the internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface is used to communicate with an external terminal in a wired or wireless manner. The wireless manner can be implemented through WIFI, a management network, NFC (near field communication) or other technologies. The display screen can be a liquid crystal display screen or an electronic ink display screen. The input device can be a touch layer covered on the display screen, or a key, trackball or touchpad set on the housing of the computing device, or an external keyboard, touchpad or mouse. The processor can call the logical instructions in the memory to execute the following method:

[0096] Obtain the original point cloud; the original point cloud includes the ground point cloud and the points of obstacles around the vehicle; according to the ground point cloud in the original point cloud, establish a ground point cloud shape model, determine the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated; calibrate the correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and use the calibrated correction model to perform online real-time correction on the original point cloud; input the corrected original point cloud into the laser odometer for posture estimation.

[0097] In addition, the logic instructions in the above-mentioned memory can be implemented in the form of software functional units and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0098] Those skilled in the art will understand that Fig. 9 The structure shown in the figure is only a block diagram of a part of the structure related to the scheme of the present application, and does not constitute a limitation on the computing device to which the scheme of the present application is applied. The specific computing device may include more or fewer components than shown in the figure, or combine certain components, or have a different arrangement of components.

[0099] In one embodiment of the present 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, and when the program instructions are executed by a computer, the computer can execute the methods provided by the above-mentioned method embodiments, for example, comprising: obtaining an original point cloud; the original point cloud comprises a ground point cloud and points of obstacles around the vehicle; based on the ground point cloud in the original point cloud, establishing a ground point cloud shape model, determining the plane of distribution of the ground point cloud and the parameters that need to be corrected and calibrated; calibrating the correction model according to the plane of distribution of the ground point cloud and the parameters that need 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 the laser odometer for pose estimation.

[0100] In one embodiment of the present invention, a non-transitory computer-readable storage medium is provided, which stores server instructions, and the computer instructions enable a computer to execute the methods provided by the above embodiments, for example, including: obtaining an original point cloud; the original point cloud includes a ground point cloud and points of obstacles around the vehicle; based on the ground point cloud in the original point cloud, a ground point cloud shape model is established to determine the plane of distribution of the ground point cloud and the parameters that need to be corrected and calibrated; according to the plane of distribution of the ground point cloud and the parameters that need to be corrected and calibrated, a correction model is calibrated, and the original point cloud is corrected online in real time by the calibrated correction model; the corrected original point cloud is input into the laser odometer for posture estimation.

[0101] The above embodiment provides a computer-readable storage medium, whose implementation principle and technical effect are similar to those of the above method embodiment, and will not be repeated here.

[0102] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0103] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 One or more processes and / or boxes Figure 1 A function specified in one or more boxes.

[0104] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0105] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A laser odometer error correction method, It is characterized in that include: Obtaining an original point cloud; the original point cloud includes a ground point cloud and points of obstacles around the vehicle; According to the ground point cloud in the original point cloud, a ground point cloud shape model is established to determine the plane of ground point cloud distribution and the parameters that need to be corrected and calibrated; A correction model is calibrated according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and the original point cloud is corrected online in real time by the calibrated correction model; Inputting the corrected original point cloud into the laser odometer for pose estimation; The step of establishing a ground point cloud shape model comprises: Performing ground segmentation on the original point cloud to obtain a ground point cloud; Set the ground point cloud distribution to the unit normal vector On the ground plane, the pitch angle correction amount is calibrated based on the distortion trend of the laser radar internal parameter modeling calibration error according to the influencing factors of the actual ground point cloud shape; The correction model is calibrated according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, including: The unit normal vector to the ground plane Make estimates; Constructing a cost function according to the unit normal vector estimation result to complete the calibration of the pitch angle correction value; The unit normal vector to the ground plane Make an estimate, including: Assume that the laser radar has N beams, and each beam rotates once to obtain M scanning points; among them, there are m points in the i-th beam. i points are segmented into ground points, i = 1, 2, ..., N; Estimate the m on the i-th line i The unit normal vector of the plane where the ground point lies And calculate its weight; Calculate the unit normal vectors respectively Elevation and azimuth angles in spherical coordinates; The pitch angle and the azimuth angle are weighted and fused respectively using the weights to obtain a unit normal vector of the ground plane. Elevation and azimuth estimation results in spherical coordinate system; The unit normal vector through the ground plane The pitch angle and azimuth angle estimation results in the spherical coordinate system are used to calculate the unit normal vector of the ground plane The estimated results of The step of constructing a cost function according to the unit normal vector estimation result to complete the calibration of the pitch angle correction amount includes: Fit the ground point cloud set to obtain the unit normal vector: and a plane with a distance error threshold of δ, and obtain plane parameters, a set of interior points that conform to the plane model, and a set of exterior points that do not conform to the plane model, and calculate the error from a certain point to the plane; Calculate the cost function value corresponding to the point according to the error; Calculating the cost function value corresponding to the single-frame point cloud as a measure of the dedistortion effect of the ground point cloud; Traverse all point clouds, construct optimization problems, and complete the calibration of pitch angle correction; The calculating of the cost function value corresponding to the single-frame point cloud includes: using a robust kernel function to calculate the cost function value for the outlier points; wherein the robust kernel function uses a Huber kernel function or a Cauchy kernel function.

2. The laser odometer error correction method according to claim 1, It is characterized in that The calculating of the cost function value corresponding to the single-frame point cloud includes: performing weighted average calculation on the average cost function value of the inner points and the average cost function value of the outer points according to the ratio of the number of the inner and outer points.

3. A laser odometer error correction system, It is characterized in that include: Data acquisition module, ground point cloud shape model building module, correction module and estimation module; The data acquisition module is used to acquire an original point cloud; the original point cloud includes a ground point cloud and points of obstacles around the vehicle; The ground point cloud shape model building module builds a ground point cloud shape model according to the ground point cloud in the original point cloud, determines the plane of ground point cloud distribution and the parameters that need to be corrected and calibrated; The correction module calibrates the correction model according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, and uses the calibrated correction model to perform online real-time correction on the original point cloud; The estimation module inputs the corrected original point cloud into the laser odometer for pose estimation; The step of establishing a ground point cloud shape model comprises: Performing ground segmentation on the original point cloud to obtain a ground point cloud; Set the ground point cloud distribution to the unit normal vector On the ground plane, the pitch angle correction amount is calibrated based on the distortion trend of the laser radar internal parameter modeling calibration error according to the influencing factors of the actual ground point cloud shape; The correction model is calibrated according to the plane of the ground point cloud distribution and the parameters that need to be corrected and calibrated, including: The unit normal vector to the ground plane Make estimates; Constructing a cost function according to the unit normal vector estimation result to complete the calibration of the pitch angle correction value; The unit normal vector to the ground plane Make an estimate, including: Assume that the laser radar has N beams, and each beam rotates once to obtain M scanning points; among them, there are m points in the i-th beam. i points are segmented into ground points, i = 1, 2, ..., N; Estimate the m on the i-th line i The unit normal vector of the plane where the ground point lies And calculate its weight; Calculate the unit normal vectors respectively Elevation and azimuth angles in spherical coordinates; The pitch angle and the azimuth angle are weighted and fused respectively using the weights to obtain a unit normal vector of the ground plane. Elevation and azimuth estimation results in spherical coordinate system; The unit normal vector through the ground plane The pitch angle and azimuth angle estimation results in the spherical coordinate system are used to calculate the unit normal vector of the ground plane The estimated results of The step of constructing a cost function according to the unit normal vector estimation result to complete the calibration of the pitch angle correction amount includes: Fit the ground point cloud set to obtain the unit normal vector: and a plane with a distance error threshold of δ, and obtain plane parameters, a set of interior points that conform to the plane model, and a set of exterior points that do not conform to the plane model, and calculate the error from a certain point to the plane; Calculate the cost function value corresponding to the point according to the error; Calculating the cost function value corresponding to the single-frame point cloud as a measure of the dedistortion effect of the ground point cloud; Traverse all point clouds, construct optimization problems, and complete the calibration of pitch angle correction; The calculating of the cost function value corresponding to the single-frame point cloud includes: using a robust kernel function to calculate the cost function value for the outlier points; wherein the robust kernel function uses a Huber kernel function or a Cauchy kernel function.

4. A computer-readable storage medium storing one or more programs, It is characterized in that The one or more programs include instructions, which, when executed by a computing device, cause the computing device to perform any one of the methods of claims 1 to 2.

5. A computing device, It is characterized in that include: One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for executing any one of the methods described in claims 1 to 2.

Citation Information

Patent Citations

  • Simultaneous localization and mapping method based on vision and laser radar

    CN112258600A

  • Laser point cloud odometer positioning error real-time correction method and system

    CN112731357A

  • Method for correcting point cloud motion distortion of laser radar

    CN112859051A

  • Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit

    CN113066105A

  • Laser radar point cloud data correction method based on wheel type odometer

    CN113219440A