CN116105772A - Laser radar and IMU calibration method, device and storage medium - Google Patents
Laser radar and IMU calibration method, device and storage medium Download PDFInfo
- Publication number
- CN116105772A CN116105772A CN202310159585.0A CN202310159585A CN116105772A CN 116105772 A CN116105772 A CN 116105772A CN 202310159585 A CN202310159585 A CN 202310159585A CN 116105772 A CN116105772 A CN 116105772A
- Authority
- CN
- China
- Prior art keywords
- imu
- laser radar
- calculating
- coordinate system
- plane
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manufacturing & Machinery (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention discloses a method, a device and a storage medium for calibrating a laser radar and an IMU (inertial measurement unit). The method comprises the steps of acquiring first point cloud data of a feature object through the laser radar and acquiring first IMU data through the IMU, transforming the gesture of a carrier corresponding to the feature object for preset times, acquiring second point cloud data of the feature object through the laser radar and acquiring second IMU data through the IMU after each transformation, so as to calculate a first original observed value and a first transformation observed value of a gravity vector in an IMU coordinate system, and calculating a second original observed value and a second transformation observed value of the gravity vector in the laser radar coordinate system, wherein the second original observed value and the second transformation observed value of the gravity vector are used for calculating a target transformation matrix between the laser radar and the IMU, and the feature object is used as the feature acquired point cloud data and is constrained with the IMU data, so that additional artificial targets and sensors are not needed to be relied on, and the requirement of calibrating conditions is reduced; by adopting a static scanning mode, the method is free from the influence of time synchronization of the laser radar and the IMU, and does not need to calibrate the motion distortion of the point cloud and estimate the motion track.
Description
Technical Field
The invention relates to the field of calibration, in particular to a method and a device for calibrating a laser radar and an IMU and a storage medium.
Background
The combination of the laser radar (Light Detection and Ranging, liDAR) and the inertial measurement unit (Inertial Measurement Unit, IMU) is a common multi-source sensor fusion mode, and the inertial navigation high-frequency angular velocity and acceleration information can provide a better priori value for laser point cloud registration, and the inertial odometer and the laser odometer are tightly coupled to improve the accuracy and the robustness of pose estimation. Each sensor has a respective space reference and a respective time reference, and before the data of the two sensors are fused for positioning, mapping and sensing, the measured values of the different sensors are converted into the same coordinate system, namely the rotation and translation relations among the coordinate systems of the sensors are acquired to realize calibration.
Nowadays, the calibration of the laser radar and the inertial measurement unit is usually carried out by a hand-eye calibration method, an online estimation method, a characteristic-based method and the like, wherein the hand-eye calibration method relies on an IMU to independently estimate the track, and the accuracy is low; the online estimation method needs sufficient linear acceleration and angular velocity excitation to enable the filtering convergence, and if the error of the initial value is large, the filtering convergence may be slow or fail; feature-based methods require that LiDAR and IMU have been synchronized accurately in time, and that the point cloud distortion caused by motion be calibrated, the impact of the motion trajectory on the observability of external parameters is complex, leading to degradation in a certain direction if the motion of the carrier is limited or the pose change is insufficient.
Disclosure of Invention
In view of the above, in order to solve at least one of the above technical problems, an object of the present invention is to provide a method, an apparatus, a device and a storage medium for calibrating a laser radar and an IMU, which reduce the requirement of calibration conditions and simplify the calibration process.
The embodiment of the invention provides a laser radar and IMU calibration method, which comprises the following steps:
acquiring first point cloud data of a feature object through a laser radar and acquiring first IMU data through an IMU; the feature is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed on a carrier;
transforming the gesture of the carrier relative to the feature object for preset times, acquiring second point cloud data of the feature object through the laser radar after each transformation, and acquiring second IMU data through the IMU;
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 transformation observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformation observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observations and the first transformed observations form a first set, and the second original observations and the second transformed observations form a second set;
A target transformation matrix between the lidar and the IMU is calculated from the first set and the second set.
Further, the computing a first original observation of a gravity vector in an IMU coordinate system according to the first IMU data includes:
determining an observation value of an accelerometer according to the first IMU data;
calculating a first difference between the observed value of the accelerometer and a preset zero offset of the accelerometer;
and obtaining a first original observed value of the gravity vector in the IMU coordinate system according to the ratio of the first gap to the modulus of the first gap.
Further, the first IMU data includes a plurality of sets of accelerometer values, the determining an accelerometer observation from the first IMU data includes:
according to the numerical values of the accelerometers, calculating the mean value of a first coordinate axis, the mean value of a second coordinate axis and the mean value of a third coordinate axis of the IMU coordinate system; and the average value of the first coordinate axis, the average value of the second coordinate axis and the average value of the third coordinate axis form an observation value of the accelerometer.
Further, the calculating a second original observed value of the gravity vector in the laser radar coordinate system according to the first point cloud data includes:
When the feature is a first plane perpendicular to the plumb line: extracting a first set of points located in the first plane from the first point cloud data; constructing a first distance equation from each point in the first point set to the first plane according to the normal vector of the first plane to be optimized and the first point set; fitting the first distance equation, and calculating a first target normal vector of a first plane which enables the sum of results of the first distance equation to be minimum as a second original observed value of a gravity vector in a laser radar coordinate system;
when the feature is a second plane and a third plane parallel to the plumb line, the second plane and the third plane intersect: extracting a second point set located in the second plane and a third point set located in the third plane from the first point cloud data respectively; constructing a second distance equation from each point in the second point set to the second plane according to the normal vector of the second plane to be optimized and the second point set, and constructing a third distance equation from each point in the third point set to the third plane according to the normal vector of the third plane to be optimized and the third point set; performing fitting processing on the second distance equation, calculating a second target normal vector of a second plane which enables the sum of the results of the second distance equation to be minimum, performing fitting processing on the third distance equation, and calculating a third target normal vector of a third plane which enables the sum of the results of the third distance equation to be minimum; and taking the cross product of the second target normal vector and the third target normal vector as a second original observed value of the gravity vector in the laser radar coordinate system.
Further, the calculating a second original observed value of the gravity vector in the laser radar coordinate system according to the first point cloud data includes:
when the feature is an object, the axis of the object is parallel to the plumb line, and a fourth point set positioned on the object is extracted from the first point cloud data:
constructing a fourth distance equation from each point in the fourth point set to the surface of the feature according to the unit vector of the axis to be optimized and the fourth point set;
and performing fitting processing on the fourth distance equation, and calculating a target unit vector of an axis which enables the sum of the results of the fourth distance equation to be minimum as a second original observed value of the gravity vector in the laser radar coordinate system.
Further, the computing a target transformation matrix between the lidar and the IMU from the first set and the second set includes:
calculating a first mean of the first set and a second mean of the second set;
calculating the difference value between each observed value in the first set and the first mean value to obtain first coordinate information, and calculating the difference value between each observed value in the second set and the second mean value to obtain second coordinate information;
Constructing a symmetrical matrix equation according to the first coordinate information, the second coordinate information and a preset matrix trace function;
calculating the symmetric matrix equation, and determining a target feature vector corresponding to the maximum value of the result of the symmetric matrix equation;
substituting the target feature vector into a preset conversion matrix to be solved between the laser radar and the IMU to obtain a target conversion matrix between the laser radar and the IMU.
Further, before the step of calculating a target transformation matrix between the lidar and the IMU according to the first set and the second set, the method further includes:
calculating an observability score according to the preset times, the second transformation observation value and a preset matrix trace solving function;
returning to the step of transforming the pose of the carrier relative to the feature for a preset number of times when the observability score is less than a score threshold until the observability score is greater than or equal to the score threshold;
and when the observability score is greater than or equal to a score threshold, calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set.
The embodiment of the invention also provides a calibration device of the laser radar and the IMU, which comprises the following steps:
the first acquisition module is used for acquiring first point cloud data of the feature object through the laser radar and acquiring first IMU data through the IMU; the feature is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed on a carrier;
the second acquisition module is used for transforming the gesture of the carrier relative to the feature object for preset times, acquiring second point cloud data of the feature object through the laser radar after each transformation, and acquiring second IMU data through the IMU;
the first calculation module is used for 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 transformation observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformation observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observations and the first transformed observations form a first set, and the second original observations and the second transformed observations form a second set;
And the second calculation module is used for calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set. The embodiment of the invention also provides a calibration device of the laser radar and the IMU, which comprises a processor and a memory, wherein at least one instruction, at least one section of program, a code set or an instruction set is stored in the memory, and the at least one instruction, the at least one section of program, the code set or the instruction set is loaded and executed by the processor to realize the method.
Embodiments of the present invention also provide a computer-readable storage medium having stored therein at least one instruction, at least one program, a set of codes, or a set of instructions, the at least one instruction, the at least one program, the set of codes, or the set of instructions being loaded and executed by a processor to implement the method.
The beneficial effects of the invention are as follows:
acquiring first point cloud data of a feature object through a laser radar and acquiring first IMU data through an IMU, wherein the feature object is parallel or perpendicular to a plumb line, transforming the gesture of the carrier relative to the feature object for preset times, acquiring second point cloud data of the feature object through the laser radar after each transformation, and acquiring second IMU data through the IMU; 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 the laser radar coordinate system is calculated, according to the second IMU data, a first transformation observation value of the gravity vector in the laser radar coordinate system is calculated, according to the second point cloud data, a first set and a second set are constructed, according to the first set and the second set, a target conversion matrix between the laser radar and the IMU is calculated, in the calibration process, point cloud data are acquired by taking feature objects as features, and constraint is constructed with the IMU data, no additional artificial targets or sensors are needed, the calibration condition requirements are reduced, and the calibration process is simplified; the IMU data and the point cloud data are acquired in a static scanning mode, the influence of time synchronization of the laser radar and the IMU is avoided, the motion distortion of the point cloud is not required to be calibrated, the motion track of the IMU and the motion track of the LiDAR are not required to be estimated, and the calibration process is simplified.
For a better understanding and implementation, the present invention is described in detail below with reference to the drawings.
Drawings
FIG. 1 is a schematic flow chart of the steps of a method for calibrating a laser radar and an IMU according to the present invention;
FIG. 2 is a schematic diagram of a carrier for changing the attitude relative to a feature in accordance with an embodiment of the present invention;
FIG. 3 is a schematic diagram of various types of features in accordance with an embodiment of the present invention.
Detailed Description
In order to make the present application solution better understood by those skilled in the art, the following description will be made in detail and with reference to the accompanying drawings in the embodiments of the present application, it is apparent that the described embodiments are only some embodiments of the present application, not all embodiments. All other embodiments, which can be made by one of ordinary skill in the art based on the embodiments herein without making any inventive effort, shall fall within the scope of the present application.
The terms "first," "second," "third," and "fourth" and the like in the description and in the claims of this application and in the drawings, are used for distinguishing between different objects and not for describing a particular sequential order. Furthermore, the terms "comprise" and "have," as well as any variations thereof, are intended to cover a non-exclusive inclusion. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those listed steps or elements but may include other steps or elements not listed or inherent to such process, method, article, or apparatus.
Reference herein to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment may be included in at least one embodiment of the present application. The appearances of such phrases in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. Those of skill in the art will explicitly and implicitly appreciate that the embodiments described herein may be combined with other embodiments.
As shown in fig. 1, an embodiment of the present invention provides a method for calibrating a laser radar and an IMU, including steps S100 to S400:
s100, acquiring first point cloud data of the feature object through a laser radar and acquiring first IMU data through an IMU.
In the embodiment of the invention, the laser radar and the IMU are fixed on the carrier, and the feature is parallel or perpendicular to the plumb line. It should be noted that, in the embodiment of the present invention, the lidar and the IMU are mounted at different positions of the carrier, and are rigidly connected, the IMU has an IMU coordinate system (b system), the IMU coordinate system coincides with the carrier coordinate system, an X axis (first coordinate axis) is defined to point to the right, a Y axis (second coordinate axis) points to the front, and a Z axis (third coordinate axis) points to the top; the lidar has a scanning coordinate system (denoted as lidar coordinate system, i.e. l-system) which is also a right-hand coordinate system, and a "northeast" geographic coordinate system is defined as navigation coordinate system (n-system).
Specifically, 1) first select a calibration site: selecting an artificial scene with the following characteristics as a calibration site:
a. the method comprises the steps of (1) measuring the levelness of a plane by an electronic level meter, and selecting the levelness smaller than 0.1 DEG and 1m 2 The left and right planar areas serve as features. In actual production, a mode of manually arranging the calibration plates can be adopted, the calibration plates are horizontally and fixedly arranged on the bases with the four corners adjustable in height, and the calibration plates are precisely leveled through the electronic level instrument, so that higher calibration precision is obtained.
b. The corners in the structured scene have two non-parallel adjacent vertical walls, requiring that the walls be approximately parallel to the plumb line.
c. Vertical bars/columns in a structured scene require that the bars/columns be approximately parallel to the plumb line.
2) And acquiring calibration data, fixing a carrier carrying the laser radar and the IMU at a certain position, requiring that a characteristic target is within the view field range of the laser radar and the distance is not more than 15m, starting the laser radar and the IMU, performing static measurement, recording first point cloud data of a characteristic object acquired by the laser radar, and acquiring first IMU data through the IMU.
S200, transforming the gesture of the carrier corresponding to the feature object for preset times, acquiring second point cloud data of the feature object through a laser radar after each transformation, and acquiring second IMU data through an IMU.
It should be noted that, in the embodiment of the present invention, the first IMU data and the second IMU data refer to IMU data related to gravitational acceleration. The preset times are set according to actual demands, for example, the degree of freedom of a matrix to be solved can be determined, and if the degree of freedom of a target conversion matrix to be finally calculated is 3, the preset times are more than or equal to 3.
In the embodiment of the invention, after the Pose Pose of the carrier relative to the feature is changed each time, the second point cloud data of the feature is acquired through the laser radar and the second IMU data is acquired through the IMU, as shown in fig. 2, Z represents the plumb line direction, and TAG represents the feature.
S300, 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 the laser radar coordinate system according to the first point cloud data, calculating a first transformation observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformation observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data.
In the embodiment of the invention, the first original observation value and the first transformation observation value form a first set, and the second original observation value and the second transformation observation value form a second set.
Optionally, the step S300 of calculating the first original observed value of the gravity vector in the IMU coordinate system according to the first IMU data includes steps S301 to S303:
s301, determining an observation value of the accelerometer according to the first IMU data.
In the embodiment of the present invention, the first IMU data includes a plurality of sets of values of accelerometers, specifically: according to the numerical values of the accelerometers, calculating the average value of the numerical values of the accelerometers in a first coordinate axis, the average value of a second coordinate axis and the average value of a third coordinate axis of an IMU coordinate system; 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 form an observation value of the accelerometer.
It should be noted that, when the carrier is placed stationary, the IMU is only affected by gravity, and in theory, the acceleration felt by the accelerometer mass of the IMU is equal to the gravity acceleration, and opposite to the gravity acceleration, the observed value of the accelerometer can be regarded as the projection of the gravity acceleration on the X, Y, Z axes of the IMU. The observation equation of the accelerometer is:
Wherein:
for the observation of the accelerometer, the mean value of the first coordinate axis of the IMU coordinate system
Mean value of the second coordinate axis->
And the mean value of the third axis +.>
Constitution (S)>
Direction cosine matrix g representing IMU coordinate system relative to navigation coordinate system n ＝[0 0 g] T T is a transposition, the superscript n represents a navigation coordinate system (n system), g is gravitational acceleration, b a The preset zero offset for the accelerometer can be obtained through pre-calibration.
S302, calculating a first difference between an observed value of the accelerometer and a preset zero offset of the accelerometer.
S303, obtaining a first original observation value of the gravity vector in the IMU coordinate system according to the ratio of the first gap to the modulus of the first gap.
Specifically, the vectorization in the formula (1) is unitized in consideration of the mode length invariance of the vector before and after rotation:
order the
F is the same as that of the above b -b a As a first difference between the first and second differences, the term "f" represents the modulus of the model b -b a And I is a module of the first difference, and m is a first original observed value of the gravity vector in the IMU coordinate system.
Optionally, the step S300 of calculating the second original observed value of the gravity vector in the lidar coordinate system according to the first point cloud data includes steps S311-S312 or S321, S322, where S311, S312, S321 do not define the execution sequence:
s311, when the feature is a first plane perpendicular to the plumb line: extracting a first set of points located in a first plane from the first point cloud data; constructing a first distance equation from each point in the first point set to the first plane according to the normal vector of the first plane to be optimized and the first point set; fitting the first distance equation, and calculating a first target normal vector of a first plane which enables the sum of the results of the first distance equation to be minimum as a second original observed value of the gravity vector in the laser radar coordinate system.
In the embodiment of the present invention, when the feature is a first plane perpendicular to the plumb line, such as the first plane 101 shown in fig. 3, a first point set p= { P located on the first plane is extracted from the first point cloud data 1 ，p 2 ，p 3 ，…，p k }，p k Is the kth point in the first set of points. Specifically, the plane equation corresponding to the first plane is:
ax+by+cz-d＝0 (5)
wherein n= [ a, b, c ]] T Representing the normal vector of the first plane to be optimized, the normal vector being in particular a unit normal vector, a, b, c representing the components of different coordinate axes, d representing the distance from the origin of the LiDAR coordinate system to the first plane, [ x, y, z ]]The three-dimensional coordinates of any point on the first plane under the LiDAR coordinate system are the three-dimensional coordinates of any point in the first point set under the LiDAR coordinate system.
Thus, any point in the first point setp i Distance to plane d i The first distance equation of (2) is:
the optimal plane parameters should minimize the following objective function values:
in the embodiment of the invention, because noise inevitably exists in the collected first point cloud data, in order to eliminate the influence of abnormal values on the precision of plane parameters, a RANSAC method is used for fitting plane parameters, namely, a RANSAC method is used for fitting a first distance equation, the fitting process is shown in a formula (7), a first target normal vector of a first plane which enables the sum of the results of the first distance equation to be minimum is calculated as a second original observed value of a gravity vector in a laser radar coordinate system, namely, the fitting result of the formula (7) is satisfied, n is the first target normal vector of the first plane at the moment and is used as a second original observed value of the gravity vector in the laser radar coordinate system, and the first target normal vector is parallel to the gravity vector and is represented by the gravity direction in the LiDAR coordinate system.
S312, when the feature is a second plane and a third plane parallel to the plumb line, the second plane and the third plane intersect: extracting a second point set positioned on a second plane and a third point set positioned on a third plane from the first point cloud data respectively; constructing a second distance equation from each point in the second point set to the second plane according to the normal vector of the second plane to be optimized and the second point set, and constructing a third distance equation from each point in the third point set to the third plane according to the normal vector of the third plane to be optimized and the third point set; fitting the second distance equation, calculating a second target normal vector of a second plane which minimizes the sum of the results of the second distance equation, fitting the third distance equation, and calculating a third target normal vector of a third plane which minimizes the sum of the results of the third distance equation; and taking the cross product of the second target normal vector and the third target normal vector as a second original observed value of the gravity vector in the laser radar coordinate system.
In the embodiment of the present invention, when the feature is a second plane and a third plane parallel to the plumb line, the second plane and the third plane intersect, as shown in 102 in fig. 3, including the intersecting second plane and third plane, the second point set located on the second plane and the third point set located on the third plane are extracted from the first point cloud data, respectively. And then, constructing a second distance equation from each point in the second point set to the second plane according to the normal vector of the second plane to be optimized and the second point set, and constructing a third distance equation from each point in the third point set to the third plane according to the normal vector of the third plane to be optimized and the third point set. It should be noted that, the second distance equation and the third distance equation are similar to the equation (6), and will not be described again.
Specifically, fitting processing is performed on the second distance equation, a second target normal vector of the second plane, which minimizes the sum of the results of the second distance equation, is calculated, fitting processing is performed on the third distance equation, and a third target normal vector of the third plane, which minimizes the sum of the results of the third distance equation, is calculated. It should be noted that, the principle of the fitting process is shown in formula (7), and will not be described again. At this time, a second target normal vector n can be obtained 1 Third target normal vector n 2 。
Then, calculate the second target normal vector n 1 Third target normal vector n 2 Is defined by the cross product of:
n＝n 1 ×n 2 (8)
the cross product n in the formula (8) is the second original observed value of the gravity vector in the laser radar coordinate system.
S321, when the feature object is an object, the axis of the object is parallel to the plumb line, and a fourth point set positioned on the object is extracted from the first point cloud data.
Optionally, a fourth set of points located on the object is extracted from the first point cloud data, and when the feature is an object, for example, 103 as shown in fig. 3, the object may be a shaft, a column, or the like.
S322, constructing a fourth distance equation from each point in the fourth point set to the surface of the feature object according to the unit vector of the axis to be optimized and the fourth point set;
And performing fitting processing on the fourth distance equation, and calculating a target unit vector of an axis which enables the sum of the results of the fourth distance equation to be minimum as a second original observed value of the gravity vector in the laser radar coordinate system.
In the embodiment of the invention, taking an object as a cylinder as an example, the axis of the cylinder is the central axis of the cylinder, and the surface of the feature is the surface of the cylinder, such as a cylindrical surface. The equation for the axis can be expressed as:
wherein: [ x, y, z]For the three-dimensional coordinates of any point on the fourth point set under the LiDAR coordinate system, [ x ] 0 ，y 0 ，z 0 ]As the coordinates of the axis start point, λ represents a scale factor, n' = [ n ] x ，n y ，n z ] T N is a unit vector in the axial direction x ，n v ，n z Representing components of different coordinate axes.
Wherein, a fourth distance equation of the distance di from any point on the fourth point set to the cylindrical surface is:
d i ＝|μ×n′|-r (10)
wherein μ= [ x-x ] 0 ，y-y 0 ，z-z 0 ]R is the radius of the cylinder. Then, fitting the fourth distance equation, and optimizing the object parameters so that the following objective function values are the smallest:
where k is the number of points of the fourth set of points.
In the embodiment of the invention, the RANSAC method is used for fitting the object parameters, namely, the RANSAC method is used for fitting the fourth distance equation, the fitting process is shown in a formula (11), and the target unit vector of the axis which enables the sum of the results of the fourth distance equation to be minimum is calculated as the second original observed value of the gravity vector in the laser radar coordinate system.
The observed value of the gravity direction in the lidar coordinate system fitted by the three types of features (101, 102, 103 in fig. 3) is related to the actual gravity vector as follows:
wherein the matrix
And (5) representing a directional cosine matrix of the laser radar coordinate system relative to the navigation coordinate system. In the embodiment of the invention, in order to ensure that the relative relation among the normal vectors is kept consistent in the LiDAR and IMU coordinate systems, the forward direction of the unit vector n' points to the opposite direction of gravity.
S400, calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set.
In the embodiment of the invention, the geometric constraint is constructed to solve the target transformation matrix later. Specifically:
constructing a constraint by taking a gravity vector as a common observation value of the IMU and the laser radar, and combining the formula (4) and the formula (12):
eliminating g n The method can obtain:
due to
The constraint equation can be expressed as:
wherein m and n are column vectors,
the method is a directional cosine matrix of the laser radar coordinate system relative to the IMU coordinate system, namely a preset conversion matrix to be solved. But->
The form of (2) is:
wherein: q= [ q ] 0 q 1 q 2 q 3 ]Is a unit quaternion.
Optionally, step S400 includes steps S410-S450:
s410, calculating a first average value of the first set and a second average value of the second set.
It should be noted that, the degree of freedom of the preset transformation matrix to be solved in the embodiment of the present invention is 3, so that at least 3 groups of observations that are not related to linearity are needed to solve, and therefore, the preset number of times is not less than 3), it is assumed that N equations can be listed according to equation (15), and the first original observations and the first transformed observations form a first set { m } 1 ，m 2 ，…，m N Second original observations and second transformed observations form a second set { n } 1 ，n 2 ，…，n N And solving the rotation parameters according to the coordinate conversion mode of the points.
Specifically, a first mean value of the first set is calculated
And a second mean n of the second set: />
S420, calculating the difference value between each observed value in the first set and the first mean value to obtain first coordinate information, and calculating the difference value between each observed value in the second set and the second mean value to obtain second coordinate information.
It should be noted that each observed value m in the first set N Refers to a first original observation or a first transformed observation, each observation n in the second set N Refers to a second original observation or a second transformed observation. Specifically:
the barycentrated coordinate n 'of normal vector' i (second coordinate information), m' i (first coordinate information):
s430, constructing a symmetrical matrix equation according to the first coordinate information, the second coordinate information and a preset matrix trace function.
Specifically, there is a defined matrix:
constructing a symmetrical matrix equation of the symmetrical matrix K:
wherein: z= [ B ] 23 -B 32 B 31 -B 13 B 12 -B 21 ]，B ij Elements representing the ith row and jth column of matrix B, I 3×3 Representing an identity matrix with a size of 3, T being the transpose, tr representing a predetermined matrix trace function.
S440, calculating the symmetrical matrix equation, and determining a target feature vector corresponding to the maximum value of the result of the symmetrical matrix equation.
Specifically, according to the quaternion principle, the result of the symmetry matrix equation for making the symmetry matrix K is determined as a maximum value (maximum eigenvalue) λ max ，λ max Corresponding target feature vector q= [ q ] 0 q 1 q 2 q 3 ]Namely the unit quaternion.
S450, substituting the target feature vector into a preset conversion matrix to be solved between the laser radar and the IMU to obtain a target conversion matrix between the laser radar and the IMU.
Specifically, the target feature vector q= [ q ] 0 q 1 q 2 q 3 ]Substituted into preset conversion matrix to be solved
In the formula (16), a target conversion matrix between the laser radar and the IMU is obtained.
Optionally, the method for calibrating the lidar and the IMU according to the embodiment of the present invention further includes step S330 before step S400, step S330 includes steps S3301 to S3303, wherein S3302 and S3303 do not limit the execution sequence:
S3301, calculating the observability score according to the preset times, the second transformation observed value and the preset matrix trace solving function.
It should be noted that, in the embodiment of the present invention, the degree of freedom of the preset transformation matrix to be solved is 3, so that at least 3 sets of observations that are not linearly related are required to be solved. In practice, when the transition between N carrier poses (assuming that the preset number of times of transition of the carrier pose with respect to the feature is N ', n=n' +1 poses in total) is small, rotation in one direction may be caused to be insignificant. To guarantee the observability of the calibration parameters, an observability score S is defined:
wherein: tr represents a preset matrix trace function, I 3×3 Represents a unitary matrix of size 3, T is transposed, m i And the observation value corresponding to the ith gesture. It should be noted that the observability score is positively correlated with the covariance of the extrinsic (preset transition matrix to be solved) estimates. In general, the larger the number of equations, the larger the included angle between observations, the smaller the observability score, and the higher the calibration accuracy and reliability.
S3302, returning to the step of transforming the posture of the carrier relative to the feature when the observability score is smaller than the score threshold value until the observability score is larger than or equal to the score threshold value.
Alternatively, the score threshold may be set according to actual needs, and in this embodiment of the present invention, the score threshold is illustrated as 1. Specifically, when S < 1, the observability is considered to be poor, the step of transforming the attitude of the carrier relative to the feature is returned, that is, the attitude of the carrier relative to the feature is transformed again, and then new second point cloud data and new second IMU data are acquired until the observability score is greater than or equal to 1.
S3303, when the observability score is larger than or equal to the score threshold value, calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set.
Specifically, when the observability score is greater than or equal to 1, step S400 is performed to calculate a target transformation matrix between the lidar and the IMU according to the first set and the second set.
Compared with the prior art, the laser radar and IMU calibration method provided by the embodiment of the invention comprises the following steps:
1) The method comprises the steps of obtaining point clouds by selecting features which are perpendicular or parallel to the gravity direction in a scene, such as planar features and rod/column features, as observation objects, obtaining IMU data, fitting and calculating the representation of the gravity direction in a laser radar coordinate system and the representation of the gravity direction in the IMU coordinate system, constructing the geometric constraint of LiDAR-IMU based on the common gravity direction, solving a rotating external parameter based on a eigenvalue decomposition method, and not depending on sensors such as a GNSS receiver and a camera, thereby being easy for engineering realization, reducing the requirement on calibrated conditions, having simple algorithm principle and not requiring the initial value of external parameters compared with a method based on filtering and graph optimization
2) The carrier is fixedly and statically placed when data are collected, the first point cloud data, the second point cloud data, the first IMU data and the second IMU data are recorded, the pose of the carrier is changed to observe characteristic targets from a plurality of different angles in order to ensure the observability of calibration, a static scanning mode is adopted, the influence of time synchronization of the laser radar and the IMU is avoided, the motion compensation of the point cloud is not required, the motion track of the IMU and the LiDAR is not required to be estimated, and the calibration flow is simplified.
3) And defining a observability score calculated and calibrated by the observation value vector set, and controlling the precision and reliability of a calibration result by setting a threshold value.
The embodiment of the invention also provides a calibration device of the laser radar and the IMU, which comprises the following steps:
the first acquisition module is used for acquiring first point cloud data of the feature object through the laser radar and acquiring first IMU data through the IMU; the feature is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed on the carrier;
the second acquisition module is used for transforming the gesture of the carrier relative to the feature object for preset times, acquiring second point cloud data of the feature object through a laser radar after each transformation, and acquiring second IMU data through an IMU;
the first calculation module is used for calculating a first original observation value of a gravity vector in the IMU coordinate system according to the first IMU data, calculating a second original observation value of the gravity vector in the laser radar coordinate system according to the first point cloud data, calculating a first transformation observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformation observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observations and the first transformed observations form a first set, and the second original observations and the second transformed observations form a second set;
And the second calculation module is used for calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set.
The content in the above method embodiment is applicable to the embodiment of the present device, and functions specifically implemented by the embodiment of the present device are the same as those of the embodiment of the above method, and beneficial effects achieved by the embodiment of the above method are the same as those achieved by the embodiment of the above method, which are not repeated.
The embodiment of the invention also provides another laser radar and IMU calibration device, which comprises a processor and a memory, wherein at least one instruction, at least one section of program, code set or instruction set is stored in the memory, and the at least one instruction, the at least one section of program, the code set or the instruction set is loaded and executed by the processor to realize the laser radar and IMU calibration method in the previous embodiment. Optionally, the calibration device of the laser radar and the IMU includes, but is not limited to, a mobile phone, a tablet computer, a vehicle-mounted computer and the like.
The content in the above method embodiment is applicable to the embodiment of the present device, and functions specifically implemented by the embodiment of the present device are the same as those of the embodiment of the above method, and beneficial effects achieved by the embodiment of the above method are the same as those achieved by the embodiment of the above method, which are not repeated.
The embodiment of the invention also provides a computer readable storage medium, wherein at least one instruction, at least one section of program, code set or instruction set is stored in the storage medium, and the at least one instruction, the at least one section of program, the code set or the instruction set is loaded and executed by a processor to realize the calibration method of the laser radar and the IMU in the previous embodiment.
Embodiments of the present invention also provide a computer program product or computer program comprising computer instructions stored in a computer readable storage medium. The processor of the 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 calibration method of the laser radar and the IMU of the foregoing embodiment.
The terms "first," "second," "third," "fourth," and the like in the description of the present application and in the above-described figures, if any, are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that embodiments of the present application described herein may be implemented in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed or inherent to such process, method, article, or apparatus.
It should be understood that in this application, "at least one" means one or more, and "a plurality" means two or more. "and/or" for describing the association relationship of the association object, the representation may have three relationships, for example, "a and/or B" may represent: only a, only B and both a and B are present, wherein a, B may be singular or plural. The character "/" generally indicates that the context-dependent object is an "or" relationship. "at least one of" or the like means any combination of these items, including any combination of single item(s) or plural items(s). For example, at least one (one) of a, b or c may represent: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", wherein a, b, c may be single or plural.
In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative, e.g., the division of elements is merely a logical functional division, and there may be additional divisions of actual implementation, e.g., multiple elements or components may be combined or integrated into another system, or some features may be omitted, or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be an indirect coupling or communication connection via some interfaces, devices or units, which may be in electrical, mechanical or other form. The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed over a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment. In addition, each functional unit in each embodiment of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied in essence or a part contributing to the prior art or all or part of the technical solution in the form of a software product stored in a storage medium, including multiple instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), a magnetic disk, or an optical disk, or other various media capable of storing a program.
The above embodiments are only for illustrating the technical solution of the present application, and not for limiting the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the corresponding technical solutions.
Claims (10)
1. The method for calibrating the laser radar and the IMU is characterized by comprising the following steps:
acquiring first point cloud data of a feature object through a laser radar and acquiring first IMU data through an IMU; the feature is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed on a carrier;
transforming the gesture of the carrier relative to the feature object for preset times, acquiring second point cloud data of the feature object through the laser radar after each transformation, and acquiring second IMU data through the IMU;
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 transformation observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformation observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observations and the first transformed observations form a first set, and the second original observations and the second transformed observations form a second set;
a target transformation matrix between the lidar and the IMU is calculated from the first set and the second set.
2. The method for calibrating the laser radar and the IMU according to claim 1, wherein the method comprises the following steps: the calculating a first original observed value of a gravity vector in an IMU coordinate system according to the first IMU data includes:
determining an observation value of an accelerometer according to the first IMU data;
calculating a first difference between the observed value of the accelerometer and a preset zero offset of the accelerometer;
and obtaining a first original observed value of the gravity vector in the IMU coordinate system according to the ratio of the first gap to the modulus of the first gap.
3. The method for calibrating the laser radar and the IMU according to claim 2, wherein the method comprises the following steps: the first IMU data includes a number of sets of accelerometer values, and the determining an accelerometer observation from the first IMU data includes:
according to the numerical values of the accelerometers, calculating the mean value of a first coordinate axis, the mean value of a second coordinate axis and the mean value of a third coordinate axis of the IMU coordinate system; and the average value of the first coordinate axis, the average value of the second coordinate axis and the average value of the third coordinate axis form an observation value of the accelerometer.
4. The method for calibrating the laser radar and the IMU according to claim 1, wherein the method comprises the following steps: the calculating a second original observed value of the gravity vector in the laser radar coordinate system according to the first point cloud data comprises the following steps:
When the feature is a first plane perpendicular to the plumb line: extracting a first set of points located in the first plane from the first point cloud data; constructing a first distance equation from each point in the first point set to the first plane according to the normal vector of the first plane to be optimized and the first point set; fitting the first distance equation, and calculating a first target normal vector of a first plane which enables the sum of results of the first distance equation to be minimum as a second original observed value of a gravity vector in a laser radar coordinate system;
when the feature is a second plane and a third plane parallel to the plumb line, the second plane and the third plane intersect: extracting a second point set located in the second plane and a third point set located in the third plane from the first point cloud data respectively; constructing a second distance equation from each point in the second point set to the second plane according to the normal vector of the second plane to be optimized and the second point set, and constructing a third distance equation from each point in the third point set to the third plane according to the normal vector of the third plane to be optimized and the third point set; performing fitting processing on the second distance equation, calculating a second target normal vector of a second plane which enables the sum of the results of the second distance equation to be minimum, performing fitting processing on the third distance equation, and calculating a third target normal vector of a third plane which enables the sum of the results of the third distance equation to be minimum; and taking the cross product of the second target normal vector and the third target normal vector as a second original observed value of the gravity vector in the laser radar coordinate system.
5. The method for calibrating the laser radar and the IMU according to claim 1, wherein the method comprises the following steps: the calculating a second original observed value of the gravity vector in the laser radar coordinate system according to the first point cloud data comprises the following steps:
when the feature is an object, the axis of the object is parallel to the plumb line, and a fourth point set positioned on the object is extracted from the first point cloud data:
constructing a fourth distance equation from each point in the fourth point set to the surface of the feature according to the unit vector of the axis to be optimized and the fourth point set;
and performing fitting processing on the fourth distance equation, and calculating a target unit vector of an axis which enables the sum of the results of the fourth distance equation to be minimum as a second original observed value of the gravity vector in the laser radar coordinate system.
6. The method for calibrating the laser radar and the IMU according to any one of claims 1 to 5, wherein the method comprises the following steps: the computing a target transformation matrix between the lidar and the IMU from the first set and the second set, comprising:
calculating a first mean of the first set and a second mean of the second set;
calculating the difference value between each observed value in the first set and the first mean value to obtain first coordinate information, and calculating the difference value between each observed value in the second set and the second mean value to obtain second coordinate information;
Constructing a symmetrical matrix equation according to the first coordinate information, the second coordinate information and a preset matrix trace function;
calculating the symmetric matrix equation, and determining a target feature vector corresponding to the maximum value of the result of the symmetric matrix equation;
substituting the target feature vector into a preset conversion matrix to be solved between the laser radar and the IMU to obtain a target conversion matrix between the laser radar and the IMU.
7. The method for calibrating the laser radar and the IMU according to any one of claims 1 to 5, wherein the method comprises the following steps: before the step of calculating the target transformation matrix between the lidar and the IMU according to the first set and the second set, the method further includes:
calculating an observability score according to the preset times, the second transformation observation value and a preset matrix trace solving function;
returning to the step of transforming the pose of the carrier relative to the feature for a preset number of times when the observability score is less than a score threshold until the observability score is greater than or equal to the score threshold;
and when the observability score is greater than or equal to a score threshold, calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set.
8. A laser radar and IMU calibration device, comprising:
the first acquisition module is used for acquiring first point cloud data of the feature object through the laser radar and acquiring first IMU data through the IMU; the feature is parallel or perpendicular to the plumb line, and the laser radar and the IMU are fixed on a carrier;
the second acquisition module is used for transforming the gesture of the carrier relative to the feature object for preset times, acquiring second point cloud data of the feature object through the laser radar after each transformation, and acquiring second IMU data through the IMU;
the first calculation module is used for 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 transformation observation value of the gravity vector in the IMU coordinate system according to the second IMU data, and calculating a second transformation observation value of the gravity vector in the laser radar coordinate system according to the second point cloud data; the first original observations and the first transformed observations form a first set, and the second original observations and the second transformed observations form a second set;
And the second calculation module is used for calculating a target conversion matrix between the laser radar and the IMU according to the first set and the second set.
9. The utility model provides a laser radar and IMU's calibration device which characterized in that: the laser radar and IMU calibration device comprises a processor and a memory, wherein at least one instruction, at least one program, a code set or an instruction set is stored in the memory, 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 implement the method according to any one of claims 1-7.
10. A computer-readable storage medium, characterized by: the storage medium having stored therein at least one instruction, at least one program, code set, or instruction set that is loaded and executed by a processor to implement the method of any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310159585.0A CN116105772A (en) | 2023-02-22 | 2023-02-22 | Laser radar and IMU calibration method, device and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310159585.0A CN116105772A (en) | 2023-02-22 | 2023-02-22 | Laser radar and IMU calibration method, device and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116105772A true CN116105772A (en) | 2023-05-12 |
Family
ID=86258004
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310159585.0A Pending CN116105772A (en) | 2023-02-22 | 2023-02-22 | Laser radar and IMU calibration method, device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116105772A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116740197A (en) * | 2023-08-11 | 2023-09-12 | 之江实验室 | External parameter calibration method and device, storage medium and electronic equipment |
-
2023
- 2023-02-22 CN CN202310159585.0A patent/CN116105772A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116740197A (en) * | 2023-08-11 | 2023-09-12 | 之江实验室 | External parameter calibration method and device, storage medium and electronic equipment |
| CN116740197B (en) * | 2023-08-11 | 2023-11-21 | 之江实验室 | External parameter calibration method and device, storage medium and electronic equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP2990828B1 (en) | Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor | |
| US10636168B2 (en) | Image processing apparatus, method, and program | |
| CN112219087A (en) | Pose prediction method, map construction method, movable platform and storage medium | |
| KR20210084622A (en) | Time synchronization processing methods, electronic devices and storage media | |
| CN110268445A (en) | It is calibrated automatically using the camera of gyroscope | |
| CN110470333B (en) | Calibration method and device of sensor parameters, storage medium and electronic device | |
| CN112815939B (en) | Pose estimation method of mobile robot and computer readable storage medium | |
| WO2018107831A1 (en) | Method and apparatus for measuring attitude angle of object | |
| CN113960622A (en) | Real-time positioning method and device fusing laser radar and IMU sensor information | |
| CN116105772A (en) | Laser radar and IMU calibration method, device and storage medium | |
| CN111932637B (en) | Vehicle body camera external parameter self-adaptive calibration method and device | |
| CN114111776A (en) | Positioning method and related device | |
| CN111998870B (en) | Calibration method and device of camera inertial navigation system | |
| CN112284381B (en) | Visual inertia real-time initialization alignment method and system | |
| CN114049401A (en) | Binocular camera calibration method, device, equipment and medium | |
| CN116721166B (en) | Binocular camera and IMU rotation external parameter online calibration method, device and storage medium | |
| CN116182905A (en) | Laser radar and combined inertial navigation space-time external parameter calibration method, device and system | |
| EP3392748B1 (en) | System and method for position tracking in a virtual reality system | |
| CN113405532B (en) | Forward intersection measuring method and system based on structural parameters of vision system | |
| CN113034615B (en) | Equipment calibration method and related device for multi-source data fusion | |
| CN114705223A (en) | Inertial navigation error compensation method and system for multiple mobile intelligent bodies in target tracking | |
| CN111307176A (en) | Online calibration method for visual inertial odometer in VR head-mounted display equipment | |
| CN113494927A (en) | Vehicle multi-sensor calibration method and device and vehicle | |
| CN110082805A (en) | A kind of 3 D locating device and method | |
| CN115439561B (en) | Robot sensor calibration method, robot and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |