Charging pile pose recognition method, device and equipment based on point cloud template matching

Abstract

The invention provides a charging pile pose recognition method, device and equipment based on point cloud template matching. The method comprises the steps of obtaining initial three-dimensional point cloud data in an area where a target charging pile is located; performing filtering preprocessing on each frame of initial three-dimensional point cloud data subjected to continuous scanning to obtain preprocessed three-dimensional point cloud data; performing classification detection on the preprocessed three-dimensional point cloud data to detect target charging pile three-dimensional point cloud data in each frame of initial three-dimensional point cloud data; and performing point cloud registration on the identified target charging pile three-dimensional point cloud data of each frame and a pre-constructed charging pile point cloud template to obtain a registration result corresponding to each frame, and determining the registration result as charging pile pose data. According to the invention, the accuracy of pose recognition is greatly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic charging, and in particular, to a method, device and equipment for identifying the pose of a charging pile based on point cloud template matching. Background Art

[0002] The autonomous recharging technology of mobile robots is an important technology in the field of intelligent robots, which enables robots to automatically find a charging pile for charging when the battery level is low, so as to achieve long-term and high-efficiency autonomous operation. In this process, the mobile robot needs to accurately identify the position and pose of the charging pile for subsequent autonomous docking charging operations.

[0003] Most of the existing technologies use single-line lidar. The single-line lidar can only provide data of fixed distances on a two-dimensional plane and cannot obtain the complete three-dimensional structure information of the charging pile. This results in low accuracy in pose estimation, especially when the pose of the charging pile changes greatly, it is difficult to provide high-precision pose estimation. In addition, due to the small amount of feature information obtained by the single-line lidar, the identification of the charging pile is easily affected by obstacles and environmental noise, resulting in inaccurate identification results. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a method, device and equipment for identifying the pose of a charging pile based on point cloud template matching to improve the accuracy of charging pile pose identification.

[0005] In a first aspect, a method for identifying the pose of a charging pile based on point cloud template matching is provided, which is applied to a robot to be charged. The method includes:

[0006] Obtain initial three-dimensional point cloud data in the area where the target charging pile is located; the initial three-dimensional point cloud data includes the three-dimensional coordinate data of the point cloud of the charging pile and other objects around it, and is continuously scanned by a multi-line lidar installed at the front end of the robot to be charged at a preset frame rate;

[0007] Perform filtering preprocessing on each frame of the continuously scanned initial three-dimensional point cloud data to obtain preprocessed three-dimensional point cloud data;

[0008] Perform classification detection on the preprocessed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of the initial three-dimensional point cloud data;

[0009] Perform point cloud registration on the three-dimensional point cloud data of the target charging pile identified in each frame with a pre-constructed charging pile point cloud template to obtain the corresponding registration result for each frame, and determine the registration result as the charging pile pose data.

[0010] Optionally, performing filtering preprocessing on each frame of the continuously scanned initial three-dimensional point cloud data to obtain preprocessed three-dimensional point cloud data includes:

[0011] For each frame of initial three-dimensional point cloud data in continuous scanning, perform a pass-through filter to filter out the three-dimensional point cloud outside the preset height;

[0012] Perform a plane filter on the three-dimensional point cloud after the pass-through filter to eliminate the point cloud of the plane occluder behind the charging pile; among them, the plane occluder is at least a wall;

[0013] Perform a radius filter on the three-dimensional point cloud after the plane filter to filter out isolated point clouds and abnormal point clouds, and obtain preprocessed three-dimensional point cloud data.

[0014] Optionally, performing a plane filter on the three-dimensional point cloud after the pass-through filter to eliminate the point cloud of the plane occluder behind the charging pile includes:

[0015] Randomly select a preset number of point clouds in the three-dimensional point cloud after the pass-through filter, and the preset number is greater than or equal to 3;

[0016] Fit the plane occluder behind the charging pile based on the three-dimensional coordinate data of the preset number of point clouds to obtain a fitted plane; among them, the expression of the fitted plane is:

[0017] Ax + By + Cz + D = 0

[0018] Among them, (A, B, C) is the normal vector of the plane, D is a constant, and (x, y, z) is the three-dimensional coordinate data of the selected point cloud;

[0019] Calculate the distance between the remaining three-dimensional point cloud after random selection and the fitted plane, and determine the inliers of the fitted plane based on the distance; the inliers refer to the points belonging to the fitted plane;

[0020] Repeat the above fitting process for multiple iterations to obtain multiple fitted planes and their inliers, and use the fitted plane with the largest number of inliers as the target fitted plane;

[0021] Filter out the inliers on the target fitted plane to eliminate the point cloud of the plane occluder behind the charging pile.

[0022] Optionally, performing classification detection on the preprocessed three-dimensional point cloud data to detect the target charging pile three-dimensional point cloud data in each frame of initial three-dimensional point cloud data includes:

