A robot grinding trajectory planning method

Abstract

The present invention discloses a robot grinding trajectory planning method, which comprises the following steps: S1, constructing a three-dimensional digital model of a complex structure, determining the outer contour coordinates of a microstructure to be ground in a three-dimensional digital coordinate system, and converting the microstructure into a point cloud; S2, generating an OBB bounding box of the microstructure based on the microstructure point cloud, and determining the coordinates of eight vertices of the bounding box in the three-dimensional digital coordinate system; S3, performing coordinate registration on the microstructure bounding box and the complex structure object, and determining the actual grinding position of the grinding tool; S4, respectively performing speed, position and posture planning on the grinding motion of a grinding ball at the end of a robotic arm; S5, adopting an inverse dynamics solution method, and obtaining the variables of the robotic arm joint by solving the posture of a grinding head at the end of the robotic arm; the method can achieve accurate grinding position, and fully considers the smoothness of the grinding path, so as to avoid large jitter and error of the grinding instrument, and has good practicality.

Description

Technical Field

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

[0002] Currently, based on the intelligence, autonomy and safety of robots, they are gradually replacing manual actions to achieve operations such as drilling, milling and grinding that require more precise and accurate operations.

[0003] For objects with complex internal structures, the focus of grinding is not only on the refinement of the grinding action, but also on the planning of the grinding path to avoid damage to other nearby structures when grinding tiny structures.

[0004] Therefore, to achieve this goal, the grinding path should start with layer-by-layer grinding, and the microstructure to be ground should be accurately removed. This grinding process is different from the grinding method of the outer structure of the structure. The grinding trajectory and method need to be planned and optimized to ensure that the approach grinding position is accurate, the grinding path is smooth, and the grinding equipment does not produce large jitter and errors. Summary of the Invention

[0005] The purpose of the present invention is to provide a robot grinding trajectory planning method suitable for grinding small structures of complex structures.

[0006] To this end, the technical solution of the present invention is as follows:

[0007] A robot grinding trajectory planning method, the steps are as follows:

[0008] S1. Constructing a three-dimensional digital model of a complex structure, and determining the outer contour coordinates of the microstructure to be ground based on the three-dimensional digital coordinate system of the three-dimensional digital model, so as to convert the microstructure to be ground into a point cloud form;

[0009] S2. Generate an OBB bounding box of the microstructure based on the microstructure point cloud and determine the coordinates of eight vertices of the bounding box in a three-dimensional digital coordinate system;

[0010] S3, performing coordinate registration of the OBB bounding box of the microstructure determined in step S2 with the complex structure fixed on the grinding table to determine the actual grinding position of the grinding tool;

[0011] S4. Plan the grinding motion trajectory of the robotic arm, which is also fixed on the grinding table and equipped with a grinding tool, including speed planning, position planning and posture planning of the grinding head at the end of the robotic arm; wherein, the specific implementation steps of this step are: 1) construct the fixed base coordinate system, the manipulator end coordinate system and the joint coordinate system of the robotic arm to obtain the kinematic equation of the grinding head at the end of the robotic arm, including the transformation matrix of each joint and the transformation matrix from the fixed base of the robotic arm to the grinding head; at the same time, design the tool path according to the shape of the bounding box, and determine the starting point and end point of the grinding on the bounding box according to the shape and size of the grinding head; 2) the speed planning adopts the seven-segment S-shaped speed curve method, that is, the entire motion process is divided into seven segments: acceleration, uniform acceleration segment, deceleration segment, uniform speed segment, acceleration and deceleration segment, uniform deceleration segment and deceleration segment; 3) the position planning adopts the linear interpolation method combined with the seven-segment S-shaped speed curve; 4) the posture planning adopts the quaternion attitude interpolation method combined with the seven-segment S-shaped speed curve;

[0012] S5. Using the inverse dynamics solution method, the position of the grinding head at the end of the robotic arm is used to solve the variables of each joint of the robotic arm.

[0013] Furthermore, the specific implementation steps of step S1 are: 1) obtaining a three-dimensional digital model of the complex structure and determining the outer contour coordinate set of the complex structure in the three-dimensional digital coordinate system; 2) determining the position of the microstructure to be ground in the three-dimensional digital model of the complex structure, and obtaining the outer contour coordinate set of the microstructure in the three-dimensional digital coordinate system; 3) converting the outer contour coordinate set of the microstructure in the three-dimensional digital coordinate system into point cloud data and visually displaying it.

[0014] Furthermore, the specific implementation steps of step S2 are: 1) using the PCA principal component analysis method to obtain the centroid of the tiny structure point cloud, and generating an OBB bounding box based on the centroid, and then establishing a bounding box coordinate system with the centroid as the origin and the length, width, and height of the bounding box as the x-axis direction, y-axis direction, and z-axis direction respectively; 2) based on the bounding box coordinate system, obtaining the coordinates of the eight vertices of the bounding box; 3) based on the coordinates of the eight vertices of the bounding box, obtaining the coordinate set of the tiny structure point cloud; based on the coordinate sets of the tiny structure point cloud in the bounding box coordinate system and the three-dimensional digital coordinate system, obtaining the transformation matrix between the bounding box coordinate system and the three-dimensional digital coordinate system; and then, using the transformation matrix, the coordinates of the eight vertices of the bounding box in the three-dimensional digital coordinate system can be obtained.

[0015] Furthermore, the specific implementation steps of step S3 are: 1) constructing a physical coordinate system based on the complex structure fixed on the grinding table to obtain the outer contour coordinate set of the complex structure; 2) using the outer contour coordinate sets of the complex structure in the physical coordinate system and the three-dimensional digital coordinate system respectively, to obtain the transformation matrix between the physical coordinate system and the three-dimensional digital coordinate system; using this transformation matrix, the coordinates of the eight vertices of the bounding box in the physical coordinate system can be obtained.

[0016] Furthermore, the specific implementation steps of step 2) in step S4 are:

[0017] The acceleration of the grinding head at the end of the robotic arm is set to J during movement, where J is a constant. The entire movement process is divided into seven sections, namely, the acceleration section T1, i.e., 0~t1; the uniform acceleration section T2, i.e., t1~t2; the deceleration section T3, i.e., t2~t3; the uniform speed section T4, i.e., t3~t4; the acceleration / deceleration section T5, i.e., t4~t5; the uniform deceleration section T6, i.e., t5~t6; and the deceleration / deceleration section T7, i.e., t6~t7.

[0018] The acceleration function expression of the seven-segment S-shaped velocity is:

[0019]

[0020] Where, Maximum acceleration a max In the uniform acceleration segment T2, a max =JT1;

[0021] The speed function expression of the seven-segment S-shaped speed is:

[0022]

[0023] Where v0 is the initial velocity, v1 to v6 are the initial velocities of the uniform acceleration segment T2 to the deceleration segment T7 respectively; the velocity of the uniform speed segment T4 is the maximum velocity v3 during the motion, i.e. V max ;

[0024] The displacement function expression of the seven-segment S-shaped velocity is:

[0025]

[0026] Where, p is the total length of the trajectory, that is, the moving distance of the end of the robot arm; p1~p6 are the trajectory lengths between the initial moment of the uniform acceleration section T2~deceleration section T7 and the starting point of grinding.

[0027] Furthermore, the specific implementation steps of step 3) in step S4 are:

[0028] The linear trajectory interpolation formula is:

[0029]

[0030] Where i is any point on the interpolation line, and its coordinates are (x i ,y i , z i ), accordingly, (x0, y0, z0) is the coordinate of the starting point before interpolation; Δt is the interpolation period; v t is the running speed; l i is the displacement from the starting point to point i, n is the number of interpolation points,

[0031] Assume that the movement of the end of the robot arm along the x-axis, y-axis, and z-axis corresponds to the movement feed of each joint driven by the joint driver Δx, Δy, and Δz; then the linear trajectory interpolation formula shows that the joint movement feed is related to the displacement of each interpolation cycle; therefore, when performing linear interpolation, it is necessary to combine the acceleration and deceleration stages of the S-shaped velocity curve;

[0032] Assume that the displacement of the robot arm during the i-th interpolation cycle Δt is p i , the maximum acceleration is a max , the speed of the uniform motion segment is v max , then combined with the displacement function expression of the seven-segment s-shaped velocity, the corresponding p i The expression is:

[0033]

[0034] Then, by combining the different stages of p i Substituting this into the linear trajectory interpolation formula, we can obtain the coordinates of the end of the robot arm in the i-th interpolation cycle. At the same time, combined with the verification of the maximum speed and maximum acceleration in speed planning, and the expression of the total time T: T = T a +T b , that is, the total time T can be solved; where T a is the acceleration time period, i.e. 0~t3, T b It is the deceleration time period, i.e. t4 to t7.

[0035] Furthermore, the specific implementation steps of step 4) in step S4 are:

[0036] The expression of the relationship between angular displacement and time during the posture change process is:

[0037]

[0038] Where, w0~w6 are the angular velocities of the acceleration section T1~deceleration section T6, θ1~θ6 are the angular displacements between the initial moment of the uniform acceleration section T2~deceleration section T7 and the starting point of grinding;

[0039] Assume that the total angular displacement θ = arccos(Q A iQ B ), and order Substituting into the spherical linear interpolation formula of the attitude quaternion, we can obtain the quaternion that changes with the interpolation period Δt;

[0040] Among them, the spherical linear interpolation formula of the attitude quaternion is:

[0041]

[0042] Where Q(u) is also a quaternion, 0≤u≤1, let the total motion time be T, the interpolation period be Δt, and the value of u be Q A is the homogeneous transformation matrix T from the grinding starting point A A The rotation matrix R extracted from A , the converted quaternion; Q B is the homogeneous transformation matrix T of the self-grinding end point B B The rotation matrix R extracted from B , the converted quaternion;

[0043] Then, the attitude quaternion is substituted into the attitude matrix corresponding to the unit quaternion q to obtain the attitude matrix.

[0044] Compared with the existing technology, the robot grinding trajectory planning method is suitable for the requirements of fine grinding of tiny structures of complex structures, and fully considers the problem of small size of tiny structures. The grinding object is converted from a tiny structure into a bounding box with a regular shape by first converting the tiny structure into point cloud data and then generating an OBB box surrounding the tiny structure. Then, the forward kinematics method is used to model the robotic arm, and the inverse dynamics solution is used to complete the planning of the speed, position and posture of the grinding ball at the end of the robotic arm. The grinding trajectory planning method of tiny structures of complex structures using this method can achieve accurate grinding position, and fully considers the smoothness of the grinding path to avoid large jitter and error of the grinding equipment, and has good practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 A flowchart of the robot grinding trajectory planning method of the present invention;

[0046] Figure 2 A point cloud visualization effect diagram of the microstructure to be ground in step S1 of the robot grinding trajectory planning method of the present invention;

[0047] Figure 3 Schematic diagram of an OBB bounding box generated based on a tiny structure point cloud in step S2 of the robot grinding trajectory planning method of the present invention;

[0048] Figure 4 Schematic diagram of the fixed base coordinate system B and the manipulator end coordinate system T in step S401 of the robot grinding trajectory planning method of the present invention;

[0049] Figure 5 Schematic diagram of modeling of the robot arm Jaka zu7 in step S401 of the robot grinding trajectory planning method of the present invention;

[0050] Figure 6 A schematic diagram of a grinding path design with a bounding box as a grinding target in step S401 of the robot grinding trajectory planning method of the present invention;

[0051] Figure 7 This is a grinding path planning flow chart of step S4 of the robot grinding trajectory planning method of the present invention;

[0052] Figure 8 Schematic diagram of planning the movement speed of the grinding head at the end of the robot arm using a seven-segment S-shaped speed curve in step S402 of the robot grinding trajectory planning method of the present invention;

[0053] Figure 9 Schematic diagram of the relationship between quaternions and rotations in step S402 of the robot grinding trajectory planning method of the present invention;

[0054] Figure 10 A simplified diagram of the inverse dynamics solution method of step S5 of the robot grinding trajectory planning method of the present invention;

[0055] Figure 11 A schematic diagram of a robot model established for a simulation experiment in an embodiment of the present invention;

[0056] Figure 12 A schematic diagram of a speed curve simulation of a simulation experiment in an embodiment of the present invention;

[0057] Figure 13 Schematic diagram of position interpolation results of a simulation experiment in an embodiment of the present invention;

[0058] Figure 14 A schematic diagram of a robot arm simulation trajectory in a simulation experiment in an embodiment of the present invention;

[0059] Figure 15 Schematic diagram of posture interpolation of a robot arm simulation trajectory in a simulation experiment in an embodiment of the present invention;

[0060] Figure 16 Graph showing the trajectory points of the grinding ball at the end of the robotic arm in a simulation experiment according to an embodiment of the present invention;

[0061] Figure 17 This is a diagram showing the trajectory simulation results of the grinding ball at the end of the robotic arm in a simulation experiment in an embodiment of the present invention. DETAILED DESCRIPTION

[0062] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the following embodiments are by no means intended to limit the present invention in any way.

[0063] See also Figure 1 ,The specific implementation steps of the robot grinding trajectory planning method are described as follows:

[0064] S1. Constructing a three-dimensional digital model of a complex structure, and determining the outer contour coordinates of the microstructure to be ground based on the three-dimensional digital coordinate system of the three-dimensional digital model, so as to convert the microstructure to be ground into a point cloud form;

[0065] Specifically, the specific implementation steps of step S1 are:

[0066] S101. Obtain a three-dimensional digital model of a complex structure, and determine an outer contour coordinate set of the complex structure in a three-dimensional digital coordinate system; wherein the three-dimensional digital model of the complex structure can be obtained by scanning the complex structure using a 3D scanner, or by three-dimensional mapping;

[0067] S102, determining the position of the microstructure to be ground in the three-dimensional digital model of the complex structure, obtaining the outer contour coordinate set of the microstructure in the three-dimensional digital coordinate system, and saving it as an obj format file;

[0068] S103, convert the outer contour coordinate set of the microstructure in the three-dimensional digital coordinate system into point cloud data and visualize it; specifically, this step is implemented using the pcl_visualiation library in the PCL library, that is, converting the obj format file obtained in the above step S102 into a point cloud file in the pcd format for reading and visualizing the point cloud. Figure 2 Shown is a point cloud visualization effect diagram of the microstructure to be ground in this embodiment.

[0069] S2. Generate an OBB bounding box of the microstructure based on the microstructure point cloud and determine the coordinates of the eight vertices of the bounding box;

[0070] Specifically, the OBB bounding box is also obtained by processing the point cloud data using the PCL library. The specific implementation steps are as follows:

[0071] S201, using the PCA principal component analysis method to obtain the centroid of the microstructure point cloud, and generate an OBB bounding box based on the centroid, and then establish a bounding box coordinate system with the centroid as the origin and the length, width, and height of the bounding box as the x-axis direction, y-axis direction, and z-axis direction respectively; Figure 3 FIG. 1 is a schematic diagram of an OBB bounding box generated based on a tiny structure point cloud in this embodiment;

[0072] S202, based on the bounding box coordinate system, obtain the coordinates of the eight vertices of the bounding box;

[0073] S203. Based on the coordinates of the eight vertices of the bounding box, a coordinate set of the micro-structure point cloud is obtained; based on the coordinate sets of the micro-structure point cloud in the bounding box coordinate system and the three-dimensional digital coordinate system, a transformation matrix between the bounding box coordinate system and the three-dimensional digital coordinate system is obtained; and then, the coordinates of the eight vertices of the bounding box in the three-dimensional digital coordinate system can be obtained using the transformation matrix.

[0074] In this embodiment, in addition to the OBB bounding box, the applicant also tried to use AABB bounding boxes, convex hull bounding boxes, etc. to form bounding boxes for the tiny structures to be ground; however, the use of AABB bounding boxes has too much redundant space, and the use of convex hull bounding boxes is too complex and not conducive to subsequent trajectory planning. Other types of bounding boxes are completely unsuitable for forming corresponding bounding boxes for the tiny structures to be ground; therefore, this application finally determined to use OBB bounding boxes to construct the minimum bounding box of the tiny structure, so as to effectively include the area where the tiny structure to be ground is located and simplify the calculation.

[0075] S3, performing coordinate registration of the OBB bounding box of the microstructure determined in step S2 with the complex structure fixed on the grinding table to determine the actual grinding position of the grinding tool;

[0076] In step S3, the OBB bounding box of the microstructure generated in step S2 is the grinding range for the grinding tool to plan the grinding trajectory. However, in the actual grinding process, the grinding tool actually acts on the physical object to be ground. Therefore, it is necessary to determine the actual grinding position of the grinding tool. The specific determination method is as follows:

[0077] S301. Constructing a physical coordinate system based on the complex structure fixed on the grinding table to obtain a coordinate set of the outer contour of the complex structure. The physical coordinate system can be constructed using any existing means, such as an optical navigation system and its associated identification stickers. Specifically, the identification stickers are applied along the outer contour of the complex structure and then read using the optical navigation system to construct the coordinate system.

[0078] S302, using the coordinate sets of the outer contour of the complex structure in the physical coordinate system and the three-dimensional digital coordinate system, respectively, to obtain a transformation matrix between the physical coordinate system and the three-dimensional digital coordinate system; further, using the transformation matrix, the coordinates of the eight vertices of the bounding box in the physical coordinate system can be obtained;

[0079] S4, planning the grinding motion trajectory of a robotic arm that is also fixed on the grinding table and equipped with a grinding tool, specifically speed planning, position planning, and posture planning for the grinding head at the end of the robotic arm;

[0080] Specifically, the implementation steps of step S4 are as follows:

[0081] S401, determining the kinematic equation of the grinding head at the end of the robotic arm;

[0082] 1) See Figure 4 , let the position of any point P in the fixed base coordinate system B of the manipulator be a 3×1 position vector B P represents that its matrix expression is:

[0083] B P=[p x p y p z ] T ,

[0084] Where p x , p y , p z are the three coordinate components of point P in the fixed base coordinate system B;

[0085] Assume that the center point of the grinding head of the grinding tool is R, and the center point of the grinding head is the origin of the robot end coordinate system T, X T , Y T , Z T , are the unit vectors of the x-axis, y-axis and z-axis in the manipulator end coordinate system T, and their vectors relative to the fixed base coordinate system B are expressed as B X T , B Y T , B Z T , then the posture of the grinding head is expressed as:

[0086] [ B R T B P],

[0087] Where, B R T =[ B X T B Y TB Z T ];

[0088] When calculating the motion equation, the scale factor is introduced into the pose matrix to become a 4×4 homogeneous matrix of the manipulator end coordinate system, which is expressed as follows:

[0089]

[0090] 2) Based on the degrees of freedom of the robotic arm, each of its joints is modeled in its joint coordinate system G, and the solution formula for the grinding head position at the end of the robotic arm is obtained using the robot's forward kinematics;

[0091] In this embodiment, see Figure 5 The robot arm adopts the Jaka zu7 robot arm with six degrees of freedom, and the improved DH method is used for its modeling. Table 1 shows the robot arm parameter table obtained by the improved DH method; Table 2 shows the range of rotation angles of each joint of the robot arm.

[0092] Table 1:

[0093]

[0094]

[0095] Table 2:

[0096] joint Rotation range 1 -270°～270° 2 -85°～265° 3 -170°～170° 4 -85°～265° 5 -270°～270° 6 -270°～270°

[0097] The transformation from the i-1th joint to the i-th joint is the x-axis rotation (α i-1 ) and then translate (a i-1 ), z-axis rotation (θ i ) and then translate (d i ), then the expression of the transformation matrix of the i-1th joint can be obtained as:

[0098]

[0099] In the formula, c is the abbreviation of cos, s is the abbreviation of sin, that is, cθ i =cosθ i ;

[0100] Since the robot's forward kinematics is mainly based on the knowledge of the variables of each joint, the end position is solved. Therefore, the transformation matrix equation from the fixed base of the manipulator to the grinding head is expressed as:

[0101]

[0102] In the formula, [p x p y pz ] T is the translation matrix of joints 0 to 6, [noa] is the rotation matrix of joints 0 to 6;

[0103] The matrix of each joint can be obtained by the above formula, and the matrix of the end can be obtained by multiplying 0 T6.

[0104] In this step, forward kinematics is used to obtain the kinematic equation of the end of the robotic arm through the coordinate transformation of each joint point. That is, after determining the numerical value of each joint variable of the robotic arm, the value of each element in the pose matrix is ​​converted; therefore, when a set of joint variable values ​​is given, there is only one calculation result of the pose matrix, that is, the solution to the forward kinematic equation of the robotic arm is unique.

[0105] 3) See Figure 6 According to the shape of the bounding box, the tool path is designed to move in a straight line to the boundary, then start to move in a vertical direction by the diameter of the maximum contact arc, continue to feed along the straight line, and gradually complete a layer of grinding; then the grinding head feeds downward, and the distance from the center point of the maximum contact arc to the bottom of the grinding head is the grinding depth of the grinding head;

[0106] 4) Based on the shape and size of the grinding head, determine the starting point A and ending point B of the grinding on the bounding box; the coordinates of the starting point A in the Cartesian coordinate system are (x1, y1, z1, α1, β1, γ1), and the coordinates of the ending point B in the Cartesian coordinate system are (x2, y2, z2, α2, β2, γ2);

[0107] S402, see Figure 7 , plan the speed of the grinding head movement process at the end of the robot arm;

[0108] Speed ​​control planning is a crucial component in achieving real-time robotic arm motion. Uniform interpolation methods can cause the robot to experience speed jumps during startup and shutdown, with acceleration approaching infinity. This can cause shock and vibration to the robot, impacting operational quality and its lifespan. To ensure smooth transitions between startup and shutdown of the robot's end effector, acceleration and deceleration control is required. This involves setting the speed change throughout the end effector's motion path.

[0109] The speed of the grinding head at the end of the robot arm of the present application is planned in the form of a seven-segment S-shaped speed curve, that is, the grinding process always remains stable; specifically, see Figure 7, set the acceleration of the grinding head at the end of the robot arm during movement to be J, where J is a constant; the entire movement process is divided into seven sections, namely, acceleration section (T1: 0~t1), uniform acceleration section (T2: t1~t2), deceleration section (T3: t2~t3), uniform speed section (T4: t3~t4), acceleration and deceleration section (T5: t4~t5), uniform deceleration section (T6: t5~t6) and deceleration section (T7: t6~t7);

[0110] Let the speed of the uniform segment be V max , which is the maximum speed in motion, and the motion time of this segment is T4, then the acceleration function expression of the seven-segment S-shaped velocity is:

[0111]

[0112] Where, jerk J is a constant, Among them, the maximum acceleration a max In the uniform acceleration segment T2, a max =JT1;

[0113] The speed function expression of the seven-segment S-shaped speed is:

[0114]

[0115] Where v0 is the initial velocity, v1 to v6 are the initial velocities of the uniform acceleration segment T2 to the deceleration segment T7 respectively; the velocity of the uniform speed segment T4 is the maximum velocity v3 during the motion, i.e. V max ;

[0116] The displacement function expression of the seven-segment S-shaped velocity is:

[0117]

[0118] in,

[0119]

[0120] Where p is the total length of the trajectory, that is, Figure 8 The area formed by the speed curve and the coordinate axis is p, which is the moving distance of the end of the robot arm; p1~p6 are the trajectory lengths between the initial moment of the uniform acceleration section T2~deceleration section T7 and the starting point of grinding.

[0121] In the specific solution process, based on the maximum velocity v max , the maximum acceleration is a max , the running time of the robot system is t, and the overall tool travel distance is p, then the specific steps are as follows:

[0122] (1) Determine whether the maximum speed v can be reached during the acceleration phase max, and for the acceleration time period T a (0~t3) and deceleration time period T b (t4~t7) to solve:

[0123] In the acceleration phase, we can get:

[0124] In the critical case, that is, without uniform acceleration, there are only acceleration and deceleration, and the time is the same, which can be converted to:

[0125] when The maximum acceleration cannot be reached when . At this time, the jerk segment is: The acceleration section is: T a =2T1;

[0126] If the maximum acceleration can be achieved, we can get: JT1 2 +a max T2=v max -v0 and T a =2T1+T2;

[0127] Then sort it out and get:

[0128] The same is true for the deceleration stage. When the maximum acceleration cannot be reached, we can get: T b =2T4,

[0129] If the maximum acceleration can be achieved -a max When , assuming the terminal velocity is v7, we can get:

[0130] (2) The time of the uniform speed section is T4. According to symmetry, we know that:

[0131] The average speed in the acceleration section is:

[0132] The average speed in the deceleration section is:

[0133] Therefore, we can get:

[0134] If T4>0, the maximum speed v can be reached max , and can move at a uniform speed, and the speed parameters of each trajectory can be obtained according to the formula; if T4=0, it can just reach the maximum speed v max , the total time T at this time is: T = T a +T b ;

[0135] If T4<0, the maximum speed v cannot be reached max, if the maximum acceleration a can be achieved max , then we can also get: T = T a +T b ; If the maximum acceleration a cannot be reached max , then gradually reduce the set maximum acceleration, setting the step size to 0.01 of the maximum acceleration until the requirement is met.

[0136] S403, see Figure 6 ,plan the position of the grinding head at the end of the robot arm during the movement process;

[0137] In position planning, the continuity of motion is usually considered, and planning methods include straight-line trajectory, circular trajectory, and spline curve planning. The basic motion trajectory of a robot is mainly composed of straight-line and circular interpolation or a combination of the two. The NURBS interpolation algorithm, which is widely used in spline curves, is suitable for scenarios where the trajectory is more complex and difficult to approximate with straight-line and circular interpolation. However, it is not suitable for real-time interpolation due to the large amount of calculation. Therefore, this application adopts a linear interpolation method combined with an S-shaped velocity curve to achieve position planning for the motion process of the grinding head at the end of the robot arm;

[0138] Specifically, the linear trajectory interpolation formula is:

[0139]

[0140] Where i is any point on the interpolation line, and its coordinates are (x i ,y i , z i ), accordingly, (x0, y0, z0) is the coordinate of the starting point before interpolation; Δt is the interpolation period; v t is the running speed; l i is the displacement from the starting point to point i, n is the number of interpolation points,

[0141] Assume that the movement of the end of the robot arm along the x-axis, y-axis, and z-axis corresponds to the movement feed of each joint driven by the joint driver Δx, Δy, and Δz; then the linear trajectory interpolation formula shows that the joint movement feed is related to the displacement of each interpolation cycle; therefore, when performing linear interpolation, it is necessary to combine the acceleration and deceleration stages of the S-shaped velocity curve;

[0142] Assume that the displacement of the robot arm during the i-th interpolation cycle Δt is p i , the maximum acceleration is a max , the speed of the uniform motion segment is v max , then combined with the displacement function expression of the seven-segment S-shaped velocity in step S403, the corresponding p of the segmented formula can be obtained. i The expression is:

[0143]

[0144] Where, p1~p6 have the same meaning as p1~p6 in the displacement function expression of the seven-segment S-shaped velocity;

[0145] Then, by dividing the p i Substituting this into the linear trajectory interpolation formula, we can obtain the coordinates of the end of the robot arm in the i-th interpolation cycle. At the same time, combined with the verification of the maximum speed and maximum acceleration in speed planning, and the expression of the total time T: T = T a +T b , that is, the total time T can be solved; where T a is the acceleration time period, i.e. 0~t3, T b is the deceleration period, i.e. t4 to t7;

[0146] S404, see Figure 6 ,plan the posture of the grinding head at the end of the robot arm during the movement process;

