Method for optimizing 6D grasping pose of robot arm based on deep reinforcement learning

Abstract

The application discloses a method for optimizing a 6D grabbing pose of a mechanical arm, comprising the following steps: adding a model of the mechanical arm and a depth camera in a Pybullet simulation environment, obtaining point cloud information of an object to be grabbed from the depth camera, and dividing the point cloud into two categories by using a K-means clustering method; constructing surrounding cuboids along the classified point cloud, and constructing suitable simulation grabbing poses according to the cuboids; selecting an optimized expert grabbing pose from an object grabbing data set according to the screened simulation grabbing poses, constructing a grabbing heuristic supervised learning target, and learning and training in combination with an imitation learning method and a deep reinforcement learning method. The application utilizes object surface structure features to construct simulation grabbing points more suitable for grabbing, selects an expert grabbing pose closest to the simulation grabbing points from an object grabbing data set for grabbing training, and finally realizes optimization of a grabbing pose of the mechanical arm by optimizing a heuristic target.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot technology application, in particular to a method for optimizing 6D grasping pose of a mechanical arm in continuous motion space. BACKGROUND

[0002] Learning human-level object manipulation skills is a high requirement for robots, and learning these skills can make robots repeat work automatically in the real world. At present, the research on intelligent robot grasping mainly focuses on how to improve the ability of robot to identify unknown objects, grasp pose selection and grasp execution. With the development of deep reinforcement learning (DRL) technology, robots have made great progress in basic tasks involving simple operation skills and have shown strong generalization. However, for robots to learn a general strategy that can handle more complex tasks, especially the 6D grasping task of the mechanical arm, such as grasping any shaped object, it is still a great challenge for deep reinforcement learning, because for complex shaped objects, we want the mechanical arm to grasp the object with a better pose, and improving the grasping pose of the mechanical arm is a problem still under research and relatively difficult compared to completing the grasping of the target object.

[0003] We cite the method of GA-DDPG and make improvements to it. The GA-DDPG method is to build a set of expert grasping poses of the target object, and then select a grasping pose closest to the robot from the set to build a heuristic target for supervised learning, so that the robot always moves towards this target during movement. We found through experiments that because the initial position of the robot is generally higher than the target object, the grasping pose closest to the robot is often the upper half of the object, which may cause the trained model to choose the upper half for grasping when grasping the object, which may cause the best object grasping pose to be missed, and the influence of the object's center of mass in the real environment is ignored, resulting in a significant decrease in the success rate of grasping in the real environment compared to the simulation environment. Therefore, we propose an improvement: obtain the point cloud of the target object from the state space, perform k-means clustering on the feature point cloud of the object, and use principal component analysis to obtain the principal direction of each class of point cloud. We construct a cuboid that can well surround the target object according to the principal direction of the point cloud, and filter the cuboids generated at different times to obtain the cuboid that can best surround the clustered point cloud. Then, we construct a simulated grasping pose that meets certain feature conditions with respect to the middle axis of the cuboid, i.e., the axis that passes through the center of the cuboid and is parallel to the long edge of the cuboid. We use point matching loss to find an expert grasping pose near the simulated grasping pose, and construct a new supervised learning target based on the expert grasping pose. That is, we optimize the original heuristic supervised learning target by filtering the simulated grasping point, so as to increase the possibility of exploration, until the optimal grasping pose is learned. SUMMARY

[0004] The present application proposes an improved method for optimizing the 6D grasping pose of a robot, which can autonomously plan a grasping path and adjust the grasping pose based on environmental feedback, thereby achieving precise grasping of any target object.

[0005] The present application proposes an improved method for optimizing the 6D grasping pose of a robot, which can autonomously plan a grasping path and adjust the grasping pose based on environmental feedback, thereby achieving precise grasping of any target object.

[0006] Step S1: Build an experimental platform for 6D pose grasping of a robot in a simulation environment, and build a 6D pose grasping network in the simulation environment;

[0007] Step S2: sample the point cloud data observed by the depth camera to obtain the target object point cloud P, and then output the point cloud set S with the target object features through the point cloud feature network, set a series of initial targets, generate expert grasping samples for these targets using the expert planner OMG, store the grasping samples in the experience replay pool, and when the number of samples reaches a certain threshold, start training the network;

[0008] Step S3: divide the target object point cloud P into P j (j = 1, 2) two categories through clustering, and construct a cuboid surrounding the two categories of point clouds along the main direction of the point cloud so that the long axis (i.e. the central axis line passing through the center of the cuboid and parallel to the longest side of the cuboid) in the cuboid is consistent with the main direction of the point cloud, and the corresponding simulated grasping pose located in the middle of the cuboid and perpendicular to the long axis of the cuboid is constructed according to the cuboid

[0009] Step S4: during the movement of the mechanical arm to the target object, new surrounding cuboids are continuously generated select a pair of cuboid groups from the accumulated surrounding cuboids that have good segmentation and surrounding point cloud effects while retaining the corresponding simulated grasping poses of the cuboid groups select the simulated grasping pose closest to the mechanical arm gripper then according to the simulated grasping pose find the expert grasping pose used to construct the supervised learning target from the object grasping dataset G

[0010] Step S5: according to the expert grasping pose set the heuristic target t t combine it with the predicted target output by the policy network and the value network for supervised learning, use supervised learning to reduce the difference between the network predicted target and the heuristic target, supervise the gripper to approach the target, and combine imitation learning and deep reinforcement learning loss to finally optimize the grasping pose;

[0011] Further, the construction of the simulation platform and the network in step S1 specifically includes the following steps:

[0012] Step S1-1: add a mechanical arm model and a two-finger gripper model in the Pybullet simulation environment, load an RGB-D camera on the two-finger gripper model, and import a table model for placing the object to be grasped;

[0013] Step S1-2: construct a PointNet++ point cloud network for extracting feature information of the target object;

[0014] ​Step S1-3: Constructing a policy network π using two fully convolutional neural networks θ and a value network Q φ The policy network is responsible for decision-making of the 6D pose grasping action of the robot arm, and the value network is responsible for evaluating the pros and cons of the action decided by the policy network in training.

[0015] Further, as described in step S2, the following steps are included:

[0016] Step S2-1: Randomly place a target object on the table, and obtain the point cloud data of the target object through the RGB-D depth camera, uniformly sample 1024 points from the point cloud data to form a target object point cloud set P.

[0017] Step S2-2: Use the PointNet point cloud feature network to extract features from the point cloud set P, and use the extracted feature point cloud as the input state S of the deep reinforcement learning network.

[0018] Step S2-3: Set a series of target objects, and obtain the state S of the target object according to step S2-2, and use the state as the input of the expert planner, and the expert planner outputs the expert action of the robot arm gripper in the current state, and records the state and the corresponding action of the robot arm gripper from the initial time to the successful grasping of the target object, and forms a grasping trajectory, and when the number of trajectories reaches a certain threshold, proceed to the next step.

[0019] Further, as described in step S3, the following steps are included for constructing the point cloud bounding box and the corresponding simulated grasping pose according to the point cloud subset:

[0020] Step S3-1: Use the K-means clustering algorithm to divide the target object point cloud set P obtained in step S2-1 into two different point cloud subsets P1 and P2.

[0021] Step S3-2: Obtain the point cloud center and the main direction of the point cloud data P j , project the main direction of the point cloud to the horizontal direction (i.e. x and y directions), and calculate the rotation angle μ j of the point cloud in the horizontal direction.

[0022] Step S3-3: Center the point cloud data so that the point cloud is centered at the origin, and then construct a rotation matrix according to μ j to rotate the point cloud to be parallel to the x-axis, and construct a bounding box around the rotated point cloud and a corresponding simulated grasping pose g j located in the middle of the bounding box and perpendicular to the long axis of the bounding box.

[0023] ​​Step S3-4: constructing the bounding cuboid and the simulated grasping pose g j by rotating and shifting coordinates and its corresponding simulated grasping pose ;

[0024] Further, the step S3-2 calculates the rotation angle μ j of the point cloud in the horizontal direction, specifically including the following steps:

[0025] Step S3-2-1: the point cloud data P j is composed of N j points p ji = [x ji , y ji , z ji ] T (i = 1, 2, …, N j ), and the center of the point cloud is calculated Then each point is subtracted by the center coordinates to obtain the centralized data The point cloud data processed in this way is centered on the origin O, which is convenient for subsequent covariance matrix calculation;

[0026] Step S3-2-2: constructing the covariance matrix to represent the distribution of point cloud data in different coordinate axes;

[0027] Step S3-2-3: constructing the characteristic equation C j v j = λ j v j , where v j is the eigenvector, representing a principal direction of the point cloud P j (j = 1, 2), and λ j is the eigenvalue, corresponding to the variance of the point cloud principal direction corresponding to the eigenvector;

[0028] Step S3-2-4: solving the eigenvalue λ j by solving the characteristic equation det(C j - λ j，1 I) = 0, and sorting the eigenvalues λ j,2 , λ j，3 according to λ j，1 < λ j，2 < λ j，3 , thereby obtaining the corresponding eigenvectors λ j，1 , λ j，2 , λ j，3 , and selecting the eigenvector λ j，3 with the largest eigenvalue as the first principal direction of the point cloud;

[0029] Step S3-2-5: If λ j，3 The projection in the horizontal direction is divided into The angle relative to the x-axis is calculated by the arctangent function

[0030] Step S3-2-6: If λ j，3 Parallel to the z-axis, that is, the first principal direction is perpendicular to the horizontal plane, λ j，2 is taken as the second principal direction, and λ j，2 is taken as the third principal direction. The projection in the horizontal direction is divided into The angle relative to the x-axis is calculated by the arctangent function

[0031] Further, the construction of the horizontal surrounding cuboid and its corresponding simulation grasping pose g j , specifically includes the following steps:

[0032] Step S3-3-1: Convert the angle μ j to radians, take its negative value, and get the rotation angle α

[0033] Step S3-3-2: According to the rotation angle α j , construct the rotation matrix R j :

[0034]

[0035] Step S3-3-3: Apply the rotation matrix R to the centralized point cloud q j，i obtained in step S3-2-1 to make the point cloud principal direction parallel to the x-axis, where R j · q jl is the matrix multiplication operation.

[0036] Step S3-3-4: Given the point cloud , construct a surrounding cuboid with the origin O as the center and the coordinate origin O as the center. In the base coordinate system, the three edges of the cuboid have coordinate axes parallel to them, and the three edges of the cuboid are respectively;

[0037] a j = x j，max - x j，min

[0038] b j = y j，max - y j，main

[0039] c j =z j，max -z j，min

[0040] Where (x) j，max ,x j，min ,y j，max y j，min ,z max , z j，min ) are vectors Extreme values ​​of each component;

[0041] Step S3-3-5: Along the horizontal cuboid Constructing the medium-length axis l j , l j Let the cuboid be a central axis passing through the origin and parallel to the longest side of the cuboid. The three faces facing the robotic arm gripper are A j B j C j medium-long axis l j Perpendicular to A j face and A j Faces intersect at the center B j The area of ​​the surface is greater than C j noodle;

[0042] Step S3-3-6: Set the gripping pose structure according to the robotic arm gripper model, using six symmetrical points to represent the gripper, corresponding to the six key parts of the gripper from top to bottom: the palm root, finger root, fingertip, etc. The two points at the palm root coincide spatially, and all six points are on the same plane. Construct the spatial coordinates of the six gripper reference points as (x... n y n , z n ) n＝1，2,...,6 ;

[0043] Step S3-3-7: Select C j center of the surface As the center point of the gripper, set the vector Set a vector for the z-axis (or orientation) of the gripper. The x-axis (perpendicular to the gripper plane) of the gripper, i.e., the direction relative to the mid-major axis l j Parallelism, achieved through the cross product of vectors Obtain the y-axis direction of the gripper, and then construct a horizontal cuboid. Simulated grasping pose g j ;

[0044] Furthermore, the enclosure of the cuboid described in steps S3-4 and its corresponding simulated grasping pose The construction of the system includes the following steps:

[0045] Step S3-4-1: Construct the rotation angle μ j The corresponding rotation matrix R′ j ,in

[0046]

[0047] According to the rotation matrix R′ j cuboid Rotate into a cuboid

[0048] Step S3-4-2: Translate the rotated cuboid back to its original position in the point cloud space to obtain the final cuboid enclosing the point cloud. in For point cloud data P j The center coordinates;

[0049] Step S3-4-3: Simulate grasping pose g j In the base system, it can be represented as g j =[x j y j , z j α j ,β j γ j ] T ,in Indicates the center point of the gripper Position, [α j ,β j γ j ] T The Euler angles representing the grippers can be converted into the form of a rotation matrix:

[0050]

[0051] Therefore, a simulated grasping pose g can be constructed. j homogeneous transformation matrix

[0052]

[0053] Step S3-4-4: Based on the rotation matrix R′ j For rotation matrix and the center position of the gripper Rotate to form the gripper pose after rotation. The corresponding homogeneous transformation matrix;

[0054]

[0055] Step S3-4-5: Perform a similar operation to step S3-4-2, translating the rotated gripper's pose to the final enclosure of the cuboid. The corresponding position is used to obtain the simulated grasping pose. homogeneous transformation matrix

[0056]

[0057] Step S3-4-6: Based on step S3-4-3, the gripper pose can be set. With homogeneous transformation matrix The conversion relationship between them is as follows, where h g (·) and h T (·) are all transformation operators, i.e., pose. The corresponding rotation matrix is Rotation matrix The corresponding gripper pose is

[0058]

[0059]

[0060] Therefore, the final enclosing cuboid can be obtained. Corresponding simulated grasping pose

[0061] Furthermore, step S4 involves simulating the grasping pose. Filtering expert grasping poses used to construct supervised learning objectives Specifically, the steps include the following:

[0062] Step S4-1: As the gripper moves closer to the target, the point cloud information of the object gradually becomes richer, and the cuboid constructed in step S3... The effect of enclosing point clouds gradually improves, the volume of the enclosing cuboid gradually decreases, and a threshold is set. Select the smaller cuboid from among many surrounding cuboids;

[0063] Step S4-2: Again, based on the center position and volume of the filtered cuboids, select a pair of cuboid groups that have a better effect on segmenting and enclosing the point cloud. Simultaneously retain the simulated grasping pose of the corresponding surrounding cuboids in the cuboid group.

[0064] Step S4-3: Based on the simulated grasping pose Select the simulated grasping pose that is closest to the current robotic arm gripper. Then, based on the simulated capture pose... Find expert grasping poses from the object grasping dataset G to construct the supervised learning objective.

[0065] Further, the setting of the volume threshold described in step S4-1 Specifically, the steps include the following:

[0066] Step S4-1-1: Repeat steps S3-2, S3-3, and S3-4 to construct the bounding cuboid of the point cloud of the entire target object P. The volume of the cuboid is calculated using step S3-3-4.

[0067] Step S4-1-2: Set the maximum volume threshold If the current time is the initial time, i.e., t = 1, then otherwise constant;

[0068] Step S4-1-3: Calculate the cuboid at time t volume if but And preserve the cuboid The center position and the corresponding simulated grab point And record the number at this time as T = t; if but No change, no preservation of the cuboid shape To avoid retaining excessively small cuboid volumes due to errors, a minimum volume threshold is set. Ensure that the volume of all cuboids is greater than 1.

[0069] Step S4-1-4: When the point cloud of the target object is updated, Update to a new target object point cloud surrounding cuboid. volume

[0070] Furthermore, step S4-2 involves screening the final cuboid group. And retain the simulated grasping pose of the corresponding cuboids surrounding the cuboid group. Specifically, the steps include the following:

[0071] Step S4-2-1: Based on the cuboids recorded in step S4-1-3, pair them up to construct m pairs of cuboids, and calculate the Euclidean distance L between the center coordinates of each pair of cuboids. m And calculate the average of all Euclidean distances.

[0072] Step S4-2-2: Analyze the cuboid numbers saved in step S4-1-3, the smaller the cuboid volume, the larger the corresponding number T, filter out two cuboid center point distances of cuboid pairs, then select the cuboid pair with the largest number and (T1+T2) from the cuboid pairs, if there are cuboid pairs with the same number and, then select the cuboid pair with the largest center distance L m as the two final cuboid groups filtered out at the current moment

[0073] Step S4-2-3: In step S3-4, the cuboid enclosing each point cloud is constructed Therefore, the final cuboid group corresponding to the two simulation grasping poses

[0074] Further, the expert grasping pose used to construct the supervised learning target in step S4-3 Specifically includes the following steps:

[0075] Step S4-3-1: The pose of the gripper in the base coordinate system at time t is denoted as g t =[x t , y t , z t , α t , β t , γ t ] T , where [x t , y t , z t ] T represents the center point position of the gripper, and [α t , β t , γ t ] T represents the Euler angle of the gripper, and f(·) is used to transform the mapping of the gripper pose g t at time t to the 6 gripper reference points (x n, y n , z n )(n=1,2,...,6), and the specific representation method is as follows:

[0076] f(g t )=[x1(t), γ1(t), z1(t),..., x6(t), y6(t), z6(t)] T

[0077] Step S4-3-2: Quantify the gripper pose g using the point matching loss formula t+1 The distance between g t :

[0078]

[0079] In the formula, ||f(g t+1 )-f(g t )||1 represents the 1-norm, and then the gripper pose g closest to the target object at time t is selected from t

[0080]

[0081] In the formula, arg min represents the operation of solving the variable value when the function value is minimum;

[0082] Step S4-3-3: There are 100 pre-prepared grasping poses on each object surface, which form a grasping set G of the object to be grasped. The distance between the simulated grasping pose g and the expert grasping pose g

[0083]

[0084] Further, the step S5 of constructing the heuristic target auxiliary network for training specifically includes the following steps:

[0085] Step S5-1: According to the improved target grasping pose g and the current pose g t of the gripper at time t obtained in step S4-3, a heuristic target t t and a corresponding loss function are constructed;

[0086] Step S5-2: The loss function calculated in step S5-1 is applied to the training process of the policy network π θ and the value network Q φ , which can promote the robot to better approach the target object;

[0087] Further, the step S5-1 of constructing the heuristic target specifically includes the following steps:

[0088] Step S5-1-1: In combination with step S3-4-6, define the homogeneous transformation matrix h g (t t ) as the transfer matrix from the current gripper pose g t to the target gripper pose g , and its calculation relationship is as follows:​

[0089]

[0090] Step S5-1-2: The improved heuristic target can be obtained according to the calculation relationship of step S5-1-1 and the conversion operator set in step S3-4-6, wherein (h g {g t )) -1 is the pose of the gripper g t The inverse transformation of the corresponding homogeneous transformation matrix is:

[0091]

[0092] Step S5-1-3: The strategy network π θ and the value network Q φ are generated by using the strategy network π and the value network Q The loss function of the network prediction target and the heuristic target is calculated according to the formula proposed in step S4-3-2, and the calculation formula is as follows:

[0093]

[0094]

[0095] In the formula, θ is the parameter of the strategy network, and φ' is the parameter of the target value network in the DDPG network, which is used to update the parameter of the main value network;

[0096] Further, the network training process of step S5-2 specifically includes the following steps:

[0097] Step S5-2-1: Assuming that the environment of the robot arm is completely observable, at each time step t, the agent reaches the state s t from the current state s t by taking the action a t+1 , wherein the state s t represents the point cloud features obtained from step S2-2, and the reward r t of the current action is obtained, and the robot arm generates a large number of trajectory samples (s t , a t , r t , s t+1 ) by interacting with the environment, and stores these samples in an experience replay pool for training the reinforcement learning network;

[0098] Step S5-2-2: The action output by the strategy network is denoted as a t , that is, the pose offset between the gripper pose g t+1 at the t+1 time step and the gripper pose gtat the t time step, and the calculation formula is as follows:

[0099] h g (a t )=(h g (g t )) -1 h g (g t+1 )

[0100] a t =h T ((h g (g t )) -1 h g (g t+1 ))=[Δx,Δy,Δz,Δα,Δβ,Δγ]

[0101] where Δx, Δy, Δz are position offsets, and Δα, Δβ, Δγ are angle offsets. In the autonomous decision-making process of the mechanical arm, the trajectory of the gripper from the 0th time step to the Tth time step is T r ={g0,g0,...,g T}, where g T represents the gripper pose when the maximum number of grasps is reached;

[0102] Step S5-2-3: generate expert actions a t under state s Combine the strategy network output action σ t in step S5-2-2 and the point matching loss formula proposed in step S4-3-2 to construct the loss function of behavior cloning:

[0103]

[0104] Step S5-2-4: The overall loss function expression of the value network Q φ is as follows:

[0105]

[0106] y represents the target Q value to be approximated, and its expression is as follows:

[0107] y=r t +γQ φ′ (s t+1 ,π θ ′(s t+1 )+ε)

[0108] where Q φ′ and π θrespectively are target value network and target policy network, θ' is parameter of target policy network, γ is reward discount factor, set γ=0.95, ε is pre-set clipping noise, r t is reward value of mechanical arm grasping, when grasping succeeds and stably lifts object 20cm, positive reward is provided, otherwise negative reward is given, and grasping reward function is defined as follows:

[0109]

[0110] Step S5-2-5: policy network π θ Overall loss function expression of is as follows:

[0111]

[0112] BC loss helps to prevent learning policy from deviating too much from expert policy, and λ is a weighted hyperparameter, set λ=0.2, mainly used to coordinate balance between expert loss and learning value function loss, L DDPG (s t , a t ) is deterministic policy loss aspect, which aims to maximize Q value, and can be expressed as follows:

[0113] L DDPG (s t , a t ) = -Q φ (s t , a t )

[0114] Step S5-2-6: soft update target value network Q φ and target policy network π θ ',

[0115] φ' <- τφ + (1-τ)φ'

[0116] θ' <- -τθ + (1-τ)θ'

[0117] In the formula, φ' is parameter of target value network Q φ′ , φ is parameter of value network Q φ , θ' is parameter of target policy network π θ′ , θ is parameter of policy network π θ , τ is soft update coefficient, set τ=0.999.

[0118] Step S5-2-7: repeat steps S3 to S5 above multiple times to realize grasping and grasping optimization of target object by mechanical arm.

[0119] Beneficial effects of the present application are as follows:

[0120] The method provides an optimized 6D grasping pose method of a mechanical arm, provides a systematic framework, combines the error correction performance of supervised learning and the perception ability and decision-making ability of deep reinforcement learning, autonomously selects a grasping pose through a network, and constantly interacts with the environment through the mechanical arm, so that the mechanical arm learns how to grasp in a manner similar to human grasping, improves the grasping pose of the mechanical arm, and thus improves the efficiency and robustness of the mechanical arm in grasping objects of any shape in an unstructured environment. BRIEF DESCRIPTION OF DRAWINGS

