Multi-line LiDAR and GNSS_INS joint calibration method, system and medium

Abstract

The present invention discloses a method, system, and medium for joint calibration of a multi-line laser radar and GNSS_INS. The method comprises: obtaining laser point cloud data and GNSS_INS data after a vehicle travels one lap on a feature-rich road; calculating the laser radar pose and performing graph optimization to construct surface and line feature point cloud maps; performing voxel gridding on the surface feature point cloud map and fitting a plane; performing Euclidean clustering on the line feature point cloud map and fitting a line; filtering each laser point by associating it with the plane or line to which it belongs; constructing a hand-eye calibration problem based on the data before graph optimization to solve initial values for external calibration parameters; performing coordinate transformation on the filtered point cloud based on the initial values; establishing point-to-line and point-to-plane feature constraints, using the external calibration parameters as the quantities to be solved, and constructing a least squares problem for solution. This method can automatically calibrate external calibration parameters without the need for external markers, with high accuracy.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a multi-line laser radar and GNSS_INS joint calibration method, system and medium. Background Art

[0002] At present, LiDAR and GNSS_INS are important sensors in unmanned driving systems. The fusion of LiDAR and GNSS_INS data can provide accurate vehicle posture information. However, the premise of multi-sensor data fusion is the need for accurate extrinsic parameter calibration values between sensors. Therefore, extrinsic parameter calibration between sensors is required.

[0003] Existing calibration methods for lidar and GNSS_INS systems are generally divided into two categories: motion-based calibration and external sensor-based calibration. 1) Sensor motion-based calibration methods generally construct hand-eye calibration equations based on the motion information of the lidar and GNSS_INS. Initial calibration values and the relative pose of the lidar and GNSS_INS are used to reduce radar point cloud data distortion and further calibrate the external parameters between the sensors. However, the hand-eye calibration equations are a relative pose solution method, which provides a good initial solution for rotation but poor accuracy for translation. Moreover, relying solely on hand-eye calibration methods for motion calibration does not meet the requirements of inter-sensor data fusion. 2) External sensor-based calibration methods generally use a lidar with known calibration parameters. The relative pose transformation between the two lidars is determined by matching the lidar data. This pose transformation and the known calibration parameters are then used to determine the external parameters of the lidar and GNSS_INS system to be calibrated. However, this method relies on external lidar and is highly dependent on the accuracy of external sensors.

[0004] Patent document CN112904317A discloses a calibration method for multiple lidars and GNSS_INS systems. The method involves making a vehicle turn at a constant speed, driving from a ramp onto level ground, and making a figure-eight (Figure-eight) movement on a constant-speed ramp. The pose relationship of the GNSS_INS at different times is then obtained. The precise pose relationship of the lidar at the corresponding time is calculated based on the initial pose parameters, and the precise pose transformation parameter T of the main lidar relative to the GNSS_INS coordinate system is obtained.

[0005] This decoupled calibration method requires the vehicle to undergo calibration experiments in three different scenarios. The complex calibration process makes automation and lightweighting difficult. Furthermore, when calibrating roll and pitch angles, the slope of the vehicle must be greater than or equal to a specific angle to provide sufficient ground excitation. In real-world scenarios, environments that meet these slope requirements, have good GNSS signals, and sufficient LiDAR scanning objects are relatively rare.

[0006] Patent document CN112147599A discloses a spline function-based method for calibrating the external parameters of 3D lidar and inertial sensors in continuous time. First, the 3D laser point cloud in the calibration data is preprocessed based on a known environment map. Then, the motion trajectory of the high-frequency inertial sensor is modeled as a continuous trajectory using a spline function. The position and posture of the inertial sensor at any time are obtained based on the spline function. The external parameters between the 3D lidar and the inertial sensor are introduced, the laser points are constrained, and an optimization problem is constructed for solution.

[0007] This method places the device in a corner and fully moves and rotates it around, front and back, left and right, up and down, to excite each axis of the inertial sensor and obtain calibration data. This method requires sufficient motion excitation between sensors to ensure operational accuracy. It also requires the environment to be pre-modeled as a high-precision point cloud map, requiring a relatively accurate laser odometry. This method matches each frame of the laser point cloud with the environment map, determining the relationship between each point and the fitted plane. It only uses planar features, which are rare and difficult to extract in outdoor environments. Summary of the Invention

[0008] To address the shortcomings of the aforementioned prior art, the present invention provides a method, system, and medium for the joint calibration of a multi-line LiDAR and GNSS_INS. This method uses a laser odometry to construct a global laser point cloud map, which is then optimized to create surface and line feature maps. Each frame of the point cloud is associated with the surface and line feature maps, respectively, and a least-squares problem is constructed to solve the LiDAR and GNSS_INS extrinsic parameters. This improves the accuracy and robustness of the LiDAR and GNSS_INS extrinsic calibration parameter solution in outdoor environments.

[0009] In order to achieve the above-mentioned purpose, the present invention adopts the following technical solutions.

[0010] In the first aspect, a multi-line lidar and GNSS_INS joint calibration method is provided, comprising:

[0011] S1: Obtain laser point cloud data and GNSS_INS data collected after the vehicle drives a circle on a feature-rich road;

[0012] S2: Calculate the position and posture of the lidar through the laser odometry;

[0013] S3: Optimize the LiDAR pose and construct surface feature point cloud maps and line feature point cloud maps;

[0014] S4: Divide the surface feature point cloud map into voxel grids, select point clouds that meet the surface features in each voxel grid and perform plane fitting on each of them; perform Euclidean clustering on the line feature point cloud map, select point clouds that meet the line features in each cluster and perform line fitting on each of them; associate each laser point in each frame of laser point cloud data with the plane or line to which it belongs, and select surface feature point clouds and line feature point clouds;

[0015] S5: Construct a hand-eye calibration problem based on the LiDAR pose data and GNSS_INS data before graph optimization, and solve the initial values of the external calibration parameters between the LiDAR and GNSS_INS systems;

[0016] S6: Using the initial values of the external calibration parameters and GNSS_INS data, the surface feature point cloud and line feature point cloud selected from each frame of laser point cloud data are converted to the map coordinate system;

[0017] S7: For the screened surface feature points, the cloud calculates the distance between them and the plane they belong to; for the screened line feature points, the cloud calculates the distance between them and the straight line they belong to; characteristic constraints of the two distances are established; the external calibration parameters are taken as the quantities to be solved; a least squares problem is constructed to solve the optimized external calibration parameters.

[0018] Furthermore, after step S7, the following steps are further included:

[0019] S8: Using the optimized external calibration parameters, GNSS_INS data is used to remove distortion from each frame of laser point cloud data, and the process returns to step S2 for iterative optimization and solution to obtain the final external calibration parameters.

[0020] Furthermore, step S3 includes:

[0021] Based on the LiDAR’s pose detection loop, a LiDAR pose graph optimization problem is constructed to optimize the LiDAR’s pose.

[0022] For each line beam in the multi-line laser radar, the curvature of each laser point in the neighborhood is calculated; if the curvature is greater than the curvature threshold, the laser point is a line feature point; if the curvature is less than the curvature threshold, the laser point is a surface feature point;

[0023] The line feature points and surface feature points in each frame of laser point cloud data are converted to the map coordinate system to construct the line feature point cloud map and the surface feature point cloud map.

[0024] Furthermore, the face feature point cloud map is divided into voxel grids, point clouds that meet the face features are screened in each voxel grid and plane fitting is performed on each point cloud, including:

[0025] Divide the surface feature point cloud map into voxel grids according to the preset voxel grid size;

[0026] Calculate the covariance matrix of the point cloud within each voxel grid, then perform eigenvalue decomposition on the covariance matrix and calculate the flatness parameter based on the three eigenvalues of the covariance matrix;

[0027] The voxel grids whose flatness parameters are greater than the preset flatness threshold are taken as the surface feature voxel grids, and a random sampling consensus algorithm is used to fit the plane in each surface feature voxel grid.

[0028] Furthermore, the line feature point cloud map is segmented by Euclidean clustering, point clouds meeting the line feature are screened in each cluster and linear fitting is performed on each cluster, including:

[0029] Perform Euclidean clustering on the line feature point cloud map and divide the line feature point cloud map into different clusters;

[0030] Calculate the covariance matrix of the point cloud in each cluster, perform eigenvalue decomposition on the covariance matrix, obtain the three eigenvalues from small to large, and calculate the straightness parameter;

[0031] The clusters whose straightness parameters are greater than the preset straightness threshold are taken as line feature point cloud clusters, and a random sampling consensus algorithm is used to fit a straight line in each line feature point cloud cluster.

[0032] Furthermore, each laser point in each frame of laser point cloud data is associated with its own plane or line to filter out surface feature point clouds and line feature point clouds, including:

[0033] Calculate the minimum bounding box of each voxel grid that can fit a plane and each cluster that can fit a line using the extreme coordinates in the point cloud;

[0034] Traverse each laser point in the surface feature point cloud map and the line feature point cloud map and determine its relationship with each minimum bounding box;

[0035] For each laser point in the line feature point cloud map, calculate the distance between it and the line corresponding to the minimum bounding box. If the distance is less than the first distance threshold, associate the laser point with the corresponding cluster.

[0036] For each laser point in the surface feature point cloud map, calculate the distance between it and the plane corresponding to the minimum bounding box to which it belongs. If the distance is less than the second distance threshold, associate the laser point with the corresponding voxel grid.

[0037] Filter out the surface feature point clouds and line feature point clouds that meet the association conditions.

[0038] Furthermore, the step S5 includes:

[0039] Calculate the laser radar pose increment A based on the laser radar pose at the previous moment and the laser radar pose at the next moment;

[0040] Calculate the GNSS_INS pose increment B based on the GNSS_INS data at the previous moment and the GNSS_INS data at the next moment;

[0041] The hand-eye calibration problem is to form AX=XB by using the lidar pose increment A, GNSS_INS pose increment B and external calibration parameter X;

