A robot arm end trajectory planning method for autonomous spraying

Abstract

The present invention discloses a method for planning the trajectory of a robotic arm end for autonomous spraying, which belongs to the field of robotic arm end trajectory planning and includes the following steps: S1, determining a grid starting point; S2, sampling to obtain a three-dimensional voxel grid based on the determined grid starting point, and determining path feature points in a point cloud based on the center of gravity of the three-dimensional voxel grid; S3, calculating the normal vectors of the path feature points in the point cloud using a principal component analysis method; S4, constructing a bidirectional non-uniform B-spline curve based on the normal vectors of the path feature points in the point cloud calculated in step S3, and performing smoothing; S5, generating a robotic arm end trajectory based on the bidirectional non-uniform B-spline curve constructed in step S4. The present invention adopts the above-mentioned method for planning the trajectory of a robotic arm end for autonomous spraying, which can generate a smooth and continuous robotic arm end trajectory, thereby improving the stability, dexterity, and Cartesian stiffness of the robotic arm joint rotation during the actual spraying process.

Description

Technical Field

[0001] The present invention relates to the field of robot arm end trajectory planning, and in particular to a robot arm end trajectory planning method for autonomous spraying. Background Art

[0002] Existing robot arm spray trajectory planning methods are mainly divided into active planning and passive tracking. Among them, active planning methods are mostly based on pre-drawn precise CAD models and cannot make adaptive adjustments for non-standardized targets, so their versatility is poor. In addition, for complex heterogeneous targets, multiple robot arms need to collaborate to complete the task, which is costly and lacks versatility. The passive tracking method has the following steps: first, the end of the robot arm is manually guided to achieve the spray trajectory, and then the robot arm is controlled to repeat this trajectory to complete the task. However, the spraying quality of this method depends largely on manual experience, and there are problems such as difficult quality control, low spraying accuracy, and a harsh working environment that is harmful to the health of employees.

[0003] To address the above issues, existing technologies have further developed a robotic arm spray trajectory planning method based on a measured point cloud model. This method, with the help of the point cloud model obtained from actual measurements, can achieve adaptive adjustment for non-standard objects, greatly expanding the spray target range. However, it still has the following shortcomings:

[0004] (1) The sampling of path feature points is unreasonable. Uneven or unrepresentative feature points will greatly reduce the spraying quality.

[0005] (2) The spraying trajectory is highly dependent on the surface points of the object and does not consider the posture of the end of the robot arm, which can easily cause the joint of the robot arm to collide with the target;

[0006] (3) The spraying trajectory lacks smooth processing at the turns, and the robot arm joints are prone to angle jumps, which further creates safety hazards. Summary of the Invention

[0007] To solve the above problems, the present invention provides a robot arm end trajectory planning method for autonomous spraying, which can generate a smooth and continuous robot arm end trajectory, thereby improving the stability, dexterity and Cartesian stiffness of the robot arm joint rotation during the actual spraying process.

[0008] To achieve the above object, the present invention provides a robot arm end trajectory planning method for autonomous spraying, comprising the following steps:

[0009] S1. Based on the target model point cloud, construct the AABB bounding box of the object point cloud to determine the grid starting point;

[0010] S2. Using the grid starting point determined in step S1 as a reference, sampling is performed to obtain a three-dimensional voxel grid, and path feature points in the point cloud are determined based on the center of gravity of the three-dimensional voxel grid;

[0011] S3, using principal component analysis to calculate the normal vectors of the path feature points in the point cloud determined in step S2;

[0012] S4. Based on the normal vectors of the path feature points in the point cloud calculated in step S3, a bidirectional non-uniform B-spline curve is constructed and smoothed.

[0013] S5. Generate a robot arm end trajectory based on the bidirectional non-uniform B-spline curve constructed in step S4.

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

[0015] S21. Set the grid side length to cell and divide the point cloud into grids N voxel for:

[0016]

[0017] Where x min 、y min 、z min are the minimum extreme values ​​of the point cloud in the X, Y, and Z directions respectively; x max 、y max 、z max are the maximum extreme values ​​of the point cloud in the X, Y, and Z directions respectively;

[0018] S22. Assuming that the number of points p contained in each 3D voxel grid is h, and that the weight of each point in the 3D voxel grid is set to be the same, calculate the centroid of the 3D voxel grid:

[0019]

[0020] Where p e is the center of gravity of the 3D voxel grid; p i is the i-th point in the 3D voxel grid;

[0021] S23. Calculate the path feature points in the point cloud within the 3D voxel grid:

[0022]

[0023] Where p f are path feature points in the point cloud within the 3D voxel grid.

[0024] Preferably, step S3 specifically includes the following steps:

[0025] S31, assuming that the path feature point p in the point cloud within the three-dimensional voxel gridf Around k f The coordinates of the neighboring points are p j =(x j ,y j ,z j ) T ,j=1,2,…,k f , build a decentralized point set:

[0026] Δp j =p j -p f ,j=1,2,…,k f (4)

[0027] Where Δp j Represents a decentralized point set;

[0028] S32. Calculate the covariance matrix:

[0029]

[0030] Where C represents the covariance matrix;

[0031] S33. Perform eigenvalue decomposition on the covariance matrix C to obtain eigenvalues ​​λ1>λ2>λ3 and their corresponding unit eigenvectors v1, v2, v3, which satisfy the following conditions:

[0032] Cv l =λ l v l ,l=1,2,3 (6)

[0033] S34. Calculate the path feature point p in the point cloud f The normal vector of :

[0034]

[0035] Where n is the path feature point p in the point cloud f The normal vector of

[0036] Preferably, step S4 specifically includes the following steps:

[0037] S41, assuming the control point P in the point cloud S ={p S1 ,p S2 ,…,p Sn}, then the B-spline curve expression is as follows:

[0038]

[0039] in

[0040]

[0041] Where C(u) is the B-spline curve expression function; n is the number of control points; N i,1 (u) is the i-th first-order basis function; u is the node vector; u i is the i-th node; u i+1 is the i+1th node; u i+k-1 is the i+k-1th node; u i+k is the i+kth node; N i,k-1 (u) is the i-th k-1 order basis function; N i+1,k-1 (u) is the i+1th k-1th order basis function; N i,k (u) is the i-th k-order basis function; k is the order of the B-spline curve;

[0042] S42. Calculate the node vector:

[0043] u={u0,u1,…,u m}, u0≤u1≤u2≤…≤u m (10)

[0044] Where u0,u1,…,u m They are the 1st, 2nd, ..., mth nodes respectively;

[0045] S43. Map equation (8) to a high-dimensional space and calculate the homogeneous coordinates to obtain a bidirectional non-uniform B-spline curve:

[0046]

[0047] Where C PS (u) is the control point P S Bidirectional non-uniform B-spline curve of ω; i is the weight value of the control point, k is the order of the bidirectional non-uniform B-spline curve;

[0048] S44, based on the control point P in the point cloud S ={p S1 ,p S2 ,…,p Sn}N={n1,n2,…,n n} in the positive direction, and get the control point Q S ={q S1 ,q S2 ,…,q Sn}, where q Si =p Si +b, and the offset distance b is defined as the distance that the end effector extends beyond the end flange of the robot arm. S Perform bidirectional non-uniform B-spline curve fitting and obtain:

[0049]

[0050] Where C QS (u) is the control point Q S The bidirectional non-uniform B-spline curve expression function;

[0051] S45, Synchronous Setting C QS (u) and C PS The parameter u of (u).

[0052] Preferably, in step S43 and step S44, the order k of the bidirectional non-uniform B-spline curve is ≥3.

[0053] Preferably, step S5 specifically includes the following steps:

[0054] S51, for C PS Each path point in (u) is QS (u) and search for the corresponding path point, and use C QS (u) is the translation vector of the end of the manipulator, and C QS (u) points to C PS The direction vector (u) is used as the end pose;

