A refueling cover accurate perception algorithm and device suitable for an automatic refueling robot

Abstract

The present application belongs to the technical field of automatic refueling of fuel vehicles, and discloses a refueling cover precise sensing algorithm suitable for an automatic refueling robot. For a refueling outer cover, a coarse-to-fine positioning scheme using 2D rough positioning and 3D precise positioning is proposed. The outer cover is roughly positioned by projecting the point cloud into a 2D image based on the normal line of the point cloud surface calculated by the normal line of the outer cover point cloud surface. The outer cover is further segmented for precise positioning using the rough positioning points. For a refueling inner cover, a coarse-to-fine template matching scheme is proposed. The key points of the point cloud edge are extracted, and the rough pose estimation is performed in combination with the geometric features of the refueling inner cover. On the basis of rough registration, the template point cloud handle and the inner cover point cloud handle are aligned for fine registration. Finally, the positioning pose is corrected to calculate the precise positioning pose of the refueling inner cover, and a small amount of template library can be used to match multiple different types of refueling plugs, avoiding the construction of a large template library and the need for high computing power.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of automatic fueling of fuel vehicles, and particularly relates to a fuel cap precise perception algorithm and device suitable for an automatic fueling robot. BACKGROUND

[0002] So far, fuel stations mainly manually fuel fuel vehicles by employing workers. Due to factors such as training time of workers, mastery of theoretical knowledge, etc., the operation of fueling workers is uncertain, which may bring certain safety threats. In addition, the manual fueling method needs a large amount of manpower and material resources. The 3D vision technology has many advantages, such as rich and fast data acquisition mode, high precision, non-contact, low power consumption of some 3D cameras, and explosion-proof performance. Therefore, based on 3D vision, an automatic fueling robot is developed, which has a wide application prospect.

[0003] At present, there are three types of solutions for fuel cap positioning of the automatic fueling robot of fuel vehicles: (1) visual servoing scheme. A camera is used to roughly position the fuel cap position and guide the robot to the fuel cap nearby. This method has low precision and insufficient real-time performance. (2) A large field of view laser camera is used to scan the three-dimensional information of the fuel cap nearby range, which is easily disturbed by the surrounding complex environment and has poor robustness. (3) Semi-automatic fueling of fuel vehicles is realized through human-computer interaction. For example, the vehicle model is identified, or the vehicle data information is manually input by the vehicle owner and the fueling worker, and then the vehicle information defined in the database in advance is matched. It needs to spend a lot of time, and the vehicle information in the database may not be complete, so only semi-automatic fueling can be realized.

[0004] Therefore, a fuel cap precise perception algorithm suitable for an automatic fueling robot is provided. It is an urgent problem for those skilled in the art to meet the high precision and strong robustness requirements of the automatic fueling robot. SUMMARY

[0005] The purpose of the present application is to provide a fuel cap precise perception algorithm suitable for an automatic fueling robot, which fuses 3D and 2D vision.

[0006] In order to achieve the above purpose, the present application adopts the following technical scheme:

[0007] A fuel cap precise perception algorithm suitable for an automatic fueling robot includes the following steps:

[0008] Step 1: A 3D camera is used to collect point cloud data of the fuel cap scene, a 2D image is obtained according to the point cloud normal projection, and then a rough positioning point coordinate of the fuel cap is obtained.

[0009] Step 2: based on the rough positioning point of the oiling outer cover, the outer cover is segmented to obtain the precise positioning point of the oiling outer cover, and the pose of the oiling outer cover relative to the 3D camera is positioned;

[0010] Step 3: a template library of the oiling inner cover is established to obtain a template point cloud; the 3D camera collects scene point cloud data of the oiling inner cover, extracts an inner cover point cloud from the scene point cloud data of the oiling inner cover, and extracts edge key points from the inner cover point cloud and the template point cloud, and performs rough matching of the template point cloud and the inner cover point cloud based on geometric features;

[0011] Step 4: based on the rough matching of step 3, the main direction of the handle of the template point cloud is aligned to the main direction of the handle of the inner cover point cloud, fine registration is performed, and the pose of the oiling inner cover relative to the 3D camera is calculated;

[0012] Step 5: a positioning pose correction scheme is proposed to obtain the accurate pose of the oiling inner cover relative to the 3D camera;

[0013] Step 6: based on the coordinate transformation relationship in the system, the accurate pose matrix of the oiling outer cover and the oiling inner cover in the camera coordinate system is transformed to the robot base coordinate system.

[0014] Further, in step 1, the method for obtaining a 2D image is as follows: the scene point cloud of the oiling outer cover is filtered to remove noise points, then the scene point cloud of the oiling outer cover is down-sampled, and then the sliding least square method is used to smooth the scene point cloud of the oiling outer cover; the K-D tree is used to quickly search for the neighboring points of each point in the scene point cloud of the oiling outer cover, the local surface of the neighboring points is fitted to estimate the normal vector, the estimation result of the normal vector is taken as the normal direction of the point (each point in the scene point cloud of the oiling outer cover), and then the normal is normalized; then the boundary information of the scene point cloud of the oiling outer cover is calculated to obtain the minimum and maximum values of the point cloud in X and Y dimensions, i.e. Xmin, Ymin, Xmax and Ymax; the coordinates (Xmin, Ymin) of the 3D oiling outer cover scene point cloud in X and Y dimensions correspond to the position of the 2D image pixel (0, 0) point, and the coordinates (Xmax, Ymax) of the oiling outer cover scene point cloud in X and Y dimensions correspond to the lower right position coordinates of the 2D image, and accordingly the position corresponding relationship of each point of the oiling outer cover scene point cloud projected to each pixel point of the 2D image is established; the normalized normal direction (0≤Xn, Yn, Zn≤1) of each point of the oiling outer cover scene point cloud is multiplied by 255, and the product result is taken as the pixel value of the R, G and B three channels of the 2D image.

[0015] Further, in step 1, the method for obtaining the rough positioning point coordinates of the refueling outer cover is as follows: according to the difference between the refueling outer cover and other position pixel points on the 2D image, the projected 2D image is binarized and morphological operation is performed to remove large-area noise and retain the complete image of the refueling outer cover; a plurality of rows of pixel points are randomly selected, the positions of the pixel points with a pixel value of 255 are found, the center of each row is found, and the center positions found in the plurality of rows are averaged to obtain Xox, which is the X coordinate of the rough positioning point of the refueling outer cover on the 2D image. Similarly, the Y coordinate of the rough positioning point of the refueling outer cover on the 2D image Yoy is obtained. The (Xox, Yoy) is corresponded to the 3D refueling outer cover scene point cloud to obtain the rough positioning point (Xor, Yor) of the refueling outer cover without Z dimension.

[0016] Further, in step 2, the method for obtaining the precise positioning point of the refueling outer cover is as follows: the refueling outer cover scene point cloud data is segmented into two parts based on region growing segmentation; a threshold β is set, and it is checked whether there is an (Xor±β, Yor±β) region in the segmented point set. If not, this part is not the outer cover point cloud; if yes, this part is the outer cover point cloud, and the centroid of the outer cover point cloud, i.e., the precise positioning point (Xoa, Yoa, Zoa) of the refueling outer cover, is calculated.

[0017] Further, in step 2, the method for positioning the pose of the refueling outer cover relative to the 3D camera is as follows: the outer cover point cloud is fitted to a plane to obtain the plane parameters (Ao, Bo, Co, Do). The first three components of the plane parameters are the coordinates (Xoz, Yoz, Zoz) of the Z direction of the refueling outer cover grabbing coordinate system in the 3D camera coordinate system. Align (Xoz, Yoz, Zoz) with the Z direction (0, 0, 1) of the 3D camera, and apply the Rodrigues formula to construct the rotation matrix R0c, i.e., there is the following relationship:

[0018]

[0019]

[0020] According to the matrix operation relationship, the inverse of the R0c matrix is solved to obtain the coordinates (Xox, Yox, Zox) and (Xoy, Yoy, Zoy) of the X and Y directions of the refueling outer cover grabbing coordinate system in the 3D camera coordinate system; and then the matrix representation of the refueling outer cover grabbing coordinate system in the 3D camera coordinate system, i.e., the precise pose matrix T0gc of the refueling outer cover relative to the 3D camera, is obtained.

[0021]

[0022] The position and orientation of the refueling outer cover in the camera coordinate system are obtained.

[0023] Further, in step 3, the method for extracting edge key points is: using an internal shape descriptor algorithm, effectively extracting the handle edge key points of the inner cover point cloud and the template point cloud and the edge key points of the bottom circular surface of the oiling inner cover;

[0024] The rough matching method of the template point cloud and the inner cover point cloud is: fitting a 3D circle to the edge key points of the inner cover point cloud and the template point cloud, obtaining the center (Xtc, Ytc, Ztc) and normal direction (Xtn, Ytn, Ztn) of the 3D circle fitting the bottom surface of the oiling inner cover in the inner cover point cloud, and obtaining the center (Xmc, Ymc, Zmc) and normal direction (Xmn, Ymn, Zmn) of the 3D circle of the oiling inner cover bottom surface of the template point cloud, noting that the center and normal direction of the template point cloud can be solved in advance to speed up; according to the normal direction (Xtn, Ytn, Ztn) and (Xmn, Ymn, Zmn), the rotation matrix Rmt is calculated by the Rodriguez formula, and the template point cloud is aligned with the oiling inner cover bottom surface in the inner cover point cloud, so that:

[0025]

[0026] where t mt is a 3x1 translation matrix, thereby constructing a rough transformation matrix of the template point cloud to the inner cover point cloud, defined as Tmt:

[0027]

[0028] Further, in step 4, the handle main direction of the template point cloud is aligned to the handle main direction of the inner cover point cloud, and fine registration is performed, and the process of calculating the pose of the oiling inner cover relative to the 3D camera includes:

[0029] Step 4.1: conditionally filtering according to the (Xtn, Ytn, Ztn) normal and (Xtc, Ytc, Ztc) center, extracting the handle of the inner cover point cloud; and calculating the handle main direction vector of the extracted inner cover point cloud according to the principal component analysis algorithm, defined as (Xth, Yth, Zth); similarly, the handle main direction vector (Xmh, Ymh, Zmh) of the template point cloud is obtained;

[0030] Step 4.2: rotating the handle main direction vector (Xmh, Ymh, Zmh) to obtain the handle main direction vector (Xmth, Ymth, Zmth) of the template point cloud after rough registration conversion, i.e.:

[0031]

[0032] Step 4.3: according to the Rodriguez formula, aligning the direction vector (Xmth, Ymth, Zmth) to the direction vector (Xth, Yth, Zth) to obtain the rotation matrix R fine ; according to Rfine Align the handle of the template point cloud with the handle of the inner cap point cloud; align the oiling inner cap bottom circle center in the rotated template point cloud with the oiling inner cap bottom circle center in the inner cap point cloud, and calculate the translation matrix obtained in the fine registration, i.e.,

[0033]

[0034] Construct the fine registration matrix T fine :

[0035]

[0036] Further, in step 4, the handle main direction of the template point cloud is aligned to the handle main direction of the inner cap point cloud, fine registration is performed, and the process of calculating the pose of the oiling inner cap relative to the 3D camera also includes:

[0037] Step 4.4: Calculate the position of the handle center point in the camera coordinate system in the inner cap point cloud:

[0038]

[0039] Where (Xm, Ym, Zm) is the handle surface center point position obtained in advance by the template point cloud; (Xt, Yt, Zt) is the handle center point position in the inner cap point cloud;

[0040] Step 4.5: Based on the (Xt, Yt, Zt) center point, define the oiling inner cap grabbing direction according to the system inner oiling inner cap grabbing coordinate system definition scheme, obtain the pose matrix representation of the oiling inner cap in the camera coordinate system, and define it as Tigcr:

[0041]

[0042] Where the first three columns of the matrix represent the grabbing coordinate system direction on the oiling inner cap in the camera coordinate system, which is obtained by the system inner oiling inner cap grabbing coordinate system definition scheme.

[0043] Further, in step 5, the method for obtaining the accurate pose of the oiling inner cap relative to the 3D camera includes the following steps:

[0044] Step 5.1: According to the registration result, the positioning point is corrected in the direction perpendicular to the handle, i.e. the grabbing X direction. Align the grabbing coordinate system X direction (Xix, Yix, Zix) to the 3D camera X direction (1, 0, 0), calculate the rotation matrix Rgx according to the Rodrigues formula, and the rotated rough center point is:

[0045]

