[go: up one dir, main page]

CN113359620B - Soft limit control method for shaft motion and open type motion controller based on RTX64 - Google Patents

Soft limit control method for shaft motion and open type motion controller based on RTX64 Download PDF

Info

Publication number
CN113359620B
CN113359620B CN202110658647.3A CN202110658647A CN113359620B CN 113359620 B CN113359620 B CN 113359620B CN 202110658647 A CN202110658647 A CN 202110658647A CN 113359620 B CN113359620 B CN 113359620B
Authority
CN
China
Prior art keywords
acceleration
deceleration
cur
section
period
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.)
Active
Application number
CN202110658647.3A
Other languages
Chinese (zh)
Other versions
CN113359620A (en
Inventor
东梁
邝应炜
董志劼
鲁吉志
李松
蔡四雄
向龙
韦傲峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen City Flag Public Intelligence Technology Co ltd
Original Assignee
Shenzhen City Flag Public Intelligence Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen City Flag Public Intelligence Technology Co ltd filed Critical Shenzhen City Flag Public Intelligence Technology Co ltd
Priority to CN202110658647.3A priority Critical patent/CN113359620B/en
Publication of CN113359620A publication Critical patent/CN113359620A/en
Application granted granted Critical
Publication of CN113359620B publication Critical patent/CN113359620B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/416—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 control of velocity, acceleration or deceleration
    • G05B19/4163—Adaptive control of feed or cutting velocity
    • 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/36—Nc in input of data, input key till input tape
    • G05B2219/36521—Select by combination of detected force, acceleration, speed, work rate

Landscapes

  • Engineering & Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Output Control And Ontrol Of Special Type Engine (AREA)

Abstract

The invention discloses a soft limit control method for shaft movement, which comprises the steps of recalculating an accelerated speed value and a maximum accelerated speed in an actual acceleration stage and calculating a period required by the stage when the acceleration stage is carried out; recalculating when entering the deceleration phase; calculating the distance between the current position value reached by the shaft movement and the software limit position; in the motion process of the deceleration and acceleration section, executing corresponding section motion by judging the size of the period of the deceleration and acceleration section; and in the motion process of the uniform velocity stage, calculating the distance between the current position value of each period and the limiting position of the software, and confirming to execute the corresponding stage motion. The invention also discloses an open type motion controller based on RTX 64. Compared with the prior art, the speed of the shaft running to the soft limiting position is just zero, and the phenomenon that the shaft stops suddenly when encountering the limiting position to cause larger impact is avoided.

Description

Soft limit control method for shaft motion and open type motion controller based on RTX64
Technical Field
The invention relates to an axis motion control method, in particular to a soft limit control method for axis motion and an open type motion controller based on RTX 64.
Background
In the process of high-speed processing, because the response speed of the motor cannot realize sudden change, the acceleration and deceleration process is very critical, and the motion is stable in the process and the impact is as small as possible. However, in some devices, the shaft has limited stroke, so that software limit and hard limit are required to be added, the shaft is directly stopped when the shaft touches a machine soft limit and a hardware limit switch, the sudden stop of the shaft causes large impact, the processing precision of the device is affected, and the device is easy to cause fatigue damage under the condition of long-term impact in use.
Disclosure of Invention
The invention aims to provide a soft limit control method for shaft motion and an open type motion controller based on RTX64, and aims to solve the technical problems of stable motion, small impact and realization of accurate control of a shaft.
In order to solve the problems, the invention adopts the following technical scheme: a soft limit control method for shaft movement comprises the following steps:
step S1, setting a motion parameter value, where the motion parameter value includes: positive limit Limit P, negative limit Limit N; initial velocity fsEnd speed feTarget speed Vtarget(ii) a Maximum acceleration A, maximum deceleration D; first jerk J given during acceleration phaseaccSecond jerk J given in deceleration phasedec；
Step S2, issuing instructions of starting movement and movement direction;
step S3, based on S-type algorithm, when the acceleration stage is carried out, recalculating the jerk value J 'in the actual acceleration stage'accAnd a maximum acceleration A' and calculating a period n required for a deceleration section in the acceleration section3；
Plus acceleration period n1And decreasing the period n of the acceleration section3The relation of (A) is as follows: n is3＝n1-1；
Required jerk period n to reach maximum acceleration1Comprises the following steps:
Figure GDA0003467941470000021
the increase of the speed after the acceleration adding section and the acceleration reducing section is as follows: Δ v ═ n1×A；
From the starting speed fsTo the target speed VtargetMaximum allowable velocity increment is Δ vallow＝Vtarget-fsIf Δ vallowIf the acceleration is more than delta v, the speed increment is in an allowable range, the maximum acceleration can be reached, otherwise, the acceleration cannot reach the set maximum acceleration, and the jerk is recalculated;
step S4, recalculating the parameters of the actual deceleration phase, including the second jerk J 'of the deceleration phase, when entering the deceleration phase'decAnd the maximum deceleration D' and calculating the period n of the acceleration and deceleration section5Period n of uniform deceleration section6Period n of deceleration section7；
Step S5, calculating the deceleration distance L 'required by the deceleration stage'decAnd a maximum speed V reached after a deceleration acceleration section in the acceleration sectionmax；
Step S6, calculating the distance LimitLen between the current position value CurPos reached by the shaft motion and the limiting position of the software;
step S7, judging the total distance L 'of the deceleration stage operation'decWhether the distance is less than or equal to the distance LimitLen calculated in the step S6 when L'decIf the value is less than or equal to the LimitLen, the step S16 is executed, otherwise, the step S8 is executed;
step S8, determining the V calculated in step S5maxWhether or not it is less than the target speed VtargetWhen V ismax＜VtargetContinuing the motion of the current segment and proceeding to step S9, otherwise proceeding to step S10 and proceeding to the motion of the deceleration segment;
step S9, comparing the current acceleration AcurAnd the recalculated maximum allowable acceleration A' and based on AcurThe sum A' continues to be added with the acceleration section motion or enters the uniform acceleration section motion, and after the execution is finished, the step S3 is returned, and the next cycle is entered;
step S10, entering the motion of the deceleration and acceleration section;
step S11, in the process of the movement of the deceleration and acceleration section, the period n of the deceleration and acceleration section is judged by3Performing a deceleration section motion or a deceleration section motion; when n is3If it is > 0, the process proceeds to step S12, where n is a decreasing acceleration segment motion3When the motion speed is less than or equal to 0, the step S13 is carried out to carry out uniform-speed section motion;
step S12, when n is3If the acceleration is more than 0, entering into a deceleration and acceleration section to move, and calculating the acceleration A of the current periodcur＝Acur-J'decCurrent cycle speed Vcur＝Vcur+AcurAnd the motion position value CurPos of the current period is CurPos + Vcur、n3＝n3-1, then returning to step S11 to continue judging n3The size of (d);
step S13, entering the uniform-speed section motion;
step S14, in the motion process of the uniform velocity stage, calculating the distance LimitLen between the current position value CurPos of each period and the limiting position of the software, and then judging the deceleration distance L'decConfirming to continue executing the constant-speed stage motion or entering the acceleration and deceleration stage motion according to the distance LimitLen;
is L'decIf the current period is more than the limit Len, the step S15 is entered to continue the uniform-speed stage motion, and the motion position value CurPos of the current period is calculated; then entering the next period cycle, and continuously judging the deceleration distance L'decThe size of the distance LimitLen;
CurPos＝CurPos+Vcur
is L'decLess than or equal to LimitLen, entering step S16, entering the acceleration and deceleration section movement, and judging the period n of the acceleration and deceleration section in the acceleration and deceleration section movement5Whether greater than 0;
when n is5If > 0, the process proceeds to step S17, the acceleration/deceleration section movement is continued, and A is calculated every cycle periodcur＝Acur-J'dec，Vcur＝Vcur+Acur，n5＝n 51, returning to the step S16 to continue judging;
when n is5When the speed is less than or equal to 0, the step S18 is carried out, and the uniform deceleration section is carried out;
step S18, determining the period n of the uniform deceleration section in the uniform deceleration section movement6Whether greater than 0;
when n is6If > 0, the process proceeds to step S20, the step of uniform deceleration is continued, and A is calculated for each cyclecur＝Acur，Vcur＝Vcur+Acur，n6＝n6-1, returning to step S18 to continue judging;
when n is6When the speed is less than or equal to 0, the step S20 is entered, the speed reduction section moves, and the period n of the speed reduction section is judged in the speed reduction section moves7Whether or not it is greater than 0, when n is greater than7If > 0, the process proceeds to step S21, where the deceleration step is continued, and A is calculated for each cyclecur＝Acur+J'dec，Vcur＝Vcur+Acur，n7＝n 71, and returning to the step S20 to continue judging;
when n is7And when the motion is less than or equal to 0, stopping the motion of the current motion planning axis.
Further, step S3 includes: when maximum acceleration is not reached, i.e. Δ vallow≤Δv：
The new first jerk and maximum acceleration are recalculated using the following equations:
Figure GDA0003467941470000041
A′＝n1×J′acc
wherein: plus period of acceleration section
Figure GDA0003467941470000042
Period n of uniform acceleration section20, decreasing the period n of the acceleration section3＝n1–1。
Further, step S3 further includes: when the maximum acceleration can be reached, Δ vallow＞Δv：
Speed increment Δ v of acceleration phase ═ vΔvallow＝n1×A+n2×A；
Let n equal n1+n2，
Figure GDA0003467941470000043
The new maximum acceleration is recalculated using:
Figure GDA0003467941470000044
wherein, adding an acceleration period
Figure GDA0003467941470000045
Period n of uniform acceleration section2＝n–n1Decreasing the period n of the acceleration section3＝n1–1；
And recalculating the new first jerk using:
Figure GDA0003467941470000046
further, step S4 includes: when the maximum deceleration is not reached:
recalculating new second jerk J 'by'decAnd a maximum deceleration D ', wherein the allowable maximum speed reduction amount is Δ v'allow＝Vtarget-fe；
Figure GDA0003467941470000051
D′＝n5×J′dec
Wherein: period of acceleration and deceleration section
Figure GDA0003467941470000052
Period n of uniform deceleration section6Decreasing the period n of the deceleration section as 07＝n5–1。
Further, step S4 further includes:
when maximum deceleration is reached:
the new maximum deceleration is recalculated by,
Figure GDA0003467941470000053
Figure GDA0003467941470000054
wherein: period of acceleration and deceleration section
Figure GDA0003467941470000055
Period n of uniform deceleration section6＝n–n5Period n of deceleration section7＝n5–1；
And recalculating the new second jerk by:
Figure GDA0003467941470000056
further, in step S5, a deceleration distance L 'required for the deceleration stage is calculated'decThe method comprises the following steps: the acceleration/deceleration section period n of the deceleration section is calculated in step S45Period n of uniform deceleration section6And decreasing the period n of the deceleration section7And a recalculated new second jerk J'decAnd a maximum deceleration D ', calculating a deceleration distance L ' required for the deceleration phase 'dec；
First, the acceleration at the end of the acceleration/deceleration section is calculated
Figure GDA0003467941470000057
Speed of rotation
Figure GDA0003467941470000058
Distance traveled
Figure GDA0003467941470000059
Respectively as follows:
Figure GDA00034679414700000510
Figure GDA00034679414700000511
Figure GDA0003467941470000061
secondly, the acceleration at the end of the uniform deceleration section is calculated
Figure GDA0003467941470000062
Speed of rotation
Figure GDA0003467941470000063
Distance traveled
Figure GDA0003467941470000064
Respectively as follows:
Figure GDA0003467941470000065
Figure GDA0003467941470000066
Figure GDA0003467941470000067
third, calculate the acceleration at the end of the deceleration segment
Figure GDA0003467941470000068
Speed of rotation
Figure GDA0003467941470000069
Distance traveled
Figure GDA00034679414700000610
Respectively as follows:
Figure GDA00034679414700000611
Figure GDA00034679414700000612
Figure GDA00034679414700000613
finally, calculating the running deceleration distance in the deceleration stage as follows:
Figure GDA00034679414700000614
further, the maximum speed V reached after the acceleration phase is calculatedmaxThe method is realized by adopting the following formula:
Figure GDA00034679414700000615
wherein, in a certain current period in the acceleration section or the uniform acceleration section, the acceleration is AcurAt a velocity of VcurThe number of cycles of the deceleration acceleration section is n3，AcurAnd VcurInitially 0, and a in the acceleration or uniform acceleration segment in each cyclecurAnd VcurCalculated in step S9.
Further, step S6 includes:
when the direction of motion is positive at this time:
obtaining a current position value CurPos reached by the movement of the shaft, and calculating the distance LimitLen between the current position value CurPos and the positive limit Limit P of the software:
LimitLen＝|CurPos-LimitP|；
wherein CurPos is initially 0 and passes through CurPos + V for each cyclecurCalculating to obtain;
when the direction of motion is negative at this time:
obtaining a current position value CurPos reached by the movement of the shaft, and calculating the distance LimitLen between the current position value CurPos and the negative limit Limit N of the software:
LimitLen＝|CurPos-LimitN|。
further, step S9 includes when A iscur< A', go to step S91 to continue adding the acceleration section movement; when A iscurIf not less than A', the flow proceeds to step S92;
step S91, when AcurIf the acceleration is smaller than A', continuing to add the acceleration segment to move, and calculating the acceleration A of the current periodcurCurrent cycle speed VcurThe moving position value CurPos of the current period returns to the step S3, and enters the next period cycle;
Acur＝Acur+J′acc
Vcur＝Vcur+Acur
CurPos＝CurPos+Vcur
step S92, when AcurNot less than A', entering into uniform acceleration segment to calculate the current period acceleration AcurCurrent cycle speed VcurThe moving position value CurPos of the current period returns to the step S3, and enters the next period cycle;
Acur＝Acur
Vcur＝Vcur+Acur
CurPos＝CurPos+Vcur。
the invention also discloses an RTX 64-based open type motion controller, which comprises a processor and a memory, wherein the memory stores instructions capable of being executed by the processor, and the instructions are executed by the processor so as to enable the processor to execute the soft limit control method for shaft motion.
Compared with the prior art, the invention realizes the accurate control of the shaft by completely planning the motor and controlling the rotating speed of the motor by arranging the acceleration section, the uniform acceleration section, the deceleration section, the uniform speed section, the acceleration and deceleration section, the uniform deceleration section and the deceleration section, and simultaneously adds soft limit judgment conditions in the acceleration section, the uniform acceleration section and the uniform speed section of the shaft speed control, and decelerates before the shaft runs to the limit position, thereby ensuring that the speed of the shaft running to the soft limit position is just reduced to zero, and avoiding larger impact caused by sudden stop when the shaft runs to the limit position. Under the condition of not increasing hardware cost, the safety performance and the processing precision of the equipment are improved, the acceleration is continuously changed in the acceleration and deceleration movement process, the movement is stable, the impact is small, and the problem of fatigue damage to the equipment under the condition that the equipment is impacted for a long time in use is solved. The control method is applied to an RTX 64-based open motion controller to improve the performance of the open motion controller and realize accurate control of the shaft.
Drawings
Fig. 1 is a schematic diagram of an S-shaped velocity profile.
FIG. 2 is a schematic diagram of the discretized acceleration segment of the present invention.
Fig. 3 is a schematic diagram of the discretized deceleration segment of the present invention.
Fig. 4 is a flow chart of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
In the prior art, the complete speed planning in the S-shaped algorithm includes: the acceleration section, the uniform acceleration section, the deceleration section, the uniform speed section, the acceleration and deceleration section, the uniform deceleration section and the deceleration and deceleration section. On the basis of the S-shaped algorithm, soft limit conditions are added in a specific stage of the S-shaped algorithm, and control over an axis is optimized.
The acceleration phase in the present invention comprises: an acceleration adding section, an acceleration homogenizing section and an acceleration reducing section; the uniform speed stage comprises a uniform speed section; the deceleration phase comprises: the speed-reducing section comprises an acceleration and deceleration section, a uniform deceleration section and a deceleration and reduction section.
As shown in fig. 4, the present invention discloses a soft limit control method (control method) for shaft motion, comprising the following steps:
step S1, setting the motion parameter value,the motion parameter values include: positive limit p, negative limit n, (unit is pulse); initial velocity fsEnd speed feTarget speed Vtarget(units are pulses/periods); maximum acceleration A, maximum deceleration D (unit is pulse/period)2) (ii) a First jerk J given during acceleration phaseaccSecond jerk J given in deceleration phasedec(unit is pulse/period)3)。
And step S2, issuing a motion starting command and a motion direction command.
Step S3, based on S-type algorithm, when the acceleration stage is carried out, recalculating the jerk value J 'in the actual acceleration stage'accAnd a maximum acceleration A' and calculating a period n required for a deceleration section in the acceleration section3。
In the present invention, a discretized acceleration phase is used, as shown in FIG. 2, plus an acceleration period n1And decreasing the period n of the acceleration section3The relation of (A) is as follows:
n3＝n1-1 (1)
because the cycle period of the controller must be an integer, in order to ensure that other motion parameters do not exceed a set value, the related period adopts upward rounding.
Required jerk period n to reach maximum acceleration1Comprises the following steps:
Figure GDA0003467941470000091
the increase of the speed after only passing through the acceleration adding section and the acceleration reducing section is as follows:
Δv＝n1×A (3)
from the starting speed fsTo the target speed VtargetMaximum allowable velocity increment is Δ vallow＝Vtarget-fs。
If Δ vallowIf the acceleration is higher than the set maximum acceleration, the acceleration is calculated again.
In the present invention, step S3 further includes: a) a condition that the maximum acceleration value is not reached, b) a condition that the maximum acceleration can be reached.
a) When maximum acceleration is not reached, i.e. Δ vallow≤Δv：
The new first jerk and maximum acceleration are recalculated using the following equations:
Figure GDA0003467941470000092
A′＝n1×J′acc (5)
wherein: plus period of acceleration section
Figure GDA0003467941470000093
Period n of uniform acceleration section20, decreasing the period n of the acceleration section3＝n1–1。
b) When maximum acceleration is reached, i.e. Δ vallow＞Δv：
Speed increment Δ v ═ Δ v in acceleration phaseallow＝n1×A+n2×A。
Let n equal n1+n2Therefore, the following steps are carried out:
Figure GDA0003467941470000101
the new maximum acceleration is recalculated using:
Figure GDA0003467941470000102
wherein, adding an acceleration period
Figure GDA0003467941470000103
Period n of uniform acceleration section2＝n–n1Decreasing the period n of the acceleration section3＝n1–1。
And recalculating the new first jerk using:
Figure GDA0003467941470000104
step S4, recalculating the parameters of the actual deceleration phase, including the second jerk J 'of the deceleration phase, when entering the deceleration phase'decAnd the maximum deceleration D' and calculating the period n of the acceleration and deceleration section5Period n of uniform deceleration section6Period n of deceleration section7。
Step S4 includes: as shown in FIG. 3, the deceleration stage is divided into two stages, and the reduction amount of the allowable maximum speed is Δ v'allow＝Vtarget-fe。
a) When the maximum deceleration is not reached:
the new second jerk and maximum deceleration are recalculated by:
Figure GDA0003467941470000105
D′＝n5×J′dec (10)
wherein: period of acceleration and deceleration section
Figure GDA0003467941470000106
Period n of uniform deceleration section6Decreasing the period n of the deceleration section as 07＝n5–1。
b) When maximum deceleration is reached:
the new maximum deceleration is recalculated by:
Figure GDA0003467941470000111
Figure GDA0003467941470000112
wherein: period of acceleration and deceleration section
Figure GDA0003467941470000113
Period n of uniform deceleration section6＝n–n5Period n of deceleration section7＝n5–1。
And recalculating the new second jerk by:
Figure GDA0003467941470000114
step S5, calculating the deceleration distance L 'required by the deceleration stage'decAnd a maximum speed V reached after a deceleration acceleration section in the acceleration sectionmax。
Calculating the deceleration distance L 'required for the deceleration stage'decThe method comprises the following steps: the acceleration/deceleration section period n of the deceleration section calculated in step S45Period n of uniform deceleration section6And decreasing the period n of the deceleration section7And a recalculated new second jerk J'decAnd a maximum deceleration D ', calculating a deceleration distance L ' required for the deceleration phase 'dec。
First, the acceleration at the end of the acceleration/deceleration section is calculated
Figure GDA0003467941470000115
Speed of rotation
Figure GDA0003467941470000116
Distance traveled
Figure GDA0003467941470000117
Respectively as follows:
Figure GDA0003467941470000118
Figure GDA0003467941470000119
Figure GDA00034679414700001110
secondly, the acceleration at the end of the uniform deceleration section is calculated
Figure GDA00034679414700001111
Speed of rotation
Figure GDA00034679414700001112
Distance traveled
Figure GDA00034679414700001113
Respectively as follows:
Figure GDA00034679414700001114
Figure GDA00034679414700001115
Figure GDA00034679414700001116
third, calculate the acceleration at the end of the deceleration segment
Figure GDA00034679414700001117
Speed of rotation
Figure GDA00034679414700001118
Distance traveled
Figure GDA00034679414700001119
Respectively as follows:
Figure GDA00034679414700001120
Figure GDA0003467941470000121
Figure GDA0003467941470000122
finally, calculating the running deceleration distance in the deceleration stage as follows:
Figure GDA0003467941470000123
calculating the maximum speed V reached after the acceleration phasemaxIn the acceleration or uniform acceleration section, it is necessary to ensure that the current speed does not exceed V after passing through the acceleration reduction sectiontarget。
Assume that in a certain current period in the acceleration or uniform acceleration section, the current acceleration is AcurAt a current speed Vcur(AcurAnd VcurInitially 0, and a in the acceleration or uniform acceleration segment in each cyclecurAnd VcurCalculated in step S91 and step S92)), the number of cycles of the acceleration section is reduced to n3Thus, the maximum speed value V reached after the end of the deceleration sectionmaxComprises the following steps:
Figure GDA0003467941470000124
and step S6, calculating the distance LimitLen between the current position value CurPos reached by the shaft motion and the software limiting position (the soft limiting position is set by the user through industrial control software).
The method comprises the following steps:
a) when the direction of motion is positive at this time:
the current position value CurPos reached by the shaft motion is obtained (CurPos is initially 0, and the passing formula CurPos ═ CurPos + V is obtained in each cycle periodcurCalculated), calculating the distance LimitLen between the current position value CurPos and the positive limit Limit P of the software:
LimitLen＝|CurPos-LimitP| (26)
b) when the direction of motion is negative at this time:
obtaining a current position value CurPos reached by the movement of the shaft, and calculating the distance LimitLen between the current position value CurPos and the negative limit Limit N of the software:
LimitLen＝|CurPos-LimitN| (27)
step S7, judging the total distance L 'of the deceleration stage operation'decWhether the distance is less than or equal to the distance LimitLen calculated in the step S6 when L'decAnd if the value is less than or equal to the limit Len, the step S16 is carried out, otherwise, the step S8 is carried out.
Step S8, determining the V calculated in step S5maxWhether or not it is less than the target speed VtargetWhen V ismax＜VtargetThe motion of the current segment is continued and the process proceeds to step S9, otherwise the process proceeds to step S10 and the motion of the deceleration segment is entered.
Step S9, comparing the current acceleration AcurAnd the magnitude of the recalculated allowable maximum acceleration A '(A' calculated in step S3), and based on AcurThe sum A' continues to be added with the acceleration section motion or enters the uniform acceleration section motion, and after the execution is finished, the step S3 is returned, and the next cycle is entered; when A iscur< A', go to step S91 to continue adding the acceleration section movement; when A iscurIf A 'is equal to or greater than A', the flow proceeds to step S92.
Step S91, when AcurIf the acceleration is smaller than A', continuing to add the acceleration segment to move, and calculating the acceleration A of the current periodcurCurrent cycle speed VcurAnd the moving position value CurPos of the current period returns to the step S3, and enters the next period cycle. (Each motion phase controls the shaft motion by pulsing the drive with each cycle, i.e. the velocity V of the current cyclecurThe moving position value CurPos of the current period is obtained by the position value reached after the shaft moves. )
Acur＝Acur+J'acc
Vcur＝Vcur+Acur
CurPos＝CurPos+Vcur
Step S92, when AcurNot less than A', entering into uniform acceleration segment to calculate the current period acceleration AcurCurrent cycle speed VcurAnd the moving position value CurPos of the current period returns to the step S3, and enters the next period cycle.
Acur＝Acur
Vcur＝Vcur+Acur
CurPos＝CurPos+Vcur
Step S10, the movement enters a deceleration section.
Step S11, in the process of the movement of the deceleration and acceleration section, the period n of the deceleration and acceleration section is judged by3Performing a deceleration section motion or a deceleration section motion; when n is3If it is > 0, the process proceeds to step S12, where n is a decreasing acceleration segment motion3And (5) when the motion speed is less than or equal to 0, entering step S13 and entering the constant-speed section motion.
Step S12, when n is3If the acceleration is more than 0, the movement enters a deceleration and acceleration section, and the acceleration V of the current period is calculatedcur＝Vcur+AcurCurrent cycle speed Vcur＝Vcur+AcurAnd the motion position value CurPos of the current period is CurPos + Vcur、n3＝n3-1, then returning to step S11 to continue judging n3The size of (2).
And step S13, entering constant-speed stage motion.
Step S14, during the motion process at the uniform speed stage, calculating the distance LimitLen between the current position value CurPos in each period and the limit position of the software, the calculation process is the same as that in step S6, and details are not repeated here. Then judging L'decAnd confirming to continue executing the constant-speed stage motion or entering the acceleration and deceleration stage motion according to the size of the Limit Len.
Is L'decIf not, the step S15 is executed to continue the constant speed stage motion, and the motion position value CurPos of the current period is calculated. Then entering the next period for circulation, and continuously judging L'decAnd the size of LimitLen.
CurPos＝CurPos+Vcur
Is L'decLess than or equal to LimitLen, entering step S16, entering the acceleration and deceleration section movement, and judging the period n of the acceleration and deceleration section in the acceleration and deceleration section movement5Whether greater than 0;
when n is5If > 0, the process proceeds to step S17, the acceleration/deceleration section movement is continued, and A is calculated every cycle periodcur＝Acur-J'dec，Vcur＝Vcur+Acur，n5＝n 51, returning to the step S16 to continue judging;
when n is5When the speed is less than or equal to 0, the step S18 is carried out, and the uniform deceleration section is carried out;
step S18, judging n in the uniform deceleration section movement6Whether greater than 0;
when n is6If > 0, the process proceeds to step S19, the step of uniform deceleration is continued, and A is calculated for each cyclecur＝Acur，Vcur＝Vcur+Acur，n6＝n6-1, returning to step S18 to continue the determination.
When n is6When the speed is less than or equal to 0, the step S20 is entered, the motion of the deceleration section is entered, and n is judged in the motion of the deceleration section7Whether or not it is greater than 0, when n is greater than7If > 0, the process proceeds to step S21, where the deceleration step is continued, and A is calculated for each cyclecur＝Acur+J'dec，Vcur＝Vcur+Acur，n7＝n7-1, and returns to step S20 to continue the judgment.
When n is7And when the motion is less than or equal to 0, stopping the motion of the current motion planning axis.
The invention also discloses an open type motion controller based on RTX64, which comprises: the control system comprises a processor and a memory, wherein the memory stores instructions which can be executed by the processor, and the instructions are executed by the processor so as to enable the processor to execute the control method.
The Memory may include, but is not limited to, ROM (Random Access Memory), PROM (Programmable Read-Only Memory), EPROM (Erasable Programmable Read-Only Memory), EEPROM (electrically Erasable Programmable Read-Only Memory), and the like.
The invention judges whether the shaft has enough distance to decelerate to the soft limit position in advance according to the soft limit position, when reaching the deceleration distance, the shaft starts decelerating, so that the speed of the device moving to the limit position is just reduced to 0, and the larger impact caused by sudden stop is avoided.

Claims (10)

1. A soft limit control method for shaft motion is characterized in that: the method comprises the following steps:
step S1, setting a motion parameter value, where the motion parameter value includes: positive limit Limit P, negative limit Limit N; initial velocity fsEnd speed feTarget speed Vtarget(ii) a Maximum acceleration A, maximum deceleration D; first jerk J given during acceleration phaseaccSecond jerk J given in deceleration phasedec；
Step S2, issuing instructions of starting movement and movement direction;
step S3, based on S-type algorithm, when the acceleration stage is carried out, recalculating the jerk value J 'in the actual acceleration stage'accAnd a maximum acceleration A' and calculating a period n required for a deceleration section in the acceleration section3；
Plus acceleration period n1And decreasing the period n of the acceleration section3The relation of (A) is as follows: n is3＝n1-1；
Required jerk period n to reach maximum acceleration1Comprises the following steps:
Figure FDA0003467941460000011
the increase of the speed after the acceleration adding section and the acceleration reducing section is as follows: Δ v ═ n1×A；
From the starting speed fsTo the target speed VtargetMaximum allowable velocity increment is Δ vallow＝Vtarget-fsIf Δ vallowIf the acceleration is more than delta v, the speed increment is in an allowable range, the maximum acceleration can be reached, otherwise, the acceleration cannot reach the set maximum acceleration, and the jerk is recalculated;
step S4, recalculating the parameters of the actual deceleration phase, including the second jerk J 'of the deceleration phase, when entering the deceleration phase'decAnd the maximum deceleration D' and calculating the period n of the acceleration and deceleration section5Period n of uniform deceleration section6Period n of deceleration section7；
Step S5, calculating the deceleration distance L 'required by the deceleration stage'decAnd a maximum speed V reached after a deceleration acceleration section in the acceleration sectionmax；
Step S6, calculating the distance LimitLen between the current position value CurPos reached by the shaft motion and the limiting position of the software;
step S7, judging the total distance L 'of the deceleration stage operation'decWhether the distance is less than or equal to the distance LimitLen calculated in the step S6 when L'decIf the value is less than or equal to the LimitLen, the step S16 is executed, otherwise, the step S8 is executed;
step S8, determining the V calculated in step S5maxWhether or not it is less than the target speed VtargetWhen V ismax＜VtargetContinuing the motion of the current segment and proceeding to step S9, otherwise proceeding to step S10 and proceeding to the motion of the deceleration segment;
step S9, comparing the current acceleration AcurAnd the recalculated maximum allowable acceleration A' and based on AcurThe sum A' continues to be added with the acceleration section motion or enters the uniform acceleration section motion, and after the execution is finished, the step S3 is returned, and the next cycle is entered;
step S10, entering the motion of the deceleration and acceleration section;
step S11, in the process of the movement of the deceleration and acceleration section, the period n of the deceleration and acceleration section is judged by3Performing a deceleration section motion or a deceleration section motion; when n is3If it is > 0, the process proceeds to step S12, where n is a decreasing acceleration segment motion3When the motion speed is less than or equal to 0, the step S13 is carried out to carry out uniform-speed section motion;
step S12,When n is3If the acceleration is more than 0, entering into a deceleration and acceleration section to move, and calculating the acceleration A of the current periodcur＝Acur-J'decCurrent cycle speed Vcur＝Vcur+AcurAnd the motion position value CurPos of the current period is CurPos + Vcur、n3＝n3-1, then returning to step S11 to continue judging n3The size of (d);
step S13, entering the uniform-speed section motion;
step S14, in the motion process of the uniform velocity stage, calculating the distance LimitLen between the current position value CurPos of each period and the limiting position of the software, and then judging the deceleration distance L'decConfirming to continue executing the constant-speed stage motion or entering the acceleration and deceleration stage motion according to the distance LimitLen;
is L'decIf the current period is more than the limit Len, the step S15 is entered to continue the uniform-speed stage motion, and the motion position value CurPos of the current period is calculated; then entering the next period cycle, and continuously judging the deceleration distance L'decThe size of the distance LimitLen;
CurPos＝CurPos+Vcur
is L'decLess than or equal to LimitLen, entering step S16, entering the acceleration and deceleration section movement, and judging the period n of the acceleration and deceleration section in the acceleration and deceleration section movement5Whether greater than 0;
when n is5If > 0, the process proceeds to step S17, the acceleration/deceleration section movement is continued, and A is calculated every cycle periodcur＝Acur-J'dec，Vcur＝Vcur+Acur，n5＝n51, returning to the step S16 to continue judging;
when n is5When the speed is less than or equal to 0, the step S18 is carried out, and the uniform deceleration section is carried out;
step S18, determining the period n of the uniform deceleration section in the uniform deceleration section movement6Whether greater than 0;
when n is6If > 0, the process proceeds to step S20, the step of uniform deceleration is continued, and A is calculated for each cyclecur＝Acur，Vcur＝Vcur+Acur，n6＝n6A value of-1, backGo back to step S18 to continue the determination;
when n is6When the speed is less than or equal to 0, the step S20 is entered, the speed reduction section moves, and the period n of the speed reduction section is judged in the speed reduction section moves7Whether or not it is greater than 0, when n is greater than7If > 0, the process proceeds to step S21, where the deceleration step is continued, and A is calculated for each cyclecur＝Acur+J'dec，Vcur＝Vcur+Acur，n7＝n71, and returning to the step S20 to continue judging;
when n is7And when the motion is less than or equal to 0, stopping the motion of the current motion planning axis.
2. The soft limit control method for shaft motion according to claim 1, characterized in that: step S3 includes: when maximum acceleration is not reached, i.e. Δ vallow≤Δv：
The new first jerk and maximum acceleration are recalculated using the following equations:
Figure FDA0003467941460000031
A′＝n1×J′acc
wherein: plus period of acceleration section
Figure FDA0003467941460000032
Period n of uniform acceleration section20, decreasing the period n of the acceleration section3＝n1–1。
3. The soft limit control method for shaft motion according to claim 2, characterized in that: step S3 further includes: when the maximum acceleration can be reached, Δ vallow＞Δv：
Speed increment Δ v ═ Δ v in acceleration phaseallow＝n1×A+n2×A；
Let n equal n1+n2，
Figure FDA0003467941460000041
The new maximum acceleration is recalculated using:
Figure FDA0003467941460000042
wherein, adding an acceleration period
Figure FDA0003467941460000043
Period n of uniform acceleration section2＝n–n1Decreasing the period n of the acceleration section3＝n1–1；
And recalculating the new first jerk using:
Figure FDA0003467941460000044
4. a soft limit control method for shaft motion according to claim 3, characterized in that: step S4 includes: when the maximum deceleration is not reached:
recalculating new second jerk J 'by'decAnd a maximum deceleration D ', wherein the allowable maximum speed reduction amount is Δ v'allow＝Vtarget-fe；
Figure FDA0003467941460000045
D′＝n5×J′dec
Wherein: period of acceleration and deceleration section
Figure FDA0003467941460000046
Period n of uniform deceleration section6Decreasing the period n of the deceleration section as 07＝n5–1。
5. The soft limit control method for shaft motion according to claim 4, characterized in that: step S4 further includes:
when maximum deceleration is reached:
the new maximum deceleration is recalculated by,
Figure FDA0003467941460000047
Figure FDA0003467941460000051
wherein: period of acceleration and deceleration section
Figure FDA0003467941460000052
Period n of uniform deceleration section6＝n–n5Period n of deceleration section7＝n5–1；
And recalculating the new second jerk by:
Figure FDA0003467941460000053
6. the soft limit control method for shaft motion according to claim 5, characterized in that: in step S5, the deceleration distance L 'required for the deceleration stage is calculated'decThe method comprises the following steps: the acceleration/deceleration section period n of the deceleration section is calculated in step S45Period n of uniform deceleration section6And decreasing the period n of the deceleration section7And a recalculated new second jerk J'decAnd a maximum deceleration D ', calculating a deceleration distance L ' required for the deceleration phase 'dec；
First, the acceleration at the end of the acceleration/deceleration section is calculated
Figure FDA0003467941460000054
Speed of rotation
Figure FDA0003467941460000055
Distance traveled
Figure FDA0003467941460000056
Respectively as follows:
Figure FDA0003467941460000057
Figure FDA0003467941460000058
Figure FDA0003467941460000059
secondly, the acceleration at the end of the uniform deceleration section is calculated
Figure FDA00034679414600000510
Speed of rotation
Figure FDA00034679414600000511
Distance traveled
Figure FDA00034679414600000512
Respectively as follows:
Figure FDA00034679414600000513
Figure FDA00034679414600000514
Figure FDA00034679414600000515
third, calculate the acceleration at the end of the deceleration segment
Figure FDA00034679414600000516
Speed of rotation
Figure FDA00034679414600000517
Distance traveled
Figure FDA00034679414600000518
Respectively as follows:
Figure FDA00034679414600000519
Figure FDA00034679414600000520
Figure FDA00034679414600000521
finally, calculating the running deceleration distance in the deceleration stage as follows:
Figure FDA0003467941460000061
7. the soft limit control method for shaft motion of claim 6, wherein: calculating the maximum speed V reached after the acceleration phasemaxThe method is realized by adopting the following formula:
Figure FDA0003467941460000062
wherein, addingAcceleration of a current period in a fast or uniform acceleration sectioncurAt a velocity of VcurThe number of cycles of the deceleration acceleration section is n3，AcurAnd VcurInitially 0, and a in the acceleration or uniform acceleration segment in each cyclecurAnd VcurCalculated in step S9.
8. The soft limit control method for shaft motion according to claim 7, characterized in that: step S6 includes:
when the direction of motion is positive at this time:
obtaining a current position value CurPos reached by the movement of the shaft, and calculating the distance LimitLen between the current position value CurPos and the positive limit Limit P of the software:
LimitLen＝|CurPos-LimitP|；
wherein CurPos is initially 0 and passes through CurPos + V for each cyclecurCalculating to obtain;
when the direction of motion is negative at this time:
obtaining a current position value CurPos reached by the movement of the shaft, and calculating the distance LimitLen between the current position value CurPos and the negative limit Limit N of the software:
LimitLen＝|CurPos-LimitN|。
9. the soft limit control method for shaft motion of claim 8, wherein: step S9 includes when Acur< A', go to step S91 to continue adding the acceleration section movement; when A iscurIf not less than A', the flow proceeds to step S92;
step S91, when AcurIf the acceleration is smaller than A', continuing to add the acceleration segment to move, and calculating the acceleration A of the current periodcurCurrent cycle speed VcurThe moving position value CurPos of the current period returns to the step S3, and enters the next period cycle;
Acur＝Acur+J′acc
Vcur＝Vcur+Acur
CurPos＝CurPos+Vcur
step S92, when AcurNot less than A', entering into uniform acceleration segment to calculate the current period acceleration AcurCurrent cycle speed VcurThe moving position value CurPos of the current period returns to the step S3, and enters the next period cycle;
Acur＝Acur
Vcur＝Vcur+Acur
CurPos＝CurPos+Vcur。
10. an RTX 64-based open motion controller, comprising a processor and a memory, characterized in that: the memory stores instructions executable by the processor to enable the processor to perform the method of soft limit control for shaft motion of any of claims 1-9.
CN202110658647.3A 2021-06-15 2021-06-15 Soft limit control method for shaft motion and open type motion controller based on RTX64 Active CN113359620B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110658647.3A CN113359620B (en) 2021-06-15 2021-06-15 Soft limit control method for shaft motion and open type motion controller based on RTX64

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110658647.3A CN113359620B (en) 2021-06-15 2021-06-15 Soft limit control method for shaft motion and open type motion controller based on RTX64

Publications (2)

Publication Number Publication Date
CN113359620A CN113359620A (en) 2021-09-07
CN113359620B true CN113359620B (en) 2022-03-15

Family

ID=77534153

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110658647.3A Active CN113359620B (en) 2021-06-15 2021-06-15 Soft limit control method for shaft motion and open type motion controller based on RTX64

Country Status (1)

Country Link
CN (1) CN113359620B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114967591B (en) * 2022-03-04 2022-12-09 弥费实业(上海)有限公司 Wafer cassette transfer control method
CN117193161A (en) * 2023-09-20 2023-12-08 苏州浩智工业控制技术有限公司 Point motion planning method and system

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1971457A (en) * 2005-11-25 2007-05-30 中国科学院沈阳计算技术研究所有限公司 Speed control method used for numerical control machine
CN101639682A (en) * 2008-07-31 2010-02-03 鸿富锦精密工业（深圳）有限公司 System and method for controlling variable motion of machine stand
CN104181860A (en) * 2014-08-25 2014-12-03 浙江理工大学 S-type acceleration and deceleration control method of numerical control machine tool
CN104483906A (en) * 2015-01-07 2015-04-01 浙江师范大学 S-curve acceleration and deceleration control method and device of discrete sampling
CN106168790A (en) * 2016-02-29 2016-11-30 华南理工大学 A kind of online change target velocity and the S-shaped Acceleration-deceleration Control Method of position
WO2017113069A1 (en) * 2015-12-28 2017-07-06 深圳配天智能技术研究院有限公司 S-shaped curve planning method and device, and numerically-controlled machine tool
CN108287527A (en) * 2017-07-18 2018-07-17 山东大学 A kind of improvement S curve Acceleration-deceleration Control Method based on trigonometric function

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7919940B2 (en) * 2007-10-21 2011-04-05 Ge Intelligent Platforms, Inc. System and method for jerk limited trajectory planning for a path planner
CN107263484B (en) * 2017-08-10 2020-04-14 南京埃斯顿机器人工程有限公司 Robot joint space point-to-point motion trajectory planning method

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1971457A (en) * 2005-11-25 2007-05-30 中国科学院沈阳计算技术研究所有限公司 Speed control method used for numerical control machine
CN101639682A (en) * 2008-07-31 2010-02-03 鸿富锦精密工业（深圳）有限公司 System and method for controlling variable motion of machine stand
CN104181860A (en) * 2014-08-25 2014-12-03 浙江理工大学 S-type acceleration and deceleration control method of numerical control machine tool
CN104483906A (en) * 2015-01-07 2015-04-01 浙江师范大学 S-curve acceleration and deceleration control method and device of discrete sampling
WO2017113069A1 (en) * 2015-12-28 2017-07-06 深圳配天智能技术研究院有限公司 S-shaped curve planning method and device, and numerically-controlled machine tool
CN106168790A (en) * 2016-02-29 2016-11-30 华南理工大学 A kind of online change target velocity and the S-shaped Acceleration-deceleration Control Method of position
CN108287527A (en) * 2017-07-18 2018-07-17 山东大学 A kind of improvement S curve Acceleration-deceleration Control Method based on trigonometric function

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"基于多项式的S曲线加减速运动控制算法构建";高伟强 等;《机床与液压》;20190228;第47卷(第4期);124-128 *

Also Published As

Publication number Publication date
CN113359620A (en) 2021-09-07

Similar Documents

Publication Publication Date Title
CN113359620B (en) Soft limit control method for shaft motion and open type motion controller based on RTX64
CN107368639B (en) Speed planning method, speed planning device, computer equipment and storage medium
JP5343081B2 (en) Jerk limited trajectory planning system and method for path planner
JP4980458B2 (en) Machining time prediction device for numerically controlled machine tools
EP1959323B1 (en) Elimination of unintended velocity reversals in S-curve velocity profiles
KR100342255B1 (en) motor speed control method for robot
CN1971457A (en) Speed control method used for numerical control machine
CN111727411B (en) Equipment motion control method, equipment and storage device
WO2009144805A1 (en) Acceleration/deceleration control device
WO2007046257A1 (en) Motor control device, method, and program storage medium
CN116330299B (en) A compliant motion control method for industrial robots based on acceleration smoothing
CN113231735A (en) Cutting head obstacle avoidance method and device, computer equipment and medium
CN102082545B (en) Motor speed controller and control method thereof
CN111262483B (en) Motor motion control method
JP7376260B2 (en) numerical control device
JP2007094936A (en) Numerical value controller
CN105388853A (en) numerical control device
CN107873122A (en) Motor control system
CN118519393A (en) A dual-axis interpolation speed planning method for SMT equipment
CN112865627B (en) Control method and device for stepping motor
Gong et al. LabVIEW-base automatic rising and falling speed control of stepper motor
KR102491234B1 (en) An apparatus and method for generating sinusoidal motion profile
CN115847395A (en) Method and system for judging in-place movement of extractor, electronic equipment and storage medium
CN114019904B (en) Motion control method, system and device
TWI399029B (en) Method, controller and manufacturing system for controlling motor speed

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
GR01 Patent grant
GR01 Patent grant