CN114995290A - Motion trail control method and system and related components - Google Patents
Motion trail control method and system and related components Download PDFInfo
- Publication number
- CN114995290A CN114995290A CN202210704105.XA CN202210704105A CN114995290A CN 114995290 A CN114995290 A CN 114995290A CN 202210704105 A CN202210704105 A CN 202210704105A CN 114995290 A CN114995290 A CN 114995290A
- Authority
- CN
- China
- Prior art keywords
- acceleration
- stage
- deceleration
- target path
- instruction
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/408—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by data handling or data format, e.g. reading, buffering or conversion of data
- G05B19/4086—Coordinate conversions; Other special calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/35—Nc in input of data, input till input file format
- G05B2219/35356—Data handling
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
Abstract
The application discloses a control method, a system and related components of a motion trail, which relate to the field of motion control and are applied to the control of the motion trail of motion equipment, wherein the control method comprises the following steps: acquiring instruction parameters of a motion track; according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to the S curve is determined; determining the acceleration and the deceleration according to all the periods; determining the command acceleration of each period corresponding to each stage in the length of the target path according to the acceleration and deceleration; and determining a control instruction of each period according to the instruction acceleration so that the motion equipment moves according to the control instruction in each period. According to the control method, the flexible motion is realized by performing discrete calculation and control in each period, so that the stability of the mechanical equipment in engineering application is adjustable and controllable, and the applicable range of the mechanical equipment is enlarged.
Description
Technical Field
The present invention relates to the field of motion control, and in particular, to a method, a system, and a related component for controlling a motion trajectory.
Background
At present, motion control is often applied to various numerical control systems, robot control systems and general motion controllers, and flexible motion control can be realized by adopting an asymmetric S-curve acceleration and deceleration method.
Although the theory of asymmetric S-curve acceleration and deceleration is mature, in the practical application process, the problem still remains to be solved if the trajectory precision and the shaft motion stability performance of the mechanical equipment during operation meet the user requirements, limited by the calculation capability and the positioning precision of the mechanical equipment.
Therefore, how to provide a solution to the above technical problems is a problem to be solved by those skilled in the art.
Disclosure of Invention
In view of the above, the present invention provides a method and a system for controlling a motion trajectory with adjustable precision and adjustable motion stability, and related components. The specific scheme is as follows:
a control method of motion trail is applied to motion equipment and comprises the following steps:
acquiring instruction parameters of the motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, starting speed, final speed and target path length;
according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined;
determining the acceleration and the deceleration according to the periodicity;
determining the command acceleration of each period corresponding to each stage in the target path length according to the accelerated speed and the accelerated speed;
and determining a control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period.
Preferably, the command parameters further include position accuracy, and the process of determining jerk and acceleration/deceleration according to the number of cycles further includes:
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
calculating an error between the total run length and the target path length;
judging whether the absolute value of the error exceeds the position precision;
and if so, updating the acceleration rate and the acceleration and deceleration rate according to the stage operation length, the periodicity and the instruction parameters.
Preferably, each stage in the target path length when moving according to the S-curve includes an acceleration stage, a uniform acceleration stage, an acceleration reduction stage, a uniform speed stage, an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration reduction stage, the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3 in sequence, and the process of updating the acceleration and the acceleration according to the stage operation length, the number of cycles, and the command parameter includes:
updating the jerk and the acceleration-deceleration by the following formulas:
wherein ja is the jerk, dsa is the sum of the operating lengths of all the phases of the jerk phase, the uniform acceleration phase and the jerk phase, rs is the error, L is the target path length, and ts is the cycle duration;
jd is the acceleration and deceleration speed, and dsd is the sum of the running lengths of all the stages in the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage.
Preferably, the step of performing discrete calculation based on the instruction parameter and the cycle duration as a unit to determine the cycle number of each stage in the target path length when moving according to an S-curve includes:
according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined;
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
judging whether the total running length is smaller than a maximum allowable distance, wherein the maximum allowable distance is the sum of the target path length and the position precision;
if so, reducing the target speed, re-executing the discrete calculation by taking the cycle duration as a unit according to the instruction parameters, and determining the cycle number of each stage in the target path length when the target path moves according to the S curve until the total running length is smaller than the maximum allowable distance.
Preferably, each stage in the target path length when moving according to the S-curve includes an acceleration stage, a uniform acceleration stage, an acceleration reduction stage, a uniform velocity stage, an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration reduction stage, the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3 in sequence, and the process of determining the command acceleration for each cycle corresponding to each stage in the target path length according to the acceleration and deceleration includes:
initializing the command acceleration to 0;
calculating the command acceleration of the current cycle according to the following formula:
zth is a cycle sequence number of the current cycle, a value range is 0 to 2 × z1+ z2+ z5+2 × z3+ z4, Acc is the command acceleration of the current cycle, Accpre is the command acceleration of the previous cycle, ja is the jerk, ts is the cycle duration, and jd is the acceleration and deceleration.
Preferably, after the obtaining of the instruction parameter of the motion trajectory, the method further includes:
judging whether each instruction parameter exceeds the corresponding performance index range;
if yes, alarm information is sent out.
Preferably, after the obtaining of the instruction parameter of the motion trajectory, the method further includes:
judging whether each instruction parameter exceeds the corresponding performance index range;
and if so, updating the instruction parameters beyond the corresponding performance index range to the maximum value of the performance index range.
Correspondingly, this application still discloses a control system of motion trail, is applied to the sports equipment, includes:
the acquisition module is used for acquiring instruction parameters of a motion track by the motion equipment, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, starting speed, final speed and target path length;
the first calculation module is used for performing discrete calculation by taking the period duration as a unit according to the instruction parameters and determining the period number of each stage in the target path length when the target path moves according to an S curve;
the second calculation module is used for determining the acceleration and the deceleration according to the periodicity;
a third calculation module, configured to determine, according to the jerk and the acceleration/deceleration, an instruction acceleration corresponding to each period of each stage in the target path length;
and the action module is used for determining the control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period.
Correspondingly, this application still discloses a motion trail's controlling means, includes:
a memory for storing a computer program;
a processor for implementing the steps of the method for controlling a motion trajectory as described in any of the above when executing the computer program.
Accordingly, the present application also discloses a readable storage medium, on which a computer program is stored, which, when being executed by a processor, implements the steps of the method for controlling a motion trajectory according to any one of the above.
The application discloses a control method of a motion trail, which is applied to motion equipment and comprises the following steps: acquiring instruction parameters of a motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, jerk, maximum acceleration, maximum deceleration, target speed, starting speed, final speed and target path length; according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined; determining the acceleration and the deceleration according to the periodicity; determining the command acceleration of each period corresponding to each stage in the target path length according to the acceleration and deceleration; and determining a control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period. According to the control method, the flexible motion is realized by performing discrete calculation and control in each period, so that the stability of the mechanical equipment in engineering application is adjustable and controllable, and the applicable range of the mechanical equipment is enlarged.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
FIG. 1 is a flowchart illustrating steps of a method for controlling a motion trajectory according to an embodiment of the present invention;
FIG. 2 is a schematic representation of the speed of motion according to an S-curve in an embodiment of the present invention;
FIG. 3 is a flow chart illustrating the sub-steps of a method for controlling a motion trajectory according to an embodiment of the present invention;
fig. 4 is a structural distribution diagram of a control system of a motion trajectory according to an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Although the theory of asymmetric S-curve acceleration and deceleration is mature, in the practical application process, the problem still remains to be solved if the calculation capability and the positioning accuracy of the mechanical device are limited, and the trajectory accuracy and the motion stability of the shaft of the mechanical device during operation meet the user requirements.
According to the control method, the flexible motion is realized by performing discrete calculation and control in each period, so that the stability of the mechanical equipment in engineering application is adjustable and controllable, and the applicable range of the mechanical equipment is enlarged.
The embodiment of the invention discloses a control method of a motion trail, which is applied to motion equipment and shown in figure 1, and comprises the following steps:
s1: acquiring instruction parameters of the motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, initial speed, final speed and target path length;
it is understood that the motion device herein includes various general motion controllers in the numerical control system and the robot control system, the control of the motion track of the motion device is implemented by a processor controlling the motion device, and the instruction parameters in step S1 are usually sent to the processor by an upper computer or a human-computer interaction interface.
It can be understood that under the condition that other command parameters are determined, the numerical change of the period duration can reflect the movement stability of the shaft in the control process. The period duration is the unit period duration of the interpolation period, the larger the value of the period duration is, the lower the motion stability of the shaft is, but the data calculation amount is smaller at the moment, and the method is suitable for the working condition with higher processor load; the smaller the value of the period duration is, the higher the motion stability of the shaft is, but the data calculation amount is larger at the moment, so that the method is suitable for the working condition that the processor load is lower.
S2: according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to the S curve is determined;
it can be understood that, as shown in fig. 2, each stage in the target path length when moving according to the S-curve includes an acceleration stage, a uniform acceleration stage, an acceleration reduction stage, a uniform speed stage, an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration reduction stage, and the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3, where z1, z2, z3, z4, and z5 are integers not less than 0. In actual calculation, because of the constraint of each instruction parameter on the number of cycles and the influence of the discrete calculation itself on the calculated value, the calculation process is not necessarily realized according to all stages of a complete S curve, the first jerk and the second jerk of the acceleration and the deceleration in each stage are not necessarily the maximum jerk in the instruction parameters, the acceleration and the deceleration are not necessarily capable of reaching the maximum acceleration and the maximum deceleration, and the parameters in actual operation need to be calculated, judge whether the constraint is met, and correct the parameters when the constraint is not met.
S3: determining the acceleration and the deceleration according to the period number;
according to the above description, since the cycle number must be an integer, the actual jerk and acceleration/deceleration are not necessarily the same as the maximum jerk in the command parameters, and the jerk and acceleration/deceleration must be reversely pushed by the cycle number to determine the jerk and acceleration/deceleration at the current cycle number.
S4: determining the command acceleration of each period corresponding to each stage in the length of the target path according to the acceleration and deceleration;
it can be understood that, at this time, the command acceleration corresponding to each cycle is still based on the S-curve, and the command acceleration of each cycle is determined at different stages, specifically, the command acceleration of the new cycle is obtained by increasing or decreasing the jerk or the acceleration/deceleration of one cycle based on the command acceleration of the previous cycle.
S5: and determining a control instruction of each period according to the instruction acceleration so that the motion equipment moves according to the control instruction in each period.
Specifically, the form of the control command is generated according to the control mode of the motion device, for example, a speed control method, a position control mode, or a torque control mode, after the command acceleration in step S4 is obtained, the command speed Vel ═ Velpre + Acc ═ ts and the command position Pos ═ postre + Vel ×, which correspond to each cycle, may be further determined, where Acc, Vel, and Pos are the command acceleration, the command speed, and the command position of a certain cycle, respectively, Velpre and postre are the command speed and the command position of a previous cycle of the cycle, respectively, and ts is the cycle duration.
The application discloses a control method of a motion trail, which is applied to motion equipment and comprises the following steps: acquiring instruction parameters of a motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, jerk, maximum acceleration, maximum deceleration, target speed, starting speed, final speed and target path length; according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined; determining the acceleration and the deceleration according to the periodicity; determining the command acceleration of each period corresponding to each stage in the target path length according to the accelerated speed and the accelerated speed; and determining a control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period. According to the control method, the flexible motion is realized by performing discrete calculation and control in each period, so that the stability of the mechanical equipment in engineering application is adjustable and controllable, and the applicable range of the mechanical equipment is enlarged.
The embodiment of the invention discloses a specific control method of a motion trail, and compared with the previous embodiment, the embodiment further explains and optimizes the technical scheme.
According to step S1 in the above embodiment: acquiring instruction parameters of the motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, initial speed, final speed and target path length.
It is understood that the control of the motion trajectory of the exercise device is implemented by a processor controlling the exercise device, and the command parameters in step S1 are usually sent to the processor by a host computer or a human-computer interface, and these specified parameters may be manually set or calculated and determined by some principle on the motion trajectory, so that there is a possibility that the performance limit of the exercise device is exceeded.
Therefore, in order to avoid such a possible influence on the control method, after the step S1 obtains the command parameters of the motion trajectory, the method further includes: judging whether each instruction parameter exceeds the corresponding performance index range; if yes, alarm information is sent out.
Further, in order to avoid the possible influence on the control method and improve the efficiency of the control method, the instruction parameter exceeding the index limit may also be directly re-assigned as the index limit, and then after the step S1 obtains the instruction parameter of the motion trajectory, the method further includes: judging whether each instruction parameter exceeds the corresponding performance index range; and if so, updating the instruction parameters beyond the corresponding performance index range to the maximum value of the performance index range.
Specifically, the requirements of the performance index ranges of the instruction parameters are as follows:
the cycle duration ts: ts > 0;
maximum jerk j erk: axis-jerk is more than or equal to jerk >0, and axis-jerk is the maximum acceleration of the shaft;
maximum acceleration acc: axis-acc is more than or equal to acc and more than 0, and axis-acc is the maximum axial acceleration;
maximum deceleration dec: axis-dec is more than or equal to dec >0, axis-dec is the maximum deceleration of the shaft;
target speed vm: the axis-speed is more than or equal to vm and more than 0, and the axis-speed is the maximum speed of the shaft;
initial speed vs: the axis-speed is more than or equal to vs > 0;
the final speed ve: the axis-speed is more than or equal to ve and more than 0;
target path length L: l is more than 0;
the maximum acceleration of the shaft, the maximum acceleration of the shaft and the maximum speed of the shaft are all larger than zero, and specific numerical values can be determined according to the performance of the sports equipment.
The embodiment of the invention discloses a specific control method of a motion trail, and compared with the previous embodiment, the embodiment further explains and optimizes the technical scheme.
According to step S2 in the above embodiment: according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to the S curve is determined;
step S3: based on all the cycle numbers, jerk and acceleration/deceleration are determined.
It can be understood that, referring to fig. 2, each stage in the target path length when moving according to the S-curve includes an acceleration increasing stage, an acceleration homogenizing stage, an acceleration decreasing stage, a uniform speed stage, an acceleration and deceleration stage, a deceleration homogenizing stage, and a deceleration decreasing stage, and the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3 in sequence, where z1, z2, z3, z4, and z5 are integers not less than 0. The initial speed reaches the target speed after undergoing an acceleration stage, a uniform acceleration stage and a deceleration stage, the acceleration of the acceleration stage is changed from 0 to the maximum acceleration with a first acceleration, the acceleration of the uniform acceleration stage is not changed, the acceleration of the deceleration stage is changed from the maximum acceleration to 0 with a first acceleration, for the acceleration, the acceleration stage and the acceleration stage are just opposite, so the time consumption of the acceleration stage and the deceleration stage is equivalent, and the corresponding cycle number is same as z 1; similarly, the target speed reaches the final speed after undergoing an acceleration and deceleration stage, a uniform deceleration stage and a deceleration stage, the deceleration of the acceleration and deceleration stage changes from 0 to the maximum deceleration at the second acceleration, the deceleration of the uniform deceleration stage does not change, and the deceleration of the deceleration stage changes from the maximum deceleration to 0 again, for the deceleration, the acceleration and deceleration stage and the deceleration stage are just opposite, so that the two take time and the corresponding number of cycles is z 3.
In actual calculation, because of the influence of the constraint of each instruction parameter on the cycle number and the discrete calculation on the calculation value, the calculation process is not necessarily realized according to all stages of a complete S curve, the first jerk and the second jerk of the acceleration and the deceleration in each stage are not necessarily the maximum jerk in the instruction parameters, the acceleration and the deceleration are not necessarily capable of reaching the maximum acceleration and the maximum deceleration, the parameters in actual operation need to be calculated while judging whether the constraint is met, and the parameters are corrected when the constraint is not met, and the specific process is as follows:
(1) the first stage is as follows: an acceleration stage, a uniform acceleration stage and an acceleration reduction stage:
the number of cycles z01 for which the acceleration changes from 0 to maximum jerk is calculated:
it is determined whether the speed exceeds the target speed after the acceleration phase and the deceleration phase are completed by the cycle number z01, that is: judgment of
Whether the result is true or not;
if yes, the acceleration stage and the acceleration and deceleration stage are enough to achieve the change from the initial speed to the target speed, the uniform acceleration stage is not needed, z2 is 0, the acceleration of the acceleration stage and the acceleration and deceleration stage is not needed to reach the maximum acceleration, and the cycle number z1 of the acceleration stage and the acceleration and deceleration stage is recalculated:
order to
Obtained by discrete rounding calculation
At this time, the jerk required in step S3 is obtained by reverse-deriving z1 as
If the difference is not satisfied, the acceleration stage and the acceleration stage are not enough to change the initial speed to the target speed, and a uniform acceleration stage is also needed, so that the z1 is made to be z01, and then the uniform acceleration stage is added, and the cycle number z2 of the uniform acceleration stage has the following change relationship:
where jerk z1 ts is the acceleration at the end of the jerk, which is not equivalent to acc, since the acceleration does not reach the exact acc after an integer number z1 cycles in the previous jerk phase; obtained by discrete rounding calculation
The jerk required in step S3 is obtained by reverse extrapolation from z1 and z2 at this time as
(2) And a second stage: acceleration and deceleration stage, uniform deceleration stage and deceleration stage:
this segment is calculated similarly to (1), first calculating the number of cycles z03 for the acceleration to change from 0 to the maximum jerk with the maximum deceleration:
judging whether the speed of the acceleration and deceleration stage and the deceleration and deceleration stage after the cycle number z03 exceeds the final speed, namely: judgment of
Whether the result is true or not;
if yes, the acceleration and deceleration stage and the deceleration reduction stage are enough to complete the change from the target speed to the final speed, the uniform deceleration stage is not needed, z4 is equal to 0, the deceleration of the acceleration and deceleration stage and the deceleration reduction stage does not need to reach the maximum deceleration, and the cycle number z3 of the acceleration and deceleration stage and the deceleration reduction stage is recalculated:
order to
Obtained by discrete rounding calculation
At this time, the acceleration and deceleration required in step S3 is obtained by reverse thrust according to z3
If the speed is not satisfied, the acceleration and deceleration stage and the deceleration stage are not enough to complete the change from the target speed to the final speed, and a uniform deceleration stage is also needed, so that the z3 is made to be z03, and then the uniform deceleration stage is added, and the cycle number z4 of the uniform deceleration stage has the following change relationship:
wherein j erk z3 ts is the acceleration at the beginning of deceleration reduction stage, and the acceleration is not equal to dec, because the acceleration variation after integer z3 cycles is not accurate dec, and is obtained by discrete integer calculation
The acceleration and deceleration required in step S3 is obtained by reverse extrapolation from z3 and z4
(3) Distance verification and uniform speed stage:
after (1) and (2) are performed, summing the phase run lengths of each period in (1) and (2), wherein:
summing (1) yields the run length dsa of the first stage as:
dsa＝vs*(2*z1+z2)*ts+0.5*jz*z1*(z1+z2+1)*(2*z1+z2+1)*ts 3 ；
the sum of (2) gives the second stage runlength dsd as:
dsd＝ve*(2*z3+z4+1)*ts+0.5*jd*z3*(z3+z4+1)*(2*z3+z4+1)*ts 3 ；
judging whether the running length dsa of the first stage and the running length dsd of the second stage are smaller than the target path length L, namely judging whether dsa + dsd is less than L;
if the sum is larger than the preset threshold value, the operation of the first stage and the second stage exceeds the target path length, the target speed vm needs to be reduced, and then the cycle number of z1-z4 needs to be recalculated until dsa + dsd is smaller than L; the target speed vm may be reduced by a preset step, or the original target speed vm may be reduced by a specific ratio smaller than 1, which may be selected from 2/3 or other values, where the specific reduction mode and control value are not limited.
If the number of the cycles z5 in the uniform speed stage is less than the following relation, the fact that the operation of the first stage and the second stage does not complete the target path length is proved, and the uniform speed stage is needed: dsa + dsd + vm z5 ts L; obtained by discrete rounding calculation
To this end, the present embodiment has completed the calculation of the cycle numbers of all the phases when moving according to the S-curve.
The embodiment of the invention discloses a specific control method of a motion trail, and compared with the previous embodiment, the embodiment further explains and optimizes the technical scheme.
Specifically, the instruction parameters in step S1 further include position precision, and the requirement of the performance index range is precision > 0. At this time, the performance index range of the target path length L is adjusted as follows: l > precision.
Step S3 is a process for determining jerk and acceleration/deceleration according to the number of cycles, further comprising:
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the 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 precision;
if so, updating the acceleration and the deceleration according to the stage operation length, the periodicity and the command parameters.
According to the description in the previous embodiment, it is known that each stage in the target path length when moving according to an S-curve includes an acceleration increasing stage, an acceleration homogenizing stage, an acceleration decreasing stage, a constant speed stage, an acceleration and deceleration stage, a deceleration homogenizing stage, and a deceleration decreasing stage, and the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3 in order, so that the total operating length is dsa + dsd + vm z5 ts, and the error rs between the total operating length and the target path length is rs- (L- (dsa + dsd + vm z 5) ts), and it is determined whether | rs | > precision is satisfied, if not, it is proved that the target path length can be completed under the requirement of satisfying the position precision by using the number of cycles of current z1-z5 and the determined acceleration and deceleration, and the current acceleration and deceleration, and the requirement of the position precision are preferably allocated to the first stage according to the only proportion, and if yes, the current acceleration and deceleration are preferably allocated to the first stage The method specifically comprises the following steps of updating the acceleration and the deceleration according to the stage operation length, the periodicity and the instruction parameters, and comprises the following steps:
the jerk and acceleration/deceleration are updated by the following formulas:
wherein ja is the acceleration, dsa is the sum of the running lengths of all stages of the acceleration stage, the uniform acceleration stage and the deceleration stage, rs is the error, L is the target path length, and ts is the cycle duration;
jd is acceleration and deceleration, and dsd is the sum of the running lengths of all the acceleration and deceleration stages, the uniform deceleration stage and the deceleration stage.
In addition, due to the occurrence of the position accuracy, when the distance is verified, the target path length used for verification may be replaced by the sum of the target path length and the position accuracy, and in this case, in step S2, discrete calculation is performed in units of cycle duration according to the command parameter, and a process of determining the number of cycles of each stage in the target path length when moving according to the S-curve includes:
according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to the S curve is determined;
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
judging whether the total running length is smaller than a maximum allowable distance, wherein the maximum allowable distance is the sum of the target path length and the position precision;
if so, reducing the target speed, re-executing the step of performing discrete calculation by taking the period duration as a unit according to the instruction parameters and determining the period number of each stage in the target path length when the target path moves according to the S curve until the total running length is less than the maximum allowable distance.
Specifically, in consideration of the calculation efficiency, the calculation sequence of step S2 may be as shown in fig. 3, and includes:
s21: calculating z1, z2, ja, dsa, z3, z4, jd, dsd;
s22: judging whether dsa + dsd > L + precision is met;
s23: if so, decreasing vm and returning to S21;
s24: if not, judging whether dsa + dsd < L-precision is met;
s25: if yes, calculating z5 and rs;
s26: judging whether rs > precision is satisfied or not;
s27: if yes, updating ja and jd.
It is understood that if dsa + dsd < L-precision is not satisfied in step S24, L-precision ≦ dsa + dsd ≦ L + precision may be determined, the first stage and the second stage may complete the target path length satisfying the position precision, and may directly reach the conclusion that z5 ≦ 0, and the error rs must satisfy the relation of | rs ≦ precision without updating ja and jd. Step S26 is the same.
Therefore, in the embodiment, the position precision and the period duration determine the control precision, the calculation amount and the motion stability in the control process, and the control effect of the motion track can be adjusted by adjusting the instruction parameters sent to the processor by the upper computer or the human-computer interaction interface.
The embodiment of the invention discloses a specific control method of a motion track, and compared with the previous embodiment, the embodiment further explains and optimizes the technical scheme.
According to the above embodiment S4: determining the command acceleration of each period corresponding to each stage in the length of the target path according to the acceleration and deceleration;
specifically, step S4 may perform an iterative operation every cycle, including:
initializing the command acceleration to 0;
calculating the command acceleration of the current period according to the following formula:
zth is the cycle number of the current cycle, the value range is 0-2 x z1+ z2+ z5+2 x z3+ z4, Acc is the command acceleration of the current cycle, Accpre is the command acceleration of the previous cycle, ja is the jerk, ts is the cycle duration, and jd is the acceleration and deceleration.
It can be understood that (0, z 1) is an acceleration stage, the command acceleration Acc of each period of the stage is the sum of the command acceleration Accpre of the previous period and a period acceleration, namely the product of the acceleration ja and a period time ts, (z1, z1+ z 2) is a uniform acceleration stage, the command acceleration Acc of each period of the stage is kept unchanged and is the same as the command acceleration Accpre of the previous period, (z1+ z2,2 x z1+ z 2) is an acceleration and deceleration stage, the command acceleration Acc of each period of the stage is the difference between the command acceleration Accpre of the previous period and the period acceleration, namely the product of the acceleration ja and the period time ts;
(2 × z1+ z2,2 × z1+ z2+ z 5) is a constant speed stage, the command acceleration of each period of the stage is 0, theoretically, the command acceleration at the end of the acceleration and deceleration stage in the present embodiment should be 0, and therefore the same control mode as the uniform acceleration stage can be used, that is, the command acceleration Acc of each period of the stage is kept unchanged and is the same as the command acceleration Accpre of the previous period, as shown in the above formula, but considering that in the actual operation process with the rounded number of periods, the error may exist in the control of the moving equipment, so that the command acceleration of each stage of the constant speed stage can be 0;
the calculation processes of the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage are similar to the acceleration stage, the uniform acceleration stage and the deceleration stage, wherein, (2 × z1+ z2+ z5,2 × z1+ z2+ z5+ z3] is an acceleration-deceleration stage, the commanded acceleration Acc of each cycle of the acceleration-deceleration stage is the difference between the commanded acceleration Accpre of the previous cycle and the cycle deceleration, and the cycle deceleration is the product of the acceleration-deceleration jd and the time duration ts of one cycle, (2 × z1+ z2+ z5+ z3,2 × z1+ z2+ z5+ z3+ z4] is a uniform deceleration stage, the commanded acceleration Acc of each cycle of the uniform deceleration stage is the same as the commanded acceleration Accpre of the previous cycle, (2 × z 53 + z2+ z5+ z3+ z4, and 2 × z1+ z1+ z1] are the deceleration stages, and the deceleration of each cycle of the acceleration-deceleration stage is the acceleration-deceleration stage and the acceleration pre of the previous cycle.
Correspondingly, the present application also discloses a control system of motion trajectory, applied to a motion device, as shown in fig. 4, including:
the acquisition module 1 is used for the motion equipment to acquire instruction parameters of a motion track, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, initial speed, final speed and target path length;
the first calculation module 21 is configured to perform discrete calculation with the cycle duration as a unit according to the instruction parameter, and determine the cycle number of each stage in the target path length when the target path moves according to an S-curve;
a second calculating module 22, configured to determine a jerk and an acceleration/deceleration according to the cycle number;
a third calculating module 23, configured to determine, according to the jerk and the acceleration/deceleration, a command acceleration corresponding to each period of each stage in the target path length;
and the action module 3 is used for determining a control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period.
According to the embodiment of the application, the flexible motion is realized by performing discrete calculation and control in each period, so that the stability of the mechanical equipment in engineering application is adjustable and controllable, and the applicable range of the mechanical equipment is enlarged.
In some specific embodiments, the instruction parameters further include position accuracy, and the determining, by the second calculation module 22, the jerk and the acceleration/deceleration according to the number of cycles further includes:
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
calculating an error between the total run length and the target path length;
judging whether the absolute value of the error exceeds the position precision or not;
and if so, updating the acceleration rate and the acceleration and deceleration rate according to the stage operation length, the periodicity and the instruction parameters.
In some specific embodiments, each stage of the target path length when the first calculating module 21 moves according to the S-curve includes an acceleration stage, an acceleration reduction stage, a constant speed stage, an acceleration and deceleration stage, a deceleration stage, and a deceleration reduction stage, the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3 in sequence, and the updating the acceleration and the deceleration by the second calculating module 22 according to the stage operation length, the number of cycles, and the command parameter includes:
updating the jerk and the acceleration-deceleration by the following formulas:
wherein ja is the jerk, dsa is the sum of the operating lengths of all the phases of the jerk phase, the uniform acceleration phase and the jerk phase, rs is the error, L is the target path length, and ts is the cycle duration;
jd is the acceleration and deceleration, and dsd is the sum of the running lengths of all the stages of the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage.
In some specific embodiments, the step of the first calculating module 21 performing discrete calculation based on the instruction parameter and the cycle duration as a unit to determine the number of cycles of each stage in the target path length when moving according to the S-curve includes:
according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined;
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
judging whether the total running length is smaller than a maximum allowable distance, wherein the maximum allowable distance is the sum of the target path length and the position precision;
if so, reducing the target speed, re-executing the discrete calculation by taking the cycle duration as a unit according to the instruction parameters, and determining the cycle number of each stage in the target path length when the target path moves according to the S curve until the total running length is smaller than the maximum allowable distance.
In some specific embodiments, the steps of the target path length when the first calculation module 21 moves according to the S-curve include an acceleration increasing step, an acceleration smoothing step, an acceleration decreasing step, a constant speed step, an acceleration and deceleration increasing step, a deceleration smoothing step, and a deceleration decreasing step, the number of the periods corresponding to each of the steps is z1, z2, z1, z5, z3, z4, and z3 in sequence, and the step of determining the command acceleration of each period corresponding to each of the steps in the target path length according to the acceleration increasing step and the acceleration decreasing step by the third calculation module 23 includes:
initializing the command acceleration to 0;
calculating the command acceleration of the current cycle according to the following formula:
zth is a cycle sequence number of the current cycle, a value range is 0 to 2 × z1+ z2+ z5+2 × z3+ z4, Acc is the command acceleration of the current cycle, Accpre is the command acceleration of the previous cycle, ja is the jerk, ts is the cycle duration, and jd is the acceleration and deceleration.
In some specific embodiments, after the obtaining module 1 obtains the instruction parameter of the motion trajectory, the method further includes:
judging whether each instruction parameter exceeds the corresponding performance index range;
if yes, alarm information is sent out.
In some specific embodiments, after the obtaining module 1 obtains the instruction parameter of the motion trajectory, the method further includes:
judging whether each instruction parameter exceeds the corresponding performance index range;
and if so, updating the instruction parameters beyond the corresponding performance index range to the maximum value of the performance index range.
Correspondingly, the embodiment of the present application further discloses a control device for motion trajectory, including:
a memory for storing a computer program;
a processor for implementing the steps of the control method of the motion trajectory according to any of the above embodiments when executing the computer program.
Correspondingly, the embodiment of the present application further discloses a readable storage medium, on which a computer program is stored, and the computer program, when executed by a processor, implements the steps of the method for controlling a motion trajectory according to any one of the above embodiments.
The details of the control method of the motion trajectory in this embodiment may refer to the detailed description in the above embodiments, and are not repeated here.
The control device and the readable storage medium for the motion trail in this embodiment have the same technical effects as the control method for the motion trail in the foregoing embodiment, and are not described herein again.
Finally, it should also be noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.
The method, the system and the related components for controlling the motion trajectory provided by the invention are described in detail above, and a specific example is applied in the text to explain the principle and the implementation of the invention, and the description of the above embodiment is only used to help understand the method and the core idea of the invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.
Claims (10)
1. A control method of motion trail is applied to motion equipment and comprises the following steps:
acquiring instruction parameters of a motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, starting speed, final speed and target path length;
according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined;
determining the acceleration and the deceleration according to the periodicity;
determining the command acceleration of each period corresponding to each stage in the target path length according to the accelerated speed and the accelerated speed;
and determining a control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period.
2. The control method of claim 1, wherein the command parameters further include position accuracy, and wherein determining the process of jerk and acceleration/deceleration based on the number of cycles further comprises:
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
calculating an error between the total run length and the target path length;
judging whether the absolute value of the error exceeds the position precision;
and if so, updating the acceleration rate and the acceleration and deceleration rate according to the stage operation length, the periodicity and the instruction parameters.
3. The control method according to claim 2,
when moving according to the S-curve, each stage in the target path length includes an acceleration stage, a uniform acceleration stage, an acceleration reduction stage, a uniform velocity stage, an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration reduction stage, the number of cycles corresponding to each stage is z1, z2, z1, z5, z3, z4, and z3 in sequence, and the process of updating the acceleration and the acceleration according to the stage operation length, the number of cycles, and the command parameter includes:
updating the jerk and the acceleration-deceleration by the following formulas:
wherein ja is the jerk, dsa is the sum of the run lengths of all the phases of the jerk phase, the even acceleration phase and the jerk phase, rs is the error, L is the target path length, and ts is the cycle duration;
jd is the acceleration and deceleration, and dsd is the sum of the running lengths of all the stages of the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage.
4. The control method according to claim 2, wherein the step of determining the number of cycles of each stage in the target path length when moving according to the S-curve by performing the discrete calculation based on the cycle duration as a unit according to the command parameter includes:
according to the instruction parameters, discrete calculation is carried out by taking the period duration as a unit, and the period number of each stage in the target path length when the target path moves according to an S curve is determined;
calculating the stage running lengths of the periods corresponding to the stages and summing the stage running lengths to obtain the total running length;
judging whether the total running length is smaller than a maximum allowable distance, wherein the maximum allowable distance is the sum of the target path length and the position precision;
if so, reducing the target speed, re-executing the discrete calculation by taking the cycle duration as a unit according to the instruction parameters, and determining the cycle number of each stage in the target path length when the target path moves according to the S curve until the total running length is smaller than the maximum allowable distance.
5. The control method according to claim 1, wherein each of the stages in the target path length when moving according to the S-curve includes an acceleration stage, a uniform acceleration stage, an acceleration reduction stage, a uniform velocity stage, an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration reduction stage, the number of the cycles corresponding to each of the stages is z1, z2, z1, z5, z3, z4, and z3 in this order, and the process of determining the command acceleration for each cycle corresponding to each of the stages in the target path length according to the acceleration and deceleration includes:
initializing the command acceleration to 0;
calculating the command acceleration of the current cycle according to the following formula:
zth is a cycle sequence number of the current cycle, a value range is 0 to 2 × z1+ z2+ z5+2 × z3+ z4, Acc is the command acceleration of the current cycle, Accpre is the command acceleration of the previous cycle, ja is the jerk, ts is the cycle duration, and jd is the acceleration and deceleration.
6. The control method according to any one of claims 1 to 5, wherein after acquiring the command parameter of the motion trajectory, the method further comprises:
judging whether each instruction parameter exceeds the corresponding performance index range;
if yes, alarm information is sent out.
7. The control method according to any one of claims 1 to 5, wherein after acquiring the command parameter of the motion trajectory, the method further comprises:
judging whether each instruction parameter exceeds the corresponding performance index range;
and if so, updating the instruction parameters beyond the corresponding performance index range to the maximum value of the performance index range.
8. A control system of motion trail is characterized in that the control system is applied to a motion device and comprises:
the acquisition module is used for acquiring instruction parameters of the motion track of the motion equipment, wherein the instruction parameters comprise cycle duration, maximum acceleration, maximum deceleration, target speed, initial speed, final speed and target path length;
the first calculation module is used for performing discrete calculation by taking the period duration as a unit according to the instruction parameters and determining the period number of each stage in the target path length when the target path moves according to an S curve;
the second calculation module is used for determining the acceleration and the deceleration according to the periodicity;
a third calculation module, configured to determine, according to the jerk and the acceleration/deceleration, an instruction acceleration corresponding to each period of each stage in the target path length;
and the action module is used for determining the control instruction of each period according to the instruction acceleration so as to enable the motion equipment to move according to the control instruction in each period.
9. A control device of a motion trajectory, comprising:
a memory for storing a computer program;
a processor for implementing the steps of the method of controlling a motion profile according to any one of claims 1 to 7 when executing the computer program.
10. A readable storage medium, characterized in that the readable storage medium has stored thereon a computer program which, when being executed by a processor, carries out the steps of the method for controlling a motion trajectory according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210704105.XA CN114995290A (en) | 2022-06-21 | 2022-06-21 | Motion trail control method and system and related components |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210704105.XA CN114995290A (en) | 2022-06-21 | 2022-06-21 | Motion trail control method and system and related components |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114995290A true CN114995290A (en) | 2022-09-02 |
Family
ID=83037850
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210704105.XA Pending CN114995290A (en) | 2022-06-21 | 2022-06-21 | Motion trail control method and system and related components |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114995290A (en) |
-
2022
- 2022-06-21 CN CN202210704105.XA patent/CN114995290A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP1956447B1 (en) | Apparatus and method for proportional-integral-derivative control | |
| JP7183446B2 (en) | Apparatus and method for controlling machine operation, and storage medium | |
| US20200342356A1 (en) | Machine learning device, numerical control system, and machine learning method | |
| WO2005019949A1 (en) | Pid parameter adjustment device | |
| CN111727411B (en) | Equipment motion control method, equipment and storage device | |
| CN114077226B (en) | S-shaped curve speed planning method, control terminal and computer readable storage medium | |
| US20190287007A1 (en) | Machine learning device, servo control device, servo control system, and machine learning method | |
| CN111103792B (en) | Robot control method, device, electronic equipment and readable storage medium | |
| US11087509B2 (en) | Output device, control device, and evaluation function value output method | |
| CN109164759A (en) | Curve interpolating method, equipment and computer readable storage medium | |
| CN107395083A (en) | PMLSM servo-control system Position And Velocity methods of estimation and device | |
| JP5187172B2 (en) | Motor control device, torque ripple correction method thereof, and motor control system | |
| EP0858626A1 (en) | Control by means of a set-point generator | |
| JP7417390B2 (en) | Object moving device, machine tool, information processing device, information processing method, and information processing program | |
| CN114995290A (en) | Motion trail control method and system and related components | |
| CN112865750A (en) | Numerical control system multiplying power change smooth control method and device based on FIR filter | |
| US10684594B2 (en) | Machine learning device, servo motor controller, servo motor control system, and machine learning method | |
| CN113721555B (en) | Method and device for determining target speed of S-shaped speed planning | |
| CN116466650A (en) | Precision compensation method and application of numerical control machine tool | |
| CN107918356A (en) | For the method for crossing quadrant friciton compensation during numerical control tool shaft linkage motion cutting | |
| CN110945423B (en) | Diaphragm control method and device, diaphragm equipment and shooting equipment | |
| CN108847807B (en) | Method, device and equipment for planning motion trail of motor and storage medium | |
| CN112673331A (en) | Synchronization control device, synchronization control system, synchronization control method, and simulation device | |
| CN111487930A (en) | Motion control system, method and device based on symmetric graph replacement technology, machine tool and storage medium | |
| Jahanpour | High speed contouring enhanced with C2 PH quintic spline curves |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |