A method and device for calibrating a laser radar and an IMU, and a storage medium

Abstract

The application discloses a kind of laser radar and the calibration method, device and storage medium of IMU, the first point cloud data of feature object is obtained by laser radar in the application and the first IMU data is obtained by IMU, the attitude of carrier relative to feature object is transformed preset number of times, the second point cloud data of feature object is obtained by laser radar after each transformation, and the second IMU data is obtained by IMU, to calculate the first original observation value of gravity vector in IMU coordinate system and the first transformed observation value, the second original observation value of gravity vector in laser radar coordinate system and the second transformed observation value, for calculating the target conversion matrix between laser radar and IMU, feature object is used as feature acquisition point cloud data and is constructed constraint with IMU data, without relying on additional artificial target and sensor, reduce the condition requirement of calibration;Adopt static scanning mode, not affected by laser radar and IMU time synchronization, without calibrating the motion distortion of point cloud and estimating motion trajectory.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of calibration, in particular to a laser radar and IMU calibration method, device and storage medium. BACKGROUND

[0002] The combination of laser radar (Light Detection and Ranging, LiDAR) and inertial measurement unit (Inertial Measurement Unit, IMU) is a common multi-source sensor fusion method. The high-frequency angular velocity and acceleration information of inertial navigation can provide better prior values for laser point cloud registration. The tight coupling of inertial odometry and laser odometry improves the accuracy and robustness of pose estimation. Each sensor has its own spatial and temporal reference. Before fusing the data of the two sensors for positioning, mapping and perception, the measurement values of different sensors need to be converted to the same coordinate system, i.e. the rotation and translation relationship between the coordinate systems of each sensor is obtained to realize calibration.

[0003] Nowadays, the calibration of laser radar and inertial measurement unit is usually carried out by hand-eye calibration method, online estimation method and feature-based method. The hand-eye calibration method relies on the IMU to estimate the trajectory independently, which has low accuracy. The online estimation method needs sufficient linear acceleration and angular velocity excitation to make the filter converge. If the error of the initial value is large, it may cause slow or failed filter convergence. The feature-based method requires accurate time synchronization of LiDAR and IMU, and needs to calibrate the point cloud distortion caused by motion. The influence of the motion trajectory on the observability of external parameters is complex. If the motion of the carrier is limited or the attitude change is not sufficient, it will lead to degeneration in a certain direction. SUMMARY

[0004] Therefore, in order to solve at least one of the above technical problems, the purpose of the present application is to provide a laser radar and IMU calibration method, device, equipment and storage medium, which reduces the condition requirement of calibration and simplifies the calibration process.

[0005] The present application provides a laser radar and IMU calibration method, which comprises:

[0006] The first point cloud data of the feature object is obtained by the laser radar, and the first IMU data is obtained by the IMU. The feature object is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed on the carrier;

[0007] The attitude of the carrier relative to the feature object is transformed for a preset number of times. After each transformation, the second point cloud data of the feature object is obtained by the laser radar, and the second IMU data is obtained by the IMU;

[0008] According to the first IMU data, a first original observation value of a gravity vector in an IMU coordinate system is calculated, according to the first point cloud data, a second original observation value of the gravity vector in a laser radar coordinate system is calculated, according to the second IMU data, a first transformed observation value of the gravity vector in the IMU coordinate system is calculated, and according to the second point cloud data, a second transformed observation value of the gravity vector in the laser radar coordinate system is calculated; the first original observation value and the first transformed observation value constitute a first set, and the second original observation value and the second transformed observation value constitute a second set;

[0009] According to the first set and the second set, a target conversion matrix between the laser radar and the IMU is calculated.

[0010] Further, the first original observation value of the gravity vector in the IMU coordinate system is calculated according to the first IMU data, comprising:

[0011] According to the first IMU data, an observation value of an accelerometer is determined;

[0012] A first difference between the observation value of the accelerometer and a preset zero offset of the accelerometer is calculated;

[0013] According to a ratio of the first difference and a modulus of the first difference, the first original observation value of the gravity vector in the IMU coordinate system is obtained.

[0014] Further, the first IMU data comprises a plurality of groups of values of the accelerometer, and the observation value of the accelerometer is determined according to the first IMU data, comprising:

[0015] According to the plurality of groups of values of the accelerometer, a mean value of a first coordinate axis, a mean value of a second coordinate axis and a mean value of a third coordinate axis of the IMU coordinate system are calculated; the mean value of the first coordinate axis, the mean value of the second coordinate axis and the mean value of the third coordinate axis constitute the observation value of the accelerometer.

[0016] Further, the second original observation value of the gravity vector in the laser radar coordinate system is calculated according to the first point cloud data, comprising:

[0017] When the feature object is a first plane perpendicular to the plumb line, a first point set located in the first plane is extracted from the first point cloud data; according to a normal vector of the first plane to be optimized and the first point set, a first distance equation of each point in the first point set to the first plane is constructed; the first distance equation is fitted to calculate a first target normal vector of the first plane which makes the sum of results of the first distance equation minimum as the second original observation value of the gravity vector in the laser radar coordinate system;

