A laser radar, camera, IMU joint calibration device and method

Abstract

The application discloses a kind of laser radar, camera, IMU joint calibration device, it is related to environmental perception technical field, joint calibration device includes rack, driving mechanism, top plate, acquisition device, and for driving top plate forward inclination and backward inclination movement inclination adjustment mechanism and for driving acquisition device horizontal swing swing angle adjustment mechanism;Joint calibration method introduces angle measuring instrument to measure the real swing angle of swing arm obtained, to correct the cumulative error of IMU by proportional integral method.The inclination adjustment mechanism and swing angle adjustment mechanism of the application are driven under the driving of the same driving mechanism, drive top plate and acquisition device linkage movement, realize the linkage of acquisition device forward inclination, backward inclination movement and horizontal swing movement, it is conducive to the detailed adjustment and continuity guarantee of shooting angle, can conveniently realize the massive shooting collection of different angle images, and the accuracy of calculation result is improved with the massive images of different angle shooting collection, finally realize the high-precision calibration of sensor joint assembly.

Description

Technical Field

[0001] This invention relates to the field of environmental sensing technology, and more specifically to a joint calibration device and method for calibrating sensor assembly. Background Technology

[0002] With the rapid development of science and technology, environmental perception technology is being applied more and more widely in various fields, including intelligent vehicles. At the same time, various fields are also putting forward higher requirements for the accuracy of the perception content of environmental perception technology.

[0003] The accuracy of environmental sensing is directly affected by the settings of the sensing scheme. Multi-sensor joint sensing is a widely used and relatively mature sensing scheme. It relies on a multi-sensor joint component composed of multiple sensors to sense the target content. Compared with single-sensor sensing schemes, it effectively improves the accuracy and comprehensiveness of the sensed content. However, the accuracy of the sensed content can be achieved is also affected by the calibration accuracy of the multi-sensor joint component.

[0004] Based on existing technology searches, the following known technical solutions exist for sensor calibration:

[0005] Prior Art 1:

[0006] Some scholars have proposed a hand-eye calibration method that can bypass the limitations of a calibration board and perform self-calibration. However, experimental results show that this calibration method inevitably loses some calibration accuracy, while the application fields of multi-sensor joint sensing often have high requirements for calibration accuracy. Therefore, this calibration method is clearly not suitable for the calibration of multi-sensor joint components.

[0007] Prior art 2:

[0008] Application No.: 2018110369542, Application Date: 2018.09.06, Publication (Announcement) Date: 2020.03.13. This prior art provides a joint calibration method, joint calibration device, and electronic device. The joint calibration method is used to calibrate a vision-inertial joint device, wherein the vision-inertial joint device includes an image acquisition unit and an inertial measurement unit. The joint calibration method includes the following steps: processing the acquired image data sequence to obtain a visual pose sequence of the image acquisition unit; fitting the visual pose sequence into a continuous spline curve to obtain a visual computation data sequence corresponding to the inertial timestamp of the inertial measurement unit; performing cross-correlation analysis on the acquired inertial data sequence and the visual computation data sequence to obtain an initial time delay value between the image acquisition unit and the inertial measurement unit; and analyzing the minimization error between the visual computation data and the inertial data sequence to obtain an initial pose value between the image acquisition unit and the inertial measurement unit.

[0009] The calibration methods described above rely on a large amount of image data acquired by the image acquisition unit from different perspectives. However, no corresponding methods or devices supporting automated image data acquisition are publicly available. Manually acquiring large batches of images from multiple angles inevitably results in long acquisition times, low efficiency, and an enormous workload. Furthermore, to eliminate the distortion characteristics of the acquired images inherent in the image acquisition unit, there is a possibility of further increasing the amount of image data acquired to meet the distortion correction requirements, which would undoubtedly exacerbate the problems of long acquisition times, low efficiency, and an enormous workload in this calibration method. In addition, while the existing technologies using IMU and camera for sensor co-processing can basically meet the needs of environmental perception in ideal working conditions, they cannot adapt to less ideal working conditions such as numerous obstacles or harsh image acquisition environments. In non-ideal conditions, incomplete information acquisition is likely to occur, which can easily lead to serious problems such as driving safety issues in some applications, such as intelligent vehicles. The existing technologies also fail to consider the accumulation of rounding errors during IMU calculations, which causes the error in IMU measurement results to increase continuously over time.

[0010] The above search results show that the above technical solutions do not affect the novelty of the present invention; and the combination of the above prior art does not destroy the inventiveness of the present invention. Summary of the Invention

[0011] To avoid the shortcomings of the prior art, the present invention provides a joint calibration device and method for lidar, camera, and IMU.

[0012] The present invention adopts the following technical solution to solve the technical problem: a joint calibration device for lidar, camera, and IMU, wherein the acquisition device is mounted on a top plate, the drive mechanism is mounted on a frame for support and fixation, and the calibration plate is located in the acquisition area of ​​the acquisition device. The device includes a tilt adjustment mechanism for driving the top plate to tilt forward and backward and a swing adjustment mechanism for driving the acquisition device to swing horizontally. The tilt adjustment mechanism and the swing adjustment mechanism drive the top plate and the acquisition device to move in linkage under the drive of the same drive mechanism.

[0013] The tilt adjustment mechanism includes a threaded driven shaft, a threaded sleeve, and a swing rod; the swing rod is provided at the bottom of the front and rear of the top plate, the threaded sleeve is located below the front of the top plate, and its top end is rotatably connected to the top plate through the swing rod located at the bottom of the front of the top plate; the threaded driven shaft is threaded into the threaded sleeve through the bottom end of the threaded sleeve.

[0014] The translational output end and the rotational output end of the drive mechanism output translational and rotational motions respectively; the translational output end is rotatably connected to the top plate via a swing rod located at the bottom of the rear part of the top plate, forming a rotating pair that supports the forward and backward tilting motion of the top plate; the translational output end drives the rear part of the top plate to reciprocate in the vertical direction; the rotational output end is installed and connected to the tilt angle adjustment rotational input end located at the bottom of the threaded driven shaft, driving the threaded driven shaft to rotate around its own axis.

[0015] Furthermore, the swing angle adjustment mechanism includes a crank, a swing rod, a sliding sleeve, a swing arm, and an angle measuring instrument for measuring the swing angle of the swing arm;

[0016] The swing arm is an L-shaped rod structure consisting of a horizontal section and a vertical section. The front end of the horizontal section is rotatably connected to the frame, forming a rotating joint that supports the horizontal swing of the horizontal section. The end of the vertical section is rotatably connected to the front end of the swing arm, forming two rotating joints that support the vertical section to drive the front end of the swing arm to perform horizontal swing, forward tilt, and backward tilt.

[0017] The end of the swing arm is fixedly installed to the acquisition device; the crank is connected to the rotation output end through the swing angle adjustment input end set at its front end, and the end is rotatably connected to the sliding sleeve that is slidably fitted on the horizontal section.

[0018] The end of the crank and the sliding sleeve form a rotating pair about a vertical axis, and the sliding sleeve and the horizontal segment form a sliding pair along the horizontal segment. The rotation output end drives the crank to rotate around the vertical axis at its front end.

[0019] Furthermore, the drive mechanism includes a drive motor, a drive cam, a lifting rod, a first rotating groove, a second rotating groove, a drive shaft, and a drive gear;

[0020] The drive motor is mounted and fixed to the frame, and its output end is fixedly connected to the drive cam. The drive motor drives the drive cam to rotate around a fixed axis of a horizontal axis. The lifting rod is located above the drive cam, and its bottom end is fitted with the drive cam. The two form a cam pair that drives the lifting rod to reciprocate vertically. The top end of the lifting rod serves as the translational output end of the drive mechanism and is rotatably connected to the bottom rear part of the top plate through the swing rod.

[0021] The drive shaft is rotatably mounted on the frame, and a drive gear serving as the rotation output end is fixed thereon; the front end of the first rotating groove is rotatably connected to the middle part of the lifting rod, and the end end is rotatably connected to the front end of the second rotating groove, and the end end of the second rotating groove is fixedly connected to the drive shaft; a rotating pair about a horizontal axis is formed between the front end of the first rotating groove and the middle part of the lifting rod, between the end end of the first rotating groove and the front end of the second rotating groove, and between the drive shaft and the frame.

[0022] Furthermore, the tilt angle adjustment rotation input end and the swing angle adjustment input end are respectively the tilt angle adjustment gear and the swing angle adjustment gear, both of which are bevel gears, symmetrically arranged on the upper and lower sides of the drive gear, and both meshing with the drive gear, which is also a bevel gear.

[0023] Furthermore, the top of the lifting rod is a support rod structure extending horizontally to both sides, and each end of the support rod is rotatably connected to the top plate via a swing rod.

[0024] Furthermore, the swing arm is a swing arm.

[0025] Furthermore, the acquisition device includes a camera, an IMU, a lidar, and a mounting plate. The camera, IMU, and lidar are mounted and fixed to the mounting plate, and the mounting plate is rotatably mounted to the top of the top plate. The end of the swing arm is mounted and fixed to the mounting plate.

[0026] The mounting plate is rotatably mounted to the top of the top plate, forming a rotating pair around a vertical axis; the end of the swing arm is fixed to the side of the mounting plate.

[0027] Furthermore, the drive mechanism and tilt adjustment mechanism are provided with a housing, and the housing has clearance holes to accommodate the swinging motion of the swing rod; the bottom of the frame is provided with multiple casters.

[0028] A method for joint calibration of lidar, camera, and IMU, using the aforementioned calibration device to calibrate a multi-sensor joint component, includes the following steps:

[0029] The first step is to install and fix the IMU, LiDAR, and camera onto the mounting plate, and adjust the position of the calibration plate so that it is in the center of the camera's image.

[0030] The second step is to perform a data acquisition cycle using the joint calibration device.

[0031] The third step is to obtain the camera extrinsic parameter matrix as the camera calibration result using the following formula:

[0032]

[0033] Among them, [uv 1] T The coordinates are in the camera pixel coordinate system, [X w Y w Z w 1] T Let f be the coordinate form in the world coordinate system of the camera, and d be the focal length of the camera. x d y Let x and y be the pixel conversion units (mm / Pixel), u0 and v0 be the offsets of the center of the projection screen relative to the optical axis, R be the rotation matrix, and t be the translation vector.

