CN113703399A - Motion trajectory planning method, device, equipment and storage medium - Google Patents

Motion trajectory planning method, device, equipment and storage medium Download PDF

Info

Publication number
CN113703399A
CN113703399A CN202110986578.9A CN202110986578A CN113703399A CN 113703399 A CN113703399 A CN 113703399A CN 202110986578 A CN202110986578 A CN 202110986578A CN 113703399 A CN113703399 A CN 113703399A
Authority
CN
China
Prior art keywords
planned
initial
maximum
acceleration
state
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110986578.9A
Other languages
Chinese (zh)
Other versions
CN113703399B (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.)
Guangdong Topstar Technology Co Ltd
Original Assignee
Guangdong Topstar Technology Co Ltd
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 Guangdong Topstar Technology Co Ltd filed Critical Guangdong Topstar Technology Co Ltd
Priority to CN202110986578.9A priority Critical patent/CN113703399B/en
Publication of CN113703399A publication Critical patent/CN113703399A/en
Application granted granted Critical
Publication of CN113703399B publication Critical patent/CN113703399B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34251—Cnc works with different operating systems, windows, os-2, vms in parallel

Landscapes

  • Engineering & Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)

Abstract

The application relates to a motion trail planning method, a motion trail planning device, motion trail planning equipment and a storage medium. The method comprises the following steps: acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned; when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing; and performing S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter. The method can process the situation that the initial acceleration is not equal to zero, so that the planned track curve is smoother, the speed can be changed frequently and has certain stability, and the actual track planning requirement of the object to be planned can be met.

Description

