Visual positioning and mechanical arm grabbing method based on ROS system

Abstract

The invention relates to the technical field of robotic arm positioning and grabbing, and provides a visual positioning and robotic arm grabbing method based on an ROS system, which automatically identifies a visual sensor and a robotic arm which are connected through the ROS system, and performs synchronization and alignment of image data and point cloud data by using a timestamp and a timestamp index table. The problem of data time mismatching is effectively solved; image data and point cloud data are processed and subjected to feature extraction, and the features of the image data and the point cloud data are combined for matching, so that the features of the target object can be described more comprehensively and accurately; when the motion path of the mechanical arm is generated, the target posture is fully considered, and the mechanical arm can approach a target object in a proper posture; visual verification and force verification are comprehensively utilized for grabbing feedback, whether a target object is in a clamping jaw or not is judged in combination with edge information, whether grabbing force is abnormal or not is judged through threshold value comparison, a verification result is fed back to the ROS system in time, and the ROS system can conduct real-time adjustment according to the verification result.

Description

Technical Field

[0001] The present application relates to the technical field of robot arm positioning and grasping, and in particular to a visual positioning and robot arm grasping method based on a ROS system. Background Art

[0002] In traditional visual positioning and robotic arm grasping methods, data obtained by different sensors often lack an effective synchronization mechanism. Most of them simply record data in chronological order without taking into account factors such as sensor hardware delay and data transmission delay, resulting in data mismatch in time, affecting the subsequent judgment of the target position, and causing the robotic arm to fail to grasp; at the same time, a single feature extraction algorithm is used, which cannot fully and accurately describe the characteristics of the target object; and traditional robotic arm motion path planning methods often only consider the starting and target positions of the robotic arm, ignoring the impact of the target posture on the path, which will cause the robotic arm to have inaccurate posture when approaching the target and fail to complete the grasping task; when verifying whether the target object is successfully grasped, the existing technology has a single means and cannot accurately judge whether the grasping posture of the object is correct, which leads to low accuracy and reliability of grasping verification. Summary of the invention

[0003] In view of the shortcomings of the prior art, the present application provides a visual positioning and robotic arm grasping method based on the ROS system, the method comprising: starting the ROS system, and acquiring image data and point cloud data of the target object, adding timestamps of the image data and point cloud data of the target object, constructing a timestamp index table to achieve data synchronization and alignment, and generating matching pairs; receiving a matching pair, processing image data of the target object, extracting image features of the target object, matching point cloud features of point cloud data of the target object to form a feature pair, and determining a target position and a target posture based on the matched feature pair; Locate the current position of the robot arm, generate an initial motion path based on the target position, and generate a motion path from the current position to the target position in combination with the target posture; The motion path is converted into motion instructions for each joint of the robotic arm so that the target object can be grasped by the robotic arm. It is verified whether the target object is successfully grasped and fed back to the ROS system.

[0004] As an optional implementation, the logic for acquiring the image data and point cloud data of the target object includes: Start the ROS system and automatically identify the connected visual sensors and robotic arms; Acquire image data and point cloud data of target objects through visual sensors; Perform data synchronization and alignment on the image data and point cloud data of the target object.

[0005] As an optional implementation, the data synchronization and alignment sub-logic includes: Add timestamps to the image data and point cloud data of the target object through the ROS system, and build a timestamp index table; Configure the time error threshold, traverse the timestamp index table, calculate the absolute value of the difference between the timestamps of adjacent image data and point cloud data, and determine whether the image data and point cloud data match; The matched image data and point cloud data are stored in the data storage area and marked as matching pairs, while the unmatched image data and point cloud data are discarded.

[0006] As an optional implementation, the target position and target posture determination logic includes: receiving a matching pair, processing image data of the target object, and extracting image features of the target object; Process the point cloud data of the target object and extract the point cloud features of the target object; The image features and point cloud features are matched to form feature pairs, and the position and posture of the target object in the world coordinate system are calculated based on the matched feature pairs to obtain the target position and target posture.

[0007] As an optional implementation, the sub-logic of obtaining the target position and target posture based on the matched feature pair includes: The similarity between image features and point cloud features is measured by Euclidean distance, and feature pairs are obtained by threshold comparison and screening; Read the intrinsic and extrinsic matrix of the camera in the visual sensor, unify the image coordinate system and the point cloud coordinate system, and obtain the world coordinate system; The rotation matrix and displacement vector of the target object are calculated by the PNP algorithm according to the matched feature pairs; The rotation matrix and displacement vector of the target object are converted to obtain the position and attitude of the target object in the world coordinate system, thereby obtaining the target position and target attitude.

[0008] As an optional implementation, the logic for generating the motion path of the robot arm from the current position to the target position includes: Locate the current position of the robot arm; Generate an initial motion path based on the current position and target position of the robot arm; Smooth and optimize the initial motion path based on the target pose.

