CN114851201B - A six-degree-of-freedom visual closed-loop grasping method for robotic arm based on TSDF 3D reconstruction - Google Patents

Abstract

The invention relates to a six-degree-of-freedom visual closed-loop grasping method of a mechanical arm based on TSDF three-dimensional reconstruction. It includes the following steps: Step 1: Calibrate the coordinate system of the base of the manipulator and the coordinate system of the camera through Zhang Zhengyou’s camera calibration method and ArUco Markers calibration method; Step 2: Use the TSDF function to reconstruct the 3D scene from the acquired image information to reduce the distance between objects. environmental noise points between them; Step 3: Establish a reinforcement learning network model; Step 4: Back-project the predicted grasping posture of the end effector to the 3D reconstruction scene to judge the quality of the predicted grasping; Step 5: Through the robot Inverse kinematics completes the grasping movement of the robotic arm; Step 6: Carry out reinforcement learning model training to enable the robotic arm to complete the grasping action; the present invention overcomes the shortcomings of the prior art and proposes an easy-to-implement, high-applicability, three-dimensional based on TSDF The rebuilt six-degree-of-freedom visual closed-loop grasping system of the robotic arm reduces the environmental noise caused by occlusion and stacking between objects, and reduces the depth error caused by the interference of a single visual sensor. In addition, the system can realize fast real-time target detection and complete grasping action while ensuring high precision.

Description

A six-degree-of-freedom visual closed-loop grasping method for robotic arm based on TSDF 3D reconstruction

technical field

The invention belongs to a six-degree-of-freedom object grasping technology of a mechanical arm in a non-structural environment, specifically the field of robot grasping control.

Background technique

Object grasping has always been an important problem in robotics, but there has been no satisfactory corresponding solution. Thanks to the multi-functional manipulation system of the robotic arm, it can flexibly control the end effector with a high degree of freedom in three-dimensional space, achieving dexterous grasping of objects and dynamic response to environmental changes. Recently, with the rapid development of deep learning and reinforcement learning and the construction of corresponding systems, various feasible ideas have been provided for intelligent grasping methods of manipulators.

Although the 6-DOF grasping control of the manipulator has high practical value and is suitable for more complex operating environments, most current data-driven grasping algorithms only perform top-down grasping in simple desktop settings (4Dof: x , y, z, yaw), or use physical analysis to master the appropriate grasping posture. However, due to the limitation of three-dimensional mobile grasping, the algorithm has great limitations on the application scenarios. The end effector of the manipulator can only approach the object from the vertical downward direction. In some cases, it cannot grasp the object along this direction. . For example, the gripper is difficult to grip a plate that is placed horizontally. In addition, these methods based on physical analysis not only need to calculate a large amount of experimental environment data, but also need to calculate and estimate an accurate target model, which inevitably results in a lot of time and calculation costs, and the physical model of an unstructured target object is often insufficient. General, so most algorithms are difficult to be directly applied to the grasping of novel objects, and the robustness and generalization of the system are poor.

Therefore, the idea of 6-degree-of-freedom (6-DOF) grasping of the manipulator is proposed. Although, the PointnetGPD proposed by Hongzhuo Liang uses a sampling-evaluation two-step method to determine a reliable grasping pose by evaluating a large number of samples. However, this method is undoubtedly quite time-consuming. (Hongzhuo Liang et al. "Pointnetgpd: Detecting grasp configurations from point sets". In: 2019 International Conference on Robotics and Automation (ICRA). IEEE. 2019, pp. 3629–3635). Florence et al. perform pose transfer from existing grasping poses. However, the success rate of this algorithm is relatively low for target objects and object geometries that are not recorded in the data set, so it cannot be extended to new application scenarios. (Peter Florence, Lucas Manuelli, and Russ Tedrake. "Dense Object Nets: Learning Dense Visual Object Descriptors By and For Robotic Manipulation". In: Conference on Robot Learning (CoRL) (2018)). Recently, the point cloud scene is reconstructed from the partial view obtained by the RGBD camera, and then the designed Pointnet++ network model is used to extract the object features, and the 6Dof grasp pose regression is directly completed and the grasp planning is performed on the reconstructed point cloud scene. (P.Ni, W.Zhang, X.Zhu, and Q.Cao, "Learning an end-to-end spatial grasp generation and refinement algorithm from simulation," Machine Vision and Applications, vol.32, no.1, pp.1 –12, 2021.).

Contents of the invention

The invention overcomes the disadvantages of the prior art, and proposes an easy-to-implement, high-applicability grasping method of a mechanical arm with six degrees of freedom. The present invention designs a feature extraction network and a Policy Gradient reinforcement learning framework, which can realize the positioning of unrecorded objects in the training data set and the six-degree-of-freedom plan grasping.

The present invention takes the image sequence as input, and first constructs the mechanical arm coordinate system and the camera coordinate system through the Zhang Zhengyou camera calibration method and the ArUco Markers calibration method. Then use the Intel Rrealsense D435 depth camera to obtain the color and depth information of the grabbing platform. Through the Truncated Signed Distance Function (TSDF), the 3D scene reconstruction is performed on the acquired image information to reduce the environmental noise points between objects. The two-dimensional top-view image of the reconstructed scene is intercepted from top to bottom, and used as the input of the grasping neural network model designed in this paper. The model encodes the input image and extracts features, and inputs the predicted grasping area of the current environment and the grasping posture of the robotic arm. The structure diagram of the network model is shown in Figure 1. The model uses a Renet fully convolutional feature extraction network architecture to learn to extract unstructured features from RGB-D partial observations. The model connects color features and depth features to channels, using 2 additional Batch Normalization (BN), Nonlinear activation functions (ReLU), and 1×1 Convolutional Layers interleaved. Then use average pooling (Average Pooling) to aggregate the feature embedding of the same pixel unit, and perform linear upsampling, so that the convolution features are shared in different positions and directions, the output image is consistent with the input resolution, and the maximum capture probability hotspot is selected The coordinates of the block correspond to the hemispherical three-dimensional grasping attitude point defined before the experiment (see Figure 2), and the corresponding 6-dimensional attitude is used as the predicted 6-degree-of-freedom grasping attitude. Secondly, the predicted grasping posture input by the model is back-projected to the reconstructed 3D scene, and the depth image under the current posture is intercepted from the 3D scene by using the TSDF function, and the grasping quality is judged according to the depth information at both ends of the gripper. Finally, when the depth of the target coordinate point in the rendered image is less than or equal to the depth of the fingertips at both ends of the gripper (15cm), input the predicted pose into the forward kinematics of the robot to obtain the corresponding movement trajectory of the robotic arm, and complete the grasping action of the robotic arm. If this capture is successful, the reward assigned to the agent is 1, otherwise the reward assigned to the agent is 0. See Figure 3 for the flow chart of the system capture.

The present invention is a six-degree-of-freedom visual closed-loop grasping method of a mechanical arm based on TSDF three-dimensional reconstruction, and the specific steps are as follows:

Step 1: Zhang Zhengyou camera calibration method and ArUco Markers calibration method to calibrate the coordinate system of the manipulator base and the camera coordinate system:

First, the Intel D415 depth camera is vertically fixed at the end of the robotic arm, so that the camera and the end effector jaws are in a fixed attitude transformation, so that the binocular camera can collect image information of objects on the operating table. Then, Zhang Zhengyou camera calibration method and ArUco Markers calibration method were used to calibrate the internal and external parameters of the camera, and construct a three-dimensional coordinate system with the base of the manipulator as the origin. The calibration formulas of camera intrinsic and extrinsic parameters are as follows:

Among them, x _w , y _w , z _w are the three-dimensional coordinates in the world coordinate system; x _c , y _c , z _c are the coordinates in the image coordinate system; u, v are the pixel coordinate system; z _m is the hypothetical camera coordinate system p is the imaging point in the image coordinate system; f is the imaging distance; d _x , d _y are the coordinates of the pixel point in the image coordinate system; R, T are the rotation matrix and translation matrix. Formula (1) is the formula from the world coordinate system to the camera coordinate system. Formula (2) is the formula from the camera coordinate system to the ideal image coordinate system. Formula (3) is the matrix from the camera coordinate system to the pixel coordinate system, that is, the internal reference matrix.

Step 2: Use the TSDF function to perform 3D scene reconstruction on the acquired image information to reduce the environmental noise points between objects:

Step 2.1: The present invention uses a single sensor to obtain visual information in actual experiments, and obtains the information (color information and depth information) captured by the binocular camera on the grasping console through OpenCV.

Step 2.2: Due to environmental factors such as light and mutual occlusion between objects, there are large noise points in the depth image acquired by the sensor, which affects the image capture prediction of the network model. Firstly, the present invention samples the spherical points of the grabbing operation table before each grabbing, and reduces the depth error in a single image by sampling local images multiple times. Second, the TSDF function is used to fuse the pose of the sampling point and the information of the sampled image into a 3D reconstruction voxel grid. For a single voxel, it not only contains the intuitive x, y, z coordinates but also uses two other values (SDF and CAM) to represent the distance from the voxel to the most surface. Among them, SDF is a signed distance function for the fusion of depth maps. It uses 0 to represent the surface, positive values represent points outside the model, and negative values represent points inside the model. The value is proportional to the distance from the point to the nearest surface. The surface is easily extracted as the zero crossings of the function.

Step 2.3: Define the coordinate system where the above voxel grid is located as the world coordinate system, and the voxel coordinates can be expressed as The pose of the camera is /> The internal parameter matrix of the camera is K.

Therefore, according to the transformation matrix obtained by ICP registration, the voxel grid in the world coordinate system Projected to the camera coordinate system, and then converted to the image coordinate system I _ij according to the camera internal reference matrix K, where i∈(0,1,...,n),j∈(0,1,...,m).

After projecting the voxel grid to the image coordinate system, it is first necessary to calculate the initial SDF value of each voxel, as shown in formula (18)