Motion trajectory planning method, device, equipment and storage medium
Technical Field
The present application relates to the field of motion control, and in particular, to a method, an apparatus, a device, and a storage medium for planning a motion trajectory.
Background
In the field of motion control, for a trajectory planning problem of an object to be planned (such as a robot), a smooth trajectory of the object to be planned is generally calculated using a ladder-type planning method or an S-type planning method. At present, a common ladder-type planning method can make a planned position curve and a planned speed curve continuous and an planned acceleration curve discontinuous, and a common S-type planning method can make the planned position curve, the planned speed curve and the planned acceleration curve continuous. Therefore, compared with a ladder-type planning method, the S-type planning method has the advantage that the speed curve obtained by planning is smoother.
However, when the initial acceleration of the object to be planned is not zero, the conventional S-type planning method is not applicable, and therefore, for the case that the initial acceleration of the object to be planned is not zero, it is necessary to provide a corresponding trajectory planning method.
Disclosure of Invention
Based on this, the embodiment of the present application provides a method, an apparatus, a device and a storage medium for planning a motion trajectory, which can meet a motion trajectory planning requirement under the condition that the initial acceleration of an object to be planned is not zero.
In a first aspect, an embodiment of the present application provides a motion trajectory planning method, including:
acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing;
and performing S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter.
In a second aspect, an embodiment of the present application provides a motion trajectory planning apparatus, including:
the system comprises an acquisition module, a planning module and a planning module, wherein the acquisition module is used for acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
the processing module is used for carrying out zero resetting processing on the initial acceleration when the initial acceleration in the initial state is not equal to zero to obtain the initial state after the zero resetting processing;
and the planning module is used for carrying out S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter.
In a third aspect, an embodiment of the present application provides a motion trajectory planning apparatus, which includes a memory and a processor, where the memory stores a computer program, and the processor implements the steps of the motion trajectory planning method provided in the first aspect of the embodiment of the present application when executing the computer program.
In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the steps of the motion trajectory planning method provided in the first aspect of the embodiment of the present application.
According to the technical scheme, under the condition that the initial acceleration of the object to be planned is not equal to zero, zero returning processing can be carried out on the initial acceleration, S-shaped curve planning is carried out on the object to be planned according to the initial state, the target state and the constraint parameters after the zero returning processing, the planned track curve is smoother, the speed can be changed frequently, certain stability is achieved, and the actual track planning requirement of the object to be planned can be met.
Drawings
Fig. 1 is a schematic flow chart of a motion trajectory planning method according to an embodiment of the present application;
FIG. 2 is a schematic diagram of a method for calculating displacement when the initial acceleration is equal to zero according to an embodiment of the present disclosure;
FIG. 3 is a schematic diagram of a method for calculating displacement when the initial acceleration is not equal to zero according to an embodiment of the present disclosure;
FIG. 4 is a schematic flow chart illustrating a zeroing process for initial acceleration according to an embodiment of the present disclosure;
FIG. 5 is a schematic diagram illustrating an exemplary embodiment of a zeroing process for initial acceleration;
FIG. 6 is a schematic diagram illustrating another exemplary embodiment of a zeroing process for initial acceleration;
FIG. 7 is a schematic diagram illustrating another exemplary embodiment of a zeroing process for initial acceleration;
FIG. 8 is a schematic diagram illustrating another exemplary embodiment of a zeroing process for initial acceleration;
fig. 9 is another schematic flow chart of a motion trajectory planning method according to an embodiment of the present application;
FIG. 10 is a schematic diagram illustrating a method for calculating a maximum achievable speed according to an embodiment of the present disclosure;
fig. 11 is a schematic flowchart of another motion trajectory planning method according to an embodiment of the present application;
fig. 12 is a schematic structural diagram of a movement trajectory planning apparatus according to an embodiment of the present application;
fig. 13 is a schematic structural diagram of a motion trajectory planning apparatus according to an embodiment of the present application.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the technical solutions in the embodiments of the present application are further described in detail by the following embodiments in combination with the accompanying drawings. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.
It should be noted that the execution main body of the following method embodiments may be a motion trajectory planning device, and the device may be implemented by software, hardware, or a combination of software and hardware to become part or all of the motion trajectory planning apparatus. Optionally, the motion trajectory planning device includes, but is not limited to, an industrial automation device, a numerical control machine, a robot, and other intelligent devices. In the motion control process of the intelligent equipment, when the equipment is started, operated and stopped, the accurate position, no overshoot and no vibration are ensured, the premise that the intelligent equipment runs at high speed and high precision is provided, and the basis for realizing the control process is to plan a smooth motion trail curve.
In the conventional technology, the motion trajectory of the intelligent device is generally planned by an S-shaped curve planning method. However, since the conventional S-shaped curve planning method is only applicable to the case where the initial acceleration is zero, when the initial acceleration of the smart device is not zero, the conventional S-shaped curve planning method cannot be directly applied to perform the motion trajectory planning. Based on this, the technical scheme provided by the embodiment of the application aims to solve the technical problems existing in the traditional technology.
Fig. 1 is a schematic flow chart of a motion trajectory planning method provided in an embodiment of the present application. As shown in fig. 1, the method may include:
s101, obtaining motion control parameters of an object to be planned.
Wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned.
The object to be planned refers to an object needing to be subjected to motion trail planning, and can be intelligent equipment such as a robot and a numerical control machine tool. The starting state of the object to be planned may comprise a starting velocity, a starting acceleration and a starting position of the object to be planned. The target state may include a target velocity, a target position, and a target acceleration of the object to be planned. The constraint parameters may include a maximum velocity, a maximum acceleration, and a maximum jerk that can be allowed for the object to be planned to move from the starting state to the target state.
As an optional implementation manner, the process of S101 may be: and acquiring the motion control parameters of the object to be planned, which are input by the user.
S102, when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing.
Usually, the speed plotted by the sigmoid curve is sigmoidal, and if a displacement curve is desired, it is common practice to integrate the speed function to obtain the displacement. However, this is more complicated, and a simpler method is to form the S-shape into a ladder shape by a cut-and-fill method, and calculate the displacement by a method of calculating the area, as shown in FIG. 2.
However, when the initial acceleration of the object to be planned is not zero, the area calculation of the displacement is erroneous, as shown in fig. 3. Then, it is necessary to perform the initial acceleration zeroing process on the object to be planned to ensure that the initial acceleration of the object to be planned is equal to zero during planning.
Therefore, after the motion control parameters of the object to be planned are acquired, it is determined whether the initial acceleration in the initial state is zero. And if the initial acceleration in the initial state is not equal to zero, carrying out zero-returning processing on the initial acceleration. It should be noted that, in the process of returning the initial acceleration to zero, in addition to changing the initial acceleration to zero, the initial speed and the initial position in the initial state are correspondingly changed.
S103, performing S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameters.
And after the initial state after the zeroing processing is obtained, planning the motion trail of the object to be planned by adopting a conventional S-shaped curve planning method based on the initial state, the target state and the constraint parameters after the zeroing processing. Specifically, the initial state, the target state and the constraint parameter after the zeroing process are substituted into the following formula 1 or a modification of the following formula 1, so that a displacement curve x (t) of the object to be planned can be obtained, and further, first-order derivation and second-order derivation are performed on the displacement curve, so that a velocity curve and an acceleration curve of the object to be planned can be obtained.
Equation 1:
Figure BDA0003230873690000061
wherein, x'0And v'0Respectively, the starting position and the starting velocity, x, in the initial state after the zeroing process1And v1Respectively target position and target velocity in the target state, vmax、amaxAnd jmaxThe maximum speed, the maximum acceleration and the maximum jerk of the object to be planned are respectively, T is the time required for moving from the initial state to the target state, and T is the planning time.
According to the movement trajectory planning method provided by the embodiment of the application, under the condition that the initial acceleration of the object to be planned is not equal to zero, zero resetting processing can be carried out on the initial acceleration, S-shaped curve planning is carried out on the object to be planned according to the initial state, the target state and the constraint parameters after the zero resetting processing, the planned trajectory curve is smoother, the speed can be frequently changed, certain stability is achieved, and the actual trajectory planning requirement of the object to be planned can be met.
In one embodiment, an alternative implementation of the zeroing process for the initial acceleration is also provided. On the basis of the foregoing embodiment, as shown in fig. 4, optionally, the process of performing a zeroing process on the initial acceleration in the foregoing S102 to obtain an initial state after the zeroing process may include:
s401, calculating the speed offset and the position offset caused by the initial acceleration return-to-zero processing.
Wherein, when the initial acceleration is changed to zero, the initial speed and the initial position at the corresponding time are also changed. Alternatively, the velocity shift amount Δ v 'caused after the initial acceleration return-to-zero processing and the position shift amount Δ x' caused after the initial acceleration return-to-zero processing may be determined based on the following procedure.
When the maximum speed is larger than a first value, calculating a speed offset amount delta v' caused by the initial acceleration zeroing processing according to the following formula 2 or a modification of the formula 2;
equation 2:
Figure BDA0003230873690000071
when the maximum speed is greater than a first value, calculating a position offset amount delta x' caused by the initial acceleration zeroing processing according to the following formula 3 or a modification of the formula 3;
equation 3:
Figure BDA0003230873690000072
wherein, a0Is the starting acceleration in the starting state, jmaxFor the maximum jerk, the first value is related to a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk, e.g., the first value may be v0And
Figure BDA0003230873690000073
and (4) summing.
Specifically, when the initial acceleration is greater than zero and the maximum speed of the object to be planned is greater than the first value, the initial acceleration may be subjected to the zeroing process with reference to the process shown in fig. 5, and the speed offset Δ v 'caused by the zeroing process of the initial acceleration is calculated according to the above formula 2 or the modification of the formula 2, and the position offset Δ x' caused by the zeroing process of the initial acceleration is calculated according to the above formula 3 or the modification of the formula 3. The circle point in fig. 5 is a return point of the initial acceleration.
When the initial acceleration is smaller than zero and the maximum speed of the object to be planned is greater than the first value, the process shown in fig. 6 may be referred to perform zeroing processing on the initial acceleration, and a speed offset Δ v 'caused by the zeroing processing on the initial acceleration is calculated according to the above formula 2 or the modification of the formula 2, and a position offset Δ x' caused by the zeroing processing on the initial acceleration is calculated according to the modification of the above formula 3 or the modification of the formula 3. The circle point in fig. 6 is a return point of the initial acceleration.
When the maximum speed is smaller than a first value, calculating a speed offset amount delta v' caused by the initial acceleration zeroing processing according to the following formula 4 or a modification of the formula 4;
equation 4:
Figure BDA0003230873690000081
when the maximum speed is smaller than a first value, calculating a position offset amount delta x' caused by the initial acceleration zeroing processing according to the following formula 5 or a modification of the formula 5;
equation 5:
Figure BDA0003230873690000082
specifically, when the initial acceleration is greater than zero and the maximum speed of the object to be planned is less than the first value, the initial acceleration may be subjected to the zeroing process with reference to the process shown in fig. 7, and the speed offset Δ v 'caused by the zeroing process of the initial acceleration is calculated according to the above formula 4 or the modification of the formula 4, and the position offset Δ x' caused by the zeroing process of the initial acceleration is calculated according to the modification of the above formula 5 or the modification of the formula 5. The circle point in fig. 7 is a return point of the initial acceleration.
When the initial acceleration is smaller than zero and the maximum velocity of the object to be planned is smaller than the first value, the process shown in fig. 8 may be referred to perform the zeroing process on the initial acceleration, and according to the above formula 4 or the modification of the formula 4, the velocity offset Δ v 'caused by the zeroing process on the initial acceleration is calculated, and according to the above formula 5 or the modification of the formula 5, the position offset Δ x' caused by the zeroing process on the initial acceleration is calculated. The circle point in fig. 8 is a return point of the initial acceleration.
S402, compensating the initial speed in the initial state according to the speed offset to obtain the initial speed after the zero returning processing.
After the speed offset is obtained, the speed offset is used to compensate the initial speed in the initial state, for example, the speed offset and the initial speed in the initial state may be summed, so as to obtain the initial speed after the zeroing process.
And S403, compensating the initial position in the initial state according to the position offset to obtain the initial position after the zeroing processing.
After the position offset is obtained, the starting position in the starting state is compensated by using the position offset, for example, the position offset and the starting position in the starting state may be summed, so as to obtain the starting position after the zeroing process.
In this embodiment, the zero-returning processing can be performed on the initial acceleration of the object to be planned in combination with different situations, and the initial velocity and the initial position after the zero-returning processing are solved, so that the motion trajectory planning can be performed on the object to be planned based on a conventional S-shaped curve planning method.
In practical applications, the constraint parameter given above, i.e. the maximum speed, is often not achieved in cases where the distance between the starting position and the target position of the object to be planned has small variations with respect to speed and acceleration. For this case, the motion trajectory planning may be performed on the object to be planned in combination with the following process. On the basis of the above embodiment, optionally, the acquired constraint parameters may include a maximum speed, a maximum acceleration, and a maximum jerk. Alternatively, as shown in fig. 9, the S103 may include:
and S901, judging whether the maximum speed of the object to be planned can be reached.
If yes, executing S902; if not, S903-S904 are executed.
Optionally, the process of S901 may be: calculating a parameter T for characterizing whether the maximum speed of the object to be planned can be reached according to the following formula 6 or a variant of formula 6lim；
Equation 6:
Figure BDA0003230873690000101
when said T islimWhen the speed is larger than zero, determining that the maximum speed of the object to be planned can be reached;
when said T islimWhen the speed is less than zero, determining that the maximum speed of the object to be planned cannot be reached;
wherein Δ x is a difference between a target position and a starting position of the object to be planned, x0' is the starting position after the zeroing process, v0' initial speed after Return-to-zero, TaTime corresponding to a complete acceleration segment constrained by said maximum jerk, TdTime, v, corresponding to a complete deceleration segment constrained by said maximum jerkmaxIs said maximum velocity, v1Is the target speed of the object to be planned.
S902, performing S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum speed, the maximum acceleration and the maximum jerk after the zeroing processing.
The initial state, the target state, the maximum speed, the maximum acceleration and the maximum jerk after the zeroing process are substituted into the formula 1 or the modification of the formula 1, so that a displacement curve x (t) of the object to be planned can be obtained, and further, first-order derivation and second-order derivation are performed on the displacement curve, so that a speed curve and an acceleration curve of the object to be planned can be obtained.
And S903, calculating the maximum reachable speed of the object to be planned.
It is generally desirable that the trajectory planned by the sigmoid curve be the shortest in time, and therefore, each segment must have a maximum amount of constraint. For a conventional S-shaped curve, the first section is that the acceleration is required to be maximum, the second section is that the acceleration is required to be maximum, the third section is that the acceleration is also required to be maximum, the fourth section is that the speed is required to be maximum, the fifth section is that the acceleration is required to be maximum, the sixth section is that the deceleration is required to be maximum, and the seventh section is that the acceleration is required to be maximum. The embodiment of the application considers the situation that the maximum speed cannot be reached, so the fourth section is not provided. In order to determine the maximum achievable speed, it is conceivable to plot the maximum achievable speed v which is varied under the given constraintslimThe sum of the displacements is calculated for the acceleration and deceleration phases only, i.e. the maximum achievable velocity v of the object to be planned is solved by newton's method by means of the following equation 7 or a variant of equation 7lim；
Equation 7:
Figure BDA0003230873690000111
wherein, f (v)lim) Is equal to said maximum achievable velocity vlimFunction of correlation, xadIs the displacement sum of the acceleration section and the deceleration section which are constrained by the maximum jerk, x1Is the target position, x, of the object to be planned0For the initial position of the object to be planned, x is the initial acceleration zero0May be the starting position after the zeroing process.
The sufficient and necessary conditions for Newton method convergence are very strict, and it needs to be ensured that the function to be solved is monotonous and the derivative value can participate in operation (cannot be zero or infinite). As can be seen from fig. 10 (the horizontal axis in fig. 10 is the maximum achievable velocity vlimThe longitudinal axis represents the displacement of the non-uniform-speed section, and the non-uniform-speed section displacement refers to the xad) Albeit f (v)lim) The condition for convergence of Newton's method is not satisfied, but f (v)lim) The segments are monotonous and the derivative values outside the two non-derivative points can participate in the operation. Therefore, the region where the solution is located may be determined in advance before the solution, and then the value of the solution in the same region is selected as an initial value of the iteration, and the iteration solution is performed according to the newton method.
And S904, carrying out S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum reachable speed, the maximum acceleration and the maximum jerk after the zeroing processing.
The initial state, the target state, the maximum reachable speed, the maximum acceleration and the maximum jerk after the zeroing process are substituted into the formula 1 or the modification of the formula 1, so that a displacement curve x (t) of the object to be planned can be obtained, and then first-order derivation and second-order derivation are performed on the displacement curve, so that a speed curve and an acceleration curve of the object to be planned can be obtained.
In this embodiment, under the condition that the maximum speed in the constraint parameter cannot be reached, the maximum reachable speed of the object to be planned can be solved, and S-shaped curve planning can be performed based on the maximum reachable speed, so that the planned motion trajectory curve better meets the actual motion control requirement of the object to be planned.
Referring to fig. 11, in order to facilitate understanding of those skilled in the art, a motion trajectory planning process for an object to be planned is described below with reference to fig. 11. As shown in fig. 11, the method may include:
and S1101, acquiring motion control parameters of the object to be planned.
And S1102, when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing.
Optionally, the process of S1102 may be: calculating the speed offset and the position offset caused by the initial acceleration return-to-zero processing; compensating the initial speed in the initial state according to the speed offset to obtain the initial speed after the zeroing processing; and compensating the initial position in the initial state according to the position offset to obtain the initial position after the zeroing processing.
S1103, judging whether the maximum speed of the object to be planned can be reached.
If yes, S1104 is performed. If yes, then S1105-S1106 are executed.
And S1104, performing S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum speed, the maximum acceleration and the maximum jerk after the zeroing processing.
And S1105, calculating the maximum reachable speed of the object to be planned.
And S1106, performing S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum reachable speed, the maximum acceleration and the maximum jerk after the zeroing processing.
Fig. 12 is a schematic structural diagram of a motion trajectory planning apparatus according to an embodiment of the present application. As shown in fig. 12, the apparatus may include: an acquisition module 1201, a processing module 1202, and a planning module 1203.
Specifically, the obtaining module 1201 is configured to obtain a motion control parameter of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
the processing module 1202 is configured to perform zeroing processing on the initial acceleration when the initial acceleration in the initial state is not equal to zero, so as to obtain an initial state after the zeroing processing;
the planning module 1203 is configured to perform S-shaped curve planning on the object to be planned according to the initial state after the zeroing process, the target state, and the constraint parameter.
The movement track planning device provided by the embodiment of the application can carry out zero-returning processing on the initial acceleration under the condition that the initial acceleration of the object to be planned is not equal to zero, and carries out S-shaped curve planning on the object to be planned according to the initial state, the target state and the constraint parameters after the zero-returning processing, so that the planned track curve is smoother, the speed can be frequently changed, certain stability is realized, and the actual track planning requirement of the object to be planned can be met.
Optionally, the constraint parameters include maximum velocity, maximum acceleration, and maximum jerk;
planning module 1203 may include: the device comprises a judging unit, a first calculating unit and a planning unit.
Specifically, the judging unit is configured to judge whether the maximum speed of the object to be planned can be reached;
the first calculating unit is used for calculating the maximum reachable speed of the object to be planned when the judging unit judges that the maximum speed of the object to be planned cannot be reached;
and the planning unit is used for carrying out S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum reachable speed, the maximum acceleration and the maximum jerk after the zeroing processing.
On the basis of the above embodiments, the processing module 1202 may optionally include a second calculating unit, a velocity compensating unit, and a position compensating unit.
Specifically, the second calculating unit is configured to calculate a speed offset and a position offset caused by the initial acceleration return-to-zero processing;
the speed compensation unit is used for compensating the initial speed in the initial state according to the speed offset to obtain the initial speed after the zero return processing;
and the position compensation unit is used for compensating the initial position in the initial state according to the position offset to obtain the initial position after the zeroing processing.
On the basis of the foregoing embodiment, optionally, when the maximum speed is greater than the first value, the second calculating unit is specifically configured to calculate a speed offset Δ v' caused after the initial acceleration zeroing process according to the following formula 1 or a modification of formula 1;
equation 1:
Figure BDA0003230873690000141
the second calculating unit is further specifically configured to calculate a position offset Δ x' caused after the initial acceleration zeroing process according to the following formula 2 or a modification of formula 2;
equation 2:
Figure BDA0003230873690000142
wherein, a0Is the starting acceleration in the starting state, jmaxThe first value is associated with a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk for the maximum jerk.
On the basis of the foregoing embodiment, optionally, when the maximum speed is smaller than the first value, the second calculating unit is specifically configured to calculate a speed offset Δ v' caused after the initial acceleration zeroing process according to the following formula 3 or a modification of formula 3;
equation 3:
Figure BDA0003230873690000143
the second calculating unit is further specifically configured to calculate a position offset Δ x' caused after the initial acceleration zeroing process according to the following formula 4 or a modification of formula 4;
equation 4:
Figure BDA0003230873690000151
wherein, a0Is the starting acceleration in the starting state, jmaxThe first value is associated with a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk for the maximum jerk.
On the basis of the foregoing embodiment, optionally, the determining unit is specifically configured to calculate a parameter T for characterizing whether the maximum speed of the object to be planned can be reached according to the following formula 5 or a modification of the following formula 5lim；
Equation 5:
Figure BDA0003230873690000152
when said T islimWhen the speed is larger than zero, determining that the maximum speed of the object to be planned can be reached; when said T islimWhen the speed is less than zero, determining that the maximum speed of the object to be planned cannot be reached; wherein Δ x is a difference between a target position and a starting position of the object to be planned, x0' is the starting position after the zeroing process, v0' initial speed after Return-to-zero, TaTime corresponding to a complete acceleration segment constrained by said maximum jerk, TdTime, v, corresponding to a complete deceleration segment constrained by said maximum jerkmaxIs said maximum velocity, v1Is the target speed of the object to be planned.
On the basis of the foregoing embodiment, optionally, the first calculating unit is specifically configured to obtain the maximum reachable velocity v of the object to be planned by solving the following formula 6 or a modification of the following formula 6 through newton's methodlim；
Equation 6:
Figure BDA0003230873690000153
wherein, f (v)lim) Is equal to said maximum achievable velocity vlimFunction of correlation, xadTo at the maximum jerkDisplacement sum, x, of acceleration and deceleration sections being constrained1Is the target position, x, of the object to be planned0Is the starting position of the object to be planned.
In one embodiment, a motion trajectory planning apparatus is provided. As shown in fig. 13, the motion trail planning apparatus may include a processor, a memory, a network interface, and a database connected through a system bus. Wherein the processor of the motion trajectory planning device is configured to provide computational and control capabilities. The memory of the motion trail planning device comprises a nonvolatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of an operating system and computer programs in the non-volatile storage medium. The database of the motion trail planning equipment is used for storing data in the motion trail planning process. The network interface of the motion trail planning equipment is used for being connected and communicated with an external terminal through a network. The computer program is executed by a processor to implement a motion trajectory planning method.
In one embodiment, a motion trajectory planning device is provided, comprising a memory and a processor, the memory having a computer program stored therein, the processor implementing the following steps when executing the computer program:
acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing;
and performing S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter.
Optionally, the constraint parameters include maximum velocity, maximum acceleration, and maximum jerk; in one embodiment, the processor, when executing the computer program, further performs the steps of: judging whether the maximum speed of the object to be planned can be reached; if not, calculating the maximum reachable speed of the object to be planned; and performing S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum reachable speed, the maximum acceleration and the maximum jerk after the zeroing processing.
In one embodiment, the processor, when executing the computer program, further performs the steps of: calculating the speed offset and the position offset caused by the initial acceleration return-to-zero processing; compensating the initial speed in the initial state according to the speed offset to obtain the initial speed after the zeroing processing; and compensating the initial position in the initial state according to the position offset to obtain the initial position after the zeroing processing.
In one embodiment, the processor, when executing the computer program, further performs the steps of: when the maximum speed is larger than a first numerical value, calculating a speed offset delta v' caused by the initial acceleration return-to-zero processing according to the following formula 1;
equation 1:
Figure BDA0003230873690000171
calculating a position offset amount delta x' caused after the initial acceleration zeroing processing according to the following formula 2;
equation 2:
Figure BDA0003230873690000172
wherein, a0Is the starting acceleration in the starting state, jmaxThe first value is associated with a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk for the maximum jerk.
In one embodiment, the processor, when executing the computer program, further performs the steps of: when the maximum speed is smaller than a first numerical value, calculating a speed offset delta v' caused by the initial acceleration return-to-zero processing according to the following formula 3;
equation 3:
Figure BDA0003230873690000181
calculating a position offset amount delta x' caused after the initial acceleration zeroing processing according to the following formula 4;
equation 4:
Figure BDA0003230873690000182
wherein, a0Is the starting acceleration in the starting state, jmaxThe first value is associated with a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk for the maximum jerk.
In one embodiment, the processor, when executing the computer program, further performs the steps of: calculating a parameter T for representing whether the maximum speed of the object to be planned can be reached according to the following formula 5lim；
Equation 5:
Figure BDA0003230873690000183
when said T islimWhen the speed is larger than zero, determining that the maximum speed of the object to be planned can be reached; when said T islimWhen the speed is less than zero, determining that the maximum speed of the object to be planned cannot be reached; wherein Δ x is a difference between a target position and a starting position of the object to be planned, x0' is the starting position after the zeroing process, v0' initial speed after Return-to-zero, TaTime corresponding to a complete acceleration segment constrained by said maximum jerk, TdTime, v, corresponding to a complete deceleration segment constrained by said maximum jerkmaxIs said maximum velocity, v1Is the target speed of the object to be planned.
In one embodiment, the processor, when executing the computer program, further performs the steps of: solving by Newton methodEquation 6, the maximum achievable velocity v of the object to be planned is obtainedlim；
Equation 6:
Figure BDA0003230873690000191
wherein, f (v)lim) Is equal to said maximum achievable velocity vlimFunction of correlation, xadIs the displacement sum of the acceleration section and the deceleration section which are constrained by the maximum jerk, x1Is the target position, x, of the object to be planned0Is the starting position of the object to be planned.
In one embodiment, a computer-readable storage medium is provided, having a computer program stored thereon, which when executed by a processor, performs the steps of:
acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing;
and performing S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter.
The movement track planning device, the equipment and the storage medium provided in the above embodiments can execute the movement track planning method provided in any embodiment of the present disclosure, and have corresponding functional modules and beneficial effects for executing the method. For technical details that are not described in detail in the above embodiments, reference may be made to a motion trajectory planning method provided in any embodiment of the present disclosure.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by hardware instructions of a computer program, which can be stored in a non-volatile computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the embodiments provided herein may include non-volatile and/or volatile memory, among others. Non-volatile memory can include read-only memory (ROM), Programmable ROM (PROM), Electrically Programmable ROM (EPROM), Electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM (SDRAM), Double Data Rate SDRAM (DDRSDRAM), Enhanced SDRAM (ESDRAM), Synchronous Link DRAM (SLDRAM), Rambus Direct RAM (RDRAM), direct bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the present application. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims (10)

