Simultaneous mapping and intrinsic calibration method for laser radar on mobile carrier

Abstract

The application discloses a kind of mobile carrier laser radar simultaneous mapping and internal parameter calibration method.The application establishes the internal parameter model of correction ranging deviation and angle measurement deviation according to the working principle and physical structure of laser radar, continuously collects environmental point cloud data and motion data when carrier travels, iteratively constructs ground picture section and updates internal parameter value, and finally obtains relatively accurate laser radar internal parameter and scene map.The application uses motion compensation, cell estimation, sensor fusion and other technologies to reduce mapping error, and updates internal parameter value by optimizing the consistency of cell in ground picture section under multi-frame observation.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of autonomous vehicles, and particularly relates to a method for simultaneous mapping and internal parameter calibration of a laser radar on a mobile carrier. BACKGROUND

[0002] The laser radar has the advantages of directly measuring three-dimensional position, long detection range, and less interference from light. It plays an important role in positioning, mapping, and perception of unmanned vehicles and robots.

[0003] The laser radar generally includes a multi-channel laser transceiver circuit inside, which uses a motor to drive the transceiver module or lens to move, achieving a large range of scanning. Influenced by factors such as nonlinearity of optoelectronic devices, differences between channels, and mechanical errors, manufacturers need to calibrate the internal parameters of each laser radar to achieve better measurement accuracy.

[0004] After the laser radar is installed on an autonomous vehicle, the internal parameters slowly drift due to factors such as long-term vibration, thermal cycling, and device aging. If the internal parameters calibrated at the factory are still used, the point cloud accuracy will decrease. Existing internal parameter calibration methods mainly include:

[0005] 1. Supervised calibration: a special calibration scene is set up using a calibration board and other devices, and a high-precision scanner is used to obtain the three-dimensional model of the scene. The laser radar to be calibrated is used to scan the above scene, and the internal parameters of the laser radar and its pose in the scene are optimized so that the point cloud falls on the true model surface as much as possible. This method has the advantages of fast calibration speed and high internal parameter accuracy, but the cost of building and modeling the calibration workshop is high, and it is unrealistic for users to send the vehicle back to the factory for calibration regularly.

[0006] 2. Unsupervised calibration: the requirement for the calibration scene is relaxed, and there is no need for a scene true model. The laser radar to be calibrated is used to collect data at multiple positions in the scene, and a first point cloud model of the scene is established. The laser radar is placed back at the center of the scene and collects data at multiple tilt angles, and a second point cloud model of the scene is established. The internal parameters of the laser radar are optimized to make the two point clouds coincide as much as possible.

[0007] The above-mentioned unsupervised calibration method to some extent overcomes the limitations of the site and cost, but in practice there are still the following problems:

[0008] 1. The scene model is obtained by registering multiple frames of point clouds, and the registration error reduces the final calibration accuracy.

[0009] 2. For internal parameter models with a large number of parameters, the constraints provided by a single scene may not be sufficient, leading to incorrect calibration results.

[0010] 3. For lidar fixed on vehicles, it is difficult to collect data by changing the tilt angle.

[0011] 4. Before acquiring each frame of point cloud data, the carrier must be brought to a stable stop, which is not very efficient in practice. Summary of the Invention

[0012] To address the shortcomings of existing technologies, this invention proposes a method for simultaneous mapping and intrinsic parameter calibration of a lidar system on a mobile platform, comprising the following steps:

[0013] S101: Based on the mechanical and optical structure of the lidar to be calibrated, design an intrinsic parameter model to correct the ranging and angle measurement errors;

[0014] S102: Rigidly fix the lidar to be calibrated and the inertial measurement unit (IMU) on the carrier, and collect the original measurement values ​​of the lidar and the motion data of the IMU during the carrier's movement on the road;

[0015] S103: Read a frame of raw measurement values ​​from the lidar, read the parameters of the intrinsic parameter model in S101, compensate for measurement deviations, and generate a frame of point cloud.

[0016] S104: Compensate for motion distortion of point cloud frames in S103, calculate the pose of point cloud frames relative to the scene point cloud map, and merge point cloud frames into the scene point cloud map.

[0017] S105: Search for the k nearest neighbors of each point in the scene point cloud map described in S104, and calculate the normal vector and local flatness of each point;

[0018] S106: At each preset number of frames, randomly downsample the scene point cloud map described in S104 to generate two sub-point cloud maps, calculate their planar consistency cost, and proceed to the next step; if not enough new frames are accumulated, return to S103 to continue processing the next frame.

