Driving methods for the end effector of a robotic arm

Abstract

This invention relates to a driving method for the end effector of a robotic arm, and pertains to the field of robot control technology. The invention aims to address the problem of existing robots' inability to achieve omnidirectional information perception of objects. The invention involves scanning an object to construct a point cloud in the object's camera coordinate system and transforming it to the world coordinate system; reconstructing the point cloud in the world coordinate system; slicing the reconstructed point cloud; calculating the angle of each point in each slice relative to its centroid; dividing the sorted angles into intervals using the minimum angle increment that satisfies the preset point cloud spacing; using the average point and average normal of the point cloud within each interval as the trajectory points of that interval; and using all trajectory points of all slices to constitute the object's point cloud trajectory; and using the object's point cloud trajectory as the reference trajectory for the end effector of the robotic arm, thereby achieving the driving of the end effector.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology. Background Technology

[0002] With the rapid development of technologies such as robotics, VR, and AR, and the ever-expanding demands for application scenarios, high-quality multimodal datasets have enormous application potential. Among various sensory modalities, tactile perception plays an irreplaceable role. For robots, tactile perception is an indispensable supplement to visual and auditory modalities. However, current robot data collection for objects is mostly limited to a few modalities and low density, making multimodal object datasets incorporating tactile perception extremely scarce. This is because tactile perception requires omnidirectional perception of objects, which current robots struggle to achieve. Summary of the Invention

[0003] This invention aims to solve the problem that existing robots have difficulty in achieving omnidirectional information perception of objects, and provides a driving method for the end effector of a robot arm.

[0004] The driving methods for the end effector of a robotic arm include:

[0005] The robotic arm's end effector circles the object once to acquire a depth image of the object. The coordinates of each pixel in the depth image are calculated in the camera coordinate system. All coordinates in the depth images constitute a point cloud in the camera coordinate system. Camera The pose matrix Pose is compared with the point cloud. Camera Multiply to obtain the original point cloud in the world coordinate system. world ;

[0006] For the original point cloud world The clean point cloud is extracted, denoised, and smoothed in sequence. The center of the processed point cloud is calculated and the normals of all points are made to face outwards from the point cloud, thus obtaining the reconstructed point cloud.

[0007] The reconstructed point cloud is sliced, and the average position of all points in each slice is taken as the centroid of the slice. The angle of each point in each slice relative to its centroid is calculated and sorted. The sorted angles are divided into intervals using the minimum angle increment that can satisfy the preset point cloud spacing. The average point and average normal of the point cloud in each interval are taken as the trajectory points of that interval. All trajectory points of all slices constitute the object point cloud trajectory.

[0008] The point cloud trajectory of the object is used as a reference trajectory for the end effector of the robot arm, thereby enabling the robot arm end effector to be driven.

[0009] Furthermore, the above-mentioned use of the object point cloud trajectory as a reference trajectory for the robot arm's end effector, thereby achieving the driving of the robot arm's end effector, includes:

[0010] As the end effector of the robot arm moves according to the reference trajectory, the force vectors [f] at each position point of the end effector are collected in real time. x ,f y ,f z ], and calculate the real-time reference error current_error based on the force threshold;

[0011] The scaling factor is determined based on the real-time reference error current_error, and then the control signal control_signal is calculated using the following formula:

[0012] control_signal = K p ×current_error×scaling factor+K d ×(current_error-previous_error),

[0013] Among them, K p For proportional gain, K d The differential gain is given by the variable, and previous_error is the reference error from the previous time step.

[0014] The control signal is converted into displacement, and the position of the next target point at the end of the robot arm is calculated based on the displacement.

[0015] Furthermore, the determination of the scaling factor value based on the real-time reference error current_error includes:

[0016] When |current_error| < 0.1, the scaling factor = 0.5;

[0017] When 0.1 ≤ |current_error| < 0.2, the scaling factor = 1.5;

[0018] When 0.2≤|current_error|<0.3, scaling factor=1.8;

[0019] When 0.3 ≤ |current_error| < 0.5, the scaling factor = 2;

[0020] When |current_error|≤0.1 or |current_error|≥0.5, the scaling factor = 0.8.

[0021] Furthermore, the above calculation of the real-time reference error current_error based on the force threshold includes:

[0022] current'_error=-f z -f z _threshold,

[0023] current_error=min(current′_error,max_error),

[0024] Where current′_error is the force deviation, f z _threshold is the force threshold, max_error is the maximum error, and current_error is the real-time reference error.

[0025] Furthermore, the above-mentioned conversion of the control signal (control_signal) into displacement includes:

[0026]