[0042] The hand-eye calibration problem is solved to obtain the initial values of the external calibration parameters between the lidar and GNSS_INS system.

[0043] Furthermore, in step S7, the initial values of the external calibration parameters are used as the quantities to be optimized and the initial values, and the point-line distance error between the line feature points and the clusters to which they belong, the point-surface distance error between the surface feature points and the voxel grids to which they belong, and the hand-eye calibration error are used as the residual F(x) of the least squares optimization problem. The overall residual equation can be written as follows:

[0044]

[0045] in,

[0046]

[0047]

[0048]

[0049] Among them, D L is the point-line distance error, D M is the point-to-surface distance error, R HE is the hand-eye calibration error, P L 、P L ′ are the line feature laser points and surface feature laser points before conversion to the map coordinate system, is the quantity to be optimized, i.e. the external calibration parameter, are a point and a normal vector on the fitted line in the cluster to which the line feature laser point belongs, converted to the map coordinate system. A, B, C, and D are the parameters in the plane equation Ax+By+Cz+D=0 fitted in the voxel grid to which the surface feature laser point belongs, converted to the map coordinate system. is the GNSS_INS pose transformation from the current frame time to the start time, is the laser pose increment, is the GNSS_INS pose increment;

[0050] The gradient descent method is used for iterative solution to obtain the optimized external calibration parameters.

[0051] Secondly, a multi-line lidar and GNSS_INS joint calibration system is provided, including:

[0052] The laser odometry and map construction module is used to obtain laser point cloud data collected after the vehicle drives a circle on a feature-rich road, calculate the position and posture of the laser radar through the laser odometry, and then perform graph optimization on the laser radar position and posture to construct surface feature point cloud maps and line feature point cloud maps;

[0053] The feature detection module is used to divide the surface feature point cloud map into voxel grids, screen the point clouds that meet the surface features in each voxel grid and perform plane fitting on them respectively; perform Euclidean clustering on the line feature point cloud map, screen the point clouds that meet the line features in each cluster and perform line fitting on them respectively; associate each laser point in each frame of laser point cloud data with the plane or line to which it belongs, and screen out surface feature point clouds and line feature point clouds;

[0054] The initial calibration extrinsic parameter solution module is used to construct a hand-eye calibration problem based on the LiDAR pose data before graph optimization and the GNSS_INS data collected after the vehicle drives a circle on a feature-rich road. It then solves the initial values of the extrinsic calibration parameters between the LiDAR and GNSS_INS systems.

[0055] The feature joint optimization module is used to convert the surface feature point cloud and line feature point cloud selected from each frame of laser point cloud data into the map coordinate system through the initial values of the external calibration parameters and the GNSS_INS data; the screened surface feature point cloud calculates its distance from the plane to which it belongs, and the screened line feature point cloud calculates its distance from the line to which it belongs, establishes feature constraints for the two distances, takes the external calibration parameters as the quantity to be solved, constructs a least squares problem, and solves the optimized external calibration parameters.

[0056] In a third aspect, a computer-readable storage medium is provided, which stores a computer program. When the computer program is executed by a processor, the steps of the multi-line lidar and GNSS_INS joint calibration method as described above are implemented.

[0057] The present invention proposes a multi-line lidar and GNSS_INS joint calibration method, system and medium, which have the following advantages over the existing technology:

[0058] 1) The present invention can automatically calibrate the external calibration parameters between the lidar and the GNSS_INS without the need for external beacons;

[0059] 2) The present invention corrects the accumulated error of the laser odometer posture by performing loop closure detection on the lidar data, establishes an accurate global map, and improves the accuracy of feature extraction in the subsequent calibration process;

[0060] 3) The present invention constructs a surface feature map and a line feature map based on the surface feature point cloud and line feature point cloud in each frame of laser point cloud data, and extracts planes and lines from these two maps respectively, thereby improving the stability of feature extraction;

[0061] 4) The present invention converts the line feature point cloud and surface feature point cloud of each frame of the lidar into the map coordinate system through the initial external calibration parameters and GNSS_INS data, and constructs constraint problems through the distance from the point to the line and the distance from the point to the plane to improve the accuracy of the solution;

[0062] 5) The present invention improves the robustness of the algorithm by solving the optimized external calibration parameters, using GNSS_INS data to dedistort the lidar, and iteratively optimizing the external calibration parameters. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0064] Figure 1 This is a schematic diagram of the process and system structure of the multi-line laser radar and GNSS_INS joint calibration method provided by an embodiment of the present invention;

[0065] Figure 2 Schematic diagram of a detection loop provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0066] To make the objectives, technical solutions, and advantages of the present invention more apparent, the technical solutions of the present invention will be described in detail below. Obviously, the embodiments described are only some of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other implementations obtained by those of ordinary skill in the art without inventive effort are within the scope of protection of the present invention.

[0067] The purpose of the present invention is to provide a method, system, and medium for the joint calibration of a multi-line laser radar and GNSS_INS. This method uses a laser odometry to construct a global laser point cloud map, which is then optimized to construct surface feature maps and line feature maps. Each frame's point cloud is associated with the surface feature map and line feature map, respectively, and a least-squares problem is constructed to solve the laser radar and GNSS_INS extrinsic parameters. This improves the accuracy and robustness of the laser radar and GNSS_INS extrinsic calibration parameter solution in outdoor environments. The technical solution of the present invention is further described below with reference to specific embodiments.

[0068] like Figure 1 As shown, an embodiment of the present invention provides a multi-line laser radar and GNSS_INS joint calibration method, including:

[0069] S1: Obtain laser point cloud data and GNSS_INS data collected by the vehicle after driving one circle on a feature-rich road. The vehicle is controlled to drive one circle on a feature-rich road, and the multi-line laser radar and GNSS_INS systems collect laser point cloud data and GNSS_INS data, respectively.

[0070] S2: For each frame of laser point cloud data, the laser radar pose is calculated using the laser odometry.

[0071] S3: Optimize the LiDAR’s pose and construct surface feature point cloud maps and line feature point cloud maps.

[0072] Specifically, the process of graph optimization of the laser radar's posture includes: constructing a laser radar posture graph optimization problem based on the laser radar's posture detection loop to optimize the laser radar's posture.

[0073] More specifically, if Figure 2 As shown in the figure, the loop detection process is as follows: for each laser frame's pose, the distance between it and the pose of all previous frames is detected. If the distance is less than 3m and the interval is greater than 15 seconds, it is considered a loop frame. ICP registration is performed on the two frame point clouds, and the reliability of the loop constraint is determined by the degree of ICP registration. If the loop is reliable, it is added to the loop candidate pair. Finally, the pair with the best registration is selected based on the degree of ICP registration to establish the loop constraint. It should be noted that the two parameters of distance 3m and time 15 seconds can be adjusted according to actual conditions. This is only an example and is not limited to these values.

[0074] The process of constructing the pose graph optimization problem of the lidar is as follows: the poses of the i-th frame and the j-th frame are T i and T j , there is a relative motion Δξ between them ij , then the relative motion between the two frames can be written algebraically as:

[0075]

[0076] Construct the error e by the relative motion between frames ij :

[0077]

[0078] T ij =T i -1 T j

[0079] Taking the poses of all key frames as the variables to be optimized and the relative motion observed between poses as constraints, we form ε optimization edges, which constitute a graph optimization problem. The overall objective function is:

[0080]

[0081] The process of constructing surface feature point cloud maps and line feature point cloud maps is as follows: calculate the curvature of each line beam in the multi-line laser radar. The calculation formula of the curvature is:

[0082]

[0083] Among them, s j represents the curvature of the j-th laser point position within the neighborhood, represents the measured value of the j-th laser point position, represents the measured value of the laser point position within the neighborhood of the jth laser point, A j represents the set of points within the neighborhood of the jth laser point;

[0084] By judging and screening the curvature of the point cloud, if the curvature is greater than the curvature threshold, the laser point is a line feature point; if the curvature is less than the curvature threshold, the laser point is a surface feature point;

[0085] The line feature points and surface feature points in each frame of the laser point cloud data after loop closure constraint are converted to the map coordinate system (first frame coordinate system) to construct the line feature point cloud map and the surface feature point cloud map.

[0086] By optimizing the pose graph of the obtained pose and constructing surface feature point cloud maps and line feature point cloud maps based on point cloud features, the false rejection rate of subsequent point cloud feature extraction is reduced by establishing two types of maps respectively.

[0087] S4: Divide the surface feature point cloud map into voxel grids, filter the point clouds that meet the surface features in each voxel grid and perform plane fitting respectively; perform Euclidean clustering on the line feature point cloud map, filter the point clouds that meet the line features in each cluster and perform line fitting respectively; associate each laser point in each frame of laser point cloud data with the plane or line to which it belongs, and filter out the surface feature point cloud and line feature point cloud.

[0088] Specifically, the surface feature point cloud map is divided into voxel grids, point clouds that meet the surface features are screened in each voxel grid and plane fitting is performed on each point cloud, including:

[0089] S411: Calculate the maximum and minimum coordinates of the x, y, and z axes of the point cloud in the surface feature point cloud map, and calculate the side length l of the minimum bounding box of the point cloud x 、l y 、l z ;

[0090]

[0091] S412: setting the point cloud voxel grid size according to the custom threshold, and dividing the surface feature point cloud map into voxel grids;

[0092] S413: Calculate the covariance matrix of the point cloud within each voxel grid:

[0093]

[0094] Where M is the covariance matrix of the point cloud in the voxel grid, K is the number of laser points in the voxel grid, and p i is each laser point in the voxel grid, and p0 is the centroid of the point cloud in the voxel grid;

[0095] S414: Perform eigenvalue decomposition on the covariance matrix and calculate the flatness parameter λ based on the three eigenvalues e0, e1, and e2 arranged from small to large;

[0096]

[0097] S415: taking voxel grids whose flatness parameters are greater than a preset flatness threshold (eg, 0.7) as surface feature voxel grids, and fitting a plane in each surface feature voxel grid using a random sampling consensus algorithm.

[0098] The method of performing Euclidean clustering on the line feature point cloud map, screening point clouds that meet the line features in each cluster and performing straight line fitting on each point cloud, includes:

[0099] S421: performing Euclidean clustering on the line feature point cloud map to divide the line feature point cloud map into different clusters;

[0100] S422: Calculate the covariance matrix of the point cloud in each cluster, and perform eigenvalue decomposition on the covariance matrix to obtain three eigenvalues e0, e1, and e2 from small to large, and calculate the straightness parameter β;

[0101]

[0102] S423: Clusters with straightness parameters greater than a preset straightness threshold (such as 0.6) are selected as line feature point cloud clusters, and a random sampling consensus algorithm is used to fit a straight line in each line feature point cloud cluster.

[0103] The process of associating each laser point in each frame of laser point cloud data with its corresponding plane or line to filter out surface feature point clouds and line feature point clouds includes:

[0104] S431: Calculating the minimum bounding box of each voxel grid that can fit a plane and each cluster that can fit a line using the extreme coordinates in the point cloud;

[0105] S432: The surface feature point cloud and line feature point cloud in each frame are converted to the map coordinate system, and each laser point in the surface feature point cloud map and the line feature point cloud map is traversed to determine its relationship with each minimum bounding box;

[0106] S433: For each laser point in the line feature point cloud map, calculate the distance between the laser point and the line corresponding to the minimum bounding box to which it belongs. If the distance is less than a first distance threshold, associate the laser point with the corresponding cluster.

[0107] For each laser point in the surface feature point cloud map, calculate the distance between it and the plane corresponding to the minimum bounding box to which it belongs. If the distance is less than the second distance threshold, associate the laser point with the corresponding voxel grid.

[0108] S434: Filter out the surface feature point clouds and line feature point clouds that meet the association conditions.

[0109] Through the optimized pose of the graph, the line feature point cloud and surface feature point cloud of each frame are transferred to the map coordinate system, and the relationship between each laser point and the surface feature voxel grid or line feature point cloud cluster is found. This method can accurately extract the surface and line features and accurately calculate the relationship between the point clouds.

[0110] S5: Construct a hand-eye calibration problem based on the LiDAR pose data and GNSS_INS data before graph optimization, and solve the initial values of the external calibration parameters between the LiDAR and GNSS_INS systems. Specifically, it includes:

[0111] S51: Calculating the laser radar posture increment A according to the laser radar posture at the previous moment and the laser radar posture at the next moment;

[0112] S52: Calculate the GNSS_INS pose increment B based on the GNSS_INS data at the previous moment and the GNSS_INS data at the next moment;

[0113] S53: A hand-eye calibration problem is constructed by using the lidar pose increment A, the GNSS_INS pose increment B, and the external calibration parameter X to form AX=XB;

[0114] S54: Solve the hand-eye calibration problem to obtain the initial values of the external calibration parameters between the lidar and the GNSS_INS system.

[0115] Where A is the laser pose increment B is the GNSS_INS pose increment X is the transformation matrix from lidar to GNSS_INS That is, the external calibration parameter between sensors, where k represents the time. The above equation AX=XB can be written as:

[0116]

[0117] Among them, R A and t A are the laser pose increments The rotation and translation, R B and t B They are GNSS_INS pose increments respectively The rotation and translation of R and t are the transformation matrices from the laser radar to the GNSS_INS. rotation and translation.

[0118] The solution process of the hand-eye calibration problem is:

[0119] First, expand the formula AX=XB and divide it into R A R=RR B 、R A t+t A =Rt B +t two steps. Then first A R=RR B Solve to get R. The solution method for this step is: convert the rotation matrix into quaternion form, as follows:

[0120]

[0121] Convert the quaternion to left- and right-multiplied form for transposition:

[0122]

[0123] in, To convert the quaternion of the lidar pose rotation increment into a left multiplication matrix, To convert the GNSS_INS pose rotation increment into the right quaternion form; Q N It is a matrix block composed of N groups of quaternion left multiplication and right multiplication matrices subtraction; the conversion form of the quaternion left multiplication matrix and right multiplication matrix is as follows, s is the real part of the quaternion, and v is the imaginary part of the quaternion:

[0124]

[0125]

[0126] α in the formula k is the weight used to remove outliers in the hand-eye calibration equation, α k The calculation method is:

[0127]

[0128]

[0129] in, is the rotation external parameter, i.e. R; is the laser radar pose rotation increment, is the GNSS_INS pose rotation increment, and threshold is the preset value;

[0130] Q N Perform SVD decomposition and obtain the smallest singular value σ4, and the corresponding right singular vector v4 is the rotation extrinsic parameter. Final result. After obtaining the rotation external parameter R, use the same method to solve R A t+t A =Rt B +t in t.

[0131] S6: Using the initial values of the external calibration parameters and the GNSS_INS data, the surface feature point cloud and line feature point cloud selected from each frame of laser point cloud data are converted to the map coordinate system.

