Object grabbing method based on RGB image incremental three-dimensional reconstruction

Abstract

The invention discloses an object grabbing method based on RGB image incremental three-dimensional reconstruction. Comprising the following steps: S0, initializing the motion of a mechanical arm; s1, extracting a key frame of the RGB image; s2, performing depth recovery on the image based on the key frame to obtain a preliminary recovery depth map and a confidence map Ci; s3, performing depth optimization on the preliminarily recovered depth map to obtain an optimized depth map; S4, performing incremental updating on a global reconstruction result by using the optimized depth map to obtain a depth map fused with multi-view information; S5, calculating a normal vector of the depth map to obtain a normal map Ni; s6, the grabbing pose is expressed; s7, estimating the captured key points; s8, estimating the grabbing approaching direction and the width of the clamping jaw; and S9, the mechanical arm executes the grabbing action according to the current motion state and the updated target pose. The mechanical arm has the universal grabbing capacity on various objects, the grabbing success rate is high, reconstruction and grabbing pose optimization are synchronously carried out in the process that the mechanical arm approaches the objects, and extra time consumption is avoided.

Description

Technical Field

[0001] This invention relates to the field of 3D reconstruction technology, specifically to an object grasping method based on incremental 3D reconstruction of RGB images. Background Technology

[0002] Object grasping is one of the core capabilities of robot operation and plays an important role in a variety of real-world applications, such as the automatic picking and assembly of components in industrial production lines, handling and stacking in the logistics field, and the operation of everyday items by home assistant robots.

[0003] In object grasping, the most commonly used device is a robotic arm with a two-finger gripper. Depending on the settings, two-finger gripper robotic arms are generally divided into three-degree-of-freedom (3-DOF) grasping in a two-dimensional plane and six-degree-of-freedom (6-DOF) grasping in three-dimensional space. In 3-DOF grasping in a two-dimensional plane, the camera is positioned at a top-down view to photograph the tabletop, and the robotic arm gripper faces perpendicular to the tabletop. The position of the gripper in the two-dimensional plane on the tabletop and the rotation angle of the gripper around the axis perpendicular to the tabletop are adjusted first, and then the gripper approaches the target object from top to bottom to grasp it. 6-DOF grasping in three-dimensional space is closer to real-world grasping scenarios. The camera can photograph the scene from any angle, and the grasping algorithm calculates the grasping pose in SE(3) space. The robotic arm gripper can approach the object from any angle. The six-degree-of-freedom grasping scheme can more flexibly adapt to complex geometric structures and spatial constraints, thus achieving a more robust operational effect.

[0004] However, most existing 6-DOF grasping methods rely on point cloud information provided by depth sensors to obtain 3D spatial information. However, the point clouds returned by depth sensors usually contain a certain degree of noise, especially on objects with special surface materials (such as transparent or reflective surfaces), which can lead to significant measurement errors, posing challenges to the accuracy and stability of grasping. The paper "Contact-GraspNet: Efficient 6-DoF Grasp Generation in Cluttered Scenes" uses PointNet to extract features from the point cloud obtained from the depth sensor and regresses the grasping contact probability and grasping direction to calculate the grasping pose. The paper "GraspNerf: Multiview-based 6-DoF Grasp Detection for Transparent and Specular Objects Using Generalizable NeRF" first controls a robotic arm to circle the table, capturing multi-view RGB images of the scene. It then infers the neural radiation field (Nerf) from the multi-view RGB images and predicts the grasping pose based on the reconstructed neural radiation field. However, Contact-GraspNet uses point clouds obtained from depth sensors for feature extraction and grasping pose estimation, making it significantly affected by the noise of the depth sensor. Without utilizing the detailed texture information provided by RGB images, effective grasping cannot be generated on objects with special surface materials (transparent, reflective). GraspNerf requires controlling the robotic arm to first circle the table horizontally once for each grasping scenario, capturing RGB images of the scene from multiple perspectives, which introduces significant additional time consumption. Furthermore, the robotic arm can only circle and photograph objects located in the center of its working range, failing to do so for objects at the edges, thus narrowing the actual grasping range. Additionally, GraspNerf is trained using simulated datasets, resulting in poor generalization in real-world scenarios. Summary of the Invention

[0005] For six-DOF object grasping tasks, this invention proposes a method that does not rely on depth sensors. It fully utilizes RGB image information and uses multi-view RGB images for 3D scene reconstruction, inferring the grasping pose based on the reconstruction. Unlike GraspNerf, this invention does not require surround-view shooting of the scene before grasping. Instead, it proposes a method for simultaneous reconstruction and grasping. An RGB camera is mounted at the end of a robotic arm in an "eye-on-hand" manner. As the robotic arm approaches the object, it uses RGB to perform progressive incremental 3D reconstruction, simultaneously predicting and optimizing the grasping pose, and sending the updated target pose to the robotic arm for execution.

[0006] To achieve the above objectives, the present invention provides an object grasping method based on incremental 3D reconstruction of RGB images, comprising the following steps:

[0007] Includes the following steps:

[0008] S0, Initialize the robotic arm movement;

[0009] S1. Extract keyframes from the RGB image;

[0010] S2. Based on the keyframes, perform depth restoration on the image to obtain a preliminary restored depth map. And confidence plot C i ;

[0011] S3. The preliminary depth map is restored. Depth optimization is performed to obtain the optimized depth map.

