A method and system for detecting and estimating the pose of a circular ring target in a warehouse scenario

Abstract

The application relates to a kind of annular target detection and pose estimation method and system in warehouse scene, wherein, obtain the multi-frame three-dimensional point cloud data in sensor coordinate system collected by robot;Through the pose transformation relationship between different coordinate systems, frame-by-frame accumulation is carried out on the multi-frame three-dimensional point cloud data, and the dense point cloud data in base coordinate system is obtained;Plane point cloud data in annular plane is extracted from the dense point cloud data;In two-dimensional alignment plane, edge point cloud data of target annular is extracted from plane point cloud data, and two-dimensional circle fitting is carried out based on edge point cloud data, to obtain the pose and radius of target annular. Through the application, the frame-by-frame accumulation of multi-frame point cloud is realized, to make up the defect of poor fitting circle precision and stability caused by single-frame sparse fitting, and the point cloud of the leading plane where the annular target is located is extracted first, and then fitting is carried out on two-dimensional alignment plane, to avoid large amount of calculation of direct three-dimensional space fitting and easily introduce a large number of irrelevant points.

Description

Technical Field

[0001] This application relates to the fields of robot perception and 3D vision technology, and in particular to a method and system for detecting and estimating the pose of circular targets in a warehouse setting. Background Technology

[0002] In smart warehousing scenarios, there are often annular reference targets (such as circular markings on the ground or circular holes on pallets). Robots need to accurately obtain the center position, radius, and normal direction of the plane in which they are located to complete tasks such as docking and alignment. Existing solutions have the following shortcomings: The point cloud density of a single sampling is limited, the edges of the ring are easily incomplete, and the stability of directly fitting a circle is poor; directly fitting a circle or cylinder in three-dimensional space involves a large amount of computation, is sensitive to initial values, and is easily affected by outliers; the ring is often located on a measurable plane (ground, platform), which can easily introduce a large number of irrelevant points; the edge points may contain parts that are occluded or beyond the effective field of view of the sensor, which can easily skew the fitting results.

[0003] Currently, no effective solution has been proposed for improving the detection and recognition accuracy of circular targets in warehousing production line scenarios. Summary of the Invention

[0004] This application provides a method and system for detecting and estimating the pose of circular targets in a warehousing scenario, so as to at least solve the problem of how to improve the detection and recognition accuracy of circular targets in warehousing production line scenarios in related technologies.

[0005] In a first aspect, embodiments of this application provide a method for detecting and estimating the pose of a circular target in a warehousing scenario, the method comprising: Acquire multiple frames of 3D point cloud data in the sensor coordinate system collected by the robot; By using the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system and the map coordinate system, the multi-frame three-dimensional point cloud data is accumulated frame by frame to obtain dense point cloud data in the base coordinate system. Planar point cloud data within the annular plane is extracted from the dense point cloud data; On the two-dimensional alignment plane, the edge point cloud data of the target ring is extracted from the planar point cloud data, and a two-dimensional circle fitting is performed based on the edge point cloud data to obtain the pose and radius of the target ring.

[0006] In some embodiments, extracting the edge point cloud data of the target ring from the planar point cloud data on a two-dimensional alignment plane includes: Based on the plane equation coefficients of the annular plane, the annular plane is converted into a two-dimensional aligned plane, and the plane point cloud data is correspondingly converted into two-dimensional projected point cloud data; On the two-dimensional alignment plane, the boundary point cloud data in the two-dimensional projected point cloud data is determined; The boundary point cloud data is clustered, and the point cloud data of the largest cluster after clustering is extracted as the edge point cloud data of the target ring. Edge points with poor visibility in the edge point cloud data are removed to obtain the edge point cloud data after removal.

[0007] In some embodiments, based on the plane equation coefficients of the annular plane, the annular plane is converted into a two-dimensional aligned plane, and the plane point cloud data is correspondingly converted into two-dimensional projected point cloud data, including: Based on the plane equation coefficients of the annular plane, a rotation matrix is ​​constructed, wherein the rotation matrix is ​​used to convert the planar point cloud into a two-dimensional alignment plane with a horizontal orientation; On the two-dimensional alignment plane, the planar point cloud data is converted into two-dimensional projected point cloud data by setting the rotation matrix and the Z-axis coordinate value to zero.

[0008] In some embodiments, determining the boundary point cloud data in the two-dimensional projected point cloud data on the two-dimensional alignment plane includes: On the two-dimensional alignment plane, normal vector estimation is performed on the two-dimensional projected point cloud data to obtain the normal vector of each point; Based on the normal vector, it is determined whether the corresponding point is a boundary point, and the determined boundary points are extracted from the two-dimensional projected point cloud data to form the boundary point cloud data.