[0121] Figure 1 The overall flowchart of the method is shown in the figure;

[0122] Figure 2 The simulation environment of the mechanical arm is shown in the figure;

[0123] Figure 3 The scene diagram of the ShapeNet part training is shown in the figure;

[0124] Figure 4 The schematic diagram of the three types of surrounding cuboids is shown in the figure;

[0125] Figure 5 The top view obtained after centering the cuboids obtained through clustering is shown in the figure;

[0126] Figure 6 The schematic diagram of the angle between the main direction of the point cloud and the x-axis is shown in the figure;

[0127] Figure 7 The schematic diagram of the cuboid obtained after rotating according to the main direction of the point cloud is shown in the figure;

[0128] Figure 8 The schematic diagram of the simulated gripper constructed according to the surrounding cuboid is shown in the figure;

[0129] Figure 9 The schematic diagram of the simulated gripper is shown in the figure;

[0130] Figure 10 The schematic diagram of the final surrounding cuboid and the corresponding simulated grasping pose obtained through screening is shown in the figure;

[0131] Figure 11 The overall algorithm structure block diagram is shown in the figure;

[0132] Figure 12 The grasping effect diagram of the object by using the trained model is shown in the figure. DETAILED DESCRIPTION

[0133] The application provides an improved 6D grasping pose method of a mechanical arm based on deep reinforcement learning, and the following will be described in detail and completely in combination with the drawings and the technical method in the embodiments of the application.

[0134] Figure 1is a flow chart of the method of the present application, which contains all the steps of the method, and the specific steps are as follows:

[0135] Step S1: build a simulation experiment platform for 6D pose grasping of a mechanical arm in an Ubuntu environment, and construct a 6D pose grasping network;

[0136] Step S1-1: the simulation experiment platform includes a Pybullet scene and an object to be grasped in the scene, an XARM mechanical arm configured with an xArm parallel mechanical gripper is built, an RGB-D depth camera is added to obtain visual information of the target object, a table model for placing the object to be grasped is imported, the position of the mechanical arm base is fixed, the pose of the target object is randomly generated by the system, and the position of the mechanical arm gripper is randomly initialized before each grasping experiment starts, so that it always faces the target object; the maximum range T of the agent decision is 20, and once the object is successfully grasped, a round will be terminated; the experimental platform environment is as shown in Figure 2 ;

[0137] Step S1-2: select 1500 objects from the ShapeNet dataset for training, which cover 169 different categories, and each object is equipped with 100 pre-set expert grasping poses, and 10 objects of different categories in the YCB (Yale-CMU-Berkeley) dataset are used for testing, and part of the training scene is as shown in Figure 3 ;

[0138] Step S1-3: construct a PointNet++ point cloud network for extracting feature information of the target object;

[0139] Step S1-4: two fully convolutional neural networks are used to construct a strategy network θ and a value network Q φ , the strategy network is responsible for decision-making of the 6D pose grasping action of the mechanical arm, and the value network is responsible for evaluating the pros and cons of the action decided by the strategy network in training;

[0140] Step S1-4-1: the value network and the strategy network each consist of a three-layer fully connected multilayer perceptron with a ReLU activation function as the core, in the training stage, the Adam optimizer is used and the learning rate I r is adjusted in sections to optimize the network performance;

[0141] Step S1-4-2: randomly initialize the parameters of the strategy network and the value network and the parameters of the target strategy network and the target value network;

[0142] Step S2: sample the point cloud data observed by the depth camera to obtain the target object point cloud P, and then output the point cloud set S with the target object features through the point cloud feature network, set a series of initial targets, use the expert planner OMG to generate expert grasping samples for these targets, store the grasping samples in the experience replay pool, and when the number of samples reaches a certain threshold, start training the network;

[0143] Step S2-1: randomly place a target object on the table, obtain the point cloud data of the target object through the RGB-D depth camera, uniformly sample 1024 points from the point cloud data to form a target object point cloud set P, and use the PointNet point cloud feature network to extract the features of the point cloud set P, and take the extracted 512-dimensional feature point cloud as the input state S of the deep reinforcement learning network;

[0144] Step S2-2: set an experience replay pool to store the experience data generated by the interaction between the robot arm and the environment, and collect a certain amount of successful grasping experience data through the expert planner before training, and the expert planner OMG will autonomously generate a continuous and successful grasping trajectory by giving the starting point and the target grasping point of the target object, the trajectory reflects the state reached by the robot arm in the exploration process and the action decided by the robot arm in the state, only when the successful grasping trajectory reaches a certain level, the following step will be entered;

[0145] Step S3: collect heuristic target grasping samples and construct corresponding loss functions;

[0146] Step S3-1: divide the target object point cloud P into two classes P1(j=1,2) through clustering, and construct a cuboid surrounding the two classes of point clouds along the main direction of the point cloud so that the long axis (i.e. the central axis parallel to the longest side of the cuboid passing through the center point of the cuboid) is consistent with the main direction of the point cloud, and the simulated grasping pose corresponding to the middle part of the cuboid perpendicular to the long axis of the cuboid is constructed

[0147] Step S3-1-1: use the K-means clustering algorithm to divide the target object point cloud set P obtained in step S2-1 into two different point cloud subsets P1 and P2;

[0148] Step S3-1-2: obtain the point cloud center and the main direction of the point cloud P j , project the main direction of the point cloud to the horizontal direction (i.e. x and y directions), calculate the rotation angle μ j of the point cloud in the horizontal direction, and the corresponding cuboid projection diagram is shown in Figure 4 and Figure 5 ;

[0149] Step S3-1-2-1: Point cloud data P j By N j Points Composition, computational point cloud center Then subtract the center coordinate from each point. Obtain centralized data The point cloud data processed in this way is centered at the origin O, which facilitates the subsequent calculation of the covariance matrix;

[0150] Step S3-1-2-2: Construct the covariance matrix To represent the distribution of point cloud data on different coordinate axes;

[0151] Step S3-1-2-3: Construct the characteristic equation C using the covariance matrix j v j =λ j v j , where v j It is an eigenvector, representing the point cloud P. j A principal direction of (j=1,2), λ j It is the eigenvalue, which corresponds to the variance of the point cloud along the principal direction of the eigenvector.

[0152] Step S3-1-2-4: Solve the characteristic equation det(C) j -λ j I) = 0 yields the eigenvalue λ j，1 , λ j,2 , λ j，3 Sort λ ​​according to eigenvalues j，1 <λ j，2 <λ j，3 Thus, the corresponding eigenvector λ is obtained. j，1 , λ j，2 , λ j，3 Select the eigenvector λ with the largest eigenvalue. j，3 As the primary direction of the point cloud;

[0153] Step S3-1-2-5: Place λ j，3 The projection in the horizontal direction is divided into Calculate its angle relative to the x-axis using the arctangent function. Angle calculation diagram as shown Figure 6 As shown;

[0154] Step S3-1-2-6: If λ j，3 If the first principal direction is parallel to the z-axis, i.e., perpendicular to the horizontal plane, then λ is used. j，2 As the second principal direction, λ j，2 The projection in the horizontal direction is divided into The angle relative to the x-axis is calculated by the inverse tangent function

[0155] Step S3-1-3: Center the point cloud data so that the point cloud is centered at the origin, and then construct a rotation matrix according to μ so that the point cloud is rotated to be parallel to the x-axis, and construct a surrounding cuboid around the rotated point cloud And construct the corresponding simulation grasping pose g located in the middle of the cuboid and perpendicular to the long axis of the cuboid j ;

[0156] Step S3-1-3-1: Convert the angle μ j to radians and take its negative value to obtain the rotation angle

[0157] Step S3-1-3-2: Construct a rotation matrix R j according to the rotation angle α j :

[0158]

[0159] Step S3-1-3-3: Apply the rotation matrix R ji to the centralized point cloud q obtained in step S3-1-2-1 to make the principal direction of the point cloud parallel to the x-axis, where R j · q ji is a matrix multiplication operation ;

[0160] Step S3-1-3-4: Given the point cloud centered at the origin O, construct a surrounding cuboid centered at the coordinate origin O so that in the base coordinate system, three edges of the cuboid have coordinate axes parallel to them, and the three edges of the cuboid are respectively:

[0161] a j = x j，max -x j，min

[0162] b j = y j，max -y j，min

[0163] c j = z j,max -z j，min

[0164] where (x j，max , x j，min , y j，max , y j，min , z max , z j，min) are vectors The extreme values ​​of each component enclose the cuboid. The schematic diagram is as follows Figure 7 As shown;

[0165] Step S3-1-3-5: Along the horizontal cuboid Constructing the medium-length axis l j , l j Let the cuboid be a central axis passing through the origin and parallel to the longest side of the cuboid. The three faces facing the robotic arm gripper are A j B j C j medium-long axis l j Perpendicular to A j face and A j Faces intersect at the center B j The area of ​​the surface is greater than C j noodle;