[0009] As an optional implementation manner, the sub-logic of generating the initial motion path includes: Construct the topological space of the robot's grasping motion; Determine whether there is a continuous connected path from the current position node to the target position node of the robot arm; Search for the shortest connected path from the current position node to the target position node of the robot arm to obtain the initial motion path.

[0010] As an optional implementation, the logic of grabbing the target object by the robotic arm includes: Analyze the joint angles and joint trajectories of the robot arm on the motion path and generate analysis results; Generate motion instructions for each joint of the robot arm based on the analysis results; Control the robotic arm to grab the target object according to the motion instructions of each joint of the robotic arm; Verify whether the target object is successfully grasped and generate verification results; Feedback the verification results to the ROS system.

[0011] As an optional implementation, the sub-logic of generating the parsing result includes: Filter nodes from the motion path; The joint angles of each joint of the robot arm at each node are calculated through the inverse solution algorithm, and a joint angle sequence is generated; The joint angle sequence is interpolated through the interpolation algorithm to form the joint trajectory of the robot.

[0012] As an optional implementation, the verification result generation sub-logic includes: Send a trigger signal to the visual sensor through the ROS system to capture the state image of the target object after it is grasped; Process the state image and extract the edge information of the target object and the gripper to determine whether the target object is in the gripper; Monitor the gripping force of the robot arm during the grasping process, and determine whether there is any abnormality in the robot arm through threshold comparison.

[0013] Compared with the prior art, the beneficial effects of this application are: The ROS system automatically identifies connected visual sensors and robotic arms, greatly improving the convenience and compatibility of device access and reducing the difficulty of ROS system configuration. The timestamp and timestamp index table are used to synchronize and align image data and point cloud data, effectively solving the problem of data time mismatch and improving data accuracy and consistency, providing a reliable data foundation for subsequent precise target positioning and posture determination.

[0014] By processing and extracting features from image data and point cloud data separately and combining their features for matching, the characteristics of the target object can be described more comprehensively and accurately, thus improving the accuracy of target positioning and posture determination. In complex environments, the position and posture of the target object can also be accurately identified.

[0015] When generating the robot arm's motion path, the target posture is fully considered, which enables the robot arm to approach the target object with a suitable posture, thereby improving the grasping success rate. By constructing a topological space and searching for the shortest connected path to generate the initial motion path, a feasible path can be quickly planned in a complex environment, thereby improving the efficiency of motion path planning.

[0016] The comprehensive use of visual verification and grasping force verification for grasping feedback, combined with edge information to determine whether the target object is in the gripper and threshold comparison to determine whether the grasping force is abnormal, improves the accuracy and reliability of grasping verification, effectively avoids grasping failure or unstable grasping, and promptly feeds back the verification results to the ROS system, so that the ROS system can make real-time adjustments based on the verification results, such as replanning the motion path or adjusting the grasping strategy, thereby improving production efficiency and product quality and enhancing the intelligence and automation level of the ROS system. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative labor. Among them: Figure 1 A method step diagram of a visual positioning and robotic arm grasping method based on a ROS system provided in an embodiment of the present application; Figure 2 A data synchronization and alignment sub-logic diagram of the visual positioning and robotic arm grasping method based on the ROS system provided in the embodiment of the present application; Figure 3 A sub-logic diagram of obtaining target position and target posture based on matched feature pairs of the visual positioning and robotic arm grasping method based on the ROS system provided in the embodiment of the present application; Figure 4 A sub-logic diagram for generating verification results of the visual positioning and robotic arm grasping method based on the ROS system provided in the embodiment of the present application. DETAILED DESCRIPTION

[0018] In order to make the objectives, technical solutions and advantages of the embodiments of the present application more obvious and easy to understand, the technical solutions in the embodiments of the present application are clearly and completely described below in conjunction with the drawings in the specification. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments.

[0019] Example like Figure 1As shown, a method step diagram of a visual positioning and robotic arm grasping method based on a ROS system is provided for an embodiment of the present application, and the method includes: S1. Start the ROS system and obtain the image data and point cloud data of the target object.

[0020] Specifically, the logic for acquiring the image data and point cloud data of the target object includes: Start the ROS system and automatically identify the connected visual sensors and robotic arms; Acquire image data and point cloud data of target objects through visual sensors; Perform data synchronization and alignment on the image data and point cloud data of the target object.

