CN115480481A - Uniaxial high-order constraint acceleration and deceleration control method - Google Patents
Uniaxial high-order constraint acceleration and deceleration control method Download PDFInfo
- Publication number
- CN115480481A CN115480481A CN202211107907.9A CN202211107907A CN115480481A CN 115480481 A CN115480481 A CN 115480481A CN 202211107907 A CN202211107907 A CN 202211107907A CN 115480481 A CN115480481 A CN 115480481A
- Authority
- CN
- China
- Prior art keywords
- acceleration
- time
- speed
- max
- deceleration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/0205—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric not using a model or a simulator of the controlled system
- G05B13/024—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric not using a model or a simulator of the controlled system in which a parameter or coefficient is automatically adjusted to optimise the performance
Abstract
The invention discloses a single-axis high-order constraint acceleration and deceleration control method considering interpolation period discrete errors, which comprises the following steps of: s1: offline pretreatment of the motion process: planning the form of the motion process according to the starting point kinematics parameters and the kinematics high-order constraint requirements, and determining the number of integral acceleration and deceleration sections according to the acceleration numerical value; rounding each motion time segment according to integral multiple of the interpolation period, and readjusting motion process parameters; finally, generating real-time interpolation parameters by combining a real-time interpolation algorithm; s2: and (3) real-time interpolation: and performing real-time interpolation operation according to the real-time interpolation parameters generated by the off-line part and a designed real-time interpolation operation program. According to the method, under the constraint of given speed, acceleration and jerk, a single-axis motion process meeting the requirement of kinematics high-order constraint is planned according to the input starting point position, the input speed, the input acceleration, the input end point position and the input speed, and the problem of motion inaccuracy caused by interpolation period discrete errors is solved.
Description
Technical Field
The invention relates to the technical field of acceleration and deceleration control, in particular to a uniaxial high-order constraint acceleration and deceleration control method considering interpolation period discrete errors.
Background
The motion speed planning is important for realizing the stable operation of the execution equipment, however, in order to ensure the stable motion process, the constraint on high-order kinematic parameters (such as jerk and the like) is needed. However, due to the existence of the real-time interpolation discrete error and the problems of real-time calculation efficiency and the like, the motion position after real-time interpolation has deviation from the speed and the like, and particularly under the condition of high-speed operation, the deviation of the tail end position is obvious.
Disclosure of Invention
The invention aims to provide a uniaxial high-order constraint acceleration and deceleration control method considering interpolation period discrete errors so as to solve the problems in the prior art.
In order to achieve the purpose, the invention provides the following technical scheme: a single-axis high-order constraint acceleration and deceleration control method considering interpolation period discrete errors comprises the following steps:
the noun variable defines:
s _ pos-start point location;
s _ vel-starting point speed;
s _ acc-starting point acceleration;
e _ pos-end point location;
e _ vel-end point velocity;
max _ v-maximum speed;
max _ a-maximum acceleration;
max _ j-maximum jerk;
min _ v-minimum velocity (i.e., reverse maximum velocity, mathematically negative);
min _ a-minimum acceleration (i.e., maximum deceleration, mathematically negative);
min _ j-minimum jerk (i.e., maximum deceleration, mathematically negative);
s1: offline pretreatment of the motion process: and planning the form of the motion process according to the starting and stopping point kinematics parameters and the kinematics high-order constraint requirements (the position, the speed, the acceleration at the starting point, the position and the speed at the ending point, and the acceleration at the ending point are set to be zero, and the high-order constraint requirements are the maximum speed, the maximum acceleration, the maximum jerk, the minimum speed, the minimum acceleration and the minimum jerk), and determining the integral acceleration and deceleration section number according to the jerk value. Rounding each motion time according to integral multiple of interpolation period, and readjusting motion process parameters. And finally, generating real-time interpolation parameters by combining a real-time interpolation algorithm. Wherein, the S1 specifically includes:
s1.1: judging and adjusting the moving direction: firstly, judging the moving direction according to the starting position and the end position, and changing the moving direction when the moving displacement is in a negative direction, namely changing the signs of all kinematic parameters, so as to keep the applicability of the algorithm;
s1.2: planning the motion process: planning a speed curve form according to different input starting and starting constant point kinematic parameters, the maximum speed, the maximum acceleration and the maximum jerk constraint value;
s1.3: rounding each motion time section according to integral multiple of the interpolation period, and recalculating and solving the acceleration polynomial curve parameters according to the rounded time length by an undetermined coefficient method;
s2: and (3) real-time interpolation operation: performing real-time interpolation operation according to the real-time interpolation parameters generated by the off-line part; the real-time algorithm requires the following input parameters: and (4) adjusting the accelerated speed polynomial curve parameters of each section and the round operation time of each section.
Compared with the prior art, the invention has the beneficial effects that: under the constraint of given speed, acceleration and jerk, planning a single-axis motion process meeting the constraint requirement of kinematic parameters according to the input starting point position, the input speed, the input acceleration, the input end point position and the input speed; the invention comprehensively considers the problems of real-time computing resources and data quantity of computing results, pre-computes the operation with more complex computation in the speed planning process through off-line pre-processing, segments the whole acceleration and deceleration process according to the difference of acceleration values, and obtains the running time of each segment; designing a real-time calculation algorithm according to an offline speed planning result to realize single-axis acceleration and deceleration motion control meeting high-order constraint; and simultaneously, in order to solve the problems of speed and displacement deviation and the like caused by real-time interpolation discrete errors, rounding the running time of each section according to a discrete interpolation period, and then readjusting the jerk curve without changing the initial and final kinematic parameters of the section according to the rounded running time, namely, representing the jerk by adopting a quadratic curve to make up and correct the errors generated by the rounding of the time.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings:
FIG. 1 is a graph of acceleration for the present invention with maximum acceleration;
FIG. 2 is a velocity profile for the present invention with acceleration at maximum acceleration;
FIG. 3 is a graph of displacement for the present invention with acceleration at maximum acceleration;
FIG. 4 is a graph of acceleration without acceleration at maximum acceleration for the present invention;
FIG. 5 is a velocity profile for the present invention without acceleration at maximum acceleration;
FIG. 6 is a graph of displacement without acceleration at maximum acceleration for the present invention;
FIG. 7 is a graph of acceleration for the present invention operating at maximum speed;
FIG. 8 is a velocity profile for the present invention when operating at maximum velocity;
FIG. 9 is a graph of displacement for the present invention when operating at maximum speed;
FIG. 10 is a graph of acceleration for the present invention when operating at maximum speed;
FIG. 11 is a speed profile for the present invention with operation at maximum speed;
FIG. 12 is a graph of displacement for the present invention when operating at maximum speed;
FIG. 13 is a schematic structural diagram of a corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 1;
FIG. 14 is a schematic structural diagram of a corresponding acceleration curve and jerk value for type 2 acceleration of the present invention;
FIG. 15 is a schematic structural diagram of a corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 3-1;
FIG. 16 is a schematic structural diagram of a corresponding acceleration curve and jerk value for the type 3-2 acceleration process of the present invention;
FIG. 17 is a schematic structural diagram of a corresponding acceleration curve and jerk value when the acceleration process is type 3-3 according to the present invention;
FIG. 18 is a schematic structural diagram of a corresponding acceleration curve and jerk value for the type 3-4 acceleration process of the present invention;
FIG. 19 is a schematic structural diagram of a corresponding acceleration curve and jerk value for the type 4-1 acceleration process of the present invention;
FIG. 20 is a schematic structural diagram of a corresponding acceleration curve and jerk value for the type 4-2 acceleration process of the present invention;
FIG. 21 is a schematic structural diagram of a corresponding acceleration curve and jerk value for the type 4-3 acceleration process of the present invention;
FIG. 22 is a schematic structural diagram of a corresponding acceleration curve and jerk value for the type 4-4 acceleration process of the present invention;
FIG. 23 is a graph of the time of each movement of the present invention;
FIG. 24 is a second graph of the time of each motion segment according to the present invention;
FIG. 25 is a graph of the displacement curve of experiment 1 of the present invention;
FIG. 26 is a velocity profile for inventive experiment 1;
FIG. 27 is a graph of acceleration curves for inventive experiment 1;
FIG. 28 is a graph of the displacement curve of experiment 2 of the present invention;
FIG. 29 is a velocity profile for experiment 2 of the present invention;
FIG. 30 is a graph of acceleration curves for experiment 2 of the present invention;
FIG. 31 is a graph of the displacement curve of experiment 3 of the present invention;
FIG. 32 is a velocity profile for experiment 3 of the present invention;
FIG. 33 is a graph of acceleration for experiment 3 of the present invention;
FIG. 34 is a graph of the displacement curve of experiment 4 of the present invention;
FIG. 35 is a velocity profile for experiment 4 of the present invention;
FIG. 36 is a graph of acceleration curves for experiment 4 of the present invention;
FIG. 37 is a graph of the displacement curve of experiment 5 of the present invention;
FIG. 38 is a velocity profile for experiment 5 of the present invention;
FIG. 39 is a graph of acceleration for experiment 5 of the present invention;
FIG. 40 is a graph of the displacement curve of experiment 6 of the present invention;
FIG. 41 is a velocity profile for experiment 6 of the present invention;
FIG. 42 is a graph of acceleration curves for experiment 6 of the present invention;
FIG. 43 is a graph of the displacement curve for experiment 7 of the present invention;
FIG. 44 is a velocity profile for experiment 7 of the present invention;
FIG. 45 is a graph of acceleration for experiment 7 of the present invention;
FIG. 46 is a graph of the displacement curve for experiment 8 of the present invention;
FIG. 47 is a velocity profile for experiment 8 of the present invention;
FIG. 48 is a graph of acceleration curves for experiment 8 of the present invention;
FIG. 49 is a graph of the displacement curve for experiment 9 of the present invention;
FIG. 50 is a velocity profile of experiment 9 of the present invention;
FIG. 51 is a graph of acceleration for experiment 9 of the present invention;
FIG. 52 is a flow chart of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings of the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, are within the scope of the present invention. Thus, the following detailed description of the embodiments of the present invention, as presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, are within the scope of the present invention.
Referring to fig. 1-52, in an embodiment of the present invention, a single-axis high-order constrained acceleration/deceleration control method considering interpolation period discrete errors includes the following steps:
the noun variable defines:
s _ pos-start point position;
s _ vel-starting point speed;
s _ acc-starting point acceleration;
e _ pos-end point location;
e _ vel-end point velocity;
max _ v-maximum speed;
max _ a-maximum acceleration;
max _ j-maximum jerk;
min _ v-minimum velocity (i.e., reverse maximum velocity, mathematically negative);
min _ a-minimum acceleration (i.e., maximum deceleration, mathematically negative);
min _ j-minimum jerk (i.e., maximum deceleration, mathematically negative);
s1: offline pretreatment of the motion process: and planning the form of the motion process according to the starting and stopping point kinematics parameters and the kinematics high-order constraint requirements (the position, the speed, the acceleration at the starting point, the position and the speed at the ending point, and the acceleration at the ending point are set to be zero, and the high-order constraint requirements are the maximum speed, the maximum acceleration, the maximum jerk, the minimum speed, the minimum acceleration and the minimum jerk), and determining the integral acceleration and deceleration section number according to the jerk value. Rounding each motion time according to integral multiple of interpolation period, and readjusting motion process parameters. And finally, generating real-time interpolation parameters by combining a real-time interpolation algorithm.
S1.1: judging and adjusting the moving direction: firstly, judging the moving direction according to the starting position and the end position, and changing the moving direction when the moving displacement is in a negative direction, namely changing the signs of all kinematic parameters, so as to keep the applicability of the algorithm;
s1.2: planning the speed of the movement process: planning a speed curve form according to different input starting and starting constant point kinematic parameters, the maximum speed, the maximum acceleration and the maximum jerk constraint value;
s1.3: rounding each motion time section according to integral multiple of the interpolation period, and recalculating and solving the acceleration polynomial curve parameters according to the rounded time length by an undetermined coefficient method;
s2: performing real-time interpolation operation according to the real-time interpolation parameters generated by the off-line part; the real-time algorithm requires the following input parameters: and (4) adjusting the accelerated speed polynomial curve parameters of each section and the round operation time of each section.
Preferably, the speed planning of the exercise process in S1.2 includes the following steps:
s1.2.1: judging the number of the integral acceleration and deceleration process segments according to the initial and final kinematic parameters and the constraint values: judging whether an acceleration section accelerated at constant acceleration exists in the whole acceleration process according to the change of the speed; because there is a constraint limit of maximum and minimum acceleration, there are cases where acceleration is performed with maximum acceleration when the start and end velocity changes are too large; when the change of the starting point and the end point is small, the acceleration process does not have the condition of accelerating with the maximum acceleration;
the maximum acceleration A _ max which can be reached when the acceleration is accelerated to the terminal point speed E _ vel with the maximum jerk by the initial point speed S _ vel and the acceleration S _ acc without the upper limit is used as an evaluation index:
when the evaluation index A _ max is larger than the maximum acceleration max _ a, the acceleration process has three stages: (1) the acceleration is continuously increased, and the acceleration of the section is J = max _ J; (2) an acceleration constant stage, wherein the jerk of the stage is J =0; (3) the acceleration is continuously decreased, and the acceleration of the section is J = min _ J;
when the evaluation index A _ max is smaller than the maximum acceleration max _ a, two stages exist in the acceleration process: (1) the acceleration is continuously increased, and the acceleration of the section is J = max _ J; (2) the acceleration is continuously decreased, and the acceleration of the section is J = min _ J;
the calculation formula of the evaluation index maximum acceleration A _ max is as follows:
Wherein D _ v is the change in velocity;
according to the acceleration and deceleration process, performing integral operation on the speed process to obtain the minimum moving distance S _ min when the requirements of the starting end point speed and the acceleration are met:
S_min＝∫V(t)dt (2)
wherein t is a time variable and V (t) is a speed curve;
according to different displacements, the whole acceleration process can be divided into an acceleration process and a deceleration process or an acceleration process, a uniform speed process and a deceleration process;
determining the number of the segments, and judging whether the conditions of constant-speed operation at the maximum speed exist or not according to the following judgment basis: the distance moved (i.e., whether the distance moved can be long, such that the segment has a maximum allowable speed of operation);
if the situation that whether the movement needs to be carried out at the maximum speed exists is that whether the maximum speed reaches the maximum constraint speed or not; the moving distance when the maximum constraint speed is reached is a judgment standard for judging whether the maximum allowable speed operation exists; the corresponding maximum moving distance S _ uni is determined by:
in the formula: t _ S _ vel is the starting point position time, t _ max _ v is the time at which the maximum constraint speed max _ v is reached, and t _ E _ vel is the end point position time. If the integral moving distance S = E _ pos-S _ pos is larger than S _ min and smaller than S _ uni, the integral acceleration and deceleration process is divided into an acceleration process and a deceleration process, and a constant speed process is not performed; in the acceleration process and the deceleration process, the two-stage or three-stage process is respectively divided into two sections or three sections according to the formula (1) according to the change of the speed;
solving V _ max by adopting an iterative solution method based on a dichotomy; for a certain acceleration and deceleration process, S _ pos, S _ vel, S _ acc, E _ pos and E _ vel are known, and the overall moving distance S _ min < = S < = S _ uni; assuming that V _ max is an unknown variable, the overall acceleration and deceleration process can be divided into an acceleration process from S _ vel to V _ max and a deceleration process from V _ max to E _ vel, and the specific number of stages is calculated according to formula (1); the distance between the two segments can be calculated as:
in the formula: t _ S _ vel is the start point position time, t _ V _ max is the time at which the maximum speed V _ max is reached, and t _ E _ vel is the end point position time. If S = S 0 If yes, the corresponding V _ max is the maximum speed value to be solved; the variable range of V _ max is max (S _ vel, E _ vel)<＝V_max<= max _ v; then the V _ max meeting the acceleration and deceleration process of the section can be quickly obtained by adopting an iterative solution method based on the dichotomy;
the whole acceleration and deceleration process can be segmented according to different values of the acceleration, and other kinematic parameters in the segments can be obtained by calculation according to a formula (2); the number of segments of the whole acceleration and deceleration process is related to the kinematic parameters required by the initial point and the terminal point; taking an acceleration process as an example, setting speed changes as D _ v = E _ vel-S _ vel and a movement distance S = E _ pos-S _ pos, calculating according to a formula (1) to obtain an evaluation index maximum acceleration a _ max, and obtaining a minimum movement distance S _ min through a formula (2). According to the numerical integration, if the jerk is determined, other kinematic parameters (displacement, velocity and acceleration) of the segment can be obtained through the numerical integration, and the calculation formula is as follows:
A(t)＝A 0 +∫J(t)dt
V(t)＝V 0 +A 0 *t+∫∫J(t)dt
S(t)＝S 0 +V 0 *t+0.5*A 0 *t 2 +∫∫∫J(t)dt (5)
wherein S, V, A and J are displacement, velocity, acceleration and jerk functions, respectively; s 0 、V 0 And A 0 Respectively obtaining initial values of the displacement, the speed and the acceleration of the section;
preferably, the type of the segment that can be segmented by the acceleration process is as follows:
(1) when S = S _ min and A _ max < = max _ a, only one acceleration and deceleration process is carried out; according to different acceleration rates, the whole acceleration process is divided into two sections; according to the given kinematic parameters and kinematic constraint values, calculating the running time of each section as follows:
the time T1 and the time T2 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process;
(2) when S = S _ min and A _ max > max _ a, only one acceleration and deceleration process is carried out; according to different acceleration rates, the whole acceleration process is divided into three sections; according to the given kinematic parameters and kinematic constraint values, calculating the running time of each section as follows:
t1 and T3 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and T2 is calculated according to the relation between the whole speed change process and the constant acceleration;
(3) when S _ min<S<When the speed is not less than S _ uni, the whole acceleration process is divided into two sections, and the maximum speed is obtained by adopting an iteration method according to a formula (4); the whole acceleration process is divided into an acceleration process from S _ vel to V _ max and a deceleration process from V _ max to E _ vel; let the change in speed during the first acceleration be D _ v 1 = V _ max-S _ vel, and the corresponding acceleration discriminant is calculated as A _ max1 by the formula (1); the speed change of the second acceleration process is D _ v 2 = E _ vel-V _ max, and the corresponding acceleration discriminant is calculated as A _ max2 by the formula (1);
(4) when S is>When the speed is S _ uni, the whole acceleration process is divided into three sections, namely the whole acceleration process is divided into an acceleration process from S _ vel to max _ v, constant speed motion is carried out by max _ v, and a deceleration process from max _ v to E _ vel; let the change in speed during the first acceleration be D _ v 1 = max _ v-S _ vel, and calculates the corresponding acceleration discriminant as a _ max1 from formula (1); the speed change of the third stage acceleration process is D _ v 2 And (4) = E _ vel-max _ v, and the corresponding acceleration discriminant is A _ max2 calculated by the formula (1).
Preferably, the two stages of acceleration and deceleration processes of the type (3) comprise the following conditions according to different speed changes:
3-1: when A _ max1< = max _ a and A _ max2< = -min _ a are detected, the acceleration is divided into four segments according to different jerks, and the running time of each segment is calculated according to given kinematic parameters and kinematic constraint values and is sequentially as follows:
the time T1, the time T2, the time T3 and the time T4 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process;
3-2: when A _ max1 is greater than max _ a and A _ max2< = -min _ a, the acceleration is divided into five sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially:
the time T1, the time T3, the time T4 and the time T5 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T2 is calculated according to the relation between the whole speed change process and the constant acceleration;
3-3: when A _ max1< = max _ a and A _ max2> -min _ a are detected, the acceleration is divided into five sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and is sequentially as follows:
the time T1, the time T2, the time T3 and the time T5 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T4 is calculated according to the relation between the whole speed change process and the constant acceleration;
3-4: when A _ max1 is larger than max _ a and A _ max2 is larger than min _ a, the acceleration is divided into six sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially:
the time T1, the time T3, the time T4 and the time T6 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T2 and the time T5 are calculated according to the relation between the overall speed change process and the constant acceleration.
Preferably, the two stages of acceleration and deceleration processes in the type (4) include the following cases according to different speed changes:
4-1: when A _ max1< = max _ a and A _ max2< = -min _ a are detected, the acceleration is divided into five sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially as follows:
the time T1, the time T2, the time T4 and the time T5 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T3 is the moving time calculated according to the remaining distance and the maximum moving speed except the acceleration and deceleration moving distance;
4-2: when A _ max1 is greater than max _ a and A _ max2< = -min _ a, the acceleration is divided into six sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially:
the time T1, the time T3, the time T5 and the time T6 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, the time T2 is calculated according to the relation between the overall speed change process and the constant acceleration, the time T4 is the movement time calculated according to the remaining distance and the maximum movement speed except the acceleration and deceleration movement distance;
4-3: when A _ max1< = max _ a and A _ max2> -min _ a are detected, the acceleration is divided into six sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially as follows:
the time T1, the time T2, the time T4 and the time T6 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, the time T5 is calculated according to the relation between the whole speed change process and the constant acceleration, the time T3 is the movement time calculated according to the remaining distance and the maximum movement speed except the acceleration and deceleration movement distance;
4-4: when A _ max1> max _ a and A _ max2> -min _ a are divided into seven sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially:
the time T1, the time T3, the time T5 and the time T7 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, the time T2 and the time T6 are calculated according to the relation between the overall speed change process and the constant acceleration, and the time T4 is calculated according to the movement time obtained by removing the acceleration and deceleration movement distance and calculating according to the residual distance and the maximum movement speed.
Preferably, after the duration of each jerk is obtained, the S1.3 uses a formula (5) to solve the motion process parameters of each segment; therefore, each section of duration time and the corresponding jerk value are input into a real-time interpolation controller for real-time interpolation operation as a speed planning result; however, due to the existence of the interpolation period, a large error exists between the real-time interpolation result and the planning result; in order to compensate for the error generated by the discrete interpolation period, the running time of each section is rounded, and the secondary curve is adopted to recalculate and characterize the acceleration of each section so as to compensate for the deviation generated by time adjustment; each section of operation time is a round rule, and the section of time is an integer of an interpolation period T; according to the formula (5), it can be found that the variation of the operation time results in the deviation of the displacement speed and the acceleration at the end of the section, thereby continuously influencing the subsequent numerical integration process; in order to ensure that the displacement speed and the acceleration are unchanged when each segment is ended, the jerk in the integration process is adjusted according to a formula (5), namely, a quadratic curve is adopted to represent the jerk of each segment, and a coefficient for representing the jerk quadratic curve is solved according to the position speed and the acceleration of the initial point and the final point of each segment;
setting the duration time of a certain period to be t1, the kinematic parameters at the starting point to be a displacement value s0, a velocity value v0 and an acceleration value a0, and setting the kinematic parameters at the end point to be the displacement value s1, the velocity value v1 and the acceleration value a1 according to a formula (5) and the duration time; the secondary acceleration curve is set as follows:
J(t)＝b 2 t 2 +b 1 t+b 0 (16)
according to equation (5), the corresponding acceleration, velocity, and position functions are:
in order to ensure that the kinematic parameters at the end point are s1, v1 and a1 according to the time span t01 after the interpolation period is rounded, the following equation is obtained according to equation (17):
s1＝S(t 01 )
v1＝V(t 01 )
a1＝A(t 01 ) (18)
wherein the initial value is S 0 ＝s0，V 0 ＝v0，A 0 ＝a0，b 0 ,b 1 ,b 2 For the three undetermined parameters of the above equation set, the final equation (18) is a linear equation set, converted to a matrix form:
L·B＝C
B＝L -1 ·C (19)
wherein B = [ B ] 0 ,b 1 ,b 2 ] T ,
C＝[a1-a0,v1-v0-a0·t 01 ,
And finally rounding the time of each section, respectively obtaining a corresponding acceleration quadratic curve parameter B of each section, and performing numerical integration on other kinematic parameters of each section according to a formula (5).
Preferably, the real-time interpolation program in S2 is designed according to the rounded operation time of each segment and the jerk parameter value of each segment, and includes:
in the formula, T i+1 For the i +1 th interpolation period, j is the sequential segment marker, S j0 ，V j0 ，A j0 Is an initial value of each acceleration and deceleration section, t 0n The duration of the nth segment.
In fig. 23 and 24:
finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that modifications may be made to the embodiments described above, or equivalents may be substituted for elements thereof. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (7)
1. A single-axis high-order constraint acceleration and deceleration control method considering interpolation period discrete errors is characterized in that: the method comprises the following steps:
s1: offline pretreatment of the motion process: planning the form of the motion process according to the starting and starting time point kinematics parameters and the kinematics high-order constraint requirement, and determining the number of integral acceleration and deceleration sections according to the acceleration numerical value; rounding each motion time according to integral multiple of the interpolation period, readjusting motion process parameters, and finally generating real-time interpolation parameters by combining a real-time interpolation algorithm; wherein, the S1 specifically includes:
s1.1: judging and adjusting the moving direction: firstly, judging the moving direction according to the starting position and the end position, and changing the moving direction when the moving displacement is in a negative direction, namely changing the signs of all kinematic parameters, and keeping the applicability of the algorithm;
s1.2: planning the speed of the movement process: planning a speed curve form according to different input starting and starting constant point kinematic parameters, the maximum speed, the maximum acceleration and the maximum jerk constraint value;
s1.3: rounding each motion time section according to integral multiple of the interpolation period, and recalculating and solving the acceleration polynomial curve parameters according to the rounded time length by an undetermined coefficient method;
s2: and (3) real-time interpolation operation: performing real-time interpolation operation according to the real-time interpolation parameters generated by the off-line part; the real-time algorithm requires the following input parameters: and (4) adjusting the accelerated speed polynomial curve parameters of each section and the round operation time of each section.
2. The method of claim 1, wherein the method comprises: the speed planning of the movement process in the S1.2 comprises the following steps:
s1.2.1: judging the number of the integral acceleration and deceleration process segments according to the initial and final kinematic parameters and the constraint values: judging whether an acceleration section accelerated at constant acceleration exists in the whole acceleration process according to the change of the speed; because there is a constraint limit of maximum and minimum acceleration, there are cases where acceleration is performed with maximum acceleration when the start and end velocity changes are too large; when the change of the starting point and the end point is small, the acceleration process does not have the condition of accelerating with the maximum acceleration;
the maximum acceleration A _ max which can be reached when the acceleration is accelerated to the terminal point speed E _ vel with the maximum jerk by the initial point speed S _ vel and the acceleration S _ acc without the upper limit is used as an evaluation index:
when the evaluation index A _ max is larger than the maximum acceleration max _ a, the acceleration process has three stages: (1) the acceleration is continuously increased, and the acceleration of the section is J = max _ J; (2) an acceleration constant stage, wherein the jerk of the stage is J =0; (3) the acceleration is continuously decreased, and the acceleration of the section is J = min _ J;
when the evaluation index A _ max is smaller than the maximum acceleration max _ a, two stages exist in the acceleration process: (1) the acceleration is continuously increased, and the acceleration of the section is J = max _ J; (2) the acceleration is continuously decreased, and the acceleration of the section is J = min _ J;
the calculation formula of the evaluation index maximum acceleration A _ max is as follows:
Wherein D _ v is the change in velocity;
according to the acceleration and deceleration process, performing integral operation on the speed process to obtain the minimum moving distance S _ min when the requirements of the starting end point speed and the acceleration are met:
S_min＝∫V(t)dt (2)
wherein t is a time variable and V (t) is a speed curve;
according to different displacements, the whole acceleration process can be divided into an acceleration process and a deceleration process or an acceleration process, a uniform speed process and a deceleration process;
if the number of segments is determined, whether the situation of constant speed operation at the maximum speed exists needs to be judged, and the judgment basis is as follows: the distance moved (i.e., whether the distance moved can be long, such that the segment has a maximum allowable speed of operation);
whether the maximum speed is up to the maximum constraint speed or not is the case of whether the movement at the maximum speed is required or not; the moving distance when the maximum constraint speed is reached is a judgment standard for judging whether the maximum allowable speed operation exists; the corresponding maximum moving distance S _ uni is determined according to the following criteria:
in the formula: t _ S _ vel is the starting point position time, t _ max _ v is the time at which the maximum constraint speed max _ v is reached, and t _ E _ vel is the end point position time. If the integral moving distance S = E _ pos-S _ pos is larger than S _ min and smaller than S _ uni, the integral acceleration and deceleration process is divided into an acceleration process and a deceleration process, and a constant speed process is not performed; in the acceleration process and the deceleration process, the two sections or three sections are respectively divided according to the formula (1) and the change of the speed;
solving V _ max by adopting an iterative solution method based on a dichotomy; for a certain acceleration and deceleration process, S _ pos, S _ vel, S _ acc, E _ pos and E _ vel are known, and the overall moving distance S _ min < = S < = S _ uni; assuming that V _ max is an unknown variable, the overall acceleration and deceleration process can be divided into an acceleration process from S _ vel to V _ max and a deceleration process from V _ max to E _ vel, and the specific number of stages is calculated according to formula (1); the distance between the two segments can be calculated as:
in the formula: t _ S _ vel is the start point position time, t _ V _ max is the time at which the maximum velocity V _ max is reached, and t _ E _ vel is the end point position time. If S = S 0 If yes, the corresponding V _ max is the maximum speed value to be solved; the variable range of V _ max is max (S _ vel, E _ vel)<＝V_max<= max _ v; then the V _ max meeting the acceleration and deceleration process of the section can be quickly obtained by adopting an iterative solution method based on the dichotomy;
the whole acceleration and deceleration process can be segmented according to different values of the acceleration, and other kinematic parameters in the segments can be obtained by calculation according to a formula (2); the number of segments of the whole acceleration and deceleration process is related to the kinematic parameters required by the initial point and the terminal point; taking an acceleration process as an example, setting speed changes to D _ v = E _ vel-S _ vel and a movement distance S = E _ pos-S _ pos, calculating to obtain an evaluation index maximum acceleration A _ max according to a formula (1), and obtaining a minimum movement distance S _ min through a formula (2); according to the numerical integration, when the jerk is determined, other kinematic parameters of the segment are obtained through the numerical integration, and the calculation formula is as follows:
A(t)＝A 0 +∫J(t)dt
V(t)＝V 0 +A 0 *t+∫∫J(t)dt
S(t)＝S 0 +V 0 *t+0.5*A 0 *t 2 +∫∫∫J(t)dt (5)
wherein S, V, A and J are displacement, velocity, acceleration and jerk functions, respectively; s 0 、V 0 And A 0 The displacement, the velocity and the acceleration of the section are initial values respectively.
3. The method according to claim 2, wherein the method comprises the steps of: the types of segmentation that the acceleration process can segment are as follows:
(1) when S = S _ min and A _ max < = max _ a, only one acceleration and deceleration process is carried out; according to different acceleration rates, the whole acceleration process is divided into two sections; according to the given kinematic parameters and kinematic constraint values, calculating the running time of each section as follows:
the time T1 and the time T2 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process;
(2) when S = S _ min and A _ max > max _ a, only one stage of acceleration and deceleration process is available; according to different acceleration rates, the whole acceleration process is divided into three sections; according to the given kinematic parameters and kinematic constraint values, calculating the running time of each section as follows:
t1 and T3 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and T2 is calculated according to the relation between the whole speed change process and the constant acceleration;
(3) when S _ min<S<When the speed is not less than S _ uni, the whole acceleration process is divided into two sections, and the maximum speed is obtained by adopting an iteration method according to a formula (4); the whole acceleration process is divided into an acceleration process from S _ vel to V _ max and a deceleration process from V _ max to E _ vel; let the change in speed during the first acceleration be D _ v 1 = V _ max-S _ vel, and the corresponding acceleration discriminant is calculated as A _ max1 by the formula (1); the speed change of the second acceleration process is D _ v 2 = E _ vel-V _ max, and the corresponding acceleration discriminant is calculated as A _ max2 by the formula (1);
(4) when S is>When the speed is S _ uni, the whole acceleration process is divided into three sections, namely the whole acceleration process is divided into an acceleration process from S _ vel to max _ v, constant speed motion is carried out by max _ v, and a deceleration process from max _ v to E _ vel; let the change in speed during the first acceleration be D _ v 1 = max _ v-S _ vel, and calculates the corresponding acceleration discriminant as a _ max1 from formula (1); the speed change of the third stage acceleration process is D _ v 2 And = E _ vel-max _ v, and the corresponding acceleration discriminant is A _ max2 calculated by the formula (1).
4. The method according to claim 3, wherein the method comprises the steps of: the two stages of acceleration and deceleration processes of the type (3) comprise the following conditions according to different speed changes:
3-1: when A _ max1< = max _ a and A _ max2< = -min _ a are detected, the acceleration is divided into four segments according to different jerks, and the running time of each segment is calculated according to given kinematic parameters and kinematic constraint values and is sequentially as follows:
the time T1, the time T2, the time T3 and the time T4 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process;
3-2: when A _ max1 is greater than max _ a and A _ max2< = -min _ a, the acceleration is divided into five sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially as follows:
the time T1, the time T3, the time T4 and the time T5 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T2 is calculated according to the relation between the whole speed change process and the constant acceleration;
3-3: when A _ max1< = max _ a and A _ max2> -min _ a are detected, the acceleration is divided into five sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially as follows:
the time T1, the time T2, the time T3 and the time T5 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T4 is calculated according to the relation between the whole speed change process and the constant acceleration;
3-4: when A _ max1 is larger than max _ a and A _ max2 is larger than min _ a, the acceleration is divided into six sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially:
the time T1, the time T3, the time T4 and the time T6 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T2 and the time T5 are calculated according to the relation between the overall speed change process and the constant acceleration.
5. The method according to claim 3, wherein the method comprises the steps of: the two stages of acceleration and deceleration processes in the type (4) comprise the following conditions according to different speed changes:
4-1: when A _ max1< = max _ a and A _ max2< = -min _ a are detected, the acceleration is divided into five sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and is sequentially as follows:
the time T1, the time T2, the time T4 and the time T5 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, and the time T3 is the moving time calculated according to the remaining distance and the maximum moving speed except the acceleration and deceleration moving distance;
4-2: when A _ max1 is greater than max _ a and A _ max2< = -min _ a, the acceleration is divided into six sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially as follows:
the time T1, the time T3, the time T5 and the time T6 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, the time T2 is calculated according to the relation between the overall speed change process and the constant acceleration, the time T4 is the movement time calculated according to the remaining distance and the maximum movement speed except the acceleration and deceleration movement distance;
4-3: when A _ max1< = max _ a and A _ max2> -min _ a are detected, the acceleration is divided into six sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and is sequentially as follows:
the time T1, the time T2, the time T4 and the time T6 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, the time T5 is calculated according to the relation between the whole speed change process and the constant acceleration, the time T3 is the movement time calculated according to the remaining distance and the maximum movement speed except the acceleration and deceleration movement distance;
4-4: when A _ max1> max _ a and A _ max2> -min _ a are divided into seven sections according to different jerks, and the running time of each section is calculated according to given kinematic parameters and kinematic constraint values and sequentially:
the time T1, the time T3, the time T5 and the time T7 are calculated according to the relation between the acceleration and the acceleration change in the acceleration and deceleration process, the time T2 and the time T6 are calculated according to the relation between the overall speed change process and the constant acceleration, and the time T4 is calculated according to the movement time obtained by removing the acceleration and deceleration movement distance and calculating according to the residual distance and the maximum movement speed.
6. The method according to claim 2, wherein the method comprises the steps of: s1.3, solving the motion process parameters of each section by adopting a formula (5) after the duration time of each jerk is obtained; therefore, each section of duration time and the corresponding jerk value are input into the real-time interpolation controller for real-time interpolation operation as a speed planning result; however, due to the existence of the interpolation period, a large error exists between the real-time interpolation result and the planning result; in order to compensate for the error generated by the discrete interpolation period, the running time of each section is rounded, and the secondary curve is adopted to recalculate and characterize the acceleration of each section so as to compensate for the deviation generated by time adjustment; each section of operation time is a round rule, and the section of time is an integer of an interpolation period T; according to the formula (5), it can be found that the variation of the operation time results in the deviation of the displacement speed and the acceleration at the end of the section, thereby continuously influencing the subsequent numerical integration process; in order to ensure that the displacement speed and the acceleration are unchanged when each segment is ended, the jerk in the integration process is adjusted according to a formula (5), namely, a quadratic curve is adopted to represent the jerk of each segment, and a coefficient for representing the jerk quadratic curve is solved according to the position speed and the acceleration of the initial point and the final point of each segment;
setting the duration time of a certain section obtained according to the planning as t1, the kinematic parameters at the starting point as a displacement value s0, a velocity value v0 and an acceleration value a0, and the final kinematic parameters obtained according to the formula (5) and the duration time as a displacement value s1, a velocity value v1 and an acceleration value a1; the secondary acceleration curve is set as follows:
J(t)＝b 2 t 2 +b 1 t+b 0 (16)
according to equation (5), the corresponding acceleration, velocity, and position functions are:
in order to ensure that the kinematic parameters at the end point are s1, v1 and a1 according to the time span t01 after the interpolation period is rounded, the following equation is obtained according to equation (17):
s1＝S(t 01 )
v1＝V(t 01 )
a1＝A(t 01 ) (18)
wherein the initial value is S 0 ＝s0，V 0 ＝v0，A 0 ＝a0，b 0 ,b 1 ,b 2 For the three undetermined parameters of the above equation set, the final equation (18) is a linear equation set, converted to a matrix form:
L·B＝C
B＝L -1 ·C (19)
in the formula
Finally, rounding the time of each section, respectively obtaining a corresponding jerk quadratic curve parameter B of each section, and performing numerical integration on other kinematic parameters of each section according to a formula (5).
7. The method for controlling the uniaxial S-shaped acceleration and deceleration facing the real-time interpolation as claimed in claim 1, wherein: the real-time interpolation program in the S2 is designed as follows according to the speed planning result, namely the rounded running time of each section and the jerk curve parameter value of each section:
in the formula, T i+1 For the i +1 th interpolation period, j is the sequential segment marker, S j0 ，V j0 ，A j0 Is an initial value of each acceleration and deceleration section, t 0n The duration of the nth segment.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211107907.9A CN115480481A (en) | 2022-09-13 | 2022-09-13 | Uniaxial high-order constraint acceleration and deceleration control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211107907.9A CN115480481A (en) | 2022-09-13 | 2022-09-13 | Uniaxial high-order constraint acceleration and deceleration control method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115480481A true CN115480481A (en) | 2022-12-16 |
Family
ID=84423417
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211107907.9A Pending CN115480481A (en) | 2022-09-13 | 2022-09-13 | Uniaxial high-order constraint acceleration and deceleration control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115480481A (en) |
-
2022
- 2022-09-13 CN CN202211107907.9A patent/CN115480481A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111158318B (en) | Flexible acceleration and deceleration planning method for asymmetric quartic curve | |
| WO2016127826A1 (en) | Curve velocity planning method and device, and numerical control machining path data processing method | |
| US20120265331A1 (en) | Five-axis flank milling system for machining curved surface and the tool-path planning method thereof | |
| CN109765887B (en) | Automatic driving control method | |
| CN108319228B (en) | Acceleration and deceleration control method in numerical control system trajectory planning | |
| CN111597702B (en) | Rocket landing track planning method and device | |
| CN109683615B (en) | Speed look-ahead method for continuous motion path of robot and robot controller | |
| US20110307096A1 (en) | blending algorithm for trajectory planning | |
| CN107844058B (en) | Motion curve discrete dynamic planning method | |
| CN107765648B (en) | CNC machining feeding speed planning method and device | |
| CN109313429B (en) | S-shaped speed planning method, device and system, robot and numerical control machine | |
| CN114237161B (en) | Industrial robot NURBS curve interpolation method based on digital filtering | |
| CN111240275B (en) | Feed rate planning method based on logarithmic probability function under motion and error limitation | |
| CN113311783A (en) | S-shaped motion curve planning method and system | |
| Ni et al. | An optimized feedrate scheduling method for CNC machining with round-off error compensation | |
| CN103672103B (en) | A kind of governor valve control method, device and intelligent valve positioner | |
| CN108829044B (en) | S-curve acceleration and deceleration planning method for coping with negative values of starting and ending speeds | |
| CN110460274A (en) | A kind of trapezoidal speed regulating control algorithm of stepper motor | |
| KR20080010723A (en) | Method for controlling motor by use of asymmetric s-curve | |
| CN115480481A (en) | Uniaxial high-order constraint acceleration and deceleration control method | |
| CN115202293B (en) | Two-section type speed planning method for industrial robot | |
| CN115963782A (en) | S-shaped velocity planning method based on non-zero initial acceleration | |
| CN110625617B (en) | Intelligent robot track planning method | |
| CN109709794A (en) | A kind of control method suitable for motion controller | |
| CN111381565A (en) | Acceleration recursive calculation limiting method for cubic polynomial S curve acceleration and deceleration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |