A trajectory planning method, device, equipment and storage medium

Abstract

The application provides a trajectory planning method and device, equipment and a storage medium, aiming at a given third-order initial state, third-order terminal state and third-order constraint, a second-order optimal trajectory from a second-order initial state to a second-order terminal state is solved; if a position advance amount is less than an original position difference, a first intermediate state determined according to a speed constraint is introduced, a third-order optimal trajectory without a position constraint is obtained by solving a second-order optimal trajectory in sections; if the trajectory does not satisfy the position constraint, a second intermediate state satisfying the position constraint is solved, and a second-order optimal trajectory is solved again in sections to obtain a third-order optimal trajectory satisfying the third-order constraint. The third-order constraint problem is decomposed into multiple second-order sub-problems, the third-order optimal trajectory is gradually solved by gradually introducing the constraint condition and the segmentation solving splicing strategy, so that the third-order optimal trajectory under the asymmetric constraint with the position constraint is solved, the solving efficiency and success rate of the trajectory are improved, and the trajectory calculation cost is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of trajectory planning, and in particular to a trajectory planning method and device, equipment and a storage medium. BACKGROUND

[0002] High-end integrated circuit (IC) equipment, numerical control machine tools, industrial robots, etc. are listed as major technical equipment for implementing the strategy of manufacturing power. Among them, trajectory planning, as a key link of motion planning and control of mechatronic systems, has important applications in lithography machine stepping scanning, high-speed high-precision numerical control machining, robot complex operation, etc. The quality of trajectory planning greatly affects and even determines key performance indicators such as process efficiency, motion smoothness, equipment life, machining precision, and part surface quality.

[0003] In the field of trajectory planning, third-order constraint time-optimal trajectory planning has received widespread attention from academia and industry and is considered as one of the core technologies of high-speed high-precision motion control. The so-called third-order constraint refers to the strict control of kinematic parameters such as position, velocity, acceleration, and jerk within a given upper and lower bound during motion. Time-optimal refers to achieving the shortest theoretical time from the given initial state of position, velocity, and acceleration to the given terminal state of position, velocity, and acceleration under third-order constraints.

[0004] Currently, related technologies can achieve interpolation level precision, for example, in the case of an interpolation period of 1 millisecond, the system will perform trajectory planning every 1 millisecond, but only plan the jerk in the next interpolation period. However, this method of planning only the current jerk in each interpolation period has certain limitations, and its trajectory planning efficiency is relatively low, making it difficult to meet the needs of high-speed high-precision numerical control machining and other scenarios. At the same time, this method cannot solve the third-order optimal trajectory with position constraints. In addition, as the trajectory length increases, the computational cost also increases accordingly, resulting in suboptimal performance in offline planning scenarios. SUMMARY

[0005] Therefore, the present application provides a trajectory planning method, device, equipment and storage medium to solve the problems in the related art.

[0006] In a first aspect of the present application, a trajectory planning method is provided, which comprises:

[0007] For a given third-order initial state (P s ,V s ,A s ), third-order terminal state (P f ,V f ,A f) and a third order constraint, a first second order trajectory from a second order initial state (V s ,A s ) to a second order terminal state (V f ,A f ) is solved using a trajectory planning model, wherein the second order trajectory solved using the trajectory planning model is a second order optimal trajectory satisfying acceleration and jerk constraints in the third order constraint;

[0008] If a position advance according to the first second order trajectory is less than an original position difference according to the (P s ,V s ,A s ) and the (P f ,V f ,A f ), then a first third order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) without position constraint is obtained according to a second second order trajectory and a third second order trajectory, wherein the second and third second order trajectories are respectively second order trajectories from the (V s ,A s ) to a first intermediate state and from the first intermediate state to the (V f ,A f ) solved using the trajectory planning model, and the first intermediate state is determined depending on a velocity constraint in the third order constraint;

[0009] If the first third order optimal trajectory does not satisfy a position constraint in the third order constraint, a second intermediate state satisfying the position constraint is solved using the trajectory planning model, and a second third order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) satisfying the third order constraint is obtained according to a fourth second order trajectory and a fifth second order trajectory, wherein the fourth and fifth second order trajectories are respectively second order trajectories from the (V s ,A s ) to a second intermediate state and from the second intermediate state to the (V f ,A f ).

[0010] According to one embodiment of the present application, the method further comprises:

[0011] If the position advancement amount is equal to the original position difference, a third third-order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) is obtained according to the first second-order trajectory.

[0012] According to one embodiment of the present application, the method further comprises:

[0013] If the position advancement amount is greater than the original position difference, it is converted into a case where the position advancement amount is less than the original position difference through time reversal, so as to solve the first third-order optimal trajectory.

[0014] According to one embodiment of the present application, the first third-order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) without position constraint is obtained according to a second second-order trajectory and a third second-order trajectory, comprising:

[0015] The velocity component of the first intermediate state is set to the upper limit of the velocity constraint, the acceleration component is set to zero, and the second second-order trajectory from the (V s ,A s ) to the first intermediate state and the third second-order trajectory from the first intermediate state to the (V f ,A f ) are solved respectively by using the trajectory planning model;

[0016] The first position component and the second position component corresponding to the first intermediate state are obtained respectively according to the second second-order trajectory and the third second-order trajectory;

[0017] If the first position component is less than or equal to the second position component, the first third-order optimal trajectory is obtained according to the second second-order trajectory and the third second-order trajectory;

[0018] If the first position component is greater than the second position component, the first intermediate state is re-determined, and the first third-order optimal trajectory is obtained according to the second second-order trajectory and the third second-order trajectory which are re-solved by using the trajectory planning model, wherein the re-determined first intermediate state satisfies: the third position component corresponding to the re-determined first intermediate state obtained according to the re-solved second second-order trajectory is equal to the fourth position component corresponding to the re-determined first intermediate state obtained according to the re-solved third second-order trajectory.

[0019] According to one embodiment of this application, the step of using the trajectory planning model to solve for the second intermediate state that satisfies the position constraints includes:

[0020] Using the trajectory planning model, the sixth second-order trajectory from the target second-order state to the target intermediate state is solved, wherein the target second-order state is the (V s A s ) or the (V) f A f The velocity component of the target intermediate state is zero, and the corresponding position component is the upper or lower bound of the position constraint.

[0021] If a feasible solution exists, the target intermediate state corresponding to the sixth second-order trajectory is determined as the second intermediate state.

[0022] According to one embodiment of this application, the third-order constraint is satisfied based on the fourth second-order trajectory and the fifth second-order trajectory, and the trajectory originates from (P). s V s A s ) to the (P) f V f A f The second and third order optimal trajectories include:

[0023] If the target second-order state is (V) s A s ), then according to the sixth second-order trajectory, the trajectory from (P) is obtained. s V s A s The fourth optimal trajectory of the third order to the third order state corresponding to the second intermediate state;

[0024] Based on the seventh and eighth second-order trajectories, the unconstrained third-order state from the second intermediate state to the (P) is obtained. f V f A f The fifth and third-order optimal trajectories of (V), wherein the seventh and eighth second-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectories obtained using the trajectory planning model. f A f The second-order trajectory of ).

[0025] By splicing the fourth and fifth optimal trajectories, the second optimal trajectory is obtained.