[0023] Use the Euclidean clustering algorithm to perform classification detection on the preprocessed three-dimensional point cloud data to obtain candidate charging pile three-dimensional point cloud data;

[0024] Use a preset screening rule to screen the candidate charging pile three-dimensional point cloud data to obtain the target charging pile three-dimensional point cloud data in each frame of initial three-dimensional point cloud data, and the preset screening rule is: screen according to the point cloud quantity threshold of the preset charging pile and the size threshold of the preset charging pile.

[0025] Optionally, the process of constructing the charging pile point cloud template includes:

[0026] Cropping the three-dimensional point cloud data of any one frame scanned by the multi-line lidar according to the size threshold of the preset charging pile to obtain the initial charging pile point cloud template;

[0027] Calculating the centroid of the initial charging pile point cloud template;

[0028] Moving the centroid of the initial charging pile point cloud template to the origin of the multi-line lidar coordinate system to obtain the charging pile point cloud template.

[0029] Optionally, the process of performing point cloud registration on the three-dimensional point cloud data of the target charging pile identified in each frame and the pre-constructed charging pile point cloud template includes:

[0030] For each point in the charging pile point cloud template, find the point with the closest distance to it in the three-dimensional point cloud of the target charging pile to form a set of point pairs;

[0031] Determining the rotation matrix and translation vector of the charging pile point cloud template relative to the three-dimensional point cloud of the target charging pile based on the point pairs in the set of point pairs, and taking the rotation matrix and translation vector as the registration result;

[0032] Updating the coordinate data of each point cloud in the charging pile point cloud template based on the rotation matrix and translation vector, and calculating the error function;

[0033] Repeating the above registration process based on the coordinate data of each point cloud in the updated charging pile point cloud template until the preset convergence condition is reached; the preset convergence condition is at least one of the following: the number of iterations reaches the maximum value, the change in the error function is less than the preset error threshold, and the change in the rotation matrix and translation vector is less than the preset threshold;

[0034] Calculating the registration score of the finally converged registration result based on the preset registration score calculation formula; the preset registration score calculation formula is:

[0035]

[0036] where, R is the rotation matrix; t is the translation vector; (s i , t i ) is the i-th point pair; N is the number of point pairs; s i is the point cloud in the charging pile point cloud template; t i is the point cloud in the three-dimensional point cloud data of the target charging pile;

[0037] If the registration score is greater than the preset score threshold, then determine the finally converged registration result as the pose data of the target charging pile.

[0038] Optionally, the method further includes:

[0039] Smoothing processing is performed on the registration results of each frame using a sliding window with a preset length, and the smoothing processing includes the following steps:

[0040] Calculate the average registration result of the registration results of all frames within the sliding window with the preset length;

[0041] Calculate the difference between the registration result of each frame within the sliding window and the average registration result;

[0042] If the difference is less than the preset registration threshold, the registration is valid, otherwise it is invalid;

[0043] Calculate the average value of all valid registration results and determine this average value as the final registration result.

[0044] In a second aspect, a charging pile pose recognition device based on point cloud template matching is provided, which is applied to a robot to be charged. The device includes:

[0045] An acquisition unit, configured to acquire initial three-dimensional point cloud data within the area where the target charging pile is located; the initial three-dimensional point cloud data includes the three-dimensional coordinate data of the point cloud of the charging pile and other objects around it, and is continuously scanned by a multi-line lidar installed at the front end of the robot to be charged at a preset frame rate;

[0046] A preprocessing unit, configured to perform filtering preprocessing on each frame of the continuously scanned initial three-dimensional point cloud data to obtain preprocessed three-dimensional point cloud data;

[0047] A classification unit, configured to perform classification detection on the preprocessed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of the initial three-dimensional point cloud data;

[0048] A registration unit, configured to perform point cloud registration on the three-dimensional point cloud data of the target charging pile identified in each frame with a pre-constructed charging pile point cloud template to obtain the corresponding registration result of each frame, and determine this registration result as the charging pile pose data.

[0049] In a third aspect, an electronic device is provided, including a processor, a communication interface, a memory, and a communication bus. Among them, the processor, the communication interface, and the memory complete mutual communication through the communication bus;

[0050] The memory is used to store a computer program;

[0051] The processor, when executing the program stored on the memory, implements the method steps described in any one of the first aspect.

[0052] In a fourth aspect, a computer-readable storage medium is provided. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method steps described in any one of the first aspect are implemented.

[0053] A method, device and equipment for identifying the pose of a charging pile based on point cloud template matching provided by the present invention obtain initial three-dimensional point cloud data within the area where the target charging pile is located; perform filtering preprocessing on each frame of the continuously scanned initial three-dimensional point cloud data to obtain preprocessed three-dimensional point cloud data; perform classification detection on the preprocessed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of the initial three-dimensional point cloud data; perform point cloud registration on the three-dimensional point cloud data of the target charging pile identified in each frame with a pre-constructed charging pile point cloud template to obtain the corresponding registration result for each frame, and determine this registration result as the pose data of the charging pile. The present invention obtains the three-dimensional point cloud data of the charging pile through multi-line lidar scanning, provides rich feature data for pose recognition, reduces the interference of other obstacles, etc. through filtering preprocessing, improves the accuracy of pose recognition, and finally obtains the pose data of the charging pile through point cloud registration with the pre-constructed charging pile point cloud template, greatly improving the accuracy of pose recognition.

[0054] In order to make the above objects, features and advantages of the present invention more obvious and understandable, the following specifically enumerates preferred embodiments and, in conjunction with the accompanying drawings, makes a detailed description as follows. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required to be used in the embodiments. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other related drawings can also be obtained based on these drawings.

[0056] Figure 1 Shows the flowchart of a method for identifying the pose of a charging pile based on point cloud template matching provided by an embodiment of the present invention;

[0057] Figure 2 Shows the structural schematic diagram of a device for identifying the pose of a charging pile based on point cloud template matching provided by an embodiment of the present invention;

[0058] Figure 3 Shows the structural schematic diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0059] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Usually, the components of the embodiments of the present invention described and illustrated in the accompanying drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed present invention, but merely represents the selected embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0060] Considering that most of the existing technologies use single-line lidar, the single-line lidar can only provide data on fixed distances in a two-dimensional plane and cannot obtain the complete three-dimensional structural information of the charging pile. This results in low accuracy in pose estimation, especially when the pose of the charging pile changes greatly, it is difficult to provide high-precision pose estimation. In addition, due to the small amount of feature information obtained by the single-line lidar, the recognition of the charging pile is easily affected by obstacles and environmental noise, resulting in inaccurate recognition results.

[0061] Based on this, the embodiments of the present invention provide a method and device for identifying the pose of a charging pile based on point cloud template matching, which are applied to the scenario of autonomous charging of a mobile robot to be charged. When the mobile robot has insufficient power, it autonomously searches for a charging pile to charge. The three-dimensional coordinate data of the charging pile is scanned by a multi-line lidar installed at its front end, and the pose data of the charging pile is identified based on the scanned three-dimensional coordinate data. The following is described through embodiments.

[0062] The embodiments of the present invention provide a method for identifying the pose of a charging pile based on point cloud template matching. The execution subject of this method is a mobile robot to be charged, such as Figure 1 shown, the method includes the following steps:

[0063] Step S101: Obtain the initial three-dimensional point cloud data within the area where the target charging pile is located.

[0064] In this step, the initial three-dimensional point cloud data includes the three-dimensional coordinate data of the point cloud of the charging pile and other objects around it (such as the electrical box next to the charging pile, the wall behind the charging pile, etc.), and is continuously scanned by a multi-line lidar installed at the front end of the mobile robot to be charged at a preset frame rate.

[0065] In one example, the preset frame rate of the multi-line lidar is 10HZ, that is, the multi-line lidar can scan 10 frames of three-dimensional point cloud data per second, and each frame of three-dimensional point cloud data includes a complete point cloud data set within the area where the target charging pile is located.

[0066] The coordinate information on the three-dimensional plane of the charging pile can be obtained through a multi-line lidar, providing rich feature information for the subsequent pose recognition of the charging pile. Even when the pose of the charging pile changes greatly, the recognition accuracy can be guaranteed.

[0067] Step S102: Perform filtering preprocessing on each frame of the initially scanned three-dimensional point cloud data to obtain preprocessed three-dimensional point cloud data.

[0068] In this step, since there are many obstacles and interferences around the charging pile, filtering preprocessing can eliminate the interferences, thereby helping to improve the accuracy of the charging pile pose recognition.

[0069] The specific process of the filtering preprocessing will be described in detail in the following embodiments and will not be elaborated here.

[0070] Step S103: Classify and detect the preprocessed three-dimensional point cloud data to detect the target charging pile three-dimensional point cloud data in each frame of the initially scanned three-dimensional point cloud data.

[0071] In this step, since the scanning range of the multi-line lidar includes not only the charging pile but also the surrounding objects, such as the electrical box beside it, etc., it is necessary to identify the three-dimensional point cloud data that only includes the charging pile from various objects, so as to provide accurate charging pile point cloud data for the subsequent point cloud registration process, reduce the computational amount of the point cloud registration, and improve the efficiency of the point cloud registration at the same time.

[0072] Among them, the specific means of the classification and detection will be described in the following embodiments and will not be elaborated here.

[0073] Step S104: Perform point cloud registration on the target charging pile three-dimensional point cloud data identified in each frame with the pre-constructed charging pile point cloud template to obtain the corresponding registration result for each frame, and determine this registration result as the charging pile pose data.

[0074] In the embodiment of the present invention, the construction process of the charging pile point cloud template includes:

[0075] Step S104A; Crop the three-dimensional point cloud data of any one frame scanned by the multi-line lidar according to the preset size threshold of the charging pile to obtain the initial charging pile point cloud template.

