Adaptive manipulator human-machine-object transmission method, system, machine-readable storage medium and data processing device based on point cloud under composite constraints

Abstract

The present invention discloses a method, system and data processing equipment for adaptive manipulator human-machine object transfer based on point cloud under composite constraints, which belongs to the field of visual perception and motion planning of mobile manipulators and includes the following steps: S1, using a depth camera at the end of the manipulator to respectively obtain a point cloud of the object to be grasped and a point cloud of the user's body; S2, based on the point cloud of the object to be grasped and the point cloud of the user's body obtained in step S1, respectively estimate the grasping posture of the object to be grasped and the object transfer posture of the user; S3, based on the grasping posture of the object to be grasped and the object transfer posture of the user estimated in step S2, plan the motion trajectory of the manipulator under composite constraints to realize adaptive manipulator human-machine object transfer planning. The present invention adopts the above-mentioned method, system and data processing equipment for adaptive manipulator human-machine object transfer based on point cloud under composite constraints, combines point cloud matching and posture estimation, realizes the adaptability and intelligence of the manipulator in the grasping and object transfer process, and the error of the estimated adaptive posture result is small and the planning success rate is high to meet the complex working environment of the manipulator.

Description

Technical Field

[0001] The present invention relates to the field of visual perception and motion planning of mobile manipulators, and in particular to a method, system and data processing equipment for point cloud-based adaptive manipulator human-machine-object transmission under composite constraints. Background Art

[0002] With the widespread use of service robots in daily life and industry, particularly in grasping and object transfer tasks, the adaptability and intelligence of robotic arms have become key challenges. Traditional robotic arm grasping methods typically rely on predefined grasping postures or fixed grasping points. This approach often performs poorly when faced with objects of varying shapes and sizes and in complex working environments, resulting in low grasping success rates and efficiency. Furthermore, in human-machine object transfer scenarios, the robotic arm must perform grasping and transfer operations based on the user's state and posture, increasing the complexity and difficulty of the task.

[0003] To address these challenges, a number of adaptive robotic arm control methods based on vision and depth perception have emerged in recent years. The development of robotic arm hand-eye systems and point cloud processing technologies has provided crucial support for intelligent grasping and delivery. These systems utilize a camera mounted at the end of the robotic arm to capture real-time images of the surrounding environment, enabling perception of key information such as the position and posture of objects. Combined with a depth camera, these systems can generate richer 3D point cloud data, providing the robot with more accurate information about the object's shape and position. Summary of the Invention

[0004] In response to the problem that a service robot's robotic arm is difficult to adaptively transfer humans and objects according to the task and the user's current status when performing a grasping task, the present invention provides a point cloud-based adaptive robotic arm human-machine object transfer method, system and data processing equipment under complex constraints. The method combines point cloud matching and posture estimation to achieve the adaptability and intelligence of the robotic arm in the grasping and object transfer process. The estimated adaptive posture result has a small error and a high planning success rate to meet the complex working environment of the robotic arm.

[0005] To achieve the above objectives, the present invention provides a point cloud-based adaptive manipulator human-machine object transfer method under composite constraints, comprising the following steps:

[0006] S1. Use the depth camera at the end of the robotic arm to obtain the point cloud of the object to be grasped and the point cloud of the user's body;

[0007] S2, based on the point cloud of the object to be grasped and the point cloud of the user's body obtained in step S1, respectively estimating the grasping posture of the object to be grasped and the object transfer posture of the user;

[0008] S3. Based on the grasping posture of the object to be grasped and the object transfer posture of the user estimated in step S2, the motion trajectory of the robot arm is planned under the composite constraints to realize adaptive robot arm human-machine object transfer planning.

[0009] Preferably, step S2 specifically includes the following steps:

[0010] S21. Obtain the Euclidean rotation matrix between the robot's camera coordinate system and the robot's end tool coordinate system tool T cam , thereby determining the coordinate transformation relationship between the target point cloud in the depth camera and the robotic arm base;

[0011] It specifically includes the following steps:

[0012] S211. Set the closed-loop coordinate system spatial relationship of the hand-eye system of the robot arm to satisfy the following formula:

[0013] tool T cam = tool T base * base T cal * cal T cam (1)

[0014] Where, tool T cam is the Euclidean rotation matrix of the depth camera coordinate system relative to the tool coordinate system at the end of the robot arm; tool T base is the rotation matrix of the tool coordinate system at the end of the robot arm relative to the coordinate system of the robot arm base; base T cal is the rotation matrix of the calibration plate relative to the manipulator base coordinate system; cal T cam is the rotation matrix of the calibration plate relative to the depth camera coordinate system;

[0015] It is deduced that:

[0016] base T cal = base T tool * tool T cam * cam T cal (2)

[0017] Where, base T tool is the rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm; tool T cam is the rotation matrix of the tool coordinate system at the end of the robot arm relative to the depth camera coordinate system;cam T cal is the rotation matrix of the depth camera coordinate system relative to the calibration plate;

[0018] S212, take the results twice at time t0 and time t1 respectively, and obtain:

[0019]

[0020] Where, is the rotation matrix of the manipulator base coordinate system relative to the manipulator end tool coordinate system at time t0; is the rotation matrix of the depth camera coordinate system relative to the calibration plate at time t0; is the rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm at time t1; is the rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm at time t1;

[0021] Multiply the left and right sides of formula (3) by and get:

[0022]

[0023] Where, is the rotation matrix of the tool coordinate system at the end of the manipulator relative to the manipulator base coordinate system at time t1; The rotation matrix of the calibration plate relative to the depth camera coordinate system at time t0;

[0024] Convert to:

[0025] AX=XB (5)

[0026] in:

[0027]

[0028]

[0029] X= tool T cam (8);

[0030] S213, use the Tsai-Lenz method to solve the Euclidean rotation matrix of the required depth camera coordinate system relative to the tool coordinate system of the robot end tool T cam Numerical solution of ;

[0031] S214, using the depth camera internal parameters and Euclidean rotation matrix tool T cam Convert the user's position coordinates to the robot arm base coordinate system:

[0032]

[0033]

[0034] Where, (u pixel ,v pixel ) is the user's position in the pixel coordinate system, which is the coordinate of the center point of the recognition frame; (x, y, z) is the three-dimensional coordinate of the user in the camera coordinate system; z is the depth value of the pixel where the user is located read in the depth map; f is the focal length of the depth camera; c x and c y are the optical center positions of the depth camera on the x-axis and y-axis respectively;

[0035] S22, respectively estimate the grasping posture of the object to be grasped and the object transmission posture of the user:

[0036] For the point cloud of the object to be grasped, a template matching method based on 2D image features is used to identify the target object to achieve segmentation of the 3D scene point cloud. Then, a point cloud matching method combining coarse matching and fine registration based on 3D point cloud features is used to estimate the transformation matrix, thereby adaptively determining the grasping position of the object to be grasped.

[0037] For the user's human body point cloud, a combined point cloud filtering and clustering method is used to screen and segment the three-dimensional human body point cloud in the scene, and then a method based on the singular value decomposition of the point cloud matrix is ​​used to estimate the human body posture, thereby adaptively determining the user's object transmission posture.

[0038] Preferably, in step S22, for the point cloud of the object to be captured, firstly, the feature points of the template image and the scene image are calculated respectively using the two-dimensional SURF feature, and then the feature points are screened using the Lowe algorithm and the RANSAC algorithm and the homography transformation matrix of the template matching is calculated to delineate the region of interest in the two-dimensional RGB image of the camera; then, the points in the region of interest are converted into a point cloud containing both three-dimensional XYZ coordinates and RGB colors, and then screened according to the depth value constraint to achieve the segmentation of the three-dimensional environment point cloud;

[0039] Then, the SAC-IA point cloud registration method based on FPFH 3D point cloud features is used to achieve coarse matching between the scene object point cloud and the template point cloud. The point-to-point point cloud ICP algorithm is used to achieve fine registration between the scene object point cloud and the template point cloud. The Euclidean transformation matrix of the template point cloud and the scene object point cloud is obtained using a point cloud matching method. The Euclidean transformation matrix is ​​applied to the corresponding manual optimal grasping pose to calculate the optimal grasping pose of the current scene object.

[0040] The point cloud matching steps are as follows:

[0041] First, the FPFH features of all points in the point cloud are calculated. That is, each point in the point cloud is used as the current source point and all points within a certain neighborhood to calculate the FPFH features and express them through histogram statistics. A coordinate system is established based on the normal vectors and connecting lines of the source point and a neighboring point in the neighborhood. The direction calculation formula of the three axes of the coordinate system is as follows:

[0042] u=n s (11)

[0043] v=(P t -P s )×u (12)

[0044] w=u×v (13)

[0045] Where u, v, and w represent the three axes of the coordinate system respectively; P s is the source point of the current calculation feature; P t P s One of the points in the neighborhood; n s Point P s Normal vector of (P t -P s ) represents point P t To point P s The symbol × represents the cross product of vectors;

[0046] According to the three-axis coordinate system, the following three eigenvalues ​​are calculated:

[0047] α=v·n t (14)

[0048]

[0049] θ=arctan(w·n t ,u·n t ) (16)

[0050] Where α is the v-axis and point P t The angle eigenvalue of the normal vector; φ is the angle eigenvalue of the line connecting the two points of the source normal vector; θ is the eigenvalue describing the direction of the u-axis, w-axis and the normal vector of the adjacent point; n t Point P t Normal vector of ||P t -P s || represents point P t To point P s The Euclidean distance of Represents point P t To point P s The unit vector in the direction of the line connecting ;

[0051] Then, the three eigenvalues ​​of α, φ, and θ are calculated for all neighboring points in the neighborhood of the source point, and statistics are performed through histograms. The method of dividing the three eigenvalue ranges into equal parts is used to form a multidimensional histogram feature, which is recorded as SPF (P s ), for k neighboring points P t , and calculate SPF(P t ), and press the neighboring point P t The distance calculation weight from the source point is added to SPF (P s ), the FPFH feature FPFH(P s ):

[0052]

[0053] Where, ω k represents the i-th neighbor point P i and source point P s distance;

[0054] After calculating the FPFH features of the template point cloud and the scene object point cloud respectively, the SAC-IA point cloud registration method is used to randomly select a set number of feature matching points, and the Euclidean rotation matrix and matching error of the current matching situation are calculated;

[0055] Different feature matching point sets are selected through a set number of iterations, and the Euclidean rotation matrix with the minimum matching error is used as the result of point cloud coarse registration;

[0056] Finally, the point-to-point point cloud ICP algorithm is used to achieve precise registration of the scene object point cloud and the template point cloud: the scene object point cloud, the template point cloud and the coarse registration Euclidean transformation matrix are input into the point-to-point point cloud ICP algorithm, and the new Euclidean transformation matrix is ​​iteratively calculated based on the coarse registration transformation until the set error requirement is met.

[0057] Preferably, in step S22, for the user's human body point cloud, a combined point cloud filtering and clustering method is used to implement screening and segmentation of the three-dimensional human body point cloud in the scene, and the specific steps are as follows:

[0058] First, define the distance threshold T dist , and the distance is greater than T dist Delete the point cloud to achieve rough segmentation of the environment point cloud;

[0059] Then, the voxel filtering method is used to downsample the roughly segmented environment point cloud: the voxel filtering method divides the space into equal parts with a radius of r voxel For a cube voxel with point cloud distribution, the centroid of all point clouds in the voxel is calculated and stored as a new point cloud in the voxel;

[0060] The centroid calculation formula for all points in any voxel grid is as follows:

[0061]

[0062]

[0063]

[0064] The point cloud is then denoised using a statistical outlier filter:

[0065] For a point cloud P with N number of points, P = {p1, p2, ..., p n Any point p in i , define point p i The k nearest neighboring points nearby are its neighborhood, and the distance from the point to all points in its neighborhood is used as a statistical sample, which satisfies the normal distribution and has a sample mean and sample variance;

[0066] Among them, point p i The sample distance M to all points in the neighborhood i The calculation formula is as follows:

[0067]

[0068] Where k is the number of neighboring points in the neighborhood; p j Represents point p i The jth point in the neighborhood; ||p i -p j || represents point p i To point p j distance;

[0069] Then calculate the point p separately i The sample variance D of all points in the neighborhood i :

[0070]

[0071] Define the outlier threshold T o , when the outlier threshold T o When the following conditions are met, it is considered that point p i To count outliers, they are filtered out by the filter:

[0072] T o <M i +λ D D i (twenty three)

[0073] Where λ D is the multiple of standard deviation;

[0074] Finally, the human body point cloud is segmented using the clustering segmentation algorithm based on Euclidean distance:

[0075] For a point cloud P with N number of points, P = {p1, p2, ..., p n Any point p in i If the current point is not assigned to any cluster, a new cluster C is created. j , and point p i Assigned to cluster C j For a neighboring point p in its neighborhood k , calculate point p i and point p k The Euclidean distance dis(p i ,p k ):

[0076]

[0077] In the formula, (x i ,y i ,z i ) is point p i Coordinates of (x k ,y k ,z k ) is point p k coordinates of

[0078] Define a distance threshold T d , when the Euclidean distance dis(p i ,p k ) is less than the distance threshold T d When , it is considered to be the point of the same cluster and is assigned to cluster C j Middle; perform Euclidean distance threshold judgment on all points in the neighborhood of the point, and add new cluster C j , continue to iteratively search the neighborhood points of the newly added point until there are no new points; continue to traverse the points that are not assigned to any cluster until all points are assigned to the corresponding cluster.

[0079] Preferably, in step S22, a method based on singular value decomposition of a point cloud matrix is ​​used to estimate the human body posture, thereby adaptively determining the user's object transfer posture. The specific steps are as follows:

[0080] First, perform singular value decomposition on the point cloud to obtain a mathematical description of the main feature directions of the point cloud:

[0081] For N-dimensional data with M points, it is described in matrix form A M×N , A iis the i-th column vector, u is the column vector of the unit projection, the data is decentralized so that the mean of the processed point cloud is 0, and the main direction of the point cloud is set to the variance S in the data distribution 2 The unit vector e in the direction of the maximum is defined as follows:

[0082]

[0083] in

[0084]

[0085] because

[0086] ((A i ) T e) 2 =((A i ) T e) T ·((A i ) T ·e)=e T ·A i ·(A i ) T ·e (27)

[0087] Therefore

[0088]

[0089] Where, A M×N The covariance matrix of Recorded as C;

[0090] Then use the Lagrange multiplier method to solve the target optimization function:

[0091] L(e,λ)=e T Ce+λ(1-e T e) (29)

[0092]

[0093] Where L(e,λ) represents the objective function of the Lagrange multiplier method;

[0094] Simplify equation (30) to

[0095] Ce=λe (31)

[0096] When the objective function takes an extreme value, λ is the largest eigenvalue of the covariance matrix C, and e is the eigenvector corresponding to the largest eigenvalue. At this time, solving the eigenvalue and eigenvector problem of the covariance matrix C is equivalent to solving the point cloud matrix A. M×N The singular value decomposition problem of AM×N The result of singular value decomposition is as follows:

[0097]

[0098] Where, For (A M×N ) T A M×N The eigenvector of the covariance matrix C; U M×M A M×N (A M×N ) T The eigenvector of and U M×M are all unit orthogonal vectors;

[0099] AndU M×M and The corresponding eigenvector in is expressed as ∑ M×N The singular values ​​are arranged in order, and the eigenvalues ​​of the covariance matrix C are ∑ M×N The squares of the singular values ​​in ;

[0100] Finally, the positive and negative directions of the desired point cloud directions are unified to form a three-dimensional Euclidean coordinate system, and the three-dimensional Euclidean coordinate system is regarded as the coordinate system of the current human body point cloud direction.

[0101] Preferably, in step S3, the human-machine object transfer task is set as: taking the object grasping posture as the starting point and the object transfer posture as the end point, using the robot arm motion planning method to plan an effective path to achieve the object transfer task, and setting the constraints in the object transfer robot arm motion planning process: passive chain constraints, environmental constraints, comfort constraints and task constraints.

[0102] Preferably, in step S3, the following constraint is also added: the judgment condition based on the quaternion angle of the end of the manipulator is used to constrain the posture change of two adjacent path nodes during the movement process. The judgment condition is as follows:

[0103] 2arccos(Q n Q n′ )≤T Q (33)

[0104] Where Q n is the current node of the robot's motion path; Q n′ is the next node in the planned path, arccos is the inverse cosine function, T Q is the posture change threshold.

[0105] The system of the point cloud-based adaptive manipulator human-machine object transfer method under composite constraints includes a manipulator, a depth camera is installed at the end of the manipulator, and the depth camera communicates with a computer, and the computer includes:

[0106] The calibration module obtains the Euclidean transformation matrix between the robot's camera coordinate system and the robot's end tool coordinate system;

[0107] The perception and target detection module is used to use the depth camera to obtain the point cloud of the object to be grasped and the point cloud of the user's body, and to obtain the Euclidean transformation matrix between the camera coordinate system of the robot arm and the tool coordinate system at the end of the robot arm. tool T cam , thereby determining the coordinate transformation relationship between the target point cloud in the depth camera and the robotic arm base;

[0108] The object grasping pose estimation module is used to identify the target object based on the point cloud of the object to be grasped using a template matching method based on 2D image features to segment the 3D scene point cloud. It then uses a point cloud matching method that combines coarse matching and fine registration based on 3D point cloud features to estimate the transformation matrix, thereby adaptively determining the grasping pose of the object to be grasped.

[0109] The user transfer pose estimation module is used to filter and segment the 3D human point cloud in the scene based on the user's human point cloud using a combined point cloud filtering and clustering method. It then uses a method based on the singular value decomposition of the point cloud matrix to estimate the human pose, thereby adaptively determining the user's object transfer pose.

[0110] The robot arm motion planning module is used to realize adaptive robot arm human-machine object transfer planning under composite constraints based on the grasping posture of the object to be grasped and the object transfer posture of the user.

[0111] A machine-readable storage medium stores machine-executable instructions, which, when executed by one or more processors, implement the point cloud-based adaptive manipulator human-machine-object transfer method under composite constraints.

[0112] A data processing device includes a processor and a machine-readable storage medium, wherein the machine-readable storage medium stores machine-executable instructions. When the machine-executable instructions are executed by the processor, the method for adaptive manipulator-human-machine-object transfer based on point cloud under composite constraints is implemented.

[0113] The present invention has the following beneficial effects:

[0114] 1. By combining point cloud matching and pose estimation, the robot arm achieves adaptability and intelligence in the process of grasping and object transfer. The estimated adaptive pose results have low error and high planning success rate, which can meet the complex working environment of the robot arm.

[0115] 2. Enable the robotic arm to perform service tasks efficiently by considering composite constraints in the planning process.

[0116] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0117] Figure 1 Flowchart of the method for adaptive manipulator-human-machine-object transfer based on point cloud under composite constraints of the present invention;

[0118] Figure 2 Schematic diagram of hand-eye calibration of the eye-on-hand robotic arm system of the adaptive robotic arm human-machine object transfer method based on point cloud under composite constraints of the present invention;

[0119] Figure 3 Schematic diagram of the three-dimensional point cloud FPFH feature description of the point cloud-based adaptive manipulator human-machine object transfer method under composite constraints of the present invention;

[0120] Figure 4 Schematic diagram of FPFH feature source points and their neighborhoods of the adaptive manipulator human-machine object transfer method based on point cloud under composite constraints of the present invention;

[0121] Figure 5 This is a rendering of the pose estimation results based on singular value decomposition of the point cloud-based adaptive manipulator human-machine object transfer method under composite constraints of the present invention. DETAILED DESCRIPTION

[0122] In order to make the purpose, technical solutions and advantages disclosed in the embodiments of the present invention clearer, the embodiments of the present invention are further described in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the embodiments of the present invention and are not intended to limit the embodiments of the present invention. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application. Examples of the embodiments are shown in the accompanying drawings, where the same or similar numbers throughout represent the same or similar elements or elements with the same or similar functions.

[0123] It should be noted that the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or server that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or are inherent to these processes, methods, products or devices.

[0124] Like reference numerals and letters denote like items in the following drawings, and thus, once an item is defined in one drawing, it does not require further definition or explanation in subsequent drawings.

[0125] As a three-dimensional representation of an object, a point cloud contains rich geometric information and can better describe the object's shape and surface features. Therefore, using point clouds for object recognition, grasping posture estimation, and target localization has become an important way to improve the adaptability of robotic arms.

[0126] On the other hand, by using a depth camera to obtain the user's point cloud data and using a posture estimation algorithm to analyze the user's state, a more comfortable human-computer interaction between the robotic arm and the user can be achieved.

[0127] Based on the above analysis, the present invention is designed as follows: Figure 1 As shown, the method for adaptive manipulator human-machine object transmission based on point cloud under composite constraints includes the following steps:

[0128] S1. Use the depth camera at the end of the robotic arm to obtain the point cloud of the object to be grasped and the point cloud of the user's body;

[0129] S2, based on the point cloud of the object to be grasped and the point cloud of the user's body obtained in step S1, respectively estimating the grasping posture of the object to be grasped and the object transfer posture of the user;

[0130] Step S2 specifically includes the following steps:

[0131] S21. Obtain the Euclidean rotation matrix between the robot's camera coordinate system and the robot's end tool coordinate system tool T cam , thereby determining the coordinate transformation relationship between the target point cloud in the depth camera and the robotic arm base;

[0132] It specifically includes the following steps:

[0133] S211. Set the closed-loop coordinate system spatial relationship of the hand-eye system of the robot arm to satisfy the following formula:

[0134] tool T cam = tool T base * base T cal *cal T cam (1)

[0135] Where, tool T cam is the Euclidean rotation matrix of the depth camera coordinate system relative to the tool coordinate system at the end of the robot arm; tool T base is the rotation matrix of the tool coordinate system at the end of the robot arm relative to the coordinate system of the robot arm base; base T cal is the rotation matrix of the calibration plate relative to the manipulator base coordinate system; cal T cam is the rotation matrix of the calibration plate relative to the depth camera coordinate system; in this embodiment, the following is used a T b The symbol represents the Euclidean transformation matrix from coordinate system a to coordinate system b, and like Figure 2 As shown, the camera of the eye-on-hand system is fixed at the end of the robotic arm, and the calibration plate is fixed in position, so base T cal and tool T cam is a fixed amount, and tool T cam The result of calibration solution.

[0136] It is deduced that:

[0137] base T cal = base T tool * tool T cam * cam T cal (2)

[0138] Where, base T tool is the rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm; tool T cam is the rotation matrix of the tool coordinate system at the end of the robot arm relative to the depth camera coordinate system; cam T cal is the rotation matrix of the depth camera coordinate system relative to the calibration plate;

[0139] S212, take the results twice at time t0 and time t1 respectively, and obtain:

[0140]

[0141] Where, is the rotation matrix of the manipulator base coordinate system relative to the manipulator end tool coordinate system at time t0; is the rotation matrix of the depth camera coordinate system relative to the calibration plate at time t0; is the rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm at time t1; is the rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm at time t1;

[0142] Multiply the left and right sides of formula (3) by and get:

[0143]

[0144] Where, is the rotation matrix of the tool coordinate system at the end of the manipulator relative to the manipulator base coordinate system at time t1; The rotation matrix of the calibration plate relative to the depth camera coordinate system at time t0;

[0145] Convert to:

[0146] AX=XB (5)

[0147] in:

[0148]

[0149]

[0150] X= tool T cam (8);

[0151] S213, use the Tsai-Lenz method to solve the Euclidean rotation matrix of the required depth camera coordinate system relative to the tool coordinate system of the robot end tool T cam Numerical solution of ;

[0152] S214, using the depth camera internal parameters and Euclidean rotation matrix tool T cam Convert the user's position coordinates to the robot arm base coordinate system:

[0153]

[0154]

[0155] Where, (u pixel ,v pixel ) is the user's position in the pixel coordinate system, which is the coordinate of the center point of the recognition frame; (x, y, z) is the three-dimensional coordinate of the user in the camera coordinate system; z is the depth value of the pixel where the user is located read in the depth map; f is the focal length of the depth camera; cx and c y are the optical center positions of the depth camera on the x-axis and y-axis respectively;

[0156] S22, respectively estimate the grasping posture of the object to be grasped and the object transmission posture of the user:

[0157] For the point cloud of the object to be grasped, a template matching method based on 2D image features is used to identify the target object to achieve segmentation of the 3D scene point cloud. Then, a point cloud matching method combining coarse matching and fine registration based on 3D point cloud features is used to estimate the transformation matrix, thereby adaptively determining the grasping position of the object to be grasped.

[0158] In step S22, as Figure 3 and Figure 4 As shown in the figure, for the point cloud of the object to be grasped, the two-dimensional SURF features are first used to calculate the feature points of the template image and the scene image respectively. Then, the Lowe algorithm and RANSAC algorithm are used to filter the feature points and calculate the homography transformation matrix for template matching to delineate the region of interest in the two-dimensional RGB image of the camera. The points in the region of interest are then converted into a point cloud containing both three-dimensional XYZ coordinates and RGB colors. Finally, the points are filtered according to the depth value constraint to achieve the segmentation of the three-dimensional environment point cloud.

[0159] Then, the SAC-IA point cloud registration method based on FPFH 3D point cloud features is used to achieve coarse matching between the scene object point cloud and the template point cloud. The point-to-point point cloud ICP algorithm is used to achieve fine registration between the scene object point cloud and the template point cloud. The Euclidean transformation matrix of the template point cloud and the scene object point cloud is obtained using a point cloud matching method. The Euclidean transformation matrix is ​​applied to the corresponding manual optimal grasping pose to calculate the optimal grasping pose of the current scene object.

[0160] The point cloud matching steps are as follows:

[0161] First, the FPFH features of all points in the point cloud are calculated. That is, each point in the point cloud is used as the current source point and all points within a certain neighborhood to calculate the FPFH features and express them through histogram statistics. A coordinate system is established based on the normal vectors and connecting lines of the source point and a neighboring point in the neighborhood. The direction calculation formula of the three axes of the coordinate system is as follows:

[0162] u=n s (11)

[0163] v=(P t -P s )×u (12)

[0164] w=u×v (13)

[0165] Where u, v, and w represent the three axes of the coordinate system respectively; P s is the source point of the current calculation feature; P t P s One of the points in the neighborhood; n s Point P s Normal vector of (P t -P s ) represents point P t To point P s The symbol × represents the cross product of vectors;

[0166] According to the three-axis coordinate system, the following three eigenvalues ​​are calculated:

[0167] α=v·n t (14)

[0168]

[0169] θ=arctan(w·n t ,u·n t ) (16)

[0170] Where α is the v-axis and point P t The angle eigenvalue of the normal vector; φ is the angle eigenvalue of the line connecting the two points of the source normal vector; θ is the eigenvalue describing the direction of the u-axis, w-axis and the normal vector of the adjacent point; n t Point P t Normal vector of ||P t -P s || represents point P t To point P s The Euclidean distance of Represents point P t To point P s The unit vector in the direction of the line connecting ;

[0171] Then, the three eigenvalues ​​of α, φ, and θ are calculated for all neighboring points in the neighborhood of the source point, and statistics are performed through histograms. The method of dividing the three eigenvalue ranges into equal parts is used to form a multidimensional histogram feature, which is recorded as SPF (P s ),like Figure 4 As shown in the solid line range, in this embodiment, the method of dividing the three feature value ranges into 11 equal parts is used to form a total of 33-dimensional histogram features. For k neighboring points P t , and calculate SPF(P t ),like Figure 4 As shown in the dotted line, and press the neighboring point P t The distance calculation weight from the source point is added to SPF (P s ), the FPFH feature FPFH(P s ):

[0172]

[0173] In the formula, represents ω k The i-th neighbor point P i and source point P s distance;

[0174] After calculating the FPFH features of the template point cloud and the scene object point cloud respectively, the SAC-IA point cloud registration method is used to randomly select a set number of feature matching points, and the Euclidean rotation matrix and matching error of the current matching situation are calculated;

[0175] Different feature matching point sets are selected through a set number of iterations, and the Euclidean rotation matrix with the minimum matching error is used as the result of point cloud coarse registration;

[0176] Finally, the point-to-point point cloud ICP algorithm is used to achieve precise registration of the scene object point cloud and the template point cloud: the scene object point cloud, the template point cloud and the coarse registration Euclidean transformation matrix are input into the point-to-point point cloud ICP algorithm, and the new Euclidean transformation matrix is ​​iteratively calculated based on the coarse registration transformation until the set error requirement is met.

[0177] For the user's human body point cloud, a combined point cloud filtering and clustering method is used to screen and segment the three-dimensional human body point cloud in the scene, and then a method based on the singular value decomposition of the point cloud matrix is ​​used to estimate the human body posture, thereby adaptively determining the user's object transmission posture.

[0178] In step S22, the user's body point cloud is filtered and segmented using a combined point cloud filtering and clustering method. The specific steps are as follows:

[0179] First, define the distance threshold T dist , its value is close to the radius of the robot's working space, and the distance is greater than T dist Delete the point cloud to achieve rough segmentation of the environment point cloud;

[0180] Then, the voxel filtering method is used to downsample the roughly segmented environment point cloud: the voxel filtering method divides the space into equal parts with a radius of r voxel For a cube voxel with point cloud distribution, the centroid of all point clouds in the voxel is calculated and stored as a new point cloud in the voxel, so as to reduce the number of point clouds while keeping the shape and characteristics of the original point cloud as much as possible;

[0181] The centroid calculation formula for all points in any voxel grid is as follows:

[0182]

[0183]

[0184]

[0185] The point cloud is then denoised using a statistical outlier filter:

[0186] For a point cloud P with N number of points, P = {p1, p2, ..., p n Any point p in i , define point p i The k nearest neighboring points nearby are its neighborhood, and the distance from the point to all points in its neighborhood is used as a statistical sample, which satisfies the normal distribution and has a sample mean and sample variance;

[0187] Among them, point p i The sample distance M to all points in the neighborhood i The calculation formula is as follows:

[0188]

[0189] Where k is the number of neighboring points in the neighborhood; p j Represents point p i The jth point in the neighborhood; ||p i -p j || represents point p i To point p j distance;

[0190] Then calculate the point p separately i The sample variance D of all points in the neighborhood i :

[0191]

[0192] Define the outlier threshold T o , when the outlier threshold T o When the following conditions are met, it is considered that point p i To count outliers, they are filtered out by the filter:

[0193] T o <M i +λ D D i (twenty three)

[0194] Where λ D is the multiple of standard deviation;

[0195] Finally, the human body point cloud is segmented using the clustering segmentation algorithm based on Euclidean distance:

[0196] For a point cloud P with N number of points, P = {p1, p2, ..., p n Any point p ini If the current point is not assigned to any cluster, a new cluster C is created. j , and point p i Assigned to cluster C j For a neighboring point p in its neighborhood k , calculate point p i and point p k The Euclidean distance dis(p i ,p k ):

[0197]

[0198] In the formula, (x i ,y i ,z i ) is point p i Coordinates of (x k ,y k ,z k ) is point p k coordinates of

[0199] Define a distance threshold T d , when the Euclidean distance dis(p i ,p k ) is less than the distance threshold T d When , it is considered to be the point of the same cluster and is assigned to cluster C j Middle; perform Euclidean distance threshold judgment on all points in the neighborhood of the point, and add new cluster C j , continue to iteratively search the neighborhood points of the newly added point until there are no new points; continue to traverse the points that are not assigned to any cluster until all points are assigned to the corresponding cluster.

[0200] It can be seen that the core idea of ​​the clustering segmentation algorithm based on Euclidean distance is to set the distance threshold T d This method assigns closely connected and closely distributed points in the point cloud to the same cluster. This effectively groups points in the point cloud into multiple clusters, leveraging the connectivity of adjacent points in the point cloud to form clusters. Experiments have shown that cluster segmentation algorithms based on Euclidean distance can effectively achieve precise segmentation of human point clouds.

[0201] Preferably, in step S22, as Figure 5 As shown in the figure, the human body posture is estimated using a method based on singular value decomposition of the point cloud matrix, so as to adaptively determine the user's object transmission posture. The specific steps are as follows:

[0202] By describing the three-dimensional coordinates of each point in the point cloud as a column vector, and then forming a matrix of the entire point cloud from the column vectors, singular value decomposition can be used to process it. By solving the characteristics of the point cloud matrix, singular value decomposition reveals the overall structure and directionality of the data and can represent the global properties of the entire point cloud data. Therefore, through singular value decomposition, accurate and stable eigenvectors can be obtained. These vectors represent the most important directions in the point cloud data. After subsequent processing, they are converted into the optimal object transmission posture corresponding to the current human body point cloud.

[0203] First, perform singular value decomposition on the point cloud to obtain a mathematical description of the main feature directions of the point cloud:

[0204] For N-dimensional data with M points, it is described in matrix form A M×N , A i is the i-th column vector, u is the column vector of the unit projection, the data is decentralized so that the mean of the processed point cloud is 0, and the main direction of the point cloud is set to the variance S in the data distribution 2 The unit vector e in the direction of the maximum is defined as follows:

[0205]

[0206] in

[0207]

[0208] because

[0209] ((A i ) T e) 2 =((A i ) T e) T ·((A i ) T ·e)=e T ·A i ·(A i ) T ·e (27)

[0210] Therefore

[0211]

[0212] Where, A M×N The covariance matrix of Recorded as C;

[0213] Then use the Lagrange multiplier method to solve the target optimization function:

[0214] L(e,λ)=e TCe+λ(1-e T e) (29)

[0215]

[0216] Where L(e,λ) represents the objective function of the Lagrange multiplier method;

[0217] Simplify equation (30) to

[0218] Ce=λe (31)

[0219] When the objective function takes an extreme value, λ is the largest eigenvalue of the covariance matrix C, and e is the eigenvector corresponding to the largest eigenvalue. At this time, solving the eigenvalue and eigenvector problem of the covariance matrix C is equivalent to solving the point cloud matrix A. M×N The singular value decomposition problem of A M×N The result of singular value decomposition is as follows:

[0220]

[0221] Where, For (A M×N ) T A M×N The eigenvector of the covariance matrix C; U M×M A M×N (A M×N ) T The eigenvector of and U M×M are all unit orthogonal vectors;

[0222] AndU M×M and The corresponding eigenvector in is expressed as ∑ M×N The singular values ​​are arranged in order, and the eigenvalues ​​of the covariance matrix C are ∑ M×N The squares of the singular values ​​in ;

[0223] Finally, the positive and negative directions of the desired point cloud directions are unified to form a three-dimensional Euclidean coordinate system, and the three-dimensional Euclidean coordinate system is regarded as the coordinate system of the current human body point cloud direction.

[0224] S3. Based on the grasping posture of the object to be grasped and the object transfer posture of the user estimated in step S2, the motion trajectory of the robot arm is planned under the composite constraints to realize adaptive robot arm human-machine object transfer planning.

[0225] In step S3, the human-machine object transfer task is set as follows: taking the object grasping posture as the starting point and the object transfer posture as the end point, the robot arm motion planning method is used to plan an effective path to achieve the object transfer task. For service robots, their working environment is mostly complex indoor scenes, and there are inevitably a large number of human-machine interaction processes. At the same time, there are different specific requirements for different task objects. The constraints in the object transfer robot arm motion planning process are set: passive chain constraints, environmental constraints, comfort constraints and task constraints.

[0226] Passive chain constraints refer to situations in which the movement of certain parts of a mechanical system or robotic control is constrained by the movement of other parts. These constraints are typically imposed by the system's mechanical structure and connections, rather than by the controller. For a six-degree-of-freedom robotic arm, each joint can move independently, but due to mechanical constraints, the movement of some joints may affect the movement of others.

[0227] Environmental constraints mainly come from complex obstacles in the indoor environment, such as tables, chairs, cabinets, etc. During the motion planning of the robotic arm, it is necessary to prevent the joints from colliding with obstacles.

[0228] The comfort constraint is based on the human-machine relationship. When the robot interacts with a human, the planning results of the motion planning algorithm do not invade the human's comfort space.

[0229] Task constraints refer to the constraints imposed by the task object on the robot's motion planning when performing a specific task. Task constraints often manifest as posture constraints. For example, when a robot is tasked with transporting a water cup, the end of the arm should grasp the cup in a forward-facing position (with the cup mouth facing up). At the same time, during the motion, the cup should exhibit minimal pitch and roll variations, with only yaw rotation at most. Only by ensuring these constraints can the robot ensure that the liquid in the cup does not spill during transport, thus meeting all the requirements of the task.

[0230] In step S3, the following constraint is added: the judgment condition based on the quaternion angle of the end of the manipulator is used to constrain the posture change of two adjacent path nodes during the movement process. The judgment condition is as follows:

[0231] 2arccos(Q n Q n′ )≤T Q (33)

[0232] Where Q n is the current node of the robot's motion path; Q n′ is the next node in the planned path, arccos is the inverse cosine function, T Qis the attitude change threshold. Since this condition directly calculates the quaternion of the robot end attitude, it limits the changes in the three-axis rotation. The planned path can be considered as the end remaining basically unchanged.

[0233] The system of the adaptive manipulator-human-machine object transmission method based on point cloud under composite constraints includes a manipulator, a depth camera is installed at the end of the manipulator, and the depth camera communicates with a computer. In this embodiment, the manipulator is a 6-DOF UR5e manipulator, the depth camera model is realsenseD435, and the computer is a NUC computer.

[0234] The computer includes:

[0235] The calibration module obtains the Euclidean transformation matrix between the robot's camera coordinate system and the robot's end tool coordinate system;

[0236] The perception and target detection module is used to use the depth camera to obtain the point cloud of the object to be grasped and the point cloud of the user's body, and to obtain the Euclidean transformation matrix between the camera coordinate system of the robot arm and the tool coordinate system at the end of the robot arm. tool T cam , thereby determining the coordinate transformation relationship between the target point cloud in the depth camera and the robotic arm base;

[0237] The object grasping pose estimation module is used to identify the target object based on the point cloud of the object to be grasped using a template matching method based on 2D image features to segment the 3D scene point cloud. It then uses a point cloud matching method that combines coarse matching and fine registration based on 3D point cloud features to estimate the transformation matrix, thereby adaptively determining the grasping pose of the object to be grasped.

[0238] The user transfer pose estimation module is used to filter and segment the 3D human point cloud in the scene based on the user's human point cloud using a combined point cloud filtering and clustering method. It then uses a method based on the singular value decomposition of the point cloud matrix to estimate the human pose, thereby adaptively determining the user's object transfer pose.

[0239] The robot arm motion planning module is used to realize adaptive robot arm human-machine object transfer planning under composite constraints based on the grasping posture of the object to be grasped and the object transfer posture of the user.

[0240] A machine-readable storage medium stores machine-executable instructions, which, when executed by one or more processors, implement the point cloud-based adaptive manipulator human-machine-object transfer method under composite constraints.

[0241] A data processing device includes a processor and a machine-readable storage medium, wherein the machine-readable storage medium stores machine-executable instructions. When the machine-executable instructions are executed by the processor, the method for adaptive manipulator-human-machine-object transfer based on point cloud under composite constraints is implemented.

[0242] Therefore, the present invention adopts the above-mentioned point cloud-based adaptive robotic arm human-machine object transfer method, system and data processing equipment under the above-mentioned composite constraints, combines point cloud matching and posture estimation, and realizes the adaptability and intelligence of the robotic arm in the process of grasping and object transfer. The estimated adaptive posture result has a small error and a high planning success rate to meet the complex working environment of the robotic arm.

[0243] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A point cloud-based adaptive manipulator human-machine object transfer method under complex constraints, characterized by: The following steps are involved: S1. Use the depth camera at the end of the robotic arm to obtain the point cloud of the object to be grasped and the point cloud of the user's body; S2, based on the point cloud of the object to be grasped and the point cloud of the user's body obtained in step S1, respectively estimating the grasping posture of the object to be grasped and the object transfer posture of the user; Step S2 specifically includes the following steps: S22, respectively estimating the grasping posture of the object to be grasped and the object transfer posture of the user; For the point cloud of the object to be grasped, a template matching method based on 2D image features is used to identify the object to be grasped, thereby achieving segmentation of the 3D scene point cloud. Then, a point cloud matching method combining coarse matching and fine registration based on 3D point cloud features is used to estimate the transformation matrix, thereby adaptively determining the grasping position of the object to be grasped. For the user's human body point cloud, a combined point cloud filtering and clustering method is used to filter and segment the 3D human body point cloud in the scene. Then, a method based on the singular value decomposition of the point cloud matrix is ​​used to estimate the human body posture, thereby adaptively determining the user's object transfer posture. In step S22, for the user's human body point cloud, a combined point cloud filtering and clustering method is used to implement the screening and segmentation of the 3D human body point cloud in the scene. The specific steps are as follows: First define the distance threshold , and the distance is greater than Delete the point cloud to achieve rough segmentation of the environment point cloud; Then, the voxel filtering method is used to downsample the roughly segmented environment point cloud: the voxel filtering method divides the space into equal parts with a radius of For a cube voxel with point cloud distribution, the centroid of all point clouds in the voxel is calculated and stored as a new point cloud in the voxel; The point cloud is then denoised using a statistical outlier filter: For the quantity Point cloud Any point in , defining point Nearby The nearest neighbor point is its neighborhood, and the distance from the point to all points in its neighborhood is used as a statistical sample, which satisfies the normal distribution and has a sample mean and sample variance; Among them, point Sample distances to all points in the neighborhood The calculation formula is as follows: (21) Where, is the number of neighboring points in the neighborhood; Indicates a point The first in the neighborhood points; Indicates a point Arrive distance; Then calculate the points separately The sample variance of all points in the neighborhood : (22) Defining the outlier threshold , when the outlier threshold When the following conditions are met, it is considered that the point To count outliers, they are filtered out by the filter: (23) Where, is the multiple of standard deviation; Finally, the human body point cloud is segmented using the clustering segmentation algorithm based on Euclidean distance: For the quantity Point cloud Any point in If the current point is not assigned to any cluster, a new cluster is created. , and point Assign to clusters For a neighbor point in its neighborhood , calculation point and point The Euclidean distance between : (24) Where, ( , , ) is a point The coordinates of , , ) is a point coordinates of Define a distance threshold , when the Euclidean distance between adjacent points Less than the distance threshold When , it is considered to be the point of the same cluster and is classified into the cluster Middle; perform Euclidean distance threshold judgment on all points in the neighborhood of the point, and , continue to iteratively search the neighboring points of the newly added point until there are no new points; continue to traverse the points that have not been assigned to any cluster until all points are assigned to the corresponding cluster; S3. Based on the grasping posture of the object to be grasped and the object transfer posture of the user estimated in step S2, the motion trajectory of the robot arm is planned under the composite constraints to realize adaptive robot arm human-machine object transfer planning.

2. The point cloud-based adaptive manipulator human-machine-object transfer method under complex constraints according to claim 1 is characterized by: Step S2 specifically includes the following steps: S21. Obtain the Euclidean rotation matrix between the tool coordinate system at the end of the robot arm and the camera coordinate system of the robot arm , thereby determining the coordinate transformation relationship between the point cloud of the object to be grasped in the depth camera and the base of the robotic arm; It specifically includes the following steps: S211. Set the closed-loop coordinate system spatial relationship of the hand-eye system of the robot arm to satisfy the following formula: (1) Where, is the Euclidean rotation matrix of the tool coordinate system at the end of the robot arm relative to the depth camera coordinate system; is the Euclidean rotation matrix of the tool coordinate system at the end of the robot arm relative to the coordinate system of the robot arm base; is the Euclidean rotation matrix of the manipulator base coordinate system relative to the calibration plate; is the Euclidean rotation matrix of the calibration plate relative to the depth camera coordinate system; It is deduced that: (2) Where, is the Euclidean rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm; is the Euclidean rotation matrix of the tool coordinate system at the end of the robot arm relative to the depth camera coordinate system; is the Euclidean rotation matrix of the depth camera coordinate system relative to the calibration plate; S212, respectively and Taking the results twice at the same time, we get: (3) Where, for The Euclidean rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm at this moment; for The Euclidean rotation matrix of the depth camera coordinate system relative to the calibration plate at this moment; for The Euclidean rotation matrix of the robot base coordinate system relative to the tool coordinate system at the end of the robot arm at this moment; for The Euclidean rotation matrix of the depth camera coordinate system relative to the calibration plate at this moment; Multiply the left and right sides of formula (3) by and get: (4) Where, for The Euclidean rotation matrix of the tool coordinate system at the end of the manipulator relative to the manipulator base coordinate system at this moment; for The Euclidean rotation matrix of the moment calibration plate relative to the depth camera coordinate system; Convert to: (5) in: (6) (7) (8)； S213, use the Tsai-Lenz method to solve the Euclidean rotation matrix of the tool coordinate system at the end of the robot arm relative to the depth camera coordinate system Numerical solution of ; S214, using the depth camera internal parameters and Euclidean rotation matrix Convert the user's position coordinates to the robot arm base coordinate system: (9) (10) Where, is the user's position in the pixel coordinate system, which is the coordinate of the center point of the recognition box; The user's three-dimensional coordinates in the depth camera coordinate system; The depth value of the pixel where the user is located is read from the depth map; is the focal length of the depth camera; and are the optical center positions of the depth camera on the x-axis and y-axis respectively; S22: Estimate the grasping posture of the object to be grasped and the object transfer posture of the user respectively.

3. The point cloud-based adaptive manipulator human-machine-object transfer method under complex constraints according to claim 2 is characterized by: In step S22, for the point cloud of the object to be captured, the two-dimensional SURF features are first used to calculate the feature points of the template image and the scene image respectively. The Lowe algorithm and the RANSAC algorithm are then used to filter the feature points and calculate the homography transformation matrix for template matching to delineate the region of interest in the two-dimensional RGB image of the camera. The points in the region of interest are then converted into a point cloud containing both three-dimensional XYZ coordinates and RGB colors. The points are then filtered according to the depth value constraint to achieve segmentation of the three-dimensional environment point cloud. Then, the SAC-IA point cloud registration method based on FPFH 3D point cloud features is used to achieve coarse matching between the scene object point cloud and the template point cloud. The point-to-point point cloud ICP algorithm is used to achieve fine registration between the scene object point cloud and the template point cloud. The Euclidean transformation matrix of the template point cloud and the scene object point cloud is obtained using a point cloud matching method. The Euclidean transformation matrix is ​​applied to the corresponding manual optimal grasping posture to calculate the optimal grasping posture of the object to be grasped. The point cloud matching steps are as follows: First, the FPFH features of all points in the point cloud are calculated. That is, each point in the point cloud is used as the current source point and all points within a certain neighborhood to calculate the FPFH features and express them through histogram statistics. A coordinate system is established based on the normal vectors and connecting lines of the source point and a neighboring point in the neighborhood. The direction calculation formula of the three-axis coordinate system is as follows: (11) (12) (13) Where, , , Represent the three axes of the coordinate system respectively; The source point of the current calculated feature; for One of the points in the neighborhood; for point The normal vector of Indicates a point Arrive The connection line of Represents the cross product of vectors; According to the three-axis coordinate system, the following three eigenvalues ​​are calculated: (14) (15) (16) Where, for Axis and Points The eigenvalue of the angle of the normal vector; is the eigenvalue of the angle between the two points of the source point normal vector; To describe axis, Eigenvalues ​​of the axis and normal vector directions of neighboring points; for point The normal vector of Indicates a point Arrive The Euclidean distance of Indicates a point Arrive The unit vector in the direction of the line connecting ; Then, all neighboring points in the neighborhood of the source point are calculated separately 、 、 Three eigenvalues ​​are counted through histograms. The three eigenvalue ranges are divided equally to form a multidimensional histogram feature, which is recorded as ,for Neighbors , and count them again , and press the neighboring point The distance calculation weight from the source point is added In the example, we get the FPFH feature of the source point : (17) Where, Indicates the Neighbors and source point distance; After calculating the FPFH features of the template point cloud and the scene object point cloud respectively, the SAC-IA point cloud registration method is used to randomly select a set number of feature matching points, and the Euclidean transformation matrix and matching error under the current matching situation are calculated; Different feature matching point sets are selected through a set number of iterations, and the Euclidean transformation matrix with the minimum matching error is used as the result of point cloud coarse registration; Finally, the point-to-point point cloud ICP algorithm is used to achieve precise registration of the scene object point cloud and the template point cloud: the scene object point cloud, the template point cloud and the coarse registration Euclidean transformation matrix are input into the point-to-point point cloud ICP algorithm, and the new Euclidean transformation matrix is ​​iteratively calculated based on the coarse registration transformation until the set error requirement is met.

4. The method for adaptive manipulator-human-machine-object transfer based on point cloud under complex constraints according to claim 3 is characterized by: In step S22, the human body posture is estimated using a method based on singular value decomposition of the point cloud matrix, thereby adaptively determining the user's object transfer posture. The specific steps are as follows: First, perform singular value decomposition on the point cloud to obtain a mathematical description of the main feature directions of the point cloud: For those with points dimensional data, described in matrix form , is the i-th column vector, the data is decentralized so that the mean of the processed point cloud is 0, and the main direction of the point cloud is set to the variance in the data distribution Unit vector in the direction of maximum , define the following objective function: (25) in (26) because (27) Therefore (28) Where, for The covariance matrix of , recorded as ; Then use the Lagrange multiplier method to solve the target optimization function: (29) (30) Where, represents the objective function of the Lagrange multiplier method; Simplify equation (30) to (31) When the objective function takes an extreme value, is the covariance matrix The largest eigenvalue, is the eigenvector corresponding to the maximum eigenvalue, and the covariance matrix is ​​solved at this time The eigenvalue and eigenvector problem is equivalent to solving the point cloud matrix The singular value decomposition problem of The result of singular value decomposition is as follows: (32) Where, for The eigenvectors of The eigenvector of for The eigenvector of and are all unit orthogonal vectors; and and The corresponding eigenvectors in Singular value size arrangement, covariance matrix The characteristic value of The squares of the singular values ​​in ; Finally, the positive and negative directions of the desired point cloud directions are unified to form a three-dimensional Euclidean coordinate system, and the three-dimensional Euclidean coordinate system is regarded as the coordinate system of the current human body point cloud direction.

5. The method for adaptive manipulator-human-machine-object transfer based on point cloud under complex constraints according to claim 4, characterized in that: In step S3, the human-machine object transfer task is set as follows: taking the object grasping posture as the starting point and the object transfer posture as the end point, the robot arm motion planning method is used to plan an effective path to achieve the object transfer task, and the constraints in the object transfer robot arm motion planning process are set: passive chain constraints, environmental constraints, comfort constraints and task constraints.

6. The method for adaptive manipulator-human-machine-object transfer based on point cloud under complex constraints according to claim 5, characterized in that: In step S3, the following constraint is added: the judgment condition based on the quaternion angle of the end of the manipulator is used to constrain the posture change of two adjacent path nodes during the movement process. The judgment condition is as follows: (33) Where, is the current node of the robot's motion path; is the next node in the planned path, is the inverse cosine function, is the posture change threshold.

7. The system for the point cloud-based adaptive manipulator-human-machine-object transfer method under complex constraints according to any one of claims 1 to 6, comprising a manipulator with a depth camera mounted at the end of the manipulator, the depth camera communicating with a computer, and characterized in that: The computer includes: The calibration module obtains the Euclidean rotation matrix between the robot's camera coordinate system and the robot's end tool coordinate system; The perception and target detection module is used to use the depth camera to obtain the point cloud of the object to be grasped and the point cloud of the user's body, and to obtain the Euclidean rotation matrix between the tool coordinate system at the end of the robot arm and the camera coordinate system of the robot arm. , thereby determining the coordinate transformation relationship between the point cloud of the object to be grasped in the depth camera and the base of the robotic arm; The object grasping pose estimation module is used to identify the object to be grasped based on its point cloud using a template matching method based on 2D image features to segment the 3D scene point cloud. It then uses a point cloud matching method that combines coarse matching and fine registration based on 3D point cloud features to estimate the transformation matrix, thereby adaptively determining the grasping pose of the object to be grasped. The user transfer pose estimation module is used to filter and segment the 3D human point cloud in the scene based on the user's human point cloud using a combined point cloud filtering and clustering method. It then uses a method based on the singular value decomposition of the point cloud matrix to estimate the human pose, thereby adaptively determining the user's object transfer pose. The robot arm motion planning module is used to realize adaptive robot arm human-machine object transfer planning under composite constraints based on the grasping posture of the object to be grasped and the object transfer posture of the user.

8. A machine-readable storage medium having machine-executable instructions stored thereon, wherein the machine-executable instructions, when executed by one or more processors, implement the point cloud-based adaptive robotic arm human-machine object transfer method under the composite constraints of any one of claims 1-6.

9. A data processing device, comprising a processor and a machine-readable storage medium, wherein the machine-readable storage medium stores machine-executable instructions, and when the machine-executable instructions are executed by the processor, the method for adaptive manipulator-based human-machine-object transfer under composite constraints according to any one of claims 1 to 6 is implemented.

Images