A single-axis high-order constrained acceleration and deceleration control method

Abstract

The present invention discloses a single-axis high-order constraint acceleration and deceleration control method considering the interpolation cycle discrete error, comprising the following steps: S1: Offline preprocessing of the motion process: planning the form of the motion process according to the kinematic parameters of the starting point and the kinematic high-order constraint requirements, determining the number of overall acceleration and deceleration segments according to the jerk value; rounding each segment of motion time according to an integer multiple of the interpolation cycle, and readjusting the motion process parameters; finally, combining the real-time interpolation algorithm to generate real-time interpolation parameters; S2: Real-time interpolation: performing real-time interpolation operations according to the real-time interpolation parameters generated by the offline part and the designed real-time interpolation operation program. The present invention, under the given speed, acceleration and jerk constraints, plans a single-axis motion process that meets the kinematic high-order constraint requirements according to the input starting position, speed and acceleration and the end position and speed, and solves the problem of motion inaccuracy caused by the interpolation cycle discrete error.

Description

Technical Field

[0001] The present invention relates to the technical field of acceleration and deceleration control, and in particular to a single-axis high-order constrained acceleration and deceleration control method taking into account interpolation period discrete error. Background Art

[0002] Motion speed planning is crucial to achieve stable operation of the execution equipment. However, in order to ensure the smooth motion process, high-order kinematic parameters (such as acceleration, etc.) need to be constrained. However, due to the existence of real-time interpolation discrete errors and real-time calculation efficiency, there are deviations in the motion position and speed after real-time interpolation, especially in the case of high-speed operation, the end position deviation is obvious. Summary of the invention

[0003] The object of the present invention is to provide a single-axis high-order constrained acceleration and deceleration control method taking into account interpolation period discrete error, so as to solve the problems in the prior art.

[0004] To achieve the above object, the present invention provides the following technical solution: a single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error, comprising the following steps:

[0005] Noun variable definition:

[0006] S_pos-starting point position;

[0007] S_vel-starting point speed;

[0008] S_acc-starting point acceleration;

[0009] E_pos-end point position;

[0010] E_vel-end point velocity;

[0011] max_v – maximum speed;

[0012] max_a – maximum acceleration;

[0013] max_j - maximum jerk;

[0014] min_v - minimum speed (i.e. maximum speed in reverse direction, mathematically a negative value);

[0015] min_a - minimum acceleration (i.e. maximum deceleration, mathematically a negative value);

[0016] min_j - minimum jerk (i.e. maximum deceleration, mathematically a negative value);

[0017] S1: Offline preprocessing of the motion process: According to the kinematic parameters of the start and end points and the kinematic high-order constraints (the position, velocity, acceleration at the start point and the position and velocity at the end point, the acceleration at the end point is set to zero; the high-order constraints are the maximum velocity, maximum acceleration and maximum jerk and the minimum velocity, minimum acceleration and minimum jerk), the form of the motion process is planned, and the number of overall acceleration and deceleration segments is determined according to the jerk value. The motion time of each segment is rounded to an integer multiple of the interpolation period, and the motion process parameters are readjusted. Finally, the real-time interpolation algorithm is combined to generate real-time interpolation parameters. Among them, the S1 specifically includes:

[0018] S1.1: Determination and adjustment of moving direction: First, determine the moving direction according to the starting and ending positions. When the moving displacement is in the negative direction, change the moving direction, that is, change the signs of all kinematic parameters to maintain the applicability of the algorithm;

[0019] S1.2: Motion process planning: Plan the form of the velocity curve according to different input start and end point kinematic parameters, as well as the maximum velocity, acceleration and jerk constraint values;

[0020] S1.3: Round off each motion time segment to an integer multiple of the interpolation period, and recalculate and solve the acceleration polynomial curve parameters based on the rounded time length using the undetermined coefficient method;

[0021] S2: Real-time interpolation operation: Perform real-time interpolation operation according to the real-time interpolation parameters generated by the offline part; the input parameters required by the real-time algorithm are: the acceleration polynomial curve parameters after adjustment of each segment and the rounded running time of each segment.

[0022] Compared with the prior art, the beneficial effects of the present invention are as follows: under given speed, acceleration and jerk constraints, a single-axis motion process that meets the kinematic parameter constraints is planned according to the input starting position, speed and acceleration and the end position and speed; taking into account the real-time computing resources and the amount of calculation result data, the present invention pre-calculates the more complex calculations in the speed planning process through offline preprocessing, divides the overall acceleration and deceleration process into segments according to different jerk values, and obtains the running time of each segment; according to the offline speed planning results, a real-time calculation algorithm is designed to achieve single-axis acceleration and deceleration motion control that meets high-order constraints; at the same time, in order to solve the speed and displacement deviation problems caused by real-time interpolation discrete errors, the running time of each segment is rounded according to the discrete interpolation period, and then according to the rounded running time, the jerk curve is readjusted without changing the initial and ending kinematic parameters of the segment, that is, a quadratic curve is used to characterize the jerk to compensate for the error caused by the rounding of time. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0024] Figure 1 The acceleration curve diagram when the present invention is accelerated at the maximum acceleration;

[0025] Figure 2 The present invention is a speed curve diagram when accelerating at the maximum acceleration;

[0026] Figure 3 The displacement curve diagram when the present invention is accelerated at the maximum acceleration;

[0027] Figure 4 The acceleration curve diagram is when the present invention does not have the maximum acceleration;

[0028] Figure 5 The present invention does not have a speed curve diagram when accelerating at the maximum acceleration;

[0029] Figure 6 The displacement curve diagram when the present invention does not have the maximum acceleration;

[0030] Figure 7 The acceleration curve diagram when the present invention is operated at the maximum speed;

[0031] Figure 8 The speed curve diagram is when the present invention is operated at the maximum speed;

[0032] Fig. 9 The displacement curve diagram is when the present invention is operated at the maximum speed;

[0033] Fig.10 It is an acceleration curve diagram when the present invention is operated at the maximum speed;

[0034] Fig.11 It is a speed curve diagram when the present invention is operated at the maximum speed;

[0035] Fig.12 It is a displacement curve diagram when the present invention is operated at the maximum speed;

[0036] Fig.13 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 1;

[0037] Fig.14 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 2;

[0038] Fig.15 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 3-1;

[0039] Fig.16 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 3-2;

[0040] Fig.17 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 3-3;

[0041] Fig.18 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 3-4;

[0042] Fig.19 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 4-1;

[0043] Fig. 20 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 4-2;

[0044] Fig.21 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 4-3;

[0045] Fig. 22 It is a structural schematic diagram of the corresponding acceleration curve and jerk value when the acceleration process of the present invention is type 4-4;

[0046] Fig.23 It is one of the motion time diagrams of each segment of the present invention;

[0047] Fig.24 This is the second time diagram of each segment of the motion of the present invention;

[0048] Fig.25 is a displacement curve diagram of Experiment 1 of the present invention;

[0049] Fig.26 It is the speed curve diagram of Experiment 1 of the present invention;

[0050] Fig. 27 is the acceleration curve diagram of Experiment 1 of the present invention;

[0051] Fig.28 It is the displacement curve diagram of Experiment 2 of the present invention;

[0052] Fig.29 It is the speed curve diagram of Experiment 2 of the present invention;

[0053] Fig.30This is the acceleration curve diagram of Experiment 2 of the present invention;

[0054] Fig.31 is a displacement curve diagram of Experiment 3 of the present invention;

[0055] Fig.32 It is the speed curve diagram of Experiment 3 of the present invention;

[0056] Fig.33 is the acceleration curve diagram of Experiment 3 of the present invention;

[0057] Fig.34 It is the displacement curve diagram of Experiment 4 of the present invention;

[0058] Fig.35 It is the speed curve diagram of Experiment 4 of the present invention;

[0059] Fig.36 It is the acceleration curve diagram of Experiment 4 of the present invention;

[0060] Fig.37 is a displacement curve diagram of Experiment 5 of the present invention;

[0061] Fig.38 It is the speed curve diagram of Experiment 5 of the present invention;

[0062] Fig.39 is the acceleration curve diagram of Experiment 5 of the present invention;

[0063] Fig.40 is a displacement curve diagram of Experiment 6 of the present invention;

[0064] Fig.41 It is the speed curve diagram of Experiment 6 of the present invention;

[0065] Fig.42 This is the acceleration curve diagram of Experiment 6 of the present invention;

[0066] Fig.43 is a displacement curve diagram of Experiment 7 of the present invention;

[0067] Fig.44 It is the speed curve diagram of Experiment 7 of the present invention;

[0068] Fig.45 is the acceleration curve diagram of Experiment 7 of the present invention;

[0069] Fig.46 is a displacement curve diagram of Experiment 8 of the present invention;

[0070] Fig.47 It is the speed curve diagram of Experiment 8 of the present invention;

[0071] Fig.48 is the acceleration curve diagram of Experiment 8 of the present invention;

[0072] Fig.49 is a displacement curve diagram of Experiment 9 of the present invention;

[0073] Fig.50 It is the speed curve diagram of Experiment 9 of the present invention;

[0074] Fig.51 is the acceleration curve diagram of Experiment 9 of the present invention;

[0075] Fig.52 It is a flow chart of the present invention. DETAILED DESCRIPTION

[0076] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the invention claimed for protection, but merely represents the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0077] See also Figure 1-52 In an embodiment of the present invention, a single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error includes the following steps:

[0078] Noun variable definition:

[0079] S_pos-starting point position;

[0080] S_vel-starting point speed;

[0081] S_acc-starting point acceleration;

[0082] E_pos-end point position;

[0083] E_vel-end point velocity;

[0084] max_v – maximum speed;

[0085] max_a – maximum acceleration;

[0086] max_j - maximum jerk;

[0087] min_v - minimum speed (i.e. maximum speed in reverse direction, mathematically a negative value);

[0088] min_a - minimum acceleration (i.e. maximum deceleration, mathematically a negative value);

[0089] min_j - minimum jerk (i.e. maximum deceleration, mathematically a negative value);

[0090] S1: Offline preprocessing of the motion process: Based on the kinematic parameters of the start and end points and the kinematic high-order constraint requirements (position, velocity, acceleration at the start point and position and velocity at the end point, the acceleration at the end point is set to zero; the high-order constraint requirements are maximum velocity, maximum acceleration and maximum jerk and minimum velocity, minimum acceleration and minimum jerk), the form of the motion process is planned, and the number of overall acceleration and deceleration segments is determined based on the jerk value. The motion time of each segment is rounded to an integer multiple of the interpolation period, and the motion process parameters are readjusted. Finally, the real-time interpolation algorithm is combined to generate real-time interpolation parameters.

[0091] S1.1: Determination and adjustment of moving direction: First, determine the moving direction according to the starting and ending positions. When the moving displacement is in the negative direction, change the moving direction, that is, change the signs of all kinematic parameters to maintain the applicability of the algorithm;

[0092] S1.2: Speed ​​planning of motion process: Plan the form of speed curve according to different input start and end point kinematic parameters, as well as maximum speed, acceleration and jerk constraint values;

[0093] S1.3: Round off each motion time segment to an integer multiple of the interpolation period, and recalculate and solve the acceleration polynomial curve parameters based on the rounded time length using the undetermined coefficient method;

[0094] S2: Perform real-time interpolation calculations based on the real-time interpolation parameters generated by the offline part; the input parameters required by the real-time algorithm are: the parameters of the acceleration polynomial curve after adjustment of each segment and the rounded running time of each segment.

[0095] Preferably, the motion process speed planning in S1.2 comprises the following steps:

[0096] S1.2.1: Determine the number of overall acceleration and deceleration process segments based on the initial and end point kinematic parameters and constraint values: Determine whether there is an acceleration segment with constant acceleration in the overall acceleration process based on the change in speed; because there are maximum and minimum constraints on acceleration, when the speed change between the start and end points is too large, there is a situation where the acceleration is performed with the maximum acceleration; when the change between the start and end points is small, there is no situation where the acceleration is performed with the maximum acceleration in the acceleration process;

[0097] Assuming that there is no upper limit to the acceleration, the maximum acceleration A_max that can be achieved when the initial point velocity S_vel and the acceleration S_acc are accelerated to the end point velocity E_vel with the maximum jerk is used as the evaluation index:

[0098] When the evaluation index A_max is greater than the maximum acceleration max_a, the acceleration process has three stages: ① the stage of increasing acceleration, the jerk of this stage is J=max_j; ② the stage of constant acceleration, the jerk of this stage is J=0; ③ the stage of decreasing acceleration, the jerk of this stage is J=min_j;

[0099] When the evaluation index A_max is less than the maximum acceleration max_a, the acceleration process has two stages: ① the stage of increasing acceleration, the jerk of this stage is J=max_j; ② the stage of decreasing acceleration, the jerk of this stage is J=min_j;

[0100] The calculation formula of the evaluation index maximum acceleration A_max is:

[0101] When D_v>0

[0102] When D_v<0 (1)

[0103] Where D_v is the change in speed;

[0104] According to the above acceleration and deceleration process, the speed process is integrated to obtain the minimum moving distance S_min that meets the start and end speed and acceleration requirements:

[0105] S_min=∫V(t)dt (2)

[0106] Where t is the time variable, V(t) is the velocity curve;

[0107] According to the different displacements, the overall acceleration process can be divided into acceleration process and deceleration process or acceleration process, uniform speed process and deceleration process;

[0108] To determine the number of segments, it is necessary to determine whether there is a situation of uniform speed running at the maximum speed. The basis for determination is: the distance moved (i.e., whether the distance moved can be long enough so that the segment can run at the maximum allowable speed);

[0109] Whether it is necessary to move at the maximum speed is whether the maximum speed reaches the maximum constraint speed; the distance moved when the maximum constraint speed is reached is the judgment standard for whether there is maximum allowable speed operation; the corresponding maximum moving distance S_uni judgment standard is:

[0110]

[0111] Where: t_S_vel is the starting point position, t_max_v is the time when the maximum constraint speed max_v is reached, and t_E_vel is the end point position. If the overall moving distance S = E_pos-S_pos is greater than S_min and less than S_uni, the overall acceleration and deceleration process is divided into two stages: acceleration and deceleration, without a uniform speed process; in the acceleration and deceleration processes, according to the change in speed, they are divided into two or three stages according to formula (1);

[0112] An iterative solution method based on the binary search method is used to obtain V_max. For a certain acceleration and deceleration process, S_pos, S_vel, S_acc, E_pos, and E_vel are all 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. The specific number of further segments is calculated according to formula (1). The distances of the two segments can be calculated as follows:

[0113]

[0114] Where: t_S_vel is the starting point position, t_V_max is the time when the maximum speed V_max is reached, and t_E_vel is the end point position. If S = S0, then the corresponding V_max is the maximum speed value of this section to be determined; the variable range of V_max is max(S_vel, E_vel) <= V_max <= max_v; then the iterative solution method based on the dichotomy method can quickly obtain the V_max that satisfies this acceleration and deceleration process;

[0115] The overall acceleration and deceleration process can be segmented according to different values ​​of jerk, and other kinematic parameters in the segment can be calculated according to formula (2); the number of segments of the overall acceleration and deceleration process is related to the kinematic parameters required at the initial and end points; taking the acceleration process as an example, set the speed change to D_v = E_vel-S_vel and the moving distance S = E_pos-S_pos, and calculate the evaluation index maximum acceleration A_max according to formula (1), and the minimum moving distance S_min is obtained by formula (2). According to numerical integration, if the jerk is determined, the other kinematic parameters of the segment (displacement, velocity and acceleration) can be obtained by numerical integration, and the calculation formula is as follows:

[0116]

[0117] A(t)=A0+∫J(t)dt

[0118] V(t)=V0+A0*t+∫∫J(t)dt

[0119] S(t)=S0+V0*t+0.5*A0*t 2 +∫∫∫J(t)dt (5)

[0120] Where S, V, A and J are displacement, velocity, acceleration and jerk functions respectively; S0, V0 and A0 are the initial values ​​of displacement, velocity and acceleration of this section respectively;

[0121] Preferably, the types of segments that the acceleration process can be divided into are as follows:

[0122] ① When S = S_min, A_max <= max_a, there is only one acceleration and deceleration process; according to the different accelerations, the overall acceleration process is divided into two sections; according to the given kinematic parameters and kinematic constraint values, the running time of each section is calculated as follows:

[0123]

[0124]

[0125] The T1 and T2 times are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process;

[0126] ② When S = S_min, A_max>max_a, there is only one acceleration and deceleration process; according to the different accelerations, the overall acceleration process is divided into three sections; according to the given kinematic parameters and kinematic constraint values, the running time of each section is calculated as follows:

[0127]

[0128]

[0129]

[0130] T1 and T3 are calculated based on the relationship between jerk and acceleration change during acceleration and deceleration, and T2 is calculated based on the relationship between the overall speed change process and constant acceleration;

[0131] ③ When S_min < S <= S_uni, the overall acceleration process is divided into two segments. According to formula (4), the maximum speed is obtained by using the iterative method. Then the overall 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 speed change in the first acceleration segment be D_v1 = V_max - S_vel, and the corresponding acceleration discriminant is calculated as A_max1 by formula (1). The speed change in the second acceleration segment is D_v2 = E_vel - V_max, and the corresponding acceleration discriminant is calculated as A_max2 by formula (1).

[0132] ④ When S > S_uni, the overall acceleration process is divided into three segments. Then the overall acceleration process is divided into an acceleration process from S_vel to max_v, a uniform motion at max_v, and a deceleration process from max_v to E_vel. Let the speed change in the first acceleration segment be D_v1 = max_v - S_vel, and the corresponding acceleration discriminant is calculated as A_max1 by formula (1). The speed change in the third acceleration segment is D_v2 = E_vel - max_v, and the corresponding acceleration discriminant is calculated as A_max2 by formula (1).

[0133] Preferably, the two-segment acceleration and deceleration process of type ③ includes the following situations according to different speed changes:

[0134] 3-1: When A_max1 <= max_a and A_max2 <= -min_a, it is divided into four segments according to different jerk values. According to the given kinematic parameters and kinematic constraint values, the running times of each segment are calculated in sequence as follows:

[0135]

[0136]

[0137]

[0138]

[0139] The times T1, T2, T3, and T4 are calculated based on the relationship between jerk and acceleration change during the acceleration and deceleration process.

[0140] 3-2: When A_max1 > max_a and A_max2 <= -min_a, it is divided into five segments according to different jerk values. According to the given kinematic parameters and kinematic constraint values, the running times of each segment are calculated in sequence as follows:

[0141]

[0142]

[0143]

[0144]

[0145]

[0146] The times T1, T3, T4 and T5 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process, and T2 is calculated based on the relationship between the overall speed change process and the constant acceleration;

[0147] 3-3: When A_max1<=max_a, A_max2>-min_a, it is divided into five segments according to different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows:

[0148]

[0149]

[0150]

[0151]

[0152]

[0153] The times T1, T2, T3 and T5 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process, and T4 is calculated based on the relationship between the overall speed change process and the constant acceleration;

[0154] 3-4: When A_max1>max_a, A_max2>-min_a, it is divided into six segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows:

[0155]

[0156]

[0157]

[0158]

[0159]

[0160]

[0161] The times T1, T3, T4 and T6 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. The times T2 and T5 are calculated based on the relationship between the overall speed change process and the constant acceleration.

[0162] Preferably, the two-stage acceleration and deceleration process in type ④ includes the following situations according to different speed changes:

[0163] 4-1: When A_max1<=max_a, A_max2<=-min_a, it is divided into five sections according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each section is calculated as follows:

[0164]

[0165]

[0166]

[0167]

[0168]

[0169] T1, T2, T4 and T5 are calculated based on the relationship between jerk and acceleration change during acceleration and deceleration. T3 is the moving time calculated based on the remaining distance and the maximum moving speed after excluding the acceleration and deceleration moving distance.

[0170] 4-2: When A_max1>max_a, A_max2<=-min_a, it is divided into six segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows:

[0171]

[0172]

[0173]

[0174]

[0175]

[0176]

[0177] T1, T3, T5 and T6 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. T2 is calculated based on the relationship between the overall speed change process and the constant acceleration. T4 is the moving time calculated based on the remaining distance and the maximum moving speed after excluding the acceleration and deceleration moving distance.

[0178] 4-3: When A_max1<=max_a, A_max2>-min_a, it is divided into six segments according to different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows:

[0179]

[0180]

[0181]

[0182]

[0183]

[0184]

[0185] T1, T2, T4 and T6 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. T5 is calculated based on the relationship between the overall speed change process and the constant acceleration. T3 is the moving time calculated based on the remaining distance and the maximum moving speed after excluding the acceleration and deceleration moving distance.

[0186] 4-4: When A_max1>max_a, A_max2>-min_a, it is divided into seven segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows:

[0187]

[0188]

[0189]

[0190]

[0191]

[0192]

[0193]

[0194] T1, T3, T5 and T7 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. T2 and T6 are calculated based on the relationship between the overall speed change process and the constant acceleration. T4 is the moving time calculated based on the remaining distance and the maximum moving speed excluding the acceleration and deceleration moving distance.

[0195] Preferably, after obtaining the duration of each jerk, S1.3 uses formula (5) to solve the motion process parameters of each segment; therefore, the duration of each segment and the corresponding jerk value are input into the real-time interpolation controller as the result of speed planning for real-time interpolation operation; however, due to the existence of the interpolation cycle, there is a large error between the real-time interpolation result and the planning result; in order to compensate for the error caused by the discrete interpolation cycle, the running time of each segment is rounded, and the jerk of each segment is recalculated and represented by a quadratic curve to compensate for the deviation caused by time adjustment. difference; the rule for rounding the running time of each segment is that the time segment is an integer of the interpolation period T; according to formula (5), it can be concluded that due to the change in running time, the displacement velocity and acceleration at the end of the segment will be deviated, which will continue to affect the subsequent numerical integration process; in order to ensure that the displacement velocity and acceleration at the end of each segment remain unchanged, according to formula (5), the jerk of the integration process is adjusted, that is, the quadratic curve is used to characterize the jerk of each segment, and the coefficient of the quadratic curve characterizing the jerk is solved according to the position velocity and acceleration at the initial and end points of each segment;

[0196] Assume that the duration of a certain period obtained according to the plan is t1, the kinematic parameters at the starting point are displacement value s0, velocity value v0, acceleration value a0, and the kinematic parameters of the end point obtained according to formula (5) and the duration are displacement value s1, velocity value v1, acceleration value a1; suppose the secondary jerk curve is:

[0197] J(t)=b2t 2 +b1t+b0 (16)

[0198] According to formula (5), the corresponding acceleration, velocity and position functions are:

[0199]

[0200]

[0201]

[0202] According to the time span after the interpolation period is rounded off to t01, in order to ensure that the kinematic parameters at the end point are still s1, v1 and a1, the following equation is obtained according to formula (17):

[0203] s1=S(t01 )

[0204] v1=V(t 01 )

[0205] a1=A(t 01 ) (18)

[0206] The initial values ​​are S0=s0, V0=v0, A0=a0, b0, b1, b2 are the three unknown parameters of the above equations, and the final equation (18) is a linear equation system, which is converted into a matrix form:

[0207] L·B=C

[0208] B=L -1 ·C (19)

[0209] Where B = [b0, b1, b2] T ,

[0210] C=[a1-a0,v1-v0-a0·t 01 ,

[0211] Finally, after rounding off the time of each segment, the corresponding acceleration quadratic curve parameter B of each segment is obtained, and the other kinematic parameters of each segment can be obtained by numerical integration according to formula (5).

[0212] Preferably, the real-time interpolation program in S2 is designed as follows based on the rounded running time of each segment and the jerk curve parameter value of each segment of the speed planning result:

[0213]

[0214]

[0215]

[0216]

[0217] Where, T i+1 is the i+1th interpolation cycle, j is the sequential segmentation mark, S j0 , V j0 , A j0 is the initial value of each acceleration and deceleration section, t 0n is the duration of the nth segment.

[0218] Fig.23 and 24 middle:

[0219]

[0220]

[0221] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error, characterized by: The following steps are involved: S1: Offline preprocessing of the motion process: According to the kinematic parameters of the start and end points and the kinematic high-order constraints, the form of the motion process is planned, and the number of overall acceleration and deceleration segments is determined according to the jerk value; the motion time of each segment is rounded to an integer multiple of the interpolation period, and the motion process parameters are readjusted, and finally the real-time interpolation algorithm is combined to generate real-time interpolation parameters; Among them, the position, velocity, acceleration at the starting point and the position and velocity at the end point, the acceleration at the end point is set to zero; the high-order constraints require the maximum velocity, maximum acceleration, maximum jerk and the minimum velocity, minimum acceleration and minimum jerk. Wherein, the S1 specifically includes: S1.1: Determination and adjustment of moving direction: First, determine the moving direction according to the starting and ending positions. When the moving displacement is in the negative direction, change the moving direction, that is, change the signs of all kinematic parameters to maintain the applicability of the algorithm; S1.2: Speed ​​planning of motion process: Plan the form of speed curve according to different input start and end point kinematic parameters, as well as maximum speed, acceleration and jerk constraint values; S1.3: Round off each motion time segment to an integer multiple of the interpolation period, and recalculate and solve the acceleration polynomial curve parameters based on the rounded time length using the undetermined coefficient method; S2: Real-time interpolation calculation: Perform real-time interpolation calculation according to the real-time interpolation parameters generated by the offline part; the input parameters required by the real-time algorithm are: the parameters of the jerk polynomial curve after adjustment of each segment and the rounded running time of each segment; The motion process speed planning in S1.2 includes the following steps: S1.2.1: Determine the number of acceleration and deceleration segments according to the initial and final kinematic parameters and constraint values: Determine whether there is an acceleration segment with constant acceleration in the overall acceleration process according to the change in speed; Assuming that there is no upper limit to the acceleration, the maximum acceleration A_max that can be achieved when the initial point velocity S_vel and the acceleration S_acc are accelerated to the end point velocity E_vel with the maximum jerk is used as the evaluation index: When the evaluation index A_max is greater than the maximum acceleration max_a, the acceleration process has three stages: ① the stage of increasing acceleration, the jerk of this stage is J=max_j; ② the stage of constant acceleration, the jerk of this stage is J=0; ③ the stage of decreasing acceleration, the jerk of this stage is J=min_j; When the evaluation index A_max is less than the maximum acceleration max_a, the acceleration process has two stages: ① the stage of increasing acceleration, the jerk of this stage is J=max_j; ② the stage of decreasing acceleration, the jerk of this stage is J=min_j; The calculation formula of the evaluation index maximum acceleration A_max is: When D_v>0 Where D_v is the change in speed; According to the above acceleration and deceleration process, the speed process is integrated to obtain the minimum moving distance S_min that meets the start and end speed and acceleration requirements: S_min=∫V(t)dt (2) Where t is the time variable, V(t) is the velocity curve; According to the different displacements, the overall acceleration process can be divided into acceleration process and deceleration process or acceleration process, uniform speed process and deceleration process; To determine the number of segments, it is necessary to determine whether there is a situation of uniform speed running at the maximum speed. The basis for judgment is: the distance moved; Whether it is necessary to move at the maximum speed is whether the maximum speed reaches the maximum constraint speed; the distance moved when the maximum constraint speed is reached is the judgment standard for whether there is maximum allowable speed operation; the corresponding maximum moving distance S_uni judgment standard is: Where: t_S_vel is the starting point position time, t_max_v is the time when the maximum constraint speed max_v is reached, and t_E_vel is the end point position time; If the overall moving distance S = E_pos-S_pos is greater than S_min and less than S_uni, where: S_pos is the starting point position, E_pos is the end point position; the overall acceleration and deceleration process is divided into two stages: acceleration process and deceleration process, and there is no uniform speed process; in the acceleration process and deceleration process, according to the change of speed, they are divided into two or three stages according to formula (1); An iterative solution method based on the binary search method is used to obtain V_max. For a certain acceleration and deceleration process, S_pos, S_vel, S_acc, E_pos, and E_vel are all 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. The specific number of further segments is calculated according to formula (1). The distances of the two segments can be calculated as follows: Where: t_S_vel is the starting point position time, t_V_max is the time when the maximum speed V_max is reached, t_E_vel is the end point position time, if S = S0, then the corresponding V_max is the maximum speed value of this section to be calculated; the variable range of V_max is max(S_vel, E_vel) <= V_max <= max_v; then the iterative solution method based on the dichotomy method can quickly obtain V_max that satisfies this acceleration and deceleration process; The overall acceleration and deceleration process can be divided into segments according to different values ​​of jerk, and other kinematic parameters in the segment can be calculated according to formula (2); the number of segments of the overall acceleration and deceleration process is related to the kinematic parameters required at the initial and end points; taking the acceleration process as an example, the speed change is set to D_v = E_vel-S_vel and the moving distance S = E_pos-S_pos, and the evaluation index maximum acceleration A_max is calculated according to formula (1), and the minimum moving distance S_min is obtained by formula (2); according to numerical integration, when the jerk is determined, the other kinematic parameters of the segment are obtained by numerical integration, and the calculation formula is as follows: A(t)=A0+∫J(t)dt V(t)=V0+A0*t+∫∫J(t)dt S(t)=S0+V0*t+0.5*A0*t 2 +∫∫∫J(t)dt (5) Where S, V, A, and J are displacement, velocity, acceleration, and jerk functions respectively; S0, V0, and A0 are the initial values of displacement, velocity, and acceleration for this section respectively.

2. The single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error according to claim 1 is characterized in that: The types into which the acceleration process can be segmented are as follows: ① When S = S_min and A_max <= max_a, there is only one acceleration and deceleration process; according to different jerks, the overall acceleration process is divided into two segments; according to the given kinematic parameters and kinematic constraint values, the running times of each segment are calculated in sequence as follows: The times T1 and T2 are calculated based on the relationship between jerk and acceleration change during the acceleration and deceleration process. ② When S = S_min and A_max > max_a, there is only one acceleration and deceleration process; according to different jerks, the overall acceleration process is divided into three segments; according to the given kinematic parameters and kinematic constraint values, the running times of each segment are calculated in sequence as follows: T1 and T3 are calculated based on the relationship between jerk and acceleration change during the acceleration and deceleration process, and T2 is calculated based on the relationship between the overall velocity change process and constant acceleration. ③ When S_min < S <= S_uni, the overall acceleration process is divided into two segments. According to formula (4), the maximum velocity is obtained by using the iterative method; then the overall 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 velocity change in the first acceleration segment be D_v1 = V_max - S_vel, and the corresponding acceleration discriminant is calculated as A_max1 by formula (1); the velocity change in the second acceleration segment is D_v2 = E_vel - V_max, and the corresponding acceleration discriminant is calculated as A_max2 by formula (1). ④ When S > S_uni, the overall acceleration process is divided into three segments, then the overall acceleration process is divided into an acceleration process from S_vel to max_v, a uniform motion at max_v, and a deceleration process from max_v to E_vel; let the velocity change in the first acceleration segment be D_v1 = max_v - S_vel, and the corresponding acceleration discriminant is calculated as A_max1 by formula (1); the velocity change in the third acceleration segment is D_v2 = E_vel - max_v, and the corresponding acceleration discriminant is calculated as A_max2 by formula (1).

3. The single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error according to claim 2 is characterized in that: The two-segment acceleration and deceleration process of the type ③ includes the following situations according to different velocity changes: 3-1: When A_max1 <= max_a and A_max2 <= -min_a, where min_a is the minimum acceleration, it is divided into four segments according to different jerks. According to the given kinematic parameters and kinematic constraint values, the running times of each segment are calculated in sequence as follows: The times T1, T2, T3, and T4 are calculated based on the relationship between jerk and acceleration change during the acceleration and deceleration process. 3-2: When A_max1 > max_a and A_max2 <= -min_a, it is divided into five segments according to different jerks. According to the given kinematic parameters and kinematic constraint values, the running times of each segment are calculated in sequence as follows: The times T1, T3, T4 and T5 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process, and T2 is calculated based on the relationship between the overall speed change process and the constant acceleration; 3-3: When A_max1<=max_a, A_max2>-min_a, it is divided into five segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows: The times T1, T2, T3 and T5 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process, and T4 is calculated based on the relationship between the overall speed change process and the constant acceleration; 3-4: When A_max1>max_a, A_max2>-min_a, it is divided into six segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows: The times T1, T3, T4 and T6 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. The times T2 and T5 are calculated based on the relationship between the overall speed change process and the constant acceleration.

4. The single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error according to claim 2 is characterized in that: The two-stage acceleration and deceleration process in type ④ includes the following situations according to the different speed changes: 4-1: When A_max1<=max_a, A_max2<=-min_a, where min_a is the minimum acceleration, it is divided into five sections according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each section is calculated as follows: T1, T2, T4 and T5 are calculated based on the relationship between jerk and acceleration change during acceleration and deceleration. T3 is the moving time calculated based on the remaining distance and the maximum moving speed after excluding the acceleration and deceleration moving distance. 4-2: When A_max1>max_a, A_max2<=-min_a, it is divided into six segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows: T1, T3, T5 and T6 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. T2 is calculated based on the relationship between the overall speed change process and the constant acceleration. T4 is the moving time calculated based on the remaining distance and the maximum moving speed after excluding the acceleration and deceleration moving distance. 4-3: When A_max1<=max_a, A_max2>-min_a, it is divided into six segments according to different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows: T1, T2, T4 and T6 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. T5 is calculated based on the relationship between the overall speed change process and the constant acceleration. T3 is the moving time calculated based on the remaining distance and the maximum moving speed after excluding the acceleration and deceleration moving distance. 4-4: When A_max1>max_a, A_max2>-min_a, it is divided into seven segments according to the different jerk. According to the given kinematic parameters and kinematic constraint values, the running time of each segment is calculated as follows: T1, T3, T5 and T7 are calculated based on the relationship between the jerk and the acceleration change during the acceleration and deceleration process. T2 and T6 are calculated based on the relationship between the overall speed change process and the constant acceleration. T4 is the moving time calculated based on the remaining distance and the maximum moving speed excluding the acceleration and deceleration moving distance.

5. The single-axis high-order constrained acceleration and deceleration control method considering interpolation period discrete error according to claim 1 is characterized in that: After obtaining the duration of each jerk, S1.3 uses formula (5) to solve the motion process parameters of each segment; therefore, the duration of each segment and the corresponding jerk value are input into the real-time interpolation controller as the result of speed planning for real-time interpolation operation; in order to compensate for the error caused by the discrete interpolation period, the running time of each segment is rounded, and the jerk of each segment is recalculated and represented by a quadratic curve to compensate for the deviation caused by time adjustment; the rule for rounding the running time of each segment is that the segment time is an integer of the interpolation period T; in order to ensure that the displacement velocity and acceleration remain unchanged at the end of each segment, the jerk of the integration process is adjusted according to formula (5), that is, the jerk of each segment is represented by a quadratic curve, and the coefficient of the quadratic curve representing the jerk is solved according to the position velocity and acceleration at the initial and end points of each segment; Assume that the duration of a certain period obtained according to the plan is t1, the kinematic parameters at the starting point are displacement value s0, velocity value v0, acceleration value a0, and the kinematic parameters of the end point obtained according to formula (5) and the duration are displacement value s1, velocity value v1, acceleration value a1; suppose the secondary jerk curve is: J(t)=b2t 2 +b1t+b0 (16) According to formula (5), the corresponding acceleration, velocity and position functions are: According to the time span after the interpolation period is rounded off to t01, in order to ensure that the kinematic parameters at the end point are still s1, v1 and a1, the following equation is obtained according to formula (17): s1=S(t 01 ) v1=V(t 01 ) a1=A(t 01 ) (18) The initial values ​​are S0=s0, V0=v0, A0=a0, b0, b1, b2 are the three unknown parameters of the above equations, and the final equation (18) is a linear equation system, which is converted into a matrix form: L·B=C B=L -1 ·C (19) Where B = [b0, b1, b2] T , Finally, after rounding off the time of each segment, the corresponding acceleration quadratic curve parameter B of each segment is obtained, and the other kinematic parameters of each segment can be obtained by numerical integration according to formula (5).

Images