[0019] S107: Calculate the partial derivative of the planar consistency cost in S106 with respect to the intrinsic parameters in the intrinsic parameter model, and update the intrinsic parameter values ​​in the intrinsic parameter model using the gradient descent method.

[0020] S108: Delete points in the scene point cloud map described in S104 that are more than a preset distance from the current LiDAR position, and decrease the step size of updating the intrinsic parameter value in S107.

[0021] S109: Return to S103 and continue processing the next frame until all the collected data is used up, and output the parameters of the intrinsic parameter model as the final calibration result.

[0022] Further, in step S101, the laser radar to be calibrated is a mechanical scanning laser radar with n lines, and its intrinsic model is as follows:

[0023] Let the ranging value of the i th laser before calibration be Let the pitch angle of the i th laser be Let the direction angle of the i th laser relative to the rotor be Let the direction angle output by the rotor encoder be

[0024] The ranging deviation is compensated using a lookup table; the lookup table is denoted as dTable, which stores the ranging deviation values of the n lasers at k distances; the distance point number k is commonly 8 to 16, and is exponentially distributed within the range of the laser radar, such as 1 m, 2 m, 4 m…128 m, 256 m; the lookup table operation is denoted as The ranging deviation value at any distance is generated by interpolation; the calibrated ranging value is

[0025] The pitch angle of the i th laser is compensated using The calibrated pitch angle of the i th laser is

[0026] The direction angle of the i th laser relative to the rotor is compensated using The calibrated direction angle of the i th laser relative to the rotor is

[0027] The direction angle of the rotor is compensated using a second harmonic model to compensate the angle error of the rotor encoder; the calibrated direction angle of the rotor is

[0028] If the user does not give the initial value of the parameters in the above intrinsic model, all the parameters are initialized to 0.

[0029] Further, in step S102, the carrier is a vehicle or robot that drives itself using wheels or tracks or mechanical feet; the laser radar raw measurement values include laser ranging values, laser exit angles, and time stamps; the IMU motion data at least includes three-axis acceleration and three-axis angular velocity. The above data acquisition is uninterrupted, and the carrier is continuously moving during the acquisition process.

[0030] Further, in step S103, the intrinsic model established in S101 and the saved intrinsic parameter values are used to compensate the deviation of the laser radar raw measurement values; the compensated measurement values [d i , θ i , Φ i , Φ r ] are converted into the point cloud in the spatial rectangular coordinate system according to the following formula:

[0031] ​

[0032] The origin is set as the center of the laser radar, the front is the x-axis, the left is the y-axis, and the up is the z-axis. For some models of laser radars, such as Velodyne HDL-64, the difference in the position of each laser beam is large, and horizOffset and vertOffset are used to compensate for the offset of the laser beam position in the horizontal and height directions.

[0033] Further, in step S104, the method for compensating the motion distortion of the point cloud frame is:

[0034] The pose of the laser radar during the collection of the frame is calculated using the IMU motion data, the above-mentioned pose is interpolated according to the point cloud timestamp, and the pose of the laser radar when each point is collected is obtained;

[0035] The transformation of the above-mentioned pose relative to the reference pose is calculated, and the above-mentioned transformation is applied to the coordinates of each point;

[0036] The reference pose is usually selected as the pose of the laser radar at the beginning, middle or end of the point cloud frame;

[0037] The method for calculating the pose of the point cloud frame relative to the scene point cloud map is:

[0038] The point cloud frame and the scene point cloud map are registered using a weighted point-to-plane ICP algorithm to obtain the pose of the laser radar, the pose of the laser radar obtained by the point cloud registration and the pose of the laser radar obtained by the IMU are input into a Kalman filter, the filtered pose of the laser radar is obtained, and the IMU drift parameter is updated.

[0039] Further, in step S105, the point cloud is organized using a KD-Tree or an octree or a voxel hash table, and the k-nearest neighbors of a point are found on this basis, and the commonly used value of k is 8 to 16; the method for calculating the normal vector of a point is:

[0040] The mean value of the coordinates of the k-nearest neighbors is calculated, the covariance matrix is calculated after the k-nearest neighbor coordinates are subtracted from the mean value, the eigenvalue decomposition is performed on the matrix, and the normal vector of the point is the eigenvector corresponding to the smallest eigenvalue; if the smallest eigenvalue is less than a threshold value, the local flatness is 1, otherwise the local flatness is the threshold value divided by the smallest eigenvalue.