[0026] According to one embodiment of this application, the third-order constraint is satisfied based on the fourth second-order trajectory and the fifth second-order trajectory, and the trajectory originates from (P).s ,V s ,A s ) to the second intermediate state corresponding to the third-order state of the (P f ,V f ,A f ) includes:

[0027] If the target second-order state is the (V f ,A f ), a sixth third-order optimal trajectory from the (P f ,V f ,A f ) to the third-order state corresponding to the second intermediate state is obtained according to the sixth second-order trajectory;

[0028] A seventh third-order optimal trajectory from the third-order state corresponding to the second intermediate state to the (P s ,V s ,A s ) without position constraints is obtained according to a ninth second-order trajectory and a twelfth second-order trajectory, wherein the ninth and twelfth second-order trajectories are second-order trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V s ,A s ), respectively, which are solved by using the trajectory planning model;

[0029] The sixth third-order optimal trajectory and the seventh third-order optimal trajectory are spliced to obtain the second third-order optimal trajectory.

[0030] In a second aspect of the present application, a trajectory planning device is provided, which comprises:

[0031] A first trajectory planning unit is configured to solve, for a given third-order initial state (P s ,V s ,A s ), a third-order terminal state (P f ,V f ,A f ) and a third-order constraint, a first second-order trajectory from a second-order initial state (V s ,A s ) to a second-order terminal state (V f ,A f ) by using a trajectory planning model, wherein the second-order trajectory solved by using the trajectory planning model is a second-order optimal trajectory satisfying the acceleration and jerk constraints in the third-order constraint;

[0032] A second trajectory planning unit is configured to, if a position advance amount obtained according to the first second-order trajectory is less than a position advance amount obtained according to the (P s ,V s ,As ) and the original position difference obtained by the (P f ,V f ,A f ), a first third-order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) is obtained without position constraint according to a second second-order trajectory and a third second-order trajectory, wherein the second and third second-order trajectories are second-order trajectories from the (V s ,A s ) to a first intermediate state and from the first intermediate state to the (V f ,A f ) respectively, and the first intermediate state is determined according to the velocity constraint in the third-order constraint;

[0033] If the first third-order optimal trajectory does not satisfy the position constraint in the third-order constraint, a third trajectory planning unit is configured to solve a second intermediate state satisfying the position constraint by using the trajectory planning model, and obtain a second third-order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) satisfying the third-order constraint according to a fourth second-order trajectory and a fifth second-order trajectory, wherein the fourth and fifth second-order trajectories are second-order trajectories from the (V s ,A s ) to a second intermediate state and from the second intermediate state to the (V f ,A f ) respectively solved by using the trajectory planning model.

[0034] In a third aspect of the present application, an electronic device is provided, comprising a processor and a memory, the memory storing machine executable instructions capable of being executed by the processor, and the processor is configured to execute the machine executable instructions to implement the steps of the method proposed in the above embodiments.

[0035] In a fourth aspect of the present application, a machine readable storage medium is provided, the machine readable storage medium storing machine executable instructions, and the machine executable instructions are executed by a processor to implement the steps of the method proposed in the above embodiments.

[0036] As can be seen from the above technical solutions, for a given third-order initial state (P s ,V sA s ), third-order terminal state (P f , V f , A f ) and third-order constraint, solving second-order optimal trajectory from second-order initial state (V s , A s ) to second-order terminal

[0037] state (V f , A f ); if the position advance is less than the original position difference, introducing a first intermediate state dependent on the speed constraint determination, obtaining a third-order optimal trajectory without position constraint according to the second-order optimal trajectory from (V s , A s ) to the first intermediate state and from the first intermediate state to (V f , A f ); if the third-order optimal trajectory without position constraint does not satisfy the position constraint, introducing the position constraint, solving a second intermediate state satisfying the position constraint, and obtaining a third-order optimal trajectory satisfying the third-order constraint according to the second-order optimal trajectory from (V s , A s ) to the second intermediate state and from the second intermediate state to (V f , A f ). The third-order constraint problem is decomposed into multiple second-order sub-problems, the third-order optimal trajectory is solved gradually by introducing constraint conditions and solving and splicing in sections, and the third-order optimal trajectory with position constraint under asymmetric constraint is solved, the solving efficiency and success rate of the trajectory are improved, and the trajectory calculation cost is reduced.

[0038] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and cannot limit the present application. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 is a flowchart of a trajectory planning method provided by an embodiment of the present application;

[0040] Figure 2 is a schematic diagram of second-order optimal trajectory classification provided by an embodiment of the present application;

[0041] Figure 3 is a flowchart of solving a third-order optimal trajectory without position constraint provided by an embodiment of the present application;

[0042] Figure 4 is a schematic diagram of a third-order optimal trajectory without position constraint provided by an embodiment of the present application;

[0043] Figure 5is a schematic diagram of a third-order optimal trajectory with position constraints provided by an embodiment of the present application.

[0044] Figure 6 is a physical part drawing of numerical control machining provided by an embodiment of the present application.

[0045] Figure 7 is a structural schematic diagram of a trajectory planning device provided by an embodiment of the present application.

[0046] Figure 8 is a hardware structure schematic diagram of an electronic device shown by an exemplary embodiment of the present application. DETAILED DESCRIPTION

[0047] The exemplary embodiments will be described in detail herein with reference to the attached drawings. The following description is with reference to the drawings, in which like numerals represent like elements, unless otherwise specified. The following exemplary embodiments described in the following detailed description are not meant to be limiting in terms of the scope of the application. Rather, they are example only and merely illustrative of apparatus and methods consistent with the present application as defined by the appended claims.

[0048] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the present application. As used in this application, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.

[0049] In order to make the technical solutions provided by the embodiments of the present application better understood by those skilled in the art, and to make the above-mentioned purposes, features and advantages of the embodiments of the present application more apparent and easy to understand, the technical solutions in the embodiments of the present application are further described in detail below with reference to the drawings.

[0050] High-end integrated circuit (IC) equipment, numerical control machine tools, industrial robots, etc. are listed as major technical equipment for implementing the manufacturing power strategy. Among them, trajectory planning as a key link of motion planning and control of mechatronic systems has important applications in lithography machine step scanning, high-speed high-precision numerical control machining, robot complex operation, etc. The quality of trajectory planning greatly affects and even determines the key performance indicators such as process efficiency, motion smoothness, equipment life, machining precision, and part surface quality.

[0051] In the field of trajectory planning, the third-order constraint time-optimal trajectory planning has been widely concerned by the academia and industry, and is regarded as one of the core technologies of high-speed and high-precision motion control. The so-called third-order constraint refers to that the kinematic parameters such as position P (Position), velocity V (Velocity), acceleration A (Acceleration) and jerk J (Jerk) are strictly controlled within a given upper bound and lower bound during the motion process. The time-optimal refers to that the shortest time is theoretically achieved from the initial state of given position, velocity and acceleration to the terminal state of given position, velocity and acceleration under the third-order constraint.

[0052] At present, the related technology can achieve interpolation level precision, for example, in the case of an interpolation period of 1 millisecond, the system will plan a trajectory every 1 millisecond, but only plan the jerk in the next interpolation period. However, this method of planning only the current jerk in each interpolation period has certain limitations, and the trajectory planning efficiency is relatively low, which is difficult to meet the needs of high-speed and high-precision numerical control machining and other scenes; at the same time, this method cannot solve the third-order optimal trajectory with position constraints; in addition, as the trajectory length increases, the calculation cost will also increase accordingly, resulting in unsatisfactory performance in the offline planning scene.

[0053] Therefore, the embodiment of the present application discloses a trajectory planning method to solve the problems in the related art.

[0054] As shown in Figure 1 , Figure 1 is a flowchart of a trajectory planning method provided by the embodiment of the present application.

[0055] The trajectory planning method can include the following steps:

[0056] S101: For a given third-order initial state (P s ,V s ,A s ), a third-order terminal state (P f ,V f ,A f ) and a third-order constraint, a trajectory planning model is used to solve a first second-order trajectory from a second-order initial state (V s ,A s ) to a second-order terminal state (V f ,A f ), wherein the second-order trajectory solved by the trajectory planning model is a second-order optimal trajectory satisfying the acceleration and jerk constraints in the third-order constraint.

[0057] In the embodiment of the present application, the solution needs to give a third-order initial state (P s ,V s ,As ), third-order terminal state (P f , V f , A f ) and third-order constraint.

[0058] wherein P s is a position component of the third-order initial state, V s is a velocity component of the third-order initial state, A s is an acceleration component of the third-order initial state. P f is a position component of the third-order terminal state, V f is a velocity component of the third-order terminal state, A f is an acceleration component of the third-order terminal state. Exemplarily, the velocity component V s of the third-order initial state and the velocity component V f of the third-order terminal state can be non-zero.

[0059] wherein the third-order constraint comprises a position constraint, a velocity constraint, an acceleration constraint and a jerk constraint. Wherein the upper and lower bounds of the position constraint can be denoted as P max and P min respectively, the upper and lower bounds of the velocity constraint can be denoted as V max and V min respectively, the upper and lower bounds of the acceleration constraint can be denoted as A max and A min respectively, and the upper and lower bounds of the jerk constraint can be denoted as J max and J min respectively. Exemplarily, the third-order constraint can be a non-symmetrical third-order constraint, i.e. the upper and lower bounds of each motion parameter in the third-order constraint can not be equal, for example, the non-symmetrical position constraint is represented as P max ≠ |P min |, the non-symmetrical velocity constraint V max ≠ |V min |, and so on, so as to realize solving the third-order optimal trajectory under the non-symmetrical constraint.

[0060] In the embodiments of the present application, for a given third-order initial state (P s , V s , A s ) and third-order terminal state (P f , V f , A f ), the corresponding second-order initial state is (V s , A s ) and the second-order terminal state is (V f , A fA trajectory planning model is pre-set, which can be used to solve for the second-order optimal trajectory from the corresponding second-order initial state to the second-order terminal state, satisfying the given acceleration and jerk constraints.

[0061] Specifically, the input parameters of this trajectory planning model include: a first input parameter related to the second-order initial state, a second input parameter related to the second-order terminal state, and a third input parameter related to the constraints. The first input parameter includes the velocity and acceleration components of the second-order initial state, the second input parameter includes the velocity and acceleration components of the second-order terminal state, and the third input parameter includes the upper and lower bounds of the acceleration constraints and the jerk constraints.

[0062] Based on the above input parameters, the trajectory planning model can solve and output the second-order time-optimal trajectory from the second-order initial state to the second-order terminal state. This trajectory ensures that the acceleration does not exceed the upper and lower bounds of the acceleration constraint and the jerk does not exceed the upper and lower bounds of the jerk constraint in almost everywhere, that is, at each time point.

[0063] In some embodiments, based on Bang-Bang control, the second-order optimal trajectory can be pre-divided into two types of trajectories, and the control quantity switching rules, i.e., control laws, for each type of trajectory can be pre-defined.

[0064] The control law for the first type of trajectory can be expressed as 01 0 or 01 0, the control law for the second type of trajectory can be expressed as 0. 0 or 0 0.

[0065] Where 0 represents the jerk being equal to the upper bound J of the jerk. max , 0 The jerk equals the lower bound J of the jerk. min 1 indicates that the acceleration is equal to the upper bound A of the acceleration. max , 1 The acceleration is equal to the lower bound A of the acceleration. min .

[0066] It should be noted that in the first type of trajectory, the control law is 01. 0 The trajectory can be called an "acceleration" type trajectory, and the control law is... 01 The trajectory of 0 can be called a "deceleration" type trajectory; in the second type of trajectory, the control law is 0. 0 The trajectory can be called an "acceleration" type trajectory, and the control law is... 0 The trajectory of 0 can be called a "deceleration" type trajectory.

[0067] The following section discusses the "acceleration" type trajectory 01. 0and 0 0 An introduction is made.

[0068] For control law 0 0 , it means that the second-order trajectory is divided into three segments to solve, in order to achieve time-optimal control, the three-segment trajectory includes the first-segment trajectory 0, the second-segment trajectory 1 and the third-segment trajectory 0 . Its system behavior includes: the first-segment trajectory 0 represents motion with maximum jerk (i.e. the upper bound of jerk J max ) until reaching the maximum acceleration (i.e. the upper bound of acceleration A max ), i.e. keeping the jerk constant as J max , from the initial state (V s , A s ) to (V = V1, A = A max ); the second-segment trajectory 1 represents motion with maximum acceleration, i.e. keeping the jerk as zero and keeping the acceleration constant as A max , from (V = V1, A = A max ) to (V = V2, A = A max ); the third-segment trajectory 0 represents motion with minimum jerk (i.e. the lower bound of jerk J min ), i.e. keeping the jerk constant as J min , from (V = V2, A = A max ) to the terminal state (V f , A f ). The time of the above three-segment trajectory can be solved explicitly.

[0069] For control law 0 0 , it means that the second-order trajectory is divided into two segments to solve, in order to achieve time-optimal control, the two-segment trajectory includes the first-segment trajectory 0 and the second-segment trajectory 0. Its system behavior includes: the first-segment trajectory 0 represents motion with maximum jerk (i.e. the upper bound of jerk J max ), i.e. keeping the jerk constant as J max , from the initial state (V s , A s ) to (V = V3, A = A3); the second-segment trajectory 0 represents motion with minimum jerk (i.e. the lower bound of jerk J min ), i.e. keeping the jerk constant as J min , from (V = V3, A = A3) to the terminal state (V f , A f). The time of the above two trajectories cannot be decoupled to solve, and exemplarily, a nonlinear equation set can be listed, including but not limited to constructing a monomial quadratic equation to realize efficient solving by using a Grobner base method, directly solving a multivariate polynomial equation set by using a Newton method and the like, and enumerating solving within a limited precision by using an online computing idea. Considering that the time is a positive number, the equation has and only has one feasible solution.

[0070] Regarding the "deceleration" type trajectory 01 0 and 0 0, since the "deceleration" type trajectory can be obtained by using a negative "acceleration" type trajectory, the system behaviors of the two are not repeated here.

[0071] As Figure 2 shown, Figure 2 is a schematic diagram of a second-order optimal trajectory classification provided by an embodiment of the present application, and the two trajectories shown in the diagram are "acceleration" type trajectories, wherein the horizontal coordinate represents acceleration, the vertical coordinate represents speed, trajectory 1 represents that the initial state x0 (1) passes through 01 0 to reach the terminal state x f , and trajectory 2 represents that the initial state x0 (2) passes through 00 to reach the terminal state x f , wherein J≡J max represents that jerk is taken as a control variable, which is constant as the upper limit J max of jerk, J≡J min represents that jerk is taken as a control variable, which is constant as the lower limit J min of jerk, and A≡A max represents that acceleration is constant as the upper limit A max of acceleration.

[0072] In some embodiments, the trajectory planning model can determine the control law of the trajectory according to the input parameters, and then solve and output the second-order time-optimal trajectory according to the system behavior corresponding to the control law.

[0073] Taking the "acceleration" type trajectory as an example, in some embodiments, determining the control law of the trajectory according to the input parameters can include the following steps:

[0074] determining whether the difference between the speed components of the second-order initial state and the second-order terminal state exceeds a first threshold value, the first threshold value being determined according to the acceleration components of the initial state and the terminal state, the upper and lower limits of the acceleration constraint, and the upper and lower limits of the jerk constraint;

[0075] if yes, determining that the control law of the trajectory is 01 0 ; otherwise, determining that the control law of the trajectory is 0 0 .

[0076] S102: If the position advance obtained from the first second-order trajectory is less than the original position difference obtained from the (P s , s , s ) and the (P f , f , f ), then a first third-order optimal trajectory from the (P s , s , s ) to the (P f , f , f ) without position constraint is obtained according to a second second-order trajectory and a third second-order trajectory, wherein the second and third second-order trajectories are second-order trajectories from the (V s , s ) to a first intermediate state and from the first intermediate state to the (V f , f ) respectively, which are solved by using the trajectory planning model, and the first intermediate state is determined depending on the velocity constraint in the third-order constraint.

[0077] After the first second-order trajectory from (V s , s ) to (V f , f ) is solved in the manner described in S101, the position advance is integrated. The difference between the position components of (P s , s , s ) and (P f , f , f ) can be determined as the original position difference. If the position advance is less than the original position difference, a first intermediate state determined depending on the velocity constraint is introduced, and a second second-order trajectory from (V s , s ) to the first intermediate state and a third second-order trajectory from the first intermediate state to (V f , f ) are solved by using the trajectory planning model, and a first third-order optimal trajectory from (P s , s , s ) to (P f , f , f ) without position constraint is obtained according to the second second-order trajectory and the third second-order trajectory.

[0078] In some embodiments, if the position advance is equal to the original position difference, it means that the third-order problem and the second-order sub-problem have the same optimal control solution, and the two time optimal problems are equivalent, then the upgrading of the first second-order trajectory can directly obtain the third third-order optimal trajectory from (P s ,V s , A s ) to (P f , V f , A f ).

[0079] In some embodiments, if the position advance is greater than the original position difference, it is converted into the case of the position advance being less than the original position difference by time reversal to solve the first third-order optimal trajectory. That is, the case of the position advance being less than the original position difference is solved by time reversal.

[0080] As shown in FIG. 1, Figure 3 Figure 3 is a flowchart for solving a third-order optimal trajectory without position constraints provided by the embodiments of the present application. In some embodiments, according to the second second-order trajectory and the third second-order trajectory, the first third-order optimal trajectory without position constraints from (P s , V s , A s ) to (P f , V f , A f ) can include the following steps:

[0081] S301: Set the velocity component of the first intermediate state to the upper limit of the speed constraint and the acceleration component to zero; and solve the second second-order trajectory from (V s , A s ) to the first intermediate state and the third second-order trajectory from the first intermediate state to (V f , A f ) respectively by using the trajectory planning model.

[0082] Let the first intermediate state be (V N1 , A N1 ), and let (V N1 = V max , A N1 = 0). Solve the second second-order trajectory from (V s , A s ) to (V max , 0) by using the trajectory planning model, and obtain the third second-order trajectory from (V max , 0) to (V f , A f ) by reverse calculation.

[0083] S302: Based on the second second-order trajectory and the third second-order trajectory, obtain the first position component and the second position component corresponding to the first intermediate state.

[0084] The position advance is obtained by integrating the second-order trajectory. Based on this position advance and (V) s A s The corresponding position component (i.e., (P)) s V s A s The position component P) s ), to obtain the first position component corresponding to the first intermediate state, denoted as P1. Here, the first position component P1 refers to the position component from (V s A s ) After passing through the second-order trajectory, it reaches (V) max The position when ,0), at this time the third-order state corresponding to the first intermediate state is (P N1 =P1,V N1 =V max A N1 =0).

[0085] The position advance is obtained by integrating the third second-order trajectory. Based on this position advance and (V) f A f The corresponding position component (i.e., (P)) f V f A f The position component P) f ), to obtain the second position component corresponding to the first intermediate state, denoted as P2. Here, the second position component P2 refers to the position component from (V f A f ) Reverse through the third and second order trajectories to reach (V) max The position when ,0), at this time the third-order state corresponding to the first intermediate state is (P N1 =P2,V N1 =V max A N1 =0).

[0086] S303: If the first position component is less than or equal to the second position component, then the first third-order optimal trajectory is obtained based on the second second-order trajectory and the third second-order trajectory.

[0087] Increasing the order of the second-order trajectory yields the result from (P) s V s A s ) to (P1,V max The optimal third-order trajectory is obtained from (P2, V). Increasing the order of the third second-order trajectory yields the optimal trajectory from (P2, V). max ,0) to (P f Vf A f The third-order optimal trajectory of ).

[0088] If the first position component is less than or equal to the second position component, it means that the endpoint of the trajectory is (P1, V). max (0) still needs to "move forward" to reach the third-order terminal state (P) f V f A f Then the solution is found from (P1, V) max ,0) with v≡V max , a≡0 motion, reach (P2,V) max The third-order optimal trajectory of (,0).

[0089] By concatenating the three third-order optimal trajectories above, we obtain an unconstrained trajectory from (P) s V s A s ) to (P f V f A f The first and third order optimal trajectory.

[0090] S304: If the first position component is greater than the second position component, then the first intermediate state is redefined, and the first and third optimal trajectories are obtained based on the second and third second-order trajectories re-solved using the trajectory planning model. The redefined first intermediate state satisfies the following: the third position component corresponding to the redefined first intermediate state obtained from the re-solved second second-order trajectory is equal to the fourth position component corresponding to the redefined first intermediate state obtained from the re-solved third second-order trajectory.

[0091] If the first position component is greater than the second position component, it indicates that the endpoint of the trajectory (P1, V) is at... max ,0) requires "backward" to reach the third-order terminal state (P) f V f A f Then, using the trajectory planning model, a first intermediate state is redefined, and the solution from (V) is re-solved. s A s The second second-order trajectory from the first intermediate state to (V) and the trajectory from the first intermediate state to (V) f A f The third second-order trajectory of ) , the first intermediate state, the second second-order trajectory, and the third second-order trajectory need to satisfy: from (V s A s The position when the second second-order trajectory reaches the first intermediate state is equal to the position from (V) f A f) to the first intermediate state.

[0092] To re-determine the first intermediate state satisfying the above requirements, Groebner basis method can be used to construct a monomial sextic equation for efficient solving, and the only feasible solution is obtained by using feasibility analysis. Of course, the embodiments of the present application do not specifically limit the solving method, for example, Newton method and other methods can also be used to directly solve the multivariate polynomial equation set, and the online calculation idea is used to enumerate the solution within a limited precision, etc.

