Robot joint layer sensing system fusing ToF and monocular vision

Abstract

The invention discloses a robot joint layer sensing system fusing ToF and monocular vision, and belongs to the field of robot sensing, the system adopts an Eye-in-Hand architecture, and a ToF camera and an RGB camera are integrated; the quality of the depth map is improved by combining bilateral filtering and Kalman filtering; the six-degree-of-freedom pose estimation of the target is realized based on SURF feature matching and KD-Tree accelerated ICP registration; and planning a grabbing path in combination with an improved RRT algorithm and hierarchical collision detection. According to the robot joint layer sensing system fusing ToF and monocular vision provided by the invention, spatial alignment and fusion are carried out on two types of visual information, respective limitations are made up, and the robustness and precision of the whole sensing system are improved; in a single-arm and double-arm grabbing experiment, the positioning precision of the system reaches + / -3mm, and the dynamic scene grabbing success rate is high. The method is suitable for the fields of industrial automation, logistics sorting and the like.

Description

Technical Field

[0001] The present invention belongs to the field of robot perception technology and relates to a robot joint-layer perception system that integrates ToF (Time of Flight) and monocular vision, which is suitable for scenarios such as industrial grasping and precision assembly. Background Art

[0002] With the rapid development of industrial automation and intelligent manufacturing, the application of robotic systems in tasks such as grasping, handling, and assembly is showing a trend of diversification. By integrating multimodal sensors such as vision, touch, and force, and fusing them with machine learning algorithms, modern robots have initially acquired the ability to operate autonomously in complex environments. Visual perception, as the core supporting technology for autonomous operation, directly impacts task execution efficiency in dynamic and complex scenarios due to its spatiotemporal accuracy and real-time performance. In particular, in industrial scenarios such as precision assembly and flexible grasping, high-precision environmental perception within a near-field area of less than 50 cm from the end effector has become a key bottleneck limiting the success rate of operations.

[0003] Traditional robotic vision systems mostly use an eye-to-hand architecture, where the sensor is fixed outside the robot's workspace. This architecture has the following drawbacks: First, the fixed field of view is easily obstructed by the robotic arm, resulting in blind spots; second, the system relies on high-precision external calibration, resulting in high deployment and maintenance costs; and third, the long sensing distance makes it difficult to balance depth accuracy and image detail, limiting its effectiveness in fine-grained grasping tasks.

[0004] Current research is evolving along three main technical routes: (1) Tactile perception enhancement solutions, such as the GTac biomimetic tactile sensor developed by Lu’s team (Lu Z, Gao X, Yu H. GTac: A biomimetic tactile sensor with skin-like heterogeneous force feedback for robots[J]. IEEE Sensors Journal, 2022, 22(14): 14491-14500.), which achieves a force resolution of 0.1N through skin-like contact feedback, but has a response delay of about 20ms and relies on physical contact triggering; (2) Global vision guidance solutions, such as Nguyen et al. (Nguyen K, Dang T, Huber M. Real-time 3D semantic scene perception for egocentric robots with binocular vision[J]. arXiv preprint arXiv:2402.11872,2024.) Based on the binocular vision system of D435i camera, the object segmentation and grasping are realized on the Baxter robot, but the end positioning error is as high as 7.3mm due to the occlusion of the main body; For example, S.Jain et al. (Jain S, Argall B.Grasp detection for assistive robotic manipulation[C] / / 2016IEEE International Conference on Robotics and Automation(ICRA).IEEE,2016:2015-2021.) proposed a new grasping detection algorithm for assistive robotic operation system. The RGB-D sensor is located outside the robotic arm. The method can detect the human-like grasping method of various invisible household objects and is successfully applied to the autonomous grasping task of the MICO robotic arm. However, in the long-distance perception mode, the depth resolution of the RGB-D camera is seriously reduced; (3) End embedded perception scheme, such as Zeng team (Zeng A, Song S, Yu KT, et al.Robotic pick-and-place of novel objects in clutter with multi-affordance grasping and cross-domain image matching[J].The International Journal of Robotics Research,2022,41(7):690-705.)'s Eye-in-Hand system uses an end-user RGB-D camera to improve positioning accuracy to 2.1mm, but the multimodal data fusion delay reaches 120ms. Experimental studies have shown that a single sensor can miss as much as 32.7% of the depth in low-texture areas, and the pose estimation error in dynamic scenes fluctuates by more than ±5°. Summary of the Invention

