CN105082156B - Space trajectory smoothing method based on speed optimum control - Google Patents

Abstract

The present invention proposes a space trajectory smoothing method based on speed optimal control, comprising the following steps: obtaining the starting point, midpoint and end point, preset turning area radius and preset parameters of the space trajectory of the robot manipulator; judging the Whether the radius r of the preset turning area exceeds half of the length of the first simple trajectory; the turning mode of the robot is selected as a turn of equal length or equal time, and the M and S formed by the intersection of the first and second simple trajectories are calculated. and the coordinates of the E point; according to the calculated coordinates of the S, M and E points, the local coordinate system is established, and the trajectory equation of the turning area of the space trajectory under the local coordinate system is calculated, wherein the trajectory equation is smooth Conic curve for the turn zone. The invention can ensure the continuity of the speed direction, can conveniently locate the spatial position of the end effector of the mechanical arm at any time, and is convenient for control.

Description

A Space Trajectory Smoothing Method Based on Velocity Optimal Control

technical field

The invention relates to the technical field of robot control, in particular to a space trajectory smoothing method based on speed optimal control.

Background technique

Spatial trajectory smoothing is a branch of spatial kinematics. Spatial trajectories can be concatenated from a series of simple trajectories. There are two types of simple trajectories: straight line and circular arc, and their start and end points are called nodes. Nodes that need to perform operational tasks are called stop points, and nodes that do not have operational tasks are called transition points. During the movement, it takes a lot of time to go from zero speed to reaching the specified speed, and to decelerate from the specified speed to zero. In order to increase the overall movement speed and improve work efficiency, it is necessary to find ways to avoid unnecessary acceleration and deceleration processes, and to maintain high-speed operation as much as possible. Transition points often exist to avoid obstacles. In the process of traveling along the trajectory, there is no need to reach the transition point accurately, so the concept of smooth turning area is proposed, that is, at the junction of the trajectories where the transition point is located, a smooth curve transition is adopted, and a new smooth trajectory is maintained at a high speed, thereby improving the turning speed.

The existing trajectory smoothing schemes mainly fall into the following two categories.

(1) Based on the speed planning curve, the smooth planning of the trajectory is carried out according to the principle of isochronism (that is, according to the original speed planning, the time from the starting point of the turn to the middle point of the turn is equal to the time from the middle point to the end point). The speed of each point on the turning path is calculated from the speed planning curve, and the space vector superposition of the displacement is carried out.

(2) Based on the speed planning curve, the smooth planning of the trajectory is carried out according to the principle of equidistance (that is, according to the original speed planning, the distance from the starting point of the turn to the middle point of the turn is equal to the distance from the middle point to the end point). The speed of each point on the turning path is calculated from the speed planning curve, and the space vector superposition of the displacement is carried out.

The disadvantages of the above two existing schemes are: there is no explicit trajectory expression, or the trajectory expression is too complicated, not a simple curve, it is difficult to complete the precise control of the trajectory planning of the manipulator, and it is not conducive to calculating the obstacle avoidance space. At the same time, according to the above scheme, the obtained trajectory in the Euclidean space is not a simple smooth curve, and its trajectory is not intuitive, which is not conducive to the operator's direct observation and evaluation of the operation results.

Contents of the invention

The aim of the present invention is to solve at least one of said technical drawbacks.

For this reason, the object of the present invention is to propose a space trajectory smoothing method based on speed optimal control, which can ensure the continuity of the speed direction, and can conveniently locate the spatial position of the end effector of the mechanical arm at any time, which is convenient for control.

In order to achieve the above object, an embodiment of the present invention provides a space trajectory smoothing method based on speed optimal control, including the following steps:

Step S1, obtain the starting point Ps, the midpoint Pm, the end point Pe, the radius r of the preset turning area and the preset parameter flag_dist of the spatial trajectory of the robot arm, wherein the first simple structure between the starting point Ps and the midpoint Pm Trajectory, the second simple trajectory is formed between the midpoint Pm and the end point Pe;

Step S2, comparing the preset turning area radius r with the lengths of the first simple trajectory and the second simple trajectory, if the preset turning area radius r does not exceed the length of the first simple trajectory half of the length of the second simple trajectory, and does not exceed half of the length of the second simple trajectory, then perform step S3;

Step S3, select the turning mode of the robot as equal-length turns or equal-time turns, and calculate the coordinates of the transition point M formed by the intersection of the first and second simple trajectories, and the first and second simple trajectories are respectively compared with the preset The coordinates of points S and E where the circles intersect;

