Gripping control method and apparatus for test tube

Abstract

Provided in the present invention is a gripping control method for a test tube. The method comprises: acquiring test tube posture information of a target test tube, and adjusting the current posture of a gripper according to the test tube posture information; acquiring test tube position information of the target test tube and test tube rack information corresponding to a test tube rack where the target test tube is located, and performing path planning according to the test tube position information and the test tube rack information, so as to obtain a target path; and controlling the gripper to move along the target path, and gripping the target test tube on the basis of the current posture of the gripper. In the present invention, the current posture of a gripper is adjusted according to test tube posture information of a target test tube, and path planning is performed according to test tube rack information and test tube position information, thereby improving the universality and efficiency of test tube gripping.

Description

A test tube grabbing control method and device Technical Field

[0001] The present invention relates to the fields of computer vision and robot control, and in particular to a test tube grasping control method and device. Background Art

[0002] With the rapid development of pharmaceutical, chemical and biological fields, there is an urgent need for test tube operation devices to operate test tubes to replace manual test tube operation.

[0003] Currently, test tube manipulation devices require pose estimation and grasping reasoning when grasping test tubes. Existing test tube manipulation devices typically combine object detection with template matching. The template stores the position information of the test tube rack and the vertically placed test tubes. If the position of the test tube rack or the test tubes changes, the template mismatch will prevent object detection and, consequently, test tube grasping. This method has low versatility. Regarding grasping reasoning, existing test tube manipulation devices use geometric reasoning to determine possible grasping poses, evaluate each possible grasping pose, select the optimal grasping pose based on the evaluation results, and use it as the initial pose for the next iteration. This results in low efficiency in grasping test tubes.

[0004] Therefore, the existing technology has defects and needs to be improved and developed.

[0005] Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a test tube grasping control method and device in response to the above-mentioned defects of the prior art, aiming to solve the problems of low versatility and low grasping efficiency of the test tube operation device in the prior art for test tube grasping.

[0007] The technical solutions adopted by the present invention to solve the technical problems are as follows:

[0008] A test tube grabbing control method, the method comprising:

[0009] Acquiring test tube posture information of the target test tube, and adjusting the current posture of the gripper according to the test tube posture information;

[0010] Obtaining test tube position information of a target test tube and test tube rack information corresponding to the test tube rack where the target test tube is located, performing path planning based on the test tube position information and the test tube rack information to obtain a target path;

[0011] The gripper is controlled to move along the target path, and the target test tube is grasped in the current posture of the gripper.

[0012] In one implementation, before acquiring the test tube posture information of the target test tube and adjusting the current posture of the gripper according to the test tube posture information, the method further includes:

[0013] Get depth image;

[0014] Processing the depth image, taking the target test tube rack as the target test tube rack, obtaining a test tube cap point cloud corresponding to each test tube in the target test tube rack, and processing each of the test tube cap point clouds to obtain test tube posture information and test tube position information corresponding to each test tube;

[0015] The depth image is obtained by photographing the target test tube rack with a depth camera.

[0016] In one implementation, processing the depth image, taking the target test tube rack as the target test tube rack, obtaining a test tube cap point cloud corresponding to each test tube in the target test tube rack, and processing each test tube cap point cloud to obtain test tube posture information and test tube position information corresponding to each test tube include:

[0017] Extracting the depth image using a depth filter to obtain a top-level depth image corresponding to the target test tube rack, saving the top-level depth image in RGB format to obtain a first image corresponding to the target test tube rack, processing the depth image and the first image to obtain a first point cloud corresponding to the target test tube rack, filtering and clustering the first point cloud to obtain a test tube cap point cloud corresponding to each test tube in the target test tube rack, obtaining an outer ring point cloud in each test tube cap point cloud, and fitting a plane to each outer ring point cloud to obtain a plane point cloud corresponding to each test tube;

[0018] Obtaining the coordinates of the center of mass of the plane point cloud corresponding to each test tube, calculating the vectors from all points in the plane point cloud corresponding to each test tube to the center of mass of the plane point cloud, saving the vectors corresponding to each test tube in matrix form to obtain an original matrix corresponding to each test tube, processing each original matrix separately to obtain a test tube posture matrix corresponding to each test tube, and storing each test tube posture matrix as test tube posture information corresponding to each test tube;

[0019] Obtaining the test tube initial coordinates corresponding to each test tube in the camera coordinate system according to the coordinates of the centroid of each plane point cloud, and performing world coordinate conversion on each test tube initial coordinate to obtain test tube position information of each test tube;

[0020] The abscissa of the initial test tube coordinates of each test tube is the abscissa of the center of mass of the plane point cloud corresponding to the test tube, and the ordinate of the initial test tube coordinates of each test tube is the ordinate of the center of mass of the plane point cloud corresponding to the test tube.

[0021] In one implementation, adjusting the current posture of the gripper according to the test tube posture information includes:

[0022] Multiplying the test tube posture matrix and the preset external parameter matrix to obtain the gripper posture matrix;

[0023] The current posture of the gripper is adjusted according to the gripper posture matrix.

[0024] In one implementation, the method further includes:

[0025] Processing the depth image to obtain a top-layer depth image corresponding to the top layer of the target test tube rack, processing the top-layer depth image to obtain a test tube rack outline of the target test tube rack, and processing the test tube rack outline to obtain coordinates of corner points of each test tube hole within the test tube rack outline;

[0026] The test tube rack information is obtained according to the coordinates of all the test tube hole corner points.

[0027] In one implementation, obtaining the test tube rack information according to the coordinates of all the test tube hole corner points includes:

[0028] Calculating the distance between any two test tube hole corner points among all the test tube hole corner points, and when the distance is within a preset range, using the two test tube hole corner points corresponding to the distance as the diagonal points of the test tube hole;

[0029] After the distance between any two corner points of all the test tube holes is calculated, the initial coordinates of each of the test tube holes in the camera coordinate system are obtained;

[0030] Obtaining the initial coordinates of the target test tube rack according to the initial coordinates of each of the test tube holes;

[0031] Performing a world coordinate transformation on the initial coordinates of each of the test tube holes to obtain a final coordinate of each test tube hole, performing a world coordinate transformation on the initial coordinates of the target test tube rack to obtain a final coordinate of the target test tube rack, and saving the final coordinates of the target test tube rack and the final coordinates of each of the test tube holes as test tube rack information.

[0032] In one implementation, obtaining the initial coordinates of the target test tube rack according to the initial coordinates of each of the test tube holes includes:

[0033] Numbering all the test tube holes in the target test tube rack according to the initial coordinates of the test tube holes to obtain a position sequence number of each test tube hole;

[0034] The test tube well ranked first in the position sequence is used as the first test tube well, and the test tube well ranked last in the position sequence is used as the second test tube well, and initial coordinates of the first test tube well and the initial coordinates of the second test tube well are obtained;

[0035] Obtaining the initial coordinates of the target test tube rack according to the initial coordinates of the first test tube hole and the initial coordinates of the second test tube hole;

[0036] The first test tube hole and the second test tube hole are diagonal points of the target test tube rack.

[0037] The present invention also provides a test tube grabbing control device, the device comprising:

[0038] a posture adjustment module, configured to obtain test tube posture information of a target test tube and adjust the current posture of the gripper according to the test tube posture information;

[0039] a path generation module, configured to obtain test tube position information of a target test tube and test tube rack information corresponding to the test tube rack where the target test tube is located, and perform path planning based on the test tube position information and the test tube rack information to obtain a target path;

[0040] The test tube grasping module is used to control the gripper to move along the target path and grasp the target test tube in the current posture of the gripper.

[0041] The present invention also provides a terminal comprising: a memory, a processor, and a test tube grabbing control program stored in the memory and executable on the processor. When the test tube grabbing control program is executed by the processor, the steps of the test tube grabbing control method described above are implemented.

[0042] The present invention also provides a computer-readable storage medium storing a computer program. The computer program can be executed to implement the steps of the test tube grabbing control method described above.

[0043] The present invention provides a test tube grasping control method, comprising: obtaining test tube posture information of a target test tube, adjusting the current posture of a gripper according to the test tube posture information; obtaining test tube position information of the target test tube and test tube rack information corresponding to the test tube rack where the target test tube is located, performing path planning based on the test tube position information and the test tube rack information to obtain a target path; and controlling the gripper to move along the target path and grasping the target test tube with the gripper's current posture. The present invention adjusts the current posture of the gripper according to the test tube posture information of the target test tube, and performs path planning based on the test tube rack information and test tube position information, thereby improving the versatility and efficiency of grasping test tubes. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] FIG1 is a flow chart of a preferred embodiment of a method for controlling the grabbing of a test tube according to the present invention.

[0045] FIG2 is a schematic diagram of a point cloud of a test tube cover without plane fitting processing in the present invention.

[0046] FIG3 is a schematic diagram of a plane point cloud after fitting in the present invention.

[0047] FIG4 is a schematic diagram of a test tube rack obtained after processing the depth image in the present invention.

[0048] FIG5 is a functional principle block diagram of a preferred embodiment of the test tube grabbing control device of the present invention.

[0049] FIG6 is a functional block diagram of a preferred embodiment of the terminal in the present invention. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solutions and advantages of the present invention more clear and distinct, the present invention is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0051] The rapid growth of the pharmaceutical and chemical industries has led to a surge in demand for test tube handling. However, sorting such a large number of test tubes is a nightmare for workers. To solve this problem, a device for grabbing test tubes is urgently needed to replace manual operation.

[0052] Many robotic manipulation systems have been proposed that can sort different objects, and some can even analyze the affordance of objects. However, sorting test tubes is an unexplored problem for most robotic manipulation systems.

[0053] Currently, test tube grasping devices mainly involve object detection and grasping reasoning. Object detection includes instance segmentation and pose estimation of test tubes. In terms of instance segmentation, currently popular methods include using boxes to segment objects, followed by object bounding box segmentation. This method usually requires pre-setting the size and shape of the target, which limits the scope of application; in order to detect the object bounding box, sliding window technology and region-based technology can be used. This method requires classification and regression of each window or region in the image, which may result in slower detection speed. Methods relying on convolutional neural networks have shown promising results in complex instance segmentation challenges, but they have high requirements for training data and computing resources and are not good at detecting small objects.

[0054] In terms of pose estimation, deep learning-based methods use convolutional neural networks (CNNs) to generate six-degree-of-freedom (6DoF) poses and can classify poses into three categories. Viewpoint-based methods predict 6DoF poses by matching 3D or projecting 2D templates. However, these two deep learning-based methods require extensive pre-training, which affects their generalization and efficiency, making them unsuitable for test tube grasping. Keypoint-based methods detect keypoints of a given object and apply the perspective-n-point problem (PnP) to estimate the final pose. However, these methods are sensitive to occlusion and complex backgrounds and are ineffective for detecting small objects. Voting-based methods generate new points near the object center through voting. These points can be grouped and aggregated to generate poses. However, these methods are not effective for generating new points for complex object shapes and textures. Template matching is also a common pose estimation method, but it is very sensitive to minor environmental disturbances, which leads to a lack of generalization and limits its application.

