A DMP-based Grinding Trajectory Planning Method for Complex Surfaces

Abstract

The present invention provides a complex surface grinding trajectory planning method based on DMP, including obtaining point cloud information within the grinding area by using a depth camera; planning a grinding path in the point cloud data of the segmented grinding workpiece; calculating the normal vectors and curvatures of each point on the grinding path, and calculating the posture of the grinding head at the grinding point according to the normal vectors; segmenting the grinding path based on the curvatures of each point on the grinding path, and dividing the entire grinding path trajectory into multiple segment trajectories; for each segment trajectory, respectively modeling the position and posture, and curvature of the trajectory by using position DMP1, direction DMP2, and curvature DMP3; the present invention can achieve trajectory segmentation, trajectory modeling, and adaptive adjustment of the grinding speed; using point cloud and DMP to plan the grinding path greatly reduces the workload of people during complex surface grinding, improves the grinding efficiency and quality; enables synchronous control of position and posture, and controls the grinding speed according to the curvature feed direction friction force.

Description

Technical Field

[0001] The present invention relates to the technical field of robot grinding, and in particular to a complex curved surface grinding trajectory planning method based on DMP. Background Art

[0002] With the continuous development of robot technology, and the gradual reduction of mechanical manufacturing costs and component costs, robots have gradually penetrated into our daily lives from the industrial field.

[0003] The continuous popularization of service robots has gradually increased people's acceptance of robots. The robots emerging on the market have gradually expanded from single food delivery and express delivery robots to robots with rich functions such as medical care and explanation and tour guide. This also has higher requirements for the stability, functionality and practicality of robots. In the field of robot grinding, the planning of grinding paths is still a difficult point, and the existing technologies rarely consider the influence of grinding speed on grinding quality. For example:

[0004] Patent CN202011493670.3 discloses a grinding path planning method based on machine vision. It proposes a method for optimizing the grinding path based on grinding parameters during the grinding process. However, its method is cumbersome and does not consider the influence of grinding speed on grinding quality.

[0005] Patent CN202111384297.2 discloses a surface grinding path planning method for large complex components based on real-time point cloud. It proposes an iterative path planning method based on surface point cloud, considering the influence of surface curvature on grinding. But it does not consider the influence of grinding speed and surface roughness. Summary of the Invention

[0006] Aiming at the deficiencies of the prior art, the present invention provides a complex curved surface grinding trajectory planning method based on DMP. The present invention proposes a method for adaptive grinding speed, which greatly reduces the workload of people during complex curved surface grinding and improves the grinding efficiency and grinding quality.

[0007] The technical solution of the present invention is as follows: A complex curved surface grinding trajectory planning method based on DMP, comprising the following steps:

[0008] S1), Use a depth camera to obtain the point cloud information in the grinding area, and then cut out the point cloud data of the workpiece part to be ground;

[0009] S2), Plan the grinding path in the point cloud data of the segmented workpiece to be ground;

[0010] S3), Calculate the normal vector and curvature of each point on the grinding path, calculate the attitude of the grinding head at the grinding point according to the normal vector, and represent it in the form of a quaternion;

[0011] S4), segment the grinding path based on the curvature of each point on the grinding path, and divide the entire grinding path trajectory into multiple segment trajectories;

[0012] S5), for each segment trajectory, model the position and attitude of each segment trajectory using position DMP1 and direction DMP2, and model the curvature of the grinding path using curvature DMP3;

[0013] S6), during grinding, the controller controls the position and attitude of the grinding head according to DMP1 and DMP2 respectively, and the grinding speed is jointly determined by DMP3 and the friction force in the grinding feed direction.

[0014] Preferably, in step S1), after obtaining the point cloud information in the grinding area using a depth camera, it is visualized through the pcl library.

[0015] Preferably, in step S1), the point cloud data of the workpiece part to be cut is: find the boundary of the workpiece in the point cloud and cut out the point cloud of the workpiece part according to the boundary.

[0016] Preferably, in step S2), according to the working radius of the grinding head, plan the grinding path in the point cloud of the workpiece so that the grinding head can cover the entire surface of the workpiece along these trajectories.

[0017] Preferably, in step S3), the normal vectors of each point on the grinding path are obtained using pcl::NormalEstimation.

[0018] Preferably, in step S3), the calculation method of the curvature of each point on the grinding path is as follows:

[0019] S31), assume that the grinding path consists of a point set P = {p1, p2......p n}, and each point p i = {X i , Q i , k i}, X i is the position of this point, Q i represents the attitude of this point, and k i represents the curvature of this point; the curvature k i of point p i is calculated from points p i-1 , p i , p i+1 ;

[0020] S32), the calculation method of curvature k i is:

[0021] Assume Xi =(x i , y i , z i ), X i-1 =(x i-1 , y i-1 , z i-1 ), X i+1 =(x i+1 , y i+1 , z i+1 ), and taking X o =(x0, y0, z0) as the center of the circle passing through the three points p i-1 , p i , p i+1 ; establish the following equations:

[0022]

[0023] By combining (1) and (2) and eliminating variables, we get:

[0024]

[0025] Denoted as:

[0026] A2 = 2×(x i-1 - x i );

[0027] B2 = 2×(y i-1 - y i );

[0028] C2 = 2×(z i-1 - z i );

[0029]

[0030] By combining (1) and (3) and eliminating variables, we get:

[0031]

[0032] Denoted as:

[0033] A3 = 2×(x i+1 - x i )

[0034] B3 = 2×(y i+1 - y i )

[0035] C3 = 2×(Z i+1 - z i )

[0036]

[0037] The plane equation can be determined according to the coplanar constraint of three points:

[0038]

[0039] A1 = y i × z i-1 -y i-1 × z i -y i × z i+1 +y i+1 × z i +y i-1 × z i+1 -y i+1 × z i-1

[0040] B1 = -(x i × z i-1 -x i-1 × z i -x i × z i+1 +x i+1 × z i +x i-1 × z i+1 -x i+1 × z i-1 )

[0041] C1 = x i × y i-1 -x i-1 × y i -x i × y i+1 +x i+1 × y i +x i-1 × y i+1 -x i+1 × y i-1

[0042] D1 = -(x i × y i-1 × z i+1 -x i × y i+1 × z i-1 -x i-1 × y i × z i+1 +x i-1 × y i+1 × z i +x i+1 × y i × z i-1 -x i+1 × y i-1 × zi )

[0043] Establish a linear equation system by obtaining coefficients A to D through the above (4), (5), and (6). For three unknowns with three equations, the center and radius of the circle can be solved:

[0044]

[0045]

[0046]

[0047]

[0048] In the formula, R is the radius of the circle passing through points p i-1 , p i , p i+1 .

[0049] Preferably, in step S3), the attitude of the grinding head at the grinding point is calculated according to the normal vector and represented in the form of a quaternion, specifically as follows:

[0050] Let the rotation matrix between the depth camera and the base of the robotic arm be A, and the calculation method of the rotation matrix H of the end of the robotic arm relative to the base when grinding at point p is:

[0051] Suppose the coordinates of the grinding point p are p = (x, y, z)

[0052] , and the normal vector is -a = (-x T , -y f , -z f ) f . Establish a coordinate system with the unit vector of the normal vector a as the z-axis. Suppose the unit vectors of the x-axis and y-axis are b = (x1, y1, y1) T , c = (x2, y2, y2) T , respectively. There are countless possibilities for the selection of the unit vectors of the x-axis and y-axis, and any one can be chosen arbitrarily; the rotation matrix H between the currently established coordinate system and the camera coordinate system is: T The rotation matrix R between the current coordinate system and the base of the robot is

[0053]

[0054] R = AH. The rotation matrix R t converted to a quaternion is obtained through the function in scipy.spatial.transform. t

[0055] Preferably, in step S4), the set of trajectory points of the entire grinding path is P = {p1, p2...... p n}, divide the grinding path into multiple segments according to the curvature to improve the fitting accuracy of the DMP for the trajectory. The segmentation method is as follows:

[0056] By setting the demarcation curvature k * , and then find the set of points that satisfy (k i - k * ) * (k i-1 - k * ) < 0. Use these points as the segmentation points to cut the set of trajectory points P of the grinding path; after segmentation, the set of segment trajectories is represented as p l = {p1, p2...... p m}, where p j (1 ≤ j ≤ m) represents the set of points of the jth segment trajectory.

[0057] Preferably, in step S5), for each segment trajectory p j after segmentation, use DMP1 to model the position and stiffness within the trajectory, use DMP2 for the orientation, and use DMP3 for the curvature.

[0058] Preferably, in step S5), using DMP1 to model the position and stiffness within the trajectory is expressed as:

[0059]

[0060]

[0061] In the formula, f(x) is the following term during fitting, which is used to control the shape of the actual trajectory and consists of a linear combination of the Gaussian kernel Φ(x) and the time decay coefficient x from the regular system; Y is the taught point trajectory, τ is the time constant for controlling the movement speed of the robot end during use, g is defined as the position of the robot end defined when using the taught trajectory, Z, respectively represent the taught trajectory speed and the taught trajectory acceleration after being scaled by the time constant τ, α z , β z are the control parameters of the controller, represents the taught trajectory speed;

[0062] The process of learning the taught trajectory Y is to adjust the parameter ω i of the following term f(x) by fitting with known taught points; when the trajectory starts to be generated, the following term f(x) changes the trajectory direction by modifying the acceleration of the robot end. When the time approaches infinity, the forced following term f(x) approaches 0; the trajectory returns to the specified point; the following term f(x) is expressed as:

[0063]

[0064] Where ω i represents the i-th adjustment parameter, and Φ i (x) represents the i-th Gaussian kernel. There are N adjustment parameters and Gaussian kernels in total, and N is specified by the user. x is the time decay coefficient;

[0065] The generation method of the i-th Gaussian kernel Φ i (x) is as follows:

[0066] Φ i (x) = exp(-h i (x - c i ) 2 );

[0067] Among them, h i represents the width of the i-th Gaussian kernel, and c i respectively represent the center of the i-th Gaussian kernel, both of which are specified by the user; x is the time decay coefficient;

[0068] The generation method of the time decay coefficient x is as follows:

[0069]

[0070] In the formula, represents the time decay term, and α x is a constant specified by the user;

[0071] The fitting method of the adjustment parameter ω i is as follows:

[0072]

[0073] In the formula, f target is the target following term generated from the known trajectory during fitting, and T represents the transpose of the matrix;

[0074] The process of learning the taught trajectory is to fit the target equation f target through the known taught points; by updating the adjustment parameter ω i , make the output of f(x) close to f target :

[0075]

[0076] The process of generating the taught trajectory is to learn f(x) from the known taught trajectory y to make it the target following term f target , and the reproduction process is to generate the reproduced trajectory y target from the known following term f r ;

[0077] In the formula, represents the acceleration of the taught trajectory.

[0078] Preferably, in step S5), the quaternion is modeled using the directional DMP as follows:

[0079]

[0080]

[0081] where q ∈ S3 is a unit quaternion, ω ∈ R3, are the angular velocity and acceleration respectively, τ is the time scale factor, * represents the product of two quaternions, e(,) represents the error between two quaternions, is the angular velocity quaternion, i.e., is a quaternion with a zero scalar part and an angular velocity as the vector part; is the derivative of q, S3 is the unit sphere in three-dimensional space, and R3 represents three-dimensional space.

[0082] Preferably, in step S6), to improve the polishing quality, the polishing rate is slowed down at places with large curvature and high roughness, and the polishing speed is controlled by adjusting the time scale factor τ in DMP1 and DMP2, as follows:

[0083]

[0084]

[0085]

[0086] v = v t - v t-1 ;

[0087] where τ(t) applies to DMP1, DMP2, and DMP3, is the set curvature threshold, τ max is the maximum value of the time coefficient, f is the polishing force in the polishing forward direction, F is the set reference friction force, k(t) is the output of DMP3, u = (f x , f y , f z ) is the vector composed of the forces of the force sensor in three directions, v t , v t-1 represent the outputs of DMP1 at the current time t and the previous time t - 1 respectively; v is the difference between the outputs of DMP1 at the current time t and the previous time t - 1. By the above method, the polishing time can be extended at places with large curvature and high roughness, improving the polishing quality.

[0088] The beneficial effects of the present invention are:

[0089] 1. The present invention can achieve the integration of trajectory segmentation, trajectory modeling, and adaptive adjustment of grinding speed; using point cloud and DMP to plan the grinding path, greatly reducing the workload of people during complex surface grinding, and improving the grinding efficiency and quality.

