CN117092624A - External parameter calibration method, system, medium and equipment - Google Patents
External parameter calibration method, system, medium and equipment Download PDFInfo
- Publication number
- CN117092624A CN117092624A CN202311255316.0A CN202311255316A CN117092624A CN 117092624 A CN117092624 A CN 117092624A CN 202311255316 A CN202311255316 A CN 202311255316A CN 117092624 A CN117092624 A CN 117092624A
- Authority
- CN
- China
- Prior art keywords
- odometer
- inertial measurement
- measurement unit
- laser
- point cloud
- 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
- 238000000034 method Methods 0.000 title claims abstract description 59
- 238000005259 measurement Methods 0.000 claims abstract description 92
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 70
- 230000002068 genetic effect Effects 0.000 claims abstract description 41
- 230000033001 locomotion Effects 0.000 claims abstract description 17
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 238000013178 mathematical model Methods 0.000 claims abstract description 16
- 238000013519 translation Methods 0.000 claims abstract description 12
- 230000001133 acceleration Effects 0.000 claims abstract description 7
- 230000006870 function Effects 0.000 claims description 17
- 230000009466 transformation Effects 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 10
- 238000012216 screening Methods 0.000 abstract description 7
- 210000000349 chromosome Anatomy 0.000 description 16
- 239000011159 matrix material Substances 0.000 description 13
- 239000013598 vector Substances 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000035772 mutation Effects 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 241000208340 Araliaceae Species 0.000 description 2
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 2
- 235000003140 Panax quinquefolius Nutrition 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 235000008434 ginseng Nutrition 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 239000009636 Huang Qi Substances 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- 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
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Manufacturing & Machinery (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention relates to the technical field of external parameter calibration, and provides an external parameter calibration method, an external parameter calibration system, a medium and equipment, wherein the external parameter calibration method comprises the following steps: removing point cloud distortion generated by laser radar motion based on laser mileage data; extracting corner points and face points based on curvature information of the point cloud, and obtaining a laser odometer through an interframe matching algorithm; integrating the acceleration and the angular velocity based on the inertial measurement unit data to obtain an inertial measurement unit odometer; based on the laser odometer and the inertial measurement unit odometer data, performing time stamp alignment on the laser odometer and the inertial measurement unit odometer data to obtain corresponding inertial measurement unit odometer data under each laser odometer time stamp; based on the laser odometer and the inertial measurement unit odometer passing the screening conditions, according to the established external parameter calibration mathematical model, a first genetic algorithm is used for solving the rotation external parameter, and a second genetic algorithm is used for solving the translation external parameter; and the Kalman filtering algorithm is adopted to continuously optimize the external parameters, so that the accuracy of the external parameter calibration is further improved.
Description
Technical Field
The invention belongs to the technical field of external parameter calibration, and particularly relates to an external parameter calibration method, an external parameter calibration system, an external parameter calibration medium and external parameter calibration equipment.
Background
The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art.
With the continuous development of sensor technology and SLAM (Simultaneous Localization And Mapping) technology, positioning and mapping by using a multi-sensor fusion method gradually becomes a trend, especially in the fields of mobile robots, automatic driving, unmanned aerial vehicles and the like. Compared with a single sensor technology, the multi-sensor fusion technology has stronger perceptibility, higher precision and better robustness. In order to fuse the data of multiple sensors, the data collected under different sensor coordinate systems needs to be converted into the same coordinate system for processing, and therefore the coordinate transformation relation between the sensors needs to be obtained, and the multi-sensor calibration technology is a method for solving the problem.
Currently, the conventional external parameter calibration method of the multi-line laser radar and the IMU (Inertial Measurement Unit ) is mostly carried out in an off-line state. One method is to measure the external parameters between the sensors by using high-precision measuring equipment, and the other method needs to collect sensor data for a period of time and then process and analyze the data to obtain the external parameters. These off-line calibration methods are inefficient and do not have real-time. Because the structure connecting the laser radar and the IMU sensor is not an ideal rigid body, once the load, the mechanical structure and the like of the robot change, the external parameters between the sensors can be changed, the SLAM system is sensitive to the accuracy of the external parameters of the sensors, and the quality of SLAM drawing can be influenced by the change of the external parameters. Therefore, the off-line calibration method cannot correct the external parameters between the sensors in time and has certain limitation.
The other external parameter calibration method of the multi-line laser radar and the IMU is carried out in an on-line state, and external parameters between the sensors can be updated in real time along with the movement of the robot, so that the requirements of a SLAM system on high precision and real-time performance are met. However, in order to obtain better real-time performance, most of the existing online calibration methods sacrifice the calibration accuracy, so that the calibration accuracy of most of the online calibration methods is not as good as that of the offline calibration methods.
Disclosure of Invention
In order to solve the technical problems in the background art, the invention provides an external parameter calibration method, an external parameter calibration system, an external parameter calibration medium and external parameter calibration equipment, which aim at the problems that the current offline calibration algorithm is poor in instantaneity and the online calibration algorithm is low in solving precision. In order to achieve the above purpose, the present invention adopts the following technical scheme:
the first aspect of the invention provides an external parameter calibration method, which comprises the following steps:
acquiring continuous laser point clouds and inertial measurement unit data of a plurality of frames;
for each frame of laser point cloud, calculating the pose increment between the laser point cloud and the previous frame of laser point cloud, calculating the pose conversion of each laser point in the laser point cloud relative to the starting moment by using an interpolation method based on the pose increment, and then converting the data corresponding to each laser point into a coordinate system at the starting moment based on the pose conversion to remove motion distortion of the laser point cloud;
obtaining a laser odometer based on laser point cloud, obtaining an inertial measurement unit odometer based on inertial measurement unit data, aligning the inertial measurement unit odometer with the laser odometer by using a time stamp, solving an external parameter according to an external parameter calibration mathematical model based on the laser odometer and the inertial measurement unit odometer, and optimizing the external parameter by using a Kalman filtering algorithm.
Further, the pose of each laser point relative to the starting moment is converted into:
wherein,I k for pose transformation of the laser point at the k moment relative to the starting moment, deltat is a pose increment between the laser point cloud of the current frame and the laser point cloud of the previous frame, T is a time stamp of the starting moment of the laser point cloud of the current frame, and Deltat is a time increment between the laser point cloud of the current frame and the laser point cloud of the previous frame.
Further, the inertial measurement unit data includes a triaxial acceleration and a triaxial angular velocity; and integrating the acceleration to obtain the position of the inertial measurement unit, and integrating the angular velocity to obtain the attitude of the inertial measurement unit, thereby obtaining the odometer of the inertial measurement unit.
Further, the step of time stamp alignment includes:
if it isThe corresponding time stamp of the current frame laser odometer is smaller than the time stamp in the inertial measurement unit odometerAfter losing the data of (a), find the distance +.>Two inertial measurement units closest odometer +.>And->；
Odometer based on two inertial measurement unitsAnd->Calculate->Inertial measurement unit odometer corresponding to moment of time:
wherein,and->Respectively->And->Is provided).
Further, the conditions for solving the external parameters include: based on the laser odometer, the pose transformation of the laser radar from the second moment to the first moment, which is calculated, meets the requirements.
Further, the step of solving the external parameters includes:
and solving the rotation external parameters by adopting a first genetic algorithm, and solving the translation external parameters by adopting a second genetic algorithm.
Further, the fitness function of the first genetic algorithm is different from that of the second genetic algorithm.
A second aspect of the present invention provides an external reference calibration system comprising:
a data acquisition module configured to: acquiring continuous laser point clouds and inertial measurement unit data of a plurality of frames;
a distortion removal module configured to: for each frame of laser point cloud, calculating the pose increment between the laser point cloud and the previous frame of laser point cloud, calculating the pose conversion of each laser point in the laser point cloud relative to the starting moment by using an interpolation method based on the pose increment, and then converting the data corresponding to each laser point into a coordinate system at the starting moment based on the pose conversion to remove motion distortion of the laser point cloud;
an external reference calibration module configured to: obtaining a laser odometer based on laser point cloud, obtaining an inertial measurement unit odometer based on inertial measurement unit data, aligning the inertial measurement unit odometer with the laser odometer by using a time stamp, solving an external parameter according to an external parameter calibration mathematical model based on the laser odometer and the inertial measurement unit odometer, and optimizing the external parameter by using a Kalman filtering algorithm.
A third aspect of the invention provides a computer readable storage medium having stored thereon a computer program which when executed by a processor performs steps in a method of calibrating an external parameter as described above.
A fourth aspect of the invention provides a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps in a method of calibrating external parameters as described above when the program is executed.
Compared with the prior art, the invention has the beneficial effects that:
aiming at the problem of low solving precision of the current online calibration algorithm, the invention optimizes the data through the steps of removing the motion distortion of the laser point cloud, aligning the IMU data with the time stamp of the laser point cloud data, screening the mileage data, optimizing the Kalman filtering algorithm and the like, thereby reducing the error and achieving the purpose of improving the external parameter calibration precision.
In order to improve the real-time performance of online calibration, the invention establishes a laser radar and IMU external parameter calibration mathematical model. Once the sensor mileage data meets the screening condition, the external parameters between the sensors can be calculated through solving the genetic algorithm twice, so that the instantaneity of the calibration algorithm can be improved.
According to the invention, through the acquired data of the multi-line laser radar and the inertial measurement unit, external parameters between the sensors are updated online, and the method has higher precision, solves the problems of low efficiency and poor real-time performance of an offline calibration method and low precision of the online calibration method, and is beneficial to improving the positioning and mapping precision of an SLAM algorithm.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention.
FIG. 1 is a flow chart of an external parameter calibration method according to a first embodiment of the present invention;
FIG. 2 is a flow chart of removing motion distortion of a point cloud according to a first embodiment of the present invention;
fig. 3 is a schematic view of point cloud data of a current frame lidar according to the first embodiment of the present invention;
FIG. 4 is a schematic diagram of a correspondence between a laser odometer timestamp and an inertial measurement unit odometer timestamp according to an embodiment of the invention;
FIG. 5 is a schematic alignment of data time stamps of a lidar and an inertial measurement unit according to a first embodiment of the present invention;
FIG. 6 is a schematic view of the pose of the lidar and inertial measurement unit at different times according to the first embodiment of the present invention;
FIG. 7 is a flowchart of a genetic algorithm for solving an extrinsic calibration mathematical model according to a first embodiment of the present invention.
Detailed Description
The invention will be further described with reference to the drawings and examples.
It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the invention. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
Example 1
The embodiment provides an external parameter calibration method.
According to the external parameter calibration method provided by the embodiment, the laser odometer and the inertial measurement unit odometer are calculated through the acquired multi-line laser radar and inertial measurement unit data, then the external parameter is calculated through the established external parameter calibration mathematical model through the genetic algorithm, and finally the external parameter between the sensors is updated on line through the Kalman filtering algorithm. The external parameter calibration method provided by the embodiment has higher precision, solves the problems of low efficiency and poor real-time performance of an off-line calibration method and low precision of an on-line calibration method, and is beneficial to improving the positioning and map building precision of an SLAM algorithm.
The external parameter calibration method provided in this embodiment, as shown in fig. 1, includes the following steps:
and step 1, acquiring laser radar data and removing point cloud distortion generated by the movement of the laser radar. And extracting corner points and face points in the point cloud according to the point cloud curvature information after the distortion is removed, constructing point line and point-face residual constraints between the point clouds of adjacent frames, minimizing the sum of the residual errors by using a nonlinear optimization method, obtaining the pose transformation relation between the point clouds of the adjacent frames, and further obtaining laser odometer data. And 2, acquiring inertial measurement unit data, namely triaxial acceleration and triaxial angular velocity. And integrating the acceleration to obtain the position information of the inertial measurement unit, and integrating the angular velocity to obtain the attitude information of the inertial measurement unit, thereby obtaining the odometer of the inertial measurement unit. And (3) aligning the time stamps of the laser odometer and the inertial measurement unit odometer to find inertial measurement unit odometer data corresponding to the laser odometer.
And step 3, acquiring laser odometer data and inertial measurement unit odometer data at two different moments. If the relative transformation value of the data at two different moments is larger than the threshold value, the data at the next moment is continuously acquired through screening, and whether the data meets the requirements is judged. And establishing a mathematical model between the external parameters to be solved and the screened data to obtain the fitness function of the genetic algorithm. And obtaining the external parameters to be solved through the steps of crossing, mutation, selection and the like of a genetic algorithm. And calculating out rotation external parameters between the sensors by using a first genetic algorithm, and calculating out translation external parameters between the sensors by using a second genetic algorithm. And 4, taking the external parameters obtained by the previous genetic algorithm as predicted values, taking the external parameters obtained by the current genetic algorithm as observed values, and obtaining the optimized external parameters by a Kalman filtering algorithm.
In step 1, the laser mileage data is used to remove motion distortion of the point cloud.
In the laser radar driving process, a frame of laser point cloud data is obtained, and each laser point data is obtained under the same coordinate system. However, due to the movement of the lidar, the coordinate system corresponding to each laser point will actually change with the movement of the lidar, which will cause distortion of the laser point cloud. Since the point cloud distortion has a large influence on the accuracy of the laser odometer, it needs to be removed. The process of removing the point cloud motion distortion is shown in fig. 2, and the specific steps of removing the point cloud motion distortion by using the laser mileage data include:
(1) obtaining the pose increment (odometer increment) between the previous frame point cloud and the current frame point cloud by the calculated laser odometer。
(2) Assuming that the lidar is in constant motion between adjacent frames, the pose delta between adjacent frames is equal. Increasing the pose between the previous frame point cloud and the current frame point cloudAs a pose increment between the current frame point cloud and the next frame point cloud.
(3) And calculating pose transformation of each laser point relative to the starting moment of the current point frame cloud by using an interpolation method. As shown in fig. 3, assume that the pose of the start time of the current frame point cloud data isThe time stamp is t, and the pose increment between the point cloud of the previous frame and the point cloud of the current frame is +.>The time increment is +.>Time corresponding to any laser point in current frameThe timestamp is k, and the pose of any laser point relative to the starting moment of the current frame point cloud is converted into +.>. The pose of the laser spot at time k with respect to time t is converted into:
（1-1）
(4) and according to the obtained pose transformation relation of each laser point (the laser point at the moment k) relative to the moment of the start of the point cloud of the current frame, converting the point cloud data corresponding to each laser point into the coordinate system of the moment of the start of the point cloud data of the current frame, and removing the point cloud distortion. And obtaining the laser odometer by using an inter-frame matching algorithm through the point cloud after distortion removal.
In step 2, the inertial measurement unit odometer data (including the position information X, Y, Z of the inertial measurement unit in space and the attitude information roll, pitch, yaw of the inertial measurement unit) and the laser odometer data (including the position information X, Y, Z of the laser radar in space and the attitude information roll, pitch, yaw of the laser radar) are aligned using interpolation.
In the SLAM algorithm and the sensor calibration algorithm, it is generally required to obtain laser radar data and inertial measurement unit data at the same time, and then perform related operations and processing. However, since the update frequency of the inertial measurement unit data is much higher than that of the laser radar data, the inertial measurement unit odometer data is not necessarily present under the corresponding time stamp of each frame of laser odometer data, as shown in fig. 4.
In order to obtain the inertial measurement unit odometer data corresponding to the laser odometer data under the same time stamp, interpolation is adopted for calculation. As shown in fig. 5, assume thatFor the timestamp corresponding to the current frame laser mileage data, the timestamp in the inertial measurement unit mileage data queue is smaller than +.>After losing the data of (a), find the time stamp +.>The two most recent inertial measurement units odometer data, noted +.>And->The corresponding timestamp is +.>And->. Calculating +.>Moment-of-time corresponding inertial measurement unit odometer data +.>The formula of (2) is as follows:
（2-1）
in step 3, a genetic algorithm is used to solve an extrinsic calibration mathematical model of the laser radar and the inertial measurement unit.
Laser radar and inertial measurement unitAnd->The position and orientation of the moment is shown in fig. 6. The laser radar and the inertial measurement unit sensor are respectively fixed at two different positions of the robot, wherein +.>Is->Pose of time laser radar->Is thatPose of time laser radar->Is->Position and orientation of moment inertial measurement unit +.>Is->Position and orientation of moment inertial measurement unit +.>The pose transformation from the laser radar coordinate system to the inertial measurement unit coordinate system is external parameters between the laser radar and the inertial measurement unit. When the robot moves, the pose of the laser radar and the inertial measurement unit also changes. The laser odometer can obtain +>Time and->Pose of time laser radar->And->Thereby obtaining->Time to->Pose transformation of time laser radar>. From the inertial measurement unit odometer +.>Time and->Position and orientation of the moment inertial measurement unit>And->Thereby obtaining->Time to->Pose transformation of moment inertial measurement unit>。
If it isOne frame of laser point cloud data in a laser radar coordinate system at moment is +.>. Point cloud->First pass->Time to->Pose transformation of time laser radar>Switch to->In the laser radar coordinate system of moment, the point cloud is added>Conversion to +.>In the moment inertial measurement unit coordinate system, then the point cloud is obtainedBy->Time to->Pose transformation of moment inertial measurement unit>Switch to->Finally, the point cloud is added in the moment inertial measurement unit coordinate system>Conversion to +.>And the laser radar coordinate system of the moment. Since the point cloud P before and after the coordinate transformation is fixed in space, the following external parameter calibration mathematical model can be constructed:
（3-1）
（3-2）
（3-3）
（3-4）
wherein,,/>,/>，/>and->For transforming matrix->Corresponding rotation matrix and translation vector, +.>And->For transforming matrix->Corresponding rotation matrix and translation vector, +.>And->Is an extrinsic matrix->The corresponding rotation matrix and translation vector are used,/>is a unitary matrix, from which:
（3-5）
（3-6）
（3-7）
（3-8）
（3-9）
formulas (3-7) and (3-8) are derived external reference calibration mathematical models of the lidar and the inertial measurement unit.
The specific steps for solving the external parameters according to the external parameter calibration mathematical model are as follows:
(1) and (5) judging conditions. Under the condition, obtaining the laser radar in the laser odometerTime and->The corresponding pose between the moments is converted into +.A corresponding pose is obtained through a formula (3-1)>Wherein the transformation matrix->Comprising a 3 x 3 rotation matrix and 3 x 1 translation vectors. Converting rotation matrix into Euler angle +.>In the form of (a), convert the translation vector intoIn the form of (translation vector is a 3X 1 vector, and the data contained is +.>) JudgingAnd->Whether greater than a threshold. If the condition is met, the odometer data passes the screening, otherwise, the data continues to be waited.
(2) First genetic algorithm. According to the formula (3-7), the parameter to be solved is the rotation matrix corresponding to the external parameter. In order to reduce the number of variables to be solved and improve the calculation efficiency and accuracy, the rotation matrix is converted into the form of Euler angles, so that nine variables of the original rotation matrix are converted into three variables of the current Euler angles. The fitness function of the first genetic algorithm is shown in the formula (3-10), the fitness value of each individual (namely the to-be-solved variable) in the genetic algorithm is calculated through the fitness function, and the individual with higher fitness value is eliminated in the selection step of each iteration. By continuously carrying out operations such as inheritance, variation, selection and the like on the initialized randomly generated population, the average fitness value of the population gradually approaches to the minimum fitness value, and finally the optimal individual, namely the rotation external parameter to be solved, is evolved.
（3-10）
(3) A second genetic algorithm. When the rotation external parameters are obtained by the first genetic algorithmThen, the translation vector corresponding to the external reference can be obtained according to the formula (3-8)>. But due to->The determinant of the matrix approaches 0, approximates a singular matrix, and the result obtained directly by using a computer according to the formula (3-9) deviates greatly from the true value, so here again the genetic algorithm is used to solve the formula (3-8). The fitness function of the second genetic algorithm is shown in the formula (3-11):
（3-11）
the general steps of the two genetic algorithms are the same, except that the solution variable of the first genetic algorithm is a rotation extrinsic, the corresponding fitness function is (3-10), and the solution variable of the second genetic algorithm is a translation extrinsic, the corresponding fitness function is (3-11). As shown in fig. 7, the two genetic algorithm solutions are as follows:
(1) initializing a chromosome population. Initially, individuals in n variable definition domains are generated using a random number function, constituting a first generation population. One body contains three chromosomes, each chromosome having a value of one variable to be solved. For the first genetic algorithm, the numerical values of the three chromosomes represent the rotating external parametersFor the second genetic algorithm, the numerical values of the three chromosomes represent the translational foreign parameter +.>。
(2) And calculating individual fitness and population average fitness. And calculating the fitness value of each individual in the kth generation population according to the fitness function, and calculating the average fitness value of the kth generation population. The fitness function corresponding to the first genetic algorithm is (3-10), and the fitness function corresponding to the second genetic algorithm is (3-11). In order to avoid the result obtained by the genetic algorithm to be in a local optimal value, the genetic algorithm is executed once after three groups of screened data are obtained, and the fitness values of the three groups of data are added to be used as the fitness value of the current genetic algorithm.
(3) And judging whether an optimal solution is obtained. The optimal fitness in the kth generation population is smaller than a threshold value; the optimal fitness of the individuals is equal to the average fitness of the population; and thirdly, the iteration times of the population are larger than a threshold value. And if the first condition is met, outputting an optimal solution. And if the condition I is not met but the condition II is met, outputting an optimal solution. If the first condition and the second condition are not met but the third condition is met, outputting an optimal solution, judging whether the fitness value corresponding to the optimal solution is too large, if so, solving the failure, otherwise, solving the failure. If none of the above three conditions is met, the iteration is continued.
(4) Crossing. Traversing all individuals in the kth generation of population, generating a random number, comparing the random number with the crossover probability, and crossing the individuals with the crossover probability. And then obtaining the individual serial numbers and the chromosome serial numbers which need to be crossed by utilizing a random number function, and exchanging the chromosome with the current traversed orthotopic chromosome data.
(5) Variation. Traversing all individuals in the kth generation population, generating a random number, comparing the random number with mutation probability, and carrying out mutation on the individuals smaller than the mutation probability. Then, the chromosome number and mutation rate to be mutated are obtained by using a random number function, and the chromosome number is multiplied by the mutation rate. In order to prevent the value of the mutated chromosome from exceeding the definition domain of the variable, the value of the chromosome whose value is smaller than the left boundary of the definition domain is set as the left boundary value of the definition domain, and the value of the chromosome whose value is larger than the right boundary of the definition domain is set as the right boundary value of the definition domain.
(6) And (5) selecting. Comparing the optimal fitness of the individuals in the current population with the historical optimal fitness, and if the historical optimal fitness is smaller than the current optimal fitness, setting the current optimal fitness and the chromosome as the historical optimal fitness and the chromosome. And determining individuals to be eliminated according to the maximum and minimum of the fitness of the individuals in the kth generation population, and replacing the eliminated individuals with the current optimal fitness and chromosomes.
In step 4, the extrinsic parameters are optimized using a kalman filter algorithm. And each time the laser odometer data and the inertial measurement unit odometer data meet the conditions, performing the solution of the external parameters once to obtain new external parameter data. The sensor has noise, the inertial measurement unit has zero offset, the interframe matching has errors and the like, so that the obtained external parameter data also has error items. In order to reduce the influence of various errors on the external parameters and improve the solving precision of the external parameters, the external parameters obtained each time are required to be optimized so as to obtain more accurate external parameters, and the precision of the SLAM algorithm map building and positioning is ensured.
The parameters obtained each time are optimized by using a Kalman filtering algorithm. And taking the external parameters obtained by the previous optimization as predicted values, taking the external parameters obtained by the current iteration as observed values, and obtaining the optimized external parameters by a Kalman filtering algorithm, thereby estimating the external parameters in real time.
The formula for optimizing the external parameters by using the Kalman filtering algorithm is as follows:
（4-1）
（4-2）
（4-3）
（4-4）
（4-5）
wherein,is prepared from Ginseng radix, and/or radix astragali by Kalman filtering>Is the ginseng after the optimization of the Kalman filtering>For predicted external parameters, < >>External parameters determined for genetic algorithm, +.>For process noise covariance, ++>For measuring noise covariance +.>For state transition matrix>For a priori estimating covariance +.>Estimating covariance for posterior ++>For observing matrix +.>Is the kalman gain.
According to the external parameter calibration method provided by the embodiment, the laser odometer and the inertial measurement unit odometer are calculated through the acquired multi-line laser radar and inertial measurement unit data, then the external parameter is obtained through solving the established external parameter calibration mathematical model through a genetic algorithm, and finally the online updating of the external parameter between the sensors is realized through a Kalman filtering algorithm. The external parameter calibration method provided by the embodiment has higher precision, solves the problems of low efficiency and poor real-time performance of an off-line calibration method and low precision of an on-line calibration method, and is beneficial to improving the positioning and map building precision of an SLAM algorithm.
In order to improve the real-time performance of online calibration, the external parameter calibration mathematical model of the laser radar and the inertial measurement unit is established, once the sensor mileage data meets the screening conditions, the external parameters between the sensors can be calculated through solving the genetic algorithm twice, and the real-time performance of the calibration algorithm can be improved.
According to the external parameter calibration method, aiming at the problem that the solving precision of the current online calibration algorithm is low, through the steps of removing the motion distortion of the laser point cloud, aligning the inertial measurement unit data with the time stamp of the laser point cloud data, screening mileage data, optimizing the Kalman filtering algorithm and the like, the data are optimized, errors can be reduced, and the purpose of improving the external parameter calibration precision is achieved.
Example two
The embodiment provides an external parameter calibration system, which specifically comprises:
a data acquisition module configured to: acquiring continuous laser point clouds and inertial measurement unit data of a plurality of frames;
a distortion removal module configured to: for each frame of laser point cloud, calculating the pose increment between the laser point cloud and the previous frame of laser point cloud, calculating the pose conversion of each laser point in the laser point cloud relative to the starting moment by using an interpolation method based on the pose increment, and then converting the data corresponding to each laser point into a coordinate system at the starting moment based on the pose conversion to remove motion distortion of the laser point cloud;
an external reference calibration module configured to: obtaining a laser odometer based on laser point cloud, obtaining an inertial measurement unit odometer based on inertial measurement unit data, aligning the inertial measurement unit odometer with the laser odometer by using a time stamp, solving an external parameter according to an external parameter calibration mathematical model based on the laser odometer and the inertial measurement unit odometer, and optimizing the external parameter by using a Kalman filtering algorithm.
It should be noted that, each module in the embodiment corresponds to each step in the first embodiment one to one, and the implementation process is the same, which is not described here.
Example III
The present embodiment provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, performs the steps of a method for calibrating an external parameter as described in the above embodiment.
Example IV
The present embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor executes the program to implement the steps in a method for calibrating external parameters according to the first embodiment.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Claims (10)
1. The external parameter calibration method is characterized by comprising the following steps of:
acquiring continuous laser point clouds and inertial measurement unit data of a plurality of frames;
for each frame of laser point cloud, calculating the pose increment between the laser point cloud and the previous frame of laser point cloud, calculating the pose conversion of each laser point in the laser point cloud relative to the starting moment by using an interpolation method based on the pose increment, and then converting the data corresponding to each laser point into a coordinate system at the starting moment based on the pose conversion to remove motion distortion of the laser point cloud;
obtaining a laser odometer based on laser point cloud, obtaining an inertial measurement unit odometer based on inertial measurement unit data, aligning the inertial measurement unit odometer with the laser odometer by using a time stamp, solving an external parameter according to an external parameter calibration mathematical model based on the laser odometer and the inertial measurement unit odometer, and optimizing the external parameter by using a Kalman filtering algorithm.
2. The method of claim 1, wherein the pose of each laser point with respect to the starting time is transformed into:
，
wherein,I k for pose transformation of the laser point at the k moment relative to the starting moment, deltat is a pose increment between the laser point cloud of the current frame and the laser point cloud of the previous frame, T is a time stamp of the starting moment of the laser point cloud of the current frame, and Deltat is a time increment between the laser point cloud of the current frame and the laser point cloud of the previous frame.
3. The method of calibrating an external reference of claim 1, wherein the inertial measurement unit data includes a tri-axial acceleration and a tri-axial angular velocity; and integrating the acceleration to obtain the position of the inertial measurement unit, and integrating the angular velocity to obtain the attitude of the inertial measurement unit, thereby obtaining the odometer of the inertial measurement unit.
4. The method of calibrating a foreign object as defined in claim 1, wherein the step of time stamping alignment includes:
if it isFor the corresponding time stamp of the current frame laser odometer, the time stamp in the inertial measurement unit odometer is smaller than +.>After losing the data of (a), find the distance +.>Two inertial measurement units closest odometer +.>And；
odometer based on two inertial measurement unitsAnd->Calculate->Inertial measurement unit odometer corresponding to moment of time:
，
wherein,and->Respectively->And->Is provided).
5. The method of calibrating an external parameter of claim 1, wherein the solving the external parameter condition comprises: based on the laser odometer, the pose transformation of the laser radar from the second moment to the first moment, which is calculated, meets the requirements.
6. The method of calibrating an external parameter of claim 1, wherein the step of solving the external parameter comprises:
and solving the rotation external parameters by adopting a first genetic algorithm, and solving the translation external parameters by adopting a second genetic algorithm.
7. The method of calibrating a reference of claim 6, wherein the fitness function of the first genetic algorithm is different from the fitness function of the second genetic algorithm.
8. An external reference calibration system, comprising:
a data acquisition module configured to: acquiring continuous laser point clouds and inertial measurement unit data of a plurality of frames;
a distortion removal module configured to: for each frame of laser point cloud, calculating the pose increment between the laser point cloud and the previous frame of laser point cloud, calculating the pose conversion of each laser point in the laser point cloud relative to the starting moment by using an interpolation method based on the pose increment, and then converting the data corresponding to each laser point into a coordinate system at the starting moment based on the pose conversion to remove motion distortion of the laser point cloud;
an external reference calibration module configured to: obtaining a laser odometer based on laser point cloud, obtaining an inertial measurement unit odometer based on inertial measurement unit data, aligning the inertial measurement unit odometer with the laser odometer by using a time stamp, solving an external parameter according to an external parameter calibration mathematical model based on the laser odometer and the inertial measurement unit odometer, and optimizing the external parameter by using a Kalman filtering algorithm.
9. A computer readable storage medium, on which a computer program is stored, characterized in that the program, when being executed by a processor, carries out the steps of a method of calibrating external parameters according to any of claims 1-7.
10. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the steps of a method for calibrating external parameters according to any of claims 1-7 when said program is executed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311255316.0A CN117092624A (en) | 2023-09-27 | 2023-09-27 | External parameter calibration method, system, medium and equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311255316.0A CN117092624A (en) | 2023-09-27 | 2023-09-27 | External parameter calibration method, system, medium and equipment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117092624A true CN117092624A (en) | 2023-11-21 |
Family
ID=88775461
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311255316.0A Pending CN117092624A (en) | 2023-09-27 | 2023-09-27 | External parameter calibration method, system, medium and equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117092624A (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112882053A (en) * | 2021-01-21 | 2021-06-01 | 清华大学深圳国际研究生院 | Method for actively calibrating external parameters of laser radar and encoder |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113947639A (en) * | 2021-10-27 | 2022-01-18 | 北京斯年智驾科技有限公司 | Self-adaptive online estimation calibration system and method based on multi-radar-point cloud line characteristics |
-
2023
- 2023-09-27 CN CN202311255316.0A patent/CN117092624A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112882053A (en) * | 2021-01-21 | 2021-06-01 | 清华大学深圳国际研究生院 | Method for actively calibrating external parameters of laser radar and encoder |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113947639A (en) * | 2021-10-27 | 2022-01-18 | 北京斯年智驾科技有限公司 | Self-adaptive online estimation calibration system and method based on multi-radar-point cloud line characteristics |
Non-Patent Citations (1)
| Title |
|---|
| 赵一凡: "基于3D激光雷达与IMU融合的室外移动机器人SLAM技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》, no. 2, pages 27 - 46 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107516326B (en) | Robot positioning method and system fusing monocular vision and encoder information | |
| CN111178385B (en) | Target tracking method for robust online multi-sensor fusion | |
| CN113311411B (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN105467382A (en) | SVM (Support Vector Machine)-based multi-sensor target tracking data fusion algorithm and system thereof | |
| CN109579824B (en) | Self-adaptive Monte Carnot positioning method integrating two-dimensional code information | |
| CN114862932B (en) | BIM global positioning-based pose correction method and motion distortion correction method | |
| CN111795686A (en) | Method for positioning and mapping mobile robot | |
| CN112965063B (en) | Robot mapping and positioning method | |
| CN112285676A (en) | Laser radar and IMU external reference calibration method and device | |
| CN110561424A (en) | online robot kinematic calibration method based on multi-sensor hybrid filter | |
| CN116929338B (en) | Map construction method, device and storage medium | |
| CN113468706B (en) | Laser point cloud power transmission line lead fitting method for distribution network live working robot | |
| CN117092624A (en) | External parameter calibration method, system, medium and equipment | |
| CN110912535A (en) | Novel pilot-free Kalman filtering method | |
| CN109670142A (en) | A kind of distinguishable motor-driven multiple targets method for estimating state based on stochastic finite collection | |
| CN116659500A (en) | Mobile robot positioning method and system based on laser radar scanning information | |
| CN112489075B (en) | Sequential inertial multi-sensor fusion filtering method based on characteristic function | |
| CN113763434B (en) | Target track prediction method based on Kalman filtering multi-motion model switching | |
| CN115950414A (en) | Adaptive multi-fusion SLAM method for different sensor data | |
| CN108107882B (en) | Automatic calibration and detection system of service robot based on optical motion tracking | |
| Chen et al. | Research on adaptive Monte Carlo location method based on fusion posture estimation | |
| CN113390376B (en) | Method, device and system for determining cabin displacement of wind generating set | |
| CN115114985A (en) | Sensor system distributed fusion method based on set theory | |
| CN113608442A (en) | State estimation method of nonlinear state model system based on characteristic function | |
| CN112581616B (en) | Nearest neighbor UKF-SLAM method based on sequential block filtering |
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 |