A robot 3D vision guided grasping method

Abstract

The application discloses a kind of robot 3D visual guidance grabbing method, including the following steps: 1) hand-eye calibration is carried out to system, under the condition that known robot end effector before and after two relative poses, and the relative pose of calibration board under camera coordinate system before and after two times, solve the hand-eye relationship matrix of robot, and introduce the cooperation scheme of multiple cameras under limited receptive field;2) real-time scene point cloud information acquisition;3) the above-mentioned scene point cloud and the point cloud under the teaching photographing pose are unified in coordinate system, that is, the coordinate system of scene point cloud is transformed;4) the model point cloud to be grabbed is matched to the object point cloud in actual scene, identifies and obtains the coordinate conversion relationship between them, then model point cloud is used instead of scene point cloud to calculate subsequent grabbing pose, and the pose result is fine-tuned using iterative closest point algorithm;5) the final grabbing pose of robot is calculated.

Description

Technical Field

[0001] This invention relates to the field of machine vision technology, and more specifically to a robot 3D vision-guided grasping method. Background Technology

[0002] Against the backdrop of China's industrial transformation and upgrading, the use of machine vision and industrial robots to intelligently transform and upgrade many industries has become an inevitable trend. In the theoretical field of machine vision and the application field of robotics, hand-eye calibration, visual recognition and detection, and automated grasping by robotic arms are the core research objects.

[0003] The hand-eye calibration problem in robotic systems can be traced back to the late 20th century. Since Shiu and Ahmad first proposed a method for solving matrix equations in the hand-eye calibration problem in the 1980s, it has attracted strong attention from scholars worldwide. Numerous hand-eye calibration methods have emerged for single or diverse application scenarios. In 2002, Tsai proposed a new analytical algorithm for hand-eye calibration and, for the first time, presented the selection criteria for hand-eye calibration datasets using this method. Fassi and Legnani proposed an algorithm for solving the hand-eye calibration matrix formula from a geometric perspective, while also investigating over-constraints and singularities in the algorithm. Other hand-eye calibration algorithms include, but are not limited to, Euclidean group theory methods, quaternion methods, dual quaternion methods, matrix direct product methods, and spiral motion methods. Furthermore, some new optimization methods, such as convex optimization methods and global optimization methods, have been used to solve the hand-eye calibration matrix. The above algorithm first needs to obtain the calibration data by moving the robot end effector along its trajectory in space, and then substitute the collected calibration dataset into the calibration algorithm for solving.

[0004] Automatic grasping by robotic arms is generally divided into two-dimensional planar grasping and six-degree-of-freedom (6DOF) grasping. In two-dimensional planar grasping, the object is placed on a plane, and the end effector directly performs the grasping operation from a single direction perpendicular to the plane. This type of grasping method only needs to sense the two-dimensional planar coordinates of the object and the rotation angle of the end effector, and determine the height and direction of grasping through the relative positional relationship between the robotic arm and the object's placement plane. However, two-dimensional planar grasping is only suitable for objects placed on planes such as conveyor belts and tabletops, which greatly limits its application scenarios. Six-degree-of-freedom (6DOF) grasping methods are applicable to objects in any pose in three-dimensional space, and can be used in a wider range of scenarios such as object interaction and human-machine collaboration, and are gradually becoming the mainstream of robotic arm grasping research. The goal of six-degree-of-freedom grasping is to obtain the 6D pose of the object's coordinate system in the robotic arm's coordinate system. Therefore, accurately sensing the 6D pose of the object in space is becoming a key issue in robotic arm grasping research. Summary of the Invention

[0005] To address the aforementioned problems, this invention proposes a 3D vision-guided grasping method for robots. First, a hand-eye calibration process is performed on the robot system. After obtaining the calibration matrix between the hand and eye, real-time scene point clouds are acquired. Next, the point cloud coordinate system is transformed and point cloud matching is performed. After obtaining the matching result, the final robot grasping pose can be calculated.

[0006] The technical solution adopted by this invention to solve its technical problem is: a robot 3D vision-guided grasping method, comprising the following steps:

[0007] Step 1) Hand-eye calibration; Calibration is performed in the entire 3D space for both "eye on the hand" and "eye outside the hand" positions; The robot arm pose is obtained by converting the Euler angles of the 6D pose data output by the robot arm into a rotation matrix; The camera pose is solved using Zhang's camera calibration and PnP algorithm; After obtaining the pose data of the two, they are substituted into the solution equation A·X=X·B for solving, where A is the robot arm pose, B is the camera pose, and X is the matrix to be solved in the hand-eye calibration problem;

[0008] Step 2) Scene point cloud information collection;

[0009] Step 3) Point cloud coordinate system transformation; After the RGBD camera acquires and transmits the scene point cloud, the coordinate system of the current point cloud is transformed, and the algorithm is used to unify the coordinate system of the point cloud and the point cloud under the teaching photo pose.

[0010] Step 4) Point cloud matching; First, match the point cloud of the model to be captured to the point cloud of the object in the actual scene, identify and obtain the coordinate transformation relationship between the two, and then use the model point cloud to replace the scene point cloud for subsequent capture pose calculation; Finally, use the iterative nearest point algorithm to fine-tune the pose result of the point cloud matching.

[0011] Step 5) Calculate the robotic arm grasping pose; divide the final 6D pose of the robotic arm required for grasping into three degrees of freedom of displacement and three degrees of freedom of spatial rotation, and calculate them separately.

[0012] The specific steps for hand-eye alignment in step 1) are as follows:

[0013] A multi-camera collaborative scheme is introduced under limited receptive field conditions. Specifically, one industrial camera is fixed at the gripper of the robotic arm, and another auxiliary positioning camera is fixed at a fixed point in the space outside the robotic arm. Based on the relative positional relationship between the hand and eye, the above two-hand-eye calibration scenarios can be defined as "eye on the hand" and "eye outside the hand," and their characteristics and differences are as follows:

[0014] (1) Eyes on hands

[0015] In this scenario, the camera is fixed to the gripper of the robotic arm, and the calibration object is placed at a fixed point within the camera's field of view. Using the transformation between the two coordinate systems of the robotic arm base and the camera as the target descriptor, the following transformation relationship can be obtained: base H gripper · gripper H cam = base H cal · cal H cam In this formula, base refers to the coordinate system of the robot arm base, gripper refers to the coordinate system of the robot arm gripper, cam refers to the coordinate system of the camera, and cal refers to the world coordinate system of the calibration object. base H gripper This represents the homogeneous transformation matrix from the robot arm base coordinate system to the robot arm gripper coordinate system. gripper H cam This represents the homogeneous transformation matrix from the robotic arm's gripper coordinate system to the camera coordinate system. base H cal This represents the homogeneous transformation matrix from the robot arm's base coordinate system to the calibration object's world coordinate system. cal H cam Let represent the homogeneous transformation matrix from the world coordinate system of the calibration object to the camera coordinate system; in this scenario, the quantity to be solved in the hand-eye calibration problem is a fixed quantity—the transformation matrix from the robotic arm gripper to the camera coordinate system. gripper H cam Since the relative positions of the calibration object and the robotic arm base remain unchanged in the scene, the transformation matrix from the robotic arm base to the world coordinate system of the calibration object... base H cal Since it is a fixed quantity, it can be eliminated by two sets of pose relationships. base H cal Establish the equation: base H gripper 0 · gripper H cam ·( cal H cam 0 ) -1 = base H gripper 1 · gripper H cam ·( cal H cam 1 ) -1 After sorting, we can obtain ( base H gripper 1 ) -1 · base H gripper 0 ·gripper H cam = gripper H cam ·( cal H cam 1 ) -1 · cal H cam 0 Its shape is A·X=X·B;

[0016] (2) Eyes outside hands

[0017] In this scenario, the calibration object is fixed at the gripper of the robotic arm, and the camera is placed at a fixed point within the space where its movement can be covered by the camera's field of view. Using the transformation between the two coordinate systems of the robotic arm base and the calibration object as the target descriptor, the following transformation relationship can be obtained: base H gripper · gripper H cal = base H cam · cam H cal In this scenario, the quantity to be solved in the hand-eye calibration problem is a fixed quantity—the transformation matrix from the robotic arm base to the camera coordinate system. base H cam Since the relative positions of the calibration object and the robotic arm gripper remain unchanged in the scene, the equations can be obtained by solving the equations simultaneously using the two sets of pose relationships and then rearranging them: base H gripper 1 ·( base H gripper 0 ) -1 · base H cam = base H cam · cam H cal 1 ·( cam H cal 0 ) -1 This equation also conforms to the form A·X=X·B;

[0018] Therefore, the hand-eye calibration problem can be transformed into solving the 4*4 homogeneous matrix X in the matrix equation A·X=X·B;

[0019] The robot arm pose can be obtained by converting the Euler angles of the 6D pose data output by the robot arm into a rotation matrix representation. The camera pose solution uses Zhang's camera calibration and the PnP (Perspective-n-Point) algorithm.

[0020] Specifically, the algorithm for determining the camera pose is as follows: A checkerboard calibration board is used as the calibration object; first, the camera intrinsic parameters are calibrated, corner points are extracted, and the corner point coordinates are refined to sub-pixel precision. Zhang's calibration algorithm is used to obtain the camera intrinsic parameters and distortion coefficients. During image distortion correction, the results of the camera calibration are used to remove distortion from the image, and sub-pixel precision corner point coordinates, i.e., 2D point coordinates, are extracted again. The world coordinate system is defined on the calibration board, and the 3D coordinates corresponding to the m x n corner points are (m*object_size.width, n*object_size.height, 0). Given the 3D coordinates of the point set, the corresponding 2D coordinates, and the camera intrinsic parameters, the perspective projection model is established using the PnP algorithm. The rotation and translation matrices from the world coordinate system to the camera coordinate system, i.e., the camera pose, can be solved.

[0021] After obtaining the robot arm pose and camera pose, substitute them into the equation A·X=X·B and use the OpenCV:calibrateHandEye operator to solve it. For the obtained hand-eye matrix, the matrix accuracy is evaluated by calculating the reprojection error: the calculation of the reprojection error requires two sets of pose data. One set is taken as the true value to estimate the pose of the other camera. The pose and the 3D point set are reprojected to obtain the coordinates of the 2D point set on the image plane. The MSE error value between the projected point coordinates and the corner point coordinates is calculated.

[0022] Step 2) Scene point cloud information acquisition includes: after the hand-eye calibration step, an RGBD camera is used to acquire the point cloud of the object image. During this process, RGB color image and depth image information can be obtained simultaneously, and the point cloud information of the scene can be obtained.

[0023] Step 3) Point cloud coordinate system transformation includes the following steps:

[0024] After the RGBD camera acquires and transmits the scene point cloud, it is necessary to unify the coordinate system of this point cloud with the point cloud under the teaching photography pose. The algorithm for transforming the coordinate system of the scene point cloud is as follows: First, multiply the eye-to-hand calibration matrix with the eye on the hand by the pose under the teaching photography pose to obtain the transformation matrix from the camera position to the robot arm base under the teaching photography pose. Then, invert this matrix to obtain the pose transformation matrix 1 from the robot arm base to the teaching photography point. Similarly, multiply the eye-to-hand calibration matrix with the eye on the hand by the pose under the scene point cloud pose to obtain the transformation matrix 2 from the camera position to the robot arm base under the real-time photography pose. Multiply the above matrix 2 by matrix 1 by the left to finally transform the coordinate system of the scene point cloud to the coordinate system under the teaching photography pose.

[0025] Step 4) Point cloud matching includes the following steps:

[0026] Under the premise of completing the coordinate system transformation of the scene point cloud, a point-pair feature-based matching method is selected for recognition and pose estimation. It matches the model point cloud to be captured to the object point cloud in the actual scene, identifies and obtains the coordinate transformation relationship between the two, and then uses the model point cloud to replace the scene point cloud for subsequent capture pose calculation. Finally, the pose result is fine-tuned using the iterative nearest point algorithm.

[0027] Point-pair features are the foundation of this algorithm. They are calculated from two spatial points with normal vectors. A point-pair feature is an eigenvector composed of four feature quantities.

[0028] Given that the spatial coordinates of the two points forming a point pair are m1 and m2, and their normal vectors are n1 and n2, the feature of the point pair is defined as: F(m i m j )=[||d||2,∠(n1,d),∠(n2,d),∠(n1,n2)];where,||d||2 represents the Euclidean distance between two points, and ∠(,) represents the angle between two normal vectors, and ∠(a,b)∈[0,π];

[0029] When point pairs with identical feature values ​​are found in both the model point cloud and the actual scene point cloud, the spatial transformation relationship between the two point pairs can be calculated. The four values ​​in the point pair's feature are discretized and represented as follows: Set two parameters d dist and d angle =2π / n angle , where n angle The discrete number of angles;

[0030] The complete matching is divided into an offline training phase and an online running phase. In the training phase, the matching algorithm extracts the above features from the model point cloud, quantizes them, and stores them in a hash table. In the running phase, the algorithm extracts the above feature vectors from the scene point cloud, searches in the hash table, and calculates the pose after finding the corresponding feature point pair.

[0031] Based on the existing four-dimensional features, the three-dimensional point cloud is sampled at two more resolutions, and the sparse and dense resolution features are concatenated to form a total of six-dimensional features. The model hash table is the key to linking the model point cloud and the scene point cloud. For the calculated six-dimensional point pair features, the MurmurHash3 algorithm is used to calculate the key value of the hash table, and then the point pair information is stored or queried under the corresponding key value.

[0032] A voting mechanism is applied to discretize the pose estimation results. The pose results calculated for different point pairs are accumulated through voting, and the quantized pose value with the highest number of votes is taken as the final result. In this voting mechanism, m... rα is a reference point for extracting point pair features from the scene point cloud. i The pose variables are discretized. When calculating the pose between two pairs of points, an intermediate coordinate system is constructed using the geometric constraints of each vector on the feature of the point pair, simplifying the six-degree-of-freedom pose calculation to a single angle α. i To indicate;

[0033] After obtaining the initial pose through coarse matching using the above algorithm, a fine matching process is performed using the iterative nearest point method to obtain the accurate pose result. Specifically, the iterative nearest point algorithm is used for matching 3D points. By finding the relationship between corresponding registration points of two point sets, it calculates the transformation parameters between point sets that satisfy a given convergence accuracy, i.e., the rotation and translation relationship. Its basic steps are as follows: from the two sets of point clouds to be registered, select initial iterative point pairs, use the initial corresponding point pairs to estimate the rigid body transformation, transform the points in the template point cloud to the target point cloud coordinate system, obtain the transformed point cloud, and then search again. Find the point in the target point cloud that is closest to the transformed point cloud and update the correspondence between the point clouds. The rigid body transformation is estimated to maximize the overlap between the template point cloud and the target point cloud. Repeat the above steps until the stopping criterion is met. The stopping criterion can be that the number of iterations reaches a set threshold or the change in the rigid body transformation is less than a set threshold. Obtain the final rigid body transformation when the iteration stops. Combined with the object pose of the coarse matching, calculate the rotation matrix R and translation vector t that minimize the error function. These are the optimal matching parameters, which describe the optimal matching relationship between the template point cloud and the target point cloud.

[0034] Step 5) Calculating the robotic arm's grasping pose includes the following steps:

[0035] After obtaining the point cloud matching results, the final grasping pose of the robot is calculated; the 6D pose result is divided into two parts (three degrees of freedom of displacement and three degrees of freedom of spatial rotation) and calculated separately.

[0036] The displacement calculation process for the three degrees of freedom is as follows: multiply the coordinates of the teaching grasp point in the camera coordinate system by the translation matrix of the point to be grasped relative to the template in the camera coordinate system, and then multiply by the pose matrix of the camera in the base coordinate system, expressed as: base H cam · template T target · cam T teach pose ,in, base H cam This represents the pose matrix of the camera in the base coordinate system. template T target This represents the translation matrix of the point to be captured relative to the template in the camera coordinate system. cam T teach poseThis indicates the coordinates of the teaching capture point in the camera coordinate system;

[0037] The three degrees of freedom spatial rotation is obtained by left-multiplying the rotation matrix of the teaching grasping point in the robotic arm coordinate system by the inverse of the robotic arm rotation variable, then left-multiplying it by the rotation matrix of the grasping point relative to the template in the camera coordinate system, and finally left-multiplying it by the robotic arm rotation variable. This is expressed as: base R cam · template R target ·( base R cam ) -1 · base R teach pose ,in, base R cam The variable representing the rotating part of the robotic arm. template R target This represents the rotation matrix of the point to be captured relative to the template in the camera coordinate system. base R teach pose This represents the rotation matrix of the teaching gripping point in the robotic arm coordinate system.

[0038] Compared with the prior art, the present invention, employing the above technical solution, has the following beneficial effects:

[0039] 1. For the application of hand-eye collaboration in industrial scenarios, this invention introduces a multi-camera collaborative scheme under limited receptive fields. After completing two hand-eye calibrations, the industrial camera, with the eye outside the hand, can expand the field of view, assist in positioning, and improve accuracy. Furthermore, considering that the calibration process requires the calibration board to move as far as possible within the field of view, but imaging at the edge of the field of view is prone to significant distortion, this invention adds a distortion correction algorithm to the image.

[0040] 2. Compared to the traditional four-dimensional point-pair feature hash table construction and voting mechanism calculation, this invention extends the dimension of the point-pair features by cascading 3D point cloud features based on a multi-resolution hash grid, and uses these six-dimensional features for calculation. Verification shows that this matching algorithm has significant advantages in both speed and accuracy compared to traditional point cloud matching algorithms.

[0041] 3. This invention performs real-time perception and detection of objects and captures point clouds of the objects. It uses the point cloud information to obtain the object's position information in a timely manner and guides the robot to position itself based on the point cloud matching results, enabling the robot to accurately complete the grasping task and avoid deviations. Attached Figure Description

[0042] Figure 1 This is a flowchart of the method of the present invention.

[0043] Figure 2 This is a flowchart of the hand-eye calibration algorithm.

[0044] Figure 3 This is a flowchart of accuracy assessment - reprojection error.

[0045] Figure 4 This is a flowchart of the Iterative Closest Point Algorithm. Detailed Implementation

[0046] The technical solution of the present invention will be further described below with reference to the accompanying drawings.

[0047] The robot 3D vision-guided grasping method of the present invention includes the following steps in application:

[0048] Step 1) Hand-eye calibration

[0049] The core of a robot's 3D vision guidance system is the hand-eye system, which includes a robotic arm (hand) and a vision camera (eye). To ensure hand-eye coordination during guided grasping, calibrating this system is crucial; this involves unifying the coordinate system relationship between the RGBD industrial camera (acting as a vision sensor) and the robot performing the target action. This is an important prerequisite for ensuring accurate target grasping by the robotic arm. The calibration object typically used in the calibration scenario is a checkerboard pattern. Therefore, the entire 3D space involves four coordinate systems: the robotic arm base coordinate system (base), the robotic arm gripper coordinate system (gripper), the camera coordinate system (cam), and the calibration object world coordinate system (cal), with a complete spatial transformation loop existing between these coordinate systems.

[0050] In practical applications, locating a specific object often requires an industrial camera to capture images of multiple other feature objects at different locations to assist in positioning and improve accuracy. However, due to the limited field of view of RGBD industrial cameras, this invention introduces a multi-camera cooperative scheme with a limited receptive field. Specifically, one industrial camera is fixed at the gripper of the robotic arm, while another auxiliary positioning camera is fixed at a fixed point in space outside the robotic arm. Based on the relative positional relationship between the hand and eye, the above two hand-eye calibration scenarios can be defined as "eye on the hand" and "eye outside the hand." Both hand-eye calibration algorithms study the problem of solving the robot's hand-eye relationship matrix given the relative poses of the robot's end effector before and after two passes, and the relative poses of the calibration plate in the camera coordinate system before and after two passes. The characteristics and differences between the two forms are as follows:

[0051] (1) Eyes on hands

[0052] In this scenario, the camera is fixed to the robotic arm's gripper, and the calibration object is placed at a fixed point within the camera's field of view. Using the transformation between the robotic arm's base and the camera's coordinate systems as the target descriptor, the following transformation relationship can be obtained: base H gripper· gripper H cam = base H cal · cal H cam In this formula, base H gripper This represents the homogeneous transformation matrix from the robot arm base coordinate system to the robot arm gripper coordinate system. gripper H cam This represents the homogeneous transformation matrix from the robotic arm's gripper coordinate system to the camera coordinate system. base H cal This represents the homogeneous transformation matrix from the robot arm's base coordinate system to the calibration object's world coordinate system. cal H cam This represents the homogeneous transformation matrix from the world coordinate system of the calibration object to the camera coordinate system. In this scenario, the quantity to be solved in the hand-eye calibration problem is a fixed quantity—the transformation matrix from the robotic arm gripper to the camera coordinate system. gripper H cam Since the relative positions of the calibration object and the robot arm base remain unchanged in the scene, the transformation matrix from the robot arm base to the world coordinate system of the calibration object... base H cal Since it is a fixed quantity, it can be eliminated by two sets of pose relationships. base Hcal establishes the equation: base H gripper 0 · gripper H cam ·( cal H cam 0 ) -1 = base H gripper 1 · gripper H cam ·( cal H cam 1 ) -1 After sorting, we can obtain ( base H gripper 1 ) -1 · base H gripper 0 · gripper H cam = gripper H cam ·( cal H cam 1 ) -1 · cal H cam 0Its form is A·X=X·B, where A is the pose of the robotic arm, B is the pose of the camera, and X is the solution quantity for the eye-on-hand problem.

[0053] (2) Eyes outside hands

[0054] In this scenario, the calibration object is fixed to the gripper of the robotic arm, and the camera is placed at a fixed point within the space where its movement can be covered by the camera's field of view. Using the transformation between the two coordinate systems of the robotic arm base and the calibration object as the target descriptor, the following transformation relationship can be obtained: base H gripper · gripper H cal = base H cam · cam H cal In this scenario, the quantity to be solved for the hand-eye calibration problem is a fixed quantity—the transformation matrix from the robot arm base to the camera coordinate system. base H cam Since the relative positions of the calibration object and the robotic arm gripper remain unchanged in the scene, the equations can be obtained by solving the two sets of pose relationships simultaneously and rearranging them: base H gripper 1 ·( base H gripper 0 ) -1 · base H cam = base H cam · cam H cal 1 ·( cam H cal 0 ) -1 This equation also conforms to the form A·X=X·B, where A is the pose of the robotic arm, B is the pose of the camera, and X is the solution quantity for the problem of the eye being outside the hand.

[0055] Therefore, the hand-eye calibration problem can be transformed into solving the 4*4 homogeneous matrix X in the matrix equation A·X=X·B, where A is the pose of the robotic arm and B is the pose of the camera.

[0056] Generally, the robotic arm pose can be obtained by converting the Euler angles of the 6D pose data output by the robotic arm into a rotation matrix representation. The camera pose uses Zhang's camera calibration and the PnP (Perspective-n-Point) algorithm. Specifically, the algorithm flow for determining the camera pose is as follows:

[0057] The calibration material used was a checkerboard calibration board. First, camera intrinsic parameters were calibrated by extracting corner points and refining their coordinates to sub-pixel accuracy. Zhang's calibration algorithm was then used to obtain the camera's intrinsic parameters and distortion coefficients. Since the calibration process requires the calibration board to move as far as possible within the field of view, and image processing at the edges of the field of view is prone to significant distortion, a distortion correction process was added to improve accuracy. The camera calibration results were used to correct the distortion in the image, and sub-pixel accuracy corner coordinates (i.e., 2D point coordinates) were re-extracted. To ensure consistency with real-world scale, the world coordinate system was defined on the calibration board, with the 3D coordinates corresponding to the m x n corner points being (m*object_size.width, n*object_size.height, 0). Given the 3D coordinates of the point set, the corresponding 2D coordinates, and the camera intrinsic parameters, the perspective projection model was established using the PnP algorithm. This allowed for the calculation of the rotation and translation matrices from the world coordinate system to the camera coordinate system, i.e., the camera pose.

[0058] After obtaining the robot arm pose and camera pose, the equations A•X=X•B can be substituted into the solution and solved using the OpenCV:calibrateHandEye operator. Mainstream algorithms include the two-step TSAI method and the dual quaternion solution DANIILIDIS. For the obtained hand-eye matrix, the matrix accuracy is evaluated by calculating the reprojection error: since there are fixed unknowns in the hand-eye system that do not participate in the calculation, the calculation of the reprojection error also requires two sets of pose data. One set is used as the true value to estimate the other camera pose. The pose and the 3D point set are reprojected to obtain the 2D point set coordinates of the image plane, and the MSE error value between the projected point coordinates and the corner coordinates is calculated.

[0059] Step 2) Point Cloud Acquisition

[0060] After the hand-eye calibration step, an RGBD camera is used to acquire point cloud images of the object. During this process, RGB color images and depth images can be obtained simultaneously, along with the point cloud information of the scene. Unlike ordinary RGB image acquisition devices, RGBD cameras, in addition to having a camera for acquiring RGB images, are also equipped with an infrared emitter and an infrared receiver. There are two main ways for RGBD cameras to acquire depth images: one is the structured light principle, which uses an infrared emitter to emit coded light onto the object surface and a reference plane, measuring the distance from the coded portion on the object surface to the coded portion on the reference plane, and calculating the distance information needed to form a depth image; the other is the Time-of-Flight (TOF) principle, which uses time-of-flight for depth imaging. Light is emitted from the emitter, reflects off an object, and returns to the receiver; the distance information is calculated using the time of flight of the light.

[0061] Step 3) Point cloud coordinate system transformation

[0062] After the RGBD camera acquires and transmits the scene point cloud, it is necessary to unify the coordinate system of this point cloud with the point cloud under the teaching photography pose, that is, to transform the coordinate system of the scene point cloud. The algorithm for coordinate system transformation is as follows: First, multiply the eye-to-hand calibration matrix with the eye on the hand by the pose under the teaching photography pose to obtain the transformation matrix from the camera position to the robot arm base under the teaching photography pose. Then, invert this matrix to obtain the pose transformation matrix 1 from the robot arm base to the teaching photography point. Similarly, multiply the eye-to-hand calibration matrix with the eye on the hand by the pose under the scene point cloud pose to obtain the transformation matrix 2 from the camera position to the robot arm base under the real-time photography pose. Multiplying matrix 2 by matrix 1 under the left will finally transform the coordinate system of the scene point cloud to the coordinate system under the teaching photography pose.

[0063] Step 4) Point cloud matching

[0064] Assuming the coordinate system transformation of the scene point cloud is completed, this invention employs a point-pair feature-based matching method for recognition and pose estimation. Before matching, a point cloud model of the object to be grasped needs to be acquired. The algorithm matches the acquired model point cloud to the object point cloud in the actual scene, identifies and obtains the coordinate transformation relationship between the two, and then uses the model point cloud to replace the scene point cloud for subsequent grasping pose calculation. Finally, the pose result is fine-tuned using an iterative nearest-point algorithm.

[0065] The point-pair feature-based matching algorithm is a pose estimation algorithm that uses voting for decision-making. It employs the idea of ​​"global modeling and local matching," achieving good results even in cluttered or occluded environments. Point-pair features are the foundation of this algorithm, calculated from two spatial points with normal vectors. A point-pair feature is a feature vector composed of four feature quantities; its structure is simple and computationally easy.