[0090] 2. The present invention cuts the grinding trajectory according to the curvature, and improves the accuracy through segmented modeling. The position and orientation of the grinding are modeled using position and orientation DMP, enabling synchronous control of the position and orientation. The grinding speed is controlled according to the curvature, feed direction, and friction force, enhancing the grinding quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0091] Figure 1 is the flow framework diagram of the method of the present invention; DETAILED DESCRIPTION OF THE INVENTION

[0092] The following further describes the specific implementation manners of the present invention in conjunction with the drawings:

[0093] As Figure 1 shown, this embodiment provides a method for planning complex surface grinding trajectories based on DMP, including the following steps:

[0094] S1). Use a depth camera to obtain point cloud information within the grinding area and visualize it through the pcl library;

[0095] Then find the boundary of the workpiece in the point cloud information and cut out the point cloud of the workpiece part according to the boundary;

[0096] S2). Plan the grinding path in the point cloud data of the segmented grinding workpiece; in this embodiment, through the working radius of the grinding head, plan the grinding path in the point cloud of the workpiece, so that the grinding head can cover the entire surface of the workpiece along these trajectories;

[0097] S3). Calculate the normal vector and curvature of each point on the grinding path, calculate the attitude of the grinding head at the grinding point according to the normal vector, and represent it in the form of a quaternion;

[0098] S4). Segment the grinding path based on the curvature of each point on the grinding path, and divide the entire grinding path trajectory into multiple segment trajectories;

[0099] S5). For each segment trajectory, model the position and attitude of the trajectory using position DMP1 and direction DMP2, and model the curvature of the grinding path using curvature DMP3;

[0100] S6). During grinding, DMP1 and DMP2 respectively control the position and attitude of the grinding head, and the grinding speed is jointly determined by DMP3 and the friction force in the feed direction of grinding.

[0101] Preferably, in step S3) of this embodiment, the normal vectors of the points on the grinding path are obtained by pcl::NormalEstimation.

[0102] Preferably, in step S3) of this embodiment, the calculation method of the curvature of each point on the grinding path is as follows:

[0103] S31), assume that the grinding path is composed of a point set P = {p1, p2......p n}, and each point p i = {X i , Q i , k i}, where X i is the position of this point, Q i represents the attitude of this point, and k i represents the curvature of this point; the curvature k i of point p i is calculated from points p i-1 , p i , p i+1 ;

[0104] S32), the calculation method of curvature k i is:

[0105] Assume X i = (x i , y i , z i ), X i-1 = (x i-1 , y i-1 , z i-1 ), X i+1 = (x i+1 , y i+1 , z i+1 ), and take X o = (x0, y0, z0) as the center of the circle where points p i-1 , p i , p i+1 are located; establish the following equations:

[0106]

[0107] Eliminate the variables by combining (1) and (2) to get:

[0108]

[0109] Denote as:

[0110] A2 = 2×(x i-1 - x i );

[0111] B2 = 2×(yi-1 -y i );

[0112] C2 = 2×(z i-1 -z i );

[0113]

[0114] Eliminating variables by combining (1) and (3) gives:

[0115]

[0116] Denoted as:

[0117] A3 = 2×(x i+1 -x i )

[0118] B3 = 2×(y i+1 -y i )

[0119] C3 = 2×(z i+1 -z i )

[0120]

[0121] According to the coplanarity constraint of three points, the plane equation can be determined as:

[0122]

[0123] A1 = y i ×z i-1 -y i-1 ×z i -y i ×z i+1 +y i+1 ×z i +y i-1 ×z i+1 -y i+1 ×z i-1

[0124] B1 = -(x i ×z i-1 -x i-1 ×z i -x i ×z i+1 +x i+1 ×z i +x i-1 ×z i+1 -x i+1 ×z i-1 )

[0125] C1 = xi ×y i-1 -x i-1 ×y i -x i ×y i+1 +x i+1 ×y i +x i-1 ×y i+1 -x i+1 ×y i-1

[0126] D1 = -(x i ×y i-1 ×z i+1 -x i ×y i+1 ×z i-1 -x i-1 ×y i ×z i+1 +x i-1 ×y i+1 ×z i

[0127] +x i+1 ×y i ×z i-1 -x i+1 ×y i-1 ×z i )

[0128] By obtaining coefficients A to D through the above (4), (5), and (6) to establish a system of linear equations, with three unknowns and three equations, the working center and radius of the grinding head can be solved:

[0129]

[0130]

[0131]

[0132]