[0027] Where displacement_per_0_1N is the distance that should be moved in the opposite direction of TCP after each 0.1N force deviation.

[0028] Furthermore, the calculation of the position of the next target point at the end of the robot arm based on displacement includes:

[0029] adjusted_position=position-displacement×tcp_z_direction,

[0030] Where tcp_z_direction is the direction vector, adjusted_position is the adjusted position of the next target point, and position is the reference position of the next target point in the reference trajectory.

[0031] Furthermore, the coordinates of each pixel in the depth image in the camera coordinate system, as described above, include:

[0032] The coordinates z of each pixel in the depth image in the camera coordinate system are calculated according to the following formula:

[0033] z=depth_image[i,j]×depth_scale,

[0034] Where depth_image[i,j] is the original depth value of pixel (i,j) in the depth image, and depth_scale is the depth scaling factor;

[0035] Retain pixels with z < 0.05 < z < 0.4, and calculate the x and y coordinates of the retained pixels in the camera coordinate system according to the following formula:

[0036]

[0037] Among them, g x and g y These are the camera focal lengths along the X and Y axes in the camera coordinate system, respectively. (c) x ,c y ) represents the coordinates of the center point of the depth image.

[0038] Furthermore, the aforementioned process of driving the robotic arm's end effector to circle the object and acquire a depth image of the object includes:

[0039] The robotic arm is driven to circle the object once, capturing five depth images of the object every 45 degrees.

[0040] Furthermore, the above describes the original point cloud. world The clean point cloud is extracted, denoised, and smoothed sequentially, including:

[0041] Remove the original point cloud world Points at heights lower than the object's height are then uniformly downsampled, and DBSCAN is used to separate the pure point cloud of the object being measured.

[0042] The clean point cloud is subjected to statistical noise removal, and then points with local density less than a threshold are removed using the radius outlier value. After that, the normal is estimated through the local neighborhood, and the average normal of the neighborhood of each point is used as its new normal to achieve smoothing.

[0043] Furthermore, the method for obtaining the minimum angle increment mentioned above includes:

[0044] The minimum angle increment that can satisfy the preset point cloud spacing is found through iteration.

[0045] The robotic arm end-effector driving method described in this invention can achieve accurate point cloud reconstruction and multimodal data scanning of objects, with a high level of automation, improving the efficiency of multimodal perception data acquisition of objects, solving the shortcomings of existing technologies that rely too much on manual labor and cannot achieve simultaneous acquisition of multiple modalities, and has low cost and is easy to implement. Attached Figure Description

[0046] Figure 1 This is a flowchart illustrating the overall process from raw data to the final point cloud of an object.

[0047] Figure 2 Flowchart of the control strategy for a single-lap trajectory;

[0048] Figure 3 This is the overall system flowchart. Detailed Implementation

[0049] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0050] Reference Figures 1 to 3 This embodiment describes the robot's end effector in detail. It includes a 3D-printed bracket connected to the robot arm flange, an RGB-D camera, an accelerometer, and a temperature sensor mounted on the bracket, and a microphone mounted on the robot arm. The 3D-printed bracket consists of a base and a fingertip. The base directly contacts the robot arm flange and is secured with four screws. The fingertip is fixed to the upper middle position of the 3D-printed bracket with two screws. The RGB-D camera is fixed to the middle of the 3D-printed bracket with three screws. The accelerometer is attached to the bottom of the fingertip, and the temperature sensor is fixed to the top of the 3D-printed bracket with screws. The microphone is mounted on the robot arm near the 3D-printed bracket.

[0051] The 3D-printed bracket design allows for quick adaptation and installation on any robot, enabling a multimodal perception system. The bracket design integrates sensors of various modalities, including vision, touch, and temperature, and has a certain degree of scalability, allowing for easy design adjustments to replace or add more types of sensors.

[0052] Through robot control and multimodal data acquisition and processing algorithms, it is possible to automatically acquire depth images using a consumer-grade depth camera and reconstruct accurate object point clouds, and then interact with the object surface to synchronously acquire multimodal data.

[0053] Equipped with an RGB-D camera, accelerometer, and temperature sensor, it can simultaneously collect multimodal information during the interaction between the tip and the object, directly mapping the multimodal data to the point cloud location information to comprehensively capture the multimodal attributes of the object.

[0054] The driving method for the end effector of the robot arm described in this embodiment includes:

[0055] 1. Drive the RGB-D camera to circle the perceived object, and acquire 5 depth images every 45 degrees. Using the depth map and the robot pose acquired and saved simultaneously, all point clouds can be converted to the world coordinate system and stacked together.