[0166] Step S3-1-3-6: Set the gripping pose structure according to the robotic arm gripper model, using six symmetrical points to represent the gripper, corresponding to the six key parts of the gripper from top to bottom: the palm root, finger root, fingertip, etc. The two points at the palm root coincide spatially, and all six points are on the same plane. Construct the spatial coordinates of the six gripper reference points as (x... n ,y n , z n (n = 1, 2, ..., 6);

[0167] Step S3-1-3-7: Select C j center of the surface As the center point of the gripper, set the vector Set a vector for the z-axis (or orientation) of the gripper. The x-axis (perpendicular to the gripper plane) of the gripper, i.e., the direction relative to the mid-major axis l j Parallelism, achieved through the cross product of vectors Obtain the y-axis direction of the gripper, and then construct a horizontal cuboid. Simulated grasping pose g j Simulated gripper g j The construction of such as Figure 8 As shown;

[0168] Step S3-1-4: Enclose the cuboid and simulated grasping pose g j Perform rotation and coordinate offset to complete the enclosure of the cuboid. and its corresponding simulated grasping pose The construction;

[0169] Step S3-1-4-1: Construct the rotation angle μ j The corresponding rotation matrix R′ j ,in

[0170]

[0171] According to the rotation matrix cuboid Rotate into a cuboid

[0172] Step S3-1-4-2: Translate the rotated cuboid back to its original position in the point cloud space to obtain the final cuboid enclosing the point cloud. in For point cloud data P j The center coordinates;

[0173] Step S3-1-4-3: Simulate grasping pose g j In the base system, it can be represented as g j =[x j y j , z j α j ,β j γ j ] T ,in Indicates the center point of the gripper Position, [α j ,β j γ j ] T The Euler angles representing the grippers can be converted into the form of a rotation matrix:

[0174]

[0175] Therefore, a simulated grasping pose g can be constructed. j homogeneous transformation matrix

[0176]

[0177] Step S3-1-4-4: Based on the rotation matrix R′ j For rotation matrix and the center position of the gripper Rotate to form the gripper pose after rotation. The corresponding homogeneous transformation matrix:

[0178]

[0179] Step S3-1-4-5: Similar to step S3-1-4-2, the rotated gripper pose is translated to the final enclosing cuboid corresponding position to obtain the simulated grasping pose of the homogeneous transformation matrix

[0180]

[0181] Step S3-1-4-6: According to step S3-1-4-3, the conversion relationship between the gripper pose and the homogeneous transformation matrix is as follows, where h g (·) and h T (·) are conversion operators, i.e., the pose corresponding rotation matrix is The rotation matrix corresponding gripper pose is

[0182]

[0183]

[0184] Therefore, the final enclosing cuboid corresponding simulated grasping pose of the gripper can be obtained as shown in Figure 9 ;

[0185] Step S3-2: During the movement of the robot arm towards the target object, new enclosing cuboids are continuously generated A group of cuboids with good segmentation and enclosing point cloud effects is selected from the accumulated enclosing cuboids Meanwhile, the corresponding simulated grasping pose of the cuboid group is retained The simulated grasping pose closest to the robot arm gripper is selected Then, according to the simulated grasping pose an expert grasping pose used to construct the supervised learning target is found from the object grasping dataset G

[0186] Step S3-2-1: As the gripper continuously approaches the target, the point cloud information of the object gradually becomes rich, and the cuboids constructed in step S3-1 enclose the point cloud with gradually improved effect, and the volume of the enclosing cuboid gradually decreases A threshold is set The cuboids with smaller volume are selected from the numerous enclosing cuboids

[0187] Step S3-2-1-1: Repeat steps S3-1-2, S3-1-3, and S3-1-4 to construct the bounding cuboid of the point cloud of the entire target object P. The volume of the cuboid is calculated using step S3-1-3-4.

[0188] Step S3-2-1-2: Set the maximum volume threshold If the current time is the initial time, i.e., t = 1, then otherwise constant;

[0189] Step S3-2-1-3: Calculate the cuboid at time t volume if but And preserve the cuboid The center position and the corresponding simulated grab point And record the number at this time T = t; if but No change, no preservation of the cuboid shape To avoid retaining excessively small cuboid volumes due to errors, a minimum volume threshold is set. Ensure that the volume of all cuboids is greater than 1.

[0190] Step S3-2-1-4: When the target object point cloud is updated, Update to a new target object point cloud surrounding cuboid. volume

[0191] Step S3-2-2: Again, based on the center position and volume of the filtered cuboids, select a pair of cuboid groups that have a better effect on segmenting and enclosing the point cloud. Simultaneously retain the simulated grasping pose of the corresponding surrounding cuboids in the cuboid group.

[0192] Step S3-2-2-1: Based on the cuboids recorded in step S3-2-1-3, pair them up to construct m pairs of cuboids, and calculate the Euclidean distance L between the center coordinates of each pair of cuboids. m And calculate the average of all Euclidean distances.

[0193] Step S3-2-2-2: Analyze the cuboid numbers saved in step S3-2-1-3. The smaller the volume of the cuboid, the larger its corresponding number T. Select the cuboids whose center points are far apart. rectangular cuboids, and then selecting a rectangular cuboid pair with the largest number and (T1+T2) from the rectangular cuboid pairs, and if there are rectangular cuboid pairs with the same number, selecting a rectangular cuboid pair with the largest center distance L m rectangular cuboid pair with the largest number and (T1+T2) from the rectangular cuboid pairs, and if there are rectangular cuboid pairs with the same number, selecting a rectangular cuboid pair with the largest center distance L

[0194] Step S3-2-2-3: In step S3-1-4, a rectangular cuboid is surrounded according to each point cloud Both the corresponding simulation grasping poses are constructed Therefore, the final rectangular cuboid group can be obtained The corresponding two simulation grasping poses The diagram of the final rectangular cuboid group obtained according to the above screening is as shown in Figure 10 ;

[0195] Step S3-2-3: According to the simulation grasping pose The simulation grasping pose closest to the current time mechanical arm gripper is selected from the simulation grasping pose Then, according to the simulation grasping pose The expert grasping pose used to construct the supervised learning target is found from the object grasping data set G

[0196] Step S3-2-3-1: The pose of the gripper in the base coordinate system at time t is denoted as g t =[x t， y t , z t , a t， b t , g t ] T , where [x t , y t , z t ] T represents the gripper center point position, and [a t , b t , g t ] T represents the Euler angle of the gripper, and f(·) is used to transform the mapping of the gripper pose g t at time t to 6 gripper reference points (x n , y n , z n )(n=1, 2,..., 6), and the specific representation method is as follows:

[0197] f(g t )=[x1(t), y1(t), z1(t),..., x6(t), y6(t), z6(t)] T

[0198] Step S3-2-3-2: Quantify the gripper pose g using the point matching loss formula. t+1 With g t Distance between:

[0199]

[0200] In the formula ||f(g t+1 )-f(g t )||1 indicates finding the 1-norm, then from Select the distance g from the robotic arm gripper at time t. t Recent grasping pose

[0201]

[0202] In the formula, arg min represents the operation of finding the variable value when the function value is minimized;

[0203] Step S3-2-3-3: Each object surface has 100 pre-prepared grasping poses. These grasping poses constitute the grasping set G of the object to be grasped. The distance is calculated using the point matching loss formula to simulate the grasping poses. Recent expert capture of pose

[0204]

[0205] Step S3-3: Based on the pose captured by the expert Set heuristic goals t t The predicted target is combined with the output of the policy network and the value network for supervised learning. Supervised learning is used to reduce the difference between the network's predicted target and the heuristic target, and the supervised gripper moves closer to the target.

[0206] Step S3-3-1: Combining steps S3-1-4-6, define the homogeneous transformation matrix h. g (t t ) represents the current gripper pose g. t To the target gripper position The transition matrix is ​​calculated as follows:

[0207]

[0208] Step S3-3-2: Based on the calculation relationship in step S3-3-1 and the transformation operator set in step S3-1-4-6, the improved heuristic objective can be obtained, where (h g (g t )) -1 For gripper position g t The inverse transformation of the corresponding homogeneous transformation matrix:

[0209]

[0210] Step S3-3-3: using the policy network π constructed in step S2 θ and value network Q φ Generate network prediction target and The loss function of the network prediction target and heuristic target is calculated according to the formula proposed in step S3-2-3-2, and the calculation formula is as follows:

[0211]

[0212]

[0213] In the formula, θ is the parameter of the policy network, and φ' is the parameter of the target value network in the DDPG network, which is used to update the parameter of the main value network;

[0214] Step S4: combining imitation learning and deep reinforcement learning loss to finally realize the optimization of the grasping pose;

[0215] Step S4-1: assuming that the environment where the robot arm is located is completely observable, at each time step t, the agent obtains the current state s t Take action a t and arrive at state s t+1 , where s t represents the components of the point cloud feature S obtained from step S2-1 at time step t, and the reward r t is obtained for the current action. A large number of trajectory samples (s t , a t , r t , s t+1 ) are generated by the robot arm through interaction with the environment, and these samples are stored in an experience replay pool for training the reinforcement learning network;

[0216] Step S4-2: the action output by the policy network is denoted as a t , i.e., the pose offset amount of the gripper pose gt+1 at time step t+1 and the gripper pose gt at time step t, and the calculation formula is as follows:

[0217] h g (a t )=(h g (g t )) -1 h g (g t+1 )

[0218] a t =h T ((h g (gt )) -1 h g (g t+1 ))=[Δx,Δy,Δz,Δα,Δβ,Δγ]

[0219] In the formula, Δx, Δy, and Δz are position offsets, and Δα, Δβ, and Δy are angular offsets. During the autonomous decision-making process of the robotic arm, the trajectory T of the gripper from time step 0 to time step T... r ={g0, g0, ..., g T}, where g T This indicates the gripper pose when the maximum number of grips is reached;

[0220] Step S4-3: Generate state s using the expert planner OMG t Expert Actions Combined with the policy network output action a from step S4-2 t Based on the point matching loss formula proposed in step S3-2-3-2, construct the loss function for behavior cloning:

[0221]

[0222] This loss helps prevent the policy learned by the policy network from deviating too much from the expert policy, while ensuring that the robotic arm can approach the target object in the early stages of training, reducing the time spent by the robotic arm exploring the space.

[0223] Step S4-4: Value Network Q φ The overall loss function expression is as follows:

[0224]

[0225] y represents the target Q value to be approximated, and its expression is as follows;

[0226] y = r t +γQ φ ′(s t+1 , π θ ′(s t+1 )+ε)

[0227] In the formula Q φ′ and π θ θ′ represents the target value network and the target policy network, respectively. θ′ represents the parameters of the target policy network, γ is the reward discount factor (set to 0.95), ε is the pre-set shearing noise, and r t This is the reward value for the robotic arm's grasping action. A positive reward is given when the object is successfully grasped and stably lifted 20 centimeters; otherwise, a negative reward is given. The grasping reward function is defined as follows:

[0228]

[0229] Step S4-5: policy network π θ The overall loss function expression is as follows:

[0230]

[0231] λ is a weighted hyperparameter mainly used to coordinate the balance between the expert loss and the learning value function loss, λ is set to 0.2, L DDPG (s t , a t ) is a deterministic policy loss, which aims to maximize the Q value, and can be expressed as follows:

[0232] L DDpG (s t , a t ) = -Q φ (s t , a t )

[0233] Step S4-6: soft update target value network Q φ′ and target policy network πτ θ Neck:

[0234] φ' <- τφ + (1-τ)φ'

[0235] θ' <- τθ + (1-τ)θ'

[0236] In the formula, φ' is the parameter of the target value network Q φ′ , φ is the parameter of the value network Q φ , θ' is the parameter of the target policy network π θ′ , θ is the parameter of the policy network π θ , τ is a soft update coefficient, τ is set to 0.999, and the network training structure is as shown in Figure 11

[0237] Step S4-7: repeat the above steps S3 and S4 multiple times to realize the grasping and grasping optimization of the target object by the robot arm.

[0238] After experimental training, the grasping effect of the robot arm model on the object is improved to a certain extent, and the specific results are as shown in Figure 12 (left to right, from left to right).

[0239] ​In summary, the application can divide target objects into two categories according to physical properties of the objects, construct a corresponding surrounding cuboid according to the point cloud distribution of the objects, and generate stable simulation grasping points, so as to autonomously and efficiently select grasping data from the grasping data set by means of the screening performance of the simulation grasping points, thereby performing supervised learning, and dynamically improve the grasping posture of the robot arm to the objects by combining the methods of imitation learning and reinforcement learning, thereby improving the grasping performance of the robot arm.

[0240] The application can also divide target objects into multiple categories (including but not limited to two categories) according to the classification method of the object point cloud, and can optimize the simulation grasping points as the classification effect improves, thereby better supervising the 6D grasping performance of the robot arm, and having strong applicability and generality.

[0241] The above is a detailed description of the implementation method of the application, and the example process of the application cannot be limited to the above. Changes made by relevant technical personnel within the original scope of the application, such as replacement, deletion, etc., also belong to the protection scope of the application.

Claims

1. A method for optimizing the 6D grasping pose of a robotic arm, characterized in that, Includes the following steps: Step S1: Build an experimental platform for 6D pose grasping of a robotic arm in a simulation environment, and construct the policy network π. θ and Value Network Q φ The policy network is responsible for deciding the 6D pose grasping action of the robotic arm, while the value network is responsible for evaluating the quality of the actions decided by the policy network during training. Step S2: Sample the point cloud data observed by the depth camera to obtain the point cloud of the target object, and then output a set of point clouds with the features of the target object through the point cloud feature network. Step S3: Divide the point cloud of the target object into two categories, and construct a bounding cuboid around the two categories of point clouds along the main direction of the point cloud. Make sure that the major axis of the cuboid, that is, the central axis that passes through the center point of the cuboid and is parallel to the longest side of the cuboid, is consistent with the main direction of the point cloud. Then, construct a simulated grasping pose for each cuboid. Step S4: As the gripper gets closer to the target, the point cloud information of the object gradually becomes richer, the effect of the cuboid constructed in step S3 surrounding the point cloud gradually improves, and the volume of the cuboid gradually decreases. Step S5: Select the two simulated grasping poses corresponding to the two final cuboids with better bounding and segmentation point cloud effects. Step S6: Select the simulated grasping pose closest to the robotic arm gripper from the simulated grasping poses obtained in Step S5, and find the expert grasping pose used to construct supervised learning from the object grasping dataset based on this simulated grasping pose. Step S7: Based on the pose captured by the expert Set the heuristic crawling target t t It combines the network's predicted target with supervised learning, using supervised learning to reduce the discrepancy between the network's predicted target and the heuristic target, guiding the gripper to approach the object to be grasped, thereby optimizing the gripper's grasping pose; Step S8: Combine imitation learning and reinforcement learning to optimize the grasping pose.

2. The method for optimizing the 6D grasping pose of a robotic arm according to claim 1, characterized in that, Step S2 specifically includes the following steps: Step S2-1: Randomly place an object to be grabbed in the scene, acquire the point cloud data of the target object through an RGB-D depth camera, and uniformly sample 1024 points from it to form the point cloud set P of the target object; Step S2-2: Use the PointNet point cloud feature network to extract features from the point cloud set P, and use the extracted feature point cloud as the input state S of the deep reinforcement learning network.

3. The method for optimizing the 6D grasping pose of a robotic arm according to claim 2, characterized in that, Step S3 describes constructing a bounding cuboid that encloses the two types of point clouds. Specifically, the steps include the following: Step S3-1: Use the K-means clustering algorithm to divide the target object point cloud set P obtained in step S2-1 into two different point cloud subsets, P1 and P2; Step S3-2: Acquire point cloud data P j Given the point cloud center and principal direction, project the principal direction onto the horizontal direction (x and y directions) and calculate the rotation angle μ of the point cloud in the horizontal direction. j ; Step S3-3: Dataficate the point cloud data so that the point cloud is centered on the origin, and then according to μ j Construct a rotation matrix to rotate the point cloud until it is parallel to the x-axis, and then construct a horizontal cuboid around the rotated point cloud. And construct the corresponding simulated grasping pose g located in the middle of the cuboid and perpendicular to the major axis of the cuboid. j ; Step S3-4: Place the horizontal cuboid and simulated grasping pose g j Perform rotation and coordinate offset to complete the enclosure of the cuboid. and its corresponding simulated grasping pose The construction of.

4. The method for optimizing the 6D grasping pose of a robotic arm according to claim 3, characterized in that, Step S3-2 involves calculating the horizontal rotation angle μ of the point cloud. j Specifically, it includes the following steps: Step S3-2-1: Point cloud data P j (j=1,2) by N j Point p ji =[x ji y ji , z ji ] T (i = 1, 2, ..., N) j Composed of, calculating the center of the point cloud. Then subtract the center coordinate from each point. Obtain centralized data The point cloud data processed in this way is centered at the origin O, which facilitates the subsequent calculation of the covariance matrix; Step S3-2-2: Construct the covariance matrix To represent the distribution of point cloud data on different coordinate axes Step S3-2-2: Construct the covariance matrix To represent the distribution of point cloud data on different coordinate axes; Step S3-2-3: Construct the characteristic equation C using the covariance matrix j v j =λ j v j , where v j It is an eigenvector, representing the point cloud P. j A principal direction of (j=1,2), λ j It is the eigenvalue, which corresponds to the variance of the point cloud along the principal direction of the eigenvector. Step S3-2-4: Solve the characteristic equation det(C) j -λ j I) = 0 yields the eigenvalue λ j，1 ,λ j，2 , λ j，3 Sort λ ​​according to eigenvalues j，1 <λ j，2 <λ j，3 Thus, the corresponding feature vector v is obtained. j,1 ,v j，2 v j，3 Select the eigenvector v with the largest eigenvalue. j，3 As the primary direction of the point cloud; Step S3-2-5: v j，3 The projection in the horizontal direction is divided into Calculate its angle relative to the x-axis using the arctangent function. Step S3-2-6: If v j，3 If the direction is parallel to the z-axis, meaning the first principal direction is perpendicular to the horizontal plane, then v is used. j，2 As the second primary direction, v j，2 The projection in the horizontal direction is divided into Calculate its angle relative to the x-axis using the arctangent function.