[0133] In the formula, R is the radius of the circle passing through points p i-1 , p i , p i+1 , and x i , y i , z i are the coordinates of position X i .

[0134] Preferably in this embodiment, in step S3), calculating the attitude of the grinding head at the grinding point according to the normal vector and representing it in the form of a quaternion, specifically:

[0135] Let the rotation matrix between the depth camera and the robotic arm base be A. The calculation method for the rotation matrix H of the end effector relative to the base when the robotic arm is at the grinding point p is as follows:

[0136] Suppose the coordinates of the grinding point p are p = (x, y, z) T , and the normal vector is -a = (-x f , -y f , -z f ). Establish a coordinate system with the unit vector of the normal vector a as the z-axis. Suppose the unit vectors of the x-axis and y-axis are b = (x1, y1, y1) T , c = (x2, y2, y2) T , respectively. There are countless possibilities for the selection of the unit vectors of the x-axis and y-axis. Any one can be chosen arbitrarily; the rotation matrix H between the currently established coordinate system and the camera coordinate system is:

[0137]

[0138] The rotation matrix R between the current coordinate system and the robotic arm base t = AH. The rotation matrix R t is converted to a quaternion and obtained through the function in scipy.spatial.transform.

[0139] Preferably, in step S4), the set of trajectory points of the entire grinding path is P = {p1, p2......p n}}. The grinding path is divided into multiple segments according to the curvature to improve the fitting accuracy of the DMP to the trajectory. The segmentation method is as follows:

[0140] By setting the boundary curvature k * , and then finding the set of points that satisfy (k i - k * ) * (k i-1 - k * ) < 0, and cutting the set of grinding path trajectory points P with these points as the segmentation points; after segmentation, the set of segment trajectories is represented as p l = {p1, p2......p m}}, where p j (1 ≤ j ≤ m) represents the set of points of the j-th segment trajectory.

[0141] Preferably, in step S5), for each segmented segment trajectory p j , use DMP1 to model the position X j and stiffness within the trajectory, use DMP2 to model the quaternion Q j , and use DMP3 to model the curvature.

[0142] ​Preferably, in step S5), the position and stiffness within the trajectory are modeled using DMP1 as follows:

[0143]

[0144]

[0145] In the formula, f(x) is the following term during fitting, which is used to control the shape of the actual trajectory and consists of a linear combination of the Gaussian kernel Φ(x) and the time decay coefficient x from the regular system; Y is the taught point trajectory, τ is the time constant for controlling the movement speed of the robot end during use, g is defined as the position of the robot end defined when using the taught trajectory, Z, respectively represent the taught trajectory speed and the taught trajectory acceleration after being scaled by the time constant τ, and α z , β z are the control parameters of the controller, represents the taught trajectory speed;

[0146] The process of learning the taught trajectory Y is to adjust the parameter ω of the following term f(x) by fitting through known taught points i ; when the trajectory starts to be generated, the following term f(x) changes the trajectory direction by modifying the acceleration of the robot end. When the time approaches infinity, the forced following term f(x) approaches 0; the trajectory returns to the specified point; the following term f(x) is expressed as:

[0147]

[0148] In the formula, ω i represents the i-th adjustment parameter, and Φ i (x) represents the i-th Gaussian kernel. There are N adjustment parameters and Gaussian kernels in total, and N is specified by the user. x is the time decay coefficient;

[0149] The generation method of the i-th Gaussian kernel Φ i (x) is as follows:

[0150] Φ i (x) = exp(-h i (x - c i )) 2 );

[0151] Among them, h i represents the width of the i-th Gaussian kernel, and c i respectively represent the center of the i-th Gaussian kernel, both specified by the user; x is the time decay coefficient;

[0152] The generation method of the time decay coefficient x is as follows:

[0153]

[0154] In the formula, represents the time decay term, and α x is a constant specified by the user;

[0155] The adjustment parameter ω i is fitted as follows:

[0156]

[0157] In the formula, f target is the target following term generated from the known trajectory during fitting, and T represents the transpose of the matrix;

[0158] The process of learning the teaching trajectory is to fit the target equation f target through the known teaching points; by updating the adjustment parameter ω i , the output of f(x) is made to approach f target :

[0159]

[0160] The process of generating the teaching trajectory is to learn f(x) from the known teaching trajectory y to make it the target following term f target , and the reproduction process is to generate the reproduction trajectory y target from the known following term f r ;