[0076] In this step, the construction process of the point cloud template can be an offline operation. Specifically, one frame can be randomly selected from multiple frames of data scanned by the mobile robot for the first time through the multi-line lidar. The three-dimensional point cloud data of this frame represents the position of the current spatial object. In one example, it is represented by x, y, and z in the relative lidar coordinate system.

[0077] The preset size threshold of the charging pile refers to setting minimum and maximum values for x, y, and z respectively, and cropping the three-dimensional point cloud data of the current frame to obtain the point cloud of the charging pile, while excluding the point clouds of other objects.

[0078] Step S104B: Calculate the centroid of the initial charging pile point cloud template.

[0079] In a feasible implementation, the coordinates of the centroid C can be calculated by the average of the coordinates of all points in the point cloud template.

[0080] Step S104C: Move the centroid of the initial charging pile point cloud template to the origin of the multi-line lidar coordinate system to obtain the charging pile point cloud template.

[0081] In this step, by moving the centroid of the initial charging pile point cloud template to the origin of the multi-line lidar coordinate system, the position offset of the charging pile point cloud template relative to the lidar is eliminated, avoiding coordinate conversion during subsequent registration of the charging pile point cloud template and the three-dimensional point cloud of the target charging pile, reducing the subsequent calculation amount, and simplifying the subsequent operations.

[0082] In this step, the pose data of the charging pile includes the position and attitude data of the charging pile.

[0083] On the one hand, point cloud registration reduces the error caused by the movement of the robot. On the other hand, through multiple registrations and scans, point cloud data with higher resolution can be obtained compared to single registration and scan.

[0084] The present invention obtains the three-dimensional point cloud data of the charging pile through multi-line lidar scanning, provides rich feature data for pose recognition, reduces the interference of other obstacles, etc. through filtering preprocessing, improves the accuracy of pose recognition, and finally obtains the pose data of the charging pile through point cloud registration with the pre-constructed charging pile point cloud template, greatly improving the accuracy of pose recognition.

[0085] Based on the above embodiments, the filtering preprocessing of each frame of initial three-dimensional point cloud data for continuous scanning to obtain the preprocessed three-dimensional point cloud data includes:

[0086] Step S102A: Perform a pass-through filter on each frame of initial three-dimensional point cloud data for continuous scanning to filter out the three-dimensional point cloud outside the preset height.

[0087] In this step, since the charging pile is placed on the ground and has a specific height at which its features are distributed, point clouds outside this height are filtered out through direct filtering. The general steps of direct filtering are as follows: Set the filtering range: Define a height range (minimum and maximum values), which is the height range of the point clouds to be retained. All points not within this preset height range will be filtered out. Apply the direct filter: Apply the direct filter to the point cloud data using the set parameters. This step will generate a new point cloud data set containing only the points within the specified height range.

[0088] Step S102B: Perform planar filtering on the three-dimensional point cloud after direct filtering to eliminate the point cloud of the planar occluder behind the charging pile.

[0089] Among them, the planar occluder is at least a wall. When the charging pile is placed in front of a wall, due to the limited accuracy of the lidar, the display of the wall in the lidar may be similar to the features of the charging pile. To eliminate the interference of the wall on the charging pile, the wall is fitted and the fitted plane is filtered out.

[0090] In a feasible implementation, the planar filtering includes the following steps:

[0091] Step S102B1: Randomly select a preset number of point clouds from the three-dimensional point cloud after direct filtering.

[0092] Among them, the preset number is greater than or equal to 3 because at least 3 non-collinear points are required to construct a plane.

[0093] Step S102B2: Fit the planar occluder behind the charging pile based on the three-dimensional coordinate data of the preset number of point clouds to obtain a fitted plane; among them, the expression of the fitted plane is:

[0094] Ax + By + Cz + D = 0 (1);

[0095] Among them, (A, B, C) is the normal vector of the plane, D is a constant, and (x, y, z) is the three-dimensional coordinate data of the selected point cloud.

[0096] In an example, assume that the three selected points are (x 1 , y 1 , z 1 ), (x 2 , y 2 , z 2 ), (x 3 , y 3 , z 3 ), then the normal vector can be calculated by the cross product of the following vectors:

[0097]

[0098] Expanding gives:

[0099] A = (y 2 - y 1 )(z 3 - z 1 ) - (z 2 - z 1 )(y 3 - y 1 )(4);

[0100] B = (z 2 - z 1 )(x 3 - x 1 ) - (x 2 - x 1 )(z 3 - z 1 )(5);

[0101] C = (x 2 - x 1 )(y 3 - y 1 ) - (y 2 - y 1 )(x 3 - x 1 )(6);

[0102] Then, substituting one of the points (e.g., (x 1 , y 1 , z 1 )) into the plane equation (1), D can be obtained:

[0103] D = -(Ax 1 + By 1 + Cz 1 )(7);

