S-curve Planning Method and System Compatible with Target Position Update during Movement

Abstract

The present invention relates to the technical field of placement machine control, and specifically discloses an S-curve planning method and system compatible with the update of the target position during movement. The method includes the following steps: dividing the curve planning into 7 stages; setting and initializing the motion parameters of the curve planning; calculating the direct deceleration distance in the curve planning according to the motion parameters, and determining whether direct deceleration is possible; calculating the acceleration time and acceleration distance to accelerate to the maximum speed according to the judgment result; calculating the time points of each stage according to the acceleration time and acceleration distance; detecting in real time whether there is a new target position. If not, update the state of each stage of the curve planning according to the time points. If so, return to the initial step to re-plan. The present invention supports curve planning with an initial velocity, is compatible with directly re-planning the position curve after the target position changes during movement, and effectively improves production efficiency.

Description

Technical Field

[0001] The present invention relates to the field of control technology, and particularly to an S-shaped curve planning method and system compatible with updating the target position during motion. Background Art

[0002] At present, in the field of machining and manufacturing, high-frequency point-to-point motion is a relatively common phenomenon. At the same time, due to functional requirements such as calibration and compensation, the target position is often updated after compensation during the motion process. For example, when a pick-and-place machine mounts components, the target position and correction angle are updated after the camera takes a flying shot of the component during the motion process. In some special high-frequency point-to-point motion control fields, such as the motion control of a pick-and-place machine, position correction may be required during the motion process, resulting in a change in the target position during the motion process and the need to update the curve planning. The traditional position planning method generally does not consider the change of the target position during the motion process. It sets a new target position after reaching the original target position and re-performs the position curve planning. However, this method increases the time to reach the new target position and reduces the production efficiency in actual machining. Summary of the Invention

[0003] The purpose of the present invention is to overcome the problems existing in the prior art. The present invention provides an S-shaped curve planning method and system compatible with updating the target position during motion, which is based on trigonometric function operations, supports curve planning with an initial velocity, is compatible with directly re-performing the position curve planning after the target position changes during the motion process, and effectively improves the production efficiency.

[0004] To achieve the above purpose, the first aspect of the present invention provides an S-shaped curve planning method compatible with updating the target position during motion, including the following steps:

[0005] Divide the curve planning into 7 stages: the jerk section of the acceleration section, the uniform acceleration section of the acceleration section, the deceleration section of the acceleration section, the constant velocity section, the jerk section of the deceleration section, the uniform acceleration section of the deceleration section, and the deceleration section of the deceleration section;

[0006] Set and initialize the motion parameters of the curve planning;

[0007] Calculate the direct deceleration distance in the curve planning according to the motion parameters, and determine whether direct deceleration is possible;

[0008] Calculate the acceleration time T acc and the acceleration distance S acc ;

[0009] According to the acceleration time T acc and the acceleration distance S acc Calculate the time points of each stage;

[0010] Detect in real time whether there is a new target position. If not, update the states of each stage of the curve planning according to the time point; if so, return to the initial step for re-planning.

[0011] The second aspect of the present invention provides an S-shaped curve planning system compatible with updating the target position during movement, including:

[0012] A division module for dividing the curve planning into 7 stages: the jerk section of the acceleration section, the uniform acceleration section of the acceleration section, the deceleration section of the acceleration section, the constant speed section, the jerk section of the deceleration section, the uniform acceleration section of the deceleration section, and the deceleration section of the deceleration section;

[0013] A motion parameter module for setting and initializing the motion parameters of the curve planning;

[0014] A judgment module for calculating the direct deceleration distance in the curve planning according to the motion parameters and judging whether direct deceleration is possible;

[0015] A calculation module for calculating the acceleration time T to accelerate to the maximum speed according to the judgment result acc and the acceleration distance S acc ;

[0016] A time point module for calculating the time points of each stage according to the acceleration time T acc and the acceleration distance S acc ;

[0017] A detection and update module for detecting in real time whether there is a new target position. If not, update the states of each stage of the curve planning according to the time point; if so, return to the initial step for re-planning.

[0018] The third aspect of the present invention provides a pick-and-place machine, including

[0019] A memory storing a program;

[0020] A processor for executing the program in the memory to implement the steps of the above S-shaped curve planning method compatible with updating the target position during movement;

[0021] A driver for responding to the curve planning process to implement the actual motion control of the pick-and-place machine.

[0022] Through the above technical solutions, based on trigonometric function operations, through the design strategy of curve planning, it is possible to directly re-plan the position after the target position changes during movement, a method compatible with changing the target position during movement and re-planning the position curve, supporting curve planning with an initial speed, which can greatly improve the operation efficiency, and the discrimination conditions for various situations in the method are clear, the formulas are concise, and the operation complexity is low, and it can be widely applied to positioning motion control. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 It is a schematic diagram of the S-curve planning method and system for updating the target position during movement compatible with the present invention;

[0024] Figure 2 is the curve of the real-time position P of Embodiment 1 of the present invention t ;

[0025] Figure 3 is the curve of the real-time speed v of Embodiment 1 of the present invention t ;

[0026] Figure 4 is the curve of the real-time acceleration a of Embodiment 1 of the present invention t ;

[0027] Figure 5 is the curve of the real-time jerk j of Embodiment 1 of the present invention t ;

[0028] Figure 6 is the curve of the real-time position P of Embodiment 2 of the present invention t ;