5. The method for optimizing the 6D grasping pose of a robotic arm according to claim 3, characterized in that, The horizontal cuboid described in step S3-3 and its corresponding simulated grasping pose g j The construction of the system includes the following steps: Step S3-3-1: Adjust the angle μ j Convert to radians, take the negative value, and you get the rotation angle. Step S3-3-2: Based on the rotation angle α j Construct the rotation matrix R j : Step S3-3-3: The centered point cloud q obtained in step S3-2-1... ji Apply rotation matrix Make the principal direction of the point cloud parallel to the x-axis, where R j ·q ji This refers to matrix multiplication. Step S3-3-4: Given point cloud Construct a bounding cuboid centered at the origin O. In the base coordinate system, the cuboid has coordinate axes parallel to its three sides, and the cuboid... The three sides are as follows: a j =x j，max -x j，min b j / and j，max -and j，min c j =z j,max -With j，min Where (x) j，max x j，min y j，max ,y j，min , z max , z j,min ) are vectors Extreme values ​​of each component; Step S3-3-5: Along the horizontal cuboid Constructing the medium-length axis l j , l j Let the cuboid be a central axis passing through the origin and parallel to the longest side of the cuboid. The three faces facing the robotic arm gripper are A j B j C j medium-long axis l j Perpendicular to A j face and A j Faces intersect at the center B j The area of ​​the surface is greater than C j noodle; Step S3-3-6: Set the gripping pose structure according to the robotic arm gripper model, using six symmetrical points to represent the gripper, corresponding to the six key parts of the gripper from top to bottom: the palm root, finger root, fingertip, etc. The two points at the palm root coincide spatially, and all six points are on the same plane. Construct the spatial coordinates of the six gripper reference points as (x... n y n , z n (n = 1, 2, ..., 6); Step S3-3-7: Select C j center of the surface As the center point of the gripper, set the vector Set a vector for the z-axis direction of the gripper. The x-axis direction of the gripper, i.e., its direction relative to the mid-major axis l j Parallelism, achieved through the cross product of vectors Obtain the y-axis direction of the gripper, and then construct a horizontal cuboid. Simulated grasping pose g j .

6. A method for optimizing the 6D grasping pose of a robotic arm according to claim 3, characterized in that, The enclosure cuboid described in steps S3-4 and its corresponding simulated grasping pose The construction of the system includes the following steps: Step S3-4-1: Construct the rotation angle μ j The corresponding rotation matrix R′ j ,in According to the rotation matrix R′ j cuboid Rotate into a cuboid Step S3-4-2: Translate the rotated cuboid back to its original position in the point cloud space to obtain the final cuboid enclosing the point cloud. in For point cloud data P j The center coordinates; Step S3-4-3: Simulate grasping pose g j In the base system, it can be represented as g j =[x j y j , z j α j ,β j γ j ] T ,in Indicates the center point of the gripper Position, [α j ,β j γ j ] T The Euler angles representing the grippers can be converted into the form of a rotation matrix: Therefore, a simulated grasping pose g can be constructed. j homogeneous transformation matrix Step S3-4-4: Based on the rotation matrix R′ j For rotation matrix and the center position of the gripper Rotate to form the gripper pose after rotation. The corresponding homogeneous transformation matrix: Step S3-4-5: Perform a similar operation to step S3-4-2, translating the rotated gripper's pose to the final enclosure of the cuboid. The corresponding position is used to obtain the simulated grasping pose. homogeneous transformation matrix Step S3-4-6: Based on step S3-4-3, the gripper pose can be set. With homogeneous transformation matrix The conversion relationship between them is as follows, where h g (·) and h T (·) are all transformation operators, i.e., pose. The corresponding rotation matrix is Rotation matrix The corresponding gripper pose is Therefore, the final enclosing cuboid can be obtained. Corresponding simulated grasping pose 7. The method for optimizing the 6D grasping pose of a robotic arm according to claim 3, characterized in that, Step S5 specifically includes the following steps: Step S5-1: Repeat steps S3-2, S3-3, and S3-4 to construct the bounding cuboid of the point cloud of the entire target object P. The volume of the cuboid is calculated using step S3-3-4. Step S5-2: Set the maximum volume threshold If the current time is the initial time, i.e., t = 1, then otherwise constant; Step S5-3: Calculate the cuboid at time t volume if but And preserve the cuboid The center position and the corresponding simulated grab point And record the number at this time as T = t; if but No change, no preservation of the cuboid shape To avoid retaining excessively small cuboid volumes due to errors, a minimum volume threshold is set. Ensure that the volume of all cuboids is greater than 1. Step S5-4: When the point cloud of the target object is updated, Update to a new target object point cloud surrounding cuboid. volume Step S5-5: Based on the cuboids recorded in Step S5-3, pair them up to construct m pairs of cuboids. Calculate the Euclidean distance L between the center coordinates of each pair of cuboids, and calculate the average of all Euclidean distances. Step S5-6: Analyze the cuboid numbers saved in step S5-3. The smaller the volume of the cuboid, the larger its corresponding number T. Select the cuboids whose center points are far apart. For each pair of cuboids, select the pair with the largest sum of their numbers (T1+T2). If a pair of cuboids with the same sum of numbers exists, then select the pair with the largest center distance L. m The largest pair of cuboids will be the two final cuboids selected at the current moment. Step S5-7: In step S3-4, the cuboid is enclosed according to each point cloud. Corresponding simulated grasping poses were constructed for each. Therefore, we can obtain the two simulated grasping poses corresponding to the two final cuboids.

8. A method for optimizing the 6D grasping pose of a robotic arm according to claim 6, characterized in that, Step S6 is based on simulated grasping pose. Constructing expert-grabbed poses for supervised learning Specifically, the steps include the following: Step S6-1: The pose of the gripper in the base coordinate system at time t is denoted as g. t =[x t y t , z t α t ,β t γ t ] T , where [x t y t , z t ] T Indicates the position of the center point of the gripper, [α] t ,β t γ t ] T Let the Euler angles of the gripper be represented, and let f(·) transform be used to represent the gripper pose g at time t. t Up to 6 gripper reference points (x n y n , z n The mapping (n = 1, 2, ..., 6) is specifically represented as follows: f(g t )=[x1(t),y1(t),z1(t),...,x6(t),y6(t),z6(t)] T Step S6-2: Quantify the gripper pose g using the point matching loss formula. t+1 With g t Distance between: In the formula ||f(g t+1 )-f(g t )||1 indicates finding the 1-norm, then from Select the distance g from the robotic arm gripper at time t. t Recent grasping pose In the formula, arg min represents the operation of finding the variable value when the function value is minimized; Step S6-3: Each object surface has 100 pre-prepared grasping poses. These grasping poses constitute the grasping set G of the object to be grasped. The distance is used to calculate the simulated grasping poses using the point matching loss formula. Recent expert capture of pose 9. The method for optimizing the 6D grasping pose of a robotic arm according to claim 8, characterized in that, Step S7 describes capturing the pose based on expert information. Set the heuristic crawling target t t Specifically, it includes the following steps: Step S7-1: Combining steps S3-4-6, define the homogeneous transformation matrix h. g (t t ) represents the current gripper pose g. t To the target gripper position The transition matrix is ​​calculated as follows: Step S7-2: Based on the calculation relationship in step S7-1 and the transformation operator set in step S3-4-6, the improved heuristic objective can be obtained, where (h g (g t )) -1 For gripper position g t The inverse transformation of the corresponding homogeneous transformation matrix: Step S7-3: Utilize the policy network π constructed in step S1 θ and Value Network Q φ Generate the corresponding network prediction target and The loss functions for the network prediction objective and the heuristic objective are calculated using the formulas proposed in step S6-2. The calculation formulas are as follows: In the formula, θ is the parameter of the policy network, and φ′ is the parameter of the target value network in the DDPG network, which is used to update the parameters of the main value network.

10. The method for optimizing the 6D grasping pose of a robotic arm according to claim 8, characterized in that, Step S8 specifically includes the following steps: Step S8-1: Assuming the environment in which the robotic arm is located is completely observable, at each time step t, the agent moves from the current state s... t Take action a t After reaching state s t+1 , where state s t This represents the component of the point cloud feature S obtained from step S2-2 at time step t, while also obtaining the reward r for the current action. t The robotic arm generates a large number of trajectory samples by interacting with the environment. t a t r t s t+1 These samples are stored in the experience replay pool and used to train the reinforcement learning network. Step S8-2: The policy network outputs an action, denoted as a. t That is, the gripper pose at time step t+1. t+1 With time step t, gripper pose g t The pose offset is calculated using the following formula: h g (a t )=(h g (g t )) -1 h g (g t+1 ) a t =h T ((h g (g t )) -1 h g (g t+1 ))=[Δx,Δy,Δz,Δα,Δβ,Δγ] In the formula, Δx, Δy, and Δz are position offsets, and Δα, Δβ, and Δγ are angular offsets; during the autonomous decision-making process of the robotic arm, the trajectory T of the gripper from time step 0 to time step T is... r ={g0, g0, ..., g T }, where g T This indicates the gripper pose when the maximum number of grips is reached; Step S8-3: Generate state s using expert planner OMG t Expert Actions Combined with the policy network output action a from step S8-2 t Based on the point matching loss formula proposed in step S6-2, construct the loss function for behavior cloning: Step S8-4: Value Network Q φ The overall loss function expression is as follows: y represents the target Q value to be approximated, and its expression is as follows: y=r t +γQ φ′ (s t+1 ,π θ′ (s t+1 )+ε) In the formula Q φ′ and π θ′ These are the target value network and the target policy network, respectively. θ′ represents the parameters of the target policy network, γ is the reward discount factor (set to 0.95), ε is the pre-set shearing noise, and r t This is the reward value for the robotic arm's grasping action. A positive reward is given when the object is successfully grasped and stably lifted 20 centimeters; otherwise, a negative reward is given. The grasping reward function is defined as follows: Step S8-5: Policy Network π θ The overall loss function expression is as follows: λ is a weighted hyperparameter, mainly used to balance the expert loss and the learning value function loss. We set λ = 0.2, L DDPG (s t a t Regarding the loss of a deterministic policy, the objective is to maximize the Q-value, which can be specifically stated as follows: L DDpG (s t ,α t )=-Q φ (s t ,a t ) Step S8-6: Soft update the target value network Q φ′ and target policy network π θ′ : φ′←τφ+(1-τ)φ′ θ′←Tθ+(1-τ)θ′ In the formula, φ′ represents the target value network Q. φ′ The parameter φ is the value network Q. φ The parameters, θ′, are the target policy network π. θ′ The parameters are θ, which is the parameter of the policy network πθ, and τ, which is the soft update coefficient, set to τ = 0.999; Step S8-7: Repeat steps S3 to S8 multiple times to achieve the robotic arm's grasping of the target object and optimize the grasping process.

Images