Joint calibration method of 3D point cloud sensor and 2D image sensor for engineering machinery with working device

Abstract

The present invention provides a joint calibration method of a three-dimensional point cloud sensor and a two-dimensional image sensor for an engineering machinery with an operating device, wherein a model coordinate system is established with an engineering machinery model to generate a three-dimensional model point cloud A of the operating device and m model dense straight line point clouds {X m}; Extract the point cloud of the operating device from the point cloud data collected by the 3D point cloud sensor and align it with A to obtain the transformation relationship F between the 3D point cloud sensor coordinate system and the model coordinate system 1 ; Using F 1 {X m}Transform to obtain m high-precision dense straight line point clouds {Y m}; extract n edge lines of the outer contour of the working device from the image data collected by the two-dimensional image sensor, and calculate n planes {Z n} and optimize; get the optimal transformation relationship F between the three-dimensional point cloud sensor and the two-dimensional image sensor 2 , achieving joint calibration.

Description

Technical Field

[0001] The present invention relates to the field of intelligent engineering machinery, and in particular to a method for jointly calibrating a three-dimensional point cloud sensor and a two-dimensional image sensor of an engineering machinery with an operating device. Background Art

[0002] Due to the problems of harsh working environment, high work intensity and frequent safety accidents in engineering operations by manpower, the use of intelligent and unmanned engineering machinery has become a development trend today. Intelligent engineering machinery needs to install sensors to perceive the surrounding working environment. Considering the different advantages and disadvantages of each sensor, it often installs multiple sensors to complement each other, such as three-dimensional point cloud sensors (lidar, depth camera, etc.) and two-dimensional image sensors (visible light camera, infrared thermal imager, etc.). However, the installation of multiple sensors brings about the problem of calibration. Whether intelligent engineering machinery can accurately complete the task depends highly on whether the calibration parameters of each sensor are accurate. Therefore, factory calibration is usually performed to ensure that the relative posture parameters between the sensors of the engineering machinery are accurate enough. However, due to the inevitable vibration and impact during the operation, the maintenance and replacement of individual sensors due to failures, etc., the installation position of the sensors changes. It is often necessary to recalculate the relative relationship between sensors in the working environment, that is, the joint calibration of multiple sensors.

[0003] A simple and direct calibration method is to perform manual measurement, but this not only requires high professional quality of engineers, but also has unavoidable human errors. Since the working environment of engineering machinery (such as underground space, mines, tunnels, etc.) usually lacks structural and texture features, and it is difficult to find cooperative targets for calibration, the joint calibration of multi-sensors of engineering machinery has become a major problem. With the development of intelligent engineering machinery, this issue has attracted widespread attention.

[0004] The patent "Bucket coordinate calibration method and device, update method and equipment, excavator (publication number CN115950356A)" uses the angle sensor of the bucket to calibrate the lidar and the excavator, and does not involve the joint calibration of multiple sensors, and needs to collect multiple sets of lidar point cloud data and angle sensor data, which is cumbersome to operate; the patent "A camera and lidar automatic joint calibration method, system and unmanned mining truck (publication number CN117347984A)" extracts the vertices of multiple checkerboard calibration plates from single-frame point cloud data and image data The laser radar and camera are calibrated by registering the two-dimensional image and the three-dimensional point cloud, but the mechanical engineering operation environment usually does not have the conditions to use special calibration devices such as checkerboard calibration plates, and the coordinate errors of the checkerboard calibration plates extracted from the sparse point cloud data are large; the patent "Camera and laser radar joint calibration method, device, electronic equipment and medium (publication number CN117388831A)" manually captures the point pairs in the two-dimensional image and the three-dimensional point cloud to perform joint calibration of the camera and the laser radar, which is cumbersome and easy to introduce human errors; the patent "automatic calibration The patent "A robust laser radar-camera self-calibration method and device, computer-readable storage medium, and terminal (publication number CN114519681A)" extracts image depth continuous line features and point cloud depth continuous line features from image data and dense point cloud data to complete the joint calibration of the laser radar and the camera, wherein the dense point cloud data is collected by multiple laser radars or high-beam laser radars, and cannot be extended to any three-dimensional point cloud sensor. In addition, the extraction of scene depth continuous line features requires sufficient edges in the external environment, such as buildings, trees, and wall corners, etc., and the requirement that the construction environment of engineering machinery has abundant edges is not possible in many cases; the patent "A robust laser radar-camera self-calibration method (publication number CN117392237A)" extracts geometric edges and intensity edges from the accumulated dense point cloud map and reflection intensity map and matches them with the two-dimensional edges in the image to complete the joint calibration of the laser radar and the camera. It is suitable for nonlinear scanning laser radars that can obtain point cloud reflection intensity, and it cannot be extended to any three-dimensional point cloud sensor, and it also requires sufficient edges in the external environment, and cannot be applied to the construction environment of engineering machinery. The patent "A method for automatic calibration of the spatial position of multiple laser radars and multiple camera sensors (publication number CN114578328A)" extracts dense point cloud line features and projects them into a pixel coordinate system, and uses a search method and a graph optimizer to optimize the error between the point cloud line feature projection image and the camera straight line edge image. The dense point cloud requires moving the laser radar position to collect multiple frames of data and using the normal distribution transform algorithm (NDT) to fuse multiple frames of point cloud data, which will introduce motion distortion and normal distribution transform algorithm (NDT) registration errors.

[0005] In summary, a joint calibration method for 3D point cloud sensors and 2D image sensors is needed, which does not require the use of structural features of the external environment and additional calibration objects, nor does it require moving the sensor position. That is, it can realize the joint calibration of 3D point cloud sensors and 2D image sensors for automated engineering machinery and improve calibration efficiency and accuracy. Summary of the invention

[0006] In order to solve the above technical defects in the prior art, the present invention utilizes the fact that engineering machinery usually has a rod, arm, hook working device, and these working devices have rich structural features, and creatively proposes a method for joint calibration of laser radar and camera using the structure of the engineering machinery itself. During calibration, no external reference is required, there is no requirement for the working environment, and the engineering machinery does not need to be driven.

[0007] The technical solution to achieve the purpose of the present invention is: a method for jointly calibrating a three-dimensional point cloud sensor and a two-dimensional image sensor of an engineering machinery with a working device, comprising the following steps:

[0008] S1: Establish a model coordinate system based on the engineering machinery model, and generate the 3D model point cloud A corresponding to the specific area of ​​the engineering machinery operation device at the optimal observation position of the 3D point cloud sensor and the 2D image sensor and the m model dense straight line point clouds {X m}; The straight line edge of the outer contour of the working device can be clearly identified in the field of view of the 3D point cloud sensor and the 2D image sensor at the best observation position;

[0009] S2: pre-process the 3D point cloud data collected by the 3D point cloud sensor to obtain the 3D point cloud data B of the specific area of ​​the working device, and register it with the 3D model point cloud A to obtain the external parameter matrix F1 of the transformation relationship between the 3D point cloud sensor coordinate system and the model coordinate system;

[0010] S3: Use the external parameter matrix F1 to transform the model's dense straight line point cloud {X m}Transform to obtain m high-precision dense straight line point clouds {Y m};

[0011] S4: extracting n straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the two-dimensional image sensor according to the straight line segment detection algorithm, and calculating the n planes {Z n};

[0012] S5: Enumerate m high-precision dense straight line point clouds {Y m} and n planes {Z n}, construct all the arrangements, and obtain the external parameter matrix of the transformation relationship of each arrangement by iteratively minimizing the distance residual from the straight line point cloud to the corresponding plane; the arrangement with the smallest error among all the arrangements is used as the external parameter matrix of the transformation relationship between the three-dimensional point cloud sensor and the two-dimensional image sensor.

[0013] Furthermore, a model coordinate system based on the engineering machinery model is established, and a 3D model point cloud A corresponding to a specific area of ​​the engineering machinery operation device at the optimal observation position of the 3D point cloud sensor and the 2D image sensor and m model dense straight line point clouds {X m},include:

[0014] The model coordinate system is established with any point on the working device model as the origin. According to the structural information with straight edge features in the working device model, the working device is generated according to the preset spacing. The 3D model point cloud A and the m model dense straight line point clouds {X m}; where {X m There are at least 3 nonlinearly related model dense straight line point clouds in}.

[0015] Furthermore, in step S2, preprocessing the three-dimensional point cloud data collected by the three-dimensional point cloud sensor to obtain three-dimensional point cloud data B of the specific area of ​​the working device includes the following steps:

[0016] S21: before calibration, the working device is adjusted to the best observation position of the three-dimensional point cloud sensor and the two-dimensional image sensor, and then a frame of point cloud data M1 containing the working device is collected by the three-dimensional point cloud sensor;

[0017] S22: Move the operating device at a certain angle and displacement, and move the operating device out of the field of view or the limit position of the 3D point cloud sensor and the 2D image sensor according to the activity range of the device itself, and collect a frame of point cloud data M2 as the background through the 3D point cloud sensor again;

[0018] S23: Based on the distance relationship between the working device and the three-dimensional point cloud sensor and the activity range of the working device, point clouds within the preset range of M1 and M2 are selected to obtain N1 and N2; the point cloud data N1 at the best observation position of the sensor is subtracted from the intersection of point cloud N1 and point cloud N2, thereby removing point clouds other than the working device in the environment and obtaining three-dimensional point cloud data B of the specific area of ​​the working device.

[0019] Furthermore, in step S2, the three-dimensional point cloud data B of the specific area of ​​the working device is aligned with the three-dimensional model point cloud A using the normal distribution transformation algorithm and the iterative closest point algorithm, so as to solve the external parameter matrix F1 of the transformation relationship between the three-dimensional point cloud sensor coordinate system and the model coordinate system.

[0020] Furthermore, in step S4, n straight line segments of the outer contour of the specific area of ​​the working device are extracted from the image data collected by the two-dimensional image sensor according to the straight line segment detection algorithm, and n planes {Z n The method comprises the following steps:

[0021] S41: while the 3D point cloud sensor collects M1, the 2D image sensor collects a frame of image data I1 including the working device;

[0022] S42: while the 3D point cloud sensor is collecting M2, a frame of image data I2 is collected again by the 2D image sensor as a background;

[0023] S43: Using the LSD straight line segment detection algorithm to obtain edge images E1 and E2 respectively for the data in the preset area of ​​images I1 and I2, and obtaining n active straight line segments {L n}, thereby reducing the impact of inactive straight line segments of the background on subsequent algorithms;

[0024] S44: For L in the pixel coordinate system n Every straight line l k , select any two non-coincident points P1 and P2 on the straight line;

[0025] S45: Assuming that the depth values ​​of pixels P1 and P2 are certain, the pixel coordinates of P1 and P2 are converted to Q1 and Q2 in the image sensor coordinate system through the known intrinsic parameter matrix of the image sensor, and the straight line l in the image sensor coordinate system is determined by three points, Q1 and Q2 in the image sensor coordinate system and the origin (0,0,0) of the image sensor coordinate system. k The plane where the image sensor is located is obtained by n}.

[0026] Furthermore, the method for solving the external parameter matrix of the three-dimensional point cloud sensor and the two-dimensional image sensor in step S5 specifically includes the following steps:

[0027] S51: Enumerate m high-precision dense straight line point clouds {Y m} and n planes {Z n}, construct all the permutations, set to K, and set the number of "edge-to-face pairs" between a single edge and a single face in each permutation to c. When m≤n, c=m; when m>n, c = n;

[0028] S52: For each arrangement, the external parameter matrix of the transformation relationship between the three-dimensional point cloud sensor and the two-dimensional image sensor is initialized to a suitable initial value, and the optimal value U of the external parameter matrix under the arrangement is obtained by iteratively minimizing the distance residuals from the straight line point cloud to the corresponding plane in the c “edge-to-face pairs”;