[0009] In some embodiments, clustering the boundary point cloud data and extracting the point cloud data of the largest cluster after clustering as the edge point cloud data of the target ring includes: A Kd search tree is constructed based on the boundary point cloud data, and Euclidean clustering is performed on the boundary point cloud data based on the data structure of the Kd search tree to obtain several clusters sorted by the number of points within the cluster from most to least. Based on the index of the largest cluster ranked first, the corresponding point cloud data is extracted from the boundary point cloud data to serve as the edge point cloud data of the target ring.

[0010] In some embodiments, edge points with poor visibility in the edge point cloud data are removed, resulting in edge point cloud data after removal, including: Based on the robot's sensor height, the visibility judgment threshold is calculated; Based on the Z-axis coordinate value of the planar point cloud data corresponding to the edge point cloud data, if the Z-axis coordinate value of the edge point is less than the visibility judgment threshold, the edge point is removed from the edge point cloud data, and the judgment of all edge points is completed to obtain the edge point cloud data after removal.

[0011] In some embodiments, the pose and radius of the target ring are obtained by performing two-dimensional circle fitting based on the edge point cloud data, including: Based on the edge point cloud data, a two-dimensional RANSAC circle fitting is performed to obtain the inner point, center, and radius of the target ring on the two-dimensional alignment plane; The inner points, center, and radius of the target ring are transformed from the two-dimensional alignment plane back to the base coordinate system to obtain the pose and radius of the target ring that are consistent with the robot base.

[0012] In some embodiments, extracting planar point cloud data within the annular plane from the dense point cloud data includes: Based on the dense point cloud data, a circular plane is obtained by performing plane segmentation processing using the RANSAC algorithm. Then, based on the interior point index of the circular plane, planar point cloud data is extracted from the dense point cloud data.

[0013] In some embodiments, the multi-frame 3D point cloud data is accumulated frame by frame using the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system, and the map coordinate system to obtain dense point cloud data in the base coordinate system, including: The sensor coordinate system has a first pose transformation relationship with the base coordinate system. The coordinate system is transformed frame by frame through the first pose transformation relationship to obtain the three-dimensional point cloud data in the base coordinate system. The three-dimensional point cloud data is cropped using the region of interest in the base coordinate system to obtain cropped three-dimensional point cloud data. The base coordinate system has a second pose transformation relationship with the odometer coordinate system. The clipped 3D point cloud data is transformed by the second pose transformation relationship to obtain the clipped 3D point cloud data in the odometer coordinate system. The clipped 3D point cloud data in the odometer coordinate system is accumulated frame by frame, and then inversely transformed to the base coordinate system through the second pose transformation relationship to obtain dense point cloud data in the base coordinate system.

[0014] Secondly, embodiments of this application provide a circular target detection and pose estimation system in a warehousing scenario, characterized in that the system is used to execute the method described in the first aspect above, and the system includes an acquisition module and a detection estimation module; The acquisition module is used to acquire multiple frames of 3D point cloud data in the sensor coordinate system collected by the robot; The detection and estimation module is used to accumulate the multi-frame three-dimensional point cloud data frame by frame through the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system and the map coordinate system to obtain dense point cloud data in the base coordinate system. The detection and estimation module is used to extract planar point cloud data within the annular plane from the dense point cloud data; The detection and estimation module is used to extract the edge point cloud data of the target ring from the planar point cloud data on the two-dimensional alignment plane, and perform two-dimensional circle fitting based on the edge point cloud data to obtain the pose and radius of the target ring.

[0015] Compared to related technologies, this application provides a method and system for detecting and estimating the pose of a circular target in a warehousing scenario. The method acquires multiple frames of 3D point cloud data in the sensor coordinate system collected by the robot. Through the pose transformation relationship between the sensor coordinate system, base coordinate system, odometer coordinate system, and map coordinate system, the multiple frames of 3D point cloud data are accumulated frame by frame to obtain dense point cloud data in the base coordinate system. Planar point cloud data within the circular plane is extracted from the dense point cloud data. On a two-dimensional alignment plane, edge point cloud data of the target circular target is extracted from the planar point cloud data, and a two-dimensional circle is fitted based on the edge point cloud data to obtain the pose and radius of the target circular target. This achieves frame-by-frame accumulation of point clouds to compensate for the poor accuracy and stability of the fitted circle caused by sparse fitting in a single frame. Furthermore, by first extracting the point cloud of the dominant plane where the circular target is located, and then fitting it on the two-dimensional alignment plane, the large computational load and the tendency to introduce a large number of irrelevant points associated with direct fitting in 3D space are avoided. This solves the problem of how to improve the detection and recognition accuracy of circular targets in warehousing production line scenarios. Attached Figure Description

[0016] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings: Figure 1 This is a schematic diagram of a common circular object in warehousing and logistics production line scenarios. Figure 2 This is a flowchart of the steps of the circular target detection and pose estimation method according to the embodiments of this application; Figure 3 This is a comparative diagram of single-frame point cloud and multi-frame point cloud accumulation according to an embodiment of this application; Figure 4 This is a schematic diagram of extracting a circular plane point cloud after planar segmentation according to an embodiment of this application; Figure 5 This is a schematic diagram of two-dimensional alignment plane transformation according to an embodiment of this application; Figure 6 This is a schematic diagram of boundary point cloud extraction according to an embodiment of this application; Figure 7 This is a schematic diagram of the edge point cloud after clustering filtering according to an embodiment of this application; Figure 8 This is a schematic diagram of two-dimensional circle fitting and attitude recovery according to an embodiment of this application; Figure 9 This is a schematic diagram of the internal structure of an electronic device according to an embodiment of this application. Detailed Implementation

[0017] To make the objectives, technical solutions, and advantages of this application clearer, the application is described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. All other embodiments obtained by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0018] Obviously, the accompanying drawings described below are merely some examples or embodiments of this application. Those skilled in the art can apply this application to other similar scenarios based on these drawings without any inventive effort. Furthermore, it is understood that although the efforts made in this development process may be complex and lengthy, for those skilled in the art related to the content disclosed in this application, any changes to design, manufacturing, or production based on the technical content disclosed in this application are merely conventional technical means and should not be construed as insufficient disclosure of the content of this application.

[0019] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment that is mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0020] Unless otherwise defined, the technical or scientific terms used in this application shall have the ordinary meaning understood by one of ordinary skill in the art to which this application pertains. The terms “a,” “an,” “an,” “the,” and similar words used in this application do not indicate quantity limitation and may indicate singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this application are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or device that includes a series of steps or modules (units) is not limited to the listed steps or units, but may also include steps or units not listed, or may include other steps or units inherent to these processes, methods, products, or devices. The terms “connected,” “linked,” “coupled,” and similar words used in this application are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. “Multiple” used in this application refers to two or more. “And / or” describes the relationship between related objects, indicating that three relationships may exist; for example, “A and / or B” can represent: A alone, A and B simultaneously, and B alone. The character " / " generally indicates that the preceding and following objects are in an "or" relationship. The terms "first," "second," and "third" used in this application are merely to distinguish similar objects and do not represent a specific ordering of the objects.

[0021] This application provides a method for detecting and estimating the pose of circular targets in a warehouse setting. Figure 1 This is a schematic diagram of a circular object commonly seen in warehousing and logistics production line scenarios. Figure 2 This is a flowchart illustrating the steps of the circular target detection and pose estimation method according to an embodiment of this application, as follows: Figure 2 As shown, the method includes the following steps: Step S102: Obtain multi-frame 3D point cloud data in the sensor coordinate system collected by the robot; It should be noted that the data obtained in step S102 specifically includes: multi-frame 3D point cloud data collected by the depth camera or LiDAR of the robot (robot operating system PointCloud2), and the robot's state (such as the height of the fork arm, the pose transformation relationship between the base coordinate system and the map coordinate system or odometry coordinate system); furthermore, the robot itself has a corresponding base coordinate system (base_footprint), its depth camera or LiDAR has a corresponding sensor coordinate system, and the odometry coordinate system (odom) and the map coordinate system (map) are also used in the point cloud data processing; the pose transformation relationship between the base coordinate system and the map coordinate system is base_footprint2map (the pose transformation of base_footprint under map), and the pose transformation relationship between the base coordinate system and the odometry coordinate system is base_footprint2odom (the pose transformation of base_footprint under odom).

[0022] Step S104: By using the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system and the map coordinate system, the multi-frame three-dimensional point cloud data is accumulated frame by frame to obtain dense point cloud data in the base coordinate system. Step S104 specifically includes the following steps: Step S1041: The sensor coordinate system has a first pose transformation relationship with the base coordinate system. The coordinate system is transformed frame by frame through the first pose transformation relationship to obtain the three-dimensional point cloud data in the base coordinate system. It should be noted that during the first callback, the first pose transformation relationship from the sensor (livox360 laser) to the base_footprint is queried (i.e., the extrinsic parameter eMc_—the pose of the sensor relative to the base coordinate system), and the installation height h1 of the sensor is read from it; at the same time, the fork arm height h2 is obtained. Then, the current actual installation height of the sensor h = h1 + h2, and the real-time height h is updated to the z-axis component of the extrinsic parameter eMc_. That is, the first pose transformation relationship is updated in real time using the height h.