[0104] Step S102B3: Calculate the distance between the remaining 3D point cloud after random selection and the fitted plane, and determine the inliers of the fitted plane based on the distance. Inliers refer to the points belonging to the fitted plane.

[0105] In this step, the distance formula from the point (x 0 , y 0 , z 0 ) to the plane Ax + By + Cz + D = 0 is:

[0106]

[0107] If the distance d is less than a predefined threshold t, the point is considered an inlier, i.e., belonging to the fitted plane, otherwise it is an outlier, i.e., not belonging to the fitted plane.

[0108] Step S102B4: Repeat the above fitting process for multiple iterations to obtain multiple fitted planes and their inliers, and take the fitted plane with the largest number of inliers as the target fitted plane.

[0109] Repeat steps S102B1 - S102B3 for N times. Record the fitted plane with the largest number of inliers for each iteration.

[0110] Step S102B5: Filter out the inliers on the target fitted plane to eliminate the point cloud of the plane occluder behind the charging pile.

[0111] Step S102C: Perform radius filtering on the three - dimensional point cloud after plane filtering to filter out isolated point clouds and abnormal point clouds, obtaining pre - processed three - dimensional point cloud data.

[0112] There may be some abnormal points in the three - dimensional point cloud. At the same time, after plane filtering, some isolated points may be left. To avoid the influence of these points on subsequent charging pile recognition, radius filtering is used to filter out outlier isolated points and abnormal points.

[0113] Based on the above embodiments, classifying and detecting the pre - processed three - dimensional point cloud data to detect the target charging pile three - dimensional point cloud data in each frame of the initial three - dimensional point cloud data includes the following steps:

[0114] Step S103A: Use the Euclidean clustering algorithm to classify and detect the pre - processed three - dimensional point cloud data to obtain candidate charging pile three - dimensional point cloud data.

[0115] In this step, the specific steps of the Euclidean clustering algorithm are as follows:

[0116] The first step, point cloud dimensionality reduction: Compress the pre - processed three - dimensional point cloud data to two - dimensional to obtain two - dimensional point cloud data, that is, assign the z - value of each point in the point cloud to 0.

[0117] This is done to simplify the Euclidean clustering process and does not affect the existence of the original three - dimensional point cloud because the original three - dimensional point cloud can be indexed back through the two - dimensional clustering results.

[0118] The second step: Construct a KD - tree based on the two - dimensional point cloud data.

[0119] This step organizes the point cloud data by using a KD - tree (k - d tree) to accelerate the nearest neighbor search. The KD - tree is a space - partitioning tree that can effectively support fast lookups in multiple dimensions, such as nearest neighbor search in three - dimensional space.

[0120] The third step, traverse the point cloud and nearest neighbor search: Select an unvisited point from the two - dimensional point cloud as the starting point, and search for all nearest neighbor points within a distance less than the threshold d1 from the starting point in the KD - tree.

[0121] Step 4, cluster expansion: Add the searched neighboring points to the current cluster, and recursively perform neighbor search on the newly added neighboring points until no new neighboring points are added.

[0122] Step 5: Repeat Steps 1 to 4 for all unvisited points in the point cloud until all points are assigned to the corresponding clusters. One cluster corresponds to one object category.

[0123] Euclidean clustering can be used to quickly identify and separate the target object, the charging pile, from complex 3D point cloud data.

[0124] Step S103B: Screen the candidate charging pile 3D point cloud data using a preset screening rule to obtain the target charging pile 3D point cloud data in each frame of the initial 3D point cloud data.

[0125] In this step, the preset screening rule is: screen according to the preset point cloud quantity threshold of the charging pile and the preset size threshold of the charging pile.

[0126] Continuing with the previous example, represent each cluster with a packaging box Box, and calculate the length, width, and height of each Box.

[0127] The charging pile has a certain number of point clouds. Set the maximum and minimum quantity thresholds, and remove the Boxes outside this range.

[0128] The charging pile has a certain size. Set the maximum and minimum thresholds for the corresponding length, width, and height, and remove the Boxes outside these threshold ranges.

[0129] It should be noted that if there are multiple Boxes after the final screening, select the Box with the smallest width among the multiple Boxes as the target charging pile 3D point cloud data to improve the convergence speed and accuracy of point cloud registration.

[0130] In a feasible implementation, the ICP (Iterative Closest Point) algorithm is used for point cloud registration. The process of registering the target charging pile 3D point cloud data identified in each frame with the pre-constructed charging pile point cloud template includes:

[0131] Step S104A: For each point in the charging pile point cloud template S, find the point in the target charging pile 3D point cloud T that is closest to it, and form a set of point pairs.

[0132] In an example, assume the point in S is s i , and the corresponding closest point in T is t i .

[0133] Step S104B: Determine the rotation matrix and translation vector of the charging pile point cloud template relative to the three-dimensional point cloud of the target charging pile based on each point pair in the point pair set, and use the rotation matrix and translation vector as the registration result;

