A method for attitude trajectory transition planning of the end of a six-axis serial industrial robot

Abstract

The present invention discloses a method for attitude trajectory transition planning at the end of a six-axis serial industrial robot. The present invention uses quaternions to describe the attitude and employs a cubic quaternion quasi-uniform B-spline curve as the attitude trajectory transition segment to achieve smooth transition of the attitude trajectory. The method simplifies the way of obtaining the control points of the quaternion spline curve and reduces the implementation difficulty of the algorithm. The angular displacement length of the attitude trajectory transition segment is obtained by using the composite six-point Gauss-Legendre integration method, and the interpolation calculation uses the Newton iteration method combined with the six-point Gauss-Legendre integration method. Thereby, it drives the end of the six-axis serial industrial robot to move continuously along the entire smooth path, obtains a smooth motion trajectory, avoids frequent acceleration and deceleration, improves the operation efficiency of the six-axis serial industrial robot, and meets the process requirements of production practice.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly relates to a method for attitude trajectory transition planning at the end of a six-axis serial industrial robot. Background Art

[0002] The attitude trajectory planning algorithm at the end of a six-axis serial industrial robot needs to construct a smooth attitude trajectory between two or more attitude teaching points. However, the attitude trajectory lacks intuitiveness and the way to describe the attitude is relatively complex, so the research on attitude trajectory planning is relatively difficult. However, the attitude trajectory planning algorithm at the end of a six-axis serial industrial robot also has a great impact on the operation efficiency of the six-axis serial industrial robot. A good attitude trajectory planning algorithm can greatly improve the motion accuracy of the six-axis serial industrial robot, reduce the vibration and shock suffered by the six-axis serial industrial robot, and thus extend the service life of the six-axis serial industrial robot. Therefore, the research on the attitude trajectory planning algorithm at the end of a six-axis serial industrial robot is very important. However, common attitude trajectory planning algorithms, such as the spherical linear interpolation algorithm based on quaternion, also have the problem of trajectory mutation at the trajectory connection, resulting in the angular velocity of the six-axis serial industrial robot having to be reduced to zero at the attitude trajectory connection, and the six-axis serial industrial robot still performs frequent acceleration and deceleration, reducing the operation efficiency of the six-axis serial industrial robot. Therefore, it is necessary to design an attitude trajectory transition planning algorithm to use an attitude spline curve to construct an attitude trajectory transition section to perform transition processing on the attitude trajectory. However, it is very difficult to deduce the parameter expression of the attitude spline curve when using Euler angles, rotation matrices, and axis-angle methods to describe the attitude, and the algorithm implementation is difficult. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a method for attitude trajectory transition planning at the end of a six-axis serial industrial robot that can achieve smooth transition of the attitude trajectory and improve the operation efficiency of the robot.

[0004] To solve the above problems, the present invention provides a method for attitude trajectory transition planning at the end of a six-axis serial industrial robot, including the following steps:

[0005] S1. Let i = 1, and N be the total number of segments of the given motion path;

[0006] S2. Perform smooth transition between the spherical linear interpolation trajectory formed by attitude teaching points Q i and Q i+1 and the spherical linear interpolation trajectory formed by attitude teaching points Q i+1 and Q i+2 ; wherein, a cubic quaternion quasi-uniform B-spline curve is used as the attitude trajectory transition section; attitude teaching points Q i and Q i+1They are respectively the starting attitude point and the ending attitude point of the i-th segment of the attitude trajectory; the attitude teaching point Q i+1 and Q i+2 They are respectively the starting attitude point and the ending attitude point of the (i + 1)-th segment of the attitude trajectory;

[0007] S3. According to the starting attitude point Q i and the ending attitude point Q i+1 of the i-th segment of the attitude trajectory, obtain the angular displacement length θ i and the parameter Q′ i (u); the expression of the parameter Q′ i (u) is as follows:

[0008]

[0009] S4. According to the starting attitude point Q i+1 and the ending attitude point Q i+2 of the (i + 1)-th segment of the attitude trajectory, obtain the angular displacement length θ i+1 and the parameter Q′ i+1 (u); the expression of the parameter Q′ i+1 (u) is as follows:

[0010]

[0011] S5. According to the angular displacement length θ i of the i-th segment of the attitude trajectory, the angular displacement length θ i+1 of the (i + 1)-th segment of the attitude trajectory, and the given attitude transition radius obtain the actual attitude transition radius

[0012] S6. According to the angular displacement length θ i of the i-th segment of the attitude trajectory and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition starting point of the transition segment of the i-th segment of the attitude trajectory, and then according to the parameter Q′ i (u) and the derivative calculation method, obtain the attitude, the first derivative, and the second derivative of the i-th segment of the attitude trajectory at the attitude transition starting point;

[0013] S7. According to the angular displacement length θ i+1 of the (i + 1)-th segment of the attitude trajectory and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition ending point of the transition segment of the i-th segment of the attitude trajectory, and then according to the parameter Q′ i+1 (u) and the derivative calculation method, obtain the attitude, the first derivative, and the second derivative of the (i + 1)-th segment of the attitude trajectory at the attitude transition ending point;

[0014] S8. Establish the parametric expression q i (u) of the i-th attitude trajectory transition segment;

[0015] S9. Establish a system of equations according to the conditions that need to be satisfied for the curvature continuity of the splicing of two attitude curves, and after obtaining the attitudes of the attitude control points, substitute them into the parametric expression q i (u) of the i-th attitude trajectory transition segment;

[0016] S10. Calculate the angular displacement length of the i-th attitude trajectory transition segment

[0017] S11. According to the angular displacement length of the i-th attitude trajectory transition segment, conduct speed planning to obtain the functional relationship between the angular displacement length and time, so as to obtain the relative angular displacement length of the interpolation point on the attitude trajectory transition segment according to the moment of the interpolation point. Then, inversely solve the parameter value corresponding to the interpolation point according to the relative angular displacement length of the interpolation point on the attitude trajectory transition segment. Finally, substitute the parameter value corresponding to the interpolation point into the parametric expression of the attitude trajectory transition segment to obtain the attitude of the interpolation point;

[0018] S12. Let i = i + 1; judge whether i < N - 2 holds. If it holds, execute step S2; if not, execute step S14;

[0019] S13. Obtain the rotation angles of each joint of the six-axis serial industrial robot through inverse kinematics to drive the six-axis serial industrial robot to rotate, so as to realize the attitude trajectory transition of the end of the six-axis serial industrial robot.

[0020] As a further improvement of the present invention, the angular displacement length θ i of the i-th attitude trajectory is as follows:

[0021] θ i = arccos(Q i , Q i+1 );

[0022] The angular displacement length θ i+1 of the (i + 1)-th attitude trajectory is as follows:

[0023] θ i+1 = arccos(Q i+1 , Q i+2 ).

[0024] As a further improvement of the present invention, step S5 further includes:

[0025] Judge whether or holds. If it does not hold, let If it holds, then judge whether θ i > θi+1 Whether it holds. If it holds, let If it does not hold, let

[0026] As a further improvement of the present invention, step S6 includes:

[0027] According to the angular displacement length θ of the i-th segment of the attitude trajectory i and the actual attitude transition radius Obtain the parameter value corresponding to the attitude transition starting point of the i-th segment of the attitude trajectory transition segment as follows:

[0028]

[0029] Then, according to the parameter Q′ i (u) and the derivative calculation method, obtain the attitude first-order derivative and second-order derivative

[0030]

[0031]

[0032]

[0033] As a further improvement of the present invention, step S7 includes:

[0034] According to the angular displacement length θ of the (i + 1)-th segment of the attitude trajectory i+1 and the actual attitude transition radius Obtain the parameter value corresponding to the attitude transition end point of the i-th segment of the attitude trajectory transition segment as follows:

[0035]

[0036] Then, according to the parameter Q′ i+1 (u) and the derivative calculation method, obtain the attitude first-order derivative and second-order derivative as follows:

[0037]

[0038]

[0039]

[0040] As a further improvement of the present invention, in step S8, the parametric expression q i (u) of the i-th attitude trajectory transition segment is established as follows:

[0041]

[0042] wherein, B j,3 (u), j = 0, 1, 2, 3, 4 is obtained as shown in Equation (12). Substitute N 0,3 (u), N 1,3 (u), N 2,3 (u), N 3,3 (u) and N 4,3 (u) into Equation (12) to sequentially obtain B 0,3 (u), B 1,3 (u), B 2,3 (u), B 3,3 (u) and B 4,3 (u); and are the attitudes of six attitude control points of the attitude trajectory transition segment;

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050] According to q i (u), q i (0) is obtained as shown in Equation (19), q i (1) is obtained as shown in Equation (20). Then, according to q i (u), the first derivative is obtained as shown in Equation (21), as shown in Equation (22). Then, the second derivative is obtained as shown in Equation (23), as shown in Equation (24);

[0051]

[0052]

[0053]

[0054]

[0055]

[0056]

[0057] As a further improvement of the present invention, step S9 includes: establishing a system of equations according to the conditions that need to be satisfied for the curvature continuity of the splicing of two attitude curves, as follows:

[0058]

[0059] Substituting formulas (19) - (24) gives formula (26), as follows:

[0060]

