A robotic grasping method based on depth camera

Abstract

The present invention relates to a robot grasping method based on a depth camera, which mainly includes the following aspects: (1) identifying the grasped object through YOLO-V5, obtaining its pixel range in the RGB image, mapping the range into the depth map, and obtaining the pixel range in the corresponding depth map; (2) calculating the point cloud data within the pixel range in the depth map in combination with the camera internal parameters, matching the point cloud data with the model database established in the offline stage to obtain the initial posture, and correcting the initial posture through the iterative closest point algorithm; (3) converting the corrected final posture result and sending it to the robot control system, accurately controlling the robot to achieve grasping. The method proposed in the present invention can improve the accuracy of posture estimation and speed up the calculation.

Description

Technical Field

[0001] The invention belongs to the technical field of automation and relates to a manipulator grasping method based on a depth camera. Background Art

[0002] Robotic autonomous grasping technology enables robots to acquire, move and transport target objects. In industrial environments, the objects grasped are often fixed-position target objects. Accurate grasping in unstructured environments is one of the key technologies for robot intelligence. In unstructured environments, the robot grasping system is mainly divided into three parts: perception, posture estimation and control. First, the target object information is acquired through sensors, then the grasping posture is calculated, and finally the robot is controlled to grasp.

[0003] Compared with traditional cameras, depth cameras can perceive the depth information of the external environment and obtain more information about external objects. Therefore, they are widely used in unmanned driving, 3D reconstruction, visual SLAM and other fields. Using depth cameras to obtain image information and depth information of objects, calculating the position of the target object, and realizing grasping in unstructured environments is an important direction of grasping method research. Summary of the invention

[0004] The present invention aims at the problems of excessive calculation and low precision when using point cloud data to estimate the pose of the target object for grasping, and proposes a manipulator grasping method based on a depth camera. First, the grasped object is identified by YOLO-V5, and its pixel range in the RGB image is obtained, and the range is mapped to the depth map to obtain the pixel range in the corresponding depth map. Then, the point cloud data within the pixel range in the depth map is calculated by combining the camera intrinsic parameters, and the point cloud data is matched with the model database established in the offline stage to obtain the initial pose, and the initial pose is corrected by the iterative closest point (ICP) algorithm. Finally, the corrected final pose result is converted and sent to the robot control system to accurately control the robot to achieve grasping.

[0005] A robot grasping method based on a depth camera specifically comprises the following steps:

[0006] Step 1: Establish a model database of the target object in the offline stage. Use the 3D model of the known target object to obtain point cloud data and the initial optimal grasping posture T0, and transform the point cloud data into a voxel size V. size =(w v ,h v ,d v) filter to downsample, and then extract the point pair features (PPF) of the point cloud data, put similar PPFs in the same group and store them in the hash table. Each pair of PPFs can be expressed as:

[0007] F m (p m ,p n )=(d f ,θ1,θ2,θ3) (1)

[0008] Among them, p m and p n are any two points in the point cloud data, d f It's point p m With point p n The Euclidean distance between them, θ1 and θ2 are respectively m With point p n The angle between the normal and the two points is the angle between the vectors, θ3 is the angle between the point p m With point p n The angle between the normals.

[0009] Step 2: Use the depth camera to obtain the RGB image data and depth image data of the current environment, use the YOLO-V5 target detection algorithm to detect the target object in the current environment, and obtain its pixel range in the RGB image:

[0010] Pixel obj ={(x obj ,y obj )|(x left ,y left )<(x obj ,y obj )<(x right ,y right )} (2)

[0011] Among them, (x obj ,y obj ) is the pixel point of the target object, (x left ,y left ) and (x right ,y right ) are the upper left pixel and lower right pixel of the pixel box respectively.

[0012] Step 3: Since the color camera and infrared sensor of the depth camera are located at different positions, there is a deviation between the original RGB image and the depth image. The depth image and the RGB image are aligned using formula (3).

[0013]

[0014] Among them, prgb and p ir is the corresponding pixel in the RGB image and the depth image, I rgb and I ir are the intrinsic parameter matrices of RGB camera and infrared camera respectively, E rgb 、E ir , T rgb and T ir are the external parameters of the RGB camera and the infrared camera.

[0015] Step 4: Map the pixel frame obtained in step 2 to the aligned depth image. Since there may be a deviation between the detected bounding box and the actual object boundary, the pixel frame range needs to be corrected in the depth image.

[0016] Step 5: Calculate the coordinates of all pixels in the corrected pixel frame in step 4 in the camera coordinate system using equation (4) to obtain scene point cloud data.

[0017]

[0018] Among them, x c ,y c 、z c is the coordinate of the pixel point (u, v) in the camera coordinate system, d is the depth distance at the pixel point (u, v), dx, dy, u0, v0 and f are the camera intrinsic parameters.

[0019] Step 6: Transform the scene point cloud into a size =(w v ,h v ,d v ) filter to downsample and then estimate the surface normal of the point cloud.

[0020] Step 7: Randomly generate a series of reference points q of the scene point cloud i , each reference point q i And the scene point cloud constitutes a point pair feature F s (q i ,q j ). Set the screening threshold θ for PPF normal angle and distance. t and d t , by searching the hash table established in step 1, find the scene point feature F within the threshold s (q i ,q j ) Similar model point pair features F m (p m ,p n ), each pair of similar F m (p m ,p n ) and Fs (q i ,q j ) j and p n The conversion relationship can be expressed as follows:

[0021]

[0022] Among them, T qi and T pm The points q are i and p m The transformation matrix converted to the origin of the camera coordinate system, α is the transformation matrix of q i and p m Transform to the origin and align its normal vector with the x-axis back vector and The angle between them, R(α) is the transformation matrix with a rotation angle of α.

[0023] Step 8: Use the voting strategy method to select each reference point q i The multiple poses obtained are voted, and the pose with the highest votes is used as the pose result of the reference point. The pose results of all reference points are clustered, and each cluster is scored. The score of a single cluster is equal to the sum of the votes of all poses in it. The average value of the poses in the cluster with the highest score is taken as the final result.

[0024] Step 9: Use the ICP algorithm to correct the pose result in step 8, and set the objective function as:

[0025]

[0026] Among them, N is the total number of point pairs, p i and q i For the neighboring point pairs in the scene point cloud and the model point cloud. Set the threshold f t , when the objective function value is less than the threshold f t , then accept the revised results R and T at this time, otherwise continue to iterate.

[0027] Step 10: The initial optimal grasping posture T0 is transformed by equation (7) to obtain the optimal grasping posture in the current environment, and a control command is sent to the robot controller to complete the grasping.

[0028]

[0029] Among them, T0 is the initial optimal grasping posture of the model, R and T are the rotation and translation matrices after ICP correction. As a preferred method, step 4 calculates the depth value d of the center point of the pixel frame c , set the threshold d t =d c-d0, d0 is a constant; calculate the depth values ​​of the pixels on the four boundary boxes. If there is a depth value on a border that is less than the set threshold d t If there are pixels on the four borders, move the border outward by n pixels until the depth values ​​of the pixels on the four borders are all greater than the set threshold.

[0030] The substantial effect of the present invention is that the present invention uses the YOLO-V5 algorithm to quickly find the range of the captured object, uses the range to limit the scene point cloud data, and removes the point cloud data of most irrelevant objects in the environment, which greatly improves the accuracy of pose estimation. At the same time, compared with using the entire scene point cloud for pose estimation, the method used by the present invention greatly reduces the pose estimation calculation time. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 Establishing a flow chart for the offline phase model database of the present invention;

[0032] Figure 2 This is a flowchart for capturing the online phase of the present invention. DETAILED DESCRIPTION

[0033] The present invention is further analyzed below in conjunction with specific embodiments.

[0034] Combined with RealSense D435i depth camera Figure 2 For example, the implementation steps of the capture method based on the depth camera are as follows:

[0035] Step (1), such as Figure 1 As shown in Figure 1, a model database of the target object is established in the offline stage. The 3D model of the known target object is used to obtain point cloud data and the initial optimal grasping posture T0. The point cloud data is transformed into a voxel size V. size =(w v ,h v ,d v ) filter to downsample, and then extract the point pair features (PPF) of the point cloud data, put similar PPFs in the same group and store them in the hash table. Each pair of PPFs can be expressed as:

[0036] F m (p m ,p n )=(d f ,θ1,θ2,θ3) (1)

[0037] Among them, p m and p n are any two points in the point cloud data, d f It's point p m With point pn The Euclidean distance between them, θ1 and θ2 are respectively m With point p n The angle between the normal and the two points is the angle between the vectors, θ3 is the angle between the point p m With point p n The angle between the normals.

[0038] Step (2): obtain the RGB image data and depth image data in the current environment through the depth camera, use the YOLO-V5 target detection algorithm to detect the target object in the current environment, and obtain its pixel range in the RGB image:

[0039] Pixel obj ={(x obj ,y obj )|(x left ,y left )<(x obj ,y obj )<(x right ,y right )} (2)

[0040] Among them, (x obj ,y obj ) is the pixel point of the target object, (x left ,y left ) and (x right ,y right ) are the upper left pixel and lower right pixel of the pixel box respectively.

[0041] Step (3): Since the color camera and infrared sensor of the depth camera are located at different positions, there is a deviation between the original RGB image and the depth image. The depth image and the RGB image are aligned using formula (3).

[0042]

[0043] Among them, p rgb and p ir is the corresponding pixel in the RGB image and the depth image, I rgb and I ir are the intrinsic parameter matrices of RGB camera and infrared camera respectively, E rgb 、E ir , T rgb and T ir are the external parameters of the RGB camera and the infrared camera.

[0044] Step (4): Map the pixel frame obtained in step (2) to the aligned depth image. Since there may be a deviation between the detected boundary box and the actual object boundary, the pixel frame range needs to be corrected in the depth image. Calculate the depth value d of the center point of the pixel frame c , set the threshold d t =d c -d0, d0 is a constant. Calculate the depth values ​​of the pixels on the four boundary boxes. If the depth value on a border is less than the set threshold d t If there are pixels on the four borders, move the border outward by n pixels until the depth values ​​of the pixels on the four borders are all greater than the set threshold.

[0045] Step (5), calculate the coordinates of all pixels in the corrected pixel frame in step (4) in the camera coordinate system by using formula (4) to obtain scene point cloud data.

[0046]

[0047] Among them, x c ,y c 、z c is the coordinate of the pixel point (u, v) in the camera coordinate system, d is the depth distance at the pixel point (u, v), dx, dy, u0, v0 and f are the camera intrinsic parameters.

[0048] Step (6), transform the scene point cloud through a voxel size V size =(w v ,h v ,d v ) filter to downsample and then estimate the surface normal of the point cloud.

[0049] Step (7), randomly generate a series of reference points q of the scene point cloud i , each reference point q i And the scene point cloud constitutes a point pair feature F s (q i ,q j ). Set the screening threshold θ for PPF normal angle and distance. t and d t , by searching the hash table established in step (1), find the scene point feature F within the threshold s (q i ,q j ) Similar model point pair features F m (p m ,p n ), each pair of similar F m (p m ,p n ) and F s (qi ,q j ) j and p n The conversion relationship can be expressed as follows:

[0050]

[0051] Among them, T qi and T pm The points q are i and p m The transformation matrix converted to the origin of the camera coordinate system, α is the transformation matrix of q i and p m Transform to the origin and align its normal vector with the x-axis back vector and The angle between them, R(α) is the transformation matrix with a rotation angle of α.

[0052] Step (8), use the voting strategy method to select each reference point q i The multiple poses obtained are voted, and the pose with the highest votes is used as the pose result of the reference point. The pose results of all reference points are clustered, and each cluster is scored. The score of a single cluster is equal to the sum of the votes of all poses in it. The average value of the poses in the cluster with the highest score is taken as the final result.

[0053] Step (9): Use the ICP algorithm to correct the pose result in step (8), and set the objective function as:

[0054]

[0055] Among them, M is the total number of point pairs, p i and q i For the neighboring point pairs in the scene point cloud and the model point cloud. Set the threshold f t , when the objective function value is less than the threshold f t , then accept the revised results R and T at this time, otherwise continue to iterate.

[0056] Step (10): transform the initial optimal grasping posture T0 through formula (7) to obtain the optimal grasping posture in the current environment, send a control command to the robot controller, and complete the grasping.

[0057]

[0058] Among them, T0 is the initial optimal grasping posture of the model, and R and T are the rotation and translation matrices after ICP correction.

Claims

1. A robot grasping method based on a depth camera, the method comprising the following steps: Step 1: Establish a model database of the target object in the offline stage; use the 3D model of the known target object to obtain point cloud data and the initial optimal grasping posture T0, and transform the point cloud data into a voxel size V size =(w v ,h v ,d v ) filter to downsample, and then extract the point pair feature PPF of the point cloud data, put similar PPFs in the same group and store them in the hash table, where each pair of PPFs can be expressed as: F m (p m ,p n )=(d f ,θ1,θ2,θ3) (1) (1) In the formula, p m and p n are any two points in the point cloud data, d f It's point p m With point p n The Euclidean distance between them, θ1 and θ2 are respectively m With point p n The angle between the normal and the two points is the angle between the vectors, θ3 is the angle between the point p m With point p n The angle between normals; Step 2: Obtain RGB image data and depth image data in the current environment through the depth camera; Obtain RGB image data and depth image data in the current environment through the depth camera, use the YOLO-V5 target detection algorithm to detect the target object in the current environment, and obtain its pixel range in the RGB image: Pixel obj ={(x obj ,y obj )|(x left ,y left )<(x obj ,y obj )<(x right ,y right )} (2) Among them, (x obj ,y obj ) is the pixel point of the target object, (x left ,y left ) and (x right ,y right ) are the upper left pixel and the lower right pixel of the pixel box respectively; Step 3: Since the color camera and infrared sensor of the depth camera are located at different positions, there is a deviation between the original RGB image and the depth image. The depth image and the RGB image are aligned using formula (3); Among them, p rgb and p ir is the corresponding pixel in the RGB image and the depth image, I rgb and I ir are the intrinsic parameter matrices of RGB camera and infrared camera respectively, E rgb 、E ir , T rgb and T ir are the external parameters of RGB camera and infrared camera; Step 4: Since there may be a deviation between the detected bounding box and the actual object boundary, it is necessary to correct the pixel frame range in the depth image; map the pixel frame obtained in step 2 to the aligned depth image; Step 4 calculates the depth value d of the center point of the pixel frame c , set the threshold d t =d c -d0, d0 is a constant; calculate the depth values ​​of the pixels on the four boundary boxes. If there is a depth value on a border that is less than the set threshold d t If there are pixels on the four borders, move the border outward by n pixels until the depth values ​​of the pixels on the four borders are all greater than the set threshold; Step 5: Calculate the coordinates of all pixels in the corrected pixel frame in step 4 in the camera coordinate system using formula (4) to obtain scene point cloud data; Among them, x c ,y c 、z c is the coordinate of the pixel point (u, v) in the camera coordinate system, d is the depth distance at the pixel point (u, v), dx, dy, u0, v0 and f are the camera intrinsic parameters; Step 6: Transform the scene point cloud into a size =(w v ,h v ,d v ) filter to downsample and then estimate the surface normal vector of the point cloud; Step 7: Randomly generate a series of reference points q of the scene point cloud i , each reference point q i And the scene point cloud constitutes a point pair feature F s (q i ,q j );Set the screening threshold θ of PPF normal angle and distance t and d t , by searching the hash table established in step 1, find the scene point feature F within the threshold s (q i ,q j ) Similar model point pair features F m (p m ,p n ), each pair of similar F m (p m ,p n ) and F s (q i ,q j ) j and p n The conversion relationship can be expressed as follows: Among them, T qi and T pm The points q are i and p m The transformation matrix converted to the origin of the camera coordinate system, α is the transformation matrix of q i and p m Transform to the origin and align its normal vector with the x-axis back vector and The angle between them, R(α) is the transformation matrix with a rotation angle of α; Step 8: Use the voting strategy method to select each reference point q i The multiple postures obtained are voted, and the posture with the highest votes is used as the posture result of the reference point; the posture results of all reference points are clustered, and each cluster is scored. The score of a single cluster is equal to the sum of the votes of all postures in it, and the average value of the postures in the cluster with the highest score is taken as the final result; Step 9: Use the ICP algorithm to correct the pose result in step 8, and set the objective function as: Among them, N is the total number of point pairs, p i and q i For the neighboring point pairs in the scene point cloud and the model point cloud; set the threshold f t , when the objective function value is less than the threshold f t , then accept the revised results R and T at this time, otherwise continue to iterate; Step 10: The initial optimal grasping posture T0 is transformed by formula (7) to obtain the optimal grasping posture in the current environment, and a control command is sent to the robot controller to complete the grasping; Among them, T0 is the initial optimal grasping posture of the model, and R and T are the rotation and translation matrices after ICP correction.

Images