[0056] 2. For any pixel (i,j) in the depth image, its coordinate z in the camera coordinate system is calculated using the following formula:

[0057] z=depth_image[i,j]×depth_scale,

[0058] Where depth_image[i,j] is the original depth value of pixel (i,j) in the depth image, and depth_scale is the depth scaling factor used to convert the unit of the depth image from millimeters to meters.

[0059] The coordinate point is retained only when 0.05 < z < 0.4, i.e., within the actual workspace that needs to be considered. For these retained coordinate points, their x and y coordinates are determined by the following formula:

[0060]

[0061] Among them, g x and g y These are the camera focal lengths along the X and Y axes in the camera coordinate system, respectively. x ,c y ) are the coordinates of the center point of the depth image.

[0062] Based on the above method, the coordinates of all pixels in each acquired depth image in the camera coordinate system are obtained, and a point cloud in the camera coordinate system is constructed. Camera The original point cloud in the world coordinate system is calculated according to the following formula. world :

[0063]

[0064] Pose is the pose matrix corresponding to the depth image, calculated from the flange pose given by the robotic arm and the relevant parameters of the support and depth camera.

[0065] 3. Convert the original point cloud world Points below the height of the object are removed by uniform downsampling using `uniform_down_sample` (a function in Open3D used to sample point clouds at uniform intervals), followed by `DBSCAN` (an Open3D function that performs density-based clustering to extract clusters of points with good connectivity and remove isolated points) to separate the clean point cloud of the object. Preliminary statistical noise removal is then performed using `remove_statistical_outlier` (an Open3D function that removes statistical noise by statistically analyzing the distribution of points in the neighborhood of the point cloud and removing outliers that do not conform to statistical laws). Finally, `remove_radius_outlier` (an Open3D function that removes outliers by radius, determining whether a point is an outlier by counting the number of points within a fixed radius) uses the radius outlier value to remove points with local density less than a threshold. The `estimate_normals` function (a function in Open3D used for normal estimation, calculating the normal direction of each point in the point cloud) and `orient_normals_consistent_tangent_plane` (a function in Open3D used to unify the normal orientation, ensuring the normal direction of the point cloud is consistent, resulting in a consistent inward or outward orientation) estimate the normals through local neighborhoods, and then use the average normal of a point's neighborhood as its new normal to achieve simple smoothing. Due to the needs of subsequent robotic arm operations, the center of the point cloud is calculated, and `orient_normals_towards_camera_location` (a function in Open3D that orients the normals toward the camera position, adjusting all normal directions to face the camera) is used to make the normals of all points face outwards from the point cloud. The `create_from_point_cloud_poisson` function (a function in Open3D that performs Poisson reconstruction based on the point cloud, generating a smooth triangular mesh model from the point cloud and its normals) reconstructs a complete and smooth surface and normals of the measured object. The final point cloud is sampled from the reconstructed mesh as the reconstructed point cloud.

[0066] Fourth, after obtaining the reconstructed point cloud, it is also necessary to segment the trajectory for the robotic arm to perform motion and contour tracking.

[0067] The height range of the reconstructed point cloud is obtained, and the reconstructed point cloud is divided into multiple slices from low to high, with the height of each slice being a preset slice interval. The average position of all points in each slice is calculated as the slice centroid, and the angle of each point in the slice relative to the centroid is calculated. Based on these angles, the slice is further subdivided into 160-360 degrees and 0-160 degrees to ensure that the generated point cloud is arranged in a certain spatial order for easy sequential movement by the robotic arm. An initial angle increment is selected, and the minimum angle increment that satisfies the preset point cloud spacing is found through iteration. Under this minimum angle increment, the average point and average normal of the point cloud within each angle interval are calculated as the trajectory points of that interval, and finally, an object point cloud trajectory that meets the set point cloud spacing is generated.

[0068] 5. The robotic arm slides along the point cloud trajectory of the object to achieve all-round information perception of the object.

[0069] After obtaining the point cloud trajectory of the object, the robotic arm continuously tracks the trajectory for each revolution and collects data synchronously. The new trajectory points are then calculated and adjusted based on the actual pose and forces. The specific process is as follows:

[0070] Let the force vector at the current position of the robot arm's end effector be [f]. x ,f y ,f z ]. f z `_threshold` is the force threshold used to determine whether adjustment is needed. Calculate `f`. z The deviation from the threshold is truncated to the set maximum value, and the formula is as follows:

[0071] current'_error=-f z -f z _threshold,

[0072] current_error=min(current′_error,max_error),

[0073] Where current'_error is the force deviation, max_error is the maximum error, and current_error is the real-time reference error.

[0074] displacement_per_0_1N is the distance that should be moved in the opposite direction of TCP (Tool Center Point) after each 0.1N force deviation. K p For proportional gain, K dHere, represents the differential gain, and `previous_error` is the reference error from the previous time step. The control signal `control_signal` is calculated by applying different scaling factors to segments based on the magnitude of the real-time reference error `current_error`, using the following formula:

[0075] control_signal = K p ×current_error×scaling factor+K d ×(current_error-previous_error).

[0076] When |current_error| < 0.1, the control signal control_signal is adjusted according to 50% of the error, that is, the scaling factor is set to 0.5. This adjustment ensures that the adjustment is smoother when the error is small, and avoids over-adjustment.

[0077] When 0.1 ≤ |current_error| < 0.2, the scaling factor is set to 1.5.

[0078] When 0.2 ≤ |current_error| < 0.3, the scaling factor is set to 1.8.

[0079] When 0.3 ≤ |current_error| < 0.5, the scaling factor is set to 2. This level of adjustment is used to quickly correct large deviations.

[0080] If none of the above conditions are met, i.e., |current_error|≤0.1 or |current_error|≥0.5, the scaling factor is set to 0.8. This usually indicates a large external force, and the control signal strength needs to be reduced to prevent potential over-adjustment.

[0081] The control signal (control_signal) is converted into displacement using the following formula:

[0082]

[0083] Finally, the position of the next target point is adjusted based on the calculated displacement, using the following formula:

[0084] adjusted_position=position-displacement×tcp_z_direction,

[0085] Where tcp_z_direction is the direction vector, adjusted_position is the adjusted position of the next target point, and position is the reference position of the next target point in the reference trajectory.

[0086] This gives us the position of the adjusted point, while the direction of the normal remains unchanged.

[0087] The robotic arm end effector driving method described in this embodiment can be quickly deployed on any existing robotic arm. As a multimodal perception data acquisition device for objects, it has broad application prospects. The following are some potential application scenarios.

[0088] 1. Education and Research:

[0089] In scientific experiments and education, this component can be installed on educational robots to help students learn robotics and data acquisition techniques, enhancing the interactivity and practicality of learning. This component can also directly perceive objects and collect multimodal interaction data to obtain virtual objects, enabling rich applications in environments such as virtual reality (VR) and the metaverse. Through this technology, users can not only see replicas of objects in the virtual world but also experience real-time tactile, temperature, and other sensory data when interacting with these objects. This functionality greatly enhances the immersion and interactivity of the virtual reality experience, providing users with a completely new and highly interactive virtual environment.

[0090] 2. Industrial Automation:

[0091] On the production line, this component can be installed at the end of a robotic arm for automated product inspection and sorting. Using an RGB-D camera, the robot can identify the size, shape, and location of items for precise handling and sorting. In quality control, integrated sensors for temperature, acceleration, and other parameters enable the robot to perform real-time interactive inspections of products, identify potential defects during processing, and check whether their materials, hardness, and other properties meet standards.

[0092] 3. Medical Care and Health:

[0093] Used in surgical assistance systems, this component can assist in delicate surgical procedures, such as cutting and suturing in minimally invasive surgery. The RGB-D camera provides precise visual information, helping doctors or robots identify surgical areas and critical structures, while the pointed tactile sensor helps assess tissue texture and response, enhancing surgical precision and safety. In rehabilitation centers, this component can be used to monitor a patient's range of motion and intensity. By collecting tactile feedback during movement through its pointed tip, it helps doctors assess patient progress and adjust treatment plans.

[0094] 4. Service industry:

[0095] In retail environments, robots can use this end-effector to interact with customers, enhancing their shopping experience and providing product information or guidance. In museums or exhibitions, the device can also be used to offer interactive learning experiences.

[0096] 5. Security monitoring:

[0097] In hazardous environments such as chemical plants or nuclear facilities, robots can carry this component into areas inaccessible to personnel to perform detection and monitoring that interact with the environment, conduct real-time assessments of environmental conditions through multimodal perception, and respond quickly to emergencies.

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

Claims

1. A driving method for the end effector of a robot arm, characterized in that, include: The robotic arm's end effector orbits the object once to acquire a depth image of the object. The coordinates of each pixel in the depth image are calculated in the camera coordinate system. All coordinates in all depth images constitute a point cloud in the camera coordinate system. Camera The pose matrix Pose is compared with the point cloud. Camera Multiply to obtain the original point cloud in the world coordinate system. world ; For the original point cloud world The clean point cloud is extracted, denoised and smoothed in sequence. The center of the processed point cloud is calculated and the normals of all points are made to face the outside of the point cloud, so as to obtain the reconstructed point cloud. The reconstructed point cloud is sliced, and the average position of all points in each slice is taken as the centroid of the slice. The angle of each point in each slice relative to its centroid is calculated and sorted. The sorted angles are divided into intervals using the minimum angle increment that can satisfy the preset point cloud spacing. The average point and average normal of the point cloud in each interval are taken as the trajectory points of that interval. All trajectory points of all slices constitute the object point cloud trajectory. The point cloud trajectory of the object is used as a reference trajectory for the end effector of the robot arm, thereby enabling the robot arm end effector to be driven.

2. The driving method for the end effector of a robot arm according to claim 1, characterized in that, The step of using the point cloud trajectory of the object as a reference trajectory for the end effector of the robot arm, thereby driving the end effector of the robot arm, includes: As the end effector of the robot arm moves according to the reference trajectory, the force vectors [f] at each position point of the end effector are collected in real time. x ,f y ,f z ], and calculate the real-time reference error current_error based on the force threshold; The scaling factor is determined based on the real-time reference error current_error, and then the control signal control_signal is calculated using the following formula: control_signal=K p ×current_error×scaling factor+K d ×(current_error-previous_error), Among them, K p For proportional gain, K d The differential gain is given by the variable, and previous_error is the reference error from the previous time step. The control signal is converted into displacement, and the position of the next target point at the end of the robot arm is calculated based on the displacement.

3. The driving method for the end effector of a robot arm according to claim 2, characterized in that, The step of determining the value of the scaling factor based on the real-time reference error current_error includes: When |current_error| < 0.1, the scaling factor = 0.5; When 0.1 ≤ |current_error| < 0.2, the scaling factor = 1.5; When 0.2≤|current_error|<0.3, scaling factor=1.8; When 0.3 ≤ |current_error| < 0.5, the scaling factor = 2; When |current_error|≤0.1 or |current_error|≥0.5, the scaling factor = 0.

8.

4. The driving method for the end effector of a robot arm according to claim 3, characterized in that, The calculation of the real-time reference error current_error based on the force threshold includes: current′_error=-f z -f z _threshold, current_error=min(current′_error,max_error), Where current′_error is the force deviation, f z _threshold is the force threshold, max_error is the maximum error, and current_error is the real-time reference error.

5. The driving method for the end effector of a robot arm according to claim 3 or 4, characterized in that, The step of converting the control signal (control_signal) into displacement includes: Where displacement_per_0_1N is the distance that should be moved in the opposite direction of TCP after each 0.1N force deviation.

6. The driving method for the end effector of a robot arm according to claim 5, characterized in that, The step of calculating the position of the next target point at the end of the robot arm based on displacement includes: adjusted_position=position-displacement×tcp_z_direction, Where tcp_z_direction is the direction vector, adjusted_position is the adjusted position of the next target point, and position is the reference position of the next target point in the reference trajectory.

7. The driving method for the end effector of a robot arm according to claim 1, 2, 3, 4 or 6, characterized in that, The coordinates of each pixel in the depth image in the camera coordinate system include: The coordinates z of each pixel in the depth image in the camera coordinate system are calculated according to the following formula: z=depth_image[i,j]×depth_scale, Where depth_image[i,j] is the original depth value of pixel (i,j) in the depth image, and depth_scale is the depth scaling factor; Retain pixels with z < 0.05 < z < 0.4, and calculate the x and y coordinates of the retained pixels in the camera coordinate system according to the following formula: Among them, g x y and gy are the camera focal lengths along the X and Y axes in the camera coordinate system, respectively, and (cx, cy) are the coordinates of the center point of the depth image.

8. The driving method for the end effector of a robot arm according to claim 7, characterized in that, The robotic arm's end effector circles the object once to acquire a depth image of the object, including: The robotic arm is driven to circle the object once, capturing five depth images of the object every 45 degrees.

9. The driving method for the end effector of a robot arm according to claim 8, characterized in that, The original point cloud world The clean point cloud is extracted, denoised, and smoothed sequentially, including: Remove the original point cloud world Points at heights lower than the object's height are then uniformly downsampled, and DBSCAN is used to separate the pure point cloud of the object being measured. The clean point cloud is subjected to statistical noise removal, and then points with local density less than a threshold are removed using the radius outlier value. After that, the normal is estimated through the local neighborhood, and the average normal of the neighborhood of each point is used as its new normal to achieve smoothing.

10. The driving method for the end effector of a robot arm according to claim 9, characterized in that, The method for obtaining the minimum angle increment includes: The minimum angle increment that can satisfy the preset point cloud spacing is found through iteration.

Images