[0021] In the ROS system, establish a communication connection with the visual sensor and the robotic arm, such as connecting through communication interface protocols such as USB and Ethernet, start the image acquisition node and the point cloud acquisition node, ensure that each node can successfully connect and receive image data and point cloud data, set the parameter configuration of the visual sensor, and perform preliminary calibration. The visual sensor includes a depth camera and a point cloud sensor to respectively obtain image data and point cloud data of the target object, and set the camera parameters of the depth camera as needed, including the resolution, frame rate, exposure time and gain of the depth camera; in terms of calibration, use Zhang's calibration method to calculate the intrinsic parameter matrix (including focal length and principal point coordinates, etc.) and distortion coefficient of the depth camera by shooting a checkerboard image to correct image distortion; for the point cloud sensor, set parameters such as the scanning range and angular resolution, and perform zero-point calibration.

[0022] Through the above steps, high-quality image data and point cloud data are obtained, which provide accurate basic data for subsequent target recognition, positioning and planning of the robot arm's motion path, and improve the accuracy and efficiency of the robot arm's grasping.

[0023] Furthermore, if Figure 2 As shown, the sub-logic of data synchronization and alignment includes: Add timestamps to the image data and point cloud data of the target object through the ROS system, and build a timestamp index table; Configure the time error threshold, traverse the timestamp index table, calculate the absolute value of the difference between the timestamps of adjacent image data and point cloud data, and determine whether the image data and point cloud data match; The matched image data and point cloud data are stored in the data storage area and marked as matching pairs, while the unmatched image data and point cloud data are discarded.

[0024] In the complex environment where the robotic arm works, due to the data transmission delay of the visual sensor and environmental interference, data synchronization of image data and point cloud data becomes particularly important, otherwise it will lead to inaccurate target positioning, thus affecting the grasping operation of the robotic arm.

[0025] At the moment when the visual sensor captures the image data and point cloud data of the target object, an accurate timestamp is added to each frame of image data and each set of point cloud data through the time acquisition function of the ROS system. The received image data and point cloud data of the target object are sorted in the order of timestamps in the ROS system, and a timestamp index table is constructed. The timestamp index table uses timestamps as keywords to associate the storage addresses of corresponding image data and point cloud data, which is convenient for quick search and comparison.

[0026] Set a time error threshold (such as 5 milliseconds), traverse the timestamp index table, use multi-threading technology to improve the traversal speed, and for each timestamp of image data, start a thread to search the timestamp of the adjacent point cloud data in the timestamp index table. By calculating the absolute value of the difference between the two timestamps, determine whether it is within the set time error threshold. When the absolute value of the difference between the two timestamps is within the time error threshold, the image data and the point cloud data match and are marked as a matching pair; when the absolute value of the difference between the two timestamps is not within the time error threshold, the image data and the point cloud data do not match and the unmatched image data and point cloud data are discarded.

[0027] At the same time, an adaptive mechanism can be introduced for the above-mentioned time error threshold to dynamically adjust the time error threshold according to the feedback information of data processing. When the amount of data of the matching pair is small, resulting in the inability to proceed normally with subsequent processing, the time error threshold is increased. When there are many false matches in the data of the matching pair due to large time differences, the time error threshold is reduced, thereby improving the accuracy of the matching.

[0028] Accurate data synchronization and alignment improves the accuracy of subsequent determination of target position and target posture, reduces the probability of robotic arm grasping errors, and improves the overall performance and reliability of the ROS system.

[0029] S2. Process the image data of the target object, extract the image features of the target object, and determine the target position and target posture by combining the point cloud data of the target object.

[0030] Specifically, the determination logic of the target position and target attitude includes: receiving a matching pair, processing image data of the target object, and extracting image features of the target object; Process the point cloud data of the target object and extract the point cloud features of the target object; The image features and point cloud features are matched to form feature pairs, and the position and posture of the target object in the world coordinate system are calculated based on the matched feature pairs to obtain the target position and target posture.

[0031] In the robot's grasping task, the position and posture of the target object need to be accurately known in order to accurately control the robotic arm to grasp it. Different types of data contain information of different dimensions of the target object. Image data focuses on the appearance characteristics of the target object, while point cloud data reflects the three-dimensional spatial structure of the target object. By extracting features from these two types of data respectively and then matching these features, the two-dimensional image information can be associated with the three-dimensional spatial information. Based on the matching feature pairs, the position and posture of the target object in the world coordinate system can be calculated, thereby achieving accurate positioning and orientation of the target object in space.

[0032] Receive the matching pair, pre-process the image data, and convert the color image into a grayscale image to reduce the dimension of the image data and improve the efficiency of subsequent processing. First, use the convolutional neural network to extract features of the image data in the matching pair to obtain the image features of the target object including shape and texture.

[0033] Voxel grid filtering is used to denoise and downsample the point cloud data to reduce the data volume, and the point cloud feature extraction algorithm is used to extract point cloud features. Point cloud features include normal vectors and curvature. Point cloud features reflect the local and global geometric structure of the point cloud.

[0034] Furthermore, a statistical filtering method is used to calculate the distance between each point in the point cloud data and its neighboring points (the neighborhood radius is set to r, for example, r = 0.05m), and the mean distance of all points is calculated. and standard deviation , for distance mean greater than ( To set the threshold, ) is identified as an outlier and removed. This step can effectively eliminate isolated points caused by visual sensor noise or measurement errors and improve the quality of point cloud data.

[0035] Determine the size of the voxel grid and adjust it according to the size of the target object and the required accuracy. For example, for small target objects, the size of the voxel grid can be set to 0.01m, while for large scenes, the size of the voxel grid can be increased to 0.1m. Divide the point cloud data into corresponding voxel grids, calculate the center of mass of all points in each voxel grid, and replace all points in the voxel grid with the center of mass point, thereby achieving downsampling, reducing the amount of data, and improving the efficiency of subsequent processing.

[0036] After completing the voxel grid filtering, the curvature of each centroid point is further calculated. For areas with larger curvature (such as the edges and corners of the target object), the size of the voxel grid is reduced to retain more detail information. For flat areas with smaller curvature, the size of the voxel grid is increased to further streamline the point cloud data. Through this adaptive filtering method, the amount of point cloud data can be optimized while ensuring data accuracy.

[0037] The k-nearest-neighbor search algorithm is used to determine the k nearest neighbor points of each centroid point. The k value can be adjusted according to the density of the point cloud data and the complexity of the target object. For example, k=20 is set. Based on the determined k nearest neighbor points, the least squares method is used for local plane fitting to obtain the normal vector of the centroid point, which reflects the local geometric structure of the point cloud data. For each centroid point and its k nearest neighbor points, the covariance matrix is ​​calculated, and the eigenvalue decomposition of the covariance matrix is ​​performed to obtain three eigenvalues. The curvature of the centroid point is calculated based on the eigenvalues. The curvature reflects the local curvature of the point cloud data at the centroid point, reflecting the local geometric characteristics of the point cloud data.

[0038] By statistically analyzing the normal vectors and curvatures of all centroid points, global geometric features are extracted, such as calculating the average curvature of the entire point cloud data and the distribution histogram of the normal vector, etc., to provide more comprehensive information for the subsequent determination of the target position and target posture.

[0039] The extracted image features and point cloud features are matched, and the matching relationship is determined by calculating the similarity between the image features and the point cloud features to form a feature pair. The position and posture of the target object in the world coordinate system are calculated based on the matched feature pairs to obtain the target position and target posture. The above technical means are used to improve the accuracy and success rate of the robot's grasping tasks, reduce the number of grasping failures caused by inaccurate judgment of the target position and target posture, and enhance the robot's ability to operate in complex environments.

[0040] Furthermore, if Figure 3 As shown, the sub-logic for obtaining the target position and target posture based on the matched feature pairs includes: The similarity between image features and point cloud features is measured by Euclidean distance, and feature pairs are obtained by threshold comparison and screening; Read the intrinsic and extrinsic matrix of the camera in the visual sensor, unify the image coordinate system and the point cloud coordinate system, and obtain the world coordinate system; The rotation matrix and displacement vector of the target object are calculated by the PNP algorithm according to the matched feature pairs; The rotation matrix and displacement vector of the target object are converted to obtain the position and attitude of the target object in the world coordinate system, thereby obtaining the target position and target attitude.

[0041] The Euclidean distance can measure the distance between image features and point cloud features. The smaller the distance, the higher the similarity between the image features and the point cloud features. By setting a threshold, feature pairs with high similarity are screened out. These feature pairs contain the corresponding information of the target object in different data modes. The purpose of unifying the image coordinate system and the point cloud coordinate system is to calculate the position and posture under the same reference. The PNP algorithm is based on known feature pairs and camera parameters. By solving nonlinear optimization problems, the rotation matrix and displacement vector of the target object are obtained, thereby determining the position and posture of the target object in the world coordinate system.

[0042] For image features and point cloud features, they are represented as vectors, and the similarity between the two vectors is measured by calculating the Euclidean distance between them. A suitable threshold is set. For example, a threshold is determined after multiple experiments. Only feature pairs with a Euclidean distance less than the threshold are retained as valid feature pairs.

[0043] Read the intrinsic parameter matrix of the depth camera (including focal length) from the calibration file of the visual sensor , and the principal point coordinates , ) and the external parameter matrix (rotation matrix and displacement vector ), and the depth value corresponding to the image pixel coordinates In order to ensure the accuracy of the intrinsic and extrinsic matrix, a calibration plate of known size is used to verify the intrinsic and extrinsic matrix. The deviation between the projection of the calibration plate in the image and the actual size is calculated to fine-tune the intrinsic and extrinsic matrix. The image pixel coordinates are converted into Convert to the coordinates in the camera coordinate system , and then use the external parameter matrix to convert the coordinates in the camera coordinate system to the coordinates in the world coordinate system; for point cloud data, the external parameter matrix is ​​also used to convert it from the coordinates in the point cloud coordinate system Convert to coordinates in the world coordinate system to achieve the unification of the image coordinate system and the point cloud coordinate system.