[0093] Finally, the order is raised and spliced, that is, the first third-order optimal trajectory without position constraint from (P s ,V s ,A s ) to (P f ,V f ,A f ) is obtained.

[0094] As shown in Figure 4 , Figure 4 is a schematic diagram of the first third-order optimal trajectory without position constraint provided by the embodiments of the present application. In the embodiments of the present application, if the position advance amount is less than the original position difference, the first intermediate state determined in dependence on the speed constraint is introduced, and the third-order problem is decomposed into multiple second-order sub-problems, the second-order optimal trajectory is solved in segments, and finally the third-order optimal trajectory without position constraint is obtained by splicing. By gradually introducing the constraint condition and the segmented solving strategy, the third-order optimal trajectory without position constraint is gradually solved, so that the solving efficiency and success rate of the trajectory can be improved, and the trajectory calculation cost can be reduced.

[0095] S103: If the first third-order optimal trajectory does not satisfy the position constraint in the third-order constraint, a second intermediate state satisfying the position constraint is solved by using the trajectory planning model, and a second third-order optimal trajectory satisfying the third-order constraint from (P s ,V s ,A s ) to (P f ,V f ,A f ) is obtained according to the fourth second-order trajectory and the fifth second-order trajectory, wherein the fourth and fifth second-order trajectories are respectively the second-order trajectories from (V s ,A s ) to the second intermediate state and from the second intermediate state to (V f ,A f ) solved by using the trajectory planning model.

[0096] In the manner described in S102, the first third-order optimal trajectory without position constraint from (P s ,Vs ,A s ) to (P f ,V f ,A f ), if the first third-order optimal trajectory exceeds the given position constraint, the tangent point of the position constraint is considered at this time, a second intermediate state satisfying the position constraint is solved, and a fourth second-order trajectory from (V s ,A s ) to the second intermediate state and a fifth second-order trajectory from the second intermediate state to (V f ,A f ) are solved by using the trajectory planning model, and a second third-order optimal trajectory from (P s ,V s ,A s ) to (P f ,V f ,A f ) satisfying the given third-order constraint is obtained according to the fourth second-order trajectory and the fifth second-order trajectory.

[0097] As shown in FIG. 1, Figure 5 Figure 5 is a schematic diagram of a third-order optimal trajectory with a position constraint provided by an embodiment of the present application, and it can be seen that the jerk switches 3 times, which cannot occur in a third-order optimal trajectory without a position constraint.

[0098] In some embodiments, solving the second intermediate state satisfying the position constraint by using the trajectory planning model can include the following steps:

[0099] S1031: solving a sixth second-order trajectory from a target second-order state to a target intermediate state by using the trajectory planning model, wherein the target second-order state is (V s ,A s ) or (V f ,A f ), the speed component of the target intermediate state is zero, and the corresponding position component is the upper limit or the lower limit of the position constraint.

[0100] The second-order optimal trajectory can be divided into two types of trajectories in advance based on Bang-Bang control, and the control law of each type of trajectory can be defined in advance.

[0101] The control law of the first type of trajectory can be expressed as 01 0 , and the control law of the second type of trajectory can be expressed as 0 0 The sixth second-order trajectory is solved by using the trajectory planning model, and the trajectory needs to satisfy: from (V s ,A s ) to pass through 0 0 or 01 0 ​Reaching the target intermediate state, or from (V) f A f After 0 0 Or 01 0 Reaching the target intermediate state. The target intermediate state includes (V) m =0, A m ≧0) or (V m =0, A m ≤0), when the target intermediate state is (V m =0, A m When ≧0), the third-order state corresponding to the intermediate state of the objective should satisfy (P m =P min V m =0, A m ≧0); when the target intermediate state is (V m =0, A m When ≤0), the third-order state corresponding to the intermediate state of the objective should satisfy (P m =P max V m =0, A m ≤0).

[0102] For the above six cases, solve the corresponding nonlinear equations in sequence, including but not limited to constructing a univariate sixth-degree equation using the Grobner method for efficient solution, directly solving multivariate polynomial equations using Newton's method, and enumerating solutions within finite precision using online computing approaches, etc.

[0103] S1032: If a feasible solution exists, then the target intermediate state corresponding to the sixth second-order trajectory is determined as the second intermediate state.

[0104] During the sequential solution process, if a feasible solution is found for any given situation, the target intermediate state corresponding to the solved sixth-order second-order trajectory is determined as the second intermediate state.

[0105] In some embodiments, based on the fourth second-order trajectory and the fifth second-order trajectory, a trajectory satisfying the third-order constraint is obtained from (P) s V s A s ) to (P f V f A f The second and third order optimal trajectories can include the following steps:

[0106] S1033: If the target's second-order state is (V s A s ), then according to the sixth second-order trajectory, we can obtain the trajectory from (P) s V s A sThe fourth optimal trajectory of the third order to the third order state corresponding to the second intermediate state.

[0107] The fourth second-order trajectory required to solve for the second and third-order optimal trajectories refers to the trajectory obtained by using a trajectory planning model from (V) s A s The second-order trajectory from the target second-intermediate state to the second intermediate state. And when the target second-order state is (V... s A s When ), the sixth second-order trajectory obtained by solving is precisely from (V) s A s The second-order trajectory from the second intermediate state. Therefore, the sixth second-order trajectory obtained at this point is the fourth second-order trajectory required to solve for the second and third optimal trajectories.

[0108] Using the obtained sixth-order second-order trajectory as the required fourth-order second-order trajectory, the sixth-order second-order trajectory is increased in order to obtain the trajectory from (P) s V s A s The fourth optimal trajectory of the third order to the third order state corresponding to the second intermediate state.

[0109] S1034: Based on the seventh and eighth second-order trajectories, obtain the unconstrained third-order state from the second intermediate state to (P). f V f A f The fifth and third-order optimal trajectories are obtained by using the trajectory planning model. The seventh and eighth second-order trajectories are the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to (V) respectively. f A f The second-order trajectory of ).

[0110] That is, planning from the third-order state corresponding to the second intermediate state to (P) f V f A f The third-order optimal trajectory is obtained from the unconstrained version of the trajectory.

[0111] You can refer to the planning in S102 from (P) s V s A s ) to (P f V f A f A method for finding the third-order optimal trajectory without position constraints is proposed. The trajectory planning model is used to solve the seventh-order second-order trajectory from the second intermediate state to the first intermediate state, and the trajectory from the first intermediate state to (V...). f A f The eighth second-order trajectory of ) is obtained, and then, based on the seventh and eighth second-order trajectories, the third-order state corresponding to the second intermediate state (P) without position constraints is obtained.f ,V f ,A f ) of the fifth third-order optimal trajectory.

[0112] It should be noted that the fifth second-order trajectory required for solving the second third-order optimal trajectory refers to the second-order trajectory from the second intermediate state to (V f ,A f ) solved by using the trajectory planning model. In the process of planning the third-order optimal trajectory without position constraints from the third-order state corresponding to the second intermediate state to (P f ,V f ,A f ), the fifth second-order trajectory at least includes the seventh second-order trajectory and the eighth second-order trajectory described above.

[0113] For obtaining the fifth third-order optimal trajectory without position constraints from the third-order state corresponding to the second intermediate state to (P f ,V f ,A f ) according to the seventh second-order trajectory and the eighth second-order trajectory, refer to S301-S304, which will not be described here.

[0114] S1035: Splicing the fourth third-order optimal trajectory and the fifth third-order optimal trajectory to obtain the second third-order optimal trajectory.

[0115] In some embodiments, obtaining the second third-order optimal trajectory satisfying the third-order constraints from (P s ,V s ,A s ) to (P f ,V f ,A f ) according to the fourth second-order trajectory and the fifth second-order trajectory can include the following steps:

[0116] S1036: If the target second-order state is (V f ,A f ), obtaining the sixth third-order optimal trajectory from (P f ,V f ,A f ) to the third-order state corresponding to the second intermediate state according to the sixth second-order trajectory.

[0117] The fifth second-order trajectory required for solving the second third-order optimal trajectory refers to the second-order trajectory from the second intermediate state to (V f ,A f ) solved by using the trajectory planning model. And when the target second-order state is (V f ,A f ), the sixth second-order trajectory obtained is from (V f ,A fsecond-order trajectory from the second intermediate state to the first intermediate state and the twelfth second-order trajectory from the first intermediate state to (V f ,A f ) are solved by using the trajectory planning model.

[0118] The sixth second-order trajectory is upgraded to obtain a sixth third-order optimal trajectory corresponding to the third-order state of the second intermediate state from (P f ,V f ,A f ).

[0119] S1037: According to the ninth second-order trajectory and the twelfth second-order trajectory, a seventh third-order optimal trajectory without position constraint from the third-order state corresponding to the second intermediate state to (P s ,V s ,A s ) is obtained, wherein the ninth and twelfth second-order trajectories are second-order trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to (V s ,A s ) respectively solved by using the trajectory planning model.

[0120] That is, a position-constrained version of the third-order optimal trajectory from the third-order state corresponding to the second intermediate state to (P s ,V s ,A s ) is planned.

[0121] Reference can be made to the method of planning the position-constrained version of the third-order optimal trajectory from (P s ,V s ,A s ) to (P f ,V f ,A f ) in S102. The ninth second-order trajectory from the second intermediate state to the first intermediate state and the twelfth second-order trajectory from the first intermediate state to (V f ,A f ) are solved by using the trajectory planning model, and then according to the ninth second-order trajectory and the twelfth second-order trajectory, the seventh third-order optimal trajectory without position constraint from the third-order state corresponding to the second intermediate state to (P s ,V s ,A s ) is obtained.

[0122] It should be noted that the fourth second-order trajectory required for solving the second third-order optimal trajectory refers to the second-order trajectory from (V s ,A s ) to the second intermediate state solved by using the trajectory planning model. In planning the third-order optimal trajectory from the third-order state corresponding to the second intermediate state to (P s ,V s ,A sThe fourth second-order trajectory at least includes the ninth second-order trajectory and the twelfth second-order trajectory.

[0123] As to the ninth second-order trajectory and the twelfth second-order trajectory, a fifth third-order optimal trajectory without position constraint is obtained from the second intermediate state corresponding to the third-order state (P s ,V s ,A s ), see S301-S304, which will not be repeated here.

[0124] S1038: Splicing the sixth third-order optimal trajectory and the seventh third-order optimal trajectory to obtain the second third-order optimal trajectory.

[0125] In the embodiments of the present application, for a given third-order initial state (P s ,V s ,A s ), a third-order terminal state (P f ,V f ,A f ) and a third-order constraint, a second-order optimal trajectory from a second-order initial state (V s ,A s ) to a second-order terminal state (V f ,A f ) is solved; if the position advance is less than the original position difference, a first intermediate state determined according to the velocity constraint is introduced, and a second-order optimal trajectory from (V s ,A s ) to the first intermediate state and from the first intermediate state to (V f ,A f ) is obtained, so as to obtain a third-order optimal trajectory without position constraint; if the third-order optimal trajectory without position constraint does not satisfy the position constraint, a position constraint is introduced, a second intermediate state satisfying the position constraint is solved, and a third-order optimal trajectory satisfying the third-order constraint is obtained according to the second-order optimal trajectory from (V s ,A s ) to the second intermediate state and from the second intermediate state to (V f ,A f ). The third-order constraint problem is decomposed into multiple second-order sub-problems, the third-order optimal trajectory is solved gradually by introducing constraint conditions and splicing, and the third-order optimal trajectory with position constraint under asymmetric constraint is solved, the solving efficiency and success rate of the trajectory are improved, and the trajectory calculation cost is reduced.

[0126] In addition, as shown in Figure 6 , Figure 6 is a numerical control machining physical part drawing provided by the embodiments of the present application. As shown in Figure 6As shown in the left figure, after machining the physical part using the traditional third-order trajectory planning method, obvious vibration marks appear on the machined surface of the part. However, as shown in the right figure... Figure 6 As shown in the right figure, after machining the physical part using the trajectory planning method proposed in this application, the machined surface of the part is almost free of vibration marks. Therefore, from a practical application perspective, the trajectory planning method proposed in this application can also improve machining accuracy and surface quality.

[0127] The above description describes the method provided in this application. The following description describes the apparatus provided in this application:

[0128] Please see Figure 7 , Figure 7 This is a schematic diagram of the structure of a trajectory planning device provided in an embodiment of this application. Figure 6 As shown, the device may include:

[0129] The first trajectory planning unit 710 is used for a given third-order initial state (P s V s A s ), third-order terminal state (P) f V f A f Given second-order initial state (V) and third-order constraints, a trajectory planning model is used to solve the problem from the third-order initial state (V). s A s ) to second-order terminal state (V f A f The first and second order trajectories of the trajectory planning model are the second order optimal trajectories that satisfy the acceleration and jerk constraints in the third order constraints.

[0130] The second trajectory planning unit 720 is configured to, if the position advance amount obtained according to the first second-order trajectory is less than that obtained according to the (P) s V s A s ) and the (P) f V f A f The original position difference obtained is then used to obtain, based on the second and third second-order trajectories, an unconstrained positional trajectories from (P) are obtained. s V s A s ) to the (P) f V f A f The first and third optimal trajectories of (V) are obtained by using the trajectory planning model, where the second and third second-order trajectories are respectively the optimal trajectories obtained by using the trajectory planning model. s A s From the first intermediate state to the (V)f ,A f ) of the first intermediate state determined according to the velocity constraint in the third-order constraint;

[0131] The third trajectory planning unit 730 is configured to, if the first third-order optimal trajectory does not satisfy the position constraint in the third-order constraint, solve a second intermediate state satisfying the position constraint by using the trajectory planning model, and obtain a second third-order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) satisfying the third-order constraint according to a fourth second-order trajectory and a fifth second-order trajectory, where the fourth and fifth second-order trajectories are second-order trajectories from the (V s ,A s ) to the second intermediate state and from the second intermediate state to the (V f ,A f ) respectively solved by using the trajectory planning model.

[0132] Optionally, the second trajectory planning unit 720 is further configured to:

[0133] if the position advance amount is equal to the original position difference, obtain a third third-order optimal trajectory from the (P s ,V s ,A s ) to the (P f ,V f ,A f ) according to the first second-order trajectory.

[0134] Optionally, the second trajectory planning unit 720 is further configured to:

[0135] if the position advance amount is greater than the original position difference, convert it into a case that the position advance amount is less than the original position difference by time reversal to solve the first third-order optimal trajectory.

[0136] Optionally, the second trajectory planning unit 720 is specifically configured to:

[0137] set a velocity component of the first intermediate state to an upper limit of the velocity constraint and an acceleration component to zero, and solve a second second-order trajectory from the (V s ,A s ) to the first intermediate state and a third second-order trajectory from the first intermediate state to the (V f ,A f ) respectively by using the trajectory planning model.

[0138] obtaining a first position component and a second position component corresponding to the first intermediate state according to the second second-order trajectory and the third second-order trajectory, respectively;

[0139] if the first position component is less than or equal to the second position component, obtaining the first third-order optimal trajectory according to the second second-order trajectory and the third second-order trajectory;

[0140] if the first position component is greater than the second position component, re-determining a first intermediate state, and obtaining the first third-order optimal trajectory according to the second second-order trajectory and the third second-order trajectory re-solved by using the trajectory planning model, wherein the re-determined first intermediate state satisfies: a third position component corresponding to the re-determined first intermediate state obtained according to the re-solved second second-order trajectory is equal to a fourth position component corresponding to the re-determined first intermediate state obtained according to the re-solved third second-order trajectory.

[0141] Optionally, the third trajectory planning unit 730 is specifically configured to:

[0142] solving a sixth second-order trajectory from a target second-order state to a target intermediate state by using the trajectory planning model, wherein the target second-order state is the (P s ,V s ) or the (P f ,V f ), a velocity component of the target intermediate state is zero, and a corresponding position component is an upper bound or a lower bound of the position constraint;

[0143] if there is a feasible solution, determining the target intermediate state corresponding to the sixth second-order trajectory as the second intermediate state.

[0144] Optionally, the third trajectory planning unit 730 is specifically configured to:

[0145] if the target second-order state is the (P s ,V s ), obtaining a fourth third-order optimal trajectory from the (P s ,V s ,A s ) to a third-order state corresponding to the second intermediate state according to the sixth second-order trajectory;

[0146] obtaining a seventh second-order trajectory and an eighth second-order trajectory from the third-order state corresponding to the second intermediate state to the (P f ,V f ,A fThe fifth and third-order optimal trajectories of (V), wherein the seventh and eighth second-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectories obtained using the trajectory planning model. f A f The second-order trajectory of ).

[0147] By splicing the fourth and fifth optimal trajectories, the second optimal trajectory is obtained.

[0148] Optionally, the third trajectory planning unit 730 is specifically used for:

[0149] If the target second-order state is (V) f A f ), then according to the sixth second-order trajectory, the trajectory from (P) is obtained. f V f A f The sixth optimal trajectory of the third order to the third order state corresponding to the second intermediate state;

[0150] Based on the 9th and 12th order trajectories, the unconstrained 3rd order state from the second intermediate state to the (P) is obtained. s V s A s The seventh and third-order optimal trajectory, wherein the ninth and twelfth-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectory obtained using the trajectory planning model. s A s The second-order trajectory of ).

[0151] By splicing the sixth and seventh optimal trajectories, the second optimal trajectory is obtained.

[0152] The specific implementation process of the functions and roles of each unit in the above device can be found in the implementation process of the corresponding steps in the above method, and will not be repeated here.

[0153] This application also provides a hardware structure. See [link to relevant documentation]. Figure 8 , Figure 8 This is a schematic diagram of the hardware structure of an electronic device provided in an embodiment of this application. Figure 8 As shown, the hardware structure may include: a processor and a machine-readable storage medium, the machine-readable storage medium storing machine-executable instructions that can be executed by the processor; the processor is used to execute the machine-executable instructions to implement the method disclosed in the above example of this application.

[0154] Based on the same application concept as the above method, the embodiments of the present application also provide a machine readable storage medium, wherein a plurality of computer instructions are stored on the machine readable storage medium, and the computer instructions can realize the method disclosed in the above examples of the present application when executed by a processor.

[0155] For example, the machine readable storage medium can be a RAM (Random Access Memory), a volatile memory, a non-volatile memory, a flash memory, a storage drive (such as a hard disk drive), a solid state drive, any type of storage disk (such as an optical disk, a DVD, etc.), or similar storage medium, or a combination thereof.

[0156] It should be noted that, in this document, the relationship terms such as target and target are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between the entities or operations. Moreover, the terms "include", "contain" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or device. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0157] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the scope of protection of the present application.

Claims

1. A trajectory planning method, characterized in that, The method includes: For a given third-order initial state (P) s V s A s ), third-order terminal state (P) f V f A f Given second-order initial state (V) and third-order constraints, a trajectory planning model is used to solve the problem from the third-order initial state (V). s A s From the second-order terminal state (V) f A f The first and second order trajectories of the trajectory planning model are the second order optimal trajectories that satisfy the acceleration and jerk constraints in the third order constraints. If the position advance amount obtained according to the first second-order trajectory is less than that obtained according to (P) s V s A s ) and the aforementioned (P f V f A f The original position difference obtained from (P) is then used to obtain, based on the second and third second-order trajectories, an unconstrained positional trajectories from the (P) s V s A s ) to the (P) f V f A f The first and third optimal trajectories of (V) are obtained by using the trajectory planning model, where the second and third second-order trajectories are respectively the optimal trajectories obtained by using the trajectory planning model. s A s From the first intermediate state to the (V) f A f The second-order trajectory of the first intermediate state depends on the velocity constraint in the third-order constraint. If the first third-order optimal trajectory does not satisfy the position constraint in the third-order constraints, then the trajectory planning model is used to solve for the second intermediate state that satisfies the position constraint, and based on the fourth and fifth second-order trajectories, the third-order optimal trajectory from (P) is obtained. s V s A s ) to the (P) f V f A f The second and third order optimal trajectories of (V) are respectively obtained by using the trajectory planning model. s A s From the second intermediate state to the second intermediate state and from the second intermediate state to the (V) f A f The second-order trajectory of ). The second-order trajectory and the third-order trajectory are used to obtain a position-unconstrained trajectory from (P). s V s A s ) to the (P) f V f A f The first and third order optimal trajectories include: The velocity component of the first intermediate state is set as the upper bound of the velocity constraint, and the acceleration component is set to zero. The trajectory planning model is then used to solve the problem from the (V) state. s A s The second second-order trajectory from the first intermediate state to the first intermediate state and from the first intermediate state to the (V) f A f The third second-order trajectory of ); Based on the second second-order trajectory and the third second-order trajectory, the first position component and the second position component corresponding to the first intermediate state are obtained respectively. If the first position component is less than or equal to the second position component, then the first third-order optimal trajectory is obtained based on the second second-order trajectory and the third second-order trajectory. If the first position component is greater than the second position component, then the first intermediate state is redefined, and the first third-order optimal trajectory is obtained based on the second second-order trajectory and the third second-order trajectory re-solved using the trajectory planning model. The redefined first intermediate state satisfies the following condition: the third position component corresponding to the redefined first intermediate state obtained from the re-solved second second-order trajectory is equal to the fourth position component corresponding to the redefined first intermediate state obtained from the re-solved third second-order trajectory. The step of using the trajectory planning model to solve for the second intermediate state that satisfies the position constraints includes: Using the trajectory planning model, the sixth second-order trajectory from the target second-order state to the target intermediate state is solved, wherein the target second-order state is the (V s A s ) or the aforementioned (V f A f The velocity component of the target intermediate state is zero, and the corresponding position component is the upper or lower bound of the position constraint. If a feasible solution exists, the target intermediate state corresponding to the sixth second-order trajectory is determined as the second intermediate state; The process involves obtaining, based on the fourth and fifth second-order trajectories, the third-order constraint satisfied by the trajectories derived from (P). s V s A s ) to the (P) f V f A f The second and third order optimal trajectories include: If the target second-order state is (V) s A s ), then according to the sixth second-order trajectory, the trajectory from (P) is obtained. s V s A s The fourth optimal trajectory from the third-order state corresponding to the second intermediate state to the third-order state is obtained based on the seventh and eighth second-order trajectories, without positional constraints, from the third-order state corresponding to the second intermediate state to the (P) f V f A f The fifth and third-order optimal trajectories of (V), wherein the seventh and eighth second-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectories obtained using the trajectory planning model. f A f The second-order trajectory of ) is obtained by splicing the fourth-third-order optimal trajectory and the fifth-third-order optimal trajectory; If the target second-order state is (V) f A f ), then according to the sixth second-order trajectory, the trajectory from (P) is obtained. f V f A f The sixth optimal trajectory from the third-order state corresponding to the second intermediate state to the second intermediate state is obtained; based on the ninth second-order trajectory and the twelfth-order trajectory, the unconstrained trajectory from the third-order state corresponding to the second intermediate state to the (P) is obtained. s V s A s The seventh and third-order optimal trajectory, wherein the ninth and twelfth-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectory obtained using the trajectory planning model. s A s The second-order trajectory of the third-order trajectory is obtained by splicing the sixth and seventh third-order optimal trajectories together.

2. The method according to claim 1, characterized in that, The method further includes: If the position advance is equal to the original position difference, then the position from (P) is obtained according to the first second-order trajectory. s V s A s ) to the (P) f V f A f The third-order optimal trajectory of ).

3. The method according to claim 1, characterized in that, The method further includes: If the position advance is greater than the original position difference, then time flipping is used to transform it into a case where the position advance is less than the original position difference, in order to solve the first third-order optimal trajectory.

4. A trajectory planning device, characterized in that, The device includes: The first trajectory planning unit is used for a given third-order initial state (P). s V s A s ), third-order terminal state (P) f V f A f Given second-order initial state (V) and third-order constraints, a trajectory planning model is used to solve the problem from the third-order initial state (V). s A s From the second-order terminal state (V) f A f The first and second order trajectories of the trajectory planning model are the second order optimal trajectories that satisfy the acceleration and jerk constraints in the third order constraints. The second trajectory planning unit is configured to, if the position advance amount obtained according to the first second-order trajectory is less than that obtained according to the (P) s V s A s ) and the aforementioned (P f V f A f The original position difference obtained from (P) is then used to obtain, based on the second and third second-order trajectories, an unconstrained positional trajectories from the (P) s V s A s ) to the (P) f V f A f The first and third optimal trajectories of (V) are obtained by using the trajectory planning model, where the second and third second-order trajectories are respectively the optimal trajectories obtained by using the trajectory planning model. s A s From the first intermediate state to the (V) f A f The second-order trajectory of the first intermediate state depends on the velocity constraint in the third-order constraint. Among them, based on the second second-order trajectory and the third second-order trajectory, a position-unconstrained trajectory is obtained from (P) s V s A s ) to the (P) f V f A f The first and third order optimal trajectories include: The velocity component of the first intermediate state is set as the upper bound of the velocity constraint, and the acceleration component is set to zero. The trajectory planning model is then used to solve the problem from the (V) state. s A s The second second-order trajectory from the first intermediate state to the first intermediate state and from the first intermediate state to the (V) f A f The third second-order trajectory of ); Based on the second second-order trajectory and the third second-order trajectory, the first position component and the second position component corresponding to the first intermediate state are obtained respectively. If the first position component is less than or equal to the second position component, then the first third-order optimal trajectory is obtained based on the second second-order trajectory and the third second-order trajectory. If the first position component is greater than the second position component, then the first intermediate state is redefined, and the first third-order optimal trajectory is obtained based on the second second-order trajectory and the third second-order trajectory re-solved using the trajectory planning model. The redefined first intermediate state satisfies the following condition: the third position component corresponding to the redefined first intermediate state obtained from the re-solved second second-order trajectory is equal to the fourth position component corresponding to the redefined first intermediate state obtained from the re-solved third second-order trajectory. The third trajectory planning unit is used to solve for the second intermediate state that satisfies the position constraint in the third-order constraints if the first third-order optimal trajectory does not satisfy the position constraint in the third-order constraints. Based on the fourth and fifth second-order trajectories, it obtains the second intermediate state that satisfies the third-order constraints from the (P) s V s A s ) to the (P) f V f A f The second and third order optimal trajectories of (V) are respectively obtained by using the trajectory planning model. s A s From the second intermediate state to the second intermediate state and from the second intermediate state to the (V) f A f The second-order trajectory of ). The step of using the trajectory planning model to solve for the second intermediate state that satisfies the position constraints includes: Using the trajectory planning model, the sixth second-order trajectory from the target second-order state to the target intermediate state is solved, wherein the target second-order state is the (V s A s ) or the aforementioned (V f A f The velocity component of the target intermediate state is zero, and the corresponding position component is the upper or lower bound of the position constraint. If a feasible solution exists, the target intermediate state corresponding to the sixth second-order trajectory is determined as the second intermediate state; The process involves obtaining, based on the fourth and fifth second-order trajectories, the third-order constraint satisfied by the trajectories derived from (P). s V s A s ) to the (P) f V f A f The second and third order optimal trajectories include: If the target second-order state is (V) s A s ), then according to the sixth second-order trajectory, the trajectory from (P) is obtained. s V s A s The fourth optimal trajectory from the third-order state corresponding to the second intermediate state to the third-order state is obtained based on the seventh and eighth second-order trajectories, without positional constraints, from the third-order state corresponding to the second intermediate state to the (P) f V f A f The fifth and third-order optimal trajectories of (V), wherein the seventh and eighth second-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectories obtained using the trajectory planning model. f A f The second-order trajectory of ) is obtained by splicing the fourth-third-order optimal trajectory and the fifth-third-order optimal trajectory; If the target second-order state is (V) f A f ), then according to the sixth second-order trajectory, the trajectory from (P) is obtained. f V f A f The sixth optimal trajectory from the third-order state corresponding to the second intermediate state to the second intermediate state is obtained; based on the ninth second-order trajectory and the twelfth-order trajectory, the unconstrained trajectory from the third-order state corresponding to the second intermediate state to the (P) is obtained. s V s A s The seventh and third-order optimal trajectory, wherein the ninth and twelfth-order trajectories are respectively the trajectories from the second intermediate state to the first intermediate state and from the first intermediate state to the (V) optimal trajectory obtained using the trajectory planning model. s A s The second-order trajectory of the third-order trajectory is obtained by splicing the sixth and seventh third-order optimal trajectories together.

5. An electronic device, characterized in that, It includes a processor and a memory, the memory storing machine-executable instructions that can be executed by the processor, the processor being used to execute the machine-executable instructions to implement the method as described in any one of claims 1-3.

6. A machine-readable storage medium, characterized in that, The machine-readable storage medium stores machine-executable instructions, which, when executed by a processor, implement the method as described in any one of claims 1-3.

Images