[0012] S4. Use the optimized depth map The global reconstruction results are incrementally updated to obtain the fused depth map.

[0013] S5. Calculate the depth map. The normal vectors are used to obtain the normal map N. i ;

[0014] S6. Represent the grasping pose;

[0015] S7. Estimate the key points to be captured;

[0016] S8. Estimate the grasping approach direction and gripper width;

[0017] S9. The robotic arm performs a grasping action based on the current motion state and the updated target pose.

[0018] Preferably, step S0 specifically includes the following steps:

[0019] S01. Adjust the initial joint angles of the robotic arm so that the camera can see the complete desktop scene, and use this set of joint angles as the fixed initial joint angles of the robotic arm for all subsequent experiments.

[0020] S02. Obtain the initial end-effector pose from the robotic arm, set the initial target grasping position to 0.1m in the positive x direction of the current end-effector position, keep the pose unchanged, and start the robotic arm to move.

[0021] Preferably, step S1 specifically includes the following steps:

[0022] S11. The RGB camera at the end of the robotic arm captures RGB images from different perspectives in real time, and the robotic arm transmits its end-effector pose T back in real time.end ;

[0023] S12. Based on the external parameters of the camera and the robotic arm end effector... Calculate the camera pose corresponding to the k-th frame RGB image:

[0024]

[0025] Where T is the pose matrix of SE(3). Includes rotation matrix R and translation t.

[0026] S13. Calculate the relative pose between the current frame i and the previous keyframe j. Relative rotation and relative translation

[0027] S14, through relative rotation and relative translation Calculate the distance between two poses like Exceeding a certain threshold θ kf If so, then the i-th frame is determined to be a keyframe.

[0028] Preferably, step S2 specifically includes the following steps:

[0029] S21, For keyframe I i and the previous keyframe I i-1 Dense image matching is performed using an image matching algorithm to find dense pixel matching pairs between two frames of images and the confidence level of each matching pair.

[0030] S22. Based on the camera pose T corresponding to the two frames of images cam (i) and T cam (i-1) and camera intrinsic parameter K are used to calculate the 3D point coordinates corresponding to each pair of pixel matching pairs through triangulation, and the confidence of the matching pair is used as the confidence of the 3D point coordinates.

[0031] S23. Triangulate the dense pixel matching pairs between the two frames to obtain the three-dimensional point cloud of the scene.

[0032] S24. Project the 3D point cloud onto keyframe I i In the camera coordinate system, keyframe I is obtained. i Preliminary recovery depth map and confidence plot C i .

[0033] Preferably, step S3 specifically includes the following steps:

[0034] S31. Estimating relative depth map using a monocular depth estimation model. The relative depth map and true depth map The relationship is Where s is the scale factor and t is the offset factor;

[0035] S32. Using the preliminary restored depth map The optimal estimates are obtained by robustly estimating the scaling factor s and the bias factor t using the least squares algorithm with RANSAC.

[0036] S33. Using the optimal estimates of the scaling factor s and the offset factor t, we obtain...

[0037] S34, will and preliminary recovery depth map According to confidence plot C i The fusion process yields an optimized depth map.

[0038] Preferably, step S32 specifically includes the following steps:

[0039] S321. Set the number of iterations in RANSAC to iter = 1000, and the interior point threshold to θ. inliner =0.01m, in each iteration, from Two depths with confidence scores higher than δ were randomly selected from the data. conf =0.6 pixels, get two points in and Depth and

[0040] S322. Construct the least squares model, i.e., solve...

[0041]

[0042] S323. Apply s and t to the entire depth map. Again with Subtraction yields the residual plot

[0043] S324. Calculate the absolute value less than δ. inliner The proportion of pixels to all pixels, i.e., the inlier rate;

[0044] S325. If the in-point rate of the current round is higher than the highest in-point rate of all previous rounds, then update the optimal s and t estimates to the estimates of the current round.

[0045] S325. Repeat the above process until the set number of iterations iter is reached to obtain the optimal estimates of s and t.

[0046] Preferably, step S4 specifically includes the following steps:

[0047] S41. Using the TSDF Fusion algorithm according to Update the voxel values ​​in the TSDF field to incrementally fuse and reconstruct the 3D scene;

[0048] S42. Extract the updated TSDF field. The isosurfaces are used to generate triangular meshes using the Marching Cubes algorithm, and this is done in keyframe I. i Render depth maps under the camera pose to obtain accurate depth maps after multi-view fusion.

[0049] Preferably, step S5 specifically includes the following steps:

[0050] S51. Backproject the pixel (u,v) into 3D space according to the depth to obtain the 3D point coordinates.

[0051] S52. Construct two tangent vectors v1 = p(u+1,v) - p(u-1,v) and v2 = p(u,v+1) - p(u,v-1) using its neighborhood points. The normalized vector obtained by the cross product of the two tangent vectors is the normal vector of that point.

[0052] S53. Calculate the normal vectors of all pixels to obtain the normal map N. i .

[0053] Preferably, the process of representing the grasping pose in step S6 is as follows:

[0054] The two-finger gripper has two contact points when grasping an object. From any given viewpoint, these are the visible contact point P1 and the invisible contact point P2, which is obscured by the object. The surface normal direction of contact point P1 is n. P1 The two fingers of the gripper are spread out with a width of w, and the gripper moves along n. z The direction is close to the object;