[0044] Specifically, the image pixel coordinates are converted into The formula for converting to the coordinates in the camera coordinate system is: , then the formula for converting the coordinates in the camera coordinate system to the coordinates in the world coordinate system through the external parameter matrix is: Similarly, the formula for converting point cloud data from the coordinates in the point cloud coordinate system to the coordinates in the world coordinate system using the external parameter matrix is: .

[0045] According to the matched feature pairs, the coordinates of the feature pairs in the world coordinate system are substituted into the PNP algorithm. During the implementation of the PNP algorithm, the rotation matrix and displacement vector of the target object are continuously adjusted through iterative optimization methods to minimize the reprojection error between the point cloud feature points and the image feature points, and the final rotation matrix and displacement vector of the target object are obtained.

[0046] Convert the resulting rotation matrix to Euler angles , in order to intuitively represent the posture of the target object, combined with the displacement vector, the position of the target object in the world coordinate system is obtained and posture , complete the determination of target position and target attitude.

[0047] S3. Locate the current position of the robot arm, and generate a motion path of the robot arm from the current position to the target position by combining the target position and the target posture.

[0048] Specifically, the logic for generating the motion path of the robot arm from the current position to the target position includes: Locate the current position of the robot arm; Generate an initial motion path based on the current position and target position of the robot arm; Smooth and optimize the initial motion path based on the target pose.

[0049] Locating the current position of the robot arm is to provide a starting point for the planning of subsequent motion paths, and generating an initial motion path based on the current position and target position of the robot arm is to find a preliminary route from the starting point to the end point in the workspace of the robot arm. Smoothing and optimizing the initial motion path based on the target posture takes into account the need for the robot arm to remain stable during actual movement, avoiding sudden turns or jitters during movement, as well as obstacles in the surrounding environment, while ensuring that the end of the robot arm can reach the target position with a suitable posture to meet the robot arm's grasping task.

[0050] The encoders at the joints of the robot arm are used to read the joint angles in real time, including rotational joints and linear joints. The forward kinematics calculation is used to obtain the position and posture of the robot arm's end effector in the world coordinate system. At the same time, a laser tracker is used to regularly measure the actual position of the robot arm's end, which is compared and verified with the calculated position of the robot arm's end in the world coordinate system. When there is a deviation, the position of the robot arm is corrected to ensure the accuracy of the robot arm's positioning.

[0051] The working space of the robot arm is divided into voxels to construct a topological space. In the topological space, each voxel is a node, and adjacent nodes are connected by edges to form a topological graph. The shortest connected path from the node at the current position to the node at the target position is found in the topological graph. This path is the initial motion path.

[0052] Cubic spline interpolation is used to fit the nodes on the initial motion path to make the initial motion path smoother. Under the premise of maintaining the overall topological structure of the initial motion path unchanged, the sharp corners and mutation parts of the initial motion path are eliminated, so that the robot arm is more stable and smooth during the movement process, reducing the impact and loss on the robot arm; considering the target posture, by adjusting the posture direction of each node on the initial motion path, the robot arm can grasp the object with the correct posture when approaching the target object. At the same time, combined with the joint speed and acceleration limits of the robot arm, the initial motion path is optimized to ensure that the initial motion path is within the movable range of the robot arm.

[0053] Through the above steps, the grasping efficiency and accuracy of the robotic arm are improved, the impact and vibration during the movement of the robotic arm are reduced, the service life of the robotic arm is extended, and the automation level of the ROS system is improved.

[0054] Furthermore, the sub-logic of generating the initial motion path includes: Construct the topological space of the robot's grasping motion; Determine whether there is a continuous connected path from the current position node to the target position node of the robot arm; Search for the shortest connected path from the current position node to the target position node of the robot arm to obtain the initial motion path.

[0055] Constructing the topological space of the robot's grasping motion is to simplify the complex continuous workspace into a topological graph composed of discrete nodes and edges, which is convenient for path planning using graph search algorithms, to determine whether there is a continuous connected path, to ensure that the robot can reach the target position from the current position, and to search for the shortest connected path. On the basis of meeting accessibility, the movement distance of the robot is the shortest, thereby improving movement efficiency.

[0056] According to the size and accuracy requirements of the robot arm's workspace, it is divided into voxels of uniform size, and each voxel is assigned a unique number as a node identifier. For adjacent nodes (nodes with shared faces), edge connections are established, and a topological graph is constructed based on these adjacent relationships to intuitively present the topological structure of the workspace. At the same time, according to the location information of the obstacles, the nodes containing the obstacles are marked to make them inaccessible during path search.