[0134] In a feasible implementation, the rotation matrix and translation vector are solved by constructing a covariance matrix and performing the SVD (Singular Value Decomposition) method.

[0135] Specifically, first use S and T to construct a covariance matrix H :

[0136] H = S T T(9);

[0137] where S T represents the transpose of S.

[0138] Then perform singular value decomposition on the covariance matrix H:

[0139] H = UΣV T (10);

[0140] where U and V are orthogonal matrices; Σ is a diagonal matrix containing the singular values of H.

[0141] Finally, calculate the rotation matrix R and translation vector t:

[0142] R = VU T (11);

[0143] t = C t - RC S (12);

[0144] where C t represents the centroid of the three-dimensional point cloud data of the target charging pile; C S represents the centroid of the charging pile point cloud template after being translated to the origin of the lidar coordinate system.

[0145] Step S104C: Update the coordinate data of each point cloud in the charging pile point cloud template based on the rotation matrix and translation vector, and calculate the error function.

[0146] In an example, perform transformation using the calculated rotation matrix R and translation vector t:

[0147] S' = RS + t (13);

[0148] where S' is the coordinate data of each point cloud in the transformed charging pile point cloud template.

[0149] In one example, the error function expression is as follows:

[0150]

[0151] where R is the rotation matrix; t is the translation vector; (s i , t i ) is the i-th point pair; N is the number of point pairs; s i is the point cloud in the charging pile point cloud template; t i is the point cloud in the three-dimensional point cloud data of the target charging pile.

[0152] The error function is minimized through multiple iterations.

[0153] Step S104D: Repeat the above registration process based on the coordinate data of each point cloud in the updated charging pile point cloud template until a preset convergence condition is reached.

[0154] The preset convergence condition is at least one of the following: the number of iterations reaches the maximum value, the change in the error function is less than the preset error threshold, and the changes in the rotation matrix and the translation vector are less than the preset thresholds;

[0155] Step S104E: Calculate the registration score of the finally converged registration result based on the preset registration score calculation formula; the preset registration score calculation formula is:

[0156]

[0157] where R is the rotation matrix; t is the translation vector; (s i , t i ) is the i-th point pair; N is the number of point pairs; s i is the point cloud in the charging pile point cloud template; t i is the point cloud in the three-dimensional point cloud data of the target charging pile.

[0158] Step S104F: If the registration score is greater than the preset score threshold, then determine the finally converged registration result as the pose data of the target charging pile.

[0159] In one example, the registration result can be expressed as T i =(t i , q i ), where t i =(x i , y i , z i ) is a three-dimensional vector representing the position data of the charging pile; is a quaternion representing the attitude data of the charging pile, represents the imaginary part component of the i-th quaternion; represents the real part component of the i-th quaternion.

[0160] Based on the above embodiments, the method further includes:

[0161] Performing smoothing processing on the registration result of each frame by using a sliding window with a preset length, and the smoothing processing includes the following steps:

[0162] Step S105: Calculate the average registration result of the registration results of all frames within a sliding window with a preset length.

[0163] In one example, the preset length is 5, that is, only 5 frames of data can be processed within one sliding window. In a specific example, the average registration result is expressed as where the average displacement is:

[0164]

[0165] where n is the length of the sliding window.

[0166] The average attitude is:

[0167]

[0168]

[0169] where S Q represents the sum of all quaternions q i .

[0170] Step S106: Calculate the difference between the registration result of each frame within the sliding window and the average registration result. In the embodiments of the present invention, the position difference Δt and the attitude difference Δθ of the registration result are calculated respectively:

[0171]

[0172]

[0173] where θ represents the rotation angle in the charging pile attitude data.

[0174] Step S107: If the difference is less than the preset registration threshold, the registration is valid, otherwise it is invalid. If it is invalid, the registration result of the previous frame is used to replace the registration result of this frame.

[0175] Step S108: Calculate the average value of all valid registration results and determine this average value as the final registration result.

[0176] When the robot moves, its position and attitude relative to the charging pile also change. If the registration results recognized from each frame of point cloud are directly used, the change in the pose of the charging pile may appear very abrupt, which is not conducive to subsequent operations (such as automatic charging). Through smoothing processing, the change in the pose of the charging pile can be made more gentle and continuous, facilitating autonomous docking charging operations.

[0177] Based on the same inventive concept, a charging pile pose recognition device based on point cloud template matching is provided, which is applied to a robot to be charged. As Figure 2 shown, the device includes:

[0178] An acquisition unit 201, configured to acquire initial three-dimensional point cloud data within the area where the target charging pile is located; the initial three-dimensional point cloud data includes the three-dimensional coordinate data of the point cloud of the charging pile and other objects around it, and is continuously scanned by a multi-line lidar installed at the front end of the robot to be charged at a preset frame rate;

[0179] A preprocessing unit 202, configured to perform filtering preprocessing on each frame of the continuously scanned initial three-dimensional point cloud data to obtain preprocessed three-dimensional point cloud data;