[0161] In the formula, represents the acceleration of the teaching trajectory.

[0162] Preferably, in step S5), the attitude is modeled using DMP2 as:

[0163]

[0164]

[0165] In the formula, q ∈ S3 is a unit quaternion, ω ∈ R3, are the angular velocity and acceleration respectively, τ is the time scale factor, * represents the product of two quaternions, e(,) represents the error between two quaternions, is the angular velocity quaternion, that is is a quaternion with a scalar part of zero and an angular velocity as the vector part; is the derivative of q, S3 is the unit sphere in three-dimensional space, and R3 represents three-dimensional space.

[0166] Preferably, in step S6), in order to improve the polishing quality, the polishing rate is slowed down at places with large curvature and high roughness, and the polishing speed is controlled by controlling the time scale factor τ in DMP1 and DMP2, specifically as follows:

[0167]

[0168]

[0169]

[0170] v = v t -v t-1 ;

[0171] where τ(t) is applicable to DMP1, DMP2, and DMP3, is the set curvature threshold, τ max is the maximum value of the time coefficient, f is the polishing force in the polishing forward direction, F is the set reference friction force, k(t) is the curvature output by DMP3, u = (f x , f y , f z ) is the vector composed of the forces of the force sensor in three directions, v t , v t-1 respectively represent the outputs of DMP1 at the current time t and the previous time t - 1; v is the difference between the outputs of DMP1 at the current time t and the previous time t - 1. Through the above method, the polishing time can be extended at places with large curvature and high roughness, and the polishing quality can be improved.

[0172] The above embodiments and the descriptions in the specification only illustrate the principles and the best embodiments of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed.

Claims

1. A method for planning a complex surface grinding trajectory based on DMP, characterized in that Including the following steps: S1), obtaining the point cloud information within the grinding area by using a depth camera; S2), planning a grinding path in the point cloud data of the segmented grinding workpiece; by the working radius of the grinding head, planning a grinding path in the point cloud of the workpiece, so that the grinding head can cover the entire surface of the workpiece along these trajectories for grinding; S3), calculating the normal vectors and curvatures of each point on the grinding path, calculating the posture of the grinding head at the grinding point according to the normal vector, and representing it in the form of a quaternion; S4), segment the grinding path based on the curvature of each point on the grinding path, and divide the entire grinding path trajectory into multiple segment trajectories; the point set of the entire grinding path trajectory is , divide the grinding path into multiple segments according to the curvature to improve the fitting accuracy of the DMP for the trajectory. The segmentation method is as follows: By setting the demarcation curvature , and then finding the set of points that satisfy , using them as the splitting points to cut the grinding path trajectory point set P; after splitting, the fragment trajectory set is represented as , where represents the set of points of the j-th fragment trajectory; S5), for each segment trajectory, model the position and pose of the trajectory using position DMP1 and orientation DMP2, and model the curvature of the grinding path using curvature DMP3; for each segment trajectory after segmentation , use DMP1 to model the position and stiffness within the trajectory, expressed as: In the formula, is the following term during fitting, which is used to control the shape of the actual trajectory and consists of the linear combination of the Gaussian kernel and the time decay coefficient x from the regular system; Y is the taught point trajectory, τ is the time constant for controlling the movement speed of the robot end during use, g is defined as the position of the robot end defined when using the taught trajectory, respectively represent the taught trajectory speed and the taught trajectory acceleration after being scaled by the time constant τ, is the control parameter of the controller, represents the taught trajectory speed; For each segmented fragment trajectory , the orientation is modeled using DMP2 as follows: wherein, is a unit quaternion, are the angular velocity and acceleration respectively, τ is the time constant for controlling the motion speed of the robot end during use, * represents the product of two quaternions, and e(,) represents the error between two quaternions, is the angular velocity quaternion, , that is, is a quaternion with a scalar part of zero and an angular velocity as the vector part; is the derivative of, S3 is the unit sphere in three-dimensional space, and R3 represents three-dimensional space; S6), during grinding, the controller controls the position and posture of the grinding head according to DMP1 and DMP2 respectively, and the grinding speed is jointly determined by DMP3 and the friction force in the grinding feed direction; in order to improve the grinding quality, the grinding rate is slowed down in places with large curvature and high roughness by controlling the time constants in DMP1 and DMP2 Control the grinding speed as follows: Among them, is applicable to DMP1, DMP2, and DMP3, is the set curvature threshold, is the maximum value of the time coefficient, is the grinding force in the grinding forward direction, and F is the set reference friction force, is the curvature output by DMP3, is the vector composed of the forces of the force sensor in three directions, respectively represent the outputs of DMP1 at the current moment t and the previous moment t - 1; v is the difference between the outputs of DMP1 at the current moment t and the previous moment t - 1. By the above method, the grinding time can be extended at places with large curvature and high roughness, improving the grinding quality.

