CN110900612B - Pose-synchronous six-axis industrial robot track smoothing method - Google Patents

Abstract

A pose-synchronous track smoothing method for a six-axis industrial robot adopts an arc curve to transition a position track and adopts a quaternion B spline to transition a posture track. The position and the attitude track after transition have high-order continuity, the transition errors of the position and the attitude can be restrained simultaneously, and the position track and the attitude track after transition have parameter synchronism. The method comprises the following two steps: step 1, determining transition parameters according to transition error thresholds, continuity conditions and pose synchronization conditions of positions and postures; and 2, respectively calculating transition tracks of the position and the posture according to the transition parameters. The smoothing method provided by the invention can generate a smooth six-axis robot track with controllable precision, so that the working efficiency and precision of the six-axis robot are improved.

Description

Pose-synchronous six-axis industrial robot track smoothing method

Technical Field

The invention belongs to the technical field of automatic control of industrial robots, and particularly relates to a pose synchronization track smoothing method for a six-axis industrial robot.

Background

The six-axis industrial robot is widely applied to complex applications such as laser cutting, welding, spraying and the like. The complex robot track is mostly composed of a linear track and a circular arc track generated by offline programming software. At each track point, the poses (positions and postures) of the linear tracks do not have the continuity of speed and acceleration, so that the problems of pause, vibration, track errors and the like of the robot in the execution process are caused.

In order to solve the problem of discontinuity of a linear track, a corner transition method is mostly adopted in the existing robot control system. In the existing corner transition research of the position track, a transition track is mostly constructed by adopting an arc, a parabola or a polynomial curve; in the corner transition research of the attitude trajectory, the continuity of the attitude can be improved by the quaternion B spline-based method. However, there are two major problems with the existing studies: firstly, the transition errors of the position track and the attitude track cannot be restrained simultaneously; and secondly, the parameter synchronism of the position and the posture is not realized, so that the geometric shapes of the position and the posture are changed along with the change of the speed.

The Chinese invention patent (application number 201610075346.7) provides a transition track planning method for industrial robot application, which adopts a parabola to realize track transition between a joint space and a Cartesian space; the chinese invention patent (application No. 201710291306.0) proposes a fitting transition method for controlling the size of the transition region by the transition level. However, the above transition methods cannot constrain the transition errors of the position and the posture, and the track shape changes with the change of the speed. Therefore, it is necessary to provide a simple and effective six-axis industrial robot track smoothing method with error constraint and synchronous pose.

Disclosure of Invention

The invention provides a pose synchronization smooth transition method for a six-axis industrial robot, which adopts an arc curve to transition a position track and a quaternion B spline to transition an attitude track.

The technical scheme adopted by the invention is as follows:

a pose-synchronous six-axis industrial robot track smoothing method comprises the following steps:

step 1, determining transition parameters of each position point and each attitude point according to transition error constraint, continuity conditions and pose synchronism, wherein the transition parameters are used for determining a starting point and an end point of a transition track;

step 2, constructing transition tracks of positions and postures according to the transition parameters;

according to the scheme, the step 1 specifically comprises the following steps:

step 1.1, using three-dimensional point P as position point of linear track of six-axis industrial robot_i(x_i,y_i,z_i) Representing the attitude point by a quaternion q_i(q_i,1,q_i,2,q_i,3,q_i,4) Wherein, the serial number i is 1,2, … N, N is the total number of track points (N is more than or equal to 3).

Step 1.2, traversing internal track points P except head and tail end points_i(x_i,y_i,z_i) I 2,3, … N-1, transition error threshold δ according to location point_pCalculating each position point P_i(x_i,y_i,z_i) Maximum position transition parameter of the transition arc of (1):

first, a line segment P is calculated_i-1P_iLength L of_i-1And a line segment P_iP_i+1Length L of_iIf the length L is_i-1Or length L_iToo short (less than the distance threshold), the trajectory segment need not be smooth; otherwise, calculating the line segment P_i-1P_iAnd a line segment P_iP_i+1Angle of (2)

If the included angle is

If the angle is close to 0 degree or 180 degrees, the track section does not need to be smooth, and the section of the position track which does not need to be smooth is marked, and a transition method needing the smooth section is introduced firstly;

then, let r_i,1And r_i,2For two position transition parameters, the start of the transition arc may be denoted as E_i,0＝P_i+r_i,1(P_i-1-P_i) The end point can be represented as E_i,1＝P_i+r_i,2(P_i+1-P_i) And establishing a relation between two position transition parameters according to the symmetry condition of the transition: r is_i,1L_i-1＝r_i,2L_i；

Finally, according to the arc transition error threshold value delta_pAnd calculating the position transition parameter r under the condition that adjacent transition tracks are not intersected_i,1Upper bound of (2):

step 1.3, traversing the track points P except the head and tail end points_iI 2,3, … N-1, transition error threshold δ according to attitude point_oCalculating each attitude point q_i(q_i,1,q_i,2,q_i,3,q_i,4) The maximum attitude transition parameter of the transition quaternion B spline:

first, an attitude point q is calculated_i-1Rotated to attitude point q_iOf a rotary shaft U_i-1And a rotation angle theta_i-1And calculate the attitude point q_iRotated to attitude point q_i+1Of a rotary shaft U_iAnd a rotation angle theta_i(ii) a If the angle of rotation theta_i-1Or angle of rotation theta_iIf the angle is close to 0 degree, the track section does not need to be smooth, and the section of the attitude track which does not need to be smooth is marked, and a transition method needing the smooth section is introduced firstly;

then, let the attitude transition quaternion B-spline have 5 control points, of which the first and second control points f_i,0,f_i,1At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation track of (3), a third control point f_i,2And attitude point q_iSame, fourth and fifth control points f_i,3,f_i,4At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation trajectory of (1), namely:

the distribution of the control points can ensure the G of the transition track and the original linear track²Continuity. Wherein s is_i,1And s_i,2Two attitude transition parameters are adopted, and c is a proportional coefficient synchronously related to the pose; establishing a relation between two position transition parameters according to the symmetry condition of the attitude transition: s_i,1θ_i-1＝s_i,2θ_i；

Finally, according to the attitude transition error threshold value delta_oCalculating attitude transition parameter s under the conditions of symmetric attitude transition tracks and non-intersection of adjacent transition tracks_i,1Upper bound of (2):

step 1.4, calculating a transition parameter t which meets track point errors and is synchronous in pose_i,1,t_i,2And the proportionality coefficient c:

firstly, calculating a proportionality coefficient according to the synchronous conditions of the position track and the attitude track

And then determining the value of a pose synchronization transition parameter according to the mark whether the position and the gesture track can be smooth:

if the position track and the gesture track are marked as smooth, taking the minimum value of the transition parameters of the position and the gesture as the transition parameters of pose synchronization, namely:

if the position track and the posture track are not smooth, the position track and the posture track do not need to be smooth;

if one of the position trajectory or the attitude trajectory can be smoothed, the method of step 1.2 or step 1.3 can be used alone to smooth.

According to the scheme, track points except the head and tail end points are traversedP_i2,3, … N-1, wherein step 2 is performed for a smooth segment, and specifically includes the following steps:

step 2.1, constructing a transition arc of the position track according to the transition parameters, wherein the arc track is expressed as:

and 2.2, constructing a transition curve of the attitude track according to the transition parameters and the proportionality coefficients, wherein the control points are defined in the step 1.3, and the node vector is that U is [0,0,0,0,0.5,1,1,1 ═]，

The quaternion B spline curve is represented as the cumulative sum basis function of the quaternion B splines:

through the technical scheme, compared with the prior art, the invention has the advantages that:

1. the proposed robot pose smoothing method can improve the continuity of the robot track, wherein the position track is G¹Continuous, attitude trajectory is G²The method is continuous, so that pause and vibration in the execution process of the robot can be avoided;

2. the proposed smooth transition method considering error constraint can simultaneously control the transition errors of the position track and the attitude track, and is beneficial to the application of a high-precision robot;

3. the pose synchronization smooth transition method ensures the parameter synchronization of the position track and the pose track, and separates the geometric smoothness from the speed planning, thereby avoiding the change of the robot track along with the change of the speed.

4. The proposed track smoothing method is suitable for independent transition or simultaneous transition of the position track and the posture track, and is further suitable for various application scenes of industrial robots.

Drawings

FIG. 1 is a schematic diagram of a position trajectory transition of a six-axis industrial robot according to an embodiment of the invention;

fig. 2 is a schematic diagram of transition of posture tracks of a six-axis industrial robot according to an embodiment of the invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

