CN105500354B - 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

A kind of transitional track planning method of industrial robot application

Technical field

The present invention relates to a kind of transitional track planning method of industrial robot application.

Background technology

Industrial robot has been widely used in multiple fields, sends out in modern industry automatization, intelligentized development Wave more and more prominent effect.Motion trajectory is a basic task of robot control system, industrial robot Movement locus generally have the straight line of cartesian space, circular arc, and the point-to-point movement locus of joint space.Practical application In, a processing tasks are completed by the order linking of a plurality of track usually, however, adjacent two tracks are present in joining place turning Angle, and many application requirement smooth trajectories do not have turning, i.e. distal point to be transitioned into next track from a smooth trajectory.Two Switching track between track is referred to as transition track.

At present, part studies the trajectory planning of changeover portion, often with circular arc, polynomial curve constructing transition track. Wherein, arc transition can ensure that smooth trajectory, speed at the uniform velocity, have wide application, such as：A kind of Chinese invention patent " weldering Welding robot transitional track planning method " (Application No. 201110000264.3), is aiming at welding robot, changeover portion weldering Seam is connected straightway weld seam and arc section weld seam using circular arc, but arc transition is added with the presence of orbit segment joining place in changeover portion The transition of speed, affects the flatness of track, may cause mechanical vibration, and arc transition cannot realize adjacent two sections only appearances Other application scenarios for having this demand are not just applied to by the track transition of state change；Polynomial curve transition, including batten letter Number, Hermite functions, quintic algebra curve etc., can ensure that the smooth of acceleration by the successional characteristic of high-order, and reduction is shaken It is dynamic, be conducive to high-speed motion, the research of wherein quintic algebra curve transition algorithm is more, such as：Document《Mechanical hand cartesian space rail Mark project study [J].》(Lin Shigao, Liu Xiaolin, it is Euro virtuous《Machine design and manufacture》,2013(3):49-52) to pose 6 Degree of freedom constructs easement curve with quintic algebra curve respectively, not only can guarantee that track, speed, acceleration it is smooth, and can complete Transition only between the track of attitudes vibration, but the geometry of quintic curve is difficult to control to, and lack engineering constraints. Research is based on the track transition between two tracks of cartesian space, to joint space track to cartesian space track above Transition research it is less, and between different tracks not stopping motion for improve work efficiency be necessary.

The content of the invention

It is an object of the present invention to overcome the defect that prior art is present, it is proposed that a kind of mistake of industrial robot application Method for planning track is crossed, one is constructed and be can guarantee that the smooth easement curve of track, speed, acceleration, realize joint space track Transition, the transition of two tracks of cartesian space with cartesian space track, for the track of only attitudes vibration can also be realized Transition, transition trajectory shape can be controlled.Border path speed and attitude rotation simultaneously from application of engineering project, to changeover portion Speed enters row constraint.

The basic technical scheme of the present invention is comprised the following steps：

Step 1：The kinematic parameter that transition trajectory planning is needed imports robot transition track planning module

Robot end's point pose is described jointly by position vector (x, y, z) and RPY orientation vectors (α, beta, gamma), is combined into The compound vector (x, y, z, α, beta, gamma) of one 6DOF, the kinematic parameter that transition trajectory planning is related to has：First track is risen Point pose P₀, terminal pose P₁, Article 2 final on trajectory pose P₂, transition parameter percentage ratio a, engineering constraints include：System Maximal rate V_max, system peak acceleration A_max, system allow most longbow high level error E_max。

Step 2：Determine track P₀P₁With track P₁P₂Between transition track beginning and end pose

As track P₀P₁When being the straight path of cartesian space, transition starting point P_sTo flex point P₁Straight length be track P₀P₁The half of straight length is multiplied by transition parameter percentage ratio a；As track P₀P₁When being the arc track of cartesian space, transition Starting point P_sTo flex point P₁Arc length be track P₀P₁The half of arc length is multiplied by transition parameter percentage ratio a；Setting transition starting point P_sTo turning Point P₁RPY orientation vectors change correspondence track P₀P₁RPY orientation vectors change half be multiplied by transition parameter percentage ratio a.