[0018] When the feature is a second plane and a third plane parallel to the plumb line, the second plane and the third plane intersect: a second point set located in the second plane and a third point set located in the third plane are extracted from the first point cloud data respectively; a second distance equation of each point in the second point set to the second plane is constructed according to a normal vector of the second plane to be optimized and the second point set, and a third distance equation of each point in the third point set to the third plane is constructed according to a normal vector of the third plane to be optimized and the third point set; the second distance equation is fitted to calculate a second target normal vector of the second plane that makes the sum of the results of the second distance equation minimum, and the third distance equation is fitted to calculate a third target normal vector of the third plane that makes the sum of the results of the third distance equation minimum; the cross product of the second target normal vector and the third target normal vector is taken as a second original observation value of the gravity vector in the laser radar coordinate system.

[0019] Further, the second original observation value of the gravity vector in the laser radar coordinate system is calculated according to the first point cloud data, comprising:

[0020] When the feature is an object, an axis of the object is parallel to the plumb line, and a fourth point set located in the object is extracted from the first point cloud data:

[0021] A fourth distance equation of each point in the fourth point set to the surface of the feature is constructed according to a unit vector of the axis to be optimized and the fourth point set;

[0022] The fourth distance equation is fitted to calculate a target unit vector of the axis that makes the sum of the results of the fourth distance equation minimum as a second original observation value of the gravity vector in the laser radar coordinate system.

[0023] Further, the target conversion matrix between the laser radar and the IMU is calculated according to the first set and the second set, comprising:

[0024] A first mean value of the first set and a second mean value of the second set are calculated;

[0025] A difference value between each observation value in the first set and the first mean value is calculated to obtain first coordinate information, and a difference value between each observation value in the second set and the second mean value is calculated to obtain second coordinate information;

[0026] A symmetric matrix equation is constructed according to the first coordinate information, the second coordinate information and a preset trace function of a matrix;

[0027] A target eigenvector corresponding to the maximum value of the result of the symmetric matrix equation is determined by calculating the symmetric matrix equation.

[0028] Substitute the target feature vector into the preset conversion matrix to be solved between the lidar and the IMU to obtain a target conversion matrix between the lidar and the IMU.

[0029] Further, before the step of calculating the target conversion matrix between the lidar and the IMU according to the first set and the second set, the method further comprises:

[0030] According to the preset number of times, the second transformed observation value and a preset matrix trace function, a observability score is calculated.

[0031] When the observability score is less than a score threshold, the step of transforming the attitude of the carrier relative to the feature object a preset number of times is returned until the observability score is greater than or equal to the score threshold.

[0032] When the observability score is greater than or equal to the score threshold, the target conversion matrix between the lidar and the IMU is calculated according to the first set and the second set.

[0033] The embodiment of the application also provides a calibration device for a lidar and an IMU, comprising:

[0034] A first acquisition module is configured to acquire first point cloud data of a feature object by a lidar and acquire first IMU data by an IMU; the feature object is parallel or perpendicular to a plumb line, and the lidar and the IMU are fixed to a carrier.

[0035] A second acquisition module is configured to transform the attitude of the carrier relative to the feature object a preset number of times, and after each transformation, second point cloud data of the feature object is acquired by the lidar and second IMU data is acquired by the IMU.

[0036] A first calculation module is configured to calculate a first original observation value of a gravity vector in an IMU coordinate system according to the first IMU data, calculate a second original observation value of the gravity vector in a lidar coordinate system according to the first point cloud data, calculate a first transformed observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculate a second transformed observation value of the gravity vector in the lidar coordinate system according to the second point cloud data; the first original observation value and the first transformed observation value constitute a first set, and the second original observation value and the second transformed observation value constitute a second set.

[0037] The second computing module is configured to calculate a target conversion matrix between the laser radar and the IMU according to the first set and the second set. The present application also provides a laser radar and IMU calibration device, which comprises a processor and a memory. The memory stores at least one instruction, at least one program, a code set or an instruction set, which are loaded and executed by the processor to implement the method.

[0038] The present application also provides a computer readable storage medium, which stores at least one instruction, at least one program, a code set or an instruction set, which are loaded and executed by a processor to implement the method.

[0039] The present application has the following advantages:

[0040] The first point cloud data of the feature object is acquired by the laser radar, and the first IMU data is acquired by the IMU. The feature object is parallel or perpendicular to the plumb line. The posture of the carrier relative to the feature object is transformed for a preset number of times. After each transformation, the second point cloud data of the feature object is acquired by the laser radar, and the second IMU data is acquired by the IMU. The first original observation value of the gravity vector in the IMU coordinate system is calculated according to the first IMU data. The second original observation value of the gravity vector in the laser radar coordinate system is calculated according to the first point cloud data. The first transformed observation value of the gravity vector in the IMU coordinate system is calculated according to the second IMU data. The second transformed observation value of the gravity vector in the laser radar coordinate system is calculated according to the second point cloud data. The first set and the second set are constructed. The target conversion matrix between the laser radar and the IMU is calculated according to the first set and the second set. In the calibration process, the feature object is used as a feature to acquire point cloud data and construct constraints with IMU data. It does not need to rely on additional artificial targets and sensors, reduces the requirements of the calibration process, and simplifies the calibration process. The IMU data and the point cloud data are acquired in a static scanning mode, which is not affected by the time synchronization of the laser radar and the IMU, does not need to calibrate the motion distortion of the point cloud, and does not need to estimate the motion trajectory of the IMU and the LiDAR, thereby simplifying the calibration process.

[0041] For better understanding and implementation, the present application is described in detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0042] Figure 1 The figure is a schematic diagram of the steps of the laser radar and IMU calibration method of the present application.

[0043] Figure 2 The figure is a schematic diagram of the posture transformation of the carrier relative to the feature object in the specific embodiment of the present application.

[0044] Figure 3 Schematic diagram of different types of feature objects for specific embodiments of the present application. DETAILED DESCRIPTION

[0045] In order to enable persons skilled in the art to better understand the scheme of the present application, the technical solutions in the embodiments of the present application will be described clearly and completely in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by persons skilled in the art without creative labor should fall within the scope of protection of the present application.

[0046] The terms "first", "second", "third", and "fourth" and the like in the specification of the present application and claims, and the drawings are used to distinguish different objects, rather than to describe a particular order. In addition, the terms "comprise" and "have" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but can optionally include steps or units not listed, or can optionally include other steps or units inherent to the process, method, product or device.

[0047] Reference herein to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the present application. The appearance of the phrase in various places in the specification does not necessarily all refer to the same embodiment, nor is it necessarily independent or alternative embodiments to other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0048] As shown in Figure 1 The embodiment of the present application provides a calibration method of a laser radar and an IMU, which comprises steps S100-S400:

[0049] S100, acquiring first point cloud data of a feature object by a laser radar and acquiring first IMU data by an IMU.

[0050] In the embodiment of the present application, the laser radar and the IMU are fixed to the carrier, and the feature and the plumb line are parallel or perpendicular. It should be noted that in the embodiment of the present application, the laser radar and the IMU are installed at different positions of the carrier, and the two are rigidly connected, the IMU has an IMU coordinate system (b system), the IMU coordinate system coincides with the carrier coordinate system, the X axis (first coordinate axis) points to the right, the Y axis (second coordinate axis) points to the front, and the Z axis (third coordinate axis) points to the top; the laser radar has a scanning coordinate system (denoted as a laser radar coordinate system, i.e., a l system), which is also a right-handed coordinate system, and a 'northeast sky' geographic coordinate system is defined as a navigation coordinate system (n system).

[0051] Specifically, 1) first, select a calibration site: select an artificial scene with the following characteristics as the calibration site:

[0052] a. The horizontal ground in the structured scene, the ground is flat and smooth, the levelness of the plane is measured by an electronic level, and a plane area with a levelness less than 0.1° and a size of 1m 2 left is selected as the feature. In actual production, the artificial arrangement of calibration boards can be adopted, the calibration boards are fixed and placed horizontally on the height-adjustable bases at four corners, and the calibration boards are accurately leveled by an electronic level to obtain higher calibration accuracy.

[0053] b. The wall corner in the structured scene, which has two adjacent vertical walls that are not parallel, and the walls should be approximately parallel to the plumb line.

[0054] c. The vertical rod / pole-shaped ground object in the structured scene, which requires that the rod / pole-shaped ground object is approximately parallel to the plumb line.

[0055] 2) Collect calibration data, fix the carrier carrying the laser radar and the IMU at a certain position, require the feature target to be within the field of view of the laser radar, and the distance to be no more than 15m, start the laser radar and the IMU, measure statically, and record the first point cloud data of the feature obtained by the laser radar, and the first IMU data obtained by the IMU.

[0056] S200, transform the attitude of the carrier relative to the feature a preset number of times, and after each transformation, obtain second point cloud data of the feature by the laser radar and second IMU data by the IMU.

[0057] It should be noted that the first IMU data and the second IMU data in the embodiment of the present application refer to the IMU data about the gravitational acceleration. The preset number is set according to actual needs, for example, it can be determined according to the degrees of freedom of the matrix to be solved, assuming that the degrees of freedom of the target transformation matrix to be finally calculated is 3, then the preset number is ≥3.

[0058] In the embodiment of the present application, after each time the pose of the carrier relative to the feature is changed, the second point cloud data of the feature is acquired by the laser radar and the second IMU data is acquired by the IMU, as shown in Figure 2 Z represents the vertical direction, and TAG represents the feature.

[0059] S300, according to the first IMU data, the first original observation value of the gravity vector in the IMU coordinate system is calculated, according to the first point cloud data, the second original observation value of the gravity vector in the laser radar coordinate system is calculated, according to the second IMU data, the first transformed observation value of the gravity vector in the IMU coordinate system is calculated, and according to the second point cloud data, the second transformed observation value of the gravity vector in the laser radar coordinate system is calculated.

[0060] In the embodiment of the present application, the first original observation value and the first transformed observation value constitute a first set, and the second original observation value and the second transformed observation value constitute a second set.

[0061] Optionally, in step S300, the first original observation value of the gravity vector in the IMU coordinate system is calculated according to the first IMU data, comprising steps S301-S303:

[0062] S301, the observation value of the accelerometer is determined according to the first IMU data.

[0063] In the embodiment of the present application, the first IMU data includes a plurality of sets of accelerometer values, specifically: according to a plurality of sets of accelerometer values, the mean value of the accelerometer value on the first coordinate axis, the mean value of the second coordinate axis and the mean value of the third coordinate axis of the IMU coordinate system are calculated; the mean value of the first coordinate axis, the mean value of the second coordinate axis and the mean value of the third coordinate axis constitute the observation value of the accelerometer.

[0064] It should be noted that when the carrier is placed still, the IMU is only affected by the gravity, and in theory, the acceleration felt by the accelerometer mass block of the IMU is equal in size and opposite in direction to the gravitational acceleration, and the observation value of the accelerometer can be regarded as the projection of the gravitational acceleration on the X, Y and Z axes of the IMU. The observation equation of the accelerometer is:

[0065]

[0066] In the formula: is the observation value of the accelerometer, which is composed of the mean value of the first coordinate axis of the IMU coordinate system the mean value of the second coordinate axis and the mean value of the third coordinate axis represents the direction cosine matrix of the IMU coordinate system relative to the navigation coordinate system, and g n =[0 0 g] T ​T stands for transpose, the superscript n represents the navigation coordinate system (n-frame), g is the gravitational acceleration, and b a The preset zero bias of the accelerometer can be obtained through pre-calibration.

[0067] S302. Calculate the first difference between the accelerometer observation and the accelerometer's preset zero bias.

[0068] S303. Based on the ratio of the first gap to the magnitude of the first gap, the first original observation value of the gravity vector in the IMU coordinate system is obtained.

[0069] Specifically, taking into account the invariance of the magnitude of the vector before and after rotation, the vectorization in equation (1) is normalized:

[0070]

[0071] make

[0072]

[0073]

[0074] It should be noted that f b -b a The first gap is represented by ||||, where ||f represents the modulo operation. b -b a || represents the magnitude of the first gap, and m is the first original observation of the gravity vector in the IMU coordinate system.

[0075] Optionally, step S300, which calculates the second original observation value of the gravity vector in the lidar coordinate system based on the first point cloud data, includes steps S311-S312 or S321 and S322, wherein the execution order of S311, S312, and S321 is not limited.

[0076] S311. When the feature is a first plane perpendicular to the vertical line: extract the first set of points located on the first plane from the first point cloud data; construct the first distance equation from each point in the first point set to the first plane based on the normal vector of the first plane to be optimized and the first set of points; perform fitting processing on the first distance equation, and calculate the first target normal vector of the first plane that minimizes the sum of the results of the first distance equation as the second original observation value of the gravity vector in the lidar coordinate system.

[0077] In this embodiment of the invention, when the feature is a first plane perpendicular to the vertical line, such as... Figure 3 The first plane 101 shown is used to extract the first point set P = {p1, p2, p3, ..., p} located on the first plane from the first point cloud data. k}, p k Let k be the k-th point in the first point set. Specifically, the equation of the plane corresponding to the first plane is:

[0078] ax+by+cz-d=0 (5)

[0079] Wherein, n=[a, b, c] T represents the normal vector of the first plane to be optimized, the normal vector is specifically a unit normal vector, a, b and c represent the components of different coordinate axes, d represents the distance from the origin of the LiDAR coordinate system (laser radar coordinate system) to the first plane, and [x, y, z] is the three-dimensional coordinates of any point on the first plane in the LiDAR coordinate system, that is, the three-dimensional coordinates of any point in the first point set in the LiDAR coordinate system.

[0080] Therefore, the first distance equation of any point p in the first point set to the plane d i i is:

[0081]

[0082] The optimal plane parameters should make the following objective function value minimum:

[0083]

[0084] In the embodiment of the application, since noise inevitably exists in the collected first point cloud data, in order to eliminate the influence of abnormal values on the accuracy of the plane parameters, the RANSAC method is used to fit the plane parameters, that is, the RANSAC method is used to fit and process the first distance equation, the fitting processing is as shown in formula (7), and the first target normal vector of the first plane which makes the sum of the results of the first distance equation minimum is calculated as the second original observation value of the gravity vector in the laser radar coordinate system, that is, the fitting result of formula (7) is satisfied, at this time, n is the first target normal vector of the first plane, which is the second original observation value of the gravity vector in the laser radar coordinate system, the first target normal vector is parallel to the gravity vector, and is the representation of the gravity direction in the LiDAR coordinate system.

[0085] ​S312, when the feature is the second plane and the third plane parallel to the plumb line and the second plane intersects the third plane, a second point set located on the second plane and a third point set located on the third plane are extracted from the first point cloud data respectively; a second distance equation of each point in the second point set to the second plane is constructed according to the normal vector of the second plane to be optimized and the second point set, and a third distance equation of each point in the third point set to the third plane is constructed according to the normal vector of the third plane to be optimized; the second distance equation is fitted to calculate a second target normal vector of the second plane that makes the sum of the results of the second distance equation minimum, and the third distance equation is fitted to calculate a third target normal vector of the third plane that makes the sum of the results of the third distance equation minimum; and the cross product of the second target normal vector and the third target normal vector is taken as the second original observation value of the gravity vector in the laser radar coordinate system.

[0086] In the embodiment of the application, when the feature is the second plane and the third plane parallel to the plumb line and the second plane intersects the third plane, as shown in 102 in the figure, Figure 3 In the embodiment of the application, when the feature is the second plane and the third plane parallel to the plumb line and the second plane intersects the third plane, as shown in 102 in the figure,

[0087] Specifically, the second distance equation is fitted to calculate a second target normal vector of the second plane that makes the sum of the results of the second distance equation minimum, and the third distance equation is fitted to calculate a third target normal vector of the third plane that makes the sum of the results of the third distance equation minimum. It should be noted that the principle of fitting is shown in formula (7), which will not be repeated. At this time, the second target normal vector n1 and the third target normal vector n2 can be obtained.

[0088] Then, the cross product of the second target normal vector n1 and the third target normal vector n2 is calculated as follows:

[0089] n = n1 x n2 (8)

[0090] In formula (8), the cross product n is the second original observation value of the gravity vector in the laser radar coordinate system.

[0091] S321, when the feature is the object and the axis of the object is parallel to the plumb line, a fourth point set located on the object is extracted from the first point cloud data.

[0092] Optionally, a fourth point set located on the object is extracted from the first point cloud data, when the feature object is an object such as a rod, a column, etc. Figure 3

[0093] S322, according to the unit vector of the axis to be optimized and the fourth point set, a fourth distance equation of each point in the fourth point set to the surface of the feature object is constructed;

[0094] The fourth distance equation is fitted, and a target unit vector of the axis that minimizes the sum of the results of the fourth distance equation is calculated as a second original observation value of the gravity vector in the LiDAR coordinate system.

[0095] In the embodiment of the application, taking a cylinder as an example, the axis of the cylinder is the central axis of the cylinder, and the surface of the feature object is the surface of the cylinder, such as a cylindrical surface. The equation of the axis can be expressed as:

[0096]

[0097] wherein [x, y, z] is the three-dimensional coordinates of any point in the fourth point set in the LiDAR coordinate system, [x0, y0, z0] is the coordinates of the starting point of the axis, λ represents the proportion factor, n' = [n x , n y , n z ] T is the unit vector along the direction of the axis, and n x , n v , n z represent the components of different coordinate axes.

[0098] wherein the fourth distance equation of the distance di of any point in the fourth point set to the surface of the cylinder is:

[0099] d i = | μ × n' | - r (10)

[0100] wherein μ = [x-x0, y-y0, z-z0], and r is the radius of the cylinder. Then, the fourth distance equation is fitted, and the optimal object parameter should minimize the following target function value:

[0101]

[0102] wherein k is the number of points in the fourth point set.

[0103] ​In the embodiment of the present application, the RANSAC method is used to fit the object parameters, that is, the RANSAC method is used to fit the fourth distance equation, and the fitting process is shown in formula (11). The target unit vector of the axis that minimizes the sum of the results of the fourth distance equation is calculated as the second original observation value of the gravity vector in the laser radar coordinate system.

[0104] The observation value of the gravity direction in the laser radar coordinate system fitted by the three types of features (101, 102, and 103) described above and the actual gravity vector are related as follows: Figure 3

[0105]

[0106] wherein the matrix represents the direction cosine matrix of the laser radar coordinate system relative to the navigation coordinate system. In the embodiment of the present application, in order to ensure that the relative relationship between the normal vectors remains consistent in the LiDAR and IMU coordinate systems, it is specified that the positive direction of the unit vector n' points to the opposite direction of the gravity.

[0107] S400, calculating the target conversion matrix between the laser radar and the IMU according to the first set and the second set.

[0108] In the embodiment of the present application, the geometric constraint is constructed for subsequent solution of the target conversion matrix. Specifically:

[0109] Taking the gravity vector as the common observation value of the IMU and the laser radar, the constraint is constructed, and formula (4) and formula (12) are combined:

[0110]

[0111] Eliminate g n It can be obtained that:

[0112]

[0113] Since The constraint equation can be expressed as:

[0114]

[0115] wherein m and n are column vectors, is the direction cosine matrix of the laser radar coordinate system relative to the IMU coordinate system, that is, the preset conversion matrix to be solved. And The form of

[0116]

[0117] In the formula: q = [q0 q1 q2 q3] is a unit quaternion.

[0118] ​Optionally, step S400 comprises steps S410-S450.

[0119] S410, a first mean of the first set and a second mean of the second set are calculated.

[0120] It should be noted that the degree of freedom of the preset conversion matrix to be solved in the embodiment of the present application is 3, and therefore at least 3 sets of linearly independent observation values are required to solve the preset conversion matrix, and therefore the preset number of times is greater than or equal to 3. It is assumed that N equations can be listed according to formula (15), the first original observation values and the first transformed observation values constitute a first set {m1, m2, …, mN}, and the second original observation values and the second transformed observation values constitute a second set {n1, n2, …, nN}. N N Then, the rotation parameters are solved according to the coordinate conversion mode of the points.

[0121] Specifically, a first mean of the first set and a second mean n of the second set are calculated.

[0122]

[0123]

[0124] S420, a difference between each observation value in the first set and the first mean is calculated to obtain first coordinate information, and a difference between each observation value in the second set and the second mean is calculated to obtain second coordinate information.

[0125] It should be noted that each observation value m in the first set N refers to the first original observation value or the first transformed observation value, and each observation value n in the second set N refers to the second original observation value or the second transformed observation value. Specifically:

[0126] The coordinates n′ i (second coordinate information), m′ i (first coordinate information) after the normal vector barycentricization are:

[0127]

[0128]

[0129] S430, a symmetric matrix equation is constructed according to the first coordinate information, the second coordinate information, and a preset trace function of a matrix.

[0130] Specifically, a defined matrix is:

[0131]

[0132] ​

[0133] The symmetric matrix equation of constructing the symmetric matrix K is:

[0134]

[0135] wherein Z=[B 23 -B 32 B 31 -B 13 B 12 -B 21 ], B ij denotes the element of the i-th row and the j-th column of the matrix B, I 3×3 denotes a unit matrix with a size of 3, T is a transpose, and tr denotes a preset matrix trace function.

