An S-type acceleration and deceleration control method with non-zero initial velocity and initial acceleration

Abstract

The present invention discloses an S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration, comprising the following steps: path classification: classifying the path to be planned, thereby generating different S-shaped acceleration and deceleration curve trajectories; parameter calculation: based on the results of the path classification, recalculating the acceleration time and jerk to obtain key parameters constituting velocity discretization, thereby constructing a path planning curve; velocity discretization: using the obtained parameters to re-discretize the velocity to obtain a discrete sequence, which is then input into the control system. The present invention adopts the above-mentioned S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration to realize velocity planning for robots, numerical control systems, and motion control, making systems such as robots and machine tools more flexible and efficient.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to an S-type acceleration and deceleration control method in which the initial velocity and initial acceleration are not zero. Background Art

[0002] Acceleration and deceleration control technology is a key technology in the development of robotic control systems and numerical control systems. Appropriate acceleration and deceleration control algorithms enable controlled objects to reach their target positions quickly and stably under varying operating conditions, while maintaining speed and acceleration within specified limits. Common acceleration and deceleration algorithms include linear, exponential, and S-shaped acceleration and deceleration. While both linear and exponential acceleration and deceleration algorithms can plan the velocity profile of the controlled object and complete the specified motion at the target speed, they both introduce a certain level of impact at the beginning and end of acceleration. In contrast, the S-shaped acceleration and deceleration algorithm constrains jerk, avoiding the impact of sudden acceleration changes and making it more suitable for flexible and high-speed control of robotic systems.

[0003] Existing acceleration and deceleration control algorithms typically require the initial velocity and acceleration to be zero at the start, meaning the controlled object must be stationary at both the starting and end points. This can introduce additional errors in practice or require more complex velocity planning to achieve the desired motion, limiting the application of these methods. Summary of the Invention

[0004] To solve the above problems, the present invention provides an S-type acceleration and deceleration control method with non-zero initial velocity and initial acceleration, which realizes speed planning of robot systems, numerical control systems, and motion control systems, making robots, machine tools, and electromechanical control systems more flexible and efficient.

[0005] To achieve the above object, the present invention provides an S-shaped acceleration and deceleration control method in which the initial velocity and initial acceleration are not zero, comprising the following steps:

[0006] S1. Path classification:

[0007] When the controlled object moves, according to the path length L, initial velocity v0, initial acceleration a0, acceleration time T1, and maximum velocity v max , maximum acceleration a max , the jerk J constraint, the path is classified into normal S-shaped acceleration and deceleration curve trajectory and degraded S-shaped acceleration and deceleration curve trajectory; the parameter T1 is expressed as T1 = N1T s , N1 is the number of discrete points in the acceleration segment trajectory, which is always an integer, T sis the interpolation sampling time; the normal S-shaped acceleration and deceleration curve trajectory includes five stages: acceleration, acceleration and deceleration, constant speed, deceleration, and acceleration and deceleration. The degenerate S-shaped acceleration and deceleration curve trajectory does not completely include the above five stages;

[0008] S2. Parameter calculation:

[0009] Based on the classification results of path planning, the jerk and the time of each stage are recalculated to obtain the key parameters that constitute the discrete speed sequence, thereby constructing the speed planning curve;

[0010] S3, speed discrete:

[0011] The speed planning curve obtained by parameter calculation is discretized again to obtain a discrete sequence, which is then input into the control system.

[0012] Preferably, step S1 specifically includes the following steps:

[0013] S11. Verify whether the initial conditions required for path classification meet the speed, acceleration, and jerk constraints;

[0014] S12. Classify the path according to the length L of the trajectory planning distance, the jerk J, the initial velocity v0, the initial acceleration a0, and the time T1 of the jerk segment.

[0015] Preferably, the initial conditions in step S11 include:

[0016] Initial velocity condition: v0≤v max ,in, v0 represents the velocity at the starting point, J represents the jerk; T1 represents the time of the jerk segment; v max The maximum speed allowed by the motion system;

[0017] Initial acceleration condition: a0≤a max , where a max The maximum acceleration allowed by the motion system;

[0018] Acceleration condition: J≤J max , where J max The maximum jerk allowed by the motion system;

[0019] When all the above conditions are met, path classification calculation is performed.