Step S4, establish a local coordinate system according to the calculated coordinates of points S, M and E, and calculate the trajectory equation of the turning area of the space trajectory in the local coordinate system, wherein the trajectory equation is a smooth turning area quadratic curve.

Further, in the step S2, the following formula is used to judge whether the radius r of the preset turning area exceeds half the length of the first simple trajectory and half the length of the second simple trajectory,

in, is a vector from the starting point Ps to the midpoint Pm, is a vector from the midpoint Pm to the end point Pe.

Further, in the step S3, when the selected turning mode is equal-length turning, calculating the coordinates of S, M and E points includes the following steps:

According to the equal length turn needs to meet Using the vector superposition method, the coordinates of the midpoint M are superimposed respectively The vectors with length r in the two directions can be used to obtain the starting point coordinate S and the ending point coordinate E of the turning area, where the midpoint M is the point Pm, and the coordinates of points S and E are calculated as follows:

Further, in the step S3, when the selected turning mode is an equal time turning, the following steps are included:

Turning according to equal time needs to satisfy t ₁ =t ₂ , where t ₁ is the turning time of SM section, t ₂ is the turning time of ME section,

t=min(t ₁ , t ₂ ),

Substituting into the speed planning, calculate the distances dist _s and dist _s from points S and E to point M,

Further, in the step S4, the calculation of the trajectory equation of the turning area of the spatial trajectory in the local coordinate system includes the following steps:

Step S41, calculating the coordinate transformation parameters θ and A, includes the following steps:

Wherein, 0 is the angle at which the coordinate axes are rotated, A is a rotation matrix, and _xSM is the distance between the S point and the M point calculated in the step S3 in the x-axis direction of the coordinate system, and _xME is based on the calculated The distance between the M point and the E point calculated in the step S3 on the x-axis direction of the coordinate system, and _ySM is the distance between the S point calculated in the step S3 and the M point in the y-axis direction of the coordinate system , y _ME is the distance between the M point and the E point in the y-axis direction of the coordinate system calculated according to the step S3;

Step S42, derive vector coordinates with Including the following steps:

in, for Representation in the local coordinate system, for Representation in the local coordinate system,

Step 43, calculating the trajectory equation y of the turning area of the space trajectory in the local coordinate system as:

y=ax ² ,

in, a is the parameter of the quadratic curve corresponding to the trajectory equation, O=S-As, r is the default turning radius, p, q are parabolic parameters, O is the representation of the coordinate origin of the local coordinate system in the global coordinate system, As is the translation part in the coordinate transformation process, s (x _s , y _s ), m(x _m , y _m ), are the coordinates of the starting point, middle point, and end point of the turning area, respectively.

According to the space trajectory smoothing method based on speed optimal control according to the embodiment of the present invention, the quadratic curve of the space trajectory of the robot manipulator can be converted into a smooth turning area expression through coordinate transformation, thereby ensuring the continuity of the velocity direction, through smooth transition Two straight-line trajectories, combined with S-shaped speed planning, can obtain a turning plan with smooth acceleration, speed, and displacement. The trajectory is simple and intuitive, which can easily locate the spatial position of the end effector of the robotic arm at any time, and is easy to control. At the same time, because the trajectory is a simple quadratic curve, the obstacle avoidance space can be easily calculated. In addition, the present invention can adapt to both equidistant and isochronous turning requirements, and the user can conveniently compare and select a better solution according to the actual situation.

Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.

Description of drawings

The above and/or additional aspects and advantages of the present invention will become apparent and comprehensible from the description of the embodiments in conjunction with the following drawings, wherein:

Fig. 1 is the flow chart of the space trajectory smoothing method based on speed optimal control according to one embodiment of the present invention;

Figure 2 is a schematic diagram of first and second simple trajectories according to an embodiment of the present invention;

Fig. 3 is a flowchart of a method for smoothing a space trajectory based on speed optimal control according to another embodiment of the present invention.

detailed description

Embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the figures are exemplary and are intended to explain the present invention and should not be construed as limiting the present invention.

The present invention provides a space trajectory smoothing method based on optimal speed control, which utilizes the smoothing characteristics of the quadratic curve to generate a quadratic curve according to the starting point, the end point, and the turning radius of the enclosed turning area of the two straight-line paths, and smoothly connects the two straight-line paths , transform the quadratic curve to generate a smooth turning area expression, so as to ensure the continuity of the speed direction.

