Method for driving tail end of mechanical arm of robot

Abstract

The invention discloses a robot mechanical arm tail end driving method and relates to the technical field of robot control. The invention aims to solve the problem that an existing robot is difficult to realize omnibearing information perception of an article. The method comprises the following steps: scanning an article to construct a point cloud under an article camera coordinate system and converting the point cloud into a world coordinate system; reconstructing the point cloud under the world coordinate system; slicing the reconstructed point cloud, calculating the angle of each point in each slice relative to the centroid of the point, performing interval division on the sorted angles by using the minimum angle increment capable of meeting the preset point cloud spacing, and taking the average point and the average normal of the point cloud in each interval as the track point of the interval; all the track points of all the slices form an object point cloud track; and the object point cloud track serves as a reference track of the tail end of the robot mechanical arm, and then driving of the tail end of the robot mechanical arm is achieved.

Description

Technical Field

[0001] The invention belongs to the technical field of robot control. Background Art

[0002] With the rapid development of technologies such as robots, VR, and AR, and the ever-expanding demand for application scenarios, high-quality multimodal datasets have huge application potential, and among various perception modalities, touch plays an irreplaceable role. For robots, touch is an indispensable supplementary perception to modalities such as vision and hearing. However, the data collection of objects by existing robots is still mostly at a low-modality, low-density stage, and multimodal object datasets that integrate touch are very scarce. This is because tactile perception requires all-round perception of objects, but existing robots find it difficult to achieve all-round perception of objects. Summary of the invention

[0003] The present invention aims to solve the problem that existing robots are difficult to realize all-round information perception of objects, and now provides a driving method for the end of a robot arm.

[0004] A driving method for the end of a robot arm, comprising:

[0005] Drive the end of the robot arm around the object to collect the depth image of the object, calculate the coordinates of each pixel in the depth image in the camera coordinate system, and all coordinates in all depth images constitute a point cloud in the camera coordinate system. Camera , the pose matrix Pose and the point cloud Pointcloud Camera Multiply to get the original point cloud Pointcloud in the world coordinate system world ;

[0006] For the original point cloud Pointcloud world Perform pure point cloud extraction, denoising and smoothing processing in sequence, calculate the center of the processed point cloud and make the normals of all points face the outside of the point cloud, so as to obtain the reconstructed point cloud;

[0007] Slicing the reconstructed point cloud, taking the average position of all points in each slice as the centroid of the slice, calculating the angle of each point in each slice relative to its centroid, and sorting the angles, dividing the sorted angles into intervals using the minimum angle increment that can meet the preset point cloud spacing, taking the average point and average normal of the point cloud in each interval as the trajectory point of the interval, and all trajectory points of all slices constitute the point cloud trajectory of the object;

[0008] The object point cloud trajectory is used as a reference trajectory of the end of the robot mechanical arm, thereby realizing the driving of the end of the robot mechanical arm.

[0009] Furthermore, the above-mentioned using the object point cloud trajectory as a reference trajectory of the end of the robot mechanical arm, and then realizing the driving of the end of the robot mechanical arm, includes:

[0010] When the end of the robot arm moves according to the reference trajectory, the force vector [f x ,f y ,f z ], and calculate the real-time reference error current_error according to the force threshold;

[0011] The value of the control gain scaling factor is determined according to the real-time reference error current_error, and then the control signal control_signal is calculated according to the following formula:

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

[0013] Among them, K p is the proportional gain, K d is the differential gain, previous_error is the reference error at the previous moment;

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

[0015] Furthermore, the above-mentioned determination of the value of the control gain scaling factor according to the real-time reference error current_error includes:

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

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

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

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

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

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

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

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

[0024] Among them, 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 converting the control signal control_signal into the displacement comprises:

[0026]

[0027] Among them, 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 above-mentioned calculation of the position of the next target point at the end of the robot arm according to the displacement includes:

[0029] adjusted_position=position-displacement×tcp_z_direction,

[0030] Among them, 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 above calculation of the coordinates of each pixel in the depth image in the camera coordinate system includes:

[0032] The coordinate z of each pixel in the depth image in the camera coordinate system is 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] Keep the pixels where 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 are the camera focal lengths of the X and Y axes in the camera coordinate system, respectively. x ,c y ) is the center point coordinate of the depth image.

[0038] Furthermore, the above-mentioned driving the end of the robot arm to circle the object once to collect the depth image of the object includes:

[0039] Drive the end of the robot arm around the object and collect 5 depth images of the object every 45 degrees.

[0040] Furthermore, the above-mentioned original point cloud Pointcloud world Perform clean point cloud extraction, denoising and smoothing in sequence, including:

[0041] Remove the original point cloud Pointcloud world The points with medium height lower than the height of the object are uniformly downsampled, and then the pure point cloud of the object is separated using DBSCAN;

[0042] Statistical noise is removed from the pure point cloud, and then the points whose local density is less than a threshold are removed using the radius outlier. The normal is then 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 includes:

[0044] The minimum angle increment that can meet the preset point cloud spacing is found in an iterative manner.

[0045] The driving method of the end of the robot manipulator arm described in the present invention can realize accurate point cloud reconstruction and multimodal data scanning of objects, has a high level of automation, improves the efficiency of multimodal perception data collection of objects, solves the shortcomings of the existing technology that is too dependent on manual labor and cannot realize simultaneous collection of multiple modalities, and has low cost and is easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 The overall flow chart from raw data to the final point cloud of the object;

[0047] Figure 2 This is the control strategy flow chart for a single-turn trajectory;

[0048] Figure 3 This is the overall flow chart of the system. DETAILED DESCRIPTION

[0049] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0050] Reference Figures 1 to 3 Specifically describing this embodiment, the end portion of the robot of this embodiment includes a 3D printed bracket connected to the flange of the robot mechanical arm, an RGB-D camera, an acceleration sensor and a temperature sensor installed on the 3D printed bracket, and a microphone installed on the mechanical arm. The 3D printed bracket is divided into two parts: a base and a fingertip. The base is directly in contact with the flange of the mechanical arm and is fixed 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 acceleration sensor is glued 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 installed on the mechanical arm near the 3D printed bracket.

[0051] Through the design of the 3D printed bracket, it can be quickly adapted and installed on any robot, thus realizing a multimodal perception system. The bracket design integrates sensors of multiple modalities such as vision, touch, and temperature, and has certain scalability. More types of sensors can be replaced or added by simply adjusting the design.

[0052] Through robot control and multimodal data acquisition and processing algorithms, consumer-grade depth cameras can be used to automatically capture depth images and reconstruct accurate object point clouds, and on this basis, interact with the object surface to synchronously collect multimodal data.

[0053] By installing an RGB-D camera, an acceleration sensor and a temperature sensor, it is possible to synchronously collect multimodal information during the interaction between the tip and the object, directly correspond the multimodal data to the point cloud position information one by one, and comprehensively capture the multimodal properties of the object.

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

[0055] 1. Drive the RGB-D camera around the perceived object and collect 5 depth images every 45 degrees. Use the depth map and the saved robot pose to convert all point clouds to the world coordinate system and stack them together.

[0056] 2. For any pixel (i, j) in the depth image, its coordinate z in the camera coordinate system is calculated by 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 units of the depth image from millimeters to meters.

[0059] Only when 0.05<z<0.4, that is, in the actual working space to be considered, the coordinate point is retained. For these retained coordinate points, their coordinates x and y are determined by the following formula:

[0060]

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

[0062] According to the above method, the coordinates of all pixels in each depth image collected in the camera coordinate system are obtained, and the point cloud Pointcloud in the camera coordinate system is constructed. Camera . Calculate the original point cloud Pointcloud in the world coordinate system according to the following formula world :

[0063]

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

[0065] 3. Original point cloud Pointcloud world Points below the height of the object are removed, uniform down sampling is performed using uniform_down_sample (a related function in open3d, used to sample the point cloud at uniform intervals), and then DBSCAN (a related function in open3d, which is based on density clustering, that is, density clustering of point cloud data, extracting point cloud groups with good connectivity, and removing isolated points) is used to separate the pure point cloud of the object being measured. Remove_statistical_outlier (a related function in open3d, which applies statistical noise removal, removes outliers that do not conform to statistical laws by statistically analyzing the distribution of points in the neighborhood of the point cloud) is used for preliminary statistical noise removal, and then remove_radius_outlier (a related function in open3d, which applies radius outlier removal, and determines whether a point is an outlier by the number of points within a fixed radius) is used to remove points whose local density is less than the threshold using the radius outlier value. Use estimate_normals (a related function in open3d, used for normal estimation, to calculate the normal direction of each point in the point cloud) and orient_normals_consistent_tangent_plane (a related function in open3d, used to unify the normal direction, so that the normal direction of the point cloud is consistent, and after implementation, it is consistent inward or outward) to estimate the normal through the local neighborhood, and use the average normal of the neighborhood of a certain point 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 related function in open3d, which turns the normal toward the camera position and adjusts all normal directions to the direction facing the camera) is used to make the normals of all points face the outside of the point cloud. Use create_from_point_cloud_poisson (a related function in open3d, based on Poisson reconstruction of point clouds, to generate a smooth triangular mesh model through point clouds and their normals) to reconstruct the complete and smooth surface and normals of the object being measured, and sample the final result point cloud from the reconstructed mesh as the reconstructed point cloud.