[0020] Preferably, in step S12, when When , the path is line segment I type, the trajectory has a uniform speed segment, which is a normal S-shaped acceleration and deceleration curve trajectory, and the other cases are degenerate S-shaped acceleration and deceleration curve trajectories;

[0021] when When , the path is line segment II type;

[0022] when When , the path is line segment type IⅡ;

[0023] when When , the path is line segment IV type;

[0024] when , the path is a V-shaped line segment.

[0025] Preferably, step S2 specifically includes the following steps:

[0026] S21. Calculate the parameters of the velocity planning curve. Under different trajectory types, calculate and obtain the key parameters jerk J, the time associated with the initial velocity T0, and the time of the uniform velocity segment T3;

[0027] The parameter T0 is expressed as T0=N0T s , parameter T3 is expressed as T3=N3T s , N0 is the number of discrete points in T0 time, N3 is the number of discrete points in the uniform speed section, both are always integers, T s is the interpolation sampling time;

[0028] S22. Construct a speed planning curve polynomial s(t) according to the parameters.

[0029] Preferably, step S21 specifically includes the following steps:

[0030] S211. Obtain the flag value based on the path classification result. Calculate the key parameters using the following calculation functions: acceleration J, time associated with initial velocity T0, and time of uniform velocity segment T3:

[0031] For line segment type I, flag = 1, Then we have:

[0032] like

[0033]

[0034]

[0035] like

[0036]

[0037]

[0038] For line segment type II, flag = 2, Then we have:

[0039] like

[0040]

[0041]

[0042] like

[0043]

[0044]

[0045] For line segment type IⅡ, flag=3, Then we have:

[0046] like

[0047]

[0048]

[0049] like

[0050]

[0051]

[0052] For line segment type IV, flag = 4, Then we have:

[0053] like

[0054]

[0055]

[0056] like

[0057]

[0058]

[0059] For line segment V-type, flag=5, Then we have:

[0060] like

[0061]

[0062]

[0063] like

[0064]

[0065] S212. Calculate the time T0 associated with the initial velocity using the function Func_T0:

[0066] For all path classification line segment types, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0067] For the three path classification types of line segments I, II, and IⅡ, if the initial velocity satisfies the relationship Then there is Derived

[0068] For the line segment IV path classification type, if the initial velocity satisfies the relationship Then there is Derived

[0069] For the line segment V-shaped path classification type, if the initial condition initial velocity satisfies the relationship Then T0 is not defined;

[0070] Using the above formula as Func_T0, we get T0 = Fun_T0(v0, T1, J), where T0 is a function of J, T0, and v0;

[0071] After calculating T0, considering the discrete characteristics of the control system, the time parameter is sampled and rounded, that is, the time parameter used in the calculation process is the sampling time T s Integer multiples of: take Where N0 is the number of discrete points within the T0 time, which is always an integer. The parameter a is rounded to [a] to obtain the actual value of the T0 parameter used in speed planning.

[0072] Use the function Func_T3 to calculate the time T3 of the uniform speed segment:

[0073] For the line segment type I path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0074] For line segment I and II path classification types, if the initial velocity satisfies the relationship Then there is It is deduced that T3=

[0075] Other path classification types do not have a constant speed segment and therefore do not need to calculate T3.

[0076] The above formula is used as Func_T3, that is, T3 = Fun_T3(L, T0, T1, J), where T3 is a function of J, T0, T1, and J;

[0077] After calculating T3, considering the discrete characteristics of the control system, T3 is sampled and rounded, and the time parameter is set as the sampling time T s An integer multiple of Get the actual value of the T3 parameter used for speed planning, where N3 is the number of discrete points in the uniform speed segment, which is always an integer;

[0078] Recalculate J using the function Func_J:

[0079] For the line segment type I path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0080] If the initial velocity satisfies the relationship Then there is Derived

[0081] For the line segment type II path classification type, if the initial velocity satisfies the relationship Then there is Derived

[0082] If the initial velocity satisfies the relationship Then there is Derived

[0083] For the line segment type IⅡ path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0084] If the initial velocity satisfies the relationship Then there is Derived

[0085] For the line segment IV path classification type, if the initial velocity satisfies the relationship Then there is Derived

[0086] If the initial velocity satisfies the relationship Then there is Derived