[0029] Figure 7 is the curve of the real-time speed v of Embodiment 2 of the present invention t ;

[0030] Figure 8 is the curve of the real-time acceleration a of Embodiment 2 of the present invention t ;

[0031] Figure 9 is the curve of the real-time jerk j of Embodiment 2 of the present invention t ;

[0032] Figure 10 is the curve of the real-time position P of Embodiment 3 of the present invention t ;

[0033] Figure 11 is the curve of the real-time speed v of Embodiment 3 of the present invention t ;

[0034] Figure 12 is the curve of the real-time acceleration a of Embodiment 3 of the present invention t ;

[0035] Figure 13 is the curve of the real-time jerk j of Embodiment 3 of the present invention t ; DETAILED DESCRIPTION OF THE INVENTION

[0036] The following will describe in detail the specific implementation manners of the embodiments of the present invention in conjunction with the accompanying drawings. It should be understood that the specific implementation manners described herein are only used to illustrate and explain the embodiments of the present invention, and are not used to limit the embodiments of the present invention.

[0037] The first aspect of the present invention provides an S-curve planning method compatible with the update of the target position during movement, as Figure 1 shown, including:

[0038] Dividing the curve planning into 7 stages: the jerk stage of the acceleration section, the constant acceleration stage of the acceleration section, the deceleration stage of the acceleration section, the constant speed stage, the jerk stage of the deceleration section, the constant acceleration stage of the deceleration section, and the deceleration stage of the deceleration section;

[0039] Dividing the curve planning into a 7-segment type, and adopting a segmented method to achieve continuous jerk in the non-symmetric trigonometric function 7-segment S-curve planning method with continuous jerk, and a multi-segment curve planning with non-symmetric point-to-point movement of the speed curve.

[0040] Setting and initializing the movement parameters of the curve planning;

[0041] Specifically including: Movement parameter setting: Setting the new target position P e1 , the current position P s , the moving distance S, the maximum speed V m , the maximum speed value V s , the average acceleration A, the average acceleration value A s and the time points of each stage of the curve planning are as follows: the acceleration section T0 - T3, the constant speed section T3 - T4, the deceleration section T4 - T7, the jerk stage of the acceleration section T0 - T1, the constant acceleration stage of the acceleration section T1 - T2, the deceleration stage of the acceleration section T2 - T3, the jerk stage of the deceleration section T4 - T5, the constant acceleration stage of the deceleration section T5 - T6, the deceleration stage of the deceleration section T6 - T7; where when the set displacement is small and the distance during the acceleration and deceleration processes is greater than or equal to the set displacement, there is no constant speed stage, that is, T 3= T4; Setting the softening factor 0 < β ≤ 1, where β is the ratio of the average acceleration of the deceleration section to the acceleration section, and the acceleration coefficient 1 < k ≤ 2, where k is the ratio of the maximum acceleration to the average acceleration within the deceleration section or the acceleration section; The parameter settings during the curve planning calculation are as follows: the current time t, the time interval is dt, the real-time position P t , the real-time movement distance s t , the real-time speed v t , the real-time acceleration a t , the real-time jerk j t ;

[0042] Initializing the movement parameters according to formula (1): (1)

[0043] Among them, t is the current moment, ν t is the real-time speed at moment t, a t is the real-time acceleration at moment t, S t is the real-time moving distance at moment t, P t is the real-time position at moment t, and V0 is the initial speed. Note: S = P e1 -P s > 0 means moving in the positive direction, S = P e1 -P s < 0 means moving in the opposite direction.

[0044] Calculate the distance of direct deceleration in curve planning based on the motion parameters, and determine whether direct deceleration is possible;

[0045] The distance S dec1 of direct deceleration and the deceleration time T dec1 in curve planning are as follows:

[0046] (2),

[0047] Calculate the distance of direct deceleration according to formula (2);

[0048] Determine whether it is possible to directly decelerate to the target position: If S < S dec1 , then the maximum speed V m and the average acceleration A values are taken as their opposites respectively, that is, updated to formula (3), otherwise remain unchanged:

[0049] (3), where V s is the absolute value of the V m value, and A s is the absolute value of the A value.

[0050] Calculate the acceleration time T acc to accelerate to the maximum speed and the acceleration distance S acc based on the judgment result;

[0051] The acceleration time T acc to the maximum speed and the acceleration distance S acc are calculated as follows:

[0052] (4)

[0053] It should be noted that if T acc calculated at this time is too small, it means rapid acceleration changes in a short time, which may cause too large an acceleration impact. An acceleration section adjustment factor α (0 < α ≤ 1) can be introduced. α can be in a functional relationship with T acc . For example, there is a jerk average limit J m ,

[0054] (5)

[0055] When it is the case that (i.e., T acc is too small), then recalculate the acceleration time T acc required to accelerate to the maximum speed and the acceleration distance S acc , the deceleration time T dec required to decelerate from the maximum speed to 0, and the deceleration distance S dec . The formulas are as follows:

[0056] (6).

[0057] Thus, perform acceleration adjustment according to formulas (4) and (5), and calculate the acceleration and deceleration distances and times according to formula (6).

[0058] Calculate the time points of each stage based on the acceleration time T acc and the acceleration distance S acc ;

[0059] Specifically, it includes the following process: Determine whether there is a constant-speed segment, and calculate the constant-speed time T avg and the constant-speed distance S avg :

[0060] If abs(S)>abs(S acc +S dec ), then there is a constant-speed segment, and the formula is as follows:

[0061] (7),

[0062] If abs(S)≤abs(S acc +S dec ), then there is no constant-speed segment, and it is necessary to recalculate the maximum speed V m that can be reached, the acceleration time T acc and the deceleration time T dec , as well as the corresponding acceleration distance S acc and deceleration distance S dec . The formulas are as follows:

[0063] (8).

[0064] Furthermore, when α = 1, it means that there is no need to consider the problem of excessive acceleration impact caused by rapid acceleration changes in a short time, and formula (8) is converted to .

[0065] Based on the acceleration time T acc and the constant-speed time Tavg and deceleration time T dec Calculate the time points of each stage as follows:

[0066] Calculate the acceleration, constant speed, and deceleration times according to formula (6)(7) or formula (8), so three time points can be obtained first:

[0067] T3 = T acc , T4 = T acc + T avg , T7 = T4 + T dec (9)

[0068] To ensure continuous acceleration and average acceleration, introduce the ratio k of the maximum acceleration to the set average acceleration. The role of the k value is to prevent the maximum acceleration corresponding to the set average acceleration from being too large (1 < k ≤ 2). In the case of insufficient hardware driving ability, the k value is preferably set between 1.5 - 2. The entire acceleration section is designed as follows:

[0069] (10)

[0070] Using integral operation of acceleration, the accumulated speed dv of each section from T0 - T acc (T3) can be calculated as:

[0071] :

[0072] :

[0073] :

[0074] To ensure the continuity and accuracy of position planning, it is necessary to ensure that the acceleration reaches the maximum speed V acc = t a1 + t a2 + t a3 at time T3 (T m ), there is

[0075] (11)

[0076] After actually setting the k value, according to formula (11), t a1 , t a2 can be solved, and the relationship between t acc and k and T

[0077] is

[0078] Generally set to be symmetric in the jerk-up phase and the jerk-down phase, that is, the time taken for the jerk-up phase is t a1 = the time taken for the jerk-down phase is t a3 , it can be obtained that:

[0079] (13)

[0080] Finally, the time points T1 and T2 of the acceleration phase can be calculated as follows:

[0081] (14).

[0082] The deceleration phase is also divided into three time points, corresponding to the calculation method of the acceleration phase, and there are:

[0083] (15)

[0084] That is, the time points T5 and T6 of the deceleration section are:

[0085] (16).

[0086] Summarize each time period as:

[0087] (17).

[0088] Calculate the values of each time point of the curve planning according to formula (17). If there is a constant-speed section and the acceleration coefficient k < 2 is set, the curve planning is a normal 7-segment curve planning, including the jerk-up section of the acceleration phase, the constant-acceleration section of the acceleration phase, the jerk-down section of the acceleration phase, the constant-speed section, the jerk-up section of the deceleration phase, the constant-acceleration section of the deceleration phase, and the jerk-down section of the deceleration phase. If there is no constant-speed section, then T3 = T4. If k = 2 is set, then T1 = T2, T4 = T5, and there is no constant-acceleration section. At this time, the entire curve planning becomes a 5-segment type, including the jerk-up section of the acceleration phase, the jerk-down section of the acceleration phase, the constant-speed section, the jerk-up section of the deceleration phase, and the jerk-down section of the deceleration phase. If the softening factor β < 1 is set, the curve planning is realized as an asymmetric curve for acceleration and deceleration; if β = 1 is set, the curve planning is a symmetric curve for acceleration and deceleration at this time. Among them, t a1 = t a3 can be used as a value in a specific implementation manner. When the two are not equal, formula (12) needs to be satisfied. The k value of the acceleration section and the k value of the deceleration section can be equal or not equal. The above discussions are all defaulted to be equal.

[0089] Real-time detect whether there is a new target position. If not, update the state of each stage of the curve planning according to the time point; if so, return to the initial step to re-plan;

[0090] With a time interval of dt, the time t is accumulated from T0 to T7 to detect whether there is a new target position P e1 , if the old target position P e0 is not equal to P e1 , then there is a new target position, return to the initial step, that is, re-divide the curve planning into 7 stages for re-planning, and loop through each step until it satisfies that the old target position P e0 =P e1 ; if P e0 =P e1 , then there is no new target position, and the real-time jerk j t , real-time acceleration α t , real-time velocity ν t are updated according to the following formulas:

[0091] The first segment :

[0092] , (18)

[0093] The second segment :

[0094] , (19)

[0095] The third segment :

[0096] , (20)

[0097] The fourth segment :

[0098] , (21)

[0099] The fifth segment :

[0100] , (22)

[0101] The sixth segment :

[0102] Note: Note: Note: Note: Note: C:\Users\Administrator\AppData\Roaming\Tencent\Users\3365705514\QQ\WinTemp\RichOle\OL}LG(XDK]~DG1]4]NF%4~J.png , (23)

[0103] The seventh segment :

[0104] , (24)

[0105] The real-time movement distance s t and the real-time position P t are updated to:

[0106] Description: Description: Description: Description: Description: Description: C:\Users\Administrator\AppData\Roaming\Tencent\Users\3365705514\QQ\WinTemp\RichOle\U8~8~U_GEH7G8[PV5]$X1AQ.png (25).

[0107] The curve planning of position control can affect the positioning accuracy and the settling time, ensure the continuity of jerk, and at the same time reducing the acceleration value in the deceleration section can reduce the vibration at the end position, thereby improving the positioning accuracy and the performance of the settling time. The method of the present invention calculates the direct deceleration distance through formula (2), judges whether to reverse the acceleration and speed according to the target distance, introduces the acceleration adjustment factor α, and adjusts the acceleration in the acceleration section through formula (4) and formula (5) to avoid hardware failures caused by excessive jerk; judges whether there is a constant-speed section by comparing the total acceleration and deceleration distance with the target distance. If there is a constant-speed section, formula (6) and formula (7) are used to calculate the acceleration and deceleration time and the maximum speed. If there is no constant-speed section, formula (8) is used to calculate the acceleration and deceleration time and the maximum speed, and the final time points are calculated through formula (17); during the time update and the movement process, it is judged whether a new target position is updated. If a new target position is updated, a new curve planning starts from the parameter initialization of formula (1), and the speed, acceleration, and each time point are updated; if the target position is not updated, the jerk, acceleration, speed, and position states are updated in real time through formula (18) - formula (25). The design of the piecewise trigonometric function relationship ensures the continuity of jerk, acceleration, speed, and displacement in the curve planning to meet the set value requirements of the curve planning; through the softening factor β (0 < β ≤ 1) in the deceleration section, an asymmetric curve planning of acceleration and deceleration curves is realized. Among them, when 0 < β < 1, the speed curve is asymmetric, and when β = 1, a special case of symmetric speed curve planning can be realized; by setting the acceleration coefficient k (0 < k ≤ 2), it is avoided that the maximum acceleration exceeds the physical limit in order to ensure the average acceleration value. Among them, when 1 < k < 2, the curve planning is a 7-stage curve planning, and when k = 2, a special case of 5-stage curve planning without a constant acceleration stage can be realized. In summary, the present invention realizes that the position planning can be directly restarted after the target position changes during the movement process through the design strategy of curve planning, and ensures the continuity of jerk and acceleration in the deceleration section. The design of the piecewise trigonometric function relationship ensures the continuity of jerk and acceleration in the deceleration section, and at the same time can be compatible with the position curve planning with an initial speed.

[0108] Based on the above S-shaped curve planning method compatible with the update of the target position during movement, the second aspect of the present invention provides an S-shaped curve planning system compatible with the update of the target position during movement, including:

[0109] A division module for dividing the curve planning into 7 stages: the jerk section in the acceleration section, the constant acceleration section in the acceleration section, the deceleration section in the acceleration section, the constant speed section, the jerk section in the deceleration section, the constant acceleration section in the deceleration section, and the deceleration section in the deceleration section;

[0110] A motion parameter module for setting and initializing the motion parameters of the curve planning;

[0111] A judgment module, configured to calculate the distance of direct deceleration in curve planning according to motion parameters and judge whether direct deceleration is possible;

[0112] A calculation module, configured to calculate the acceleration time T for accelerating to the maximum speed according to the judgment result acc and the acceleration distance S acc ;

[0113] A time point module, configured to calculate the time points of each stage according to the acceleration time T acc and the acceleration distance S acc ;

[0114] A detection and update module, configured to detect in real time whether there is a new target position. If not, update the states of each stage of curve planning according to the time points; if so, return to the initial step to re-plan.

[0115] The third aspect of the present invention provides a mounter, including

[0116] A memory, storing a program;

[0117] A processor, configured to execute the program in the memory to implement the steps of the S-shaped curve planning method for updating the target position in the above compatible motion;

[0118] A driver, configured to respond to the curve planning process to implement the actual motion control of the mounter.

[0119] The method, system and mounter of the present invention support curve planning with an initial speed, and ensure the continuity of the jerk and acceleration in the deceleration section, which can greatly improve the operation efficiency and reduce the in-place impact. And the discrimination conditions for various situations in the solution are clear, the formulas are simple, and the operation complexity is low, which can be widely applied to positioning motion control.

[0120] Embodiment 1:

[0121] Initialize the curve planning parameters. Assume the current position P s = 0m, the new target position P e1 = 0.4m, set the initial speed to V0 = 1m / s, the maximum speed value to V s = 3m / s, the average acceleration value A s = 30m / s 2 , and initialize the motion parameters according to formula (1)

[0122] ,

[0123] The ratio of the average acceleration in the deceleration section to that in the acceleration section is defined as the softening factor, and the ratio of the maximum acceleration to the average acceleration in the deceleration section or the acceleration section is defined as the acceleration coefficient, and the acceleration coefficients in the deceleration section and the acceleration section are equal (the same in the following embodiments). Let the softening factor β = 1 and the acceleration coefficient k = 1.5. That is, at this time, the 7-segment curve is a symmetric curve, and the maximum acceleration is 1.5 times the average acceleration. Calculate the direct deceleration distance S in the curve planning according to the current speed according to formula (2). dec1 = 0.0167m < S. Therefore, the maximum speed and acceleration remain unchanged, and calculate the acceleration time T according to formula (4). acc and the acceleration distance S acc are as follows:

[0124] .