[0057] Among them, obstacles are identified and segmented by the target detection algorithm on the image of the working environment, and the shape, position and size information of the obstacles are accurately obtained, and the relevant information of the obstacles is combined with the workspace. When constructing the topological map, in addition to the adjacent relationship between voxels, the relevant information of the obstacles is also integrated into it. For the areas identified as obstacles, the corresponding voxel nodes are marked as non-passable nodes in the topological map, so that the initial motion path around the obstacles can be generated.

[0058] A depth-first search algorithm is used to comprehensively traverse the constructed topological graph, starting from the node at the current position of the robot arm, to determine whether the node corresponding to the current position of the robot arm can reach the node corresponding to the target position. If the node corresponding to the target position is found during the search, there is a connected path. At this time, the node access order recorded during the search process can be backtracked to generate a continuous connected path; if the node corresponding to the target position is not found at the end of the search, there is no connected path. This is because there are obstacles blocking the workspace, or the target position is beyond the reachable range of the robot arm. It is necessary to rebuild the topological space of the robot arm's grasping motion, such as checking whether the position and size of the obstacle are accurate, whether there are new obstacles, and considering adjusting the target position so that the target object is within the reachable area of ​​the robot arm, and then re-judge the connected path.

[0059] After determining that there is a connected path from the current position of the robot to the target position, the Dijkstra algorithm is used to search for the shortest connected path in the topological graph. The shortest connected path is the initial motion path of the robot from the current position to the target position, which enables the robot to quickly plan the shortest passable motion path in a complex environment, improves the robot's grasping efficiency, and reduces the robot's movement time and energy consumption.

[0060] When the robot arm is executing the initial motion path, it detects changes in obstacles in real time. If a new obstacle is detected entering the motion path of the robot arm, the dynamic obstacle avoidance mechanism is immediately triggered. By determining the shape, position and size information of the new obstacle, the execution of the current motion path of the robot arm is suspended. Based on the relevant information of the new obstacle, the motion path is replanned. Taking the current position of the robot arm as the starting point, with the goal of avoiding new obstacles and reaching the target position, a new local motion path is quickly generated, so that the robot arm can continue to perform the task safely.

[0061] When an obstacle is detected and no feasible path around it can be found, the warning judgment mechanism is activated. For example, when the obstacle almost occupies the entire working space channel of the robot arm and the robot arm cannot avoid it by adjusting the joint angle or position, it is determined that the obstacle cannot be bypassed. Once it is determined that the obstacle cannot be bypassed, a warning message is generated. The warning information includes the location, shape, size, warning reason and task impact of the obstacle. For example, the warning message shows "at coordinates A large obstacle is detected at the robot arm. The size of the obstacle is (length L, width W, height H), which occupies the main movement channel of the robot arm. The current path planning cannot bypass it, which will cause the grasping task to fail." The warning information is sent to the display screen and the operator through the ROS system, and corresponding measures are taken, such as stopping the movement of the robot arm and removing or adjusting the obstacle.

[0062] S4. Convert the motion path into motion instructions for each joint of the robotic arm so as to grasp the target object through the robotic arm, verify whether the target object is successfully grasped and feed back to the ROS system.

[0063] Specifically, the logic of grabbing the target object by the robotic arm includes: Analyze the joint angles and joint trajectories of the robot arm on the motion path and generate analysis results; Generate motion instructions for each joint of the robot arm based on the analysis results; Control the robotic arm to grab the target object according to the motion instructions of each joint of the robotic arm; Verify whether the target object is successfully grasped and generate verification results; Feedback the verification results to the ROS system.

[0064] The movement of the robotic arm is achieved by the coordinated movement of each joint, so it is necessary to convert the planned motion path into motion instructions for each joint. The joint angles and joint trajectories are obtained by analyzing the motion path, and then motion instructions are generated based on this information to drive the robotic arm to the target position to grasp the object. After the grasping is completed, the grasping verification results are generated through visual verification and grasping force verification, and the results are fed back to the ROS system for subsequent decision-making, forming a complete closed-loop control process.

[0065] Furthermore, the sub-logic of generating the parsing result includes: Filter nodes from the motion path; The joint angles of each joint of the robot arm at each node are calculated through the inverse solution algorithm, and a joint angle sequence is generated; The joint angle sequence is interpolated through the interpolation algorithm to form the joint trajectory of the robot.

[0066] The motion path is the continuous trajectory of the end effector of the robot arm in space. To control the movement of each joint of the robot arm, it is necessary to convert it into changes in the joint angles of each joint. By screening the key posture points, calculating the corresponding joint angles, and then using the interpolation algorithm to fill in the intermediate states, a continuous joint trajectory is formed, providing an accurate control basis for the movement of the robot arm.