[0023] Step S1042: Use the region of interest in the base coordinate system to crop the 3D point cloud data to obtain cropped 3D point cloud data. It should be noted that the Region of Interest (ROI) initially belongs to the map coordinate system. Through the pose transformation relationship between the base coordinate system and the map coordinate system, base_footprint2map, the ROI under the map coordinate system is transformed to the base coordinate system to obtain the ROI under the base. Further, through the above step S1041, the point cloud of this frame is transformed from the sensor coordinate system to the base coordinate system, and in step S1042, the ROI is cropped: the ROI is the ROI under the base, and the point cloud that is not in the ROI is cropped to obtain the cropped 3D point cloud data base_pc_cropped.

[0024] Step S1043: The base coordinate system has a second pose transformation relationship with the odometer coordinate system. The clipped 3D point cloud data is transformed by the second pose transformation relationship to obtain the clipped 3D point cloud data in the odometer coordinate system. Step S1044: The clipped 3D point cloud data in the odometer coordinate system is accumulated frame by frame, and then inversely transformed to the base coordinate system through the second pose transformation relationship to obtain dense point cloud data in the base coordinate system.

[0025] It should be noted that, Figure 3 This is a comparative diagram of single-frame point cloud and multi-frame point cloud accumulation according to an embodiment of this application, as shown below. Figure 3 As shown, step S1043 transforms the cropped point cloud of the current frame to the odom coordinate system to obtain the cropped 3D point cloud data odom_pc in the odometry coordinate system; step S1044 then pushes the (timestamp, odom_pc, base_pc_cropped) of the current frame into the data buffer queue pc_queue_. When the queue length reaches a set number of frames (e.g., 20 frames), the earliest frame is popped; the odom_pc of all frames in the queue is accumulated to obtain the dense point cloud odom_merge_pc under odom; odom_merge_pc is inversely transformed to the base coordinate system using the second pose transformation relationship at the current moment to obtain the dense point cloud base_merge_pc under base_footprint, which is used as the input point cloud for subsequent detection. This achieves the accumulation of multiple frames under odometry to compensate for the sparsity and occlusion of a single frame. After transformation back to the current base, it is consistent with the current robot pose, which facilitates the output of a circular pose that can be directly used for control.

[0026] Step S106: Extract planar point cloud data within the annular plane from the dense point cloud data; Specifically, in step S106, based on the dense point cloud data, a circular plane is obtained by performing plane segmentation processing using the RANSAC algorithm, and then planar point cloud data is extracted from the dense point cloud data based on the interior point index of the circular plane.

[0027] It should be noted that, Figure 4 This is a schematic diagram of extracting the annular planar point cloud after planar segmentation according to an embodiment of this application, as shown below. Figure 4 As shown, step S106 performs voxel downsampling on the dense point cloud data base_merge_pc (or the original point cloud can be used directly), and uses the RANSAC algorithm to segment the plane, obtaining the plane equation Ax + By + Cz + D = 0 (where A, B, C, and D are the coefficients of the plane equation) and the inlier index; then, based on the inlier index, the inlier points are extracted from the point cloud to obtain the plane point cloud plane_cloud. Furthermore, the target ring is usually located on a dominant plane (such as the ground or a platform). Segmenting this plane in step S106 removes a large number of non-planar points, retaining only the points on the plane where the ring is located, providing support for the subsequent "inlier circle" and reducing interference from subsequent edge and circle fitting.

[0028] Step S108: On the two-dimensional alignment plane, extract the edge point cloud data of the target ring from the planar point cloud data, and perform two-dimensional circle fitting based on the edge point cloud data to obtain the pose and radius of the target ring.

[0029] Step S108 specifically includes the following steps: Step S1081: Based on the plane equation coefficients of the annular plane, the annular plane is converted into a two-dimensional alignment plane, and the plane point cloud data is correspondingly converted into two-dimensional projected point cloud data. Specifically, step S1081 involves constructing a rotation matrix based on the plane equation coefficients of the annular plane. The rotation matrix is ​​used to convert the planar point cloud into a two-dimensional alignment plane with a horizontal orientation. On the two-dimensional alignment plane, the planar point cloud data is converted into two-dimensional projected point cloud data by setting the rotation matrix and the Z-axis coordinate value to zero.

[0030] It should be noted that, Figure 5 This is a schematic diagram of two-dimensional alignment plane transformation according to an embodiment of this application, such as... Figure 5As shown, the unit normal vector normal(A,B,C) and z-axis z_axis=(0,0,1) are obtained from the plane equation coefficients of the annular plane. Based on the rotation axis (axis=normal×z_axis) and rotation angle (angle=arccos(normal•z_axis)), a rotation matrix R is constructed to transform the planar point cloud into a horizontally aligned two-dimensional plane. That is, the point p on the annular plane is p_aligned=R*p on the two-dimensional aligned plane. Each point in the planar point cloud data plane_cloud is transformed to the two-dimensional aligned plane through the rotation matrix R to obtain the planar projection point aligned_point(aligned_point.x(), aligned_point.y(), aligned_point.z()). Then, the z-axis coordinate value of the planar projection point is set to zero to form the "flattened" two-dimensional projection point cloud data aligned_cloud. At the same time, aligned_point.z() is accumulated and averaged to obtain average_z, which represents the average height of the two-dimensional aligned plane. Furthermore, in step S1081, the planar point cloud is rotated to a "horizontal" orientation so that the plane normal vector coincides with the z-axis. Then, the z-axis of the points on the plane is approximately constant in the new coordinate system. The ring degenerates into a two-dimensional circle in the plane, which is convenient for fitting using a two-dimensional circle model. That is, the ring becomes a two-dimensional circle in the plane, and the two-dimensional circle RANSAC model can be directly used, which is simple to calculate and robust.