[0005] The purpose of the present invention is to provide a robot joint-level perception system that integrates ToF and monocular vision, overcomes the shortcomings of the existing technology, and improves the target detection accuracy, posture estimation robustness, and trajectory planning efficiency in dynamic and complex scenes. The visual sensor is installed at the end of the robot (Eye-in-Hand structure) to achieve real-time environmental perception close to the operating area. The ToF depth camera can provide high-frame rate, interference-resistant three-dimensional depth information, which is suitable for close-range mapping; while the monocular RGB camera can provide rich texture information, which is conducive to target recognition and feature extraction. By spatially aligning and fusing the two types of visual information, their respective limitations can be compensated to a certain extent, and the robustness and accuracy of the overall perception system can be improved. The system provided by the present invention utilizes the complementary advantages of dual sensors, combines joint bilateral filtering and Kalman filtering technology to optimize the ToF depth map, and proposes an ICP (Iterative Closest Point) registration method based on SURF (Speeded Robust Features) features and KD-Tree (k-dimensional tree) acceleration to achieve high-precision six-degree-of-freedom pose estimation of the target; at the same time, a complete perception-recognition-estimation-grasping integrated process is constructed based on the ROS platform to support path planning and control execution of single-arm and dual-arm collaborative tasks. The purpose of the present invention is achieved through the following specific technical solutions.

[0006] A robot joint-layer perception system that integrates ToF and monocular vision, including:

[0007] Perception module: Integrates a ToF depth camera and a monocular RGB camera, and is installed at the end of the robot in an eye-in-hand structure;

[0008] Depth map optimization processing module: Combines bilateral filtering and Kalman filtering for two-stage optimization, uses the RGB image as a guide image, and performs edge-preserving filtering on the ToF depth map to suppress noise while retaining object contour information;

[0009] Target Recognition and Pose Estimation Module: Target recognition uses the SURF algorithm to extract local feature points from RGB images, and then uses the FLANN (Fast Nearest Neighbor Search) algorithm to accelerate matching and establish a matching relationship with the target template. After filtering out incorrect matching points using the RANSAC (Random Sample Consensus) algorithm, the image homography matrix is calculated to obtain the target region (ROI). Pose estimation uses KD-Tree-accelerated ICP point cloud registration to achieve six-degree-of-freedom pose estimation.

[0010] Path planning and grasping control module: Based on the ROS platform, path planning and grasping control are achieved through the improved RRT (rapid random tree) path planning algorithm, hierarchical collision detection mechanism, single-arm grasping control process and dual-arm collaborative control strategy.

[0011] Furthermore, the perception module uses Zhang Zhengyou's checkerboard calibration method to calibrate the intrinsic parameters of the monocular RGB camera.

[0012] Furthermore, the specific process of combining bilateral filtering and Kalman filtering for two-stage optimization in the depth map optimization processing module is as follows:

[0013] Joint bilateral filtering: Use RGB image to guide ToF depth map filtering, the weight function is:

[0014] Among them, p is the depth value of the target pixel, I(p) is the final new depth value of the target pixel, q is the original depth value of the neighbor pixel in the original target depth map, G s (||pq||) is the spatial distance weight, G r (|G(q)-G(p)|) is the color similarity weight function;

[0015] Kalman filter:

[0016] (1) Establishing a state equation for depth values

[0017] x k =Agx k-1 +w k

[0018] z k =Hgx k +v k

[0019] Where: x k 、x k-1 are the depth values of the kth and k-1th frames, respectively, in mm;

[0020] A: state transfer matrix, describing the change pattern of depth value between each frame;

[0021] w k 、x k : Process noise and measurement noise obey a Gaussian distribution with a mean of 0 and a variance of Q;

[0022] z k : The measured depth value of the kth frame, in mm;

[0023] H: observation matrix, usually the identity matrix;

[0024] (2) Prediction and update, processing each frame of data recursively; the error covariance matrix in the prediction stage for:

[0025]

[0026] in: The prediction error covariance matrix of the kth frame; P k-1 : error covariance matrix of the k-1th frame;

[0027] Kalman gain K in the update phase k for:

[0028]

[0029] Where R is the error covariance matrix of the observation noise.

[0030] Furthermore, the improved RRT path planning algorithm includes:

[0031] (1) Target point biased sampling: When expanding the tree structure, the system directly selects the target pose as the sampling point with a certain probability to enhance the guidance of the search tree and effectively improve the path convergence speed and search efficiency;

[0032] (2) Dynamic optimization of parent nodes and path pruning: After a new node is generated, a local search area is constructed with the node as the center, and its parent node is dynamically updated to the local optimal connection point; at the same time, the trajectory reconnection algorithm is used to delete redundant nodes to make the generated path shorter and smoother, thereby reducing the jitter or energy consumption of the robot arm during execution.

[0033] Furthermore, the layered collision detection mechanism includes:

[0034] (1) Construct the geometric model of the robotic arm based on the minimum cylinder envelope method;

[0035] (2) Decoupling detection of the master and slave arms in dual-arm tasks: First, the master arm path is planned and the temporal position is recorded. The master uses the master arm movement as a dynamic obstacle to plan the slave arm path.

[0036] Furthermore, the single-arm grasping control process includes:

[0037] (1) Pre-grasp posture generation: a set of safety buffer postures is set before the target position to ensure that the grasping direction is consistent with the main axis of the object;

[0038] (2) Synchronous control mechanism: The gripper starts closing at the same time when it reaches the preset position at the end, and the system adaptively adjusts the closing speed and force according to the gripper status feedback;

[0039] (3) Grasping exit path planning: After confirming that the grip is successful, a shortest obstacle avoidance path is replanned based on the current environment to place the object in the designated area.

[0040] Furthermore, the dual-arm collaborative control strategy includes:

[0041] (1) Fix the position of the slave arm, treat it as an obstacle, and plan the path of the master arm;

[0042] (2) Based on the master arm trajectory, the synchronous motion of the slave arm is inverted and coordination is achieved through the relative posture constraints between the ends of the two arms and the object;

[0043] (3) Perform synchronized time interpolation to ensure the dynamic consistency of both arms throughout the entire task.

[0044] The present invention has the following beneficial technical effects: the present invention provides a robot joint-layer perception system that integrates ToF and monocular vision, which compensates for the respective limitations of the two types of visual information by spatially aligning and fusing them, thereby improving the robustness and accuracy of the overall perception system; in single-arm and double-arm grasping experiments, the system positioning accuracy reaches ±3mm, and the success rate of dynamic scene grasping is high. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a relationship diagram of different coordinate systems.

[0046] Figure 2 It is the feature description graph of the SURF algorithm.

[0047] Figure 3 It is a flow chart of collision detection strategy.

[0048] Figure 4 This is a simulation diagram of the robot grasping experimental platform.

[0049] Figure 5 This is a diagram of the positioning and grasping process of Example 1.

[0050] Figure 6 This is a diagram of the positioning and grasping process of Example 2.

[0051] Figure 7 This is the RGB image of the book in Example 3.

[0052] Figure 8 This is the TOF depth image of the book in Example 3.

[0053] Figure 9 It is a three-dimensional point cloud image of the book in Example 3.

[0054] Figure 10 This is the ROI point cloud image of the book in Example 3.

[0055] Figure 11 This is a diagram of the book positioning and crawling process in Example 3. DETAILED DESCRIPTION

[0056] The following is a clear and complete description of the technical solution of the present invention in conjunction with the accompanying drawings. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts are within the scope of protection of the present invention.

[0057] In the description of the present invention, it should be understood that the terms "center," "longitudinal," "lateral," "upper," "lower," "front," "back," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," and the like, indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings and are intended solely to facilitate description and simplify the present invention. They are not intended to indicate or imply that the devices or components referred to must have, be constructed, or operate in a specific orientation, and therefore should not be construed as limiting the present invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance, quantity, or position.

[0058] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they may refer to fixed, detachable, or integral connections; mechanical or electrical connections; direct or indirect connections through an intermediate medium; and internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0059] A robot joint-level perception system that integrates Time of Flight (ToF) and monocular vision includes: a perception module that integrates a ToF depth camera and a monocular RGB camera, mounted on the end of the robot in an eye-in-hand configuration; a depth map optimization processing module that combines bilateral filtering and Kalman filtering for two-stage optimization, using the RGB image as a guide image to perform edge-preserving filtering on the ToF depth map to suppress noise while preserving object contour information; a target recognition and pose estimation module that uses the SURF algorithm to extract local feature points from the RGB image, combines FLANN accelerated matching with the target template image to establish a matching relationship, and uses the RANSAC algorithm to filter out erroneous matching points before calculating the image homography matrix to obtain the target region (ROI); pose estimation uses KD-Tree-accelerated ICP point cloud registration to achieve six-degree-of-freedom pose estimation; and a path planning and grasping control module that implements path planning and grasping control based on the ROS platform through an improved RRT path planning algorithm, a hierarchical collision detection mechanism, a single-arm grasping control process, and a dual-arm collaborative control strategy. Details are as follows.