[0055] The 3D coordinates [x1, y1, z1] of contact point P1 are obtained by passing through the key point coordinates kp on the RGB image. 2d Combined with its depth d P1 Calculate with camera intrinsic parameter K:

[0056] [x1,y1,z1] T =d P1 K -1 kp 2d

[0057] To ensure gripping stability, the grippers are opened along the surface normal at point P1:

[0058] n x =-n P1

[0059] After determining P1 and the opening direction of the grippers, calculate the center point P of the line connecting the ends of the grippers based on the width w. c :

[0060] [x c ,y c ,z c ] T =[x1,y1,z1] T +0.5w·n x

[0061] n is predicted through subsequent steps. z Then according to n y =n z ×n x Calculate n y This allows us to solve for all components of the grasping pose [x]. c ,y c ,z c ,n x ,n y ,n z ].

[0062] Preferably, step S7 specifically includes the following steps:

[0063] S71, Transform the normal graph N i and RGB image I i The components are stitched together along the channel dimension.

[0064] S72. Input the concatenation result into the Vision Transformer (ViT) backbone network to extract deep features, and rearrange the sequence features output by ViT according to the original block order to form a two-dimensional spatial feature map F. i ;

[0065] S73. Use the Keypoint Head decoder to decode the confidence score of each pixel belonging to the capture keypoint from the feature map;

[0066] S74. Select the point with the highest confidence level on the image as the 2D keypoint kp for subsequent generation of the grasping pose. 2d ;

[0067] S75, kp 2d Based on its depth map The depth value is back-projected into 3D space to obtain the 3D visible contact point P1 of the grasp.

[0068] Preferably, step S8 specifically includes the following steps:

[0069] S81, 2D key points The ViT features corresponding to the surrounding l×l pixel region are cropped out;

[0070] S82. Perform average pooling to obtain the mean value of features within the region;

[0071] S83. Input the features into the proximity direction prediction subnetwork and the width prediction subnetwork respectively, and predict the gripper proximity direction n. app And the width w of the gripper opening required for grasping;

[0072] S84. Eliminate the predicted gripper approach direction n through vector projection. app In n x The components in the direction are normalized to obtain the gripper approach direction n in the aforementioned gripping pose representation. z ;

[0073] Compared with the prior art, the beneficial effects of the present invention are:

[0074] 1. This invention uses only an RGB camera, eliminating the reliance on depth cameras in grasping tasks and avoiding the influence of depth camera measurement noise. Based on state-of-the-art image matching methods, this invention can recover accurate and detailed depth maps from RGB images. It also incorporates prior knowledge from monocular depth estimation methods to optimize depth, ensuring accurate depth even on objects with transparent, reflective, or other special surface materials. Furthermore, this invention uses the TSDF Fusion algorithm to fuse depth estimates from multiple perspectives, further suppressing noise and obtaining globally consistent and accurate 3D reconstruction results. This accurate 3D reconstruction results in a high success rate for grasping both ordinary and special-material objects compared to existing technologies, demonstrating universal grasping capabilities across various object types.

[0075] 2. The method of this invention simultaneously reconstructs and optimizes the grasping pose as the robotic arm approaches the object, continuously updating the target pose for grasping. It eliminates the need for prior panoramic photography of the grasping scene, thus avoiding additional time consumption. Furthermore, the grasping range of this invention is not limited by panoramic photography, enabling the grasping of objects located at the edge of the robotic arm's workspace.

[0076] 3. The grasping pose representation and calculation method employed in this invention obtains the coordinates and normals of the 3D visible contact points corresponding to the 2D keypoints on the image from the 3D reconstruction results, and then allows the gripper to open and close along the normal. This representation method eliminates the need to estimate the gripper opening and closing direction; it only requires estimating the gripper approach direction and gripper width from the network to calculate the complete grasping pose. Therefore, this invention reduces the number of variables requiring neural network estimation, reduces the number of network parameters that need to be trained, and improves the accuracy of grasping pose estimation. Attached Figure Description

[0077] Figure 1 This is a flowchart of the three-dimensional reconstruction process of the present invention;

[0078] Figure 2 This is a flowchart of the pose estimation process for the present invention.

[0079] Figure 3 This is a diagram illustrating the grasping pose representation method of the present invention;

[0080] Figure 4 This is a physical image of the object grasping experimental platform of the present invention;

[0081] Figure 5 This is a photograph of a target object (ordinary) being grasped by the inventor.

[0082] Figure 6 This is a photograph of the actual object (made of a special material) that this invention captures. Detailed Implementation

[0083] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0084] The application scenario of this invention is the grasping of objects placed on a table by a robotic arm. The hardware system includes a robotic arm equipped with a two-finger gripper for performing the grasping operation, an RGB camera fixedly mounted at the end of the robotic arm in an "eye-on-hand" manner for providing environmental information, and a computer for receiving RGB camera images and sending the target grasping pose to the robotic arm.

[0085] After system startup, the computer begins receiving real-time RGB video streams captured by the RGB camera. Simultaneously, it acquires the end-effector pose of each frame from the robotic arm and calculates the camera pose based on the extrinsic parameters between the robotic arm and the camera, obtaining the camera pose corresponding to each RGB image frame. As the robotic arm moves closer to the tabletop object, the system determines keyframes based on changes in camera pose. Each new keyframe combines its own RGB and camera pose information from the previous keyframe for depth estimation and incorporates the global TSDF (Truncation Distance Field) for incremental 3D reconstruction, obtaining a more accurate depth map. The system uses the RGB data from the keyframes and the reconstructed depth map to infer the grasping keypoints and grasping pose on the object. Then, the system sends the grasping pose to the robotic arm for execution, repeating the reconstruction-update grasping pose steps during the robotic arm's movement until the distance between the camera and the target grasping position is less than a certain threshold. At this point, the grasping pose is no longer updated, and grasping is performed directly.

[0086] This invention provides a method for object grasping based on incremental 3D reconstruction of RGB images, comprising the following steps:

[0087] S0, Initialize the robotic arm movement.

[0088] The RGB camera was fixedly mounted on the end of the robotic arm in a "handheld" manner. The initial joint angles of the robotic arm were adjusted so that the camera could see the complete desktop scene, and these joint angles were used as the initial joint angles for all subsequent experiments.

[0089] Obtain the end-effector pose T from the robotic arm at the initial moment. end (0), where T is the pose matrix of SE(3). This includes rotation matrix R and translation t. To acquire multi-view RGB images, the robotic arm and camera need to begin moving. The initial target grasping position is set to 0.1m in the positive x-direction of the current end-effector position, with the orientation remaining constant, i.e.:

[0090] t goal (0)=t end (0)+[0.1,0,0] T

[0091] R goal (0)=R end (0)

[0092]

[0093] T goal (0) The signal is sent to the robotic arm, which then begins to move. Subsequent algorithms will continuously optimize the target grasping pose during the movement.

[0094] S1. Extract keyframes from the RGB image.

[0095] During the movement of the robotic arm, an RGB camera, fixedly mounted on the end effector in a "eye-on-hand" manner, captures RGB images from different perspectives in real time. Simultaneously, the robotic arm also transmits its end-effector pose T back in real time. end Based on the extrinsic parameter transformation of the camera and the robotic arm end effector The camera pose corresponding to the k-th frame RGB image can then be calculated:

[0096]

[0097] Where T is the pose matrix of SE(3). Including rotation matrix R and translation t, to determine keyframes, the relative pose between the current frame i and the previous keyframe j is first calculated. Relative rotation and relative translation Calculate the distance between two poses by combining rotation and translation. like Exceeding a certain threshold θ kf If , then the i-th frame is determined to be a keyframe. Typically, δ kf Set it to 0.06~0.08.

[0098] S2. Based on the keyframes, perform depth restoration on the image to obtain a preliminary restored depth map. And confidence plot C i .

[0099] To reconstruct the 3D structure of a scene, this invention uses keyframes for depth recovery of the image. For keyframe I... i Combine it with the previous keyframe I i-1 Dense image matching is performed using the state-of-the-art image matching algorithm RoMa (Robust dense feature matching) to find dense pixel matching pairs between two images and the confidence score of each pair. Then, based on the camera pose T corresponding to the two images... cam (i) and T cam (i-1) and the camera intrinsic parameter K are used to calculate the coordinates of the 3D spatial point corresponding to each pair of matched pixels using a triangulation method. Let the homogeneous pixel coordinates of a pixel matching pair in the two frames be (u... i-1 ,v i-1 ,1) and (u i ,v i The corresponding homogeneous coordinates of the 3D point are X = (x, y, z, 1). The camera projection matrices of the two frames are respectively... and Then, cross-product equations can be written for the observation points from both perspectives, resulting in a linear equation AX = 0.

[0100]

[0101] Solving this equation using singular value decomposition yields the 3D point coordinates corresponding to this matching pair, and the confidence score of the matching pair is used as the confidence score of the 3D point. Performing the above triangulation operation on dense pixel matching pairs between two image frames reconstructs the 3D point cloud of the scene. Projecting the 3D point cloud onto the keyframe kf... i In the camera coordinate system, the keyframe kf is obtained. i Preliminary recovery depth map and confidence plot C i .

[0102] S3. The preliminary depth map is restored. Depth optimization is performed to obtain the optimized depth map.

[0103] The depth maps recovered through image matching and triangulation often have some problems. In low-texture areas and on objects with transparent or reflective materials, the image matching error is relatively large and the confidence level is low. Therefore, the initially recovered depth maps need to be optimized.

[0104] This invention utilizes the state-of-the-art monocular depth estimation model, DepthAnythingV2, to estimate relative depth maps. Thanks to the powerful priors learned by DepthAnythingV2 on a large training set, depth maps It provides accurate relative depth across various scenes and objects, but lacks absolute scale information. (Estimated relative depth map) and true depth map The relationship is Where s is the scale factor and t is the offset factor. Using the initially reconstructed depth map with absolute scale... This invention robustly estimates s and t using a least squares algorithm with RANSAC. The main steps are as follows: ① Set the number of RANSAC iterations (iter = 1000) and the interior point threshold θ. inliner =0.01m. ② In each iteration, from Two depths with confidence scores higher than δ were randomly selected from the data. conf =0.6 pixels, get two points in and Depth and ③ Construct the least squares model, i.e., solve

[0105]

[0106] ④ Apply s and t to the entire depth map Again with Subtraction yields the residual plot Calculate the absolute value less than δ inliner The proportion of pixels in a given round to all pixels is called the inlier rate. If the inlier rate in the current round is higher than the highest inlier rate in all previous rounds, then the optimal estimates of s and t are updated to the estimates for the current round. ⑤ Repeat the above process until the set number of iterations (iter) is reached to obtain the optimal estimates of s and t.

[0107] The optimal estimates of s and t are obtained Will and According to confidence plot C i To merge get Fusion absolute scale and Its fine structure allows for more accurate depth perception in low-texture areas and on objects with special surface materials.

[0108] S4. Use the optimized depth map Incremental updates are performed on the global reconstruction results to obtain a depth map that integrates information from multiple perspectives.

[0109] During the movement of the robotic arm, the depth estimation results of different keyframes may be inconsistent to some extent. Fusion of depth maps from multiple frames can yield a more accurate reconstruction result with global consistency. This invention uses Truncated Signed Distance Function Fusion (TSDF) to represent the 3D scene and uses the TSDFFusion algorithm to fuse the depth estimates from different keyframes. TSDF discretizes the space into a voxel grid, with each voxel x recording a truncated signed distance value. This represents the distance from the point to the surface. In this invention, the spatial range of x∈(0,1.5m), y∈(-0.75m,0.75m), z∈(0,1m) with the installation position of the robotic arm on the desktop as the origin is set as the range of the TSDF field, and discretized into a voxel mesh with a side length of 0.005m. The target object is placed within this spatial range.

[0110] During the movement of the robotic arm, for each keyframe I i After completing its deep restoration and optimization, the TSDFFusion algorithm is used according to... Update the voxel values ​​in the TSDF field to incrementally fuse and reconstruct the 3D scene. Then extract the updated TSDF field. The isosurfaces are used to generate triangular meshes using the Marching Cubes algorithm, and this is done in keyframe I. i By rendering the depth map under the camera pose, an accurate depth map after multi-view fusion can be obtained.

[0111] S5. Calculate the depth map. The normal vectors are used to obtain the normal map N. i .

[0112] Normal maps are crucial for guiding the object's grasping direction. This invention utilizes camera intrinsic parameters K and depth maps fused from multiple viewpoints. Calculate the normal map. For a pixel (u,v), it can be back-projected into 3D space according to depth to obtain the 3D point coordinates. To calculate the normal vector of a point, two tangent vectors are constructed using its neighboring points: v1 = p(u+1,v) - p(u-1,v) and v2 = p(u,v+1) - p(u,v-1). The cross product of these two tangent vectors, after normalization, yields the normal vector of that point. By calculating the normal vectors of all pixels using this method, the normal map N can be obtained. i .

[0113] S6. Represent the grasping pose.

[0114] The goal of grasp pose calculation is to solve the 6-DOF grasp pose in SE(3) space, i.e. Figure 3 The center point P of the line connecting the ends of the two fingers' claws c 3D coordinates [x c ,y c ,z c And the origin is located at P c coordinate axes [n x ,n y ,n z ], where n x The direction in which the two fingers are spread, n y Let n be the normal direction of the gripper plane. z The direction in which the gripper approaches the object. This invention designs the following gripping pose representation method to solve [x] c ,y c ,z c ,n x ,n y ,n z ].

[0115] The two-finger gripper has two contact points when grasping an object. From any given viewpoint, these are the visible contact point P1 and the invisible contact point P2, which is obscured by the object. The surface normal direction of contact point P1 is n. P1 The two fingers of the gripper are spread out with a width of w, and the gripper moves along n.z The direction is close to the object.

[0116] The 3D coordinates [x1, y1, z1] of contact point P1 can be obtained from the keypoint coordinates kp on the RGB image. 2d Combined with its depth d P1 Calculate with camera intrinsic parameter K:

[0117] [x1,y1,z1] T =d P1 K -1 kp 2d

[0118] To ensure gripping stability, the grippers are opened along the surface normal at point P1:

[0119] n x =-n P1

[0120] Once P1 and the opening direction of the grippers are determined, the center point P of the line connecting the ends of the grippers can be calculated based on the width w. c :

[0121] [x c ,y c ,z c ] T =[x1,y1,z1] T +0.5w·n x

[0122] n is predicted through subsequent steps. z Then according to n y =n z ×n x Calculate n y This allows us to solve for all components of the grasping pose [x] c ,y c ,z c ,n x ,n y ,n z ].

[0123] During the aforementioned reconstruction process, the depth map on keyframe i has already been completed. and normal diagram N i The calculation allows for the convenient acquisition of any kp. 2d depth d P1 and normal n P1 In subsequent steps, this invention will complete the processing of 2D key points kp. 2d and the direction n z Estimate the grasp width w to solve for the complete grasp pose.

[0124] S7. Estimate the key points to be captured.

[0125] Complete the keyframe I i After 3D reconstruction, in order to determine a suitable position for grasping, this invention designs a key point detection method on the RGB image. To simultaneously utilize image texture and geometric information, the normal map N is first... i and RGB image I i The sequence features are concatenated along the channel dimension and then fed into the Vision Transformer (ViT) backbone network to extract deep features. The output sequence features of ViT are then rearranged into a two-dimensional spatial feature map F according to the original block order. i Subsequently, this invention designs a Keypoint Head decoder based on multi-layer convolution and upsampling to decode the confidence score of each pixel belonging to a keypoint from the feature map. The keypoint decoder consists of the following four modules connected in sequence:

[0126] • 3×3 convolution (output channels are 256) + ReLU + upsampling × 2

[0127] • 3×3 convolution (64 output channels) + ReLU + upsampling × 2