[0041] Further, in step S106, the frame interval is 50 to 200;

[0042] The method for random down-sampling is to traverse the points in the scene point cloud map, and perform one of {put into sub-point cloud A, put into sub-point cloud B, directly skip} according to a predetermined probability;

[0043] The plane consistency cost C = ∑|dot(p A -p B ,nB k B )|, the formula p A is a point in point cloud A, p B is the nearest neighbor of p A in point cloud B, n B is the normal vector of p B , k B is the corresponding weight of p B , dot is the vector dot product, and the absolute value represents the absolute value.

[0044] Further, in step S107, the partial derivative matrix J = d(C + R) / dP, wherein C is the plane consistency cost, P is the intrinsic parameter, and R is the regularization term.

[0045] The intrinsic parameter value updating method is P = P - aJ, wherein a is the step size; the regularization strength and the intrinsic parameter updating step size are adjusted according to the calibration scene quality.

[0046] Compared with the existing laser radar intrinsic parameter calibration method, the method has the following advantages:

[0047] 1. The method continuously collects data when the carrier is running, and has no strict requirements on path accuracy and carrier attitude. Compared with the method of collecting data at a preset position and attitude, the operation difficulty is reduced, the collection efficiency is improved, and the method is especially suitable for calibrating a laser radar fixed on a vehicle or a robot.

[0048] 2. The method collects point clouds with a view point concentrated on the carrier running path, and the contents of adjacent point cloud frames are relatively close, thereby reducing the difficulty of point cloud registration. The method also uses sensor fusion, face estimation and other technologies to construct a relatively fine scene point cloud map, which is also conducive to improving the final calibration accuracy.

[0049] 3. The method iteratively constructs a scene point cloud map segment and updates the laser radar intrinsic parameter value, and the plane information of each scene segment participates in the update of the intrinsic parameter value, thereby reducing the interference of random noise and improving the accuracy of intrinsic parameter calibration. The method can also be used when the plane of a single scene segment is not rich enough. BRIEF DESCRIPTION OF DRAWINGS

[0050] Figure 1 is the flowchart of the laser radar simultaneous mapping and intrinsic parameter calibration method on a mobile carrier proposed by the application;

[0051] Figure 2 is a schematic diagram of the intrinsic parameter model used by the application;

[0052] Figure 3 is the flowchart of the weighted point-to-plane ICP used by the application. DETAILED DESCRIPTION

[0053] In order to describe the present application more specifically, the technical solutions of the present application are described in further detail below in combination with the drawings and specific embodiments.

[0054] The laser radar emits laser to the surrounding environment and receives the laser reflected by the object, determines the distance of the measured object by measuring the time-of-flight (TOF) of the laser, and determines the angle of the measured object by measuring the exit angle of the laser. The ranging value, the angle measurement value and the time stamp are often referred to as the original measurement value of the laser radar, and the internal parameter model designed in the present application mainly corrects the ranging deviation and the angle deviation of the laser radar.

[0055] Generally, the laser radar uses a crystal oscillator to generate a reference clock, and uses hardware logic to control the firing sequence timing of the laser. Such timing scheme has low offset, small jitter and slow aging, so the error of the time stamp of the laser radar can be ignored.

[0056] Due to cost considerations, some laser radar manufacturers only calibrate part of the internal parameters or use an excessively simplified internal parameter model. Even if the manufacturer performs fine calibration, the internal parameters will still slowly drift due to the influence of long-term vibration, cold and hot cycle, device aging and other factors after the laser radar is installed on an autonomous vehicle. Therefore, it is necessary to design a perfect internal parameter model and an easy-to-use calibration method to improve and maintain the accuracy of the laser radar data.

[0057] There are mainly two types of internal parameter calibration methods for laser radars, supervised calibration and unsupervised calibration. The supervised calibration needs to use a calibration board or other devices to build a special calibration scene, and uses a high-precision scanner to obtain the three-dimensional model true value of the scene. The laser radar to be calibrated scans the above-mentioned scene, and optimizes the internal parameters and the pose of the radar in the scene so that the point cloud falls on the surface of the true value model as much as possible. This method has the advantages of fast calibration speed and high internal parameter accuracy, but the construction and modeling cost of the calibration plant is high, and it is unrealistic to ask users to regularly send vehicles back to the factory for calibration.

[0058] The unsupervised calibration relaxes the requirement for the calibration scene, does not need the true value model of the scene, and assumes that the scene is locally flat. A typical method first uses the laser radar to be calibrated to collect data at multiple positions in the scene, and establishes a first point cloud model of the scene. Then the above-mentioned laser radar is placed back to the center of the scene, and data is collected at multiple tilt angles, and a second point cloud model of the scene is established. The internal parameters of the laser radar are optimized so that the planes in the two point clouds coincide as much as possible.

[0059] The above-mentioned unsupervised calibration method to some extent overcomes the limitations of the site and cost, but there are still many problems in practice:

[0060] In terms of ease of use, the existing method requires static data collection, and it takes about 5 seconds for the vehicle to complete a set of forward driving, deceleration and static collection, which is not efficient. In addition, the existing method requires changing the inclination of the laser radar, because the laser radar is fixed on the vehicle, the vehicle needs to drive to different slopes, which increases the requirements for the site.

[0061] In terms of accuracy, the scene model used in unsupervised calibration is obtained by multi-frame point cloud registration, and the registration error restricts the calibration accuracy. In addition, for the internal parameter model with a large number of parameters, the constraints provided by a single scene may not be sufficient, resulting in incorrect calibration results. Increasing the number of data collection points and inclination can alleviate the above problems, but it increases the complexity of the calibration process.

[0062] To improve the ease of use and accuracy of laser radar internal parameter calibration on a mobile carrier, the present application provides a laser radar simultaneous mapping and internal parameter calibration method on a mobile carrier, Figure 1 The method flowchart of the present application is given. According to the working principle and physical structure of the laser radar, the present application establishes an internal parameter model that corrects the ranging bias and angle measurement bias. The environment point cloud data and motion data are continuously collected while the carrier is driving, the map segment is iteratively constructed and the internal parameter value is updated, and finally the accurate laser radar internal parameter and scene map are obtained. The present application uses motion compensation, cell estimation, sensor fusion and other technologies to reduce the mapping error, and updates the internal parameter value by optimizing the consistency of the cells in the map segment under multiple observations.

[0063] This part takes 32-line mechanical scanning laser radar Velodyne VLP-32C as an example to introduce, but the method is also applicable to other models. Detailed information of VLP-32C can be found at https: / / velodynelidar.com / products / ultra-puck / .

[0064] The specific steps of the laser radar simultaneous mapping and internal parameter calibration method on a mobile carrier proposed by the present application are as follows:

[0065] As shown in Figure 1 , in step S101, the internal parameter model of the laser radar is designed. The sources of ranging bias include temperature change, device aging, focusing error, nonlinearity of laser transceiver circuit, etc. For example, the light intensity returned by objects near and far differs by several orders of magnitude, and the signal distortion and time delay change under a large dynamic range. Because the laser transceiver circuit of each channel of VLP-32C is independent, the ranging bias is described using a lookup table dTable about laser line number i and original ranging value . The lookup table saves the ranging bias values of 32 laser beams at 15 distances, and the distance points are exponentially distributed in the range of 2m to 256m. The lookup table operation is recorded as The ranging bias value is generated by interpolation The calibrated ranging value is

[0066] The angle value of VLP-32C contains two parts. The first part is the pitch angle of laser beam relative to the rotor and the direction angle The laser transceiver and lens are rigidly fixed on the rotor, and The bias of the above mainly comes from assembly error, and slowly drifts with the aging of the lidar. The pitch angle of the i-th laser beam is compensated by The calibrated pitch angle of the i-th laser beam is The direction angle of the i-th laser beam relative to the rotor is compensated by The calibrated direction angle of the i-th laser beam relative to the rotor is

[0067] The second part is the direction angle of the rotor relative to the base VLP-32C uses a magnetic encoder to measure The bias sources include eccentricity and tilt of the magnetic sensing element installation, axis sensitivity difference of the magnetic sensing element, external magnetic field interference, etc. The second harmonic model can better fit the above bias, and the calibrated direction angle of the rotor is

[0068] Figure 2 The schematic diagram of the above internal parameter model is given. The user can give the initial value of the parameters in the above internal parameter model, and the default is 0 if not given.

[0069] In step S102, the experimental vehicle drives on the road and collects sensor data. The laser radar, IMU, and GNSS receiver are installed on the roof, and a customized metal structure is used to reliably fix the sensors together. A special hardware controller is used to achieve time synchronization of the sensors. The laser radar frame rate is 10HZ, the IMU data rate is 2KHZ, and the GNSS data rate is 5HZ. Drive on the internal road of a certain park, the fastest speed is about 10m / s, and the buildings beside the road are relatively dense.