[0060] (1) Perception module

[0061] A ToF depth camera and an industrial-grade monocular RGB camera are mounted above the end gripper. The two cameras are mounted on the same bracket with overlapping fields of view, forming a multimodal perception base. Zhang Zhengyou's checkerboard calibration method is used to calibrate the RGB camera's intrinsic parameters, obtaining parameters such as focal length, principal point, and distortion coefficient.

[0062] In the RGB visual system, the conversion relationship of four spatial coordinate systems is mainly involved, such as Figure 1 As shown. The pixel coordinate system uv takes the upper left corner vertex of the captured image as the coordinate origin; the physical coordinate system xy takes the image center as the origin; the camera coordinate system X c -Y c -Z c , the optical center is the origin, and the lens direction is +Z c Direction; world coordinate system X w -Y w -Z w .

[0063] Use the Tsai-Lenz method (Tsai RY, Lenz R KA new technique for fully autonomous and efficient3d robotics hand / eye calibration [J]. IEEE Transactions on Robotics and automation, 1989, 5(3): 345-358.), Park method (Park FC, Martin BJ. Robot sensor calibration: solving AX=XB on the Euclidean group [J]. IEEE Transactions on Robotics and Automation,1994,10(5):717-721.), Horaud method (Horaud R,Dornaika F.Hand-eye calibration[J].The internationaljournal ofrobotics research,1995,14(3):195-210.) and Daniilidis method (Daniilidis K.Hand-eye calibration using dual quaternions[J].The International Journal ofRobotics Research, 1999, 18(3): 286-298.) Complete the transformation matrix calculation between the end of the robotic arm and the camera, realize the conversion of the image coordinate system to the robot base coordinate system, and provide a geometric basis for subsequent point cloud projection and grasping planning.

[0064] (2) Depth map optimization processing module

[0065] Because ToF depth maps are susceptible to ambient light interference, edge transitions, and surface reflectivity, the raw data contains significant noise and jitter. To improve the quality of depth information, this paper proposes a two-stage optimization strategy that combines bilateral filtering and Kalman filtering. Using the RGB image as a guide, the ToF depth map is filtered using edge-preserving filtering to suppress noise while preserving object outline information.

[0066] The core idea of joint bilateral filtering (Wang Decheng, Chen Xiangning, Yi Hui, et al. Depth image hole filling and optimization algorithm based on adaptive joint bilateral filtering [J]. Chinese Journal of Lasers, 2019, 46(10): 1009002.) is to perform a weighted average of each pixel value in the target depth map, where the weight is determined not only by the spatial distance between pixels but also by the similarity of the pixel values in the RGB image. For the target pixel p, the filtering result I(p) is the weighted sum of the pixels in the neighborhood φ, as shown in the following formula:

[0067]

[0068] Among them, p is the depth value of the target pixel, I(p) is the final new depth value of the target pixel, q is the original depth value of the neighbor pixel in the original target depth map, G s (||pq||) is the spatial distance weight, G r (|G(q)-G(p)|) is the color similarity weight function;

[0069] To address the problem of inter-frame jumps in depth maps, a one-dimensional Kalman filter model is introduced to establish a prediction-update process for the depth value of each pixel to achieve inter-frame smoothing and improve stability and depth continuity.

[0070] Assume the depth value x k The change of satisfies the following state transition equation:

[0071] x k =Agx k-1 +w k

[0072] z k =Hgx k +v k

[0073] Where: x k 、x k-1 : Depth value of the kth and k-1th frames, in millimeters (mm);

[0074] A: state transfer matrix, describing the change pattern of depth value between each frame;

[0075] w k 、x k : Process noise and measurement noise obey a Gaussian distribution with a mean of 0 and a variance of Q;

[0076] z k : The measured depth value of the k-th frame.