[0147] For robots, the different postures of the end of the manipulator in space directly affect the work of the instrument. Therefore, simply indicating the position change of the end effector during the movement process cannot guarantee the safe conduct of the operation. It is also necessary to plan and adjust the posture of the end of the manipulator in order to adjust the posture of the instrument approach. The entire manipulator and the grinding approach adjustment mechanism can be regarded as a rigid body as a whole, so the posture planning is specifically a rigid body posture planning problem. The interpolation of the posture is usually planned using quaternions, which only requires four parameters. It can represent the posture and can also be represented as a rotational transformation, and does not produce singular points. Therefore, the present application adopts a posture interpolation method combined with an S-shaped velocity curve to achieve posture planning for the movement process of the grinding head at the end of the manipulator.

[0148] Among them, any quaternion q is a hypercomplex number composed of three real numbers plus imaginary units i, j, k, and its form is the formula:

[0149]

[0150] In the above formula, q s represents the scalar part of q; q v represents the vector part of q; i, j, k represent unit direction vectors, and i 2 =j 2 =k 2 =-1; q1, q2, q3 are arbitrary real constants;

[0151] Quaternion modulus When the modulus is 1, q is the unit quaternion;

[0152] The unit quaternion is expressed in the form of Euler angles as:

[0153]

[0154] Where, is the unit direction vector of the Euler axis, and θ is the Euler angle.

[0155] When the end-of-arm device performs posture interpolation, the commonly used algorithm is the spherical linear interpolation algorithm of the unit quaternion, which can construct a smooth posture trajectory between two posture teaching points; this application uses the unit quaternion for posture description, and the spherical linear interpolation algorithm can be regarded as interpolating along the arc between the two posture teaching points on the hypersphere, see Figure 9 ;

[0156] In order to use the unit quaternion to realize the rotation transformation of the rigid body in space, let the unit quaternion be A point A in space, when rotated, is equivalent to rotating by an angle θ around the vector axis u = (x, y, z);

[0157] When performing a rotation transformation, point A is expanded to the quaternion space, and a = (a, 0) is set. The expression of the quaternion corresponding to the rotated point is:

[0158] a′=qaq -1 ,

[0159] In the formula, the real part of a′ is 0, and the imaginary part is the coordinate of a after rotation;

[0160] The interpolation between two postures represented by unit quaternions is the interpolation of the angular displacement θ of the two posture points under the action of the rotation operator. The relationship between the angular displacement and the unit quaternions of the two posture points is expressed as:

[0161] cosθ=a·a′,

[0162] θ is the real part of the unit quaternion q that rotates a to a′;

[0163] The homogeneous transformation matrix T at the known starting point A and end point B A and T B After that, the quaternion-based interpolation steps are as follows:

[0164] (1) First, in the homogeneous transformation matrix T A and T B Extract the respective rotation matrix R A and R B , and convert it to a quaternion Q A and Q B , the angle between the two quaternions is θ, and its cosine value can be expressed as:

[0165] cosθ=QA Q B ,

[0166] Where θ is Q A Rotate to Q B The real part of the unit quaternion, the value range of θ is [0,π];

[0167] Quaternions can be used to represent rotation and posture in three-dimensional space. Its representation is equivalent to orthogonal matrix, Euler angle and other methods. Let but:

[0168] The rotation matrix conversion formula of quaternion is:

[0169]

[0170] Therefore, the expression of the attitude matrix corresponding to the unit quaternion q is:

[0171]

[0172] The corresponding expression of the posture matrix is:

[0173]

[0174] Then we get the expression of quaternion Q:

[0175]

[0176] (2) Since quaternion interpolation usually uses spherical linear interpolation to interpolate the angular displacement between two postures, the parameter expression Q(u) of the posture trajectory corresponding to spherical linear interpolation with respect to the parameter u is Q(u), and the formula for spherical linear interpolation of posture quaternion is:

[0177]

[0178] Where: θ=arccos(Q A Q B ). Q(u) is also a quaternion, 0≤u≤1, let the total motion time be T, the interpolation period be Δt, and the value of u is

[0179] Considering that the position and posture of the grinding ball at the end of the robot arm usually change simultaneously during the movement of the robot arm, the posture trajectory also needs to be planned in combination with the S-shaped velocity curve, just like the position trajectory planning in step S403; therefore, after sorting,

[0180] The expression of the relationship between angular displacement and time during the posture change process is:

[0181]

[0182] Wherein, w0 to w6 are the angular velocities of the acceleration increasing section T1 to the deceleration decreasing section T6 respectively, and θ1 to θ6 are the angular displacements between the initial moments of the uniform acceleration section T2 to the deceleration decreasing section T7 and the starting point of grinding respectively;

[0183] Similar to the position planning, for the angular displacement in the i-th interpolation period Δt, replace t in the above expression with iΔt, which represents the angular displacement after the i-th interpolation period;

[0184] Let the total angular displacement θ = arccos(Q A iQ B ), and let Substitute it into the spherical linear interpolation formula of the attitude quaternion, and the quaternion varying with the interpolation period Δt can be obtained; furthermore, substitute the attitude quaternion into the attitude matrix corresponding to the unit quaternion q to obtain the attitude matrix.

[0185] S5. Adopt the inverse dynamics solution method to solve the variables of each joint of the robotic arm from the pose of the grinding head at the end of the robotic arm, see Figure 10 ;

[0186] Specifically, when the pose at the end is known, [p x p y p z T and [n o a] are both known quantities, then the expression of the transformation matrix equation from the fixed base of the robotic arm to the grinding head:

[0187]

[0188] Further transform it into:

[0189]

[0190] After setting the angle, T5 can be obtained through the above formula; and by making the left and right sides of the transformation matrix expression from the base to the end flange of the robotic arm and the deformation formula of the transformation matrix from the fixed base of the robotic arm to the grinding head equal, θ 1 (0 < i ≤ 6) can be sorted out. The above is the principle of the inverse kinematics solution by the analytical method. i (0 < i ≤ 6). The above is the principle of the inverse kinematics solution by the analytical method. ​​​​​In this embodiment, the simulation software uses Matlab 2021b, and the Robotics Toolbox 10.4 toolbox is used to simulate the trajectory motion of the Jaka zu7 collaborative robot arm. According to the improved DH parameters of Jaka zu7, the motion range of each joint of the robot arm is set, and the robot model is established, such as Figure 11 shown.

[0193] Experimental design and programming were conducted for velocity planning, position interpolation, and attitude interpolation. The velocity planning algorithm segmented the velocity changes, combining position isochronous interpolation with attitude angular displacement. Furthermore, the model was verified in Matlab by designing reasonable tool speed data and setting speed and travel distance. Position interpolation obtained visual interpolation trajectories and data by setting starting and ending path points and the number of interpolations. Attitude interpolation generated attitude change diagrams by setting starting and ending poses. Finally, trajectory points were set.

[0194] (1) Speed ​​curve simulation:

[0195] Select a set of appropriate grinding speed data for the grinding tool: maximum speed v max =10mm / s, maximum acceleration a max =15mm / s 2 , acceleration J = 30 mm / s 3 , the running distance p = 30mm, and the time t is 4.14s; by designing the simulation experiment of the S-shaped speed curve, we get the following Figure 12 The velocity curve simulation diagram shown in the figure can also obtain the coordinates of a certain moment. According to the change of motion parameters in the figure, the change pattern of velocity and acceleration, as well as the change curve of displacement, can be intuitively reflected.

[0196] (2) Position interpolation simulation:

[0197] Assume that according to the path in the overall plan, the path points are set, and the starting point A (300, 220, 40) and the ending point B (320, -50, 220) of a straight line interpolation are set; assuming that only the position is interpolated and the posture remains unchanged, the number of interpolations is set to 50 times. For the sake of intuitiveness, the unit of the figure below is set to mm; through simulation, the following is obtained Figure 13 The interpolation result diagram shown in the figure and Figure 14 Schematic diagram of the robot arm simulation trajectory shown;

[0198] (3) Posture interpolation simulation:

[0199] According to the process of the angular displacement of the posture changing with the position, the unit quaternion linear trajectory interpolation program is written; among them, the starting posture is set to The final position is Get as Figure 15 The diagram of posture interpolation is shown.

[0200] The posture of the end of the robot arm starts from the starting state and gradually changes according to the interpolation cycle until it reaches the end posture;

[0201] (4) Motion trajectory drawing:

[0202] Based on the shape of the bounding box, a three-dimensional cuboid was created, with the lengths in the three directions set to 50 to 80 for x, -5 to 5 for y, and 0 to 10 for z. Furthermore, based on the grinding radius and feed direction during tool grinding, the spacing between two track points in the plane was set to 4 mm, and the spacing between the tracks of the upper and lower layers was set to 2 mm, resulting in the following: Figure 16 The simulation diagram of the trajectory points shown and Figure 17 The trajectory simulation diagram is shown.

[0203] Through the simulation experiment of the above motion path planning, it was possible to guide the end of the robotic arm to move and draw a trajectory, verifying the feasibility and effectiveness of the grinding trajectory planning method of the present application.

[0204] The above content is only a preferred embodiment proposed according to the present invention. For ordinary technicians engaged in the technical field to which the present invention belongs, new improvements and upgrades can be made without departing from the concept of the present invention. These improvements and upgrades should also be regarded as the scope of protection of the present invention.

Claims

1. A robot grinding trajectory planning method, characterized in that: Here are the steps: S1. Constructing a three-dimensional digital model of a complex structure, and determining the outer contour coordinates of a microstructure to be ground based on the three-dimensional digital coordinate system of the three-dimensional digital model, so as to convert the microstructure to be ground into a point cloud. The specific implementation steps of step S1 are: 1) obtaining a three-dimensional digital model of the complex structure, and determining an outer contour coordinate set of the complex structure in the three-dimensional digital coordinate system; 2) determining a position of the microstructure to be ground in the three-dimensional digital model of the complex structure, and obtaining an outer contour coordinate set of the microstructure in the three-dimensional digital coordinate system; 3) converting the outer contour coordinate set of the microstructure in the three-dimensional digital coordinate system into point cloud data and visually displaying it; S2. Generate an OBB bounding box of the microstructure based on the microstructure point cloud and determine the coordinates of eight vertices of the bounding box in a three-dimensional digital coordinate system; S3, performing coordinate registration of the OBB bounding box of the microstructure determined in step S2 with the complex structure fixed on the grinding table to determine the actual grinding position of the grinding tool; S4. Plan the grinding motion trajectory of the robotic arm fixed on the grinding table and equipped with a grinding tool, including speed planning, position planning and posture planning of the grinding head at the end of the robotic arm; wherein, the specific implementation steps of this step are: 1) construct the fixed base coordinate system of the robotic arm, the end coordinate system of the manipulator and the joint coordinate system to obtain the kinematic equation of the grinding head at the end of the robotic arm, including the transformation matrix of each joint and the transformation matrix from the fixed base of the robotic arm to the grinding head; at the same time, design the tool path according to the shape of the bounding box, and determine the starting point and end point of the grinding on the bounding box according to the shape and size of the grinding head; 2) the speed planning adopts the seven-segment S-shaped speed curve method, that is, the entire motion process is divided into seven segments: acceleration, uniform acceleration segment, deceleration segment, uniform speed segment, acceleration and deceleration segment, uniform deceleration segment and deceleration segment; 3) the position planning adopts the linear interpolation method combined with the seven-segment S-shaped speed curve; 4) the posture planning adopts the quaternion attitude interpolation method combined with the seven-segment S-shaped speed curve; S5. Using the inverse dynamics solution method, the position of the grinding head at the end of the robotic arm is used to solve the variables of each joint of the robotic arm.

2. The robot grinding trajectory planning method according to claim 1, characterized in that: The specific implementation steps of step S2 are as follows: 1) using the PCA principal component analysis method to obtain the centroid of the microstructure point cloud, and generating an OBB bounding box based on the centroid, and then establishing a bounding box coordinate system with the centroid as the origin and the length, width, and height of the bounding box as the x-axis direction, y-axis direction, and z-axis direction respectively; 2) Based on the bounding box coordinate system, the coordinates of the eight vertices of the bounding box are obtained; 3) Based on the coordinates of the eight vertices of the bounding box, the coordinate set of the microstructure point cloud is obtained; Based on the coordinate sets of the tiny structure point cloud in the bounding box coordinate system and the three-dimensional digital coordinate system, the transformation matrix between the bounding box coordinate system and the three-dimensional digital coordinate system is obtained; then, the coordinates of the eight vertices of the bounding box in the three-dimensional digital coordinate system can be obtained using the transformation matrix.

3. The robot grinding trajectory planning method according to claim 1, characterized in that: The specific implementation steps of step S3 are: 1) constructing a physical coordinate system based on the complex structure fixed on the grinding table to obtain the outer contour coordinate set of the complex structure; 2) using the outer contour coordinate sets of the complex structure in the physical coordinate system and the three-dimensional digital coordinate system respectively, to obtain the transformation matrix between the physical coordinate system and the three-dimensional digital coordinate system; using this transformation matrix, the coordinates of the eight vertices of the bounding box in the physical coordinate system can be obtained.

4. The robot grinding trajectory planning method according to claim 1, characterized in that: The specific implementation steps of step 2) in step S4 are: The entire motion process is divided into seven sections, namely, acceleration section T1, i.e., 0~t1, uniform acceleration section T2, i.e., t1~t2, deceleration section T3, i.e., t2~t3, uniform speed section T4, i.e., t3~t4, acceleration / deceleration section T5, i.e., t4~t5, uniform deceleration section T6, i.e., t5~t6 and deceleration section T7, i.e., t6~t7; The acceleration function expression of the seven-segment S-shaped velocity is: Where, Maximum acceleration a max In the uniform acceleration section T2, a max =JT1; The speed function expression of the seven-segment S-shaped speed is: Where v0 is the initial velocity, v1 to v6 are the initial velocities of the uniform acceleration segment T2 to the deceleration segment T7 respectively; the velocity of the uniform speed segment T4 is the maximum velocity v3 during the motion, i.e. V max ; The displacement function expression of the seven-segment S-shaped velocity is: Where, p is the total length of the trajectory, that is, the moving distance of the grinding head at the end of the robot arm; p1~p6 are the trajectory lengths between the position of the grinding head at the end of the robot arm and the starting point of its grinding at the initial moment of segments T2~T7.

5. The robot grinding trajectory planning method according to claim 1, characterized in that: The specific implementation steps of step 3) in step S4 are: The linear trajectory interpolation formula is: Where i is any point on the interpolation line, and its coordinates are (x i ,y i , z i ), accordingly, (x0, y0, z0) is the coordinate of the starting point before interpolation; Δt is the interpolation period; v t is the running speed; l i is the displacement from the starting point to point i, n is the number of interpolation points, Assume that the movement of the end of the robot arm along the x-axis, y-axis, and z-axis corresponds to the movement feed amount Δx, Δy, and Δz of each joint driven by the joint driver; Assume that the displacement of the robot arm during the i-th interpolation cycle Δt is p i , the maximum acceleration is a max , the uniform speed is v max , then combined with the displacement function expression of the seven-segment s-shaped velocity, the corresponding p i The expression is: Then, by combining the different stages of p i Substituting this into the linear trajectory interpolation formula, we get the coordinates of the end of the robot arm in the i-th interpolation cycle; the expression for the total time T is: T = T a +T b ,,T a is the acceleration time period, i.e. 0~t3, T b It is the deceleration time period, i.e. t4 to t7.

6. The robot grinding trajectory planning method according to claim 1, characterized in that: The specific implementation steps of step 4) in step S4 are: The expression of the relationship between angular displacement and time during the posture change process is: Where w0~w6 are the angular velocities of the acceleration section T1 to the deceleration section T6, θ1~θ6 are the angular displacements between the grinding head position at the end of the manipulator and its grinding starting point at the initial moment of the uniform acceleration section T2 to the deceleration section T7; Assume that the total angular displacement θ = arccos(Q A iQ B ), and order Substituting into the spherical linear interpolation formula of the attitude quaternion, we can obtain the quaternion that changes with the interpolation period Δt; Among them, the spherical linear interpolation formula of the attitude quaternion is: Where Q(u) is a quaternion, 0≤u≤1, the total motion time is T, the interpolation period is Δt, and the value of u is Q A is the homogeneous transformation matrix T from the grinding starting point A A The rotation matrix R extracted from A , the converted quaternion; Q B is the homogeneous transformation matrix T of the self-grinding end point B B The rotation matrix R extracted from B , the converted quaternion; Then, the attitude quaternion is substituted into the attitude matrix corresponding to the unit quaternion q to obtain the attitude matrix.

Images