In the formula Indicates the position information of the jth voxel in the world coordinate system, /> Indicates the position information of the i-th camera pose in the world coordinate system, and dep(I _ij ) indicates the depth value of the j-th voxel in the i-th camera depth image coordinate system.

Then truncate the sdf value of each voxel according to formula (19).

In the formula, tsdf _i is the voxel truncation distance value, and trunc represents the artificially set truncation distance, which can be understood as the error value of the camera depth information. If the error is larger, the value should be set larger, otherwise it may cause a lot of depth camera acquisition The information is lost, and the trunc of this item is set to 1.

After truncating the sdf value of each voxel, recalculate the tsdf value of each voxel according to formula (20):

Where ω _j (x, y, z) is the weight of the voxel in the global voxel grid of the current frame, ω _j-1 (x, y, z) is the weight of the voxel in the previous frame of the voxel grid, tsdf _j is the distance from the voxel in the global data cube in the current frame to the object surface, tsdf _j-1 is the distance from the voxel in the global data cube in the previous frame to the object surface, V _.z represents the z-axis of the voxel in the camera coordinate system Coordinates, D(u,v) represents the depth value at the depth image (u,v) of the current frame.

Equation (9) calculates the weight value of each voxel.

w _j =w _j-1 +w _j (9)

The environment information of the grabbing console is rendered from top to bottom and used as the input of the network model.

Step 3: Build a reinforcement learning network model:

Step 3.1: Design the grasping strategy of the robotic arm and define the action space for reinforcement learning: the present invention designs and uses a two-dimensional convolutional network to directly return the 6-DoF grasping posture by increasing the network dimension. See attached drawing 2 for the gripping action of the gripper definition.

Step 3.2: Convert each element in the array to the angle at which the end of the robot arm rotates around the three coordinate axes of x, y, and z respectively. The specific conversion formula is as follows:

r _z ＝(best_pix_ind[2]*θ) (12)

where a _x represents the rotation angle of the end of the mechanical arm around the x-axis, which is the roll angle of the end effector; b _y represents the rotation angle of the end of the mechanical arm around the y-axis, which is the pitch angle of the end effector; r _z represents the mechanical The rotation angle of the end of the arm around the z-axis is the yaw angle of the end effector; Set the bias value for the experiment.

Step 3.3: Design the feature extraction network model: The model uses the Resnet fully convolutional feature extraction network architecture to learn to extract unstructured features from RGB-D partial observations. After the multi-layer feature extraction convolutional layer extracts the features of the color image and the depth image, the color feature and the depth feature are channel-connected through the Concat function, and 2 additional BatchNormalization (BN), Nonlinear activation functions (ReLU), 1× 1ConvolutionalLayers, and upsampling will keep the resolution of the output heatmap consistent with the input image. The corresponding pose in the action space corresponding to the coordinate of the largest vector value in the tensor graph is selected as the predicted grasping pose output.

Step 3.4: Perform forward inference on the network by the following formula:

Among them, formula (13) represents the expected return under state s and action a, where g _t represents the grasping action taken at time t, s _t represents the state at time t, and r _t represents the reward at time t; formula (14) expresses The total reward function of the network; Formula (15) is the state distribution function; Formula (16) represents the state-action function.

Step 3.5 designs the reinforcement learning network loss function, and uses the calculation cross-entropy function to further improve the detection accuracy of the algorithm:

Where τ=s ₀ a ₀ s ₁ a ₁ ...s _n a _n ... represents a Markov process.

because P _r {a|s}=π(s,a), so formula (18) can be obtained.

The weight update function looks like this:

where f _ω :S×A→R is the pair Approximate function of , when f _ω takes the minimum value, Δω=0, formula (21) can be deduced

Step 3.6: Set reinforcement learning rewards: If the grasping is successful, the reward assigned to the agent is 1, otherwise the reward assigned to the agent is 0.

Step 4: Back-project the predicted grasping posture of the end effector into the 3D reconstruction scene to judge the quality of the predicted grasping: back-project the 6-DOF grasping posture output by the network to the 3D volume created before grasping In the pixel scene, the depth image under the predicted pose is intercepted through the perspective cone projection. See Figure 4 for the perspective cone projection. The viewing frustum is a three-dimensional body whose position is related to the camera's extrinsic parameter matrix. The shape of the viewing frustum determines how the model is projected from the pixel space to the plane. Perspective projection uses a pyramid as the viewing frustum, and the camera is positioned at the apex of the pyramid. The pyramid is truncated by the front and rear planes to form a truss called View Frustum. Its model is shown in Figure 5, and only the model inside the Frustum is visible. There are two special planes in this clipping space, which are called the near clip plane and the far clip plane.

By comparing the depth information at both ends of the gripper in the rendered image, the distance position between the parallel grippers and the object is inferred, so as to judge the grasping quality of the predicted grasping posture, and form a closed-loop feedback of the 6-DOF grasping system of the robotic arm.

Step 5: Complete the grasping movement of the robotic arm through the forward and reverse kinematics of the robot:

First, when the depth of both ends of the gripper in the depth image rendered in step 4 above is less than or equal to the depth of the fingertip of the gripper, that is, the predicted posture of the gripper is located on both sides of the object, it is judged that the predicted posture can be grasped this time. Then, the manipulator is controlled to execute the grasping plan according to the predicted grasping posture: the 6 joint angles of the manipulator in the current state are obtained through inverse kinematics of the robot. Then input the 6-dimensional grasping posture of the end of the mechanical arm predicted by the reinforcement learning network in step 3 into the forward kinematics of the robot, and obtain the movement trajectory of the end effector of the mechanical arm from the current posture to the predicted posture point. When the end effector of the robotic arm moves to the predicted grasping posture, the robot sends a signal to close the clamp and tries to grasp. After the gripper is closed, the end effector moves up 15cm vertically, and the depth image after the actuator moves up is obtained from the Intel Rrealsense D435 camera, and the actual grasping is judged by calculating the depth at both ends of the gripper. When the grasping is successful, the reward of the reinforcement learning model is assigned a value of 1; when the grasping fails, the reward of the reinforcement learning network is assigned a value of 0.

Step 6: Carry out reinforcement learning model training so that the robotic arm can complete the grasping action:

In the CoppeliaSim Edu simulation environment, the above steps 2 to 5 are repeated continuously, and the loss function in the reinforcement learning model is reduced through reinforcement learning rewards, and the model weight parameters are updated. Finally, import the trained weight parameters into the real robotic arm UR5 for experimental debugging, repeat steps 1 to 5, and complete the six-degree-of-freedom closed-loop grasping task of the robotic arm.

In summary, the present invention has the advantage of directly predicting and outputting the 6-dimensional grasping posture of the manipulator by increasing the dimension of the two-dimensional neural network. At the same time, the present invention performs three-dimensional reconstruction on the complex grasping environment through the TSDF function, reduces the environmental noise caused by occlusion and stacking between objects, and reduces the depth error caused by the interference of a single visual sensor. At the same time, a reinforcement learning network is designed for this method, which overcomes the shortcomings of cumbersome calculation and high time cost of the traditional physical derivation of the grasping attitude of the manipulator, and solves the problem that the 6-DOF grasping attitude of the manipulator cannot be applied to unrecorded targets in the data set. object problem. It not only ensures a high grasping success rate of the manipulator model, but also benefits from the generalization of reinforcement learning, that is, the method can be applied to new grasping objects, which solves the time-consuming calculation of traditional methods and reduces the input part. Instability of point cloud models. The invention realizes the real-time detection of the captured object and the function of 6-DOF grasping.

Description of drawings

Fig. 1 is the structural diagram of network model among the present invention;

Fig. 2 is a hemispherical three-dimensional grasping attitude angle diagram defined by the present invention;

Fig. 3 is the grab flow chart of the system of the present invention;

Fig. 4 is the perspective cone projection diagram;

Figure 5 is a view of the View Frustum model;

Figure 6 is an example diagram of experimental capture;

Detailed ways

Further illustrate the present invention below in conjunction with accompanying drawing.

The 6-DOF visual closed-loop grasping method of the robotic arm based on the TSDF three-dimensional reconstruction of the present invention, an example diagram of the physical grasping is shown in Figure 6, and the specific process is as follows:

Step 1: Zhang Zhengyou camera calibration method and ArUco Markers calibration method to calibrate the coordinate system of the manipulator base and the camera coordinate system:

First, the Intel D415 depth camera is vertically fixed at the end of the robotic arm, so that the camera and the end effector jaws are in a fixed attitude transformation, so that the binocular camera can collect image information of objects on the operating table. Then, Zhang Zhengyou camera calibration method and ArUco Markers calibration method were used to calibrate the internal and external parameters of the camera, and construct a three-dimensional coordinate system with the base of the manipulator as the origin. The calibration formulas of camera intrinsic and extrinsic parameters are as follows:

Among them, x _w , y _w , z _w are the three-dimensional coordinates in the world coordinate system; x _c , y _c , z _c are the coordinates in the image coordinate system; u, v are the pixel coordinate system; z _m is the hypothetical camera coordinate system p is the imaging point in the image coordinate system; f is the imaging distance; d _x , d _y are the coordinates of the pixel point in the image coordinate system; R, T are the rotation matrix and translation matrix. Formula (1) is the formula from the world coordinate system to the camera coordinate system. Formula (2) is the formula from the camera coordinate system to the ideal image coordinate system. Formula (3) is the matrix from the camera coordinate system to the pixel coordinate system, that is, the internal reference matrix.

Step 2: Use the TSDF function to perform 3D scene reconstruction on the acquired image information to reduce the environmental noise points between objects:

Step 2.1: The present invention uses a single sensor to obtain visual information in actual experiments, and obtains the information (color information and depth information) captured by the binocular camera on the grasping console through OpenCV.