[0070] It is worth noting that the carrier applicable to the present method is not limited to a vehicle. The GNSS data in the present method is not necessary, but helps to improve the accuracy and efficiency. The present method can select plane features from natural scenes, but selecting scenes with rich planes and less interference helps to improve the accuracy and efficiency. The present method requires that the IMU and the laser radar are rigidly connected, and a metal structure is used to fix the IMU near the base of the laser radar, which can obtain high rigidity and reduce the error caused by connection deformation during driving.

[0071] It is worth noting that the density of the collected point cloud is inversely proportional to the driving speed. When the point cloud density is below a certain limit, the error of point cloud registration will increase sharply, resulting in that the mapping and calibration accuracy cannot meet the requirements. When the point cloud density is higher than a certain limit, the accuracy of point cloud registration basically no longer improves, and continuing to reduce the vehicle speed will only increase the time consumption of collecting data. The more suitable speed range of VLP-32C is 2m / s to 10m / s. For a laser radar with higher line number, the driving speed can be correspondingly increased.

[0072] 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 ] is converted into the point cloud in the spatial rectangular coordinate system (laser radar coordinate system) as follows:

[0073]

[0074] It is agreed that the center of the laser radar is the origin, the front is the x-axis, the left is the y-axis, and the upper is the z-axis. The definition of the center point coordinates and the front (Φ r = 0) can be found in the user manual. In the above formula, horizOffset and vertOffset represent the offset of the laser emission position in the horizontal and height directions. Since all the laser beams inside VLP-32C share a lens for collimation and emission, there is only one optical center, and horizOffset and vertOffset can be ignored.

[0075] In step S104, the motion distortion of the point cloud frame in S103 is compensated, the pose of the point cloud frame relative to the scene point cloud map is calculated, and the point cloud frame is fused into the scene point cloud map. The collection time of each point in the point cloud frame is quite different. Although the time consumption of laser radar ranging is very short, the laser beam needs to be scanned and manipulated to point to different angles, and the results are accumulated to obtain a frame of point cloud. For example, VLP-32C needs 100ms to complete a scanning cycle. When the car is driving, the pose of the laser radar changes constantly, and the coordinate systems of the points collected at different times are inconsistent. Intuitively, the point cloud appears distorted and torn, which is the motion distortion of the laser radar point cloud. Motion distortion causes the point cloud to be unable to directly correspond to the three-dimensional structure of the scene. When the vehicle is driving straight or turning quickly, the motion distortion can reach a meter level. Therefore, the motion distortion must be compensated in order to construct a more accurate environment point cloud map.

[0076] The basic principle of motion distortion compensation is to calculate the pose of the lidar during the acquisition of the point cloud frame, apply the transformation from the lidar coordinate system to the reference coordinate system to each point at that time, and unify all the points to the reference coordinate system after the transformation. The pose changes during the driving of the vehicle are wideband, and the use of IMU data to estimate the wideband motion has high accuracy and small computational overhead.

[0077] The position of the IMU at the end of the first frame of point cloud is agreed to be the origin, and the East-North-Up (ENU) coordinate system is established. The angular velocity and acceleration of the IMU measured by the sensor frame are denoted as The pose, velocity, and position of the IMU relative to the ENU system at the end of the kth frame of point cloud are denoted as The bias of the angular velocity and acceleration of the IMU at the end of the kth frame of point cloud are denoted as Point P is an arbitrary point in the k+1th frame of point cloud, and its timestamp is k+p.

[0078] The pose, acceleration, velocity, and displacement of the IMU relative to the ENU system at the k+p time are calculated according to the following formula. The skew-symmetric matrix of ω is S(ω), and the local g value can be accurately calculated using the latitude and altitude.

[0079]

[0080] In actual engineering, the IMU coordinate system and the lidar coordinate system do not exactly coincide, and the transformation from the lidar coordinate system to the IMU coordinate system is used. The transformation from the lidar coordinate system to the IMU coordinate system is described. Then the lidar pose corresponding to point P is

[0081] In this example, point P is transformed from the lidar coordinate system at the k+p time to the lidar coordinate system at the end of the k+1th frame, and the point after compensation for motion distortion is obtained.

[0082] Next, the weighted point-to-plane ICP algorithm is used to register the k+1th frame of point cloud and the scene point cloud map. The pose of the k+1th frame of point cloud relative to the scene point cloud map is iteratively optimized to minimize the matching cost C, and the detailed process of the algorithm is shown in Figure 3 .

[0083]

