CN105500354A - Transitional track planning method applied by industrial robot - Google Patents

Abstract

The invention discloses a transitional tracking planning method applied by an industrial robot, which can realize the transition between a joint space track and a cartesian space track, and the transition between two tracks of the cartesian space, the transitional track between different movement tracks are planned under the cartesian space, and has an intuitive shape; by adopting the algorithm that two parabolas are fused into one transitional curve, the smoothness of the track, speed and acceleration can be ensured, and the curve shape is controllable; the transitional track is formed by six independent curves, and the transition can be realized on the track with only posture change without position change; from the engineering application angle, the path velocity of transitional track boundary is restrained by utilizing the included angle between the tracks and the system allowable chord error, the boundary posture rotation speed is restrained by the similar mode, so that the large impact on a mechanical system caused by overhigh engagement speed can be prevented.

Description

Transition track planning method applied to industrial robot

Technical Field

The invention relates to a transition track planning method applied to an industrial robot.

Background

Industrial robots have been widely used in many fields, and play an increasingly prominent role in the development of modern industrial automation and intelligence. The planning of a motion trajectory is a basic task of a robot control system, and the motion trajectory of an industrial robot generally has a straight line and a circular arc in a cartesian space and a point-to-point motion trajectory in a joint space. In practical applications, a plurality of tracks are connected in sequence to complete a processing task, however, corners exist at the joint of two adjacent tracks, and many applications require smooth tracks without corners, that is, end points smoothly transition from one track to the next track. The switching trajectory between two trajectories is called a transition trajectory.

At present, partial research is conducted on the trajectory planning of the transition section, and the transition trajectory is often constructed by using circular arcs and polynomial curves. The arc transition can ensure smooth track and uniform speed, and has wide application, such as: the Chinese patent of the invention, a method for planning a transition track of a welding robot (application number 201110000264.3), aims at the welding robot, adopts an arc to connect a straight-line section welding line and an arc section welding line at a transition section welding line, but the arc transition has acceleration jump at the connection part of the transition section and the track section, influences the smoothness of the track, possibly causes mechanical vibration, and the arc transition can not realize the track transition of two adjacent sections with only posture change, so the method is not suitable for other application occasions with the requirement; the polynomial curve transition comprises a spline function, a Hermite function, a fifth-order polynomial and the like, the smoothness of acceleration can be ensured by means of the characteristic of high-order continuity, vibration is reduced, and high-speed motion is facilitated, wherein the fifth-order polynomial transition algorithm is researched more, for example: in the literature, "mechanical arm cartesian space trajectory planning research [ J ]," mechanical design and manufacturing, "2013 (3): 49-52") transition curves are constructed by using quintic polynomials for 6 degrees of freedom of alignment posture, so that not only can the smoothness of the trajectory, speed and acceleration be ensured, but also the transition among the trajectories with only posture change can be completed, but the geometric shape of the quintic curves is difficult to control, and the engineering constraint conditions are lacked. The research is based on the track transition between two tracks in the Cartesian space, the research on the transition from the joint space track to the Cartesian space track is less, and the nonstop movement between different tracks is necessary for improving the working efficiency.

Disclosure of Invention

The invention aims to overcome the defects in the prior art, provides a transition track planning method applied to an industrial robot, constructs a transition curve which can ensure smooth tracks, speeds and accelerations, realizes the transition between a joint space track and a Cartesian space track and the transition between two Cartesian spaces tracks, can realize the transition for the track with only posture change, and can control the shape of the transition track. Meanwhile, from the aspect of engineering application, the boundary path speed and the posture rotation speed of the transition section are restrained.

The basic technical scheme of the invention comprises the following steps:

step 1: importing motion parameters required by transition track planning into robot transition track planning module

The pose of the end point of the robot is described by a position vector (x, y, z) and an RPY attitude vector (α, gamma) together, and is combined into a composite vector (x, y, z, α, gamma) with 6 degrees of freedom, and the motion parameters related to the planning of the transition track comprise a first track starting point pose P₀End position pose P₁Second track end position P₂Transition parameter percentage a, and engineering constraint conditions comprise: maximum speed V of system_maxMaximum acceleration of the system A_maxMaximum allowable bow height error of system E_max。