[0029] S53: Initialize a counter to 0 for each arrangement, use the optimal value U of the external parameter matrix obtained by the arrangement, and convert {Y m All three-dimensional point clouds are projected into the image data E1 captured by the two-dimensional image sensor. If the projection of a certain point appears in {L n}, the counter value is increased by 1. A total of K counters are obtained, and the external parameter matrix corresponding to the arrangement of the counter with the largest value is used as the external parameter matrix of the three-dimensional point cloud sensor and the two-dimensional image sensor.

[0030] Furthermore, when the best observation position of the three-dimensional point cloud sensor and the two-dimensional image sensor is the normal parking state of the engineering machinery, the patent can realize the calibration of the sensor coordinate system and the vehicle body coordinate system, which specifically includes the following steps:

[0031] S71: Generate a 3D model point cloud A' corresponding to a specific area of ​​the engineering machinery operation device in the vehicle body coordinate system at the optimal observation position of the sensor and m' model dense straight line point clouds {X' m};

[0032] S72: pre-processing the 3D point cloud data collected by the 3D point cloud sensor to obtain the 3D point cloud data B' of the specific area of ​​the working device, and registering it with the 3D model point cloud A' to obtain the external parameter matrix F'1 of the transformation relationship between the 3D point cloud sensor coordinate system and the vehicle body coordinate system;

[0033] S73: Use the external parameter matrix F'1 to transform the model dense straight line point cloud {X' m}Transform to obtain high-precision m' dense straight line point clouds {Y' m};

[0034] S74: extracting n' straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the two-dimensional image sensor according to the straight line segment detection algorithm, and calculating n' planes {Z' n};

[0035] S75: Enumerate m' dense straight line point clouds {Y' m} and n' planes {Z' n}, construct all the arrangements, and obtain the external parameter matrix of the transformation relationship of each arrangement by iteratively minimizing the distance residual from the straight point cloud to the corresponding plane. The arrangement with the smallest error among all the arrangements is used as the external parameter matrix F'2 of the transformation relationship between the 3D point cloud sensor and the 2D image sensor;

[0036] S76: Obtain an extrinsic parameter matrix F'3 of the transformation relationship between the two-dimensional image sensor coordinate system and the vehicle body coordinate system by calculating the product of the extrinsic parameter matrix F'2 and the extrinsic parameter matrix F'1.

[0037] Compared with the prior art, the present invention has the following significant advantages:

[0038] 1. The present invention only uses a single frame of three-dimensional point cloud to align with the model dense point cloud to obtain a high-precision dense point cloud, without the need to obtain a dense point cloud through inter-frame accumulation of point clouds, and thus does not require the construction machinery to travel;

[0039] 2. The present invention calculates the point cloud of the straight edge of the engineering machinery in the three-dimensional point cloud sensor coordinate system by the method of post-registration coordinate conversion, which cleverly solves the problem that high-precision three-dimensional edges cannot be directly detected from sparse laser point clouds;

[0040] 3. The present invention uses the structural characteristics of the engineering machinery itself as the cooperative target for calibration, and does not use additional calibration reference objects;

[0041] 4. The present invention does not require moving the three-dimensional point cloud sensor and the image sensor, and is suitable for the calibration of sensors of automated engineering machinery;

[0042] 5. The present invention detects the changing edges of engineering machinery in two-dimensional images, eliminates the interference of the external environment on calibration, and improves the reliability and calculation speed of the algorithm. When the main body of the engineering machinery is stationary, only the working device part is moved, so as to extract the image edges that change between multiple frames of images. The image edges involved in the calibration only come from the specific area used for calibration on the main body of the engineering machinery, thereby eliminating the interference of the external environment on the calibration. On the one hand, the reliability and calculation speed of the algorithm are improved. Fewer edges will definitely increase the calculation speed, and only coming from specific areas will definitely improve the reliability; on the other hand, the robustness of the algorithm is improved, and it is not dependent on the external environment of the construction machinery.

[0043] 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

[0044] The drawings are only for the purpose of illustrating specific embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0045] Figure 1 A flowchart of a method for jointly calibrating a 3D point cloud sensor and a 2D image sensor for an engineering machinery with an operating device;

[0046] Figure 2 A schematic diagram of the installation positions of the laser radar and camera of the excavator and the origin of the model coordinate system of the present invention;

[0047] Figure 3 A schematic diagram of a specific area of ​​an excavator of the present invention;

[0048] Figure 4 It is a straight line schematic diagram of the excavator working device model of the present invention;

[0049] Figure 5 This is a schematic diagram of the highest position of the excavator working device of the present invention;

[0050] Figure 6 A schematic diagram of the installation positions of the laser radar and camera of the truck crane of the present invention and the origin of the model coordinate system;

[0051] Figure 7 This is a schematic diagram of a specific area of ​​the truck crane of the present invention;

[0052] Figure 8 It is a straight line schematic diagram of the model of the truck crane operation device of the present invention;

[0053] Fig. 9 This is a schematic diagram of the highest position of the truck crane operating device of the present invention;

[0054] Fig.10 A schematic diagram of the installation positions of the laser radar and camera of the bucket truck of the present invention and the origin of the model coordinate system;

[0055] Fig.11 A schematic diagram of a specific area of ​​a bucket truck of the present invention;

[0056] Fig.12 It is a straight line schematic diagram of the bucket truck working device model of the present invention;

[0057] Fig.13 It is a schematic diagram of the highest position of the bucket truck operating device of the present invention.

[0058] Description of reference numerals:

[0059] 1- Hikvision visible light camera; 2- 64-line mechanical laser radar (Hesai Pandar64);

[0060] 3- boom; 4- boom cylinder; 5- arm cylinder; 6- arm;

[0061] 7-bucket cylinder; 8-arm connecting rod; 9-connecting rod; 10-bucket;

[0062] 11- Line l1; 12- Line l2; 13- Line l3; 14- Line l4

[0063] 15-Senyun visible light camera; 16-Hybrid solid-state laser radar (Sato RS-LIDAR-M1);

[0064] 17- crane arm; 18- luffing cylinder; 19- lifting rope; 20- crane hook;

[0065] 21- straight line l5; 22- straight line l6; 23- straight line l7;

[0066] 24-Hikvision infrared camera; 25-Hybrid solid-state laser radar (DJI AVIA Aolan);

[0067] 26-bucket cylinder; 27-rocker arm; 28-pull rod;

[0068] 29-straight line 18; 30-straight line 19; 31-straight line 1 10 ; 32-straight line l 11 . DETAILED DESCRIPTION

[0069] 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.

[0070] It should be noted that the specific types of solid-state laser radars mentioned in the embodiments of this application, such as the RS-LIDAR-M1 and the DJI AVIA Aolan; or the cameras mentioned, such as the Senyun visible light camera and the Hikvision infrared camera, are only tools for implementing this application. All tools that can implement this application are tools that can be used in this application. This application is not limited to the listed tools.

[0071] Example 1

[0072] The working device is an excavator equipped with a 64-line mechanical laser radar (Hesai Pandar64) and a Hikvision visible light camera. The 3D point cloud sensor is a 64-line mechanical laser radar (Hesai Pandar64) and the 2D image sensor is a Hikvision visible light camera. The installation position is as follows: Figure 2 shown.

[0073] like Figure 1 As shown, a joint calibration method of a 3D point cloud sensor and a 2D image sensor of an engineering machinery with an operating device comprises the following steps:

[0074] S1: Establish a model coordinate system based on the excavator model, and generate the three-dimensional model point cloud A and three model dense straight line point clouds {X3} of the outer contour of the specific area of ​​the excavator operating device corresponding to the best observation position of the laser radar and visible light camera; the straight line edges of the outer contour of the excavator operating device can be clearly identified in the field of view of the laser radar and visible light camera at the best observation position.

[0075] S2: Preprocess the 3D point cloud data collected by the laser radar to obtain the 3D point cloud data B of the specific area of ​​the working device, and align it with the 3D model point cloud A to obtain the external parameter matrix F1 of the transformation relationship between the laser radar coordinate system and the model coordinate system;

[0076] S3: Use the external parameter matrix F1 to transform the model dense straight line point cloud {X3} to obtain three high-precision dense straight line point clouds {Y3} in the lidar coordinate system;

[0077] S4: extracting six straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the visible light camera according to the straight line segment detection algorithm, and calculating six planes {Z6} of these straight line segments in the camera coordinate system;

[0078] S5: Enumerate the three high-precision dense straight line point clouds {Y3} in the laser radar coordinate system and the six planes {Z6} in the camera coordinate system, construct all the arrangements, and iteratively minimize the distance residual from the straight line point cloud to the corresponding plane to obtain the external parameter matrix of each arrangement. The arrangement with the smallest error among all arrangements is used as the external parameter matrix of the transformation relationship between the laser radar and the visible light camera.

[0079] Specifically, Figure 2 As shown, in step S1, the model coordinate system is established with the upper left corner vertex of the upper surface of the excavator engine housing 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. Since the laser radar and visible light camera are installed in front of the cab, the working device in front of the excavator cab (boom, boom, bucket, boom connecting rod, bucket connecting rod, etc.) is generated in the model coordinate system according to the structural information of the excavator working device model. Figure 3The 3D model point cloud A with a point spacing of 1 cm at the optimal observation position of the sensor as shown, and three model dense straight line point clouds {X3} of its outer contour.

[0080] More specifically, as Figure 4 shown, in the step S1, three model dense straight line point clouds {X3} of the outer contour of the excavator are generated. To ensure that the subsequent algorithm can obtain correct results, at least three non-linearly related straight lines are selected. Figure 4 In [reference], two straight lines l1 and l2 of the bucket and one straight line edge l3 of the boom are selected to generate the model dense straight line point cloud. On this basis, more straight lines can be selected, such as another edge l4 of the boom.

[0081] Specifically, in the step S2, the point cloud of a specific area of the excavator working device is extracted from the point cloud data collected by the lidar, registered with the model point cloud of the specific area in the model coordinate system, and the conversion relationship between the lidar coordinate system and the model coordinate system is obtained, including the following steps:

[0082] S21: Before calibration, adjust the excavator working device (boom, boom cylinder, arm, arm cylinder, arm connecting rod, bucket, bucket connecting rod, bucket cylinder) to the optimal observation position of the lidar and the visible light camera, and then collect a frame of point cloud data M1 containing the working device through the lidar.

[0083] S22: Move the working device at a certain angle and displacement, and move the working device out of the field of view or the limit position of the lidar and the visible light camera according to the movement range of the device itself, and then collect another frame of point cloud data M2 as the background through the lidar.

[0084] S23: According to the distance relationship between the working device and the lidar and the movement range of the working device, select the point clouds within a certain range (such as Figure 2 for the excavator in [reference], the range is the area in the lidar coordinate system where 0m < x < 15m, -5m < y < 5m, -5m < z < 5m) from M1 and M2 to obtain N1 and N2. Subtract the intersection of the point cloud N1 and the point cloud N2 from the point cloud data N1 at the optimal observation position of the sensor, so as to remove the non-working device point clouds in the environment and obtain the three-dimensional point cloud data B of the specific area of the working device. Step S23 greatly reduces the size of B and excludes the interference point clouds from the non-engineering device specific area in the environment, thereby improving the speed of the registration algorithm with A and enhancing the anti-interference ability.

[0085] Specifically, as Figure 5 shown, in the step S22, move the excavator working device at a certain angle and displacement. The optimal situation is that the boom, arm, and bucket are all at the highest position to ensure that no moving device appears in the fields of view of the lidar and the visible light camera.

[0086] Since various attributes of the point cloud, such as the three-dimensional coordinates and reflection intensity in the laser radar coordinate system, are in floating-point numbers, direct subtraction cannot be performed. In S23, the point cloud data of N1 and N2 are converted into voxels using an octree and then subtracted.

[0087] Specifically, in step S2, the three-dimensional point cloud data B of the specific area of ​​the working device is aligned with the three-dimensional model point cloud A using the normal distribution transformation algorithm and the iterative closest point algorithm, so as to solve the external parameter matrix F1 of the transformation relationship between the lidar coordinate system and the model coordinate system.

[0088] In step S4, the method of extracting six straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the visible light camera according to the straight line segment detection algorithm, and calculating six planes {Z6} of the extracted straight line segments in the camera coordinate system includes the following steps:

[0089] S41: while the laser radar collects M1, the visible light camera collects a frame of image data I1 containing the working device;

[0090] S42: while the laser radar collects M2, a frame of image data I2 is collected again by the visible light camera as the background;

[0091] S43: Using the LSD straight line segment detection algorithm to obtain edge images E1 and E2 respectively for the data in the preset area of ​​images I1 and I2, and obtaining n active straight line segments {L n}, thereby reducing the impact of inactive straight line segments of the background on subsequent algorithms;

[0092] S44: For L in the pixel coordinate system n Every straight line l k , select any two non-coincident points P1 and P2 on the straight line;

[0093] S45: Assuming that the depth values ​​of pixels P1 and P2 are constant, the pixel coordinates of P1 and P2 are converted to Q1 and Q2 in the camera coordinate system through the known intrinsic parameter matrix of the visible light camera, and the straight line l in the camera coordinate system is determined by three points, Q1 and Q2 in the camera coordinate system and the origin (0,0,0) of the camera coordinate system. k The plane where it is located, thus obtaining 6 planes {Z6} in the camera coordinate system.

[0094] Specifically, the method for solving the external parameter matrix of the laser radar and the visible light camera in step S5 specifically includes the following steps:

[0095] S51: Enumerate the three high-precision dense straight line point clouds {Y3} in the laser radar coordinate system and the six planes {Z6} in the two-dimensional image sensor coordinate system, and construct all the arrangements, with a total of There are three arrangement modes, and the number of "edge-to-face pairs" between a single edge and a single face in each arrangement mode is 3;

[0096] S52: For each arrangement, the external parameter matrix of the transformation relationship between the laser radar and the visible light camera is initialized to a suitable initial value, and the optimal value U of the external parameter matrix of the arrangement is obtained by iteratively minimizing the distance residuals from the straight line point cloud to the corresponding plane in the three "edge-to-face" pairs;

[0097] S53: For each arrangement, a counter is initialized to 0, and the external parameter matrix U obtained by the arrangement is used to project all the three-dimensional point clouds of {Y3} into the image data E1 captured by the visible light camera. If the projection of a certain point appears within a certain distance of {L6}, the counter value is increased by 1. A total of 3 counters are obtained, and the external parameter matrix of the arrangement corresponding to the counter with the largest value is used as the external parameter matrix of the laser radar and the visible light camera.

[0098] Example 2

[0099] The operating device is a truck crane equipped with a hybrid solid-state laser radar (Sagitar RS-LIDAR-M1) and a Senyun visible light camera. The 3D point cloud sensor is a hybrid solid-state laser radar (Sagitar RS-LIDAR-M1) and the 2D image sensor is a Senyun visible light camera. The installation position is as follows: Figure 6 shown.

[0100] like Figure 1 As shown, a joint calibration method of a 3D point cloud sensor and a 2D image sensor of an engineering machinery with an operating device comprises the following steps:

[0101] S1: Establish a model coordinate system based on the truck crane model, and generate the three-dimensional model point cloud A and three model dense straight line point clouds {X3} of the outer contour of the specific area of ​​the truck crane operating device corresponding to the best observation position of the laser radar and visible light camera; the straight line edges of the outer contour of the truck crane operating device can be clearly identified in the field of view of the laser radar and visible light camera at the best observation position.

[0102] S2: Preprocess the 3D point cloud data collected by the laser radar to obtain the 3D point cloud data B of the specific area of ​​the working device, and align it with the 3D model point cloud A to obtain the external parameter matrix F1 of the transformation relationship between the laser radar coordinate system and the model coordinate system;

[0103] S3: Use the external parameter matrix F1 to transform the model dense straight line point cloud {X3} to obtain three high-precision dense straight line point clouds {Y3} in the lidar coordinate system;

[0104] S4: extracting six straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the visible light camera according to the straight line segment detection algorithm, and calculating six planes {Z6} of these straight line segments in the camera coordinate system;

[0105] S5: Enumerate the three high-precision dense straight line point clouds {Y3} in the laser radar coordinate system and the six planes {Z6} in the camera coordinate system, construct all the arrangements, and iteratively minimize the distance residual from the straight line point cloud to the corresponding plane to obtain the external parameter matrix of each arrangement. The arrangement with the smallest error among all arrangements is used as the external parameter matrix of the transformation relationship between the laser radar and the visible light camera.

[0106] Specifically, Figure 6 As shown, in step S1, the model coordinate system is established with the vertex of the lower right corner of the upper surface of the truck crane cab 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. Since the laser radar and visible light camera are installed in front of the control room, the working device in front of the truck crane control room (crane arm, luffing cylinder, lifting hook, lifting rope, etc.) is generated in the model coordinate system according to the structural information of the truck crane working device model. Figure 7 (As shown in the figure) is a model point cloud A with a point spacing of 0.5 cm at the optimal observation position of the sensor, and three model dense straight line point clouds {X3} of its outer contour.

[0107] More specifically, Figure 8 As shown, in step S1, the edge straight line point cloud {X3} of the truck crane operating device model is generated. To ensure that the subsequent algorithm can obtain correct results, at least three nonlinearly related straight lines are selected. Figure 8 A straight edge l5 of the boom, a straight edge l6 of the boom cylinder and a suspension rope l7 were selected to generate the model edge straight line point cloud.

[0108] Specifically, in step S2, extracting the point cloud of the specific area of ​​the truck crane operating device from the point cloud data collected by the laser radar, registering it with the model point cloud of the specific area in the model coordinate system, and obtaining the conversion relationship between the laser radar coordinate system and the model coordinate system includes the following steps:

[0109] S21: Before calibration, the truck crane operating device (crane arm, luffing cylinder, lifting hook, and lifting rope) is adjusted to the best observation position of the laser radar and visible light camera, and then a frame of point cloud data M1 containing the operating device is collected through the laser radar;

[0110] S22: Move the working device at a certain angle and displacement, and move the working device out of the fields of view of the lidar and visible light camera or to the limit position according to the movement range of the device itself. Then collect a frame of point cloud data M2 as the background through the lidar again;

[0111] S23: According to the distance relationship between the working device and the lidar and the movement range of the working device, select the point clouds within a certain range (such as Figure 6 in the truck crane, the range is the area where -10m < x < 10m, -10m < y < 5m, -3m < z < 15m in the lidar coordinate system) in M1 and M2 to obtain N1 and N2. Subtract the intersection of the point cloud N1 and the point cloud N2 from the point cloud data N1 at the best observation position of the sensor, so as to remove the point clouds of non-working devices in the environment and obtain the three-dimensional point cloud data B of the specific area of the working device. This step greatly reduces the size of B and excludes the interfering point clouds from the non-engineering device specific areas in the environment, thus improving the speed of the registration algorithm with A and the anti-interference ability.

[0112] Specifically, as Fig. 9 shown, in the step S22, move the working device of the truck crane at a certain angle and displacement. The optimal situation is that the boom and the luffing cylinder are at the highest position to ensure that there are no moving devices in the fields of view of the lidar and visible light camera.

[0113] Since the attributes of the point cloud, such as the three-dimensional coordinates and reflection intensity in the lidar coordinate system, are floating-point numbers, subtraction cannot be directly performed. In S23, after converting the two point cloud data N1 and N2 into voxels using an octree, subtraction is performed.

[0114] Specifically, in the step S2, use the normal distribution transformation algorithm and the iterative closest point algorithm to register the three-dimensional point cloud data B of the specific area of the working device with the three-dimensional model point cloud A, so as to calculate the external parameter matrix F1 of the transformation relationship between the lidar coordinate system and the model coordinate system.

[0115] Specifically, the method for solving the external parameter matrix of the lidar and the visible light camera in the S5 step specifically includes the following steps:

[0116] S51: Enumerate 3 dense straight line point clouds {Y3} with high precision in the lidar coordinate system and 6 planes {Z6} in the camera coordinate system, construct all permutation ways, and there are permutation ways in total. The number of "edge-face pairs" of the pairing relationship between a single edge and a single face in each permutation way is 3;

[0117] S52: For each arrangement, the external parameter matrix of the transformation relationship between the laser radar and the visible light camera is initialized to a suitable initial value, and the optimal value U of the external parameter matrix of the arrangement is obtained by iteratively minimizing the distance residuals from the straight line point cloud to the corresponding plane in the three "edge-to-face" pairs;

[0118] S53: For each arrangement, a counter is initialized to 0, and the external parameter matrix U obtained by the arrangement is used to project all the three-dimensional point clouds of {Y3} into the image data E1 captured by the visible light camera. If the projection of a certain point appears within a certain distance of {L6}, the counter value is increased by 1. A total of 3 counters are obtained, and the external parameter matrix of the arrangement corresponding to the counter with the largest value is used as the external parameter matrix of the laser radar and the visible light camera.

[0119] Example 3

[0120] The working device is a bucket truck equipped with a hybrid solid-state laser radar (DJI AVIA Aolan) and a Hikvision infrared camera. The 3D point cloud sensor is a hybrid solid-state laser radar (DJI AVIA Aolan) and the 2D image sensor is a Hikvision infrared camera. The installation position is as follows: Fig.10 .

[0121] like Figure 1 As shown, a joint calibration method of a 3D point cloud sensor and a 2D image sensor of an engineering machinery with an operating device comprises the following steps:

[0122] S1: Establish a model coordinate system based on the bucket truck model, and generate the three-dimensional model point cloud A and three model dense straight line point clouds {X3} of the outer contour of the specific area of ​​the bucket truck operating device corresponding to the best observation position of the laser radar and infrared camera; the straight line edges of the outer contour of the bucket truck operating device can be clearly identified in the field of view of the laser radar and infrared camera at the best observation position.

[0123] S2: Preprocess the 3D point cloud data collected by the laser radar to obtain the 3D point cloud data B of the specific area of ​​the working device, and align it with the 3D model point cloud A to obtain the external parameter matrix F1 of the transformation relationship between the laser radar coordinate system and the model coordinate system;

[0124] S3: Use the external parameter matrix F1 to transform the model dense straight line point cloud {X3} to obtain three high-precision dense straight line point clouds {Y3} in the lidar coordinate system;

[0125] S4: extracting six straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the infrared camera according to the straight line segment detection algorithm, and calculating six planes {Z6} of these straight line segments in the camera coordinate system;

[0126] S5: Enumerate the three high-precision dense straight line point clouds {Y3} in the laser radar coordinate system and the six planes {Z6} in the camera coordinate system, construct all the arrangements, and obtain the external parameter matrix of the transformation relationship of each arrangement by iteratively minimizing the distance residual from the straight line point cloud to the corresponding plane. The arrangement with the smallest error among all arrangements is used as the external parameter matrix of the transformation relationship between the laser radar and the infrared camera.

[0127] Specifically, Fig.10 As shown, in step S1, the model coordinate system is established with the vertex of the lower right corner of the upper surface of the bucket truck engine housing 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. Since the laser radar and infrared camera are installed in front of the cab, the bucket truck cab front working device (boom, boom cylinder, bucket cylinder, rocker arm, pull rod, bucket, etc.) is generated in the model coordinate system according to the structural information of the bucket truck working device model. Fig.11 (As shown in the figure) is the model point cloud A with a point spacing of 1 cm at the optimal observation position of the sensor, and the three model dense straight line point clouds {X3} of its outer contour.

[0128] More specifically, Fig.12 As shown, in step S1, the edge straight line point cloud {X3} of the bucket truck operating device model is generated. To ensure that the subsequent algorithm can obtain correct results, at least three nonlinearly related straight lines are selected. Fig.12 A straight line edge l8 of the pull rod and two vertical straight lines l9 and l1 of the bucket are selected. 10 Generate a point cloud of the model edge straight lines, based on which you can select more straight lines, such as another long edge of the bucket l 11 .

[0129] Specifically, in step S2, extracting the point cloud of the specific area of ​​the bucket truck operating device from the point cloud data collected by the laser radar, and registering it with the model point cloud of the specific area in the model coordinate system, and obtaining the conversion relationship between the laser radar coordinate system and the model coordinate system includes the following steps:

[0130] S21: Before calibration, adjust the bucket truck operating device (boom, boom cylinder, boom, boom cylinder, boom connecting rod, bucket, bucket connecting rod, bucket cylinder) to the best observation position of the laser radar and infrared camera, and then collect a frame of point cloud data M1 containing the operating device through the laser radar;

[0131] S22: Move the working device at a certain angle and displacement, and move the working device out of the field of view of the laser radar and infrared camera or the extreme position according to the activity range of the device itself, and collect a frame of point cloud data M2 as the background through the laser radar again;

[0132] S23: According to the distance relationship between the working device and the lidar and the movement range of the working device, select the point clouds within a certain range (such as Fig.10 forklift, the range is the area where 0m < x < 5m, -5m < y < 5m, -5m < z < 5m in the lidar coordinate system) in M1 and M2 to obtain N1 and N2. Subtract the intersection of the point cloud N1 at the best viewing position from the point clouds N1 and N2, thereby removing the point clouds of non-working devices in the environment and obtaining the three-dimensional point cloud data B of the specific area of the working device. This step greatly reduces the size of B and excludes the interfering point clouds from the non-engineering device specific area in the environment, thereby improving the speed of the registration algorithm with A and enhancing the anti-interference ability.

[0133] Specifically, such as Fig.13 , in the step S22, the forklift working device is moved at a certain angle and displacement. The optimal situation is that the boom, swing arm, and bucket are all at the highest position to ensure that no moving device appears in the fields of view of the lidar and the infrared camera.

[0134] Since the attributes of the point cloud, such as the three-dimensional coordinates and reflection intensity in the lidar coordinate system, are floating-point numbers, subtraction cannot be directly performed. In S23, after converting the two point cloud data N1 and N2 into voxels using an octree, subtraction is then carried out.

[0135] Specifically, in the step S2, the normal distribution transformation algorithm and the iterative closest point algorithm are used to register the three-dimensional point cloud data B of the specific area of the working device with the three-dimensional model point cloud A, thereby calculating the external parameter matrix F1 of the transformation relationship between the lidar coordinate system and the model coordinate system.

[0136] Specifically, the method for solving the external parameter matrix of the lidar and the infrared camera in the S5 step specifically includes the following steps:

[0137] S51: Enumerate 3 dense straight line point clouds {Y3} with high precision in the lidar coordinate system and 6 planes {Z6} in the camera coordinate system, construct all permutation methods, there are kinds of permutation methods, and the number of "edge-plane pair" of the pairing relationship between a single edge and a single plane in each permutation method is 3;

[0138] S52: For each permutation method, initialize the external parameter matrix of the transformation relationship between the lidar and the infrared camera to a suitable initial value, and obtain the optimal value U of the external parameter matrix of this permutation method by iteratively minimizing the distance residuals from the straight line point cloud to the corresponding plane in 3 "edge-plane pairs".

[0139] S53: For each arrangement, a counter is initialized to 0, and the external parameter matrix U obtained by the arrangement is used to project all the three-dimensional point clouds of {Y3} to the image data E1 captured by the infrared camera. If the projection of a certain point appears within a certain distance of {L6}, the counter value is increased by 1. A total of 3 counters are obtained, and the external parameter matrix of the arrangement corresponding to the counter with the largest value is used as the external parameter matrix of the laser radar and the infrared camera.

[0140] 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.

[0141] 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.

[0142] 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 method for jointly calibrating a 3D point cloud sensor and a 2D image sensor of an engineering machinery with an operating device, characterized in that: The method comprises the following steps: S1: Establish a model coordinate system based on the engineering machinery model, and generate the 3D model point cloud A corresponding to the specific area of ​​the engineering machinery operation device at the optimal observation position of the 3D point cloud sensor and the 2D image sensor and the m model dense straight line point clouds {X m }; The straight line edge of the outer contour of the working device can be clearly identified in the field of view of the 3D point cloud sensor and the 2D image sensor at the best observation position; S2: pre-process the 3D point cloud data collected by the 3D point cloud sensor to obtain the 3D point cloud data B of the specific area of ​​the working device, and register it with the 3D model point cloud A to obtain the external parameter matrix F1 of the transformation relationship between the 3D point cloud sensor coordinate system and the model coordinate system; S3: Use the external parameter matrix F1 to transform the model's dense straight line point cloud {X m }Transform to obtain m high-precision dense straight line point clouds {Y m }; S4: extracting n straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the two-dimensional image sensor according to the straight line segment detection algorithm, and calculating the n planes {Z n }; S5: Enumerate m high-precision dense straight line point clouds {Y m } and n planes {Z n }, construct all the arrangements, and obtain the external parameter matrix of the transformation relationship of each arrangement by iteratively minimizing the distance residual from the straight line point cloud to the corresponding plane; the arrangement with the smallest error among all the arrangements is used as the external parameter matrix of the transformation relationship between the three-dimensional point cloud sensor and the two-dimensional image sensor; In step S4, the image data collected by the two-dimensional image sensor are used to extract n straight line segments of the outer contour of the specific area of ​​the working device according to the straight line segment detection algorithm, and the n planes {Z n The method comprises the following steps: S41: while the 3D point cloud sensor collects M1, the 2D image sensor collects a frame of image data I1 including the working device; S42: while the 3D point cloud sensor is collecting M2, a frame of image data I2 is collected again by the 2D image sensor as a background; S43: Using the LSD straight line segment detection algorithm to obtain edge images E1 and E2 respectively for the data in the preset area of ​​images I1 and I2, and obtaining n active straight line segments {L n }; S44: For L in the pixel coordinate system n Every straight line l k , select any two non-coincident points P1 and P2 on the straight line; S45: Assuming that the depth values ​​of pixels P1 and P2 are certain, the pixel coordinates of P1 and P2 are converted to Q1 and Q2 in the image sensor coordinate system through the known intrinsic parameter matrix of the image sensor, and the straight line l in the image sensor coordinate system is determined by three points, Q1 and Q2 in the image sensor coordinate system and the origin (0, 0, 0) of the image sensor coordinate system. k The plane where the image sensor is located is obtained by n }; The method for solving the external parameter matrix of the three-dimensional point cloud sensor and the two-dimensional image sensor in step S5 includes the following steps: S51: Enumerate m high-precision dense straight line point clouds {Y m } and n planes {Z n }, construct all the permutations, set to K, and set the number of edge-face pairs between a single edge and a single face in each permutation to c; when m≤n, c=m; when m>n, c = n; S52: For each arrangement, the external parameter matrix of the transformation relationship between the three-dimensional point cloud sensor and the two-dimensional image sensor is initialized to a suitable initial value, and the optimal value U of the external parameter matrix under the current arrangement is obtained by iteratively minimizing the distance residuals from the straight line point cloud to the corresponding plane in the c edge-face midpoints; S53: Initialize a counter to 0 for each arrangement, use the optimal value U of the external parameter matrix obtained by the arrangement, and convert {Y m All three-dimensional point clouds are projected into the image data E1 captured by the two-dimensional image sensor. If the projection of a certain point appears in {L n }, the counter value is increased by one; a total of K counters are obtained, and the external parameter matrix corresponding to the arrangement of the counter with the largest value is used as the external parameter matrix of the three-dimensional point cloud sensor and the two-dimensional image sensor; In step S2, preprocessing the three-dimensional point cloud data collected by the three-dimensional point cloud sensor to obtain three-dimensional point cloud data B of the specific area of ​​the working device includes the following steps: S21: before calibration, the working device is adjusted to the best observation position of the three-dimensional point cloud sensor and the two-dimensional image sensor, and then a frame of point cloud data M1 containing the working device is collected by the three-dimensional point cloud sensor; S22: Move the operating device at a certain angle and displacement, and move the operating device out of the field of view or the limit position of the 3D point cloud sensor and the 2D image sensor according to the activity range of the device itself, and collect a frame of point cloud data M2 as the background through the 3D point cloud sensor again; S23: Based on the distance relationship between the working device and the three-dimensional point cloud sensor and the activity range of the working device, select the point clouds within the preset range of M1 and M2 to obtain N1 and N2; subtract the intersection of point cloud N1 and point cloud N2 from the point cloud data N1 at the best observation position of the sensor, thereby removing the non-working device point clouds in the environment and obtaining the three-dimensional point cloud data B of the specific area of ​​the working device.

2. The method for jointly calibrating a 3D point cloud sensor and a 2D image sensor of an engineering machinery with a working device according to claim 1, characterized in that: A model coordinate system based on the engineering machinery model is established, and the three-dimensional model point cloud A corresponding to the specific area of ​​the engineering machinery operation device at the optimal observation position of the three-dimensional point cloud sensor and the two-dimensional image sensor and the m model dense straight line point clouds {X m },include: The model coordinate system is established with any point on the working device model as the origin. According to the structural information with straight edge features in the working device model, the working device is generated according to the preset spacing. The 3D model point cloud A and the m model dense straight line point clouds {X m }; where {X m There are at least 3 nonlinearly related model dense straight line point clouds in}.

3. The method for joint calibration of a 3D point cloud sensor and a 2D image sensor for an engineering machinery with a working device according to claim 1, characterized in that: In step S2, the three-dimensional point cloud data B of the specific area of ​​the working device is aligned with the three-dimensional model point cloud A using the normal distribution transformation algorithm and the iterative closest point algorithm, so as to solve the external parameter matrix F1 of the transformation relationship between the three-dimensional point cloud sensor coordinate system and the model coordinate system.

4. The method for jointly calibrating a 3D point cloud sensor and a 2D image sensor of an engineering machinery with a working device according to claim 1, characterized in that: When the best observation position of the 3D point cloud sensor and the 2D image sensor is the normal parking state of the construction machinery, the calibration of the sensor coordinate system and the vehicle coordinate system can be realized, which specifically includes the following steps: S71: Generate a 3D model point cloud A′ corresponding to a specific area of ​​the engineering machinery operation device in the vehicle body coordinate system at the optimal observation position of the sensor and m′ model dense straight line point clouds {X′ m }; S72: pre-processing the 3D point cloud data collected by the 3D point cloud sensor to obtain the 3D point cloud data B′ of the specific area of ​​the working device, and registering it with the 3D model point cloud A′ to obtain the external parameter matrix F′1 of the transformation relationship between the 3D point cloud sensor coordinate system and the vehicle body coordinate system; S73: Use the external parameter matrix F'1 to transform the model dense straight line point cloud {X′ m } is transformed to obtain high-precision m′ dense straight line point clouds {Y′ m }; S74: extracting n′ straight line segments of the outer contour of the specific area of ​​the working device from the image data collected by the two-dimensional image sensor according to the straight line segment detection algorithm, and calculating the n′ planes {Z′ n }; S75: Enumerate m′ dense straight line point clouds {Y′ m } and n′ planes {Z′ n }, construct all the arrangements, and obtain the external parameter matrix of the transformation relationship of each arrangement by iteratively minimizing the distance residual from the straight line point cloud to the corresponding plane; the arrangement with the smallest error among all the arrangements is used as the external parameter matrix F'2 of the transformation relationship between the three-dimensional point cloud sensor and the two-dimensional image sensor; S76: Obtain an extrinsic parameter matrix F'3 of the transformation relationship between the two-dimensional image sensor coordinate system and the vehicle body coordinate system by calculating the product of the extrinsic parameter matrix F'2 and the extrinsic parameter matrix F'1.

Images