Step 2.2: Due to environmental factors such as light and mutual occlusion between objects, there are large noise points in the depth image acquired by the sensor, which affects the image capture prediction of the network model. Firstly, the present invention samples the spherical points of the grabbing operation table before each grabbing, and reduces the depth error in a single image by sampling local images multiple times. Second, the TSDF function is used to fuse the pose of the sampling point and the information of the sampled image into a 3D reconstruction voxel grid. For a single voxel, it not only contains the intuitive x, y, z coordinates but also uses two other values (SDF and CAM) to represent the distance from the voxel to the most surface. Among them, SDF is a signed distance function for the fusion of depth maps. It uses 0 to represent the surface, positive values represent points outside the model, and negative values represent points inside the model. The value is proportional to the distance from the point to the nearest surface. The surface is easily extracted as the zero crossings of the function. If the depth value D(u,v) at the depth image D(u,v) of the i-th frame is not 0, compare D(u,v) with the z phase in the voxel camera coordinates V(x,y,z) In comparison, if D(u,v) is greater than z, it means that this voxel is closer to the camera and is used to reconstruct the outer surface of the scene; if D(u, v) is smaller than z, it means that this voxel is farther away from the camera, which is used to reconstruct the scene The inner surface. That is, if the voxel point is outside the surface (closer to the camera side), the SDF value is positive, and if the voxel is inside the surface, the SDF value is negative.

Step 2.3: Define the coordinate system where the above voxel grid is located as the world coordinate system, and the voxel coordinates can be expressed as The pose of the camera is /> The internal parameter matrix of the camera is K.

Therefore, according to the transformation matrix obtained by ICP registration, the voxel grid in the world coordinate system Projected to the camera coordinate system, and then converted to the image coordinate system I _ij according to the camera internal reference matrix K, where i∈(0,1,...,n),j∈(0,1,...,m).

After projecting the voxel grid to the image coordinate system, it is first necessary to calculate the initial SDF value of each voxel, as shown in formula (18)

In the formula Indicates the position information of the jth voxel in the world coordinate system, /> Indicates the position information of the i-th camera pose in the world coordinate system, and dep(I _ij ) indicates the depth value of the j-th voxel in the i-th camera depth image coordinate system.

Then truncate the sdf value of each voxel according to formula (19).

In the formula, trunc represents the artificially set truncation distance, which can be understood as the error value of the camera’s depth information. If the error is larger, the value should be set larger. Otherwise, a lot of information acquired by the depth camera may be lost. The trunc of this project is set to 1.

After truncating the sdf value of each voxel, recalculate the tsdf value of each voxel according to formula (20):

Where ω _j (x, y, z) is the weight of the voxel in the global voxel grid of the current frame, ω _j-1 (x, y, z) is the weight of the voxel in the previous frame of the voxel grid, tsdf _j is the distance from the voxel in the global data cube in the current frame to the object surface, tsdf _j-1 is the distance from the voxel in the global data cube in the previous frame to the object surface, V _.z represents the z-axis of the voxel in the camera coordinate system Coordinates, D(u,v) represents the depth value at the depth image (u,v) of the current frame.

Equation (9) calculates the weight value of each voxel.

w _j =w _j-1 +w _j (9)

The environment information of the grabbing console is rendered from top to bottom and used as the input of the network model.

Step 3: Build a reinforcement learning network model:

Step 3.1: Design the grasping strategy of the manipulator and define the action space for reinforcement learning: usually, the target grasping is divided into two grasping position areas and three-dimensional rotation, and the two network models are used to predict the grasping area and three-dimensional rotation respectively. However, the present invention is designed to use a two-dimensional convolutional network to directly return to the 6-DoF grasping posture by increasing the network dimension. In the present invention, the grasping action space of a single object is 16*28*28 actions, of which 16 are the rotation actions of the end effector around the z coordinate axis (22.5°), and 28*28 are the terminal execution on the x and y coordinate axes Number of rotation actions. See attached drawing 2 for the gripping action of the gripper definition.

Step 3.2: Convert each element in the array to the angle at which the end of the robot arm rotates around the three coordinate axes of x, y, and z respectively. The specific conversion formula is as follows:

r _z ＝(best_pix_ind[2]*θ) (12)

where a _x represents the rotation angle of the end of the mechanical arm around the x-axis, which is the roll angle of the end effector; b _y represents the rotation angle of the end of the mechanical arm around the y-axis, which is the pitch angle of the end effector; r _z represents the mechanical The rotation angle of the end of the arm around the z-axis is the yaw angle of the end effector.

In the actual crawling example, bring in the deviation value θ = 180/16, calculate the predicted grasp pose.

Step 3.3: Design the feature extraction network model: The model uses the Resnet fully convolutional feature extraction network architecture to learn to extract unstructured features from RGB-D partial observations. After the multi-layer feature extraction convolutional layer extracts the features of the color image and the depth image, the color feature and the depth feature are channel-connected through the Concat function, and 2 additional BatchNormalization (BN), Nonlinear activation functions (ReLU), 1× 1ConvolutionalLayers, and upsampling will keep the resolution of the output heatmap consistent with the input image. The corresponding pose in the action space corresponding to the coordinate of the largest vector value in the tensor graph is selected as the predicted grasping pose output.

Step 3.4: Perform forward inference on the network by the following formula:

Among them, formula (13) represents the expected return under state _s and action a, where g _t represents the grasping action taken at time t, s _t represents the state at time t, and r _t represents the reward at time t; formula (14) expresses The total reward function of the network; Formula (15) is the state distribution function; Formula (16) represents the state-action function.

Step 3.5 designs the reinforcement learning network loss function, and uses the calculation cross-entropy function to further improve the detection accuracy of the algorithm:

Where τ=s ₀ a ₀ s ₁ a ₁ ...s _n a _n ... represents a Markov process.

because P _r {a|s}=π(s,a), so formula (18) can be obtained.

The weight update function looks like this:

where f _ω :S×A→R is the pair Approximate function of , when f _ω takes the minimum value, Δω=0, formula (21) can be deduced

Step 3.6: Set reinforcement learning rewards: If the grasping is successful, the reward assigned to the agent is 1, otherwise the reward assigned to the agent is 0.

Step 4: Back-project the predicted grasping posture of the end effector into the 3D reconstruction scene to judge the quality of the predicted grasping: back-project the 6-DOF grasping posture output by the network to the 3D volume created before grasping In the pixel scene, the depth image under the predicted pose is intercepted through the perspective cone projection. See Figure 4 for the perspective cone projection. The viewing frustum is a three-dimensional body whose position is related to the camera's extrinsic parameter matrix. The shape of the viewing frustum determines how the model is projected from the pixel space to the plane. Perspective projection uses a pyramid as the viewing frustum, and the camera is positioned at the apex of the pyramid. The pyramid is truncated by the front and rear planes to form a truss called View Frustum. Its model is shown in Figure 5, and only the model inside the Frustum is visible. There are two special planes in this clipping space, which are called the near clip plane and the far clip plane.

By comparing the depth information at both ends of the gripper in the rendered image, the distance position between the parallel grippers and the object is inferred, so as to judge the grasping quality of the predicted grasping posture, and form a closed-loop feedback of the 6-DOF grasping system of the robotic arm.

Step 5: Complete the grasping movement of the robotic arm through the forward and reverse kinematics of the robot:

First, when the depth of both ends of the gripper in the depth image rendered in step 4 above is less than or equal to the depth of the fingertip of the gripper, that is, the predicted posture of the gripper is located on both sides of the object, it is judged that the predicted posture can be grasped this time. Then, the manipulator is controlled to execute the grasping plan according to the predicted grasping posture: the 6 joint angles of the manipulator in the current state are obtained through inverse kinematics of the robot. Then input the 6-dimensional grasping posture of the end of the mechanical arm predicted by the reinforcement learning network in step 3 into the forward kinematics of the robot, and obtain the movement trajectory of the end effector of the mechanical arm from the current posture to the predicted posture point. When the end effector of the robotic arm moves to the predicted grasping posture, the robot sends a signal to close the clamp and tries to grasp. After the gripper is closed, the end effector moves up 15cm vertically, and the depth image after the actuator moves up is obtained from the Intel Rrealsense D435 camera, and the actual grasping is judged by calculating the depth at both ends of the gripper. When the grasping is successful, the reward of the reinforcement learning model is assigned a value of 1; when the grasping fails, the reward of the reinforcement learning network is assigned a value of 0.

Step 6: Carry out reinforcement learning model training so that the robotic arm can complete the grasping action:

Import the trained model weight parameters into the real robotic arm UR5, conduct experimental debugging, repeat steps 1 to 5, and complete the six-degree-of-freedom closed-loop grasping task of the robotic arm. After 200 rounds of grasping tests, the TSDF-based six-degree-of-freedom grasping method of the present invention has a grasping success rate of 85.3% for unstructured objects (10 target objects are randomly stacked). When the number of target objects increases to 15 random stacking situations, the grabbing success rate of the present invention still has 81.5%. To sum up, the present invention designs a reinforcement learning network, which overcomes the cumbersome calculation of the grasping attitude of the manipulator through traditional physical derivation and the shortcomings of high time cost, and solves the problem that the 6-DOF grasping attitude of the manipulator cannot be applied to the unrecorded data set. target audience problem. It not only ensures a high grasping success rate of the manipulator model, but also benefits from the generalization of reinforcement learning. The method of the present invention can be applied to unrecorded grasping objects, which solves the time-consuming calculation of the traditional method and reduces the input Instability of some point cloud models.

Claims (4)