Step 2: determining a trajectory P₀P₁And the track P₁P₂Pose of starting point and end point of transition track between

When the track P is₀P₁Transition starting point P when it is a straight-line locus of Cartesian space_sTo the inflection point P₁Is the track P₀P₁Multiplying half of the length of the straight line by the percentage a of the transition parameter; when the track P is₀P₁When the arc locus of Cartesian space is defined, the transition starting point P_sTo the inflection point P₁Arc length of (1) is the locus P₀P₁Half the arc length times the transition parameter percentage a; setting a transition starting point P_sTo the inflection point P₁Corresponding track P of RPY attitude vector change₀P₁Is multiplied by the transition parameter percentage a.

When the track P is₀P₁When the joint space trajectory is obtained, a transition starting point P is set_sTo the inflection point P₁Each joint position change corresponding trajectory P₀P₁Multiplying half of the position change of each joint by the percentage a of the transition parameter, and further calculating the starting point P of the transition according to the forward kinematics_sThe pose of (1).

When the track P is₁P₂Point of inflection P in a Cartesian space₁To the transition end point P_eOfThe length of the line being the locus P₁P₂Multiplying half of the length of the straight line by the percentage a of the transition parameter; when the track P is₁P₂Point of inflection P in a Cartesian space₁To the transition end point P_eArc length of (1) is the locus P₁P₂Half the arc length times the transition parameter percentage a; set inflection point P₁To the transition end point P_eCorresponding track P of RPY attitude vector change₁P₂Is multiplied by the transition parameter percentage a.

When the track P is₁P₂When the joint space trajectory is obtained, an inflection point P is set₁To the transition end point P_eEach joint position change corresponding trajectory P₁P₂Multiplying half of the change of the position of each joint by the percentage a of the transition parameter, and further calculating the transition end point P according to the positive kinematics_eThe pose of (1).

And step 3: determining boundary velocities of transition trajectories

Inputting a track P by an acceleration and deceleration track planning algorithm of an external software module robot₀P₁At the starting point P of the transition_sVelocity, trajectory P of₁P₂At the transition end point P_eThe speed is determined to be equal to the boundary speed of the start point and the end point of the transition track for simplifying calculation.

Path velocity V of terminal point corresponding to Cartesian space trajectory_pathTerminal point attitude rotation speed V_oriAnd the space track of the joint corresponds to the speed of each joint, and under the condition, the speed of 6 degrees of freedom of the pose of the tail end point of the robot can be directly calculated according to the speed of each joint by means of the Jacobian matrix.

And 4, step 4: bounding velocity for constraining transition trajectory using bow height error

From an engineering application perspective, when the included angle between the tangential velocity vectors of adjacent tracks is large, the too large track joining speed can generate large impact on a mechanical system, so that the track joining speed needs to be restrained. Engineering of the inventionThe upper bow height error parameter restrains the track connection speed to ensure the boundary speed V of the transition track_pathAnd V_oriThe allowable track connection speed is not exceeded, and the specific method is as follows:

assumed trajectory P₀P₁And the track P₁P₂A small imaginary arc is arranged between the two, the curvature radius r is adjusted through the bow height error, and then the trajectory joining speed is restrained by utilizing r, so that the restraint of the transition trajectory boundary speed V is achieved_pathAnd V_oriThe purpose of (1). The imaginary arc is only a constraint condition for acquiring the speed and does not influence the actual track. Because the joint space track is not visual, the straight line connecting line between the transition point and the inflection point is used for replacing the original track to calculate the included angle at the inflection point.

Track P₀P₁And the track P₁P₂At the inflection point P₁The included angle of the tangential velocity vector is theta, corresponding to the path velocity V_pathThe constraint formula of (2) is as follows:

$V_{path}\≤\left\{\begin{array}{c}{V}_{\max }\\ \sqrt{A_{\max }r}\end{array}\right.---\left(1\right)$

wherein, V_maxAt maximum speed of the system, A_maxThe maximum acceleration of the system is the maximum acceleration,E_maxthe allowable bow height error parameter of the system,

$r=\frac{E_{max}cos\left(0.5\mathrm{\θ}\right)}{1-cos\left(0.5\mathrm{\θ}\right)}.$

defining a dimension conversion coefficient lambda from angle to millimeter, and equating attitude rotation speed to a speed vector lambda V on a length_oriReference path velocity V_pathFor λ V_oriConstraining to achieve a rotation speed V of the attitude_oriFor the purpose of the constraint of (1).

Track P₀P₁And the track P₁P₂At the inflection point P₁The included angle of the tangential attitude rotation speed vector is theta_oriTo the attitude rotation speed V_oriThe constraint formula of (2) is as follows:

$V_{ori}\≤\left\{\begin{array}{c}\frac{1}{\mathrm{\λ}}{V}_{max}\\ \frac{1}{\mathrm{\λ}}\sqrt{A_{max}{r}_{ori}}\end{array}\right.---\left(2\right)$

wherein, $r_{ori}=\frac{E_{max}cos\left(0.5{\mathrm{\θ}}_{ori}\right)}{1-cos\left(0.5{\mathrm{\θ}}_{ori}\right)};$

and 5: velocity V of Cartesian space trajectory at boundary point of transition trajectory_pathAnd V_oriDecomposed to pose 6 degrees of freedom

If the track P₀P₁Is a Cartesian space trajectory and starts a transition at a point P_sVelocity V of_pathAnd V_oriDecomposing to pose 6 degrees of freedom; if the track P₁P₂Is a Cartesian space trajectory and will transition to the end point P_eVelocity V of_pathAnd V_oriDecomposing to pose 6 degrees of freedom. For the joint space trajectory, the transition starting point P has already been determined in step 3_sAnd end point of transition P_eAnd the speed of the pose in 6 degrees of freedom.

The cartesian space trajectory is divided into two types of straight and circular trajectories, and the velocity V is explained below_pathAnd V_oriThe decomposition method of (2):

for the straight-line track type, V can be directly converted according to the unit vector of the straight-line track in the Cartesian coordinate system_pathDecomposing the material into various shafts;

for circular arc trajectory types, the document industrial robot cartesian space trajectory planning [ J]141- (5) 141-143) to obtain the central point O of the circular arc track based on the calculation step of the spatial circular arc interpolation scheme of the local coordinate system_arcThe position coordinates of (3) and a unit direction vector Z 'of the Z axis of the local coordinate system, which is composed of the vector Z' and the central point O_arcThe unit direction vector O' pointed to a certain point on the circular arc is cross-multiplied to calculate the unit velocity tangential vector of the point, namely V_pathAre split onto the respective axes.

Then equivalently processing the posture track in a linear position processing mode to convert the posture rotation speed V into a linear position_oriDecomposed into 3 degrees of freedom of attitude axis.

Step 6: calculating the starting point P of the transition track_sAnd end point P_eAcceleration in pose 6 degrees of freedom

The acceleration is calculated from the velocity difference, step 5 has determined the boundary velocity V_pathAnd V_oriDecompose to 6 degrees of freedom, assuming a transition origin P_sAt the instant of time, the constant speed and the transition end point P_eThe instant of the transition is the uniform speed, and the starting point P of the transition track can be calculated_sAnd end point P_eAcceleration in 6 degrees of freedom of pose.

And 7: calculating transition track running time T

Assuming the execution time of the transition track and the starting point P of the transition_sMove to the inflection point P at uniform speed₁Then moves at uniform speed to the transition end point P_eThe straight line segments are equal in time, and the running time T of the position transition is calculated according to the straight line segments₁And the runtime T of the attitude transition₂Then T chooses a longer time:

$T=\left\{\begin{array}{cc}{T}_1& ,T_1\&GreaterEqual;T_2\\ T_2& ,T_1<T_2\end{array}\right.---\left(3\right)$

if the position or the posture does not transit, the corresponding operation time is set to be zero.

And 8: respectively constructing a transition curve equation P (sigma) for 6 degrees of freedom of the pose of the tail end point of the transition track of the robot, and adopting the superposition and fusion of two parabolic motions as the motion of the transition track, wherein a matrix equation is as follows:

P(σ)＝P₁(σ)+η(σ)[P₂(σ)-P₁(σ)](4)

where σ is a parameterized variable of the transition time t; p₁(σ) is the point corresponding to the starting point of the transition P_sThe connected parabola is a quadratic function of the variable sigma and tangent to the first segment of the track at the transition starting point P_s；P₂(σ) is the end point of transition P_eThe connected parabola is a quadratic function of the variable sigma and tangent to the second segment of the track at the transition end point P_eη (σ) is the guarantee curve P₁(sigma) transition to curve P₂The transition function of (sigma) can be designed to η (sigma) is 6 sigma⁵-15σ⁴+10σ³To ensure that the P (σ) boundary is continuous in trajectory, velocity, acceleration.

Starting point P of transition track_sAnd end point P_eThe boundary conditions of (1): and (4) substituting the pose, the speed and the acceleration into a matrix equation of a formula (4), so that a curve equation of 6 degrees of freedom of the pose of the transition track can be determined.

The designed transition track ensures the smoothness of the track, the speed and the acceleration, the transition can be realized for the track with only posture change, the transition curve is formed by fusing two parabolas, and the curve shape is controllable.

According to the technical scheme, the following beneficial effects can be realized:

1) can realize the transition between joint space track and the cartesian space track to and the transition between two tracks in cartesian space, cartesian space track includes: straight lines and circular arcs; the transition track is planned in a Cartesian space in a unified mode, the curve is visual, the boundary condition solving only relates to forward kinematics, and the multi-solution problem of the reverse kinematics is avoided.

2) The transition curve is formed by fusing two parabolas, so that the smoothness of the track, the speed and the acceleration is ensured, and the curve shape is controllable; the transition track is formed by 6 independent curves, and the transition can be realized for the track without position change and only with posture change.