[0031] Step S1082: On the two-dimensional alignment plane, determine the boundary point cloud data in the two-dimensional projected point cloud data; Specifically, in step S1082, normal vector estimation is performed on the two-dimensional projected point cloud data on the two-dimensional alignment plane to obtain the normal vector of each point; based on the normal vector, it is determined whether the corresponding point is a boundary point, and the determined boundary points are extracted from the two-dimensional projected point cloud data to form the boundary point cloud data.

[0032] Step S1082 preferably involves, given the two-dimensional projected point cloud data and the normal vector of each point, using the angle criterion on the tangent plane to determine whether each point is located on the surface boundary (edge). The core idea is: if there is a "large gap" in the surrounding angle of a point's neighbors on the tangent plane, it indicates that the point lacks neighbors on one side, i.e., it is on the boundary. Specifically: Input and preconditions: 1) Input two-dimensional projected point cloud data (input_array); 2) A normal vector array `normals_` that corresponds one-to-one with each input point; 3) Search radius search_parameter_ (e.g., 0.1m) or K-nearest neighbor number k_; 4) Angle threshold (e.g., π / 2, used to determine whether the "maximum angle gap" is sufficient to mark a point as a boundary point).

[0033] Single-point processing flow (for each point to be estimated): 1) Using the current point as the center, perform a radius search (or K-nearest neighbor search) on the input point cloud to obtain the neighboring point index nn_indices and distance nn_dists. If the neighborhood is empty or the current point is not a finite value, set the boundary marker of the point to NaN and skip it. 2) Construct an orthogonal coordinate system u–v–n (v=n is a unit orthogonal vector, u=n×v) on the tangent plane using the normal vector n of the current point. Then u and v span the tangent plane of the point, and n is the normal vector. u and v are used to project the neighboring points onto the tangent plane and calculate the azimuth angle around the current point.

[0034] 3) For each valid neighbor point within the search radius, calculate the vector δ relative to the current point, which is the neighbor coordinate. The azimuth angle of the current point in u–v is θ=atan2(v•δ,u•δ). Calculate θ for all neighboring points to form an angle sequence.

[0035] 4) After sorting the above angle sequence, calculate the difference between adjacent angles (including the first and last closed angles: the gap between the last and the first angle is 2π). (θ_last θ_first)). Take the maximum value of all gaps, max_dif; 5) If max_dif is greater than the angle threshold angle_threshold_, then the current point is determined to be a boundary point (boundary_point=true), otherwise it is an internal point. The intuitive meaning is: the neighborhood of an internal point is approximately wrapped around the tangent plane, and the maximum gap is small; the boundary point has only one neighbor on one side, and there is a "gap" greater than the angle threshold (such as π / 2), so it is marked as an edge.

[0036] Multi-point parallel processing: 1) Perform parallel loops on the points in the point cloud, with each thread independently executing the above single-point processing flow on several points, thereby accelerating the overall boundary estimation on a multi-core system.

[0037] 2) Output a boundary point cloud of the same size as the input point cloud. Each point contains only a boundary_point flag (true / false or NaN). Then, based on this flag, filter out all points in aligned_cloud that have boundary_point==true to obtain the boundary point cloud data edge_cloud.

[0038] It should be noted that, Figure 6 This is a schematic diagram of boundary point cloud extraction according to an embodiment of this application, as shown below. Figure 6 As shown, the geometric representation of the circular target on the plane is a ring edge (contour). Extracting points on this contour can significantly reduce the number of points involved in circle fitting and highlight the geometric features of the circle. By step S1082, only the contour points of the planar region are retained, which includes both the circular contour and removes redundant points inside the plane, thereby improving the efficiency and stability of circle fitting.

[0039] Step S1083: Perform clustering processing on the boundary point cloud data, and extract the point cloud data of the largest cluster after clustering as the edge point cloud data of the target ring. Specifically, step S1083 involves constructing a Kd search tree based on the boundary point cloud data, and performing Euclidean clustering on the boundary point cloud data based on the data structure of the Kd search tree to obtain several clusters sorted by the number of points within each cluster from most to least; and extracting the corresponding point cloud data from the boundary point cloud data based on the index of the largest cluster in the first ranking, as the edge point cloud data of the target ring.