[0046] Step 5.2: Create the inner cover point cloud bounding box, align the inner cover point cloud to the camera Z direction, and extract the handle point cloud of the inner cover point cloud based on the center point (Xt, Yt, Zt), and cut the rectangular point cloud from the handle point cloud, project the handle point cloud to the plane and downsample the point cloud;

[0047] Step 5.3: Rotate the handle point cloud, calculate the mean value X" of the X coordinate of the rotated handle point cloud t , and correct X' t ; t , and rotate the corrected handle center point back to the original space to obtain the final accurate positioning point (Xia, Yia, Zia), that is:

[0048]

[0049] Step 5.4: Obtain the accurate pose matrix Tigc of the refueling inner cover relative to the 3D camera:

[0050]

[0051] A refueling cover accurate perception device suitable for an automatic refueling robot, which can implement the above algorithm, comprising:

[0052] A point cloud data acquisition module for acquiring refueling outer cover scene point cloud data, refueling inner cover scene point cloud data, and template point cloud data;

[0053] A refueling outer cover positioning module: including a refueling outer cover scene point cloud preprocessing unit for removing noise points, down-sampling and smoothing point cloud processing on the refueling outer cover scene point cloud, and obtaining the normalized normal direction of each point in the refueling outer cover scene point cloud; a 2D image conversion unit for calculating the 2D image pixel position corresponding to the refueling outer cover scene point, and calculating the pixel value of the 2D image using the normalized normal direction of each point in the outer cover scene point cloud; a refueling outer cover rough positioning point calculation unit for obtaining the coordinates of the refueling outer cover rough positioning point on the 2D image, and converting it to the corresponding coordinates in the refueling outer cover scene point cloud; a refueling outer cover accurate point calculation unit for extracting the outer cover point cloud from the refueling outer cover scene point cloud, and calculating the centroid of the outer cover point cloud; a refueling outer cover pose positioning unit for calculating the accurate pose matrix of the refueling outer cover relative to the 3D camera according to the outer cover point cloud and the centroid of the outer cover point cloud, and transforming the pose matrix to the robot base coordinate system;

[0054] The refueling inner cover positioning module comprises a refueling inner cover template library unit for storing a refueling inner cover template; a refueling inner cover point cloud extraction unit for extracting inner cover point cloud from refueling inner cover scene point cloud; a refueling inner cover rough matching unit for calculating a rough transformation matrix of template point cloud to inner cover point cloud according to inner cover point cloud and template point cloud; a refueling inner cover fine matching unit for calculating a fine registration matrix according to inner cover point cloud and template point cloud; a refueling inner cover pose positioning unit for calculating a pose matrix of refueling inner cover in a 3D camera coordinate system according to the rough transformation matrix and the fine registration matrix; a refueling inner cover pose correction unit for correcting the pose matrix of refueling inner cover in the 3D camera coordinate system to obtain an accurate pose matrix of refueling inner cover relative to the 3D camera; and a refueling inner cover pose matrix transformation unit for transforming the accurate pose matrix of refueling inner cover relative to the 3D camera to a robot base coordinate system.

[0055] Compared with the prior art, the present application has the following beneficial effects:

[0056] The application discloses a refueling cover precise perception algorithm fusing 3D and 2D vision and suitable for an automatic refueling robot, and can be applied to the technical field of automatic refueling of fuel vehicles.

[0057] For the refueling inner cover, a rough-to-fine template matching scheme is proposed, rough pose estimation is performed by extracting key points of the point cloud edge and combining the geometric features of the refueling inner cover, the template inner cover point cloud handle and the scene inner cover point cloud handle are aligned for fine registration on the basis of rough registration, and finally the positioning pose is corrected to calculate the accurate positioning pose of the refueling inner cover. BRIEF DESCRIPTION OF DRAWINGS

[0058] Figure 1 The present application is an embodiment of the whole flowchart;

[0059] Figure 2 The present application is a refueling outer cover scene point cloud graph of embodiment 1;

[0060] Figure 3 The present application is an outer cover point cloud graph of embodiment 1;

[0061] Figure 4 The present application is a pose visualization graph of the refueling outer cover of embodiment 1 in the camera coordinate system;

[0062] Figure 5 The inner cover point cloud graph of Example 1 of the present application;

[0063] Figure 6 The edge key point graph of Example 1 of the present application;

[0064] Figure 7 The final matching effect graph of the refueling inner cover of Example 1 of the present application;

[0065] Figure 8 The pose visualization graph of the refueling inner cover of Example 1 of the present application in the camera coordinate system;

[0066] Figure 9 The effect graph of Example 1 of the present application using a single template to match multiple different types of refueling inner covers;

[0067] Figure 10 The pose visualization result graph of Example 1 of the present application using a single template to match different types of refueling inner covers in the camera coordinate system. DETAILED DESCRIPTION

[0068] Example 1

[0069] As shown in the reference Figure 1 , the overall flowchart of a refueling cover precise perception algorithm suitable for an automatic refueling machine of the present application mainly includes the following parts: refueling outer cover scene point cloud collection, refueling outer cover scene point cloud data preprocessing, outer cover point cloud extraction, refueling outer cover rough positioning, refueling outer cover precise positioning, refueling inner cover scene point cloud collection and inner cover point cloud extraction, inner cover point cloud edge key point extraction, refueling inner cover rough matching based on geometric features, refueling inner cover fine matching, and refueling inner cover pose correction. The specific steps include the following:

[0070] Step 1: Collect scene point cloud data containing a refueling outer cover by a 3D camera, obtain a 2D image by projecting the point cloud normal to perform outer cover rough positioning, and obtain a refueling outer cover rough position point coordinate, including the following steps:

[0071] Step 1.1: Collect refueling outer cover scene point cloud data by a 3D camera, as shown in the reference Figure 2 The refueling outer cover scene point cloud data collected in this embodiment, and then preprocess the refueling outer cover scene point cloud data: first, filter the outliers of the refueling outer cover scene point cloud to remove noise points, then downsample the refueling outer cover scene point cloud, and then smooth the refueling outer cover scene point cloud by using the sliding least squares method. The K-D tree is used to quickly search for the near neighbor points of each point in the refueling outer cover scene point cloud, the local surface of the near neighbor points is fitted to estimate the normal vector, and the normal vector estimation result is used as the normal direction of the point (the point refers to each point in the refueling outer cover scene point cloud) and the normal is normalized. The normal vector estimation process adopts OpenMP standard multi-threaded calculation,

[0072] Step 1.2: Obtain 2D image: Calculate the boundary information of the refueling cap scene point cloud (without considering the Z dimension coordinates), that is, obtain the minimum and maximum values of the point cloud in the X and Y dimensions, that is, Xmin, Ymin, Xmax, Ymax. The X and Y dimension coordinates of the refueling cap scene point cloud (Xmin, Ymin) correspond to the 2D image pixel (0, 0) point position, and the point cloud (Xmax, Ymax) coordinates correspond to the right lower position coordinates of the 2D image. According to this, the position correspondence relationship of each point of the 3D refueling cap scene point cloud projected to each pixel point of the 2D image is established. Multiply the normalized normal direction (Xn, Yn, Zn) (0≤Xn, Yn, Zn≤1) of each point of the refueling cap scene point cloud by 255, and the product will be used as the pixel value of the R, G, B three channels of the 2D image (3D and 2D position correspondence relationship is described above). That is, the minimum pixel value of the 2D image is 0, and the maximum pixel value is 255, which meets the pixel value requirement of the 2D image. In this embodiment, the calculated point cloud boundary (Xmin, Ymin) = (-70.4508, -150.466), (Xmax, Ymax) = (178.118, 128.712).

[0073] Step 1.3: Obtain the rough positioning point coordinates of the refueling cap: Since the normal direction at the refueling cap is approximately uniform (not uniform with the vehicle body), the pixel points of the refueling cap on the 2D image are approximately uniform, and there is a pixel value difference with other positions on the image. According to this, the image is binarized and morphological operation is performed to remove large area noise and retain the complete 2D image of the refueling cap. Randomly select several rows of pixel points of the 2D image, find the pixel point position with a pixel value of 255, and take the center of each row of found positions. Take the average of the center positions found in several rows to obtain the X coordinate Xox of the rough positioning point of the cap in the 2D image position. Similarly, obtain the Y coordinate Yoy of the rough positioning point in the image Y direction, thereby obtaining (Xox, Yoy). According to the 2D and 3D position correspondence relationship described in step 1.2, the 2D position point is corresponded to the 3D refueling cap scene point cloud to obtain the refueling cap rough position point (Xor, Yor) without containing the Z dimension. In this embodiment, the obtained refueling cap rough position point (Xor, Yor) in the 3D space is (40.1184, 3.71152).

[0074] Step 2: Based on the rough positioning point of the cap described in step 1, the cap is segmented to obtain point cloud data containing only the refueling cap, that is, the cap point cloud, and the pose of the refueling cap relative to the 3D camera is accurately positioned based on the cap point cloud, including the following steps:

[0075] Step 2.1: When the oil cover is opened, there is a gap between the oil cover and the vehicle body interface. Based on region growing segmentation, the oil cover scene point cloud data is segmented into two parts: the oil cover and the vehicle body (only two parts are segmented here, but it is not determined which part is the oil cover and which part is the vehicle body). According to the rough position point of the outer cover obtained in step 1.3, a threshold β is set, and it is checked whether there is an (Xor±β, Yor±β) region in the segmented point set. If not, this part is not the outer cover point cloud; if so, this part is the outer cover point cloud, to determine the oil cover area (refer to Figure 3 For the extracted outer cover point cloud data) and calculate the centroid of this area, that is, obtain the precise positioning point (Xoa, Yoa, Zoa) of the oil cover. In this embodiment, the threshold β is set to 2, the search region is (Xor±β, Yor±β) = (40.1184±2, 3.71152±2), and the precise positioning point (Xoa, Yoa, Zoa) of the oil cover is (81.1242, 37.7113, 466.872).

[0076] Step 2.2: Plane fitting is performed on the outer cover point cloud to obtain plane parameters (Ao, Bo, Co, Do). The first three components of the plane parameters are the coordinates of the Z direction of the oil cover grabbing coordinate system in the camera coordinate system, defined as (Xoz, Yoz, Zoz). In this embodiment, the plane parameters (Ao, Bo, Co, Do) = (0.318347, 0.333027, 0.887552, -452.479), and the coordinates of the Z direction of the oil cover grabbing coordinate system in the camera coordinate system (Xoz, Yoz, Zoz) = (0.318347, 0.333027, 0.887552). Further, the representation of the X and Y directions of the oil cover grabbing coordinate system in the camera coordinate system is described. Align (Xoz, Yoz, Zoz) with the camera Z direction (0, 0, 1), and apply the Rodrigues formula to construct the rotation matrix R0c, that is, there is the following relationship:

[0077]

[0078]

[0079] According to the matrix operation relationship, the inverse of the R0c matrix is solved to obtain the coordinate representation of the X and Y directions of the oil cover grabbing coordinate system in the camera coordinate system (Xox, Yox, Zox) and (Xoy, Yoy, Zoy). In this embodiment, (Xox, Yox, Zox) = (0.946309, -0.056167, -0.318347), and (Xoy, Yoy, Zoy) = (-0.056167, 0.941243, -0.333027). Further, the oil cover grabbing coordinate system is obtained (refer to Figure 4For the pose visualization result of the oil cover in the camera coordinate system, the blue direction line is the Z direction of the oil cover grabbing, the pink direction line is the X direction of the oil cover grabbing, the cyan is the Y direction of the oil cover grabbing, and the red point is the matrix representation of the origin of the oil cover grabbing coordinate system in the camera coordinate system, that is, the accurate pose matrix Togc of the oil cover relative to the 3D camera:

[0080]

[0081] In the present embodiment, Togc:

[0082]

[0083] Accordingly, the position and orientation of the oil cover in the camera coordinate system can be known.

[0084] Step 3: Collect the scene point cloud data of the oil cover by the 3D camera, extract the inner cover point cloud from the scene point cloud based on the system data preprocessing scheme (here, the system data preprocessing scheme uses the prior art, which is not described in detail), and extract the edge key points from the inner cover point cloud and the template point cloud. Rough matching of the template point cloud and the inner cover point cloud is carried out based on geometric features, including the following steps:

[0085] Step 3.1: Collect the scene point cloud data of the oil cover by the 3D camera, and extract the inner cover point cloud from the oil cover scene point cloud by the system data preprocessing scheme. As shown in reference Figure 5 For the extracted inner cover point cloud.

[0086] Step 3.2: The oil cover has obvious geometric edge features, and the bottom is usually an approximate circular surface, and the upper handle is usually a rectangular curved surface. The internal shape descriptor (ISS) algorithm is adopted to effectively extract the handle edge key points and the oil cover bottom circular surface key points (usually a spatial circle) of the inner cover point cloud. The template point cloud is extracted in the same way. As shown in reference Figure 6 For the edge key point collection result, (a) is the edge key point of the template point cloud, and (b) is the edge key point in the inner cover point cloud.

[0087] Step 3.3: 3D space circle fitting is performed on the extracted edge key points (including the inner cover point cloud and the template point cloud). The center (Xtc, Ytc, Ztc) and normal direction (Xtn, Ytn, Ztn) of the 3D circle fitting of the inner cover point cloud are obtained; the center (Xmc, Ymc, Zmc) and normal direction (Xmn, Ymn, Zmn) of the 3D circle fitting of the template point cloud are obtained, and the center and normal fitting values of the template point cloud can be solved in advance to speed up. In this embodiment, the center (Xtc, Ytc, Ztc) of the inner cover point cloud is (57.5244, 41.5891, 491.103), and the normal direction (Xtn, Ytn, Ztn) is (-0.136844, 0.0115901, 0.990525); the center (Xmc, Ymc, Zmc) of the template point cloud is (-7.88307, 53.8734, 385.867), and the normal direction (Xmn, Ymn, Zmn) is (0.0193574, 0.0101322, 0.999761).

[0088] Step 3.4: According to the normal direction (Xtn, Ytn, Ztn) and (Xmn, Ymn, Zmn), the rotation matrix Rmt is calculated by the Rodrigues formula, and the template point cloud is aligned with the inner cover point cloud, so that:

[0089]

[0090] where t mt is a 3x1 translation matrix, which can be calculated by the formula described in step 3.4. Thus, the rough transformation matrix of the template point cloud to the scene point cloud is constructed, which is defined as Tmt, that is:

[0091]

[0092] In this embodiment, the rough transformation matrix Tmt is:

[0093]

[0094] Step 4: Based on the rough matching described in step 3, the handle main direction of the template point cloud is aligned to the handle main direction of the inner cover point cloud, and fine registration is performed to calculate the pose of the oil cover relative to the 3D camera, and the specific steps are as follows:

[0095] Step 4.1: After step 3, the oiling inner cover bottom of the template point cloud and the inner cover point cloud has been approximately in the same position. Based on the (Xtc, Ytc, Ztc), (Xtn, Ytn, Ztn) described in step 3.3, the handle of the inner cover point cloud is extracted (remove the oiling inner cover bottom part, only leave the handle), and the principal direction vector of the handle of the inner cover point cloud is calculated according to the principal component analysis algorithm, defined as (Xth, Yth, Zth). In this embodiment, (Xth, Yth, Zth) = (0.694146, -0.716421, 0.0700167).

[0096] Step 4.2: According to the rotation matrix Rmt described in step 3.4, the handle principal direction vector (Xmh, Ymh, Zmh) of the template point cloud is rotated to obtain the handle principal direction vector (Xmth, Ymth, Zmth) of the template point cloud after coarse registration conversion, that is:

[0097]

[0098] In this embodiment, (Xmh, Ymh, Zmh) = (0.98917, -0.185343, -0.00783353), (Xmth, Ymth, Zmth) = (0.978559, -0.18364, 0.146868).

[0099] Step 4.3: According to the Rodrigues formula, the direction vector (Xmth, Ymth, Zmth) is aligned to the direction vector (Xth, Yth, Zth), and the obtained rotation matrix is R fine , which is the rotation matrix obtained by fine registration. According to R fine , the handle of the template point cloud is aligned with the handle of the inner cover point cloud. The oiling inner cover bottom center of the rotated template point cloud is aligned with the oiling inner cover bottom center of the inner cover point cloud, and the translation matrix T

[0100]

[0101] Therefore, the fine registration matrix T fine is constructed, that is:

[0102]

[0103] In this embodiment:

[0104]

[0105] As shown in the reference Figure 7 , the effect of the two-stage matching from coarse to fine of the oiling inner cover is shown, where green is the template point cloud and red is the inner cover point cloud to be matched.

[0106] Step 4.4: Obtain the position of the handle center point of the oiling inner cover in the camera coordinate system from the T mt and T fine Obtain the position of the handle center point of the oiling inner cover in the camera coordinate system from the T

[0107]

[0108] Wherein, (Xm, Ym, Zm) is the handle surface center point position obtained in advance from the template point cloud. (Xt, Yt, Zt) is the handle center point position of the oiling inner cover in the inner cover point cloud obtained by matching the template point cloud. In this embodiment, (Xm, Ym, Zm) = (-7.483612, 53.156475, 368.88894), (Xt, Yt, Zt) = (59.525, 40.9622, 474.235).

[0109] Step 4.5: In order to represent the orientation of the oiling inner cover, based on the (Xt, Yt, Zt) center positioning point (i.e. the center point), the system oiling inner cover grasping coordinate system definition scheme (the system oiling inner cover grasping coordinate system definition scheme uses prior art) is used to define the oiling inner cover grasping direction (the grasping direction corresponds to the orientation), and the pose matrix representation of the oiling inner cover in the 3D camera coordinate system is obtained, which is defined as Tigcr:

[0110]

[0111] Wherein, the first three columns of the matrix represent the representation of the grasping coordinate system direction on the oiling inner cover in the camera coordinate system.

[0112] In this embodiment:

[0113]

[0114] Step 5: In order to solve the problem of insufficient accuracy caused by matching most of the oiling inner covers with a small number of template libraries, and the problem of reduced matching accuracy caused by the 3D camera shooting position, a positioning pose correction scheme is proposed to obtain the accurate pose of the oiling inner cover relative to the 3D camera, which includes the following steps:

[0115] Step 5.1: Due to the fact that the shape of the oiling inner cover in the template and the scene to be registered is only similar, and due to factors such as the inclination of the camera shooting angle, the oiling inner cover scene collected is incomplete, etc., the above (Xt, Yt, Zt) center point position is only a relatively rough positioning point obtained by registration. According to the registration result, the positioning point needs to be corrected in the direction perpendicular to the handle, i.e. the grasping X direction. Align the grasping coordinate system X direction (Xix, Yix, Zix) to the camera X direction (1, 0, 0), calculate the rotation matrix Rgx according to the Rodrigues formula, and the rough center point after rotation is:

[0116]

[0117] In this embodiment, (X′ t ,Y′ t ,Z′ t )=(41.4874,1.05536,477.909)。

[0118] Step 5.2: Create an inner cover point cloud bounding box, align the inner cover point cloud to the 3D camera Z direction, and extract the handle point cloud of the inner cover point cloud based on the center point (Xt, Yt, Zt), and cut a rectangular point cloud from the handle point cloud, perform plane fitting on the rectangular point cloud, obtain the plane equation ax+by+cz+d=0, project the handle point cloud to this plane and perform point cloud down-sampling.

[0119] Step 5.3: Based on the rotation matrix described in step 5.1, rotate the down-sampled handle point cloud described in step 5.2, calculate the mean value X″ t of the X coordinate of the rotated handle point cloud X′ t . Correct X′ t with X″ t . After the calculation is completed, rotate the corrected handle center point back to the original space to obtain the final accurate positioning point (Xia, Yia, Zia), that is:

[0120]

[0121] In this embodiment, X″ t =43.0041, (Xia, Yia, Zia)=(60.6292, 41.9976, 474.139).

[0122] Step 5.4: Based on steps 5.1, 5.2, and 5.3, obtain the pose matrix representation of the oil filling inner cover in the camera (all cameras of the present application refer to 3D cameras) coordinate system, that is, the accurate pose matrix Tigc of the oil filling inner cover relative to the 3D camera:

[0123]

[0124] As shown in the reference Figure 8 , the pose visualization result of the oil filling inner cover in the camera coordinate system, wherein the blue direction line is the inner cover grabbing Z direction, the red direction line is the inner cover grabbing X direction, and the pink direction line is the inner cover grabbing Y direction. The red point is the inner cover handle center point obtained by template matching, and the blue point is the corrected inner cover handle center point.

[0125] In this embodiment:

[0126]

[0127] Step 6: The accurate pose matrix of the fuel cap in the camera coordinate system described in steps 2 and 5 is transformed to the robot base coordinate system by the coordinate transformation relationship in the system, so as to realize the accurate perception of the fuel cap by the automatic fueling robot, including the following steps:

[0128] Step 6.1: According to the coordinate transformation relationship Tocb of the fuel cap in the system, the accurate pose matrix Togb of the fuel cap in the robot base coordinate system is obtained, wherein T ogb =T ocb *T ogc ; according to the coordinate transformation relationship Ticb of the fuel cap in the system, the accurate pose matrix Togb of the fuel cap in the robot base coordinate system is obtained, wherein T igb =T icb *T igc .

[0129] In the embodiment, the accurate perception algorithm for the fuel cap of the automatic fueling robot is obtained by fusing 3D and 2D vision.

[0130]

[0131]

[0132]

[0133]

[0134] In summary, the application discloses an accurate perception algorithm for a fuel cap of an automatic fueling robot, which is suitable for fusing 3D and 2D vision and can be applied to the technical field of automatic fueling of fuel vehicles. For the fuel cap, a coarse-to-fine positioning scheme using 2D rough positioning and 3D accurate positioning is proposed, the surface normal of the scene point cloud of the fuel cap is calculated, the point cloud is projected as a 2D image based on the normal for rough positioning of the cap, and the cap is further segmented for accurate positioning by using the rough positioning points. For the fuel cap, a coarse-to-fine template matching scheme is proposed, the edge key points of the point cloud are extracted, and the rough pose estimation is performed in combination with the geometric features of the fuel cap; on the basis of rough registration, the handle of the template point cloud and the handle of the cap point cloud are aligned for fine registration (for reference Figure 9 , a single template is used to match different types of fuel caps, and good matching effect is obtained, wherein the green point cloud is a template point cloud, and the red point cloud is a scene point cloud to be matched); finally, the positioning pose is corrected to calculate the accurate positioning pose of the fuel cap (for reference Figure 10 , a single template is used to match different types of fuel caps, and good matching effect is obtained, wherein the green point cloud is a template point cloud, and the red point cloud is a scene point cloud to be matched); finally, the positioning pose is corrected to calculate the accurate positioning pose of the fuel cap (for reference Figure 9), avoid building a large template library, and do not require high computing power.

[0135] Embodiment 2

[0136] A fuel cap precise perception device suitable for an automatic fueling robot, which can implement the algorithm of embodiment 1, comprising:

[0137] A point cloud data acquisition module is configured to acquire fuel outer cover scene point cloud data, fuel inner cover scene point cloud data, and template point cloud data.

[0138] The fuel outer cover positioning module comprises a fuel outer cover scene point cloud preprocessing unit, which is configured to perform outlier filtering to remove noise points, reduce sampling, and smooth point cloud processing on the fuel outer cover scene point cloud, and acquire the normalized normal direction of each point in the fuel outer cover scene point cloud; a 2D image conversion unit configured to calculate the 2D image pixel position corresponding to the fuel outer cover scene points, and calculate the pixel value of the 2D image using the normalized normal direction of each point in the fuel outer cover scene point cloud; a fuel outer cover rough positioning point calculation unit configured to obtain the coordinates of the fuel outer cover rough positioning point on the 2D image, and convert them into the coordinates in the corresponding fuel outer cover scene point cloud; and a fuel outer cover precise point calculation unit configured to extract the outer cover point cloud from the fuel outer cover scene point cloud, and calculate the centroid of the outer cover point cloud; and a fuel outer cover pose positioning unit configured to calculate the accurate pose matrix of the fuel outer cover relative to the 3D camera according to the outer cover point cloud and the centroid of the outer cover point cloud, and transform the pose matrix to the robot base coordinate system.

[0139] The fuel inner cover positioning module comprises a fuel inner cover template library unit configured to store the fuel inner cover template; a fuel inner cover point cloud extraction unit configured to extract the inner cover point cloud from the fuel inner cover scene point cloud; a fuel inner cover rough matching unit configured to calculate the rough transformation matrix of the template point cloud to the inner cover point cloud according to the inner cover point cloud and the template point cloud; a fuel inner cover fine matching unit configured to calculate the fine registration matrix according to the inner cover point cloud and the template point cloud; a fuel inner cover pose positioning unit configured to calculate the pose matrix of the fuel inner cover in the 3D camera coordinate system according to the rough transformation matrix and the fine registration matrix; a fuel inner cover pose correction unit configured to correct the pose matrix of the fuel inner cover in the 3D camera coordinate system to obtain the accurate pose matrix of the fuel inner cover relative to the 3D camera; and a fuel inner cover pose matrix conversion unit configured to transform the accurate pose matrix of the fuel inner cover relative to the 3D camera to the robot base coordinate system.

Claims

1. A precise sensing algorithm for refueling caps suitable for automated refueling robots, characterized in that, Includes the following steps: Step 1: Collect point cloud data of the refueling cover scene using a 3D camera, obtain a 2D image based on the projection of the point cloud normals, and then obtain the coordinates of the rough positioning point of the refueling cover. Step 2: Segment the outer cover based on the rough positioning points of the refueling cover, obtain the precise positioning points of the refueling cover, and determine the pose of the refueling cover relative to the 3D camera; Step 3: Establish a fuel filler cap template library and obtain template point clouds; collect fuel filler cap scene point cloud data with a 3D camera, extract the inner cap point cloud from the fuel filler cap scene point cloud data, and extract edge key points from the inner cap point cloud and template point cloud, and perform a rough matching of template point cloud and inner cap point cloud based on geometric features. Step 4: Based on the coarse matching described in Step 3, align the main direction of the template point cloud handle to the main direction of the inner cover point cloud handle, perform fine registration, and calculate the pose of the refueling inner cover relative to the 3D camera. Step 5: Propose a positioning and pose correction scheme to obtain the precise pose of the inner refueling cover relative to the 3D camera; Step 6: Using the coordinate transformation relationship within the system, transform the precise pose matrix of the outer and inner refueling covers in the camera coordinate system to the robot base coordinate system; In step 1, the method for obtaining the rough coordinates of the refueling cover's location point is as follows: Based on the difference between the refueling cover and other pixel points in the 2D image, the projected 2D image is binarized and morphological operations are performed to remove large-area noise and retain the complete image of the refueling cover; several rows of pixels in the image are randomly selected, the pixel position with a pixel value of 255 is found, and the center of the found position in each row is taken. The average of the center positions found in several rows is taken to obtain Xox, which is the X coordinate of the rough location point of the refueling cover in the 2D image. Similarly, the coordinates Yoy of the rough location point of the refueling cover in the Y direction of the 2D image are obtained. (Xox, Yoy) is mapped to the 3D refueling cover scene point cloud to obtain the rough location point (Xor, Yor) of the refueling cover that does not include the Z dimension. In step 2, the method for obtaining the precise positioning point of the refueling cover is as follows: the point cloud data of the refueling cover scene is divided into two parts based on region growing segmentation; a threshold β is set, and the existence of the (Xor±β,Yor±β) region in the segmented point set is checked. If it does not exist, this part is not the outer cover point cloud; if it exists, this part is the outer cover point cloud, and the centroid of the outer cover point cloud is calculated, which is the precise positioning point (Xoa,Yoa,Zoa) of the refueling cover.

2. The precise sensing algorithm for refueling caps for automated refueling robots as described in claim 1, characterized in that, In step 1, the method for obtaining the 2D image is as follows: outlier filtering is performed on the point cloud of the refueling cover scene to remove noise, then the point cloud of the refueling cover scene is downsampled, and then the moving least squares method is used to smooth the point cloud of the refueling cover scene. A KD-tree is used to quickly search for the nearest neighbors of each point in the point cloud of the refueling cover scene. The local surface of the nearest neighbors is fitted to estimate the normal vector, and the normal vector estimation result is used as the normal direction of that point. Then, the normal is normalized. Next, the boundary information of the refueling cover scene point cloud is calculated to obtain the minimum and maximum values ​​of the point cloud in the X and Y dimensions, namely Xmin, Ymin, Xmax, and Ymax. The coordinates (Xmin, Ymin) of the 3D refueling cover scene point cloud in the X and Y dimensions are mapped to the position of the 2D image pixel (0, 0), and the coordinates (Xmax, Ymax) of the refueling cover scene point cloud in the X and Y dimensions are mapped to the coordinates of the lower right position of the 2D image. Based on this, the positional correspondence between each point of the refueling cover scene point cloud and each pixel of the 2D image is established. The normalized normal direction (Xn, Yn, Zn) (0≤Xn, Yn, Zn≤1) at each point of the refueling cover scene point cloud is multiplied by 255, and the product is used as the pixel value of the R, G, and B channels of the 2D image.

3. The precise sensing algorithm for refueling caps for automated refueling robots as described in claim 2, characterized in that, In step 2, the method for locating the pose of the refueling cover relative to the 3D camera is as follows: Plane fitting is performed on the point cloud of the cover to obtain the plane parameters (Ao, Bo, Co, Do). The first three components of the plane parameters are the coordinates (Xoz, Yoz, Zoz) of the Z-axis of the refueling cover's capture coordinate system in the 3D camera coordinate system. (Xoz, Yoz, Zoz) are aligned with the 3D camera's Z-axis (0, 0, 1), and the Rodriguez formula is applied to construct the rotation matrix Roc, resulting in the following relationship: Based on matrix operations, the inverse of the Roc matrix is ​​calculated to obtain the coordinates (Xox, Yox, Zox) and (Xoy, Yoy, Zoy) of the refueling cover gripping coordinate system in the 3D camera coordinate system. This leads to the matrix representation of the refueling cover gripping coordinate system in the 3D camera coordinate system, i.e., the precise pose matrix Togc of the refueling cover relative to the 3D camera. The position and orientation of the refueling cover in the camera coordinate system are obtained.

4. The precise sensing algorithm for refueling caps for automated refueling robots as described in any one of claims 1-3, characterized in that, In step 3, the method for extracting edge key points is as follows: the internal shape descriptor algorithm is used to effectively extract the handle edge key points and the bottom circular surface edge key points of the inner cover point cloud and the template point cloud. The rough matching method between the template point cloud and the inner cover point cloud is as follows: Perform 3D spatial circle fitting on the edge key points of the inner cover point cloud and the template point cloud to obtain the center (Xtc, Ytc, Ztc) and normal direction (Xtn, Ytn, Ztn) of the fitted 3D circle of the inner cover bottom surface in the inner cover point cloud, and obtain the center (Xmc, Ymc, Zmc) and normal direction (Xmn, Ymn, Zmn) of the 3D circle of the template point cloud inner cover bottom surface; based on the normal directions (Xtn, Ytn, Ztn) and (Xmn, Ymn, Zmn), calculate the rotation matrix Rmt using the Rodriguez formula, and align the template point cloud with the bottom surface of the inner cover in the inner cover point cloud, thus: Among them, t mt It is a 3x1 translation matrix, from which a coarse transformation matrix, Tmt, is constructed from the template point cloud to the inner cover point cloud:

5. The precise sensing algorithm for refueling caps for automated refueling robots as described in claim 4, characterized in that, In step 4, the process of aligning the main direction of the template point cloud handle to the main direction of the inner cover point cloud handle, performing fine registration, and calculating the pose of the refueling inner cover relative to the 3D camera includes: Step 4.1: Perform conditional filtering based on the (Xtn,Ytn,Ztn) normal and the (Xtc,Ytc,Ztc) circle center to extract the handle of the inner cover point cloud; and calculate the main direction vector of the handle of the extracted inner cover point cloud using the principal component analysis algorithm, defined as (Xth,Yth,Zth); similarly, obtain the main direction vector of the handle (Xmh,Ymh,Zmh) of the template point cloud; Step 4.2: Rotate the handle principal direction vector (Xmh, Ymh, Zmh) to obtain the handle principal direction vector (Xmth, Ymth, Zmth) of the coarsely registered template point cloud, i.e.: Step 4.3: According to Rodriguez's formula, align the direction vectors (Xmth, Ymth, Zmth) to the direction vectors (Xth, Yth, Zth) to obtain the rotation matrix R. fine According to R fine Align the handle of the template point cloud with the handle of the inner cover point cloud; align the center of the bottom surface of the inner cover in the rotated template point cloud with the center of the bottom surface of the inner cover in the inner cover point cloud, and calculate the translation moment obtained from the fine registration, i.e.: Constructing the fine registration matrix T fine :

6. The precise sensing algorithm for refueling caps for automated refueling robots as described in claim 5, characterized in that, In step 4, the process of aligning the main direction of the template point cloud handle to the main direction of the inner cover point cloud handle for fine registration and calculating the pose of the refueling inner cover relative to the 3D camera further includes: Step 4.4: Calculate the position of the handle center point in the camera coordinate system within the inner cover point cloud: Where (Xm,Ym,Zm) are the positions of the center points of the handle surface obtained in advance from the template point cloud; (Xt,Yt,Zt) are the positions of the center points of the handle in the inner cover point cloud; Step 4.5: Based on the center point (Xt, Yt, Zt), define the coordinate system for grasping the refueling cap within the system, define the grasping direction of the refueling cap, and obtain the pose matrix representation of the refueling cap in the camera coordinate system, defined as Tigcr: The first three columns of the matrix represent the orientation of the grabbing coordinate system on the inner refueling cover in the camera coordinate system, which is obtained from the system's inner refueling cover grabbing coordinate system definition scheme.

7. The precise sensing algorithm for refueling caps for automated refueling robots as described in claim 6, characterized in that, In step 5, the method for obtaining the precise pose of the refueling inner cover relative to the 3D camera includes the following steps: Step 5.1: Align the X-axis (Xix, Yix, Zix) of the grabbing coordinate system to the X-axis (1, 0, 0) of the 3D camera. Calculate the rotation matrix Rgx according to the Rodriguez formula. The approximate center point after rotation is: Step 5.2: Create a bounding box for the inner cover point cloud, align the inner cover point cloud with the camera's Z direction, and extract the handle point cloud from the inner cover point cloud based on the center point (Xt,Yt,Zt). Cut out a rectangular point cloud from the handle point cloud, perform plane fitting on the rectangular point cloud, and obtain the plane equation ax+by+cz+d=0. Project the handle point cloud onto this plane and perform point cloud downsampling. Step 5.3: Rotate the handle point cloud and calculate the mean X-coordinate X″ of the rotated handle point cloud. t , using X″ t Correction X′ t Rotate the corrected handle center point back to its original position to obtain the final precise positioning point (Xia, Yia, Zia), that is: Step 5.4: Obtain the precise pose matrix Tigc of the refueling inner cover relative to the 3D camera:

8. A precise sensing device for a refueling cap suitable for an automated refueling robot, capable of implementing the algorithm described in any one of claims 1-7, characterized in that, include: The point cloud data acquisition module is used to acquire point cloud data for the refueling outer cover scenario, the refueling inner cover scenario, and template point cloud data; The refueling cover positioning module includes a refueling cover scene point cloud preprocessing unit, which performs outlier filtering to remove noise, downsampling, and smoothing on the refueling cover scene point cloud, and obtains the normalized normal direction of each point in the refueling cover scene point cloud; a 2D image conversion unit, which calculates the 2D image pixel position corresponding to the refueling cover scene point, and calculates the pixel value of the 2D image using the normalized normal direction of each point in the cover scene point cloud; and a refueling cover coarse positioning point calculation unit, which obtains the coordinates of the refueling cover coarse positioning point on the 2D image and converts them into the corresponding coordinates in the refueling cover scene point cloud. The precise positioning calculation unit for the refueling cover is used to extract the cover point cloud from the scene point cloud and calculate the centroid of the cover point cloud. The refueling cover pose localization unit is used to calculate the precise pose matrix of the refueling cover relative to the 3D camera based on the point cloud of the cover and the centroid of the point cloud of the cover, and transform this pose matrix into the robot base coordinate system. Fuel filler cap positioning module: includes a fuel filler cap template library unit for storing fuel filler cap templates; The refueling inner cover point cloud extraction unit is used to extract the inner cover point cloud from the scene point cloud of the refueling inner cover; The inner cover coarse matching unit is used to calculate the coarse transformation matrix from the template point cloud to the inner cover point cloud based on the inner cover point cloud and the template point cloud. The inner cover fine matching unit is used to finely register the matrix based on the inner cover point cloud and the template point cloud; The inner cap pose positioning unit is used to calculate the pose matrix of the inner cap in the 3D camera coordinate system based on the coarse transformation matrix and the fine registration matrix. The inner refueling cover pose correction unit is used to correct the pose matrix of the inner refueling cover in the 3D camera coordinate system to obtain the accurate pose matrix of the inner refueling cover relative to the 3D camera; the inner refueling cover pose matrix transformation unit is used to transform the accurate pose matrix of the inner refueling cover relative to the 3D camera to the robot base coordinate system.

Images