[0066] Given that the spatial coordinates of the two points forming a point pair are m1 and m2, and their normal vectors are n1 and n2, the feature of the point pair is defined as: F(m i m j F1 = [||d||2, ∠(n1, d), ∠(n2, d), ∠(n1, n2)]. Here, ||d||2 represents the Euclidean distance between two points, and ∠(a, b) represents the angle between two normal vectors, where ∠(a, b) ∈ [0, π]. That is, F1 is the Euclidean distance between two points, and F2, F3, and F4 are the angles between each pair of the three normal vectors.

[0067] When point pairs with identical feature values ​​are found in both the model point cloud and the actual scene point cloud, the spatial transformation relationship between the two point pairs can be calculated. For ease of processing, the four values ​​in the point pair feature are discretized, as shown in the formula... Set two parameters d dist and dangle =2π / n angle , where n angle Let be the discrete number of angles.

[0068] The complete matching process is divided into an offline training phase and an online execution phase. During the training phase, the matching algorithm extracts the aforementioned features from the model point cloud, quantizes them, and stores them in a hash table. During the execution phase, the algorithm extracts the aforementioned feature vectors from the scene point cloud, searches the hash table, and calculates the pose after finding the corresponding feature point pair.

[0069] To improve the matching speed and accuracy, this invention, based on the existing four-dimensional features, further samples the three-dimensional point cloud at two resolution levels and concatenates the sparse and dense resolution features to form a total of six-dimensional features. The model hash table is crucial for linking the model point cloud and the scene point cloud. For the calculated six-dimensional point pair features, the MurmurHash3 algorithm is used to calculate the key value of the hash table, and then the point pair information is stored or retrieved under the corresponding key value. MurmurHash3 is a non-cryptographic hash function with high computational performance and a low collision rate. The hash table allows for quick retrieval of point pairs corresponding to features.

[0070] On the same object, there may be multiple pairs of points corresponding to the same feature vector. Therefore, the feature vectors of point pairs extracted from the scene point cloud may simultaneously correspond to multiple pairs of points in the model point cloud, thus obtaining multiple poses. To solve this problem, this invention adopts a voting mechanism, discretizing the pose estimation results, accumulating votes on the pose results calculated from different point pairs, and taking the quantized pose value with the highest vote as the final result.

[0071] In the voting mechanism, m r α is a reference point for extracting point pair features from the scene point cloud. i The pose variables are discretized. When calculating the pose between two pairs of points, an intermediate coordinate system is constructed using the geometric constraints of each vector on the feature of the point pair, simplifying the six-degree-of-freedom pose calculation to a single angle α. i To express.

[0072] After obtaining the initial pose through coarse matching using the above algorithm, a fine matching process is performed using iterative nearest-point matching to obtain the accurate pose result. Considering that the calculation process requires repeated iterations and is sensitive to initial values, this method is applied to optimize the pose after coarse matching.

[0073] Specifically, the Iterative Closest Point Algorithm is used for 3D point matching. It calculates the transformation parameters (rotation and translation) between point sets that satisfy a given convergence accuracy by finding the relationship between corresponding registration points of two point sets. The basic steps are as follows: First, select initial iterative point pairs from the two point clouds to be registered. Then, estimate the rigid body transformation using these initial corresponding point pairs, transforming the points in the template point cloud to the target point cloud coordinate system, obtaining the transformed point cloud. Next, search again for the point in the target point cloud that is closest to the transformed point cloud, updating the correspondence between the point clouds. Estimating the rigid body transformation maximizes the overlap between the template and target point clouds. Repeat these steps until a stopping criterion is met. The stopping criterion can be that the number of iterations reaches a set threshold or the change in the rigid body transformation is less than a set threshold. Finally, obtain the final rigid body transformation at the point where iteration stops. Combined with the object pose from the coarse matching, calculate the rotation matrix R and translation vector t that minimize the error function, i.e., the optimal matching parameters, which describe the optimal matching relationship between the template and target point clouds.

[0074] Step 5) Calculation of robotic arm grasping pose

[0075] After obtaining the point cloud matching results, the final grasping pose of the robot needs to be calculated. This invention divides the 6D pose result into two parts and calculates them separately. The displacement calculation process for the three degrees of freedom is as follows: the coordinates of the taught grasping point in the camera coordinate system are multiplied by the translation matrix of the point to be grasped relative to the template in the camera coordinate system, and then multiplied by the pose matrix of the camera in the base coordinate system, expressed as: base H cam · template T target · cam T teach pose ,in, base H cam This represents the pose matrix of the camera in the base coordinate system. template T target This represents the translation matrix of the point to be captured relative to the template in the camera coordinate system. cam T teach pose This represents the coordinates of the teaching gripping point in the camera coordinate system. The three degrees of freedom spatial rotation are obtained by left-multiplying the rotation matrix of the teaching gripping point in the robotic arm coordinate system by the inverse of the robotic arm rotation variable, then left-multiplying it by the rotation matrix of the point to be gripped relative to the template in the camera coordinate system, and finally left-multiplying it by the robotic arm rotation variable. This is expressed as: base R cam · template R target ·( base R cam ) -1 · base R teach pose ,in,base R cam The variable representing the rotating part of the robotic arm. template R target This represents the rotation matrix of the point to be captured relative to the template in the camera coordinate system. base R teach pose This represents the rotation matrix of the teaching gripping point in the robotic arm coordinate system.

[0076] The embodiments described in this specification are merely examples of implementations of the inventive concept. The scope of protection of this invention should not be considered as limited to the specific forms stated in the embodiments. The scope of protection of this invention also extends to equivalent technical means that can be conceived by those skilled in the art based on the inventive concept.

Claims

