CN112731357A - Real-time correction method and system for positioning error of laser point cloud odometer - Google Patents
Real-time correction method and system for positioning error of laser point cloud odometer Download PDFInfo
- Publication number
- CN112731357A CN112731357A CN202011609084.0A CN202011609084A CN112731357A CN 112731357 A CN112731357 A CN 112731357A CN 202011609084 A CN202011609084 A CN 202011609084A CN 112731357 A CN112731357 A CN 112731357A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- laser point
- real
- cloud data
- laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 238000012937 correction Methods 0.000 title claims abstract description 35
- 238000006243 chemical reaction Methods 0.000 claims description 32
- 238000004364 calculation method Methods 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 11
- 238000012935 Averaging Methods 0.000 claims description 9
- 230000008030 elimination Effects 0.000 claims description 6
- 238000003379 elimination reaction Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 4
- 150000001875 compounds Chemical class 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 description 6
- 238000005452 bending Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000002238 attenuated effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Traffic Control Systems (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention relates to a real-time correction method and a real-time correction system for positioning errors of a laser point cloud odometer, which are characterized by comprising the following steps of: 1) extracting road surface point clouds from historical laser point cloud data collected by an intelligent networked vehicle equipped with a laser radar, calculating to obtain model parameters, and constructing an empirical model; 2) and correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting an empirical model to obtain the laser point cloud data with the error eliminated. The method adopts the correction model to correct the accumulated positioning error of the laser point cloud odometer, can provide more accurate self-parking position estimation, can correct the error of the laser point cloud odometer on line, and can be widely applied to the field of intelligent networked automobile environment sensing.
Description
Technical Field
The invention relates to a real-time correction method and a real-time correction system for positioning errors of a laser point cloud odometer of a road vehicle, and belongs to the field of intelligent networked automobile environment sensing.
Background
The intelligent networked automobile needs to accurately sense the surrounding environment and the state of the intelligent networked automobile so as to support subsequent decision and control. And accurate estimation of the self pose is the basis for the functions of track planning, control and the like of the intelligent network connection vehicle. And the passive pose estimation based on the GPS is limited by satellite signals, so that the whole running working condition of high-level automatic driving is difficult to support. The laser radar can directly measure the distance information of the surrounding environment, and has accurate measurement precision and a longer measurement range. Therefore, the laser radar point cloud-based mileage estimation is widely used in the positioning system of the intelligent networked automobile.
However, the position and pose estimation based on the odometer has accumulated errors, namely the track can drift after long-time work, so that the application of the method to the field of intelligent networked automobiles has certain limitation. On the other hand, as the vehicle runs on the road surface, the collected laser radar point cloud is not uniformly distributed in the direction vertical to the road surface, and the drift in the direction vertical to the running road is mostly presented on the basis of the mileage estimation of the laser radar point cloud.
For this problem, there are two main solutions in the existing research: 1. the track is corrected at intervals by detecting a driving closed loop and introducing absolute positioning information such as a GPS (global positioning system) and the like, but the method cannot obtain accurate pose estimation in real time and depends on the input of an external information source; 2. the method comprises the steps of collecting and estimating track drift for multiple times, training a calibration function, and uniformly correcting a track which is driven before after a period of driving, wherein similarly, the method cannot estimate the self-vehicle positioning information in real time. Therefore, the existing method cannot correct the positioning error of the laser point cloud odometer in real time.
Disclosure of Invention
Aiming at the problems, the invention aims to provide a real-time correction method and a real-time correction system for positioning errors of a laser point cloud odometer, which are mainly used for eliminating reverse track drift perpendicular to a road surface almost in an application scene of an intelligent networked automobile.
In order to achieve the purpose, the invention adopts the following technical scheme:
the invention provides a real-time correction method for positioning errors of a laser point cloud odometer, which comprises the following steps:
1) extracting road surface point clouds from historical laser point cloud data collected by an intelligent networked vehicle equipped with a laser radar, constructing an empirical model, and calculating to obtain model parameters;
2) and correcting real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model and model parameters to obtain the laser point cloud data with the error eliminated.
Further, in the step 1), extracting road surface point cloud from historical laser point cloud data collected by an intelligent networked vehicle equipped with a laser radar, constructing an empirical model, and calculating to obtain model parameters, the method comprises the following steps:
1.1) analyzing the distortion rule of historical laser point cloud data, and constructing an empirical model s according to the analysis result;
1.2) extracting a multi-frame pavement laser point cloud fitting spherical surface from historical laser point cloud data, and averaging the spherical radius of the fitting spherical surface to obtain model parameters of the empirical model
Further, in the step 1.1), the empirical model s is:
in the formula (I), the compound is shown in the specification,
the parameters of the empirical model are approximately equal to the curvature radius of the plane point cloud distortion at the position where | z | ═ 1; d2＝x2+y2And x, y and z are coordinates in a laser radar coordinate system.
Further, in the step 1.2), extracting a multi-frame pavement laser point cloud fitting spherical surface from the laser point cloud data, averaging the spherical radius of the fitting spherical surface, and calculating to obtain an empirical modeModel parameters of model
The method comprises the following steps:
1.2.1) randomly extracting m frames of laser point clouds from the collected laser point cloud data;
1.2.2) extracting points { X) belonging to the road surface by using a plane random sampling consistency method for each frame of laser point cloud1，X2，...，XmJudging whether coordinate conversion is needed or not according to the difference between the z axis of the original coordinate system of the laser radar and the vertical direction of the ground, and entering step 1.2.3 when the coordinate conversion is needed, or entering step 1.2.5);
1.2.3) normalized Normal vector n from road surface1，n2，...，nmCalculating an average normal vector
And according to the obtained average normal vector
Calculating to obtain a conversion matrix T, carrying out coordinate conversion on the collected laser point cloud data, and entering the step 1.2.4);
1.2.4) pairs of points { X) of the sets of road surfaces extracted in step 1.2.2) using a rotation matrix T1，X2，...，XmCoordinate conversion is carried out to obtain the average z value of each group of points
And fitting a sphere on each group of points based on the average z value of each group of points to obtain the radius of the sphere of each group of points
1.2.5) points { X) of the respective sets of road surfaces extracted according to step 1.2.2)1，X2，...，XmObtaining the average value of each group of points, and fitting a curved surface on each group of points to obtain the spherical radius of each group of points;
1.2.6) spherical radius of each set of points obtained according to step 1.2.4) or 1.2.5)
Calculating to obtain model parameters
Further, in the step 1.2.2), the method for determining whether the coordinate conversion is required according to the difference between the z-axis of the original coordinate system of the laser radar and the vertical direction of the ground includes the following steps: judging the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground, if the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground is larger than a preset threshold value, carrying out coordinate conversion, otherwise, carrying out coordinate conversion.
Further, in the step 1.2.3), according to the normalized normal vector { n) of the road surface1，n2，...，nmCalculating an average normal vector
And according to the obtained average normal vector
The method for obtaining the transformation matrix T by calculation comprises the following steps:
first, based on the average normal vector
Obtaining a conversion formula:
wherein v is a rotation axis and θ is a rotation angle;
then, a rotation quaternion q is constructed according to the above conversion equation:
in the formula, q0、q1、q2、q3Four components of a quaternion, q0Real part called quaternion, q1，q2，q3An imaginary part called a quaternion;
and finally, converting the rotation quaternion into a rotation matrix T to obtain:
further, in the step 1.2.6), model parameters
The calculation formula of (2) is as follows:
in the formula (Q)1，Q3) Respectively the upper and lower quartiles in all spherical radii.
Further, in the step 2), the method for real-time correcting the real-time laser point cloud data collected by the intelligent networked vehicle by using the established empirical model and the model parameters comprises the following steps:
2.1) judging an included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground, if the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground is greater than a preset threshold value, entering a step 2.2), and if not, entering a step 2.4);
2.2) carrying out coordinate conversion on the original laser point cloud data to ensure that the included angle between the laser radar z-axis of the converted intelligent networked vehicle and the vertical direction of the ground is smaller than a preset threshold value, and entering the step 2.3);
2.3) real-time correction is carried out on real-time laser point cloud data collected by the intelligent networked vehicle by adopting the established empirical model, and coordinate inverse transformation is carried out on the point cloud data after the real-time correction so as to transform the point cloud data back to an original coordinate system, thus obtaining the laser point cloud data after the error elimination;
the calculation formula for real-time correction is as follows:
z′＝z·s
in the formula, z' is a corrected laser point cloud z coordinate;
and 2.4) correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model to obtain the laser point cloud data with the error eliminated.
In a second aspect of the present invention, a real-time correction system for positioning errors of a laser point cloud odometer is provided, which includes: the system comprises an empirical model building module, a data acquisition module and a data processing module, wherein the empirical model building module is used for extracting road surface point clouds from historical laser point cloud data acquired by an intelligent networked vehicle equipped with a laser radar, building an empirical model and calculating to obtain model parameters; and the error correction module is used for correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model and the model parameters to obtain the laser point cloud data with the error eliminated.
Further, the empirical model building module comprises: the model building module is used for analyzing the distortion rule of the laser point cloud data and building an experience model s according to the analysis result; a model parameter calculation module for extracting multi-frame pavement laser point cloud fitting sphere from the historical laser point cloud data, and averaging the spherical radius of the fitting sphere to obtain model parameters of the empirical model
Due to the adoption of the technical scheme, the invention has the following advantages: 1. the invention adopts the correction model to correct the accumulated positioning error of the laser point cloud odometer, and can provide more accurate self-parking position estimation; 2. compared with methods such as closed loop and post-processing, the method can correct the error of the laser point cloud odometer on line; 3. the invention is suitable for various mechanical laser radars, and the algorithm is decoupled from hardware; 4. the method provided by the invention only needs to preprocess the input point cloud, has small calculation amount and cannot influence the real-time performance of the odometer. Therefore, the method can be widely applied to the field of intelligent networked automobile environment perception.
Drawings
FIG. 1 is an example of distortion of a laser point cloud measurement;
FIG. 2 is a schematic view of a point cloud distortion processing flow of the present invention;
fig. 3 is a schematic diagram of a particular embodiment of the present invention.
Detailed Description
The invention is described in detail below with reference to the figures and examples. It is to be understood, however, that the drawings are provided solely for the purposes of promoting an understanding of the invention and that they are not to be construed as limiting the invention.
Because the laser radar adopts the ranging based on TOF (time of flight), and the ranging precision is influenced by a plurality of influences such as incident angle, external illumination, air humidity and the like, the measurement result of the same laser radar under the similar scene often shows a transverse deviation error, the returned point cloud is distorted, and the characteristic matching among the multi-frame point cloud is influenced, so that the odometer based on the laser point cloud has constant drift.
As shown in FIG. 1, the road surface point cloud measured by the Velodyne-64E laser radar is shown, and the gray scale of the measuring point in the map reflects the height (unit: meter) of the measuring point on a horizontal plane. The road surface which is supposed to be horizontal (the road surface is estimated to be approximately horizontal through the running track of the vehicle measured by the differential GPS), and the measurement result shows obvious arc-shaped bending.
As shown in fig. 2, based on the above analysis, the present invention provides a real-time correction method for positioning error of a laser point cloud odometer, comprising the following steps:
1) according to historical laser point cloud data collected by an intelligent networked vehicle equipped with a laser radar, extracting road surface point cloud, constructing an empirical model, and calculating to obtain model parameters.
2) And performing real-time distortion elimination on the real-time laser point cloud data acquired by the intelligent networked vehicle by adopting the established empirical model to obtain the laser point cloud data after error elimination.
In the step 1), the method for constructing the empirical model includes the following steps:
1.1) analyzing the distortion rule of the existing laser point cloud data, and constructing an empirical model s according to the analysis result.
Because the laser point cloud ranging error is complex in reason, the invention discovers that the measured point cloud distortion has a fixed rule by observing the deformation condition of multi-frame point clouds in a driving environment: 1. bending the points on the same horizontal plane into curved surfaces with the same radius; 2. to a certain extent, the farther the lidar is from the horizontal plane, the larger the radius of curvature of the bend.
Based on these two results, the present invention constructs an empirical model s:
in the formula (I), the compound is shown in the specification,
is a parameter of the empirical model, which is approximately equal to the curvature radius of the plane point cloud distortion at | z | ═ 1; d2＝x2+y2And x, y and z are coordinates in a laser radar coordinate system.
To improve the calculation efficiency, the above formula (1) can be simplified as follows:
further reducing the formula (2) to:
due to the selection of parameters
Thus, it is possible to provide
Tends to be zero, pair
Performing a first order Taylor expansion can simplify equation (3) to:
1.2) extracting a multi-frame pavement laser point cloud fitting spherical surface from the existing laser point cloud data, averaging the spherical radius of the fitting spherical surface, and calculating to obtain model parameters of the empirical model
After the empirical model s is determined, model parameters are reasonably selected
As has been said in the foregoing, the present invention,
the value is approximately equal to | z | -1 radius of curvature of the plane point cloud distortion. The invention collects the data because it is practically almost impossible to find a plane that is level and comparable to the range of the lidarMulti-frame road surface point cloud fitting spherical surface and averaging method for calculating parameters
The specific implementation method is as follows. It should be noted that the following model parameters
The calculation result of (2) does not necessarily lead the correction result to be optimal, and the model parameters can be manually compared with the actual driving track according to the estimation result of the laser point cloud odometer
Fine tuning is performed.
Specifically, the method comprises the following steps:
1.2.1) randomly extracting m frames of laser point clouds from the collected laser point cloud data, and generally selecting about 50 frames of point clouds.
When the laser point cloud data is obtained, the intelligent networked vehicle equipped with the laser radar is driven on a road in a preset range to form a closed loop, and the road surface is required to be as flat as possible. Specifically, a part of scenes in which the vehicle travels to calibrate the parameters may be selected in an actual application scene. In actual operation, the calibration result can be determined according to the calibration result, and the calibration can be stopped when the calibration result is not changed greatly or the calibration result is considered to meet the requirement.
1.2.2) extracting points { X) belonging to the road surface by using a plane random sampling consistency (RANSAC) method for each frame of laser point cloud1，X2，...，XmAnd when coordinate conversion is needed, entering step 1.2.3), otherwise, entering step 1.2.5).
Before the fitting of the spherical surface, if the difference between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground is large, the difference mainly refers to the condition that the laser radar is specially obliquely installed on the roof, if the laser radar is approximately horizontally installed on the roof, errors caused by installation can be disregarded, coordinate conversion can be firstly carried out on the laser point cloud so that the directions of the laser point cloud and the laser point cloud are approximately coincident, namely the z axis of the laser radar is approximately vertical to the road surface.
1.2.3) normalized Normal vector n from road surface1，n2，...，nmCalculating an average normal vector
And according to the obtained average normal vector
And (5) calculating to obtain a conversion matrix T for carrying out coordinate conversion on the acquired laser point cloud data, and entering the step 1.2.4).
As mentioned above, the lidar z-axis is required to be as perpendicular as possible to the ground, and therefore can be based on an average normal vector
The collected laser point cloud is subjected to coordinate conversion to meet the condition, and the conversion formula is as follows:
where v is the rotation axis and θ is the rotation angle.
And constructing a rotation quaternion q according to the conversion formula:
in the formula, q0、q1、q2、q3Four components of a quaternion, q0Real part called quaternion, q1，q2，q3Referred to as the imaginary part of the quaternion.
Converting the rotation quaternion into a rotation matrix T to obtain:
1.2.4) pairs of points { X) of the sets of road surfaces extracted in step 1.2.2) using a rotation matrix T1，X2，...，XmCoordinate conversion is carried out to obtain the average z value of each group of points
And fitting a sphere on each group of points based on the average z value of each group of points to obtain the radius of the sphere of each group of points
It should be noted here that the bending direction is not fixed because the deformation of the laser point cloud measurement result is influenced by many factors, so that here
Is a signed radius. In order to keep the unity with the following, the circle center of the road surface fitting spherical surface is specified to be positive below the road surface, and the circle center is negative.
1.2.5) points { X) of the respective sets of road surfaces extracted according to step 1.2.2)1，X2，...，XmAnd solving the average value of each group of points, and fitting a curved surface at each group of points to obtain the spherical radius of each group of points.
1.2.6) calculating to obtain model parameters according to the spherical radius of each group of points obtained in the step 1.2.4) or the step 1.2.5)
For determining model parameters
The invention calculates by the above-mentioned multigroup result. Because the running road surface condition of the vehicle is complex, the radius distribution is often more discrete, more interference exists, and the invention uses the upper and lower quartiles (Q) in all spherical radii1，Q3) These effects are attenuated as a threshold method. In addition, the radius of the fitting spherical surface is generally larger, and positive and negative large jumps are possibly caused, so the calculation is carried out through the curvatureParameter(s)
The calculation formula is as follows:
in the step 2), all the points acquired by the laser radar are processed according to the determined empirical model and the model parameters, so that distortion elimination can be realized. Specifically, the method comprises the following steps:
2.1) judging an included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground, if the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground is greater than a preset threshold value, entering a step 2.2), and if not, entering a step 2.4);
2.2) carrying out coordinate conversion on the original laser point cloud data to ensure that the included angle between the laser radar z-axis of the converted intelligent networked vehicle and the vertical direction of the ground is smaller than a preset threshold value, and entering the step 2.3);
2.3) real-time correction is carried out on real-time laser point cloud data collected by the intelligent networked vehicle by adopting the established empirical model, and coordinate inverse transformation is carried out on the point cloud data after the real-time correction so as to transform the point cloud data back to an original coordinate system, thus obtaining the laser point cloud data after the error elimination;
the calculation formula for real-time correction is as follows:
z′＝z·s (11)
in the formula, z' is a corrected laser point cloud z coordinate;
and 2.4) correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model to obtain the laser point cloud data with the error eliminated.
The invention also provides a real-time correction system for the positioning error of the laser point cloud odometer, which comprises the following steps: the system comprises an empirical model building module, a data acquisition module and a data processing module, wherein the empirical model building module is used for extracting road surface point clouds from historical laser point cloud data acquired by an intelligent networked vehicle equipped with a laser radar, building an empirical model and calculating to obtain model parameters; and the error correction module is used for correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model and the model parameters to obtain the laser point cloud data with the error eliminated.
Further, the empirical model building module comprises: the model building module is used for analyzing the distortion rule of the laser point cloud data and building an experience model s according to the analysis result; a model parameter calculation module for extracting multi-frame pavement laser point cloud fitting sphere from the historical laser point cloud data, and averaging the spherical radius of the fitting sphere to obtain model parameters of the empirical model
Examples
In one example, the present invention uses a point cloud of the K TTI-odometric data set, which is collected by the velodyne-64E lidar, and has completed motion compensation. Firstly, randomly selecting 50 frames of point clouds belonging to a road surface point { X1，X2，...，X50And according to the fitted spherical radius
And the average z value of each set of points
Calculating parameters
In the example, 50 frames are uniformly extracted from the data set sequence 00 to obtain model parameters
The coordinate conversion step in equations (6) to (8) can be omitted because of its vertical horizontal plane placement of the lidar. Then, the model parameters are selected and the method is applied to process all the input laser point clouds. The processed laser point cloud can be subjected to mileage calculation by adopting various methods. In this example, a planar point is extracted from the k frame point cloud according to the curvature of each point
And corner point
Then based on an Iterative Closest Point (ICP) algorithm and
and
matching is carried out, and further the pose transformation between the kth frame and the (k-1) th frame is calculated
The overall flow of this example is shown in fig. 3.
Parameters selected in this example
The application has better effect in all sequences in the data set, because all data are collected by the same laser radar in similar scenes in the example. It should be noted that different parameters need to be selected when different lidar is used or when scene changes are large
The method can well correct the positioning error of the conventional laser point cloud odometer by testing various laser radars and operating environments through a real vehicle test, and also well improves the positioning precision of the laser point cloud odometer by testing on an opening data set.
The present invention is not limited to the above embodiments, and any changes or substitutions that can be easily made by those skilled in the art within the technical scope of the present invention are also within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (10)
1. A real-time correction method for positioning errors of a laser point cloud odometer is characterized by comprising the following steps:
1) extracting road surface point clouds from historical laser point cloud data collected by an intelligent networked vehicle equipped with a laser radar, constructing an empirical model, and calculating to obtain model parameters;
2) and correcting real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model and model parameters to obtain the laser point cloud data with the error eliminated.
2. The method for real-time correction of laser point cloud odometer positioning error as claimed in claim 1, wherein: in the step 1), extracting road surface point clouds from historical laser point cloud data collected by an intelligent networked vehicle equipped with a laser radar, constructing an empirical model, and calculating to obtain model parameters, the method comprises the following steps:
1.1) analyzing the distortion rule of historical laser point cloud data, and constructing an empirical model s according to the analysis result;
1.2) extracting a multi-frame pavement laser point cloud fitting spherical surface from historical laser point cloud data, and averaging the spherical radius of the fitting spherical surface to obtain model parameters of the empirical model
3. The method for real-time correction of laser point cloud odometer positioning error as claimed in claim 2, wherein: in step 1.1), the empirical model s is:
in the formula (I), the compound is shown in the specification,
the parameters of the empirical model are approximately equal to the curvature radius of the plane point cloud distortion at the position where | z | ═ 1; d2＝x2+y2And x, y and z are coordinates in a laser radar coordinate system.
4. The method for real-time correction of laser point cloud odometer positioning error as claimed in claim 2, wherein: in the step 1.2), extracting a multi-frame pavement laser point cloud fitting spherical surface from the laser point cloud data, averaging the spherical radius of the fitting spherical surface, and calculating to obtain model parameters of the empirical model
The method comprises the following steps:
1.2.1) randomly extracting m frames of laser point clouds from the collected laser point cloud data;
1.2.2) extracting points { X) belonging to the road surface by using a plane random sampling consistency method for each frame of laser point cloud1，X2，...，XmJudging whether coordinate conversion is needed or not according to the difference between the z axis of the original coordinate system of the laser radar and the vertical direction of the ground, and entering step 1.2.3 when the coordinate conversion is needed, or entering step 1.2.5);
1.2.3) normalized Normal vector n from road surface1，n2，...，nmCalculating an average normal vector
And according to the obtained average normal vector
Calculating to obtain a conversion matrix T, carrying out coordinate conversion on the collected laser point cloud data, and entering the step 1.2.4);
1.2.4) pairs of points { X) of the sets of road surfaces extracted in step 1.2.2) using a rotation matrix T1，X2，...，XmCoordinate conversion is carried out to obtain the average z value of each group of points
And fitting a sphere on each group of points based on the average z value of each group of points to obtain the radius of the sphere of each group of points
1.2.5) points { X) of the respective sets of road surfaces extracted according to step 1.2.2)1，X2，...，XmObtaining the average value of each group of points, and fitting a curved surface on each group of points to obtain the spherical radius of each group of points;
1.2.6) spherical radius of each set of points obtained according to step 1.2.4) or 1.2.5)
Calculating to obtain model parameters
5. The method for real-time correction of laser point cloud odometer positioning error as claimed in claim 1, wherein: in the step 1.2.2), the method for judging whether the coordinate conversion is needed according to the difference between the z axis of the original coordinate system of the laser radar and the vertical direction of the ground comprises the following steps: judging the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground, if the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground is larger than a preset threshold value, carrying out coordinate conversion, otherwise, carrying out coordinate conversion.
6. The method of claim 4, wherein the method comprises the following steps: in the step 1.2.3), according to the normalized normal vector { n of the road surface1，n2，...，nmCalculating an average normal vector
And according to the obtained average normal vector
The method for obtaining the transformation matrix T by calculation comprises the following steps:
first, based on the average normal vector
Obtaining a conversion formula:
wherein v is a rotation axis and θ is a rotation angle;
then, a rotation quaternion q is constructed according to the above conversion equation:
in the formula, q0、q1、q2、q3Four components of a quaternion, q0Real part called quaternion, q1，q2，q3An imaginary part called a quaternion;
and finally, converting the rotation quaternion into a rotation matrix T to obtain:
7. the method of claim 4, wherein the method comprises the following steps: in said step 1.2.6), the model parameters
The calculation formula of (2) is as follows:
in the formula (Q)1，Q3) Respectively the upper and lower quartiles in all spherical radii.
8. The method for real-time correction of laser point cloud odometer positioning error as claimed in claim 1, wherein: in the step 2), the method for real-time correcting the real-time laser point cloud data collected by the intelligent networked vehicle by using the established empirical model and the model parameters comprises the following steps:
2.1) judging an included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground, if the included angle between the z axis of the original coordinate system of the laser radar of the intelligent networked vehicle and the vertical direction of the ground is greater than a preset threshold value, entering a step 2.2), and if not, entering a step 2.4);
2.2) carrying out coordinate conversion on the original laser point cloud data to ensure that the included angle between the laser radar z-axis of the converted intelligent networked vehicle and the vertical direction of the ground is smaller than a preset threshold value, and entering the step 2.3);
2.3) real-time correction is carried out on real-time laser point cloud data collected by the intelligent networked vehicle by adopting the established empirical model, and coordinate inverse transformation is carried out on the point cloud data after the real-time correction so as to transform the point cloud data back to an original coordinate system, thus obtaining the laser point cloud data after the error elimination;
the calculation formula for real-time correction is as follows:
z′＝z·s
in the formula, z' is a corrected laser point cloud z coordinate;
and 2.4) correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model to obtain the laser point cloud data with the error eliminated.
9. A real-time correction system for positioning errors of a laser point cloud odometer is characterized by comprising:
the system comprises an empirical model building module, a data acquisition module and a data processing module, wherein the empirical model building module is used for extracting road surface point clouds from historical laser point cloud data acquired by an intelligent networked vehicle equipped with a laser radar, building an empirical model and calculating to obtain model parameters;
and the error correction module is used for correcting the real-time laser point cloud data acquired by the intelligent networked vehicle in real time by adopting the established empirical model and the model parameters to obtain the laser point cloud data with the error eliminated.
10. The system of claim 9 for real-time correction of laser point cloud odometer positioning errors, wherein: the empirical model building module comprises:
the model building module is used for analyzing the distortion rule of the laser point cloud data and building an experience model s according to the analysis result;
a model parameter calculation module for extracting multi-frame pavement laser point cloud fitting sphere from the historical laser point cloud data, and averaging the spherical radius of the fitting sphere to obtain model parameters of the empirical model
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011609084.0A CN112731357A (en) | 2020-12-30 | 2020-12-30 | Real-time correction method and system for positioning error of laser point cloud odometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011609084.0A CN112731357A (en) | 2020-12-30 | 2020-12-30 | Real-time correction method and system for positioning error of laser point cloud odometer |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112731357A true CN112731357A (en) | 2021-04-30 |
Family
ID=75610882
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011609084.0A Pending CN112731357A (en) | 2020-12-30 | 2020-12-30 | Real-time correction method and system for positioning error of laser point cloud odometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112731357A (en) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180306922A1 (en) * | 2017-04-20 | 2018-10-25 | Baidu Online Network Technology (Beijing) Co., Ltd | Method and apparatus for positioning vehicle |
| CN109313707A (en) * | 2016-06-01 | 2019-02-05 | 维迪私人有限公司 | Optical measurement and scanning system and application method |
| CN110244321A (en) * | 2019-04-22 | 2019-09-17 | 武汉理工大学 | A kind of road based on three-dimensional laser radar can traffic areas detection method |
| WO2019242174A1 (en) * | 2018-06-21 | 2019-12-26 | 华南理工大学 | Method for automatically detecting building structure and generating 3d model based on laser radar |
| CN111239757A (en) * | 2020-03-12 | 2020-06-05 | 湖南大学 | Automatic extraction method and system for road surface characteristic parameters |
| CN111398984A (en) * | 2020-03-22 | 2020-07-10 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
| CN111521195A (en) * | 2020-04-10 | 2020-08-11 | 广州铁路职业技术学院（广州铁路机械学校） | Intelligent robot |
| US20200284883A1 (en) * | 2019-03-08 | 2020-09-10 | Osram Gmbh | Component for a lidar sensor system, lidar sensor system, lidar sensor device, method for a lidar sensor system and method for a lidar sensor device |
| CN111929699A (en) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | Laser radar inertial navigation odometer considering dynamic obstacles and mapping method and system |
| CN112082545A (en) * | 2020-07-29 | 2020-12-15 | 武汉威图传视科技有限公司 | Map generation method, device and system based on IMU and laser radar |
-
2020
- 2020-12-30 CN CN202011609084.0A patent/CN112731357A/en active Pending
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109313707A (en) * | 2016-06-01 | 2019-02-05 | 维迪私人有限公司 | Optical measurement and scanning system and application method |
| US20180306922A1 (en) * | 2017-04-20 | 2018-10-25 | Baidu Online Network Technology (Beijing) Co., Ltd | Method and apparatus for positioning vehicle |
| WO2019242174A1 (en) * | 2018-06-21 | 2019-12-26 | 华南理工大学 | Method for automatically detecting building structure and generating 3d model based on laser radar |
| US20200284883A1 (en) * | 2019-03-08 | 2020-09-10 | Osram Gmbh | Component for a lidar sensor system, lidar sensor system, lidar sensor device, method for a lidar sensor system and method for a lidar sensor device |
| CN110244321A (en) * | 2019-04-22 | 2019-09-17 | 武汉理工大学 | A kind of road based on three-dimensional laser radar can traffic areas detection method |
| CN111239757A (en) * | 2020-03-12 | 2020-06-05 | 湖南大学 | Automatic extraction method and system for road surface characteristic parameters |
| CN111398984A (en) * | 2020-03-22 | 2020-07-10 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
| CN111521195A (en) * | 2020-04-10 | 2020-08-11 | 广州铁路职业技术学院（广州铁路机械学校） | Intelligent robot |
| CN111929699A (en) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | Laser radar inertial navigation odometer considering dynamic obstacles and mapping method and system |
| CN112082545A (en) * | 2020-07-29 | 2020-12-15 | 武汉威图传视科技有限公司 | Map generation method, device and system based on IMU and laser radar |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Dellenbach et al. | Ct-icp: Real-time elastic lidar odometry with loop closure | |
| US11131752B2 (en) | Vehicle navigation system using pose estimation based on point cloud | |
| CN108731670B (en) | Inertial/visual odometer integrated navigation positioning method based on measurement model optimization | |
| CN110859044B (en) | Integrated sensor calibration in natural scenes | |
| CN109685858B (en) | Monocular camera online calibration method | |
| CN108868268B (en) | Unmanned parking space posture estimation method based on point-to-surface distance and cross-correlation entropy registration | |
| CN112230243B (en) | Indoor map construction method for mobile robot | |
| CN112612862B (en) | Grid map positioning method based on point cloud registration | |
| CN113466890B (en) | Light laser radar inertial combination positioning method and system based on key feature extraction | |
| CN110889808A (en) | Positioning method, device, equipment and storage medium | |
| CN110187375A (en) | A kind of method and device improving positioning accuracy based on SLAM positioning result | |
| CN115840196B (en) | Laser radar inter-calibration method and device based on entity calibration | |
| CN112346104A (en) | Unmanned aerial vehicle information fusion positioning method | |
| CN114755662A (en) | Calibration method and device for laser radar and GPS with road-vehicle fusion perception | |
| CN113819904B (en) | polarization/VIO three-dimensional attitude determination method based on zenith vector | |
| CN102830391B (en) | Accuracy index calculating method of infrared search and track system | |
| CN113759364A (en) | Millimeter wave radar continuous positioning method and device based on laser map | |
| CN112558044A (en) | Automatic correction method for vehicle-mounted laser radar pitch angle | |
| CN112731357A (en) | Real-time correction method and system for positioning error of laser point cloud odometer | |
| CN114690230A (en) | Automatic driving vehicle navigation method based on visual inertia SLAM | |
| CN113390376B (en) | Method, device and system for determining cabin displacement of wind generating set | |
| CN114705223A (en) | Inertial navigation error compensation method and system for multiple mobile intelligent bodies in target tracking | |
| CN114004949A (en) | Airborne point cloud assisted mobile measurement system arrangement parameter calibration method and system | |
| Pishehvari et al. | Robust range-doppler registration with hd maps | |
| CN107796417B (en) | Method for adaptively estimating scene matching and inertial navigation installation error |
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 |