An irregular object stacking method based on force sensor perception and compensation

Abstract

The application discloses an irregular object stacking method based on force sensor perception and compensation, comprising the following steps: 1, the robot moves to the grabbing area, and the hand-eye camera acquires the scene point cloud; 2, the maximum object point cloud is obtained by using a color threshold segmentation method and a contour detection algorithm; 3, the maximum object point cloud of step 2 is subjected to grabbing planning, and the robot performs a grabbing action; 4, the robot carries the object to run the recognition track, records the six-dimensional force sensor data and TCP data; 5, a load position recognition algorithm is run to obtain the position of the load centroid in the gripper coordinate system; 6, the robot moves to the stacking area, the hand-eye camera acquires the scene height map, and the highest area center coordinates are acquired; 7, the load centroid position is compensated to the highest area center coordinates of step 6, and the robot runs to the compensated stacking position; 8, steps 1 to 7 are repeated until there is no target object in the grabbing scene. The application can realize stable stacking of irregular objects.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of object stacking, and in particular to an irregular object stacking method based on force sensor sensing and compensation. BACKGROUND

[0002] Facility construction is a complex and multi-step robotic manipulation task, including robot grasping, planning and control, and other aspects. Outer space exploration is a human exploration activity on the moon and further celestial bodies or space environment. Using irregular natural objects for stacking construction in complex environments is one of the key tasks of celestial body exploration. In recent years, the demand for safe, inexpensive and sustainable buildings has grown, making facility construction by robots an active research area. Developing an automatic construction method, the environment is a sand scene, and the manipulated object is an irregularly shaped stone. The robot can be used for automatic stacking construction of facilities in unstructured environments such as outer space or construction sites.

[0003] Currently, most existing object stacking construction work is focused on fields such as item stacking and pallet stacking. After searching existing patent literature, for example, patent (publication number CN114359402A) discloses a pallet stacking control method, which uses a TOF camera to obtain point cloud information of a pallet to be stacked and a stacked pallet, thereby obtaining position information of the two pallets, and achieving accurate stacking of the pallets. Patent (publication number CN112123342B) discloses a method of measuring and controlling to grasp and stack objects, which requires more ranging sensors and cameras to find a reference surface to control the robot to move the end plate to a parallel posture with the surface of the object, or the surface of the object to be stacked is parallel to the surface of the stacked object. The method can achieve precise stacking tasks well, but the cost of measurement and control is high, the steps are more, and the surface of the stacked object is required to be flat, preferably a cube, which is not conducive to unmanned systems to perform tasks. Most of the above are structured scenes, and even in unstructured scenes, the stacked objects are often determined and regularly shaped cubes or cylinders.

[0004] Neural networks are often used for object grasping in unstructured scenes. For irregularly shaped objects, the grasping points obtained by neural network regression may appear at the corners of the object. During grasping, the surrounding environment will disturb the position of the object, and the position of the centroid of the object in the gripper coordinate system is unknown. To address this uncertainty, patent (publication number CN115502985A) discloses a method based on force sensor information, which releases the object and regrasps it to adjust the position of the object in the case of known centroid offset. This method is slightly cumbersome, has some uncertainty in adjustment, and is for regular objects.

[0005] In recent years, reinforcement learning and human demonstration-based methods have attempted to stack objects in unstructured scenes, but none of them have considered the impact of grasping uncertainty on the stacking task, and all of them have used regular objects such as building blocks.

[0006] Therefore, in view of the above problems and requirements, the skilled in the art is committed to developing an irregular object stacking method based on force sensor to solve the above problems. The method is simple in steps, easy to arrange, effective, and can realize stable stacking of irregular objects in unstructured scenes, and reduce the impact of uncertainty on construction tasks. SUMMARY

[0007] In view of the above defects of the prior art, the technical problem to be solved by the present application is that the robot grasping uncertainty causes the position information of the object in the gripper to be unknown, which affects the facility construction tasks such as stacking.

[0008] To achieve the above purpose, the present application provides an irregular object stacking method based on force sensor sensing and compensation, characterized in that it comprises the following steps:

[0009] Step 1: The robot moves to the grasping area, and the hand-eye camera obtains the scene point cloud;

[0010] Step 2: Use color threshold segmentation method and contour detection algorithm to screen and obtain the maximum object point cloud in the scene;

[0011] Step 3: Perform grasping planning on the maximum object point cloud of step 2, and the robot performs grasping action;

[0012] Step 4: The robot carries the object to run the recognition trajectory, records the six-dimensional force sensor data and TCP (Tool Center Point end flange center) data;

[0013] Step 5: Run the load position recognition algorithm, and transform to get the position of the load center of mass in the gripper coordinate system;

[0014] Step 6: The robot moves to the stacking area, the hand-eye camera obtains the scene height map, and the depth-first search algorithm is used to obtain the highest region center coordinates;

[0015] Step 7: Compensate the load center of mass position to the highest region center coordinates of step 6, and the robot runs to the compensated stacking position in servo mode;

[0016] Step 8: Repeat steps 1 to 7 until there is no target object in the grasping scene.

[0017] Further, in step 1, the depth image obtained by the hand-eye camera is converted into point cloud data represented in the world coordinate system according to the following formula:

[0018]

[0019]

[0020]

[0021]

[0022] where P C is the representation of the point cloud in the camera coordinate system, P W is the representation of the point cloud in the world coordinate system, c x , c y , f x , f y and scale are the camera intrinsic parameters, is the homographic transformation matrix of the camera coordinate system relative to the world coordinate system.

[0023] Further, in step 2, the color image obtained by the hand-eye camera is segmented by using the HSV (Hue, Saturation, Value) threshold segmentation method to obtain an object mask; the object mask is filtered, expanded, eroded and edge contour extracted to obtain a mask of the largest object; the mask of the largest object is used to screen the scene point cloud in step 1 to obtain the largest object point cloud.

[0024] Further, in step 3, the largest object point cloud is subjected to a grasping network planning to obtain a grasping pose, and the robot performs a grasping action according to the grasping pose.

[0025] Further, in step 4, a robot control node running on a computer communicates through a wireless local area network of a robot control cabinet, and the robot control node is used to control the robot to move.

[0026] Further, an information acquisition and processing node is arranged to read in the six-dimensional force sensor data and the TCP data in real time; the six-dimensional force sensor data and the TCP data are taken out from the robot control cabinet and published by the robot control node.

[0027] Further, in step 5, the relationship between the original data acquired by the information acquisition and processing node and the load centroid is as follows:

[0028] [F x0 F x0 F x0 -mg] T= (A T A) -1 A T F (1)

[0029] [r x r y r z M x0 M y0 M z0 ] T = (B T B) -1 B T M (2)

[0030] In the formula, m is the load mass, [F x0 F y0 F z0 M x0 M y0 M z0 ] T is the zero drift of the six-dimensional force sensor, [r x r y r z ] T is the coordinate of the load mass center in the force sensor coordinate system, and g is the gravity acceleration;

[0031] A and B are 3N*4 and 3N*6 regression matrices composed of a rotation matrix and a unit matrix; the rotation matrix is the representation of the robot base coordinate system in the force sensor coordinate system, as shown in the following formula; N is the number of groups of recorded data; F and M are 3N-dimensional column vectors arranged by the recorded force sensor data;

[0032]

[0033]

[0034] In the formula, R ij is the i-th row and j-th column element of the R rotation matrix;

[0035] The information acquisition and processing node quickly calculates the load mass and the position of the mass center in the force sensor coordinate system by using formulas (1) and (2) after completing the acquisition task.

[0036] Further, in step 6, the scene height map is divided into several height regions, the center of the highest region is taken as the pre-stacking position, and the final stacking position is compensated by the mass center position; the scene height map is represented in the stacking coordinate system, and the point cloud is pixelated into a height map with a pixel block length of 2 mm, and the height value of each pixel block is the z value of the previous point cloud.

[0037] Further, the depth-first search algorithm starts searching from the top-left pixel block of the height map, stores the pixel block P into a height region H, assigns the height of P to Cur (a variable storing the height value of P), and then sets the height value of P in the height map to 0. The height values of the pixel blocks above, below, left and right of P are compared with Cur. If the height difference is less than a threshold, it is considered that the pixel block R and P are in the same height region, and then the above operation is performed on R. If the height difference is greater than the threshold, it is considered that R is in another height region, and then other pixel blocks around P are detected. If the pixel blocks above, below, left and right of P are not in the same height region as P, the height region H is searched completely. Then, the non-zero pixel blocks in the height map are searched until all height regions are divided. The height region with the highest average z value is taken as the highest region, and the center coordinates of the highest region are the pre-stacking position.

[0038] Further, in step 7, the robot control node controls the robot to run to the compensated stacking position in a servo mode, and controls the robot to release the gripper, thereby completing the stacking of the target object.

[0039] Compared with the conventional method, the method provided by the application is simple to operate, can reduce the deviation caused by the uncertainty of unstructured scene grabbing, improves the stability and precision of the stacking task, and has the following beneficial effects:

[0040] 1. The application provides a method for selecting a stacking position of irregular objects, which is also applicable to regular objects.

[0041] 2. The application can effectively adapt to a relatively complex and real construction environment, can successfully screen the largest object point cloud, and can execute the stacking task in order, which is beneficial to the stability of the task.

[0042] The concept, specific structure and generated technical effects of the application will be further described below with reference to the drawings, so as to fully understand the purpose, features and effects of the application. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 The figure is a schematic diagram of the system structure of a preferred embodiment of the application.

[0044] Figure 2 The figure is a schematic diagram of the coordinate definition of a preferred embodiment of the application.

[0045] Figure 3 The figure is a schematic diagram of the running flow of a preferred embodiment of the application.

[0046] Among them, 1-robot body, 2-six-dimensional force sensor, 3-camera mounting plate, 4-RGBD camera, 5-two-finger gripper, 6-computer, 7-USB cable, 8-irregular object, 9-world coordinate system, 10-robot base coordinate system, 11-TCP coordinate system, 12-force sensor coordinate system, 13-center of mass coordinate system, 14-stacked coordinate system. Detailed Implementation

[0047] The following description, with reference to the accompanying drawings, illustrates several preferred embodiments of the present invention to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0048] In the accompanying drawings, components with the same structure are indicated by the same numerical designation, and components with similar structures or functions are indicated by similar numerical designations. The dimensions and thicknesses of each component shown in the drawings are arbitrary, and the present invention does not limit the dimensions and thicknesses of each component. To make the illustrations clearer, the thickness of some components has been appropriately exaggerated in the drawings.

[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.

[0050] In the description of this invention, it should be noted that the terms "middle", "upper", "lower", "left", "right", "horizontal", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting this invention.

[0051] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0052] like Figure 1As shown, the irregular object stacking method based on force sensor mainly includes a JAKA Zu7 robot 1, a JAKA II type six-axis force sensor 2, a carbon fiber camera mounting plate 3, an RGBD camera 4 (RealSense TMD435), a ROBOTIQ two-finger gripper 5, a computer 6 and irregular objects 8. The software part includes a point cloud processing node, an information acquisition and processing node, a robot control node and a stacking position search node.

[0053] The point cloud processing node is used to process all data from the camera and convert it into the type required by other nodes. The point cloud processing node subscribes to color images and depth images from the RGBD camera, and converts the depth image into point cloud data represented in the world coordinate system according to the following formula.

[0054]

[0055]

[0056]

[0057]

[0058] In the formula, P C is the representation of the point cloud in the camera coordinate system, P W is the representation of the point cloud in the world coordinate system, c x , c y , f x , f y and scale are camera intrinsic parameters, is the homogeneous transformation matrix of the camera coordinate system relative to the world coordinate system.

[0059] The point cloud processing node uses HSV threshold segmentation method to segment the color image to obtain the object mask; filters, dilates, erodes and extracts the edge profile of the mask to obtain the mask of the largest object; and screens the above point cloud with the mask of the largest object to successfully screen out the largest object point cloud. The largest object point cloud is planned through a grasping network to obtain a grasping pose.

[0060] The robot control node is used to control the robot to realize the movement, mainly the following movement modes: joint movement, Cartesian space movement, servo mode movement and gripper opening and closing. The joint movement includes robot movement to a grasping area observation point, robot movement to a load identification trajectory, and robot movement to a stacking position observation point; the Cartesian space movement includes robot movement to a grasping posture; the servo mode movement includes robot movement to an optimal stacking point; and the gripper opening and closing includes gripper opening before grasping, robot movement to a grasping posture and then closing, and gripper opening after the servo mode force exceeds a threshold. The robot control node running on the computer communicates through the wireless local area network of the robot control cabinet, and the gripper control instructions and the data of the camera are transmitted to the computer through the USB line.

[0061] The information acquisition and processing node is used to read the raw data of the force sensor and the corresponding data of the TCP in real time in the identification stage; the data are all taken out from the robot control cabinet by the robot control node and published.

[0062] As Figure 2 defined in the present application, the following six coordinate systems need to be defined in advance: a world coordinate system 9, a robot base coordinate system 10, a TCP coordinate system 11, a force sensor coordinate system 12, a center of mass coordinate system 13 and a stacking coordinate system 14. The world coordinate system is used for the above-mentioned point cloud processing node. The processing part of the information acquisition and processing node needs the transformation relationship between the robot base coordinate system, the TCP coordinate system, the force sensor coordinate system and the center of mass coordinate system, and the relationship between the raw data collected and the load center of mass is as follows:

[0063] [F x0 F x0 F x0 -mg] T =(A T A) -1 A T F (1)

[0064] [r x r y r z M x0 M y0 M z0 ] T =(B T B) -1 B T M (2)

[0065] In the formula, m is the load mass, is the zero drift of the six-dimensional force sensor, is the coordinate of the load center of mass in the force sensor coordinate system, and g is the gravitational acceleration.

[0066] A and B are rotation matrices and the identity matrix; the rotation matrix is the representation of the robot base coordinate system in the force sensor coordinate system, as shown in the following formula; the N is the number of groups of recorded data. F and M are 3N-dimensional column vectors arranged by the recorded force sensor data.

[0067]

[0068]

[0069] wherein R ij is the i-th row and j-th column element of the R rotation matrix.

[0070] The information acquisition and processing node quickly calculates the load mass and the position of the center of mass in the force sensor coordinate system by using formulas (1) and (2) after completing the acquisition task; and the position of the center of mass is published to the stacking position search node for compensation.

[0071] The stacking position search node performs height region division on the height map point by point, and uses a depth-first search algorithm. The height map is divided into several height regions, and the center of the highest region is taken as the pre-stacking position. The final stacking position is compensated by the position of the center of mass. The height map is represented in the stacking coordinate system, and its acquisition method is similar to that of the point cloud, but the point cloud form cannot be regionally divided. The point cloud is pixelated into a height map with a 2mm pixel block length. The height value of each pixel block is the z value of the previous point cloud. The depth-first search algorithm starts searching from the top-left pixel block of the height map, stores the pixel block P into a height region H, assigns its height to Cur, and then sets the height value of P in the height map to 0. The height values of the pixel blocks above, below, left and right of P are compared with Cur. If the height difference is less than a threshold, it is considered that the pixel block R and P are in the same height region, and then the operation on R is performed. If the height difference is greater than the threshold, it is considered that R is in another height region, and then the other pixel blocks around P are detected. If the pixel blocks above, below, left and right of a certain pixel are not in the same height region as the pixel, the height region H is searched. Then the non-0 pixel blocks of the height map are searched until all height regions are divided. The highest region with the highest average z value is taken, and the center coordinates are the pre-stacking position.

[0072] As Figure 3 shown, the specific steps of the present application are as follows:

[0073] 1. The robot moves to the grabbing area, and the hand-eye camera acquires the scene point cloud.

[0074] 2. The color threshold segmentation method and the contour detection algorithm are used to screen and obtain the point cloud of the largest object in the scene.

[0075] 3. Plan the grasping action for the largest object point cloud obtained in step 2, and the robot will perform the grasping action.

[0076] 4. The robot carries the object and identifies its trajectory, recording six-dimensional force sensor data and TCP data.

[0077] 5. Run the load position identification algorithm and transform it to obtain the position of the load centroid in the gripper coordinate system.

[0078] 6. The robot moves to the stacking area, the hand-eye camera acquires the scene height map, and the depth-first search algorithm is used to obtain the coordinates of the center of the highest area.

[0079] 7. Compensate the load centroid position to the highest region center coordinates mentioned in step 6, and the robot runs in servo mode to the compensated stack position.

[0080] 8. Repeat steps 1 to 7 until there are no target objects in the scene to be captured.

[0081] This invention is not limited to the above-described embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of this invention shall be considered equivalent substitutions, including but not limited to changes in name, changes in part model and size, adjustments to the installation position or angle of the mechanism, overall or partial enlargement or reduction, and adjustments to the relative positions of parts. All of these are included within the scope of protection of this invention.

