CN114019904B - Motion control method, system and device - Google Patents
Motion control method, system and device Download PDFInfo
- Publication number
- CN114019904B CN114019904B CN202111320261.8A CN202111320261A CN114019904B CN 114019904 B CN114019904 B CN 114019904B CN 202111320261 A CN202111320261 A CN 202111320261A CN 114019904 B CN114019904 B CN 114019904B
- Authority
- CN
- China
- Prior art keywords
- motion
- interpolation period
- moving part
- shaft speed
- planning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 483
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000006073 displacement reaction Methods 0.000 claims abstract description 99
- 230000001133 acceleration Effects 0.000 claims description 67
- 238000004364 calculation method Methods 0.000 claims description 17
- 230000008859 change Effects 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 6
- 230000006870 function Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 description 8
- 230000009471 action Effects 0.000 description 3
- 230000009467 reduction Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000003754 machining Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
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/404—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 arrangements for compensation, e.g. for backlash, overshoot, tool offset, tool wear, temperature, machine construction errors, load, inertia
-
- 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/35408—Calculate new position data from actual data to compensate for contour error
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
Abstract
The application discloses a motion control method, a motion control system and a motion control device, which plan the motion state of a motion part in each interpolation period according to the target motion position and motion constraint conditions of the motion part; controlling the motion of the motion part in the M interpolation period according to the motion state of the M interpolation period; m is more than or equal to 1 and less than or equal to N; n represents the total number of interpolation cycles planned for the moving part; judging whether the motion part reaches a target motion position after the operation of the Nth interpolation period is finished; and if the motion state of the motion part in the compensation period is not reached, planning the motion state of the motion part in the compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period. Therefore, the application can compensate the motion position error of the moving part caused by the discretization error, thereby improving the motion accuracy of the moving part.
Description
Technical Field
The present invention relates to the field of motion control, and in particular, to a motion control method, system, and apparatus.
Background
Currently, motion control systems (e.g., numerical control machining control systems, robotic control systems) require that the acceleration and deceleration motions of the moving parts be planned in advance to control the operation of the moving parts in accordance with the previously planned acceleration and deceleration motions. It should be noted that, the acceleration/deceleration planning of the motion control system to the motion component is implemented in a computer, and the implementation of the acceleration/deceleration planning in the computer must implement continuous discretization processing (the time amount is converted into an integer multiple of the interpolation period) of a mathematical relationship, but the discretization processing may introduce discretization errors, which leads to errors between the planned target motion position and the actual target motion position, and finally leads to a reduction in the motion accuracy of the motion component.
Therefore, how to provide a solution to the above technical problem is a problem that a person skilled in the art needs to solve at present.
Disclosure of Invention
The invention aims to provide a motion control method, a motion control system and a motion control device, which can compensate the motion position error of a motion part caused by discretization error, thereby improving the motion accuracy of the motion part.
In order to solve the above technical problems, the present invention provides a motion control method, including:
Planning the motion state of the motion part in each interpolation period according to the target motion position and motion constraint conditions of the motion part;
Controlling the motion of the motion component in the M-th interpolation period according to the motion state of the M-th interpolation period; wherein M is more than or equal to 1 and less than or equal to N, and M is an integer; n represents the total number of interpolation cycles planned for the moving part;
Judging whether the motion part reaches the target motion position after the Nth interpolation period is finished;
And if the motion state of the motion part does not reach the target motion position, planning the motion state of the motion part in a compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period.
Optionally, planning a motion state of the motion component in each interpolation period according to a target motion position and a motion constraint condition of the motion component includes:
Calculating the residual planning displacement of the moving part in the current interpolation period according to the target movement position of the moving part and the movement position of the moving part in the current interpolation period, and calculating the estimated movement displacement of the moving part under the condition that the axial speed of the moving part in the current interpolation period is reduced to 0 according to the movement constraint condition of the moving part;
judging whether the predicted movement displacement is larger than or equal to the residual planning displacement;
if the speed of the motion component is greater than or equal to the preset value, planning the shaft speed of the motion component in the next interpolation period according to a preset non-acceleration planning strategy;
If the motion speed is smaller than the preset value, planning the shaft speed of the motion component in the next interpolation period according to a preset non-deceleration planning strategy so as to execute the step of controlling the motion of the motion component in the next interpolation period according to the shaft speed of the next interpolation period;
And calculating the motion position of the motion part in the next interpolation period according to the shaft speed of the motion part in the next interpolation period, and returning to the step of executing calculation of the residual planning displacement of the motion part in the current interpolation period until the shaft speeds and the motion positions of the motion part in N interpolation periods are planned.
Optionally, planning the shaft speed of the moving part in the next interpolation period according to a preset non-acceleration planning strategy includes:
Calculating the reference axis deceleration of the moving part in the next interpolation period according to the deceleration constraint condition of the moving part, and calculating the reference axis speed of the moving part in the next interpolation period according to the reference axis deceleration;
judging whether the speed of the reference shaft is smaller than a preset allowable calculation error or not;
If the reference shaft speed is not smaller than the preset allowable calculation error, taking the reference shaft speed as the shaft speed of the moving part in the next interpolation period;
If the calculated displacement is smaller than the preset allowable calculation error, calculating the displacement increment of the motion component in one interpolation period at the axial speed of the current interpolation period, and judging whether the residual planned displacement of the current interpolation period is smaller than the displacement increment;
if the displacement increment is not smaller than the displacement increment, taking the shaft speed of the current interpolation period as the shaft speed of the moving part in the next interpolation period;
And if the displacement increment is smaller than the displacement increment, determining that N interpolation period operations are finished, and determining that the moving part does not reach the target moving position after the Nth interpolation period operation is finished, so as to execute the step of remaining planning displacement according to the distance between the moving part and the target moving position.
Optionally, planning the shaft speed of the moving part in the next interpolation period according to a preset non-deceleration planning strategy includes:
judging whether the shaft speed of the moving part in the current interpolation period is smaller than a preset maximum planning shaft speed or not;
If the speed is smaller than the preset maximum planned shaft speed, judging whether the moving part is in a shaft deceleration state in the current interpolation period;
if the motion component is in the shaft speed-reducing state, taking the shaft speed of the current interpolation period as the shaft speed of the motion component in the next interpolation period;
If the motion component is not in the shaft speed-down state, calculating the shaft speed of the motion component in the next interpolation period according to a preset acceleration planning strategy;
and if the preset maximum planned shaft speed is not smaller than the preset maximum planned shaft speed, taking the preset maximum planned shaft speed as the shaft speed of the moving part in the next interpolation period.
Optionally, calculating a motion position of the motion part in a next interpolation period according to an axial speed of the motion part in the next interpolation period, and calculating a remaining planned displacement of the motion part in the next interpolation period according to a target motion position of the motion part and the motion position of the motion part in the next interpolation period, including:
Calculating the motion position of the motion component in the next interpolation period according to CurInterPos (M+1)＝CurInterPos(M)+CurActionSpeed(M+1)*TS; wherein CurInterPos (M) on the right side of the equation is the motion position of the motion component in the current interpolation period; curActionSpeed (M+1) is the shaft speed of the moving part at the next interpolation period; t S is the interpolation period duration; curInterPos (M+1) on the left side of the equation is the motion position of the motion part in the next interpolation period;
Calculating a remaining planned displacement sr of the moving part in a next interpolation period according to sr= CmdPos-CurInterPos (M+1); wherein CmdPos is the target motion position.
Optionally, calculating the estimated motion displacement of the moving component in the case that the shaft speed of the current interpolation period is reduced to 0 according to the motion constraint condition of the moving component includes:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed CurActionSpeed of the current interpolation period is decelerated to 0; wherein floor () is a downward rounding function; deceleration is a preset maximum planned axis deceleration; t S is the interpolation period duration; decN is the total number of cycles that the shaft speed of the current interpolation period is decelerated to 0;
If JerkTime is greater than 0, according to
Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed CurActionSpeed of the current interpolation period is decelerated to 0; wherein myJerk is the axis deceleration value for each interpolation period; the speed is a preset maximum planned shaft speed; (2 x myn1+myn2) is the total number of cycles that pass by which the shaft speed of the current interpolation period is slowed down to 0.
Optionally, calculating a reference axis deceleration of the moving part in a next interpolation period according to a deceleration constraint condition of the moving part, and calculating a reference axis speed of the moving part in the next interpolation period according to the reference axis deceleration, including:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to Calculating the reference shaft speed of the moving part in the next interpolation period; wherein DecFlag is a deceleration flag bit; tempDec is the reference axis deceleration of the moving part at the next interpolation period; deceleration is a preset maximum planned axis deceleration; curActionSpeed (M) on the right side of the equation is the shaft speed of the moving part at the current interpolation period; t S is the interpolation period duration; curActionSpeed (M+1) on the left side of the equation is the reference axis speed of the moving part at the next interpolation period;
If JerkTime is greater than 0, according to Calculating the reference shaft speed of the moving part in the next interpolation period; wherein JerkDecN (M) on the right side of the equation is the accumulated interpolation period number of the shaft deceleration of the motion component under the current interpolation period, and the initial value of the accumulated interpolation period number is 0; jerkDecN (M+1) on the left side of the equation is the accumulated interpolation period number of the lower shaft deceleration of the motion component in the next interpolation period; deceleration is a preset maximum planned axis deceleration; jerkDec is the deceleration delta for one interpolation period.
Optionally, calculating the shaft speed of the moving part in the next interpolation period according to a preset acceleration planning strategy includes:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to
Calculating the shaft speed of the moving part in the next interpolation period; wherein TempAcc is the axial acceleration of the moving part in the next interpolation period; accelerationis a preset maximum planned axis Acceleration; curActionSpeed (M) on the left side of the inequality condition and on the right side of the equality condition are the shaft speeds of the moving part in the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the shaft speed of the moving part at the next interpolation period; the speed is a preset maximum planned shaft speed; t S is the interpolation period duration;
If JerkTime is greater than 0, according to
Wherein JerkAccN (M) on the right side of the equation is the accumulated interpolation period number of the shaft acceleration of the moving part in the current interpolation period, and the initial value of the accumulated interpolation period number is 0; jerkAccN (M+1) on the left side of the equation is the accumulated interpolation period number of the shaft acceleration of the moving part in the next interpolation period; jerkAcc is the acceleration increment of one interpolation period; tempAcc (M) on the left side of the inequality condition is the axial acceleration of the moving part at the current interpolation period; tempAcc (M+1) in the equation is the axial acceleration of the moving part at the next interpolation period.
In order to solve the above technical problems, the present invention further provides a motion control system, including:
the planning module is used for planning the motion state of the motion component in each interpolation period according to the target motion position and the motion constraint condition of the motion component;
The control module is used for controlling the motion of the motion component in the M-th interpolation period according to the motion state of the M-th interpolation period; wherein M is more than or equal to 1 and less than or equal to N, and M is an integer; n represents the total number of interpolation cycles planned for the moving part;
The compensation module is used for judging whether the motion component reaches the target motion position after the Nth interpolation period is finished; and if the motion state of the motion part does not reach the target motion position, planning the motion state of the motion part in a compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period.
In order to solve the technical problem, the present invention further provides a motion control device, including:
A memory for storing a computer program;
a processor for implementing the steps of any of the motion control methods described above when executing the computer program.
The application provides a motion control method, which is used for planning the motion state of a motion part in each interpolation period according to the target motion position and motion constraint condition of the motion part; controlling the motion of the motion part in the M interpolation period according to the motion state of the M interpolation period; m is more than or equal to 1 and less than or equal to N; n represents the total number of interpolation cycles planned for the moving part; judging whether the motion part reaches a target motion position after the operation of the Nth interpolation period is finished; and if the motion state of the motion part in the compensation period is not reached, planning the motion state of the motion part in the compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period. Therefore, the application can compensate the motion position error of the moving part caused by the discretization error, thereby improving the motion accuracy of the moving part.
The invention also provides a motion control system and a motion control device, which have the same beneficial effects as the motion control method.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required in the prior art and the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a motion control method according to an embodiment of the present invention;
FIG. 2 is a flowchart of a motion control method according to an embodiment of the present invention;
fig. 3 is a schematic structural diagram of a motion control system according to an embodiment of the present invention.
Detailed Description
The core of the invention is to provide a motion control method, a motion control system and a motion control device, which can compensate the motion position error of a motion part caused by discretization error, thereby improving the motion accuracy of the motion part.
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Referring to fig. 1, fig. 1 is a flowchart of a motion control method according to an embodiment of the invention.
The motion control method comprises the following steps:
Step S1: and planning the motion state of the motion part in each interpolation period according to the target motion position and the motion constraint condition of the motion part.
Specifically, the application plans the motion state of each interpolation period of the motion part between the current position and the target motion position according to the target motion position of the motion part (such as a digital processing motion part and a robot) and the motion constraint condition of the motion part moving from the current position to the target motion position, so as to control the motion of the motion part in each interpolation period.
Step S2: controlling the motion of the motion part in the M interpolation period according to the motion state of the M interpolation period; wherein M is more than or equal to 1 and less than or equal to N; n represents the total number of interpolation cycles planned for the moving part.
It should be noted that the total number of interpolation periods planned for the moving part in the present application is N.
Specifically, taking the M (M is more than or equal to 1 and less than or equal to N and M is an integer) interpolation period as an example, after the motion state of the M interpolation period is planned, the motion of the motion component in the M interpolation period is controlled according to the motion state of the M interpolation period, so that the motion component is controlled to move to a target motion position according to the planned motion state.
Step S3: judging whether the motion part reaches a target motion position after the operation of the Nth interpolation period is finished; if not, step S4 is performed. Step S4: and planning the motion state of the motion part in the compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to compensate and move to the target motion position according to the motion state of the compensation period.
Specifically, considering that the motion component may not accurately reach the target motion position after the nth interpolation period operation is finished, the application judges whether the motion component reaches the target motion position after the nth interpolation period operation is finished; if the target motion position is reached, indicating that motion compensation is not needed, ending the motion of the motion component; if the target motion position is not reached, the motion compensation is required, and the motion state of the motion part in the compensation period is planned according to the residual planning displacement between the current position of the motion part and the target motion position, so that the motion part is controlled to carry out compensation motion to the target motion position according to the motion state of the compensation period, and the motion part is ensured to accurately reach the target motion position.
Therefore, the application can compensate the motion position error of the moving part caused by the discretization error, thereby improving the motion accuracy of the moving part.
Based on the above embodiments:
Referring to fig. 2, fig. 2 is a specific flowchart of a motion control method according to an embodiment of the invention.
As an alternative embodiment, planning the motion state of the motion component in each interpolation period according to the target motion position and the motion constraint condition of the motion component includes:
Calculating the residual planning displacement of the moving part in the current interpolation period according to the target moving position of the moving part and the moving position of the moving part in the current interpolation period, and calculating the estimated moving displacement of the moving part under the condition that the axial speed of the moving part in the current interpolation period is reduced to 0 according to the motion constraint condition of the moving part;
judging whether the predicted movement displacement is larger than or equal to the residual planning displacement;
If the speed is greater than or equal to the preset value, planning the shaft speed of the moving part in the next interpolation period according to a preset non-acceleration planning strategy;
If the speed of the motion component is smaller than the preset speed of the motion component in the next interpolation period, planning the shaft speed of the motion component in the next interpolation period according to a preset non-deceleration planning strategy, and executing the step of controlling the motion of the motion component in the next interpolation period according to the shaft speed of the next interpolation period;
And calculating the motion position of the motion part in the next interpolation period according to the shaft speed of the motion part in the next interpolation period, and returning to the step of executing the calculation of the residual planning displacement of the motion part in the current interpolation period until the shaft speeds and the motion positions of the motion part in N interpolation periods are planned.
Specifically, the process of planning the motion state of the motion component in each interpolation period comprises the following steps: 1) Subtracting the motion position of the motion part in the current interpolation period from the target motion position of the motion part to obtain the residual planning displacement of the motion part in the current interpolation period. 2) And calculating the estimated motion displacement of the moving part under the condition that the shaft speed of the current interpolation period is reduced to 0 according to the motion constraint condition of the moving part. 3) Judging whether the predicted movement displacement is larger than or equal to the residual planning displacement; if the predicted movement displacement is greater than or equal to the remaining planned displacement, planning the axial speed of the moving part in the next interpolation period according to a preset non-acceleration planning strategy (the axial speed of the moving part in the next interpolation period is not changed or is reduced in the axial speed at this stage), so as to execute the step of controlling the movement of the moving part in the next interpolation period according to the axial speed of the next interpolation period (namely, the movement state of the embodiment, the moving part moves by a motor, wherein the axial speed refers to the axial speed of the motor in the moving part); if the predicted movement displacement is smaller than the remaining planned displacement, the axial speed of the moving part in the next interpolation period is planned according to a preset non-deceleration planning strategy (the axial speed of the moving part in the next interpolation period is not changed or the axial speed of the moving part is accelerated in the stage), so that the step of controlling the movement of the moving part in the next interpolation period according to the axial speed of the next interpolation period is executed. 4) Calculating the motion position of the motion part in the next interpolation period according to the shaft speed of the motion part in the next interpolation period, and returning to the step of executing the calculation of the residual planning displacement of the motion part in the current interpolation period according to the target motion position of the motion part and the motion position of the motion part in the current interpolation period until the shaft speeds and the motion positions of the motion part in N interpolation periods are planned.
The axial velocity and the movement position of the moving member at the beginning of the first interpolation period are set to known values, so that the axial velocity and the movement position of the remaining interpolation period are planned based on the known values. The motion unit plans the axis speed and the motion position of the next interpolation period in the previous interpolation period, and the axis speed of the next interpolation period may be accelerated, may be unchanged, or may be decelerated relative to the axis speed of the previous interpolation period. The speed in the same interpolation period is the same, namely after the shaft speed of the moving part in the next interpolation period is planned, the moving part is controlled to uniformly run in the next interpolation period according to the planned shaft speed.
As an alternative embodiment, planning the shaft speed of the moving part in the next interpolation period according to a preset non-accelerating planning strategy includes:
calculating the reference axis deceleration of the moving part in the next interpolation period according to the deceleration constraint condition of the moving part, and calculating the reference axis speed of the moving part in the next interpolation period according to the reference axis deceleration;
Judging whether the speed of the reference shaft is smaller than a preset allowable calculation error or not;
If the calculated speed is not smaller than the preset allowable calculation error, taking the reference shaft speed as the shaft speed of the moving part in the next interpolation period;
If the calculated displacement is smaller than the preset allowable calculation error, calculating a displacement increment of the motion component running one interpolation period at the axial speed of the current interpolation period, and judging whether the residual planned displacement of the current interpolation period is smaller than the displacement increment;
If the displacement increment is not smaller than the displacement increment, taking the shaft speed of the current interpolation period as the shaft speed of the moving part in the next interpolation period;
and if the displacement increment is smaller than the displacement increment, determining that N interpolation period operations are finished, and determining that the moving part does not reach the target moving position after the Nth interpolation period operation is finished, so as to execute the step of remaining planning displacement according to the moving part from the target moving position.
Specifically, the planning principle of the non-acceleration planning strategy of the application is as follows: calculating the reference axis deceleration of the moving part in the next interpolation period according to the deceleration constraint condition of the moving part, and calculating the reference axis speed of the moving part in the next interpolation period according to the reference axis deceleration of the moving part in the next interpolation period; judging whether the speed of a reference shaft of the moving part in the next interpolation period is smaller than a preset allowable calculation error or not; if the reference shaft speed of the moving part in the next interpolation period is not less than the preset allowable calculation error, taking the reference shaft speed of the moving part in the next interpolation period as the shaft speed of the moving part in the next interpolation period, namely, reducing the shaft speed of the moving part in the next interpolation period relative to the shaft speed of the last interpolation period; if the reference axis speed of the moving part in the next interpolation period is smaller than a preset allowable calculation error, multiplying the axis speed of the moving part in the current interpolation period by the time length of one interpolation period to obtain a displacement increment of the moving part running in the current interpolation period at the axis speed of the current interpolation period, and judging whether the residual planning displacement of the moving part in the current interpolation period is smaller than the displacement increment of the moving part running in the current interpolation period at the axis speed of the current interpolation period; if the residual planned displacement is not less than the displacement increment, taking the axial speed of the moving part in the current interpolation period as the axial speed of the moving part in the next interpolation period, namely, the axial speed of the moving part in the next interpolation period is unchanged relative to the axial speed of the previous interpolation period; if the remaining planned displacement is smaller than the displacement increment, determining that the N interpolation period is finished, and determining that the motion component does not reach the target motion position after the N-th interpolation period is finished, so as to execute the step of remaining planned displacement according to the motion component from the target motion position, namely, the motion compensation step.
It can be seen that under the planning of the non-accelerating planning strategy, there are three cases of the moving part in the next interpolation period: the shaft is decelerated; the shaft speed is unchanged; the next interpolation period is the compensation period.
As an alternative embodiment, planning the shaft speed of the moving part in the next interpolation period according to a preset non-deceleration planning strategy includes:
judging whether the shaft speed of the moving part in the current interpolation period is smaller than a preset maximum planning shaft speed or not;
If the speed is smaller than the preset maximum planned shaft speed, judging whether the moving part is in a shaft speed reduction state in the current interpolation period;
if the motion component is in the shaft speed-reducing state, taking the shaft speed of the current interpolation period as the shaft speed of the motion component in the next interpolation period;
if the motion component is not in the shaft speed-reducing state, calculating the shaft speed of the motion component in the next interpolation period according to a preset acceleration planning strategy;
and if the preset maximum planned shaft speed is not smaller than the preset maximum planned shaft speed, taking the preset maximum planned shaft speed as the shaft speed of the moving part in the next interpolation period.
Specifically, the planning principle of the non-deceleration planning strategy of the application is as follows: judging whether the shaft speed of the moving part in the current interpolation period is smaller than a preset maximum planning shaft speed or not; if the axis speed of the moving part in the current interpolation period is smaller than the preset maximum planned axis speed, comparing with the previous interpolation period, judging whether the moving part is in an axis deceleration state in the current interpolation period (if the moving part is planned to be axially decelerated in the next interpolation period in the previous interpolation period, setting a preset deceleration flag bit DecFlag to 1, otherwise setting to 0, determining whether the moving part is in an axis deceleration state in the current interpolation period through a value of the preset deceleration flag bit DecFlag, specifically, if DecFlag =1, the moving part is in an axis deceleration state in the current interpolation period, and if DecFlag =0, the moving part is not in an axis deceleration state in the current interpolation period); if the moving part is in an axial deceleration state in the current interpolation period, taking the axial speed of the moving part in the current interpolation period as the axial speed of the moving part in the next interpolation period, namely, relative to the axial speed of the previous interpolation period, the axial speed of the moving part in the next interpolation period is unchanged; if the moving part is not in the shaft speed-reducing state in the current interpolation period, calculating the shaft speed of the moving part in the next interpolation period according to a preset acceleration planning strategy (the moving part is planned to accelerate in the shaft in the next interpolation period in the stage); and if the axial speed of the moving part in the current interpolation period is not less than the preset maximum planning axial speed, taking the preset maximum planning axial speed as the axial speed of the moving part in the next interpolation period.
It can be seen that under the planning of the non-deceleration planning strategy, there are three cases of the moving part in the next interpolation period: accelerating a shaft; the shaft speed is unchanged; run at maximum planned shaft speed.
As an alternative embodiment, calculating the motion position of the motion part in the next interpolation period according to the shaft speed of the motion part in the next interpolation period, and calculating the remaining planned displacement of the motion part in the next interpolation period according to the target motion position of the motion part and the motion position of the motion part in the next interpolation period, includes:
Calculating the motion position of the motion part in the next interpolation period according to CurInterPos (M+1)＝CurInterPos(M)+CurActionSpeed(M+1)*TS; wherein CurInterPos (M) on the right side of the equation is the motion position of the motion part in the current interpolation period; curActionSpeed (M+1) is the shaft speed of the moving part in the next interpolation period; t S is the interpolation period duration; curInterPos (M+1) on the left side of the equation is the motion position of the motion part in the next interpolation period;
Calculating a remaining planning displacement sr of the moving part in the next interpolation period according to sr= CmdPos-CurInterPos (M+1); wherein CmdPos is the target motion position.
Specifically, the process of calculating the residual planning displacement of the motion component in each interpolation period comprises the following steps: calculating a target movement position CmdPos of the moving part according to CmdPos = CurInterPos +distance ①; in this equation ①, curInterPos is the initial motion position of the moving part in the first interpolation period; distance is the total planned displacement of the moving part (both of which are known preset amounts). Calculating a remaining planned displacement sr of the moving part in a first interpolation period according to sr= CmdPos-CurInterPos ②; in this equation ②, curInterPos is the initial motion position of the moving part in the first interpolation period. After calculating the shaft speed CurActionSpeed (M+1) of the moving part in the next interpolation period, calculating the moving position of the moving part in the next interpolation period according to CurInterPos (M+1)＝CurInterPos(M)+CurActionSpeed(M+1)*TS③; in this equation ③, curInterPos (M) on the right side of the equation is the motion position of the motion component in the current interpolation period; curInterPos (M+1) on the left side of the equation is the motion position of the motion component at the next interpolation period, i.e., update CurInterPos. After calculating the movement position CurInterPos (M+1) of the moving part in the next interpolation period, the remaining planned displacement sr of the moving part in the next interpolation period is calculated again according to sr= CmdPos-CurInterPos (M+1), i.e. the sr is updated.
The variable right subscripts (M) and (m+1) of the relational expression in the present application are only for distinguishing the variable values of the variable in the current interpolation period and the next interpolation period.
As an alternative embodiment, calculating the estimated motion displacement of the moving part in the case where the shaft speed of the current interpolation period is decelerated to 0 according to the motion constraint condition of the moving part includes:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed CurActionSpeed of the current interpolation period is decelerated to 0; wherein floor () is a downward rounding function; deceleration is a preset maximum planned axis deceleration; t S is the interpolation period duration; decN is the total number of cycles that the shaft speed of the current interpolation period is decelerated to 0;
If JerkTime is greater than 0, according to
Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed CurActionSpeed of the current interpolation period is decelerated to 0; wherein myJerk is the axis deceleration value for each interpolation period; the speed is a preset maximum planned shaft speed; (2×myn1+myn2) is the total number of cycles that pass by which the shaft speed of the current interpolation period is slowed down to 0.
Specifically, the process of calculating the estimated motion displacement of the motion component under the condition that the shaft speed of the current interpolation period is reduced to 0 according to the preset maximum allowable change period number JerkTime of the acceleration or the deceleration is more than or equal to 0 comprises the following steps: if JerkTime =0, then according to DecN =floor (CurActionSpeed/(Deceleration ×t S))④, calculate the total number of cycles DecN that the motion component's axis speed in the current interpolation period is reduced to 0; in this formula ④, deceleration is the preset maximum planned axis deceleration; curActionSpeed is the motion component's axis speed in the current interpolation period; then according to the followingCalculating an estimated movement displacement sd of the moving part in the case that the shaft speed of the current interpolation period is reduced to 0; in this equation ⑤, curActionSpeed is the shaft speed of the moving part at the current interpolation period. If JerkTime > 0, according to/>Calculating an axis deceleration value myJerk of the moving part at each interpolation period; then according to
Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed of the current interpolation period is reduced to 0; in ⑦, the Velocity is a preset maximum planned shaft speed; (2×myn1+myn2) is the total number of cycles that the motion component has passed by decelerating to 0 in the axial velocity of the current interpolation period; curActionSpeed is the shaft speed of the moving part at the current interpolation period.
As an alternative embodiment, calculating the reference axis deceleration of the moving part in the next interpolation period according to the deceleration constraint condition of the moving part, and calculating the reference axis speed of the moving part in the next interpolation period according to the reference axis deceleration, includes:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to Calculating the reference shaft speed of the moving part in the next interpolation period; wherein DecFlag is a deceleration flag bit; tempDec is the reference axis deceleration of the moving part in the next interpolation period; deceleration is a preset maximum planned axis deceleration; curActionSpeed (M) on the right side of the equation is the shaft speed of the moving part at the current interpolation period; t S is the interpolation period duration; curActionSpeed (M+1) on the left side of the equation is the reference axis speed of the moving part at the next interpolation period;
If JerkTime is greater than 0, according to Calculating the reference shaft speed of the moving part in the next interpolation period; wherein JerkDecN (M) on the right side of the equation is the accumulated interpolation period number of the shaft deceleration of the motion component in the current interpolation period, and the initial value of the accumulated interpolation period number is 0; jerkDecN (M+1) on the left side of the equation is the accumulated interpolation period number of the lower shaft deceleration of the motion component in the next interpolation period; deceleration is a preset maximum planned axis deceleration; jerkDec is the deceleration delta for one interpolation period.
Specifically, the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is more than or equal to 0, based on which, the process of calculating the reference axis speed of the moving part in the next interpolation cycle comprises the following steps: if JerkTime =0, according toCalculating a reference axis deceleration TempDec of the moving part in a next interpolation period; in the formula ⑧, decFlag is a speed reduction flag bit; deceleration is a preset maximum planned axis deceleration; then calculating the reference shaft speed of the moving part in the next interpolation period according to CurActionSpeed (M+1)＝CurActionSpeed(M)-TempDec*TS⑨; in this equation ⑨, tempDec is the reference axis deceleration of the moving part in the next interpolation period; curActionSpeed (M) on the right side of the equation is the shaft speed of the moving part at the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the reference axis speed of the moving part at the next interpolation period, i.e., update CurActionSpeed. If JerkTime > 0, according to/> Calculating an accumulated interpolation period number JerkDecN (M+1) of the lower shaft deceleration of the motion component in the next interpolation period; in ⑩, jerkDecN (M) on the right side of the equation is the cumulative interpolation period number of the axis deceleration of the motion component in the current interpolation period; jerkDecN (M+1) on the left side of the equation is the accumulated interpolation period number of the shaft deceleration of the motion component in the next interpolation period; jerkDecN has an initial value of 0; then according to JerkDec = Deceleration/JerkTime/>Calculating a deceleration increment JerkDec for one interpolation period; according to TempDec = JerkDecN (M+1) × JerkDec/>Calculating a reference axis deceleration TempDec of the moving part in a next interpolation period; according to CurActionSpeed (M+1)＝CurActionSpeed(M)-TempDec*TS/>Calculating the reference shaft speed of the moving part in the next interpolation period; this/>TempDec is the reference axis deceleration of the moving part in the next interpolation period; curActionSpeed (M) on the right side of the equation is the shaft speed of the moving part at the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the reference axis speed of the moving part at the next interpolation period.
As an alternative embodiment, calculating the shaft speed of the moving part in the next interpolation period according to the preset acceleration planning strategy includes:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to
Calculating the shaft speed of the moving part in the next interpolation period; wherein TempAcc is the axial acceleration of the moving part in the next interpolation period; accelerationis a preset maximum planned axis Acceleration; curActionSpeed (M) on the left side of the inequality condition and on the right side of the equation are all the shaft speeds of the moving part in the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the shaft speed of the moving part at the next interpolation period; the speed is a preset maximum planned shaft speed; t S is the interpolation period duration;
If JerkTime is greater than 0, according to
；
Wherein JerkAccN (M) on the right side of the equation is the accumulated interpolation period number of the shaft acceleration of the moving part in the current interpolation period, and the initial value of the accumulated interpolation period number is 0; jerkAccN (M+1) on the left side of the equation is the accumulated interpolation period number of the shaft acceleration of the moving part in the next interpolation period; jerkAcc is the acceleration increment of one interpolation period; tempAcc (M) on the left side of the inequality condition is the axial acceleration of the moving part at the current interpolation period; tempAcc (M+1) in the equation is the axial acceleration of the moving part in the next interpolation period.
Specifically, the maximum allowable variation cycle number JerkTime of the preset acceleration or deceleration is larger than or equal to 0, based on which, the planning process of the acceleration planning strategy of the application comprises: if JerkTime =0, then according to TempAcc =acceptionCalculating the axial acceleration TempAcc of the moving part in the next interpolation period; then judging whether the shaft speed CurActionSpeed (M) of the moving part in the current interpolation period is greater than the preset maximum planning shaft speed vector; if CurActionSpeed (M) is equal to or less than the Velocity, then according to CurActionSpeed (M+1)＝CurActionSpeed(M)+TempAcc*TS/>Calculating the shaft speed of the moving part in the next interpolation period; this/>TempAcc is the axial acceleration of the moving part in the next interpolation period; curActionSpeed (M) on the left side of the inequality condition and on the right side of the equation is the shaft speed of the moving part in the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the shaft speed of the moving part at the next interpolation period; if CurActionSpeed (M) > Vecity, then CurActionSpeed (M+1) =Vecity/>Calculating the shaft speed of the moving part in the next interpolation period; this/>In the equation, curActionSpeed (M+1) on the left side is the shaft speed of the moving part at the next interpolation period. If JerkTime > 0, then according to JerkAccN (M+1)＝JerkAccN(M) +1/>Calculating the accumulated interpolation period number JerkAccN (M+1) of the shaft acceleration of the moving part in the next interpolation period; this/>Wherein JerkAccN (M) on the right side of the equation is the accumulated interpolation period number of the axial acceleration of the motion component in the current interpolation period; jerkAccN (M+1) on the left side of the equation is the accumulated interpolation period number of the axial acceleration of the motion component in the next interpolation period; jerkAccN has an initial value of 0; then according to JerkAcc =acception/JerkTime/>Calculating an Acceleration increment JerkAcc of an interpolation period, judging whether the axial Acceleration TempAcc (M) of the moving component in the current interpolation period is larger than a preset maximum planned axial Acceleration accelerationor not, and if TempAcc (M) is smaller than accelerationor equal to accelerationaccording to TempAcc (M+1)＝JerkAccN(M+1) x JerkAcc/>Calculating the axial acceleration TempAcc (M+1) of the moving part in the next interpolation period; this/>JerkAccN (M+1) is the accumulated interpolation period number of the axial acceleration of the motion component in the next interpolation period; if TempAcc (M) > acception, then according to TempAcc (M+1) =acception/>Calculating the axial acceleration TempAcc (M+1) of the moving part in the next interpolation period; then judging whether the shaft speed CurActionSpeed (M) of the moving part in the current interpolation period is greater than the preset maximum planning shaft speed vector; if CurActionSpeed (M) is equal to or less than the Velocity, then according to CurActionSpeed (M+1)＝CurActionSpeed(M)+TempAcc(M+1)*TS/>Calculating the shaft speed of the moving part in the next interpolation period; this/>TempAcc (M+1) is the axial acceleration of the moving part in the next interpolation period; curActionSpeed (M) on the right side of the equation is the shaft speed of the moving part at the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the shaft speed of the moving part at the next interpolation period; if CurActionSpeed (M) > Vecity, then CurActionSpeed (M+1) =Vecity/>Calculating the shaft speed of the moving part in the next interpolation period; this/>In the equation, curActionSpeed (M+1) on the left side is the shaft speed of the moving part at the next interpolation period.
In addition, as shown in fig. 2, after the next interpolation period is run and after the compensation period is run, whether the running motion position CurInterPos is the target motion position CmdPos and whether CurActionSpeed at that time is equal to 0 is determined, and if the determination results are all yes, the motion control of the motion component is ended; if no, returning to execute the step of calculating the residual planning displacement sr of the moving component in the current interpolation period according to the target moving position of the moving component and the moving position of the moving component in the current interpolation period, and calculating the estimated movement displacement sd of the moving component under the condition that the axial velocity of the moving component in the current interpolation period is reduced to 0 according to the movement constraint condition of the moving component so as to judge whether the estimated movement displacement sd is larger than or equal to the residual planning displacement sr.
In summary, compared with the existing acceleration and deceleration control method, the acceleration and deceleration control method provided by the application ensures planning accuracy by compensating discretization errors. Moreover, the method has lower requirement on computer resources and smaller calculated amount, and can be suitable for a motion controller with lower performance of an MCU (Microcontroller Unit, micro control unit) to efficiently realize real-time planning of the shaft speed. In addition, the input parameters JerkTime can be flexibly adjusted according to the working condition of the executing mechanism to change the pulse frequency or the voltage change rate input to the motor driver, so that flexible speed planning is realized, and mechanical vibration is avoided.
Referring to fig. 3, fig. 3 is a schematic structural diagram of a motion control system according to an embodiment of the invention.
The motion control system includes:
The planning module 1 is used for planning the motion state of the motion component in each interpolation period according to the target motion position and the motion constraint condition of the motion component;
A control module 2, configured to control the motion of the motion component in the mth interpolation period according to the motion state of the mth interpolation period; wherein M is more than or equal to 1 and less than or equal to N, and M is an integer; n represents the total number of interpolation cycles planned for the moving part;
The compensation module 3 is used for judging whether the motion component reaches a target motion position after the operation of the Nth interpolation period is finished; and if the motion state of the motion part in the compensation period is not reached, planning the motion state of the motion part in the compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period.
The description of the motion control system provided by the present application refers to the embodiment of the motion control method, and the present application is not repeated here.
The application also provides a motion control device, comprising:
A memory for storing a computer program;
a processor for implementing the steps of any of the motion control methods described above when executing a computer program.
The description of the motion control device provided by the present application refers to the embodiment of the motion control method, and the present application is not repeated here.
It should also be noted that in this specification, relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (9)
1. A method of motion control, comprising:
Planning the motion state of the motion part in each interpolation period according to the target motion position and motion constraint conditions of the motion part;
Controlling the motion of the motion component in the M-th interpolation period according to the motion state of the M-th interpolation period; wherein M is more than or equal to 1 and less than or equal to N, and M is an integer; n represents the total number of interpolation cycles planned for the moving part;
Judging whether the motion part reaches the target motion position after the Nth interpolation period is finished;
If the motion state of the motion part does not reach the target motion position, planning the motion state of the motion part in a compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period;
The step of planning the motion state of the motion component in each interpolation period according to the target motion position and the motion constraint condition of the motion component comprises the following steps:
Calculating the residual planning displacement of the moving part in the current interpolation period according to the target movement position of the moving part and the movement position of the moving part in the current interpolation period, and calculating the estimated movement displacement of the moving part under the condition that the axial speed of the moving part in the current interpolation period is reduced to 0 according to the movement constraint condition of the moving part;
judging whether the predicted movement displacement is larger than or equal to the residual planning displacement;
if the speed of the motion component is greater than or equal to the preset value, planning the shaft speed of the motion component in the next interpolation period according to a preset non-acceleration planning strategy;
If the motion speed is smaller than the preset value, planning the shaft speed of the motion component in the next interpolation period according to a preset non-deceleration planning strategy so as to execute the step of controlling the motion of the motion component in the next interpolation period according to the shaft speed of the next interpolation period;
And calculating the motion position of the motion part in the next interpolation period according to the shaft speed of the motion part in the next interpolation period, and returning to the step of executing calculation of the residual planning displacement of the motion part in the current interpolation period until the shaft speeds and the motion positions of the motion part in N interpolation periods are planned.
2. The motion control method of claim 1, wherein planning the shaft speed of the moving part in a next interpolation period according to a preset non-acceleration planning strategy comprises:
Calculating the reference axis deceleration of the moving part in the next interpolation period according to the deceleration constraint condition of the moving part, and calculating the reference axis speed of the moving part in the next interpolation period according to the reference axis deceleration;
judging whether the speed of the reference shaft is smaller than a preset allowable calculation error or not;
If the reference shaft speed is not smaller than the preset allowable calculation error, taking the reference shaft speed as the shaft speed of the moving part in the next interpolation period;
If the calculated displacement is smaller than the preset allowable calculation error, calculating the displacement increment of the motion component in one interpolation period at the axial speed of the current interpolation period, and judging whether the residual planned displacement of the current interpolation period is smaller than the displacement increment;
if the displacement increment is not smaller than the displacement increment, taking the shaft speed of the current interpolation period as the shaft speed of the moving part in the next interpolation period;
And if the displacement increment is smaller than the displacement increment, determining that N interpolation period operations are finished, and determining that the moving part does not reach the target moving position after the Nth interpolation period operation is finished, so as to execute the step of remaining planning displacement according to the distance between the moving part and the target moving position.
3. The motion control method of claim 1, wherein planning the shaft speed of the moving part in a next interpolation period according to a preset non-deceleration planning strategy comprises:
judging whether the shaft speed of the moving part in the current interpolation period is smaller than a preset maximum planning shaft speed or not;
If the speed is smaller than the preset maximum planned shaft speed, judging whether the moving part is in a shaft deceleration state in the current interpolation period;
if the motion component is in the shaft speed-reducing state, taking the shaft speed of the current interpolation period as the shaft speed of the motion component in the next interpolation period;
If the motion component is not in the shaft speed-down state, calculating the shaft speed of the motion component in the next interpolation period according to a preset acceleration planning strategy;
and if the preset maximum planned shaft speed is not smaller than the preset maximum planned shaft speed, taking the preset maximum planned shaft speed as the shaft speed of the moving part in the next interpolation period.
4. The motion control method according to claim 1, wherein calculating a motion position of the moving member in a next interpolation period based on an axial velocity of the moving member in the next interpolation period, and calculating a remaining planned displacement of the moving member in the next interpolation period based on a target motion position of the moving member and the motion position thereof in the next interpolation period, includes:
Calculating the motion position of the motion component in the next interpolation period according to CurInterPos (M+1)＝CurInterPos(M)+CurActionSpeed(M+1)*Ts; wherein CurInterPos (M) on the right side of the equation is the motion position of the motion component in the current interpolation period; curActionSpeed (M+1) is the shaft speed of the moving part at the next interpolation period; t S is the interpolation period duration; curInterPos (M+1) on the left side of the equation is the motion position of the motion part in the next interpolation period;
Calculating a remaining planned displacement sr of the moving part in a next interpolation period according to sr= CmdPos-CurInterPos (M+1); wherein CmdPos is the target motion position.
5. The motion control method according to claim 1, wherein calculating an estimated motion displacement of the moving member in a case where an axis speed of a current interpolation period is decelerated to 0, based on a motion constraint condition of the moving member, comprises:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed CurActionSpeed of the current interpolation period is decelerated to 0; wherein floor () is a downward rounding function; deceleration is a preset maximum planned axis deceleration; t S is the interpolation period duration; decN is the total number of cycles that the shaft speed of the current interpolation period is decelerated to 0;
If JerkTime is greater than 0, according to
Calculating an estimated movement displacement sd of the moving part in the case that the shaft speed CurActionSpeed of the current interpolation period is decelerated to 0; wherein myJerk is the axis deceleration value for each interpolation period; the speed is a preset maximum planned shaft speed; (2 x myn1+myn2) is the total number of cycles that pass by which the shaft speed of the current interpolation period is slowed down to 0.
6. The motion control method according to claim 2, wherein calculating a reference axis deceleration of the moving member in a next interpolation period based on a deceleration constraint condition of the moving member, and calculating a reference axis speed of the moving member in the next interpolation period based on the reference axis deceleration, comprises:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to Calculating the reference shaft speed of the moving part in the next interpolation period; wherein DecFlag is a deceleration flag bit; tempDec is the reference axis deceleration of the moving part at the next interpolation period; deceleration is a preset maximum planned axis deceleration; curActionSpeed (M) on the right side of the equation is the shaft speed of the moving part at the current interpolation period; t S is the interpolation period duration; curActionSpeed (M+1) on the left side of the equation is the reference axis speed of the moving part at the next interpolation period;
If JerkTime is greater than 0, according to Calculating the reference shaft speed of the moving part in the next interpolation period; wherein JerkDecN (M) on the right side of the equation is the accumulated interpolation period number of the shaft deceleration of the motion component under the current interpolation period, and the initial value of the accumulated interpolation period number is 0; jerkDecN (M+1) on the left side of the equation is the accumulated interpolation period number of the lower shaft deceleration of the motion component in the next interpolation period; deceleration is a preset maximum planned axis deceleration; jerkDec is the deceleration delta for one interpolation period.
7. A motion control method according to claim 3, wherein calculating the shaft speed of the moving part in the next interpolation period according to a preset acceleration planning strategy comprises:
If the maximum allowable change cycle number JerkTime of the preset acceleration or deceleration is equal to 0, according to
Calculating the shaft speed of the moving part in the next interpolation period; wherein TempAcc is the axial acceleration of the moving part in the next interpolation period; accelerationis a preset maximum planned axis Acceleration; curActionSpeed (M) on the left side of the inequality condition and on the right side of the equality condition are the shaft speeds of the moving part in the current interpolation period; curActionSpeed (M+1) on the left side of the equation is the shaft speed of the moving part at the next interpolation period; the speed is a preset maximum planned shaft speed; t S is the interpolation period duration;
If JerkTime is greater than 0, according to
；
Wherein JerkAccN (M) on the right side of the equation is the accumulated interpolation period number of the shaft acceleration of the moving part in the current interpolation period, and the initial value of the accumulated interpolation period number is 0; jerkAccN (M+1) on the left side of the equation is the accumulated interpolation period number of the shaft acceleration of the moving part in the next interpolation period; jerkAcc is the acceleration increment of one interpolation period; tempAcc (M) on the left side of the inequality condition is the axial acceleration of the moving part at the current interpolation period; tempAcc (M+1) in the equation is the axial acceleration of the moving part at the next interpolation period.
8. A motion control system, comprising:
the planning module is used for planning the motion state of the motion component in each interpolation period according to the target motion position and the motion constraint condition of the motion component;
The control module is used for controlling the motion of the motion component in the M-th interpolation period according to the motion state of the M-th interpolation period; wherein M is more than or equal to 1 and less than or equal to N, and M is an integer; n represents the total number of interpolation cycles planned for the moving part;
The compensation module is used for judging whether the motion component reaches the target motion position after the Nth interpolation period is finished; if the motion state of the motion part does not reach the target motion position, planning the motion state of the motion part in a compensation period according to the residual planning displacement of the motion part from the target motion position, so as to control the motion part to carry out compensation motion to the target motion position according to the motion state of the compensation period;
The planning module is specifically configured to:
Calculating the residual planning displacement of the moving part in the current interpolation period according to the target movement position of the moving part and the movement position of the moving part in the current interpolation period, and calculating the estimated movement displacement of the moving part under the condition that the axial speed of the moving part in the current interpolation period is reduced to 0 according to the movement constraint condition of the moving part;
judging whether the predicted movement displacement is larger than or equal to the residual planning displacement;
if the speed of the motion component is greater than or equal to the preset value, planning the shaft speed of the motion component in the next interpolation period according to a preset non-acceleration planning strategy;
If the motion speed is smaller than the preset value, planning the shaft speed of the motion component in the next interpolation period according to a preset non-deceleration planning strategy so as to execute the step of controlling the motion of the motion component in the next interpolation period according to the shaft speed of the next interpolation period;
And calculating the motion position of the motion part in the next interpolation period according to the shaft speed of the motion part in the next interpolation period, and returning to the step of executing calculation of the residual planning displacement of the motion part in the current interpolation period until the shaft speeds and the motion positions of the motion part in N interpolation periods are planned.
9. A motion control apparatus, comprising:
A memory for storing a computer program;
processor for implementing the steps of the motion control method according to any of claims 1-7 when executing said computer program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111320261.8A CN114019904B (en) | 2021-11-09 | 2021-11-09 | Motion control method, system and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111320261.8A CN114019904B (en) | 2021-11-09 | 2021-11-09 | Motion control method, system and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114019904A CN114019904A (en) | 2022-02-08 |
| CN114019904B true CN114019904B (en) | 2024-05-17 |
Family
ID=80063118
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111320261.8A Active CN114019904B (en) | 2021-11-09 | 2021-11-09 | Motion control method, system and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114019904B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103180791A (en) * | 2010-10-13 | 2013-06-26 | 欧姆龙株式会社 | Control device, control system and control method |
| 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 |
| CN111390902A (en) * | 2020-01-06 | 2020-07-10 | 广州视源电子科技股份有限公司 | Trajectory planning method, trajectory planning device, equipment and storage medium |
| CN113009861A (en) * | 2021-02-08 | 2021-06-22 | 广东智源机器人科技有限公司 | Motor motion control method and device, computer equipment and storage medium |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021036630A1 (en) * | 2019-08-29 | 2021-03-04 | 山东大学 | Laser-cladding joining control method, and laser head dynamic height adjustment method and system |
-
2021
- 2021-11-09 CN CN202111320261.8A patent/CN114019904B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103180791A (en) * | 2010-10-13 | 2013-06-26 | 欧姆龙株式会社 | Control device, control system and control method |
| 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 |
| CN111390902A (en) * | 2020-01-06 | 2020-07-10 | 广州视源电子科技股份有限公司 | Trajectory planning method, trajectory planning device, equipment and storage medium |
| CN113009861A (en) * | 2021-02-08 | 2021-06-22 | 广东智源机器人科技有限公司 | Motor motion control method and device, computer equipment and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 位置随动系统轨迹规划算法研究;张得礼;周来水;;机械科学与技术(03);全文 * |
| 张得礼 ; 周来水 ; .位置随动系统轨迹规划算法研究.机械科学与技术.2013,(03),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114019904A (en) | 2022-02-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106168790B (en) | S-shaped acceleration and deceleration control method for changing target speed and position on line | |
| CN103466452B (en) | Crane operation control method and system | |
| CN101114167A (en) | Numerical controller | |
| CN105814503A (en) | Numerical control device and numerical control method | |
| CN108153246B (en) | Parameter self-adaptive S-shaped speed planning interpolation method based on designated speed | |
| JP2007094936A (en) | Numerical value controller | |
| JP2010170435A (en) | System and method for instructing motion control, and motion control system | |
| US5073748A (en) | Method for limiting the rate-of-change of acceleration in numerical driving systems | |
| US4815007A (en) | Apparatus for controlling a robot | |
| US5070287A (en) | Method for a numerical positioning control system | |
| CN109313429B (en) | S-shaped speed planning method, device and system, robot and numerical control machine | |
| CN106774445B (en) | Servo control device | |
| CN114019904B (en) | Motion control method, system and device | |
| CN110398993B (en) | Speed control method, apparatus and computer readable storage medium | |
| KR0168065B1 (en) | Motor controlling method & device on numeric controlling system | |
| CN115847395A (en) | Method and system for judging in-place movement of extractor, electronic equipment and storage medium | |
| JPS596782A (en) | Speed control device for motor | |
| CN113359620B (en) | Soft limit control method for shaft motion and open type motion controller based on RTX64 | |
| JP5573664B2 (en) | Numerical control apparatus, movement control method, movement control program, and storage medium | |
| JP2007052590A (en) | Numerical controller and numerical control method | |
| CN108847807B (en) | Method, device and equipment for planning motion trail of motor and storage medium | |
| JP2735126B2 (en) | Robot control method and control device thereof | |
| JPS6232508A (en) | Positioning control method for servo-system of numerical controller | |
| JPH04362710A (en) | Optimum acceleration/deceleration control system for servo motor | |
| Zeng et al. | Control Algorithm of Acceleration Curve for Stepper Motor |
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 |