[0066] 4. After obtaining the reconstructed point cloud, it is necessary to segment the trajectory for the robot arm to perform movement and contour tracking.

[0067] Get the height range of the reconstructed point cloud, and divide the reconstructed point cloud into multiple slices from low to high, with the height of each slice being the preset slice interval. Calculate the average position of all points in each slice as the slice centroid, calculate the angle of each point in the slice relative to the centroid, and further subdivide the slice into 160-360 degrees and 0-160 degrees based on these angles, so that the generated point cloud can be conveniently reached by the robotic arm in a certain spatial order. Select an initial angle increment, and find the minimum angle increment that can meet the preset point cloud spacing through iteration. Under this minimum angle increment, calculate the average point and average normal of the point cloud in each angle interval as the trajectory point of the interval, and finally generate the object point cloud trajectory that meets the set point cloud spacing.

[0068] 5. Operate the robotic arm to slide 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 robot arm is operated to continuously track each circle of the trajectory and collect data synchronously. The new trajectory points are adjusted and calculated according to the actual arrival posture and force. The specific process is as follows:

[0070] Assume that the force vector at the current position of the robot arm end is [f x ,f y ,f z ]. z _threshold is the stress threshold, which is used to determine whether adjustment is needed. z The deviation from the threshold is truncated to the set maximum value, and its formula is as follows:

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

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

[0073] Among them, 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 is the proportional gain, K dis the differential gain, previous_error is the reference error at the previous moment. According to the size of the real-time reference error current_error, different control gain scaling factors are used in segments to calculate the control signal control_signal. The formula is as follows:

[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] When none of the above conditions are met, that is, |current_error|≤0.1 or |current_error|≥0.5, the scaling factor is set to 0.8. This situation 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, and the formula is as follows:

[0082]

[0083] Finally, the position of the next target point is adjusted according to the calculated displacement. The formula is as follows:

[0084] adjusted_position=position-displacement×tcp_z_direction,

[0085] Among them, 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] Thus, the position of the adjusted point is obtained, while the direction of the normal line remains unchanged.

[0087] The driving method of the robot arm end described in this embodiment can be quickly deployed on any existing robot 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 teaching robots to help students learn robotics and data collection technology, and enhance the interactivity and practicality of learning. This component can also directly sense objects and collect multimodal interaction data to obtain virtual objects, and realize rich applications in virtual reality (VR), metaverse and other environments. Through this technology, users can not only see replicas of objects in the virtual world, but also feel various sensory data such as touch and temperature when interacting with these objects in real time. Such functions greatly enhance the immersion and interactivity of the virtual reality experience, providing users with a new and highly interactive virtual environment.

[0090] 2. Industrial Automation:

[0091] On the production line, this component can be installed at the end of the robot arm for automatic inspection and sorting of products. Through the RGB-D camera, the robot can identify the size, shape and position of objects for precise handling and sorting. In the quality control link, through integrated temperature, acceleration and other sensors, the robot can perform real-time interactive inspection of products, identify defects that may occur during the processing, and detect whether the material, hardness, etc. meet the standards.

[0092] 3. Medical and Health:

[0093] Used in surgical assistance systems, this component can be used to assist in delicate surgical operations, such as cutting and suturing in minimally invasive surgery. The RGB-D camera provides precise visual information to help doctors or robots identify surgical areas and key structures, while the tip's tactile sensor can help assess the texture and response of tissues, enhancing surgical precision and safety. In rehabilitation centers, this component can be used to monitor a patient's range of motion and strength, and collect tactile feedback from the patient during movement, such as changes in muscle hardness and joint mobility, through the tip in contact with the patient, to assist doctors in assessing the patient's rehabilitation progress and adjusting treatment plans.