[0082] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A method for sensing and compensating an irregular object stack based on force sensors, characterized by, The method comprises the following steps: Step 1: the robot moves to a grabbing area, and a hand-eye camera acquires a scene point cloud; Step 2: a color threshold segmentation method and a contour detection algorithm are used to screen a maximum object point cloud in the scene; Step 3: the maximum object point cloud in step 2 is subjected to grabbing planning, and the robot performs a grabbing action; Step 4: the robot runs a recognition track while carrying the object, and records six-dimensional force sensor data and TCP data; Step 5: a load position recognition algorithm is run, and a load centroid position in a gripper coordinate system is obtained through transformation; Step 6: the robot moves to a stacking area, a hand-eye camera acquires a scene height map, and a highest area center coordinate is obtained by using a depth-first search algorithm; Step 7: the load centroid position is compensated to the highest area center coordinate in step 6, and the robot runs to a compensated stacking position in a servo mode; Step 8: steps 1 to 7 are repeated until there is no target object in the grabbing scene.

2. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 1, wherein, In step 1, a depth image obtained by the hand-eye camera is converted into point cloud data represented in a world coordinate system according to the following formula: ； ； ； ； wherein is the representation of the point cloud in the camera coordinate system, is the representation of the point cloud in the world coordinate system, , , , and scale are the camera intrinsic parameters, is the homogeneous transformation matrix of the camera coordinate system relative to the world coordinate system.

3. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 1, wherein, In step 2, an HSV threshold segmentation method is used to segment a color image obtained by the hand-eye camera to obtain an object mask; filtering, expansion, corrosion and edge contour extraction operations are performed on the object mask to obtain a mask of the maximum object; the mask of the maximum object is used to screen the scene point cloud in step 1 to obtain the maximum object point cloud.

4. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 1, wherein, In step 3, the maximum object point cloud is subjected to grabbing network planning to obtain a grabbing pose, and the robot performs a grabbing action according to the grabbing pose.

5. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 1, wherein, In step 4, a robot control node running on a computer communicates with a robot control cabinet through a wireless local area network, and the robot control node is used to control the robot to move.

6. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 5, wherein, An information acquisition and processing node is configured to read in the six-dimensional force sensor data and the TCP data in real time; the six-dimensional force sensor data and the TCP data are taken out of the robot control cabinet and published by the robot control node.

7. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 6, wherein, In step 5, a relationship between original data acquired by the information acquisition and processing node and a load centroid is as follows: （1） （2） where m is the mass of the load, is the zero drift of the six-dimensional force sensor, is the coordinate of the load's center of mass in the force sensor coordinate system, and g is the acceleration of gravity. A and B are 3Nx4 and 3Nx6 regression matrices composed of a rotation matrix and an identity matrix; the rotation matrix is the representation of the robot base coordinate system in the force sensor coordinate system as shown in the following equation; N is the number of groups of recorded data; F and M are 3N-dimensional column vectors arranged by recorded force sensor data; ； ； wherein is the i-th row, j-th column element of R rotation matrix; After completing the acquisition task, the information acquisition and processing node quickly calculates the load mass and the position of the centroid in the force sensor coordinate system by using formulas (1) and (2).

8. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 7, wherein, In step 6, the scene height map is divided into a plurality of height regions, the center of the highest region is taken as a pre-stacking position, and a final stacking position is compensated by the centroid position; the scene height map is represented in a stacking coordinate system, a point cloud is pixelized into a height map with a 2mm pixel block edge length, and a height value of each pixel block is a z value of the previous point cloud.

9. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 8, wherein, The depth-first search algorithm starts searching from the top-left pixel block of the height map, stores the pixel block P into a height region H, assigns the height of P to Cur, and then sets the height value of P in the height map to 0; compares the height values of the pixel blocks above, below, left and right of P with Cur, if the height difference is less than a threshold value, it is considered that the pixel block R and P are in the same height region, and then the above operation is performed on R; if the height difference is greater than the threshold value, it is considered that R is in another height region, and then other pixel blocks around P are detected; if the pixel blocks above, below, left and right of a certain pixel are not in the same height region as the pixel, the height region H is searched completely; then the non-0 pixel blocks of the height map are continuously searched until all height regions are divided out; the height region with the highest average z value among all height regions is taken as the highest region, and the center coordinates of the highest region are the pre-stacking position.

10. The method for sensing and compensating an irregular object stack based on a force sensor according to claim 9, wherein, In step 7, the robot control node controls the robot to run in a servo mode to the compensated stacking position, the robot control node controls the robot to release the clamping jaw, and the stacking of the target object is completed.

Images