[0180] A classification unit 203, configured to perform classification detection on the preprocessed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of the initial three-dimensional point cloud data;

[0181] A registration unit 204, configured to perform point cloud registration on the three-dimensional point cloud data of the target charging pile recognized in each frame with a pre-constructed charging pile point cloud template to obtain the corresponding registration result for each frame, and determine the registration result as the charging pile pose data.

[0182] Based on the same technical concept, an embodiment of the present invention further provides an electronic device. As Figure 3 shown, it includes a processor 301, a communication interface 302, a memory 303, and a communication bus 304. Among them, the processor 301, the communication interface 302, and the memory 303 complete mutual communication through the communication bus 304.

[0183] The memory 303 is used to store a computer program;

[0184] The processor 301, when executing the program stored on the memory 303, implements the steps of the charging pile pose recognition method based on point cloud template matching.

[0185] The communication bus mentioned in the above electronic device can be a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, etc. This communication bus can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience in representation, only a thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0186] The communication interface is used for communication between the above electronic device and other devices.

[0187] The memory can include a Random Access Memory (RAM), and can also include a Non-Volatile Memory (NVM), such as at least one disk memory. Optionally, the memory can also be at least one storage device located far from the aforementioned processor.

[0188] The above-mentioned processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it can also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0189] The computer program product for the method of identifying the pose of a charging pile based on point cloud template matching provided by the embodiments of the present invention includes a computer-readable storage medium storing program code, and the instructions included in the program code can be used to execute the method described in the foregoing method embodiments. For specific implementation, reference can be made to the method embodiments, which will not be elaborated here.

[0190] The device for identifying the pose of a charging pile based on point cloud template matching provided by the embodiments of the present invention can be specific hardware on the device, or software or firmware installed on the device, etc. The device provided by the embodiments of the present invention has the same implementation principle and the same technical effects as the foregoing method embodiments. For the sake of brief description, for the parts not mentioned in the device embodiments, reference can be made to the corresponding content in the foregoing method embodiments. Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the foregoing described systems, devices, and units can all refer to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0191] In the embodiments provided by the present invention, it should be understood that the disclosed device and method can be implemented in other ways. The device embodiments described above are only illustrative. For example, the division of the units is only a logical function division. In actual implementation, there can be other division methods. For another example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed mutual coupling or direct coupling or communication connection can be through some communication interfaces. The indirect coupling or communication connection of the device or unit can be in an electrical, mechanical, or other form.

[0192] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0193] In addition, each functional unit in the embodiments provided by the present invention can be integrated into a processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit.

[0194] If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0195] It should be noted that like reference numerals and letters indicate like items in the following figures. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. In addition, the terms "first", "second", "third", etc. are only used for descriptive distinction and should not be construed as indicating or implying relative importance.

[0196] Finally, it should be noted that the above-described embodiments are only specific embodiments of the present invention, used to illustrate the technical solutions of the present invention, rather than limiting it. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that any person skilled in the art within the technical scope disclosed by the present invention can still modify the technical solutions recorded in the foregoing embodiments or can easily think of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention. All should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A charging pile posture recognition method based on point cloud template matching, applied to a robot to be charged, characterized in that: The method comprises: Acquire initial three-dimensional point cloud data in the area where the target charging pile is located; the initial three-dimensional point cloud data includes three-dimensional coordinate data of the point cloud of the charging pile and other objects around it, and is obtained by continuous scanning at a preset frame rate by a multi-line laser radar installed at the front end of the robot to be charged; Performing filtering preprocessing on each frame of the initial three-dimensional point cloud data of the continuous scan to obtain preprocessed three-dimensional point cloud data; Classify and detect the pre-processed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of initial three-dimensional point cloud data; The three-dimensional point cloud data of the target charging pile identified in each frame is point cloud registered with the pre-built charging pile point cloud template to obtain the registration result corresponding to each frame, and the registration result is determined as the charging pile posture data.

2. The method according to claim 1, characterized in that The filtering preprocessing of each frame of the initial three-dimensional point cloud data scanned continuously to obtain preprocessed three-dimensional point cloud data comprises: Performing through filtering on each frame of the initial three-dimensional point cloud data of the continuous scan to filter out the three-dimensional point cloud beyond a preset height; Performing plane filtering on the three-dimensional point cloud after the through filtering to eliminate the point cloud of the plane obstruction behind the charging pile; wherein the plane obstruction is at least a wall; The three-dimensional point cloud after plane filtering is subjected to radius filtering to filter out isolated point clouds and abnormal point clouds to obtain preprocessed three-dimensional point cloud data.