[0077] H: Observation matrix, usually the identity matrix.

[0078] In the present invention, since the image is captured in a stationary state,

[0079]

[0080] Kalman filtering is divided into two stages: prediction and update, and processes each frame of data recursively. The error covariance matrix in the prediction stage is for:

[0081]

[0082] in: The prediction error covariance matrix of the kth frame; P k-1 : error covariance matrix of the k-1th frame;

[0083] In the update phase, the Kalman gain K k for:

[0084]

[0085] Where R is the error covariance matrix of the observation noise.

[0086] (3) Target recognition and posture estimation module

[0087] The present invention uses a method based on combining image features with point cloud registration to complete target recognition and three-dimensional posture estimation of objects. The SURF algorithm (Bay H, Tuytelaars T, Van Gool L. Surf: Speeded up robust features [C] / / Computer Vision–ECCV 2006: 9th European Conference on Computer Vision, Graz, Austria, May 7-13, 2006. Proceedings, Part I 9. Springer Berlin Heidelberg, 2006: 404-417.) is used to extract local feature points from the RGB image. The FLANN accelerated matching (Badri F, Yuniarno EM, Mardi SN S. 3D point cloud data registration based on multiview image using SIFT method for Djago temple relief reconstruction [C] / / 2015 4th International Conference on Instrumentation, Communications, Information Technology, and Biomedical Engineering (ICICI-BME). IEEE, 2015: 191-195.) is then used to establish a matching relationship with the target template image. After filtering out the incorrect matching points using the RANSAC algorithm, the image homography matrix is calculated to obtain the target region ROI.

[0088] The core idea of the SURF algorithm is detection and descriptor generation based on the Hessian matrix. The algorithm divides the entire process into three main stages:

[0089] (1) Feature point detection: SURF uses the determinant of the Hessian matrix to detect salient points in an image. To improve computational efficiency, the algorithm uses an integral image to accelerate the convolution operation of the Hessian matrix, greatly reducing the amount of computation.

[0090] (2) Direction assignment: By analyzing the gradient information of the feature point neighborhood, SURF assigns a main direction to each feature point to maintain the rotation invariance of the feature.

[0091] (3) Feature description: In the main direction and neighborhood of the feature point, SURF constructs a feature descriptor based on the Haar wavelet response. Figure 2 As shown in the feature description diagram, the descriptor forms a 64-dimensional or 128-dimensional feature vector by counting the changes in image brightness, which has strong distinguishing ability and stability.

[0092] For the original point cloud data, the present invention adopts the ROI region extraction method, projects the optimized ToF depth map to generate a point cloud, determines the target point cloud position according to the 2D image, separates the effective point cloud of the target object from the complex background, and extracts the local point cloud corresponding to the ROI region. In order to obtain complete and high-precision model point cloud data, the present invention adopts an RGB-D camera for data acquisition. According to the actual application requirements, a single-view or multi-view point cloud acquisition method can be selected to ensure that the detailed information of the target object surface is fully presented. The predefined object model point cloud is loaded and the ICP algorithm (Yang J, Li H, Campbell D, et al. Go-ICP: A globally optimal solution to 3D ICP point-set registration [J]. IEEE transactions on pattern analysis and machine intelligence, 2015, 38 (11): 2241-2254.) is used for three-dimensional registration. In order to improve efficiency and accuracy, the KD-Tree structure is introduced to accelerate the nearest neighbor search process.

[0093] In the field of point cloud registration, the ICP algorithm is one of the most widely used techniques. It does not rely on feature points, but rather directly performs rigid registration based on point cloud data by minimizing the Euclidean distance error between two sets of points. The ICP algorithm is a classic iterative optimization method used to calculate the optimal rigid transformation matrix between two point clouds, namely the rotation matrix R and the translation vector T. Its core concept is:

[0094] Input is a given template point cloud and ROI target point cloud The output is to solve a rigid transformation (R, T) so that the transformed point cloud Q′=RP+T, and the transformed Q′ and Q are aligned as much as possible.

[0095] The main steps of using the ICP algorithm are as follows:

[0096] (1) For each point p in the template point cloud i , find the nearest corresponding point q in the ROI target point cloud Q i :

[0097]

[0098] Here, ||g|| represents the Euclidean distance.

[0099] (2) Calculate the optimal rotation matrix R and translation vector T to minimize the error:

[0100]

[0101] This problem can be solved by SVD (singular value decomposition) method. The specific steps are as follows:

[0102] Calculate the mean of the template point cloud and the target point cloud:

[0103]

[0104] Calculate the decentralized point:

[0105]

[0106] Compute the covariance matrix:

[0107]

[0108] Perform SVD decomposition on the matrix H:

[0109] H=U∑V T

[0110] Calculate the rotation matrix:

[0111] R=VU T

[0112] If det(R) = -1, correction is required:

[0113] V′=[v1 v2 -v3],R=V′U T

[0114] Calculate the translation vector:

[0115]

[0116] (3) Update the point cloud P according to the calculated transformation matrix (R, T):

[0117] P′=RP+T

[0118] Then the template point cloud is updated and the next iteration is performed.

[0119] (4) Calculate the root mean square error (RMS Error):

[0120]

[0121] If the error is lower than the set threshold e0 or the error decreases very little, the registration is considered complete, otherwise it enters the next iteration.

[0122] (5) Conditions for stopping iteration:

[0123] The error change is lower than the threshold e0:

[0124] |E k -E k-1 |<e0

[0125] The number of iterations reaches the maximum value:

[0126] k>k max

[0127] The pose information of the target relative to the model point cloud is obtained through point cloud alignment calculation, providing basic input for subsequent grasping action generation.

[0128] (4) Path planning and grasping control module

[0129] To ensure a humanoid robot can efficiently, safely, and smoothly perform grasping operations in complex dynamic environments, this paper designs and implements a complete path planning and grasping control module based on the ROS platform. This module includes an improved RRT path planning algorithm, a hierarchical collision detection mechanism, a dual-arm collaborative control strategy, and a grasping execution process. These modules are integrated into the MoveIt motion control framework and support in-the-loop experimental verification using a real robot platform.

[0130] The RRT (Rapidly-Exploring Random Tree) algorithm (Ding Chengjun, Wang Zhenlin, Geng Yukun, et al. Efficient Sampling Adaptive RRT Algorithm for Mobile Robot Path Planning [J / OL]. Mechanical Science and Technology, 1-8 [2025-05-23].) is widely used in robot path planning due to its excellent high-dimensional space search capabilities. However, traditional RRT algorithms have problems such as uneven paths, low expansion efficiency, and unstable obstacle avoidance. To address these shortcomings, the present invention proposes the following two optimizations:

[0131] (1) Goal-biased Sampling

[0132] When expanding the tree structure, the system directly selects the target pose as the sampling point with a certain probability (such as 30%) to enhance the guidance of the search tree and effectively improve the path convergence speed and search efficiency.

[0133] (2) Parent Node Optimization and Path Pruning (Parent Optimization + Shortcutting)

[0134] After a new node is generated, a local search region is constructed with that node as the center, and its parent node is dynamically updated to become the local optimal connection point. Simultaneously, a trajectory reconnection algorithm is used to prune redundant nodes, making the generated path shorter and smoother, reducing robot arm jitter and energy consumption during execution.

[0135] Simulation verification shows that the improved RRT algorithm outperforms traditional methods in terms of path quality, planning time and success rate, and is particularly suitable for the needs of fast-response path generation in dynamic environments.

[0136] To ensure the feasibility of the grasping path and the movement safety of the robotic arm, this paper introduces a two-layer collision detection mechanism based on geometric modeling for single-arm and dual-arm systems:

[0137] Geometric modeling method: The minimum cylindrical envelope (MCE) method is used to build a compact geometric model based on the actual shape of the robotic arm to improve detection accuracy and real-time performance.

[0138] Hierarchical detection mechanism: For dual-arm collaborative tasks, the present invention designs a master-slave arm decoupling detection strategy: first plan the master arm path and record its discrete time-series posture, and then introduce the master arm motion as a dynamic obstacle to avoid in the slave arm path planning, which significantly reduces the computational complexity.

[0139] The single-arm grasping control process includes: pre-grasping posture generation: setting a set of safety buffer postures before the target position to ensure that the grasping direction is consistent with the main axis of the object; synchronous control mechanism: the gripper starts closing at the same time when it reaches the preset position at the end, and the system adaptively adjusts the closing speed and force based on the gripper status feedback; grasping exit path planning: after confirming that the gripping is successful, a shortest obstacle avoidance path is re-planned based on the current environment to place the object in the designated area.