As track P₀P₁When being joint space track, transition starting point P is set_sTo flex point P₁Each joint position change correspondence Track P₀P₁The half of each joint position change is multiplied by transition parameter percentage ratio a, further calculates transition according to direct kinematics Starting point P_sPose.

As track P₁P₂When being the straight path of cartesian space, flex point P₁To transition terminal P_eStraight length be track P₁P₂The half of straight length is multiplied by transition parameter percentage ratio a；As track P₁P₂When being the arc track of cartesian space, flex point P₁ To transition terminal P_eArc length be track P₁P₂The half of arc length is multiplied by transition parameter percentage ratio a；Setting flex point P₁To transition terminal P_eRPY orientation vectors change correspondence track P₁P₂RPY orientation vectors change half be multiplied by transition parameter percentage ratio a.

As track P₁P₂When being joint space track, flex point P is set₁To transition terminal P_eEach joint position change correspondence Track P₁P₂The half of each joint position change is multiplied by transition parameter percentage ratio a, further calculates transition according to direct kinematics Terminal P_ePose.

Step 3：Determine the boundary speed of transition track

By the acceleration and deceleration trajectory planning algorithm of external software modular robot, input trajectory P₀P₁In transition starting point P_sPlace Speed, track P₁P₂In transition terminal P_eThe speed at place, is to simplify calculating, arranges the boundary speed phase of transition track start, end Deng.

The path velocity V of cartesian space track respective ends point_path, distal point attitude rotary speed V_ori, joint sky Between the track each joint of correspondence speed, in the case of this, by Jacobian matrix, machine can directly be calculated by the speed in each joint The speed of people's distal point 6 degree of freedom of pose.

Step 4：The boundary speed of transition track is constrained using bow high level error

From application of engineering project, when the angle of adjacent track tangential velocity vector is very big, excessive track linking speed Larger impact can be produced to mechanical system, it is therefore necessary to which row constraint is entered to track linking speed.On utilizing works of the present invention Bow high level error parameter come constrained trajectory linking speed, it is ensured that transition track boundary speed V_pathAnd V_oriLess than the rail for allowing Mark is connected speed, and concrete grammar is as follows：

It is assumed that track P₀P₁With track P₁P₂Between have an imaginary small arc-shaped, adjust radius of curvature by bending high level error R, then enters row constraint to track linking speed using r, so as to reach constraint transition track boundary speed V_pathAnd V_oriMesh 's.Imaginary circular arc is used only to the constraints of acquisition speed, has no effect on actual path.Due to joint space track it is not straight See, the angle at former trajectory calculation flex point is substituted with the straight line line between transition point and flex point.

Track P₀P₁With track P₁P₂In flex point P₁The angle of the tangential velocity vector at place is θ, to path velocity V_pathPact Beam formula is as follows：

Wherein, V_maxFor system maximal rate, A_maxFor system peak acceleration, E_maxThe bow high level error parameter that system is allowed,

An angle is defined to the dimension conversion coefficient λ of millimeter, attitude rotary speed is equivalent to into the speed in a length Degree vector λ V_ori, with reference to path velocity V_pathThe way of restraint, to λ V_oriEnter row constraint, you can reach to attitude rotary speed V_oriConstraint purpose.

Track P₀P₁With track P₁P₂In flex point P₁The angle of the tangential attitude rotation velocity vector at place is θ_ori, attitude is revolved Rotary speed V_oriConstraint formulations it is as follows：

Wherein,

Step 5：By cartesian space track transition track boundary point speed V_pathAnd V_oriDecompose pose 6 certainly By on degree