1. A robot 3D vision-guided grasping method, comprising the following steps: Step 1) Hand-eye alignment; Calibration was performed in the entire 3D space under both "eye on hand" and "eye outside hand" orientations. The robotic arm pose was obtained by converting the Euler angles of the 6D pose data output by the robotic arm into a rotation matrix. The camera pose was solved using Zhang's camera calibration and the PnP algorithm. After obtaining the pose data from both approaches, they were substituted into the solution equations. To solve, where, For the robotic arm pose, For camera pose, The matrix to be solved for the hand-eye calibration problem; Step 2) Scene point cloud information collection; Step 3) Point cloud coordinate system transformation; After the RGBD camera acquires and transmits the scene point cloud, the coordinate system of the current point cloud is transformed, and the algorithm is used to unify the coordinate system of the point cloud and the point cloud under the teaching photo pose. Step 4) Point cloud matching; First, match the point cloud of the model to be captured to the point cloud of the object in the actual scene, identify and obtain the coordinate transformation relationship between the two, and then use the model point cloud to replace the scene point cloud for subsequent capture pose calculation; Finally, use the iterative nearest point algorithm to fine-tune the pose result of the point cloud matching. Step 5) Calculate the robotic arm grasping pose; divide the final 6D pose of the robotic arm required for grasping into three degrees of freedom of displacement and three degrees of freedom of spatial rotation, and calculate them separately.

2. The robot 3D vision-guided grasping method as described in claim 1, characterized in that: Step 1) includes the following steps: A multi-camera cooperative method is introduced under limited receptive field conditions. Specifically, one industrial camera is fixed at the gripper of the robotic arm, and another auxiliary positioning camera is fixed at a fixed point in the space outside the robotic arm. Based on the relative positional relationship between the hand and eye, the above two-hand-eye calibration scenarios can be defined as "eye on the hand" and "eye outside the hand," as detailed below: (1) Eyes on hands In this scenario, the camera is fixed to the gripper of the robotic arm, and the calibration object is placed at a fixed point within the camera's field of view. Using the transformation between the two coordinate systems of the robotic arm base and the camera as the target descriptor, the following transformation relationship can be obtained: In this formula, base refers to the coordinate system of the robot arm base, gripper refers to the coordinate system of the robot arm gripper, cam refers to the coordinate system of the camera, and cal refers to the world coordinate system of the calibration object. This represents the homogeneous transformation matrix from the robot arm base coordinate system to the robot arm gripper coordinate system. This represents the homogeneous transformation matrix from the robotic arm's gripper coordinate system to the camera coordinate system. This represents the homogeneous transformation matrix from the robot arm's base coordinate system to the calibration object's world coordinate system. Let represent the homogeneous transformation matrix from the world coordinate system of the calibration object to the camera coordinate system; in this scenario, the quantity to be solved in the hand-eye calibration problem is a fixed quantity—the transformation matrix from the robotic arm gripper to the camera coordinate system. Since the relative positions of the calibration object and the robotic arm base remain unchanged in the scene, the transformation matrix from the robotic arm base to the world coordinate system of the calibration object... Since it is a fixed quantity, the fixed quantity is eliminated by the two sets of pose relationships. Establish the equation: After sorting, we can obtain Its shape is like ; (2) Eyes outside hands In this scenario, the calibration object is fixed at the gripper of the robotic arm, and the camera is placed at a fixed point within the space where its movement can be covered by the camera's field of view. Using the transformation between the two coordinate systems of the robotic arm base and the calibration object as the target descriptor, the following transformation relationship can be obtained: In this scenario, the quantity to be solved in the hand-eye calibration problem is a fixed quantity—the transformation matrix from the robotic arm base to the camera coordinate system. Since the relative positions of the calibration object and the robotic arm gripper remain unchanged in the scene, the equations can be obtained by solving the equations simultaneously using the two sets of pose relationships and then rearranging them: This equation also conforms to The form; Therefore, the hand-eye calibration problem can be transformed into matrix equations. In the context of a 4x4 homogeneous matrix... Solve for; The robot arm pose can be obtained by converting the Euler angles of the 6D pose data output by the robot arm into a rotation matrix representation. The camera pose is solved using Zhang's camera calibration and the PnP (Perspective-n-Point) algorithm. The algorithm for determining camera pose is as follows: A checkerboard calibration board is used as the calibration object; firstly, camera intrinsic parameters are calibrated, corner points are extracted, and their coordinates are refined to sub-pixel precision. Zhang's calibration algorithm is used to obtain the camera's intrinsic parameters and distortion coefficients. During image distortion correction, the results of the camera calibration are used to remove distortion from the image, and sub-pixel precision corner coordinates (i.e., 2D point coordinates) are extracted again. The world coordinate system is defined on the calibration board, and the 3D coordinates corresponding to the m x n corner points are (m * object_size.width, n * object_size.height, 0). Given the 3D coordinates of the point set, the corresponding 2D coordinates, and the camera intrinsic parameters, the perspective projection model is established using the PnP algorithm. The rotation and translation matrices from the world coordinate system to the camera coordinate system, i.e., the camera pose, can be solved. After obtaining the robot arm pose and camera pose, substitute them into the solution equation. The OpenCV calibrateHandEye operator is used to solve the problem. For the obtained hand-eye matrix, the accuracy of the matrix is ​​evaluated by calculating the reprojection error. The calculation of the reprojection error requires two sets of pose data. One set is used as the true value to estimate the pose of the other camera. The pose and the 3D point set are reprojected to obtain the coordinates of the 2D point set on the image plane. The MSE error value between the projected point coordinates and the corner point coordinates is calculated.

3. The robot 3D vision-guided grasping method as described in claim 1, characterized in that: Step 2) includes the following steps: After the hand-eye calibration step, an RGBD camera is used to acquire point cloud data of the object image. During this process, RGB color image and depth image information are acquired simultaneously, as well as point cloud information of the scene.