[0055] S52. According to the definition of the rotation axis and axis angle of the quaternion, C QS (u) points to C PS (u) The direction vector is mapped to a quaternion, and the quaternion is expanded to a rotation matrix;

[0056] S53. Generate the end trajectory of the robotic arm based on the rotation matrix.

[0057] Preferably, step S52 specifically includes the following steps:

[0058] S521. Assume that the normal vector is a = (x, y, z), define the unit vector i = (1, 0, 0), and after normalizing the normal vector a and the unit vector i, calculate the rotation angle θ between the normal vector and the unit vector using the vector dot product formula:

[0059]

[0060] S522. Calculate the rotation axis n between the normal vector and the unit vector using the vector cross product formula:

[0061] n=(n x ,n y ,n z )=a×i=(0,-z,y) (14)

[0062] Where n x ,n y ,nz are the components of vector n in three directions respectively;

[0063] S523. From the definition of quaternion:

[0064]

[0065] Where q represents a quaternion;

[0066] Expanding quaternions to rotation matrices E R (u):

[0067]

[0068] Where (q1, q2, q3, q4) are the four components of the quaternion q.

[0069] Preferably, in step S53, the generated robot arm end trajectory expression is:

[0070] E(u)=E T (u)+E R (u) (17)

[0071] Where, E(u) is the posture matrix of the end of the robot arm; E T (u) is the translation vector, and

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

[0073] (1) By introducing a bidirectional non-uniform B-spline curve, a spraying trajectory with high coverage, smoothness and continuity can be generated;

[0074] (2) Based on the point cloud, the grid starting point is determined and the voxel grid based on the center of gravity is used to sample the path feature points, so that the distribution of the feature points is more uniform and reasonable, ensuring that the grid is uniformly generated in the point cloud, thereby ensuring the uniformity of the cleaning trajectory;

[0075] (3) Use PCA (principal component analysis) to calculate its normal vector, map it to a bidirectional non-uniform B-spline curve, and further calculate the end posture based on the curve point pairs to ensure that the end of the robotic arm is always perpendicular to the object surface;

[0076] (4) A bidirectional non-uniform B-spline curve is introduced. Based on the bidirectional non-uniform B-spline curve, the end posture of the manipulator is calculated using corresponding path point pairs. This not only determines the position information of the end of the manipulator in the trajectory, but also plans the posture information of the end of the manipulator, ensuring the continuity and smoothness of the trajectory to achieve better fit to the target surface.

[0077] (5) The application scenarios of non-uniform B-spline curves are expanded. First, a non-uniform B-spline curve is constructed based on the path feature points. Then, the distance points are offset in the direction of the normal vector to construct a second curve, so that the normal vector of the feature point has a more concrete representation.

[0078] In summary, the present invention can reasonably sample path feature points based on the point cloud of the target object according to the grid centroid, calculate the normal vector of the feature point with the help of PCA, and then introduce non-uniform B-spline curves to construct a bidirectional non-uniform B-spline curve based on the feature points and their normal vectors. The end posture is determined by the path point pairs, and finally the end trajectory of the robot arm is generated. A smooth and continuous end trajectory of the robot arm can be generated, which improves the stability, dexterity and Cartesian stiffness of the robot arm joint rotation during the actual spraying process.

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

[0080] Figure 1 This is a flow chart of a method for trajectory planning of a robotic arm end for autonomous spraying according to the present invention;

[0081] Figure 2 Schematic diagram of an AABB bounding box of a robot arm end trajectory planning method for autonomous spraying according to the present invention;

[0082] Figure 3 A schematic diagram of a bidirectional non-uniform B-spline curve for a trajectory planning method of a robotic arm end for autonomous spraying according to the present invention;

[0083] Figure 4 A schematic diagram of the end trajectory of a robotic arm in a method for planning the end trajectory of a robotic arm for autonomous spraying according to the present invention. DETAILED DESCRIPTION

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

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

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

[0087] like Figures 1-4 As shown, a robot arm end trajectory planning method for autonomous spraying includes the following steps:

[0088] S1. Based on the target model point cloud, construct the AABB bounding box of the object point cloud to determine the grid starting point;

[0089] In this embodiment, the minimum vertex of the AABB bounding box is used as the grid construction starting point P s =(X min ,Y min ,Z min ), Figure 2 The middle grid point is the starting point of the grid.

[0090] S2. Using the grid starting point determined in step S1 as a reference, sample to obtain a 3D voxel grid, and determine the path feature points in the point cloud based on the center of gravity of the 3D voxel grid (for a 3D point cloud grid, use the point closest to the grid center of gravity to replace all points in the grid, thereby obtaining the path feature points in the point cloud);

[0091] Step S2 specifically includes the following steps:

[0092] S21. Set the grid side length to cell and divide the point cloud into grids N voxel for:

[0093]

[0094] Where x min 、y min 、z min are the minimum extreme values ​​of the point cloud in the X, Y, and Z directions respectively; x max 、y max 、z max are the maximum extreme values ​​of the point cloud in the X, Y, and Z directions respectively;

[0095] S22. Assuming that the number of points p contained in each 3D voxel grid is h, and that the weight of each point in the 3D voxel grid is set to be the same, calculate the centroid of the 3D voxel grid:

[0096]

[0097] Where p e is the center of gravity of the 3D voxel grid; p i is the i-th point in the 3D voxel grid;

[0098] S23. Calculate the path feature points in the point cloud within the 3D voxel grid:

[0099]

[0100] Where p f are path feature points in the point cloud within the 3D voxel grid.

[0101] S3, using principal component analysis to calculate the normal vectors of the path feature points in the point cloud determined in step S2;

[0102] Step S3 specifically includes the following steps:

[0103] S31, assuming that the path feature point p in the point cloud within the three-dimensional voxel grid f Around k f The coordinates of the neighboring points are p j =(x j ,y j ,z j ) T ,j=1,2,…,k f , build a decentralized point set:

[0104] Δp j =p j -p f ,j=1,2,…,k f (4)

[0105] Where Δp j Represents a decentralized point set;

[0106] S32. Calculate the covariance matrix:

[0107]

[0108] Where C represents the covariance matrix;

[0109] S33. Perform eigenvalue decomposition on the covariance matrix C to obtain eigenvalues ​​λ1>λ2>λ3 and their corresponding unit eigenvectors v1, v2, v3, which satisfy the following conditions:

[0110] Cvl =λ l v l ,l=1,2,3 (6)

[0111] S34. Calculate the path feature point p in the point cloud f The normal vector of :

[0112]

[0113] Where n is the path feature point p in the point cloud f The normal vector of

[0114] S4. Based on the normal vectors of the path feature points in the point cloud calculated in step S3, a bidirectional non-uniform B-spline curve is constructed and smoothed.

[0115] Step S4 specifically includes the following steps:

[0116] S41, assuming the control point P in the point cloud S ={p S1 ,p S2 ,…,p Sn}, then the B-spline curve expression is as follows:

[0117]

[0118] in

[0119]

[0120] Where C(u) is the B-spline curve expression function; n is the number of control points; N i,1 (u) is the i-th first-order basis function; u is the node vector; u i is the i-th node; u i+1 is the i+1th node; u i+k-1 is the i+k-1th node; u i+k is the i+kth node; N i,k-1 (u) is the i-th k-1 order basis function; N i+1,k-1 (u) is the i+1th k-1th order basis function; N i,k (u) is the i-th k-order basis function; k is the order of the B-spline curve, corresponding to the (k-1)-order basis function;

[0121] S42. Calculate the node vector:

[0122] u={u0,u1,...,u m}, u0≤u1≤u2≤…≤u m (10)

[0123] Where u0,u1,...,u mThey are the 1st, 2nd, ..., mth nodes respectively;

[0124] S43. Map equation (8) to a high-dimensional space and calculate the homogeneous coordinates to obtain a bidirectional non-uniform B-spline curve:

[0125]

[0126] Where, is the control point P S Bidirectional non-uniform B-spline curve of ω; i is the weight value of the control point, k is the order of the bidirectional non-uniform B-spline curve;

[0127] S44, based on the control point P in the point cloud S ={p S1 ,p S2 ,…,p Sn}N={n1,n2,…,n n} in the positive direction, and get the control point Q S ={q S1 ,q S2 ,…,q Sn}, where q Si =p Si +b, and the offset distance b is defined as the distance that the end effector extends beyond the end flange of the robot arm. S Perform bidirectional non-uniform B-spline curve fitting and obtain:

[0128]

[0129] Where, is the control point Q S The bidirectional non-uniform B-spline curve expression function;

[0130] In step S43 and step S44, in order to maintain the continuity of the second-order derivative of the trajectory, the order k of the bidirectional non-uniform B-spline curve is ≥3.

[0131] S45. In order to obtain a continuous trajectory of the robot end, the path points on the two curves need to correspond one by one and be set synchronously. and Parameter u of .

[0132] S5. Generate a robot arm end trajectory based on the bidirectional non-uniform B-spline curve constructed in step S4.

[0133] Step S5 specifically includes the following steps:

[0134] S51, for For each path point in Search for the corresponding path point in As the translation vector of the end of the robot arm, point to The direction vector of is taken as the end posture;

[0135] S52. Due to point to The direction vector of has only two degrees of freedom, and the rotation matrix is ​​defined as three degrees of freedom. Therefore, according to the definition of the rotation axis and axis angle of the quaternion, point to The direction vector is mapped to a quaternion and the quaternion is expanded to a rotation matrix;

[0136] Step S52 specifically includes the following steps:

[0137] S521. Assume that the normal vector is a = (x, y, z), define the unit vector i = (1, 0, 0), and after normalizing the normal vector a and the unit vector i, calculate the rotation angle θ between the normal vector and the unit vector using the vector dot product formula:

[0138]

[0139] S522. Calculate the rotation axis n between the normal vector and the unit vector using the vector cross product formula (which only represents the rotation direction in the rigid body rotation):

[0140] n=(n x ,n y ,n z )=a×i=(0,-z,y) (14)

[0141] Where n x ,n y ,n z are the components of vector n in three directions respectively;

[0142] S523. From the definition of quaternion:

[0143]

[0144] Where q represents a quaternion;

[0145] Expanding quaternions to rotation matrices E R (u):

[0146]

[0147] Where (q1, q2, q3, q4) are the four components of the quaternion q.

[0148] S53. Generate the end trajectory of the robotic arm based on the rotation matrix.

[0149] In step S53, the generated robot arm end trajectory expression is:

[0150] E(u)=E T (u)+E R (u) (17)

[0151] Where, E(u) is the posture matrix of the end of the robot arm; E T (u) is the translation vector, and

[0152] Therefore, the present invention adopts the above-mentioned robot arm end trajectory planning method for autonomous spraying, which can generate a smooth and continuous robot arm end trajectory, thereby improving the stability, dexterity and Cartesian stiffness of the robot arm joint rotation during actual spraying applications.