[0067] Filter each node on the generated motion path. Each node contains the position coordinates and posture information of the robot end effector in the world coordinate system. For each node, substitute the position coordinates and posture information contained in each node into the motion inverse solution algorithm to solve the corresponding joint angles of each joint of the robot. Since there may be multiple solutions for the inverse solution, select the solution that meets the actual situation by setting constraints such as the minimum and maximum rotation angles of the joint or whether it is within the physical limit of the joint. Store the calculated joint angles in the order of the nodes to obtain a joint angle sequence.

[0068] The screening of each node on the motion path can be determined according to the task requirements. For example, for high-speed grasping tasks, the node interval can be appropriately increased, while for high-precision grasping tasks, the node interval should be reduced. At the same time, the equal-interval screening method can be used to filter nodes according to a fixed distance or time interval. The adaptive screening method can also be used to dynamically adjust the screening interval according to the curvature and speed changes of the motion path, and add screening points where the motion path changes dramatically to ensure the accuracy of the motion.

[0069] A cubic spline interpolation algorithm is used to construct a cubic spline function based on the angle values ​​of the starting point, end point and intermediate point in the joint angle sequence. This function ensures that the function value (joint angle), first-order derivative (joint angular velocity) and second-order derivative (joint angular acceleration) are continuous at the interpolation point, making the joint movement smooth and obtaining the joint angles of each joint of the robotic arm at any time point, thereby forming a complete joint trajectory.

[0070] By rationally screening nodes, accurately calculating joint angles, and using cubic spline interpolation to generate smooth joint trajectories, the robotic arm can move accurately along the planned trajectory, improving the accuracy of target object grasping and the smoothness of the robotic arm's movement, reducing the positioning error of the target object, and improving the grasping efficiency and quality.

[0071] According to the maximum speed and maximum acceleration of each joint of the robot arm and the requirements of the grasping task, reasonable speed and acceleration are planned for each joint at different time points, and the joint angle, speed and acceleration information are encoded according to the communication protocol supported by the robot arm controller to generate motion instructions. The motion instructions are sent to the robot arm controller through the hardware interface to control the joints of the robot arm according to the motion instructions to achieve the grasping operation.

[0072] When the end effector of the robotic arm approaches the target object, the movement of the robotic arm is fine-tuned according to the target position and target posture acquired in real time to ensure that the robotic arm is accurately aligned with the target position. After reaching the target position, the robotic arm is controlled to perform a grasping action. For example, for a gripper-type end effector, the target object is grasped by controlling the degree of opening and closing and the strength of the gripper. The grasping strength is adjusted according to the material, shape and weight of the target object to avoid the object falling due to too loose grasping or damaging the object due to too tight grasping.

[0073] Furthermore, if Figure 4 As shown, the sub-logic for generating the verification result includes: Send a trigger signal to the visual sensor through the ROS system to capture the state image of the target object after it is grasped; Process the state image and extract the edge information of the target object and the gripper to determine whether the target object is in the gripper; Monitor the gripping force of the robot arm during the grasping process, and determine whether there is any abnormality in the robot arm through threshold comparison.

[0074] The visual sensor is used to obtain the image after grasping, and the image processing technology is used to analyze the relative position of the target object and the gripper to determine whether the grasping is successful. The force sensor is used to monitor the grasping force in real time and compare it with the preset threshold to determine whether there is any abnormality in the grasping process. The verification result is generated by combining the visual verification information and the force verification information.

[0075] When the robot arm grasps the target object, a trigger signal is sent to the visual sensor through the ROS system to capture the state image of the target object after being grasped, and the state image is grayed out to convert the color image into a grayscale image to reduce the amount of data processing. Gaussian filtering is used to remove noise from the state image to improve the quality of the state image. The canny edge detection algorithm is used to extract the edge contour of the target object and the gripper, and the edge is optimized through morphological operations (such as dilation and erosion). Then, the image data of the target object is matched with the state image after grasping, and the matching degree of the two is calculated. According to the preset matching degree threshold, it is judged whether the target object is in the gripper.

[0076] A force sensor is selected and installed at the point where the end effector of the robot arm contacts the target object, and the analog signal collected by the force sensor is converted into a digital signal and transmitted to the ROS system. In the ROS system, a reasonable force threshold range is set according to the characteristics of the target object (such as material, shape and weight) and the grasping requirements. The collected force signal is compared with the set force threshold in real time to determine whether the grasping force is within the normal range, thereby determining whether there is any abnormality in the robot arm.