The invention relates to a pose synchronization six-axis industrial robot track smoothing method, wherein a position track adopts circular arc transition, a posture track adopts quaternion B spline transition, and transition track calculation mainly comprises two main steps of transition parameter calculation and transition track construction.

Step 1, determining transition parameters of each position point and each attitude point according to transition error constraint, continuity conditions and pose synchronization;

step 1.1, the linear track of the six-axis industrial robot is composed of a series of track points, and each track point contains position point and attitude point information. Three-dimensional point P available for position point_i(x_i,y_i,z_i) Representing, attitude points by quaternion q_i(q_i,1,q_i,2,q_i,3,q_i,4) Wherein, the serial number i is 1,2, … N, N is the total number of track points (N is more than or equal to 3).

Step 1.2, traversing internal track points P except head and tail end points_i(x_i,y_i,z_i) I 2,3, … N-1, transition error threshold δ according to location point_pCalculating each position point P_i(x_i,y_i,z_i) Maximum position transition parameter of the transition arc of (1):

first, calculate the line segment P_i-1P_iLength L of_i-1＝|P_i-1P_iL, line segment P_iP_i+1Length L of_i＝|P_iP_i+1Setting a distance error threshold value epsilon, such as epsilon-1 e-6mm, if the length L_i-1<Epsilon or L_i<Epsilon, the distance is considered to be too short, and the index i is marked as a position without a smooth segment; otherwise, calculating the line segment P_i-1P_iAnd a line segment P_iP_i+1Angle of (2)

If the included angle is

If the angle is close to 0 degree or 180 degrees, marking the index i as a position without a smooth segment; the processing in step 1.4 is performed without a smooth segment, and a transition method requiring a smooth segment is described below;

let r_i,1And r_i,2Two position transition parameters are used to determine the start and end positions of the arc representing the transition. As shown in FIG. 1, the start of the arc may be denoted as E_i,0＝P_i+r_i,1(P_i-1-P_i) The end point can be represented as E_i,1＝P_i+r_i,2(P_i+1-P_i). Requiring bilateral symmetry of the transition arc, i.e. r_i,1L_i-1＝r_i,2L_iThen the maximum error point of the transition trajectory and the linear trajectory is located at the point Q shown in FIG. 1_iTherefore, the error threshold value delta can be determined according to the arc transition_pDeriving a position transition parameter r_i,1Upper bound of (2)

In addition, in order to prevent the intersection of two adjacent transition tracks, a transition parameter r is required_i,1,r_i,2Is not more than

The position transition parameter r is obtained by integrating the above_i,1Upper bound of (2):

step 1.3, traversing the track points P except the head and tail end points_iI 2,3, … N-1, transition error threshold δ according to attitude point_oCalculate each perAn attitude point q_i(q_i,1,q_i,2,q_i,3,q_i,4) The maximum attitude transition parameter of the transition quaternion B spline:

first, calculate the attitude point q_i-1Rotated to attitude point q_iOf a rotary shaft U_i-1And a rotation angle theta_i-1And calculate the attitude point q_iRotated to attitude point q_i+1Of a rotary shaft U_iAnd a rotation angle theta_i(ii) a If the angle of rotation theta_i-1Or angle of rotation theta_iIf the angle is close to 0 degree, the posture of the track section does not need to be smooth, and the track section which does not need to be smooth is processed in step 1.4, and a transition method needing to be smooth is introduced below;

secondly, an attitude transition quaternion B spline is set to have 5 control points f_i,0～f_i,4Wherein the first and second control points f_i,o,f_i,1At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation track of (3), a third control point f_i,2And attitude point q_iSame, fourth and fifth control points f_i,3,f_i,4At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation trajectory, five control points can be calculated by the following formula:

wherein s is_i,1And s_i,2Firstly, establishing a relation between two position transition parameters by adopting symmetrical attitude tracks: s_i,1θ_i-1＝s_i,2θ_i；

The same principle can prove that the error of the transition track is maximum in the midpoint error of the parameters according to the attitude transition error threshold value delta_oThe attitude transition parameter s can be reversely deduced_i,1Upper bound of (2):

similarly, two adjacent attitude transition tracks cannot intersect to finally obtain an attitude transition parameter s_i,1Upper bound of (2):

step 1.4, calculating a transition parameter t which meets track point errors and is synchronous in pose_i,1,t_i,2And the proportionality coefficient c:

firstly, a proportionality coefficient c is deduced according to the synchronous conditions of the position track and the attitude track, and the condition is that the position track and the attitude track meet the G with the same parameters at the starting point and the ending point of transition¹Continuity, with starting point (E)_i,0,f_i,0) The continuity of (c) is for example: as shown in FIG. 1, L_i(u) is P_i-1E_i,0Linear locus of (A) between, C_i(u) is a position transition trajectory; as shown in FIG. 2, S_i(u) is q_{i- 1}f_i,0Spherical linear locus of (q) between_iAnd (u) is a posture transition track. The position and attitude respectively G can be proved according to the setting of the control point¹Continuously, if the position and attitude have G of the same parameters¹Continuity, then satisfy

From which the scaling factor can be derived

The scaling factor is also applicable to the end point of the transition trajectory.

And then determining the value of a pose synchronization transition parameter according to the mark whether the position track and the gesture track can be smooth:

if the position track and the gesture track are marked as smooth, taking the minimum value of the position transition parameter and the gesture transition parameter as a pose synchronization transition parameter, namely:

if the position track and the posture track are not smooth, the position track and the posture track do not need to be smooth, namely the step 2 does not need to be carried out; the problems are the same as above

If one of the position trajectory or the attitude trajectory can be smoothed, the transition parameters calculated in step 1.2 or step 1.3 can be used alone for smoothing.

Step 2, traversing the track points P except the head and tail end points _i2,3, … N-1, for the smooth segment, the method specifically includes the following steps:

step 2.1, for a track section with a smooth position, constructing a transition arc of the position track according to the transition parameters, as follows:

step 2.2, for the trajectory segment with smooth attitude, constructing a transition curve of the attitude trajectory according to the transition parameters and the scaling coefficients, where the control points are calculated as defined in step 1.3, the node vector is [0,0,0,0,0.5,1,1, 1], and the quaternion B spline curve is expressed as:

wherein

The basis functions, which are quaternion B splines, are defined in two node intervals as follows:

Claims (5)

1. a pose-synchronous track smoothing method for a six-axis industrial robot is characterized by comprising the following steps: the method comprises the following steps:

step 1, determining transition parameters of each position point and each attitude point according to transition error constraint, continuity conditions and pose synchronism, wherein the transition parameters are used for determining a starting point and an end point of a transition track;

step 2, constructing transition tracks of positions and postures according to the transition parameters;

wherein, the step 1 specifically comprises the following steps:

step 1.1, using three-dimensional point P as position point of linear track of six-axis industrial robot_i(x_i，y_i，z_i) Representing the attitude point by a quaternion q_i(q_i，1，q_i，2，q_i，3，q_i，4) The sequence number i is 1,2,. N, N is the total number of track points (N is more than or equal to 3);

step 1.2, traversing internal track points P except head and tail end points_i(x_i，y_i，z_i) N-1, transition error threshold δ according to location point_pCalculating each position point P_i(x_i，y_i，z_i) The maximum position transition parameter of the transition arc;

step 1.3, traversing the track points P except the head and tail end points_iN-1, transition error threshold δ according to attitude point_oCalculating each attitude point q_i(q_i，1，q_i，2，q_i，3，q_i，4) The maximum attitude transition parameter of the attitude transition quaternion B spline; the attitude transition quaternion B-spline has 5 control points, a first and a second control point f_i，0，f_i，1At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation track of (3), a third control point f_i，2And attitude point q_iSame, fourth and fifth control points f_i，3，f_i，4At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation trajectory;

step 1.4, calculating a transition parameter t which meets track point errors and is synchronous in pose_i，1，t_i，2And a proportionality coefficient c;

the step 2 specifically comprises the following steps: traversing track points P except head and tail end points_iN-1, wherein step 2 is performed for a smoothable segment, and specifically includes the following steps:

step 2.1, constructing a transition arc of the position track according to the transition parameters, wherein the arc track is expressed as:

wherein the content of the first and second substances,

is a line segment P_i-1P_iAnd a line segment P_iP_i+1The included angle of (A);

and 2.2, constructing a transition curve of the attitude track according to the transition parameters and the proportionality coefficients, wherein the control points are defined in the step 1.3, and the node vector is that U is [0,0,0,0,0.5,1,1,1 ═]，

The quaternion B spline curve is represented as the cumulative sum basis function of the quaternion B splines:

2. the pose-synchronized six-axis industrial robot trajectory smoothing method according to claim 1, characterized in that: the specific algorithm of the step 1.2 is as follows: first, a line segment P is calculated_i-1P_iLength L of_i-1And a line segment P_iP_i+1Length L of_iIf the length L is_i-1Or length L_iToo short (less than the distance threshold), the trajectory segment need not be smooth; otherwise, calculating the line segment P_i-1P_iAnd a line segment P_iP_i+1Angle of (2)

If the included angle is

Approach toIf the angle is 0 degree or 180 degrees, the track section does not need to be smooth, and the section of the position track which does not need to be smooth is marked, and a transition method needing the smooth section is introduced firstly;

then, let r_i，1And r_i，2For two position transition parameters, the start of the transition arc may be denoted as E_i，0＝P_i+r_i，1(P_i-1-P_i) The end point can be represented as E_i，1＝P_i+r_i，2(P_i+1-P_i) And establishing a relation between two position transition parameters according to the symmetry condition of the transition: r is_i，1L_i-1＝r_i，2L_i；

Finally, according to the arc transition error threshold value delta_pAnd calculating the position transition parameter r under the condition that adjacent transition tracks are not intersected_i，1Upper bound of (2):

3. the pose-synchronized six-axis industrial robot trajectory smoothing method according to claim 1 or 2, characterized in that: the specific algorithm of the step 1.3 is as follows: first, an attitude point q is calculated_i-1Rotated to attitude point q_iOf a rotary shaft U_i-1And a rotation angle theta_i-1And calculate the attitude point q_iRotated to attitude point q_i-1Of a rotary shaft U_iAnd a rotation angle theta_i(ii) a If the angle of rotation theta_i-1Or angle of rotation theta_iIf the angle is close to 0 degree, the track section does not need to be smooth, and the section of the attitude track which does not need to be smooth is marked, and a transition method needing the smooth section is introduced firstly;

then, let the attitude transition quaternion B-spline have 5 control points, of which control point f_i，0，f_i，1At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation trajectory, the control point f_i，2And attitude point q_iSame, control point f_i，3，f_i，4At the attitude point q_i-1And attitude point q_iOn the spherical linear interpolation track of the optical system,namely:

the distribution of the control points can ensure the G of the transition track and the original linear track²Continuity of, wherein s_i，1And s_i，2Two attitude transition parameters are adopted, and c is a proportional coefficient synchronously related to the pose; establishing a relation between two position transition parameters according to the symmetry condition of the attitude transition:

s_i，1θ_i-1＝s_i，2θ_i；

finally, according to the attitude transition error threshold value delta_oCalculating attitude transition parameter s under the conditions of symmetric attitude transition tracks and non-intersection of adjacent transition tracks_i，1Upper bound of (2):

4. the pose-synchronized six-axis industrial robot trajectory smoothing method according to claim 1, characterized in that: the specific algorithm of the step 1.4 is as follows: firstly, calculating a proportionality coefficient according to the synchronous condition of the position track and the attitude track

Then, determining the value of a pose synchronization transition parameter according to the mark whether the position and the gesture track can be smooth: if the position track and the gesture track are marked as smooth, taking the minimum value of the transition parameters of the position and the gesture as the transition parameters of pose synchronization, namely: t is t_i，1＝min(r_i，1，s_i，1)，

If the position track and the posture track are not smooth, the position track and the posture track do not need to be smooth; if one of the position track or the posture track can be smooth, the method of the step 1.2 or the step 1.3 is independently adopted for smoothing;

wherein r is_i，1Is a position transition parameter; length L_i-1Is a line segment P_i-1P_iLength L of_iIs a line segment P_iP_i-1Length of (d); s_i，1Is an attitude transition parameter.

5. The pose-synchronized six-axis industrial robot trajectory smoothing method according to claim 2, characterized in that: the specific algorithm of the step 1.4 is as follows: firstly, calculating a proportionality coefficient according to the synchronous condition of the position track and the attitude track

Then, determining the value of a pose synchronization transition parameter according to the mark whether the position and the gesture track can be smooth: if the position track and the gesture track are marked as smooth, taking the minimum value of the transition parameters of the position and the gesture as the transition parameters of pose synchronization, namely: t is t_i，1＝min(r_i，1，s_i，1)，

If the position track and the posture track are not smooth, the position track and the posture track do not need to be smooth; if one of the position track or the posture track can be smooth, the method of the step 1.2 or the step 1.3 is independently adopted for smoothing;

wherein s is_i，1Is an attitude transition parameter.

Images