A robust lidar-insulator calibration method

Abstract

The present invention discloses a robust laser radar-inertial navigation joint calibration method. First, the data collected by the inertial navigation and the laser radar are preprocessed respectively to obtain the initial pose estimation results of the inertial navigation and the laser radar respectively, and then the pose alignment is performed to obtain the initial external parameter matrix between the laser radar and the inertial navigation; then the data collected by the laser radar is dedistorted to obtain the dedistorted point cloud, and then the line feature point cloud is extracted from the dedistorted point cloud, and the surface map and the line feature map are constructed based on the dedistorted point cloud and the line feature point cloud respectively; finally, the dedistorted point cloud and the surface map, the line feature point cloud and the line feature map are iteratively optimized and registered, and the external parameters between the laser radar and the inertial navigation and the internal parameters of the inertial navigation are optimized and estimated. The present invention can effectively ensure the robustness of the laser radar-inertial navigation joint calibration method, realize high-precision parameter calibration in various scenarios, and provide a basis for multi-sensor fusion autonomous driving technology.

Description

Technical Field

[0001] The invention relates to a laser radar-inertial navigation calibration method in the field of multi-sensor fusion of intelligent vehicles, and in particular to a robust laser radar-inertial navigation calibration method. Background Art

[0002] With the rapid development of the field of autonomous driving, multi-sensor fusion technology has become a major trend. LiDAR can capture 3D information in the environment in real time, has high accuracy and field of view, and is one of the mainstream sensors for autonomous driving technology. Inertial navigation can obtain vehicle motion information in real time, has high positioning accuracy in a short period of time, and is not easily affected by the environment. It is an indispensable component of the current autonomous driving positioning module. The difficulty of current autonomous driving technology lies in the need to overcome challenges in various complex scenarios. The powerful environmental perception capability of LiDAR and the stability of inertial navigation in different scenarios form a good complement. The autonomous driving technology that integrates LiDAR and inertial navigation has become one of the current research hotspots.

[0003] The calibration method of LiDAR and INS needs to calibrate the external parameters such as translation and rotation between LiDAR and INS, as well as the internal parameters such as drift of INS accelerometer and gyroscope. As the most basic module of multi-sensor fusion technology, the calibration method needs to have high calibration accuracy and robustness. The current mainstream algorithm requires data collection in a specific structured scene and offline calibration to achieve a certain calibration effect, but it cannot complete the calibration task well for daily complex indoor environments or outdoor road environments. The current autonomous driving technology requires the calibration module to realize online high-precision calibration of LiDAR and INS in actual complex scenes. This requires the LiDAR-INS joint calibration method to always achieve high calibration accuracy in various complex scenes, and the overall process has good robustness. Therefore, the current research on the joint calibration method of LiDAR and INS is developing towards the trend of high precision and high robustness. Summary of the invention

[0004] In order to solve the problems existing in the background technology, the purpose of the present invention is to provide a robust lidar-inertial navigation joint calibration method, which is applicable to various environments, such as complex indoor environments and outdoor road environments.

[0005] The present invention introduces the translation vector between the laser radar and the inertial navigation and the gravity direction measurement value of the inertial navigation in the calibration initialization stage, which can effectively reduce the situation where abnormal values ​​appear in the external parameter matrix obtained by initialization estimation. In the optimization stage, the structural features in various environments are effectively extracted by constructing line feature residual terms and patch residual terms, so that the calibration method can operate in various environments. And through two-stage iterative optimization, the calibration parameters are continuously refined, so that in various scenarios, the optimal solution can be converged. The overall method has high robustness and lays the foundation for subsequent multi-sensor fusion autonomous driving technology.

[0006] The steps of the technical solution adopted by the present invention are as follows:

[0007] The present invention comprises the following steps:

[0008] 1) After preprocessing the data collected by the inertial navigation and the lidar, the initial pose estimation results of the inertial navigation and the lidar are obtained respectively, and then the pose estimation results of the inertial navigation and the lidar are aligned to obtain the initial external parameter matrix between the lidar and the inertial navigation;

[0009] 2) Based on the initial pose estimation result of the inertial navigation and the initial external parameter matrix between the lidar and the inertial navigation, the data collected by the lidar is dedistorted to obtain the dedistorted point cloud, and then the line feature point cloud is extracted from the dedistorted point cloud. Based on the dedistorted point cloud and the line feature point cloud, a patch map and a line feature map are constructed respectively;

[0010] 3) According to the initial pose estimation result of the INS and the initial external parameter matrix between the LiDAR and the INS, the dedistorted point cloud and patch map, as well as the line feature point cloud and line feature map are iteratively optimized and registered to optimize the external parameters between the LiDAR and the INS and the internal parameters of the INS.

[0011] The step 1) is specifically as follows:

[0012] 1.1) Perform pre-integration processing on the data collected by the inertial navigation to obtain the initial position and attitude estimation result of the inertial navigation;

[0013] 1.2) Use the normal distribution transform (NDT) algorithm to match the original point cloud collected by the lidar frame by frame to obtain the initial pose estimation result of the lidar;

[0014] 1.3) Measure and obtain the translation vector between the lidar and the inertial navigation system and the measurement value of the gravity direction of the inertial navigation system. Based on the translation vector between the lidar and the inertial navigation system and the measurement value of the gravity direction of the inertial navigation system, perform posture alignment according to the posture estimation results of the inertial navigation system and the lidar to obtain the initial external parameter matrix between the lidar and the inertial navigation system.

[0015] The calculation formula of the initial external parameter matrix between the laser radar and the inertial navigation in step 1.3) is as follows:

[0016]

[0017]

[0018]

[0019]

[0020] in, represents the initial external parameter matrix between the lidar and the inertial navigation, represents the residual error of the k-th frame pose alignment between the lidar and the inertial navigation, represents the residual between the estimated value and the measured value of the gravity direction of the kth frame inertial navigation, r t represents the residual error of the translation vector between the lidar and the inertial navigation, Represents the pose increment from the 0th frame to the kth frame in the initial pose estimation result of the lidar, K is the total number of frames, k is the frame number, Represents the pose increment from the 0th frame to the kth frame in the initial pose estimation result of the inertial navigation, () -1 represents the matrix inversion, and They represent the estimated values ​​of the flip angle and pitch angle of the kth frame of the inertial navigation, and They represent the measured values ​​of the flip angle and pitch angle of the kth frame of the inertial navigation system, represents the estimated value of the translation vector between the lidar and the inertial navigation, Represents the measured value of the translation vector between the lidar and the inertial navigation, || || 2 represents the binary norm operation, || represents the absolute value operation, and arg min{*} represents the minimum value operation.

[0021] In the step 2), the initial patch map and the initial line feature map are respectively constructed based on the dedistorted point cloud and the line feature point cloud, specifically:

[0022] S1: Use the NDT algorithm to match the dedistorted point cloud frame by frame, so as to update the initial pose estimation result of the lidar and obtain the updated pose estimation result of the lidar;

[0023] S2: Based on the updated pose estimation result of the LiDAR, the dedistorted point cloud is projected into the map coordinate system, and the plane structure is extracted based on the random sampling consistency RANSAC method to construct the initial patch map;

[0024] S3: Based on the updated pose estimation result of the lidar, the line feature point cloud is projected to the map coordinate system to construct an initial line feature map.

[0025] The step 3) is specifically as follows:

[0026] 3.1) According to the current external parameter matrix between the lidar and the inertial navigation system and the current pose estimation result of the inertial navigation system, the dedistorted point cloud is projected into the map coordinate system to obtain the dedistorted point cloud in the map coordinate system, and then the nearest neighbor search method is used to construct the matching pair relationship between the dedistorted point cloud in the map coordinate system and the current patch map, and then the current patch residual term is obtained based on the matching pair relationship;

[0027] 3.2) According to the current external parameter matrix between the lidar and the inertial navigation and the current pose estimation result of the inertial navigation, the line feature point cloud is projected into the map coordinate system to obtain the line feature point cloud in the map coordinate system, and then the nearest neighbor search method is used to construct the matching pair relationship between the line feature point cloud in the map coordinate system and the current line feature map, and then the current line feature residual term is obtained based on the matching pair relationship;

[0028] 3.3) Pre-integrate the data collected by the inertial navigation system based on the offset of the accelerometer and gyroscope of the inertial navigation system to obtain the current pre-integration increment of the inertial navigation system, and calculate the current pre-integration residual term of the inertial navigation system based on the current posture estimation result of the inertial navigation system and the current pre-integration increment of the inertial navigation system;

[0029] 3.4) Based on the current patch residual term and the inertial navigation pre-integrated residual term, the current calibration parameters are estimated using the optimization algorithm of the first stage. The calibration parameters include the external parameter matrix between the lidar and the inertial navigation The position increment of each frame of the inertial navigation relative to the 0th frame, the time delay Δt between the lidar and the inertial navigation, and the offset b of the accelerometer of the inertial navigation a And the offset b of the inertial navigation gyroscope ω ;

[0030]

[0031]

[0032]

[0033] Among them, x represents the calibration parameters, including the external parameter matrix between the lidar and the inertial navigation The pose increment from frame 0 to frame k in the updated pose estimation result of the inertial navigation The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω And the time delay Δt between the lidar and the inertial navigation, K is the total number of frames, k is the frame number, Represents the current residuals of each patch in the kth frame, represents the current inertial navigation pre-integration residual term of the kth frame; t k represents the time of the kth frame, t k +Δt time pre-integrated function, a k represents the accelerometer measurement value of the kth frame of the inertial navigation, ω k represents the gyro measurement value of the kth frame of the inertial navigation, b a and b ω are the offsets of the inertial navigation accelerometer and gyroscope measurements, respectively;

[0034] 3.5) According to the external parameter matrix between the laser radar and the inertial navigation estimated in step 3.4) The pose increment of each frame of the INS relative to the 0th frame and the time delay Δt between the LiDAR and the INS are used to update the pose estimation result of the INS. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the data collected by the LiDAR are dedistorted to obtain an updated dedistorted point cloud. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the updated pose estimation result of the LiDAR is obtained, and then the line feature map and the patch map are updated.

[0035] 3.6) Repeat steps 3.1) and steps 3.3) to 3.5) to perform the first stage of iterative optimization until convergence. The convergence condition is the external parameter matrix between the laser radar and the inertial navigation. The pose increment from frame 0 to frame k in the pose estimation result after inertial navigation update The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω and the difference between the time delay Δt between the laser radar and the inertial navigation system after the update and before the update is less than the corresponding preset threshold of the first stage;

[0036] 3.7) Based on the current line feature residual term, patch residual term and inertial navigation pre-integration residual term, the current calibration parameters are estimated using the second stage optimization algorithm;

[0037]

[0038] in, Represents the current line feature residuals of the k-th frame;

[0039] 3.8) According to the external parameter matrix between the laser radar and the inertial navigation estimated in step 3.7) The pose increment of each frame of the INS relative to the 0th frame and the time delay Δt between the LiDAR and the INS are used to update the pose estimation result of the INS. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the data collected by the LiDAR are dedistorted to obtain an updated dedistorted point cloud. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the updated pose estimation result of the LiDAR is obtained, and then the line feature map and the patch map are updated.

[0040] 3.9) Repeat steps 3.1) to 3.3) and step 3.7) to perform the second stage of iterative optimization until convergence. The convergence condition is the external parameter matrix between the laser radar and the inertial navigation. The pose increment from frame 0 to frame k in the pose estimation result after inertial navigation update The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω The difference between the time delay Δt between the laser radar and the inertial navigation system after the update and before the update is less than the corresponding preset threshold of the second stage; the external parameter matrix between the final laser radar and the inertial navigation system The time delay Δt constitutes the external parameter between the laser radar and the inertial navigation, and the final offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω It constitutes the internal parameters of inertial navigation.

[0041] The step 3.1) is specifically as follows:

[0042] 3.1.1) According to the current external parameter matrix between the laser radar and the inertial navigation Project the current dedistorted point cloud in the local coordinate system of each frame of the lidar to the local coordinate system of the corresponding frame of the inertial navigation system to obtain the dedistorted point cloud in the local coordinate system of each frame of the inertial navigation system;

[0043] 3.1.2) According to the current pose estimation result of the INS, the dedistorted point cloud in the local coordinate system of each frame of the INS is projected into the global coordinate system of the INS to obtain the dedistorted point cloud in the global coordinate system of the INS;

[0044] 3.1.3) According to the current external parameter matrix between the laser radar and the inertial navigation The dedistorted point cloud in the inertial navigation global coordinate system is projected back to the map coordinate system to obtain the dedistorted point cloud in the map coordinate system.

[0045] 3.1.4) The current patch map is divided into multiple square grids of equal size, each square grid includes multiple patches, each point in the dedistorted point cloud in the map coordinate system is projected into a square grid of the current patch map, and the point-to-surface distance between the current point and each patch in the current square grid is calculated. If the minimum point-to-surface distance is less than the preset point-to-surface distance threshold, the patch corresponding to the minimum point-to-surface distance and the current point are taken as a pair of matching pairs, and the point-to-surface distance between the current point and the current patch is taken as the point-to-surface distance residual of the current point;

[0046] 3.1.5) Traverse the remaining points in the dedistorted point cloud in the map coordinate system, repeat step 3.1.4) to perform point-to-surface matching on the remaining points, and finally obtain the matching pair relationship between the dedistorted point cloud in the map coordinate system and the current patch map and the point-to-surface distance residuals of all points. The point-to-surface distance residuals of all points constitute the current patch residual term.

[0047] The step 3.2) is specifically as follows:

[0048] 3.2.1) According to the current external parameter matrix between the laser radar and the inertial navigation Project the current line feature point cloud in the local coordinate system of each frame of the laser radar to the local coordinate system corresponding to the inertial navigation system to obtain the line feature point cloud in the local coordinate system of each frame of the inertial navigation system;

[0049] 3.2.2) According to the current pose estimation result of the INS, the line feature point cloud in the local coordinate system of each frame of the INS is projected into the global coordinate system of the INS to obtain the line feature point cloud in the global coordinate system of the INS;

[0050] 3.2.3) According to the current external parameter matrix between the laser radar and the inertial navigation Project the line feature point cloud in the inertial navigation global coordinate system back to the map coordinate system to obtain the line feature point cloud in the map coordinate system;

[0051] 3.2.4) For each point in the line feature point cloud in the map coordinate system, a point in the current line feature map corresponding to the point is used as the target point, and several points in the current line feature map closest to the current target point are found and a straight line is fitted to the several points closest to the current target point; if a straight line is fitted and the distance from the current point in the line feature point cloud in the map coordinate system to the straight line is less than a preset point-line distance threshold, the current point and the straight line are taken as a pair of matching pairs, and the distance from the current point in the line feature point cloud in the map coordinate system to the straight line is taken as the point-line distance residual of the current point;

[0052] 3.2.5) Traverse the remaining points in the line feature point cloud in the map coordinate system, repeat step 3.2.4) to perform point-line matching on the remaining points, and finally obtain the matching pair relationship between the line feature point cloud in the map coordinate system and the current line feature map and the point-line distance residuals of all points. The point-line distance residuals of all points constitute the current line feature residual term.

[0053] Compared with the background technology, the present invention has the following beneficial effects:

[0054] (1) The present invention uses an optimization algorithm based on the translation vector between the laser radar and the inertial navigation system and the gravity direction measurement value of the inertial navigation system during the initialization phase, which can improve the accuracy of initialization, reduce the number of initialization failures, and accelerate the convergence of the calibration method;

[0055] (2) The present invention uses line features and patch matching pairs to iteratively optimize and optimize calibration parameters, so that the entire calibration method can be run in most environments and can achieve laser radar-inertial navigation calibration tasks in complex indoor environments and outdoor road environments;

[0056] (3) The present invention uses a two-stage iterative optimization process. The iterative optimization in the first stage optimizes the calibration parameters based on the patch residual term and the inertial navigation pre-integration residual term. The iterative optimization in the second stage optimizes the calibration parameters based on the line feature residual term, the patch residual term and the inertial navigation pre-integration residual term. This two-stage iterative optimization method can make the calibration method converge better, achieve higher calibration accuracy in most scenarios, have higher robustness, and ultimately improve the accuracy and robustness of the calibration method. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 It is a flow chart of the method of the present invention.

[0058] Figure 2 is a line feature map of an embodiment;

[0059] Figure 3 is a global map of the embodiment;

[0060] Figure 4 is a patch map of an embodiment;

[0061] Figure 5 It is the optimization result of the first to third rounds of the embodiment;

[0062] Figure 6 It is the optimization result of the 4th to 6th rounds of the embodiment;

[0063] Figure 7 It is the optimization result of the 7th to 9th rounds of the embodiment;

[0064] Figure 8is the optimization result of the 10th to 12th rounds of the embodiment;

[0065] Fig. 9 It is the optimization result of the 13th to 15th rounds of the embodiment. DETAILED DESCRIPTION

[0066] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0067] The calibration method of the present invention is more clearly described below with an example.

[0068] The embodiment and implementation process of the complete method according to the invention content of the present invention are as follows:

[0069] like Figure 1 As shown, the present invention comprises the following steps:

[0070] 1) After preprocessing the data collected by the inertial navigation and the lidar, the initial pose estimation results of the inertial navigation and the lidar are obtained respectively, and then the pose estimation results of the inertial navigation and the lidar are aligned to obtain the initial external parameter matrix between the lidar and the inertial navigation;

[0071] Step 1) is specifically as follows:

[0072] 1.1) Perform pre-integration processing on the data collected by the inertial navigation to obtain the initial position and attitude estimation result of the inertial navigation;

[0073] 1.2) Use the Normal Distribution Transform (NDT) algorithm to match the original point cloud collected by the LiDAR frame by frame to obtain the initial pose estimation result of the LiDAR;

[0074] 1.3) Measure and obtain the translation vector between the lidar and the inertial navigation system and the measurement value of the gravity direction of the inertial navigation system. Based on the translation vector between the lidar and the inertial navigation system and the measurement value of the gravity direction of the inertial navigation system, perform posture alignment according to the posture estimation results of the inertial navigation system and the lidar to obtain the initial external parameter matrix between the lidar and the inertial navigation system.

[0075] The external parameter matrix is ​​mainly divided into two parts: rotation matrix and translation vector The decomposition formula is as follows:

[0076]

[0077] Considering the initialization phase, the initial pose estimation result of the lidar estimated in step 1.1) is The initial pose estimation result of the inertial navigation obtained by step 1.2) There will be a certain error, and the initial external parameter matrix can be estimated directly by solving the formula There is a certain probability that outliers will appear, causing initialization failure.

[0078] Therefore, the following method is adopted to perform posture alignment optimization correction. The calculation formula of the initial external parameter matrix between the lidar and the inertial navigation in step 1.3) is as follows:

[0079]

[0080]

[0081]

[0082]

[0083] in, represents the initial external parameter matrix between the lidar and the inertial navigation, represents the residual error of the k-th frame pose alignment between the lidar and the inertial navigation, represents the residual between the estimated value and the measured value of the gravity direction of the kth frame inertial navigation, r t represents the residual error of the translation vector between the lidar and the inertial navigation, Represents the pose increment from the 0th frame to the kth frame in the initial pose estimation result of the lidar, K is the total number of frames, k is the frame number, Represents the pose increment from the 0th frame to the kth frame in the initial pose estimation result of the inertial navigation, () -1 represents the matrix inversion, and They represent the estimated values ​​of the roll and pitch angles of the kth frame of the inertial navigation system, and The pose increment from the 0th frame to the kth frame in the initial pose estimation result of the inertial navigation Decompose to obtain, and They represent the measured values ​​of the roll angle and pitch angle of the kth frame of the inertial navigation system. represents the estimated value of the translation vector between the lidar and the inertial navigation, Represents the measured value of the translation vector between the lidar and the inertial navigation, || || 2 represents the binary norm operation, || represents the absolute value operation, and arg min{*} represents the minimum value operation.

[0084] Among them, according to the measured translation vector between the lidar and the inertial navigation, the estimated value of the translation vector between the lidar and the inertial navigation is corrected; according to the measured value of the gravity direction of the inertial navigation, the pose increment of each frame relative to the 0th frame in the initial pose estimation result of the inertial navigation is corrected.

[0085] 2) Based on the initial pose estimation result of the inertial navigation and the initial external parameter matrix between the lidar and the inertial navigation, the data collected by the lidar is dedistorted to obtain the dedistorted point cloud, and then the line feature point cloud is extracted from the dedistorted point cloud. Based on the dedistorted point cloud and the line feature point cloud, a patch map and a line feature map are constructed respectively;

[0086] In step 2), the initial patch map and the initial line feature map are constructed based on the dedistorted point cloud and line feature point cloud, respectively, as follows:

[0087] S1: Use the NDT algorithm to match the dedistorted point cloud frame by frame, so as to update the initial pose estimation result of the lidar and obtain the updated pose estimation result of the lidar; due to the dedistortion processing, the accuracy of the pose estimation result estimated here is higher than the accuracy of the initial pose estimation result in step 1.2).

[0088] S2: Based on the updated pose estimation results of the LiDAR, the dedistorted point cloud is projected into the map coordinate system to construct a global map, such as Figure 3 As shown, the plane structure is extracted based on the random sampling consistency RANSAC method (Random Sample Consensus), thereby constructing the initial patch map, as shown in Figure 4 As shown;

[0089] S3: According to the updated pose estimation result of the laser radar, the line feature point cloud is projected to the map coordinate system. In the specific implementation, the map coordinate system is the local coordinate system of the laser radar at the 0th frame, so as to construct the initial line feature map, such as Figure 2 shown.

[0090] 3) According to the initial pose estimation result of the INS and the initial external parameter matrix between the LiDAR and the INS, the dedistorted point cloud and patch map, as well as the line feature point cloud and line feature map are iteratively optimized and registered to optimize the external parameters between the LiDAR and the INS and the internal parameters of the INS.

[0091] Step 3) is specifically:

[0092] 3.1) According to the current external parameter matrix between the lidar and the inertial navigation system and the current pose estimation result of the inertial navigation system, the dedistorted point cloud is projected into the map coordinate system to obtain the dedistorted point cloud in the map coordinate system, and then the nearest neighbor search method is used to construct the matching pair relationship between the dedistorted point cloud in the map coordinate system and the current patch map, and then the current patch residual term is obtained based on the matching pair relationship;

[0093] Step 3.1) is specifically:

[0094] 3.1.1) According to the current external parameter matrix between the laser radar and the inertial navigation Project the current dedistorted point cloud in the local coordinate system of each frame of the lidar to the local coordinate system of the corresponding frame of the inertial navigation system to obtain the dedistorted point cloud in the local coordinate system of each frame of the inertial navigation system;

[0095] 3.1.2) According to the current pose estimation result of the INS, the dedistorted point cloud in the local coordinate system of each frame of the INS is projected into the global coordinate system of the INS to obtain the dedistorted point cloud in the global coordinate system of the INS;

[0096] 3.1.3) According to the current external parameter matrix between the laser radar and the inertial navigation The dedistorted point cloud in the inertial navigation global coordinate system is projected back to the map coordinate system to obtain the dedistorted point cloud in the map coordinate system. The dedistorted point cloud in the map coordinate system is in the same coordinate system as the line feature map and the patch map constructed in step S2 and step S3, respectively.

[0097] 3.1.4) The current patch map is divided into multiple square grids of equal size, each square grid includes multiple patches, each point in the dedistorted point cloud in the map coordinate system is projected into a square grid of the current patch map, and the point-to-surface distance between the current point and each patch in the current square grid is calculated. If the minimum point-to-surface distance is less than the preset point-to-surface distance threshold, the patch corresponding to the minimum point-to-surface distance and the current point are taken as a pair of matching pairs, and the point-to-surface distance between the current point and the current patch (i.e., the minimum point-to-surface distance) is taken as the point-to-surface distance residual of the current point, otherwise there is no matching patch for the current point in the current patch map;

[0098] 3.1.5) Traverse the remaining points in the dedistorted point cloud in the map coordinate system, repeat step 3.1.4) to perform point-to-surface matching on the remaining points, and finally obtain the matching pair relationship between the dedistorted point cloud in the map coordinate system and the current patch map and the point-to-surface distance residuals of all points. The point-to-surface distance residuals of all points constitute the current patch residual term;

[0099] 3.2) According to the current external parameter matrix between the lidar and the inertial navigation and the current pose estimation result of the inertial navigation, the line feature point cloud is projected into the map coordinate system to obtain the line feature point cloud in the map coordinate system, and then the nearest neighbor search method is used to construct the matching pair relationship between the line feature point cloud in the map coordinate system and the current line feature map, and then the current line feature residual term is obtained based on the matching pair relationship;

[0100] Step 3.2) is specifically:

[0101] 3.2.1) According to the current external parameter matrix between the laser radar and the inertial navigation Project the current line feature point cloud in the local coordinate system of each frame of the laser radar to the local coordinate system corresponding to the inertial navigation system to obtain the line feature point cloud in the local coordinate system of each frame of the inertial navigation system;

[0102] 3.2.2) According to the current pose estimation result of the INS, the line feature point cloud in the local coordinate system of each frame of the INS is projected into the global coordinate system of the INS to obtain the line feature point cloud in the global coordinate system of the INS;

[0103] 3.2.3) According to the current external parameter matrix between the laser radar and the inertial navigation The line feature point cloud in the inertial navigation global coordinate system is projected back to the map coordinate system to obtain the line feature point cloud in the map coordinate system. The line feature point cloud in the map coordinate system is in the same coordinate system as the line feature map and the patch map constructed in step S2 and step S3 respectively.

[0104] 3.2.4) In the specific implementation, the line feature map is stored in the form of a k-dimensional tree. For each point in the line feature point cloud under the map coordinate system, a point corresponding to the current line feature map is used as the target point, and several points in the current line feature map that are closest to the current target point are found and a straight line is fitted to the several points closest to the current target point; if a straight line is fitted and the distance from the current point in the line feature point cloud under the map coordinate system to the straight line is less than the preset point-line distance threshold, the current point and the straight line are taken as a pair of matching pairs, and the distance from the current point in the line feature point cloud under the map coordinate system to the straight line is taken as the point-line distance residual of the current point, otherwise there is no matching straight line for the current point in the line feature map;

[0105] 3.2.5) Traverse the remaining points in the line feature point cloud in the map coordinate system, repeat step 3.2.4) to perform point-line matching on the remaining points, and finally obtain the matching pair relationship between the line feature point cloud in the map coordinate system and the current line feature map and the point-line distance residuals of all points. The point-line distance residuals of all points constitute the current line feature residual term;

[0106] 3.3) Pre-integrate the data collected by the inertial navigation based on the offset of the accelerometer and gyroscope of the inertial navigation, the initial value of the offset of the accelerometer and gyroscope of the inertial navigation is 0, obtain the current pre-integration increment of the inertial navigation, and calculate the current pre-integration residual term of the inertial navigation based on the current posture estimation result of the inertial navigation and the current pre-integration increment of the inertial navigation;

[0107] 3.4) Based on the current patch residual term and the inertial navigation pre-integrated residual term, the current calibration parameters are estimated using the optimization algorithm of the first stage. The calibration parameters include the external parameter matrix between the lidar and the inertial navigation The position increment of each frame of the inertial navigation relative to the 0th frame, the time delay Δt between the lidar and the inertial navigation, and the offset b of the accelerometer of the inertial navigation a And the offset b of the inertial navigation gyroscope ω ;

[0108]

[0109]

[0110]

[0111] Among them, x represents the calibration parameters, including the external parameter matrix between the lidar and the inertial navigation The pose increment from frame 0 to frame k in the updated pose estimation result of the inertial navigation The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω And the time delay Δt between the lidar and the inertial navigation, K is the total number of frames, k is the frame number, Represents the current residuals of each patch in the kth frame, represents the current inertial navigation pre-integration residual term of the kth frame; t k represents the time of the kth frame, t k +Δt time, the output result is the position increment of the inertial navigation relative to the 0th frame at the time of the kth frame plus the time delay Δt, a k represents the accelerometer measurement value of the kth frame of the inertial navigation, ω k represents the gyro measurement value of the kth frame of the inertial navigation, b a and b ω are the offsets of the inertial navigation accelerometer and gyroscope measurements, respectively;

[0112] 3.5) According to the external parameter matrix between the laser radar and the inertial navigation estimated in step 3.4) The pose estimation result of the inertial navigation is updated based on the pose increment of each frame of the inertial navigation relative to the 0th frame and the time delay Δt between the lidar and the inertial navigation. Then, based on the updated pose estimation result of the inertial navigation and the updated extrinsic parameter matrix between the lidar and the inertial navigation, the data collected by the lidar is dedistorted to obtain an updated dedistorted point cloud. Then, based on the updated pose estimation result of the inertial navigation and the updated extrinsic parameter matrix between the lidar and the inertial navigation, the updated pose estimation result of the lidar is obtained, and then the line feature map and the patch map are updated. The updated line feature map and the patch map have higher accuracy.

[0113] 3.6) Repeat steps 3.1) and steps 3.3) to 3.5) to perform the first stage of iterative optimization until convergence. The convergence condition is the external parameter matrix between the laser radar and the inertial navigation. The pose increment from frame 0 to frame k in the pose estimation result after inertial navigation update The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω and the difference between the time delay Δt between the laser radar and the inertial navigation system after the update and before the update is less than the corresponding preset threshold of the first stage;

[0114] 3.7) Based on the current line feature residual term, patch residual term and inertial navigation pre-integration residual term, the current calibration parameters are estimated using the second stage optimization algorithm;

[0115]

[0116] in, Represents the current line feature residuals of the k-th frame;

[0117] 3.8) According to the external parameter matrix between the laser radar and the inertial navigation estimated in step 3.7) The pose estimation result of the inertial navigation is updated based on the pose increment of each frame of the inertial navigation relative to the 0th frame and the time delay Δt between the lidar and the inertial navigation. Then, based on the updated pose estimation result of the inertial navigation and the updated extrinsic parameter matrix between the lidar and the inertial navigation, the data collected by the lidar is dedistorted to obtain an updated dedistorted point cloud. Then, based on the updated pose estimation result of the inertial navigation and the updated extrinsic parameter matrix between the lidar and the inertial navigation, the updated pose estimation result of the lidar is obtained, and then the line feature map and the patch map are updated. The updated line feature map and the patch map have higher accuracy.

[0118] 3.9) Repeat steps 3.1) to 3.3) and step 3.7) to perform the second stage of iterative optimization until convergence. The convergence condition is the external parameter matrix between the laser radar and the inertial navigation. The pose increment from frame 0 to frame k in the pose estimation result after inertial navigation update The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω The difference between the time delay Δt between the laser radar and the inertial navigation system after the update and before the update is less than the corresponding preset threshold of the second stage; the external parameter matrix between the final laser radar and the inertial navigation system The time delay Δt constitutes the external parameter between the laser radar and the inertial navigation, and the final offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω It constitutes the internal parameters of inertial navigation.

[0119] Figures 5 to 9 The visualization effect of the global point cloud map constructed by each round of optimization is shown as the iteration converges. As the intermediate output of the calibration optimization algorithm, the point cloud map can qualitatively reflect the accuracy of the intermediate results in the calibration process. Figure 6 The fifth round is the result after the iterative optimization phase 1 converges. It can be found that the global map accuracy is low, and the optimal calibration solution is not reached at this time. By adding the line feature residual term to continue the optimization in phase 2, it can further converge and obtain the final optimal calibration result.

[0120] The effect of this method is verified by collecting data from complex indoor environments. The sensors used include Velodyne VLP-16 series lidar and MEMSIC VG800 series inertial navigation. The two sensors are fixed on the same platform in a rigid connection. By rotating the platform by hand, about 1 minute of data is collected and stored in the form of rosbag for offline calibration. Among them, the frequency of lidar data is 10Hz, and the frequency of inertial navigation data is 100Hz. The inertial navigation outputs eight-axis data, including three-axis accelerometer data, three-axis gyroscope data, and two-axis gravity direction data. The entire data set contains 10 sequences, which are data collected in the same complex indoor environment. The effect of this method is evaluated by calibrating the 10 sequences and calculating the mean and standard deviation of the calibration results.

[0121] Table 1 lists the comparison of the calibration results of this method and other calibration methods (LI-Calib) under the indoor complex environment dataset. Each column of the table is the six-axis component of the calibration extrinsic parameter matrix, and the calibration extrinsic parameters of LI-Calib and this method in 10 sequences under the indoor complex environment dataset are counted, where the results are listed in the form of mean ± standard deviation. Since the true value of the calibration extrinsic parameters between the lidar and the inertial navigation is difficult to obtain, the standard deviation of the calibration results of multiple sequences in the same scene is usually used as an evaluation criterion to indirectly reflect the accuracy and robustness of the calibration method. As can be seen from the table, the standard deviation of the calibration results of this method has reached the millimeter level, which is much smaller than the results of LI-Calib, which can reflect that this method has good calibration performance.

[0122] Table 1 Calibration results of this method and other methods

[0123] x(m) y(m) z(m) roll(°) pitch(°) yaw(°) LI-Calib 0.0645±0.0147 0.0442±0.0183 -0.2457±0.0266 179.43±0.78 6.37±0.76 2.90±0.44 This method 0.0710±0.0032 0.0428±0.0014 -0.2579±0.0021 179.51±0.06 6.18±0.02 2.69±0.05

Claims

