Joint calibration method of multiple lidars based on point cloud of construction machinery model
Through the method based on the point cloud of the engineering machinery model, the dense model point cloud and sparse point cloud registration are used to solve the accuracy and complexity of multi-lidar calibration in the engineering machinery operation environment, and high-precision multi-lidar joint calibration is achieved , suitable for situations where the field of view is small or there is no overlap between multiple lidars.
Patent Information
- Application Number
- CN202410514143.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-04-26
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-04-26
AI Technical Summary
The prior art has problems such as artificial errors, lack of overlapping geometric structures and field of view in the construction machinery operation environment, and it is difficult to achieve high-precision multi-lidar joint calibration.
The multi-lidar joint calibration method based on the construction machinery model point cloud is adopted, and the construction machinery itself is not dependent on external references, and the construction machinery itself is used for calibration. The laser radar is calculated by registering dense model point clouds and sparse point clouds. Transform matrix to achieve accurate alignment between lidars.
Without additional calibration reference and lidar movement, the accuracy and robustness of multi-lidar joint calibration is improved, the calibration process is simplified, and the operation complexity is reduced. It is suitable for small field of view between multiple lidars. or no overlap.
Smart Images

Figure CN118294937B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of intelligent engineering machinery, and in particular to a multi-laser radar joint calibration method based on an engineering machinery model point cloud. Background Art
[0002] In order to achieve the goals of improving safety, improving work efficiency and reducing operating costs, construction machinery has gradually developed towards intelligent and unmanned operation. Intelligent construction machinery needs to install sensors to perceive the surrounding working environment. As a sensor that can provide high-precision ranging information and is not affected by light, LiDAR is naturally used in intelligent construction machinery. Since the vertical field of view of existing LiDARs is small, intelligent construction machinery usually needs to install multiple LiDARs to meet different autonomous operation tasks, which also brings up the problem of multi-LiDAR calibration. Factory calibration can ensure that the relative posture parameters between multiple LiDARs installed on construction machinery are accurate enough, but due to the inevitable vibration and impact during the operation, the maintenance and replacement of individual LiDARs due to failures, etc., the installation position of the LiDAR changes. It is often necessary to recalculate the relative relationship between the lasers in the working environment, that is, the joint calibration of multiple LiDARs.
[0003] It is very challenging to calibrate multiple lidars in an engineering machinery operating environment: first, if manual measurement is used, it not only requires high professional quality of engineers, but also has unavoidable human errors; second, the engineering machinery operating environment (such as underground space, mines, tunnels, etc.) usually lacks sufficient geometric structure, and it is difficult to find cooperative targets for calibration; in addition, in order to maximize the use of lidar, the overlapping field of view between the lidars on the engineering machinery is often small (or even non-existent); finally, the point cloud data scanned by the lidar is usually sparse, and the matching points of each lidar may not be scanned to the same physical position, resulting in matching errors. If multi-frame point cloud fusion is performed through mobile lidar, additional errors will also be introduced.
[0004] The patent "Multi-LiDAR Calibration System, Unmanned Mining Vehicle and Calibration Method (Publication No. CN114236515A)" calibrates the two LiDARs installed on the mining vehicle, manually selects the areas of interest of the two LiDAR point cloud data and directly aligns them. However, since the aligned point cloud data is sparse original point cloud, additional matching errors will be introduced, and it cannot be applied to scenes without common vision between LiDARs. The patent "A Distributed Multi-LiDAR Calibration Method (Publication No. CN116068537A)" uses a three-sided cube as a marker, and fits and detects the intersection of the three cubes. The coordinates of multiple laser radars under different laser radars are jointly calibrated, but in the mechanical engineering operation environment, there are usually no conditions for using special calibration devices such as three-sided cubes and calibration plates, and additional errors are easily introduced in the process of solving the intersection points; the patent "A multi-laser radar calibration method based on automatic mapping (publication number CN116520295A)" uses the point cloud data of the main laser radar of the vehicle to be calibrated to construct a point cloud map, and uses the point cloud data of each laser radar to align with the point cloud data. The calibration result is highly dependent on the quality of the point cloud map, which will introduce matching errors when constructing the point cloud map.
[0005] In summary, a robust and high-precision joint calibration method for multiple lidars in mechanical engineering is needed, which can complete the joint calibration of multiple lidars without the need for additional calibration reference objects, without the need for lidar movement, and with a small overlap in the field of view of each lidar. Summary of the invention
[0006] In order to solve the above technical defects in the prior art, the present invention utilizes the characteristics that engineering machinery usually has rich geometric structures and usually has working devices, and creatively proposes a method for joint calibration of multiple laser radars using the structure of the engineering machinery itself. During calibration, no external reference is required, the engineering machinery does not need to move, and there is no need for a large common field of view between the laser radars.
[0007] The present application provides a multi-lidar joint calibration method based on an engineering machinery model point cloud, comprising the following steps:
[0008] S1: Establish a model coordinate system based on the engineering machinery model, generate the engineering machinery Mesh grid and sample to obtain a dense engineering machinery model point cloud P m1 , from the engineering machinery model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ;
[0009] S2: Use N laser radars to be calibrated to collect the original point cloud data and pre-process them to obtain the sparse point cloud of the working device in each laser radar coordinate system {P i|i=1,2,...,N}; N≥2;
[0010] S3: For each sparse point cloud P i With the dense model point cloud P m2 Perform registration to obtain the transformation matrix {T im |i=1,2,...,N};
[0011] S4: Using the transformation matrix {T im |i=1,2,...,N}, calculate the transformation matrix {T ij |i,j=1,2,…,N}.
[0012] Furthermore, a model coordinate system is established according to the engineering machinery model, the engineering machinery Mesh grid is generated and sampled to obtain a dense engineering machinery model point cloud P m1 , from the engineering machinery model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ,include:
[0013] S11: Establish a model coordinate system with any position on the engineering machinery model as the origin;
[0014] S12: define the best observation position as the position of the working device when all laser radars on the engineering machinery can see part of the working device in their field of view; use the engineering machinery model to generate the engineering machinery Mesh when the working device is in the best observation position; under the condition of the best position, the fields of view of each laser radar partially overlap, or there is no overlapping field of view;
[0015] S13: Set the sampling interval parameter to α, uniformly sample the engineering machinery Mesh at the sampling interval of α, and obtain a dense engineering machinery model point cloud P in the model coordinate system. m1 ; Dense engineering machinery model point cloud P m1 Covers the surface of construction machinery and contains a large number of points at the same physical locations as the sparse point cloud collected in real time.
[0016] S14: From the point cloud P of the engineering machinery model m1 The dense point cloud P of the working device model is intercepted m2 .
[0017] Furthermore, N laser radars to be calibrated are used to collect the original point cloud data and preprocess them to obtain the sparse point cloud of the working device in each laser radar coordinate system {P i |i=1,2,...,N}, including:
[0018] S21: Adjust the working device to the best observation position, and use N laser radars to be calibrated to collect one frame of point cloud data in sequence;
[0019] S22: Drag the preset target box to select the point cloud of the working device area from the point cloud data collected by the N laser radars to be calibrated, and obtain the sparse point cloud of the working device in each laser radar coordinate system {P i |i=1,2,...,N}; the preset target frame size is set according to the size of the working device.
[0020] Furthermore, for each sparse point cloud P i With the dense model point cloud P m2 Perform registration to obtain the transformation matrix {T im |i=1,2,...,N}, including:
[0021] S31: Use the normal distribution algorithm to sequentially analyze the sparse point cloud P i With the dense model point cloud P m2 Perform rough registration to obtain the initial transformation matrix from each lidar coordinate system to the model coordinate system
[0022] S32: Use the K nearest neighbor algorithm to calculate N sparse point clouds P in sequence i and dense model point cloud P m2 The K nearest neighbor points of each point in , and the covariance matrix of each point is calculated based on the distance between each point and its corresponding K nearest neighbor points;
[0023] S33: Initial transformation matrix As the initial value, the covariance matrix of each point is used as the weight, and the sparse point cloud P is sequentially quantized using the generalized iterative closest point algorithm. i With the dense model point cloud P m2 Perform precise registration to obtain the transformation matrix {T im |i=1,2,...,N}.
[0024] Furthermore, in step S4, the transformation matrix of the laser radar i and laser radar j coordinate systems is calculated by the following formula:
[0025]
[0026] Where T im is the transformation matrix from the laser radar i coordinate system to the model coordinate system, T jm is the transformation matrix from the laser radar j coordinate system to the model coordinate system.
[0027] Furthermore, the method provided by the present application further includes: generating a dense engineering machinery model point cloud P generated by the engineering machinerym1 , used for the joint calibration of multiple lidars on other construction machinery.
[0028] Furthermore, the dense engineering machinery model point cloud P generated by the engineering machinery m1 , used for joint calibration of multiple laser radars on other construction machinery, including:
[0029] S61: Obtain the dense ontology model point cloud P of the current engineering machinery m1 ;
[0030] S62: Divide the N laser radars to be calibrated into M groups with intersections between them according to their field of view, each group including {N k |k=1,2,...,M} laser radars;
[0031] S63: Drive the current construction machinery to the kth group N k In the public field of view of the laser radars to be calibrated, the original point cloud data is collected and preprocessed by each laser radar to obtain the sparse engineering machinery point cloud {P i |i=1,2,...,N k};
[0032] S64: For each sparse point cloud P i With the dense model point cloud P m1 Perform registration to obtain the transformation matrix {T im |i=1,2,...,N k};
[0033] S65: Using the transformation matrix {T im |j=1,2,...,N k}, calculate the transformation matrix {T ij |i,j=1,2,...,N k};
[0034] S66: Repeat steps S63-S65 to obtain the transformation matrix between each laser radar in the M groups of laser radars to be calibrated; use the transformation matrix of the intersection laser radar between each group of laser radars to be calibrated to other laser radars to calculate the transformation matrix {T ij |i, j=1,2,...,N}.
[0035] Compared with the prior art, the present invention has the following significant advantages:
[0036] 1. The present invention does not rely on additional calibration reference objects, but uses construction machinery (especially the operating device area) as a cooperative target for multi-lidar joint calibration, so that the construction machinery multi-lidar joint calibration can be carried out at any time in the operating environment;
[0037] 2. The present invention uses construction machinery (especially the working device area) as the calibration cooperation target. It only needs to meet the requirement that each laser radar can see part of the construction machinery (especially the working device area). When the overlapping fields of view between multiple laser radars are small or even non-overlapping, joint calibration of multiple laser radars can be achieved.
[0038] 3. The present invention uses the dense model point cloud in the model coordinate system as the calibration reference point cloud. The dense model point cloud contains points in the same physical position as the sparse point cloud collected by each laser radar, which reduces the registration error caused by the insufficient resolution of the laser radar point cloud and greatly improves the joint calibration accuracy of multiple laser radars.
[0039] 4. The present invention uses the dense model point cloud in the model coordinate system as the calibration reference point cloud, so there is no need to use the method of multi-frame sparse point cloud data fusion to obtain dense point cloud data, that is, there is no need to move the laser radar, only need to use each laser radar to collect one frame of point cloud data respectively, which reduces the complexity of the joint calibration operation of multiple laser radars, and also avoids the additional errors caused by steps such as multi-frame point cloud data timestamp alignment and multi-frame point cloud data fusion.
[0040] 5. The present invention can complete the calibration by simply dragging and selecting the point cloud data collected by each lidar, without the need to copy data, move calibration reference objects, or operate complex calibration software like other calibration methods. This greatly simplifies the calibration process, allowing non-professionals (operators, ordinary maintenance personnel, etc.) to easily complete the calibration work.
[0041] Other features and advantages of the present invention will be described in the following description, and part of them will become obvious from the description, or will be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures particularly pointed out in the written description, claims, and drawings. BRIEF DESCRIPTION OF THE DRAWINGS
[0042] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.
[0043] Figure 1 is a flow chart of the method of the present invention;
[0044] Figure 2It is a schematic diagram of the installation position of each laser radar of the excavator of the present invention, the origin of the model coordinate system, the specific area of the working device and the best observation position;
[0045] Figure 3 A schematic diagram of the installation positions of the laser radars of the bucket truck of the present invention and the origin of the model coordinate system;
[0046] Figure 4 A schematic diagram of a specific area and optimal observation position of a bucket truck operating device of the present invention;
[0047] Figure 5 The installation positions of the laser radars of the carrier vehicle of the present invention;
[0048] Description of reference numerals:
[0049] 1-Hesai AT128 LiDAR; 2-Hesai QT128 LiDAR; 3-DJI Mid70 LiDAR; 4-Excavator arm; 5-Excavator bucket;
[0050] 6-Sagitar M1 laser radar; 7-Sagitar Helios32 laser radar;
[0051] 8- bucket truck arm; 9- bucket truck bucket;
[0052] 10-Hesai Pandar128 LiDAR; 11-DJI Avia LiDAR on the right; 12-DJI Avia LiDAR on the left; DETAILED DESCRIPTION
[0053] It is easy to understand that according to the technical solution of the present invention, without changing the essential spirit of the present invention, a person skilled in the art can imagine various embodiments of the present invention. Therefore, the following specific embodiments and drawings are only exemplary descriptions of the technical solution of the present invention, and should not be regarded as the whole of the present invention or as a limitation or limitation of the technical solution of the present invention. On the contrary, the purpose of providing these embodiments is to enable those skilled in the art to understand the present invention more thoroughly. The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the innovative concept of the present invention.
[0054] Example 1
[0055] like Figure 2As shown, the laser radars to be calibrated are a hybrid solid-state laser radar (Hesai AT128), a 128-line mechanical short-range blind laser radar (Hesai QT128), and a hybrid solid-state laser radar (DJI Livox Mid70) installed on the excavator, wherein the DJI Livox Mid70 laser radar is used to observe the working conditions of the working device. It should be noted that in this embodiment, the Hesai AT128 laser radar, the Hesai QT128 laser radar, and the DJI Livox Mid70 laser radar are all tools provided by this application to realize specific functions, and do not constitute a limitation on this application.
[0056] like Figure 1 As shown in FIG. 1 , a multi-lidar joint calibration method based on an engineering machinery model point cloud includes the following steps:
[0057] S1: Establish a model coordinate system based on the excavator model, generate the excavator Mesh grid and sample to obtain a dense excavator model point cloud P m1 , from the excavator model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ;
[0058] S2: Use three laser radars to collect original point cloud data respectively and perform preprocessing to obtain sparse working device point cloud P1 in the Hesai AT128 laser radar coordinate system, sparse working device point cloud P2 in the Hesai QT128 laser radar coordinate system, and sparse working device point cloud P3 in the DJI Mid70 laser radar coordinate system;
[0059] S3: Compare the sparse point clouds P1, P2, and P3 with the dense model point cloud P m2 Perform registration to obtain the transformation matrix T from the Hesai AT128 laser radar coordinate system to the model coordinate system 1m 、Transformation matrix T from Hesai QT128 laser radar coordinate system to model coordinate system 2m And the transformation matrix T from DJI Mid70 LiDAR coordinate system to model coordinate system 3m ;
[0060] S4: Using the transformation matrix T 1m 、T 2m and T 3m , calculate the transformation matrix {T ij |i, j=1,2,3}.
[0061] Specifically, a model coordinate system is established according to the excavator model, the excavator Mesh grid is generated and sampled to obtain a dense excavator model point cloud P m1 , from the excavator model point cloud P m1 Obtain a dense point cloud P of the working device modelm2 ,include:
[0062] S11: Figure 2 As shown, the model coordinate system is established with the upper left corner vertex of the upper surface of the excavator engine casing as the origin, the front as the positive direction of the X axis, the left as the positive direction of the Y axis, and the top as the positive direction of the Z axis;
[0063] S12: Figure 2 As shown in the figure, the arm and bucket in front of the excavator cab are defined as the specific area of the working device; the best observation position is defined as Figure 2 The positions of the working devices when part of the working device can be seen in the field of view of the Hesai AT128 LiDAR, Hesai QT128 LiDAR and DJI Mid70 LiDAR are shown; the excavator engineering machinery model is used to generate the excavator Mesh when the working device is in the best observation position;
[0064] S13: Set the sampling interval parameter to α = 1 cm, uniformly sample the excavator Mesh at the sampling interval of α, and obtain a dense excavator model point cloud P in the model coordinate system. m1 ;
[0065] S14: From the excavator model point cloud P m1 The dense point cloud P of the working device model is intercepted m2 .
[0066] Specifically, three laser radars are used to collect and preprocess the original point cloud data respectively, and the sparse working device point cloud P1 in the Hesai AT128 laser radar coordinate system, the sparse working device point cloud P2 in the Hesai QT128 laser radar coordinate system, and the sparse working device point cloud P3 in the DJI Mid70 laser radar coordinate system are obtained, including:
[0067] S21: Adjust the excavator working device to Figure 2 The best observation position shown is the one where Hesai AT128 LiDAR, Hesai QT128 LiDAR and DJI Mid70 LiDAR each collect one frame of point cloud data;
[0068] S22: Drag the preset target box and select the point clouds of the working device area from the point cloud data collected by the three laser radars to be calibrated in turn to obtain the sparse working device point cloud P1 in the Hesai AT128 laser radar coordinate system, the sparse working device point cloud P2 in the Hesai QT128 laser radar coordinate system, and the sparse working device point cloud P3 in the DJI Mid70 laser radar coordinate system.
[0069] It should be noted that the default size of the preset target frame is set according to the size of the operating device before leaving the factory, and the size of the preset target frame can also be adjusted during the actual calibration process.
[0070] Specifically, each sparse point cloud P1, P2, and P3 is compared with the dense model point cloud P m2 Perform registration to obtain the transformation matrix T from the Hesai AT128 laser radar coordinate system to the model coordinate system 1m 、Transformation matrix T from Hesai QT128 laser radar coordinate system to model coordinate system 2m And the transformation matrix T from DJI Mid70 LiDAR coordinate system to model coordinate system 3m ,include:
[0071] S31: Use the normal distribution algorithm implemented in the open source point cloud library PCL to compare the sparse point clouds P1, P2, and P3 with the dense model point cloud P m2 Perform rough registration to obtain the initial transformation matrix from the Hesai AT128 laser radar coordinate system to the model coordinate system Initial transformation matrix from Hesai QT128 LiDAR coordinate system to model coordinate system And the initial transformation matrix from DJI Mid70 LiDAR coordinate system to model coordinate system
[0072] S32: Use the K nearest neighbor algorithm implemented by the open source library FastGICP to calculate the sparse point clouds P1, P2, P3 and the dense model point cloud P in turn. m2 The K nearest neighbor points of each point in , and the covariance matrix of each point is calculated based on the distance between each point and its corresponding K nearest neighbor points;
[0073] S33: Initial transformation matrix and As the initial value, the covariance matrix of each point is used as the weight, and the generalized iterative closest point algorithm implemented by the open source library FastGICP is used to compare the sparse point clouds P1, P2, and P3 with the dense model point cloud P m2 Perform precise registration to obtain the transformation matrix T from the Hesai AT128 laser radar coordinate system to the model coordinate system 1m 、Transformation matrix T from Hesai QT128 laser radar coordinate system to model coordinate system 2m And the transformation matrix T from DJI Mid70 LiDAR coordinate system to model coordinate system 3m .
[0074] Specifically, the transformation matrix {T ij |i, j = 1, 2, 3} is calculated by the following formula:
[0075]
[0076] Where T im is the transformation matrix from the laser radar i coordinate system to the model coordinate system, T jmis the transformation matrix from the laser radar j coordinate system to the model coordinate system.
[0077] More specifically, for example, the transformation matrix from the Hesai AT128 laser radar coordinate system to the Hesai QT128 laser radar coordinate system is
[0078] Example 2
[0079] like Figure 3 As shown, the laser radar to be calibrated is a hybrid solid-state laser radar (Sagitar RS-LIDAR-M1) and a 32-line mechanical laser radar (Sagitar Helios 32) installed on a bucket truck. It should be noted that in this embodiment, the Sagitar RS-LIDAR-M1 laser radar and the Sagitar Helios 32 laser radar are both tools provided by this application to realize specific functions, and do not constitute a limitation on this application.
[0080] like Figure 1 As shown in FIG. 1 , a multi-lidar joint calibration method based on an engineering machinery model point cloud includes the following steps:
[0081] S1: Establish a model coordinate system based on the bucket truck model, generate the bucket truck Mesh grid and sample to obtain a dense bucket truck model point cloud P m1 , from the bucket truck model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ;
[0082] S2: Use the Sagitar M1 laser radar and the Sagitar Helios 32 laser radar to collect and preprocess the original point cloud data respectively, and obtain the sparse working device point cloud P1 in the Sagitar M1 laser radar coordinate system and the sparse working device point cloud P2 in the Sagitar Helios 32 laser radar coordinate system;
[0083] S3: Compare the sparse point clouds P1 and P2 with the dense model point cloud P m2 Perform registration to obtain the transformation matrix T from the Suteng M1 laser radar coordinate system to the model coordinate system 1m And the transformation matrix T from the Suteng Helios 32 laser radar coordinate system to the model coordinate system 2m ;
[0084] S4: Using the transformation matrix T 1m and T 2m , calculate the transformation matrix T from the coordinate system of the Suteng M1 laser radar to the coordinate system of the Suteng Helios 32 laser radar 12 .
[0085] Specifically, a model coordinate system is established according to the bucket truck model, a bucket truck Mesh grid is generated, and a dense bucket truck model point cloud P is obtained by sampling.m1 , from the bucket truck model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ,include:
[0086] S11: Figure 3 As shown, the model coordinate system is established with the vertex of the lower right corner of the upper surface of the bucket truck engine casing as the origin, the front as the positive direction of the X axis, the left as the positive direction of the Y axis, and the top as the positive direction of the Z axis;
[0087] S12: Figure 4 As shown in the figure, the arm and bucket in front of the bucket truck cab are defined as the specific area of the working device; the best observation position is defined as Figure 4 The position of the working device when part of the working device can be seen in the two laser radar fields of view shown, and the bucket truck engineering machinery model is used to generate the bucket truck Mesh when the working device is in the best observation position;
[0088] S13: Set the sampling interval parameter to α = 1 cm, uniformly sample the bucket truck Mesh grid at the α sampling interval, and obtain a dense bucket truck model point cloud P in the model coordinate system m1 ;
[0089] S14: From the bucket truck model point cloud P m1 The dense point cloud P of the working device model is intercepted m2 .
[0090] Specifically, the original point cloud data are collected and preprocessed using the Sagitar M1 laser radar and the Sagitar Helios 32 laser radar respectively, to obtain the sparse working device point cloud P1 in the Sagitar M1 laser radar coordinate system and the sparse working device point cloud P2 in the Sagitar Helios 32 laser radar coordinate system, including:
[0091] S21: Adjust the bucket truck operating device to Figure 4 The best observation position shown, using the Sagitar M1 LiDAR and the Sagitar Helios 32 LiDAR to collect one frame of point cloud data each;
[0092] S22: Drag the preset target box to select the point clouds of the working device area from the point cloud data collected by the two laser radars to be calibrated in turn, and obtain the sparse working device point cloud P1 in the Sagitar M1 laser radar coordinate system and the sparse working device point cloud P2 in the Sagitar Helios 32 laser radar coordinate system.
[0093] It should be noted that the default size of the preset target frame is set according to the size of the operating device before leaving the factory, and the size of the preset target frame can also be adjusted during the actual calibration process.
[0094] Specifically, each sparse point cloud P1, P2 is compared with the dense model point cloud P m2 Perform registration to obtain the transformation matrix T from the Suteng M1 laser radar coordinate system to the model coordinate system 1m And the transformation matrix T from the Suteng Helios 32 laser radar coordinate system to the model coordinate system 2m ,include:
[0095] S31: Use the normal distribution algorithm implemented in the open source point cloud library PCL to compare the sparse point clouds P1 and P2 with the dense model point cloud P m2 Perform rough registration to obtain the initial transformation matrix from the Sagitar M1 lidar coordinate system to the model coordinate system And the initial transformation matrix from the Suteng Helios 32 laser radar coordinate system to the model coordinate system
[0096] S32: Use the K nearest neighbor algorithm implemented by the open source library FastGICP to calculate the sparse point clouds P1, P2 and the dense model point cloud P in turn. m2 The K nearest neighbor points of each point in , and the covariance matrix of each point is calculated based on the distance between each point and its corresponding K nearest neighbor points;
[0097] S33: Initial transformation matrix and As the initial value, the covariance matrix of each point is used as the weight, and the generalized iterative closest point algorithm implemented by the open source library FastGICP is used to compare the sparse point clouds P1 and P2 with the dense model point cloud P m2 Perform precise registration to obtain the transformation matrix T from the Suteng M1 laser radar coordinate system to the model coordinate system 1m And the transformation matrix T from the Suteng Helios 32 laser radar coordinate system to the model coordinate system 2m .
[0098] Specifically, the transformation matrix from the Sagitar M1 laser radar coordinate system to the Sagitar Helios 32 laser radar coordinate system is
[0099] Example 3
[0100] like Figure 5 As shown in the figure, the laser radar to be calibrated is a 128-line mechanical laser radar (Hesai Pandar128) and two hybrid solid-state laser radars (DJI Livox Avia, used to observe the situation on both sides of the carrier respectively) installed on the carrier, with the following Figure 2 It should be noted that, in this embodiment, the Hesai Pandar128 laser radar and the DJI Livox Avia laser radar are tools provided by this application to achieve specific functions, and do not constitute a limitation on this application.
[0101] Since the carrier has no operating device, the dense excavator body model point cloud of the surrounding excavators is used to complete the joint calibration of multiple laser radars on the carrier, including the following steps:
[0102] S1: Get the dense ontology model point cloud P of the excavator m1 ;
[0103] S2: The three laser radars to be calibrated on the carrier are divided into two groups. The first group includes the Hesai Pandar128 laser radar and the DJI Avia laser radar on the left, and the second group includes the Hesai Pandar128 laser radar and the DJI Avia laser radar on the right.
[0104] S3: Drive the excavator to the left front of the carrier, that is, in the public field of view of the first group of laser radars to be calibrated (Hesai Pandar128 laser radar and left DJI Avia laser radar), use the first group of two laser radars to be calibrated to collect original point cloud data and pre-process them, and obtain the sparse excavator point cloud P1 in the Hesai Pandar128 laser radar coordinate system and the sparse working device point cloud P2 in the left DJI Avia laser radar coordinate system;
[0105] S4: Compare the sparse point clouds P1 and P2 with the dense model point cloud P m1 Perform registration to obtain the transformation matrix T from the Hesai Pandar128 laser radar coordinate system to the model coordinate system 1m And the transformation matrix T from the DJI Avia LiDAR coordinate system to the model coordinate system on the left 2m ;
[0106] S5: Using the transformation matrix T 1m and T 2m , calculate the transformation matrix T from the Hesai Pandar128 laser radar coordinate system to the DJI Avia laser radar coordinate system on the left 12 ;
[0107] S6: Repeat steps S3-S5 for the second set of laser radars to be calibrated, and obtain the transformation matrix T from the Hesai Pandar128 laser radar coordinate system to the DJI Avia laser radar coordinate system on the right. 13 ; Using the transformation matrix T 12 and the transformation matrix T 13 , calculate the transformation matrix T from the left DJI Avia LiDAR coordinate system to the right DJI Avia LiDAR coordinate system 23 .
[0108] Specifically, obtain the dense ontology model point cloud P of the excavator m1 ,include:
[0109] S11: Figure 2 As shown, the model coordinate system is established with the upper left corner vertex of the upper surface of the excavator engine casing as the origin, the front as the positive direction of the X axis, the left as the positive direction of the Y axis, and the top as the positive direction of the Z axis;
[0110] S12: Figure 2 As shown in the figure, the arm and bucket in front of the excavator cab are defined as the specific area of the working device; the best observation position is defined as Figure 2 The position of the excavator working device shown is used to generate the excavator Mesh when the working device is in the best observation position using the excavator engineering machinery model;
[0111] S13: Set the sampling interval parameter to α = 1 cm, uniformly sample the excavator Mesh at the sampling interval of α, and obtain a dense excavator model point cloud P in the model coordinate system. m1 ;
[0112] S14: From the excavator model point cloud P m1 The dense point cloud P of the working device model is intercepted m2 .
[0113] It should be noted that the dense model point cloud generated for calibrating the multiple laser radars on this engineering machinery and the dense model point cloud generated for calibrating the multiple laser radars on other engineering machinery are generated at the same time, that is, each engineering machinery only needs to generate a dense model point cloud once before leaving the factory.
[0114] It should be noted that the dense ontology model point cloud P generated by the excavator m1 and dense point cloud P of the working device model m2 Both can complete the joint calibration of multiple laser radars on the carrier, but using the dense ontology model point cloud P of the excavator m1 It can cover more of the laser radar field of view to be calibrated, so the dense body model point cloud P of the excavator is preferred. m1 To complete the joint calibration of multiple lidars on the carrier.
[0115] Specifically, the excavator is driven in front of the left side of the carrier, that is, in the public field of view of the first group of laser radars to be calibrated (Hesai Pandar128 laser radar and left DJI Avia laser radar), and the original point cloud data are collected and preprocessed using the first group of two laser radars to be calibrated, to obtain the sparse excavator point cloud P1 in the Hesai Pandar128 laser radar coordinate system and the sparse operating device point cloud P2 in the left DJI Avia laser radar coordinate system, including:
[0116] S21: Adjust the excavator working device to Figure 2The excavator is driven to the left front of the carrier vehicle, so that the first set of laser radars to be calibrated on the carrier vehicle (Hesai Pandar128 laser radar and DJI Avia laser radar on the left) can see part of the excavator. Use the first set of laser radars to be calibrated on the carrier vehicle to collect one frame of point cloud data each.
[0117] S22: Drag the preset target box and select the point clouds of the excavator body area in turn from the point cloud data collected by the first set of laser radars to be calibrated, and obtain the sparse point cloud P1 of the working device excavator in the Hesai Pandar128 laser radar coordinate system and the sparse point cloud P2 of the excavator in the DJI Avia laser radar coordinate system on the left.
[0118] Specifically, each sparse point cloud P1, P2 is compared with the dense model point cloud P m1 Perform registration to obtain the transformation matrix T from the Hesai Pandar128 laser radar coordinate system to the model coordinate system 1m And the transformation matrix T from the DJI Avia LiDAR coordinate system to the model coordinate system on the left 2m ,include:
[0119] S31: Use the normal distribution algorithm implemented in the open source point cloud library PCL to compare the sparse point clouds P1 and P2 with the dense model point cloud P m1 Perform rough registration to obtain the initial transformation matrix from the Hesai Pandar128 lidar coordinate system to the model coordinate system And the initial transformation matrix from the DJI Avia LiDAR coordinate system to the model coordinate system on the left
[0120] S32: Use the K nearest neighbor algorithm implemented by the open source library FastGICP to calculate the sparse point clouds P1, P2 and the dense model point cloud P in turn. m1 The K nearest neighbor points of each point in , and the covariance matrix of each point is calculated based on the distance between each point and its corresponding K nearest neighbor points;
[0121] S33: Initial transformation matrix and As the initial value, the covariance matrix of each point is used as the weight, and the generalized iterative closest point algorithm implemented by the open source library FastGICP is used to compare the sparse point clouds P1 and P2 with the dense model point cloud P m1 Perform precise registration to obtain the transformation matrix T from the Hesai Pandar128 laser radar coordinate system to the model coordinate system 1m And the transformation matrix T from the DJI Avia LiDAR coordinate system to the model coordinate system on the left 2m .
[0122] Specifically, the transformation matrix from the Hesai Pandar128 LiDAR coordinate system to the DJI Avia LiDAR coordinate system on the left is:
[0123] Specifically, the excavator is driven to the right front of the carrier, that is, in the public field of view of the second group of laser radars to be calibrated (Hesai Pandar128 laser radar and DJI Avia laser radar on the right), and the calibration steps of the first group of laser radars to be calibrated are repeated to obtain the transformation matrix from the Hesai Pandar128 laser radar coordinate system to the DJI Avia laser radar coordinate system on the right, which is T 13 .
[0124] Specifically, using the transformation matrix T 12 and the transformation matrix T 13 , calculate the transformation matrix from the DJI Avia LiDAR coordinate system on the left to the DJI Avia LiDAR coordinate system on the right
[0125] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.
[0126] It should be understood that in order to simplify the present invention and help those skilled in the art understand the various aspects of the present invention, in the above description of the exemplary embodiments of the present invention, the various features of the present invention are sometimes described in a single embodiment or described with reference to a single figure. However, the present invention should not be interpreted as the features included in the exemplary embodiments are all necessary technical features of the patent claims.
[0127] It should be understood that the modules, units, components, etc. included in the device of an embodiment of the present invention can be adaptively changed to be arranged in a device different from the embodiment. The different modules, units, or components included in the device of the embodiment can be arranged into one module, unit, or component, or they can be divided into multiple sub-modules, sub-units, or sub-components.
Claims
1. A multi-lidar joint calibration method based on engineering machinery model point cloud, characterized in that: The method comprises the following steps: S1: Establish a model coordinate system based on the engineering machinery model, generate the engineering machinery Mesh grid and sample to obtain a dense engineering machinery model point cloud P m1 , from the engineering machinery model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ; S2: Use N laser radars to be calibrated to collect the original point cloud data and pre-process them to obtain the sparse point cloud of the working device in each laser radar coordinate system {P i |i=1,2,...,N}; N≥2; S3: For each sparse point cloud P i With the dense model point cloud P m2 Perform registration to obtain the transformation matrix {T im |i=1,2,...,N}; S4: Using the transformation matrix {T im |i=1,2,...,N}, calculate the transformation matrix {T ij |i, j = 1, 2, ..., N}; According to the engineering machinery model, a model coordinate system is established, the engineering machinery Mesh grid is generated, and the dense engineering machinery model point cloud P is obtained by sampling. m1 , from the engineering machinery model point cloud P m1 Obtain a dense point cloud P of the working device model m2 ,include: S11: Establish a model coordinate system with any position on the engineering machinery model as the origin; S12: Define the best observation position as the position of the working device when all laser radars on the engineering machinery can see part of the working device in their field of view; use the engineering machinery model to generate the engineering machinery Mesh when the working device is in the best observation position; under the condition of the best position, the fields of view of each laser radar partially overlap or there is no overlapping field of view; S13: Set the sampling interval parameter to α, uniformly sample the engineering machinery Mesh at the sampling interval of α, and obtain a dense engineering machinery model point cloud P in the model coordinate system. m1 ; Dense engineering machinery model point cloud P m1 Covering the surface of the construction machinery, including points in the same physical location as the sparse point cloud collected in real time; S14: From the point cloud P of the engineering machinery model m1 The dense point cloud P of the working device model is intercepted m2 .
2. The multi-lidar joint calibration method based on the engineering machinery model point cloud according to claim 1 is characterized in that: Use N laser radars to be calibrated to collect the original point cloud data and preprocess them to obtain the sparse working device point cloud {P i |i=1,2,...,N}, including: S21: Adjust the working device to the best observation position, and use N laser radars to be calibrated to collect one frame of point cloud data in sequence; S22: Drag the preset target frame to select the point cloud of the working device area from the point cloud data collected by the N laser radars to be calibrated, and obtain the sparse point cloud of the working device in each laser radar coordinate system {P i |i=1,2,...,N}; the preset target frame size is set according to the size of the working device.
3. The multi-lidar joint calibration method based on engineering machinery model point cloud according to claim 1 is characterized in that: For each sparse point cloud P i With the dense model point cloud P m2 Perform registration to obtain the transformation matrix {T im |i=1,2,...,N}, include: S31: Use the normal distribution algorithm to sequentially analyze the sparse point cloud P i With the dense model point cloud P m2 Perform rough registration to obtain the initial transformation matrix from each lidar coordinate system to the model coordinate system S32: Use the K nearest neighbor algorithm to calculate N sparse point clouds P in sequence i and dense model point cloud P m2 The K nearest neighbor points of each point in , and the covariance matrix of each point is calculated based on the distance between each point and its corresponding K nearest neighbor points; S33: Initial transformation matrix As the initial value, the covariance matrix of each point is used as the weight, and the sparse point cloud P is sequentially quantized using the generalized iterative closest point algorithm. i With the dense model point cloud P m2 Perform precise registration to obtain the transformation matrix {T im |i=1,2,...,N}.
4. The multi-lidar joint calibration method based on engineering machinery model point cloud according to claim 1 is characterized in that: In step S4, the transformation matrix of the laser radar i and laser radar j coordinate systems is determined by the following method: Where T im is the transformation matrix from the laser radar i coordinate system to the model coordinate system, T jm is the transformation matrix from the laser radar j coordinate system to the model coordinate system.
5. The multi-lidar joint calibration method based on engineering machinery model point cloud according to claim 1 is characterized in that: The method further comprises: The dense engineering machinery model point cloud P generated by the engineering machinery m1 , used for the joint calibration of multiple lidars on other construction machinery.
6. The multi-lidar joint calibration method based on engineering machinery model point cloud according to claim 1 is characterized in that: The dense engineering machinery model point cloud P generated by the engineering machinery m1 , used for joint calibration of multiple laser radars on other construction machinery, including: S61: Obtain the dense ontology model point cloud P of the current engineering machinery m1 ; S62: Divide the N laser radars to be calibrated into M groups with intersections between them according to their field of view, each group including {N k |k=1,2,...,M} laser radars; S63: Drive the current construction machinery to the kth group N k In the public field of view of the laser radars to be calibrated, the original point cloud data is collected and preprocessed by each laser radar to obtain the sparse engineering machinery point cloud {P i |i=1,2,...,N k }; S64: For each sparse point cloud P i With the dense model point cloud P m1 Perform registration to obtain the transformation matrix {T im |i=1,2,...,N k }; S65: Using the transformation matrix {T im |j=1,2,...,N k }, calculate the transformation matrix {T ij |i,j=1,2,...,N k }; S66: Repeat steps S63-S65 to obtain the transformation matrix between each laser radar in the M groups of laser radars to be calibrated; use the transformation matrix of the intersection laser radar between each group of laser radars to be calibrated to other laser radars to calculate the transformation matrix {T ij |i, j=1,2,...,N}.