[0040] It should be noted that the edge point cloud data edge_cloud may contain multiple closed contours (such as multiple circles or other object boundaries). Step S1083 uses clustering to group points of the same contour into one class, and then selects the cluster with the most points as the candidate edge of the "main ring". Specifically, a KdTree is built for edge_cloud, Euclidean clustering (such as PCL's EuclideanClusterExtraction) is performed, and the clustering distance tolerance (such as 0.1m), minimum cluster size (such as 10), and maximum cluster size (such as 1000) are set. Then, the clusters are sorted from most to least number of points, and the index of the first cluster (the largest cluster) is taken to extract the point cloud of that cluster from edge_cloud, resulting in the edge point cloud data rc_cluster_pc of the target ring. This achieves the priority selection of the most complete and most ring-like edge when multiple contours or noisy edges exist, avoiding the mistaken selection of small fragments or other object edges.

[0041] Step S1084: Remove edge points with poor visibility from the edge point cloud data to obtain the edge point cloud data after removal.

[0042] Specifically, in step S1084, a visibility judgment threshold is calculated based on the robot's sensor height; based on the Z-axis coordinate value of the planar point cloud data corresponding to the edge point cloud data, if the Z-axis coordinate value of the edge point is less than the visibility judgment threshold, the edge point is removed from the edge point cloud data, and the judgment of all edge points is completed to obtain the edge point cloud data after removal.

[0043] It should be noted that, Figure 7 This is a schematic diagram of the edge point cloud after clustering filtering according to an embodiment of this application, as shown below. Figure 7 As shown, the edge points may include parts that the sensor cannot directly observe due to its installation height and viewing angle (such as the back side of the ring). If these are directly used in the fitting, they will skew the center and radius of the circle. Step S1084 estimates the "visible" height range through geometric relationships and filters out low (invisible) edge points. That is, in the base coordinate system, a point on the plane is taken as plane_center based on the plane coefficient (e.g., let y=0, z=1 to solve for x) to approximate the plane position, and the visibility judgment threshold based on the sensor height is calculated: height_thresh=sensor_height-|plane_center.x()-sensor_x|*tan(5°) Where sensor_height is the sensor height (i.e., the z component of the aforementioned extrinsic parameter eMc_), sensor_x is the sensor's mounting position relative to the X-axis of the base coordinate system, and plane_center.x() is the position of the fitting plane relative to the X-axis of the base coordinate system; this formula indicates that as the horizontal distance between the plane and the sensor increases, the minimum height of the visible area increases linearly (approximately the line of intersection between the line of sight and the plane is invisible). For each point in the edge point cloud data rc_cluster_pc, the Z-axis coordinate value of the corresponding planar point cloud data is obtained by restoring the z-axis of the point (pt.x, pt.y, pt.z) in the alignment system to pt.z + average_z, and then transforming it back to the original planar coordinate system using inverse rotation R_inv. If the Z-axis coordinate value (height) is greater than height_thresh, it is retained, resulting in the filtered edge point cloud edge_picked_cloud. This removes edge points that are likely to be invisible, making the circle fitting closer to the arc actually observed by the sensor, and improving the accuracy of the circle center and radius.

[0044] Step S1085: Perform 2D RANSAC circle fitting based on edge point cloud data to obtain the inner point, center, and radius of the target ring on the 2D alignment plane; transform the inner point, center, and radius of the target ring from the 2D alignment plane back to the base coordinate system to obtain the pose and radius of the target ring consistent with the robot base.

[0045] It should be noted that, Figure 8 This is a schematic diagram of two-dimensional circle fitting and attitude recovery according to an embodiment of this application, as shown below. Figure 8As shown, a circle is fitted on the filtered edge points (still within the alignment plane, which can be considered as a two-dimensional point set) to obtain the center and radius. Specifically, the PCL random consistency segmentation algorithm (SACSegmentation) is preferably used, with the model type being 2D circle (SACMODEL_CIRCLE2D) and the method being random sampling consensus algorithm (SAC_RANSAC). A distance threshold (e.g., 0.01m), a maximum number of iterations (e.g., 100), and a radius range (e.g., [0.1, 1]m) are set. Step S1085 segments the edge point cloud data edge_picked_cloud to obtain the center (cx, cy), radius (radius), and inlier index, and performs verification: if the number of inliers is less than the set threshold (e.g., 50), or the radius is not within a reasonable range (e.g., [0.2, 1]m), the fitting is deemed invalid and a failure is returned. This implements RANSAC circle fitting to resist outliers, and the radius and inlier count verification can filter out false detections or non-circular contours. Step S1085 uses inverse rotation R_inv to transform the center and radius of the circle in the two-dimensional alignment plane and the plane orientation back to the base coordinate system, and constructs a circular coordinate system for downstream use. The pose transformation relationship between the circular coordinate system and the base coordinate system is circle2base. The output is a circular pose and radius consistent with the robot base, which is convenient for docking, alignment and other control uses.

[0046] It should be further noted that the steps shown in the above process or in the flowchart of the accompanying figures can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0047] This application provides a circular target detection and pose estimation system in a warehouse scenario. The system is characterized in that it is used to execute the method of the first aspect described above, and the system includes an acquisition module and a detection estimation module. The acquisition module is used to acquire multiple frames of 3D point cloud data in the sensor coordinate system collected by the robot; The detection and estimation module is used to accumulate multiple frames of 3D point cloud data frame by frame through the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system and the map coordinate system to obtain dense point cloud data in the base coordinate system. The detection and estimation module is used to extract planar point cloud data within the annular plane from dense point cloud data; The detection and estimation module is used to extract the edge point cloud data of the target ring from the planar point cloud data on the two-dimensional alignment plane, and to perform two-dimensional circle fitting based on the edge point cloud data to obtain the pose and radius of the target ring.