[0153] 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 robot arm end trajectory planning method for autonomous spraying, characterized by: The following steps are involved: S1. Based on the target model point cloud, construct the AABB bounding box of the object point cloud to determine the grid starting point; S2. Using the grid starting point determined in step S1 as a reference, sampling is performed to obtain a three-dimensional voxel grid, and path feature points in the point cloud are determined based on the center of gravity of the three-dimensional voxel grid; S3, using principal component analysis to calculate the normal vectors of the path feature points in the point cloud determined in step S2; S4. Based on the normal vectors of the path feature points in the point cloud calculated in step S3, a bidirectional non-uniform B-spline curve is constructed and smoothed. Step S4 specifically includes the following steps: S41. Assume control points in point cloud , then the B-spline curve expression is as follows: （8） in （9） Where, Express the function as a B-spline curve; is the number of control points; For the 1st-order basis functions; is the node vector; For the nodes; For the nodes; For the nodes; For the nodes; For the indivual order basis functions; For the indivual order basis functions; For the indivual order basis functions; is the order of the B-spline curve; S42. Calculate the node vector: （10） Where, Respectively nodes; S43. Map equation (8) to a high-dimensional space and calculate the homogeneous coordinates to obtain a bidirectional non-uniform B-spline curve: （11） Where, For control points Bidirectional non-uniform B-spline curve; is the control point weight value, is the order of the bidirectional non-uniform B-spline curve; S44, based on control points in point cloud Normal vector Positive offset distance , get the control point set ,in , and the offset distance It is defined as the distance that the end effector extends beyond the end flange of the robot arm. Perform bidirectional non-uniform B-spline curve fitting and obtain: （12） Where, A set of control points The bidirectional non-uniform B-spline curve expression function; S45. Synchronous settings and Parameters ; S5. Generate a trajectory of the end of the robotic arm based on the bidirectional non-uniform B-spline curve constructed in step S4.

2. A robot arm end trajectory planning method for autonomous spraying according to claim 1, characterized in that: Step S2 specifically includes the following steps: S21, set the grid side length to , divide the point cloud into the number of grids for: （1） Where, 、 、 are the minimum extreme values ​​of the point cloud in the X, Y, and Z directions respectively; 、 、 are the maximum extreme values ​​of the point cloud in the X, Y, and Z directions respectively; S22, assuming that each 3D voxel grid contains points The number of , and set the weight of each point in the 3D voxel grid to be the same, and calculate the center of gravity of the 3D voxel grid: （2） Where, is the centroid of the 3D voxel grid; The first points; S23. Calculate the path feature points in the point cloud within the 3D voxel grid: （3） Where, are path feature points in the point cloud within the 3D voxel grid.

3. The method for trajectory planning of a robot arm end for autonomous spraying according to claim 2, characterized in that: Step S3 specifically includes the following steps: S31. Assume that the path feature points in the point cloud within the 3D voxel grid around The coordinates of the neighboring points are , build a decentralized point set: （4） Where, Represents a decentralized point set; S32. Calculate the covariance matrix: （5） Where, represents the covariance matrix; S33, covariance matrix Perform eigenvalue decomposition to obtain eigenvalues and its corresponding unit eigenvector , the following conditions are met: （6） S34. Calculate path feature points in point cloud The normal vector of : （7） Where, is the path feature point in the point cloud The normal vector of .

4. The method for trajectory planning of a robot arm end for autonomous spraying according to claim 3, characterized in that: The order of the bidirectional non-uniform B-spline curve in steps S43 and S44 is .

5. The method for trajectory planning of a robot arm end for autonomous spraying according to claim 4, characterized in that: Step S5 specifically includes the following steps: S51, for For each path point in Search for the corresponding path point in As the translation vector of the end of the robot arm, point to The direction vector of is taken as the end posture; S52. According to the definition of the rotation axis and axis angle of the quaternion, point to The direction vector is mapped to a quaternion and the quaternion is expanded to a rotation matrix; S53. Generate the end trajectory of the robotic arm based on the rotation matrix.

6. The method for trajectory planning of a robot arm end for autonomous spraying according to claim 5, characterized in that: Step S52 specifically includes the following steps: S521、Set point to The vector is , define the unit vector , and for the vector With unit vector After normalization, the vector is calculated by the vector dot product formula With unit vector The rotation angle between : （13） S522, calculate vector by vector cross product formula With unit vector Vector between : （14） Where, are vectors Components in three directions; S523. From the definition of quaternion: （15） Where, Represents a quaternion; Extending quaternions to rotation matrices : （16） Where, Quaternions The four components of .

7. The method for trajectory planning of a robot arm end for autonomous spraying according to claim 6, characterized in that: In step S53, the generated robot arm end trajectory expression is: （17） Where, is the posture matrix of the end of the robotic arm; is the translation vector, and .

Images