[0094] 4. Service Industry:

[0095] In a retail environment, the robot can use this end unit to interact with customers, enhance their shopping experience, provide product information or guide services. In a museum or exhibition, the device can also be used to provide an interactive learning experience.

[0096] 5. Security Monitoring:

[0097] In hazardous environments such as chemical plants or nuclear facilities, robots can carry the component into areas that are difficult for people to access to detect and monitor interactions with the environment, conduct real-time assessments of environmental conditions through multimodal perception, and respond quickly to emergencies.

[0098] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.

Claims

1. A method for driving a robot arm end, characterized in that: include: Drive the end of the robot arm around the object to collect the depth image of the object, calculate the coordinates of each pixel in the depth image in the camera coordinate system, and all coordinates in all depth images constitute a point cloud Pointcloud in the camera coordinate system Camera , the pose matrix Pose and the point cloud Pointcloud Camera Multiply to get the original point cloud Pointcloud in the world coordinate system world ; For the original point cloud Pointcloud world Perform pure point cloud extraction, denoising and smoothing processing in sequence, calculate the center of the processed point cloud and make the normals of all points face the outside of the point cloud, so as to obtain the reconstructed point cloud; Slicing the reconstructed point cloud, taking the average position of all points in each slice as the centroid of the slice, calculating the angle of each point in each slice relative to its centroid, and sorting the angles, dividing the sorted angles into intervals using the minimum angle increment that can meet the preset point cloud spacing, taking the average point and average normal of the point cloud in each interval as the trajectory point of the interval, and all trajectory points of all slices constitute the point cloud trajectory of the object; The object point cloud trajectory is used as a reference trajectory of the end of the robot mechanical arm, thereby realizing the driving of the end of the robot mechanical arm.

2. The driving method of the robot arm end according to claim 1, characterized in that: The method of using the object point cloud trajectory as a reference trajectory of the end of the robot mechanical arm to drive the end of the robot mechanical arm includes: When the end of the robot arm moves according to the reference trajectory, the force vector [f x ,f y ,f z ], and calculate the real-time reference error current_error according to the force threshold; The value of the control gain scaling factor is determined according to the real-time reference error current_error, and then the control signal control_signal is calculated according to the following formula: control_signal=K p ×current_error×scaling factor+K d ×(current_error-previous_error), Among them, K p is the proportional gain, K d is the differential gain, previous_error is the reference error at the previous moment; The control signal control_signal is converted into a displacement, and the position of the next target point at the end of the robot arm is calculated according to the displacement.

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

8.

4. The driving method of the robot arm end according to claim 3, characterized in that: The calculating of the real-time reference error current_error according to the force threshold comprises: current′_error=-f z -f z _threshold, current_error=min(current′_error,max_error), Among them, 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 of the robot manipulator end according to claim 3 or 4, characterized in that: The converting the control signal control_signal into a displacement displacement comprises: Among them, 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 of the robot arm end 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 according to the displacement comprises: adjusted_position=position-displacement×tcp_z_direction, Among them, 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 method for driving the end of a robot arm according to claim 1, 2, 3, 4 or 6, characterized in that: The calculation of the coordinates of each pixel in the depth image in the camera coordinate system includes: The coordinate z of each pixel in the depth image in the camera coordinate system is 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; Keep the pixels where 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 and gy are the focal lengths of the camera on the X and Y axes in the camera coordinate system, respectively, and (cx, cy) is the coordinate of the center point of the depth image.

8. The driving method of the robot arm end according to claim 7, characterized in that: The driving robot arm end circles around the object to collect a depth image of the object, including: Drive the end of the robot arm around the object and collect 5 depth images of the object every 45 degrees.

9. The driving method of the robot manipulator end according to claim 8, characterized in that: The original point cloud Pointcloud world Perform clean point cloud extraction, denoising and smoothing in sequence, including: Remove the original point cloud Pointcloud world The points with medium height lower than the height of the object are uniformly downsampled, and then the pure point cloud of the object is separated using DBSCAN; Statistical noise is removed from the pure point cloud, and then the points whose local density is less than a threshold are removed using the radius outlier. The normal is then 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 of the end of the robot arm according to claim 9, characterized in that: The method for obtaining the minimum angle increment comprises: The minimum angle increment that can meet the preset point cloud spacing is found in an iterative manner.

Images