[0087] For the line segment V-shaped path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0088] If the initial velocity satisfies the relationship Then there is Derived

[0089] The above calculation formula is used as Func_J, that is, J=Fun_J(L, T0, T1, T3), and the recalculation of J is a function of L, T0, T1, and T3.

[0090] Preferably, in step S22: the line segment type I path classification type curve polynomial s(t) is expressed as follows, where τ1-τ5 are the time variables of acceleration, acceleration / deceleration, constant speed, deceleration, and acceleration / deceleration stages respectively:

[0091] like

[0092]

[0093] like

[0094]

[0095] in, The line segment type II path classification type curve polynomial s(t) is expressed as follows:

[0096] like

[0097]

[0098] like

[0099]

[0100] in

[0101] The line segment IⅡ type path classification type curve polynomial s(t) is expressed as follows:

[0102] like

[0103]

[0104] like

[0105]

[0106] The line segment IV type path classification type curve polynomial s(t) is expressed as follows:

[0107] like

[0108]

[0109] like

[0110]

[0111] in

[0112] The line segment V-shaped path classification type curve polynomial s(t) is expressed as follows:

[0113] like

[0114]

[0115] like

[0116]

[0117] Substitute the values ​​of parameters J, T0, T1, and T3 into the above formula to obtain the actual speed planning curve.

[0118] Preferably, step S3 specifically includes the following steps:

[0119] S31. According to the calculated path planning curve s(t), replace its independent variable t with t=nT s , where n is the serial number of a point in the discrete sequence of speed planning, that is, the speed planning at time t corresponds to the speed planning at point n. Discretize the speed planning curve and calculate the discrete sequence V of speed d =s n+1 -S n ;

[0120] S32. The velocity discrete sequence polynomials for different initial conditions are as follows:

[0121] when hour,

[0122] like

[0123]

[0124] When flag=2, hour,

[0125] like

[0126]

[0127] like

[0128]

[0129] When flag=3, hour,

[0130] like

[0131]

[0132] like

[0133]

[0134] (4) When flag=4, hour,

[0135] like

[0136]

[0137] like

[0138]

[0139] (5) When flag = 2, hour,

[0140] like

[0141]

[0142] like

[0143]

[0144] Where s(t) represents the function of displacement with respect to time in velocity planning, T srepresents the minimum sampling time of the control system, L represents the total distance of speed planning, J represents jerk, flag represents the classification identifier of path planning, n1-n5 are the serial numbers of discrete sequence points in the acceleration, acceleration / deceleration, constant speed, deceleration, and acceleration / deceleration stages respectively, N0, N1, and N3 are the numbers of discrete points corresponding to T0, T1, and T3 at sampling time Ts respectively, and are always integers.

[0145] The present invention has the following beneficial effects:

[0146] 1. The controlled motion of the S-shaped acceleration and deceleration curve trajectory can be replanned at any time during the motion process to achieve S-shaped speed regulation, pause, reselection of the end point and other functions.

[0147] 2. It is suitable for the discrete output of control signals, avoiding errors caused by mismatch between robot control signals and interpolation.

[0148] 3. The calculation is simple and the execution time is short, which can meet the high real-time planning of robot motion.

[0149] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0150] Figure 1 This is an overall flow chart of an S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration according to the present invention;

[0151] Figure 2 A flow chart of speed planning for an S-type acceleration and deceleration control method in which the initial velocity and initial acceleration are not zero according to the present invention;

[0152] Figure 3 The present invention is a parameter calculation flow chart of an S-type acceleration and deceleration control method in which the initial velocity and initial acceleration are not zero. DETAILED DESCRIPTION

[0153] The present invention will be further described below in conjunction with the accompanying drawings. It should be noted that this embodiment is based on the technical solution and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to this embodiment.

[0154] like Figure 1-Figure 3 As shown, an S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration includes the following steps:

[0155] S1. Path classification:

[0156] When the controlled object moves, according to the path length L, initial velocity v0, initial acceleration a0, acceleration time T1, and maximum velocity v max , maximum acceleration amax , the jerk J constraint, the path is classified into normal S-shaped acceleration and deceleration curve trajectory and degraded S-shaped acceleration and deceleration curve trajectory; the parameter T1 is expressed as T1 = N1T s , N1 is the number of discrete points in the acceleration segment trajectory, which is always an integer, T s is the interpolation sampling time; the normal S-shaped acceleration and deceleration curve trajectory includes five stages: acceleration, acceleration and deceleration, constant speed, deceleration, and acceleration and deceleration. The degenerate S-shaped acceleration and deceleration curve trajectory does not completely include the above five stages;