[0061] where both α and β are arbitrary constants greater than zero;

[0062] After obtaining the attitudes of the attitude control points and according to formula (26), substitute them into formula (11). As a further improvement of the present invention, the obtaining of the attitudes of the attitude control points and according to formula (26) includes:

[0063] Obtain formula (27) according to formula (26), and then substitute the obtained attitude control points and as well as the parameters α and β into formula (28) to obtain the attitude control points and Finally, substitute the obtained attitude control points and as well as the parameters α and β into formula (29) to obtain the attitude control points and

[0064]

[0065]

[0066]

[0067] Since α and β can be any constants greater than zero, there are countless sets of attitude control points that satisfy the conditions; introduce the parameter λ to replace the parameters α and β, and the relationship between the parameter λ and the parameters α and β is shown in formula (30);

[0068]

[0069] As a further improvement of the present invention, step S10 includes:

[0070] The composite six-point Gauss-Legendre integration method is used to obtain the angular displacement length of the transition section of the i-th attitude trajectory As shown in Equation (31):

[0071]

[0072] where x k is the integration node, and A k is the integration coefficient. The specific numerical values are as shown in Equation (32):

[0073]

[0074] As a further improvement of the present invention, step S11 includes:

[0075] S111. Given the relative angular displacement length θ l ,

[0076] of the interpolation point along the trajectory direction on the hypersphere from the attitude transition starting point of the i-th attitude trajectory transition section l and the angular displacement lengths obtained in each interval, find the segment number m of the interval where the interpolation point is located, m = 1, 2, ···, 6, so as to obtain the search interval of the Newton iteration method as and correct the relative angular displacement length θ l according to Equation (33);

[0077]

[0078] S113. Let the initial iteration value u l of the Newton iteration method be (m - 1) / 6;

[0079] S114. Construct an error function f(u l ) with u l as the independent variable as shown in Equation (34), and substitute the value of u l to calculate the value of f(u l );

[0080]

[0081] where, when calculating the value of the error function f(u l ), integration is also required. The six-point Gauss-Legendre integration method is used for integration, and f(ul ) is calculated according to the formula shown in Equation (35).

[0082]

[0083] Among them, x k is the integration node, and A k is the integration coefficient, and the specific values are shown in Equation (32);

[0084] S115. Determine whether the absolute value of f(u l ) is greater than 0.00001. If it holds, jump to step S6; otherwise, jump to step (8);

[0085] S116. Iterate u l using the iteration formula shown in Equation (36);

[0086]

[0087] S117. Substitute the value of the iterated u l into Equation (35) to obtain the value of f(u l ), and then jump to step S5;

[0088] S118. Let u = u l and substitute it into Equation (11) of the i-th segment of the attitude trajectory transition section to obtain the attitude q l of the interpolation point = q i (u).

[0089] Advantages of the present invention:

[0090] The present invention uses quaternions to describe the attitude, and utilizes the cubic quaternion quasi-uniform B-spline curve as the attitude trajectory transition section to achieve smooth transition of the attitude trajectory. It simplifies the method of obtaining the control points of the quaternion spline curve and reduces the implementation difficulty of the algorithm. The angular displacement length of the attitude trajectory transition section is calculated using the composite six-point Gaussian Legendre integration method, and the interpolation calculation uses the Newton iteration method combined with the six-point Gaussian Legendre integration method. Thereby, the end of the six-axis serial industrial robot is driven to move continuously along the entire smooth path, obtaining a smooth motion trajectory, avoiding frequent acceleration and deceleration, improving the operating efficiency of the six-axis serial industrial robot, and meeting the process requirements of production practice.

[0091] The above description is only an overview of the technical solution of the present invention. In order to understand the technical means of the present invention more clearly, it can be implemented according to the content of the specification. And in order to make the above and other purposes, features, and advantages of the present invention more obvious and understandable, the following preferred embodiments are specifically given and described in detail in conjunction with the accompanying drawings. Brief Description of the Drawings

[0092] Figure 1 Flow chart of the attitude trajectory transition planning method for the end of a six-axis serial industrial robot in an embodiment of the present invention;

[0093] Figure 2 Schematic diagram of constructing an attitude trajectory transition path between given motion paths in an embodiment of the present invention;

[0094] Figure 3 Schematic diagram of attitude trajectory transition when different values of λ are taken in an embodiment of the present invention. Detailed implementation manners

[0095] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and be able to implement it, but the specific embodiments cited are not intended to limit the present invention.

[0096] As Figure 1-2 shown, the attitude trajectory transition planning method for the end of a six-axis serial industrial robot in a preferred embodiment of the present invention includes the following steps:

[0097] Step S1: Let i = 1, and N be the total number of segments of the given motion path.

