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 PDF

Info

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
Application number
CN202010613688.6A
Other languages
Chinese (zh)
Other versions
CN111665851A (en
Inventor
王�华
郭庆洪
邰文涛
吴自翔
于振中
李文兴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HRG International Institute for Research and Innovation
Original Assignee
HRG International Institute for Research and Innovation
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HRG International Institute for Research and Innovation filed Critical HRG International Institute for Research and Innovation
Priority to CN202010613688.6A priority Critical patent/CN111665851B/en
Publication of CN111665851A publication Critical patent/CN111665851A/en
Application granted granted Critical
Publication of CN111665851B publication Critical patent/CN111665851B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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)
  • Numerical Control (AREA)
  • Manipulator (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

Trajectory planning method and device for dynamically adjusting running speed of robot
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:
Figure BDA0002563022040000031
wherein, aiIs the current acceleration.
Preferably, the first step further comprises:
if the current speed constraint cannot be satisfied, and when
Figure BDA0002563022040000032
The first jerk change law Ω1Comprises the following steps:
Figure BDA0002563022040000033
wherein the content of the first and second substances,
Figure BDA0002563022040000034
viis the current speed;
if the current speed constraint cannot be satisfied, and when
Figure BDA0002563022040000035
The first jerk change law Ω1Comprises the following steps:
Figure BDA0002563022040000041
wherein the content of the first and second substances,
Figure BDA0002563022040000042
Figure BDA0002563022040000043
preferably, the current speed constraint can be satisfied by: if logical expression
Figure BDA0002563022040000044
Figure BDA0002563022040000045
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
Figure BDA0002563022040000046
Figure BDA0002563022040000047
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:
Figure BDA0002563022040000051
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 when
Figure BDA0002563022040000052
The first jerk change law Ω1Comprises the following steps:
Figure BDA0002563022040000053
wherein the content of the first and second substances,
Figure BDA0002563022040000054
viis the current speed;
if the current speed constraint cannot be satisfied, and when
Figure BDA0002563022040000055
The first jerk change law Ω1Comprises the following steps:
Figure BDA0002563022040000061
wherein the content of the first and second substances,
Figure BDA0002563022040000062
Figure BDA0002563022040000063
preferably, the current speed constraint can be satisfied by: if logical expression
Figure BDA0002563022040000064
Figure BDA0002563022040000065
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
Figure BDA0002563022040000066
Figure BDA0002563022040000067
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
Figure BDA0002563022040000101
Figure BDA0002563022040000102
If there is
Figure BDA0002563022040000103
Current 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
Figure BDA0002563022040000104
Figure BDA0002563022040000105
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:
Figure BDA0002563022040000111
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 be
Figure BDA0002563022040000112
I.e. the speed does not decrease below zero. Considering the acceleration constraint, should also be am≥-amax. Definition of
Figure BDA0002563022040000113
According 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
Figure BDA0002563022040000114
Figure BDA0002563022040000115
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 with
Figure BDA0002563022040000116
The 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
Figure BDA0002563022040000121
If ac>-amaxNeed to consider acTo amIs limited, is defined
Figure BDA0002563022040000122
Figure BDA0002563022040000123
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 with
Figure BDA0002563022040000124
The 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 is
Figure BDA0002563022040000125
The 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
Figure BDA0002563022040000131
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
Figure BDA0002563022040000132
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 laws
Figure BDA0002563022040000133
Comprises the following steps: if it is
Figure BDA0002563022040000134
Ω1Is composed of
Figure BDA0002563022040000135
Wherein the content of the first and second substances,
Figure BDA0002563022040000136
if it is
Figure BDA0002563022040000141
Ω1Is composed of
Figure BDA0002563022040000142
Wherein the content of the first and second substances,
Figure BDA0002563022040000143
Figure BDA0002563022040000144
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:
Figure BDA0002563022040000171
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 that
Figure BDA0002563022040000181
The first jerk change law Ω1Comprises the following steps:
Figure BDA0002563022040000182
wherein the content of the first and second substances,
Figure BDA0002563022040000183
viis the current speed;
if the current speed constraint cannot be satisfied, and when
Figure BDA0002563022040000184
The first jerk change law Ω1Comprises the following steps:
Figure BDA0002563022040000185
wherein the content of the first and second substances,
Figure BDA0002563022040000186
Figure BDA0002563022040000187
specifically, the current speed constraint can be satisfied by: if logical expression
Figure BDA0002563022040000188
Figure BDA0002563022040000189
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
Figure BDA00025630220400001810
Figure BDA00025630220400001811
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:
Figure FDA0003418680960000011
wherein, JmaxIs the maximum allowable jerk, aiIs the current acceleration;
if the current speed constraint cannot be satisfied, and when
Figure FDA0003418680960000012
First jerk change law Jt obtained1Comprises the following steps:
Figure FDA0003418680960000021
wherein v ismaxTo the maximum allowable speed, amIndicating a change in velocity to vmaxAcceleration of time and
Figure FDA0003418680960000022
viis the current speed, vcIndicating jerk holding JmaxFor a period of time (a)max+ai)/JmaxThen the speed reached and
Figure FDA0003418680960000023
when in use
Figure FDA0003418680960000024
First jerk change law Jt obtained1Comprises the following steps:
Figure FDA0003418680960000025
wherein v isc1Indicates the acceleration of aiAccording to maximum allowable jerk JmaxDecrease to acVelocity of time and
Figure FDA0003418680960000026
vc2indicates the acceleration of aiAccording to jerk-JmaxDecrease to-amaxThen according to the maximum allowable jerk JmaxLinear increase to acA speed of time, and
Figure FDA0003418680960000027
Figure FDA0003418680960000028
am1is v isc2<vmax≤vc1Minimum acceleration achieved during the entire process and
Figure FDA0003418680960000029
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 expression
Figure FDA00034186809600000210
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.
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 expression
Figure FDA00034186809600000211
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.
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:
Figure FDA0003418680960000031
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 when
Figure FDA0003418680960000032
First jerk change law Jt obtained1Comprises the following steps:
Figure FDA0003418680960000041
wherein v ismaxTo the maximum allowable speed, amIndicating a change in velocity to vmaxAcceleration of time and
Figure FDA0003418680960000042
viis the current speed, vcIndicating jerk holding JmaxFor a period of time (a)max+ai)/JmaxThen the speed reached and
Figure FDA0003418680960000043
when in use
Figure FDA0003418680960000044
First jerk change law Jt obtained1Comprises the following steps:
Figure FDA0003418680960000045
wherein v isc1Indicates the acceleration of aiAccording to maximum allowable jerk JmaxDecrease to acVelocity of time and
Figure FDA0003418680960000046
vc2indicates the acceleration of aiAccording to jerk-JmaxDecrease to-amaxThen according to the maximum allowable jerk JmaxLinear increase to acA speed of time, and
Figure FDA0003418680960000047
Figure FDA0003418680960000048
am1is v isc2<vmax≤vc1Minimum acceleration achieved during the entire process and
Figure FDA0003418680960000049
CN202010613688.6A 2020-06-30 2020-06-30 Trajectory planning method and device for dynamically adjusting running speed of robot Active CN111665851B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (7)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
Bartolini et al. Second-order sliding-mode control of container cranes
Din et al. A comparative experimental study of robust sliding mode control strategies for underactuated systems
CN110673611B (en) Under-actuated unmanned ship control method based on event triggering scheme and T-S fuzzy system
US20140012419A1 (en) Robot control apparatus and robot control method
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
CN108406765B (en) Impedance control method for open-chain multi-arm robot
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
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
CN112629339B (en) Rocket soft landing trajectory planning method based on direct method
CN114690767A (en) Robot trajectory planning method and system and robot
CN113001547B (en) Robot teleoperation control method based on mixed reality
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
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

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