4. The robot 3D vision-guided grasping method as described in claim 1, characterized in that: Step 3) includes the following steps: After the RGBD camera acquires and transmits the scene point cloud, the coordinate system of this point cloud and the point cloud under the teaching photography pose are unified. The algorithm for transforming the coordinate system of the scene point cloud is as follows: First, multiply the eye-to-hand calibration matrix with the eye on the hand by the pose during teaching photography to obtain the transformation matrix from the camera position to the robot arm base during teaching photography. Then, invert this matrix to obtain the pose transformation matrix 1 from the robot arm base to the teaching photography point. Similarly, multiply the eye-to-hand calibration matrix with the eye on the hand by the pose during scene point cloud photography to obtain the transformation matrix 2 from the camera position to the robot arm base during real-time photography. Multiply the above matrix 2 by matrix 1 by the left to finally transform the coordinate system of the scene point cloud to the coordinate system during teaching photography.

5. The robot 3D vision-guided grasping method as described in claim 1, characterized in that: Step 4) includes the following steps: Under the premise of completing the coordinate system transformation of the scene point cloud, a point-pair feature-based matching method is selected for recognition and pose estimation. It matches the model point cloud to be captured to the object point cloud in the actual scene, identifies and obtains the coordinate transformation relationship between the two, and then uses the model point cloud to replace the scene point cloud for subsequent capture pose calculation. Finally, the pose result is fine-tuned using the iterative nearest point algorithm. A point-to-point feature is calculated from two spatial points with normal vectors, and a point-to-point feature is an eigenvector composed of four feature quantities. Given that the spatial coordinates of the two points forming a point pair are respectively and The normal vectors are respectively and Then the definition of point-pair features is: ;in, Represents the Euclidean distance between two points, in the form of... This represents the angle between two normal vectors, and ; When point pairs with identical feature values ​​are found in both the model point cloud and the actual scene point cloud, the spatial transformation relationship between the two point pairs can be calculated. The four values ​​in the point pair's feature are discretized and represented as follows: Set two parameters and , in The discrete number of angles; The complete matching is divided into an offline training phase and an online running phase. In the training phase, the matching algorithm extracts the above features from the model point cloud, quantizes them, and stores them in a hash table. In the running phase, the algorithm extracts the above feature vectors from the scene point cloud, searches in the hash table, and calculates the pose after finding the corresponding feature point pair. Based on the existing four-dimensional features, the three-dimensional point cloud is sampled at two more resolutions, and the sparse and dense resolution features are concatenated to form a total of six-dimensional features. For the calculated six-dimensional point pair features, the MurmurHash3 algorithm is used to calculate the key value of the hash table, and then the point pair information is stored or queried under the corresponding key value. A voting mechanism is applied to discretize the pose estimation results. The pose results calculated for different point pairs are accumulated through voting, and the quantized pose value with the highest number of votes is taken as the final result. In this voting mechanism... A reference point for extracting point pair features from the scene point cloud. The pose variables are discretized. When calculating the pose between two pairs of points, an intermediate coordinate system is constructed using the geometric constraints of each vector on the feature of the point pair, simplifying the six-degree-of-freedom pose calculation to a single angle. To indicate; After obtaining the initial pose through coarse matching using the above algorithm, a fine matching process is performed using the iterative nearest point method to obtain accurate pose results. The iterative nearest point algorithm is used for matching 3D points. By finding the relationship between corresponding registration points of two point sets, it calculates the transformation parameters between point sets that satisfy a given convergence accuracy, namely rotation and translation relationships. The steps are as follows: From the two sets of point clouds to be registered, select initial iterative point pairs, estimate the rigid body transformation using the initial corresponding point pairs, transform the points in the template point cloud to the target point cloud coordinate system, obtain the transformed point cloud, and re-search for the point in the target point cloud that is closest to the transformed point cloud, updating the correspondence of the point clouds. Among these steps, estimating the rigid body transformation maximizes the overlap between the template point cloud and the target point cloud. Repeat the above steps until the stopping criterion is met. The stopping criterion can be that the number of iterations reaches a set threshold or the change in the rigid body transformation is less than a set threshold. Obtain the final rigid body transformation when the iteration stops, and combine it with the object pose obtained from the coarse matching to calculate the rotation matrix R and translation vector t that minimize the error function, which are the optimal matching parameters.

6. The robot 3D vision-guided grasping method as described in claim 1, characterized in that: Step 5) includes the following steps: After obtaining the point cloud matching results, the final grasping pose of the robot is calculated; the 6D pose result is divided into three degrees of freedom of displacement and three degrees of freedom of spatial rotation, and calculated separately. The displacement calculation process for the three degrees of freedom is as follows: multiply the coordinates of the teaching grasp point in the camera coordinate system by the translation matrix of the point to be grasped relative to the template in the camera coordinate system, and then multiply by the pose matrix of the camera in the base coordinate system, expressed as: ,in, This represents the pose matrix of the camera in the base coordinate system. This represents the translation matrix of the point to be captured relative to the template in the camera coordinate system. This indicates the coordinates of the teaching capture point in the camera coordinate system; The calculation process for the three degrees of freedom spatial rotation is as follows: First, multiply the rotation matrix of the teaching gripping point in the robotic arm coordinate system by the inverse of the robotic arm rotation variable. Then, multiply this by the rotation matrix of the gripping point relative to the template in the camera coordinate system. Finally, multiply this by the robotic arm rotation variable to obtain the result, expressed as: ,in, The variable representing the rotating part of the robotic arm. This represents the rotation matrix of the point to be captured relative to the template in the camera coordinate system. This represents the rotation matrix of the teaching gripping point in the robotic arm coordinate system.

Images