[0055] In terms of grasping reasoning, deep learning-based methods extract rich features from image data and use a set of training objects to learn a mapping from this feature set to grasp quality. Some deep learning-based methods use synthetic data based on a set of 2D image features (edges and color) to learn grasping hypotheses. Some deep learning-based methods use edge or texture features or shape-based approximations to learn grasping. However, deep learning-based methods typically require a large amount of labeled data for training to obtain accurate mapping models, requiring significant time and human resources to collect and annotate data. Even if the model performs well on the training data, it may perform poorly on new, unseen data and be unable to adapt to new grasping scenarios. In addition to deep learning, reinforcement learning has also been applied to grasping reasoning. Some reinforcement learning methods have proposed grasping frameworks based on reinforcement learning (Q-learning), while others have proposed self-supervised grasping systems. However, most reinforcement learning-based methods focus on learning a single task and lack broad applicability. Methods based on geometric reasoning have also been proposed to determine grasps. These methods use an iterative approach to search for grasping poses, which is essentially a brute-force search method with low information utilization.

[0056] This invention primarily focuses on two aspects: object detection and grasping reasoning. Object detection involves detecting the position of the test tube rack containing the target test tube and the positions of all test tube holes within the test tube rack. Grasping reasoning involves estimating the pose of the target test tube and reasoning about the gripper's pose. For object detection, a corner localization method combining vision and depth is proposed to calculate the positions of the test tube rack and its holes. First, a depth filter is used to extract the top layer of the test tube rack from the depth image and save it as an RGB image. The Hough transform algorithm is then used to detect straight lines in the RGB image. Next, the Harris Corner method is used to obtain the intersections of these lines, and a density-based clustering algorithm (DBSCAN) is used to remove outliers. Furthermore, the positions of the test tube rack and its holes are calculated using the diagonal points of each test tube hole. The present invention separately calculates the positions of the target test tube, the test tube rack containing the target test tube, and all test tube holes. For grasping reasoning, a zero-search space reasoning method, assisted by pose estimation, is proposed to significantly accelerate grasping reasoning and adaptively adjust the gripper's pose. First, a random sampling consensus algorithm (RANSAC) is used to fit the point cloud plane existing around the test tube cap. Next, the normal of the point cloud in the plane is estimated using the principal component analysis (PCA) technique. This method is suitable for estimating the posture of vertical and randomly placed test tubes. Then, based on the posture of the test tube, the gripper adaptively adjusts its posture to grasp and place the test tube. During grasping, the axis of the gripper is aligned with the axis of the target test tube, automatically narrowing the search space for grasping reasoning to a specific posture, improving the versatility and work efficiency of this method. In addition, during motion planning and execution, the robotic arm will drive the gripper to approach and leave the target test tube along the axis of the target test tube.

[0057] In response to the above-mentioned shortcomings of the prior art, the present invention provides a test tube grasping control method, the method comprising: obtaining test tube posture information of a target test tube, and adjusting the current posture of a gripper according to the test tube posture information; obtaining test tube position information of the target test tube and test tube rack information corresponding to the test tube rack where the target test tube is located, performing path planning based on the test tube position information and the test tube rack information to obtain a target path; and controlling the gripper to move along the target path and grasping the target test tube with the gripper's current posture. The present invention adjusts the current posture of the gripper according to the test tube posture information of the target test tube, and performs path planning based on the test tube rack information and test tube position information, thereby improving the versatility and efficiency of grasping test tubes.

[0058] Referring to FIG1 , the test tube grabbing control method according to an embodiment of the present invention includes the following steps:

[0059] Step S100: Acquire test tube posture information of the target test tube, and adjust the current posture of the gripper according to the test tube posture information.

[0060] Specifically, a test tube rack containing a target test tube contains multiple test tubes, each positioned in a variety of positions. After determining the target test tube, test tube position information of the target test tube is obtained, and the current position of the gripper is adjusted based on the test tube position information. By adjusting the current position of the gripper based on the test tube position information, the present invention enables the gripper to accurately grasp test tubes in any position, avoiding the restrictive nature of templates and effectively improving the versatility of gripping test tubes.

[0061] In one implementation, before step S100, the following steps are further included:

[0062] Step A1: Acquire a depth image;

[0063] Step A2: Processing the depth image, using the target test tube rack as the target test tube rack, obtaining a test tube cap point cloud corresponding to each test tube in the target test tube rack, and processing each test tube cap point cloud to obtain test tube posture information and test tube position information corresponding to each test tube;

[0064] The depth image is obtained by photographing the target test tube rack with a depth camera.

[0065] Specifically, the present invention uses a depth camera to capture the target test tube rack to obtain a depth image, while the depth camera remains in a fixed position. The present invention processes the point cloud of each test tube cap to obtain the test tube posture information and test tube position information corresponding to each test tube, thereby facilitating subsequent grasping of the test tube.

[0066] In one implementation, step A2 includes the following steps:

[0067] Step B1: extracting the depth image using a depth filter to obtain a top-level depth image corresponding to the target test tube rack; saving the top-level depth image in RGB format to obtain a first image corresponding to the target test tube rack; processing the depth image and the first image to obtain a first point cloud corresponding to the target test tube rack; filtering and clustering the first point cloud to obtain a test tube cap point cloud corresponding to each test tube in the target test tube rack; obtaining an outer ring point cloud within each test tube cap point cloud; and fitting a plane to each outer ring point cloud to obtain a plane point cloud corresponding to each test tube.

