A joint calibration method of vehicle-mounted LiDAR-IMU external parameters

Abstract

A vehicle-mounted LiDAR‑IMU external parameter joint calibration method relates to the field of sensor calibration technology, and is aimed at the problems of difficulty in establishing constraints and low calibration accuracy under vehicle-mounted conditions in the prior art. Compared with the target-assisted method, this application does not need to rely on known external reference objects, the calibration results are less affected by the test environment, and it is simpler and more convenient to implement; compared with the traditional hand-eye calibration method and point cloud optimization method, this application shortens the calibration time and improves the calibration accuracy; this application solves the problem of difficulty in establishing sensor external parameter constraints due to low freedom of movement under vehicle-mounted conditions.

Description

Technical Field

[0001] The present invention relates to the field of sensor calibration technology, and in particular to a vehicle-mounted LiDAR-IMU external parameter joint calibration method. Background Art

[0002] Simultaneous localization and mapping (SLAM) technology plays an important role in autonomous driving, robot autonomous navigation, AR / VR and other fields. This technology can use the surrounding environment information to build a map and perform positioning based on the built map. SLAM relies entirely on its own sensors and has strong adaptability to various unknown environments. Compared with traditional inertial navigation, satellite navigation and other methods, it has great advantages. The method using laser radar (LiDAR) as the main sensor has become the most stable and mainstream method in the current SLAM technology due to its advantages such as high resolution, wide field of view, and no influence of light changes. However, the method based on LiDAR also has certain limitations. In order to solve the problems of point cloud distortion, feature similarity and sparseness in corridors or open environments, LiDAR is usually fused with inertial navigation, but the calibration accuracy of the external parameter matrix between sensors will greatly affect the accuracy of data association. Generally, commercial robots or cars carrying multiple sensors are calibrated in the factory using precision instruments. However, for self-designed unmanned vehicle systems, sensors sometimes cannot be installed on the same tooling board due to design requirements. This not only increases the difficulty of manual measurement, but the measured relative pose relationship is often inaccurate. Therefore, it is of great significance to use sensor data to obtain accurate external parameters from the algorithm level.

[0003] The calibration methods of external parameters between sensors are divided into target-assisted methods and self-calibration methods according to whether they rely on external object information. The target-assisted method is to establish constraints on the external parameters of the sensor based on the observation information of the sensor on the reference target by establishing an external reference target, and then solve the external parameters between sensors. Although the target-assisted method can strengthen the constraints to a certain extent, the establishment and maintenance of a certain number of artificial targets will undoubtedly increase the cost and complexity. In addition, in some special cases, the sensor may need to be frequently calibrated automatically to maintain the system accuracy. Therefore, under the premise that real-time calibration cannot be completed by relying on manual equipment, the self-calibration method is of great significance. The self-calibration method is to establish constraints only through the carrier's own motion and the sensor's observation information of the external environment without relying on external reference targets, and solve the external parameters between sensors through hand-eye calibration or point cloud optimization methods. However, the current self-calibration method has the disadvantages of high algorithm complexity, high requirements for structured environment, difficulty in establishing constraints under vehicle conditions, and low calibration accuracy. Summary of the invention

[0004] The purpose of the present invention is to propose a vehicle-mounted LiDAR-IMU external parameter joint calibration method to address the problems of difficulty in establishing constraints and low calibration accuracy under vehicle-mounted conditions in the prior art.

[0005] The technical solution adopted by the present invention to solve the above technical problems is:

[0006] A vehicle-mounted LiDAR-IMU external parameter joint calibration method, comprising:

[0007] Step 1: The carrier moves so that the LiDAR odometer and the GPS / IMU integrated navigation system respectively obtain a large-scale trajectory, where the large-scale trajectory is a trajectory with a radius R greater than 500m;

[0008] Step 2: Based on the LiDAR odometer trajectory, the GPS / IMU integrated navigation system trajectory is temporally interpolated to align the timestamps of the LiDAR odometer and the GPS / IMU integrated navigation system, thereby obtaining equivalent information of the GPS / IMU integrated navigation system. The equivalent information of the GPS / IMU integrated navigation system is then used to synchronize the large-scale trajectory of the LiDAR odometer and the GPS / IMU integrated navigation system.

[0009] Step 3: Convert the position points on the two synchronized large-scale trajectories into two sets of point clouds, and solve the two sets of point clouds based on the normal distribution transformation and iterative nearest point point cloud matching algorithm to obtain the rotation extrinsic parameter R X ;

[0010] Step 4: Determine whether the carrier is rotating according to the z-axis gyroscope data of the IMU in the GPS / IMU integrated navigation system, and remove the LiDAR point cloud data of non-rotational motion;

[0011] Step 5: Remove abnormal points in the rotational LiDAR point cloud data, then calculate the smoothness of the points in the rotational LiDAR point cloud data, and extract feature points in the remaining point cloud according to the smoothness, wherein the abnormal points are parallel points and occlusion points, and the feature points are corner points and plane points;

[0012] Step 6: Perform temporal interpolation on the trajectory of the GPS / IMU integrated navigation system to obtain the GPS / IMU data corresponding to each laser point, and use the GPS / IMU data and the external parameters to be determined Unify all laser points in the initial inertial navigation system;

[0013] Step 7: In the initial inertial navigation system, a new objective function is constructed by combining the statistical error average effect and the displacement constraint, and based on the rotation external parameter R X , that is, the initial value of the rotation parameter Using nonlinear optimization method to solve The external parameters to be requested

[0014] Step 8: Fit the feature points in the rotating LiDAR point cloud data to obtain the ground points, and then use the average z coordinates of all ground points as the height of the LiDAR base relative to the ground, and finally subtract the height from the designed installation height of the GPS / IMU integrated navigation system to obtain the translation parameter z';

[0015] Step 9: Exploitation And the translation parameter z' to obtain the vehicle sensor joint calibration parameter

[0016]

[0017] Further, the time interpolation is expressed as:

[0018]

[0019] scale=(t l -t i ) / (t i+1 -t i )

[0020] Among them, t i ,t i+1 and t l represents three different moments, and t i <t l <t i+1 , T l I Represents LiDAR at t l The inertial navigation data corresponding to the time, T i I and Represents t i and t i+1 The inertial navigation data at the moment, scale represents the scale factor, that is, the proportion of LiDAR time in the corresponding time of two adjacent inertial navigation data.

[0021] Furthermore, the specific steps of step three are:

[0022] Step 31: Convert the position points on the two large-scale trajectories into two sets of point clouds, and use one set of point clouds as the target point cloud and the other set of point clouds as the source point cloud;

[0023] Step 32: Divide the space occupied by the target point cloud into voxels of fixed size, and calculate the multidimensional normal distribution mean μ and variance Σ of each voxel in the target point cloud, specifically:

[0024]

[0025]

[0026] Where n is the number of voxel midpoints, x i is the coordinate of the i-th point in the voxel;

[0027] Step 33: Convert the source point cloud into the voxel space of the target point cloud by rotating the external parameters;

[0028] Step 3 and 4: Calculate the probability density S of each conversion point in the source point cloud according to the normal distribution parameters of the target point cloud, expressed as:

[0029]

[0030] Then the sum of the probability densities of all points in the source point cloud is calculated, and the objective function is established. The objective function is expressed as:

[0031]

[0032] Wherein, x is the coordinate of each point in the source point cloud, and u is the mean of the multidimensional normal distribution of each voxel in the target point cloud; Step 35: Use the Gauss-Newton method to optimize and solve the above objective function to maximize the value of Score, obtain the rotation extrinsic parameter, and use the rotation extrinsic parameter as the rotation extrinsic parameter in step 3, and repeat steps 33 to 35 until the convergence condition is reached to obtain the initial rotation extrinsic parameter;

[0033] Step 36: Use the initial value rotation external parameter as the initial value R0 of the iterative nearest point solution parameter, perform a rotation transformation on the source point cloud, and obtain the nearest point of each point in the source point cloud in the target point cloud;

[0034] Step 37: Match the points in the source point cloud with the nearest points of the points in the target point cloud to obtain matching point pairs in the source point cloud and the target point cloud;

[0035] Step 38: Calculate the matrix H based on the matching point pairs in the source point cloud and the target point cloud:

[0036]

[0037] in, and represents the centroid of the source point cloud and the target point cloud, represents the i-th point in the source point cloud, represents the i-th point in the target point cloud, represents the centroid coordinates of the i-th point in the source point cloud, represents the centroid coordinates of the i-th point in the target point cloud;

[0038] Step 39: Perform SVD decomposition on the matrix H to obtain the rotation external parameter RX , judge R X Whether it converges, if R X If it does not converge, jump to step 36. X Convergence ends.

[0039] Furthermore, the SVD decomposition of the matrix H is expressed as:

[0040] H=UΣV T ,R * =R X =VU T

[0041] Among them, U and V are orthogonal matrices, Σ is a singular value matrix, R * is the rotation external parameter R calculated in this cycle X .

[0042] Furthermore, the specific steps of step 4 are:

[0043] Step 41: Perform time interpolation based on the GPS / IMU combined navigation system trajectory to obtain the IMU gyroscope data corresponding to the starting time of each frame of LiDAR point cloud data, and then obtain the IMU gyroscope z-axis gyroscope data corresponding to the starting time of each frame of LiDAR trajectory;

[0044] Step 42: Determine whether the carrier is rotating based on the z-axis gyroscope data, and remove the LiDAR point cloud data of non-rotational motion based on the determination result.

[0045] Furthermore, the parallel points in the LiDAR point cloud data with rotational motion removed in step 5 are expressed as:

[0046]

[0047] The occluded points in the LiDAR point cloud data after eliminating rotational motion are expressed as:

[0048] r E -r D >β&|roll(r E )-roll(r D )|<10

[0049] Among them, r X Indicates the depth value of the laser point. AE corresponds to the serial number of the laser point in the figure. diff1 indicates the difference in depth value between point A and point B. diff2 indicates the difference in depth value between point B and point C. α and β are proportional factors. roll() indicates the number of LiDAR lines where the laser point is located.

[0050] Furthermore, α=0.02, β=0.3.

[0051] Furthermore, the smoothness C in step 5 is expressed as:

[0052]

[0053] in, It represents the coordinate value of the laser point on the k-axis of the LiDAR coordinate system, k = x, y, z, and q is the laser point serial number q = in, ... i-1, i, i+1, ..., i+n.

[0054] Furthermore, the objective function in step seven is expressed as:

[0055]

[0056] Where s represents the number of LiDAR frames, Indicates the LiDAR position change in adjacent frames by Convert to the inertial navigation coordinate system, represents the combined navigation position change corresponding to the LiDAR position change, P i and P j They represent the i-th and j-th laser points in the laser point cloud after being unified into the initial inertial navigation system. By continuously iteratively optimizing the objective function, we can get

[0057] Furthermore, the height of the LiDAR base relative to the ground is expressed as:

[0058]

[0059] Among them, P g represents the coordinates of the gth ground point, z() represents the z-axis coordinate value of the ground point, and m represents the total number of ground points.

[0060] The beneficial effects of the present invention are:

[0061] 1. Compared with the target-assisted method, this application does not need to rely on external known reference objects, the calibration results are less affected by the test environment, and it is simpler and more convenient to implement;

[0062] 2. Compared with the traditional hand-eye calibration method and point cloud optimization method, this application shortens the calibration time and improves the calibration accuracy;

[0063] 3. This application solves the problem of difficulty in establishing sensor external parameter constraints due to low freedom of movement under vehicle conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a schematic diagram of the sensor's motion trajectory during the turning process;

[0065] Figure 2Schematic diagram of sensor trajectory differences during turning;

[0066] Figure 3 Schematic diagram of parallel points and occlusion points;

[0067] Figure 4 This is a schematic diagram of the unmanned vehicle experimental platform;

[0068] Figure 5 This is the map of the self-test dataset 1;

[0069] Figure 6 This is the map of the self-test dataset 2;

[0070] Figure 7 Comparison diagram of the original trajectory and the aligned trajectory;

[0071] Figure 8 This is the point cloud stitching effect diagram;

[0072] Fig. 9 This is a comparison chart of the local effects of point cloud stitching. DETAILED DESCRIPTION

[0073] It should be particularly noted that, in the absence of conflict, the various embodiments disclosed in this application can be combined with each other.

[0074] Specific implementation method 1: A vehicle-mounted LiDAR-IMU external reference joint calibration method described in this implementation method includes the following steps:

[0075] Step 1: The vehicle moves so that the LiDAR odometer and the GPS / IMU integrated navigation system obtain a large-scale trajectory respectively; the large-scale trajectory means that the radius R of the trajectory is greater than 500m;

[0076] In step 1, the influence of translational external parameters can be ignored by using the large-scale motion trajectory of the vehicle, so that the rotational external parameters can be solved separately. The principle is explained below through the robot hand-eye calibration method. The hand-eye calibration calculation formula is:

[0077]

[0078] Among them, R I and t I represents the position change of GPS / IMU integrated navigation system, R L and t L represents the LiDAR odometer pose change, R X and t X are the rotation and translation extrinsics to be determined. Assuming that the carrier performs pure translation motion, then R in equation (1) I and R L is the unit matrix, the hand-eye calibration formula is simplified to:

[0079] tI =R X t L (2)

[0080] From the above formula, we can see that when the carrier performs pure translational motion, the trajectory between the LiDAR and the integrated navigation system only differs by rotation R X Therefore, the rotation extrinsic calibration problem is transformed into solving the rotation between different sensor trajectories. However, in actual situations, the carrier will not only perform translational motion, such as Figure 1 As shown in the figure, due to the existence of the lever arm, the trajectory shapes of the LiDAR and the integrated navigation system are different during the turning process. The distance between the {L} system and the {I} system is defined as l, and the angle between C{L} and C{I} is The angle between {I}{L} and {I}C is θ, and the turning radius of {I} is r I , The angle calculation formula is:

[0081]

[0082] When θ=90°, Calculated by the following formula:

[0083]

[0084] From formula (4), we can see that Will follow r I The increase of r decreases rapidly. When l = 1m, r I =500m, And usually θ < 90°, so when the carrier trajectory radius is large enough, the influence of the translation extrinsic parameter can be ignored, and equation (2) still holds.