3. The method according to claim 2, characterized in that The process of performing plane filtering on the three-dimensional point cloud after the through filtering to eliminate the point cloud of the plane obstruction behind the charging pile includes: Randomly selecting a preset number of point clouds from the three-dimensional point cloud after the straight-through filtering, where the preset number is greater than or equal to 3; Based on the three-dimensional coordinate data of the preset number of point clouds, the plane obstruction behind the charging pile is fitted to obtain a fitting plane; wherein the expression of the fitting plane is: Ax+By+Cz+D=0 Among them, (A, B, C) is the normal vector of the plane, D is a constant, and (x, y, z) is the three-dimensional coordinate data of the selected point cloud; Calculating the distance between the remaining three-dimensional point cloud after random selection and the fitting plane, and determining the inner point of the fitting plane based on the distance; the inner point refers to a point belonging to the fitting plane; Repeat the above process for multiple iterations to obtain multiple fitting planes and their inliers, and take the fitting plane with the largest number of inliers as the target fitting plane; The inner points on the target fitting plane are filtered out to eliminate the point cloud of the planar occluder behind the charging pile.

4. The method according to claim 1, characterized in that: The classifying and detecting the pre-processed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of the initial three-dimensional point cloud data includes: Using a Euclidean clustering algorithm to classify and detect the pre-processed three-dimensional point cloud data to obtain three-dimensional point cloud data of candidate charging piles; The candidate charging pile three-dimensional point cloud data is screened using preset screening rules to obtain the target charging pile three-dimensional point cloud data in each frame of initial three-dimensional point cloud data, and the preset screening rules are: screening according to a preset charging pile point cloud quantity threshold and a preset charging pile size threshold.

5. The method according to claim 1, characterized in that: The construction process of the charging pile point cloud template includes: The three-dimensional point cloud data of any frame scanned by the multi-line laser radar is cropped according to the preset size threshold of the charging pile to obtain an initial charging pile point cloud template; Calculating the centroid of the initial charging pile point cloud template; The centroid of the initial charging pile point cloud template is moved to the origin of the multi-line laser radar coordinate system to obtain the charging pile point cloud template.

6. The method according to claim 1, characterized in that The process of performing point cloud registration of the target charging pile three-dimensional point cloud data identified in each frame with the pre-built charging pile point cloud template includes: For each point in the charging pile point cloud template, find the point closest to it in the target charging pile three-dimensional point cloud to form a point pair set; Determine the rotation matrix and translation vector of the charging pile point cloud template relative to the target charging pile three-dimensional point cloud based on each point pair in the point pair set, and use the rotation matrix and translation vector as the registration result; Based on the rotation matrix and the translation vector, the coordinate data of each point cloud in the charging pile point cloud template is updated, and an error function is calculated; Repeat the above process based on the coordinate data of each point cloud in the updated charging pile point cloud template until a preset convergence condition is reached; the preset convergence condition is at least one of the following: the number of iterations reaches a maximum value, the change of the error function is less than a preset error threshold, and the change of the rotation matrix and translation vector is less than a preset threshold; The registration score of the final converged registration result is calculated based on a preset registration score calculation formula; the preset registration score calculation formula is: Where R is the rotation matrix; t is the translation vector; (s i , t i ) is the i-th point pair; N is the number of point pairs; s i is the point cloud in the charging pile point cloud template; t i It is the point cloud in the three-dimensional point cloud data of the target charging pile; If the registration score is greater than a preset score threshold, the finally converged registration result is determined as the position and posture data of the target charging pile.

7. The method according to claim 1, characterized in that The method further comprises: A sliding window of a preset length is used to smooth the registration result of each frame, and the smoothing process includes the following steps: Calculate the average registration result of the registration results of all frames within the sliding window of a preset length; Calculate the difference between the registration result of each frame in the sliding window and the average registration result; If the difference is less than the preset registration threshold, the registration is valid, otherwise it is invalid; The average value of all valid registration results is calculated and determined as the final registration result.

8. A charging pile posture recognition device based on point cloud template matching, applied to a robot to be charged, characterized in that: The device comprises: An acquisition unit is used to acquire initial three-dimensional point cloud data in the area where the target charging pile is located; the initial three-dimensional point cloud data includes three-dimensional coordinate data of the point cloud of the charging pile and other objects around it, and is obtained by continuous scanning at a preset frame rate by a multi-line laser radar installed at the front end of the robot to be charged; A preprocessing unit, used for performing filtering preprocessing on each frame of the initial three-dimensional point cloud data scanned continuously to obtain preprocessed three-dimensional point cloud data; A classification unit, used for classifying and detecting the pre-processed three-dimensional point cloud data to detect the three-dimensional point cloud data of the target charging pile in each frame of the initial three-dimensional point cloud data; The registration unit is used to perform point cloud registration on the three-dimensional point cloud data of the target charging pile identified in each frame with a pre-built charging pile point cloud template to obtain a registration result corresponding to each frame, and determine the registration result as the charging pile posture data.

9. An electronic device, characterized in that: It includes a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other through the communication bus; Memory, used to store computer programs; A processor, for implementing the method steps described in any one of claims 1 to 7 when executing a program stored in a memory.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method steps described in any one of claims 1 to 7 are implemented.

Images