[0084] In the above formula, p is a point in the k+1th frame of point cloud, and the transformation from the lidar coordinate system to the scene point cloud map coordinate system is used. The transformed point is denoted as q is the point in the scene point cloud map closest to p ′ n q is the normal vector corresponding to q, and k qis the weight corresponding to q, and dot is the vector dot product.

[0085] Analytical solution is very difficult, because the correspondence between p and q is a discontinuous function of ICP uses an iterative method to find its approximate solution, the steps are as follows:

[0086] Transform p using the current , search for the corresponding q. Assuming the correspondence between p and q is constant, calculate to minimize the matching cost. Loop iteration until numerically convergent or exceeds the maximum number of iterations.

[0087] The IMU motion estimation is highly accurate for a short time, but the error will quickly accumulate over time and needs to be corrected by combining other sensors. The pose obtained by point cloud registration has small drift, but it may be matched incorrectly when there are insufficient environmental features or many interfering objects. GNSS can directly measure the absolute position, eliminating the cumulative error of mapping, but the random noise is larger. In this example, the Kalman filter is used to fuse the IMU estimation results, point cloud registration pose, and GNSS position to obtain a more accurate state quantity at the end of the k+1 frame

[0088] The above state quantities need to be initialized before starting mapping, and incorrect initial values will cause the starting part of the map to have reduced accuracy or even fail to map. Among them, can be set according to the latest IMU intrinsic calibration result, can be estimated by the IMU-based attitude algorithm, can be estimated by registering adjacent point cloud frames, Under the coordinate convention of this example, it is

[0089] Finally, according to the estimated by the Kalman filter, the k+1 frame of point cloud is transformed, and the transformed point cloud frame is merged into the scene point cloud map. In the actual road environment, the car may temporarily stop due to waiting for the green light or avoiding pedestrians, at which time the scanned point cloud frame is almost repetitive. Repetitive point clouds cannot provide new scene information and only reduce the uniformity of the scene point cloud map. In this example, the displacement of the current point cloud frame to the last frame in the scene point cloud map is used to evaluate the repetitiveness of the current point cloud frame, and if the displacement is <0.1m, it is not merged into the scene point cloud map.

[0090] In step S105, the k-nearest neighbors of each point in the scene point cloud map S104 are searched, and the normal vector and local flatness of each point are calculated. The point cloud can be regarded as a sampling of the real scene, and the essence of calculating the point cloud normal vector is to restore the plane information (also known as the facet) in the scene, which plays an important role in many point cloud processing algorithms. The method for calculating the normal vector of a point is to use its k-nearest neighbors for local plane fitting, and the plane equation and fitting residual in space can be written as:

[0091]

[0092]

[0093] There is an efficient analytical solution to the above plane fitting problem. Calculate the mean of the k-nearest neighbor coordinates, subtract the mean from the k-nearest neighbor coordinates, and calculate the covariance matrix. The eigenvector corresponding to the smallest eigenvalue of the matrix is the normal vector, and the smallest eigenvalue is the fitting residual.

[0094] The number of points k used for fitting is usually 8 to 16. Too few points are prone to fitting errors due to noise interference, and too many points will flatten small three-dimensional structures and increase the computational burden. In order to speed up the k-nearest neighbor search, the point cloud is usually organized using spatial data structures such as KD-Tree, Octree, and Voxel Hash Table.

[0095] In the actual point cloud map, the points on the surface close to the laser radar and approximately perpendicular to the laser beam are very dense, and the points on the surface far from the laser radar and approximately parallel to the laser beam are very sparse. A fixed k value cannot adapt to the large changes in point density, and adaptive algorithms have high complexity. Let the scene point cloud map be M, and the method performs voxel average downsampling on M to obtain M ds ds contains the number of points and average coordinates in the non-empty voxel. In M ds , find the k-nearest neighbors of M, and calculate the mean and covariance matrix of the k-nearest neighbor coordinates after weighting by the number of points. That is, after limiting the highest density of the point cloud by downsampling, fixed k-nearest neighbor plane fitting is performed.

[0096] Due to the limitations of point cloud density and accuracy, there are many errors in the calculated normal vectors on complex structures such as branches and grass. This method uses the plane fitting residual to evaluate the quality of the corresponding normal vector. If the smallest eigenvalue is less than a threshold value, the local flatness is 1, otherwise the local flatness is the threshold value divided by the smallest eigenvalue. The weight of the point during mapping and calibration is proportional to the local flatness, which reduces the influence of unreliable normal vectors.

