Calibration method for mutual constraints of laser radar, camera and inertial sensor

Abstract

The application discloses a kind of laser radar, camera and the external parameter calibration method of mutual restraint of inertial sensor, it includes the following steps: making checkerboard radar reflector plate, and form calibration system;Static calibration laser radar and camera, obtain the conversion external parameter of laser radar to camera;Dynamic calibration is carried out to inertial sensor from visual angle and laser radar angle respectively;State estimator is constructed;Minimization target is constructed, and the conversion external parameter between camera and inertial sensor in optimal state estimator is obtained;The conversion external parameter between laser radar and inertial sensor is obtained by the mutual conversion relationship between external parameter, and external parameter calibration is completed.This method combines the method that static calibration laser radar and visual camera, dynamic calibration laser radar and IMU, dynamic calibration visual camera and IMU mutually, so that each sensor forms mutually restrained triangle structure, realizes the time-space consistency and stability between high-precision multi-sensor.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of sensor fusion, in particular to a method for calibrating external parameters of mutual constraints of a laser radar, a camera and an inertial sensor. BACKGROUND

[0002] SLAM (Simultaneous Localization and Mapping) technology, which is translated as "simultaneous localization and mapping technology". It refers to a subject installed with a sensor, which estimates its own pose under the condition of no surrounding environment information through the sensor and its own motion, and establishes an environment model at the same time. The odometry technology is the front end of the SLAM technology, which estimates the changes of the position and attitude of the subject through different sensor data.

[0003] At present, there are many successful odometry frameworks using single measurement sensors, most of which are based on laser radars or visual cameras. However, as people's demand and requirement for intelligent robots increase, SLAM technology also needs to cope with more complex and challenging environments, so in recent years, sensors such as IMU (Inertial Measurement Unit) and GPS (Global Positioning System) have been introduced for assistance. The multi-sensor combined multi-modal fusion odometry system can integrate the working characteristics and intervals of each sensor, make the best of advantages and avoid disadvantages, and still work robustly in the case of degradation of some sensors, showing stronger and more extensive application.

[0004] Although the multi-sensor fusion odometry has obvious advantages, it is more complex when it is applied in the SLAM algorithm. In order to fully believe and use the data measured by each sensor, the multi-modal fusion odometry requires the coordinate axis conversion (i.e. external parameters) between sensors to be prior and determined. In this way, the information from different sensors can be converted to a common physical reference frame for calculation. Most existing external parameter calibration methods are only suitable for static systems and do not include the use of IMU. A few methods that include the calibration of IMU only stay in LIO systems or VIO systems, and there is no working example of combining the three sensors together, so a complete constraint system cannot be formed. SUMMARY

[0005] In view of the above problems in the prior art, the present application provides a method for calibrating external parameters of mutual constraints of a laser radar, a camera and an inertial sensor, which solves the problem of poor precision and robustness of the prior art in calibrating external parameters of a laser radar, a camera and an inertial sensor.

[0006] In order to achieve the above-mentioned application purposes, the technical scheme adopted by the present application is as follows:

[0007] A method for calibrating external parameters of mutual constraints of a laser radar, a camera and an inertial sensor is provided, which comprises the following steps:

[0008] S1, a chessboard radar reflector plate is made; the relative positions between the laser radar, the camera and the inertial sensor are fixed, and a calibration system is formed; the chessboard radar reflector plate is provided with a black-and-white square chessboard pattern, and the radar reflectivity of the black-and-white part patterns is different;

[0009] S2, based on the chessboard radar reflector plate, the laser radar and the camera are statically calibrated to obtain the conversion external parameter of the laser radar to the camera;

[0010] S3, the calibration system is moved, and the inertial sensor is dynamically calibrated from the visual angle and the laser radar angle respectively to obtain the initial value of the external parameter between the camera and the inertial sensor, the initial value of the external parameter between the laser radar and the inertial sensor, the multi-frame camera poses in a period of time in the world coordinate system and the multi-frame laser radar poses in the same period of time in the world coordinate system;

[0011] S4, a state estimator is constructed based on the camera pose, the conversion external parameter between the camera and the inertial sensor, the inverse depth of the reference point and the laser radar pose; wherein the initial value of the conversion external parameter between the camera and the inertial sensor is the initial value of the external parameter between the camera and the inertial sensor;

[0012] S5, a reference point is selected, and the marginalization residual in the fusion positioning process, the inertial sensor measurement residual between two frames, the visual re-projection error and the LO attitude error are obtained;

[0013] S6, a minimum target is constructed according to the data obtained in step S5 and the state estimator in step S4, and the optimal conversion external parameter between the camera and the inertial sensor in the state estimator is obtained;

[0014] S7, based on the conversion external parameter between the camera and the inertial sensor and the conversion external parameter of the laser radar to the camera, the conversion external parameter between the laser radar and the inertial sensor is obtained from the mutual conversion relationship between the camera-inertial sensor-laser radar external parameters, and the external parameter calibration is completed.

[0015] Further, the specific method of step S2 includes the following sub-steps:

[0016] S2-1, the internal parameter of the camera is obtained;

[0017] S2-2, the chessboard radar reflector plate is fixed, and the laser radar and the camera are respectively made to recognize the points on the calibration plate to correspondingly obtain 3D radar point cloud data and 2D image data;

[0018] S2-3, the 3D radar point cloud data is projected to 2D to obtain the projected data;

[0019] S2-4, align the projected data with the corresponding points in the 2D image data to obtain the conversion parameters of the laser radar to the camera

[0020] Further, the specific method of dynamically calibrating the inertial sensor from the visual angle in step S3 includes the following sub-steps:

[0021] A1, moving the calibration system to obtain inertial sensor data and camera data;

[0022] A2, aligning the inertial sensor data to the camera data;

[0023] A3, integrating the inertial sensor data between the kth frame of camera data and the k+1th frame of camera data to obtain the relative position, speed and rotation change of the calibration system between the two frames of images;

[0024] A4, taking the change value of the relative position of the inertial sensor to the camera as the initial value, taking the feature point re-projection error obtained from each frame of image as the optimization object, and performing nonlinear optimization on the optimization object to obtain the camera pose in the world coordinate system at the time points t = [t v1 ,t v2 ,…,t vK ] represented by different camera frames in the collection time and the initial value of the external parameter between the camera and the inertial sensor

[0025] Further, the specific method of dynamically calibrating the inertial sensor from the laser radar in step S3 includes the following sub-steps:

[0026] B1, moving the calibration system to obtain inertial sensor data and laser radar data;

[0027] B2, aligning the inertial sensor data to the laser radar data;

[0028] B3, calculating the motion estimation value of the laser radar from the i-th frame to the i+1-th frame by NDT scan matching and obtaining the laser radar pose in the world coordinate system at different time nodes in a period of time and the corresponding time points {t L1 ,t L2 ·…t LN};

[0029] B4, according to the formula:

[0030]

[0031] obtain the initial value of the external parameter between the laser radar and the inertial sensor wherein is the measurement of the inertial sensor corresponding to the i-th frame of lidar data; is the measurement of the inertial sensor corresponding to the i+1-th frame of lidar data.

[0032] Further, the expression of the state estimate in step S4 is:

[0033]

[0034] where X denotes the state estimate; x0, x1, … x N denote the camera pose at the corresponding camera frame, respectively, is the transformation parameter between the camera and the inertial sensor; {λ0, λ1, …, λ M denote the inverse depth of the M reference points in the image where they first appear; {l0, l1, … l A denote the radar pose at the corresponding radar frame, respectively.

[0035] Further, the marginalized residual expression in the fusion positioning process in step S5 is:

[0036] ‖r P -H P X‖ 2

[0037] The measurement residual expression of the inertial sensor between two frames is:

[0038]

[0039] The expression of the visual re-projection error is:

[0040]

[0041] The expression of the LO pose error is:

[0042]

[0043] where r P and H P are the prior parameters from the marginalization; is the acceleration change between two frames, is the velocity change between two frames, is the angle change between two frames, δb a is the acceleration offset change between two frames, δb w is the angular acceleration offset change between two frames; is the coordinate of the l-th pixel point observed in the d-th normalized camera coordinate system, is a pair of orthogonal bases on the tangent plane, T denotes the transpose of a matrix, is the coordinate of the lth pixel point observed in the dth initial camera coordinate system, and ‖.‖ denotes a norm; is the coordinate of the mth 3D coordinate point observed in the fth normalized radar coordinate system, is a set of orthogonal bases in a three-dimensional space, is the coordinate of the mth 3D coordinate point observed in the fth initial radar coordinate system.

[0044] Further, the expression of the minimization target in step S6 is:

[0045]

[0046] Further, the specific method of step S7 is:

[0047] The product of the conversion external parameter between the camera and the inertial sensor and the conversion external parameter of the laser radar to the camera is taken as the conversion external parameter between the laser radar and the inertial sensor, and the external parameter calibration is completed.

[0048] The method has the advantages that: the method combines the static calibration of the laser radar and the vision camera, the dynamic calibration of the laser radar and the IMU, and the dynamic calibration of the vision camera and the IMU, so that the sensors form a mutual constraint triangular structure, and the method can further be extended to a multi-factor constraint external parameter space associated with more sensors, and the spatio-temporal consistency and stability between the high-precision multiple sensors are realized. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a flowchart of the method;

[0050] Figure 2 is a schematic diagram of a checkerboard radar reflector;

[0051] Figure 3 is a schematic diagram of a calibration system. DETAILED DESCRIPTION

[0052] The specific embodiments of the present application are described below to facilitate the understanding of the present application by those skilled in the art, but it should be clear that the present application is not limited to the scope of the specific embodiments, and for those skilled in the art, it is obvious that various changes are within the spirit and scope of the present application defined and determined by the appended claims, and all the inventions utilizing the concept of the present application are within the scope of protection.

[0053] As Figure 1 ,Figure 2 and Figure 3 As shown in the laser radar, camera and inertial sensor mutual constraint external parameter calibration method includes the following steps:

[0054] S1, making a chessboard radar reflector plate; the relative position between the laser radar, the camera and the inertial sensor is fixed, and a calibration system is formed; the chessboard radar reflector plate is provided with a black and white square chessboard pattern, and the radar reflectivity of the black and white pattern is different;

[0055] S2, based on the chessboard radar reflector plate, static calibration of the laser radar and the camera is carried out, and the conversion external parameter of the laser radar to the camera is obtained;

[0056] S3, moving the calibration system, the inertial sensor is dynamically calibrated from the visual angle and the laser radar angle respectively, and the initial value of the external parameter between the camera and the inertial sensor, the initial value of the external parameter between the laser radar and the inertial sensor, the multi-frame camera pose in a period of time under the world coordinate system and the multi-frame laser radar pose in the same period under the world coordinate system are obtained respectively;

[0057] S4, constructing state estimator based on camera pose, conversion external parameter between camera and inertial sensor, inverse depth of reference point and laser radar pose; wherein the initial value of the conversion external parameter between the camera and the inertial sensor is the initial value of the external parameter between the camera and the inertial sensor;

[0058] S5, selecting a reference point, obtaining the marginalization residual in the fusion positioning process, the inertial sensor measurement residual between two frames, the visual re-projection error and the LO attitude error;

[0059] S6, constructing a minimization target according to the data obtained in step S5 and the state estimator in step S4, and obtaining the optimal conversion external parameter between the camera and the inertial sensor in the state estimator;

[0060] S7, based on the conversion external parameter between the camera and the inertial sensor and the conversion external parameter of the laser radar to the camera, obtaining the conversion external parameter between the laser radar and the inertial sensor from the mutual conversion relationship between the camera-inertial sensor-laser radar external parameter, and completing the external parameter calibration.

[0061] The specific method of step S2 includes the following sub-steps:

[0062] S2-1, obtaining the internal parameter of the camera;

[0063] S2-2, fixing the chessboard radar reflector plate, respectively making the laser radar and the camera identify the points on the calibration plate, and correspondingly obtaining 3D radar point cloud data and 2D image data;

[0064] S2-3, projecting the 3D radar point cloud data to 2D to obtain the projected data;

[0065] S2-4, align the projected data with the corresponding points in the 2D image data to obtain the conversion parameters of the laser radar to the camera

[0066] The specific method of dynamically calibrating the inertial sensor from the visual angle in step S3 includes the following sub-steps:

[0067] A1, moving the calibration system to obtain inertial sensor data and camera data;

[0068] A2, aligning the inertial sensor data to the camera data;

[0069] A3, integrating the inertial sensor data between the kth frame of camera data and the k+1th frame of camera data to obtain the relative position, velocity and rotation change of the calibration system between the two frames of images;

[0070] A4, taking the change value of the relative position of the inertial sensor to the camera as the initial value, taking the feature point re-projection error obtained from each frame of image as the optimization object, and performing nonlinear optimization on the optimization object to obtain the camera pose in the world coordinate system at the time points t = [t v1 ,t v2 ,…,t vK ] represented by different camera frames in the collection time and the initial value of the external parameter between the camera and the inertial sensor

[0071] The specific method of dynamically calibrating the inertial sensor from the laser radar in step S3 includes the following sub-steps:

[0072] B1, moving the calibration system to obtain inertial sensor data and laser radar data;

[0073] B2, aligning the inertial sensor data to the laser radar data;

[0074] B3, calculating the motion estimation value of the laser radar from the i-th frame to the i+1-th frame by NDT scan matching and obtaining the laser radar pose in the world coordinate system at different time nodes in a period of time and the corresponding time points {t L1 ,t L2 ·…t LN};

[0075] B4, according to the formula:

[0076]

[0077] obtain the initial value of the external parameter between the laser radar and the inertial sensor where is the measurement of the inertial sensor corresponding to the i-th frame of lidar data; is the measurement of the inertial sensor corresponding to the i+1-th frame of lidar data.

[0078] The expression of the state estimate in step S4 is:

[0079]

[0080] where X represents the state estimate; x0, x1, … x N represent the camera pose at the corresponding camera frame, respectively, is the transformation parameter between the camera and the inertial sensor; {λ0, λ1, …, λ M} represents the inverse depth of the M reference points in the image where they first appear; {l0, l1, … l A} are the radar poses at the corresponding radar frame, respectively.

[0081] The four kinds of residuals in step S5 are represented by Mahalanobis distance, and the marginalized residual expression in the fusion positioning process is:

[0082] ‖r P -H P X‖ 2

[0083] where r P and H P are the prior parameters from the marginalization; since the entire optimization process adopts the sliding window method, the oldest frame needs to be removed when the window slides, and the joint probability distribution is decomposed into the marginal probability distribution and the conditional probability distribution, and there is a prior residual calculation and optimization for this part.

[0084] The inertial sensor measurement residual expression between two frames is:

[0085]

[0086] which represents the difference between the change amount of PVQ and bias between two frames; represents that the inertial sensor measurement residual is composed of a 15-dimensional vector, including the acceleration change amount (three-dimensional) velocity change amount (three-dimensional) angle change amount (three-dimensional) acceleration offset change amount (three-dimensional) δb a and angular acceleration offset change amount (three-dimensional) δb w represents the prediction of the PVQ increment between two frames obtained by pre-integrating the IMU measurement value and the residual of the to-be-optimized quantity X. Participate in the overall solution by minimizing the residual.

[0087] The visual residual error is the reprojection error. For the lth pixel point P, the visual reprojection error is expressed as:

[0088]

[0089] is the coordinate of the lth pixel point observed in the dth normalized camera coordinate system, is a pair of orthogonal basis on the tangent plane, T denotes the transpose of a matrix, is the coordinate of the lth pixel point observed in the dth initial camera coordinate system, and ||.|| denotes the norm.

[0090] Similar to the visual constraint, the laser radar constraint on the calibration system is represented as the residual error of the radar point cloud projection. For the mth radar point L, the laser radar pose error is expressed as:

[0091]

[0092] is the coordinate of the mth 3D coordinate point observed in the normalized radar coordinate system of the fth laser radar, is a set of orthogonal basis in three-dimensional space, is the coordinate of the mth 3D coordinate point observed in the fth initial radar coordinate system. The pixel point and the reference point are feature points extracted in the image. The 3D coordinate point is a feature point extracted from the radar point cloud.

[0093] The expression of the minimization target in step S6 is:

[0094]

[0095] In the process of realizing the minimization target, the optimal X is obtained, and the optimal X is obtained. That is, the conversion external parameter between the camera and the inertial sensor is obtained.

[0096] The specific method of step S7 is: taking the product of the conversion external parameter between the camera and the inertial sensor and the conversion external parameter of the laser radar to the camera as the conversion external parameter between the laser radar and the inertial sensor to complete the external parameter calibration, that is

[0097] In one embodiment of the application, the accuracy of the calibration of the three sensors is approximately in the order of centimeters, wherein the dynamic effect is slightly worse than the static, and the influence of the calibration environment will also have more obvious fluctuations. The method improves the calibration accuracy of the IMU by one time (halves the error) through the guidance of the mutual calibration of the other two groups of sensors from the camera and radar calibration. At the same time, the multi-sensor constraint will bring higher stability, and the variance of the result is effectively improved.

Claims

1. A method for calibrating extrinsic parameters of a laser radar, a camera and an inertial sensor, characterized in that, The method comprises the following steps: S1, a chessboard radar reflector is made; the relative positions among the laser radar, the camera and the inertial sensor are fixed, and a calibration system is formed; the chessboard radar reflector is provided with a black-and-white square chessboard pattern, and the radar reflectivity of the black-and-white pattern is different; S2, the laser radar and the camera are calibrated based on the chessboard radar reflector, and the conversion external parameter of the laser radar to the camera is obtained; S3, the calibration system is moved, and the inertial sensor is dynamically calibrated from the visual angle and the laser radar angle respectively, so that the initial value of the external parameter between the camera and the inertial sensor, the initial value of the external parameter between the laser radar and the inertial sensor, the multi-frame camera poses in a period of time in the world coordinate system and the multi-frame laser radar poses in the same period of time in the world coordinate system are obtained respectively; S4, a state estimation quantity is constructed based on the camera pose, the conversion external parameter between the camera and the inertial sensor, the inverse depth of the reference point and the laser radar pose; the initial value of the conversion external parameter between the camera and the inertial sensor is the initial value of the external parameter between the camera and the inertial sensor; S5, a reference point is selected, and the marginalization residual in the fusion positioning process, the inertial sensor measurement residual between two frames, the visual re-projection error and the LO attitude error are obtained; S6, a minimization target is constructed according to the data obtained in step S5 and the state estimation quantity in step S4, and the conversion external parameter between the camera and the inertial sensor in the optimal state estimation quantity is obtained; S7, the conversion external parameter between the laser radar and the inertial sensor is obtained from the mutual conversion relationship among the camera-inertial sensor-laser radar external parameters based on the conversion external parameter between the camera and the inertial sensor and the conversion external parameter of the laser radar to the camera, and the external parameter calibration is completed.

2. The method of claim 1, wherein, The specific method of step S2 comprises the following sub-steps: S2-1, the internal parameter of the camera is obtained; S2-2, the chessboard radar reflector is fixed, and the laser radar and the camera recognize the points on the calibration board respectively, so that the 3D radar point cloud data and the 2D image data are obtained correspondingly; S2-3, the 3D radar point cloud data is projected to 2D to obtain the projected data; S2-4, align the projected data with the corresponding points in the 2D image data to obtain the conversion parameters of the laser radar to the camera 3. The method of claim 1, wherein, The specific method of dynamically calibrating the inertial sensor from the visual angle in step S3 comprises the following sub-steps: A1, the calibration system is moved, and the inertial sensor data and the camera data are obtained; A2, the inertial sensor data is aligned to the camera data; A3, the inertial sensor data between the kth frame of camera data and the k+1th frame of camera data is integrated to obtain the relative position, speed and rotation change of the calibration system between the two frames of images; A4. Using the change in the relative position between the inertial sensor and the camera as the initial value, the reprojection error of the feature points obtained from the image in each frame is taken as the optimization object. Nonlinear optimization is performed on the optimization object to obtain the time point t represented by different camera frames within the acquisition time. v1 ,t v2 ,…,t vK At that time, the camera pose in the world coordinate system Initial values ​​of extrinsic parameters between the camera and the inertial sensor 4. The method of claim 3, wherein, The specific method of dynamically calibrating the inertial sensor from the laser radar in step S3 comprises the following sub-steps: B1, the calibration system is moved, and the inertial sensor data and the laser radar data are obtained; B2, the inertial sensor data is aligned to the laser radar data; B3. Match the NDT scan to calculate the motion estimation value of the laser radar from the i-th frame to the i+1-th frame from the laser radar point cloud And get the pose of the laser radar at different time nodes in the world coordinate system in a period of time And its corresponding time point{t L1 ,t L2 ·…t LN}; B4, according to the formula: Obtaining initial value of external parameter between lidar and inertial sensor wherein is a measurement value of the inertial sensor corresponding to the i-th frame of lidar data; is a measurement value of the inertial sensor corresponding to the i+1-th frame of lidar data.

5. The method of claim 4, wherein, The expression of the state estimation quantity in step S4 is: where X denotes the state estimate; x0, x1, … x N denote the camera pose at the corresponding camera frame, is the transformation parameter between the camera and the inertial sensor;{λ0, λ1, …, λ M denote the inverse depth of the M reference points in the image where they first appear;{l0, l1, … l A denote the radar pose at the corresponding radar frame.

6. The method of claim 5, wherein, The expression of the marginalization residual in the fusion positioning process in step S5 is: || r P - H P X‖ 2 The expression of the inertial sensor measurement residual between two frames is: The expression of the visual re-projection error is: The expression of the LO attitude error is: where r P and H P are both marginalized prior parameters; is the acceleration change between two frames, is the velocity change between two frames, is the angle change between two frames, δb a is the acceleration offset change between two frames, δb w is the angular acceleration offset change between two frames; is the coordinate of the lth pixel observed in the dth normalized camera coordinate system, is a pair of orthogonal bases on the tangent plane, [. T denotes the transpose of a matrix, is the coordinate of the lth pixel observed in the dth initial camera coordinate system, ‖.‖ denotes the norm; is the coordinate of the mth 3D coordinate point observed in the fth normalized radar coordinate system, is a set of orthogonal bases in three-dimensional space, is the coordinate of the mth 3D coordinate point observed in the fth initial radar coordinate system.

7. The method of claim 6, wherein, The expression of the minimization target in step S6 is:

8. The method of claim 7, wherein, The specific method of step S7 is: The product of the conversion external parameter between the camera and the inertial sensor and the conversion external parameter of the laser radar to the camera is taken as the conversion external parameter between the laser radar and the inertial sensor, and the external parameter calibration is completed.

Images