[0077] The verification results of visual verification information and force verification information are integrated. If visual verification determines that the target object is in the gripper and force verification grasping force is within the normal range, the grasping is judged to be successful, otherwise the grasping is judged to be failed. Then the verification result (success or failure) and related status information (grasping force, target position and target posture, etc.) are encapsulated to form a feedback message to be fed back to the ROS system. According to the verification result, it is decided whether to re-plan the motion path of the robot arm for another grasping, or to execute the next operation process, which improves the reliability of the robot arm grasping the target object, reduces the drop and damage of the target object caused by grasping failure, and improves the operation efficiency of the robot arm.

Claims

1. The visual positioning and robotic arm grasping method based on the ROS system is characterized by: include: Start the ROS system, obtain the image data and point cloud data of the target object, add the timestamps of the image data and point cloud data of the target object, build a timestamp index table to achieve data synchronization and alignment, and generate matching pairs; receiving a matching pair, processing image data of the target object, extracting image features of the target object, matching point cloud features of point cloud data of the target object to form a feature pair, and determining a target position and a target posture based on the matched feature pair; Locate the current position of the robot arm, generate an initial motion path based on the target position, and generate a motion path from the current position to the target position in combination with the target posture; The motion path is converted into motion instructions for each joint of the robotic arm so that the target object can be grasped by the robotic arm. It is verified whether the target object is successfully grasped and fed back to the ROS system.

2. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 1, characterized in that: The acquisition logic of the image data and point cloud data of the target object includes: Start the ROS system and automatically identify the connected visual sensors and robotic arms; Acquire image data and point cloud data of target objects through visual sensors; Perform data synchronization and alignment on the image data and point cloud data of the target object.

3. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 2, characterized in that: The data synchronization and alignment sub-logic includes: Add timestamps to the image data and point cloud data of the target object through the ROS system, and build a timestamp index table; Configure the time error threshold, traverse the timestamp index table, calculate the absolute value of the difference between the timestamps of adjacent image data and point cloud data, and determine whether the image data and point cloud data match; The matched image data and point cloud data are stored in the data storage area and marked as matching pairs, while the unmatched image data and point cloud data are discarded.

4. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 1, characterized in that: The determination logic of the target position and target posture includes: receiving a matching pair, processing image data of the target object, and extracting image features of the target object; Process the point cloud data of the target object and extract the point cloud features of the target object; The image features and point cloud features are matched to form feature pairs, and the position and posture of the target object in the world coordinate system are calculated based on the matched feature pairs to obtain the target position and target posture.

5. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 4, characterized in that: The sub-logic of obtaining the target position and target posture based on the matched feature pair includes: The similarity between image features and point cloud features is measured by Euclidean distance, and feature pairs are obtained by threshold comparison and screening; Read the intrinsic and extrinsic matrix of the camera in the visual sensor, unify the image coordinate system and the point cloud coordinate system, and obtain the world coordinate system; The rotation matrix and displacement vector of the target object are calculated by the PNP algorithm according to the matched feature pairs; The rotation matrix and displacement vector of the target object are converted to obtain the position and attitude of the target object in the world coordinate system, thereby obtaining the target position and target attitude.

6. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 1, characterized in that: The logic for generating the motion path of the robot arm from the current position to the target position includes: Locate the current position of the robot arm; Generate an initial motion path based on the current position and target position of the robot arm; Smooth and optimize the initial motion path based on the target pose.

7. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 6, characterized in that: The generation sub-logic of the initial motion path includes: Construct the topological space of the robot's grasping motion; Determine whether there is a continuous connected path from the current position node to the target position node of the robot arm; Search for the shortest connected path from the current position node to the target position node of the robot arm to obtain the initial motion path.

8. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 1, characterized in that: The logic of grabbing the target object by the robotic arm includes: Analyze the joint angles and joint trajectories of the robot arm on the motion path and generate analysis results; Generate motion instructions for each joint of the robot arm based on the analysis results; Control the robotic arm to grab the target object according to the motion instructions of each joint of the robotic arm; Verify whether the target object is successfully grasped and generate verification results; Feedback the verification results to the ROS system.

9. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 8, characterized in that: The generation sub-logic of the analysis result includes: Filter nodes from the motion path; The joint angles of each joint of the robot arm at each node are calculated through the inverse solution algorithm, and a joint angle sequence is generated; The joint angle sequence is interpolated through the interpolation algorithm to form the joint trajectory of the robot.

10. The visual positioning and robotic arm grasping method based on the ROS system as claimed in claim 9, characterized in that: The generation sub-logic of the verification result includes: Send a trigger signal to the visual sensor through the ROS system to capture the state image of the target object after it is grasped; Process the state image and extract the edge information of the target object and the gripper to determine whether the target object is in the gripper; Monitor the gripping force of the robot arm during the grasping process, and determine whether there is any abnormality in the robot arm through threshold comparison.

Images