3) The boundary path speed and the attitude rotation speed of the transition track are restrained from the aspect of engineering application, and the phenomenon that when the included angle of adjacent tracks is large, the mechanical system is possibly greatly impacted by an overlarge connecting speed is prevented.

The key points are as follows:

1) the joint of the Cartesian space trajectory and the joint space trajectory is processed into a transition trajectory which is intuitively planned in the Cartesian space in a unified mode;

2) an algorithm that two parabolas are fused into a transition curve is adopted to ensure the smoothness of the track, the speed and the acceleration and the controllable curve shape;

3) and from the engineering application angle, the boundary path speed is restrained by utilizing the included angle and the bow height error between the tracks, and the boundary attitude rotation speed is restrained in a similar mode.

Drawings

FIG. 1 is a flow chart of a transition trajectory planning method according to the present invention.

FIG. 2 is a schematic diagram of straight and circular trajectories taught by the teach pendant.

FIG. 3 is a schematic diagram of an insertion of an imaginary arc in the bow height error constrained boundary velocity method.

Fig. 4 is a path diagram of a robot end point where a straight line transitions to a circular arc.

Fig. 5 is a graph of the position of the end point of the robot on the transition trajectory.

Detailed Description

The system consists of a SCARA industrial robot, a control cabinet and a teaching box, wherein the large arm length of the SCARA robot is 300mm, the small arm length of the SCARA robot is 300mm, the up-and-down stroke of a joint 3 is 200mm, the SCARA robot only has a gamma-axis attitude, α -axis attitude and β -axis attitude are reserved for unified description, but the numerical values are zero, the calculation process is not influenced, and the teaching box is used for teaching a linear track P₀P₁Then demonstrating the circular arc track P₁P₂，P_MIs a point on the circular arc locus, as shown in FIG. 2, taught as P₀Pose (-200, -300,200,0,0, -45), taught P₁Pose (0, -300,200,0,0,45), taught P₂Pose (100, -400,200,0,0,45), taught P_MPose is (50.2429, -386.7423,200,0,0,45), where position units are millimeters and pose units are degrees.