1. A motion trajectory planning method is characterized by comprising the following steps:
acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
when the initial acceleration in the initial state is not equal to zero, carrying out zero returning processing on the initial acceleration to obtain the initial state after the zero returning processing;
and performing S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter.
2. The method of claim 1, wherein the constraint parameters include a maximum velocity, a maximum acceleration, and a maximum jerk;
performing S-shaped curve planning on the object to be planned according to the initial state, the target state and the constraint parameter after the zeroing process, including:
judging whether the maximum speed of the object to be planned can be reached;
if not, calculating the maximum reachable speed of the object to be planned;
and performing S-shaped curve planning on the object to be planned according to the initial state, the target state, the maximum reachable speed, the maximum acceleration and the maximum jerk after the zeroing processing.
3. The method of claim 2, wherein the zeroing the initial acceleration to obtain a zeroed initial state comprises:
calculating the speed offset and the position offset caused by the initial acceleration return-to-zero processing;
compensating the initial speed in the initial state according to the speed offset to obtain the initial speed after the zeroing processing;
and compensating the initial position in the initial state according to the position offset to obtain the initial position after the zeroing processing.
4. The method of claim 3, wherein said calculating a velocity offset and a position offset caused by said initial acceleration zeroing process when said maximum velocity is greater than a first value comprises:
calculating a speed offset delta v' caused by the initial acceleration return-to-zero processing according to the following formula 1;
equation 1:
Figure FDA0003230873680000021
calculating a position offset amount delta x' caused after the initial acceleration zeroing processing according to the following formula 2;
equation 2:
Figure FDA0003230873680000022
wherein, a0Is the starting acceleration in the starting state, jmaxThe first value is associated with a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk for the maximum jerk.
5. The method of claim 3, wherein when the maximum velocity is less than a first value, the calculating the velocity offset and the position offset caused by the initial acceleration zeroing process comprises:
calculating a speed offset delta v' caused by the initial acceleration return-to-zero processing according to the following formula 3;
equation 3:
Figure FDA0003230873680000023
calculating a position offset amount delta x' caused after the initial acceleration zeroing processing according to the following formula 4;
equation 4:
Figure FDA0003230873680000031
wherein, a0Is the starting acceleration in the starting state, jmaxIs the maximum jerkThe first value is associated with a starting velocity in the starting state, a starting acceleration in the starting state, and the maximum jerk.
6. The method according to any of claims 2 to 5, wherein said determining whether a maximum speed of the object to be planned can be reached comprises:
calculating a parameter T for representing whether the maximum speed of the object to be planned can be reached according to the following formula 5lim；
Equation 5:
Figure FDA0003230873680000032
when said T islimWhen the speed is larger than zero, determining that the maximum speed of the object to be planned can be reached;
when said T islimWhen the speed is less than zero, determining that the maximum speed of the object to be planned cannot be reached;
wherein Δ x is a difference between a target position and a starting position of the object to be planned, x0' is the starting position after the zeroing process, v0' initial speed after Return-to-zero, TaTime corresponding to a complete acceleration segment constrained by said maximum jerk, TdTime, v, corresponding to a complete deceleration segment constrained by said maximum jerkmaxIs said maximum velocity, v1Is the target speed of the object to be planned.
7. The method according to any of claims 2 to 5, wherein said calculating a maximum achievable velocity of the object to be planned comprises:
solving the following formula 6 by a Newton method to obtain the maximum reachable speed v of the object to be plannedlim；
Equation 6:
Figure FDA0003230873680000033
wherein, f (v)lim) Is equal to said maximum achievable velocity vlimFunction of correlation, xadIs the displacement sum of the acceleration section and the deceleration section which are constrained by the maximum jerk, x1Is the target position, x, of the object to be planned0Is the starting position of the object to be planned.
8. A motion trajectory planning apparatus, comprising:
the system comprises an acquisition module, a planning module and a planning module, wherein the acquisition module is used for acquiring motion control parameters of an object to be planned; wherein the motion control parameters comprise a starting state, a target state and constraint parameters required for moving from the starting state to the target state of the object to be planned;
the processing module is used for carrying out zero resetting processing on the initial acceleration when the initial acceleration in the initial state is not equal to zero to obtain the initial state after the zero resetting processing;
and the planning module is used for carrying out S-shaped curve planning on the object to be planned according to the initial state after the zeroing processing, the target state and the constraint parameter.
9. A motion trajectory planning device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor realizes the steps of the method according to any one of claims 1 to 7 when executing the computer program.
10. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 7.
CN202110986578.9A 2021-08-26 2021-08-26 Motion trajectory planning method, device, equipment and storage medium Active CN113703399B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110986578.9A CN113703399B (en) 2021-08-26 2021-08-26 Motion trajectory planning method, device, equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110986578.9A CN113703399B (en) 2021-08-26 2021-08-26 Motion trajectory planning method, device, equipment and storage medium

Publications (2)

Publication Number Publication Date
CN113703399A true CN113703399A (en) 2021-11-26
CN113703399B CN113703399B (en) 2022-09-16

Family

ID=78654999

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110986578.9A Active CN113703399B (en) 2021-08-26 2021-08-26 Motion trajectory planning method, device, equipment and storage medium

Country Status (1)

Country Link
CN (1) CN113703399B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0070654A2 (en) * 1981-07-10 1983-01-26 Gould Inc. Motion controller
JP2011191267A (en) * 2010-03-16 2011-09-29 Mitsubishi Space Software Kk Apparatus, program and method of specifying target speed
CN103135501A (en) * 2013-02-01 2013-06-05 北京配天大富精密机械有限公司 Acceleration and deceleration controlling method based on S-shaped curve and equipment using the same and numerically-controlled machine tool
CN103253565A (en) * 2013-04-08 2013-08-21 深圳市海浦蒙特科技有限公司 Elevator, and method and device for setting operating speed of elevator
CN106020122A (en) * 2016-06-17 2016-10-12 浙江理工大学 Newton's method-based numerical control trajectory control method
CN109991934A (en) * 2019-03-10 2019-07-09 天津大学 A kind of time optimal online S type deceleration planning method
CN111515548A (en) * 2020-04-09 2020-08-11 大连理工大学 Method for optimizing laser processing scanning track of micro-curvature radius antenna
CN111813052A (en) * 2020-08-06 2020-10-23 陕西开普欣信息科技有限公司 Numerical control system structure and general numerical control system
US20200384645A1 (en) * 2019-06-10 2020-12-10 Glowbuzzer Ltd Method and apparatus for trajectory-planning
US20210156713A1 (en) * 2018-08-06 2021-05-27 Tencent Technology (Shenzhen) Company Ltd Method and apparatus for reconstructing motion track, storage medium, and electronic device

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0070654A2 (en) * 1981-07-10 1983-01-26 Gould Inc. Motion controller
JP2011191267A (en) * 2010-03-16 2011-09-29 Mitsubishi Space Software Kk Apparatus, program and method of specifying target speed
CN103135501A (en) * 2013-02-01 2013-06-05 北京配天大富精密机械有限公司 Acceleration and deceleration controlling method based on S-shaped curve and equipment using the same and numerically-controlled machine tool
CN103253565A (en) * 2013-04-08 2013-08-21 深圳市海浦蒙特科技有限公司 Elevator, and method and device for setting operating speed of elevator
CN106020122A (en) * 2016-06-17 2016-10-12 浙江理工大学 Newton's method-based numerical control trajectory control method
US20210156713A1 (en) * 2018-08-06 2021-05-27 Tencent Technology (Shenzhen) Company Ltd Method and apparatus for reconstructing motion track, storage medium, and electronic device
CN109991934A (en) * 2019-03-10 2019-07-09 天津大学 A kind of time optimal online S type deceleration planning method
US20200384645A1 (en) * 2019-06-10 2020-12-10 Glowbuzzer Ltd Method and apparatus for trajectory-planning
CN111515548A (en) * 2020-04-09 2020-08-11 大连理工大学 Method for optimizing laser processing scanning track of micro-curvature radius antenna
CN111813052A (en) * 2020-08-06 2020-10-23 陕西开普欣信息科技有限公司 Numerical control system structure and general numerical control system

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
徐剑琴等: "冗余度机械臂零初始加速度运动规划方案研究", 《计算机仿真》 *
方懿等: "基于谐波轨迹和中枢模式发生器的工业机器人在线轨迹规划", 《上海交通大学学报》 *

Also Published As

Publication number Publication date
CN113703399B (en) 2022-09-16

Similar Documents

Publication Publication Date Title
CN109959381B (en) Positioning method, positioning device, robot and computer readable storage medium
CN107703756B (en) Kinetic model parameter identification method and device, computer equipment and storage medium
JP7183446B2 (en) Apparatus and method for controlling machine operation, and storage medium
CN107561933B (en) Speed adjusting method and device, computer equipment and storage medium
CN112180925B (en) AGV track following method and device and processing equipment
CN111177934B (en) Method, apparatus and storage medium for reference path planning
JP6808106B1 (en) Machining program conversion device, numerical control device, machining program conversion method and machine learning device
CN115388899A (en) Mobile robot vision inertia fusion SLAM method based on variational Bayes
CN112975954B (en) Control method of robot arm, computer device, and storage medium
CN111256727B (en) Method for improving accuracy of odometer based on Augmented EKF
CN112540620A (en) Reinforced learning method and device for foot type robot and electronic equipment
JP2018087753A (en) Nonlinear optimization program for continuous value optimization problem, and route search program and route search device
CN113703399B (en) Motion trajectory planning method, device, equipment and storage medium
US11305429B2 (en) Synchronization of multiple robots
CN115712384A (en) Gesture control method, device and system based on attitude sensor
CN116079714A (en) Six-axis mechanical arm track planning method based on B spline
CN113561175B (en) Path planning method and device of mechanical arm, computer equipment and storage medium
CN113260936A (en) Mobile body control device, mobile body control learning device, and mobile body control method
CN110962120B (en) Network model training method and device, and mechanical arm motion control method and device
CN114046800B (en) High-precision mileage estimation method based on double-layer filtering frame
CN112873209B (en) Positioning sensor time delay calibration method and device, computer equipment and storage medium
CN106289273A (en) A kind of data processing method and system
CN113467484A (en) Biped robot gait control method and device and computer equipment
Zhang et al. An incremental feedback control for uncertain mechanical system
CN112068490A (en) Trajectory planning method and device, electronic equipment and storage medium

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