1. A six-DOF vision closed-loop grasping method based on TSDF three-dimensional reconstruction, is characterized in that: comprise the steps: Step 1: Calibrate the coordinate system of the manipulator base and the camera coordinate system through the Zhang Zhengyou camera calibration method and the ArUco Markers calibration method; Step 2: Use the TSDF function to perform 3D scene reconstruction on the acquired image information to reduce the environmental noise points between objects; the specific steps are as follows: 2.1): A single sensor is used to obtain visual information, and the information captured by the binocular camera on the grabbing console is obtained through OpenCV. The captured information includes color information and depth information; 2.2): Due to the environmental factors of mutual occlusion between light and objects, there are large noise points in the depth image acquired by the sensor, which affects the image capture prediction of the network model; first, the capture operation is performed before each capture Spherical point sampling is carried out on the platform, and the depth error in a single image is reduced by sampling local images multiple times; secondly, the TSDF function is used to fuse the sampling point pose and sampled image information into a 3D reconstruction voxel grid; for a single voxel, It not only contains the intuitive x, y, z coordinates, but also uses SDF and CAM two values to represent the distance from the voxel to the most surface, where SDF is a symbolic distance function used for the fusion of depth maps. It uses 0 to represent the surface and a positive value Indicates the external point of the model, the negative value indicates the internal point of the model, and the value is proportional to the distance from the point to the nearest surface; the surface is easily extracted as the zero-crossing point of the function; if the depth value D(u , v) is not 0, then compare D(u, v) with z in the voxel camera coordinates V(x, y, z), if D(u, v) is greater than z, it means that this voxel is far from the camera Closer, used to reconstruct the outer surface of the scene; if D(u, v) is less than z, it means that this voxel is farther away from the camera, which is to reconstruct the inner surface of the scene; the voxel point is located outside the surface and closer to the camera side, then SDF The value is positive; if the voxel is within the surface, the SDF value is negative; 2.3): Define the coordinate system where the above voxel grid is located as the world coordinate system, and the voxel coordinates are expressed as The pose of the camera is /> The internal parameter matrix of the camera is K; Therefore, according to the transformation matrix obtained by ICP registration, the voxel grid in the world coordinate system Project to the camera coordinate system, and then convert to the image coordinate system I _ij according to the camera internal reference matrix K, where i∈(0,1,...,n), j∈(0,1,...,m); After projecting the voxel grid to the image coordinate system, it is first necessary to calculate the initial SDF value of each voxel, as shown in formula (6) In the formula Indicates the position information of the jth voxel in the world coordinate system, /> Indicates the position information of the i-th camera pose in the world coordinate system, dep(I _ij ) indicates the depth value of the j-th voxel in the i-th camera depth image coordinate system; The sdf value of each voxel is truncated according to formula (7); In the formula, tsdf _i is the voxel truncated distance value, and trunc represents the artificially set truncated distance, which can be understood as the error value of the camera depth information. If the error is larger, the trunc setting should be larger, otherwise it may cause a lot of information obtained by the depth camera Lost, set trunc to 1; After truncating the sdf value of each voxel, recalculate the tsdf value of each voxel according to formula (8): where ω _j (x, y, z) is the weight of voxels in the global voxel grid of the current frame, ω _j-1 (x, y, z) is the weight of voxels in the previous frame of voxel grid, tsdf _j is the distance from the voxel in the global data cube in the current frame to the object surface, tsdf _j-1 is the distance from the voxel in the global data cube in the previous frame to the object surface, V _.z represents the z-axis of the voxel in the camera coordinate system Coordinates, D(u, v) represent the depth value at the depth image (u, v) of the current frame; Formula (9) calculates the weight value of each voxel; w _j =w _j-1 +w _j (9) Render the environment information of the grab console from top to bottom and use it as the input of the network model; Step 3: Establish a reinforcement learning network model; Step 4: Back-project the predicted grasping posture of the end effector into the 3D reconstruction scene to judge the quality of the predicted grasping; the specific steps are as follows: Back-project the predicted grasping posture of the end effector into the 3D reconstruction scene to judge the predicted grasping quality: back-project the 6-DOF grasping posture output by the network into the 3D voxel scene created before grasping , the depth image under the predicted attitude is intercepted through the perspective cone projection; by comparing the depth information at both ends of the gripper in the rendered image, the distance between the parallel jaws and the object is deduced, so as to judge the grasping quality of the predicted grasping attitude and form a mechanical Closed-loop feedback of grasping system with 6 degrees of freedom of the arm; Step 5: Complete the grasping movement of the robotic arm through the forward and reverse kinematics of the robot; the specific steps are as follows: When the depth of both ends of the gripper in the rendered depth image is less than or equal to the depth of the fingertip of the gripper, that is, the predicted posture of the gripper is located on both sides of the object, it is judged that the predicted posture can be grasped; then, control the manipulator according to the predicted posture Execute the grasping plan: solve the 6 joint angles of the manipulator in the current state through the inverse kinematics of the robot; then input the 6-dimensional grasping posture at the end of the manipulator predicted by the reinforcement learning network model in step 3 into the forward motion of the robot Learning, to obtain the movement trajectory of the end effector of the manipulator from the current attitude to the predicted attitude point; when the end effector of the manipulator moves to the predicted attitude, the robot sends a signal to close the gripper and tries to grasp the action; the gripper After closing, the end effector moves up vertically by 15cm, obtains the depth image of the end effector after moving up from the binocular camera, and judges whether the actual grasp is successful by calculating the depth at both ends of the gripper; when the grasp is successful, the reinforcement learning model rewards The assigned value is 1; when the capture fails, the reinforcement learning network reward is assigned a value of 0; Step 6: Perform reinforcement learning model training to make the robotic arm complete the grasping action. 2. The TSDF three-dimensional reconstruction-based robotic arm six-degree-of-freedom visual closed-loop grasping method according to claim 1, characterized in that: the specific steps of step 1 are as follows: First, fix the binocular camera vertically at the end of the robotic arm. The binocular camera uses an Intel D415 depth camera, so that the camera and the end effector are in a fixed attitude transformation, so that the binocular camera can collect and capture image information of objects on the console; The end effector is a gripper. Then, the internal and external parameters of the camera are calibrated by Zhang Zhengyou’s camera calibration method and ArUco Markers calibration method, and a three-dimensional coordinate system with the base of the manipulator as the origin is constructed. The calibration formulas of the internal and external parameters of the camera are as follows: Among them, x _w , y _w , z _w are three-dimensional coordinates in the world coordinate system; x _c , y _c , z _c are coordinates in the image coordinate system; u, v are pixel coordinate systems; (x _m , y _m , z _m ) is a point in the assumed camera coordinate system; (x _p , y _p , z _p ) is the imaging point in the image coordinate system; f is the imaging distance; d _x , d _y are the coordinates of the pixel point in the image coordinate system; R, T are the rotation matrix and translation matrix; formula (1) is the formula from the world coordinate system to the camera coordinate system, formula (2) is the formula from the camera coordinate system to the ideal image coordinate system, and formula (3) is the formula from the camera coordinate system to the pixel coordinates Coordinate matrix, that is, internal reference matrix. 3. The TSDF three-dimensional reconstruction-based robotic arm six-degree-of-freedom visual closed-loop grasping method according to claim 1, characterized in that: the specific steps of step 3 are as follows: 3.1): Design the grasping strategy of the manipulator and define the action space of reinforcement learning: design and use a two-dimensional convolutional network, and directly return the 6-DoF grasping posture by increasing the network dimension, so as to distinguish it from dividing the target grasping into two parts. A grasping position area and three-dimensional rotation amount, respectively predicting the grasping area and three-dimensional rotation amount through two network models; defining the grasping action space of a single object as 16*28*28 actions, 16 of which are end effectors Rotate around the z coordinate axis, 28*28 is the number of rotations of the end effector on the x and y coordinate axes; 3.2): Convert each element in the array to the angle at which the end of the robot arm rotates around the three coordinate axes of x, y, and z respectively. The specific conversion formula is as follows: a _x = ((best_pix_ind[0]-14)*30/28)-pi (10) b _y =((best_pix_ind[1]-14)*30/28) (11) r _z =(best_pix_ind[2]*180/16) (12) where a _x represents the rotation angle of the end of the mechanical arm around the x-axis, which is the roll angle of the end effector; b _y represents the rotation angle of the end of the mechanical arm around the y-axis, which is the pitch angle of the end effector; r _z represents the mechanical The rotation angle of the end of the arm around the z-axis is the yaw angle of the end effector, and θ is the deviation value set in the experiment; 3.3): Design feature extraction network model: The model adopts Resnet full convolution feature extraction network architecture, learning to extract unstructured features from RGB-D partial observations; multi-layer feature extraction convolution layer extracts the features of color images and depth images Finally, the color features and depth features are channel-connected through the Concat function, and 2 additional BatchNormalization, Nonlinear activation functions, 1×1 Convolutional Layers are used to interweave, and upsampling is used to keep the resolution of the output heat map consistent with the input image; Select the corresponding gesture in the action space corresponding to the coordinate of the largest vector value in the tensor graph as the output of the predicted grasping gesture; 3.4): Perform forward reasoning on the network by the following formula: Among them, formula (13) represents the expected return under state s and action a, where g _t represents the grasping action taken at time t, s _t represents the state at time t, and r _t represents the reward at time t; formula (14) expresses The total reward function of the network; formula (15) is the state distribution function; formula (16) represents the state-action function; 3.5): Design the reinforcement learning network loss function, and use the calculation cross entropy loss function to further improve the detection accuracy of the algorithm: Where τ=s ₀ a ₀ s ₁ a ₁ ... s _n a _n ... represents a Markov process; because P _r {a|s}=π(s, a), so formula (18) is obtained; The weight update function looks like this: where f _ω : S×A→R is the pair The approximate function of , when f _ω takes the minimum value, Δω=0, formula (20) can be deduced; 3.6): Set reinforcement learning rewards: if the capture is successful, the reward assigned to the agent is 1, otherwise the reward assigned to the agent is 0. 4. The TSDF three-dimensional reconstruction-based robotic arm six-degree-of-freedom visual closed-loop grasping method according to claim 1, characterized in that: the specific steps of step 6 are as follows: In the CoppeliaSim Edu simulation environment, repeat the above steps 2 to 5, reduce the cross-entropy loss function in the reinforcement learning network model through reinforcement learning rewards, and update the model weight parameters; finally, import the trained weight parameters into the real robot arm UR5 , carry out experimental debugging, repeat steps 1 to 5, and complete the six-degree-of-freedom closed-loop grasping task of the robotic arm.