CN115963782A - S-shaped velocity planning method based on non-zero initial acceleration - Google Patents
S-shaped velocity planning method based on non-zero initial acceleration Download PDFInfo
- Publication number
- CN115963782A CN115963782A CN202310046849.1A CN202310046849A CN115963782A CN 115963782 A CN115963782 A CN 115963782A CN 202310046849 A CN202310046849 A CN 202310046849A CN 115963782 A CN115963782 A CN 115963782A
- Authority
- CN
- China
- Prior art keywords
- speed
- acceleration
- planning
- initial
- velocity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000001133 acceleration Effects 0.000 title claims abstract description 160
- 238000000034 method Methods 0.000 title claims abstract description 94
- 230000008859 change Effects 0.000 claims abstract description 35
- 230000008569 process Effects 0.000 claims abstract description 29
- 238000006073 displacement reaction Methods 0.000 claims description 36
- 230000036461 convulsion Effects 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 abstract description 7
- 238000004422 calculation algorithm Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- POFWRMVFWIJXHP-UHFFFAOYSA-N n-benzyl-9-(oxan-2-yl)purin-6-amine Chemical compound C=1C=CC=CC=1CNC(C=1N=C2)=NC=NC=1N2C1CCCCO1 POFWRMVFWIJXHP-UHFFFAOYSA-N 0.000 description 2
- 230000001052 transient effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- 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
- Manipulator (AREA)
Abstract
The invention relates to the technical field of numerical control systems, and discloses an S-shaped speed planning method based on non-zero initial acceleration, which comprises a speed planning layer and a position planning layer, wherein the speed planning layer carries out speed smooth planning meeting requirements according to given input parameters to obtain the duration time of an acceleration adding section, an acceleration reducing section, a uniform acceleration section, an acceleration and deceleration section and a deceleration and deceleration section in a speed change process; and the position planning layer calculates the position, the speed and the acceleration at any moment according to the output result of the speed planning layer. Compared with the traditional speed planning method, the method avoids acceleration step when the motion state is switched, has higher overall stability, can keep the acceleration not zero for the continuous motion with zero-crossing speed, and ensures the optimal time for planning the track. The invention ensures continuous and smooth transition of position, speed and acceleration, ensures smoothness and precision when reaching a tail end point, and ensures optimal time for planning a track.
Description
Technical Field
The invention relates to the technical field of numerical control systems, in particular to an S-shaped speed planning method based on non-zero initial acceleration.
Background
At present, there are many motion speed curves for numerical control systems and industrial robot operation, such as T-type speed planning, S-type speed planning and polynomial speed planning. The speed planning has the function of ensuring that no oscillation, step change and impact are generated when actuators such as a motor and the like are restarted, stopped, changed in speed or in state transition, so that the precision of motion control and the stability of motion are improved.
At present, most speed planning algorithms require that a given initial acceleration and a given final acceleration are zero, so that a given contour speed and a given terminal point speed cannot be adjusted randomly after planning is finished, and the algorithms have the defects of difficulty in realizing functions of speed regulation, pause and the like and cannot realize state switching under the condition that the acceleration is not stepped.
In addition, the formula of the current S-shaped speed planning algorithm is too complex when the maximum speed is calculated, and the derivation and programming difficulty is large, so that the realization is difficult, and the calculation amount is large when the algorithm runs. There is a need for a sigmoidal velocity planning method based on an initial acceleration that is not zero.
Disclosure of Invention
The invention aims to provide an S-shaped velocity planning method based on non-zero initial acceleration, and a smooth track can be planned under the condition that the initial acceleration is not zero. The method can ensure the continuous smooth transition of the position, the speed and the acceleration, ensure the smoothness and the precision when reaching the tail end point, and simultaneously ensure that the acceleration does not have step when the state is switched in the motion process. In addition, the acceleration can be kept not to be zero for the continuous motion of the velocity zero crossing, and the time for planning the track is ensured to be optimal.
The invention is realized in the following way: the method comprises the following steps:
S 1 speed planning is carried out under three different conditions of same-direction acceleration, same-direction deceleration and initial and final speed reversal of the industrial robot;
firstly, when accelerating in the same direction, setting | v _ e | > | v _ s |, the method can be divided into the following steps according to a _ s direction: the a _ s direction is the same as the speed change direction and the a _ s direction is opposite to the speed change direction 2 cases;
S 1。1 when the direction of a _ s is the same as the speed change direction, the speed change amount deltaV1 of a _ s directly reduced to 0 is first calculated, and the following two cases are divided according to whether the deltaV1 is greater than | v _ e-v _ s |: the speed does not exceed v _ e or the speed exceeds v _ e;
wherein, the speed does not exceed v _ e, if the acceleration can reach the profile acceleration accel, the orbit is divided into: the acceleration is from a _ s to accel, the acceleration is uniformly accelerated according to the accel, and the acceleration is from the accel to 0; if the acceleration can not reach the contour acceleration accel, firstly calculating the actually achievable acceleration: a _ m = sqrt ((| v _ e-v _ s | + a _ s ^ 2/jerk/2) × jerk);
if the velocity exceeds v _ e, the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, and the speed is reduced from v1 to v _ e; if the acceleration cannot reach the profile deceleration decel, the achievable deceleration is first calculated: a _ m = -sqrt (| v _ e-v1 |).
When the industrial robots decelerate in the same direction;
the initial velocity v _ s and the ending velocity v _ e are in the same direction and | v _ e | < | v _ s | can be divided into the following 2 cases according to the direction of the initial acceleration a _ s:
S 2.1 : the initial acceleration direction is the same as the speed change direction;
first, a speed variation deltaV1 of a _ s directly reduced to 0 is calculated, and the following three cases are classified into 3 cases according to whether deltaV1 is greater than | v _ e-v _ s | and | v _ s |: the speed will not exceed v _ e, the speed will exceed v _ e but will not reverse, the speed will exceed v _ e and will reverse;
wherein the speed does not exceed v _ e, and if the acceleration can reach the profile deceleration decel, the track is divided into: a _ s is accelerated to decel, and is uniformly decelerated by the decel, and the acceleration is from the decel to 0;
if the acceleration cannot reach the profile deceleration decel, the actually achievable deceleration is first calculated: a _ m = -sqrt ((| v _ e-v _ s | + a _ s ^ 2/jerk/2) × jerk), when the velocity exceeds v _ e but not reverses, the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, and the speed is accelerated from v1 to v _ e;
if the acceleration can not reach the contour acceleration accel, firstly calculating the achievable acceleration: a _ m = -sqrt (| v _ e-v1 |), the speed exceeds v _ e at the moment and the direction is reversed, and the speed reversing method is used for planning;
S 2.2 : the initial acceleration direction is opposite to the speed change direction;
the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, v1 is accelerated to v _ e, if the profile deceleration decel can not be reached in the second step, the reachable deceleration is firstly calculated: a _ m = -sqrt (| v _ e-v1 |).
When the initial and final speeds of the industrial robot are reversed; when the speed is subjected to deceleration zero crossing and reverse acceleration, the acceleration is not 0, and the obtained track is the time optimal track; first, the speed variation deltaV1 of the acceleration directly reduced from a _ s to 0 is calculated, and the method is divided into the following steps according to whether the deltaV1 is larger than | v _ e-v _ s |: the speed can exceed v _ e, and the speed can not exceed v _ e;
further, when the velocity would exceed v _ e, the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, v1 is accelerated to v _ e, if the profile deceleration decel can not be reached, the reachable deceleration is firstly calculated: a _ m = -sqrt (| v _ e-v1 |);
when the speed does not exceed v _ e, the method is divided into the following steps according to the initial acceleration direction: the initial acceleration and the speed change direction are the same, and the initial acceleration and the speed change direction are opposite;
when the change directions are the same, the track is divided into: v _ s is reduced to 0, and the speed is accelerated from 0 to v _ e;
when the change directions are opposite, the track is divided into: a _ s is reduced to 0 and the velocity v1, v1 is accelerated to v _ e.
S 2 According to the influence of the intermediate speed under different initial and final speeds on the total displacement, the method can be divided into the following conditions:
the initial and final speeds are all larger than 0, and the initial acceleration is larger than 0;
the initial and final speeds are all larger than 0, and the initial acceleration is smaller than 0;
the initial speeds are all larger than 0, the final speeds are smaller than 0, and the initial acceleration is larger than 0;
the initial speeds are all larger than 0, the final speed is smaller than 0, and the initial acceleration is smaller than 0;
the initial speed is less than 0, the final speed is greater than 0, and the initial acceleration is greater than 0;
the initial speed is less than 0, the final speed is greater than 0, and the initial acceleration is less than 0;
the initial and final speeds are all less than 0, and the initial acceleration is greater than 0;
the initial and final speeds are all less than 0, and the initial acceleration is less than 0;
S 3 : planning the track of the industrial robot, specifically finding out an intermediate speed v _ m which can be reached according to the segmentation method, and then respectively planning the tracks of two processes from v _ s to v _ m and from v _ m to v _ e by using a speed planning methodAnd (4) tracing.
Further, when the industrial robot can reach the positive profile velocity v _ p, the velocity planning method is firstly used for respectively planning two processes from v _ s to v _ p and from v _ p to v _ e, if the total displacement is larger than the sum of the displacements of the two processes, the situation that the profile velocity v _ p can be reached is judged, and at the moment, the whole track can be divided into the following three stages:
in the first stage, from v _ S to v _ p, a speed planning method is used for planning, and the total displacement is S1;
in the second stage, the motion is performed at a constant speed according to v _ p, the displacement S2= S-S1-S3, and the time T2= S2/v _ p;
in the third stage, from v _ p to v _ e, planning by using a speed planning method, wherein the total displacement is S3;
further, when the industrial robot can reach the negative profile speed-v _ p,
firstly, planning the displacement S _ fast from v _ S direct deceleration or deceleration to v _ e by using a speed planning method, and then respectively planning the two processes from v _ S to-v _ p and from-v _ p to v _ e by using the speed planning method, if the total displacement is
(L = | p _ e-p _ S |) less than S _ fast and less than the sum of the two process displacements (L1 + L2) then the velocity needs to be reversed to reach the reverse profile velocity-v _ p, where the whole trajectory can be divided into the following three phases:
the first stage is from v _ s to-v _ p, planning by velocity planning method, total displacement is L1
The second stage moves at a constant speed according to-v _ p, the displacement is L2= L1+ L3-L, and the time T2= L2/v _ p
The third stage is from-v _ p to v _ e, planning is carried out by using a speed planning method, and the total displacement is L3
When the industrial robot cannot reach the contour velocity, the actually achievable velocity is calculated using a binary interpolation method.
Further, the present invention provides a computer readable storage medium having stored thereon a computer program which, when executed by a master controller, implements the method of any one of the above.
Compared with the prior art, the invention has the beneficial effects that:
1. a smooth track can be planned under the condition that the initial acceleration is not zero. The method can ensure the continuous smooth transition of the position, the speed and the acceleration, ensure the smoothness and the precision when reaching the tail end point, and simultaneously ensure that the acceleration does not have step when the state is switched in the motion process. In addition, the acceleration can be kept not to be zero for the continuous motion with zero-crossing speed, and the time for planning the track is ensured to be optimal.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are required to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained according to the drawings without inventive efforts.
FIG. 1 is a flow chart of the method of the present invention;
FIG. 2 is a diagram of the planning effect of other planning algorithms on the acceleration step problem;
FIG. 3 is a diagram of the effect of the present invention on the planning of the acceleration step problem;
FIG. 4 is a graph of the effect of other planning algorithms on the trajectory planning through 0 speed;
FIG. 5 is a graph of the effect of the algorithm of the present invention on the trajectory planning for a speed pass through 0.
Detailed Description
In order to make 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 of the embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, are within the scope of the present invention. Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, are within the scope of the present invention.
Referring to fig. 1-5, a method for S-shaped velocity planning based on non-zero initial acceleration,
in this embodiment, the method specifically comprises the following steps: the speed gauge divides the following three cases: acceleration in the same direction, deceleration in the same direction and reverse initial and final speeds. A speed plan that divides the following three cases: acceleration in the same direction, deceleration in the same direction and reversal of initial and final speeds.
S 1 : when the industrial robots accelerate in the same direction, the initial speed v _ s and the ending speed v _ e are in the same direction and | v _ e>L v _ s l, the following 2 cases can be classified according to the initial acceleration a _ s direction: the initial acceleration direction is the same as the speed change direction and the initial acceleration direction is opposite to the speed change direction.
S 1.1 : when the initial acceleration direction is the same as the speed change direction, first, the speed change amount deltaV1= a _ s ^2/jerk/2, in which the acceleration is directly reduced from a _ s to 0, is calculated, and the following two cases are divided according to whether deltaV1 is greater than | v _ e-v _ s |: the speed will not exceed v _ e, the speed will exceed v _ e.
a) The speed does not exceed v _ e, and if the acceleration can reach the profile acceleration accel, the track is divided into the following 3 processes:
firstly, the acceleration is from a _ s to accel;
secondly, uniformly accelerating according to the acell;
thirdly, acceleration is from acell to 0;
if the acceleration can not reach the contour acceleration accel, firstly calculating the acceleration a _ m which can be actually reached:
deltaV＝|v_e-v_s|，a_m＝sqrt((deltaV+a_s^2/jerk/2)*jerk)
b) When the speed exceeds v _ e, the trajectory is divided into the following two processes:
first, acceleration goes from a _ s to 0, velocity goes to v1
Second, the speed is decelerated from v1 to v _ e
In this embodiment, if the acceleration cannot reach the profile deceleration decel in the second step, the achievable deceleration is first calculated:
deltaV＝|v_e-v1|，a_m＝-sqrt(deltaV)
S 1.2 : when the initial acceleration direction is opposite to the speed change direction, the speed change amount deltaV1= a _ s ^2/jerk/2.0 of the acceleration directly reduced from a _ s to 0 is calculated first, and the following two cases are divided according to whether deltaV1 is greater than | v _ s |: the speed is not reversed and the speed will be reversed.
a) When the speed is not reversed, the trajectory is divided into the following two processes:
first, acceleration from a _ s to 0, velocity to v1
Second, the speed is accelerated from v1 to v _ e
And secondly, if the acceleration cannot reach the contour acceleration accel, firstly calculating the achievable acceleration:
deltaV＝|v_e-v1|，a_m＝sqrt(deltaV)
b) And when the speed is not reversed, planning by using a speed reversing method.
S 2 : when decelerating in the same direction
The initial velocity v _ s and the ending velocity v _ e are in the same direction and | v _ e | < | v _ s | can be divided into the following 2 cases according to the direction of the initial acceleration a _ s:
S 2.1 : the initial acceleration direction is the same as the speed change direction
First, the speed variation deltaV1 when a _ s is directly reduced to 0 is calculated, and the following three cases are classified according to whether deltaV1 is larger than | v _ e-v _ s | and | v _ s |: speed will not exceed v _ e, speed will exceed v _ e but will not reverse, speed will exceed v _ e and will reverse.
a) The speed does not exceed v _ e, and if the acceleration can reach the profile deceleration decel, the track is divided into the following 3 processes:
firstly, acceleration is from a _ s to decel;
secondly, uniformly accelerating according to decel;
thirdly, acceleration from decel to 0
When the acceleration can not reach the profile deceleration decel, firstly calculating the reachable deceleration a _ m;
deltaV＝|v_e-v_s|，a_m＝-sqrt((deltaV+a_s^2/jerk/2)*jerk)
b) The velocity will exceed v _ e but will not reverse, and the trajectory will be divided into the following two processes:
step one, the acceleration is from a _ s to 0, and the speed reaches v1;
secondly, accelerating the speed from v1 to v _ e;
when the acceleration cannot reach the contour acceleration accel in the second step, firstly calculating the reachable acceleration:
deltaV＝|v_e-v1|，a_m＝sqrt(deltaV)；
the velocity will exceed v _ e and will reverse, planning using the velocity reversal method.
S 2.2 : the direction of initial acceleration is opposite to the direction of speed change
The trajectory is divided into the following 2 processes:
the first step, the acceleration is from a _ s to 0, and the speed reaches v1;
secondly, accelerating the speed from v1 to v _ e;
when the acceleration cannot reach the profile deceleration decel in the second step, the achievable deceleration is firstly calculated as follows:
deltaV＝|v_e-v1|，a_m＝-sqrt(deltaV)
S 3 : speed reversal
First, the speed variation deltaV1 when a _ s is directly reduced to 0 is calculated, and the following two cases are divided according to whether deltaV1 is greater than | v _ e-v _ s |: the speed will exceed v _ e and the speed will not exceed v _ e.
S 3.1 : the speed will exceed v _ e
The trajectory is divided into the following 2 processes:
the first step, the acceleration is from a _ s to 0, and the speed reaches v1;
secondly, accelerating the speed from v1 to v _ e;
when the acceleration cannot reach the profile deceleration decel in the second step, the achievable deceleration is firstly calculated:
deltaV＝|v_e-v1|，a_m＝-sqrt(deltaV)；
in this example, S 3.2 : the speed does not exceed v _ e;
the following two cases are classified according to the initial acceleration direction: the initial acceleration and the speed change direction are the same, and the initial acceleration and the speed change direction are opposite.
When the initial acceleration and the speed change direction are the same, the track is divided into: v _ s decelerates to 0, accelerates from 0 to v _ e2 processes. The key is to select the appropriate transient acceleration at 0 to optimize the overall time:
calculating the acceleration a _ m that can be reached when decelerating from v _ s to 0, if a _ m is less than a _ s:
when a _ s ^2/jerk/2> | v _ s |, a1_ min = sqrt ((a _ s ^2/jerk/2- | v _ s |)/jerk | 2).
When a _ s ^2/jerk/2< = | v _ s |, a1_ min =0.
If decel is greater than | a _ s |, a1_ max = sqrt ((| v _ s | + a _ s ^ 2/jerk/2) × jerk ^ 2).
If decel < = | a _ s |, a1_ max =0.
Calculating the acceleration a _ m that can be reached when the speed is accelerated from 0 to v _ e:
a2_m＝sqrt(|v_e|*jerk*2)
a2_m＝min(a2_m,min(accel,decel))
in this embodiment, the transient acceleration when the optimal speed passes through 0 is determined according to the accelerations that can be achieved in the two processes:
if a2_ m is greater than a1_ max, the transition speed a _ m = a1_ max.
If a2_ m is smaller than a1_ min, the transition speed, a _ m = a1_ min.
If a2_ m is between a1_ min and a1_ max, the transition speed a _ m = a2_ m.
After determining the transitional acceleration, two processes from v _ s to 0 and from 0 to v _ e are planned respectively.
In this embodiment, when the initial acceleration and the speed change direction are opposite, the trajectory is divided into the following 2 processes:
first, acceleration goes from a _ s to 0, velocity goes to v1
And secondly, programming the acceleration of the speed from v1 to v _ e according to the method that the initial acceleration and the speed change direction are the same.
In the embodiment, the position of the industrial robot is planned;
s4: according to the influence of the intermediate speed under different initial and final speeds on the total displacement, the method can be divided into the following steps:
a. the initial and final speeds are all larger than 0, and the initial acceleration is larger than 0;
b. the initial and final speeds are all larger than 0, and the initial acceleration is smaller than 0;
c. the initial speeds are all larger than 0, the final speed is smaller than 0, and the initial acceleration is larger than 0;
d. the initial speeds are all larger than 0, the final speeds are smaller than 0, and the initial acceleration is smaller than 0;
e. the initial speed is less than 0, the final speed is greater than 0, and the initial acceleration is greater than 0;
f. the initial speed is less than 0, the final speed is greater than 0, and the initial acceleration is less than 0;
g. the initial and final speeds are all less than 0, and the initial acceleration is greater than 0;
h. the initial and final speeds are all less than 0, and the initial acceleration is less than 0;
S 4.1 : the forward profile velocity v _ p can be reached;
firstly, planning two processes from v _ s to v _ p and from v _ p to v _ e respectively by using a speed planning method, if the total displacement is greater than the sum of the displacements of the two processes, indicating that the profile speed v _ p can be reached, and then the whole track can be divided into the following three stages:
the first stage is from v _ S to v _ p, and is planned by using a velocity planning method, wherein the total displacement is S1
The second stage moves at a constant speed according to v _ p, the displacement S2= S-S1-S3, and the time T2= S2/v _ p
In the third stage, from v _ p to v _ e, the speed planning method is used for planning, and the total displacement is S3
In this embodiment, when the velocity of the industrial robot can reach the negative profile velocity-v _ p, the displacement S _ fast from v _ S to v _ e is planned by the velocity planning method.
And respectively planning two processes from v _ s to-v _ p and from-v _ p to v _ e by using a speed planning method, wherein if the total displacement (L = | p _ e-p _ s |) is less than L _ fast and less than the sum of the displacements (L1 + L2) of the two processes, the speed needs to be reversed to reach a reverse profile speed-v _ p, and the whole track can be divided into the following three stages:
the first stage is from v _ s to-v _ p, and is planned by velocity planning method, and the total displacement is L 1
The second stage moves at a constant speed according to-v _ p, the displacement is S2= S1+ S3-S, and the time T2= S2/v _ p
The third stage is from-v _ p to v _ e, planning is carried out by using a speed planning method, and the total displacement is L 3
S 4.3 : the profile speed cannot be reached;
at this point, the actual achievable speed can be calculated using a binary interpolation method.
S 5 : trajectory planning
Finding the intermediate speed v _ m which can be reached according to the segmentation method, and then respectively planning the tracks of the two processes from v _ s to v _ m and from v _ m to v _ e by using a speed planning method.
S 6 : by giving the same input parameters, the planning effect of the algorithm on the acceleration step problem and the velocity zero-crossing optimization problem is compared with that of the S-shaped velocity planning algorithm with zero initial acceleration, the simulation results are shown in figures 2 to 5, and a velocity curve (upper) is compared with an acceleration curve (lower).
In this embodiment, the invention provides a computer-readable storage medium having stored thereon a computer program which, when executed by a host controller, implements the method of any one of the above.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (6)
1. An S-shaped velocity planning method based on non-zero initial acceleration is characterized in that: the method comprises the following steps:
S 1 speed planning is carried out under three different conditions of same-direction acceleration, same-direction deceleration and initial and final speed reversal of the industrial robot;
S 2 according to the influence of the intermediate speed under different initial and final speeds on the total displacement, the method can be divided into the following conditions:
the initial and final speeds are all larger than 0, and the initial acceleration is larger than 0;
the initial and final speeds are all larger than 0, and the initial acceleration is smaller than 0;
the initial speeds are all larger than 0, the final speeds are smaller than 0, and the initial acceleration is larger than 0;
the initial speeds are all larger than 0, the final speeds are smaller than 0, and the initial acceleration is smaller than 0;
the initial speed is less than 0, the final speed is greater than 0, and the initial acceleration is greater than 0;
the initial speed is less than 0, the final speed is greater than 0, and the initial acceleration is less than 0;
the initial and final speeds are all less than 0, and the initial acceleration is greater than 0;
the initial and final speeds are all less than 0, and the initial acceleration is less than 0;
S 3 : and planning the track of the industrial robot, specifically finding an intermediate velocity v _ m which can be reached according to the segmentation method, and then respectively planning the track of two processes from v _ s to v _ m and from v _ m to v _ e by using a velocity planning method.
2. The S-shaped velocity planning method based on non-zero initial acceleration as claimed in claim 1, wherein in step S 1 In the first acceleration in the same direction, | v _ e>L v _ s l, according to a _ s direction, can be divided into: the a _ s direction is the same as the speed change direction and the a _ s direction is opposite to the speed change direction 2 cases;
S 1。1 when the direction of a _ s is the same as the speed change direction, the speed change amount deltaV1 of a _ s directly reduced to 0 is first calculated, and the following two cases are divided according to whether the deltaV1 is greater than | v _ e-v _ s |: the speed does not exceed v _ e or the speed exceeds v _ e;
wherein, the speed does not exceed v _ e, if the acceleration can reach the profile acceleration accel, the orbit is divided into: the acceleration is uniformly accelerated from a _ s to the accel according to the accel, and the acceleration is from the accel to 0; if the acceleration can not reach the contour acceleration accel, firstly calculating the actually achievable acceleration: a _ m = sqrt ((| v _ e-v _ s | + a _ s ^ 2/jerk/2) × jerk);
if the velocity exceeds v _ e, the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, and the speed is reduced from v1 to v _ e; if the acceleration cannot reach the profile deceleration decel, the achievable deceleration is first calculated: a _ m = -sqrt (| v _ e-v1 |).
3. The S-shaped speed planning method based on non-zero initial acceleration according to claim 2, characterized in that when the industrial robots decelerate in the same direction;
the initial velocity v _ s and the ending velocity v _ e are in the same direction and | v _ e | < | v _ s | can be divided into the following 2 cases according to the direction of the initial acceleration a _ s:
S 2.1 : the initial acceleration direction is the same as the speed change direction;
first, a speed variation deltaV1 in which a _ s is directly reduced to 0 is calculated, and the following three cases are classified into 3 cases according to whether deltaV1 is greater than | v _ e-v _ s | and | v _ s |: the speed will not exceed v _ e, the speed will exceed v _ e but will not reverse, the speed will exceed v _ e and will reverse;
wherein the speed does not exceed v _ e, if the acceleration can reach the profile deceleration decel, the track is divided into: a _ s is accelerated to decel, and is uniformly decelerated by the decel, and the acceleration is from the decel to 0;
if the acceleration cannot reach the profile deceleration decel, the actually achievable deceleration is first calculated: a _ m = -sqrt ((| v _ e-v _ s | + a _ s ^ 2/jerk/2) × jerk), when the speed exceeds v _ e but not reverses, the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, and the speed is accelerated from v1 to v _ e;
if the acceleration can not reach the contour acceleration accel, firstly calculating the achievable acceleration: a _ m = -sqrt (| v _ e-v1 |), the speed exceeds v _ e and the direction is reversed, and the speed reversing method is used for planning;
S 2.2 : initial accelerationThe direction is opposite to the speed change direction;
the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, v1 is accelerated to v _ e, if the profile deceleration decel can not be reached in the second step, the reachable deceleration is firstly calculated: a _ m = -sqrt (| v _ e-v1 |).
4. The S-shaped velocity planning method based on non-zero initial acceleration according to claim 3, characterized in that when the initial and final velocities of the industrial robot are reversed; when the speed is subjected to deceleration zero crossing and reverse acceleration, the acceleration is not 0, and the obtained track is the time optimal track; first, the speed variation deltaV1 of the acceleration directly reduced from a _ s to 0 is calculated, and the method is divided into the following steps according to whether the deltaV1 is larger than | v _ e-v _ s |: the speed can exceed v _ e, and the speed can not exceed v _ e;
S 4.1 : when the velocity would exceed v _ e, the trajectory is divided into: a _ s is reduced to 0, the speed reaches v1, v1 is accelerated to v _ e, if the profile deceleration decel can not be reached, the reachable deceleration is firstly calculated: a _ m = -sqrt (| v _ e-v1 |);
S 4.2 : when the speed does not exceed v _ e, the method is divided into the following steps according to the initial acceleration direction: the initial acceleration and the speed change direction are the same, and the initial acceleration and the speed change direction are opposite;
when the change directions are the same, the track is divided into: v _ s is reduced to 0, and the speed is accelerated from 0 to v _ e;
when the change directions are opposite, the track is divided into: a _ s is reduced to 0, and the velocity v1, v1 is accelerated to v _ e.
5. A method of S-shaped velocity planning based on non-zero initial acceleration according to claim 4,
S 5.1 when the industrial robot can reach a positive profile velocity v _ p, respectively planning two processes from v _ s to v _ p and from v _ p to v _ e by using a velocity planning method, and judging that the profile velocity v _ p can be reached if the total displacement is greater than the sum of the displacements of the two processes, wherein the whole track can be divided into the following three stages:
in the first stage, from v _ S to v _ p, planning by using a speed planning method, wherein the total displacement is S1;
the second stage moves at a constant speed according to v _ p, the displacement is S2= S-S1-S3, and the time T2= S2/v _ p;
in the third stage, planning is carried out from v _ p to v _ e by using a speed planning method, and the total displacement is S3;
S 5.2 : when the industrial robot can reach the negative profile velocity-vp,
firstly, planning the displacement S _ fast from v _ S direct deceleration or deceleration to v _ e by using a speed planning method, then respectively planning the two processes from v _ S to-v _ p and from-v _ p to v _ e by using the speed planning method, and if the total displacement is generated
(L = | p _ e-p _ S |) less than S _ fast and less than the sum of the two process displacements (L1 + L2) then the velocity needs to be reversed to reach the reverse profile velocity-v _ p, where the whole trajectory can be divided into the following three phases:
the first stage is from v _ s to-v _ p, planning by velocity planning method, total displacement is L1
The second stage moves at a constant speed according to-v _ p, wherein the displacement is L2= L1+ L3-L, and the time T2= L2/v _ p
The third stage is from-v _ p to v _ e, planning is carried out by using a speed planning method, and the total displacement is L3
S 5.3 : when the industrial robot cannot reach the contour velocity, the actually achievable velocity is calculated using a binary interpolation method.
6. A computer-readable storage medium, on which a computer program is stored, which, when executed by a master controller, implements the method of any one of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310046849.1A CN115963782B (en) | 2023-01-31 | 2023-01-31 | S-shaped speed planning method based on initial acceleration being non-zero |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310046849.1A CN115963782B (en) | 2023-01-31 | 2023-01-31 | S-shaped speed planning method based on initial acceleration being non-zero |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115963782A true CN115963782A (en) | 2023-04-14 |
| CN115963782B CN115963782B (en) | 2023-08-18 |
Family
ID=87358055
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310046849.1A Active CN115963782B (en) | 2023-01-31 | 2023-01-31 | S-shaped speed planning method based on initial acceleration being non-zero |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115963782B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116166061A (en) * | 2023-04-26 | 2023-05-26 | 中国农业大学 | Unmanned speed control method and device, unmanned plane and electronic equipment |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102467109A (en) * | 2010-11-12 | 2012-05-23 | 上海微电子装备有限公司 | Realization method for efficient and precise motion trajectory planning |
| CN103135501A (en) * | 2013-02-01 | 2013-06-05 | 北京配天大富精密机械有限公司 | Acceleration and deceleration controlling method based on S-shaped curve and equipment using the same and numerically-controlled machine tool |
| US20160224018A1 (en) * | 2015-01-29 | 2016-08-04 | National Taiwan University | Trajectory Planning System For Integrated Computer Numerical control (CNC) Machine, Trajectory Planning Device, Trajectory Planning method, And Computer Program Thereof |
| CN106354102A (en) * | 2016-10-18 | 2017-01-25 | 西平县朗玛数控设备有限公司 | Control method of S-shaped curve speed with non-zero initial accelerated speed |
| CN108279644A (en) * | 2018-02-02 | 2018-07-13 | 上海维宏电子科技股份有限公司 | Linear interpolation control method based on superposition instruction |
| CN109901518A (en) * | 2019-02-28 | 2019-06-18 | 清华大学 | A kind of numerically-controlled machine tool acceleration and deceleration speed planning method under constant force constraint condition |
| CN109991934A (en) * | 2019-03-10 | 2019-07-09 | 天津大学 | A kind of time optimal online S type deceleration planning method |
-
2023
- 2023-01-31 CN CN202310046849.1A patent/CN115963782B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102467109A (en) * | 2010-11-12 | 2012-05-23 | 上海微电子装备有限公司 | Realization method for efficient and precise motion trajectory planning |
| CN103135501A (en) * | 2013-02-01 | 2013-06-05 | 北京配天大富精密机械有限公司 | Acceleration and deceleration controlling method based on S-shaped curve and equipment using the same and numerically-controlled machine tool |
| US20160224018A1 (en) * | 2015-01-29 | 2016-08-04 | National Taiwan University | Trajectory Planning System For Integrated Computer Numerical control (CNC) Machine, Trajectory Planning Device, Trajectory Planning method, And Computer Program Thereof |
| CN106354102A (en) * | 2016-10-18 | 2017-01-25 | 西平县朗玛数控设备有限公司 | Control method of S-shaped curve speed with non-zero initial accelerated speed |
| CN108279644A (en) * | 2018-02-02 | 2018-07-13 | 上海维宏电子科技股份有限公司 | Linear interpolation control method based on superposition instruction |
| CN109901518A (en) * | 2019-02-28 | 2019-06-18 | 清华大学 | A kind of numerically-controlled machine tool acceleration and deceleration speed planning method under constant force constraint condition |
| CN109991934A (en) * | 2019-03-10 | 2019-07-09 | 天津大学 | A kind of time optimal online S type deceleration planning method |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116166061A (en) * | 2023-04-26 | 2023-05-26 | 中国农业大学 | Unmanned speed control method and device, unmanned plane and electronic equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115963782B (en) | 2023-08-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106168790B (en) | S-shaped acceleration and deceleration control method for changing target speed and position on line | |
| JP3900789B2 (en) | Motor speed / acceleration determination method, acceleration / deceleration generation method, acceleration / deceleration control method, acceleration / deceleration control device, and motor control device | |
| US9678500B2 (en) | Machining program creating device numerical control device, machining system, machining program creating method, numerical control method, and machining program | |
| CN108319228B (en) | Acceleration and deceleration control method in numerical control system trajectory planning | |
| CN110879569B (en) | Prospective control method | |
| CN111158318B (en) | Flexible acceleration and deceleration planning method for asymmetric quartic curve | |
| CN115963782B (en) | S-shaped speed planning method based on initial acceleration being non-zero | |
| CN101477354A (en) | Position S type instruction generation method | |
| CN112965443B (en) | High-precision interpolation control method for corner trajectory tracking of cutting bed | |
| CN109901518B (en) | Method for planning acceleration and deceleration speed of numerical control machine tool under constant force constraint condition | |
| JP2016172293A (en) | Trajectory generation apparatus for robot to generate trajectory including curved portion | |
| CN107272598A (en) | Nurbs curve interpolating method known to a kind of machining path length | |
| CN103941647A (en) | Flexible accelerating and decelerating control interpolation method for embedded NC equipment | |
| CN105867376A (en) | Industrial robot track planning vertical speed control method based on trigonometric function | |
| CN110398993B (en) | Speed control method, apparatus and computer readable storage medium | |
| CN108829044B (en) | S-curve acceleration and deceleration planning method for coping with negative values of starting and ending speeds | |
| CN113311783A (en) | S-shaped motion curve planning method and system | |
| CN113189938A (en) | Speed curve planning method for continuous machining path | |
| JP4627740B2 (en) | Numerical controller | |
| CN115480481A (en) | Uniaxial high-order constraint acceleration and deceleration control method | |
| KR20200047123A (en) | Numerical control devices using virtual machine tool to limit machining error | |
| JP4407083B2 (en) | Command value generation method and command value generation system | |
| JP2001216008A (en) | Robot control method | |
| JP5028674B2 (en) | Command generation method and command generation apparatus for moving body | |
| CN118226802B (en) | Speed planning method, device, numerical control equipment, numerical control machine tool, medium and product |
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 |