[0098] Step S2: Perform smooth transition between the spherical linear interpolation trajectories formed by the attitude teaching points Q i and Q i+1 and the spherical linear interpolation trajectories formed by the attitude teaching points Q i+1 and Q i+2 ; among them, a cubic quaternion quasi-uniform B-spline curve is used as the attitude trajectory transition segment; among them, the attitude teaching points Q i and Q i+1 are respectively the starting attitude point and the ending attitude point of the i-th attitude trajectory, the given maximum angular velocity constraint is w i , and the given attitude transition radius is The attitude teaching points Q i+1 and Q i+2 are respectively the starting attitude point and the ending attitude point of the (i + 1)-th attitude trajectory, and the given maximum angular velocity constraint is w i+1 ; is the actual attitude transition radius when the i-th attitude trajectory and the (i + 1)-th attitude trajectory are in transition, and are respectively the attitude transition starting point and the attitude transition ending point of the i-th attitude trajectory transition segment, is the angular displacement length of the i-th attitude trajectory transition segment, is the maximum angular velocity constraint of the angular velocity sensitive point on the attitude trajectory transition segment, and It is the attitude of six attitude control points of the cubic quaternion quasi-uniform B-spline curve as the attitude trajectory transition segment.

[0099] Step S3: According to the starting attitude point Q of the attitude teaching of the i-th attitude trajectory i and the ending attitude point Q i+1 obtain the angular displacement length θ of the i-th attitude trajectory i and the parameter Q′ i (u); The expression of the parameter Q′ i (u) is as follows:

[0100]

[0101] The angular displacement length θ of the i-th attitude trajectory i , is as follows:

[0102] θ i = arccos(Q i , Q i+1 ).

[0103] Step S4: According to the starting attitude point Q of the attitude teaching of the (i + 1)-th attitude trajectory i+1 and the ending attitude point Q i+2 obtain the angular displacement length θ of the (i + 1)-th attitude trajectory i+1 and the parameter Q′ i+1 (u); The expression of the parameter Q′ i+1 (u) is as follows:

[0104]

[0105] The angular displacement length θ of the (i + 1)-th attitude trajectory i+1 , is as follows:

[0106] θ i+1 = arccos(Q i+1 , Q i+2 ).

[0107] Step S5: According to the angular displacement length θ of the i-th attitude trajectory i , the angular displacement length θ of the (i + 1)-th attitude trajectory i+1 and the given attitude transition radius obtain the actual attitude transition radius It also includes:

[0108] Judge or Whether it holds. If it does not hold, then let If it holds, then judge whether θ i > θ i+1 holds. If it holds, then let If it does not hold, then let

[0109] Step S6. According to the angular displacement length θ of the i-th segment of the attitude trajectory i and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition starting point of the transition section of the i-th segment of the attitude trajectory, and then according to the parameter Q′ i (u) and the derivative calculation method, obtain the attitude, first derivative, and second derivative of the i-th segment of the attitude trajectory at the attitude transition starting point; optionally, Step S6 includes:

[0110] According to the angular displacement length θ of the i-th segment of the attitude trajectory i and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition starting point of the transition section of the i-th segment of the attitude trajectory as follows:

[0111]

[0112] Then, according to the parameter Q′ i (u) and the derivative calculation method, obtain the attitude first derivative and second derivative

[0113]

[0114]

[0115]

[0116] Step S7. According to the angular displacement length θ of the (i + 1)-th segment of the attitude trajectory i+1 and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition end point of the transition section of the i-th segment of the attitude trajectory, and then according to the parameter Q′ i+1 (u) and the derivative calculation method, obtain the attitude, first derivative, and second derivative of the (i + 1)-th segment of the attitude trajectory at the attitude transition end point; optionally, Step S7 includes:

[0117] According to the angular displacement length θ of the (i + 1)-th segment of the attitude trajectory i+1 and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition end point of the transition section of the i-th segment of the attitude trajectory as follows:

[0118]

[0119] Then, according to the parameter Q′ i+1Obtain the attitude at the attitude transition end point of the (i + 1)-th segment attitude trajectory by the method of obtaining (u) and derivatives First derivative and second derivative as follows:

[0120]

[0121]

[0122]

[0123] Step S8: Establish the parameter expression q i (u) of the transition segment of the i-th segment attitude trajectory; as follows:

[0124]

[0125] wherein, B j,3 (u), j = 0, 1, 2, 3, 4 are obtained as shown in Equation (12). Substitute N 0,3 (u), N 1,3 (u), N 2,3 (u), N 3,3 (u) and N 4,3 (u) into Equation (12) to sequentially obtain B 0,3 (u), B 1,3 (u), B 2,3 (u), B 3,3 (u) and B 4,3 (u); and are the attitudes of six attitude control points of the attitude trajectory transition segment;

[0126]

[0127]

[0128]

[0129]

[0130]

[0131]

[0132]

[0133] According to q i (u), obtain q i (0) as shown in Equation (19), q i(1) As shown in Equation (20), then according to q i (u), the first derivative is obtained As shown in Equation (21), As shown in Equation (22), then the second derivative is obtained As shown in Equation (23), As shown in Equation (24);

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] Step S9: Establish a system of equations according to the conditions that need to be satisfied for the curvature continuity in the curve splicing of the two attitude curves, and after obtaining the attitudes of the attitude control points, substitute them into the parameter expression q i (u);

[0141] Among them, establishing a system of equations according to the conditions that need to be satisfied for the curvature continuity in the curve splicing of the two attitude curves is as follows:

[0142]

[0143] Substituting Formulas (19) - (24) gives Formula (26), as follows:

[0144]

[0145] Among them, both α and β are arbitrary constants greater than zero;

[0146] After obtaining the attitudes of the attitude control points and according to Formula (26), substitute them into Formula (11).

[0147] It can be seen from Equation (26) that the obtaining of the attitude control points and is not affected by the parameters α and β. Among them, the obtaining of the attitudes of the attitude control points and according to Formula (26) includes:

[0148] Obtain Formula (27) according to Formula (26), and then according to the obtained attitude control points and Substitute and the parameters α and β into Equation (28) to obtain the attitude control points and Finally, substitute the obtained attitude control points and as well as the parameters α and β into Equation (29) to obtain the attitude control points and

[0149]

[0150]

[0151]

[0152] α and β can be any constants greater than zero, so there are countless sets of attitude control points that meet the conditions; introduce the parameter λ to replace the parameters α and β, and the relationship between the parameter λ and the parameters α and β is shown in Equation (30);

[0153]

[0154] Perform a simulation experiment on the attitude trajectory transition planning algorithm in the MATLAB software, and compare the transition effects of the attitude trajectory transition segments constructed when the parameter λ takes different values more intuitively through the visualization processing of the attitude trajectory; the attitude trajectory transition segment constructed when λ is 0.18 achieves smooth connection with the adjacent attitude trajectories, and the transition effect meets the expected goal, which can effectively improve the operation efficiency of the end of the six-axis serial industrial robot; finally, select λ = 0.18 to obtain the appropriate attitude control points and obtain the parameter expression of the attitude trajectory transition segment.

[0155] Step S10: Calculate the angular displacement length of the i-th attitude trajectory transition segment Optionally, Step S10 includes:

[0156] Use the composite six-point Gauss-Legendre integration method to obtain the angular displacement length of the i-th attitude trajectory transition segment As shown in Equation (31):

[0157]

[0158] where x k is the integration node, A k is the integration coefficient, and the specific numerical values are shown in Equation (32):

[0159]

[0160] Step S11: According to the angular displacement length of the i-th attitude trajectory transition segment Perform velocity planning to obtain the functional relationship between the angular displacement length and time, so as to obtain the relative angular displacement length of the interpolation point on the attitude trajectory transition segment according to the time of the interpolation point. Then, inversely solve the parameter value corresponding to the interpolation point according to the relative angular displacement length of the interpolation point on the attitude trajectory transition segment. Finally, substitute the parameter value corresponding to the interpolation point into the parameter expression of the attitude trajectory transition segment to obtain the attitude of the interpolation point. Optionally, step S11 includes:

[0161] S111. Given the relative angular displacement length θ of the interpolation point along the trajectory direction on the i-th attitude trajectory transition segment from the attitude transition starting point of the i-th attitude trajectory transition segment l ,

[0162] S112. Find the segment number m of the interval where the interpolation point is located according to the relative angular displacement length θ l and the angular displacement lengths obtained in each interval, where m = 1, 2, ···, 6, so as to obtain the search interval of the Newton iteration method as and correct the relative angular displacement length θ l according to formula (33);

[0163]

[0164] S113. Let the initial iteration value u l of the Newton iteration method be (m - 1) / 6;

[0165] S114. Construct an error function f(u l ) with u l as the independent variable as shown in formula (34), and substitute the value of u l to calculate the value of f(u l );

[0166]

[0167] Among them, when calculating the value of the error function f(u l ), integral calculation is also required. The six-point Gauss-Legendre integral method is used for integral calculation, and the calculation formula of f(u l ) is as shown in formula (35).

[0168]

[0169] Among them, x k is the integral node, and A k is the integral coefficient, and the specific numerical values are as shown in formula (32);

[0170] S115. Judge f(u lIs the absolute value of () greater than 0.00001? If it holds, jump to step S6; if not, jump to step (8);

[0171] S116. Iterate on u l The iteration formula is as shown in Equation (36);

[0172]

[0173] S117. Substitute the value of the iterated u l into Equation (35) to obtain the value of f(u l ), and then jump to step S5;

[0174] S118. Let u = u l and substitute it into Equation (11) of the i-th segment of the attitude trajectory transition section, so as to obtain the attitude q l = q i (u).

[0175] Step S12. Let i = i + 1; determine whether i is less than N - 2 holds. If it holds, execute step S2; if not, execute step S14;

[0176] Step S13. Obtain the rotation angles of each joint of the six-axis serial industrial robot through inverse kinematics to drive the six-axis serial industrial robot to rotate, so as to realize the attitude trajectory transition of the end of the six-axis serial industrial robot.

[0177] In one embodiment, in order to analyze the influence of the value of the parameter λ on the attitude trajectory transition section, it is necessary to perform visualization processing on the attitude trajectory, establish the mapping relationship between the unit quaternion on the surface of the hypersphere and the points on the unit sphere, so as to project the attitude trajectory described by the unit quaternion on the surface of the hypersphere onto the unit sphere, and more intuitively analyze the transition effect of the attitude trajectory transition section. To perform visualization processing on the attitude trajectory, it is necessary to first give the initial point position p x , p x can be any point on the unit sphere. The point position p x obtained after applying the rotation transformation q to the initial point position p n is the point on the unit sphere corresponding to the projection of the rotation transformation q of the unit quaternion. Let p x be the pure quaternion q x = [0, p x , and there is a mapping relationship as shown in Equation (37) with the quaternion q n , and the imaginary part of the quaternion q n is p n .

[0178] q n = q × q x × q -1(37)

[0179] The attitude trajectory transition planning algorithm is simulated in MATLAB software. Through the visualization processing of the attitude trajectory, the transition effects of the attitude trajectory transition segments constructed when the parameter λ takes different values are more intuitively compared. Figure 3 Figures (a), (b), (c), and (d) in the middle respectively represent the schematic diagrams of attitude trajectory transitions when λ takes 0.05, 0.18, 0.3, and 0.5.

[0180] As can be seen from Figure 3 (a) in the middle, when λ is close to 0, there are still cases of trajectory mutations at the starting and ending points of attitude transitions in the attitude trajectory, and the transition effect is not ideal, which will still lead to a reduction in the operating efficiency of the six-axis serial industrial robot. As Figure 3 shown in (b) in the middle, the attitude trajectory transition segment constructed when λ is 0.18 achieves smooth connection with adjacent attitude trajectories, and the transition effect meets the expected goal, which can effectively improve the operating efficiency of the six-axis serial industrial robot. In Figure 3 (c) in the middle, when λ takes 0.3, it can be seen that there are still cases of trajectory mutations in the part of the attitude trajectory transition segment close to the original attitude trajectory, and the attitude transition effect does not meet the expectation, which will still lead to a reduction in the operating efficiency of the six-axis serial industrial robot. Through Figure 3 (d) in the middle, it can be seen that there is a problem of trajectory winding in the attitude trajectory transition segment when λ = 0.5, which does not meet the attitude transition requirements of the six-axis serial industrial robot and will lead to a reduction in the operating efficiency of the six-axis serial industrial robot. By analyzing and comparing the attitude trajectory transition effects when λ takes 0.05, 0.18, 0.3, and 0.5, finally, λ = 0.18 is selected to obtain appropriate attitude control points and obtain the parameter expression of the attitude trajectory transition segment.

[0181] The present invention uses quaternions to describe the attitude, and uses cubic quaternion quasi-uniform B-spline curves as the attitude trajectory transition segments to achieve smooth transition of the attitude trajectory. It simplifies the method of obtaining the control points of the quaternion spline curve and reduces the implementation difficulty of the algorithm. The calculation of the angular displacement length of the attitude trajectory transition segment uses the composite six-point Gauss-Legendre integration method, and the interpolation calculation uses the method of combining Newton's iteration method with the six-point Gauss-Legendre integration method. Thus, it drives the end of the six-axis serial industrial robot to move continuously along the entire smooth path, obtains a smooth motion trajectory, avoids frequent acceleration and deceleration, improves the operating efficiency of the six-axis serial industrial robot, and meets the technological requirements of production practice.

[0182] The above embodiments are only preferred embodiments cited to fully illustrate the present invention, and the protection scope of the present invention is not limited thereto. Equivalent substitutions or transformations made by those skilled in the art on the basis of the present invention are all within the protection scope of the present invention. The protection scope of the present invention is subject to the claims.

Claims

1. A method for attitude trajectory transition planning at the end of a six-axis serial industrial robot, characterized in that It includes the following steps: S1. Let i = 1, and N be the total number of segments of the given motion path; S2. Perform smooth transition between the spherical linear interpolation trajectories formed by the pose teaching points Q i and Q i+1 and the spherical linear interpolation trajectories formed by the pose teaching points Q i+1 and Q i+2 ; among them, use the cubic quaternion quasi-uniform B-spline curve as the pose trajectory transition segment; the pose teaching points Q i and Q i+1 are respectively the starting pose point and the ending pose point of the i-th pose trajectory; the pose teaching points Q i+1 and Q i+2 are respectively the starting pose point and the ending pose point of the (i + 1)-th pose trajectory; S3. Obtain the starting posture point Q of the posture teaching of the i-th segment of the posture trajectory i and the ending posture point Q i+1 to obtain the angular displacement length θ of the i-th segment of the posture trajectory i and the parameter Q′ i (u); The expression of the parameter Q′ i (u) is as follows: S4. According to the starting pose point Q of the pose teaching of the (i + 1)-th section of the pose trajectory i+1 and the ending pose point Q i+2 obtain the angular displacement length θ of the (i + 1)-th section of the pose trajectory i+1 and the parameter Q′ i+1 (u); The expression of the parameter Q′ i+1 (u) is as follows: S5. Obtain the actual attitude transition radius according to the angular displacement length θ of the attitude trajectory of the i-th segment i , the angular displacement length θ of the attitude trajectory of the (i + 1)-th segment i+1 and the given attitude transition radius ​ S6. According to the angular displacement length θ of the attitude trajectory of the i-th segment i and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition starting point of the attitude trajectory transition segment of the i-th segment, and then according to the parameter Q i '(u) and the derivative calculation method to obtain the attitude, first derivative and second derivative of the i-th segment attitude trajectory at the attitude transition starting point; S7. Obtain the parameter value corresponding to the attitude transition end point of the attitude transition section of the ith attitude trajectory according to the angular displacement length θ of the (i + 1)th attitude trajectory i+1 and the actual attitude transition radius Obtain the parameter value corresponding to the attitude transition end point of the attitude transition section of the ith attitude trajectory, and then obtain the attitude, first derivative, and second derivative of the (i + 1)th attitude trajectory at the attitude transition end point according to the parameter Q′ i+1 (u) and the calculation method of the derivative S8. Establish the parameter expression q i (u) of the attitude trajectory transition section of the i-th segment; S9. Establish a system of equations based on the conditions that need to be satisfied for the curvature continuity in the curve splicing of two attitude curves, and after obtaining the attitudes of the attitude control points, substitute them into the parameter expression q i (u) of the transition section of the i-th attitude trajectory S10. Calculate the angular displacement length of the attitude trajectory transition section of the i-th segment S11. According to the angular displacement length of the i-th attitude trajectory transition segment perform velocity planning to obtain the functional relationship between the angular displacement length and time, so as to obtain the relative angular displacement length of the interpolation point on the attitude trajectory transition segment according to the moment of the interpolation point. Then, inversely solve the parameter value corresponding to the interpolation point according to the relative angular displacement length of the interpolation point on the attitude trajectory transition segment. Finally, substitute the parameter value corresponding to the interpolation point into the parameter expression of the attitude trajectory transition segment to obtain the attitude of the interpolation point; S12. Let i = i + 1; determine whether i < N - 2 holds. If it holds, execute step S2; if not, execute step S13; S13. Obtain the rotation angles of each joint of the six-axis serial industrial robot through inverse kinematics to drive the six-axis serial industrial robot to rotate, thereby realizing the attitude trajectory transition of the end of the six-axis serial industrial robot.

2. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot according to claim 1, characterized in that, The angular displacement length θ of the i-th posture trajectory i ,as follows: θ i = arccos(Q i , Q i+1 ) The angular displacement length θ of the attitude trajectory of the (i + 1)-th segment i+1 , is as follows: θ i+1 = arccos(Q i+1 , Q i+2 ).

3. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot as described in claim 1, characterized in that, Step S5 further includes: Determine Or Whether it holds. If it does not hold, then let If it holds, then further determine θ i > θ i+1 Whether it holds. If it holds, then let If it does not hold, then let 4. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot according to claim 1, characterized in that, Step S6 includes: The angular displacement length θ of the attitude trajectory in the i-th segment i and the actual attitude transition radius Obtain the parameter value corresponding to the attitude transition starting point of the attitude trajectory transition segment in the i-th segment as follows: Then, according to parameter Q i '(u) and the derivative calculation method, obtain the attitude of the i-th segment of the attitude trajectory at the attitude transition starting point First derivative and second derivative 5. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot according to claim 1, characterized in that, Step S7 includes: According to the angular displacement length θ of the attitude trajectory of the (i + 1)-th segment i+1 and the actual attitude transition radius obtain the parameter value corresponding to the attitude transition end point of the attitude trajectory transition segment of the i-th segment as follows: Then, according to the parameter and the derivative calculation method, calculate the attitude of the (i + 1)-th segment attitude trajectory at the attitude transition end point first-order derivative and second-order derivative as follows:

6. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot according to claim 1, characterized in that, In step S8, establish the parameter expression q i (u) of the i-th attitude trajectory transition segment as follows: Among them, B j,3 (u), where j = 0, 1, 2, 3, 4 is obtained as shown in Equation (12). Substitute N in Equations (13)-(18) 0,3 (u), N 1,3 (u), N 2,3 (u), N 3,3 (u), and N 4,3 (u) into Equation (12) to sequentially obtain B 0,3 (u), B 1,3 (u), B 2,3 (u), B 3,3 (u), and B 4,3 (u); and are the attitudes of six attitude control points as the attitude trajectory transition section; According to q i (u) Obtain q i (0) As shown in Equation (19), q i (1) As shown in Equation (20), then according to q i (u) Obtain the first derivative As shown in Equation (21), As shown in Equation (22), and then obtain the second derivative As shown in Equation (23), As shown in Equation (24); 7. The attitude trajectory transition planning method at the end of the six-axis serial industrial robot according to claim 6, wherein Step S9 includes: Establish a system of equations according to the conditions that need to be satisfied for the curvature continuity of the two attitude curves during curve splicing, as follows: Substituting formulas (19)-(24) into the formula, formula (26) can be obtained, as follows: Where both α and β are arbitrary constants greater than zero; Obtain the attitude control points according to formula (26). and After obtaining the attitudes, substitute them into formula (11).

8. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot according to claim 7, characterized in that, The attitude control points obtained according to formula (26) and the attitude includes: Obtain formula (27) according to formula (26), and then substitute the obtained attitude control points and as well as parameters α and β into formula (28) to obtain attitude control points and Finally, substitute the obtained attitude control points and as well as parameters α and β into formula (29) to obtain attitude control points and Since α and β are arbitrary constants greater than zero, there are countless sets of attitude control points that satisfy the conditions; introduce the parameter λ to replace the parameters α and β, and the relationship between the parameter λ and the parameters α and β is shown in formula (30); 9. The attitude trajectory transition planning method for the end of a six-axis serial industrial robot according to claim 8, characterized in that, Step S10 includes: The composite six-point Gauss-Legendre integration method is used to obtain the angular displacement length of the transition section of the attitude trajectory in the i-th section. As shown in Equation (31): where x k is an integration node, A k is an integration coefficient, and the specific values are as shown in Equation (32):

10. The attitude trajectory transition planning method at the end of the six-axis serial industrial robot according to claim 9, wherein, Step S11 includes: S111. The relative angular displacement length θ of the given interpolation point along the trajectory direction on the hypersphere from the attitude transition starting point of the ith attitude trajectory transition segment in the ith attitude trajectory transition segment l , S112. According to the relative angular displacement length θ l and the angular displacement lengths obtained for each interval, find the segment number m of the interval where the interpolation point is located, m = 1, 2, …, 6, so as to obtain the search interval for the Newton iteration method as and correct the relative angular displacement length θ according to Equation (33) l ; S113. Let the initial iteration value u of the Newton iteration method l =(m - 1) / 6; S114. Construct an error function \(f(u)\) with \(u\) as the independent variable as shown in formula (34). l Substitute the value of \(u\) into \(f(u)\). l Calculate the value of \(f(u)\). l l ​​ Among them, when calculating the value of the error function f(u l ), integration also needs to be performed. The six-point Gauss-Legendre integration method is used for integration. The calculation formula for f(u l ) is shown in Equation (35); where x k is the integration node, and A k is the integration coefficient, and the specific values are shown in Equation (32); S115. Determine whether the absolute value of f(u l ) is greater than 0.00001. If it holds, jump to step S6; otherwise, jump to step (8). S116. Iterate u l using the iteration formula shown in Equation (36); S117. Substitute the iterated value of u l into Equation (35) to obtain the value of f(u l ), and then jump to Step S5; S118. Let \(u = u\) l Substitute it into formula (11) of the attitude trajectory transition section of the \(i\)-th segment to obtain the attitude \(q\) of the interpolation point l = \(q\) i (u).

Images