The system parameters are set as follows: maximum speed V of system_max500mm/s, maximum acceleration of the system A_max＝2000mm/s²Maximum allowable bow height error of system E_max10mm, 2 mm/degree for the conversion factor λ of the angle to the mm, 100% for the percentage of the transition parameter a.

Determining a transition starting point P of a transition trajectory_sPose of and transition end point P_eThe pose of (1). Track P₀P₁Is a straight line of Cartesian spaceLine trajectory, according to step 2, transition starting point P_sTo the inflection point P₁Is the track P₀P₁Half the length of the straight line times the percentage of the transition parameter a, the transition starting point P_sTo the inflection point P₁Gamma-axis attitude change corresponding trajectory P₀P₁Is multiplied by the percentage a of the transition parameter to calculate the starting point P of the transition_sThe pose of (1) is (-100, -300,200,0,0, 0). Track P₁P₂Is a circular arc trajectory of Cartesian space, according to step 2, with an inflection point P₁To the transition end point P_eArc length of (1) is the locus P₁P₂Half of the arc length times the percentage of the transition parameter a, the inflection point P₁To the transition end point P_eGamma-axis attitude change corresponding trajectory P₁P₂The transition endpoint P can be calculated by multiplying half of the gamma-axis attitude change by the percent a of the transition parameter_eThe pose of (29.2893, -370.7107,200,0,0, 45).

Inputting the boundary speed V of the transition track by an acceleration and deceleration track planning algorithm of the external software module robot according to the convention of the step 3_path500mm/s and V_ori＝150°/s。

From an engineering application perspective, when the included angle between the tangential velocity vectors of adjacent tracks is large, the too large track joining speed can generate large impact on a mechanical system, so that the track joining speed needs to be restrained. The invention utilizes the height error parameter of the engineering to restrict the track joining speed and ensure the boundary speed V of the transition track_pathAnd V_oriNot exceeding the allowed trace joining speed. According to step 4, θ is a straight line P₀P₁At the inflection point P₁At tangential velocity vector and arc P₁P₂At the inflection point P₁The included angle of the tangential velocity vector is shown in fig. 3, theta is 90 degrees, and the included angle brings the bow height error E_maxR is 24.1421mm, and the maximum bow height error limit speed is calculated from equation (1) to be 219.7368mm/s, V_pathBeyond this limit, a constraint V is therefore required_path219.7368 mm/s. Reference path according to step 4Velocity V_pathLimit on attitude rotation speed V_oriIs limited to a trajectory P₀P₁Change in gamma-axis attitude of (1), trajectory P₁P₂Is unchanged in the gamma-axis attitude, i.e. trajectory P₀P₁And the track P₁P₂At the inflection point P₁The included angle of the tangential attitude rotation speed vector is theta_ori0 °, theoretically to V_oriIs infinite, so V_ori＝150°/s。

Velocity V of Cartesian space trajectory at boundary point of transition trajectory_pathAnd V_oriDecomposing to pose 6 degrees of freedom. According to step 5, trace P₀P₁Is a linear trajectory type, unit vector in a Cartesian coordinate systemV_pathThe velocity resolved onto the x, y, z axes is denoted asThe unit is mm/s. Track P₁P₂Is of the circular arc trajectory type and is planned by the literature industrial robot Cartesian space trajectory [ J]141- (5) 141-143) to obtain the central point O of the circular arc track based on the calculation step of the spatial circular arc interpolation scheme of the local coordinate system_arcHas a position coordinate of (100, -300,200), and a unit direction vector Z' of the Z axis of the local coordinate system is (0,0,1), and a central point O_arcPoint on the circular arc P_eUnit direction vector of pointP can be calculated by cross-multiplying the vector Z' with the vector O_eUnit velocity tangential vector at pointCan make V_pathThe velocity resolved onto the x, y, z axes is denoted as $V_{\mathrm{path}}\&CenterDot;K_{p_e}=(155.3774,-\mathrm{155.3774,0}),$ The unit is mm/s.

SCARA robot has only gamma-axis attitude, so V_oriIs the rotation speed of the gamma axis, and the transition starting point P_sThe rotational speed of the posture at which the rotational speed is resolved to α, β, and the gamma axis is expressed as (0, 150) in the unit of °/s, and the trajectory P₁P₂Has no change in the posture of the gamma axis and has a transition end point P_eThe rotational speed of the posture at (g) is decomposed into α, β, and the rotational speed on the γ axis is represented as (0,0,0), and the unit is °/s.

Calculating the starting point P of the transition track_sAnd end point P_eAcceleration in 6 degrees of freedom of pose. According to step 6, assume a transition starting point P_sAt the instant of time is at constant speed, track P₀P₁Is a straight line track, and the unit velocity tangent vectors at any point on the straight line are allThe acceleration is obtained by calculating the speed difference, and the transition starting point P can be calculated_sThe acceleration in the x, y, z axes is (0,0,0) in mm/s². Track P₁P₂Is of the circular arc trajectory type, with centripetal acceleration, according to step 6, assuming a transition end point P_eThe point P is calculated at constant speed at the moment of the point P, through the speed difference_eThe acceleration in the x, y, z axes is (341.0459,341.7961,0) in mm/s²。

The SCARA robot has only the attitude of the gamma axis and therefore only the rotational acceleration of the gamma axis, assuming a transition starting point P according to step 6_sAt and transition end point P_eThe instant of the transition is at a constant speed, thus obtaining a transition starting point P_sAt and transition end point P_ePosture of standingThe rotational acceleration of the attitude rotation speed resolved to α, β and gamma axis is represented as (0,0,0) in the unit of °/s²。

And calculating the transition track running time T. According to step 7, the execution time of the transition track and the starting point P of the transition are assumed_sMove to the inflection point P at uniform speed₁Then moves at uniform speed to the transition end point P_eThe straight line segment time is the same, and the running time T of the position transition is calculated according to the setting₁0.8034s and the operating time T of the attitude transition₂T takes a longer time of 0.8034s, 0.3 s.

Respectively constructing a transition curve equation P (sigma) for 6 degrees of freedom of the pose of the tail end point of the transition track of the robot by using a matrix equation (4), wherein the sigma is a parameterized variable of time T, T is more than or equal to 0 and less than or equal to T, sigma is more than or equal to 0 and less than or equal to 1, and P₁(σ) is the point corresponding to the starting point of the transition P_sThe connected parabolas being quadratic functions with respect to the variable σ, let P₁(σ)＝A₁σ²+B₁σ+C₁，P₂(σ) is the end point of transition P_eThe connected parabolas being quadratic functions with respect to the variable σ, let P₂(σ)＝A₂σ²+B₂σ+C₂According to step 8, η (σ) is 6 σ⁵-15σ⁴+10σ³Starting point P of the transition track_sEnd point P_eThe boundary conditions of (1): pose, speed and acceleration are substituted into a matrix equation of a formula (4), and a coefficient matrix A is calculated₁＝(0,0,0,0,0,0)、B₁＝(132.4025,0,0,0,0,90.3826)、C₁＝(-100,-300,200,0,0,0)、A₂＝(11.0064,11.0307,0,0,0,0)、B₂＝(77.113,-110.1687,0,0,0,0)、C₂After the coefficient matrix is determined (-58.8302, -271.5726,200,0,0,45), the curve equation of the pose 6 degrees of freedom of the transition trajectory is determined. FIG. 4 depicts a path diagram of robot end points for a straight line transition to a circular arc using MATLAB software, where point P_sTo point P_eThe solid line between the two is the transition track, and fig. 5 is a curve of the pose of the transition track along with the change of time during the operation.

Claims (1)

1. A transition trajectory planning method for industrial robot application comprises the following steps:

step 1: importing motion parameters required by transition track planning into robot transition track planning module

The pose of the end point of the robot is described by a position vector (x, y, z) and an RPY attitude vector (α, gamma) together, and is combined into a composite vector (x, y, z, α, gamma) with 6 degrees of freedom, and the motion parameters required by the planning of the transition track comprise a first track starting point pose P₀End position pose P₁Second track end position P₂Percent of transition parameter a; the engineering constraints include: maximum speed V of system_maxMaximum acceleration of the system A_maxMaximum allowable bow height error of system E_max；

Step 2: determining a trajectory P₀P₁And the track P₁P₂Pose of starting point and end point of transition track between

When the track P is₀P₁Transition starting point P when it is a straight-line locus of Cartesian space_sTo the inflection point P₁Is the track P₀P₁Multiplying half of the length of the straight line by the percentage a of the transition parameter; when the track P is₀P₁When the arc locus of Cartesian space is defined, the transition starting point P_sTo the inflection point P₁Arc length of (1) is the locus P₀P₁Half the arc length times the transition parameter percentage a; setting a transition starting point P_sTo the inflection point P₁Corresponding track P of RPY attitude vector change₀P₁Multiplying half of the RPY pose vector change by the transition parameter percentage a;

when the track P is₀P₁When the joint space trajectory is obtained, a transition starting point P is set_sTo the inflection point P₁Each joint position change corresponding trajectory P₀P₁Multiplying half of the position change of each joint by the percentage a of the transition parameter, and further calculating the starting point P of the transition according to the forward kinematics_sThe pose of (a);

when the track P is₁P₂Point of inflection P in a Cartesian space₁To the transition end point P_eIs the track P₁P₂Multiplying half of the length of the straight line by the percentage a of the transition parameter; when the track P is₁P₂Point of inflection P in a Cartesian space₁To the transition end point P_eArc length of (1) is the locus P₁P₂Half the arc length times the transition parameter percentage a; set inflection point P₁To the transition end point P_eCorresponding track P of RPY attitude vector change₁P₂Is multiplied by the transition parameter percentage a;

when the track P is₁P₂Is a jointSetting an inflection point P in the space trajectory₁To the transition end point P_eEach joint position change corresponding trajectory P₁P₂Multiplying half of the change of the position of each joint by the percentage a of the transition parameter, and further calculating the transition end point P according to the positive kinematics_eThe pose of (a);

and step 3: determining boundary velocities of transition trajectories

Inputting a track P by an acceleration and deceleration track planning algorithm of an external software module robot₀P₁At the starting point P of the transition_sVelocity, trajectory P of₁P₂At the transition end point P_eThe speed is determined to be equal to the boundary speed of the starting point and the ending point of the transition track for simplifying calculation;

path velocity V of terminal point corresponding to Cartesian space trajectory_pathTerminal point attitude rotation speed V_oriThe space track of the joint corresponds to the speed of each joint, and under the condition, the speed of 6 degrees of freedom of the pose of the tail end point of the robot is directly calculated by the speed of each joint by means of a Jacobian matrix;

and 4, step 4: bounding velocity for constraining transition trajectory using bow height error

Assumed trajectory P₀P₁And the track P₁P₂A small imaginary arc is arranged between the two arcs, the curvature radius r is adjusted through the arch height error, then the trajectory joining speed is restrained by utilizing the r, and the transition trajectory boundary speed V is ensured_pathAnd V_oriNot exceeding an allowable trace joining speed; the joint space track replaces the original track with a straight line connecting line between the transition point and the inflection point to calculate the included angle at the inflection point; track P₀P₁And the track P₁P₂At the inflection point P₁The included angle of the tangential velocity vector is theta, corresponding to the path velocity V_pathThe constraint formula of (2) is as follows:

$V_{path}\&le;\left\{\begin{array}{c}{V}_{\max }\\ \sqrt{A_{\max }r}\end{array}\right.---\left(1\right)$

wherein, V_maxAt maximum speed of the system, A_maxFor maximum acceleration of the system, E_maxThe allowable bow height error parameter of the system, $r=\frac{E_{max}cos\left(0.5\mathrm{\&theta;}\right)}{1-cos\left(0.5\mathrm{\&theta;}\right)};$

defining a dimension conversion coefficient lambda from angle to millimeter, and equating attitude rotation speed to a speed vector lambda V on a length_ori；

Track P₀P₁And the track P₁P₂At the inflection point P₁The included angle of the tangential attitude rotation speed vector is theta_oriTo the attitude rotation speed V_oriThe constraint formula of (2) is as follows:

$V_{ori}\&le;\left\{\begin{array}{c}\frac{1}{\mathrm{\&lambda;}}{V}_{max}\\ \frac{1}{\mathrm{\&lambda;}}\sqrt{A_{max}{r}_{ori}}\end{array}\right.---\left(2\right)$

wherein, $r_{ori}=\frac{E_{max}cos\left(0.5{\mathrm{\&theta;}}_{ori}\right)}{1-cos\left(0.5{\mathrm{\&theta;}}_{ori}\right)};$

and 5: velocity V of Cartesian space trajectory at boundary point of transition trajectory_pathAnd V_oriDecomposed to pose 6 degrees of freedom

If the track P₀P₁Is a Cartesian space trajectory and starts a transition at a point P_sVelocity V of_pathAnd V_oriDecomposing to pose 6 degrees of freedom; if the track P₁P₂Is a Cartesian space trajectory and will transition to the end point P_eVelocity V of_pathAnd V_oriDecomposing to pose 6 degrees of freedom;

step 6: calculating the starting point P of the transition track_sAnd end point P_eAcceleration in pose 6 degrees of freedom

Assume transition starting point P_sAt the instant of time, the constant speed and the transition end point P_eThe instant of the transition is the uniform speed, and the starting point P of the transition track is obtained by calculating the speed difference_sAnd end point P_eThe pose of (2) is accelerated upwards in 6 degrees of freedom;

and 7: calculating transition track running time T

Assuming the execution time of the transition track and the starting point P of the transition_sMove to the inflection point P at uniform speed₁Then moves at uniform speed to the transition end point P_eThe straight line segments are equal in time, and the running time T of the position transition is calculated according to the straight line segments₁And the runtime T of the attitude transition₂And T is selected for a longer time:

$T=\left\{\begin{array}{cc}{T}_1& ,T_1\&GreaterEqual;T_2\\ T_2& ,T_1<T_2\end{array}\right.---\left(3\right)$

if the position or the posture does not transit, setting the corresponding running time to be zero;

and 8: respectively constructing a transition curve equation P (sigma) for 6 degrees of freedom of the pose of the tail end point of the transition track of the robot, and adopting the superposition and fusion of two parabolic motions as the motion of the transition track, wherein a matrix equation is as follows:

P(σ)＝P₁(σ)+η(σ)[P₂(σ)-P₁(σ)](4)

where σ is a parameterized variable of the transition time t; p₁(σ) is the point corresponding to the starting point of the transition P_sThe connected parabola is a quadratic function of the variable sigma and tangent to the first segment of the track at the transition starting point P_s；P₂(σ) is the end point of transition P_eConnected parabolas, relating to variablesA quadratic function of sigma tangent to the transition end point P_eη (σ) is the guarantee curve P₁(sigma) transition to curve P₂(σ) transition function, η (σ) 6 σ⁵-15σ⁴+10σ³；

Starting point P of transition track_sAnd end point P_eThe boundary conditions of (1): and (3) substituting the pose, the speed and the acceleration into a matrix equation of a formula (4) to determine a curve equation of 6 degrees of freedom of the pose of the transition track.