[0140] Dual-arm grasping tasks are divided into two categories based on coordination requirements: Loosely Coordinated mode: The two arms perform independent tasks, such as grasping multiple objects simultaneously on the left and right sides, each planning its own path and detecting collisions in real time; Tightly Coordinated mode: The two arms work together on a single object, forming a closed-loop system.

[0141] The present invention proposes a master-slave arm timing coordinated control strategy: fix the slave arm posture, regard it as an obstacle, and plan the master arm path; inversely analyze the synchronous motion of the slave arm based on the master arm trajectory, and complete coordination through the relative posture constraints between the ends of the two arms and the object; perform synchronous time interpolation to ensure the dynamic consistency of the two arms throughout the entire task process.

[0142] This strategy is suitable for complex scenarios such as collaborative handling, folding and assembly, and improves the stability and task accuracy of the dual-arm system.

[0143] (5) Experimental verification

[0144] A robotic grasping experimental platform was built. The platform is based on a 7-DOF industrial robotic arm with a custom gripper installed at the end. It integrates an RGB-D vision system and uses an Eye-in-Hand structure to achieve near-field perception. The computing platform runs a virtual machine under the Windows 11 system, with Ubuntu 20.04 and ROS Noetic deployed internally. The simulation is completed through the ROS platform, which has real-time visual processing and motion planning capabilities. The simulation diagram is as follows: Figure 4 As shown. The entire system is built based on the ROS framework, integrating perception, recognition, planning and control modules, and combined with MoveIt to complete trajectory generation and execution, ensuring the efficiency and stability of the system operation. In order to verify the performance of the proposed grasping system based on joint layer perception, the present invention designed single-arm grasping experiments and dual-arm collaborative grasping experiments respectively. The single-arm grasping task is based on the Sawyer collaborative robot arm and the Eye-in-Hand multimodal vision system, and mainly evaluates the comprehensive performance of target recognition, pose estimation and trajectory planning; the dual-arm collaborative task simulates a complex grasping environment to examine the stability and robustness of the system in dynamic collaboration.

[0145] Example 1

[0146] Single-arm grasping experiment: A standard-shaped express box was selected as the target, and positioning and grasping tests were carried out under normal lighting. The positioning and grasping process is as follows: Figure 5 50 tests were conducted, and the average depth image filtering time (s), average object recognition and pose detection time (s), and positioning accuracy (mm) were calculated. The results are shown in Table 1.

[0147] Example 2

[0148] Single-arm grasping experiment: We select low-reflectivity objects as targets and conduct positioning and grasping tests under complex material backgrounds. Figure 6 50 tests were conducted, and the average depth image filtering time (s), average object recognition and pose detection time (s), and positioning accuracy (mm) were calculated. The results are shown in Table 1.

[0149] Table 1 Test results of Example 1 and Example 2

[0150]

[0151] As can be seen from Table 1, under different object conditions, the system can stably achieve positioning accuracy within ±3mm, meeting the requirements of grasping operations.

[0152] Example 3

[0153] Dual-arm collaborative grasping experiment: Aiming at the application of intelligent book grasping, a human-machine collaborative platform was built to complete the book grasping and classification tasks through a 7-DOF Sawyer robotic arm and an auxiliary vacuum suction cup device. Due to the limitations of experimental equipment conditions, this study adopted a single robotic arm and human-machine collaborative experimental scheme. The experimental platform consists of a Sawyer 7-DOF collaborative robotic arm and an experimenter, simulating the dual-arm collaborative book grasping task. The RGB image of the book is as follows Figure 7 As shown, the TOF depth image is as follows Figure 8 As shown, the three-dimensional point cloud is as follows Figure 9 As shown, the ROI point cloud is as follows Figure 10 As shown, the crawling process is as follows Figure 11 The experiment simulated a library environment and examined the system's gripping stability and grasping success rate when facing open-structured objects (such as books). This verified the effectiveness and promotion potential of the robot joint-layer perception system that integrates ToF and monocular vision in practical application scenarios.

[0154] Although the embodiments of the present invention have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art may make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention without departing from the principles and intent of the present invention. The scope of protection of the present invention is defined by the claims and their equivalents.

Claims

