CN111665851B - Trajectory planning method and device for dynamically adjusting running speed of robot - Google Patents
Trajectory planning method and device for dynamically adjusting running speed of robot Download PDFInfo
- Publication number
- CN111665851B CN111665851B CN202010613688.6A CN202010613688A CN111665851B CN 111665851 B CN111665851 B CN 111665851B CN 202010613688 A CN202010613688 A CN 202010613688A CN 111665851 B CN111665851 B CN 111665851B
- Authority
- CN
- China
- Prior art keywords
- jerk
- acceleration
- max
- speed
- change law
- 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
- 238000000034 method Methods 0.000 title claims abstract description 69
- 230000036461 convulsion Effects 0.000 claims abstract description 176
- 230000001133 acceleration Effects 0.000 claims abstract description 156
- 230000008859 change Effects 0.000 claims abstract description 153
- 230000033001 locomotion Effects 0.000 claims abstract description 87
- 230000008569 process Effects 0.000 claims abstract description 29
- 230000007704 transition Effects 0.000 abstract description 4
- 230000014509 gene expression Effects 0.000 description 11
- 239000000126 substance Substances 0.000 description 8
- 238000012885 constant function Methods 0.000 description 7
- 230000010354 integration Effects 0.000 description 6
- 238000013461 design Methods 0.000 description 4
- 230000007423 decrease Effects 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011089 mechanical engineering Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001172 regenerating effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention discloses a track planning method and a device for dynamically adjusting the running speed of a robot, wherein the track planning method comprises the following steps: designing an acceleration zero setting motion, and acquiring a jerk change law of the acceleration zero setting motion, namely a first jerk change law; planning the process of transferring the state after the acceleration zero setting movement to the target state to obtain a second jerk change law; combining the first jerk change law and the second jerk change law to obtain a third jerk change law; continuously integrating the third jerk change law to obtain a change law of the acceleration, the speed and the position along with time, and completing the trajectory planning of the transition from the current state to the target state; the method can process the situations that the current acceleration is not zero and the current speed constraint cannot be met, and can be used for realizing the dynamic adjustment of the running speed of the robot.
Description
Technical Field
The invention relates to the field of robot motion planning, in particular to a track planning method and a track planning device for dynamically adjusting the running speed of a robot.
Background
The robot trajectory planning is divided into an off-line planning and an on-line planning according to whether the trajectory can be dynamically adjusted in the robot motion process. One of the purposes of online trajectory planning is to enable the robot to respond to changes in the external environment in real time. Compared with offline trajectory planning, online trajectory planning has a requirement on real-time performance, and results need to be given within a certain time.
And updating the constraint conditions according to the acquired external information by the online track planning, and regenerating the track to respond to the change of the constraint conditions. The constraint conditions include kinematic constraints and dynamic constraints, and most of the practical applications adopt the kinematic constraints. Kinematic constraints are limits on speed, acceleration, jerk (jerk), and the like. One application of online trajectory planning is to improve the safety of a robot during operation by dynamically adjusting the operation speed of the robot, and the specific process is as follows: when the environment sensor monitors that a person approaches or leaves the robot, the maximum allowable speed of the robot is reduced or increased, namely, the speed constraint is updated; planning a track from the current state to the target state according to the updated speed constraint; the controller takes the track as a reference input to control the robot to move, and the running speed of the robot is dynamically adjusted.
The running speed of the machine is dynamically adjusted, and two challenges are brought to trajectory planning: firstly, in order to ensure the stable movement, the continuous acceleration is expected, when the speed constraint is updated, the acceleration is not necessarily zero, and the track planning can process the situation that the current acceleration is not zero; second, after the speed constraint is updated, the current speed may exceed the current maximum allowable speed, but the speed cannot jump, the current speed constraint cannot be satisfied, and the trajectory planning should also be able to handle such a situation. The track planning method in the prior art cannot handle the two situations, so that the dynamic adjustment of the running speed of the robot is difficult to realize.
Disclosure of Invention
The invention aims to provide a track planning method and a track planning device capable of dynamically adjusting the running speed of a robot.
The invention solves the technical problems through the following technical means: a trajectory planning method for dynamically adjusting the running speed of a robot comprises the following steps:
the method comprises the following steps: designing an acceleration zero setting motion according to the whole-course kinematic constraint, the current speed, the current acceleration and the current speed constraint, so that after the motion, the acceleration is 0 and the current speed constraint can be met, and obtaining a jerk change law of the acceleration zero setting motion as a first jerk change law;
step two: planning a process of transferring the state after the acceleration zero setting movement to a target state, and acquiring a second jerk change law, wherein the state comprises the following steps: velocity, acceleration, and position;
step three: and combining the first jerk change law and the second jerk change law, obtaining a third jerk change law of the current state to the target state, continuously integrating the third jerk change law to obtain a change law of the acceleration, the speed and the position along with time, and completing the trajectory planning of the current state to the target state.
The invention designs an acceleration zero setting movement, so that after the movement, the acceleration is changed into 0 and meets the current speed constraint, the situation that the current speed constraint can not be met can be processed, after a jerk change law of the acceleration zero setting movement, namely a first jerk change law, is obtained, the prior art is combined to plan the process of transferring the state after the acceleration zero setting movement to a target state, a second jerk change law is obtained, the first jerk change law and the second jerk change law are combined, a third jerk change law transferring the current state to the target state is obtained, the third jerk change law is continuously integrated to obtain the change laws of the acceleration, the speed and the position along with time, and finally the trajectory planning transferring from the current state to the target state is completed, the method provided by the invention can process the situations that the current acceleration is not zero and the current speed constraint can not be met, the method can be used for realizing the dynamic adjustment of the running speed of the robot.
Preferably, in the step one, the global kinematic constraint is: maximum allowable acceleration amaxMaximum allowable jerk magnitude Jmax(ii) a The current speed constraint is: maximum allowable speed vmaxAnd the speed is greater than or equal to zero.
Preferably, said stepThe method comprises the following steps: if the current speed constraint can be met, the obtained first jerk change law omega1Comprises the following steps:
wherein, aiIs the current acceleration.
Preferably, the first step further comprises:
if the current speed constraint cannot be satisfied, and whenThe first jerk change law Ω1Comprises the following steps:
if the current speed constraint cannot be satisfied, and whenThe first jerk change law Ω1Comprises the following steps:
preferably, the current speed constraint can be satisfied by: if logical expression If false, the current speed constraint can be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
Preferably, the current speed constraint cannot be satisfied includes: if logical expression If true, the current speed constraint cannot be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
The invention also provides a track planning device for dynamically adjusting the running speed of the robot, which comprises:
the first jerk change law acquisition module is used for designing an acceleration zero setting motion according to the whole-course kinematic constraint, the current speed, the current acceleration and the current speed constraint, so that after the motion, the acceleration is 0 and the current speed constraint can be met, and the obtained jerk change law of the acceleration zero setting motion is used as a first jerk change law;
a second jerk change law obtaining module, configured to plan a process of transferring a state after the acceleration zero motion to a target state, and obtain a second jerk change law, where the state includes: velocity, acceleration, and position;
and the track planning module is used for combining the first jerk change law and the second jerk change law, acquiring a third jerk change law for transferring from the current state to the target state, continuously integrating the third jerk change law to obtain a change law of the acceleration, the speed and the position along with time, and completing the track planning for transferring from the current state to the target state.
Preferably, the global kinematic constraint is: maximum allowable acceleration amaxMaximum allowable jerk magnitude Jmax(ii) a The current speed constraint is: maximum allowable speed vmaxAnd the speed is greater than or equal to zero.
Preferably, the first jerk change law obtaining module is further configured to: if the current speed constraint can be met, the obtained first jerk change law omega1Comprises the following steps:
wherein, aiIs the current acceleration.
Preferably, the first jerk change law obtaining module is further configured to:
if the current speed constraint cannot be satisfied, and whenThe first jerk change law Ω1Comprises the following steps:
if the current speed constraint cannot be satisfied, and whenThe first jerk change law Ω1Comprises the following steps:
preferably, the current speed constraint can be satisfied by: if logical expression If false, the current speed constraint can be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
Preferably, the current speed constraint cannot be satisfied includes: if logical expression If true, the current speed constraint cannot be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
The invention has the advantages that: the invention designs an acceleration zero setting movement, so that after the movement, the acceleration is changed into 0 and meets the current speed constraint, the situation that the current speed constraint can not be met can be processed, after a jerk change law of the acceleration zero setting movement, namely a first jerk change law, is obtained, the prior art is combined to plan the process of transferring the state after the acceleration zero setting movement to a target state, a second jerk change law is obtained, the first jerk change law and the second jerk change law are combined, a third jerk change law transferring the current state to the target state is obtained, the third jerk change law is continuously integrated to obtain the change laws of the acceleration, the speed and the position along with time, and finally the trajectory planning transferring from the current state to the target state is completed, the method provided by the invention can process the situations that the current acceleration is not zero and the current speed constraint can not be met, the method can be used for realizing the dynamic adjustment of the running speed of the robot.
Drawings
FIG. 1 shows that a current speed constraint can be satisfied in a trajectory planning method for dynamically adjusting a robot operating speed disclosed in embodiment 1 of the present invention, where aiWhen the acceleration and jerk change along with time, the change rule is more than or equal to 0;
FIG. 2 shows that a current speed constraint can be satisfied in the trajectory planning method for dynamically adjusting the operating speed of the robot disclosed in embodiment 1 of the present invention, and ai<The change rule of the acceleration and the jerk along with time at 0;
fig. 3 shows that a current speed constraint cannot be satisfied and a is a in the trajectory planning method for dynamically adjusting the operating speed of the robot disclosed in embodiment 1 of the present inventionc≤-amaxWhen, vc<vmaxAnd the change rule of the acceleration and the jerk of the acceleration zero setting motion along with the time under the condition.
Fig. 4 shows a trajectory planning method for dynamically adjusting the operation speed of a robot according to embodiment 1 of the present invention, where the current speed constraint is not satisfied and ac≤-amaxWhen, vc≥vmaxAnd the change rule of the acceleration and the jerk of the acceleration zero setting motion along with the time under the condition.
Fig. 5 shows that a current speed constraint cannot be satisfied and a is a in the trajectory planning method for dynamically adjusting the operation speed of the robot disclosed in embodiment 1 of the present inventionc>-amaxWhen, vc1<vmaxAnd the change rule of the acceleration and the jerk of the acceleration zero setting motion along with the time under the condition.
Fig. 6 shows that a current speed constraint cannot be satisfied and a is a in the trajectory planning method for dynamically adjusting the operation speed of the robot disclosed in embodiment 1 of the present inventionc>-amaxWhen, vc2<vmax≤vc1And the change rule of the acceleration and the jerk of the acceleration zero setting motion along with the time under the condition.
Fig. 7 shows that a current speed constraint cannot be satisfied and a is a in the trajectory planning method for dynamically adjusting the operation speed of the robot disclosed in embodiment 1 of the present inventionc>-amaxWhen, vc2≥vmaxAcceleration and jerk of acceleration-nulling motion under conditions of timeThe change rule of (2).
Fig. 8 is a specific example of a trajectory planning method for dynamically adjusting a robot operating speed according to embodiment 1 of the present invention, where the trajectory planning method is from a position 0 to a position sfThe change curve of the position along with the time in the motion process;
fig. 9 is a specific example of a trajectory planning method for dynamically adjusting a running speed of a robot according to embodiment 1 of the present invention, where the trajectory planning method is from a position 0 to a position sfThe change curve of the speed along with the time in the motion process;
fig. 10 shows an embodiment of a trajectory planning method for dynamically adjusting a robot operating speed according to embodiment 1 of the present invention, in which a position is from 0 to sfA curve of the variation of the acceleration with time in the movement process;
fig. 11 is a block diagram of a trajectory planning apparatus for dynamically adjusting a robot operating speed according to embodiment 2 of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the embodiments of the present invention, and it is obvious that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
A trajectory planning method for dynamically adjusting the running speed of a robot comprises the following steps:
the robot moves from the starting position 0 to the target position sfMovement, sf>0, acceleration to target position is 0, velocity is vf,vf>0. In order to ensure the stable movement, the acceleration is required to be continuous in the whole course. Given global kinematic constraints: maximum allowable acceleration amaxMaximum allowable jerk magnitude Jmax。amaxAnd JmaxThe whole process is kept unchanged. Except for the whole courseBesides the kinematic constraints, there are also speed constraints: the speed is not greater than the maximum allowable speed and not less than zero. In the running process of the robot, the maximum allowable speed can be changed according to the change of the external environment, and the speed constraint is also dynamically changed.
The current position, the current velocity and the current acceleration are respectively si,viAnd aiAnd has 0. ltoreq. si≤sf,vi≧ 0 and | ai|≤amax. The current maximum allowable speed is vmaxThe current speed constraint is: speed not greater than maximum allowable speed vmaxAnd is not less than zero.
The state at a certain moment is the combination of the position, the speed and the acceleration at the moment and is represented by a position, speed and acceleration triplet, and the current state is msi=(si,vi,ai) Target state is msf=(sf,vf,0). The task of the trajectory planning is to consider the whole-course kinematic constraint and the current speed constraint, and to solve the problem that the current state ms is not a current stateiTransition to target state msfThe process of (2) is planned. If the change rule of the jerk in the state transition process can be determined, the jerk is continuously integrated with the time to obtain the change rule of the acceleration, the speed and the position along with the time, and the change rule is calculated according to the current state msiTransition to target state msfWill be fully determined and the trajectory planning task is thus completed.
From the current state msiTransition to target state msfTwo points need to be considered. First, the current acceleration aiThe acceleration is not necessarily 0, and the existing track method cannot directly process the condition that the current acceleration is not 0; second point, current velocity viMay exceed the current maximum allowable speed vmaxAnd because the whole-course acceleration is required to be continuous, the speed is not allowed to jump, and the current speed constraint condition cannot be met.
Considering the above two points, ms is the current stateiTransition to target state msfThe trajectory planning of (2) is performed in the following steps,
the method comprises the following steps: current speed, and current speed, and current speed, current speedDesigning an acceleration zero setting motion by the aid of front acceleration and current speed constraints, enabling the acceleration to be 0 and meeting the current speed constraints after the motion, and enabling the obtained jerk change law of the acceleration zero setting motion to serve as a first jerk change law; by s1、v1And ms1Respectively, the position, speed and state after the movement, with ms1=(s1,v10) and v1≤vmax。
Depending on whether the current velocity constraint can be met, the design of the acceleration nulling motion and the representation of the first jerk change law are discussed in detail case by case.
First, how to judge whether the current speed constraint can be satisfied is discussed, and the current speed constraint is divided into three cases: one is vi>vmaxI.e. the current speed is greater than the maximum permitted speed vmaxThe current speed constraint is obviously not satisfied; second, vi≤vmaxAnd a isiNot less than 0, the subsequent speed at least reaches to the following speed due to the continuous acceleration requirement If there isCurrent speed constraints cannot be met; III is vi≤vmaxAnd has ai<0, the speed can be kept at the maximum allowable speed vmaxWithin, the current speed constraint can be satisfied. In summary, whether the current speed constraint can be satisfied by a logical expression Determining that V is logic or symbol, A is logic and symbol, if the logic expression is true, the current speed constraint cannot be satisfied, if the logic expression is false, the current speed constraint is satisfiedThe constraints can be satisfied.
If the current speed constraint can be satisfied, considering the acceleration from a in the shortest timeiTo become 0, the acceleration nulling motion should be: if aiNot less than 0, jerk J ═ JmaxAnd for a period of time ai/JmaxAccordingly, the acceleration decreases linearly to 0 over time; if ai<0, jerk J ═ JmaxAnd for a period of time-ai/JmaxAccordingly, the acceleration increases linearly to 0 with time. FIG. 1 illustrates that the current speed constraint can be satisfied and aiWhen the acceleration is more than or equal to 0, the change rule of the acceleration and jerk of the acceleration zero setting motion along with the time, and fig. 2 shows that the current speed constraint can be satisfied and ai<And at 0, the acceleration and the jerk of the acceleration zero setting motion change along with the time. In both cases, jerk is a piecewise constant function of a single segment with respect to time. The piecewise constant function may be represented by a list of function values and interval lengths, such that the first jerk change law Ω is satisfied when the current speed constraint is satisfied1Expressed in the list of jerk magnitude and duration, there are:
if the current speed constraint condition cannot be met, the speed inevitably exceeds the maximum allowable speed vmaxThe acceleration zero motion should comprise two motions: a first movement for a short time exceeding the speed vmaxBack to vmaxAnd after that the current speed constraint can be re-satisfied, with amIndicating a change in velocity to vmaxAcceleration in time; a second motion for accelerating the acceleration from a in the shortest timemBecomes zero. The law of jerk change of the first and second movements is respectively represented by Ω11And Ω12And (4) showing.
For the first motion, jerk should first be equal to-JmaxAnd for a period of time, the acceleration will be reduced accordingly to a minimum value, so that the speed is changed as quickly as possiblevmax. In addition, to make the current speed constraint satisfied again after the first segment of motion, a should bemNot more than 0, that is, the speed will not increase any more, and should also beI.e. the speed does not decrease below zero. Considering the acceleration constraint, should also be am≥-amax. Definition ofAccording to acAnd-amaxThe design of the first stage motion is discussed in terms of the relative magnitude difference.
If ac≤-amaxDefinition only by considering the acceleration constraint vcIndicates a jerk of J ═ JmaxAnd maintained for a period of time (a)max+ai)/JmaxThe velocity reached thereafter. According to vmaxWhether or not greater than vcThere are two cases. One is vc<vmaxFor this case, the acceleration is decreasing to-amaxThe speed having previously changed to vmaxIs provided withThe first motion should then be: jerk J ═ JmaxAnd maintained for a period of time (a)i-am)/Jmax(ii) a Second, vc≥vmaxFor this case, the acceleration is decreasing to-amaxThe rear speed is still greater than vmaxAcceleration should be maintained-amaxFor a period of time until the speed decreases to vmaxHas am=-amaxThe first motion should then be: jerk J ═ JmaxAnd maintained for a period of time (a)i+amax)/JmaxBecomes jerk J equal to 0 and is maintained for a certain period of time (v)c-vmax)/amax. In both cases, the jerk of the first motion is a piecewise constant function with respect to time, and the jerk variation law Ω of the first motion is then11Expressed in a list of jerk magnitude and duration, there are
If ac>-amaxNeed to consider acTo amIs limited, is defined vc1Indicates the acceleration of aiAccording to formula J ═ JmaxDecrease to acVelocity of time, vc2Indicates the acceleration of aiAccording to formula J ═ JmaxDecrease to-amaxThen according to J ═ JmaxLinear increase to acThe velocity of the time. In three cases, one is vc1<vmaxIn this case, the acceleration is reduced to acThe speed having previously changed to vmaxIs provided withThe first motion should then be: jerk J ═ JmaxAnd for a period of time (a)i-am)/Jmax(ii) a Second, vc2<vmax≤vc1In this case, the acceleration can be reduced to acThe following, -amaxAbove a and am=acThe minimum acceleration reached isThe first motion should then be: jerk J ═ JmaxAnd for a period of time (a)i-am1)/JmaxChange to jerk J ═ JmaxAnd continues toFor a period of time (a)c-am1)/Jmax(ii) a III is vc2≥vmaxIn this case, the acceleration can be reduced to-amaxAnd a ism=acThe first motion should then be: jerk J ═ JmaxAnd for a period of time (a)i+amax)/JmaxBecomes jerk J equal to 0 for a period of time (v)c2-vmax)/amaxFinally, J is changed to J againmaxAnd for a period of time (a)c+amax)/Jmax. In all three cases, the jerk of the first motion segment is a piecewise constant function with respect to time, and thus the jerk variation law Ω of the first motion segment is11Expressed in a list of jerk magnitude and duration, there
After the first movement is discussed, the second movement is considered, and the second movement makes the acceleration from a within the shortest timemBecomes zero and should be: jerk J ═ JmaxAnd maintained for a period of time-am/Jmax. Law of jerk change omega for the second stage of motion12Expressed in a list of jerk magnitude and duration, there
After determining the first segment of motion and the second segment of motion, an acceleration nulling motion is also determined. Since the jerk of the two motion segments is a piecewise constant function with respect to time, the jerk of the acceleration zero motion is also a piecewise constant function with respect to time and can still be represented by a list of jerk magnitudes and durations. And combining the jerk change laws of the first and second motion segments to obtain the jerk change law of the acceleration zero-setting motion. For example, if Ω11=**-Jmax,td1+,*Jmax,td2++,Ω12=**Jmax,td3+ + combined omega11And Ω12To obtain omega1=**-Jmax,td1+,*Jmax,td2+td3++。
When the current speed constraint cannot be met, combining jerk change laws of two-stage motions included in the acceleration zero setting motion according to the formula (2), the formula (3) and the formula (4), and substituting the jerk change laws into the jerk change lawsComprises the following steps: if it isΩ1Is composed of
FIGS. 3 and 4 illustrate that the current speed constraint cannot be met and ac≤-amaxThe change rule of the acceleration and jerk of the acceleration zero setting motion along with time; FIG. 3 corresponds to case vc<vmaxFIG. 4 corresponds to case vc≥vmax。
FIG. 5, FIG. 6 andFIG. 7 illustrates that the current speed constraint can be satisfied and ac>-amaxThe change rule of the acceleration and jerk of the acceleration zero setting motion along with time; FIG. 5 corresponds to case vc1<vmaxFIG. 6 corresponds to case vc2<vmax≤vc1FIG. 7 corresponds to case vc2≥vmax。
In summary, expression (1) of the first jerk change law when the current speed constraint can be satisfied, and expressions (5) and (6) of the first jerk change law when the current speed constraint cannot be satisfied are obtained.
After the first jerk change law is determined, the current state ms is determinediContinuously integrating the jerk with respect to time to obtain the position, velocity and state ms after the acceleration zero-setting motion1. The jerk integration obtains acceleration, the acceleration integration obtains velocity, and the velocity integration obtains displacement, and the process belongs to the prior conventional technology and is not described in detail herein.
Step two: state ms after acceleration zero setting motion1Transition to target state msfPlanning the process to obtain a second jerk change law omega2(ii) a Due to the state ms1And state msfThe corresponding accelerations are all zero, and the second jerk change law Ω2The acquisition of the speed v can be completed by the prior art, and the speed v after the zero setting movement of the acceleration is realized by specifically referring to a method in 'S-shaped acceleration and deceleration time planning algorithm research with non-zero starting speed and ending speed' in volume 52 and 23 of mechanical engineering journal1As initial velocity vsTarget velocity vfAs the termination velocity ve,(sf-s1) As displacement, solving a high-order equation, planning a change rule of the jerk along with time, and using the change rule as a second jerk change rule omega2Wherein jerk remains a piecewise constant function with respect to time, Ω2It can still be represented by a list of jerk magnitudes and durations.
Step three: merging the first jerk change law Ω1And a second jerk change law Ω2Obtaining the current state msiTransfer to the eyeStandard state msfAnd (3) continuously integrating the third jerk change law omega to obtain the change law of the acceleration, the speed and the position along with time, and completing the trajectory planning of the transition from the current state to the target state. Wherein Ω is Ω1~Ω2And represents a first jerk change law Ω of the combination1And a second jerk change law Ω2Form a third jerk change law Ω, e.g., if Ω1=**Jmax,td1+,*-Jmax,td2++,Ω2=**Jmax,td3+,*-Jmax,td4+ + +, then omega equals omega1~Ω2=**Jmax,td1+,*-Jmax,td2+,*Jmax,td3+,*-Jmax,td4++. It should be noted that jerk is referred to in many contexts as jerk, i.e., the derivative of acceleration with respect to time. The jerk integration obtains acceleration, the acceleration integration obtains velocity, and the velocity integration obtains displacement, and the process belongs to the prior conventional technology and is not described in detail herein.
The following explains the track planning method provided by the invention for realizing the dynamic adjustment of the running speed of the robot by specific examples. The robot moves from the initial position 0 to the target position sfWhen the robot moves 0.16, the robot is in a static state at the initial position, the acceleration is zero after the robot reaches the target position, and the speed v is zerofAlso zero. The global kinematic constraint given is: maximum allowable acceleration amaxMaximum allowable jerk size J of 2max=60。
Initial time t0Maximum allowable speed v of 0maxFrom position 0 to position s can be determined according to prior art methodsfThe change rule of the position, the speed and the acceleration along with the time in the motion process, and the solid lines in the figures 8, 9 and 10 are the change curves of the position, the speed and the acceleration along with the time respectively. If the velocity constraint is not allowed to be updated, the position, velocity and acceleration will change to the target state according to the solid lines in fig. 8, 9 and 10, respectively.
At time t1Update speed constraint, maximum allowed speed adjusted to v 0.2max0.25, when the current position s is presenti0.0337, current velocity vi0.367, the current acceleration ai2. After the speed constraint is updated, the current speed is greater than the maximum allowable speed, and according to the track planning method of the invention, the current state(s) is adjusted according to the updated current speed constrainti,vi,ai) To the target state(s)f0,0) to obtain the change rule of the position, the speed and the acceleration along with the time. The dashed lines in fig. 8, 9 and 10 show the position, velocity and acceleration curves over time, respectively, after the velocity constraint is updated.
If at time t1After 0.2 the velocity constraint is not updated any more and the position, velocity and acceleration will change to the target state according to the dashed lines shown in fig. 8, 9 and 10, respectively. t is t1Velocity v at timei0.367 exceeds the updated maximum allowable speed vmaxAs can be seen from the speed change curve shown by the broken line in fig. 9 at 0.25, the speed changes to the maximum speed v in a short timemaxWithin the range defined by 0.25, dynamic speed adjustment is achieved.
Through the technical scheme, the invention provides a track planning method for dynamically adjusting the running speed of a robot, which comprises the steps of firstly designing an acceleration zero setting motion to ensure that the acceleration is changed into 0 and the current speed constraint can be met, obtaining a jerk change law, namely a first jerk change law, of the state after the current state is transferred to the acceleration zero setting motion, then planning the process of transferring the state after the acceleration zero setting movement to the target state by combining the prior art, obtaining a second jerk change law, combining the first jerk change law and the second jerk change law, obtaining a third jerk change law transferring the current state to the target state, and continuously integrating the third jerk change law to obtain the change law of the acceleration, the speed and the position along with time, and finally completing the trajectory planning of the transition from the current state to the target state, so that the dynamic adjustment of the running speed of the robot can be realized.
Example 2
As shown in fig. 11, corresponding to embodiment 1 of the present invention, embodiment 2 of the present invention further provides a trajectory planning device for dynamically adjusting an operation speed of a robot, where the trajectory planning device includes:
the first jerk change law acquisition module is used for designing an acceleration zero setting motion according to the whole-course kinematic constraint, the current speed, the current acceleration and the current speed constraint, so that after the motion, the acceleration is 0 and the current speed constraint can be met, and the obtained jerk change law of the acceleration zero setting motion is used as a first jerk change law;
a second jerk change law obtaining module, configured to plan a process of transferring a state after the acceleration zero motion to a target state, and obtain a second jerk change law, where the state includes: velocity, acceleration, and position;
and the track planning module is used for combining the first jerk change law and the second jerk change law, acquiring a third jerk change law for transferring from the current state to the target state, continuously integrating the third jerk change law to obtain a change law of the acceleration, the speed and the position along with time, and completing the track planning for transferring from the current state to the target state.
Specifically, the global kinematic constraint is as follows: maximum allowable acceleration amaxMaximum allowable jerk magnitude Jmax(ii) a The current speed constraint is: maximum allowable speed vmaxAnd the speed is greater than or equal to zero.
Specifically, the first jerk change law obtaining module is further configured to: if the current speed constraint can be met, the obtained first jerk change law omega1Comprises the following steps:
wherein, aiIs the current acceleration.
Specifically, the first jerk change law obtaining module is further configured to:
if the current speed constraint cannot be metMeet the requirements thatThe first jerk change law Ω1Comprises the following steps:
if the current speed constraint cannot be satisfied, and whenThe first jerk change law Ω1Comprises the following steps:
specifically, the current speed constraint can be satisfied by: if logical expression If false, the current speed constraint can be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
Specifically, the current speed constraint cannot be satisfied by the method including: if logical expression If true, the current speed constraint cannot be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (4)
1. A trajectory planning method for dynamically adjusting the running speed of a robot is characterized by comprising the following steps:
the method comprises the following steps: designing an acceleration zero setting motion according to the whole-course kinematic constraint, the current speed, the current acceleration and the current speed constraint, so that after the motion, the acceleration is 0 and the current speed constraint can be met, and obtaining a jerk change law of the acceleration zero setting motion as a first jerk change law;
step two: planning a process of transferring the state after the acceleration zero setting motion to a target state, and acquiring a second jerk change law, wherein the state comprises the following steps: velocity, acceleration, and position;
step three: combining the first jerk change law and the second jerk change law, obtaining a third jerk change law which is transferred from the current state to the target state, continuously integrating the third jerk change law to obtain a change law of the acceleration, the speed and the position along with time, and completing the trajectory planning transferred from the current state to the target state;
in the first step, the whole-course kinematic constraint is as follows: maximum allowable acceleration amaxMaximum allowable jerk magnitude Jmax(ii) a When in useThe front velocity constraint is: maximum allowable speed vmaxAnd the speed is greater than or equal to zero;
the first step comprises the following steps:
if the current speed constraint can be satisfied, the obtained first jerk change law Jt1Comprises the following steps:
wherein, JmaxIs the maximum allowable jerk, aiIs the current acceleration;
if the current speed constraint cannot be satisfied, and whenFirst jerk change law Jt obtained1Comprises the following steps:
wherein v ismaxTo the maximum allowable speed, amIndicating a change in velocity to vmaxAcceleration of time andviis the current speed, vcIndicating jerk holding JmaxFor a period of time (a)max+ai)/JmaxThen the speed reached and
wherein v isc1Indicates the acceleration of aiAccording to maximum allowable jerk JmaxDecrease to acVelocity of time andvc2indicates the acceleration of aiAccording to jerk-JmaxDecrease to-amaxThen according to the maximum allowable jerk JmaxLinear increase to acA speed of time, and am1is v isc2<vmax≤vc1Minimum acceleration achieved during the entire process and
2. the trajectory planning method for dynamically adjusting the operating speed of a robot according to claim 1, wherein the current speed constraint can be satisfied by: if logical expressionIf true, the current speed constraint cannot be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
3. The trajectory planning method for dynamically adjusting the operating speed of a robot according to claim 1, wherein the current speed constraint being not satisfied comprises: if logical expressionIf false, the current speed constraint can be satisfied; wherein, the V-shaped is logic or symbol, and the A-shaped is logic and symbol.
4. A trajectory planning device for dynamically adjusting the running speed of a robot is characterized by comprising:
the first jerk change law acquisition module is used for designing an acceleration zero setting motion according to the whole-course kinematic constraint, the current speed, the current acceleration and the current speed constraint, so that after the motion, the acceleration is 0 and the current speed constraint can be met, and the obtained jerk change law of the acceleration zero setting motion is used as a first jerk change law;
a second jerk change law obtaining module, configured to plan a process of transferring a state after the acceleration zero motion to a target state, and obtain a second jerk change law, where the state includes: velocity, acceleration, and position;
the track planning module is used for combining the first jerk change law and the second jerk change law, acquiring a third jerk change law for transferring from the current state to the target state, continuously integrating the third jerk change law to obtain a change law of acceleration, speed and position along with time, and completing the track planning for transferring from the current state to the target state;
the global kinematic constraint is: maximum allowable acceleration amaxMaximum allowable jerk magnitude Jmax(ii) a The current speed constraint is: maximum allowable speed vmaxAnd the speed is greater than or equal to zero;
the first jerk change law obtaining module is further configured to:
if the current speed constraint can be satisfied, the obtained first jerk change law Jt1Comprises the following steps:
wherein, JmaxIs the maximum allowable jerk, aiIs the current acceleration;
the first jerk change law obtaining module is further configured to:
if the current speed constraint cannot be satisfied, and whenFirst jerk change law Jt obtained1Comprises the following steps:
wherein v ismaxTo the maximum allowable speed, amIndicating a change in velocity to vmaxAcceleration of time andviis the current speed, vcIndicating jerk holding JmaxFor a period of time (a)max+ai)/JmaxThen the speed reached and
wherein v isc1Indicates the acceleration of aiAccording to maximum allowable jerk JmaxDecrease to acVelocity of time andvc2indicates the acceleration of aiAccording to jerk-JmaxDecrease to-amaxThen according to the maximum allowable jerk JmaxLinear increase to acA speed of time, and am1is v isc2<vmax≤vc1Minimum acceleration achieved during the entire process and
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010613688.6A CN111665851B (en) | 2020-06-30 | 2020-06-30 | Trajectory planning method and device for dynamically adjusting running speed of robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010613688.6A CN111665851B (en) | 2020-06-30 | 2020-06-30 | Trajectory planning method and device for dynamically adjusting running speed of robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111665851A CN111665851A (en) | 2020-09-15 |
CN111665851B true CN111665851B (en) | 2022-02-11 |
Family
ID=72390586
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010613688.6A Active CN111665851B (en) | 2020-06-30 | 2020-06-30 | Trajectory planning method and device for dynamically adjusting running speed of robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111665851B (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113325846B (en) * | 2021-05-31 | 2024-02-27 | 西安建筑科技大学 | Strip mine unmanned mine card dynamic path planning method based on improved TEB method |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6216058B1 (en) * | 1999-05-28 | 2001-04-10 | Brooks Automation, Inc. | System of trajectory planning for robotic manipulators based on pre-defined time-optimum trajectory shapes |
CN101939711A (en) * | 2007-10-21 | 2011-01-05 | 通用电气智能平台有限公司 | System and method for jerk limited trajectory planning for a path planner |
CN108549328A (en) * | 2018-03-22 | 2018-09-18 | 汇川技术(东莞)有限公司 | Adaptive speed method and system for planning |
CN109623820A (en) * | 2018-12-25 | 2019-04-16 | 哈工大机器人(合肥)国际创新研究院 | A kind of robot space tracking transition method |
CN109683615A (en) * | 2018-12-25 | 2019-04-26 | 上海新时达机器人有限公司 | The speed look-ahead approach and robot controller in the path that robot continuously moves |
CN110703684A (en) * | 2019-11-01 | 2020-01-17 | 哈工大机器人(合肥)国际创新研究院 | Trajectory planning method and device with unlimited endpoint speed |
CN110757455A (en) * | 2019-10-25 | 2020-02-07 | 上海新时达机器人有限公司 | Speed planning method |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9857795B2 (en) * | 2016-03-24 | 2018-01-02 | Honda Motor Co., Ltd. | System and method for trajectory planning for unexpected pedestrians |
-
2020
- 2020-06-30 CN CN202010613688.6A patent/CN111665851B/en active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6216058B1 (en) * | 1999-05-28 | 2001-04-10 | Brooks Automation, Inc. | System of trajectory planning for robotic manipulators based on pre-defined time-optimum trajectory shapes |
CN101939711A (en) * | 2007-10-21 | 2011-01-05 | 通用电气智能平台有限公司 | System and method for jerk limited trajectory planning for a path planner |
CN108549328A (en) * | 2018-03-22 | 2018-09-18 | 汇川技术(东莞)有限公司 | Adaptive speed method and system for planning |
CN109623820A (en) * | 2018-12-25 | 2019-04-16 | 哈工大机器人(合肥)国际创新研究院 | A kind of robot space tracking transition method |
CN109683615A (en) * | 2018-12-25 | 2019-04-26 | 上海新时达机器人有限公司 | The speed look-ahead approach and robot controller in the path that robot continuously moves |
CN110757455A (en) * | 2019-10-25 | 2020-02-07 | 上海新时达机器人有限公司 | Speed planning method |
CN110703684A (en) * | 2019-11-01 | 2020-01-17 | 哈工大机器人(合肥)国际创新研究院 | Trajectory planning method and device with unlimited endpoint speed |
Non-Patent Citations (2)
Title |
---|
High performance final state control system based on minimum-jerk trajectory for industrial robots;Takashi Yoshioka,et al.;《IECON 2011 - 37th Annual Conference of the IEEE Industrial Electronics Society》;20111231;全文 * |
Minimum-Jerk Robot Joint Trajectory Using Particle Swarm Optimization;Lin Hsien-I,et al.;《2011 First International Conference on Robot, Vision and Signal Processing》;20111231;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN111665851A (en) | 2020-09-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9421687B2 (en) | Robot control apparatus and robot control method | |
CN110673611B (en) | Under-actuated unmanned ship control method based on event triggering scheme and T-S fuzzy system | |
CN109434831B (en) | Robot operation method and device, robot, electronic device and readable medium | |
CN105629729A (en) | Network mobile robot locus tracking control method based on linearity auto-disturbance rejection | |
CN107831761A (en) | A kind of path tracking control method of intelligent vehicle | |
CN111665851B (en) | Trajectory planning method and device for dynamically adjusting running speed of robot | |
CN106950999B (en) | mobile stage trajectory tracking control method adopting active disturbance rejection control technology | |
CN108406765A (en) | A kind of fisher's formula multi-arm robot impedance adjustment | |
CN113400313B (en) | Impedance control method of robot-environment compliant contact process based on Zener model | |
CN106272436B (en) | A kind of service robot self-adaptation control method based on varying load | |
JP6664807B2 (en) | CONTROL DEVICE, CONTROL METHOD, AND CONTROL PROGRAM | |
CN114690767A (en) | Robot trajectory planning method and system and robot | |
Liu et al. | Control strategy design based on fuzzy logic and LQR for 3-DOF helicopter model | |
CN112650217B (en) | Robot trajectory tracking strategy dynamic optimization method based on evaluation function | |
CN110647161B (en) | Under-actuated UUV horizontal plane trajectory tracking control method based on state prediction compensation | |
CN112947501A (en) | Multi-AUV hybrid formation method based on improved artificial potential field method and state switching | |
Xie et al. | Research on the Control Performance of Depth‐Fixed Motion of Underwater Vehicle Based on Fuzzy‐PID | |
CN112629339B (en) | Rocket soft landing trajectory planning method based on direct method | |
KR20200093874A (en) | System and method for controlling quadrotor based on fuzzy model | |
CN113568312B (en) | Track tracking method for single-chain mechanical arm under interference condition | |
JP3536084B2 (en) | Gait control method for walking robot | |
Li et al. | Adaptive dynamic surface control of a flexible-joint robot with parametric uncertainties | |
Almeida et al. | Control structure with disturbance identification for Lagrangian systems | |
CN113221349A (en) | Vehicle distance sliding mode control algorithm and system suitable for intelligent driving vehicle | |
CN110815217B (en) | Robot servo torque control method based on speed control mode |
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 |