[0136] S440, the symmetric matrix equation is calculated and processed to determine the target eigenvector corresponding to the maximum value of the result of the symmetric matrix equation.

[0137] Specifically, according to the quaternion principle, the target eigenvector q=[q0 q1 q2 q3] corresponding to the maximum value (the maximum eigenvalue) λ max of the symmetric matrix equation of the symmetric matrix K is determined. max The target eigenvector q=[q0 q1 q2 q3] is the unit quaternion to be solved.

[0138] S450, the target eigenvector is substituted into the preset conversion matrix to be solved between the laser radar and the IMU to obtain the target conversion matrix between the laser radar and the IMU.

[0139] Specifically, the target eigenvector q=[q0 q1 q2 q3] is substituted into the preset conversion matrix to be solved , that is, formula (16), that is, the target conversion matrix between the laser radar and the IMU is obtained.

[0140] Optionally, the laser radar and the IMU calibration method of the embodiment of the application further includes a step S330 before the step S400, and the step S330 includes steps S3301-S3303, wherein S3302 and S3303 are not limited in execution order:

[0141] S3301, according to a preset number of times, a second transformation observation value, and a preset matrix trace function, a observability score is calculated.

[0142] It should be noted that in the embodiment of the present application, the degree of freedom of the preset conversion matrix to be solved is 3, and therefore at least 3 groups of linearly independent observation values are required to solve it. In practice, when the transformation between N carrier poses (assuming that the preset number of transformations of the poses of the carrier relative to the feature object is N', and there are N=N'+1 poses in total) is small, the rotation in a certain direction can not be observed. To ensure the observability of the calibration parameters, the observability score S is defined:

[0143]

[0144]

[0145] wherein tr represents a preset matrix trace function, I 3×3 represents a unit matrix of size 3, T is a transpose, m i is the observation value corresponding to the i-th pose. It should be noted that the observability score is positively correlated with the covariance of the external parameter (the preset conversion matrix to be solved) estimate. Generally, the more the number of equations, the larger the angle between the observation values, and the smaller the observability score, the higher the calibration accuracy and reliability.

[0146] S3302, when the observability score is less than the score threshold, return to the step of transforming the pose of the carrier relative to the feature object until the observability score is greater than or equal to the score threshold.

[0147] Alternatively, the score threshold can be set according to actual needs, and in the embodiment of the present application, the score threshold is taken as 1 for example. Specifically, when S<1, it is considered that the observability is poor, and the step of transforming the pose of the carrier relative to the feature object is returned, i.e. the pose of the carrier relative to the feature object is transformed again, and then new second point cloud data and new second IMU data are obtained until the observability score is greater than or equal to 1.

[0148] S3303, when the observability score is greater than or equal to the score threshold, the target conversion matrix between the laser radar and the IMU is calculated according to the first set and the second set.

[0149] Specifically, when the observability score is greater than or equal to 1, step S400 is performed to calculate the target conversion matrix between the laser radar and the IMU according to the first set and the second set.

[0150] Compared with the prior art, the laser radar and IMU calibration method of the embodiment of the present application has the following advantages:

[0151] 1) By selecting the features in the scene that are perpendicular or parallel to the gravity direction, such as planar features, rod / cylinder features as observation objects to obtain point clouds, and obtaining IMU data, fitting and calculating the representation of the gravity direction in the laser radar coordinate system and the representation in the IMU coordinate system, constructing the geometric constraint of LiDAR-IMU based on the common gravity direction, solving the rotation external parameter based on the eigenvalue decomposition method, not relying on GNSS receiver, camera and other sensors, easy to implement in engineering, reducing the requirements for calibration conditions, and compared with the method based on filtering and graph optimization, the algorithm principle is simple, and the initial value of the external parameter is not required

[0152] 2) The carrier is fixed and placed still during data collection, and the first point cloud data, the second point cloud data, the first IMU data and the second IMU data are recorded, in order to ensure the observability of calibration, the pose of the carrier is transformed, and the feature target is observed from multiple different angles. The static scanning mode is adopted, which is not affected by the time synchronization of the laser radar and the IMU, does not need to compensate the motion of the point cloud, and also does not need to estimate the motion trajectory of the IMU and the LiDAR, so that the calibration process is simplified.

[0153] 3) The observability score of the calibration is calculated from the observation value vector set, and the threshold is set to control the accuracy and reliability of the calibration result.

[0154] The embodiment of the application also provides a laser radar and IMU calibration device, which comprises:

[0155] The first acquisition module is used for acquiring the first point cloud data of the feature object by the laser radar and acquiring the first IMU data by the IMU; the feature object is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed to the carrier;

[0156] The second acquisition module is used for transforming the pose of the carrier relative to the feature object for a preset number of times, and acquiring the second point cloud data of the feature object by the laser radar and acquiring the second IMU data by the IMU after each transformation;

[0157] The first calculation module is used for calculating the first original observation value of the gravity vector in the IMU coordinate system according to the first IMU data, calculating the second original observation value of the gravity vector in the laser radar coordinate system according to the first point cloud data, calculating the first transformed observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating the second transformed observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observation value and the first transformed observation value constitute a first set, and the second original observation value and the second transformed observation value constitute a second set;

[0158] The second calculation module is used for calculating the target conversion matrix between the laser radar and the IMU according to the first set and the second set.

[0159] The contents in the method embodiments are applicable to the device embodiments, the device embodiments specifically implement the functions same as the method embodiments, and achieve the same beneficial effects as the method embodiments, which will not be repeated.

[0160] The embodiment of the application further provides another laser radar and IMU calibration device, which comprises a processor and a memory, and the memory stores at least one instruction, at least one program, a code set or an instruction set, which are loaded and executed by the processor to implement the laser radar and IMU calibration method in the foregoing embodiment. Optionally, the laser radar and IMU calibration device comprises, but is not limited to, a mobile phone, a tablet computer, a computer, a vehicle-mounted computer and the like.

[0161] The contents in the method embodiments are applicable to the device embodiments, the device embodiments specifically implement the functions same as the method embodiments, and achieve the same beneficial effects as the method embodiments, which will not be repeated.

[0162] The embodiment of the application further provides a computer readable storage medium, which stores at least one instruction, at least one program, a code set or an instruction set, which are loaded and executed by the processor to implement the laser radar and IMU calibration method in the foregoing embodiment.

[0163] The embodiment of the application further provides a computer program product or a computer program, which comprises computer instructions stored in a computer readable storage medium. The processor of a computer device reads the computer instructions from the computer readable storage medium, and the processor executes the computer instructions, so that the computer device executes the laser radar and IMU calibration method in the foregoing embodiment.

[0164] The terms "first", "second", "third", "fourth" and the like (if any) in the specification and the above drawings of the present application are used to distinguish similar objects, and do not necessarily indicate a specific order or a chronological sequence. It should be understood that the data thus used can be interchanged under appropriate circumstances, so that the embodiments of the application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "comprise" and "have" and any variations thereof are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device comprising a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but can include other steps or units not clearly listed or inherent to the process, method, product or device.

[0165] It should be understood that, in the application, "at least one" means one or more, "multiple" means two or more. "And / or" is used to describe the relationship between the associated objects, which means that there can be three relationships, for example, "A and / or B" can represent: only A, only B, and A and B exist at the same time, where A and B can be singular or plural. The character " / " generally represents an "or" relationship between the associated objects. "At least one of the following" or similar expressions means any combination of these items, including any combination of single or multiple items. For example, at least one of a, b or c, can represent: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, and c can be single or multiple.

[0166] In several embodiments provided in the application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only illustrative, for example, the division of units is only a logical function division, and actual implementation can have another division method, for example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the displayed or discussed units can be indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms. The units described as separate components can be or can not be physically separated, and the components shown as units can be or can not be physical units, that is, they can be located in one place, or they can be distributed on multiple network units. According to actual needs, some or all of the units can be selected to achieve the purpose of the embodiment scheme. In addition, the functional units in each embodiment of the application can be integrated in one processing unit, or each unit can be physically present, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0167] The integrated unit, if implemented in the form of a software function unit and sold or used as an independent product, can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application or the part of the prior art that essentially contributes or the whole or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes multiple instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the embodiments of the present application. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various program storage media.

[0168] The above, the above embodiments are only to illustrate the technical solutions of the present application, not to limit them; although the present application is described in detail with reference to the foregoing embodiments, those skilled in the art should understand that the technical solutions recorded in the foregoing embodiments can still be modified, or some technical features can be replaced by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for calibrating a lidar and an IMU, the method comprising: The application comprises: acquiring first point cloud data of a feature object by a laser radar and acquiring first IMU data by an IMU; the feature object is parallel or perpendicular to a plumb line, and the laser radar and the IMU are fixed on a carrier; transforming the attitude of the carrier relative to the feature object for a preset number of times, and acquiring second point cloud data of the feature object by the laser radar and acquiring second IMU data by the IMU after each transformation; calculating a first original observation value of a gravity vector in an IMU coordinate system according to the first IMU data, calculating a second original observation value of the gravity vector in a laser radar coordinate system according to the first point cloud data, calculating a first transformed observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformed observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observation value and the first transformed observation value form a first set, and the second original observation value and the second transformed observation value form a second set; calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set; the method for calculating the second original observation value of the gravity vector in the laser radar coordinate system according to the first point cloud data comprises: when the feature object is a first plane perpendicular to the plumb line: extracting a first point set located on the first plane from the first point cloud data; constructing a first distance equation of each point in the first point set to the first plane according to a normal vector of the first plane to be optimized and the first point set; performing fitting processing on the first distance equation to calculate a first target normal vector of the first plane that makes the sum of the results of the first distance equation minimum as the second original observation value of the gravity vector in the laser radar coordinate system; when the feature object is a second plane and a third plane parallel to the plumb line, and the second plane and the third plane intersect: extracting a second point set located on the second plane and a third point set located on the third plane from the first point cloud data respectively; constructing a second distance equation of each point in the second point set to the second plane according to a normal vector of the second plane to be optimized and the second point set, and constructing a third distance equation of each point in the third point set to the third plane according to a normal vector of the third plane to be optimized; performing fitting processing on the second distance equation to calculate a second target normal vector of the second plane that makes the sum of the results of the second distance equation minimum, and performing fitting processing on the third distance equation to calculate a third target normal vector of the third plane that makes the sum of the results of the third distance equation minimum; taking the cross product of the second target normal vector and the third target normal vector as the second original observation value of the gravity vector in the laser radar coordinate system.

2. The method of claim 1, wherein: the method for calculating the first original observation value of the gravity vector in the IMU coordinate system according to the first IMU data comprises: determining an observation value of an accelerometer according to the first IMU data; calculating a first difference between the observation value of the accelerometer and a preset zero offset of the accelerometer; According to a ratio of the first difference and a modulus of the first difference, a first original observation value of a gravity vector in an IMU coordinate system is obtained.

3. The method of claim 2, wherein: The first IMU data includes values of a plurality of groups of accelerometers, and the determining of the observation value of the accelerometer according to the first IMU data includes: According to the values of the plurality of groups of accelerometers, a mean value of a first coordinate axis, a mean value of a second coordinate axis and a mean value of a third coordinate axis of the IMU coordinate system are calculated; and the mean value of the first coordinate axis, the mean value of the second coordinate axis and the mean value of the third coordinate axis constitute the observation value of the accelerometer.

4. The method of claim 1, wherein: The calculating of the second original observation value of the gravity vector in the LiDAR coordinate system according to the first point cloud data includes: When the feature object is an object, an axis of the object is parallel to the plumb line, and a fourth point set located on the object is extracted from the first point cloud data: According to a unit vector of an axis to be optimized and the fourth point set, a fourth distance equation of each point in the fourth point set to a surface of the feature object is constructed; The fourth distance equation is fitted to calculate a target unit vector of the axis that minimizes a sum of results of the fourth distance equation as the second original observation value of the gravity vector in the LiDAR coordinate system.

5. The method of claim 1-4, wherein: The calculating of the target conversion matrix between the LiDAR and the IMU according to the first set and the second set includes: A first mean value of the first set and a second mean value of the second set are calculated; A difference between each observation value in the first set and the first mean value is calculated to obtain first coordinate information, and a difference between each observation value in the second set and the second mean value is calculated to obtain second coordinate information; According to the first coordinate information, the second coordinate information and a preset trace function, a symmetric matrix equation is constructed; A target eigenvector corresponding to a maximum value of a result of the symmetric matrix equation is determined by calculating the symmetric matrix equation; The target eigenvector is substituted into a preset conversion matrix to be solved between the LiDAR and the IMU to obtain the target conversion matrix between the LiDAR and the IMU.

6. The method of claim 1-4, wherein: Before the calculating of the target conversion matrix between the LiDAR and the IMU according to the first set and the second set, the method further includes: According to the preset number of times, the second transformed observation value and a preset trace function, an observability score is calculated; When the observability score is less than a score threshold, the step of returning the posture of the carrier relative to the feature object for the preset number of times of transformation is performed until the observability score is greater than or equal to the score threshold; When the observability score is greater than or equal to the score threshold, the target conversion matrix between the LiDAR and the IMU is calculated according to the first set and the second set.

7. A device for calibrating a lidar and an IMU, characterized in that, The method includes: A first acquisition module is configured to acquire first point cloud data of a feature object by a LiDAR and acquire first IMU data by an IMU; the feature object is parallel or perpendicular to a plumb line, and the LiDAR and the IMU are fixed to a carrier. The second acquisition module is configured to transform the pose of the carrier relative to the feature object for a preset number of times, and after each transformation, the second point cloud data of the feature object is acquired by the laser radar and the second IMU data is acquired by the IMU; The first calculation module is configured to calculate a first original observation value of the gravity vector in the IMU coordinate system according to the first IMU data, calculate a second original observation value of the gravity vector in the laser radar coordinate system according to the first point cloud data, calculate a first transformed observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculate a second transformed observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observation value and the first transformed observation value constitute a first set, and the second original observation value and the second transformed observation value constitute a second set; The second calculation module is configured to calculate the target conversion matrix between the laser radar and the IMU according to the first set and the second set. The second original observation value of the gravity vector in the laser radar coordinate system calculated according to the first point cloud data comprises: When the feature object is a first plane perpendicular to the plumb line, a first point set located on the first plane is extracted from the first point cloud data; a first distance equation of each point in the first point set to the first plane is constructed according to the normal vector of the first plane to be optimized and the first point set; the first distance equation is fitted to calculate a first target normal vector of the first plane that minimizes the sum of the results of the first distance equation as the second original observation value of the gravity vector in the laser radar coordinate system; When the feature object is a second plane and a third plane parallel to the plumb line, and the second plane and the third plane intersect, a second point set located on the second plane and a third point set located on the third plane are extracted from the first point cloud data, respectively; a second distance equation of each point in the second point set to the second plane is constructed according to the normal vector of the second plane to be optimized and the second point set, and a third distance equation of each point in the third point set to the third plane is constructed according to the normal vector of the third plane to be optimized; the second distance equation is fitted to calculate a second target normal vector of the second plane that minimizes the sum of the results of the second distance equation, and the third distance equation is fitted to calculate a third target normal vector of the third plane that minimizes the sum of the results of the third distance equation; the cross product of the second target normal vector and the third target normal vector is taken as the second original observation value of the gravity vector in the laser radar coordinate system.

8. A device for calibrating a lidar and an IMU, characterized in that: The calibration device of the laser radar and the IMU comprises a processor and a memory, the memory stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by the processor to realize the method in any one of claims 1-6.

9. A computer-readable storage medium, characterized in that: The storage medium stores at least one instruction, at least one program, a code set or an instruction set, which is loaded and executed by the processor to implement the method as claimed in any one of claims 1-6.

Images