[0048] The system provided in this application embodiment realizes the frame-by-frame accumulation of point clouds from multiple frames to compensate for the defects of poor accuracy and stability of the fitted circle caused by sparse fitting of a single frame. Furthermore, it first extracts the point cloud of the dominant plane where the circular target is located, and then performs fitting on the two-dimensional alignment plane, avoiding the large amount of computation and the easy introduction of a large number of irrelevant points when fitting directly in three-dimensional space. This solves the problem of how to improve the detection and recognition accuracy of circular targets in warehouse production line scenarios.

[0049] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0050] This embodiment provides an electronic device including a memory and a processor. The memory stores a computer program, and the processor is configured to run the computer program to perform the steps in any of the above method embodiments.

[0051] Optionally, the electronic device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor and the input / output device is connected to the processor.

[0052] Optionally, the electronic device may further include a processor, memory, network interface, display screen, and input device connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface is used to communicate with external terminals via a network connection. When the computer program is executed by the processor, it implements a method for detecting and estimating the pose of circular targets in a warehousing scenario. The display screen may be a liquid crystal display (LCD) or an e-ink display. The input device may be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the device's casing, or an external keyboard, touchpad, or mouse.

[0053] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementations, and will not be repeated here.

[0054] Furthermore, in conjunction with the circular target detection and pose estimation method for a warehousing scenario in the above embodiments, this application embodiment can provide a storage medium for implementation. This storage medium stores a computer program; when executed by a processor, the computer program implements any of the circular target detection and pose estimation methods for a warehousing scenario in the above embodiments.

[0055] In one embodiment, Figure 9 This is a schematic diagram of the internal structure of an electronic device according to an embodiment of this application, such as... Figure 9 As shown, an electronic device is provided, which can be a server, and its internal structure diagram can be as follows. Figure 9 As shown, the electronic device includes a processor, a network interface, internal memory, and non-volatile memory connected via an internal bus. The non-volatile memory stores the operating system, computer programs, and a database. The processor provides computing and control capabilities, the network interface communicates with external terminals via a network, the internal memory provides an environment for the operating system and computer programs to run, and the computer programs, when executed by the processor, implement a method for detecting and estimating the pose of circular targets in a warehouse setting. The database stores the data.

[0056] Those skilled in the art will understand that Figure 9 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the electronic device to which the present application is applied. A specific electronic device may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0057] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. This computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0058] Those skilled in the art should understand that the technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments have been described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0059] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A method for detecting and estimating the pose of circular targets in a warehouse setting, characterized in that, The method includes: Acquire multiple frames of 3D point cloud data in the sensor coordinate system collected by the robot; By using the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system and the map coordinate system, the multi-frame three-dimensional point cloud data is accumulated frame by frame to obtain dense point cloud data in the base coordinate system. Planar point cloud data within the annular plane is extracted from the dense point cloud data; On the two-dimensional alignment plane, the edge point cloud data of the target ring is extracted from the planar point cloud data, and a two-dimensional circle fitting is performed based on the edge point cloud data to obtain the pose and radius of the target ring.

2. The method according to claim 1, characterized in that, Extracting the edge point cloud data of the target ring from the planar point cloud data on the two-dimensional alignment plane includes: Based on the plane equation coefficients of the annular plane, the annular plane is converted into a two-dimensional aligned plane, and the plane point cloud data is correspondingly converted into two-dimensional projected point cloud data; On the two-dimensional alignment plane, the boundary point cloud data in the two-dimensional projected point cloud data is determined; The boundary point cloud data is clustered, and the point cloud data of the largest cluster after clustering is extracted as the edge point cloud data of the target ring. Edge points with poor visibility in the edge point cloud data are removed to obtain the edge point cloud data after removal.

3. The method according to claim 2, characterized in that, Based on the plane equation coefficients of the annular plane, the annular plane is converted into a two-dimensional aligned plane, and the plane point cloud data is correspondingly converted into two-dimensional projected point cloud data, including: Based on the plane equation coefficients of the annular plane, a rotation matrix is ​​constructed, wherein the rotation matrix is ​​used to convert the planar point cloud into a two-dimensional alignment plane with a horizontal orientation; On the two-dimensional alignment plane, the planar point cloud data is converted into two-dimensional projected point cloud data by setting the rotation matrix and the Z-axis coordinate value to zero.

4. The method according to claim 2, characterized in that, On the two-dimensional alignment plane, determining the boundary point cloud data in the two-dimensional projected point cloud data includes: On the two-dimensional alignment plane, normal vector estimation is performed on the two-dimensional projected point cloud data to obtain the normal vector of each point; Based on the normal vector, it is determined whether the corresponding point is a boundary point, and the determined boundary points are extracted from the two-dimensional projected point cloud data to form the boundary point cloud data.

5. The method according to claim 2, characterized in that, Clustering the boundary point cloud data and extracting the point cloud data of the largest cluster after clustering as the edge point cloud data of the target ring includes: A Kd search tree is constructed based on the boundary point cloud data, and Euclidean clustering is performed on the boundary point cloud data based on the data structure of the Kd search tree to obtain several clusters sorted by the number of points within the cluster from most to least. Based on the index of the largest cluster ranked first, the corresponding point cloud data is extracted from the boundary point cloud data to serve as the edge point cloud data of the target ring.

6. The method according to claim 2, characterized in that, Edge points with poor visibility in the edge point cloud data are removed, resulting in edge point cloud data after removal, including: Based on the robot's sensor height, the visibility judgment threshold is calculated; Based on the Z-axis coordinate value of the planar point cloud data corresponding to the edge point cloud data, if the Z-axis coordinate value of the edge point is less than the visibility judgment threshold, the edge point is removed from the edge point cloud data, and the judgment of all edge points is completed to obtain the edge point cloud data after removal.

7. The method according to claim 2, characterized in that, Based on the edge point cloud data, a two-dimensional circle is fitted to obtain the pose and radius of the target ring, including: Based on the edge point cloud data, a two-dimensional RANSAC circle fitting is performed to obtain the inner point, center, and radius of the target ring on the two-dimensional alignment plane; The inner points, center, and radius of the target ring are transformed from the two-dimensional alignment plane back to the base coordinate system to obtain the pose and radius of the target ring that are consistent with the robot base.

8. The method according to claim 1, characterized in that, The planar point cloud data within the annular plane extracted from the dense point cloud data includes: Based on the dense point cloud data, a circular plane is obtained by performing plane segmentation processing using the RANSAC algorithm. Then, based on the interior point index of the circular plane, planar point cloud data is extracted from the dense point cloud data.

9. The method according to claim 1, characterized in that, By utilizing the pose transformation relationships between the sensor coordinate system, the base coordinate system, the odometer coordinate system, and the map coordinate system, the multi-frame 3D point cloud data is accumulated frame by frame to obtain dense point cloud data in the base coordinate system, including: The sensor coordinate system has a first pose transformation relationship with the base coordinate system. The coordinate system is transformed frame by frame through the first pose transformation relationship to obtain the three-dimensional point cloud data in the base coordinate system. The three-dimensional point cloud data is cropped using the region of interest in the base coordinate system to obtain cropped three-dimensional point cloud data. The base coordinate system has a second pose transformation relationship with the odometer coordinate system. The clipped 3D point cloud data is transformed by the second pose transformation relationship to obtain the clipped 3D point cloud data in the odometer coordinate system. The clipped 3D point cloud data in the odometer coordinate system is accumulated frame by frame, and then inversely transformed to the base coordinate system through the second pose transformation relationship to obtain dense point cloud data in the base coordinate system.

10. A circular target detection and pose estimation system for a warehouse scenario, characterized in that, The system is used to perform the method according to any one of claims 1 to 9, the system comprising an acquisition module and a detection estimation module; The acquisition module is used to acquire multiple frames of 3D point cloud data in the sensor coordinate system collected by the robot; The detection and estimation module is used to accumulate the multi-frame three-dimensional point cloud data frame by frame through the pose transformation relationship between the sensor coordinate system, the base coordinate system, the odometer coordinate system and the map coordinate system to obtain dense point cloud data in the base coordinate system. The detection and estimation module is used to extract planar point cloud data within the annular plane from the dense point cloud data; The detection and estimation module is used to extract the edge point cloud data of the target ring from the planar point cloud data on the two-dimensional alignment plane, and perform two-dimensional circle fitting based on the edge point cloud data to obtain the pose and radius of the target ring.

Images