If track P₀P₁It is cartesian space track, by transition starting point P_sSpeed V at place_pathAnd V_oriDecompose pose 6 On degree of freedom；If track P₁P₂It is cartesian space track, by transition terminal P_eSpeed V at place_pathAnd V_oriDecompose pose 6 On degree of freedom.For joint space track, transition starting point P has been obtained in step 3_sWith transition terminal P_e6 freedom of pose at place Speed on degree.

Cartesian space track is divided to straight line and circular arc two kinds of type of gesture, illustrates speed V individually below_pathAnd V_oriPoint Solution method：

For straight path type, the unit vector according to straight path in cartesian coordinate system can directly by V_path Decompose on each axle；

For arc track type, using document《Industrial robot cartesian space trajectory planning [J].》(mechanical engineering With automatization, 2014 (5):Circle can be drawn based on the calculation procedure in the space circular arc interpolation scheme of local coordinate system 141-143) Arc track centre point O_arcPosition coordinateses and local coordinate system Z axis unit direction vector Z ', by vector Z ' and centre point O_arcThe unit direction vector O ' multiplication crosses for pointing to certain point on circular arc can calculate the unit speed tangent vector at the point, you can will V_pathDecompose on each axle.

Then equivalent process attitude track is come with the position processing mode of straight line, by attitude rotary speed V_oriDecompose appearance On 3 degree of freedom of state axle.

Step 6：Calculate transition track starting point P_sWith terminal P_e6 degree of freedom of pose on acceleration

Acceleration is calculated by speed difference, and step 5 is by boundary speed V_pathAnd V_oriDecompose on 6 degree of freedom, It is assumed that transition starting point P_sThe moment at place is at the uniform velocity transition terminal P_eThe moment at place is at the uniform velocity, can to calculate transition track starting point P_sAnd end Point P_e6 degree of freedom of pose on acceleration.

Step 7：Calculate transition track running time T

It is assumed that the execution time of transition track with from transition starting point P_sUniform motion is to flex point P₁Uniform motion is to transition end again Point P_eThe straightway time it is equal, set according to this, respectively calculate position transition running time T₁With the run time of attitude transition T₂, then T choose longer time：

If wherein existence position or attitude not transition, only corresponding run time need to be set to zero.

Step 8：Easement curve equation P (σ) is constructed respectively to 6 degree of freedom of robot transition track distal point pose, Adopt the additive fusion of two parabolic motions for the motion of transition track, matrix equation is as follows：

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

Wherein, σ is the parameterized variables of transit time t；P₁(σ) it is and transition starting point P_sConnected parabola, is with regard to becoming The quadratic function of amount σ, with first paragraph trajectory tangential in transition starting point P_s；P₂(σ) it is and transition terminal P_eConnected parabola, be With regard to the quadratic function of variable σ, with second segment trajectory tangential in transition terminal P_e；η (σ) is to ensure curve P₁(σ) it is transitioned into curve P₂(σ) transition function, can design η (σ)=6 σ⁵-15σ⁴+10σ³, to ensure that P (σ) borders connect on track, speed, acceleration It is continuous.

By transition track starting point P_sWith terminal P_eBoundary condition：Pose, speed, acceleration, bring the matrix of formula (4) into Equation, you can determine the curvilinear equation of 6 degree of freedom of pose of transition track.

The transition track of design ensure that the flatness of track, speed, acceleration, for the track of only attitudes vibration also can Transition is realized, easement curve is formed by two parabola fusions, and curve shape is controllable.

Technical scheme according to more than, it is possible to achieve beneficial effect below：

1) transition being capable of achieving between joint space track and cartesian space track, and two tracks of cartesian space Between transition, cartesian space track includes：Straight line, circular arc；Transition track is unified in planning in cartesian space, and curve is straight See, boundary condition asks for pertaining only to direct kinematics, it is to avoid many solution problems of inverse kinematics.