[0085] Step 2: Based on the LiDAR odometer data, the GPS / IMU combined system trajectory is time interpolated to align the timestamps of the LiDAR odometer and the GPS / IMU combined system, and the equivalent information of the GPS / IMU combined system is obtained. The equivalent information of the GPS / IMU combined system is used to synchronize the large-scale trajectory of the LiDAR odometer and the GPS / IMU combined navigation system.

[0086] To align the timestamps of the sensor data, the GPS / IMU combined system trajectory is temporally interpolated;

[0087] In step 2, hardware time synchronization is used to ensure that the data of each sensor is distributed on a time axis. However, due to different data frequencies, interpolation is often required to obtain equivalent values ​​corresponding to the same timestamp. The sensor data is arranged in chronological order in the container. Since LiDAR is the main sensor, each time a frame of point cloud data is obtained, the time of the current laser frame is used as the timestamp to be aligned, and the closest GPS / IMU combined system data is searched before and after. Equivalent information is obtained based on linear interpolation, as shown in formula (5).

[0088]

[0089] Where i, l represent the time and i<l<i+1. l I represents the inertial navigation data corresponding to LiDAR at time l, T i I and They represent the inertial navigation data at time i and i+1 respectively, and scale represents the scale factor, that is, the proportion of LiDAR time in the corresponding time of two adjacent inertial navigation data.

[0090] Step 3: Convert the position points on the two synchronized large-scale trajectories into two sets of point clouds, and solve the two sets of point clouds based on the normal distribution transform (NDT) and iterative closest point (ICP) point cloud matching algorithm to obtain the rotation extrinsic parameter R X ;

[0091] The objective function solution problem is transformed into the posture change between two point clouds, and the rough calibration result R of the rotation parameters is obtained based on the point cloud matching algorithm of normal distribution transform (NDT) and iterative closest point (ICP). X0 ,Right now

[0092] In step 3, the point cloud matching method of NDT+ICP is used to solve R X It can save computing resources.

[0093] The position points on the two large-scale trajectories are converted into two sets of point clouds, and one set of point clouds is used as the target point cloud and the other set of point clouds is used as the source point cloud;

[0094] First, the space occupied by the target point cloud is divided into voxels of fixed size, and the mean and variance of the multidimensional normal distribution of each voxel are calculated, as shown in equations (6) and (7):

[0095]

[0096]

[0097] Then by rotating the external parameter R XThe source point cloud is converted into the voxel space of the target point cloud, the sum of the probability densities of all points in the source point cloud is calculated, and the objective function shown in equation (8) is established:

[0098]

[0099] Use the Gauss-Newton method to optimize the objective function to maximize the value of Score, and obtain the rotation external parameter R after convergence. X .

[0100] Then the NDT is matched to the calculated initial R X As the initial value R0 of the ICP solution parameter, the source point cloud is rotated to obtain the nearest points of each point in the source point cloud in the target point cloud. The points in the source point cloud are matched with the nearest points of the point in the target point cloud to obtain the matching points in the source point cloud and the target point cloud, and then the matrix H is calculated based on the matching point pairs in the source point cloud and the target point cloud:

[0101]

[0102] in, and Represents the centroid of the source point cloud and the target point cloud. Finally, perform SVD decomposition on the matrix H to obtain the rotation extrinsic parameter R X , repeat the ICP matching calculation several times until R X Convergence. As shown in formula (10):

[0103] H=UΣV T ,R * =R X =VU T (10)

[0104] Step 4: Determine whether the carrier is rotating according to the z-axis gyroscope data of the IMU in the GPS / IMU integrated navigation system, and remove the LiDAR point cloud data of non-rotational motion;

[0105] In step 4, the vehicle is limited to a turning motion state by using the z-axis gyroscope data of the IMU, and a strong constraint on the translational external parameter can be established. Figure 2 As shown in Figure 1, when the vehicle is in the process of turning, due to the lever arm effect, the displacement of the adjacent inertial navigation is not equal to the displacement of the adjacent LiDAR. L Through the external parameter matrix Go to the {I} system and use the pose change of the combined system Transform to the {Io} system and we get Similarly, we can use the P of adjacent moments L' go through and Converting to the {Io} system yields Finally, in the {Io} system based on and There are a large number of common areas, and the statistical error average effect is used to solve When the translation extrinsic parameters are unknown, it is impossible to unify the point clouds of two adjacent frames into the initial inertial navigation system {Io} through pose transformation. That is, a strong constraint is established on the translation extrinsic parameters by using the difference in sensor trajectories at the turning point of the vehicle. Therefore, limiting the calibration process to the curve environment can improve the efficiency and accuracy of the algorithm.

[0106] The specific process of environmental limitation is as follows:

[0107] (1) Based on GPS / IMU data interpolation, the IMU gyroscope data corresponding to the start time of each frame of LiDAR data is obtained;

[0108] (2) judging whether the carrier is rotating according to the z-axis gyroscope data;

[0109] (3) Based on the judgment result of (2), remove the LiDAR data of non-rotational motion.

[0110] Step 5: Calculate the smoothness of the points in the rotating LiDAR point cloud data, remove abnormal points such as parallel points and occlusion points, and extract feature points such as corner points and plane points in the remaining point cloud;

[0111] The calibration algorithm based on point cloud optimization needs to fuse all LiDAR point cloud data and optimize the objective function through iterative calculation, but the huge amount of LiDAR data will cause the algorithm to take a long time. In order to improve the efficiency of the algorithm, Lidar_align uses random sampling to reduce the number of points. Although this method greatly reduces the number of points, it fails to change the shortcomings of the original point cloud that it lacks distinctiveness and stability. Therefore, before randomly sampling the point cloud, this application extracts corner points and plane points that can reflect the characteristics of the object.

[0112] In step 5, firstly, parallel points and occluded points in the point cloud are removed. Figure 3 As shown in the figure, when the laser beam is almost parallel to the object, the beam is stretched, which will cause a large error in the data. The determination of parallel points is shown in formula (11). Since the LiDAR is in the moving process shown in the figure, point E and the dotted part in the figure will disappear in the subsequent data, so it is necessary to remove the occlusion point as an outlier. The determination formula is shown in formula (12). If the formula conditions are met, points EFGH are marked as occlusion points. In the formula, r X represents the depth value of the laser point, and roll(·) represents the number of lines where the laser point is located.

[0113]

[0114] rE -r D >β&|roll(r E )-roll(r D )|<10 (12)

[0115] Then, the corner points and plane points in the point cloud are extracted by calculating the smoothness of the laser points. The smoothness calculation is shown in formula (13):

[0116]

[0117] The greater the distance difference between the corner point and the surrounding points, the larger the calculated curvature value, that is, the higher the smoothness; the smaller the distance difference between the plane point and the surrounding points, the lower the calculated curvature value, that is, the lower the smoothness. Therefore, a smoothness threshold can be set to extract corner points and plane points.

[0118] Step 6: Interpolate the trajectory of the GPS / IMU integrated navigation system to obtain the GPS / IMU data corresponding to each laser point, and use the GPS / IMU data and the external parameters to be determined Unify all laser points in the initial inertial navigation system;

[0119] Step 7: Construct a new objective function based on the statistical error average effect and displacement constraint, and based on the initial value of the rotation parameter That is, the rotation external parameter R X , and the nonlinear optimization method is used to solve The external parameters to be requested

[0120] The calibration algorithm based on point cloud optimization aims to minimize the k-neighbor error of each point in the spliced ​​point cloud by continuously updating the external parameter matrix. Its objective function is shown in formula (14):

[0121]

[0122] The core idea is to search for the nearest point of each point and accumulate the error sum, but the method of searching for the nearest point is greatly affected by noise and is prone to fall into local extreme values. Therefore, this application adds the displacement constraints of Lidar and inertial navigation on the basis of the original objective function to construct a new objective function, as shown in formula (15):

[0123]

[0124] Where s represents the number of LiDAR frames, Indicates the LiDAR position change in adjacent frames by Convert to the inertial navigation coordinate system, Represents the combined navigation position change corresponding to LiDAR. By continuously iteratively optimizing the objective function, we can get

[0125] Step 8: Fit the midpoints of the rotating LiDAR point cloud data to obtain the ground points.

[0126] The average value of the z coordinates of all ground points is taken as the height of the LiDAR base relative to the ground, and then the translation parameter z' is obtained by subtracting it from the designed installation height of the GPS / IMU combination system;

[0127] Due to the lack of movement in the z-axis direction, the z-axis translation parameters of the sensor cannot be solved based on the above steps. In step 8, the height H of the LiDAR base relative to the ground is solved by fitting the ground points in the LiDAR point cloud using formula (16) L and with the integrated navigation system installed at an altitude of H I After the difference, we get the z' axis translation parameter, that is, z'=|H L -H I |.

[0128]

[0129] Step 9: Utilization And the translation parameter z' to obtain the vehicle sensor joint calibration parameter

[0130] The present application is described in detail below with reference to specific embodiments.

[0131] This application is based on a self-test data set in an outdoor urban environment. Through qualitative and quantitative analysis experiments, the vehicle-mounted sensor joint calibration algorithm of this application is compared with the calibration algorithm based on point cloud optimization, the calibration algorithm based on hand-eye calibration, and the improved vehicle-mounted hand-eye calibration algorithm based on trajectory.

[0132] like Figure 4 As shown in the figure, the experimental verification is based on the unmanned vehicle experimental platform, which is mainly composed of a test vehicle, a laser radar and a fiber-optic integrated navigation system. The laser radar model is a 32-line three-dimensional laser radar Velodyne HDL_32E, with a ranging range of 100m, a vertical angle range of 41.33°, a horizontal angle range of 360°, and a scanning frequency of 5Hz to 20Hz. The fiber-optic integrated navigation system model is XW-GI7660, which uses multi-sensor technology to combine satellite positioning with inertial measurement. The heading accuracy of the integrated navigation system is 0.05°, the attitude accuracy is 0.03°, the position accuracy is 2cm+1ppm, and the data update frequency can be selected from 1Hz / 5Hz / 10Hz / 100Hz. The inertial navigation raw data is combined with the LiDAR data for LIO-SAM algorithm positioning and mapping. The integrated navigation system is used to provide accurate IMU poses during the calibration process and provide trajectory reference values ​​in the positioning experiment.

[0133] Two sets of data were collected in a closed factory environment and an urban road environment based on the unmanned vehicle platform.

[0134] Data 1: A closed factory environment with moving obstacles, based on the LOAM algorithm, as follows Figure 5 As shown in the overall map, the carrier moved about 1.5 km during the entire collection process and the total time was about 307 seconds.

[0135] Data 2: A closed-loop urban environment with moving obstacles, obtained based on the LOAM algorithm: Figure 6 As shown in the overall map, the carrier moves about 1.0 km during the entire collection process and the total time is about 198 seconds.

[0136] 1. Qualitative verification of this application:

[0137] Based on the self-test data 1, the local LiDAR trajectory is obtained through the SLAM algorithm, and the latitude and longitude data of the GPS / IMU integrated navigation system are converted into coordinates in the rectangular coordinate system to obtain the local trajectory of the combined system; then the rotation extrinsic parameter rough calibration method proposed in this application is used to solve the rotation parameter R X0 , based on R X0 The LiDAR trajectory is rotated to align with the reference trajectory. The original trajectory of the sensor is as follows Figure 7 As shown in the upper part of Figure 7 The figure shows the top and side view comparison of the sensor trajectories before and after alignment, where the red trajectory represents the reference trajectory of the combined system and the green trajectory represents the LiDAR trajectory. It can be clearly seen that the original trajectories do not overlap due to the rotational external parameters between the sensors.

[0138] Depend on Figure 6 From the comparison in , we can see that the two sensor tracks can be well aligned and overlapped based on the coarse rotation parameters, which proves the effectiveness of the coarse calibration results. Figure 6 It can be seen that there are deviations in roll and pitch angles between the LiDAR trajectory and the reference trajectory, which can be attributed to two factors: one is that the LiDAR trajectory drifts over a large range; the other is that there is an installation angle error between the sensors that is invisible to the naked eye.

[0139] Based on the improved point cloud optimization full calibration scheme proposed in this application, after iterative solution, all valid points are unified in the initial position coordinate system of the combined system using the calibration results, and the following is obtained: Figure 8 The stitched point cloud shown in Figure 1. The point cloud stitched based on accurate external parameters can clearly reflect the current environment, proving the effectiveness of the full calibration results. In order to more intuitively show the effect of point cloud stitching, extract Figure 8 The local position is framed by the black frame. Fig. 9As shown, through the comparison in the figure, it can be seen that the spliced ​​point cloud map before the sensor external parameter calibration (the two pictures above) is blurred as a whole, and even has ghosting, while the spliced ​​point cloud map after the parameter calibration (the two pictures below) has clear features, and ghosting and deformation are basically eliminated. This proves that the results of this application are reasonable.

[0140] 2. Quantitative verification of this application:

[0141] When conducting quantitative analysis and verification, for the convenience of expression, Scheme 1 is set as the calibration algorithm based on point cloud optimization, Scheme 2 is set as the calibration algorithm based on hand-eye calibration, Scheme 3 is set as the improved vehicle-mounted hand-eye calibration algorithm based on trajectory, and Scheme 4 is the vehicle-mounted sensor joint calibration algorithm of this application. The running time and calibration results of the four schemes obtained based on self-test data 1 and self-test data 2 are shown in Table 1.

[0142] Table 1 Running time and calibration results of four schemes / m

[0143]

[0144] From the perspective of algorithm operation time consumption, the operation efficiency of Scheme 4 is significantly improved compared with Scheme 1 and Scheme 2. Although Scheme 4 takes longer than Scheme 3, it is acceptable to sacrifice some efficiency in exchange for higher accuracy. From the analysis of the external parameter calibration results, although Scheme 1 has the smallest pitch angle error, its translation parameter error is too large; although Scheme 2 has the smallest roll angle error, its pitch angle and heading angle errors are too large, both exceeding 1°; Scheme 4 has the smallest heading angle error and x-direction translation error. After comprehensive consideration, Scheme 4 has the best calibration result.

[0145] Due to the installation error of the sensor platform, the obtained external reference value is not absolutely accurate. Here, the calibration results of different schemes are introduced into the LIO-SAM algorithm to obtain the carrier trajectory based on the self-test data set, and the performance of the calibration algorithm is further verified by the positioning results. This application uses the root mean square error (RMSE) and standard deviation (SD) between the estimated trajectory and the true trajectory to evaluate the positioning effect, where the true trajectory is provided by the integrated navigation system. As mentioned above, the integrated navigation system has a positioning accuracy of 2cm+1ppm and can provide a reliable trajectory reference value. Table 2 comprehensively shows the comparison results of the root mean square error and standard deviation of different trajectories.

[0146] Table 2 Comparison of trajectory errors based on different calibration algorithms

[0147]

[0148]

[0149] From Table 2, it can be seen that the positioning accuracy and data stability of Scheme 4 are better than those of other schemes. Compared with the uncalibrated algorithm, the accuracy of self-test data 1 is improved by 0.1588m, and that of self-test data 2 is improved by 0.0244m. The positioning accuracy based on self-test data 2 is improved less because the number of dynamic objects in self-test data 2 is less than that in self-test data 1. The algorithm is based on the positioning result preference obtained by the lidar, and the introduction of better inertial navigation data has limited effect on improving the performance of the algorithm.

[0150] By comparing data such as algorithm time consumption, calibration results, trajectory root mean square error and standard deviation, it can be proved that the vehicle-mounted sensor joint calibration algorithm of the present application has improved accuracy compared with the traditional calibration algorithm, and the algorithm time consumption has been effectively controlled.

[0151] It should be noted that the specific implementation is only an explanation and description of the technical solution of the present invention, and cannot be used to limit the scope of protection of the rights. Any partial changes made according to the claims and description of the present invention should still fall within the scope of protection of the present invention.

Claims

1. A vehicle-mounted LiDAR-IMU external parameter joint calibration method, characterized in that include: Step 1: The carrier moves so that the LiDAR odometer and the GPS / IMU integrated navigation system respectively obtain a large-scale trajectory, where the large-scale trajectory is a trajectory with a radius R greater than 500m; Step 2: Based on the LiDAR odometer trajectory, the GPS / IMU integrated navigation system trajectory is temporally interpolated to align the timestamps of the LiDAR odometer and the GPS / IMU integrated navigation system, thereby obtaining equivalent information of the GPS / IMU integrated navigation system. The equivalent information of the GPS / IMU integrated navigation system is then used to synchronize the large-scale trajectory of the LiDAR odometer and the GPS / IMU integrated navigation system. Step 3: Convert the position points on the two synchronized large-scale trajectories into two sets of point clouds, and solve the two sets of point clouds based on the normal distribution transformation and iterative nearest point point cloud matching algorithm to obtain the rotation extrinsic parameter R X ; Step 4: Determine whether the carrier is rotating according to the z-axis gyroscope data of the IMU in the GPS / IMU integrated navigation system, and remove the LiDAR point cloud data of non-rotational motion; Step 5: Remove abnormal points in the rotational LiDAR point cloud data, then calculate the smoothness of the points in the rotational LiDAR point cloud data, and extract feature points in the remaining point cloud according to the smoothness, wherein the abnormal points are parallel points and occlusion points, and the feature points are corner points and plane points; Step 6: Perform temporal interpolation on the trajectory of the GPS / IMU integrated navigation system to obtain the GPS / IMU data corresponding to each laser point, and use the GPS / IMU data and the external parameters to be determined Unify all laser points in the initial inertial navigation system; Step 7: In the initial inertial navigation system, a new objective function is constructed by combining the statistical error average effect and the displacement constraint, and based on the rotation external parameter R X , that is, the initial value of the rotation parameter Using nonlinear optimization method to solve The external parameters to be requested Step 8: Fit the feature points in the rotating LiDAR point cloud data to obtain the ground points, and then use the average z coordinates of all ground points as the height of the LiDAR base relative to the ground, and finally subtract the height from the designed installation height of the GPS / IMU integrated navigation system to obtain the translation parameter z'; Step 9: Exploitation And the translation parameter z' to obtain the vehicle sensor joint calibration parameter 2. A vehicle-mounted LiDAR-IMU external parameter joint calibration method according to claim 1, characterized in that The time interpolation is expressed as: scale=(t l -t i ) / (t i+1 -t i ) Among them, t i ,t i+1 and t l represents three different moments, and t i <t l <t i+1 , Represents LiDAR at t l The inertial navigation data corresponding to the time, and Represents t i and t i+1 The inertial navigation data at the moment, scale represents the scale factor, that is, the proportion of LiDAR time in the corresponding time of two adjacent inertial navigation data.

3. A vehicle-mounted LiDAR-IMU external reference joint calibration method according to claim 2, characterized in that The specific steps of step three are: Step 31: Convert the position points on the two large-scale trajectories into two sets of point clouds, and use one set of point clouds as the target point cloud and the other set of point clouds as the source point cloud; Step 32: Divide the space occupied by the target point cloud into voxels of fixed size, and calculate the multidimensional normal distribution mean μ and variance Σ of each voxel in the target point cloud, specifically: Where n is the number of voxel midpoints, x i is the coordinate of the i-th point in the voxel; Step 33: Convert the source point cloud into the voxel space of the target point cloud by rotating the external parameters; Step 3 and 4: Calculate the probability density S of each conversion point in the source point cloud according to the normal distribution parameters of the target point cloud, expressed as: Then the sum of the probability densities of all points in the source point cloud is calculated, and the objective function is established. The objective function is expressed as: Wherein, x is the coordinate of each point in the source point cloud, and u is the mean of the multidimensional normal distribution of each voxel in the target point cloud; Step 35: Use the Gauss-Newton method to optimize and solve the above objective function to maximize the value of Score, obtain the rotation extrinsic parameter, and use the rotation extrinsic parameter as the rotation extrinsic parameter in step 3, and repeat steps 33 to 35 until the convergence condition is reached to obtain the initial rotation extrinsic parameter; Step 36: Use the initial value rotation external parameter as the initial value R0 of the iterative nearest point solution parameter, perform a rotation transformation on the source point cloud, and obtain the nearest point of each point in the source point cloud in the target point cloud; Step 37: Match the points in the source point cloud with the nearest points of the points in the target point cloud to obtain matching point pairs in the source point cloud and the target point cloud; Step 38: Calculate the matrix H based on the matching point pairs in the source point cloud and the target point cloud: in, and represents the centroid of the source point cloud and the target point cloud, represents the i-th point in the source point cloud, represents the i-th point in the target point cloud, represents the centroid coordinates of the i-th point in the source point cloud, represents the centroid coordinates of the i-th point in the target point cloud; Step 39: Perform SVD decomposition on the matrix H to obtain the rotation external parameter R X , judge R X Whether it converges, if R X If it does not converge, jump to step 36. X Convergence ends.

4. A vehicle-mounted LiDAR-IMU external parameter joint calibration method according to claim 3, characterized in that The SVD decomposition of the matrix H is expressed as: H=UΣV T ,R * =R X =VU T Among them, U and V are orthogonal matrices, Σ is a singular value matrix, R * is the rotation external parameter R calculated in this cycle X .

5. A vehicle-mounted LiDAR-IMU external reference joint calibration method according to claim 4, characterized in that The specific steps of step 4 are: Step 41: Perform time interpolation based on the GPS / IMU combined navigation system trajectory to obtain the IMU gyroscope data corresponding to the starting time of each frame of LiDAR point cloud data, and then obtain the IMU gyroscope z-axis gyroscope data corresponding to the starting time of each frame of LiDAR trajectory; Step 42: Determine whether the carrier is rotating based on the z-axis gyroscope data, and remove the LiDAR point cloud data of non-rotational motion based on the determination result.

6. A vehicle-mounted LiDAR-IMU external reference joint calibration method according to claim 5, characterized in that In step 5, parallel points in the LiDAR point cloud data from which rotational motion is eliminated are expressed as: The occluded points in the LiDAR point cloud data after eliminating rotational motion are expressed as: r E -r D >β&|roll(r E )-roll(r D )|<10 Among them, r X Indicates the depth value of the laser point. AE corresponds to the serial number of the laser point in the figure. diff1 indicates the difference in depth value between point A and point B. diff2 indicates the difference in depth value between point B and point C. α and β are scale factors. roll() indicates the number of LiDAR lines where the laser point is located.

7. A vehicle-mounted LiDAR-IMU external reference joint calibration method according to claim 6, characterized in that The α=0.02, β=0.

3.

8. A vehicle-mounted LiDAR-IMU external parameter joint calibration method according to claim 7, characterized in that The smoothness C in step 5 is expressed as: in, It represents the coordinate value of the laser point on the k-axis of the LiDAR coordinate system, k = x, y, z, and q is the laser point serial number q = in, ... i-1, i, i+1, ..., i+n.

9. A vehicle-mounted LiDAR-IMU external reference joint calibration method according to claim 8, characterized in that The objective function in step seven is expressed as: Where s represents the number of LiDAR frames, Indicates the LiDAR position change in adjacent frames by Convert to the inertial navigation coordinate system, represents the combined navigation position change corresponding to the LiDAR position change, P i and P j They represent the i-th and j-th laser points in the laser point cloud after being unified into the initial inertial navigation system. By continuously iteratively optimizing the objective function, we can get 10. A vehicle-mounted LiDAR-IMU external parameter joint calibration method according to claim 9, characterized in that The height of the LiDAR base relative to the ground is expressed as: Among them, P g represents the coordinates of the gth ground point, z() represents the z-axis coordinate value of the ground point, and m represents the total number of ground points.

Images