[0128] • 3×3 convolution (16 output channels) + ReLU + upsampling × 2

[0129] • 3×3 convolution (output channel number is 1) + Sigmoid + upsampled to the original image I i resolution

[0130] This invention selects the point with the highest confidence level in the image as the 2D key point kp for subsequent generation of the grasping pose. 2d . kp 2d Based on its depth map The depth value is back-projected into 3D space to obtain the 3D visible contact point P1 of the grasp.

[0131] The ViT backbone network and keypoint decoder are trained using the Hammer and Housecat6d datasets, both of which contain a large number of 6-DOF grasp pose labels on objects in 3D scenes. This invention obtains ground truth keypoints for training by projecting the 3D grasp contact points from the ground truth labels onto the RGB image. The network training loss function uses focal loss to balance easy and difficult samples.

[0132] S8. Estimate the grasping approach direction and gripper width.

[0133] In keyframe I i In the middle, using 2D key points The aforementioned F corresponds to a square neighborhood of size l×l pixels. i The ViT features (l=56 in the experiment) are used to estimate the approach direction n of the keypoint using a multilayer perceptron (MLP). app And the width w of the gripper opening required for grasping.

[0134] Specifically, 2D key points The ViT features corresponding to the surrounding l×l pixels are cropped out. First, average pooling is performed to obtain the mean value of the features within the region. Then, the features are input into the Approach Head and Width Head subnetworks respectively to calculate the approach direction of the gripper and the opening width of the gripper. Both the Approach Head and Width Head are MLPs with one hidden layer.

[0135] Due to the network's prediction error, the n output by the Approach Head... app It may not be the same as n in the aforementioned grasping pose representation. x Vertical, therefore we cannot directly set n. z =n app First, n is eliminated through vector projection. app In n x The components in the direction are then normalized to obtain:

[0136]

[0137] The width w output by the Width Head can be directly applied to the aforementioned grasping pose representation.

[0138] Both the Approach Head and Width Head were trained using the Hammer and Housecat6d datasets. This invention uses ground truth 2D keypoints obtained by projecting 3D grasping contact points from labels onto RGB images, cropping the ViT features corresponding to the surrounding l×l square neighborhood, and inputting this data into the network. The network output n... app The absolute value of the difference between w and the truth value in the captured tags is used as the loss.

[0139] S9. The robotic arm performs a grasping action based on the current motion state and the updated target pose.