[0125] Judge that α = 1 according to formula (5). Therefore, continue to calculate the deceleration section time T according to formula (6). dec = 0.1s and the deceleration distance S dec = 0.15m. Since abs(S) > abs(S acc + S dec ), it can be seen that there is a constant speed section in the curve planning. Calculate the constant speed distance S avg = 0.1167m and the constant speed time T avg = 0.0389s.

[0126] Then, according to the acceleration time T acc , the constant speed time T avg and the deceleration time T dec , calculate the time points of each stage and the acceleration coefficient k = 1.5, and calculate each time period according to formula (17):

[0127] ,

[0128] Taking dt as the time interval, the time t is accumulated from T0 to T7, and start to update the real-time jerk, real-time acceleration, real-time speed and real-time distance according to formula (18) - formula (25). Detect whether there is a new target position during the update, that is, P e0 ≠ P e1 . Here, it is assumed that at t = 0.1s, the current position is 0.2333m, and a new target position P e1 = 0.5m is received. Then it is necessary to return to the initial parameters and re-plan each stage of the 7-segment type, and there are

[0129] ,

[0130] Keep the softening factor β = 1 and the acceleration coefficient k = 1.5. Calculate the direct deceleration distance S in the curve planning according to the current speed according to formula (2). dec1 = 0.15m < S, so the maximum speed and acceleration remain unchanged. Since the current speed is already the maximum speed, calculate the acceleration time T according to formula (4). acc = 0s and the acceleration distance S acc = 0m. Continue to calculate the deceleration section time T according to formula (6). dec = 0.1s and the deceleration distance S dec = 0.15m. Since abs(S) > abs(S acc + S dec ), it can be seen that there is a constant speed section in the curve planning. Calculate the constant speed distance S according to formula (7). avg = 0.1167m and the time T avg = 0.0389s.

[0131] Then, according to the acceleration time T acc , the constant speed time T avg and the deceleration time T dec calculate the time points of each stage and the acceleration coefficient k = 1.5, and calculate each time period according to formula (17):

[0132] ,

[0133] Taking dt as the time interval, the time t starts to accumulate from T0 to T7 again, and update the real-time jerk, real-time acceleration, real-time speed and real-time position according to formula (18) - formula (25). Assuming that no new target position appears before reaching the target position, the curve planning result of Example 1 is as Figures 2 - 5 shown. The solid line is the state trajectory before the target position is updated, and the dotted line is the new state trajectory after the target position is updated.

[0134] Example 1 realizes a symmetric 7-segment curve planning. Figure 2 For the curve of the real-time position in Example 1, the position is 0.2667m at 0.1s. At this time, the target position is updated to 0.5m and re-planned. Finally, the curve planning reaches the final position of 0.5m; Figure 3 For the real-time speed curve of Example 1, the initial speed is 1m / s, the speed continuously reaches the maximum of 3m / s, and it still needs to run at a constant speed when the position is updated at 0.1s, and finally decelerates to 0m / s; Figure 4 For the real-time acceleration curve of Example 1, the acceleration is continuous and the maximum acceleration is 45m / s 2 which is 1.5 times the average acceleration; Figure 5For the jerk curve of Embodiment 1, the jerk is continuous and the final value is 0. The results of Embodiment 1 show that the method of the present invention can handle the replanning problem of the target position changing to a farther distance. The states in the deceleration section are all continuous, and the in-place vibration can be minimized to the greatest extent.

[0135] Embodiment 2:

[0136] Initialize the curve planning parameters, assuming the current position P s = 0m, the new target position P e1 = 0.4m, set the initial velocity as V0 = -1m / s, the maximum velocity value as V s = 3m / s, the average acceleration value A s = 30m / s 2 , initialize the motion parameters according to formula (1)

[0137] ,

[0138] Let the softening factor β = 0.75 and the acceleration coefficient k = 1.5. That is, at this time, the 7-segment curve is an asymmetric curve, and the acceleration value in the deceleration section is 0.75 times that in the acceleration section, and the maximum acceleration is 1.5 times the average acceleration. Calculate the directly decelerating distance S in the curve planning according to the current velocity according to formula (2) dec1 = -0.0222m < S, so the maximum velocity and acceleration directions remain unchanged. Calculate the acceleration time T acc and the acceleration distance S acc as

[0139] .

[0140] Judge according to formula (5), and then continue to calculate the deceleration section time T dec = 0.1333s and the deceleration distance S dec = 0.2m. Since abs(S) > abs(S acc + S dec ), it can be known that there is a constant velocity section in the curve planning. Calculate the constant velocity distance S avg = 0.0667m and the constant velocity time T avg = 0.0222s.

[0141] Then, according to the acceleration time T acc , the constant velocity time T avg and the deceleration time T dec , calculate the time points of each stage and the acceleration coefficient k = 1.5, and calculate each time period according to formula (17):

[0142] ,

[0143] With a time interval of dt, the time t is accumulated from T0 to T7, and the real-time jerk, real-time acceleration, real-time velocity, and real-time distance are updated starting from formulas (18) to (25). During the update, it is detected whether there is a new target position, i.e., P e0 ≠P e1 , assuming that at t = 0.15 s, the current position is 0.1833 m and a new target position P e1 = 0.5 m is received, then it is necessary to return to the initial parameters and re-plan each stage of the seven-segment type, and there are

[0144] ,

[0145] Keep the softening factor β = 0.75 and the acceleration coefficient k = 1.5. Calculate the direct deceleration distance S in the curve planning according to the current velocity according to formula (2) dec1 = 0.15 m < S, so it is necessary to take the opposite values of the maximum velocity V m and the average acceleration A respectively according to formula (3), that is:

[0146] ,

[0147] Actually calculate the acceleration time T acc = 0.2 s and the acceleration distance S acc = -0.3 m according to formula (4). Continue to calculate the deceleration section time T dec = 0.1333 s and the deceleration distance S dec = -0.2 m according to formula (6). Since abs(S) < abs(S acc + S dec ), it can be seen that there is no constant velocity section in the curve planning, so it is necessary to recalculate the maximum velocity V that can be reached m , the acceleration time T acc and the deceleration time T dec as well as the corresponding acceleration distance S acc and the deceleration distance S dec , and obtain according to formula (8)

[0148] ,

[0149] Then according to the acceleration time T acc , the constant velocity time T avg and the deceleration time T dec calculate the time points of each stage and the acceleration coefficient k = 1.5, and calculate each time period according to formula (17):

[0150] ,

[0151] Taking dt as the time interval, the time t starts to accumulate from T0 to T7 again, and the real-time jerk, real-time acceleration, real-time velocity, and real-time position are updated according to formulas (18) - (25). Assuming that no new target position appears before reaching the target position, the curve planning result of the final Example 2 is as follows Figures 6 - 9 As shown, the solid line is the state trajectory before the target position is updated, and the dotted line is the new state trajectory after the target position is updated.

[0152] Example 2 realizes an asymmetric 7-segment curve planning. Figure 6 For the curve of the real-time position in Example 2, the position is 0.1833m at 0.15s. At this time, the target position is updated to 0m and re-planned. Finally, the curve planning reaches the final position of 0m; Figure 7 For the real-time velocity curve in Example 2, the initial velocity is -1m / s, and the velocity continuously reaches a maximum of 1m / s. When the position is updated at 0.15s, it is necessary to immediately reverse and accelerate to -2.9277m / s; Figure 8 For the real-time acceleration curve in Example 2, the acceleration is continuous and the maximum acceleration is 45m / s 2 which is 1.5 times the average acceleration; Figure 9 For the real-time jerk curve in Example 2, the jerk is continuous and the final value is 0. The result of Example 2 shows that the method of the present invention can handle the re-planning problem of the target position changing to the reverse position. The states in the deceleration section are all continuous, and the in-place vibration can be reduced to the greatest extent.

[0153] Example 3:

[0154] Initialize the curve planning parameters. Assume the current position P s = 0m, the new target position P e1 = 0.1m, set the initial velocity as V0 = 1m / s, the maximum velocity value as V s = 3m / s, the average acceleration value A s = 30m / s 2 , and initialize the motion parameters according to formula (1)

[0155] ,

[0156] Let the softening factor β = 0.75 and the acceleration coefficient k = 1.5. That is, at this time, the 7-segment curve is an asymmetric curve, and the acceleration value in the deceleration section is 0.75 times that in the acceleration section, and the maximum acceleration is 1.5 times the average acceleration. Calculate the directly decelerated distance S in the curve planning according to the current velocity according to formula (2) dec1 = 0m < S, so the maximum velocity and acceleration directions remain unchanged. Calculate the acceleration time and acceleration distance according to formula (4) as

[0157] .

[0158] According to formula (5), it is judged that α = 1. Therefore, continue to calculate the deceleration section time T according to formula (6). dec = 0.1333 s and the acceleration distance S dec = 0.2 m. Since abs(S) < abs(S acc + S dec ), it can be seen that there is no constant speed section in the curve planning, that is, the constant speed distance S avg = 0 m and the constant speed time T avg = 0 s. It is necessary to recalculate the maximum speed V that can be reached m , the acceleration time T acc and the deceleration time T dec as well as the corresponding acceleration distance S acc , the deceleration distance S dec

[0159] Description: Description: Description: Description: Description: C:\Users\Administrator\AppData\Roaming\Tencent\Users\3365705514\QQ\WinTemp\RichOle\GHKFPQ[PW5]V7F(S~APKZ0G.png

[0160] Then, according to the acceleration time T acc , the constant speed time T avg and the deceleration time T dec calculate the time points of each stage and the jerk coefficient k = 1.5, and calculate each time period according to formula (17):

[0161] ,

[0162] Taking dt as the time interval, the time t is accumulated from T0 to T7, and start to update the real-time jerk, real-time acceleration, real-time speed and real-time distance according to formula (18) - formula (25). Detect whether there is a new target position during the update, that is, P e0 ≠ P e1 . Here, it is assumed that at t = 0.1 s, the current position is 0.0968 m and the speed is 0.4335 m / s. When receiving a new target position P e1 = 0.15 m, it is necessary to return to the initial parameters and re-plan each stage of the 7-segment type, and there are

[0163] ,

[0164] At this time, the softening factor β = 0.75, the acceleration coefficient k = 2 is changed, and there is no uniformly decelerated section in the deceleration section. According to formula (2), calculate the direct deceleration distance S in the curve planning based on the current speed dec1 = 0.0042m < S, so the maximum speed V m and the average acceleration A remain unchanged. According to formula (4), actually calculate the time T required to accelerate to the maximum speed acc = 0.2085s and the acceleration distance S acc = 0.1469m. Continue to calculate the deceleration section time T according to formula (6) dec = 0.1333s and the deceleration distance S dec = 0.2m. Since abs(S) < abs(S acc + S dec ), it can be seen that there is no uniform speed section in the curve planning, so it is necessary to recalculate the maximum speed V that can be reached m , the acceleration time T acc and the deceleration time T dec as well as the corresponding acceleration distance S acc , the deceleration distance S dec . According to formula (8), we get

[0165] ,

[0166] Then, according to the acceleration time T acc , the uniform speed time T avg and the deceleration time T dec calculate the time points of each stage and the acceleration coefficient k = 2, and calculate each time period according to formula (17):

[0167] ,

[0168] Taking dt as the time interval, the time t starts to accumulate from T0 to T7 again, and starts to update the jerk, acceleration, speed and position in real time according to formula (18) - formula (25). Assuming that no new target position appears before reaching the target position, the curve planning result of Example 3 is as Figures 10 - 13 shown. The solid line is the state trajectory before the target position is updated, and the dotted line is the new state trajectory after the target position is updated

[0169] Example 3 realizes an asymmetric 7-segment curve planning Figure 10 For the curve of the real-time position in Example 3, the position is 0.0968m at 0.1s. At this time, the target position is updated to 0.15m and then re-planned, and the final curve planning reaches the final position of 0.15m Figure 11For the real-time speed curve of Example 3, the initial speed is 0 m / s, there is no constant-speed section in the first position planning, the maximum speed reaches 1.6036 m / s, and the speed is 0.4335 m / s when the position is updated at 0.1 s. It is necessary to immediately accelerate to 1.2039 m / s and then decelerate to 0 m / s; Figure 12 For the real-time acceleration curve of Example 3, k = 1.5 is set in the first position planning, the acceleration is continuous and the maximum acceleration is 45 m / s 2 which is 1.5 times the average acceleration. After the target position changes, k is changed to k = 2. At this time, there is no constant-acceleration section, the acceleration is continuous and the maximum acceleration is 60 m / s 2 which is 2 times the average acceleration; Figure 13 For the real-time jerk curve of Example 3, the jerk is continuous and the final value is 0. The results of Example 3 show that the method of the present invention can handle the replanning problem of accelerating again when the target position changes to a farther position, and by changing k to adjust the number of segments, the deceleration section states are all continuous, which can minimize the in-place vibration.

[0170] The preferred embodiments of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited thereto. Within the technical concept of the present invention, various simple modifications can be made to the technical solutions of the present invention, including combining various specific technical features in any suitable manner. To avoid unnecessary repetition, the present invention will not separately describe various possible combination methods. But these simple modifications and combinations should also be regarded as the content disclosed by the present invention and fall within the protection scope of the present invention.

Claims

1. An S-curve planning method compatible with target position update during movement, characterized in that, It includes the following steps: Divide the curve planning into 7 stages: the jerk stage of the acceleration section, the constant acceleration stage of the acceleration section, the deceleration stage of the acceleration section, the constant speed stage, the jerk stage of the deceleration section, the constant acceleration stage of the deceleration section, and the deceleration stage of the deceleration section; Set and initialize the motion parameters of the curve planning; Calculate the direct deceleration distance in the curve planning according to the motion parameters, and judge whether direct deceleration is possible; Calculate the acceleration time T to accelerate to the maximum speed based on the judgment result acc and the acceleration distance S acc , set the average jerk J m , introduce the acceleration section adjustment factor α, and there is the following relational expression: When , recalculate the acceleration time T to accelerate to the maximum speed acc and the acceleration distance S acc ; According to the acceleration time T acc and the acceleration distance S acc Calculate the time points of each stage. The time points of each stage are as follows: the jerk section of the acceleration stage T0 - T1, the uniform acceleration section of the acceleration stage T1 - T2, the deceleration section of the acceleration stage T2 - T3, the constant speed section T3 - T4, the jerk section of the deceleration stage T4 - T5, the uniform acceleration section of the deceleration stage T5 - T6, the deceleration section of the deceleration stage T6 - T7; Real-time detection is performed to check if there is a new target position. If not, the states of each stage of the curve planning are updated according to the time point, and the real-time jerk j t 、real-time acceleration α t 、real-time velocity ν t The update formulas are as follows: The first stage \(T_0 \lt t\leq T_1\): The second stage T1 < t ≤ T2: The third stage T2 < t ≤ T3: The fourth stage T3 < t ≤ T4: The fifth stage T4 < t ≤ T5: The sixth stage T5 < t ≤ T6: The seventh stage T6 < t ≤ T7: where t is the current time, A is the average acceleration A, β is the ratio of the average acceleration in the deceleration section to that in the acceleration section and 0 < β ≤ 1, k is the ratio of the maximum acceleration to the average acceleration in the deceleration section or the acceleration section and 1 < k ≤ 2, V0 is the initial velocity, and V m is the maximum velocity; If so, return to the initial step to re-plan.

2. The method according to claim 1, wherein The setting and initializing the motion parameters of the curve planning include: Set a new target position P e1 , the current position P s , the moving distance S, the maximum speed V m , the average acceleration A and the time points of each stage of the curve planning are as follows: the jerk section T0 - T1 of the acceleration section, the uniform acceleration section T1 - T2 of the acceleration section, the deceleration section T2 - T3 of the acceleration section, the constant speed section T3 - T4, the jerk section T4 - T5 of the deceleration section, the uniform acceleration section T5 - T6 of the deceleration section, the deceleration section T6 - T7 of the deceleration section; Initialize motion parameters: Among them, t is the current moment, v t is the real-time speed at moment t, a t is the real-time acceleration at moment t, s t is the real-time moving distance at moment t, P t is the real-time position at moment t, and V0 is the initial speed.

3. The method according to claim 2, wherein The distance S of direct deceleration in curve planning is calculated according to the motion parameters dec1 , and the formula is as follows: Wherein, β is the ratio of the average acceleration in the deceleration stage to that in the acceleration stage, and 0 < β ≤ 1.

4. The method according to claim 3, wherein The judgment on whether direct deceleration is possible is as follows: If S < S dec1 , then the maximum speed V m and the average acceleration A are respectively taken as their opposites; otherwise, they remain unchanged.

5. The method according to claim 4, wherein Calculating the acceleration time T to accelerate to the maximum speed based on the judgment result acc and the acceleration distance S acc , the formula is as follows:

6. The method according to claim 5, characterized in that, According to the acceleration time T acc and the acceleration distance S acc Calculate the time points of each stage, including: Determine whether there is a uniform speed segment, and calculate the uniform speed time T avg and the uniform speed distance S avg : If abs(S) > abs(S acc + S dec ), then there is a uniform speed section, and the formula is as follows: If abs(S) ≤ abs(S acc + S dec ), there is no constant-speed segment, and the formula is as follows: S avg = 0, T avg = 0, and recalculate the maximum achievable speed V m , acceleration time T acc and deceleration time T dec as well as the corresponding acceleration distance S acc , deceleration distance S dec , and the formula is as follows: According to the acceleration time T acc , the constant speed time T avg and the deceleration time T dec Calculate the time points of each stage, and the formula is as follows: Where k is the ratio of the maximum acceleration to the average acceleration in the deceleration section or the acceleration section, and 1 < k ≤ 2.

7. The method according to claim 5 or 6, characterized in that, When recalculate the acceleration time T to reach the maximum speed acc and the acceleration distance S acc , the deceleration time T corresponding to the maximum speed decelerating to 0 dec and the deceleration distance S dec , the formula is as follows:

8. The method according to claim 7, wherein If abs(S) ≤ abs(S acc + S dec ), then the maximum achievable speed V m , acceleration time T acc and deceleration time T dec as well as the corresponding acceleration distance S acc , deceleration distance S dec are as follows:

9. The method according to claim 8, characterized in that, The real-time detection of whether there is a new target position. If not, update the state of each stage of the curve planning according to the time point; if so, the return to the initial step to re-plan specifically is: With dt as the time interval, the time t is accumulated from T0 to T7, and it is detected whether there is a new target position. If there is, return to the initial step to re-plan; The real-time moving distance s t and the real-time position P t are updated to:

10. An S-curve planning system compatible with updating the target position during movement, characterized in that, It includes: A division module for dividing the curve planning into 7 stages: the jerk stage of the acceleration section, the constant acceleration stage of the acceleration section, the deceleration stage of the acceleration section, the constant speed stage, the jerk stage of the deceleration section, the constant acceleration stage of the deceleration section, and the deceleration stage of the deceleration section; A motion parameter module for setting and initializing the motion parameters of the curve planning; A judgment module for calculating the direct deceleration distance in the curve planning according to the motion parameters and judging whether direct deceleration is possible; A calculation module, configured to calculate the acceleration time T to accelerate to the maximum speed according to the judgment result acc and the acceleration distance S acc , set the average jerk J m , introduce an acceleration section adjustment factor α, and there is the following relational expression: When , recalculate the acceleration time T to accelerate to the maximum speed acc and the acceleration distance S acc ; A time point module, configured to calculate time points of each stage according to the acceleration time T acc and the acceleration distance S acc The time points of each stage are calculated as follows: the jerk stage of the acceleration stage is T0-T1, the uniform acceleration stage of the acceleration stage is T1-T2, the deceleration stage of the acceleration stage is T2-T3, the constant speed stage is T3-T4, the jerk stage of the deceleration stage is T4-T5, the uniform acceleration stage of the deceleration stage is T5-T6, and the deceleration stage of the deceleration stage is T6-T7; The detection and update module is used to detect in real time whether there is a new target position. If not, it updates the states of each stage of the curve planning according to the time point, and the jerk j t 、real-time acceleration α t 、real-time speed ν t The update formulas are as follows: The first stage T0 < t ≤ T1: The second stage T1 < t ≤ T2: The third stage T2 < t ≤ T3: The fourth stage T3 < t ≤ T4: The fifth stage T4 < t ≤ T5: The sixth stage T5 < t ≤ T6: The seventh stage T6 < t ≤ T7: where t is the current moment, A is the average acceleration A, β is the ratio of the average acceleration in the deceleration section to that in the acceleration section and 0 < β ≤ 1, k is the ratio of the maximum acceleration to the average acceleration in the deceleration section or the acceleration section and 1 < k ≤ 2, V0 is the initial velocity, and V m is the maximum velocity; If so, return to the initial step to re-plan.

11. A mounter, characterized in that, It includes A memory storing a program; A processor for executing the program in the memory to implement the steps of the method described in any one of claims 1-9; A driver for responding to the curve planning process to implement the actual motion control of the mounter.

Images