As shown in Figure 1, the space trajectory smoothing method based on speed optimal control of the embodiment of the present invention includes the following steps:

Step S1, obtaining the starting point Ps, the midpoint Pm, the ending point Pe, the radius r of the preset turning area and the preset parameter flag_dist of the spatial trajectory of the robot arm. Among them, the first simple trajectory is formed between the starting point Ps and the midpoint Pm, and the second simple trajectory is formed between the midpoint Pm and the end point Pe. The preset parameter flag_dist is a parameter for taking an equidistant turn or an isochronous turn. When its value is 1, it is an equidistant turn, otherwise it is an isochronous turn.

Figure 2 is a schematic diagram of first and second simple trajectories according to an embodiment of the present invention.

First, the definition of the turning area is explained: At the transition point M formed by the intersection of two simple trajectories, the transition point is taken as the origin, and a certain value R is used as the radius to make a circle, and the two simple trajectories intersect at S and E. The area enclosed by the line segments SM, ME, and arc ES is the turning area. Referring to FIG. 2 , the starting point of the first simple trajectory is Ps, and the ending point is transition point M; the starting point of the second simple trajectory is M, and the ending point is Pe. Both the first and second simple trajectories are straight-line trajectories.

Before starting to plan the trajectory of the turning area, it is necessary to determine the plane of the current path from the two intersecting simple trajectories, and establish the coordinate conversion equation between the two-dimensional plane and the three-dimensional space, and then determine the coordinates of each key point on the two-dimensional plane. Subsequent steps are all carried out on this plane.

Step S2, comparing the preset turning zone radius r with the lengths of the first simple trajectory and the second simple trajectory, if the preset turning zone radius r does not exceed half of the length of the first simple trajectory, and does not exceed the length of the second simple trajectory half of the length of the track, execute step S3.

In this step, the following formula is used to judge whether the radius r of the preset turning area exceeds half of the length of the first simple trajectory and half of the length of the second simple trajectory,

in, is the vector from the starting point Ps to the midpoint Pm, that is, the first simple trajectory in the original trajectory, is the vector from the midpoint Pm to the end point Pe, that is, the second simple trajectory in the original trajectory.

Step S3, select the turning mode of the robot as an equal-length turn or an equal-time turn, and calculate the coordinates of the transition point M formed by the intersection of the first and second simple trajectories, and the intersection points of the first and second simple trajectories with the preset circle respectively. The coordinates of points S and E.

Specifically, equidistant planning or isochronous planning is performed according to the starting point Ps, the midpoint Pm, the ending point Pe, and the radius r of the preset turning area. Among them, the equidistant planning can be obtained directly from the turning radius r. For isochronous planning, the time from the starting point of the turn to the transition point is obtained from the S-shaped speed planning, and the position of the end point of the turn is calculated based on this, so as to obtain the coordinates of the starting point and end point of the turning area. That is, the present invention can be based on S-type speed planning and simultaneously support two kinds of turning area algorithms, isochronous and equidistance planning.

The isometric planning and isochronous planning are described respectively below.

When the selected turning mode is equal-length turning, the calculation of the coordinates of S, M and E points includes the following steps:

Equal-length turns require that the lengths from the start point of the turn zone to the middle point of the turn zone, and from the middle point of the turn zone to the end point of the turn zone are equal, that is, According to the equal length turn needs to meet Using the vector superposition method, the coordinates of the midpoint M (the midpoint M is the point Pm) are superimposed respectively The vectors with length r in the two directions can be used to obtain the coordinates of the starting point S and the ending point E of the turning area.

When the selected turning method is an equal-time turn, the following steps are included:

The equal-time turning method requires that the time from the starting point of the turning area to the middle point of the turning area, and from the middle point of the turning area to the end point of the turning area be equal, that is, t ₁ =t ₂ , where t ₁ is the turning time of the SM section, and t ₂ is the turning time of the ME section.

t=min(t ₁ , t ₂ ), (4)

Substituting into the speed planning, calculate the distances dist _s and dist _s from points S and E to point M,

According to the S, M, E three-point coordinates calculated in step S3, a local coordinate system is established, and the problem is transferred to the local coordinate system through coordinate transformation to solve, so that the obtained trajectory is in the trajectory equation of the turning area under the local coordinate system is a simple quadratic curve. The following is the solution process of calculating the curve equation of the turning area satisfying the smooth condition.

Step S4, establish a local coordinate system based on the calculated coordinates of points S, M and E, and calculate the trajectory equation of the turning area of the spatial trajectory in the local coordinate system, that is, the trajectory equation of the turning area. Among them, the trajectory equation is a smooth quadratic curve in the turning area.

In the algorithm flow, the expression of the quadratic curve equation is established in the local coordinate system, which is mapped with the global coordinate system through linear transformation.

Step S41, calculating the coordinate transformation parameters θ and A, includes the following steps:

Wherein, θ is the angle at which the coordinate axis is rotated, A is a rotation matrix, and _xSM is the distance between the S point and the M point calculated in the step S3 in the x-axis direction of the coordinate system, and _xME is the distance between the point S and the M point in the coordinate system according to the specified The distance between the M point and the E point calculated in the step S3 on the x-axis direction of the coordinate system, and _ySM is the distance between the S point calculated in the step S3 and the M point in the y-axis direction of the coordinate system , y _ME is the distance between the M point and the E point in the y-axis direction of the coordinate system calculated according to the step S3;

Step S42, derive vector coordinates with Including the following steps:

in, for Representation in the local coordinate system, for Represented in the local coordinate system, the coordinate transformation can be decoupled into translation and rotation. For the transformation of the vector, the translation part of the coordinate transformation is eliminated, and only the rotation part remains, so the representations under the two coordinates can be directly linked by the transformation matrix A.

Step S43, calculating the trajectory equation y of the turning area of the space trajectory in the local coordinate system is:

y=ax ² ,

in, a is the parameter of the quadratic curve corresponding to the trajectory equation, O=S-As, r is the preset turning radius, p, q are parabolic parameters, O is the representation of the coordinate origin of the local coordinate system in the global coordinate system, As is the translation part in the process of coordinate transformation, s(x _s , y _s ), m(x _n , y _m ), c(x _e , y _e ) are the coordinates of the starting point, middle point and end point of the turning area respectively.

Fig. 3 is a flowchart of a method for smoothing a space trajectory based on speed optimal control according to another embodiment of the present invention.

Step S301, input Ps, Pm, r, flag_dist.

Step S302, judging the turning radius, which is not more than half of any simple trajectory.

Specifically, judging that the radius r of the turning area does not exceed half of the length of the first simple trajectory, and does not exceed half of the length of the second simple trajectory,

Step S303, using an equal-length turn strategy.

Equal-length turns require that the lengths from the start point of the turn zone to the middle point of the turn zone, and from the middle point of the turn zone to the end point of the turn zone are equal, that is,

Step S304, using an equal-time turning strategy.

Equal time turning mode requires equal time from the starting point of the turning area to the middle point of the turning area, and from the middle point of the turning area to the end point of the turning area, that is, t1=t2.

It should be noted that, step S303 and step S304 can be performed alternatively.

In step S305, three coordinates of S, M and E are obtained.

Step S306, solving the coordinate transformation matrix.

in,

Step S307, solving the local coordinates of the SM and ME vectors.

in,

Step S308, deriving the parameters of the parabola, the coordinates of each point, and the relative coordinates of the origin of the local coordinate system from the formula.

Step S309, outputting trajectory parameters and coordinate transformation parameters.

y=ax ² ,

in, a is the parameter of the quadratic curve corresponding to the trajectory equation, O=S-As, r is the preset turning radius, p and q are parabolic parameters, O is the representation of the coordinate origin of the local coordinate system in the global coordinate system, As is the translation part in the process of coordinate transformation, s(x _a , y _a ), m(x _n , y _m ), e(x _e , y _e ) are the coordinates of the starting point, middle point and end point of the turning area respectively.

According to the space trajectory smoothing method based on speed optimal control according to the embodiment of the present invention, the quadratic curve of the space trajectory of the robot manipulator can be converted into a smooth turning area expression through coordinate transformation, thereby ensuring the continuity of the speed direction, through smooth transition Two straight-line trajectories, combined with S-shaped speed planning, can obtain a turning plan with smooth acceleration, speed, and displacement. The trajectory is simple and intuitive, which can easily locate the spatial position of the end effector of the robotic arm at any time, and is easy to control. At the same time, because the trajectory is a simple quadratic curve, the obstacle avoidance space can be easily calculated. In addition, the present invention can adapt to both equidistant and isochronous turning requirements, and the user can conveniently compare and select a better solution according to the actual situation.

In the description of this specification, descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific examples", or "some examples" mean that specific features described in connection with the embodiment or example , structure, material or feature is included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be construed as limitations to the present invention. Variations, modifications, substitutions, and modifications to the above-described embodiments are possible within the scope of the present invention. The scope of the invention is defined by the appended claims and their equivalents.

Claims (5)

1. A space trajectory smoothing method based on optimal speed control is characterized by comprising the following steps:

step S1, acquiring a starting point Ps, a middle point Pm, a terminal point Pe, a preset turning area radius r and a preset parameter flag _ dist of a space track of the robot mechanical arm operation, wherein a first simple track is formed between the starting point Ps and the middle point Pm, and a second simple track is formed between the middle point Pm and the terminal point Pe;

step S2, comparing the preset turning zone radius r with the lengths of the first simple trajectory and the second simple trajectory, and if the preset turning zone radius r does not exceed half of the length of the first simple trajectory and does not exceed half of the length of the second simple trajectory, performing step S3;

step S3, selecting the turning mode of the robot as equal-length turning or equal-time turning, and calculating to obtain the coordinates of a transition point M formed by the intersection of a first simple track and a second simple track and the coordinates of points S and E of the intersection of the first simple track and the second simple track and a preset circle respectively;

and S4, establishing a local coordinate system according to the coordinates of the S, M point and the E point obtained by calculation, and calculating a track equation of the turning area of the space track under the local coordinate system, wherein the track equation is a quadratic curve of the smooth turning area.

2. The spatial trajectory smoothing method based on the speed optimal control according to claim 1, wherein in the step S2, it is judged whether the preset turning zone radius r exceeds a half of the length of the first simple trajectory and a half of the length of the second simple trajectory using the following formula,

wherein,is a vector pointing from the starting point Ps to the midpoint Pm,is a vector pointing from the midpoint Pm to the end point Pe.

3. The spatial trajectory smoothing method based on the optimal speed control as claimed in claim 2, wherein in the step S3, when the selected turning manner is an equal length turn, the calculating of S, M and E point coordinates comprises the steps of:

according to equal length turningNeed to satisfyRespectively superposing the coordinates of the middle points M by adopting a vector superposition methodThe vector with the length r in the two directions can obtain the starting point coordinate S and the end point coordinate E of the turning area, wherein the middle point M is the point Pm, and the point coordinates of the points S and E are calculated as follows:

4. the spatial trajectory smoothing method based on the speed optimal control as set forth in claim 2, wherein in the step S3, when the selected turning manner is an equal time turning, comprising the steps of:

according to the requirement of waiting time to turn, t is satisfied₁＝t₂Wherein, t₁For SM section turn time, t₂In order to obtain the turning time of the ME section,

t＝min(t₁，t₂)，

substituting the speed plan, and calculating to obtain S, E point-to-M point distance dist_S、dist_E，

5. The method for smoothing spatial trajectory based on optimal speed control of claim 1, wherein in the step S4, the calculating the trajectory equation of the turning area of the spatial trajectory in the local coordinate system comprises the following steps:

step S41, calculating coordinate transformation parameters θ and a, including the steps of:

$\mathrm{\θ}=\arctan \left(-\frac{x_{SM}-x_{ME}}{y_{SM}-y_{ME}}\right),$

$A=\left[\begin{array}{cc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right],$

where θ is the angle of rotation transformation of coordinate axis, A is the rotation matrix, and x_SMX is a distance in the x-axis direction of the coordinate system from the S point and the M point calculated in the step S3_MEY is a distance in the x-axis direction of the coordinate system from the M point and the E point calculated in the step S3_SMY is the distance in the y-axis direction of the coordinate system from the S point and the M point calculated in the step S3_MEIs the distance in the y-axis direction of the coordinate system according to the M point and the E point calculated in the step S3;

step S42, deriving vector coordinatesAndthe method comprises the following steps:

wherein,is composed ofA representation in the local coordinate system is provided,is composed ofA representation in the local coordinate system is provided,

step 43, calculating a trajectory equation y of the turning area of the space trajectory in the local coordinate system as follows:

y＝ax²，

wherein,a is a parameter of a quadratic curve corresponding to the trajectory equation, o is S-As, r is a preset turning radius, p and q are parabolic parameters, O is the representation of the coordinate origin of the local coordinate system in the global coordinate system, As is the translation part in the coordinate transformation process, and S (x)_s，y_s)，m(x_m，y_m)，e(x_e，y_e) Respectively are the coordinates of the starting point, the middle point and the end point of the turning area.