1. A robot joint layer perception system integrating ToF and monocular vision, characterized by: include: Perception module: Integrates a ToF depth camera and a monocular RGB camera, and is installed at the end of the robot in an eye-in-hand structure; Depth map optimization processing module: Combines bilateral filtering and Kalman filtering for two-stage optimization, uses the RGB image as a guide image, and performs edge-preserving filtering on the ToF depth map to suppress noise while retaining object contour information; Target recognition and pose estimation module: Target recognition uses the SURF algorithm to extract local feature points from the RGB image, combines FLANN accelerated matching with the target template image to establish a matching relationship, and uses the RANSAC algorithm to filter out incorrect matching points. The image homography matrix is calculated to obtain the target area ROI; Pose estimation uses KD-Tree accelerated ICP point cloud registration to achieve six-degree-of-freedom pose estimation; Path planning and grasping control module: Based on the ROS platform, path planning and grasping control are achieved through the improved RRT path planning algorithm, hierarchical collision detection mechanism, single-arm grasping control process and dual-arm collaborative control strategy.

2. The system according to claim 1, wherein: The perception module uses Zhang Zhengyou's checkerboard calibration method to calibrate the intrinsic parameters of the monocular RGB camera.

3. The system according to claim 1, wherein: The specific process of combining bilateral filtering and Kalman filtering for two-stage optimization in the depth map optimization processing module is as follows: Joint bilateral filtering: Use RGB image to guide ToF depth map filtering, the weight function is: Among them, p is the depth value of the target pixel, I(p) is the final new depth value of the target pixel, q is the original depth value of the neighboring pixel in the original target depth map, G s (||pq||) is the spatial distance weight, G r (|G(q)-G(p)|) is the color similarity weight function; Kalman filter: (1) Establishing a state equation for depth values x k =Agx k-1 +w k z k =Hgx k +v k Where: x k 、x k-1 are the depth values of the kth and k-1th frames, respectively, in mm; A: state transfer matrix, describing the change pattern of depth value between each frame; w k 、x k : Process noise and measurement noise obey a Gaussian distribution with a mean of 0 and a variance of Q; z k : The measured depth value of the kth frame, in mm; H: observation matrix, usually the identity matrix; (2) Prediction and update, processing each frame of data recursively; the error covariance matrix in the prediction stage for: in: The prediction error covariance matrix of the kth frame; P k-1 : error covariance matrix of the k-1th frame; Kalman gain K in the update phase k for: Where R is the error covariance matrix of the observation noise.

4. The system according to claim 1, wherein: The improved RRT path planning algorithm includes: (1) Target point biased sampling: When expanding the tree structure, the system directly selects the target pose as the sampling point with a certain probability to enhance the guidance of the search tree and effectively improve the path convergence speed and search efficiency; (2) Dynamic optimization of parent nodes and path pruning: After a new node is generated, a local search area is constructed with the node as the center, and its parent node is dynamically updated to the local optimal connection point; at the same time, the trajectory reconnection algorithm is used to delete redundant nodes to make the generated path shorter and smoother, thereby reducing the jitter or energy consumption of the robot arm during execution.

5. The system according to claim 1, wherein: The layered collision detection mechanism includes: (1) Construct the geometric model of the robotic arm based on the minimum cylinder envelope method; (2) Decoupling detection of the master and slave arms in dual-arm tasks: First, the master arm path is planned and the temporal position is recorded. The master uses the master arm movement as a dynamic obstacle to plan the slave arm path.

6. The system according to claim 1, wherein: The single-arm grasping control process includes: (1) Pre-grasp posture generation: a set of safety buffer postures is set before the target position to ensure that the grasping direction is consistent with the main axis of the object; (2) Synchronous control mechanism: The gripper starts closing at the same time when it reaches the preset position at the end, and the system adaptively adjusts the closing speed and force according to the gripper status feedback; (3) Grasping exit path planning: After confirming that the grip is successful, a shortest obstacle avoidance path is replanned based on the current environment to place the object in the designated area.

7. The system according to claim 1, wherein: The dual-arm collaborative control strategy includes: (1) Fix the position of the slave arm, treat it as an obstacle, and plan the path of the master arm; (2) Based on the master arm trajectory, the synchronous motion of the slave arm is inverted and coordination is achieved through the relative posture constraints between the ends of the two arms and the object; (3) Perform synchronized time interpolation to ensure the dynamic consistency of both arms throughout the entire task.

Images