IMU and laser radar calibration method, device, equipment and medium
Through the total station calculation of the conversion matrix and optimizing the external parameters of IMU and lidar, the problem of low calibration efficiency and accuracy of IMU and lidar is solved, and a faster and more accurate calibration process is achieved.
Patent Information
- Application Number
- CN202411256695.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-09
- Publication Date
- 2024-12-03
AI Technical Summary
The existing IMU and lidar calibration methods are not efficient and accurate, and require a large amount of data to be iteratively adjusted, resulting in a complex and time-consuming calibration process.
The conversion matrix of the IMU coordinate system to the total station coordinate system and the total station coordinate system to the lidar coordinate system is calculated through the total station, and the external parameters of the IMU coordinate system to the lidar coordinate system are initially calibrated, and then the lidar point cloud data is used. and IMU pose data optimization and adjustment to obtain more accurate target external parameters.
The iterative convergence time of subsequent precision calibration is accelerated, calibration efficiency and accuracy are improved, and iterative adjustment needs for sensor data are reduced.
Smart Images

Figure CN119064903A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of sensor calibration technology, and in particular to an IMU and laser radar calibration method, device, equipment and medium. Background Art
[0002] In modern autonomous driving and robotics, LiDAR and IMU (Inertial Measurement Unit) are two key sensors. LiDAR measures the distance to the target by emitting laser beams and receiving reflected signals, thereby generating high-precision three-dimensional point cloud data, which can help vehicles or robots understand the surrounding environment and achieve accurate obstacle detection and map construction. IMU is a device that measures the acceleration and angular velocity of an object through an accelerometer and a gyroscope. IMU can provide dynamically changing high-frequency data, which is essential for detecting the motion state of a vehicle or robot. The combination of IMU and LiDAR enables more accurate positioning and navigation, but this also requires a high degree of coordination and calibration between the two. The external parameter calibration technology of LiDAR and IMU aims to solve the error and deviation problems between sensors to ensure the overall accuracy of the system. The external parameter calibration process refers to determining the coordinate transformation relationship between LiDAR and IMU. This process usually requires complex calculations and experiments. At present, the commonly used calibration method is to directly collect sensor data through IMU and lidar respectively, and realize calibration through the corresponding relationship of data. This method requires the collection of a large amount of data to determine the conversion relationship between coordinate systems. The analysis is difficult, and the efficiency and accuracy of calibration are not high enough. Summary of the invention
[0003] In view of this, the present invention provides a calibration method, device, equipment and medium for IMU and laser radar to solve the problem of inaccurate calibration of IMU and laser radar.
[0004] In the first aspect, the present invention provides a calibration method for an IMU and a laser radar, the method comprising: calculating a first transformation matrix from an IMU coordinate system to a total station coordinate system; calculating a second transformation matrix from a total station coordinate system to a laser radar coordinate system; calculating initial external parameters from the IMU coordinate system to the laser radar coordinate system through the first transformation matrix and the second transformation matrix; obtaining point cloud data scanned by the laser radar and posture data of the IMU for a current moving object; optimizing and adjusting the initial external parameters based on the point cloud data and the posture data to obtain target external parameters from the IMU coordinate system to the laser radar coordinate system.
[0005] In some optional embodiments, the calculation of the first transformation matrix from the IMU coordinate system to the total station coordinate system includes: determining the reference positions of the origin, X-axis and Y-axis of the IMU coordinate system on the IMU housing based on the IMU model data; using the total station to mark the reference positions on the IMU housing to obtain the absolute coordinates of the origin, points on the X-axis and points on the Y-axis of the IMU coordinate system mapped in the total station coordinate system, and constructing the IMU's XOY coordinate system in the total station coordinate system based on the absolute coordinates; based on the perpendicular relationship between the IMU's Z axis and the XOY coordinate system plane, calculating the Z-axis direction vector of the IMU's Z axis in the total station coordinate system; orthogonalizing the X-axis vector, Y-axis vector and Z-axis direction vector of the XOY coordinate system to obtain three coordinate axis vectors of the IMU coordinate system in the total station coordinate system, and calculating the first transformation matrix using the obtained three coordinate axis vectors.
[0006] In some optional embodiments, the calculation of the second transformation matrix from the total station coordinate system to the laser radar coordinate system includes: placing a calibration plate and determining feature points in the calibration plate; scanning the calibration plate by laser radar to obtain a feature point cloud representing the feature points; determining the coordinates of the first feature points in the laser radar coordinate system based on the feature point cloud, and placing the first feature point coordinates into a first feature point set; marking the feature points by a total station to obtain the second feature point coordinates of the feature points in the total station coordinate system, and placing the second feature point coordinates into a second feature point set; keeping the relative position of the laser radar and the total station unchanged, moving the calibration plate, and returning to the step of scanning the calibration plate by laser radar to add data to the first feature point set and the second feature point set; after the calibration plate has moved a preset number of times, calculating the second transformation matrix based on the first feature point set and the second feature point set with the added data.
[0007] In some optional embodiments, the acquisition of point cloud data scanned by the lidar and the pose data of the IMU includes: synchronizing the clocks of the lidar and the IMU; acquiring the point cloud data scanned by the lidar; obtaining the pose data by integrating the initial linear acceleration and angular velocity of the IMU; and interpolating the pose data according to the timestamp of the point cloud data.
[0008] In some optional implementations, the initial extrinsic parameters are optimized and adjusted based on the point cloud data and the pose data to obtain the target extrinsic parameters from the IMU coordinate system to the lidar coordinate system, including: calculating the product of the pose data and the initial extrinsic parameters to obtain a third transformation matrix from the current frame point cloud to the first frame point cloud in the point cloud data; using the third transformation matrix to map the current frame point cloud to the first frame to obtain a mapped point cloud; adjusting the initial extrinsic parameters according to the error between the mapped point cloud and the first frame point cloud to obtain the target extrinsic parameters. In some optional implementations, adjusting the initial extrinsic parameters according to the error between the mapped point cloud and the first frame point cloud to obtain the target extrinsic parameters includes: performing voxel grid division on the point cloud space; aligning the first frame point cloud and the mapped point cloud to corresponding voxel grids based on an octree algorithm; and adjusting the initial extrinsic parameters based on the minimum number of voxel grids occupied by point cloud data.
[0009] In some optional implementations, the acquiring of point cloud data scanned by the laser radar includes: eliminating distortion of the point cloud data by using the initial external parameters to obtain dedistorted point cloud data.
[0010] In the second aspect, the present invention provides a calibration device for an IMU and a laser radar, the device comprising: a first conversion matrix calculation module, used to calculate a first conversion matrix from an IMU coordinate system to a total station coordinate system; a second conversion matrix calculation module, used to calculate a second conversion matrix from a total station coordinate system to a laser radar coordinate system; an external parameter initial calibration module, used to calculate the initial external parameters from the IMU coordinate system to the laser radar coordinate system through the first conversion matrix and the second conversion matrix; a sensor data acquisition module, used to acquire point cloud data scanned by the laser radar and posture data of the IMU for a current moving object; an external parameter optimization module, used to optimize and adjust the initial external parameters based on the point cloud data and the posture data to obtain target external parameters from the IMU coordinate system to the laser radar coordinate system.
[0011] In a third aspect, the present invention provides a computer device, comprising: a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the method of the first aspect or any corresponding embodiment thereof by executing the computer instructions.
[0012] In a fourth aspect, the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to cause a computer to execute the method of the first aspect or any corresponding embodiment thereof.
[0013] The technical solution provided by the present invention has the following advantages:
[0014] The first conversion matrix from the IMU coordinate system to the total station coordinate system and the second conversion matrix from the total station coordinate system to the laser radar coordinate system are calculated by the total station. Then, a relatively accurate initial external parameter from the IMU coordinate system to the laser radar coordinate system is roughly calibrated using the first conversion matrix and the second conversion matrix; then, the initial external parameter is optimized and adjusted through the collected laser radar point cloud data and IMU posture data, and a more accurate target external parameter from the IMU coordinate system to the laser radar coordinate system is precisely calibrated. The present invention obtains a relatively accurate initial transformation relationship from the IMU coordinate system to the laser radar coordinate system through the total station, and provides a calibration guidance direction for the external parameter calibration of the IMU and the laser radar. In the subsequent process, only fine-tuning is required based on the initial external parameter, and it is not necessary to iteratively adjust the external parameter using a large amount of sensor data from the beginning, which accelerates the iterative convergence time of the subsequent precise calibration. After that, a small amount of sensor data is used to fine-tune the initial external parameter, while improving the calibration efficiency and calibration accuracy. BRIEF DESCRIPTION OF THE DRAWINGS
[0015] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0016] Figure 1 is a flow chart of a calibration method of an IMU and a laser radar according to an embodiment of the present invention;
[0017] Figure 2 is another flowchart of a calibration method for an IMU and a laser radar according to an embodiment of the present invention;
[0018] Figure 3 is a structural schematic diagram of an IMU and laser radar calibration device according to an embodiment of the present invention;
[0019] Figure 4 It is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION
[0020] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.
[0021] According to an embodiment of the present invention, an embodiment of a calibration method for an IMU and a lidar is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.
[0022] In this embodiment, a calibration method for an IMU and a laser radar is provided. Figure 1 1 is a flow chart of a calibration method of an IMU and a laser radar according to an embodiment of the present invention, and the process includes the following steps:
[0023] Step S101, calculating a first conversion matrix from an IMU coordinate system to a total station coordinate system;
[0024] Step S102, calculating a second conversion matrix from the total station coordinate system to the laser radar coordinate system;
[0025] Step S103, calculating the initial external parameters from the IMU coordinate system to the lidar coordinate system through the first transformation matrix and the second transformation matrix.
[0026] Specifically, the total station, also known as the total station type electronic distance meter or the total station type electronic tachometer, is a new generation of measuring instruments born with the development of computers and electronic distance measurement technology. It combines the functions of electronic angle measurement, electronic distance measurement, electronic calculation and data storage unit to form a three-dimensional coordinate measurement system. Its working principle is to set a reference point (dot) during the measurement, then measure the distance by laser or electronic beam, and calculate the three-dimensional coordinates between points in combination with the angle measurement function. The accuracy of the total station is usually higher than that of the theodolite because it has an electronic distance measurement function and can calculate the exact distance between points.
[0027] Based on the ability of the total station to mark environmental objects, the embodiment of the present invention proposes a new calibration method for the lidar coordinate system and the IMU coordinate system. It no longer directly performs matching calculations through a large amount of lidar data and IMU data, thus saving a large number of iterative adjustment steps.
[0028] Taking a 128-line laser radar and a 9-axis IMU device as an example (the present invention is only used as an example and is not limited to this), first, the first conversion matrix from the IMU coordinate system to the total station coordinate system and the second conversion matrix from the total station coordinate system to the laser radar coordinate system are calculated by the total station. Among them, since the IMU coordinate system is obtained by creating X, Y, and Z axes with the IMU device body as the origin, the principle of calculating the first conversion matrix in the embodiment of the present invention is to use the total station to mark points on the IMU device and measure the coordinates of the IMU device body in the total station coordinate system, so as to map the IMU coordinate system to the total station coordinate system based on the position of the measured coordinates in the IMU coordinate system, and obtain the first conversion matrix representing the relationship between the two. Among them, the calculation of the second conversion matrix is to use the functions of the total station marking and the laser radar scanning to let the total station and the laser radar scan the same object, and calculate the mapping relationship of the coordinates based on the coordinates of the object in the two coordinate systems, so as to obtain the second conversion matrix representing the total station coordinate system to the laser radar coordinate system.
[0029] Then, assume that the first transformation matrix of the IMU coordinate system I relative to the total station coordinate system Q is A, that is, Q=AI, and the second transformation matrix of the total station coordinate system Q relative to the radar coordinate system L is B, that is, L=BQ, so L=BQ=BAI, that is, the initial external parameter of the IMU coordinate system relative to the lidar coordinate system is BA, and the initial external parameter is a relatively accurate but not precise enough transformation matrix.
[0030] Step S104, obtaining point cloud data scanned by the laser radar and position data of the IMU for the current moving object;
[0031] Step S105, optimizing and adjusting the initial external parameters based on the point cloud data and the pose data to obtain the target external parameters from the IMU coordinate system to the lidar coordinate system.
[0032] Specifically, after obtaining the initial external parameters, the embodiment of the present invention respectively obtains the point cloud data scanned by the laser radar and the posture data of the IMU, wherein the posture data of the IMU is calculated by integrating the angular velocity, linear acceleration, direction, etc. measured by the IMU, thereby obtaining the current position of the object, and then calculating the conversion matrix from the current IMU data to the first frame of IMU data through the positional relationship between each frame of IMU data and the first frame, i.e., the posture data of the IMU. Usually, the laser radar and IMU devices are installed on robots or autonomous vehicles, and the point cloud data and posture data scanned by the laser radar and IMU devices are for the currently moving robot or autonomous vehicle, i.e., the same moving object, thereby facilitating data matching and external parameter calibration. The more accurate the initial extrinsic parameter calibration is, the more similar the mapping result is to the first frame point cloud data when the subsequent frame point cloud data in the lidar coordinate system is mapped to the first frame point cloud data through the initial extrinsic parameters and posture data. The initial extrinsic parameters are optimized and adjusted according to this rule to improve the matching degree of the two data, and finally the target extrinsic parameters after adjustment from the IMU coordinate system to the lidar coordinate system are obtained, that is, the precise conversion matrix from the IMU coordinate system to the lidar coordinate system.
[0033] Through the technical solution provided by the present invention, a total station is used in the rough calibration stage to obtain a relatively accurate initial transformation relationship from the IMU coordinate system to the lidar coordinate system, which provides a calibration guidance direction for the external parameter calibration of the IMU and the lidar. Subsequently, only fine-tuning needs to be performed based on the initial external parameters, and there is no need to iteratively adjust the external parameters using a large amount of sensor data from the beginning, thereby accelerating the iterative convergence time of the subsequent fine calibration. Afterwards, a small amount of sensor data is used in the fine calibration stage to fine-tune the initial external parameters, thereby improving the calibration efficiency and calibration accuracy.
[0034] In some optional implementations, the above step S101 includes:
[0035] Step a1, determining the origin of the IMU coordinate system, the reference positions of the X-axis and the Y-axis on the IMU housing based on the IMU model data;
[0036] Step a2, using the total station to mark the reference position on the IMU housing, obtain the absolute coordinates of the origin of the IMU coordinate system, the points on the X-axis and the points on the Y-axis mapped in the total station coordinate system, and construct the XOY coordinate system of the IMU in the total station coordinate system based on the absolute coordinates;
[0037] Step a3, based on the vertical relationship between the Z axis of the IMU and the XOY coordinate system plane, calculate the Z axis direction vector of the IMU Z axis in the total station coordinate system;
[0038] Step a4, orthogonalize the X-axis vector, Y-axis vector and Z-axis direction vector of the XOY coordinate system to obtain three coordinate axis vectors of the IMU coordinate system in the total station coordinate system, and calculate the first transformation matrix through the obtained three coordinate axis vectors.
[0039] Specifically, the present invention provides a method for calculating the conversion relationship between the IMU coordinate system and the total station coordinate system. First, according to the mathematical model of the IMU and the IMU appearance structure size mark (model data), the origin, X-axis diagram, and Y-axis diagram of the IMU shell mark are found, that is, the reference position of the origin, X-axis, and Y-axis of the IMU coordinate system on the IMU shell is determined. After that, the total station is used to mark the above-mentioned reference position of the IMU shell, and then the absolute coordinates of the origin of the IMU coordinate system, the points on the X-axis, and the points on the Y-axis mapped in the total station coordinate system are obtained through the measurement of the total station, so that the XOY plane coordinate system of the IMU can be constructed in the total station coordinate system based on the absolute coordinates obtained by the measurement.
[0040] Afterwards, since the Z axis of the IMU is perpendicular to the XOY plane, based on the perpendicular relationship, the Z axis direction vector of the Z axis of the IMU coordinate system in the total station coordinate system can be calculated through the orthogonal matrix; after determining the direction of the Z axis, since the X axis, Y axis, and Z axis of the IMU coordinate system intersect at the origin, the embodiment of the present invention further performs Gram-Schmidt orthogonalization on the X axis vector, Y axis vector, and Z axis direction vector to obtain three coordinate axis vectors orthogonal to the IMU origin, thereby obtaining the IMU coordinate system in the total station coordinate system, and the first transformation matrix between the two can be calculated through the obtained three coordinate axis vectors of the IMU coordinate system, which is used to convert the point coordinate values in the two coordinate systems.
[0041] Through the processing of the embodiments of the present invention, a new method for calculating the conversion relationship between the IMU coordinate system and the total station coordinate system is proposed, thereby introducing the total station as an intermediate device into the calibration work of the IMU device and the lidar.
[0042] In some optional implementations, the above step S102 includes:
[0043] Step b1, placing a calibration plate and determining feature points in the calibration plate;
[0044] Step b2, scanning the calibration plate by laser radar to obtain a feature point cloud representing the feature points;
[0045] Step b3, determining the coordinates of the first feature point in the laser radar coordinate system based on the feature point cloud, and putting the coordinates of the first feature point into the first feature point set;
[0046] Step b4, marking the feature points by a total station, obtaining the second feature point coordinates of the feature points in the total station coordinate system, and putting the second feature point coordinates into a second feature point set;
[0047] Step b5, keeping the relative positions of the laser radar and the total station unchanged, moving the calibration plate, and returning to the step of scanning the calibration plate by the laser radar to add data in the first feature point set and the second feature point set;
[0048] Step b6, after the calibration plate has moved a preset number of times, a second transformation matrix is calculated based on the first feature point set and the second feature point set with added data.
[0049] Specifically, the method for calculating the second transformation matrix provided by the embodiment of the present invention is implemented through a specific calibration plate. First, place the calibration plate at a specific position, and the calibration plate includes some feature points that are easy to scan, for example: place the triangular grid calibration plate on a hollow shelf, ensure that there are no objects placed behind the upper, left, and right corner points of the calibration plate, and the laser radar can be clearly scanned, and the upper, left, and right corner points of the calibration plate are the feature points. Feature points can also be selected from other special positions on the calibration plate, points with special shapes or special colors. The present invention is only an example and is not limited to this.
[0050] Afterwards, the total station is calibrated and set to zero to ensure the accuracy of the total station. Calibration and setting to zero is an existing technology for debugging total station equipment, which will not be described in detail in this embodiment. It should be noted that the laser radar and the total station need to always keep their relative positions unchanged in the process of acquiring the coordinates of the feature points to ensure that the matching relationship between the data scanned by the two does not change, thereby improving the accuracy of the subsequent calculation process. In some optional implementations, a rigid connection setting between the laser radar and the total station can be adopted to ensure that the relative positions of the two remain unchanged. In addition, the present invention only requires that the relative position relationship between the laser radar and the IMU is fixed, and does not specifically limit the specific relative position distance and angle size of the laser radar and the IMU.
[0051] Afterwards, the feature point cloud of the feature points is obtained by laser radar scanning, and the first feature point coordinates of the three feature points on the top, left and right of the current position calibration plate in the laser radar coordinate system are obtained. At the same time, the second feature point coordinates of the three feature points in the total station coordinate system are obtained by total station marking. Keeping the relative position of the total station and the laser radar unchanged, move the calibration plate, obtain data of multiple groups of feature points in the two coordinate systems, and obtain the first feature point set and the second feature point set respectively. Finally, the relationship between the first feature point set and the second feature point set is fitted by the least squares method, and the second transformation matrix from the total station coordinate system to the laser radar coordinate system is calculated, and the rotation matrix and the translation vector are obtained accordingly. Through the technical solution provided by the embodiment of the present invention, sensor data in two coordinate systems are collected based on the calibration plate with feature points, and then a simple and efficient calculation scheme for the transformation relationship from the total station coordinate system to the laser radar coordinate system is realized according to the matching relationship of the data.
[0052] Specifically, in some optional implementations, the above step S104 includes:
[0053] Step c1, synchronizing the clocks of the lidar and IMU;
[0054] Step c2, obtaining point cloud data scanned by the laser radar;
[0055] Step c3, obtaining the posture data by integrating the initial linear acceleration and angular velocity of the IMU;
[0056] Step c4, interpolating the pose data according to the timestamp of the point cloud data.
[0057] Specifically, before the precise calibration process is performed, it is necessary to collect the point cloud data of the vehicle or robot through the laser radar, and collect the posture data of the vehicle or robot through the IMU device (the posture data refers to the data of the position and posture changes of the vehicle or robot relative to the first frame collected by the IMU device). Since the initial external parameters need to be accurately adjusted using the matching relationship between the posture data and the point cloud data, the collection time of the posture data and the point cloud data is particularly important. If the posture data and the point cloud data collected at the same time point do not correspond to the same position, it will seriously affect the subsequent calibration accuracy. Based on this, an embodiment of the present invention provides a method for time alignment of a laser radar and an IMU, including two stages of hardware alignment and software alignment. Among them, hardware alignment refers to synchronizing the clocks of the laser radar and the IMU, for example, synchronizing the laser radar and the IMU through PTP (Precision Time Protocol, Precision Time Protocol), unifying the clock sources of the two devices, ensuring that the clock error of the two is at the microsecond level, and controlling the synchronization of instructions. Thus, after the device clock is synchronized, a bag with point cloud data is recorded during the movement, and then the point cloud data scanned by the laser radar is obtained by parsing the bag. At the same time, the posture data is calculated by IMU. Among them, the working principle of IMU is based on Newton's second law and the principle of conservation of angular momentum. It can measure the acceleration and angular velocity of an object. After these data are integrated, the speed and position information of the object can be indirectly obtained. Specifically, the integration of acceleration can obtain speed, the integration of speed can obtain distance, and the integration of angular velocity can obtain angle. Therefore, the motion trajectory and posture of the object can be calculated by the acceleration and angular velocity data measured by IMU. Because IMU has error terms such as zero bias, these errors will make the result of integration inaccurate during the process of motion integration, and the result will become more and more inaccurate as the motion time increases, so it is necessary to process the error. In the process of calculating the posture data, the embodiment of the present invention propagates the error amount, and calculates the covariance matrix corresponding to the error, and obtains the optimal estimate of the posture data based on the error state Kalman filter, thereby reducing the error of the posture data and further improving the accuracy of the posture data. The process of obtaining the optimal estimate of the posture data based on the error state Kalman filter is a prior art, and this embodiment will not be repeated.
[0058] Although the control clock of the device is synchronized, the frequency of the laser radar may be 10Hz, that is, it collects a frame of data every 100ms, and the collection frequency of the IMU may be 100Hz, collecting data every 10ms. Although the two are in a multiple relationship, their collection frequencies are not strictly fixed. For example, the laser radar may collect a frame of data at 99ms, or it may collect a frame of data at 101ms, not strictly collecting a frame of data at 100ms. The same is true for the IMU. This makes it so that the laser radar and the IMU still have the problem of misalignment in timestamps. If used directly for calibration, the data points used for matching may not be in the same position, affecting the accuracy of the external parameter calibration, and the IMU data is denser than the laser radar data. In response to the above problems, the embodiment of the present invention interpolates the IMU data at the corresponding timestamp position according to the timestamp of the laser radar point cloud and the change law of the IMU data, and obtains IMU posture data that accurately matches the timestamp of the laser radar point cloud data to the millisecond position. The IMU posture data and point cloud data are precisely synchronized with the timestamp millisecond through the interpolation algorithm, which ensures the consistency of the data and improves the accuracy of the subsequent calibration process.
[0059] In some optional embodiments,
[0060] Step d1, calculating the product of the pose data and the initial external parameter to obtain a third transformation matrix from the current frame point cloud to the first frame point cloud in the point cloud data;
[0061] Step d2, mapping the current frame point cloud to the first frame using the third transformation matrix to obtain a mapped point cloud;
[0062] Step d3, adjusting the initial external parameters according to the error between the mapped point cloud and the first frame point cloud to obtain the target external parameters.
[0063] Specifically, the IMU's posture data represents the conversion matrix of the IMU at the current moment relative to the IMU's first frame data. The initial external parameters calculated in the previous steps are multiplied by the IMU's posture data. The result obtained can actually represent the conversion matrix (third conversion matrix) of the laser radar's current frame point cloud relative to the laser radar's first frame point cloud. By multiplying the current frame point cloud by the third conversion matrix, the current frame point cloud and the first frame point cloud can be aligned. If the initial external parameters are accurate, the result of the alignment of the current frame point cloud and the first frame point cloud is also accurate. If the initial external parameters are inaccurate, the alignment accuracy will be low. Based on this principle, the embodiment of the present invention first obtains the angular velocity, speed, direction and other values measured by the IMU, and then obtains the IMU's posture data through integral calculation. Since the initial external parameters from the IMU coordinate system to the laser radar coordinate system are obtained by calculation, the initial external parameters are multiplied by the posture data to obtain the conversion matrix from the laser radar's current frame point cloud to the first frame point cloud, that is, the third conversion matrix. The third transformation matrix is used to map the point cloud data of the current frame to the first frame, and the mapped point cloud and the first frame point cloud are aligned. The initial external parameters are iteratively adjusted according to the error between the two until accurate external parameters are obtained to achieve accurate point cloud alignment, thereby completing the precision calibration process and making the adjusted target external parameters more accurate.
[0064] In some optional implementations, the above step d3 includes:
[0065] Step e1, dividing the point cloud space into voxel grids;
[0066] Step e2, registering the mapping point cloud and the first frame point cloud into corresponding voxel grids based on the octree algorithm;
[0067] Step e3, adjusting the initial external parameters based on the minimum number of voxel grids occupied by the point cloud data.
[0068] Specifically, when performing point cloud registration based on external parameters, if the initial external parameters are accurate, the point cloud registration is accurate, the mapping point cloud and the first frame point cloud overlap more, and occupy less space in the space; if the initial external parameters are inaccurate, the point cloud registration is inaccurate, which will cause the mapping point cloud and the first frame point cloud to not overlap, and occupy a lot of space in the space. Based on this, Figure 2 As shown, in the embodiment of the present invention, during the precise calibration process, the initial extrinsic parameters are optimized based on the octree space planning, the coordinate system space of the first frame point cloud data is divided into voxel grids, the mapped point cloud and the first frame point cloud are aligned to the corresponding voxel grids based on the octree algorithm, and the initial extrinsic parameters are iteratively adjusted based on the minimum number of voxel grids occupied by the point cloud data. The number of voxel grids occupied is verified each time the adjustment is made to improve the accuracy of the point cloud alignment, thereby improving the accuracy of the target extrinsic parameters.
[0069] In some optional implementation manners, before the above step c2, the following steps are also included:
[0070] Step f1, eliminating the distortion of the point cloud data through the initial external parameters to obtain the dedistorted point cloud data.
[0071] Specifically, it often takes 100ms for a laser radar to collect a frame of point cloud data, and data needs to be collected during the movement of the point cloud. When the first frame of point cloud data is first collected, the laser radar moves very slowly, and the distortion caused by movement during the time of collecting data from 0-100ms can be almost ignored. However, when collecting subsequent data, the laser radar moves faster, and when aligning the subsequent point cloud with the first frame of point cloud, in addition to scene changes, the movement will cause non-negligible distortion of the point cloud data, resulting in inaccurate matching when the point cloud is aligned based on external parameters. To address this problem, the embodiment of the present invention also performs a step-by-step distortion elimination process on the point cloud data through the initial external parameters to obtain dedistorted point cloud data, thereby eliminating the motion distortion of the point cloud data and improving the accuracy of the point cloud data used to calibrate the external parameters.
[0072] The technical solution provided by the present invention mainly has the following advantages:
[0073] 1. The present invention uses a total station to obtain a relatively accurate initial transformation relationship from the IMU coordinate system to the lidar coordinate system, which can greatly accelerate the iterative convergence time of subsequent fine calibration and improve the calibration accuracy.
[0074] 2. The present invention performs PTP time synchronization processing on the IMU and lidar, and uses an interpolation algorithm to accurately synchronize the timestamp milliseconds of the IMU posture data and point cloud data, thereby ensuring data consistency.
[0075] 3. The present invention refines the space occupancy based on the octree and further refines the initial external parameters; improves the registration effect of the lidar point cloud through the IMU posture data, and obtains an accurate conversion matrix from the IMU coordinate system to the lidar coordinate system.
[0076] In this embodiment, a calibration device for an IMU and a laser radar is also provided, which is used to implement the above-mentioned embodiments and preferred implementation modes, and will not be repeated hereafter. As used below, the term "module" may be a combination of software and / or hardware that implements a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.
[0077] This embodiment provides a calibration device for an IMU and a laser radar, such as Figure 3 As shown, including:
[0078] A first conversion matrix calculation module 301 is used to calculate a first conversion matrix from an IMU coordinate system to a total station coordinate system;
[0079] A second conversion matrix calculation module 302 is used to calculate a second conversion matrix from the total station coordinate system to the laser radar coordinate system;
[0080] The external parameter initial calibration module 303 is used to calculate the initial external parameters from the IMU coordinate system to the laser radar coordinate system through the first conversion matrix and the second conversion matrix;
[0081] The sensor data acquisition module 304 is used to acquire the point cloud data scanned by the laser radar and the position and posture data of the IMU for the current moving object;
[0082] The external parameter optimization module 305 is used to optimize and adjust the initial external parameters based on the point cloud data and the posture data to obtain the target external parameters from the IMU coordinate system to the laser radar coordinate system.
[0083] In some optional implementations, the first conversion matrix calculation module 301 includes:
[0084] A reference point determination unit, used to determine the origin of the IMU coordinate system, the reference positions of the X-axis and the Y-axis on the IMU housing based on the IMU model data;
[0085] A first dot-marking unit is used to mark a reference position on the IMU housing through a total station, obtain the absolute coordinates of the origin of the IMU coordinate system, the points on the X-axis and the points on the Y-axis mapped in the total station coordinate system, and construct the XOY coordinate system of the IMU in the total station coordinate system based on the absolute coordinates;
[0086] A Z-axis direction determination unit, used for calculating the Z-axis direction vector of the IMU Z-axis in the total station coordinate system based on the vertical relationship between the IMU Z-axis and the XOY coordinate system plane;
[0087] The orthogonal processing unit is used to orthogonalize the X-axis vector, Y-axis vector and Z-axis direction vector of the XOY coordinate system to obtain three coordinate axis vectors of the IMU coordinate system in the total station coordinate system, and calculate the first transformation matrix through the obtained three coordinate axis vectors.
[0088] In some optional implementations, the second conversion matrix calculation module 302 includes:
[0089] A calibration object placement unit, used for placing a calibration plate and determining feature points in the calibration plate;
[0090] A radar scanning unit, used for scanning the calibration plate through a laser radar to obtain a feature point cloud representing feature points;
[0091] A first data collection unit, used to determine the coordinates of a first feature point in a laser radar coordinate system based on the feature point cloud, and put the coordinates of the first feature point into a first feature point set;
[0092] A second marking unit is used to mark the feature point by using a total station, obtain the second feature point coordinates of the feature point in the total station coordinate system, and put the second feature point coordinates into a second feature point set;
[0093] A cyclic data collection unit is used to keep the relative positions of the laser radar and the total station unchanged, move the calibration plate, and return to the step of scanning the calibration plate through the laser radar to increase the data in the first feature point set and the second feature point set;
[0094] The calculation unit is used to calculate the second transformation matrix based on the first feature point set and the second feature point set with added data after the calibration plate moves a preset number of times.
[0095] In some optional implementations, the sensor data acquisition module 304 includes:
[0096] Clock synchronization unit, used to synchronize the clocks of lidar and IMU;
[0097] A laser radar data acquisition unit, used to obtain point cloud data scanned by the laser radar;
[0098] A posture data acquisition unit is used to obtain posture data by integrating the initial linear acceleration and angular velocity of the IMU;
[0099] The interpolation unit is used to interpolate the pose data according to the timestamp of the point cloud data.
[0100] In some optional implementations, the external parameter optimization module 305 includes:
[0101] A third conversion matrix calculation unit, used to calculate the product of the pose data and the initial external parameter, to obtain a third conversion matrix from the current frame point cloud to the first frame point cloud in the point cloud data;
[0102] A mapping unit, used to map the current frame point cloud to the first frame using a third conversion matrix to obtain a mapped point cloud;
[0103] The adjustment unit is used to adjust the initial external parameters according to the error between the mapped point cloud and the first frame point cloud to obtain the target external parameters.
[0104] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.
[0105] The calibration device of the IMU and lidar in this embodiment is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.
[0106] The embodiment of the present invention also provides a computer device having the above Figure 3 The calibration setup for the IMU and lidar is shown.
[0107] See also Figure 4 , Figure 4 is a schematic diagram of the structure of a computer device provided by an optional embodiment of the present invention, such as Figure 4 As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components are connected to each other using different buses for communication, and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in or on the memory to display the graphical information of the GUI on an external input / output device (such as, a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Figure 4 A processor 10 is taken as an example.
[0108] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.
[0109] The memory 20 stores instructions executable by at least one processor 10, so that at least one processor 10 executes the method shown in the above embodiment.
[0110] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created according to the use of the computer device, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely arranged relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.
[0111] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid state drive; the memory 20 may also include a combination of the above types of memory.
[0112] The computer device further comprises a communication interface 30 for the computer device to communicate with other devices or a communication network.
[0113] The embodiment of the present invention also provides a computer-readable storage medium. The method according to the embodiment of the present invention can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.
[0114] A part of the present invention may be applied as a computer program product, such as a computer program instruction, which, when executed by a computer, can call or provide the method and / or technical solution according to the present invention through the operation of the computer. Those skilled in the art should understand that the existence of the computer program instruction in a computer-readable medium includes, but is not limited to, a source file, an executable file, an installation package file, etc., and accordingly, the way in which the computer program instruction is executed by the computer includes, but is not limited to: the computer directly executes the instruction, or the computer compiles the instruction and then executes the corresponding compiled program, or the computer reads and executes the instruction, or the computer reads and installs the instruction and then executes the corresponding installed program. Here, the computer-readable medium may be any available computer-readable storage medium or communication medium accessible to the computer.
[0115] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.
Claims
1. A calibration method for IMU and laser radar, characterized in that: The method comprises: Calculate the first transformation matrix from the IMU coordinate system to the total station coordinate system; Calculate the second transformation matrix from the total station coordinate system to the laser radar coordinate system; Calculate the initial external parameters from the IMU coordinate system to the laser radar coordinate system through the first conversion matrix and the second conversion matrix; For the current moving object, obtain the point cloud data scanned by the lidar and the position and posture data of the IMU; The initial external parameters are optimized and adjusted based on the point cloud data and the pose data to obtain target external parameters from the IMU coordinate system to the lidar coordinate system.
2. The method according to claim 1, characterized in that The first conversion matrix from the IMU coordinate system to the total station coordinate system is calculated, including: Determine the origin of the IMU coordinate system, the reference positions of the X-axis and the Y-axis on the IMU housing based on the IMU model data; Using a total station to mark the reference position on the IMU housing, the origin of the IMU coordinate system, the point on the X-axis and the point on the Y-axis are mapped to the absolute coordinates of the total station coordinate system, and the XOY coordinate system of the IMU is constructed in the total station coordinate system based on the absolute coordinates; Based on the perpendicular relationship between the IMU's Z axis and the XOY coordinate plane, calculate the Z axis direction vector of the IMU's Z axis in the total station coordinate system; The X-axis vector, Y-axis vector and Z-axis direction vector of the XOY coordinate system are orthogonalized to obtain three coordinate axis vectors of the IMU coordinate system in the total station coordinate system, and the first transformation matrix is calculated using the obtained three coordinate axis vectors.
3. The method according to claim 1, characterized in that: The calculating of the second transformation matrix from the total station coordinate system to the laser radar coordinate system includes: Placing a calibration plate and determining feature points in the calibration plate; Scanning the calibration plate by a laser radar to obtain a feature point cloud representing the feature points; Determine the coordinates of a first feature point in a laser radar coordinate system based on the feature point cloud, and put the first feature point coordinates into a first feature point set; Mark the feature point by a total station to obtain the second feature point coordinates of the feature point in the total station coordinate system, and put the second feature point coordinates into a second feature point set; Keeping the relative positions of the laser radar and the total station unchanged, moving the calibration plate, and returning to the step of scanning the calibration plate by the laser radar, and adding data in the first feature point set and the second feature point set; After the calibration plate is moved a preset number of times, the second transformation matrix is calculated based on the first feature point set and the second feature point set with added data.
4. The method according to claim 1, characterized in that: The step of obtaining the point cloud data scanned by the laser radar and the position and posture data of the IMU includes: Synchronizing the clocks of the laser radar and the IMU; Get the point cloud data scanned by LiDAR; The posture data is obtained by integrating the initial linear acceleration and angular velocity of the IMU; The pose data is interpolated according to the timestamp of the point cloud data.
5. The method according to claim 1, characterized in that The initial external parameters are optimized and adjusted based on the point cloud data and the pose data to obtain target external parameters from the IMU coordinate system to the laser radar coordinate system, including: Calculate the product of the pose data and the initial external parameter to obtain a third transformation matrix from the current frame point cloud to the first frame point cloud in the point cloud data; Mapping the current frame point cloud to the first frame using the third transformation matrix to obtain a mapped point cloud; The initial extrinsic parameter is adjusted according to an error between the mapping point cloud and the first frame point cloud to obtain the target extrinsic parameter.
6. The method according to claim 5, characterized in that The adjusting the initial extrinsic parameter according to the error between the mapping point cloud and the first frame point cloud to obtain the target extrinsic parameter includes: Divide the point cloud space into voxel grids; Registering the first frame point cloud and the mapping point cloud to corresponding voxel grids based on an octree algorithm; The initial extrinsic parameters are adjusted based on the minimum number of voxel grids occupied by the point cloud data.
7. The method according to claim 4, characterized in that The step of obtaining the point cloud data scanned by the laser radar includes: The distortion of the point cloud data is eliminated by using the initial external parameters to obtain dedistorted point cloud data.
8. A calibration device for IMU and laser radar, characterized in that: The device comprises: A first conversion matrix calculation module, used to calculate a first conversion matrix from an IMU coordinate system to a total station coordinate system; A second conversion matrix calculation module, used for calculating a second conversion matrix from the total station coordinate system to the laser radar coordinate system; An external parameter initial calibration module, used to calculate the initial external parameters from the IMU coordinate system to the laser radar coordinate system through the first conversion matrix and the second conversion matrix; The sensor data acquisition module is used to obtain the point cloud data scanned by the laser radar and the position and posture data of the IMU for the current moving object; The external parameter optimization module is used to optimize and adjust the initial external parameters based on the point cloud data and the posture data to obtain the target external parameters from the IMU coordinate system to the lidar coordinate system.
9. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the method according to any one of claims 1 to 7 by executing the computer instructions.
10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the method according to any one of claims 1 to 7.
Citation Information
Cited By
Multi-sensor data calibration method, system, medium, product and equipment
CN120907588A
