A motion trajectory control method, system and related components

Abstract

The application discloses a kind of control method of motion trajectory, system and related components, it is related to motion control field, it is applied to the control of motion trajectory of motion equipment, this control method includes: obtaining the instruction parameter of motion trajectory;According to instruction parameter, it is carried out discrete calculation with periodic length as unit, determine the periodic number of each stage in target path length when moving according to S curve;According to all periodic numbers, determine jerk and acceleration change rate;According to jerk and acceleration change rate, determine the instruction acceleration of each cycle corresponding to each stage in target path length;According to instruction acceleration, determine the control instruction of each cycle, to make motion equipment move according to control instruction in each cycle.The control method in the application is carried out discrete calculation and control with each cycle to realize flexible motion, so that the stability of mechanical equipment in engineering application is adjustable and controllable, increase the applicable range of mechanical equipment.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of motion control, in particular to a motion trajectory control method and system and related components. BACKGROUND

[0002] Currently, motion control is commonly applied in various numerical control systems, robot control systems and general-purpose motion controllers, and a method of asymmetric S-curve acceleration and deceleration can be used to achieve flexible motion control.

[0003] Although the theory of asymmetric S-curve acceleration and deceleration is relatively mature, in actual application, the trajectory accuracy and axis motion stability of mechanical equipment in operation cannot meet user requirements due to the computing power and positioning accuracy of mechanical equipment.

[0004] Therefore, how to provide a solution to the above technical problems is a problem that needs to be solved by the technical personnel in the field. SUMMARY

[0005] Therefore, the present application aims to provide a motion trajectory control method and system with adjustable precision and motion stability, and related components. The specific solutions are as follows:

[0006] A motion trajectory control method applied to a motion device, comprising:

[0007] Obtaining instruction parameters of the motion trajectory of the motion device, the instruction parameters including cycle length, maximum jerk, maximum acceleration, maximum deceleration, target speed, initial speed, final speed and target path length;

[0008] Discretely calculating according to the instruction parameters in units of cycle length to determine the number of cycles of each stage in the target path length when moving according to an S-curve;

[0009] Determining jerk and acceleration-deceleration according to the number of cycles;

[0010] Determining the instruction acceleration of each cycle corresponding to each stage in the target path length according to the jerk and acceleration-deceleration;

[0011] Determining the control instruction of each cycle according to the instruction acceleration, so that the motion device moves according to the control instruction in each cycle.

[0012] Preferably, the instruction parameters further include position accuracy, and the process of determining jerk and acceleration-deceleration according to the number of cycles further includes:

[0013] calculating stage running lengths corresponding to the period numbers of each of the stages and summing up the stage running lengths to obtain a total running length;

[0014] calculating an error between the total running length and the target path length;

[0015] judging whether an absolute value of the error exceeds the position accuracy;

[0016] if yes, updating the jerk and the surge according to the stage running lengths, the period numbers and the instruction parameters.

[0017] Preferably, the target path length comprises a jerk stage, a uniform acceleration stage, a surge stage, a uniform speed stage, a surge stage, a uniform deceleration stage and a jerk stage in sequence when the S-curve motion is performed, the period numbers corresponding to the stages are z1, z2, z1, z5, z3, z4 and z3 in sequence, and the process of updating the jerk and the surge according to the stage running lengths, the period numbers and the instruction parameters comprises:

[0018] the jerk and the surge are updated by the following formulae:

[0019]

[0020] wherein ja is the jerk, dsa is a sum of the stage running lengths of the jerk stage, the uniform acceleration stage and the surge stage, rs is the error, L is the target path length, and ts is the period length;

[0021]

[0022] jd is the surge, and dsd is a sum of the stage running lengths of the surge stage, the uniform deceleration stage and the jerk stage.

[0023] Preferably, the process of determining the period numbers of each of the stages in the target path length when the S-curve motion is performed according to the instruction parameters and in the unit of the period length comprises:

[0024] determining the period numbers of each of the stages in the target path length when the S-curve motion is performed according to the instruction parameters and in the unit of the period length;

[0025] calculating stage running lengths corresponding to the period numbers of each of the stages and summing up the stage running lengths to obtain a total running length;

[0026] judging whether the total running length is less than a maximum allowable distance, the maximum allowable distance being a sum of the target path length and the position accuracy;

[0027] If yes, the target speed is reduced, and the step of calculating the number of periods of each stage in the target path length when moving according to the S-curve is re-executed until the total running length is less than the maximum allowed distance.

[0028] Preferably, each stage in the target path length when moving according to the S-curve includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration stage, a uniform deceleration stage, and a deceleration stage, and the number of periods corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3, respectively. The process of determining the command acceleration of each period corresponding to each stage in the target path length according to the jerk and the acceleration includes:

[0029] initializing the command acceleration to 0;

[0030] calculating the command acceleration of the current period according to the following formula:

[0031]

[0032] where zth is the period sequence number of the current period, and the value range is 0 to 2*z1+z2+z5+2*z3+z4, Acc is the command acceleration of the current period, Accpre is the command acceleration of the previous period, ja is the jerk, ts is the period length, and jd is the acceleration.

[0033] Preferably, after obtaining the command parameters of the motion trajectory, the method further includes:

[0034] determining whether each command parameter exceeds the corresponding performance index range;

[0035] If yes, an alarm message is sent.

[0036] Preferably, after obtaining the command parameters of the motion trajectory, the method further includes:

[0037] determining whether each command parameter exceeds the corresponding performance index range;

[0038] If yes, the command parameter exceeding the corresponding performance index range is updated to the maximum value of the performance index range.

[0039] Correspondingly, the application also discloses a motion trajectory control system applied to a motion device, which comprises:

[0040] An acquisition module is configured to acquire instruction parameters of a motion trajectory by the motion device, the instruction parameters comprising a period length, a maximum jerk, a maximum acceleration, a maximum deceleration, a target speed, a start speed, a final speed, and a target path length.

[0041] A first calculation module is configured to perform discrete calculation according to the instruction parameters and in the period length unit, to determine a period number of each stage in the target path length when moving according to an S curve;

[0042] A second calculation module is configured to determine a jerk and an acceleration-deceleration according to the period number.

[0043] A third calculation module is configured to determine an instruction acceleration of each period corresponding to each stage in the target path length according to the jerk and the acceleration-deceleration.

[0044] An action module is configured to determine a control instruction of each period according to the instruction acceleration, so that the motion device moves according to the control instruction in each period.

[0045] Correspondingly, the application further discloses a control device of a motion trajectory, comprising:

[0046] A memory is configured to store a computer program.

[0047] A processor is configured to execute the computer program to realize the steps of the control method of the motion trajectory according to any one of the above.

[0048] Correspondingly, the application further discloses a readable storage medium, wherein the readable storage medium stores a computer program, and the computer program is executed by a processor to realize the steps of the control method of the motion trajectory according to any one of the above.

[0049] The application discloses a control method of a motion trajectory, which is applied to a motion device and comprises the following steps: obtaining instruction parameters of a motion trajectory of the motion device, wherein the instruction parameters comprise a period length, a jerk, a maximum acceleration, a maximum deceleration, a target speed, a starting speed, a final speed and a target path length; performing discrete calculation according to the instruction parameters and taking the period length as a unit to determine the number of periods of each stage in the target path length when the motion is performed according to an S curve; determining the jerk and the acceleration and deceleration according to the number of periods; determining instruction accelerations of each period corresponding to each stage in the target path length according to the jerk and the acceleration and deceleration; and determining control instructions of each period according to the instruction accelerations, so that the motion device is controlled to move according to the control instructions in each period. In the application, the control method is performed to realize flexible motion through discrete calculation and control in each period, so that the stability of a mechanical device in engineering application is adjustable and controllable, and the applicable range of the mechanical device is increased. BRIEF DESCRIPTION OF DRAWINGS

[0050] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description only constitute a part of the embodiments of the present application, and for those skilled in the art, other drawings can be obtained based on the provided drawings without creative labor.

[0051] Figure 1 A step flow chart of the control method of the motion trajectory in the embodiment of the present application;

[0052] Figure 2 A speed schematic diagram of the motion according to the S curve in the embodiment of the present application;

[0053] Figure 3 A sub-step flow chart of the control method of the motion trajectory in the embodiment of the present application;

[0054] Figure 4 A structure distribution diagram of the control system of the motion trajectory in the embodiment of the present application. DETAILED DESCRIPTION

[0055] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments only constitute a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the present application.

[0056] Although the theory of asymmetric S-curve acceleration and deceleration is relatively mature, in actual application, due to the limitations of the computing power and positioning accuracy of mechanical equipment, whether the trajectory accuracy of mechanical equipment during operation and the smoothness of axis movement can meet user requirements remains an issue that needs to be resolved.

[0057] The control method in this application realizes flexible motion by performing discrete calculation and control in each cycle, so that the stability of mechanical equipment in engineering applications can be adjusted and controlled, thereby increasing the applicable scope of the mechanical equipment.

[0058] The embodiment of the present invention discloses a motion trajectory control method, which is applied to sports equipment. Figure 1 Shown, including:

[0059] S1: Obtain the command parameters of the motion trajectory of the motion device, which include cycle length, maximum jerk, maximum acceleration, maximum deceleration, target speed, starting speed, final speed and target path length;

[0060] It can be understood that the motion equipment here includes various general motion controllers in CNC systems and robot control systems. The control of the motion trajectory of the motion equipment is achieved by the processor that controls the motion equipment. The instruction parameters in step S1 are usually sent to the processor by the host computer or the human-computer interaction interface.

[0061] It is understandable that, given the other instruction parameters are fixed, the numerical change in the cycle duration can reflect the smoothness of the axis's motion during the control process. This cycle duration is actually the unit cycle duration of the interpolation period. A larger cycle duration value results in lower smoothness of the axis's motion, but the amount of data calculation is smaller, making it suitable for working conditions with higher processor loads. A smaller cycle duration value results in higher smoothness of the axis's motion, but the amount of data calculation is larger, making it suitable for working conditions with lower processor loads.

[0062] S2: Based on the instruction parameters, discrete calculation is performed in units of cycle length to determine the number of cycles in each stage of the target path length when moving according to the S curve;