[0068] Step B2: Obtain the coordinates of the centroid of the plane point cloud corresponding to each test tube, calculate the vectors from all points in the plane point cloud corresponding to each test tube to the centroid of the plane point cloud, save the vectors corresponding to each test tube in matrix form, obtain the original matrix corresponding to each test tube, process each original matrix separately to obtain the test tube posture matrix corresponding to each test tube, and store each test tube posture matrix as the test tube posture information corresponding to each test tube;

[0069] Step B3: obtaining the test tube initial coordinates corresponding to each test tube in the camera coordinate system according to the coordinates of the centroid of each plane point cloud, and performing world coordinate conversion on each test tube initial coordinate to obtain test tube position information of each test tube;

[0070] The abscissa of the initial test tube coordinates of each test tube is the abscissa of the center of mass of the plane point cloud corresponding to the test tube, and the ordinate of the initial test tube coordinates of each test tube is the ordinate of the center of mass of the plane point cloud corresponding to the test tube.

[0071] Specifically, the screening and clustering of the first point cloud to obtain the test tube cap point cloud corresponding to each test tube in the target test tube rack includes: using a straight-through filter to screen the first point cloud according to preset depth information to obtain a second point cloud; using a radius filter to remove noise from the second point cloud to obtain a third point cloud; using a density-based clustering algorithm (DBSCAN) to cluster the third point cloud to obtain the test tube cap point cloud corresponding to each test tube in the target test tube rack; wherein the second point cloud is the point cloud after the test tube rack is removed. The test tube cap point cloud corresponding to each test tube in the target test tube rack can be expressed as {D1, D2, D3, ..., D L}, where D l A point cloud representing each test tube.

[0072] Figure 2 shows the test tube cap point cloud before plane fitting. Because the structure of the central portion of the test tube cap point cloud cannot describe the test tube's posture, the density-based clustering algorithm (DBSCAN) was used to divide the test tube cap point cloud into an outer ring point cloud and an inner ring point cloud. The outer ring point cloud was obtained from each test tube cap point cloud, and a plane was fitted to each outer ring point cloud using the Random Sampling Consensus Algorithm (RANSAC) algorithm to obtain the plane point cloud corresponding to each test tube. The plane point cloud is shown in Figure 3. After fitting, the normal vectors of each point in the plane point cloud are consistent.

[0073] The coordinates of any point in the plane point cloud in the camera coordinate system include abscissa, ordinate, and vertical coordinates. Calculating the coordinates of the center of mass of the plane point cloud corresponding to each test tube includes: averaging the abscissas of all points in the plane point cloud corresponding to each test tube to obtain the abscissa of the center of mass of the plane point cloud corresponding to the test tube; averaging the ordinates of all points in the plane point cloud corresponding to each test tube to obtain the ordinate of the center of mass of the plane point cloud corresponding to the test tube; and averaging the vertical coordinates of all points in the plane point cloud corresponding to each test tube to obtain the vertical coordinate of the center of mass of the plane point cloud corresponding to the test tube. The coordinates of the center of mass of the plane point cloud are the coordinates in the camera coordinate system.

[0074] The centroid coordinates of the planar point cloud can be expressed as Among them, m is the coordinate of the center of mass of the plane point cloud, p i ∈R 3 is any point in the plane point cloud, and N is the number of points in the plane point cloud.

[0075] The vectors from all points in the plane point cloud to the center of mass of the plane point cloud are saved in matrix form to obtain the original matrix corresponding to each test tube. The original matrix is ​​expressed as V = [v1 v2…v N ]; where v i Represents the vector from any point in a planar point cloud to the centroid of the planar point cloud.

[0076] The processing of each of the original matrices to obtain a test tube posture matrix corresponding to each test tube includes:

[0077] Performing singular value decomposition on each of the original matrices to obtain a first matrix corresponding to each test tube;

[0078] Calculating the eigenvalues ​​of each of the first matrices, selecting a minimum eigenvalue among the eigenvalues, calculating an eigenvector corresponding to the minimum eigenvalue, and using the eigenvector as a normal vector of the plane point cloud corresponding to the test tube;

[0079] Obtaining the tilt angle and direction of each test tube according to the normal vectors of each plane point cloud;

[0080] The tilt angle and the direction of each test tube are calculated and saved in a matrix form to obtain a test tube posture matrix of each test tube.

[0081] Specifically, after performing singular value decomposition on the original matrix, the original matrix is ​​expressed as V=UΣW T ; Among them, U∈R 3×k , W T ∈R k×n represents an orthogonal matrix, Σ∈R k×k represents a rectangular diagonal matrix. The first matrix is ​​U. The plane point cloud normal vector is the eigenvector corresponding to the minimum eigenvalue of U. The direction of each plane point cloud normal vector is used as the axial direction of each test tube.

[0082] In one implementation, the point cloud used to estimate the plane point cloud normal vector comes from the test tube detection in Section III-B.

[0083] After obtaining the plane point cloud normal vector, we need to perform attitude estimation. The attitude estimation includes the tilt angle θ∈R of the test tube, the direction γ∈R of the test tube, and the attitude matrix The tilt angle, direction and attitude matrix of the test tube can be obtained according to the normal vector of the plane point cloud.