[0034] Fourth step: Obtain the camera and lidar rotation and translation matrices as the joint calibration result of the camera and lidar by following Equations 2 to 4.

[0035]

[0036] Among them, [X w Y w Z w 1] T R represents the coordinate form in the world coordinate system of the lidar. L to C It is the rotation matrix that transforms the lidar coordinate system to the camera coordinate system, T L to C It is the translation vector that transforms the lidar coordinate system to the camera coordinate system;

[0037] ||N L ||-||N C ||=T L to C n L (Formula 3);

[0038] R L to C n L n C =1 (Equation 4);

[0039] Where, n L and n C The spatial normals N of the camera coordinate system origin and the lidar coordinate system origin are respectively. L and N C , unit vector;

[0040] Fifth step: Obtain the corrected swing angle θ at time k using Equation 5. c (k);

[0041]

[0042] Where, θ r(k) represents the true swing angle of the swing arm at time k, measured by the angle measuring instrument; i represents the time before time k; Δt represents the sampling time interval between time k and time t; θ m (k) represents the swing angle measured by the IMU at time k, where K p K is the proportional gain coefficient. i This is the integral gain coefficient;

[0043] The three-dimensional rotation q of the IMU is represented using quaternions. M :

[0044] q M =q0+q1i1+q2i2+q3i3;

[0045] Where i1 represents the rotation of the plane intersecting the X and Y axes about the positive X-axis and towards the positive Y-axis, i2 represents the rotation of the plane intersecting the Z-axis and the X-axis about the positive Z-axis and towards the positive X-axis, and i3 represents the rotation of the plane intersecting the Y-axis and the Z-axis about the positive Y-axis and towards the positive Z-axis.

[0046] With θ c (k) adds constraints to the quaternions of the IMU:

[0047]

[0048] Combined with the three-dimensional rotation amount q of the camera (61) C The quaternion q is obtained by maximizing the function. M to C ;

[0049] The optimal quaternion is obtained by following formula six.

[0050]

[0051] The best quaternion Converting to matrix form yields the optimally estimated rotation matrix R between the camera and the IMU. M to C :

[0052]

[0053] The translation matrix T between the camera (61) and the IMU (62) is obtained by following formula 7. M to C :

[0054] T M to C =P M -R M to C P c (Formula 7);

[0055] Among them, P c P represents the camera translation amount obtained through feature point tracking on the calibration board. MThis refers to the inertial translation obtained through IMU pre-integration;

[0056] The camera-IMU rotation and translation matrix is ​​then obtained as the result of the joint calibration of the camera and IMU.

[0057] The sixth step is to integrate and obtain the joint calibration results of the camera, IMU, and LiDAR, including the camera extrinsic parameter matrix. Camera LiDAR rotation and translation matrix and camera IMU rotation and translation matrix

[0058] Furthermore, the data collection process in the second step includes the following steps:

[0059] The drive motor drives the drive cam to rotate half a revolution, simultaneously achieving the following three motions. At the same time, the camera, IMU, and lidar of the data acquisition device continuously collect data:

[0060] The first type of sport:

[0061] The drive cam causes the lifting rod to move upward and then downward, so that the rear side of the top plate follows the lifting rod to move upward and then downward.

[0062] The second type of exercise:

[0063] The drive cam drives the lifting rod to move upward and then downward. This motion drives the drive shaft and the drive gear fixed on it to rotate through the first and second rotating grooves. Then, the tilt adjustment gear meshing with the drive gear drives the threaded driven shaft to rotate. The rotation is converted into linear motion through the threaded sleeve, so that the front side of the top plate follows the threaded sleeve to move downward and then upward.

[0064] The first and second types of motion described above together constitute the tilting and then backward movement of the top plate and the data collection device.

[0065] The third type of exercise:

[0066] The drive cam drives the lifting rod to move upward and then downward. This movement drives the drive shaft and the drive gear fixed on it to rotate through the first and second rotating grooves. Then, the swing angle adjustment gear meshing with the drive gear drives the crank to swing, so that the sliding sleeve slides back and forth along the horizontal section of the swing rod once, while driving the swing rod to swing back and forth horizontally once. Then, the swing arm drives the acquisition device to swing back and forth horizontally once.

[0067] Subsequently, the drive motor continues to drive the drive cam to rotate half a revolution, so that the various mechanisms of the joint calibration device are reset.

[0068] This invention provides a joint calibration device and method for lidar, camera, and IMU, which has the following beneficial effects:

[0069] 1. The tilt adjustment mechanism and swing adjustment mechanism of the present invention, driven by the same drive mechanism, drive the top plate and the acquisition device to move in linkage, realize the linkage of the forward tilt, backward tilt and horizontal swing of the acquisition device, which is conducive to the detailed adjustment of the shooting angle and the guarantee of continuity. It can conveniently realize the large number of images captured from different angles, and improve the accuracy of the calculation results by capturing a large number of images from different angles, and finally realize the high-precision calibration of the sensor joint components.

[0070] 2. This invention can effectively reduce the waste rate of collected samples and improve the efficiency of joint calibration by reasonably setting the linkage between the tilt adjustment mechanism and the swing angle adjustment mechanism.

[0071] 3. This invention introduces the actual swing angle of the swing arm obtained by the angle measuring instrument, and uses the proportional-integral method to correct the cumulative error of the IMU, so as to avoid the cumulative measurement error of the IMU leading to a continuous increase in calibration error, which is conducive to ensuring calibration accuracy.