[0063] It is understandable that, referring to Figure 2As shown, each stage in the target path length according to the S-curve motion includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration stage, a uniform deceleration stage, and a deceleration stage, and the number of periods corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3, respectively, where z1, z2, z3, z4, and z5 are all integers not less than 0. In actual calculation, the discrete calculation selected due to the constraints of the period number by each instruction parameter and the fact that the period number must be an integer itself affects the calculation value, and the calculation process is not necessarily implemented according to all stages of the complete S-curve, the first jerk of the acceleration and the second jerk of the deceleration in each stage are not necessarily the maximum jerk in the instruction parameter, and the acceleration and deceleration are not necessarily able to reach the maximum acceleration and deceleration. The actual running parameters need to be calculated and judged whether the constraints are met, and the parameters are corrected when the constraints are not met.

[0064] S3: determining the jerk and the acceleration-deceleration according to the period number;

[0065] According to the description above, due to the limitation that the period number must be an integer, the actual jerk and the acceleration-deceleration are not necessarily the same as the maximum jerk in the instruction parameter, and the period number must be used to inversely calculate the jerk and the acceleration-deceleration to determine the jerk and the acceleration-deceleration under the current period number.

[0066] S4: determining the instruction acceleration of each period corresponding to each stage in the target path length according to the jerk and the acceleration-deceleration;

[0067] It can be understood that the instruction acceleration of each period is still based on the S-curve at this time, and the acceleration of each period is determined according to different stages, and specifically, the instruction acceleration of a new period is obtained by increasing or decreasing the jerk or acceleration-deceleration of a period based on the instruction acceleration of the last period.

[0068] S5: determining the control instruction of each period according to the instruction acceleration, so that the motion device moves according to the control instruction in each period.

[0069] Specifically, the form of the control instruction will be generated according to the control method of the motion device, such as a speed control method, a position control method, or a torque control method. After obtaining the instruction acceleration of step S4, the corresponding instruction speed of each period can be further determined as Vel=Velpre+Acc*ts, and the instruction position of each period can be further determined as Pos=Pospre+Vel*ts, where Acc, Vel, and Pos are the instruction acceleration, the instruction speed, and the instruction position of a certain period, Velpre and Pospre are the instruction speed and the instruction position of the last period of the period, and ts is the period length.

[0070] The application discloses a control method of a motion trajectory, which is applied to a motion device and comprises the following steps: acquiring instruction parameters of a motion trajectory of the motion device, wherein the instruction parameters comprise a period length, a maximum jerk, a maximum acceleration, a maximum deceleration, a target speed, a starting speed, a final speed and a target path length; performing discrete calculation in the unit of the period length according to the instruction parameters, and determining the number of periods of each stage in the target path length when the motion is performed according to an S curve; determining the maximum jerk and the maximum acceleration according to the number of periods; determining instruction accelerations of each period corresponding to each stage in the target path length according to the maximum jerk and the maximum acceleration; and determining control instructions of each period according to the instruction accelerations, so that the motion device is controlled to move according to the control instructions in each period. In the application, the control method is discretely calculated and controlled in each period to realize flexible motion, so that the stability of a mechanical device in engineering application is adjustable and controllable, and the applicable range of the mechanical device is increased.

[0071] The embodiment of the application discloses a specific control method of a motion trajectory, and the embodiment further describes and optimizes the technical scheme compared with the previous embodiment.

[0072] According to the step S1 in the above embodiment, the instruction parameters of the motion trajectory of the motion device are acquired, and the instruction parameters comprise a period length, a maximum jerk, a maximum acceleration, a maximum deceleration, a target speed, a starting speed, a final speed and a target path length.

[0073] It can be understood that the control of the motion trajectory of the motion device is realized by a processor of the motion device, and the instruction parameters in the step S1 are usually sent to the processor by an upper computer or a man-machine interaction interface. These specified parameters can be artificially set or determined by calculating the motion trajectory according to a certain principle, and therefore, it is possible that the specified parameters exceed the performance limit of the motion device.

[0074] Therefore, to avoid the influence of the possible situation on the control method, after the step S1 of acquiring the instruction parameters of the motion trajectory, the method further comprises the following steps: judging whether each instruction parameter exceeds the corresponding performance index range; and if yes, issuing an alarm information.

[0075] Further, to avoid the influence of the possible situation on the control method and improve the efficiency of the control method, the instruction parameters exceeding the index limit can also be directly revalued as the index limit, and therefore, after the step S1 of acquiring the instruction parameters of the motion trajectory, the method further comprises the following steps: judging whether each instruction parameter exceeds the corresponding performance index range; and if yes, updating the instruction parameter exceeding the corresponding performance index range to the maximum value of the performance index range.

[0076] Specifically, the performance index ranges of the instruction parameters are as follows.

[0077] Period duration ts: ts>0;

[0078] Maximum jerk jerk: axis-jerk≥jerk>0, axis-jerk is the axis maximum jerk;

[0079] Maximum acceleration acc: axis-acc≥acc>0, axis-acc is the axis maximum acceleration;

[0080] Maximum deceleration dec: axis-dec≥dec>0, axis-dec is the axis maximum deceleration;

[0081] Target speed vm: axis-speed≥vm>0, axis-speed is the axis maximum speed;

[0082] Initial speed vs: axis-speed≥vs>0;

[0083] Final speed ve: axis-speed≥ve>0;

[0084] Target path length L: L>0;

[0085] Wherein, the axis maximum jerk, the axis maximum acceleration, the axis maximum acceleration, and the axis maximum speed are all greater than zero, and the specific values can be determined according to the performance of the motion device.

[0086] The embodiment of the application discloses a specific motion trajectory control method, and relative to the previous embodiment, the technical scheme is further described and optimized.

[0087] According to step S2 in the above embodiment: according to the instruction parameters, the period duration is discretely calculated to determine the number of periods of each stage according to the target path length when moving according to the S curve;

[0088] Step S3: according to all the period numbers, determine the jerk and the acceleration and deceleration.

[0089] It can be understood that, with reference to Figure 2As shown, each stage in the target path length according to the S-curve motion includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration stage, a uniform deceleration stage, and a deceleration stage, and the corresponding cycle numbers of the stages are z1, z2, z1, z5, z3, z4, and z3, respectively, where z1, z2, z3, z4, and z5 are all integers not less than 0. The initial speed reaches the target speed after experiencing the jerk stage, the uniform acceleration stage, and the deceleration stage. The acceleration of the jerk stage changes from 0 to the maximum acceleration with the first jerk, the acceleration of the uniform acceleration stage is constant, the acceleration of the deceleration stage changes from the maximum acceleration to 0 with the first jerk. For the acceleration, the jerk stage and the acceleration stage are just opposite, so the time consumed by the two stages is the same, and the corresponding cycle numbers are both z1. Similarly, the target speed reaches the final speed after experiencing the acceleration stage, the uniform deceleration stage, and the deceleration stage. The deceleration of the acceleration stage changes from 0 to the maximum deceleration with the second jerk, the deceleration of the uniform deceleration stage is constant, and the deceleration of the deceleration stage changes from the maximum deceleration to 0 again. For the deceleration, the acceleration stage and the deceleration stage are just opposite, so the time consumed by the two stages is the same, and the corresponding cycle numbers are both z3.

[0090] In actual calculation, due to the constraints of the instruction parameters on the cycle number and the influence of the discrete calculation itself on the calculation value, the calculation process may not be implemented according to all stages of the complete S-curve. The first jerk of the acceleration and the second jerk of the deceleration in each stage may not be the maximum jerk in the instruction parameters, and the acceleration and the deceleration may not reach the maximum acceleration and the maximum deceleration. The actual running parameters need to be calculated and judged whether the constraints are met, and the parameters are corrected when the constraints are not met. The specific process is as follows:

[0091] (1) First stage: jerk stage, uniform acceleration stage, and deceleration stage

[0092] The cycle number z01 of the acceleration changing from 0 to the maximum acceleration with the maximum jerk is calculated.

[0093] Where int(x) is the rounding processing of x.

[0094] It is judged whether the speed after completing the jerk stage and the deceleration stage with the cycle number z01 will exceed the target speed, that is, whether is established.

[0095] If yes, the jerk stage and the acceleration stage are sufficient to change from the initial speed to the target speed, and the uniform acceleration stage is not needed, z2=0. Meanwhile, the acceleration of the jerk stage and the acceleration stage does not need to reach the maximum acceleration, and the cycle number z1 of the jerk stage and the acceleration stage is recalculated.

[0096] Let Through discrete integer calculation, we get

[0097] At this time, according to z1, we get the jerk required in step S3 as

[0098] If not, the jerk phase and the acceleration-deceleration phase are not enough to change the initial speed to the target speed, and a uniform acceleration phase is also needed, so let z1=z01, and then add the uniform acceleration phase. The number of periods z2 of the uniform acceleration phase should have the following change relationship: Where jerk*z1*ts is the acceleration at the end of the jerk phase. This acceleration is not equal to acc because the acceleration at the end of the jerk phase cannot reach the exact acc after z1 periods. Through discrete integer calculation, we get

[0099] At this time, according to z1 and z2, we get the jerk required in step S3 as

[0100] (2) Second stage: acceleration-deceleration phase, uniform deceleration phase, and deceleration-deceleration phase:

[0101] The calculation of this section is similar to (1). First, calculate the number of periods z03 of the acceleration-deceleration phase and the deceleration-deceleration phase, during which the acceleration changes from 0 to the maximum deceleration with the maximum jerk:

[0102]

[0103] Determine whether the speed after completing the acceleration-deceleration phase and the deceleration-deceleration phase with the number of periods z03 will exceed the final speed, that is, determine whether is established;

[0104] If yes, the acceleration-deceleration phase and the deceleration-deceleration phase are enough to change from the target speed to the final speed, and a uniform deceleration phase is not needed. z4=0, and the deceleration of the acceleration-deceleration phase and the deceleration-deceleration phase does not need to reach the maximum deceleration. Recalculate the number of periods z3 of the acceleration-deceleration phase and the deceleration-deceleration phase:

[0105] Let Through discrete integer calculation, we get At this time, according to z3, we get the jerk required in step S3 as

[0106] If not, the acceleration-deceleration phase and the deceleration-deceleration phase are not enough to change from the target speed to the final speed, and a uniform deceleration phase is also needed. Therefore, let z3=z03, and then add the uniform deceleration phase. The number of periods z4 of the uniform deceleration phase should have the following change relationship:

[0107]

[0108] Where jerk * z3 * ts is the acceleration at the start of the deceleration-deceleration phase, which is not equal to dec, because the amount of acceleration change after an integer z3 cycles is not exactly dec, which is calculated by discrete rounding At this time, according to z3 and z4, the required acceleration and deceleration of step S3 are obtained by inverse calculation

[0109] (3) Distance verification and uniform speed phase:

[0110] After (1) and (2) are performed, the lengths of the phases of each cycle in (1) and (2) are summed and calculated, where:

[0111] The length of the first phase dsa is obtained by summing (1):

[0112] dsa = vs * (2 * z1 + z2) * ts + 0.5 * jz * z1 * (z1 + z2 + 1) * (2 * z1 + z2 + 1) * ts 3 ;

[0113] The length of the second phase dsd is obtained by summing (2):

[0114] dsd = ve * (2 * z3 + z4 + 1) * ts + 0.5 * jd * z3 * (z3 + z4 + 1) * (2 * z3 + z4 + 1) * ts 3 ;

[0115] It is determined whether the length of the first phase dsa and the length of the second phase dsd are less than the target path length L, that is, whether dsa + dsd < L is established;

[0116] If it is greater, it is proved that the running of the first phase and the second phase exceeds the target path length, and the target speed vm needs to be reduced to recalculate the cycle numbers z1-z4 until dsa + dsd < L. Here, the reduction of the target speed vm can be a gradual reduction of the original vm by a preset step, or a reduction of the original target speed by a specific proportion less than 1, which can be 2 / 3 or other values. The specific reduction method and control value are not limited.

[0117] If it is less, it is proved that the running of the first phase and the second phase does not complete the target path length, and a uniform speed phase is needed. The cycle number z5 of the uniform speed phase should have the following relationship: dsa + dsd + vm * z5 * ts = L; which is calculated by discrete rounding

[0118] At this point, the embodiment has completed the calculation of the cycle numbers of all phases according to the S-curve motion.

[0119] The embodiment of the application discloses a specific motion trajectory control method, and relative to the previous embodiment, the embodiment further describes and optimizes the technical scheme.

[0120] Specifically, the instruction parameter in step S1 further includes a position precision precision, and the performance index range requirement is precision>0. At this time, the performance index range of the target path length L is adjusted to L>precision.

[0121] Then, the process of determining the jerk and the jerk and deceleration according to the cycle number in step S3 further includes:

[0122] The stage running lengths corresponding to the respective cycle numbers are calculated, and the total running length is obtained by summation;

[0123] The error between the total running length and the target path length is calculated;

[0124] It is judged whether the absolute value of the error exceeds the position precision;

[0125] If yes, the jerk and the jerk and deceleration are updated according to the stage running length, the cycle number and the instruction parameter.

[0126] According to the description in the previous embodiment, it is known that when the S-curve motion is performed, the target path length includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, a jerk and deceleration stage, a uniform deceleration stage and a deceleration and deceleration stage, and the cycle numbers corresponding to the respective stages are z1, z2, z1, z5, z3, z4 and z3 in turn. Then, the total running length is dsa+dsd+vm*z5*ts, the error rs between the total running length and the target path length is rs=L-(dsa+dsd+vm*z5*ts), and it is judged whether |rs|>precision is satisfied. If not, it is proved that the cycle numbers z1-z5 and the determined jerk and the jerk and deceleration can complete the target path length under the requirement of the position precision. If yes, the current jerk and the jerk and deceleration do not meet the requirement of the position precision, and therefore the cycle numbers need to be appropriately distributed to the first stage, the uniform speed stage and the second stage according to the unique proportion of the extreme value. Specifically, the process of updating the jerk and the jerk and deceleration according to the stage running length, the cycle number and the instruction parameter includes:

[0127] The jerk and the jerk and deceleration are updated through the following formula:

[0128]

[0129] Wherein, ja is the jerk, dsa is the sum of the stage running lengths of the jerk stage, the uniform acceleration stage and the deceleration stage, rs is the error, L is the target path length, and ts is the cycle time length.

[0130]

[0131] jd is the acceleration and deceleration speed, dsd is the sum of all stage running lengths of the acceleration and deceleration stage, the uniform deceleration stage and the deceleration and deceleration stage.

[0132] In addition, due to the presence of the position accuracy, the target path length used for verification can also be replaced by the sum of the target path length and the position accuracy when verifying the distance. At this time, the process of step S2 of determining the number of cycles of each stage in the target path length when moving according to the S-curve according to the instruction parameters in units of cycle length, includes:

[0133] discretely calculating in units of cycle length according to the instruction parameters to determine the number of cycles of each stage in the target path length when moving according to the S-curve;

[0134] calculating the stage running length corresponding to the number of cycles of each stage and summing up to obtain the total running length;

[0135] judging whether the total running length is less than the maximum allowed distance, the maximum allowed distance being the sum of the target path length and the position accuracy;

[0136] If yes, the target speed is reduced, and the step of discretely calculating in units of cycle length according to the instruction parameters to determine the number of cycles of each stage in the target path length when moving according to the S-curve is re-executed until the total running length is less than the maximum allowed distance.

[0137] Specifically, considering the calculation efficiency, the calculation order of step S2 can be as shown in Figure 3 , including:

[0138] S21: calculating z1, z2, ja, dsa, z3, z4, jd, dsd;

[0139] S22: judging whether dsa+dsd>L+precision is satisfied;

[0140] S23: if yes, reducing vm, and returning to S21;

[0141] S24: if no, judging whether dsa+dsd<L-precision is satisfied;

[0142] S25: if yes, calculating z5, rs;

[0143] S26: judging whether |rs|>precision is satisfied;

[0144] S27: if yes, updating ja, jd.

[0145] It can be understood that if dsa+dsdL-precision is not satisfied in step S24, it can be determined that L-precision≤dsa+dsd≤L+precision, the first stage and the second stage can complete the target path length satisfying the position precision, and the conclusion that z5=0 can be directly obtained, and the error rs must satisfy the relationship of |rs|≤precision, and ja and jd do not need to be updated again. Step S26 is the same.

[0146] It can be seen that in the embodiment, the position precision and the cycle length determine the control precision, the operation amount and the motion stability in the control process. The host computer or the man-machine interaction interface can adjust the control effect of the motion trajectory by adjusting the instruction parameters sent to the processor.

[0147] The embodiment of the application discloses a specific motion trajectory control method, and relative to the previous embodiment, the technical solution is further described and optimized.

[0148] According to the above embodiment S4: determining the instruction acceleration of each period corresponding to each stage in the target path length according to the jerk and the jolt;

[0149] Specifically, step S4 can perform iterative operation every period, including:

[0150] The instruction acceleration is initialized to 0;

[0151] The instruction acceleration of the current period is calculated according to the following formula:

[0152]

[0153] Wherein zth is the period sequence number of the current period, the value range is 0 to 2*z1+z2+z5+2*z3+z4, Acc is the instruction acceleration of the current period, Accpre is the instruction acceleration of the previous period, ja is the jerk, ts is the cycle length, and jd is the jolt.

[0154] It can be understood that (0, z1] is the jerk acceleration stage, the instruction acceleration Acc of each period of the stage is the sum of the instruction acceleration Accpre of the previous period and the cycle acceleration, and the cycle acceleration is the product of the jerk ja and the cycle length ts; (z1, z1+z2] is the uniform acceleration stage, the instruction acceleration Acc of each period of the stage remains unchanged and is the same as the instruction acceleration Accpre of the previous period; (z1+z2, 2*z1+z2] is the jolt acceleration stage, the instruction acceleration Acc of each period of the stage is the difference between the instruction acceleration Accpre of the previous period and the cycle acceleration, and the cycle acceleration is the product of the jerk ja and the cycle length ts;

[0155] (2*z1+z2,2*z1+z2+z5] is a uniform speed stage, the instruction acceleration of each period of the stage is 0; theoretically, the instruction acceleration of the last instruction of the acceleration-deceleration stage in the embodiment should be 0, so the same control mode as the uniform acceleration stage can be used, that is, the instruction acceleration Acc of each period of the stage remains unchanged and is the same as the instruction acceleration Accpre of the previous period, as shown in the above formula, but considering that there may be errors in the control of the motion device in the actual running process with the integral period number, therefore, it can be further specified that the instruction acceleration of each period of the uniform speed stage is 0;

[0156] The calculation process of the acceleration-deceleration stage, the uniform deceleration stage and the deceleration-deceleration stage is similar to that of the jerk acceleration stage, the uniform acceleration stage and the deceleration acceleration stage, wherein (2*z1+z2+z5,2*z1+z2+z5+z3] is the acceleration-deceleration stage, the instruction acceleration Acc of each period of the acceleration-deceleration stage is the difference between the instruction acceleration Accpre of the previous period and the period deceleration, and the period deceleration is the product of the acceleration-deceleration jd and the period length ts, (2*z1+z2+z5+z3,2*z1+z2+z5+z3+z4] is the uniform deceleration stage, the instruction acceleration Acc of each period of the uniform deceleration stage is the same as the instruction acceleration Accpre of the previous period, and (2*z1+z2+z5+z3+z4,2*z1+z2+z5+2*z3+z4] is the deceleration-deceleration stage, the instruction acceleration Acc of each period of the deceleration-deceleration stage is the sum of the instruction acceleration Accpre of the previous period and the period deceleration.

[0157] Correspondingly, the application also discloses a control system of a motion trajectory, which is applied to a motion device, and refers to Figure 4 as shown in the figure, comprising:

[0158] An acquisition module 1 is configured to acquire instruction parameters of a motion trajectory by the motion device, wherein the instruction parameters comprise a period length, a maximum jerk, a maximum acceleration, a maximum deceleration, a target speed, a starting speed, a final speed and a target path length;

[0159] A first calculation module 21 is configured to perform discrete calculation in units of the period length according to the instruction parameters, and determine the period number of each stage in the target path length when moving according to an S-curve;

[0160] A second calculation module 22 is configured to determine a jerk and an acceleration-deceleration according to the period number;

[0161] A third calculation module 23 is configured to determine the instruction acceleration of each period corresponding to each stage in the target path length according to the jerk and the acceleration-deceleration;

[0162] An action module 3 is configured to determine a control instruction for each of the periods according to the instruction acceleration, so that the motion device moves according to the control instruction in each of the periods.

[0163] The embodiment of the present application realizes flexible motion by discrete calculation and control in each period, so that the stability of the mechanical device in engineering application is adjustable and controllable, and the application range of the mechanical device is increased.

[0164] In some specific embodiments, the instruction parameters further include a position accuracy, and the second calculation module 22 is configured to determine the jerk and the jerk rate according to the number of periods, and further include:

[0165] The stage running lengths corresponding to the number of periods of each stage are calculated and summed to obtain a total running length;

[0166] An error between the total running length and the target path length is calculated;

[0167] It is judged whether the absolute value of the error exceeds the position accuracy;

[0168] If yes, the jerk and the jerk rate are updated according to the stage running length, the number of periods and the instruction parameters.

[0169] In some specific embodiments, when the first calculation module 21 moves according to the S-curve, the target path length includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, a jerk stage, a uniform deceleration stage and a deceleration stage, and the number of periods corresponding to each stage is z1, z2, z1, z5, z3, z4 and z3 in turn. The process in which the second calculation module 22 updates the jerk and the jerk rate according to the stage running length, the number of periods and the instruction parameters includes:

[0170] The jerk and the jerk rate are updated by the following formula:

[0171]

[0172] Wherein, ja is the jerk, dsa is the sum of the stage running lengths of the jerk stage, the uniform acceleration stage and the deceleration stage, rs is the error, L is the target path length, and ts is the period length.

[0173]

[0174] jd is the jerk rate, and dsd is the sum of the stage running lengths of the jerk stage, the uniform deceleration stage and the deceleration stage.

[0175] In some embodiments, the first calculation module 21 determines the number of periods of each stage in the target path length when moving according to the S-curve based on the instruction parameters and discretely calculates in units of the period length, including:

[0176] discretely calculating in units of the period length based on the instruction parameters to determine the number of periods of each stage in the target path length when moving according to the S-curve;

[0177] calculating the stage running length corresponding to the number of periods of each stage and summing up to obtain the total running length;

[0178] judging whether the total running length is less than the maximum allowed distance, the maximum allowed distance being the sum of the target path length and the position accuracy;

[0179] If yes, the target speed is reduced, and the step of discretely calculating in units of the period length based on the instruction parameters to determine the number of periods of each stage in the target path length when moving according to the S-curve is re-executed until the total running length is less than the maximum allowed distance.

[0180] In some embodiments, each stage in the target path length when moving according to the S-curve includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration stage, a uniform deceleration stage, and a deceleration stage, and the number of periods corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3, respectively. The third calculation module 23 determines the instruction acceleration of each period corresponding to each stage in the target path length based on the jerk and the acceleration, including:

[0181] initializing the instruction acceleration to 0;

[0182] calculating the instruction acceleration of the current period according to the following formula:

[0183]

[0184] where zth is the period number of the current period, the value range being 0 to 2*z1+z2+z5+2*z3+z4, Acc is the instruction acceleration of the current period, Accpre is the instruction acceleration of the previous period, ja is the jerk, ts is the period length, and jd is the acceleration.

[0185] In some embodiments, after the instruction parameters of the motion trajectory are obtained by the obtaining module 1, the process further includes:

[0186] judging whether each instruction parameter exceeds the corresponding performance index range;

[0187] If yes, an alarm information is sent out.

[0188] In some specific embodiments, after the acquisition module 1 acquires the instruction parameters of the motion trajectory, the method further includes:

[0189] determining whether each of the instruction parameters exceeds the corresponding performance index range;

[0190] If yes, the instruction parameter exceeding the corresponding performance index range is updated to the maximum value of the performance index range.

[0191] Correspondingly, the embodiments of the present application further disclose a control device of a motion trajectory, comprising:

[0192] a memory for storing a computer program;

[0193] a processor for executing the computer program to implement the steps of the control method of the motion trajectory according to any one of the above embodiments.

[0194] Correspondingly, the embodiments of the present application further disclose a readable storage medium, wherein the readable storage medium stores a computer program, and the computer program is executed by a processor to implement the steps of the control method of the motion trajectory according to any one of the above embodiments.

[0195] In the embodiments, the details of the control method of the motion trajectory can be referred to the specific description in the above embodiments, and will not be repeated here.

[0196] In the embodiments, the control device of the motion trajectory and the readable storage medium have the same technical effects as the control method of the motion trajectory in the above embodiments, and will not be repeated here.

[0197] Finally, it should be noted that in this document, the relationship terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between the entities or operations. Moreover, the terms "include", "contain" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or device. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0198] The above describes in detail the motion trajectory control method, system and related components provided by the present application. The principles and implementation manners of the present application are described by using specific examples. The above description of the examples is only used to help understand the method of the present application and its core idea. Meanwhile, for those skilled in the art, the specific implementation manners and application ranges can be changed according to the idea of the present application. In summary, the content of the description should not be understood as a limitation of the present application.

Claims

1. A method of controlling a motion trajectory, characterized by, The application is applied to a motion device, comprising: obtaining instruction parameters of a motion trajectory of the motion device, the instruction parameters comprising a period length, a maximum jerk, a maximum acceleration, a maximum deceleration, a target speed, a start speed, a final speed and a target path length; discretely calculating according to the instruction parameters and the period length, determining the number of periods of each stage in the target path length when moving according to an S-curve; determining the jerk and the acceleration and deceleration according to the number of periods; determining the instruction acceleration of each period corresponding to each stage in the target path length according to the jerk and the acceleration and deceleration; determining the control instruction of each period according to the instruction acceleration, so that the motion device moves according to the control instruction in each period; the instruction parameters further comprise a position accuracy, and the process of determining the jerk and the acceleration and deceleration according to the number of periods further comprises: calculating the stage running length corresponding to the number of periods of each stage and summing up to obtain a total running length; calculating the error between the total running length and the target path length; judging whether the absolute value of the error exceeds the position accuracy; if yes, updating the jerk and the acceleration and deceleration according to the stage running length, the number of periods and the instruction parameters; when moving according to the S-curve, each stage in the target path length comprises a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration and deceleration stage, a uniform deceleration stage and a deceleration and deceleration stage, and the number of periods corresponding to each stage is z1, z2, z1, z5, z3, z4 and z3 in turn, and the process of updating the jerk and the acceleration and deceleration according to the stage running length, the number of periods and the instruction parameters comprises: updating the jerk and the acceleration and deceleration by the following formula: ； wherein ja is the jerk, dsa is the sum of all stage running lengths of the jerk stage, the uniform acceleration stage and the deceleration stage, rs is the error, L is the target path length and ts is the period length; ； jd is the acceleration and deceleration, dsd is the sum of all stage running lengths of the acceleration and deceleration stage, the uniform deceleration stage and the deceleration and deceleration stage, vs represents the start speed and ve represents the final speed.

2. The control method according to claim 1, characterized by, the process of discretely calculating according to the instruction parameters and the period length, determining the number of periods of each stage in the target path length when moving according to the S-curve, comprises: discretely calculating according to the instruction parameters and the period length, determining the number of periods of each stage in the target path length when moving according to the S-curve; calculating the stage running length corresponding to the number of periods of each stage and summing up to obtain a total running length; judging whether the total running length is less than a maximum allowable distance, the maximum allowable distance being the sum of the target path length and the position accuracy; If yes, the target speed is reduced, and the step of calculating the number of periods of each stage in the target path length according to the S-curve motion based on the instruction parameters is re-executed until the total running length is less than the maximum allowable distance.

3. The control method according to claim 1, characterized by, According to the S-curve motion, each stage in the target path length includes a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration stage, a uniform deceleration stage, and a deceleration stage, and the corresponding number of periods of each stage is z1, z2, z1, z5, z3, z4, and z3, respectively. The process of determining the instruction acceleration of each period corresponding to each stage in the target path length based on the jerk and the acceleration-deceleration includes: Initializing the instruction acceleration to 0; Calculating the instruction acceleration of the current period according to the following formula: ； wherein is a cycle number of the current cycle, and has a value range of 0 to , is the command acceleration of the current cycle, is the command acceleration of the previous cycle, ja is the jerk, ts is the cycle duration, and jd is the jounce.

4. The control method according to any one of claims 1 to 3, characterized by, After obtaining the instruction parameters of the motion trajectory of the motion device, the method further includes: Determining whether each instruction parameter exceeds the corresponding performance index range; If yes, an alarm message is sent.

5. The control method according to any one of claims 1 to 3, characterized by, After obtaining the instruction parameters of the motion trajectory of the motion device, the method further includes: Determining whether each instruction parameter exceeds the corresponding performance index range; If yes, the instruction parameter exceeding the corresponding performance index range is updated to the maximum value of the performance index range.

6. A control system for a motion trajectory, characterized by The method is applied to a motion device, and includes: An obtaining module is configured to obtain instruction parameters of a motion trajectory of the motion device, the instruction parameters including a period length, a maximum jerk, a maximum acceleration, a maximum deceleration, a target speed, a starting speed, a final speed, and a target path length; A first calculation module is configured to calculate the number of periods of each stage in the target path length according to the S-curve motion based on the instruction parameters in a discrete manner with the period length as a unit; A second calculation module is configured to determine a jerk and an acceleration-deceleration based on the number of periods; A third calculation module is configured to determine an instruction acceleration of each period corresponding to each stage in the target path length based on the jerk and the acceleration-deceleration; An action module is configured to determine a control instruction of each period based on the instruction acceleration, so that the motion device moves according to the control instruction in each period. The instruction parameters further include a position accuracy, and the second calculation module is further configured to: calculate stage running lengths corresponding to the number of periods of each stage and sum the stage running lengths to obtain a total running length; calculate an error between the total running length and the target path length; determine whether an absolute value of the error exceeds the position accuracy; and if yes, update the jerk and the acceleration-deceleration based on the stage running lengths, the number of periods, and the instruction parameters. According to the S-curve motion, each stage in the target path length comprises a jerk stage, a uniform acceleration stage, a deceleration stage, a uniform speed stage, an acceleration stage, a uniform deceleration stage, and a deceleration stage, and the corresponding period numbers of the stages are z1, z2, z1, z5, z3, z4, and z3, respectively. The second calculation module updates the jerk and the acceleration according to the stage running length, the period number, and the instruction parameter, and the process specifically comprises: The jerk and the acceleration are updated by the following formula: ； Wherein, ja is the jerk, dsa is the sum of the stage running lengths of the jerk stage, the uniform acceleration stage, and the deceleration stage, rs is the error, L is the target path length, and ts is the period length. ； jd is the acceleration, dsd is the sum of the stage running lengths of the acceleration stage, the uniform deceleration stage, and the deceleration stage, vs represents the initial speed, and ve represents the final speed.

7. A control device for a motion trajectory, characterized in that It comprises: A memory for storing a computer program; A processor for executing the computer program to implement the steps of the motion trajectory control method according to any one of claims 1 to 5.

8. A readable storage medium, characterized by, The computer program is stored on the readable storage medium, and the computer program is executed by the processor to implement the steps of the motion trajectory control method according to any one of claims 1 to 5.

Images