[0084] Specifically, the plane point cloud normal vector Expressed as

[0085] The inclination angle θ of the test tube is expressed as: The direction γ of the test tube is expressed as γ= The test tube posture matrix Expressed as

[0086] The initial coordinates of each test tube in the camera coordinate system are expressed as (X t ,Y t ); where X t is the horizontal coordinate of the center of mass of the plane point cloud corresponding to the test tube, Y t The vertical coordinate of the center of mass of the plane point cloud corresponding to the test tube is converted into the world coordinate to obtain the test tube coordinates of each test tube. The coordinates of each test tube are stored as position information of each test tube.

[0087] In one implementation, adjusting the current posture of the gripper according to the test tube posture information includes:

[0088] Multiplying the test tube posture matrix and the preset external parameter matrix to obtain the gripper posture matrix;

[0089] The current posture of the gripper is adjusted according to the gripper posture matrix.

[0090] Specifically, the test tube posture matrix is ​​a matrix in the camera coordinate system, which needs to be converted into a matrix in the world coordinate system to adjust the current posture of the gripper. The gripper posture matrix is ​​obtained by multiplying the test tube posture matrix and the preset camera extrinsic parameter matrix. The formula of the gripper posture matrix is Among them, the This is the camera's preset parameter matrix, used to convert the test tube pose matrix from camera coordinates to world coordinates. By adjusting the gripper's current pose based on the test tube's pose information, this method narrows the search space for grasping reasoning to a specific pose, improving the versatility and efficiency of grasping test tubes.

[0091] In one implementation, before adjusting the current posture of the gripper according to the test tube posture information, the method further includes:

[0092] The preset initial posture of the gripper is obtained, and the preset initial posture is used as the current posture of the gripper.

[0093] Specifically, the preset initial posture is used as the current posture of the gripper, and after the gripper posture matrix is ​​subsequently acquired, the current posture of the gripper is adjusted to match the posture of the test tube.

[0094] In one implementation, after adjusting the current posture of the gripper according to the test tube posture information, the method further includes:

[0095] Obtaining a homogeneous matrix of the end effector in a world coordinate system according to the gripper posture matrix, the target test tube coordinates, and the gripper length matrix;

[0096] The current posture of the end effector is adjusted according to the homogeneous matrix.

[0097] Specifically, the homogeneous matrix is ​​expressed as Among them, the L gripper is the gripper length matrix, expressed as L gripper =[0 0 l], l is the length of the gripper.

[0098] In one implementation, the test tube grabbing control method further includes the following steps:

[0099] Step C1: Processing the depth image to obtain a top-layer depth image corresponding to the top layer of the target test tube rack, processing the top-layer depth image to obtain a test tube rack outline of the target test tube rack, and processing the test tube rack outline to obtain coordinates of corner points of each test tube hole within the test tube rack outline;

[0100] Step C2: obtaining the test tube rack information according to the coordinates of all the test tube hole corner points.

[0101] Specifically, the depth image is processed using a straight-through filter to obtain a top-layer depth image corresponding to the top layer of the target test tube rack. The salt and pepper noise of the top-layer depth image is removed using a median filter to obtain a second top-layer depth image. The contour of the second top-layer depth image is recognized using Canny edge detection to obtain the test tube rack contour of the target test tube rack.

[0102] The processing of the test tube rack outline to obtain the coordinates of the corner points of each test tube hole within the test tube rack outline includes:

[0103] The straight lines in the test tube rack outline are detected using the Hough transform algorithm to obtain multiple straight lines corresponding to the edges of each test tube hole in the test tube rack outline; the intersection points between all the straight lines are extracted using the Harris Corner method; the density-based clustering algorithm (DBSCAN) is used to cluster all the intersection points to obtain four clusters corresponding to each test tube hole; wherein the cluster is represented as Calculate the average coordinates of all points in each cluster to obtain the cluster center coordinates of each cluster The cluster centers are taken as the corner points of the test tube holes.

[0104] Please refer to Figure 4, which shows a schematic diagram of the test tube rack obtained after processing the depth image. Since multiple straight lines are extracted from the edge of each test tube well, there are multiple intersections between these lines. A density-based clustering algorithm is used to cluster all of these intersections to obtain four clusters corresponding to each test tube well. The center of each cluster is used as the test tube well corner point of the test tube well.

[0105] By processing the depth image, the present invention obtains four clusters corresponding to each test tube hole, that is, four corner points corresponding to each test tube hole, which can be accurately used for subsequent test tube rack information estimation, thereby improving the accuracy of test tube grasping and further improving the efficiency of test tube grasping.

[0106] In one implementation, obtaining the test tube rack information according to the coordinates of all the test tube hole corner points includes:

[0107] Calculating the distance between any two test tube hole corner points among all the test tube hole corner points, and when the distance is within a preset range, using the two test tube hole corner points corresponding to the distance as the diagonal points of the test tube hole;

[0108] After the distance between any two corner points of all the test tube holes is calculated, the initial coordinates of each of the test tube holes in the camera coordinate system are obtained;

[0109] Obtaining the initial coordinates of the target test tube rack according to the initial coordinates of each of the test tube holes;

[0110] Performing a world coordinate transformation on the initial coordinates of each of the test tube holes to obtain a final coordinate of each test tube hole, performing a world coordinate transformation on the initial coordinates of the target test tube rack to obtain a final coordinate of the target test tube rack, and saving the final coordinates of the target test tube rack and the final coordinates of each of the test tube holes as test tube rack information.

[0111] Specifically, the distance between any two test tube hole corner points among all the test tube hole corner points is calculated, and the formula for the distance is: in, and Are any corner points C p The coordinates of the arbitrary corner point C q 's coordinates.

[0112] The initial coordinates of each test tube hole are transformed into world coordinates to obtain the final coordinates of each test tube hole. Convert the initial coordinates of the target test tube rack into world coordinates to obtain the final coordinates of the target test tube rack

[0113] In the present invention, the test tube rack information of the target test tube rack is obtained based on the coordinates of the corner points of all the test tube holes. Therefore, no matter the position of the test tube rack is not restricted, as long as it is below the depth camera, the position of the target test tube rack and the positions of each test tube hole in the test tube rack can be calculated, thereby improving the versatility of test tube grasping. At the same time, the test tube rack information of the target test tube rack can be accurately obtained by using the cluster center coordinates (i.e., the test tube hole corner point coordinates), thereby improving the accuracy of test tube grasping and further improving the efficiency of test tube grasping.

[0114] The coordinates of each of the test tube holes in the camera coordinate system are expressed as in, and are the coordinates of the diagonal point of any test tube hole.

[0115] The initial coordinates of the test tube hole include a horizontal coordinate and a vertical coordinate. The initial coordinates of the test tube hole are obtained by averaging the horizontal coordinates of the diagonal points corresponding to the test tube hole and averaging the horizontal coordinates of the diagonal points corresponding to the test tube hole.

[0116] In one implementation, obtaining the initial coordinates of the target test tube rack according to the initial coordinates of each of the test tube holes includes:

[0117] Numbering all the test tube holes in the target test tube rack according to the initial coordinates of the test tube holes to obtain a position sequence number of each test tube hole;

[0118] The test tube well ranked first in the position sequence is used as the first test tube well, and the test tube well ranked last in the position sequence is used as the second test tube well, and initial coordinates of the first test tube well and the initial coordinates of the second test tube well are obtained;

[0119] Obtaining the initial coordinates of the target test tube rack according to the initial coordinates of the first test tube hole and the initial coordinates of the second test tube hole;

[0120] The first test tube hole and the second test tube hole are diagonal points of the target test tube rack.

[0121] Specifically, the initial coordinates of the target test tube rack are expressed as (X r ,Y r ),in, (u1,v1) is the coordinate of the first test tube hole, (u -1 ,v -1 ) are the coordinates of the second test tube hole.

[0122] The initial coordinates of the target test tube rack include a horizontal coordinate and a vertical coordinate. The initial coordinates of the target test tube rack are obtained by averaging the horizontal coordinates of the first test tube hole and the horizontal coordinates of the second test tube hole, and averaging the vertical coordinates of the first test tube hole and the vertical coordinates of the second test tube hole.

[0123] The present invention uses the initial coordinates of each test tube hole to determine the coordinates of the target test tube rack. This allows the test tube rack's position to be unrestricted by the template, allowing the coordinates of the target test tube rack to be calculated regardless of its placement. This improves the versatility of test tube grasping and facilitates subsequent path planning. Because the gripper of the present invention can adjust its current posture based on the test tube's posture and position information, it can accurately grasp test tubes in any posture, improving the efficiency and versatility of test tube grasping.

[0124] As shown in FIG1 , the test tube grabbing control method further includes the following steps:

[0125] Step S200: Obtain test tube position information of a target test tube and test tube rack information corresponding to the test tube rack where the target test tube is located, perform path planning based on the test tube position information and the test tube rack information, and obtain a target path.

[0126] Specifically, the test tube coordinates of the target test tube and the coordinates of the test tube rack where the target test tube is located are obtained, and path planning is performed based on the test tube coordinates and the test tube rack coordinates in the world coordinate system to obtain a target path. The present invention performs path planning based on the test tube coordinates and the test tube rack coordinates, effectively and accurately grasping the test tube, thereby improving the efficiency of test tube grasping.

[0127] In one implementation, the target path is obtained by the test tube coordinates, the test tube rack coordinates, and preset target test tube hole coordinates.

[0128] Specifically, the preset target test tube hole is used to grab the test tube and place it.

[0129] As shown in FIG1 , the test tube grabbing control method further includes the following steps:

[0130] Step S300: Control the gripper to move along the target path and grasp the target test tube in the current posture of the gripper.

[0131] Specifically, the mechanical device for grasping in the present invention includes a gripper and a robotic arm; the robotic arm includes an end effector, the end effector is located at the end of the robotic arm, and the gripper is connected to the end effector. The gripper on the robotic arm is controlled to move along the target path, the current posture of the end effector is maintained, and the target test tube is grasped in the current posture of the gripper, wherein the axial direction of the gripper is consistent with the axial direction of the target test tube. The present invention controls the gripper to move along the target path and grasps the target test tube in the current posture of the gripper, thereby improving the efficiency of grasping the test tube.

[0132] In one implementation, after the target test tube is grasped, it is moved along a target path and vertically placed in a preset target test tube hole.

[0133] In order to verify the versatility and high efficiency of the test tube grasping control method of the present invention, a test tube grasping device in the prior art was selected as a comparison device for comparison. Two experiments were used to illustrate the success rate of test tube grasping of the device of the present invention and the comparison device and the execution time of different modules.

[0134] In this comparison, the present invention was conducted in a vision-based robotic system consisting of a UR3e robotic arm, a two-finger parallel gripper (Robotiq), a 1920×1200 resolution 3D camera (Mech Eye LOG M Industrial), and a PC running Ubuntu 20.04 LTS (CPU: Intel Core i9 11900K, RAM: 32GB). The 3D camera was located in a fixed position above the robotic arm and the test tube rack and connected to the PC. The robotic arm was connected to the PC and included an end effector at the end of the robotic arm, on which the gripper was located.

[0135] First, the robot successfully completes the test tube sorting task if it can pick up the target test tube and place it into the target test tube hole in the target test tube rack. Then, the test tubes are distributed on the test tube rack in vertical and random positions. The device of the present invention can grasp test tubes in any position.

[0136] After the preparatory work was completed, the device of the present invention and the comparison device were each used to perform test tube sorting tasks. In a comparison of versatility, as shown in Table 1, a total of 35 vertical test tube sorting tasks were performed, with the device of the present invention having a success rate of 100% and the comparison device having a success rate of 91.4%. A total of multiple random test tube sorting tasks were performed, with the device of the present invention having a success rate of 96.6% and the comparison device having a success rate of 0%. Because the comparison device uses template matching to obtain the test tube's posture, it assumes that the posture of the actual test tube is the same as the pre-stored template. However, in reality, test tubes with random postures and pre-stored templates are not identical. Clearly, the system of the present invention is more versatile in test tube sorting tasks.

[0137] Table 1

[0138] In a comparison of test tube grasping efficiency, the time spent on perception and grasping reasoning for the device of the present invention and the comparison device was evaluated 10 times. The time required to detect the target test tube hole on the test tube rack was measured from the moment the depth image was acquired until the location of the target test tube hole was determined. The time required for pose estimation was evaluated from the time the point cloud of all test tubes was acquired to the time the target test tube's pose was determined. Similarly, the time required for reasoning was calculated from the time the target test tube's pose was acquired to the time the gripper's final pose was determined. The average time for each system over the 10 evaluations is shown in Table 2. The device of the present invention completed target test tube hole detection in only 5.39 seconds and target test tube pose estimation in 3.62 seconds. Because the gripper's pose is adaptively adjusted based on the estimated test tube pose, no time is spent on grasping reasoning. However, due to the large search space, the comparison device required 6.38 seconds to detect the target test tube hole and 8.26 seconds to perform grasping reasoning. The test tube classification network of the comparison device has not yet been released, so the time cost of pose estimation for the comparison device was not evaluated.

[0139] Table 2

[0140] In general, in 10 operations, the device of the present invention spent an average of 9.01 seconds on perception and grasping reasoning, while the comparison device spent an average of 14.64 seconds. The efficiency of the device of the present invention in perception and grasping reasoning was improved by 38.46%.

[0141] The present invention utilizes geometric principles to improve the versatility and efficiency of test tube grasping. A corner point positioning method combining vision and depth is proposed to detect the position of the target test tube rack and the positions of all test tube holes on the target test tube rack. For grasping reasoning, the posture of the end effector and the gripper can be adaptively adjusted based on the estimated test tube posture, effectively improving the versatility and efficiency of test tube grasping.

[0142] In one implementation, as shown in FIG5 , based on the above-mentioned test tube grasping control method, the present invention also provides a test tube grasping control device, including:

[0143] The posture adjustment module 100 is used to obtain the test tube posture information of the target test tube and adjust the current posture of the gripper according to the test tube posture information;

[0144] The path generation module 200 is configured to obtain the test tube position information of a target test tube and the test tube rack information corresponding to the test tube rack where the target test tube is located, and perform path planning based on the test tube position information and the test tube rack information to obtain a target path;

[0145] The test tube grasping module 300 is used to control the gripper to move along the target path and grasp the target test tube in the current posture of the gripper.

[0146] In one implementation, the present invention also provides a terminal, as shown in Figure 6, comprising: a memory 20, a processor 10, and a test tube grabbing control program 30 stored in the memory 20 and executable on the processor 10. When the test tube grabbing control program 30 is executed by the processor 10, the steps of the test tube grabbing control method described above are implemented.

[0147] The present invention also provides a computer-readable storage medium storing a computer program. The computer program can be executed to implement the steps of the test tube grabbing control method described above.

[0148] In summary, the present invention provides a test tube grasping control method, the method comprising: obtaining test tube posture information of a target test tube, adjusting the current posture of a gripper according to the test tube posture information; obtaining test tube position information of the target test tube and test tube rack information corresponding to the test tube rack where the target test tube is located, performing path planning based on the test tube position information and the test tube rack information to obtain a target path; and controlling the gripper to move along the target path and grasping the target test tube with the gripper's current posture. The present invention adjusts the current posture of the gripper according to the test tube posture information of the target test tube, and performs path planning based on the test tube rack information and test tube position information, thereby improving the versatility and efficiency of grasping test tubes.

[0149] It should be understood that the application of the present invention is not limited to the above examples. For those skilled in the art, improvements or changes can be made based on the above description. All these improvements and changes should fall within the scope of protection of the claims attached to the present invention.

Claims

1. A test tube grabbing control method, characterized in that: The method comprises: Acquire test tube posture information of the target test tube, and adjust the current posture of the gripper according to the test tube posture information; Acquire the test tube position information of the target test tube and the test tube rack information corresponding to the test tube rack where the target test tube is located, perform path planning according to the test tube position information and the test tube rack information, and obtain the target path; The gripper is controlled to move along the target path, and the target test tube is grasped in the current posture of the gripper.

2. The test tube grabbing control method according to claim 1, characterized in that: Before acquiring the test tube posture information of the target test tube and adjusting the current posture of the gripper according to the test tube posture information, the method further includes: Get the depth image; Processing the depth image, taking the target test tube rack as the target test tube rack, obtaining a test tube cover point cloud corresponding to each test tube in the target test tube rack, and processing each of the test tube cover point clouds to obtain test tube posture information and test tube position information corresponding to each test tube; The depth image is obtained by photographing the target test tube rack with a depth camera.

3. The test tube grabbing control method according to claim 2, characterized in that: The processing of the depth image, taking the target test tube rack as the target test tube rack, obtaining a test tube cover point cloud corresponding to each test tube in the target test tube rack, and processing each of the test tube cover point clouds to obtain test tube posture information and test tube position information corresponding to each test tube, includes: Use a depth filter to extract the depth image to obtain a top-level depth image corresponding to the target test tube rack, save the top-level depth image in RGB format, obtain a first image corresponding to the target test tube rack, process the depth image and the first image to obtain a first point cloud corresponding to the target test tube rack, screen and cluster the first point cloud to obtain a test tube cover point cloud corresponding to each test tube in the target test tube rack, obtain an outer ring point cloud in each of the test tube cover point clouds, fit a plane to each of the outer ring point clouds, and obtain a plane point cloud corresponding to each test tube; Obtain the coordinates of the centroid of the plane point cloud corresponding to each test tube, calculate the vectors from all points in the plane point cloud corresponding to each test tube to the centroid of the plane point cloud, save the vectors corresponding to each test tube in matrix form, obtain the original matrix corresponding to each test tube, process each of the original matrices respectively, obtain the test tube posture matrix corresponding to each test tube, and store each of the test tube posture matrices as the test tube posture information corresponding to each test tube; According to the coordinates of the centroid of each plane point cloud, the initial coordinates of each test tube corresponding to the test tube in the camera coordinate system are obtained. Convert the initial coordinates of each test tube into world coordinates to obtain test tube position information of each test tube; Among them, the horizontal coordinate of the test tube initial coordinate of each test tube is the horizontal coordinate of the centroid of the plane point cloud corresponding to the test tube, and the vertical coordinate of the test tube initial coordinate of each test tube is the vertical coordinate of the centroid of the plane point cloud corresponding to the test tube.

4. The test tube grabbing control method according to claim 3, characterized in that: The adjusting the current posture of the gripper according to the test tube posture information comprises: Multiplying the test tube posture matrix and the preset external parameter matrix to obtain a gripper posture matrix; The current posture of the gripper is adjusted according to the gripper posture matrix.

5. The test tube grabbing control method according to claim 3, characterized in that: The method further comprises: Processing the depth image to obtain a top-layer depth image corresponding to the top layer of the target test tube rack, processing the top-layer depth image to obtain a test tube rack contour of the target test tube rack, and processing the test tube rack contour to obtain coordinates of corner points of each test tube hole within the test tube rack contour; The test tube rack information is obtained according to the coordinates of all the test tube hole corner points.

6. The test tube grabbing control method according to claim 5, characterized in that: The step of obtaining the test tube rack information according to the coordinates of all the test tube hole corner points includes: Calculating the distance between any two test tube hole corner points among all the test tube hole corner points, and when the distance is within a preset range, taking the two test tube hole corner points corresponding to the distance as the diagonal points of the test tube hole; When the distance between any two corner points of all the test tube holes is calculated, the initial coordinates of each of the test tube holes in the camera coordinate system are obtained; Obtaining the initial coordinates of the target test tube rack according to the initial coordinates of each of the test tube holes; The initial coordinates of each of the test tube holes are transformed into world coordinates to obtain the final coordinates of each of the test tube holes, the initial coordinates of the target test tube rack are transformed into world coordinates to obtain the final coordinates of the target test tube rack, and the final coordinates of the target test tube rack and the final coordinates of each of the test tube holes are saved as test tube rack information.

7. The test tube grabbing control method according to claim 6, characterized in that: The step of obtaining the initial coordinates of the target test tube rack according to the initial coordinates of each of the test tube holes comprises: According to the initial coordinates of each of the test tube holes, all of the test tube holes in the target test tube rack are numbered to obtain a position sequence number of each of the test tube holes; The test tube hole ranked first in the position sequence is used as the first test tube hole, and the test tube hole ranked last in the position sequence is used as the second test tube hole, and the initial coordinates of the first test tube hole and the initial coordinates of the second test tube hole are obtained; Obtaining the initial coordinates of the target test tube rack according to the initial coordinates of the first test tube hole and the initial coordinates of the second test tube hole; Wherein, the first test tube hole and the second test tube hole are diagonal points of the target test tube rack.

8. A test tube grabbing control device, characterized in that: include: A posture adjustment module, used to obtain test tube posture information of the target test tube, and adjust the current posture of the gripper according to the test tube posture information; A path generation module, used to obtain the test tube position information of the target test tube and the test tube rack information corresponding to the test tube rack where the target test tube is located, and perform path planning according to the test tube position information and the test tube rack information to obtain the target path; The test tube grasping module is used to control the gripper to move along the target path and grasp the target test tube in the current posture of the gripper.

9. A terminal, characterized in that: include: A memory, a processor, and a test tube grabbing control program stored in the memory and executable on the processor, wherein the test tube grabbing control program, when executed by the processor, implements the steps of the test tube grabbing control method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program can be executed to implement the steps of the test tube grabbing control method according to any one of claims 1 to 7.

Images