[0072] 4. This invention improves the speed and accuracy of joint calibration, solves the problem of low efficiency in high-precision joint calibration, and fills the gap in high-precision calibration mechanisms for large-scale multi-sensor calibration. Attached Figure Description

[0073] Figure 1 This is a schematic diagram of the structure of the present invention;

[0074] Figure 2 This is a schematic diagram of the internal structure of the present invention;

[0075] Figure 3 This is a schematic diagram of the drive mechanism of the present invention;

[0076] Figure 4 This is a schematic diagram of the tilt adjustment mechanism of the present invention;

[0077] Figure 5 This is a schematic diagram of the swing angle adjustment mechanism of the present invention;

[0078] Figure 6 This is a right-side view of the internal structure of the present invention.

[0079] In the picture:

[0080] 1. Drive mechanism; 11. Drive motor; 12. Drive cam; 13. Lifting rod; 14. First rotating groove; 15. Second rotating groove; 16. Drive shaft; 17. Drive gear; 18. Support rod; 2. Tilt adjustment mechanism; 21. Tilt adjustment gear; 22. Threaded driven shaft; 23. Threaded sleeve; 24. Swing rod; 3. Swing angle adjustment mechanism; 31. Swing angle adjustment gear; 32. Crank; 33. Swing rod; 34. Sliding sleeve; 35. Swing arm; 4. Frame; 5. Top plate; 6. Data acquisition device; 61. Camera; 62. IMU; 63. LiDAR; 64. Mounting plate; 7. Calibration plate; 8. Housing; 81. Clearance hole. Detailed Implementation

[0081] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0082] like Figures 1-6 As shown, the structural relationship is as follows: the acquisition device 6 is installed on the top plate 5, the drive mechanism 1 is installed on the frame 4 for support and fixation, and the calibration plate 7 is located in the acquisition area of ​​the acquisition device 6. It includes a tilt adjustment mechanism 2 for driving the top plate 5 to tilt forward and backward and a swing adjustment mechanism 3 for driving the acquisition device 6 to swing horizontally. The tilt adjustment mechanism 2 and the swing adjustment mechanism 3 drive the top plate 5 and the acquisition device 6 to move in linkage under the drive of the same drive mechanism 1.

[0083] The tilt adjustment mechanism 2 includes a threaded driven shaft 22, a threaded sleeve 23, and a swing rod 24; the top plate 5 is provided with a swing rod 24 at the bottom of the front and rear parts, the threaded sleeve 23 is located below the front of the top plate 5, and its top end is rotatably connected to the top plate 5 through the swing rod 24 located at the bottom of the front of the top plate 5; the threaded driven shaft 22 is threaded into the threaded sleeve 23 through the bottom end of the threaded sleeve 23.

[0084] The translational output end and the rotational output end of the drive mechanism 1 output translational and rotational motions respectively. The translational output end is rotatably connected to the top plate 5 via the swing rod 24 located at the bottom rear of the top plate 5, forming a rotating pair that supports the forward and backward tilting motion of the top plate 5. The translational output end drives the rear of the top plate 5 to reciprocate in the vertical direction. The rotational output end is installed and connected to the tilt angle adjustment rotational input end located at the bottom of the threaded driven shaft 22, driving the threaded driven shaft 22 to rotate around its own axis.

[0085] Preferably, the swing angle adjustment mechanism 3 includes a crank 32, a swing rod 33, a sliding sleeve 34, a swing arm 35, and an angle measuring instrument for measuring the swing angle of the swing arm 35;

[0086] The swing rod 33 is an L-shaped rod structure consisting of a horizontal section and a vertical section. The front end of the horizontal section is rotatably connected to the frame 4, forming a rotating pair that supports the horizontal swing of the horizontal section. The end of the vertical section is rotatably connected to the front end of the swing arm 35, forming two rotating pairs that support the vertical section to drive the front end of the swing arm 35 to perform horizontal swing, forward tilting and backward tilting movements.

[0087] In actual installation, the end of the vertical section and the front end of the swing arm 35 can be connected by a two-way swivel joint;

[0088] Considering that the trajectory of the end of the swing arm 35 is an arc very close to a straight line under the forward and backward tilting movements of the top plate 5 and the swing arm 35, the front end of the swing arm 35 can also be set as a sleeve-like structure with a hole diameter slightly larger than the diameter of the end of the vertical section, and fitted onto the end of the vertical section in a clearance fit assembly form, such as... Figure 5 As shown; under this structure, a rotating pair around a vertical axis is formed between the end of the vertical section and the front end of the swing arm 35, and a vertical sliding pair with a certain horizontal clearance is formed, which can also support the implementation of various functions of the joint calibration device;

[0089] The end of the swing arm 35 is fixedly installed to the acquisition device 6; the crank 32 is connected to the rotation output end through the swing angle adjustment input end set at its front end, and the end is rotatably connected to the sliding sleeve 34 that is slidably fitted on the horizontal section.

[0090] The end of the crank 32 and the sliding sleeve 34 form a rotating pair around the vertical axis, and the sliding sleeve 34 and the horizontal section form a sliding pair along the horizontal section. The rotating output end drives the crank 32 to rotate around the vertical axis at its front end.