[0140] The keyframe I was solved using the method described above. i Capture pose in the camera coordinate system [x c ,y c ,z c ,n x ,n y ,n z This yields a 4×4 grasping pose matrix. in To control the robotic arm to perform grasping poses, camera poses are used to... Transform to the robot arm base coordinate system. Will The data is sent to the robotic arm to update the target pose it is grasping. The robotic arm will then re-plan and execute the trajectory based on the current motion state and the updated target pose (the trajectory planning method uses a library developed by the robotic arm's official team, which is not within the scope of this invention).

[0141] During subsequent robotic arm movements, the system will repeat the above process, continuing to judge keyframes, performing incremental 3D reconstruction and grasping pose estimation on the keyframes, and updating the robotic arm's target grasping pose. When the target position [x c ,y c ,z c Distance from the camera (θ in the experiment) dist When the target position is 0.4m, the subsequent reconstruction and grasping pose update process will be stopped, and the grasping will be completed directly based on the current target pose.

[0142] Object grasping evaluation metrics:

[0143] This invention uses Success Rate (SR) to evaluate the effectiveness of object grasping. It is calculated by dividing the number of successful grasps by the total number of attempts. The criterion for judging a successful grasp is: each time a grasping attempt is made, the algorithm controls the robotic arm to reach the grasping pose, closes the gripper to clamp the object, and then lifts it upwards by 5cm. If the object follows the robotic arm's gripper upwards without falling, the grasp is considered successful.

[0144] We also evaluated the time required to grasp a single object (Runtime), which is calculated as the time from algorithm startup to the robotic arm reaching the grasping pose.

[0145] Object grasping experimental platform:

[0146] Experimental platform for object grasping, such as Figure 4 As shown, a robotic arm is fixedly mounted on a tabletop. The end of the robotic arm is equipped with a two-finger gripper, and a camera is fixedly mounted on the gripper in an "eye-on-hand" manner. It grasps the target object and places it on the tabletop.

[0147] To evaluate the algorithm's effectiveness in grasping different types of objects, this invention prepared 14 target objects for testing. For example... Figure 5 , Figure 6Seven of the objects shown are made of ordinary materials, and seven are made of special surface materials that are transparent and reflective. None of these objects appeared during the training of the aforementioned neural network model of this invention.

[0148] Result:

[0149] The method of this invention was compared with Contact-GraspNet and GraspNerf, and the success rate of grasping objects of ordinary and special materials was calculated respectively. In the experiment, 10 grasping attempts were made on 14 target objects using the three algorithms respectively. As shown in Table 1, it can be seen that the success rate of grasping objects of this invention on both ordinary and special materials significantly exceeds that of the comparison methods. Furthermore, the difference in the success rate of grasping objects of this invention is the smallest between ordinary and special materials. This demonstrates that the method of this invention has universal grasping capability on various types of objects.

[0150] Table 1 Comparison of average crawling success rate with other methods

[0151] method SR (Ordinary Object) SR (Special Material Object) Contact-GraspNet 68.6％ 42.9％ GraspNerf 77.1％ 61.4％ Method of the present invention 91.4％ 87.1％

[0152] In this invention, ablation experiments were conducted on key design aspects of the method to demonstrate its effectiveness. Figure 5 Ordinary objects number 1 and 4 in the middle and Figure 6 Ablation experiments were conducted on objects with special materials, numbered 1 and 7. Ten grasping attempts were made for each object using different ablation settings, as shown in Table 2. In ablation setting 1, not using depth optimization based on DepthAnythingV2 significantly reduced the grasping success rate on objects with special materials, demonstrating the important role of the depth optimization module in grasping these objects. In ablation setting 2, not using multi-view depth fusion reduced the grasping success rate on both ordinary and special material objects to some extent, highlighting the necessity of filtering depth estimation errors based on multi-view fusion. In ablation setting 3, not using keypoint normals as n... x Instead, an additional subnetwork is trained to estimate n from the ViT features in the keypoint neighborhood. x This increases the number of network parameters and the difficulty of training, resulting in a slight decrease in the success rate of grasping. This indicates that using the contact point normal as n... x The effectiveness.

[0153] Table 2 Ablation Experiment

[0154] Ablation settings SR (Ordinary Object) SR (Special Material Object) Complete method of the present invention 92.5％ 87.5％ ① Do not use the deep optimization module 82.5％ 55.0％ ② Do not use multi-view depth fusion 80.0％ 75.0％ <![CDATA[③Do not use the contact point normal as n x > 87.5％ 82.5％

[0155] To demonstrate the advantage of the method of the present invention in terms of grasping range, Figure 5Object No. 2 was placed at fixed positions 0.5, 0.8, and 1.1 meters away from the robotic arm base. At each position, Contact-GraspNet, GraspNerf, and the method of this invention were used to grasp the object 10 times each, and the success rate was calculated. The results are shown in Table 3. When grasping objects at greater distances, Contact-GraspNet's success rate decreases to some extent because the point cloud provided by the depth camera is sparse at distant locations. GraspNerf's success rate drops significantly when panoramic imaging is not possible for distant objects. The method of this invention, by continuously optimizing the reconstruction results and grasping pose as it approaches the object, has a success rate less affected by distance and can stably grasp objects at the edge of the robotic arm's workspace.

[0156] Table 3 Success Rate of Capture at Different Locations

[0157] method 0.5m 0.8m 1.1m Contact-GraspNet 80％ 80％ 60％ GraspNerf 80％ 50％ 10％ Method of the present invention 100％ 90％ 90％

[0158] This invention also compares the average time taken by each method to grasp a single object. Figure 5 Object #5 was placed at a fixed position 0.5 meters away from the robotic arm base. Contact-GraspNet, GraspNerf, and the method of this invention were used to grasp the object multiple times until five successful grasps were achieved. The average time for the five successful grasps was calculated, and the results are shown in Table 4. GraspNerf, based on multi-view RGB images, requires surround shooting and 3D reconstruction of the object before grasping, resulting in significant additional time consumption. The method of this invention performs reconstruction and grasping pose optimization simultaneously as the robotic arm approaches the object, without incurring additional time consumption. The average grasping time is essentially equivalent to that of Contact-GraspNet based on depth camera point clouds.

[0159] Table 4 Comparison of Average Fetch Time

[0160] method Runtime Contact-GraspNet 5.24s GraspNerf 26.6s Method of the present invention 5.38s

[0161] While the invention has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that different dependent claims and features described herein can be combined in ways different from those described in the original claims. It is also understood that features described in conjunction with individual embodiments can be used in other described embodiments.

Claims

1. A method for object grasping based on incremental 3D reconstruction of RGB images, characterized in that, Includes the following steps: S0, Initialize the robotic arm movement; S1. Extract keyframes from the RGB image; S2. Based on the keyframes, perform depth restoration on the image to obtain a preliminary restored depth map. And confidence plot C i ; S3. The preliminary depth map is restored. Depth optimization is performed to obtain the optimized depth map. S4. Use the optimized depth map Incremental updates are performed on the global reconstruction results to obtain a depth map that integrates information from multiple perspectives. S5. Calculate the depth map. The normal vectors are used to obtain the normal map N. i ; S6. Represent the grasping pose; S7. Estimate the key points to be captured; S8. Estimate the grasping approach direction and gripper width; S9. The robotic arm performs a grasping action based on the current motion state and the updated target pose.

2. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 1, characterized in that, Step S1 specifically includes the following steps: S11. The RGB camera at the end of the robotic arm captures RGB images from different perspectives in real time, and the robotic arm transmits its end-effector pose T back in real time. end ; S12. Based on the external parameters of the camera and the robotic arm end effector, the transformation is performed. Calculate the camera pose corresponding to the k-th frame RGB image: Where T is the pose matrix of SE(3). Includes rotation matrix R and translation t. S13. Calculate the relative pose between the current frame i and the previous keyframe j. Relative rotation and relative translation S14, through relative rotation and relative translation Calculate the distance between two poses like Exceeding a certain threshold θ kf If so, then the i-th frame is determined to be a keyframe.

3. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 2, characterized in that, Step S2 specifically includes the following steps: S21, For keyframe I i and the previous keyframe I i-1 Dense image matching is performed using an image matching algorithm to find dense pixel matching pairs between two frames of images and the confidence level of each matching pair. S22. Based on the camera pose T corresponding to the two frames of images cam (i) and T cam (i-1) and camera intrinsic parameter K are used to calculate the 3D point coordinates corresponding to each pair of pixel matching pairs through triangulation, and the confidence of the matching pair is used as the confidence of the 3D point coordinates. S23. Triangulate the dense pixel matching pairs between the two frames to obtain the three-dimensional point cloud of the scene. S24. Project the 3D point cloud onto keyframe I i In the camera coordinate system, keyframe I is obtained. i Preliminary recovery depth map and confidence plot C i .

4. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 3, characterized in that: Step S3 specifically includes the following steps: S31. Estimating relative depth map using a monocular depth estimation model. The relative depth map and true depth map The relationship is Where s is the scale factor and t is the offset factor; S32. Using the preliminary restored depth map The optimal estimates are obtained by robustly estimating the scaling factor s and the bias factor t using the least squares algorithm with RANSAC. S33. Using the optimal estimates of the scaling factor s and the offset factor t, we obtain... S34, will and preliminary recovery depth map According to confidence plot C i The fusion process yields an optimized depth map.

5. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 4, characterized in that, Step S32 specifically includes the following steps: S321. Set the number of iterations in RANSAC to iter = 1000, and the interior point threshold to θ. inliner =0.01m, in each iteration, from Two depths with confidence scores higher than δ were randomly selected from the data. conf =0.6 pixels, get two points in and Depth and S322. Construct the least squares model, i.e., solve... S323. Apply s and t to the entire depth map. Again with Subtraction yields the residual plot S324. Calculate the absolute value less than δ. inliner The proportion of pixels to all pixels, i.e., the inlier rate; S325. If the in-point rate of the current round is higher than the highest in-point rate of all previous rounds, then update the optimal s and t estimates to the estimates of the current round. S325. Repeat the above process until the set number of iterations iter is reached to obtain the optimal estimates of s and t.

6. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 1, characterized in that, Step S4 specifically includes the following steps: S41. Using the TSDF Fusion algorithm according to... Update the voxel values ​​in the TSDF field to incrementally fuse and reconstruct the 3D scene; S42. Extract the updated TSDF field. The isosurfaces are used to generate triangular meshes using the Marching Cubes algorithm, and this is done in keyframe I. i Render depth maps under the camera pose to obtain accurate depth maps after multi-view fusion.

7. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 1, characterized in that, Step S5 specifically includes the following steps: S51. Project the pixel (u,v) back onto 3D space according to the depth to obtain the 3D point coordinates p(u,v) = S52. Construct two tangent vectors v1 = p(u+1,v) - p(u-1,v) and v2 = p(u,v+1) - p(u,v-1) using its neighborhood points. The normalized vector obtained by the cross product of the two tangent vectors is the normal vector of that point. S53. Calculate the normal vectors of all pixels to obtain the normal map N. i .

8. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 1, characterized in that: Step S6, representing the grasping pose, is as follows: The two-finger gripper has two contact points when grasping an object. From any given viewpoint, these are the visible contact point P1 and the invisible contact point P2, which is obscured by the object. The surface normal direction of contact point P1 is n. P1 The two fingers of the gripper are spread out to a width of w, and the gripper moves along n. z The direction is close to the object; The 3D coordinates [x1, y1, z1] of contact point P1 are obtained by passing through the key point coordinates kp on the RGB image. 2d Combined with its depth d P1 Calculate with camera intrinsic parameter K: [x1,y1,z1] T =d P1 K -1 kp 2d To ensure gripping stability, the grippers are opened along the surface normal at point P1: n x =-n P1 After determining P1 and the opening direction of the grippers, calculate the center point P of the line connecting the ends of the grippers based on the width w. c : [x c ,y c ,z c ] T [x1,y1,z1] T +0.5w·n x n is predicted through subsequent steps. z Then according to n y =n z ×n x Calculate n y This allows us to solve for all components of the grasping pose [x] c ,y c ,z c ,n x ,n y ,n z ].

9. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 1, characterized in that, Step S7 specifically includes the following steps: S71, Transform the normal graph N i and RGB image I i The components are stitched together along the channel dimension. S72. Input the concatenation result into the Vision Transformer (ViT) backbone network to extract deep features, and rearrange the sequence features output by ViT according to the original block order to form a two-dimensional spatial feature map F. i ; S73. Use the Keypoint Head decoder to decode the confidence score of each pixel belonging to the keypoint from the feature map; S74. Select the point with the highest confidence level on the image as the 2D keypoint kp for subsequent generation of the grasping pose. 2d ; S75, kp 2d Based on its depth map The depth value is back-projected into 3D space to obtain the 3D visible contact point P1 of the grasp.

10. The object grasping method based on incremental 3D reconstruction of RGB images according to claim 1, characterized in that, Step S8 specifically includes the following steps: S81, 2D key points The ViT features corresponding to the surrounding l×l pixel region are cropped out; S82. Perform average pooling to obtain the mean value of features within the region; S83. Input the features into the proximity direction prediction subnetwork and the width prediction subnetwork respectively, and predict the gripper proximity direction n. app And the width w of the gripper opening required for grasping; S84. Eliminate the predicted gripper approach direction n through vector projection. app In n x The components in the direction are normalized to obtain the gripper approach direction n in the aforementioned gripping pose representation. z .

Images