2. A complex surface grinding trajectory planning method based on DMP according to claim 1, characterized in that: In step S3), the normal vectors of each point on the grinding path are obtained by using pcl::NormalEstimation.

3. A method for planning a grinding trajectory of a complex curved surface based on DMP according to claim 1, characterized in that: In step S3), the calculation method of the curvature of each point on the grinding path is as follows: S31), assume that the grinding path is composed of a point set Each point , is the position of this point, represents the pose of this point, represents the curvature of this point; the curvature of point is calculated from point ​ S32), curvature The calculation method is as follows: Setting and using as the center of the circle passing through the three points, the following equation is established: Eliminating the unknowns by combining (1) and (2) to obtain: Denoted as: ； Eliminating the unknowns by combining (1) and (3) to obtain: Denoted as: The plane equation can be determined according to the three-point coplanarity constraint: Establishing a linear equation system by obtaining coefficients A to D through the above (4), (5), and (6). With three unknowns and three equations, the working center and radius of the grinding head can be solved: where R is the radius of the circle where it is located, and xi, yi, zi are the coordinates of the position Xi.

4. A method for planning a grinding trajectory of a complex curved surface based on DMP according to claim 3, characterized in that: In step S3), calculating the posture of the grinding head at the grinding point according to the normal vector and representing it in the form of a quaternion, specifically: Let the rotation matrix between the depth camera and the base of the robotic arm be A, and the calculation method of the rotation matrix H of the end of the robotic arm relative to the base when at the grinding point p is: Assume the grinding point has coordinates , and the normal vector is . Establish a coordinate system with the unit vector of the normal vector a as the z-axis. Assume the unit vectors of the x-axis and y-axis are respectively . There are countless possibilities for the selection of the unit vectors of the x-axis and y-axis. Any one can be chosen arbitrarily; the rotation matrix H between the currently established coordinate system and the camera coordinate system is as follows: Rotation matrix between the current coordinate system and the robot base , the rotation matrix is obtained by the function in scipy.spatial.transform.

5. A method for planning a grinding trajectory of a complex surface based on DMP according to claim 1, characterized in that: In step S5), the process of learning the teaching trajectory Y is to fit the following term through known teaching points of the adjustment parameter ; when the trajectory starts to be generated, the following term changes the trajectory direction by modifying the acceleration at the end of the robot. When the time approaches infinity, the forced following term tends to 0; the trajectory returns to the specified point; the said following term is expressed as: wherein, represents the i-th adjustment parameter, represents the i-th Gaussian kernel, there are N adjustment parameters and Gaussian kernels in total, N is specified by the user, and x is the time decay coefficient; The generation method of the i-th Gaussian kernel Φi(x) is as follows: Among them, represents the width of the i-th Gaussian kernel, and represents the center of the i-th Gaussian kernel, both of which are specified by the user; is the time decay coefficient; The generation method of the time decay coefficient x is as follows: In the formula, represents the time decay term, is a constant specified by the user; Adjust the parameters The fitting method is as follows: In the formula, is the target following term generated from the known trajectory during fitting, and T represents the transpose of a matrix; The process of learning the teaching trajectory is to fit the target equation through known teaching points ; by updating and adjusting the parameters , so that the output of is close to The process of teaching trajectory generation is to learn from the known teaching trajectory y to make it the target following item , and the reproduction process is from the known following item to generate the reproduction trajectory ; In the formula, represents the acceleration of the teaching trajectory.

6. A method for complex surface grinding trajectory planning based on DMP according to claim 1, characterized in that: In step S1), after obtaining the point cloud information, visualizing it through the pcl library; then finding the boundary of the workpiece in the point cloud information, and cutting out the point cloud of the workpiece part according to the boundary.

Images