[0091] Preferably, the drive mechanism 1 includes a drive motor 11, a drive cam 12, a lifting rod 13, a first rotating groove 14, a second rotating groove 15, a drive shaft 16, and a drive gear 17;

[0092] The drive motor 11 is mounted and fixed on the frame 4, and its output end is fixedly connected to the drive cam 12. The drive motor 11 drives the drive cam 12 to rotate around the fixed axis of the horizontal axis. The lifting rod 13 is located above the drive cam 12, and its bottom end is fitted with the drive cam 12. The two form a cam pair that drives the lifting rod 13 to reciprocate in the vertical direction. The top end of the lifting rod 13 serves as the translational output end of the drive mechanism 1 and is rotatably connected to the bottom rear of the top plate 5 through the swing rod 24.

[0093] The drive shaft 16 is rotatably mounted on the frame 4, and a drive gear 17 serving as the rotation output end is fixed on it; the front end of the first rotating groove 14 is rotatably connected to the middle part of the lifting rod 13, and the end is rotatably connected to the front end of the second rotating groove 15, and the end of the second rotating groove 15 is fixedly connected to the drive shaft 16; a rotating pair around a horizontal axis is formed between the front end of the first rotating groove 14 and the middle part of the lifting rod 13, between the end of the first rotating groove 14 and the front end of the second rotating groove 15, and between the drive shaft 16 and the frame 4.

[0094] Preferably, the tilt angle adjustment rotation input end and the swing angle adjustment input end are tilt angle adjustment gear 21 and swing angle adjustment gear 31, respectively. Both are bevel gears, symmetrically arranged on the upper and lower sides of the drive gear 17, and both are meshed with the drive gear 17, which is also a bevel gear.

[0095] Preferably, the top end of the lifting rod 13 is a support rod 18 structure extending horizontally to both sides, and each end of the support rod 18 is rotatably connected to the top plate 5 through a swing rod 24.

[0096] Preferably, the swing arm 24 is a swing arm.

[0097] Preferably, the acquisition device 6 includes a camera 61, an IMU 62, a lidar 63, and a mounting plate 64. The camera 61, IMU 62, and lidar 63 are mounted and fixed on the mounting plate 64. During installation, it should be ensured that there is no relative displacement between the camera 61, IMU 62, and lidar 63. The mounting plate 64 is rotated and installed on the top of the top plate 5. The end of the swing arm 35 is fixed and installed on the mounting plate 64.

[0098] Mounting plate 64 is rotatably mounted to the top of top plate 5, forming a rotating pair around a vertical axis; the end of swing arm 35 is mounted and fixed to the side of mounting plate 64.

[0099] Preferably, the drive mechanism 1 and the tilt adjustment mechanism 2 are provided with a housing 8, and the housing 8 is provided with a clearance hole 81 to accommodate the swinging motion of the swing rod 33; the bottom of the frame 4 is provided with multiple casters.

[0100] The calibration of a multi-sensor joint assembly using the aforementioned joint calibration device includes the following steps:

[0101] The first step is to install and fix the IMU62, LiDAR 63 and camera 61 onto the mounting plate 64, and adjust the position of the calibration plate 7 so that the calibration plate 7 is located in the center of the image captured by the camera 61.

[0102] The second step is to perform a data acquisition cycle using the joint calibration device.

[0103] The third step is to obtain the camera extrinsic parameter matrix as the calibration result of camera 61 using the following formula:

[0104]

[0105] Among them, [uv 1] T The coordinates are in the camera pixel coordinate system, [X w Y w Z w 1] T Let f be the coordinate form in the world coordinate system of the camera, and d be the focal length of the camera. x d y Let x and y be the pixel conversion units (mm / Pixel), u0 and v0 be the offsets of the center of the projection screen relative to the optical axis, R be the rotation matrix, and t be the translation vector.

[0106] For specific methods on obtaining the calibration results of camera 61, please refer to Zhang Zhengyou's calibration method;

[0107] Fourth step: Obtain the camera-lidar rotation and translation matrix as the joint calibration result of camera 61 and lidar 63 by following Equations 2 to 4.

[0108]

[0109] Among them, [X w Y w Z w 1] T R represents the coordinate form in the world coordinate system of the lidar. L to C It is the rotation matrix that transforms the lidar coordinate system to the camera coordinate system, T L to C It is the translation vector that transforms the lidar coordinate system to the camera coordinate system;

[0110] ||N L ||-||N C ||=T L to C n L (Formula 3);

[0111] R L to C n L n C =1 (Equation 4);

[0112] Where, n L and n C The spatial normals N of the camera coordinate system origin and the lidar coordinate system origin are respectively. L and N C , unit vector;

[0113] For the specific method of obtaining the joint calibration results of camera 61 and lidar 63, please refer to the checkerboard joint calibration method (Park SU, Chung M J. Extrinsic calibration between a 3D laser scanner and a Camera using PCA method[C] / / International Conference on Ubiquitous Robots & Ambient Intelligence.IEEE, 2013);

[0114] The spatial normal N between the origin of the camera coordinate system and the origin of the lidar coordinate system L and N C unit vector n L and n C The calibration board point cloud in the point cloud collected by the LiDAR can be obtained using relevant algorithms in the PCL (Point Cloud Library) open-source programming library, and then calculated using mathematical methods.

[0115] When performing the fitting calculation, T can be solved using the least squares method based on the linear relationship of Equations 3 and 4 respectively. L to C and R L to C ;

[0116] Fifth step: Obtain the corrected swing angle θ at time k using Equation 5. c (k);

[0117]

[0118] Where, θ r (k) represents the true swing angle of the swing arm at time 35k, measured by the angle measuring instrument; i represents the time before time k; Δt represents the sampling time interval between time k and time t; θ m (k) represents the swing angle measured by the IMU at time k, where K p K is the proportional gain coefficient. i This is the integral gain coefficient;

[0119] proportional gain coefficient K p and integral gain coefficient K i It can be obtained through extensive simulation and experimental debugging using existing technologies;

[0120] The three-dimensional rotation q of the IMU is represented using quaternions. M :

[0121] q M =q0+q1i1+q2i2+q3i3;

[0122] Where i1 represents the rotation of the plane intersecting the X and Y axes about the positive X-axis and towards the positive Y-axis, i2 represents the rotation of the plane intersecting the Z-axis and the X-axis about the positive Z-axis and towards the positive X-axis, and i3 represents the rotation of the plane intersecting the Y-axis and the Z-axis about the positive Y-axis and towards the positive Z-axis.

[0123] With θ c (k) adds constraints to the quaternions of the IMU:

[0124]

[0125] Combined with the three-dimensional rotation q of camera 61 C The quaternion q is obtained by maximizing the function. M to C ;

[0126] The optimal quaternion is obtained by following formula six.

[0127]

[0128] The best quaternion Converting to matrix form yields the optimally estimated rotation matrix R between camera 61 and IMU 62. M to C :

[0129]

[0130] The translation matrix T between camera 616 and IMU62 is obtained by following formula 7. M to C :

[0131] T M to C =P M -R M to C P c (Formula 7);

[0132] Among them, P c P represents the camera translation amount obtained through feature point tracking on the calibration board. M This refers to the inertial translation obtained through IMU pre-integration;

[0133] The camera IMU rotation and translation matrix is ​​then obtained as the joint calibration result of camera 61 and IMU 62.

[0134] The sixth step is to integrate and obtain the joint calibration results of camera 61, IMU 62, and LiDAR 63, including the camera extrinsic parameter matrix. Camera LiDAR rotation and translation matrix and camera IMU rotation and translation matrix

[0135] For details on the method of obtaining the joint calibration results of camera 61 and IMU 62, please refer to the offline joint calibration method (FURGAL EP, REHDER J, SIEGWAR TR. Unified temporal and spatial calibration for multi-sensor systems[C]. IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), 2013, 46: 1280-1286).

[0136] Preferably, the data acquisition process in the second step includes the following steps:

[0137] The drive motor 11 drives the drive cam 12 to rotate half a revolution, simultaneously driving the following three types of motion. At the same time, the camera 61, IMU 62, and lidar 63 of the acquisition device 6 continuously acquire data:

[0138] The first type of sport:

[0139] The drive cam 12 drives the lifting rod 13 to move upward and then downward, so that the rear side of the top plate 5 follows the lifting rod to move upward and then downward.

[0140] The second type of exercise:

[0141] The drive cam 12 drives the lifting rod 13 to move upward and then downward. This movement drives the drive shaft 16 and the drive gear 17 fixed on it to rotate through the first rotating groove 14 and the second rotating groove 15. Then, the tilt adjustment gear 21 meshing with the drive gear 17 drives the threaded driven shaft 22 to rotate. The rotation is converted into linear motion through the threaded sleeve 23, so that the front side of the top plate 5 follows the threaded sleeve 23 to move downward and then upward.

[0142] The first and second movements mentioned above together constitute the tilting and then backward movement of the top plate 5 and the collecting device 6.

[0143] The third type of exercise:

[0144] The drive cam 12 drives the lifting rod 13 to move upward and then downward. This movement drives the drive shaft 16 and the drive gear 17 fixed on it to rotate through the first rotating groove 14 and the second rotating groove 15. Then, the swing angle adjustment gear 31 meshing with the drive gear 17 drives the crank 32 to swing. At the same time, the sliding sleeve 34 slides back and forth along the horizontal section of the swing rod 33, while driving the swing rod 33 to swing back and forth horizontally once. Then, the swing arm 35 drives the acquisition device 6 to swing back and forth horizontally once.

[0145] During the above process, the actual action of the data acquisition device 6 is a combination of a forward tilting and backward tilting motion and a horizontal reciprocating swinging motion.

[0146] Subsequently, the drive motor 11 continues to drive the drive cam 12 to rotate half a revolution, so that the various mechanisms of the joint calibration device are reset.

[0147] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0148] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A joint calibration device for lidar, camera, and IMU, wherein a data acquisition device (6) is mounted on a top plate (5), a drive mechanism (1) is mounted on a frame (4) for support and fixation, and a calibration plate (7) is disposed within the acquisition area of ​​the data acquisition device (6), characterized in that: It includes a tilt adjustment mechanism (2) for driving the top plate (5) to tilt forward and backward and a swing adjustment mechanism (3) for driving the acquisition device (6) to swing horizontally. The tilt adjustment mechanism (2) and the swing adjustment mechanism (3) drive the top plate (5) and the acquisition device (6) to move in linkage under the drive of the same driving mechanism (1). The tilt adjustment mechanism (2) includes a threaded driven shaft (22), a threaded sleeve (23), and a swing rod (24); the swing rod (24) is provided at the bottom of the front and rear of the top plate (5), the threaded sleeve (23) is located below the front of the top plate (5), and its top end is rotatably connected to the top plate (5) through the swing rod (24) located at the bottom of the front of the top plate (5). The threaded driven shaft (22) is inserted into the threaded sleeve (23) by the threaded engagement of the bottom end of the threaded sleeve (23). The translational output end and the rotational output end of the drive mechanism (1) output translational and rotational motion respectively; the translational output end is rotatably connected to the top plate (5) through the swing rod (24) located at the bottom of the rear part of the top plate (5), and a rotating pair supporting the forward and backward tilting motion of the top plate (5) is formed between the two; the translational output end drives the rear part of the top plate (5) to reciprocate in the vertical direction; the rotational output end is installed and connected to the tilt angle adjustment rotational input end located at the bottom of the threaded driven shaft (22), driving the threaded driven shaft (22) to rotate around its own axis.

2. The joint calibration device for lidar, camera, and IMU according to claim 1, characterized in that: The swing angle adjustment mechanism (3) includes a crank (32), a swing rod (33), a sliding sleeve (34), a swing arm (35), and an angle measuring instrument for measuring the swing angle of the swing arm (35); The swing rod (33) is an L-shaped rod structure consisting of a horizontal section and a vertical section. The front end of the horizontal section is rotatably connected to the frame (4), forming a rotating pair that supports the horizontal swing of the horizontal section. The end of the vertical section is rotatably connected to the front end of the swing arm (35), forming two rotating pairs that support the vertical section to drive the front end of the swing arm (35) to perform horizontal swing, forward tilting and backward tilting movements. The end of the swing arm (35) is fixedly installed with the acquisition device (6); the crank (32) is connected to the rotation output end through the swing angle adjustment input end set at its front end, and the end is rotatably connected with the sliding sleeve (34) that is slidably fitted on the horizontal section; The end of the crank (32) and the sliding sleeve (34) form a rotating pair about a vertical axis, and the sliding sleeve (34) and the horizontal segment form a sliding pair along the horizontal segment. The rotation output end drives the crank (32) to rotate around the vertical axis at its front end.

3. The joint calibration device for lidar, camera, and IMU according to claim 2, characterized in that: The drive mechanism (1) includes a drive motor (11), a drive cam (12), a lifting rod (13), a first rotating groove (14), a second rotating groove (15), a drive shaft (16), and a drive gear (17). The drive motor (11) is mounted and fixed on the frame (4), and its output end is fixedly connected to the drive cam (12). The drive motor (11) drives the drive cam (12) to rotate around a fixed axis of a horizontal axis. The lifting rod (13) is located above the drive cam (12), and its bottom end is fitted with the drive cam (12). The two form a cam pair that drives the lifting rod (13) to reciprocate in the vertical direction. The top end of the lifting rod (13) serves as the translational output end of the drive mechanism (1), and is rotatably connected to the bottom rear of the top plate (5) through the swing rod (24). The drive shaft (16) is rotatably mounted on the frame (4), and a drive gear (17) serving as the rotation output end is fixed thereon; the front end of the first rotating groove (14) is rotatably connected to the middle part of the lifting rod (13), and the end is rotatably connected to the front end of the second rotating groove (15), and the end of the second rotating groove (15) is connected and fixed to the drive shaft (16); a rotating pair around a horizontal axis is formed between the front end of the first rotating groove (14) and the middle part of the lifting rod (13), between the end of the first rotating groove (14) and the front end of the second rotating groove (15), and between the drive shaft (16) and the frame (4).

4. The joint calibration device for lidar, camera, and IMU according to claim 3, characterized in that: The tilt angle adjustment rotation input end and the swing angle adjustment input end are respectively the tilt angle adjustment gear (21) and the swing angle adjustment gear (31), both of which are bevel gears, symmetrically arranged on the upper and lower sides of the drive gear (17), and both mesh with the drive gear (17), which is also a bevel gear.

5. The joint calibration device for lidar, camera, and IMU according to claim 3, characterized in that: The top of the lifting rod (13) is a support rod (18) structure that extends horizontally to both sides. Each end of the support rod (18) is rotatably connected to the top plate (5) through a swing rod (24).

6. A joint calibration device for lidar, camera, and IMU according to any one of claims 1 to 4, characterized in that: The swing rod (24) is a swing rod.

7. A joint calibration device for lidar, camera, and IMU according to any one of claims 2 to 4, characterized in that: The acquisition device (6) includes a camera (61), an IMU (62), a lidar (63), and a mounting plate (64). The camera (61), IMU (62), and lidar (63) are mounted and fixed on the mounting plate (64), and the mounting plate (64) is rotatably mounted on the top of the top plate (5). The end of the swing arm (35) is fixed to the mounting plate (64). The mounting plate (64) is rotatably mounted to the top of the top plate (5), forming a rotating pair around a vertical axis; the end of the swing arm (35) is fixed to the side of the mounting plate (64).

8. The joint calibration device for lidar, camera, and IMU according to claim 1, characterized in that: The drive mechanism (1) and the tilt adjustment mechanism (2) are provided with a housing (8), and the housing (8) has a clearance hole (81) to accommodate the swinging motion of the swing rod (33); the bottom of the frame (4) is provided with multiple casters.

9. A method for joint calibration of a lidar, camera, and IMU, using the calibration device as described in claim 3 to calibrate a multi-sensor joint component, characterized in that, Includes the following steps: First, install and fix the IMU (62), lidar (63) and camera (61) onto the mounting plate (64), and adjust the position of the calibration plate (7) so that the calibration plate (7) is located in the center of the image captured by the camera (61); The second step is to perform a data acquisition cycle using the joint calibration device. The third step is to obtain the camera extrinsic parameter matrix as the calibration result of the camera (61) using the following formula: : (Formula 1); in, This refers to the coordinate form of the camera pixel coordinate system. Let f be the coordinates in the world coordinate system of the camera, and let f be the focal length of the camera. , The unit for pixel conversion along the x and y axes is mm / pixel. , Let R be the offset of the center of the projection screen relative to the optical axis, R be the rotation matrix, and t be the translation vector. Fourth step: Obtain the camera-lidar rotation and translation matrix as the joint calibration result of the camera (61) and lidar (63) according to Equations 2 to 4. ; (Formula 2); in, This refers to the coordinate form in the world coordinate system of the lidar. It is the rotation matrix that transforms the lidar coordinate system to the camera coordinate system. It is the translation vector that transforms the lidar coordinate system to the camera coordinate system; (Formula 3); =1 (Formula 4); in, and These are the spatial normals of the origin of the lidar coordinate system and the origin of the camera coordinate system, respectively. and , unit vector; Fifth step, obtain the corrected swing angle at time k using Equation 5. ; (Formula 5); in, The true swing angle of the swing arm at time k is obtained by the angle measuring instrument, where i is the time before time k. Let k be the sampling time interval between time k and time t. Let be the swing angle measured by the IMU at time k. This is the proportional gain coefficient. This is the integral gain coefficient; Represent the three-dimensional rotation of the IMU using quaternions. : + ； in This represents the rotation of the plane where the X and Y axes intersect about the positive X-axis towards the positive Y-axis. This indicates the rotation of the plane where the Z-axis and X-axis intersect about the positive Z-axis towards the positive X-axis. The plane intersecting the Y-axis and Z-axis rotates about the positive Y-direction toward the positive Z-direction; by Add constraints to the quaternions of the IMU: ； Combined with the three-dimensional rotation of the camera (61) Quaternions are obtained by maximizing the function. ; The optimal quaternion is obtained by following formula six. : (Formula 6); The best quaternion Converting to matrix form, we obtain the optimally estimated rotation matrix between the camera (61) and the IMU (62). : ； The translation matrix between the camera (61) and the IMU (62) is obtained by following formula 7. : (Formula 7); in, This refers to the camera translation amount obtained through feature point tracking on the calibration board. This refers to the inertial translation obtained through IMU pre-integration; Then, the camera-IMU rotation and translation matrix is ​​obtained as the joint calibration result of the camera (61) and IMU (62). ; The sixth step is to integrate and obtain the joint calibration results of the camera (61), IMU (62), and lidar (63), including the camera extrinsic parameter matrix. Camera, LiDAR, rotation and translation matrix and camera IMU rotation and translation matrix .

10. The method for joint calibration of lidar, camera, and IMU according to claim 9, characterized in that, The data collection process in the second step includes the following steps: The drive motor (11) drives the drive cam (12) to rotate half a revolution, thereby driving the following three types of motion. At the same time, the camera (61), IMU (62), and lidar (63) of the acquisition device (6) continuously acquire data: The first type of sport: The drive cam (12) drives the lifting rod (13) to move upward and then downward, so that the rear side of the top plate (5) follows the lifting rod to move upward and then downward. The second type of exercise: The drive cam (12) drives the lifting rod (13) to move upward and then downward. This movement drives the drive shaft (16) and the drive gear (17) fixed on it to rotate through the first rotating groove (14) and the second rotating groove (15). Then the tilt adjustment gear (21) meshing with the drive gear (17) drives the threaded driven shaft (22) to rotate. The rotation is converted into linear motion through the threaded sleeve (23), so that the front side of the top plate (5) follows the threaded sleeve (23) to move downward and then upward. The first and second movements mentioned above together constitute the tilting and then backward movement of the top plate (5) and the collecting device (6); The third type of exercise: The drive cam (12) drives the lifting rod (13) to move upward and then downward. This movement drives the drive shaft (16) and the drive gear (17) fixed on it to rotate through the first rotating groove (14) and the second rotating groove (15). Then the swing angle adjustment gear (31) meshing with the drive gear (17) drives the crank (32) to swing. This causes the sliding sleeve (34) to slide back and forth along the horizontal section of the swing rod (33) once, while driving the swing rod (33) to swing back and forth horizontally once. Then, the swing arm (35) drives the collection device (6) to swing back and forth horizontally once. Subsequently, the drive motor (11) continues to drive the drive cam (12) to rotate half a revolution, so that the various mechanisms of the joint calibration device are reset.

Images