[0097] ​In step S106, the scene point cloud map described in S104 is randomly down-sampled to generate two sub-point cloud maps, and the plane consistency cost of the two sub-point cloud maps is calculated. When the vehicle is running, the same plane in the scene will be repeatedly observed from multiple positions and angles by multiple laser beams. After the map is constructed using multiple frames of point clouds, geometric inconsistency caused by intrinsic parameter errors will be revealed. The method calculates the laser radar intrinsic parameters by optimizing the plane consistency cost.

[0098] This step is usually performed every 50 to 200 frames, because only when the map geometry changes significantly can new calibration information be provided. If there is not enough new frames accumulated in the map, return to S103 to continue processing the next frame. The method of random down-sampling is to traverse the points in the scene point cloud map, and perform one of the following operations {put into sub-point cloud A, put into sub-point cloud B, skip directly} according to a preset probability. The plane consistency cost C = ∑|dot(p A -p B ,n B k B )|, where p A is a point in point cloud A, p B is the nearest neighbor of p A in point cloud B, n B is the normal vector of p B , k B is the weight corresponding to p B , dot is the dot product of vectors, and the absolute value is the absolute value.

[0099] In step S107, the partial derivative of the plane consistency cost in S106 with respect to the intrinsic parameters in S101 is calculated, and the gradient descent method is used to update the intrinsic parameter values in S101. The partial derivative matrix J = d(C + R) / dP, where C is the plane consistency cost, P is the intrinsic parameter, and R is the regularization term. The intrinsic parameter value updating method is P = P - aJ, where a is the step size. When the scene has insufficient planes, increasing the regularization coefficient helps to suppress the calibration error caused by overfitting. If the scene contains a large number of planes at different positions and angles, the regularization coefficient can be reduced.

[0100] In step S108, the points in the scene point cloud map described in S104 that are too far away from the current laser radar position are deleted, and the step size of the updated intrinsic parameter value in S107 is attenuated. The probability of points far away from the laser radar returning to the field of view again is low, and if the distance of a point exceeds 1.1 times the range of the laser radar, the point is deleted to save memory and computing resources. In the initial stage of calibration, a larger step size is used to speed up the convergence speed, and then the step size is gradually attenuated to suppress the interference of random noise and obtain more accurate intrinsic parameter values. The attenuation curve used in this example is a logarithmic curve.

[0101] If the data collected in step S102 is used up, the intrinsic parameters in step S101 are output as the final calibration result in step S109. Otherwise, the process returns to step S103 to continue processing the next frame. If the data collected in a single run is not sufficient, or if it is desired to further improve the calibration accuracy, multiple data sets can be collected in multiple road segments, and the method can be run multiple times, with the results of the previous calibration serving as the initial values for the next calibration, and the step size for updating the parameters being appropriately reduced.

[0102] The above merely describes specific embodiments of the present application, and should not be used to limit the scope of the present application. Any equivalent changes made by those skilled in the art based on the present application, and any changes well known to those skilled in the art, should still fall within the scope of the present application.

Claims

1. A method for simultaneous mapping and intrinsic calibration of a laser radar on a mobile carrier, characterized in that, The method comprises the following steps: S101: according to the mechanical and optical structure of the laser radar to be calibrated, an internal parameter model for correcting ranging deviation and angle measuring deviation is designed; S102: the laser radar to be calibrated and the inertial measurement unit are rigidly fixed on a carrier, and original measurement values of the laser radar and motion data of the inertial measurement unit during driving of the carrier on a road are collected; S103: a frame of original measurement values of the laser radar is read, parameters of the internal parameter model in S101 are read, measurement deviation is compensated, and a frame of point clouds is generated; S104: motion distortion of the frame of point clouds in S103 is compensated, a pose of the frame of point clouds relative to a scene point cloud map is calculated, and the frame of point clouds is fused into the scene point cloud map; S105: k-nearest neighbors of each point in the scene point cloud map in S104 are searched, and a normal vector and a local flatness of each point are calculated; S106: every interval of a preset frame number, the scene point cloud map in S104 is randomly down-sampled to generate two sub-point cloud maps, a plane consistency cost of the two sub-point cloud maps is calculated, and the next step is performed; if not enough new frames are accumulated, the next frame is processed in S103; S107: partial derivatives of the plane consistency cost in S106 to internal parameters in the internal parameter model are calculated, and the internal parameter values in the internal parameter model are updated by using a gradient descent method; S108: points in the scene point cloud map in S104 that are beyond a preset distance from a current position of the laser radar are deleted, and a step of the updated internal parameter values in S107 is attenuated; S109: the next frame is processed in S103 until all the collected data are used up, and the parameters of the internal parameter model are output as a final calibration result.

2. The method of claim 1, wherein, In step S101, the laser radar to be calibrated is an n-line mechanical scanning laser radar, and the internal parameter model is as follows: The ranging value of the i-th laser before calibration is denoted as The pitch angle of the i-th laser is denoted as The direction angle of the i-th laser relative to the rotor is denoted as The direction angle output by the rotor encoder is denoted as Compensate the ranging deviation by using a look-up table; the look-up table is denoted as dTable, which saves the ranging deviation values of n laser beams at k distances; the look-up table operation is denoted as Generate the ranging deviation values at any distance by interpolation ​ Calibrated range value Using compensate the pitch angle of the i-th laser beam, the calibrated pitch angle Using Compensate the direction angle of the i-th laser beam relative to the rotor, the calibrated direction angle relative to the rotor Compensate the angle measurement error of the rotor encoder using a second harmonic model, the calibrated direction angle of the rotor If the user does not give an initial value of a parameter in the internal parameter model, all the parameters are initialized as 0.

3. The method of claim 1, wherein, In step S102, the carrier is a vehicle or robot that drives itself by using wheels or tracks or mechanical feet; the original measurement values of the laser radar include laser ranging values, laser exit angles and time stamps; and the motion data of the inertial measurement unit at least include three-axis accelerations and three-axis angular velocities.

4. The method of claim 2, wherein, In step S103, the bias of the raw 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 ] is converted into the point cloud in the spatial rectangular coordinate system according to the following formula: The laser radar center is agreed as the origin, the front is the x-axis, the left is the y-axis, and the up is the z-axis. In the above formula, horizOffset and vertOffset represent offsets of the laser exit position in the horizontal and height directions.

5. The method of claim 1, wherein, In step S104, the method for compensating motion distortion of the frame of point clouds is as follows: The motion data of the inertial measurement unit are used to calculate the pose of the laser radar during collection of the frame, the above pose is interpolated according to the time stamp of the point cloud to obtain the pose of the laser radar when each point is collected; A transformation of the above pose relative to a reference pose is calculated, and the above transformation is applied to the coordinates of each point; The reference pose is usually selected as the pose of the laser radar at the beginning, middle or end of the frame of point clouds; The method for calculating the pose of the frame of point clouds relative to the scene point cloud map is as follows: The point cloud frame and the scene point cloud map are registered by using a weighted point-to-plane ICP algorithm to obtain the pose of the laser radar, and the pose of the laser radar obtained by the point cloud registration and the pose of the laser radar calculated by the inertial measurement unit are input into a Kalman filter to obtain the filtered pose of the laser radar and update the inertial measurement unit drift parameter.

6. The method of simultaneous mapping and intrinsic calibration for mobile-lidar according to claim 1, wherein, In step S105, the point cloud is organized by using a KD-Tree or an octree or a voxel hash table, and the k-nearest neighbors of a point are searched on this basis; the method for calculating the normal vector of the point is as follows: The mean value of the k-nearest neighbor coordinates is calculated, the covariance matrix is calculated after the k-nearest neighbor coordinates are subtracted from the mean value, the matrix is subjected to eigenvalue decomposition, and the normal vector of the point is the eigenvector corresponding to the minimum eigenvalue; if the minimum eigenvalue is less than a threshold value, the local flatness is 1, otherwise the local flatness is the threshold value divided by the minimum eigenvalue.

7. The method of claim 1, wherein, In step S106, the frame interval is 50 to 200; The method of random down-sampling is to traverse the points in the scene point cloud map, and perform one of {put into sub-point cloud A, put into sub-point cloud B, directly skip} according to a preset probability; C = ∑ |dot(p A -p B n B k B )|, where p A is a point in point cloud A, p B is the nearest neighbor of p A in point cloud B, n B is the normal vector of p B , k B is the corresponding weight of p B , dot is the dot product of vectors, and the vertical bar represents the absolute value.

8. The method of claim 1, wherein, In step S107, the partial derivative matrix J = d(C + R) / dP, wherein C is the plane consistency cost, P is the intrinsic parameter, and R is the regularization term; The intrinsic parameter value updating method is P = P - aJ, wherein a is a step size; the regularization strength and the intrinsic parameter updating step size are adjusted according to the calibration scene quality.

Images