CN114740806A - S-shaped curve planning method and system compatible with target position updating in motion - Google Patents
S-shaped curve planning method and system compatible with target position updating in motion Download PDFInfo
- Publication number
- CN114740806A CN114740806A CN202210507053.7A CN202210507053A CN114740806A CN 114740806 A CN114740806 A CN 114740806A CN 202210507053 A CN202210507053 A CN 202210507053A CN 114740806 A CN114740806 A CN 114740806A
- Authority
- CN
- China
- Prior art keywords
- acceleration
- time
- section
- deceleration
- acc
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/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
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/35—Nc in input of data, input till input file format
- G05B2219/35473—Input limit values of speed, position, acceleration or force
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position Or Direction (AREA)
Abstract
The invention relates to the technical field of chip mounter control, and particularly discloses an S-shaped curve planning method and system compatible with target position updating in motion, wherein the method comprises the following steps: dividing the curve gauge into 7 stages; setting and initializing motion parameters of curve planning; calculating the direct deceleration distance in the curve planning according to the motion parameters, and judging whether the direct deceleration can be carried out or not; calculating the acceleration time and the acceleration distance for accelerating to the maximum speed according to the judgment result; calculating the time point of each stage according to the acceleration time and the acceleration distance; detecting whether a new target position exists in real time, if not, updating the curve according to the time point to plan the state of each stage; if yes, returning to the initial step for re-planning. The invention supports curve planning with initial speed, is compatible with the relocation curve planning directly after the target position is changed in the motion process, and effectively improves the production efficiency.
Description
Technical Field
The invention relates to the technical field of control, in particular to an S-shaped curve planning method and system compatible with target position updating in motion.
Background
In the field of machining and manufacturing at present, high-frequency point-to-point motion is a common phenomenon, and meanwhile due to functional requirements such as correction and compensation, a target position is often updated after compensation is performed in a motion process, for example, when a component is mounted on a chip mounter, the target position and a correction angle are updated after a camera shoots the component in the motion process. In some special high-frequency point-to-point motion control fields, such as chip mounter motion control, position correction may be required during a motion process, so that a target position is changed in a motion project and curve planning needs to be updated. In the conventional position planning mode, generally, the target position change in the motion process is not considered, and the position curve planning is performed again by setting a new target position after the target position is reached.
Disclosure of Invention
The invention aims to overcome the problems in the prior art, and provides an S-shaped curve planning method and system compatible with target position updating in motion.
In order to achieve the above object, a first aspect of the present invention provides a sigmoid curve planning method compatible with target position update in motion, comprising the following steps:
the curve gauge is divided into 7 stages: a jerk section of the acceleration section, a uniform acceleration section of the acceleration section, a jerk section of the acceleration section, a uniform velocity section, a jerk section of the deceleration section, a uniform acceleration section of the deceleration section, and a jerk section of the deceleration section;
setting and initializing motion parameters of curve planning;
calculating the direct deceleration distance in the curve planning according to the motion parameters, and judging whether the direct deceleration can be carried out or not;
calculating the acceleration time T for accelerating to the maximum speed according to the judgment resultaccAnd an acceleration distance Sacc;
According to the acceleration timeTaccAnd an acceleration distance SaccCalculating the time point of each stage;
detecting whether a new target position exists in real time, if not, updating the curve according to the time point to plan the state of each stage; if yes, returning to the initial step for re-planning.
The second aspect of the present invention provides an S-shaped curve planning system compatible with target position update in motion, comprising:
a dividing module for dividing the curve gauge into 7 stages: a jerk section of the acceleration section, a uniform acceleration section of the acceleration section, a deceleration section of the acceleration section, a uniform velocity section, a jerk section of the deceleration section, a uniform acceleration section of the deceleration section, and a deceleration section of the deceleration section;
the motion parameter module is used for setting and initializing the motion parameters of curve planning;
the judging module is used for calculating the direct deceleration distance in the curve planning according to the motion parameters and judging whether the direct deceleration can be carried out or not;
a calculation module for calculating the acceleration time T for accelerating to the maximum speed according to the judgment resultaccAnd an acceleration distance Sacc;
A time point module for determining the acceleration time TaccAnd an acceleration distance SaccCalculating the time point of each stage;
the detection updating module is used for detecting whether a new target position exists in real time, and if not, the states of all stages are planned according to the time point updating curve; if yes, returning to the initial step for re-planning.
A third aspect of the present invention provides a chip mounter, comprising
A memory storing a program;
a processor for executing the program in the memory to implement the steps of the sigmoid curve planning method compatible with target position update in motion;
and the driver is used for responding to the curve planning process so as to realize the actual motion control of the chip mounter.
According to the technical scheme, based on trigonometric function operation, position planning can be directly carried out again after the target position is changed in the motion process through a design strategy of curve planning, the method for changing the target position and carrying out the curve planning for the repositioning in the motion process is compatible, the curve planning with the initial speed is supported, the operation efficiency can be improved to a great extent, in addition, various conditions in the method are clear in distinguishing conditions, the formula is simple, the operation complexity is low, and the method can be widely applied to positioning motion control.
Drawings
FIG. 1 is a schematic diagram of a sigmoid curve planning method and system compatible with target location update in motion according to the present invention;
FIG. 2 shows a real-time position P in embodiment 1 of the present inventiontThe curve of (d);
FIG. 3 shows the real-time velocity v of embodiment 1 of the present inventiontA curve;
FIG. 4 shows the real-time acceleration a of embodiment 1 of the present inventiontA curve;
FIG. 5 shows the real-time jerk j according to embodiment 1 of the present inventiontA curve;
FIG. 6 shows a real-time position P in embodiment 2 of the present inventiontThe curve of (d);
FIG. 7 shows the real-time velocity v of embodiment 2 of the present inventiontA curve;
FIG. 8 shows the real-time acceleration a of embodiment 2 of the present inventiontA curve;
FIG. 9 shows the real-time jerk j according to embodiment 2 of the present inventiontA curve;
FIG. 10 shows a real-time position P according to embodiment 3 of the present inventiontThe curve of (c);
FIG. 11 shows the real-time velocity v of embodiment 3 of the present inventiontA curve;
FIG. 12 shows the real-time acceleration a of embodiment 3 of the present inventiontA curve;
FIG. 13 shows the real-time jerk j according to embodiment 3 of the present inventiontA curve.
Detailed Description
The following detailed description of embodiments of the invention refers to the accompanying drawings. It should be understood that the detailed description and specific examples, while indicating embodiments of the invention, are given by way of illustration and explanation only, not limitation.
A first aspect of the present invention provides an S-shaped curve planning method compatible with target position update in motion, as shown in fig. 1, including:
the curve gauge is divided into 7 stages: a jerk section of the acceleration section, a uniform acceleration section of the acceleration section, a jerk section of the acceleration section, a uniform velocity section, a jerk section of the deceleration section, a uniform acceleration section of the deceleration section, and a jerk section of the deceleration section;
the curve gauge is divided into 7 sections, and the 7-section S-shaped curve planning method of the asymmetric trigonometric function with continuous acceleration adopts the subsection to realize the multistage curve planning with continuous acceleration and asymmetric speed curve point-to-point motion.
Setting and initializing motion parameters of curve planning;
the method specifically comprises the following steps: setting motion parameters: setting a new target position Pe1Current position PsDistance of movement S, maximum speed VmMaximum speed value VsAverage acceleration A, average acceleration value AsAnd the time points of each phase of the curve planning are as follows: acceleration section T0-T3Constant velocity segment T3-T4Speed reduction section T4-T7Acceleration section T of acceleration section0-T1Uniform acceleration section T of acceleration section1-T2Acceleration section T of the acceleration section2-T3Acceleration section T of deceleration section4-T5Uniform acceleration section T of deceleration section5-T6Acceleration reduction section T of deceleration section6-T7(ii) a When the set displacement is smaller, the distance in the acceleration and deceleration process is greater than or equal to the set displacement, and no constant speed stage, namely T, exists3=T4(ii) a Setting a softening factor of 0<Beta is less than or equal to 1, beta is the ratio of the average acceleration of the deceleration section to the average acceleration of the acceleration section, and the acceleration coefficient is 1<k is less than or equal to 2, and k is the ratio of the maximum acceleration to the average acceleration in the deceleration section or the acceleration section; the parameters in the curve planning calculation process are set as follows: current time t, time interval dt, real time position PtReal-time movement distance stReal time velocity vtReal time acceleration atReal-time jerk jt;
wherein t is the current time vtFor real-time speed at time t, atFor real-time acceleration at time t, StFor real-time movement distance at time t, PtFor real-time position at time t, V0Is the initial velocity. Note that: s = Pe1-Ps>0 then moves in positive direction, S = Pe1-Ps<0 is moving in the opposite direction.
Calculating the direct deceleration distance in the curve planning according to the motion parameters, and judging whether the direct deceleration can be carried out or not;
distance S of direct deceleration in curve planningdec1And a deceleration time Tdec1The formula is as follows:
calculating the distance of direct deceleration according to formula (2);
judging whether the speed can be directly reduced to the target position: if S<Sdec1Then the maximum speed VmAnd respectively taking the opposite number of the average acceleration A value, namely updating the average acceleration A value into a formula (3), otherwise, keeping the average acceleration A value unchanged:
wherein VsIs a VmAbsolute value of the number, AsIs the absolute value of the A value.
Calculating the acceleration time T for accelerating to the maximum speed according to the judgment resultaccAnd an acceleration distance Sacc;
Acceleration time T of maximum speedaccAnd an acceleration distance SaccThe calculation formula of (a) is as follows:
it is worth noting that if T is calculated at this timeaccToo small, meaning a rapid acceleration change over a short period of time, which may result in an excessive acceleration shock, may be introduced as an acceleration step adjustment factor α (0)<Alpha is less than or equal to 1), alpha can be reacted with TaccIn a functional relationship, e.g. with jerk mean limit Jm,
When in useWhen (i.e. T)accToo small) to recalculate the acceleration time T to the maximum speedaccAnd an acceleration distance SaccDeceleration time T corresponding to the maximum speed being decelerated to 0decAnd a deceleration distance SdecThe formula is as follows:
thus, the acceleration adjustment is performed according to the formula (4) and the formula (5), and the acceleration and deceleration distances and time are calculated according to the formula (6).
According to the acceleration time TaccAnd an acceleration distance SaccCalculating the time point of each stage;
the method specifically comprises the following steps: judging whether a uniform velocity segment exists or not, and calculating the uniform velocity time TavgAnd a uniform distance Savg:
If abs (S)>abs(Sacc+Sdec) Then there is a uniform velocity segment, the formula is as follows:
if abs (S) is less than abs (S)acc+Sdec) There is no constant velocity segmentThen the maximum speed V that can be reached needs to be recalculatedmAcceleration time TaccAnd a deceleration time TdecAnd corresponding acceleration distance SaccA deceleration distance SdecThe formula is as follows:
further, when α =1, it means that it is not necessary to consider a rapid acceleration change in a short time, and there is a possibility that the acceleration shock is too large, and the formula (8) is converted into 。
According to acceleration time TaccConstant speed time TavgAnd a deceleration time TdecCalculating the time points of each stage as follows:
the acceleration, uniform speed and deceleration times are calculated according to the equations (6), (7) or (8), so that three time points can be obtained first:
T3 = Tacc,T4 = Tacc + Tavg,T7 = T4 + Tdec (9)
in order to ensure continuous acceleration and acceleration average value, a ratio k of the maximum acceleration to the acceleration average value is introduced, the k value has the function of preventing the maximum acceleration corresponding to the set average acceleration from being too large (1 < k ≦ 2), and the k value is preferably set to be between 1.5 and 2 under the condition that the hardware driving capacity is insufficient, and the whole acceleration section is designed according to the following steps:
t can be calculated by integral operation of acceleration0-Tacc(T3) The cumulative velocity dv for each segment of (a) is:
to ensure the continuity and accuracy of the position planning, T is ensured3(Tacc=ta1+ta2+ta3) Acceleration reaches maximum speed V at momentmIs provided with
After the k value is actually set, t can be solved according to equation (11)a1、ta2And k and TaccThe relationship between is
The acceleration stage and the deceleration stage are generally arranged symmetrically, i.e. the acceleration stage takes time ta1= reduction of acceleration phase elapsed time ta3It is possible to obtain:
finally, the time T of the acceleration phase can be calculated1And T2:
The deceleration stage is also divided into three time points, which correspond to the calculation mode of the acceleration stage, and the three time points comprise:
i.e. the point in time T of the deceleration section5And T6Comprises the following steps:
the various time periods are summarized as:
the values at the various time points of the curve plan are calculated from equation (17). If the constant speed section exists, setting an acceleration coefficient k<And 2, planning the curve to be a normal 7-section curve plan, wherein the curve plan comprises an acceleration section, a uniform acceleration section, a deceleration and acceleration section, a constant speed section, a deceleration section, a uniform acceleration section and a deceleration and acceleration section. If there is no uniform velocity segment, T3=T4. If k =2 is set, T1=T2,T4=T5And the uniform acceleration section does not exist, and the whole curve plan is changed into a 5-section type at the moment, wherein the 5-section type comprises a jerk section of the acceleration section, a uniform speed section, a jerk section of the deceleration section and a jerk section of the deceleration section. If the softening factor beta is set<1, realizing curve planning into an acceleration and deceleration asymmetric curve; if β =1 is set, the curve is now formulated as a symmetrical acceleration and deceleration curve. Wherein, ta1=ta3It can be taken as a value in the specific embodiment, and when the two values are not equal, the formula (12) needs to be satisfied. K value of acceleration sectionThe values of k may or may not be equal to the deceleration segment, and the above discussion is intended to be equal by default.
Detecting whether a new target position exists in real time, if not, updating the curve according to the time point to plan the state of each stage; if yes, returning to the initial step for re-planning;
with dt as the time interval, time T is from T0Is accumulated to T7Detecting whether there is a new target position Pe1If old target position Pe0Is not equal to Pe1If the target position is new, returning to the initial step, namely dividing the curve gauge into 7 stages for replanning, and circulating each step until the old target position P is mete0=Pe1(ii) a If Pe0=Pe1Then there is no new target location, real-time jerk j for the entire curve planning processtReal time acceleration alphatReal-time speed vtThe update formula is as follows:
Real-time movement distance s in the whole curve planning processtAnd a real-time position PtThe updating is as follows:
the curve planning of position control can influence positioning accuracy and stable time, guarantees that the jerk is continuous, and the acceleration value that reduces the deceleration section simultaneously can reduce the vibration to the position to promote positioning accuracy and stable time performance. The method calculates the direct deceleration distance through a formula (2), judges whether acceleration and speed reversal are needed or not through the target distance, introduces an acceleration adjusting factor alpha, adjusts the acceleration of an acceleration section through a formula (4) and a formula (5), and avoids hardware faults caused by overlarge acceleration; judging whether a uniform speed section exists or not by comparing the total acceleration and deceleration distance with the target distance, calculating acceleration and deceleration time and maximum speed by adopting a formula (6) and a formula (7) if the uniform speed section exists, calculating the acceleration and deceleration time and the maximum speed by adopting a formula (8) if the uniform speed section does not exist, and calculating final time points by adopting a formula (17); judging whether a new target position is updated or not in the time updating and moving processes, and if the new target position is updated, restarting to initialize the parameter of the formula (1) to start new curve planning, and updating the speed, the acceleration and each time point; and if the target position is not updated, updating the states of the jerk, the acceleration, the speed and the position in real time through the formula (18) to the formula (25). The design of a piecewise trigonometric function relation ensures that the acceleration continuity, the speed and the displacement of the curve planning meet the set value requirement of the curve planning; the asymmetric curve planning of the acceleration curve and the deceleration curve is realized through a softening factor beta (0 < beta is less than or equal to 1) of the deceleration section, wherein, the speed curve is asymmetric when the value of 0< beta <1, and the curve planning special case of the speed symmetry can be realized when the value of beta = 1; the acceleration coefficient k (0 < k ≦ 2) is set, so that the maximum acceleration is prevented from exceeding the physical limit to ensure the average acceleration value, wherein when 1< k <2, the curve plan is a 7-segment curve plan, and when k =2, a special 5-segment curve plan case without a uniform acceleration stage can be realized. In conclusion, the invention realizes that the position planning can be directly carried out again after the target position is changed in the motion process through the design strategy of curve planning, the jerk and the acceleration of the deceleration section are continuous through the design of a segmented trigonometric function relation, and the position curve planning with the initial speed can be compatible.
Based on the above S-shaped curve planning method compatible with target position update in motion, a second aspect of the present invention provides an S-shaped curve planning system compatible with target position update in motion, including:
a dividing module for dividing the curve gauge into 7 stages: a jerk section of the acceleration section, a uniform acceleration section of the acceleration section, a jerk section of the acceleration section, a uniform velocity section, a jerk section of the deceleration section, a uniform acceleration section of the deceleration section, and a jerk section of the deceleration section;
the motion parameter module is used for setting and initializing the motion parameters of curve planning;
the judging module is used for calculating the direct deceleration distance in the curve planning according to the motion parameters and judging whether the direct deceleration can be carried out or not;
a calculation module for calculating the acceleration time T for accelerating to the maximum speed according to the judgment resultaccAnd an acceleration distance Sacc;
A time point module for determining the acceleration time TaccAnd an acceleration distance SaccCalculating the time point of each stage;
the detection updating module is used for detecting whether a new target position exists in real time, and if not, the states of all stages are planned according to the time point updating curve; if yes, returning to the initial step for re-planning.
A third aspect of the present invention provides a chip mounter, including
A memory storing a program;
a processor for executing the program in the memory to implement the steps of the sigmoid curve planning method compatible with target position update in motion;
and the driver is used for responding to the curve planning process so as to realize the actual motion control of the chip mounter.
The method, the system and the chip mounter support curve planning with initial speed, guarantee the acceleration and the acceleration continuity in the deceleration section, and can greatly improve the operation efficiency and reduce in-place impact. In addition, various conditions in the scheme are distinguished clearly, the formula is simple, the operation complexity is low, and the method can be widely applied to positioning motion control.
Example 1:
initializing curve planning parameters, assuming a current position Ps=0m, new target position Pe1=0.4m, and the initial velocity is set to V0=1m/s, maximum speed value Vs=3m/s, mean acceleration value As=30m/s2Initializing the motion parameters according to equation (1)
The ratio of the average acceleration of the named deceleration segment to the acceleration segment is a softening factor, the ratio of the maximum acceleration to the average acceleration in the named deceleration segment or the acceleration segment is an acceleration coefficient, and the acceleration coefficients in the deceleration segment and the acceleration segment are equal (the same as the following embodiments). Let the softening factor β =1 and the acceleration coefficient k =1.5, i.e. the 7-segment curve is a symmetric curve, and the maximum acceleration is 1.5 times the average acceleration. Calculating the distance S of the direct deceleration in the curve planning according to the formula (2) and the current speeddec1=0.0167m<S, so that the maximum speed and acceleration are not changed, the acceleration time T is calculated according to the formula (4)accAnd an acceleration distance SaccComprises the following steps:
since α =1 is determined according to equation (5), the deceleration duration time T is calculated according to equation (6)dec=0.1S and deceleration distance Sdec=0.15 m. Due to abs (S)>abs(Sacc+Sdec) The curve plan can be known to exist in a constant speed section, and the constant speed distance S is calculated according to the formula (7)avg=0.1167m and uniform velocity time Tavg=0.0389s。
Then according to the acceleration time TaccConstant speed time TavgAnd a deceleration time TdecThe time point and the acceleration coefficient k =1.5 of each stage are calculated, and each time slot is calculated according to the formula (17):
with dt as time interval, time T from T0Is accumulated to T7According to the publicationEquation (18) -equation (25) begins updating the real-time jerk, real-time acceleration, real-time velocity, and real-time distance. Detecting if there is a new target position, P, at updatee0≠Pe1Here, assuming that when t =0.1s, the position is 0.2333m, and a new target position P is receivede1If =0.5m, then the initialization parameters need to be returned and 7-segment stages need to be planned again, including
Keeping the softening factor β =1 and the acceleration coefficient k = 1.5. Calculating the distance S of the direct deceleration in the curve planning according to the formula (2) and the current speeddec1=0.15m<S, therefore, the maximum speed and the acceleration are not changed, and the acceleration time T is actually calculated according to the formula (4) because the current speed is the maximum speedacc=0S and acceleration distance Sacc=0 m. Continuously calculating the time T of the deceleration section according to the formula (6)dec=0.1S and deceleration distance Sdec=0.15 m. Due to abs (S)>abs(Sacc+Sdec) It can be known that the curve plan has a uniform speed segment, and the uniform speed distance S is calculated according to the formula (7)avg=0.1167m and time Tavg=0.0389s。
Then according to the acceleration time TaccConstant speed time TavgAnd a deceleration time TdecThe time point and the acceleration coefficient k =1.5 of each stage are calculated, and each time slot is calculated according to the formula (17):
at dt intervals, time T restarts from T0Is accumulated to T7The real-time jerk, real-time acceleration, real-time velocity, and real-time position are updated according to equation (18) -equation (25). Assuming that a new target position does not appear before the target position is reached, the curve planning results of the final embodiment 1 are shown in fig. 2-5, where the solid line is the state track before the target position is updated, and the dotted line is the state track after the target position is updatedA new state trace.
Example 1 a symmetric 7-segment curve plan was achieved. FIG. 2 is a graph of the real-time position of example 1, the position is 0.2667m at 0.1s, the planning is performed again after the target position is updated to 0.5m, and the final curve planning reaches the final position of 0.5 m; FIG. 3 is a real-time speed curve of example 1, where the initial speed is 1m/s, the maximum speed reaches 3m/s continuously, the position of 0.1s is updated while the speed still needs to be kept at a constant speed, and finally the speed is reduced to 0 m/s; FIG. 4 is a real-time acceleration curve of example 1, with continuous acceleration and a maximum acceleration of 45m/s21.5 times the average acceleration; FIG. 5 is a real-time jerk profile of example 1, with jerk being continuous and the final value being 0. The results of example 1 show that the method of the present invention can handle the problem of replanning where the target location changes to a greater distance. The states of the deceleration section are continuous, and the in-place vibration can be reduced to the maximum extent.
Example 2:
initializing curve planning parameters, assuming a current position Ps=0m, new target position Pe1=0.4m, and the initial speed is set to V0=1m/s, maximum speed value Vs=3m/s, mean acceleration value As=30m/s2Initializing the motion parameters according to equation (1)
Let the softening factor β =0.75 and the acceleration coefficient k =1.5, i.e. when the 7-segment curve is an asymmetric curve, the acceleration value of the deceleration segment is 0.75 times of the acceleration segment, and the maximum acceleration is 1.5 times of the average acceleration. Calculating the distance S of the direct deceleration in the curve planning according to the formula (2) and the current speeddec1=-0.0222m<S, so that the maximum speed and the acceleration direction are not changed, and the acceleration time T is calculated according to the formula (4)accAnd an acceleration distance SaccIs composed of
According to the formula (5) Judging, and calculating deceleration period time T according to formula (6)dec=0.1333S and deceleration distance Sdec=0.2 m. Due to abs (S)>abs(Sacc+Sdec) It can be known that the curve plan has a uniform speed segment, and the uniform speed distance S is calculated according to the formula (7)avg=0.0667m and uniform speed time Tavg=0.0222s。
Then according to the acceleration time TaccConstant speed time TavgAnd a deceleration time TdecThe time point and the acceleration coefficient k =1.5 of each stage are calculated, and each time slot is calculated according to the formula (17):
with dt as time interval, time T from T0Is accumulated to T7The real-time jerk, real-time acceleration, real-time velocity, and real-time distance are updated according to equation (18) -equation (25). Detecting if there is a new target position, P, at updatee0≠Pe1Here, assuming that when t =0.15s, the position is 0.1833m, and a new target position P is receivede1If =0.5m, then the initialization parameters need to be returned and 7-segment stages need to be planned again, including
Keeping the softening factor β =0.75 and the acceleration coefficient k = 1.5. Calculating the distance S of the direct deceleration in the curve planning according to the formula (2) and the current speeddec1=0.15m<S, therefore, the maximum speed V is determined according to the formula (3)mAnd the average acceleration A numerical value is respectively the opposite number of the average acceleration A numerical value, namely:
actually calculating the acceleration time T according to the formula (4)acc=0.2S and acceleration distance Sacc= 0.3 m. The deceleration section is continuously calculated according to the formula (6)Time Tdec=0.1333S and deceleration distance Sdec=0.2 m. Due to abs (S)<abs(Sacc+Sdec) If the curve plan does not have a uniform speed segment, the maximum speed V which can be reached needs to be recalculatedmAcceleration time TaccAnd a deceleration time TdecAnd corresponding acceleration distance SaccA deceleration distance SdecObtained according to the formula (8)
Then according to the acceleration time TaccConstant speed time TavgAnd a deceleration time TdecThe time point and the acceleration coefficient k =1.5 of each stage are calculated, and each time slot is calculated according to the formula (17):
at dt intervals, time T restarts from T0Is accumulated to T7The real-time jerk, real-time acceleration, real-time velocity, and real-time position are updated according to equation (18) -equation (25). Assuming that a new target position does not appear before the target position is reached, the curve planning result of the final embodiment 2 is shown in fig. 6-9, where the solid line is the state trajectory before the target position is updated, and the dotted line is the new state trajectory after the target position is updated.
Example 2 an asymmetric 7-segment curve plan was achieved. Fig. 6 is a graph of the real-time position of example 2, the position is 0.1833m at 0.15s, the planning is carried out again after the update target position is 0m, and the final curve planning reaches the final position 0 m; FIG. 7 is a real-time velocity profile of example 2, with an initial velocity of-1 m/s, a continuous maximum velocity of 1m/s, and an immediate reverse acceleration to-2.9277 m/s when the 0.15s position is updated; FIG. 8 is a graph showing the real-time acceleration curve of example 2, in which the acceleration is continuous and the maximum acceleration is 45m/s21.5 times the average acceleration; FIG. 9 is a real-time jerk profile of example 2, with jerk being continuous and the final value being 0. Knot of example 2The result shows that the method can process the problem of replanning when the target position is changed into the reverse position. The states of the deceleration section are continuous, and the in-place vibration can be reduced to the maximum extent.
Example 3:
initializing curve planning parameters, assuming a current position Ps=0m, new target position Pe1=0.1m, and the initial speed is set to V0=1m/s, maximum speed value Vs=3m/s, mean acceleration value As=30m/s2Initializing the motion parameters according to equation (1)
Let the softening factor β =0.75 and the acceleration coefficient k =1.5, i.e. when the 7-segment curve is an asymmetric curve, the acceleration value of the deceleration segment is 0.75 times of the acceleration segment, and the maximum acceleration is 1.5 times of the average acceleration. Calculating the distance S of direct deceleration in curve planning according to formula (2) and the current speeddec1=0m<S, so that the maximum speed and the acceleration direction are not changed, and the acceleration time and the acceleration distance are calculated according to the formula (4) as
Since α =1 is determined according to equation (5), the deceleration duration time T is calculated according to equation (6)dec=0.1333S and acceleration distance Sdec=0.2 m. Due to abs (S)<abs(Sacc+Sdec) It can be known that the curve plan has no uniform speed section, i.e. uniform speed distance Savg=0m and uniform speed time TavgAnd =0 s. The maximum speed V that can be reached needs to be recalculatedmAcceleration time TaccAnd a deceleration time TdecAnd corresponding acceleration distance SaccA deceleration distance Sdec
Then according to the acceleration time TaccConstant speed time TavgAnd a deceleration time TdecThe time point and the acceleration coefficient k =1.5 of each stage are calculated, and each time slot is calculated according to the formula (17):
with dt as time interval, time T from T0Is accumulated to T7The real-time jerk, real-time acceleration, real-time velocity, and real-time distance are updated according to equation (18) -equation (25). Detecting if there is a new target position, P, at updatee0≠Pe1Here, assume that when t =0.1s, the position is 0.0968m, the speed is 0.4335m/s, and the target position to which a new signal is received is Pe1If =0.15m, then the initialization parameters need to be returned and 7-segment phases need to be planned again, including
At this time, the softening factor β =0.75, and the modified acceleration coefficient k =2, the deceleration section does not have a uniform deceleration section. Calculating the distance S of the direct deceleration in the curve planning according to the formula (2) and the current speeddec1=0.0042m<S, therefore maximum speed VmKeeping the average acceleration A constant, and actually calculating the time T required for accelerating to the maximum speed according to the formula (4)acc=0.2085S and acceleration distance Sacc=0.1469 m. Continuously calculating the time T of the deceleration section according to the formula (6)dec=0.1333S and deceleration distance Sdec=0.2 m. Due to abs (S)<abs(Sacc+Sdec) If the curve plan does not have the uniform speed segment, the maximum speed V which can be reached needs to be recalculatedmAcceleration time TaccAnd a deceleration time TdecAnd corresponding acceleration distance SaccAnd a deceleration distance SdecObtained according to the formula (8)
Then according to the acceleration time TaccConstant speed time TavgAnd a deceleration time TdecCalculating the time point and the acceleration coefficient k =2 of each stage, and calculating each time period according to the formula (17):
at dt intervals, time T restarts from T0Is accumulated to T7The real-time jerk, real-time acceleration, real-time velocity, and real-time position are updated according to equation (18) -equation (25). Assuming that a new target position does not appear before the target position is reached, the curve planning result of the final embodiment 3 is shown in fig. 10-13, where the solid line is the state trajectory before the target position is updated, and the dotted line is the new state trajectory after the target position is updated.
Example 3 an asymmetric 7-segment curve plan was achieved. FIG. 10 is a plot of the real-time position of example 3, at 0.1s the position is 0.0968m, at which time the target position is updated to 0.15m and then the planning is re-planned, and the final plot plan reaches the final position of 0.15 m; FIG. 11 is a real-time speed curve of example 3, where the initial speed is 0m/s, the first position planning does not have a uniform speed segment, the speed reaches 1.6036m/s at maximum, the speed is 0.4335m/s when the 0.1s position is updated, and the speed needs to be accelerated to 1.2039m/s immediately and then decelerated to 0 m/s; FIG. 12 is a real-time acceleration curve for example 3 with a first position planning setting of k =1.5, with continuous acceleration and a maximum acceleration of 45m/s2K is changed to be set to k =2 after the target position is changed at 1.5 times of the average acceleration, and there is no uniform acceleration section, the acceleration is continuous and the maximum acceleration is 60m/s 22 times the average acceleration; FIG. 13 is a real-time jerk curve of example 3, with jerk being continuous and the final value being 0. The result of the embodiment 3 shows that the method can solve the problem that the target position changes to a farther position and needs to be accelerated again for replanning, k is changed to adjust the number of the segments, the states of the deceleration segments are continuous, and the in-place vibration can be reduced to the maximum extent.
The preferred embodiments of the present invention have been described in detail above with reference to the accompanying drawings, but the present invention is not limited thereto. Within the scope of the technical idea of the invention, numerous simple modifications can be made to the technical solution of the invention, including combinations of the individual specific technical features in any suitable way. The invention is not described in detail in order to avoid unnecessary repetition. Such simple modifications and combinations should be considered within the scope of the present disclosure as well.
Claims (11)
1. A S-shaped curve planning method compatible with target position updating in motion is characterized by comprising the following steps:
the curve gauge is divided into 7 stages: a jerk section of the acceleration section, a uniform acceleration section of the acceleration section, a deceleration section of the acceleration section, a uniform velocity section, a jerk section of the deceleration section, a uniform acceleration section of the deceleration section, and a deceleration section of the deceleration section;
setting and initializing motion parameters of curve planning;
calculating the direct deceleration distance in the curve planning according to the motion parameters, and judging whether the direct deceleration can be carried out or not;
calculating the acceleration time T for accelerating to the maximum speed according to the judgment resultaccAnd an acceleration distance Sacc;
According to the acceleration time TaccAnd an acceleration distance SaccCalculating the time points of each stage;
detecting whether a new target position exists in real time, and if not, updating the curve according to the time point to plan the state of each stage; if yes, returning to the initial step for re-planning.
2. The method of claim 1, wherein the setting and initializing motion parameters for a curve plan comprises:
setting a new target position Pe1Current position PsDistance of movement S, maximum speed VmThe average acceleration a and the time points of the phases of the curve planning are as follows: acceleration of acceleration sectionDegree section T0-T1Uniform acceleration section T of acceleration section1-T2Acceleration section T of the acceleration section2-T3Constant velocity section T3-T4Acceleration section T of deceleration section4-T5Uniform acceleration section T of deceleration section5-T6Acceleration reduction section T of deceleration section6-T7;
wherein t is the current time, vtFor real-time speed at time t, atFor real-time acceleration at time t, stFor real-time movement distance at time t, PtFor the real-time position at time t, V0Is the initial velocity.
3. Method according to claim 2, characterized in that the distance S of the direct deceleration in the curve planning is calculated from the motion parametersdec1The formula is as follows:
4. The method of claim 3, wherein the determining whether direct deceleration is possible is specifically as follows: if S<Sdec1Then the maximum speed VmAnd taking the average acceleration A numerical value as the opposite number respectively, otherwise, keeping the average acceleration A numerical value unchanged.
6. method according to claim 5, characterised in that it consists in accelerating time TaccAnd an acceleration distance SaccCalculating the time points of the phases comprises:
judging whether a uniform velocity segment exists or not, and calculating the uniform velocity time TavgAnd a uniform distance Savg:
If abs (S)>abs(Sacc+Sdec) Then there is a constant velocity segment, and the formula is as follows:
if abs (S) is less than abs (S)acc+Sdec) If the uniform velocity segment does not exist, the formula is as follows: s. theavg= 0,Tavg=0, and recalculates the maximum speed V that can be reachedmAcceleration time TaccAnd a deceleration time TdecAnd corresponding acceleration distance SaccA deceleration distance SdecThe formula is as follows:
according to acceleration time TaccUniform speed time TavgAnd a deceleration time TdecCalculating the time points of each stage, wherein the formula is as follows:
wherein k is the ratio of the maximum acceleration to the average acceleration in the deceleration section or the acceleration section, and k is more than 1 and less than or equal to 2.
7. The method according to claim 5 or 6, wherein the acceleration time T for accelerating to the maximum speed is calculated according to the judgment resultaccAnd addSpeed distance SaccIn (1), a jerk average value J is setmAn acceleration segment adjustment factor α is introduced, and the following relation exists:,
9. the method of claim 8, wherein the real-time detection of whether there is a new target position, and if not, the updating of the curve according to the time point plans the states of each stage; if yes, returning to the initial step to re-plan specifically as follows:
with dt as time interval, time T from T0Is accumulated to T7Detecting whether a new target position exists, and if so, returning to the initial step for re-planning; if not, the real-time acceleration j of the whole curve planning processtReal time acceleration alphatReal-time speed vtUpdating formulasThe following were used:
Real-time movement distance s in the whole curve planning processtAnd a real-time position PtThe updating is as follows:
10. an S-shaped curve planning system compatible with target position updating in motion, comprising:
a dividing module for dividing the curve gauge into 7 stages: a jerk section of the acceleration section, a uniform acceleration section of the acceleration section, a deceleration section of the acceleration section, a uniform velocity section, a jerk section of the deceleration section, a uniform acceleration section of the deceleration section, and a deceleration section of the deceleration section;
the motion parameter module is used for setting and initializing the motion parameters of curve planning;
a judging module for calculating the direct deceleration distance in the curve planning according to the motion parameters,
and judging whether the speed can be directly reduced or not;
a calculation module for calculating the acceleration time T for accelerating to the maximum speed according to the judgment resultaccAnd an acceleration distance Sacc;
A time point module for determining the acceleration time TaccAnd an acceleration distance SaccCalculate each stageThe time point of (a);
the detection updating module is used for detecting whether a new target position exists in real time, and if not, the states of all stages are planned according to the time point updating curve; if yes, returning to the initial step for replanning.
11. A chip mounter is characterized by comprising
A memory storing a program;
a processor for executing the program in the memory to perform the steps of the method of any one of claims 1 to 9;
and the driver is used for responding to the curve planning process so as to realize the actual motion control of the chip mounter.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210507053.7A CN114740806A (en) | 2022-05-11 | 2022-05-11 | S-shaped curve planning method and system compatible with target position updating in motion |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210507053.7A CN114740806A (en) | 2022-05-11 | 2022-05-11 | S-shaped curve planning method and system compatible with target position updating in motion |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114740806A true CN114740806A (en) | 2022-07-12 |
Family
ID=82284914
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210507053.7A Pending CN114740806A (en) | 2022-05-11 | 2022-05-11 | S-shaped curve planning method and system compatible with target position updating in motion |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114740806A (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115032949A (en) * | 2022-08-12 | 2022-09-09 | 合肥安迅精密技术有限公司 | Curve planning method and system supporting target position change in motion |
CN117850174A (en) * | 2024-03-07 | 2024-04-09 | 合肥安迅精密技术有限公司 | Automatic focusing motion control method, system, storage medium and processor |
-
2022
- 2022-05-11 CN CN202210507053.7A patent/CN114740806A/en active Pending
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115032949A (en) * | 2022-08-12 | 2022-09-09 | 合肥安迅精密技术有限公司 | Curve planning method and system supporting target position change in motion |
CN115032949B (en) * | 2022-08-12 | 2022-11-04 | 合肥安迅精密技术有限公司 | Curve planning method and system supporting target position change in motion |
CN117850174A (en) * | 2024-03-07 | 2024-04-09 | 合肥安迅精密技术有限公司 | Automatic focusing motion control method, system, storage medium and processor |
CN117850174B (en) * | 2024-03-07 | 2024-05-10 | 合肥安迅精密技术有限公司 | Automatic focusing motion control method, system, storage medium and processor |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114740806A (en) | S-shaped curve planning method and system compatible with target position updating in motion | |
CN107850887B (en) | S-shaped curve planning method and device and numerical control machine tool | |
CN107844058B (en) | Motion curve discrete dynamic planning method | |
KR100221520B1 (en) | A scan-type radar apparatus for a vehicle to accurately detect an object lane of the radar equipped vehicle | |
CN110109491A (en) | Continuous S curve acceleration and deceleration motion control method, system and electronic equipment | |
CN105955203B (en) | CANopen servo-drive system medium velocities feedover and the processing method of torque feedforward | |
CN111158318B (en) | Flexible acceleration and deceleration planning method for asymmetric quartic curve | |
CN109048091A (en) | Laser cutting speed planing method, device, storage medium and computer equipment | |
CN111209361A (en) | Car following target selection method and device, electronic equipment and readable storage medium | |
CN111739342A (en) | Method, device, medium, and vehicle for avoiding vehicle ahead of side | |
CN110815219A (en) | Trajectory tracking method and device, electronic equipment and storage medium | |
CN109313429B (en) | S-shaped speed planning method, device and system, robot and numerical control machine | |
CN116277038B (en) | Mechanical arm track planning method and system for given time and initial and final speeds | |
CN114879609A (en) | Trigonometric function type curve planning method and system with continuous acceleration and chip mounter | |
CN109508050B (en) | Speed control method for automatic point drilling machine | |
CN113253772B (en) | Servo positioning system and method based on error estimation and compensation | |
CN116015141A (en) | Closed-loop control system and method for position of stepping motor | |
CN111381565B (en) | Acceleration recursive calculation limiting method for cubic polynomial S curve acceleration and deceleration | |
CN114013450B (en) | Vehicle operation control method, system and computer equipment | |
CN113763434B (en) | Target track prediction method based on Kalman filtering multi-motion model switching | |
CN114035582A (en) | S-shaped track planning method and device, computer equipment and storage medium | |
CN113031525B (en) | Polynomial acceleration and deceleration motion control method and device applied to numerical control machining | |
JP2003076410A (en) | Speed control device, speed control method and program for carrying the method with computer | |
CN116382361A (en) | Acceleration continuous real-time position planning control method | |
CN115542732B (en) | Acceleration and deceleration track planning method and device for servo motion control system |
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 |