1. A robust laser radar-inertial navigation joint calibration method, characterized in that: The steps include: 1) After preprocessing the data collected by the inertial navigation and the lidar, the initial pose estimation results of the inertial navigation and the lidar are obtained respectively, and then the pose estimation results of the inertial navigation and the lidar are aligned to obtain the initial external parameter matrix between the lidar and the inertial navigation; 2) Based on the initial pose estimation result of the inertial navigation and the initial external parameter matrix between the lidar and the inertial navigation, the data collected by the lidar is dedistorted to obtain the dedistorted point cloud, and then the line feature point cloud is extracted from the dedistorted point cloud. Based on the dedistorted point cloud and the line feature point cloud, a patch map and a line feature map are constructed respectively; 3) According to the initial pose estimation result of the INS and the initial external parameter matrix between the LiDAR and the INS, the dedistorted point cloud and the patch map, the line feature point cloud and the line feature map are iteratively optimized and registered, and the external parameters between the LiDAR and the INS and the internal parameters of the INS are optimized and estimated; The step 3) is specifically as follows: 3.1) According to the current external parameter matrix between the lidar and the inertial navigation system and the current pose estimation result of the inertial navigation system, the dedistorted point cloud is projected into the map coordinate system to obtain the dedistorted point cloud in the map coordinate system, and then the nearest neighbor search method is used to construct the matching pair relationship between the dedistorted point cloud in the map coordinate system and the current patch map, and then the current patch residual term is obtained based on the matching pair relationship; 3.2) According to the current external parameter matrix between the lidar and the inertial navigation and the current pose estimation result of the inertial navigation, the line feature point cloud is projected into the map coordinate system to obtain the line feature point cloud in the map coordinate system, and then the nearest neighbor search method is used to construct the matching pair relationship between the line feature point cloud in the map coordinate system and the current line feature map, and then the current line feature residual term is obtained based on the matching pair relationship; 3.3) Pre-integrate the data collected by the inertial navigation system based on the offset of the accelerometer and gyroscope of the inertial navigation system to obtain the current pre-integration increment of the inertial navigation system, and calculate the current pre-integration residual term of the inertial navigation system based on the current posture estimation result of the inertial navigation system and the current pre-integration increment of the inertial navigation system; 3.4) Based on the current patch residual term and the inertial navigation pre-integrated residual term, the current calibration parameters are estimated using the optimization algorithm of the first stage. The calibration parameters include the external parameter matrix between the lidar and the inertial navigation The position increment of each frame of the inertial navigation relative to the 0th frame, the time delay Δt between the lidar and the inertial navigation, and the offset b of the accelerometer of the inertial navigation a And the offset b of the inertial navigation gyroscope ω ; Among them, x represents the calibration parameters, including the external parameter matrix between the lidar and the inertial navigation The pose increment from frame 0 to frame k in the updated pose estimation result of the inertial navigation The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω And the time delay Δt between the lidar and the inertial navigation, K is the total number of frames, k is the frame number, Represents the current residuals of each patch in the kth frame, represents the current inertial navigation pre-integration residual term of the kth frame; t k represents the time of the kth frame, t k +Δt time pre-integrated function, a k represents the accelerometer measurement value of the kth frame of the inertial navigation, ω k represents the gyro measurement value of the kth frame of the inertial navigation, b a and b ω are the offsets of the inertial navigation accelerometer and gyroscope measurements, respectively; 3.5) According to the external parameter matrix between the laser radar and the inertial navigation estimated in step 3.4) The pose increment of each frame of the INS relative to the 0th frame and the time delay Δt between the LiDAR and the INS are used to update the pose estimation result of the INS. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the data collected by the LiDAR are dedistorted to obtain an updated dedistorted point cloud. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the updated pose estimation result of the LiDAR is obtained, and then the line feature map and the patch map are updated. 3.6) Repeat steps 3.1) and steps 3.3) to 3.5) to perform the first stage of iterative optimization until convergence. The convergence condition is the external parameter matrix between the laser radar and the inertial navigation. The pose increment from frame 0 to frame k in the pose estimation result after the inertial navigation update The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω and the difference between the time delay Δt between the laser radar and the inertial navigation system after the update and before the update is less than the corresponding preset threshold of the first stage; 3.7) Based on the current line feature residual term, patch residual term and inertial navigation pre-integration residual term, the current calibration parameters are estimated using the second stage optimization algorithm; in, Represents the current line feature residuals of the k-th frame; 3.8) According to the external parameter matrix between the laser radar and the inertial navigation estimated in step 3.7) The pose increment of each frame of the INS relative to the 0th frame and the time delay Δt between the LiDAR and the INS are used to update the pose estimation result of the INS. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the data collected by the LiDAR are dedistorted to obtain an updated dedistorted point cloud. Then, based on the updated pose estimation result of the INS and the updated external parameter matrix between the LiDAR and the INS, the updated pose estimation result of the LiDAR is obtained, and then the line feature map and the patch map are updated. 3.9) Repeat steps 3.1) to 3.3) and step 3.7) to perform the second stage of iterative optimization until convergence. The convergence condition is the external parameter matrix between the laser radar and the inertial navigation. The pose increment from frame 0 to frame k in the pose estimation result after the inertial navigation update The offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω The difference between the time delay Δt between the laser radar and the inertial navigation system after the update and before the update is less than the corresponding preset threshold of the second stage; the external parameter matrix between the final laser radar and the inertial navigation system The time delay Δt constitutes the external parameters between the lidar and the inertial navigation system, and the final offset b of the inertial navigation accelerometer a , the offset b of the inertial navigation gyroscope ω It constitutes the internal parameters of inertial navigation.

2. A robust laser radar-inertial navigation joint calibration method according to claim 1, characterized in that: The step 1) is specifically as follows: 1.1) Perform pre-integration processing on the data collected by the inertial navigation to obtain the initial position and attitude estimation result of the inertial navigation; 1.2) Use the normal distribution transform (NDT) algorithm to match the original point cloud collected by the lidar frame by frame to obtain the initial pose estimation result of the lidar; 1.3) Measure and obtain the translation vector between the lidar and the inertial navigation system and the measurement value of the gravity direction of the inertial navigation system. Based on the translation vector between the lidar and the inertial navigation system and the measurement value of the gravity direction of the inertial navigation system, perform posture alignment according to the posture estimation results of the inertial navigation system and the lidar to obtain the initial external parameter matrix between the lidar and the inertial navigation system.

3. A robust laser radar-inertial navigation joint calibration method according to claim 2, characterized in that: The calculation formula of the initial external parameter matrix between the laser radar and the inertial navigation in step 1.3) is as follows: in, represents the initial external parameter matrix between the lidar and the inertial navigation, represents the residual error of the k-th frame pose alignment between the lidar and the inertial navigation, represents the residual between the estimated value and the measured value of the gravity direction of the kth frame inertial navigation, r t represents the residual error of the translation vector between the lidar and the inertial navigation, Represents the pose increment from the 0th frame to the kth frame in the initial pose estimation result of the lidar, K is the total number of frames, k is the frame number, Represents the pose increment from the 0th frame to the kth frame in the initial pose estimation result of the inertial navigation, () -1 represents the matrix inversion, and They represent the estimated values ​​of the flip angle and pitch angle of the kth frame of the inertial navigation, and They represent the measured values ​​of the flip angle and pitch angle of the kth frame of the inertial navigation system, represents the estimated value of the translation vector between the lidar and the inertial navigation, Represents the measured value of the translation vector between the lidar and the inertial navigation, ‖‖ 2 represents the binary norm operation, || represents the absolute value operation, and arg min{*} represents the minimum value operation.

4. The robust laser radar-inertial navigation joint calibration method according to claim 1, characterized in that: In the step 2), the initial patch map and the initial line feature map are respectively constructed based on the dedistorted point cloud and the line feature point cloud, specifically: S1: Use the NDT algorithm to match the dedistorted point cloud frame by frame, so as to update the initial pose estimation result of the lidar and obtain the updated pose estimation result of the lidar; S2: Based on the updated pose estimation result of the LiDAR, the dedistorted point cloud is projected into the map coordinate system, and the plane structure is extracted based on the random sampling consistency RANSAC method to construct the initial patch map; S3: Based on the updated pose estimation result of the lidar, the line feature point cloud is projected to the map coordinate system to construct an initial line feature map.

5. The robust laser radar-inertial navigation joint calibration method according to claim 1, characterized in that: The step 3.1) is specifically as follows: 3.1.1) According to the current external parameter matrix between the laser radar and the inertial navigation Project the current dedistorted point cloud in the local coordinate system of each frame of the lidar to the local coordinate system of the corresponding frame of the inertial navigation system to obtain the dedistorted point cloud in the local coordinate system of each frame of the inertial navigation system; 3.1.2) According to the current pose estimation result of the INS, the dedistorted point cloud in the local coordinate system of each frame of the INS is projected into the global coordinate system of the INS to obtain the dedistorted point cloud in the global coordinate system of the INS; 3.1.3) According to the current external parameter matrix between the laser radar and the inertial navigation Project the dedistorted point cloud in the inertial navigation global coordinate system back to the map coordinate system to obtain the dedistorted point cloud in the map coordinate system; 3.1.4) The current patch map is divided into multiple square grids of equal size, each square grid includes multiple patches, each point in the dedistorted point cloud in the map coordinate system is projected into a square grid of the current patch map, and the point-to-surface distance between the current point and each patch in the current square grid is calculated. If the minimum point-to-surface distance is less than the preset point-to-surface distance threshold, the patch corresponding to the minimum point-to-surface distance and the current point are taken as a pair of matching pairs, and the point-to-surface distance between the current point and the current patch is taken as the point-to-surface distance residual of the current point; 3.1.5) Traverse the remaining points in the dedistorted point cloud in the map coordinate system, repeat step 3.1.4) to perform point-to-surface matching on the remaining points, and finally obtain the matching pair relationship between the dedistorted point cloud in the map coordinate system and the current patch map and the point-to-surface distance residuals of all points. The point-to-surface distance residuals of all points constitute the current patch residual term.

6. The robust laser radar-inertial navigation joint calibration method according to claim 1, characterized in that: The step 3.2) is specifically as follows: 3.2.1) According to the current external parameter matrix between the laser radar and the inertial navigation Project the current line feature point cloud in the local coordinate system of each frame of the laser radar to the local coordinate system corresponding to the inertial navigation system to obtain the line feature point cloud in the local coordinate system of each frame of the inertial navigation system; 3.2.2) According to the current pose estimation result of the INS, the line feature point cloud in the local coordinate system of each frame of the INS is projected into the global coordinate system of the INS to obtain the line feature point cloud in the global coordinate system of the INS; 3.2.3) According to the current external parameter matrix between the laser radar and the inertial navigation Project the line feature point cloud in the inertial navigation global coordinate system back to the map coordinate system to obtain the line feature point cloud in the map coordinate system; 3.2.4) For each point in the line feature point cloud in the map coordinate system, a point in the current line feature map corresponding to the point is used as the target point, and several points in the current line feature map closest to the current target point are found and a straight line is fitted to the several points closest to the current target point; if a straight line is fitted and the distance from the current point in the line feature point cloud in the map coordinate system to the straight line is less than a preset point-line distance threshold, the current point and the straight line are taken as a pair of matching pairs, and the distance from the current point in the line feature point cloud in the map coordinate system to the straight line is taken as the point-line distance residual of the current point; 3.2.5) Traverse the remaining points in the line feature point cloud in the map coordinate system, repeat step 3.2.4) to perform point-line matching on the remaining points, and finally obtain the matching pair relationship between the line feature point cloud in the map coordinate system and the current line feature map and the point-line distance residuals of all points. The point-line distance residuals of all points constitute the current line feature residual term.

Images