CN113359620A - 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
CN113359620A
CN113359620A CN202110658647.3A CN202110658647A CN113359620A CN 113359620 A CN113359620 A CN 113359620A CN 202110658647 A CN202110658647 A CN 202110658647A CN 113359620 A CN113359620 A CN 113359620A
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.)
Granted
Application number
CN202110658647.3A
Other languages
Chinese (zh)
Other versions
CN113359620B (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 BDA0003114229550000021
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 it is less than or equal to the calculation of step S6Distance of LimitLen, 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 cycle and continuing toJudging 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 BDA0003114229550000041
A′＝n1×J′acc
wherein: plus period of acceleration section
Figure BDA0003114229550000042
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 ═ Δ v in acceleration phaseallow＝n1×A+n2×A；
Let n equal n1+n2，
Figure BDA0003114229550000043
The new maximum acceleration is recalculated using:
Figure BDA0003114229550000044
wherein, adding an acceleration period
Figure BDA0003114229550000045
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 BDA0003114229550000046
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 BDA0003114229550000051
D′＝n5×J′dec
Wherein: period of acceleration and deceleration section
Figure BDA0003114229550000052
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 BDA0003114229550000053
Figure BDA0003114229550000054
wherein: period of acceleration and deceleration section
Figure BDA0003114229550000055
Period n of uniform deceleration section6＝n–n5Period n of deceleration section7＝n5–1；
And recalculating the new second jerk by:
Figure BDA0003114229550000056
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 new after recalculationSecond 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 BDA0003114229550000057
Speed of rotation
Figure BDA0003114229550000058
Distance traveled
Figure BDA0003114229550000059
Respectively as follows:
an5＝-J′dec×n5
Figure BDA00031142295500000510
Figure BDA0003114229550000061
secondly, the acceleration at the end of the uniform deceleration section is calculated
Figure BDA0003114229550000062
Speed of rotation
Figure BDA0003114229550000063
Distance traveled
Figure BDA0003114229550000064
Respectively as follows:
Figure BDA0003114229550000065
Figure BDA0003114229550000066
Figure BDA0003114229550000067
third, calculate the acceleration at the end of the deceleration segment
Figure BDA0003114229550000068
Speed of rotation
Figure BDA0003114229550000069
Distance traveled
Figure BDA00031142295500000610
Respectively as follows:
Figure BDA00031142295500000611
Figure BDA00031142295500000612
Figure BDA00031142295500000613
finally, calculating the running deceleration distance in the deceleration stage as follows:
Figure BDA00031142295500000614
further, the maximum speed V reached after the acceleration phase is calculatedmaxThe method is realized by adopting the following formula:
Figure BDA00031142295500000615
wherein, in a certain current period in the acceleration section or the uniform acceleration section, the acceleration is acurVelocity 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 a motion parameter value, where the motion parameter value includes: 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 BDA0003114229550000091
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 BDA0003114229550000092
A′＝n1×J′acc (5)
wherein: plus period of acceleration section
Figure BDA0003114229550000093
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 BDA0003114229550000101
the new maximum acceleration is recalculated using:
Figure BDA0003114229550000102
wherein, adding an acceleration period
Figure BDA0003114229550000103
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 BDA0003114229550000104
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 BDA0003114229550000105
D′＝n5×J′dec (10)
wherein: period of acceleration and deceleration section
Figure BDA0003114229550000106
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 BDA0003114229550000111
Figure BDA0003114229550000112
wherein: period of acceleration and deceleration section
Figure BDA0003114229550000113
Period n of uniform deceleration section6＝n–n5Period n of deceleration section7＝n5–1。
And recalculating the new second jerk by:
Figure BDA0003114229550000114
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 BDA0003114229550000115
Speed of rotation
Figure BDA0003114229550000116
Distance traveled
Figure BDA0003114229550000117
Respectively as follows:
Figure BDA0003114229550000118
Figure BDA0003114229550000119
Figure BDA00031142295500001110
secondly, the acceleration at the end of the uniform deceleration section is calculated
Figure BDA00031142295500001111
Speed of rotation
Figure BDA00031142295500001112
Distance traveled
Figure BDA00031142295500001113
Respectively as follows:
Figure BDA00031142295500001114
Figure BDA00031142295500001115
Figure BDA00031142295500001116
third, calculate the acceleration at the end of the deceleration segment
Figure BDA00031142295500001117
Speed of rotation
Figure BDA00031142295500001118
Distance traveled
Figure BDA00031142295500001119
Respectively as follows:
Figure BDA00031142295500001120
Figure BDA0003114229550000121
Figure BDA0003114229550000122
finally, calculating the running deceleration distance in the deceleration stage as follows:
Figure BDA0003114229550000123
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 BDA0003114229550000124
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 periodcurWhen inFront period velocity 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, at uniform speedIn the phase movement process, the distance LimitLen from the current position value CurPos to the limit position of the software in each period is calculated, and the calculation process is the same as that in step S6, and is not described again 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 FDA0003114229540000011
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 acceleration a' allowed,and according to 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＝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＝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 FDA0003114229540000031
A′＝n1×J′acc
wherein: plus period of acceleration section
Figure FDA0003114229540000032
Period of uniform accelerationPeriod n20, 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 FDA0003114229540000041
The new maximum acceleration is recalculated using:
Figure FDA0003114229540000042
wherein, adding an acceleration period
Figure FDA0003114229540000043
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 FDA0003114229540000044
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 FDA0003114229540000045
D′＝n5×J′dec
Wherein: period of acceleration and deceleration section
Figure FDA0003114229540000046
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 FDA0003114229540000047
Figure FDA0003114229540000051
wherein: period of acceleration and deceleration section
Figure FDA0003114229540000052
Period n of uniform deceleration section6＝n–n5Period n of deceleration section7＝n5–1；
And recalculating the new second jerk by:
Figure FDA0003114229540000053
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 FDA0003114229540000054
Speed of rotation
Figure FDA0003114229540000055
Distance traveled
Figure FDA0003114229540000056
Respectively as follows:
Figure FDA0003114229540000057
Figure FDA0003114229540000058
Figure FDA0003114229540000059
secondly, the acceleration at the end of the uniform deceleration section is calculated
Figure FDA00031142295400000510
Speed of rotation
Figure FDA00031142295400000511
Distance traveled
Figure FDA00031142295400000512
Respectively as follows:
Figure FDA00031142295400000513
Figure FDA00031142295400000514
Figure FDA00031142295400000515
third, calculate the acceleration at the end of the deceleration segment
Figure FDA00031142295400000516
Speed of rotation
Figure FDA00031142295400000517
Distance traveled
Figure FDA00031142295400000518
Respectively as follows:
Figure FDA00031142295400000519
Figure FDA00031142295400000520
Figure FDA00031142295400000521
finally, calculating the running deceleration distance in the deceleration stage as follows:
Figure FDA0003114229540000061
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 FDA0003114229540000062
wherein, in a certain current period in the acceleration section or the uniform acceleration section, the acceleration is acurVelocity 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', entering stepStep S91 adding acceleration segment motion continuously; 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 true CN113359620A (en) 2021-09-07
CN113359620B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114967591A (en) * 2022-03-04 2022-08-30 弥费实业(上海)有限公司 Wafer cassette transfer control method

Citations (9)

* 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
US20090102411A1 (en) * 2007-10-21 2009-04-23 Miller Daniel H System and method for jerk limited trajectory planning for a path planner
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
US20190160674A1 (en) * 2017-08-10 2019-05-30 Nanjing Estun Robotics Co., Ltd Robot joint space point-to-point movement trajectory planning method

Patent Citations (9)

* 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
US20090102411A1 (en) * 2007-10-21 2009-04-23 Miller Daniel H System and method for jerk limited trajectory planning for a path planner
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
US20190160674A1 (en) * 2017-08-10 2019-05-30 Nanjing Estun Robotics Co., Ltd Robot joint space point-to-point movement trajectory planning method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
高伟强 等: ""基于多项式的S曲线加减速运动控制算法构建"", 《机床与液压》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114967591A (en) * 2022-03-04 2022-08-30 弥费实业(上海)有限公司 Wafer cassette transfer control method

Also Published As

Publication number Publication date
CN113359620B (en) 2022-03-15

Similar Documents

Publication Publication Date Title
CN107368639B (en) Speed planning method, speed planning device, computer equipment and storage medium
CN106168790B (en) S-shaped acceleration and deceleration control method for changing target speed and position on line
JP4980458B2 (en) Machining time prediction device for numerically controlled machine tools
EP2022608A2 (en) Improved blending algorithm for trajectory planning
KR100342255B1 (en) motor speed control method for robot
CN111711387B (en) Flexible speed control method for motor of intelligent glove knitting machine
EP1959323B1 (en) Elimination of unintended velocity reversals in S-curve velocity profiles
CN108189038A (en) A kind of industry six shaft mechanical arm straight-line trajectory method and system for planning of practicality
JP2011501292A (en) Jerk limited trajectory planning system and method for path planner
CN1971457A (en) Speed control method used for numerical control machine
CN108037737B (en) Quick-response point-to-point S-shaped speed planning method
CN113359620B (en) Soft limit control method for shaft motion and open type motion controller based on RTX64
JP2007094936A (en) Numerical value controller
CN111727411B (en) Equipment motion control method, equipment and storage device
WO2007046257A1 (en) Motor control device, method, and program storage medium
CN110460274B (en) Trapezoidal speed regulation control algorithm of stepping motor
JP7376260B2 (en) numerical control device
RU2005127678A (en) METHOD AND DEVICE FOR GATE DRIVE CONTROL
CN111262483B (en) Motor motion control method
CN112865627B (en) Control method and device for stepping motor
CN115847395A (en) Method and system for judging in-place movement of extractor, electronic equipment and storage medium
JP2001327194A (en) Pulse oscillation ic and motor positioning control method and device using the ic
JP2007052590A (en) Numerical controller and numerical control method
CN114019904B (en) Motion control method, system and device
CN116382361A (en) Acceleration continuous real-time position planning control method

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