A method for constructing motion constraints for vehicle-mounted lidar-inertial calibration

Abstract

A method for constructing motion constraints for vehicle-mounted lidar-inertial calibration, which relates to the field of multi-sensor fusion perception technology for autonomous vehicles. Calculate the relative pose transformation between the IMU and LiDAR coordinate systems from time k-1 to time k, construct an external parameter calibration equation, consider multiple sets of measurements, transform the calibration problem into an optimization problem, and obtain the external parameters including roll angle and pitch angle by solving. Model the speed in the vehicle's forward direction, model the speed of the vehicle at time k through the vehicle's on-board wheel odometer, use the IMU to construct a prediction model of the speed, use the speed constraint to restore the constraint in the degraded direction, calculate the external parameters of LiDAR-IMU, and correct the heading angle. It is completely driven by data, simple and easy to deploy, does not rely on additional sensors for assistance and human participation, and has a high degree of automation.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor fusion perception for autonomous vehicles, and specifically to a method for constructing motion constraints for vehicle-mounted lidar-inertial calibration. Background Art

[0002] With the continuous development of technology, autonomous driving is becoming a key technology affecting the future industry. The autonomous positioning and navigation of vehicles rely on sensors such as lidar (Light Detection and Ranging, LiDAR) and inertial measurement unit (IMU). When these sensors are integrated into a vehicle, the extrinsic calibration of the sensors must be carried out to determine the relative position relationship between different sensor coordinate systems, so that the data from different sensors can be unified into a common reference coordinate system. Accurate extrinsic calibration is a prerequisite for multi-sensor information fusion and even directly determines the performance of multi-sensor fusion to a large extent.

[0003] Currently, most extrinsic calibration methods for LiDAR-IMU require the carrier to have sufficient motion excitation to ensure the reliability of the calibration results. However, since cars generally drive on flat roads and lack motion in the lateral and vertical directions of the vehicle, the extrinsic calibration problem of LiDAR-IMU degrades and all parameters cannot be accurately calibrated. Therefore, the extrinsic calibration of LiDAR-IMU often requires the assistance of other devices, such as calibration poles or camera sensors, or GPS sensors for auxiliary calibration, which is cumbersome to operate and introduces redundant sensors. Summary of the Invention

[0004] To solve the deficiencies in the background art, the present invention provides a method for constructing motion constraints for vehicle-mounted lidar-inertial calibration, which is completely driven by data itself, is simple and easy to deploy, does not rely on additional sensors for assistance and human participation, and has a high degree of automation.

[0005] To achieve the above object, the present invention adopts the following technical solution: A method for constructing motion constraints for vehicle-mounted lidar-inertial calibration, comprising the following steps:

[0006] Step 1: Obtain IMU and LiDAR data, and respectively calculate the relative pose transformation of the IMU coordinate system and the relative pose transformation of the LiDAR coordinate system between the (k - 1)th moment and the kth moment and

[0007] Step 2: Construct the following extrinsic calibration equation:

[0008]

[0009] In the formula, represents the rotation matrix from the LiDAR coordinate system to the IMU coordinate system, which is the variable to be solved, that is, the extrinsic parameters of LiDAR-IMU, and it is decomposed into three Euler angles: roll angle, pitch angle, and heading angle;

[0010] Step 3: Considering multiple sets of measurements, the calibration problem is transformed into an optimization problem as follows:

[0011]

[0012] In the formula, represents the right multiplication quaternion matrix corresponding to represents the left multiplication quaternion matrix corresponding to. The extrinsic parameters of LiDAR-IMU including roll angle and pitch angle are obtained by solving this optimization problem

[0013] Step 4: Model the speed in the vehicle's forward direction. The speed v of the vehicle at time k is modeled through the vehicle's on-board wheel odometer as follows: B,k The modeling is as follows:

[0014]

[0015] In the formula, v wheel,k is the speed measurement of the wheel odometer at time k, and n y and n z are the speed measurement errors in the lateral and vertical directions respectively;

[0016] Step 5: Use the IMU to construct a speed prediction model as follows:

[0017] v I,k = v I,k-1 - gΔt + R I,k-1 (a k-1 - b a )Δt (4)

[0018] In the formula, v I,k and v I,k-1 are the speeds of the IMU in the navigation coordinate system at time k and time k-1 respectively, g is the local gravitational acceleration vector, Δt is the time interval from time k-1 to time k, and R I,k-1 represents the attitude of the IMU in the navigation coordinate system at time k-1, a k-1 is the measurement value of the IMU accelerometer at time k-1, and b a is the zero bias of the accelerometer;

[0019] Step 6: Use the velocity constraint to recover the constraint in the degenerate direction, expressed as follows:

[0020]

[0021] where v B,k-1 represents the velocity of the vehicle at time k - 1, and respectively represent the rotation matrices from the IMU and LiDAR to the vehicle body coordinate system, and respectively represent the positions of the LiDAR in the map coordinate system at time k and k - 1;

[0022] Step 7: Calculate the extrinsic parameters of the LiDAR - IMU as follows:

[0023]

[0024] Step 8: The specific relationships of the three Euler angles of the extrinsic parameters of the LiDAR - IMU are as follows:

[0025]

[0026] where α represents the roll angle, β represents the pitch angle, γ represents the yaw angle, and only the estimated result of the yaw angle is corrected. The yaw angle is corrected as follows:

[0027]

[0028] where and respectively represent the element in the second row and first column and the element in the first row and first column of

[0029] Compared with the prior art, the beneficial effects of the present invention are as follows: By considering the non - holonomic constraints of the vehicle and using the on - vehicle wheel odometer to model the vehicle velocity, the present invention further constructs the velocity constraint in the motion degenerate direction. This method is simple and easy to deploy, completely driven by data, does not rely on other additional sensors for assistance and human participation, and has a high degree of automation. Brief Description of the Drawings

[0030] Figure 1 is the technical roadmap of the method of the present invention;

[0031] Figure 2 is the angular velocity projection diagram based on the calibration result in the embodiment;

[0032] Figure 3 is the angular velocity projection error diagram in the embodiment. Detailed Embodiment

[0033] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0034] As Figures 1 to 3 shown, a method for constructing motion constraints for vehicle-mounted lidar-inertial calibration, the technical route is combined with Figure 1 shown, and includes the following steps:

[0035] Step 1: Obtain IMU and LiDAR data, and calculate the relative pose transformation of the IMU coordinate system and the relative pose transformation of the LiDAR coordinate system respectively between the (k - 1)th moment and the kth moment. and

[0036] Step 2: Construct the external parameter calibration equation as follows:

[0037]

[0038] In the formula, represents the rotation matrix from the LiDAR coordinate system to the IMU coordinate system, which is the variable to be solved, that is, the external parameter of LiDAR-IMU, and it can be decomposed into three Euler angles: roll angle, pitch angle, and yaw angle;

[0039] Step 3: Considering multiple groups of measurements, transform the calibration problem into an optimization problem as follows:

[0040]

[0041] In the formula, represents the right multiplication quaternion matrix corresponding to represents the left multiplication quaternion matrix corresponding to

[0042] Obtain the external parameter of LiDAR-IMU including roll angle and pitch angle by solving this optimization problem. Since the calibration accuracy of the yaw angle in the external parameter is poor due to the planar motion of the vehicle, the vehicle motion constraint is used to correct the yaw angle in the external parameter.

[0043] Step 4: Model the speed of the vehicle in the forward direction. Since the vehicle is driving on a flat road surface and has no displacement and speed in the lateral and vertical directions, the speed v of the vehicle at the kth moment is modeled by the on-vehicle wheel odometer. B,k The modeling is as follows:

[0044] vB,k = [v wheel,k , n y , n z T (3)

[0045] where v wheel,k is the speed measurement of the wheel odometer at time k, and n y and n z are the speed measurement errors in the lateral and vertical directions respectively. Assume the errors are Gaussian white noise, σ y and σ z represent the standard deviations of n y and n z respectively;

[0046] Step Five: Use the IMU to construct the following speed prediction model:

[0047] v I,k = v I,k-1 - gΔt + R I,k-1 (a k-1 - b a )Δt (4)

[0048] where v I,k and v I,k-1 are the speeds of the IMU in the navigation coordinate system at time k and time k - 1 respectively, g is the local gravitational acceleration vector, Δt is the time interval from time k - 1 to time k, and R I,k-1 represents the attitude of the IMU in the navigation coordinate system at time k - 1, a k-1 is the measurement value of the IMU accelerometer at time k - 1, and b a is the zero bias of the accelerometer;

[0049] Step Six: Use the speed constraint to restore the constraint in the degraded direction, which is expressed as follows:

[0050]

[0051] where v B,k-1 represents the speed of the vehicle at time k - 1, and represent the rotation matrices from the IMU and LiDAR to the body coordinate system respectively, and represent the positions of the LiDAR in the map coordinate system (the LiDAR coordinate system at the initial time is the map coordinate system) at time k and time k - 1 respectively;

[0052] Step Seven: Calculate the extrinsic parameters of the LiDAR - IMU as follows:

[0053] ​

[0054] Step Eight: Extrinsic Parameters of LiDAR-IMU The specific relationships of the three Euler angles are as follows:

[0055]

[0056] In the formula, α represents the roll angle, β represents the pitch angle, and γ represents the yaw angle;

[0057] When using vehicle nonholonomic constraints to handle the calibration degradation problem, only the estimated result of the yaw angle is corrected, while the pitch angle and roll angle remain unchanged. The yaw angle is corrected as follows:

[0058]

[0059] In the formula, and respectively represent the element in the 2nd row and 1st column and the element in the 1st row and 1st column of

[0060] Embodiment

[0061] In this embodiment, the benchmark method using a camera sensor as an auxiliary is compared with the method proposed in the present invention to indirectly calibrate the extrinsic parameters of LiDAR-IMU. Among them, g = [0, 0, 9.8066] T , Δt = 0.005s, b a = [2.0000e-03, 2.0000e-03, 2.0000e-03] T , and the final calibration results are shown in Table 1:

[0062] Table 1 Comparison of Extrinsic Parameter Calibration Results

[0063]

[0064]

[0065] It can be seen that using the camera-assisted calibration method, the maximum error of the extrinsic rotation component is 3.56°, while using the calibration method of the present invention, the maximum error of the extrinsic rotation component is 0.87°, with a 75.9% performance improvement, which is better than the camera-assisted calibration method and does not require any other facilities for assistance.

[0066] Based on the calibration results, the angular velocity in the LiDAR coordinate system is projected into the IMU coordinate system through the extrinsic parameters. The angular velocity projection is combined with Figure 2 shown. In the same coordinate system, the curves of the two almost coincide. The error of the projection is combined with Figure 3As shown, the projection errors are all maintained near 0, indicating that the motion constraint construction method proposed by the present invention is effective, the external parameter calibration result is accurate, and it has comparable accuracy to the calibration methods that require additional equipment assistance.

[0067] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and without departing from the spirit or basic characteristics of the present invention, the present invention can be implemented in other forms. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent conditions of the claims are intended to be included in the present invention. Any reference signs in the claims should not be construed as limiting the claimed claim.

[0068] In addition, it should be understood that although this specification is described according to embodiments, not every embodiment only contains an independent technical solution. This narrative way of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A motion constraint construction method for vehicle-mounted laser radar-inertial navigation calibration, characterized in that: The following steps are involved: Step 1: Obtain IMU and LiDAR data, and calculate the relative pose transformation of the IMU coordinate system between time k-1 and time k. Relative pose transformation with LiDAR coordinate system Step 2: Construct the external parameter calibration equation as follows: In the formula, Represents the rotation matrix between the LiDAR coordinate system and the IMU coordinate system. It is the variable to be solved, that is, the external parameter of the LiDAR-IMU, which is decomposed into three Euler angles: roll angle, pitch angle and heading angle; Step 3: Consider multiple sets of measurements and transform the calibration problem into an optimization problem as follows: In the formula, express The corresponding right-multiplied quaternion matrix is, express The corresponding left-multiplied quaternion matrix is ​​obtained by solving the optimization problem to obtain the external parameters of the LiDAR-IMU including the roll angle and pitch angle Step 4: Model the vehicle's forward speed and use the vehicle's built-in wheel odometer to calculate the vehicle's speed v at time k. B,k The modeling is as follows: v B,k =[v wheel,k ,n y ,n z ] T (3) In the formula, v wheel,k is the speed measurement of the wheel odometer at time k, n y and n z are the velocity measurement errors in the lateral and vertical directions, respectively; Step 5: Use IMU to build a speed prediction model as follows: v I,k =v I,k-1 -gΔt+R I,k-1 (a k-1 -b a )Δt (4) In the formula, v I,k and v I,k-1 are the velocities of the IMU in the navigation coordinate system at time k and time k-1, g is the local gravity acceleration vector, Δt is the time interval from time k-1 to time k, and R I,k-1 represents the posture of the IMU in the navigation coordinate system at time k-1, a k-1 is the IMU accelerometer measurement value at time k-1, b a is the zero bias of the accelerometer; Step 6: Use the velocity constraint to restore the constraints in the degenerate direction, as shown below: In the formula, v B,k-1 represents the speed of the vehicle at time k-1, and Respectively represent the rotation matrices of IMU and LiDAR to the vehicle coordinate system, and Respectively represent the position of LiDAR in the map coordinate system at time k and time k-1; Step 7: Calculate the external parameters of LiDAR-IMU as follows: Step 8: External parameters of LiDAR-IMU The specific relationship between the three Euler angles is as follows: In the formula, α represents the roll angle, β represents the pitch angle, and γ represents the heading angle. Only the estimated result of the heading angle is corrected. The heading angle correction is as follows: In the formula, and Respectively The 2nd row and 1st column element and the 1st row and 1st column element.

Images