[0132] The coordinate conversion process is as follows: the points in each frame of the lidar coordinate system L are converted to the map coordinate system using the external calibration parameters of the hand-eye calibration and the GNSS_INS data. The conversion formula is as follows:

[0133]

[0134] Among them, P L is the point in the laser radar coordinate system of each frame, P M is the laser point in the map coordinate system, is the GNSS_INS pose transformation from the current frame time to the start time, The quantity to be optimized is also the external calibration parameter.

[0135] S7: For the screened surface feature points, the cloud calculates the distance between them and the plane they belong to; for the screened line feature points, the cloud calculates the distance between them and the straight line they belong to; characteristic constraints of the two distances are established; the external calibration parameters are taken as the quantities to be solved; a least squares problem is constructed to solve the optimized external calibration parameters.

[0136] Specifically, the two distance feature constraints include point-to-line constraints and point-to-plane constraints.

[0137] The constraint from point to line is: for each line feature point transferred to the map coordinate system, calculate the straight line distance D from it to the line feature point cloud cluster to which it belongs. L :

[0138]

[0139] in, They are respectively a point on the straight line and the normal vector of the line feature point cloud cluster fitting.

[0140] The constraint from point to plane is: for each feature laser point transferred to the map coordinate system, calculate the straight-line distance D from the feature voxel grid to which it belongs. M :

[0141]

[0142] Among them, A, B, C, and D are parameters in the plane equation Ax+By+Cz+D=0 fitted in the voxel grid to which the surface feature laser point belongs, converted into the map coordinate system.

[0143] Construct a least squares problem to solve the optimized external calibration parameters, including:

[0144] The initial values of the external calibration parameters are used as the quantities to be optimized and the initial values. The point-line distance error from the line feature point to the cluster to which it belongs, the point-surface distance error from the surface feature point to the voxel grid to which it belongs, and the hand-eye calibration error are used as the residual F(x) of the least squares optimization problem. The overall residual equation can be written as follows:

[0145]

[0146] in:

[0147]

[0148]

[0149]

[0150] Among them, D L is the point-line distance error, D M is the point-to-surface distance error, R HE is the hand-eye calibration error;

[0151] By D L 、D M 、R HE The three residuals constitute the total residual of the current feature joint optimization problem. The gradient descent method is used for iterative solution. The iteration termination condition is set to the tolerable error threshold to solve the external calibration parameters of the lidar to GNSS_INS system.

[0152] Using the initial external calibration parameters, the line and surface feature points of a frame of LiDAR are transferred to the global LiDAR point cloud map coordinate system using GNSS_INS data. A least squares problem is then constructed for the distances between surface feature points and planes, and between line feature points and line segments. Multiple constraints are used to prevent the solution from falling into local optimality, improving the robustness of the optimization problem. It should be noted that step S5 can be executed before step S3 or simultaneously with steps S3 and S4.

[0153] As a preferred embodiment of the present invention, based on the above embodiment, it also includes:

[0154] S8: Using the optimized external calibration parameters, dedistort each frame of the laser point cloud data using the GNSS_INS data. The process then returns to step S2 for iterative optimization and solution, obtaining the final external calibration parameters. The iteration termination condition can be a tolerance error threshold or reaching the maximum number of iterations. Multiple iterations can yield more accurate external calibration parameters, improving the accuracy and stability of the algorithm.

[0155] like Figure 1 As shown, an embodiment of the present invention also provides a multi-line laser radar and GNSS_INS joint calibration system, including a laser odometer and map construction module, a feature detection module, an initial calibration external parameter solution module and a feature joint optimization module.

[0156] The system first runs the laser odometry and map construction module, extracting surface feature point clouds and line feature point clouds from the laser point cloud data collected by the vehicle for each frame. The system then calculates the pose using the laser odometry. Using the laser odometry pose data, it detects loops and constructs a pose graph optimization problem. Based on the optimized lidar pose, it builds surface feature point cloud maps and line feature point cloud maps. The system then runs the feature detection module, which performs voxel meshing on the surface feature point cloud map and filters point clouds that match surface features within each voxel. The system then performs Euclidean clustering on the line feature point cloud map, filtering point clouds that match line features within each cluster. For each laser point in each frame, the system determines its relationship with the calculated plane or line. The system also runs the initial calibration extrinsic parameter solution module to determine the initial values of the extrinsic calibration parameters between the two sensors. Finally, the feature joint optimization module uses the distances from points to their respective lines and planes to construct a least-squares problem to determine the precise extrinsic parameters.

[0157] Specifically, the laser odometry and map construction module is used to obtain laser point cloud data collected after the vehicle drives a circle on a feature-rich road, calculate the laser radar pose through the laser odometry, and then optimize the laser radar pose through the distance detection loop constraint of the pose to construct the surface feature point cloud map and line feature point cloud map;

[0158] The feature detection module is used to divide the surface feature point cloud map into voxel grids, screen the point clouds that meet the surface features in each voxel grid and perform plane fitting on them respectively; perform Euclidean clustering on the line feature point cloud map, screen the point clouds that meet the line features in each cluster and perform line fitting on them respectively; associate each laser point in each frame of laser point cloud data with the plane or line to which it belongs, and screen out surface feature point clouds and line feature point clouds;

[0159] The initial calibration extrinsic parameter solution module is used to construct a hand-eye calibration problem based on the LiDAR pose data before graph optimization and the GNSS_INS data collected after the vehicle drives a circle on a feature-rich road. It then solves the initial values of the extrinsic calibration parameters between the LiDAR and GNSS_INS systems.

[0160] The feature joint optimization module is used to convert the surface feature point cloud and line feature point cloud selected from each frame of laser point cloud data into the map coordinate system through the initial values of the external calibration parameters and the GNSS_INS data; the screened surface feature point cloud calculates its distance from the plane to which it belongs, and the screened line feature point cloud calculates its distance from the line to which it belongs, establishes feature constraints for the two distances, takes the external calibration parameters as the quantity to be solved, constructs a least squares problem, and solves the optimized external calibration parameters.

[0161] As a preferred embodiment of the present invention, based on the aforementioned embodiment, the feature joint optimization module is further used to feed back the optimized external calibration parameters to the laser odometry and map construction module and the initial calibration external parameter solution module. Using the optimized external calibration parameters, each frame of laser point cloud data is dedistorted using GNSS_INS data. The four modules are then combined again for iterative optimization and solution to obtain the final external calibration parameters. The iterative termination condition can be a tolerable error threshold or reaching the maximum number of iterations. Multiple iterative runs can yield more accurate external calibration parameters, improving the accuracy and stability of the algorithm.

[0162] It should be understood that the functional unit modules in various embodiments of the present invention may be integrated into a single processing unit, each unit module may exist physically separately, or two or more unit modules may be integrated into a single unit module, and may be implemented in the form of hardware or software. It should be understood that the same or similar parts of the above embodiments may be referenced to each other, and for content not described in detail in some embodiments, reference may be made to the same or similar content in other embodiments.

[0163] An embodiment of the present invention further provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the multi-line lidar and GNSS_INS joint calibration method described above.

[0164] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0165] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the steps in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0166] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0167] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0168] It can be understood that the same or similar parts of the above embodiments can be referenced to each other, and the contents not described in detail in some embodiments can refer to the same or similar contents in other embodiments.

[0169] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A multi-line laser radar and GNSS_INS joint calibration method, characterized in that: include: S1: Obtain laser point cloud data and GNSS_INS data collected after the vehicle drives a circle on a feature-rich road; S2: Calculate the position and posture of the lidar through the laser odometry; S3: Optimize the LiDAR pose and construct surface feature point cloud maps and line feature point cloud maps; S4: Divide the surface feature point cloud map into voxel grids, select point clouds that meet the surface features in each voxel grid and perform plane fitting on each of them; perform Euclidean clustering on the line feature point cloud map, select point clouds that meet the line features in each cluster and perform line fitting on each of them; associate each laser point in each frame of laser point cloud data with the plane or line to which it belongs, and select surface feature point clouds and line feature point clouds; S5: Construct a hand-eye calibration problem based on the LiDAR pose data and GNSS_INS data before graph optimization, and solve the initial values of the external calibration parameters between the LiDAR and GNSS_INS systems; S6: Using the initial values of the external calibration parameters and GNSS_INS data, the surface feature point cloud and line feature point cloud selected from each frame of laser point cloud data are converted to the map coordinate system; S7: For the screened surface feature points, the cloud calculates the distance between them and the plane they belong to; for the screened line feature points, the cloud calculates the distance between them and the straight line they belong to; characteristic constraints of the two distances are established; the external calibration parameters are taken as the quantities to be solved; a least squares problem is constructed to solve the optimized external calibration parameters.

2. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: After step S7, the following steps are also included: S8: Using the optimized external calibration parameters, GNSS_INS data is used to remove distortion from each frame of laser point cloud data, and the process returns to step S2 for iterative optimization and solution to obtain the final external calibration parameters.

3. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: The step S3 comprises: Based on the LiDAR’s pose detection loop, a LiDAR pose graph optimization problem is constructed to optimize the LiDAR’s pose. For each line beam in the multi-line laser radar, the curvature of each laser point in the neighborhood is calculated; if the curvature is greater than the curvature threshold, the laser point is a line feature point; if the curvature is less than the curvature threshold, the laser point is a surface feature point; The line feature points and surface feature points in each frame of laser point cloud data are converted to the map coordinate system to construct the line feature point cloud map and the surface feature point cloud map.

4. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: The method of dividing the surface feature point cloud map into voxel grids, screening point clouds that meet the surface features in each voxel grid and performing plane fitting on each point cloud includes: Divide the surface feature point cloud map into voxel grids according to the preset voxel grid size; Calculate the covariance matrix of the point cloud within each voxel grid, then perform eigenvalue decomposition on the covariance matrix and calculate the flatness parameter based on the three eigenvalues of the point cloud covariance matrix; The voxel grids whose flatness parameters are greater than the preset flatness threshold are taken as the surface feature voxel grids, and a random sampling consensus algorithm is used to fit the plane in each surface feature voxel grid.

5. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: The method of performing Euclidean clustering on the line feature point cloud map, screening point clouds that meet the line features in each cluster and performing straight line fitting on each point cloud, includes: Perform Euclidean clustering on the line feature point cloud map and divide the line feature point cloud map into different clusters; Calculate the covariance matrix of the point cloud in each cluster, perform eigenvalue decomposition on the covariance matrix, obtain the three eigenvalues from small to large, and calculate the straightness parameter; The clusters whose straightness parameters are greater than the preset straightness threshold are taken as line feature point cloud clusters, and a random sampling consensus algorithm is used to fit a straight line in each line feature point cloud cluster.

6. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: The process of associating each laser point in each frame of laser point cloud data with its corresponding plane or line to filter out surface feature point clouds and line feature point clouds includes: Calculate the minimum bounding box of each voxel grid that can fit a plane and each cluster that can fit a line using the extreme coordinates in the point cloud; Traverse each laser point in the surface feature point cloud map and the line feature point cloud map and determine its relationship with each minimum bounding box; For each laser point in the line feature point cloud map, calculate the distance between it and the line corresponding to the minimum bounding box. If the distance is less than the first distance threshold, associate the laser point with the corresponding cluster. For each laser point in the surface feature point cloud map, calculate the distance between it and the plane corresponding to the minimum bounding box to which it belongs. If the distance is less than the second distance threshold, associate the laser point with the corresponding voxel grid. Filter out the surface feature point clouds and line feature point clouds that meet the association conditions.

7. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: The step S5 comprises: Calculate the laser radar pose increment A based on the laser radar pose at the previous moment and the laser radar pose at the next moment; Calculate the GNSS_INS pose increment B based on the GNSS_INS data at the previous moment and the GNSS_INS data at the next moment; The hand-eye calibration problem is to form AX=XB by using the lidar pose increment A, GNSS_INS pose increment B and external calibration parameter X; The hand-eye calibration problem is solved to obtain the initial values of the external calibration parameters between the lidar and GNSS_INS system.

8. The multi-line laser radar and GNSS_INS joint calibration method according to claim 1 is characterized in that: In step S7, the initial values of the external calibration parameters are used as the quantities to be optimized and the initial values. The point-line distance error between the line feature points and the clusters to which they belong, the point-surface distance error between the surface feature points and the voxel grids to which they belong, and the hand-eye calibration error are used as the residual F(x) of the least squares optimization problem. The overall residual equation can be written as follows: in, Among them, D L is the point-line distance error, D M is the point-to-surface distance error, R HE is the hand-eye calibration error P L 、P L ′ are the line feature laser points and surface feature laser points before conversion to the map coordinate system, is the quantity to be optimized, i.e. the external calibration parameter, are a point and a normal vector on the fitted line in the cluster to which the line feature laser point belongs, converted to the map coordinate system. A, B, C, and D are the parameters in the plane equation Ax+By+Cz+D=0 fitted in the voxel grid to which the surface feature laser point belongs, converted to the map coordinate system. is the GNSS_INS pose transformation from the current frame time to the start time, is the laser pose increment, is the GNSS_INS pose increment; The gradient descent method is used for iterative solution to obtain the optimized external calibration parameters.

9. A multi-line laser radar and GNSS_INS joint calibration system, characterized in that: include: The laser odometry and map construction module is used to obtain laser point cloud data collected after the vehicle drives a circle on a feature-rich road, calculate the position and posture of the laser radar through the laser odometry, and then perform graph optimization on the laser radar position and posture to construct surface feature point cloud maps and line feature point cloud maps; The feature detection module is used to divide the surface feature point cloud map into voxel grids, screen the point clouds that meet the surface features in each voxel grid and perform plane fitting on them respectively; perform Euclidean clustering on the line feature point cloud map, screen the point clouds that meet the line features in each cluster and perform line fitting on them respectively; associate each laser point in each frame of laser point cloud data with the plane or line to which it belongs, and screen out surface feature point clouds and line feature point clouds; The initial calibration extrinsic parameter solution module is used to construct a hand-eye calibration problem based on the LiDAR pose data before graph optimization and the GNSS_INS data collected after the vehicle drives a circle on a feature-rich road. It then solves the initial values of the extrinsic calibration parameters between the LiDAR and GNSS_INS systems. The feature joint optimization module is used to convert the surface feature point cloud and line feature point cloud selected from each frame of laser point cloud data into the map coordinate system through the initial values of the external calibration parameters and the GNSS_INS data; the screened surface feature point cloud calculates its distance from the plane to which it belongs, and the screened line feature point cloud calculates its distance from the line to which it belongs, establishes feature constraints for the two distances, takes the external calibration parameters as the quantity to be solved, constructs a least squares problem, and solves the optimized external calibration parameters.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the multi-line lidar and GNSS_INS joint calibration method as described in any one of claims 1 to 8 are implemented.

Images