2) easement curve is formed by two parabola fusions, it is ensured that track, speed, the flatness of acceleration, shaped form Shape is controllable；Transition track is made up of 6 independent curves, for the track for only having attitudes vibration without change in location can also be realized Cross.

3) row constraint is entered to the border path speed and attitude rotary speed of transition track from application of engineering project, prevents from working as When the angle of adjacent track is very big, excessive linking speed may produce larger impact to mechanical system.

Key point：

1) cartesian space track and joint space track are connected to be uniformly processed into and intuitively advise under cartesian space Streak and cross track；

2) algorithm of easement curve is fused into using two parabolas, with ensure track, speed, acceleration it is smooth, with And curve shape is controllable；

3) row constraint is entered to border path velocity using the angle between track, bow high level error from application of engineering project, and Row constraint is entered to border attitude rotary speed in a similar manner.

Description of the drawings

Fig. 1 is the flow chart of transitional track planning method in the present invention.

Fig. 2 is the straight line and arc track schematic diagram that teaching box teaching goes out.

Fig. 3 is the schematic diagram for bending the imaginary circular arc of insertion in high level error restrained boundary speed method.

Fig. 4 is the pathway figure of the robot end's point in straight transitions to circular arc.

Fig. 5 is the position curve figure of the robot end's point on transition track.

Specific embodiment

With reference to SCARA robot systems, the invention will be further described.The system is by a SCARA industrial machine People, switch board, teaching box composition, the wherein big brachium 300mm of SCARA robots, little brachium 300mm, joint about 3 stroke be 200mm.SCARA robots only have γ axle attitudes, are Unify legislation, retain α axles attitude and β axle attitudes, but make its numerical value be zero, Calculating process is not affected.Using teaching box teaching straight path P₀P₁, then teaching arc track P₁P₂, P_MIt is one on arc track Point, as shown in Fig. 2 the P that teaching goes out₀Pose be (- 200, -300,200,0,0, -45), the P that teaching goes out₁Pose for (0, -300, 200,0,0,45), the P that teaching goes out₂Pose for (100, -400,200,0,0,45), the P that teaching goes out_MPose for (50.2429 ,- 386.7423,200,0,0,45), wherein, position unit is millimeter, attitude unit degree of being.

System parameter settings are as follows：System maximal rate V_max=500mm/s, system peak acceleration A_max=2000mm/ s², system allow most longbow high level error E_max=10mm, the dimension conversion coefficient λ of angle to millimeter=2mm/ °, transition parameter Percentage ratio a=100%.

Determine transition starting point P of transition track_sPose and transition terminal P_ePose.Track P₀P₁It is cartesian space Straight path, according to step 2, transition starting point P_sTo flex point P₁Straight length be track P₀P₁The half of straight length It is multiplied by transition parameter percentage ratio a, transition starting point P_sTo flex point P₁γ axles attitudes vibration correspondence track P₀P₁γ axle attitudes vibrations Half be multiplied by transition parameter percentage ratio a, transition starting point P can be calculated_sPose be (- 100, -300,200,0,0,0).Rail Mark P₁P₂It is the arc track of cartesian space, according to step 2, flex point P₁To transition terminal P_eArc length be track P₁P₂Arc Long half is multiplied by transition parameter percentage ratio a, flex point P₁To transition terminal P_eγ axles attitudes vibration correspondence track P₁P₂γ axles The half of attitudes vibration is multiplied by transition parameter percentage ratio a, can calculate transition terminal P_ePose for (29.2893 ,- 370.7107,200,0,0,45)。

According to the agreement of step 3, by the acceleration and deceleration trajectory planning algorithm of external software modular robot, input transition track Boundary speed V_path=500mm/s and V_ori=150 °/s.

From application of engineering project, when the angle of adjacent track tangential velocity vector is very big, excessive track linking speed Larger impact can be produced to mechanical system, it is therefore necessary to which row constraint is entered to track linking speed.On utilizing works of the present invention Bow high level error parameter come constrained trajectory linking speed, it is ensured that transition track boundary speed V_pathAnd V_oriLess than the rail for allowing Mark is connected speed.According to step 4, θ is straight line P₀P₁In flex point P₁Place's tangential velocity vector and circular arc P₁P₂In flex point P₁Place The angle of tangential velocity vector, as shown in figure 3, θ=90 °, bring bow high level error E into_max, r=24.1421mm is obtained, further The maximal rate that bow high level error restriction is calculated by formula (1) is 219.7368mm/s, V_pathExceed the restriction, it is therefore desirable to Constraint V_path=219.7368mm/s.According to step 4, reference path speed V_pathRestriction to attitude rotary speed V_oriEnter Row is limited, track P₀P₁γ axle attitudes change, track P₁P₂γ axle attitudes it is unchanged, i.e. track P₀P₁With track P₁P₂ Flex point P₁The angle of the tangential attitude rotation velocity vector at place is θ_ori=0 °, in theory to V_oriRestriction speed be it is infinitely great, Therefore V_ori=150 °/s.

By cartesian space track transition track boundary point speed V_pathAnd V_oriDecompose on 6 degree of freedom of pose. According to step 5, track P₀P₁It is straight path type, the unit vector in cartesian coordinate system V_pathThe speed decomposed on x, y, z axle is expressed asUnit is mm/s.Track P₁P₂It is round Arc type of gesture, using document《Industrial robot cartesian space trajectory planning [J].》(mechanical engineering and automatization, 2014 (5):Arc track centre point can be drawn based on the calculation procedure in the space circular arc interpolation scheme of local coordinate system 141-143) O_arcPosition coordinateses for (100, -300,200), and the unit direction vector Z ' of the Z axis of local coordinate system=(0,0,1), the center of circle Point O_arcPoint to P on circular arc_eThe unit direction vector of pointCan by vector Z ' and vector O ' multiplication crosses Calculate P_eUnit speed tangent vector at pointCan be by V_pathDecompose the speed on x, y, z axle Degree is expressed asUnit is mm/s.

SCARA robots only have γ axle attitudes, therefore V_oriSpeed be γ axles rotary speed, transition starting point P_sPlace The tachometer that decomposes on α, β, γ axle of attitude rotary speed be shown as that (0,0,150), unit is °/s, track P₁P₂γ Axle attitude is unchanged, transition terminal P_eThe tachometer that the attitude rotary speed at place is decomposed on α, β, γ axle be shown as (0,0, 0), unit is °/s.

Calculate transition track starting point P_sWith terminal P_e6 degree of freedom of pose on acceleration.According to step 6, it is assumed that Transition starting point P_sThe moment at place is at the uniform velocity track P₀P₁It is straight path, on straight line, at any point, unit speed tangent line vector isAcceleration is calculated by speed difference, can calculate transition starting point P_sAcceleration on the x, y, z axle at place for (0,0, 0), unit is mm/s².Track P₁P₂It is arc track type, there is centripetal acceleration, according to step 6, it is assumed that transition end Point P_eThe moment at place is, at the uniform velocity, by speed difference, to calculate point P_eAcceleration on the x, y, z axle at place for (341.0459, 341.7961,0), unit is mm/s²。

SCARA robots only have γ axle attitudes, therefore the only rotary acceleration of γ axles, according to step 6, it is assumed that transition rises Point P_sPlace and transition terminal P_eThe moment at place is at the uniform velocity, therefore to draw transition starting point P_sPlace and transition terminal P_eThe attitude rotation at place Resolution of velocity is expressed as that (0,0,0), unit is °/s to the rotary acceleration on α, β, γ axle²。

Calculate transition track running time T.According to step 7, it is assumed that the execution time of transition track with from transition Point P_sUniform motion is to flex point P₁Uniform motion is to transition terminal P again_eThe straightway time be the same, set according to this, count respectively Calculate the running time T of position transition₁=0.8034s and the running time T of attitude transition₂=0.3s, T choose longer time 0.8034s。

Using matrix equation (4) formula, transition song is constructed respectively to 6 degree of freedom of robot transition track distal point pose Line equation P (σ), wherein, σ is the parameterized variables of time t, 0≤t≤T, 0≤σ≤1, P₁(σ) it is and transition starting point P_sConnected Parabola, is the quadratic function with regard to variable σ, if P₁(σ)=A₁σ²+B₁σ+C₁, P₂(σ) it is and transition terminal P_eConnected parabolic Line, is the quadratic function with regard to variable σ, if P₂(σ)=A₂σ²+B₂σ+C₂, according to step 8, η (σ)=6 σ⁵-15σ⁴+10σ³, incited somebody to action Cross track starting point P_s, terminal P_eBoundary condition：Pose, speed, acceleration, bring the matrix equation of formula (4) into, calculate and are Matrix number A₁=(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₂=(- 58.8302, -271.5726,200,0,0,45), after coefficient matrix determines, that is, determine 6 degree of freedom of pose of transition track Curvilinear equation.Fig. 4 utilizes the pathway figure of the robot end's point in MATLAB Software on Drawing straight transitions to circular arc, wherein Point P_sTo point P_eBetween solid line be exactly transition track, Fig. 5 is the time dependent curve of transition track run duration pose.

Claims (1)

1. a kind of transitional track planning method of industrial robot application, comprises the following steps：

Step 1：The kinematic parameter that transition trajectory planning is needed imports robot transition track planning module

Robot end's point pose is described jointly by position vector (x, y, z) and RPY orientation vectors (α, beta, gamma), is combined into one The compound vector (x, y, z, α, beta, gamma) of 6DOF, the kinematic parameter that transition trajectory planning needs have：First track starting point position Appearance P₀, terminal pose P₁, Article 2 final on trajectory pose P₂, transition parameter percentage ratio a；Engineering constraints include：System is maximum Speed V_max, system peak acceleration A_max, system allow most longbow high level error E_max；

Step 2：Determine track P₀ P₁With track P₁ P₂Between transition track beginning and end pose

As track P₀ P₁When being the straight path of cartesian space, transition starting point P_sTo flex point P₁Straight length be track P₀ P₁ The half of straight length is multiplied by transition parameter percentage ratio a；As track P₀ P₁When being the arc track of cartesian space, transition starting point P_sTo flex point P₁Arc length be track P₀ P₁The half of arc length is multiplied by transition parameter percentage ratio a；Setting transition starting point P_sTo flex point P₁ RPY orientation vectors change correspondence track P₀ P₁The half of RPY orientation vectors change is multiplied by transition parameter percentage ratio a；

As track P₀ P₁When being joint space track, transition starting point P is set_sTo flex point P₁Each joint position change correspondence track P₀ P₁The half of each joint position change is multiplied by transition parameter percentage ratio a, further calculates transition according to direct kinematics and rises Point P_sPose；

As track P₁ P₂When being the straight path of cartesian space, flex point P₁To transition terminal P_eStraight length be track P₁ P₂ The half of straight length is multiplied by transition parameter percentage ratio a；As track P₁ P₂When being the arc track of cartesian space, flex point P₁Arrive Transition terminal P_eArc length be track P₁ P₂The half of arc length is multiplied by transition parameter percentage ratio a；Setting flex point P₁To transition terminal P_e RPY orientation vectors change correspondence track P₁ P₂RPY orientation vectors change half be multiplied by transition parameter percentage ratio a；

As track P₁ P₂When being joint space track, flex point P is set₁To transition terminal P_eEach joint position change correspondence track P₁ P₂The half of each joint position change is multiplied by transition parameter percentage ratio a, further calculates transition end according to direct kinematics Point P_ePose；

Step 3：Determine the boundary speed of transition track

By the acceleration and deceleration trajectory planning algorithm of external software modular robot, input trajectory P₀ P₁In transition starting point P_sThe speed at place Degree, track P₁ P₂In transition terminal P_eThe speed at place, is to simplify calculating, and the boundary speed for arranging transition track start, end is equal；

The path velocity V of cartesian space track respective ends point_path, distal point attitude rotary speed V_ori, joint space rail The speed in each joint of mark correspondence, in the case of this, by Jacobian matrix, directly calculates robot end by the speed in each joint The speed of point 6 degree of freedom of pose；

Step 4：The boundary speed of transition track is constrained using bow high level error

It is assumed that track P₀ P₁With track P₁ P₂Between have an imaginary small arc-shaped, adjust radius of curvature r by bending high level error, so Row constraint is entered to track linking speed using r afterwards, it is ensured that transition track boundary speed V_pathAnd V_oriHold in the mouth less than the track for allowing Connect speed；Straight line line between joint space track transition point and flex point substitutes the angle at former trajectory calculation flex point；Rail Mark P₀ P₁With track P₁ P₂In flex point P₁The angle of the tangential velocity vector at place is θ, to path velocity V_pathConstraint formulations such as Under：

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

Wherein, V_maxFor system maximal rate, A_maxFor system peak acceleration, E_maxThe bow high level error parameter that system is allowed,

An angle is defined to the dimension conversion coefficient λ of millimeter, attitude rotary speed is equivalent to into the arrow of the speed in a length Amount λ V_ori；

Track P₀ P₁With track P₁ P₂In flex point P₁The angle of the tangential attitude rotation velocity vector at place is θ_ori, attitude is rotated Speed V_oriConstraint formulations it 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,

Step 5：By cartesian space track transition track boundary point speed V_pathAnd V_oriDecompose on 6 degree of freedom of pose

If track P₀P₁It is cartesian space track, by transition starting point P_sSpeed V at place_pathAnd V_oriDecompose 6 degree of freedom of pose On；If track P₁P₂It is cartesian space track, by transition terminal P_eSpeed V at place_pathAnd V_oriDecompose 6 degree of freedom of pose On；

Step 6：Calculate transition track starting point P_sWith terminal P_e6 degree of freedom of pose on acceleration

It is assumed that transition starting point P_sThe moment at place is at the uniform velocity transition terminal P_eThe moment at place is at the uniform velocity, to be calculated by speed difference Transition track starting point P_sWith terminal P_e6 degree of freedom of pose on acceleration；

Step 7：Calculate transition track running time T

It is assumed that the execution time of transition track with from transition starting point P_sUniform motion is to flex point P₁Uniform motion is to transition terminal P again_e The straightway time it is equal, set according to this, respectively calculate position transition running time T₁With the running time T of attitude transition₂, T Choose longer time：

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

If wherein corresponding run time is set to zero by existence position or attitude not transition；

Step 8：Easement curve equation P (σ) is constructed respectively to 6 degree of freedom of robot transition track distal point pose, is adopted Motion of the additive fusion of two parabolic motions for transition track, matrix equation are as follows：

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

Wherein, σ is the parameterized variables of transit time t；P₁(σ) it is and transition starting point P_sConnected parabola, is with regard to variable σ Quadratic function, with first paragraph trajectory tangential in transition starting point P_s；P₂(σ) it is and transition terminal P_eConnected parabola, be with regard to The quadratic function of variable σ, with second segment trajectory tangential in transition terminal P_e；η (σ) is to ensure curve P₁(σ) it is transitioned into curve P₂ (σ) transition function, η (σ)=6 σ⁵-15σ⁴+10σ³；

By transition track starting point P_sWith terminal P_eBoundary condition：Pose, speed, acceleration, bring the matrix equation of formula (4) into, Determine the curvilinear equation of 6 degree of freedom of pose of transition track.