[0157] Preferably, step S1 specifically includes the following steps:

[0158] S11. Verify whether the initial conditions required for path classification meet the speed, acceleration, and jerk constraints;

[0159] Preferably, the initial conditions in step S11 include:

[0160] Initial velocity condition: v0≤v max ,in, v0 represents the velocity at the starting point, J represents the jerk; T1 represents the time of the jerk segment; v max The maximum speed allowed by the motion system;

[0161] Initial acceleration condition: a0≤a max , where a max The maximum acceleration allowed by the motion system;

[0162] Acceleration condition: J≤J max , where J max The maximum jerk allowed by the motion system;

[0163] When all the above conditions are met, path classification calculation is performed.

[0164] S12. Classify the path according to the length L of the trajectory planning distance, the jerk J, the initial velocity v0, the initial acceleration a0, and the time T1 of the jerk segment.

[0165] Preferably, in step S12, when When , the path is line segment I type, the trajectory has a uniform speed segment, which is a normal S-shaped acceleration and deceleration curve trajectory, and the other cases are degenerate S-shaped acceleration and deceleration curve trajectories;

[0166] when When , the path is line segment II type;

[0167] when When , the path is line segment type IⅡ;

[0168] when When , the path is line segment IV type;

[0169] when , the path is a V-shaped line segment.

[0170] S2. Parameter calculation:

[0171] Based on the classification results of path planning, the jerk and the time of each stage are recalculated to obtain the key parameters that constitute the discrete speed sequence, thereby constructing the speed planning curve;

[0172] Preferably, step S2 specifically includes the following steps:

[0173] S21. Calculate the parameters of the velocity planning curve. Different calculations are used for different trajectory types to obtain the key parameters jerk J, the time associated with the initial velocity T0, and the time of the uniform velocity segment T3.

[0174] The parameter T0 is expressed as T0=N0T s , parameter T3 is expressed as T3=N3T s , N0 is the number of discrete points in T0 time, N3 is the number of discrete points in the uniform speed section, both are always integers, T s is the interpolation sampling time;

[0175] Preferably, step S21 specifically includes the following steps:

[0176] S211, obtain fla according to the path classification result g The key parameters are calculated using the different calculation functions Func_J, Func_T0, and Func_T3, which are the key parameters jerk J, the time associated with the initial velocity T0, and the time of the uniform velocity segment T3:

[0177] For line segment type I, flag = 1, Then we have:

[0178] like

[0179]

[0180]

[0181] like

[0182]

[0183]

[0184] For line segment type II, flag = 2, Then we have:

[0185] like

[0186]

[0187]

[0188] like

[0189]

[0190]

[0191] For line segment type IⅡ, flag=3, Then we have:

[0192] like

[0193]

[0194]

[0195] like

[0196]

[0197]

[0198] For line segment type IV, flag = 4, Then we have:

[0199] like

[0200]

[0201]

[0202] like

[0203]

[0204]

[0205] For line segment V-type, flag=5, Then we have:

[0206] like

[0207]

[0208]

[0209] like

[0210]

[0211] S212. Calculate the time T0 associated with the initial velocity using the function Func_T0:

[0212] For all path classification line segment types, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0213] For the three path classification types of line segments I, II, and IⅡ, if the initial velocity satisfies the relationship Then there is Derived

[0214] For the line segment IV path classification type, if the initial velocity satisfies the relationship Then there is Derived

[0215] For the line segment V-shaped path classification type, if the initial condition initial velocity satisfies the relationship Then T0 is not defined;

[0216] Using the above formula as Func_T0, we get T0 = Fun_T0(v0, T1, J), where T0 is a function of J, T0, and v0;

[0217] After calculating T0, considering the discrete characteristics of the control system, the time parameter is sampled and rounded, that is, the time parameter used in the calculation process is the sampling time T s Integer multiples of: take Where N0 is the number of discrete points within the T0 time, which is always an integer. The parameter a is rounded to [a] to obtain the actual value of the T0 parameter used in speed planning.

[0218] Use the function Func_T3 to calculate the time T3 of the uniform speed segment:

[0219] For the line segment type I path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0220] For line segment I and II path classification types, if the initial velocity satisfies the relationship Then there is Derived

[0221] Other path classification types do not have a constant speed segment and therefore do not need to calculate T3.

[0222] The above formula is used as Func_T3, that is, T3 = Fun_T3(L, T0, T1, J), where T3 is a function of J, T0, T1, and J;

[0223] After calculating T3, considering the discrete characteristics of the control system, T3 is sampled and rounded, and the time parameter is set as the sampling time T s An integer multiple of Get the actual value of the T3 parameter used for speed planning, where N3 is the number of discrete points in the uniform speed segment, which is always an integer;

[0224] Recalculate J using the function Func_J:

[0225] For the line segment type I path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0226] If the initial velocity satisfies the relationship Then there is Derived

[0227] For the line segment type II path classification type, if the initial velocity satisfies the relationship Then there is Derived

[0228] If the initial velocity satisfies the relationship Then there is Derived

[0229] For the line segment type IⅡ path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0230] If the initial velocity satisfies the relationship Then there is Derived

[0231] For the line segment IV path classification type, if the initial velocity satisfies the relationship Then there is Derived

[0232] If the initial velocity satisfies the relationship Then there is Derived

[0233] For the line segment V-shaped path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived

[0234] If the initial velocity satisfies the relationship Then there is Derived

[0235] The above calculation formula is used as Func_J, that is, J=Fun_J(L, T0, T1, T3), and the recalculation of J is a function of L, T0, T1, and T3.

[0236] S22. Construct a path planning curve polynomial s(t) based on key parameters.

[0237] Preferably, in step S22: the line segment type I path classification type curve polynomial s(t) is expressed as follows, where τ1-τ5 are the time variables of acceleration, acceleration / deceleration, constant speed, deceleration, and acceleration / deceleration stages respectively:

[0238] like

[0239]

[0240] like

[0241]

[0242] in, The line segment type II path classification type curve polynomial s(t) is expressed as follows:

[0243] like

[0244]

[0245] like

[0246]

[0247] in

[0248] The line segment IⅡ type path classification type curve polynomial s(t) is expressed as follows:

[0249] like

[0250]

[0251] like

[0252]

[0253] The line segment IV type path classification type curve polynomial s(t) is expressed as follows:

[0254] like

[0255]

[0256] like

[0257]

[0258] in

[0259] The line segment V-shaped path classification type curve polynomial s(t) is expressed as follows:

[0260] like

[0261]

[0262] like

[0263]

[0264] Substituting the values ​​of parameters J, T0, T1, and T3 into the above formula, the actual path planning curve can be obtained.

[0265] S3, speed discrete:

[0266] The speed planning curve obtained by parameter calculation is discretized again to obtain a discrete sequence, which is then input into the control system;

[0267] Preferably, step S3 specifically includes the following steps:

[0268] S31. According to the calculated path planning curve s(t), replace its independent variable t with t=nT s , where n is the serial number of a point in the discrete sequence of speed planning, that is, the speed planning at time t corresponds to the speed planning at point n. Discretize the speed planning curve and calculate the discrete sequence V of speed d =sn+1 -s n ;

[0269] S32. The velocity discrete sequence polynomials for different initial conditions are as follows:

[0270] When flag=1, hour,

[0271] like

[0272]

[0273] like

[0274]

[0275] When flag=2, hour,

[0276] like

[0277]

[0278] like

[0279]

[0280] When flag=3, hour,

[0281] like

[0282]

[0283] like

[0284]

[0285] (4) When flag=4, hour,

[0286] like

[0287]

[0288] like

[0289]

[0290] (5) When flag = 2, hour,

[0291] like

[0292]

[0293] like

[0294]

[0295] Where s(t) represents the function of displacement with respect to time in velocity planning, T s represents the minimum sampling time of the control system, L represents the total distance of speed planning, J represents jerk, flag represents the classification identifier of path planning, n1-n5 are the serial numbers of discrete sequence points in the acceleration, acceleration / deceleration, constant speed, deceleration, and acceleration / deceleration stages respectively, N0, N1, and N3 are the numbers of discrete points corresponding to T0, T1, and T3 at sampling time Ts respectively, and are always integers.

[0296] Therefore, the present invention adopts the above-mentioned S-type acceleration and deceleration control method in which the initial velocity and initial acceleration are not zero. At the starting position of the velocity planning, it is assumed that the initial velocity and initial acceleration are v0 and a0 respectively. The initial value can be obtained arbitrarily in each stage of the movement of the controlled object, so that the acceleration and deceleration algorithm can enable the robot system to perform acceleration and deceleration re-planning, speed regulation and other actions in real time according to the velocity planning requirements during the movement process, making the robot control system more flexible and efficient.

[0297] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. An S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration, characterized in that: The following steps are involved: S1. Path classification: When the controlled object moves, according to the path length L, initial velocity v0, initial acceleration a0, acceleration time T1, and maximum velocity v max , maximum acceleration a max , jerk J constraint, the path is classified into normal S-shaped acceleration and deceleration curve trajectory and degraded S-shaped acceleration and deceleration curve trajectory; the parameter T1 is expressed as T1=N1T s , N1 is the number of discrete points in the acceleration segment trajectory, which is always an integer, T s is the interpolation sampling time; the normal S-shaped acceleration and deceleration curve trajectory includes five stages: acceleration, deceleration, constant speed, deceleration, and acceleration and deceleration. The degenerate S-shaped acceleration and deceleration curve trajectory does not completely include the above five stages; S2. Parameter calculation: Based on the classification results of path planning, the jerk and the time of each stage are recalculated to obtain the key parameters that constitute the discrete speed sequence, thereby constructing the speed planning curve; Step S2 specifically includes the following steps: S21. Calculate the parameters of the velocity planning curve. Under different trajectory types, calculate and obtain the key parameters jerk J, the time associated with the initial velocity T0, and the time of the uniform velocity segment T3; The parameter T0 is expressed as T0=N0T s , parameter T3 is expressed as T3=N3T s , N0 is the number of discrete points in T0 time, N3 is the number of discrete points in the uniform speed section, both are always integers, T s is the interpolation sampling time; S22, constructing a speed planning curve polynomial s(t) according to the parameters; Step S21 specifically includes the following steps: S211. Obtain the flag value based on the path classification result. Calculate the key parameters using the following calculation functions: acceleration J, time associated with initial velocity T0, and time of uniform velocity segment T3: For line segment type I, flag = 1, Then we have: like like For line segment type II, flag = 2, Then: If like For line segment type III, flag = 3, Then: If like For line segment type IV, flag = 4, Then we have: like like For line segment V, flag = 5, Then we have: like like S212. Calculate the time T0 associated with the initial velocity using the function Func_T0: For all path classification types, if the initial velocity satisfies the relationship Then there is Derived For the three path classification types of line segments I, II, and III, if the initial velocity satisfies the relationship Then there is Derived For the line segment type IV path classification type, if the initial velocity satisfies the relationship Then there is Derived For the line segment V-type path classification type, if the initial condition initial velocity satisfies the relationship Then T0 is not defined; Using the above formula as Func_T0, we get T0 = Fun_T0(v0, T1, J), that is, T0 is a function of J, T0, and v0; After calculating T0, considering the discrete characteristics of the control system, the time parameter is sampled and rounded, that is, the time parameter used in the calculation process is the sampling time T s Integer multiples of: take Where N0 is the number of discrete points within the T0 time, which is always an integer. The parameter a is rounded to [a] to obtain the actual value of the T0 parameter used in speed planning. Use the function Func_T3 to calculate the time T3 of the uniform speed segment: For the line segment type I path classification type, if the initial velocity satisfies the relationship Then there is Derived For line segment type I and II path classification types, if the initial velocity satisfies the relationship Then there is Derived Other path classification types do not have a constant speed segment and do not need to calculate T3; The above formula is used as Func_T3, that is, T3 = Fun_T3(L, T0, T1, J), where T3 is a function of J, T0, T1, and J; After calculating T3, considering the discrete characteristics of the control system, T3 is sampled and rounded, and the time parameter is set as the sampling time T s An integer multiple of Get the actual value of the T3 parameter used for speed planning, where N3 is the number of discrete points in the uniform speed segment, which is always an integer; Recalculate J using the function Func_J: For the line segment type I path classification type, if the initial velocity satisfies the relationship Then there is Derived If the initial velocity satisfies the relationship Then there is Derived For the line segment type II path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived If the initial velocity satisfies the relationship Then there is Derived For the line segment type III path classification type, if the initial velocity satisfies the relationship Then there is Derived If the initial velocity satisfies the relationship Then there is Derived For the line segment type IV path classification type, if the initial velocity satisfies the relationship Then there is Derived If the initial velocity satisfies the relationship Then there is Derived For the line segment V-type path classification type, if the initial condition initial velocity satisfies the relationship Then there is Derived If the initial velocity satisfies the relationship Then there is Derived The above calculation formula is used as Func_J, that is, J = Fun_J(L, T0, T1, T3), and the recalculation of J is a function of L, T0, T1, and T3; S3, speed discrete: The speed planning curve obtained by parameter calculation is discretized again to obtain a discrete sequence, which is then input into the control system.

2. The S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration according to claim 1, characterized in that: Step S1 specifically includes the following steps: S11. Verify whether the initial conditions required for path classification meet the speed, acceleration, and jerk constraints; S12. Classify the path according to the length L of the trajectory planning distance, the jerk J, the initial velocity v0, the initial acceleration a0, and the time T1 of the jerk segment.

3. The S-shaped acceleration / deceleration control method with non-zero initial velocity and initial acceleration according to claim 2, characterized in that: The speed, acceleration, and jerk constraints described in step S11 include: Initial velocity condition: v0≤v max ,in, v0 represents the velocity at the starting point, J represents the jerk; T1 represents the time of the jerk segment; v max The maximum speed allowed by the motion system; Initial acceleration condition: a0≤a max , where a max The maximum acceleration allowed by the motion system; Jerk condition: J≤J max , where J max The maximum jerk allowed by the motion system; When all the above conditions are met, path classification calculation is performed.

4. The S-shaped acceleration / deceleration control method with non-zero initial velocity and initial acceleration according to claim 2, characterized in that: In step S12, when When , the path is line segment type I, and the trajectory has a uniform speed segment; it is a normal S-shaped acceleration and deceleration curve trajectory, and other cases are degenerate S-shaped acceleration and deceleration curve trajectories; when When , the path is line segment type II; when When , the path is line segment type III; when When , the path is line segment type IV; when , the path is a line segment V-shaped.

5. The S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration according to claim 1, characterized in that: In step S22: the line segment type I path classification type curve polynomial s(t) is expressed as follows, where τ1-τ5 are the time variables of acceleration, acceleration / deceleration, constant speed, deceleration, and acceleration / deceleration stages respectively: like like in, The line segment type II path classification type curve polynomial s(t) is expressed as follows: like like in The expression of the line segment type III path classification type curve polynomial s(t) is as follows: like like The expression of the line segment IV type path classification type curve polynomial s(t) is as follows: like like in The line segment V-type path classification type curve polynomial s(t) is expressed as follows: like like Substitute the values ​​of parameters J, T0, T1, and T3 into the above formula to obtain the actual speed planning curve.

6. The S-shaped acceleration and deceleration control method with non-zero initial velocity and initial acceleration according to claim 1, characterized in that: Step S3 specifically includes the following steps: S31. According to the calculated speed planning curve s(t), replace its independent variable t with t=nT s , where n is the serial number of a point in the discrete sequence of speed planning, that is, the speed planning at time t corresponds to the speed planning at point n, the speed planning curve is discretized, and the discrete sequence of speed V is calculated d =s n+1 -s n ; S32. The velocity discrete sequence polynomials for different initial conditions are as follows: When flag=1, hour, like like When flag=2, hour, like like When flag=3, When, if like (4) When flag=4, hour, like like (5) When flag = 2, hour, like like Where s(t) represents the function of displacement with respect to time in velocity planning, T s represents the minimum sampling time of the control system, L represents the total distance of speed planning, J represents jerk, flag represents the classification identifier of path planning, n1-n5 are the serial numbers of discrete sequence points in the acceleration, acceleration / deceleration, constant speed, deceleration, and acceleration / deceleration stages respectively, N0, N1, and N3 are the numbers of discrete points corresponding to T0, T1, and T3 at sampling time Ts respectively, and are always integers.

Images