CN116413706A - Method for simultaneously establishing graph and calibrating internal reference of laser radar on mobile carrier - Google Patents
Method for simultaneously establishing graph and calibrating internal reference of laser radar on mobile carrier Download PDFInfo
- Publication number
- CN116413706A CN116413706A CN202211614574.9A CN202211614574A CN116413706A CN 116413706 A CN116413706 A CN 116413706A CN 202211614574 A CN202211614574 A CN 202211614574A CN 116413706 A CN116413706 A CN 116413706A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- laser radar
- internal reference
- value
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 71
- 238000005259 measurement Methods 0.000 claims abstract description 29
- 239000013598 vector Substances 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 238000013507 mapping Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 7
- 230000036961 partial effect Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 5
- 238000011478 gradient descent method Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 2
- 230000004927 fusion Effects 0.000 abstract description 3
- 230000002829 reductive effect Effects 0.000 description 11
- 230000032683 aging Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 239000012634 fragment Substances 0.000 description 3
- 230000002238 attenuated effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000010304 firing Methods 0.000 description 2
- 230000000670 limiting effect Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 239000002184 metal Substances 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 244000025254 Cannabis sativa Species 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 239000013078 crystal Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000009432 framing Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
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)
Abstract
The invention discloses a method for simultaneously establishing a graph and calibrating internal parameters of a laser radar on a mobile carrier. According to the working principle and the physical structure of the laser radar, the internal reference model for correcting the ranging deviation and the angle measurement deviation is established, the environment point cloud data and the motion data are continuously collected when the carrier runs, the map segments are constructed in an iterative mode, the internal reference values are updated, and finally the accurate laser radar internal reference and scene map is obtained. The invention uses techniques such as motion compensation, surface element estimation, sensor fusion and the like to reduce the drawing error, and updates the internal parameter value by optimizing the consistency of the surface element in the map segment under multi-frame observation.
Description
Technical Field
The invention relates to the field of automatic driving automobiles, in particular to a method for simultaneously establishing a graph and calibrating internal parameters by a laser radar on a mobile carrier.
Background
The laser radar has the advantages of direct measurement of three-dimensional position, long detection range, small interference caused by illumination, and the like. Plays an important role in the tasks of positioning, drawing, perception and the like of the unmanned vehicles and robots.
The laser radar generally comprises a multichannel laser transceiver circuit, and a motor is used for driving a transceiver module or a lens to move, so that scanning in a large range is realized. The method is influenced by factors such as nonlinearity of photoelectric devices, differences among channels, mechanical errors and the like, and manufacturers need to perform internal reference calibration on each laser radar to achieve better measurement accuracy.
After the laser radar is installed on an automatic driving vehicle, the internal parameters are influenced by factors such as long-term vibration, cold-hot circulation, device aging and the like to slowly drift, and if the internal parameters calibrated in factory leaving are still used, the point cloud precision can be reduced. The existing internal reference calibration method mainly comprises the following steps:
1. and (5) supervision type calibration: and constructing a special calibration scene by using devices such as a calibration plate and the like, and acquiring a three-dimensional model true value of the scene by using a high-precision scanner. And scanning the scene by using the laser radar to be calibrated, and optimizing internal parameters of the radar and the pose in the scene to enable the point cloud to fall on the surface of the truth model as much as possible. The method has the advantages of high calibration speed, high internal parameter precision and the like, but the construction and modeling cost of a calibration factory building is high, and the calibration of a user for sending vehicles back to the factory building at regular intervals is impractical.
2. Unsupervised calibration: the requirements for calibrating the scene are relaxed, a scene truth model is not needed, and the scene is assumed to be locally flat. Firstly, statically acquiring data at a plurality of positions in a scene by using a laser radar to be calibrated, and establishing a first point cloud model of the scene. And then the laser radar is put back to the center of the scene, data are collected statically at a plurality of inclined angles, and a second point cloud model of the scene is built. The internal parameters of the laser radar are optimized so that the middle planes of the two point clouds are overlapped as much as possible.
The non-supervision calibration method overcomes the limitations of the field and the cost to a certain extent, but in practice, the following problems still exist:
1. the scene model is registered by a plurality of frame point clouds, and the final calibration precision is reduced due to registration errors.
2. For reference models with large parameter amounts, the constraint provided by a single scene may be insufficient, resulting in erroneous calibration results.
3. For lidar fixed on vehicles, it is difficult to acquire data by changing the tilt angle.
4. Before each frame of point cloud is collected, the carrier must be stopped and stabilized, and the actual operation efficiency is low.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a method for simultaneously constructing a graph and calibrating an internal reference of a laser radar on a mobile carrier, which comprises the following steps:
s101: according to the mechanical and optical structures of the laser radar to be calibrated, an internal reference model for correcting the ranging deviation and the angle measurement deviation is designed;
s102: rigidly fixing a laser radar to be calibrated and an inertial measurement unit (Inertial Measurement Unit, IMU) on a carrier, and collecting an original measured value of the laser radar and IMU motion data in the process of running the carrier on a road;
s103: reading an original measured value of a frame of laser radar, reading parameters of an internal reference model in S101, compensating measurement deviation, and generating a frame of point cloud;
s104: compensating the motion distortion of the point cloud frame, calculating the pose of the point cloud frame relative to the scene point cloud map, and fusing the point cloud frame into the scene point cloud map;
s105: searching for k nearest neighbors of each point in the scene point cloud map, and calculating the normal vector and the local flatness of each point;
s106: randomly downsampling the scene point cloud map at S104 by a preset frame number at each interval to generate two sub-point cloud maps, calculating the plane consistency cost of the sub-point cloud maps, and executing the next step; if not enough new frames have been accumulated, returning to S103 to continue processing the next frame;
s107: solving partial derivatives of plane consistency cost in S106 on the internal parameters in the internal reference model, and updating the internal parameter values in the internal reference model by using a gradient descent method;
s108: deleting the points, which are beyond a preset distance from the current laser radar position, in the scene point cloud map at S104, and attenuating the step length of updating the internal parameter value at S107;
s109: and returning to S103, continuing to process the next frame until all the acquired data are used up, and outputting the parameters of the internal reference model as a final calibration result.
Further, in step S101, the laser radar to be calibrated is an n-line mechanical scanning laser radar, and the internal reference model is as follows:
the distance measurement value of the ith laser beam before calibration is recorded as
The pitch angle of the ith laser is marked as +.>
The direction angle of the ith laser beam relative to the rotor is marked +.>
The direction angle of the output of the rotor encoder is recorded as +.>
Compensating for a ranging bias using a look-up table; the lookup table is dTable, and distance measurement deviation values of n laser beams under k distances are stored; the number k of the distance points is usually 8 to 16, and the distance points are distributed exponentially in the range of the laser radar, such as 1m, 2m, 4m … m and 256m; the look-up operation is recorded as
Generating arbitrary ∈by interpolation>
A lower ranging bias value; calibrated distance measurement value +.>
Using
Compensating the pitch angle of the ith laser beam, and calibrating the pitch angle +.>
Using
Compensating the direction angle of the ith laser beam relative to the rotor, and calibrating the direction angle of the ith laser beam relative to the rotor
Compensating for angular errors of a rotor encoder using a second harmonic modelDirection angle of stator and rear rotor
If the initial value of the parameters in the internal reference model is not given by the user, initializing all the parameters to 0.
Further, in step S102, the carrier is a vehicle or a robot that drives itself to travel using wheels or tracks or mechanical feet; the laser radar original measured value comprises a laser ranging value, a laser emergence angle and a time stamp; the IMU motion data includes at least tri-axial acceleration and tri-axial angular velocity. The data acquisition is uninterrupted, and the carrier continuously moves in the acquisition process.
Further, in step S103, the deviation of the original measurement value of the laser radar is compensated by using the internal parameter model and the stored internal parameter value established in step S101; the compensated measurement value d i ,θ i ,Φ i ,Φ r ]The formula for converting the point cloud into the space rectangular coordinate system is as follows:
the laser radar center is defined as an origin, the front is an x-axis, the left is a y-axis, and the upper is a z-axis. A part of types of laser radars such as Velodyne HDL-64 have large difference of laser emergent positions of each beam, and the offset of the laser emergent positions in the horizontal and height directions is compensated by using horizOffset and vertOffset.
Further, in step S104, the method for compensating the motion distortion of the point cloud frame is as follows:
calculating the pose of the laser radar in the frame acquisition process by using IMU motion data, and interpolating the pose according to a point cloud time stamp to obtain the pose of the laser radar when each point is acquired;
calculating the transformation of the pose relative to the reference pose, and applying the transformation to the coordinates of each point;
the reference pose is usually the pose of the laser radar at the beginning or middle or ending moment of the point cloud frame;
the method for calculating the pose of the point cloud frame relative to the scene point cloud map comprises the following steps:
registering the point cloud frame and the scene point cloud map by using a weighted point-to-point ICP algorithm to obtain the pose of the laser radar, inputting the pose of the laser radar obtained by registering the point cloud and the pose of the laser radar obtained by calculating the IMU into a Kalman filter to obtain the pose of the laser radar after filtering and updating the IMU drift parameters.
Further, in step S105, the point cloud is organized by using KD-Tree or octree or voxel hash table, and k nearest neighbor of a point is searched on the basis, and the common value of k is 8 to 16; the method for calculating the normal vector of a certain point is as follows:
calculating the mean value of k neighbor coordinates, subtracting the mean value from the k neighbor coordinates, calculating a covariance matrix, and decomposing the characteristic value of the matrix, wherein the normal vector of the point is the characteristic vector corresponding to the minimum characteristic value; if the minimum characteristic value is smaller than the threshold value, the local flatness is set to be 1, otherwise, the local flatness is divided by the minimum characteristic value.
Further, in step S106, the frame number interval is 50 to 200;
the random downsampling method is that points in a scene point cloud map are traversed, one operation of { putting a sub point cloud A, putting a sub point cloud B and directly skipping } is executed according to preset probability;
plane consistency cost c=Σ|dot (p A -p B ,n B k B ) I, p in the above formula A For a point in point cloud A, p B P in point cloud B A N, n B Is p B Normal vector, k B Is p B Corresponding weights, dot is vector dot product, and vertical line represents absolute value.
Further, in step S107, the partial derivative matrix j=d (c+r)/dP, where C is a plane consistency cost, P is an internal reference, and R is a regular term;
the updating mode of the internal parameter value is P=P-alpha J, wherein alpha is the step size; and adjusting regularization strength and an internal reference updating step length according to the calibrated scene quality.
Compared with the existing laser radar internal reference calibration method, the method has the following advantages:
1. the method continuously collects data when the carrier runs, and has no strict requirements on path precision and carrier posture. Compared with a method for statically acquiring data at a preset position and gesture, the method reduces operation difficulty, improves acquisition efficiency, and is particularly suitable for calibrating the laser radar fixed on the vehicle or the robot.
2. According to the method, the View Point (View Point) of the acquired Point cloud is concentrated on the carrier driving path, the content of adjacent Point cloud frames is relatively close, and the difficulty of Point cloud registration is reduced. The method also uses the technologies of sensor fusion, surface element estimation and the like, can construct a finer scene point cloud map, and is also beneficial to improving the final calibration precision.
3. The method iteratively builds scene point cloud map fragments and updates internal parameter values of the laser radar, plane information of each scene fragment participates in updating the internal parameter values, interference of random noise is reduced, accuracy of internal parameter calibration is improved, and calibration can be performed when single scene fragment planes are not abundant enough.
Drawings
FIG. 1 is a flow chart of a method for simultaneously establishing a graph and calibrating internal parameters of a laser radar on a mobile carrier;
FIG. 2 is a schematic diagram of an internal model used in the present invention;
fig. 3 is a flow chart of a weighted point-to-face ICP used in the present invention.
Detailed Description
In order to more particularly describe the present invention, the following technical solutions of the present invention are described in further detail with reference to the accompanying drawings and the detailed description.
The laser radar emits laser to the surrounding environment and receives laser reflected by an object, the distance of the object to be measured is determined by measuring the time of flight (TOF) of the laser, and the angle of the object to be measured is determined by measuring the outgoing angle of the laser. The range value, the angle value and the time stamp are often called as original measured values of the laser radar, and the internal reference model designed by the invention mainly corrects the range deviation and the angle deviation of the laser radar.
Generally, lidar uses a crystal oscillator to generate a reference clock and hardware logic to control the firing order and timing of the laser (Firing Sequence Timing). The timing scheme has low offset, small jitter and slow aging, so the error of the laser radar time stamp is negligible.
For cost reasons, some lidar manufacturers have calibrated only a portion of the reference, or have used an overly simplified reference model. Even if the manufacturer performs fine calibration, after the laser radar is installed on an automatic driving vehicle, the internal parameters still slowly drift under the influence of factors such as long-term vibration, cold-hot circulation, device aging and the like. Therefore, a perfect internal reference model and an easy-to-use calibration method are designed, and the method is necessary for improving and maintaining the accuracy of laser radar data.
The internal reference calibration method of the laser radar mainly comprises two types, namely supervised calibration and unsupervised calibration. The supervision type calibration needs to build a special calibration scene by using devices such as a calibration plate and the like, and a high-precision scanner is used for obtaining the true value of a three-dimensional model of the scene. And scanning the scene by using the laser radar to be calibrated, and optimizing internal parameters of the radar and the pose in the scene to enable the point cloud to fall on the surface of the truth model as much as possible. The method has the advantages of high calibration speed, high internal parameter precision and the like, but the construction and modeling cost of a calibration factory building is high, and the calibration of a user for sending vehicles back to the factory building at regular intervals is impractical.
Unsupervised calibration relaxes the requirements for calibrating the scene, does not require a scene truth model, and assumes that the scene is locally flat. A typical method firstly uses a laser radar to be calibrated to collect data statically at a plurality of positions in a scene, and establishes a first point cloud model of the scene. And then the laser radar is put back to the center of the scene, data are collected statically at a plurality of inclined angles, and a second point cloud model of the scene is built. The internal parameters of the laser radar are optimized so that the middle planes of the two point clouds are overlapped as much as possible.
The non-supervision calibration method overcomes the limitations of the field and the cost to a certain extent, but in practice, the method still has more problems:
in terms of usability, the existing method requires static data acquisition, the vehicle takes about 5 seconds to complete a group of actions of forward running, deceleration and stabilization and static acquisition, and the data acquisition efficiency is low. In addition, the existing method requires changing the inclination angle of the laser radar, because the laser radar is fixed on the vehicle, the vehicle is required to run on slopes with different gradients, and the phase change increases the requirements on the field.
In terms of accuracy, a scene model used for unsupervised calibration is registered by a plurality of frame point clouds, and the calibration accuracy is restricted by registration errors. In addition, for an internal reference model with a large parameter amount, the constraint provided by a single scene may be insufficient, so that the calibration result is wrong. Increasing the number of data acquisition points and placement angles can alleviate the above problems, but increase the complexity of the calibration process.
In order to improve the usability and accuracy of the internal reference calibration of the laser radar on the mobile carrier, the invention provides a method for simultaneously constructing a graph and calibrating the internal reference of the laser radar on the mobile carrier, and a flow chart of the method is shown in fig. 1. According to the working principle and the physical structure of the laser radar, the internal reference model for correcting the ranging deviation and the angle measurement deviation is established, the environment point cloud data and the motion data are continuously collected when the carrier runs, the map segments are constructed in an iterative mode, the internal reference values are updated, and finally the accurate laser radar internal reference and scene map is obtained. The invention uses techniques such as motion compensation, surface element estimation, sensor fusion and the like to reduce the drawing error, and updates the internal parameter value by optimizing the consistency of the surface element in the map segment under multi-frame observation.
This section is described by way of example with respect to 32-line mechanically scanned laser radar Velodyne VLP-32C, but the method is equally applicable to other models. For details of VLP-32C see https:// velodynlidar.com/products/ultra-puck/.
The specific steps of the method for simultaneously constructing the graph and calibrating the internal parameters of the laser radar on the mobile carrier provided by the invention are introduced as follows:
as shown in fig. 1, in step S101, an internal model of the lidar is designed. Sources of ranging bias include temperature variation, device aging, and focus errorThe difference, nonlinearity of the laser transceiver circuit, etc., for example, the light intensities returned by near and far objects differ by several orders of magnitude, and the signal of the receiving circuit is deformed and time delay is changed in a large dynamic range. Also because the VLP-32C per channel laser transceiver circuitry is independent, use is made of the laser line number i and the original ranging value
The table dTable describes the ranging bias. The lookup table stores the ranging deviation values of 32 laser beams at 15 distances, and the distance points are exponentially distributed within the range of 2m to 256 m. The look-up operation is recorded as->
Generating arbitrary ∈by interpolation>
Lower ranging bias value. Calibrated distance measurement value
The goniometric value of VLP-32C comprises two parts. The first part being the pitch angle of the laser beam relative to the rotor
And direction angle->
The laser receiving and transmitting circuit and the lens are rigidly fixed on the rotor, < >>
And->
Mainly due to assembly errors and drift slowly as the lidar ages. Use->
Compensating the pitch angle of the ith laser beam, and calibrating the pitch angle +.>
Use->
Compensating the direction angle of the ith laser beam relative to the rotor, and calibrating the direction angle of the ith laser beam relative to the rotor>
The second part is the direction angle of the rotor relative to the base
VLP-32C measured +.>
Sources of bias include eccentricity and tilt of the installation of the magnetically sensitive element, differences in the sensitivity of the axis of the magnetically sensitive element, interference of external magnetic fields, etc. The second harmonic model can better simulate the deviation, and the direction angle of the rotor after calibration
Fig. 2 shows a schematic diagram of the internal reference model. The user may give the initial value of the parameter in the reference model, and default to 0 is not given.
In step S102, the experimental car runs on the road and collects sensor data. The laser radar, the IMU and the GNSS receiver are arranged on the roof, the sensors are reliably fixed together by using the customized metal structural parts, and the time synchronization of the sensors is realized by using the special hardware controller. The lidar frame rate was 10HZ, the imu data rate was 2khz, and the gnss data rate was 5HZ. The road in a certain park runs at the fastest speed of about 10m/s, and the buildings beside the road are more dense.
It is noted that the carrier to which the present method is applicable is not limited to automobiles. GNSS data is not necessary in this approach, but helps to improve accuracy and efficiency. The method can screen the surface features from natural scenes, but the selection of scenes with rich planes and less interference is beneficial to improving the precision and efficiency. The method requires the IMU to be rigidly connected with the laser radar, and the IMU is fixed near the laser radar base by using the metal structural member, so that higher rigidity can be obtained, and errors caused by connection deformation in running are reduced.
It is worth noting that the acquired point cloud density is inversely proportional to the running speed, and when the point cloud density is lower than a certain limit, the error of point cloud registration is increased sharply, so that the subsequent map building and calibration accuracy cannot meet the requirements. When the point cloud density is higher than a certain limit, the accuracy of point cloud registration is not improved basically, and the time consumption for collecting data is only increased when the vehicle speed is reduced continuously. The vehicle speed range of VLP-32C is 2m/s to 10m/s, and the traveling speed can be correspondingly improved for the laser radar with higher line number.
In step S103, the deviation of the original measurement value of the laser radar is compensated using the internal parameter model established in S101 and the saved internal parameter value. The compensated measurement value d i ,θ i ,Φ i ,Φ r ]The formula of the point cloud converted into the space rectangular coordinate system (laser radar coordinate system) is as follows:
the laser radar center is defined as the origin, the front is the x-axis, the left is the y-axis, the upper is the z-axis, the center point coordinates and the front (phi) r Definition of =0) can be found in the user manual. In the above formula, horizOffset and vertOffset represent the offset of the laser emergent position in the horizontal and height directions, and because all laser beams in the VLP-32C are collimated by sharing one lens and then emitted, that is, a unique optical center exists, horizOffset and vertOffset are negligible.
In step S104, compensating for motion distortion of the point cloud frame in step S103, calculating pose of the point cloud frame relative to the scene point cloud map, and fusing the point cloud frame into the scene point cloud map. The acquisition time of each point in the point cloud frame has a larger difference, although the laser radar ranging takes a short time, the scanning machinery is required to operate the laser beam to point at different angles, and the results are accumulated to obtain a frame of point cloud, for example, 100ms is required for VLP-32C to complete one circle of scanning. The laser radar pose changes continuously when the automobile runs, the coordinate systems of the point references acquired at different moments are inconsistent, and the point cloud is visually deformed and torn, namely the motion distortion of the laser radar point cloud. The motion distortion causes that the point cloud cannot directly correspond to the three-dimensional structure of the scene, and when the vehicle moves straight or turns, the motion distortion can reach the level of meters, so that the motion distortion must be compensated to construct a more accurate environment point cloud map.
The basic principle of motion distortion compensation is that the pose of a laser radar in the process of collecting point cloud frames is calculated, the transformation from a laser radar coordinate system to a reference coordinate system at the time is applied to each point, and all the points after transformation are unified to the reference coordinate system. The pose change in the automobile driving is broadband, and the IMU data is used for estimating broadband motion with higher precision and lower calculation cost.
And setting the position of the IMU at the end moment of the first frame point cloud as an origin, and establishing an northeast day coordinate system (ENU). The IMU measured its own (Sensor Frame) angular velocity and acceleration were recorded as
The posture, speed and position of IMU relative to ENU system at the end time of the kth frame point cloud are marked as +.>
The offset of IMU angular velocity and acceleration at the end of the kth frame point cloud is recorded as +.>
The point P is any point in the k+1st frame point cloud, and the timestamp is k+p.
And calculating the posture, acceleration, speed and displacement of the IMU relative to the ENU system at the moment k+p according to the following formula. Where S (ω) is an antisymmetric matrix of ω, the local g-value can be accurately calculated using latitude and altitude.
In actual engineering, the IMU coordinate system and the laser radar coordinate system are not exactly coincident, and are used
Transformation of a lidar coordinate system to an IMU coordinate system is described. Then the lidar pose corresponding to point P +.>
In the example, the point P is transformed from a laser radar coordinate system at the moment k+p to a laser radar coordinate system at the end of the k+1 frame to obtain the point after compensating the motion distortion
Next, the k+1 frame point cloud is registered with the scene point cloud map using a weighted point-to-point ICP algorithm that pairs the pose of the k+1 frame point cloud with respect to the scene point cloud map
Iterative optimization is performed to minimize the matching cost C, and the detailed flow of the algorithm is shown in fig. 3.
In the above formula, p is a point in the k+1st frame point cloud, using
Transforming p into a scene point cloud map coordinate system, and marking the transformed points as +.>
q is distance p in scene point cloud map ′ Nearest point, n q For the normal vector corresponding to q, k q And the dot is vector dot multiplication, which is the weight corresponding to q.
Analytical solution
It is very difficult because the pairing relationship (corespondence) of p and q is about +.>
The ICP uses an iterative method to find an approximate solution, and the specific steps are as follows:
using the current
P is transformed and q of the pairing is searched. Assuming that the pairing relationship of p and q is unchanged, calculating +.>
The matching cost is minimized. Iterating through the loop until->
The maximum number of iterations is converged or exceeded in value.
The accuracy of IMU motion estimation is high in short time, but errors can be accumulated rapidly along with time, and correction needs to be carried out by combining with other sensors. The pose drift obtained by the point cloud registration is smaller, but the matching is wrong when the environmental characteristics are insufficient or the interference objects are more. GNSS can directly measure absolute position, eliminates accumulated errors of mapping, but has larger random noise. In the example, a Kalman filter is used for fusing an IMU calculation result, a point cloud registration pose and a GNSS position to obtain a relatively accurate state quantity at the end moment of a k+1 frame
The state quantity needs to be initialized before the drawing is built, and incorrect initial values can lead to the initial part of the mapThe accuracy is reduced and even the drawing cannot be constructed. Wherein the method comprises the steps of
Can be set according to the last IMU internal parameter calibration result, < + >>
Estimated by IMU-based pose algorithm,/-j>
Estimated by registering neighboring point cloud frames, +.>
In the coordinate convention of this example is +.>
Finally according to Kalman filter
And carrying out coordinate transformation on the k+1 frame point cloud, and framing the transformed point cloud into a scene point cloud map. In an actual road environment, the automobile may stop temporarily due to waiting for a green light and avoiding pedestrians, and the scanned point cloud frame is almost repeated. The repeated point clouds cannot provide new scene information, and only the uniformity of the scene point cloud map is reduced. In the example, the repeatability of the current point cloud frame is evaluated by using the displacement from the current point cloud frame to the last frame in the scene point cloud map, if the displacement<And if 0.1m is not fit into the scene point cloud map.
In step S105, the k nearest neighbor of each point in the scene point cloud map is searched S104, and the normal vector and the local flatness of each point are calculated. The point cloud can be regarded as sampling of a real scene, and the essence of solving the normal vector of the point cloud is to restore plane information (also called a bin) in the scene, and plays an important role in various point cloud processing algorithms. The method for solving the normal vector of a certain point is to use k neighbor of the normal vector to carry out local plane fitting, and a plane equation and fitting residual in space can be written as:
the above-described planar fitting problem has an efficient analytical solution. Calculating the mean value of the k nearest neighbor coordinates, subtracting the mean value from the k nearest neighbor coordinates, calculating a covariance matrix, decomposing the characteristic value of the matrix, wherein the characteristic vector corresponding to the minimum characteristic value is the normal vector, and the minimum characteristic value is the fitting residual error.
The number k of points used for fitting is often 8 to 16, the number k of points is too small, the fitting error is easily caused by noise interference, the small three-dimensional structure can be smoothed if the number k of points is too large, and the calculation load is increased. To expedite k-nearest neighbor searches, point clouds are typically organized using spatial data structures, such as KD-Tree, octree, voxel hash tables.
In an actual point cloud map, points on a surface which is close to the laser radar and is approximately perpendicular to the laser beam are very dense, points on a surface which is far away from the laser radar and is approximately parallel to the laser beam are very sparse, a fixed k value cannot adapt to large changes of the point density, and the complexity of an adaptive algorithm is high. The scene point cloud map is recorded as M, and the method carries out voxel average downsampling (Grid Average Downsample) on M to obtain M ds ，M ds Including the number of points and average coordinates within non-empty voxels. At M ds And searching k neighbor of M, and calculating a mean value and a covariance matrix after the k neighbor coordinates are weighted according to the number of points. And (3) after the highest density of the down-sampling limiting point cloud is used, carrying out fixed k neighbor plane fitting.
The normal vector calculated on objects with complex structures, such as branches and leaves and grass, is more wrong due to the limitation of the density and the precision of the point cloud. The method uses plane fitting residual error to evaluate the quality of the corresponding normal vector, if the minimum characteristic value is smaller than the threshold value, the local flatness is 1, otherwise, the local flatness is divided by the threshold value. The weights of the map building and the calibration time point are proportional to the local flatness, so that the influence of unreliable normal vectors is reduced.
In step S106, the scene point cloud map in step S104 is randomly downsampled, two sub point cloud maps are generated, and the plane consistency cost of the two sub point cloud maps is calculated. When a vehicle runs, the same plane in a scene can be repeatedly observed from a plurality of positions and angles by a plurality of laser beams, geometrical inconsistency caused by internal parameter errors can be displayed after a map is constructed by using multi-frame point clouds, and the method calculates the internal parameters of the laser radar by optimizing the plane consistency cost.
This step is typically performed every 50 to 200 frames, since new calibration information can only be provided when there is a significant change in map geometry. If there are not enough new frames accumulated in the map, the process goes back to S103 to continue with the next frame. The random downsampling method is that points in a scene point cloud map are traversed, and one operation of { putting a sub point cloud A, putting a sub point cloud B and directly skipping } is executed according to preset probability. Plane consistency cost c=Σ|dot (p A -p B ,n B k B ) I, p in the above formula A For a point in point cloud A, p B P in point cloud B A N, n B Is p B Normal vector, k B Is p B Corresponding weight, dot is vector dot product, and vertical line is absolute value.
In step S107, the partial derivative of the plane consistency cost in S106 to the internal parameter in S101 is obtained, and the internal parameter value in S101 is updated by using a gradient descent method. The partial guide matrix J=d (C+R)/dP, wherein in the above formula, C is the plane consistency cost, P is an internal reference, and R is a regular term. The internal parameter value is updated by p=p- αj, where α is the step size. When the planes in the scene are not rich enough, the improvement of the regular coefficient is helpful to inhibit calibration errors caused by over fitting. If the scene contains a large number of planes of different positions and angles, the regularization coefficients can be reduced.
In step S108, the points far from the current laser radar position in the scene point cloud map in step S104 are deleted, and the step of updating the internal parameter value in step S107 is attenuated. The probability that the point far away from the laser radar returns to the field of view again is low, and if the distance of the point exceeds 1.1 times of the range of the laser radar, the point is deleted to save memory and calculation resources. In the calibration starting stage, a larger step length is used for accelerating the convergence speed, and then the step length is gradually attenuated, so that the interference of random noise is restrained, and a more accurate internal parameter value is obtained. The decay curve used in this example is a logarithmic curve.
In step S109, if the data collected in S102 is used up, the internal reference in S101 is output as the final calibration result. Otherwise, the process returns to S103 to continue with the next frame. If the data collected at a time is not abundant enough or the calibration accuracy is expected to be further improved, a plurality of data sets can be collected at a plurality of road sections, the method is operated for a plurality of times, the result of the previous calibration is used as the initial value of the next calibration, and the step length of parameter updating is properly reduced.
The above description is only of embodiments of the present invention and should not be construed as limiting the scope of the present invention, and equivalent changes, which are known to those skilled in the art based on the present invention, should be construed as falling within the scope of the present invention.
Claims (8)
1. The method for simultaneously constructing the graph and calibrating the internal reference of the laser radar on the mobile carrier is characterized by comprising the following steps of:
s101: according to the mechanical and optical structures of the laser radar to be calibrated, an internal reference model for correcting the ranging deviation and the angle measurement deviation is designed;
s102: rigidly fixing the laser radar to be calibrated and the inertial measurement unit on a carrier, and collecting the original measured value of the laser radar and the motion data of the inertial measurement unit in the process of the carrier running on a road;
s103: reading an original measured value of a frame of laser radar, reading parameters of an internal reference model in S101, compensating measurement deviation, and generating a frame of point cloud;
s104: compensating the motion distortion of the point cloud frame, calculating the pose of the point cloud frame relative to the scene point cloud map, and fusing the point cloud frame into the scene point cloud map;
s105: searching for k nearest neighbors of each point in the scene point cloud map, and calculating the normal vector and the local flatness of each point;
s106: randomly downsampling the scene point cloud map at S104 by a preset frame number at each interval to generate two sub-point cloud maps, calculating the plane consistency cost of the sub-point cloud maps, and executing the next step; if not enough new frames have been accumulated, returning to S103 to continue processing the next frame;
s107: solving partial derivatives of plane consistency cost in S106 on the internal parameters in the internal reference model, and updating the internal parameter values in the internal reference model by using a gradient descent method;
s108: deleting the points, which are beyond a preset distance from the current laser radar position, in the scene point cloud map at S104, and attenuating the step length of updating the internal parameter value at S107;
s109: and returning to S103, continuing to process the next frame until all the acquired data are used up, and outputting the parameters of the internal reference model as a final calibration result.
2. The method for simultaneous mapping and internal reference calibration of a lidar on a mobile carrier according to claim 1, wherein in step S101, the lidar to be calibrated is an n-line mechanically scanned lidar, and the internal reference model is as follows:
the distance measurement value of the ith laser beam before calibration is recorded as
The pitch angle of the ith laser is marked as +.>
The direction angle of the ith laser beam relative to the rotor is marked +.>
The direction angle of the output of the rotor encoder is recorded as +.>
Compensating for a ranging bias using a look-up table; the lookup table is dTable, and distance measurement deviation values of n laser beams under k distances are stored; the look-up operation is recorded as
Generating arbitrary ∈by interpolation>
A lower ranging bias value; calibrated distance measurement value +.>
Using
Compensating the pitch angle of the ith laser beam, and calibrating the pitch angle +.>
Using
Compensating the direction angle of the ith laser beam relative to the rotor, and calibrating the direction angle of the ith laser beam relative to the rotor>
Compensating for angular errors of a rotor encoder using a second harmonic model, calibrating the directional angle of the rotor>
If the initial value of the parameters in the internal reference model is not given by the user, initializing all the parameters to 0.
3. The method for simultaneous mapping and internal reference calibration of a lidar on a mobile carrier according to claim 1, wherein in step S102, the carrier is a vehicle or a robot that drives itself to travel using wheels or tracks or mechanical feet; the laser radar original measured value comprises a laser ranging value, a laser emergence angle and a time stamp; the motion data of the inertial measurement unit includes at least a triaxial acceleration and a triaxial angular velocity.
4. The method for simultaneous mapping and internal reference calibration of a mobile carrier laser radar according to claim 2, wherein in step S103, the deviation of the original measurement value of the laser radar is compensated by using the internal reference model established in S101 and the stored internal reference value; the compensated measurement value d i ,θ i ,Φ i ,Φ r ]The formula for converting the point cloud into the space rectangular coordinate system is as follows:
the center of the laser radar is taken as an origin, the front is an x axis, the left is a y axis, and the upper is a z axis, wherein horizOffset and vertOffset in the above description represent the offset of the laser emergent position in the horizontal and height directions.
5. The method for simultaneous mapping and internal reference calibration of a lidar on a mobile carrier according to claim 1, wherein in step S104, the method for compensating for motion distortion of a point cloud frame is as follows:
calculating the pose of the laser radar in the frame acquisition process by using the motion data of the inertial measurement unit, and interpolating the pose according to the point cloud time stamp to obtain the pose of the laser radar when each point is acquired;
calculating the transformation of the pose relative to the reference pose, and applying the transformation to the coordinates of each point;
the reference pose is usually the pose of the laser radar at the beginning or middle or ending moment of the point cloud frame;
the method for calculating the pose of the point cloud frame relative to the scene point cloud map comprises the following steps:
registering the point cloud frame and the scene point cloud map by using a weighted point-to-point ICP algorithm to obtain the pose of the laser radar, inputting the pose of the laser radar obtained by registering the point cloud and the pose of the laser radar obtained by calculating by an inertial measurement unit into a Kalman filter to obtain the pose of the laser radar after filtering and updating the drift parameter of the inertial measurement unit.
6. The method for simultaneous mapping and internal reference calibration of a lidar on a mobile carrier according to claim 1, wherein in step S105, point clouds are organized using KD-Tree or octree or voxel hash table, and k neighbors of a point are found on the basis of the point clouds; the method for calculating the normal vector of a certain point is as follows:
calculating the mean value of k neighbor coordinates, subtracting the mean value from the k neighbor coordinates, calculating a covariance matrix, and decomposing the characteristic value of the matrix, wherein the normal vector of the point is the characteristic vector corresponding to the minimum characteristic value; if the minimum characteristic value is smaller than the threshold value, the local flatness is set to be 1, otherwise, the local flatness is divided by the minimum characteristic value.
7. The method for simultaneous mapping and internal reference calibration of a lidar on a mobile carrier according to claim 1, wherein in step S106, the frame interval is 50 to 200;
the random downsampling method is that points in a scene point cloud map are traversed, one operation of { putting a sub point cloud A, putting a sub point cloud B and directly skipping } is executed according to preset probability;
plane consistency cost c=Σ|dot (p A -p B ,n B k B ) I, p in the above formula A For a point in point cloud A, p B P in point cloud B A N, n B Is p B Normal vector, k B Is p B Corresponding weights, dot is vector dot product, and vertical line represents absolute value.
8. The method for simultaneous mapping and internal reference calibration of a lidar on a mobile carrier according to claim 1, wherein in step S107, the partial derivative matrix j=d (c+r)/dP, where C is a plane consistency cost, P is an internal reference, and R is a regularization term;
the updating mode of the internal parameter value is P=P-alpha J, wherein alpha is the step size; and adjusting regularization strength and an internal reference updating step length according to the calibrated scene quality.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211614574.9A CN116413706A (en) | 2022-12-13 | 2022-12-13 | Method for simultaneously establishing graph and calibrating internal reference of laser radar on mobile carrier |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211614574.9A CN116413706A (en) | 2022-12-13 | 2022-12-13 | Method for simultaneously establishing graph and calibrating internal reference of laser radar on mobile carrier |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116413706A true CN116413706A (en) | 2023-07-11 |
Family
ID=87057089
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211614574.9A Pending CN116413706A (en) | 2022-12-13 | 2022-12-13 | Method for simultaneously establishing graph and calibrating internal reference of laser radar on mobile carrier |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116413706A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117907965A (en) * | 2024-03-19 | 2024-04-19 | 江苏省气象台 | Three-dimensional radar echo proximity forecasting method for convection storm fine structure |
-
2022
- 2022-12-13 CN CN202211614574.9A patent/CN116413706A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117907965A (en) * | 2024-03-19 | 2024-04-19 | 江苏省气象台 | Three-dimensional radar echo proximity forecasting method for convection storm fine structure |
| CN117907965B (en) * | 2024-03-19 | 2024-05-24 | 江苏省气象台 | Three-dimensional radar echo proximity forecasting method for convection storm fine structure |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10705219B2 (en) | Method and apparatus for updating maps | |
| He et al. | An integrated GNSS/LiDAR-SLAM pose estimation framework for large-scale map building in partially GNSS-denied environments | |
| CN107340522B (en) | Laser radar positioning method, device and system | |
| JP6880080B2 (en) | Vehicle navigation system using attitude estimation based on point cloud | |
| CN108921947B (en) | Method, device, equipment, storage medium and acquisition entity for generating electronic map | |
| US10860871B2 (en) | Integrated sensor calibration in natural scenes | |
| CN112859051B (en) | Laser radar point cloud motion distortion correction method | |
| CN112113574B (en) | Method, apparatus, computing device and computer-readable storage medium for positioning | |
| WO2022127532A1 (en) | Method and apparatus for calibrating external parameter of laser radar and imu, and device | |
| CN105866762A (en) | Laser-radar automatic calibration method and device thereof | |
| JP2021508814A (en) | Vehicle positioning system using LiDAR | |
| CN110187375A (en) | A kind of method and device improving positioning accuracy based on SLAM positioning result | |
| CN114413887B (en) | Sensor external parameter calibration method, device and medium | |
| CN113763548B (en) | Vision-laser radar coupling-based lean texture tunnel modeling method and system | |
| CN113947639B (en) | Self-adaptive online estimation calibration system and method based on multi-radar point cloud line characteristics | |
| CN116413706A (en) | Method for simultaneously establishing graph and calibrating internal reference of laser radar on mobile carrier | |
| CN115046540A (en) | Point cloud map construction method, system, equipment and storage medium | |
| Wu et al. | AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry | |
| CN117518196A (en) | Motion compensation method, device, system, equipment and medium for laser radar | |
| CN117388830A (en) | External parameter calibration method, device, equipment and medium for laser radar and inertial navigation | |
| CN115421153B (en) | Laser radar and UWB combined positioning method and system based on extended Kalman filtering | |
| CN116465393A (en) | Synchronous positioning and mapping method and device based on area array laser sensor | |
| CN113495281B (en) | Real-time positioning method and device for movable platform | |
| CN114413893A (en) | Dynamic position and attitude synchronization measuring method integrating inertial measurement information | |
| CN117804448B (en) | Autonomous system positioning method, device, computer equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |