A trajectory planning method based on sampling and path parameterization

Abstract

The present invention discloses a trajectory planning method based on sampling and path parameterization, comprising the following steps: S1, using the TOPPRA algorithm to obtain the sampling range K of the square of the path speed of each step i (i=1,...,N); S2, starting from i=1 to N, each step is in K i After sampling in the range, calculate whether the third-order constraints are met to ensure that the final trajectory does not violate each constraint; S3, calculate the time required between two points in each step if they can be connected, and finally calculate the trajectory of the points involved in the shortest time under the entire path; S4, solve the system configuration of the system at each moment according to the sampling time for control. The present invention can effectively handle the impact constraints in the time optimal trajectory planning with third-order constraints, improve the safety of robot use, and effectively protect the hardware such as robot joints from being damaged too quickly due to rapid execution of tasks, reducing the risk of frequent daily maintenance.

Description

Technical Field

[0001] The present invention relates to the technical field of robot trajectory planning, and in particular to a trajectory planning method based on sampling and path parameterization. Background Art

[0002] The autonomy of robots is becoming increasingly important for applications in industry and daily life. They can further improve production efficiency and enable the manufacturing industry to compete more favorably in terms of price and quality. In daily life, they can replace people in handling repetitive and boring work, or improve people's work efficiency through human-machine collaboration.

[0003] The improvement of robot autonomy involves the technological development of many related fields in recent decades, including computer vision, machine learning, control, sensors, etc. Compared with traditional robots, an important feature of modern intelligent robot systems is motion planning.

[0004] In the working environment, given the target state, motion planning can find a series of control inputs that allow the robot to move from the initial state to the target state without violating the constraints. Motion planning consists of path planning (space) and trajectory planning (time).

[0005] Path planning aims to find a series of path points in space from the initial position to the target position. The goal is to make the distance between the path and the obstacles as large as possible and the length of the path as short as possible. It does not involve information such as the time and speed taken to pass through the path. One of the most popular methods of path planning is sampling-based planning methods, such as RRT (Rapidly Exploring Random Trees), PRM (Probabilistic Roadmaps) and a series of improved algorithms. In these planners, collision detection is the most important and time-consuming module. Methods to reduce the time-consuming collision detection have also been widely studied.

[0006] Trajectory planning is to add time series information to path planning, and plan the speed and acceleration of the robot when performing tasks to meet the requirements of smoothness and speed controllability. Trajectory planning can be divided into time optimal trajectory planning, impact optimal trajectory planning, energy optimal trajectory planning, and hybrid optimal trajectory planning (such as time-impact optimal trajectory planning). Time optimal trajectory planning is the most needed in actual tasks because it can improve productivity and work efficiency, and it is also the most widely studied. The research on time optimal trajectory planning that meets second-order constraints (such as acceleration constraints) is very mature, but there is still a lot of room for development in the research of third-order constraints (impact or torque rate) or impact optimization. The higher the order of the constraint, the higher the uncertainty, and the nonlinear characteristics increase the difficulty of finding a solution with short computational time. General trajectory planning methods include interpolation algorithms (such as polynomial interpolation, B-spline interpolation, etc.), S-Curve algorithms, path parameterization algorithms, etc. The TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) algorithm in the path parameterization algorithm is a relatively efficient algorithm among the current time-optimal trajectory planning algorithms that handle second-order constraints.

[0007] As robots are increasingly used in daily life services, in addition to the requirements for robot execution time, safety issues are also crucial. Effectively handling impact constraints can not only improve the safety of robot use, but also effectively protect robot joints and other hardware from being damaged too quickly due to rapid execution of tasks, reducing the risk of frequent daily maintenance.

[0008] The TOPPRA algorithm is the most efficient method in the existing time-optimal trajectory planning. It covers profound theoretical knowledge and uses the reachability analysis method in MPC (Model Predictive Control) to improve computational efficiency. However, the existing TOPPRA algorithm does not consider the optimal time under impact constraints. It only requires a short time, which will cause the acceleration to change too quickly and cause a very large impact. It has safety risks and is easy to damage the system.

[0009] Based on the TOPPRA algorithm, some scholars proposed an improved method that considers impact constraints, by adding a constraint condition and obtaining the upper and lower bounds of the path acceleration from this condition. However, this method has the following disadvantages: (1) It assumes that the minimum value of the third-order derivative of the time function s with respect to time t and maximum value is known and is symmetric about the origin, that is And each step is constant. In fact, from the impact The third derivative of s with respect to time t Even if the impact It is known and symmetrical, and it is impossible to guarantee the third-order derivative of s with respect to time t. is symmetrical and unchanged at each step, which can easily lead to the situation where the optimization process has no solution. (2) This method does not solve the most fundamental and difficult problem, that is, how to Expressed as the third derivative of s with respect to time t (3) In addition to the impact, the third-order constraint is the torque rate, which means that the third-order derivative of the torque rate to s with respect to time t The mapping is also a big problem.

[0010] It should be noted that the information disclosed in the above background technology section is only used for understanding the background of the present application, and therefore may include information that does not constitute prior art known to ordinary technicians in the field. Summary of the invention

[0011] The main purpose of the present invention is to overcome the defects of the above-mentioned background technology and provide a trajectory planning method based on sampling and path parameterization to solve the problem that the optimization process is prone to have no solution in the time optimal trajectory planning with third-order constraints, and the problem of difficulty in mapping the impact or torque rate to the third-order derivative with respect to time.

[0012] To achieve the above object, the present invention adopts the following technical solutions:

[0013] The present invention discloses a trajectory planning method based on sampling and path parameterization, comprising the following steps:

[0014] S1. Use the TOPPRA algorithm to obtain the sampling range K of the square of the path speed for each step i (i=1,...,N);

[0015] S2, starting from i=1 to N, each step is in K i After sampling the range, calculate whether the third-order constraints are met to ensure that the final trajectory does not violate each constraint;

[0016] S3, in each step, the time required between two points is calculated if they can be connected, and finally the trajectory of the points involved in the shortest time on the entire path is calculated;

[0017] S4. Solve the system configuration of the system at each moment according to the sampling time and perform control.

[0018] In some embodiments, in step S1, the specific algorithm flow of the TOPPRA algorithm is:

[0019] S1-1, from N-1 to N-2, ... to 0, reverse solution of the control set ; Where N is the path point;

[0020] S1-2, from 0 to 1..., to N-1, forward solve the maximum path acceleration u of each step;

[0021] S1-3. Get the sampling range K of the square of the path speed x for each step i (i=1,...,N).

[0022] In some embodiments, in step S2, starting from i=1 to N, each step is in K i After sampling the range, the algorithm flow for calculating whether the third-order constraints are met and ensuring that the final trajectory does not violate each constraint is as follows:

[0023] 1) Initialize the tree The root node is x0;

[0024] 2) Place the node of x0 into In the collection;

[0025] 3) In the path speed square K i Samples(K i ) function to sample m points, denoted as Put in In the collection;

[0026] 4) Traversal Each node in the collection

[0027] 5) Traversal Each node in the collection

[0028] 6) If Nodes in a collection and Nodes in a collection Satisfying the impact constraint, Add to In the collection, connected to the tree The sampling nodes in ;

[0029] 7) When the tree The depth reaches N, and whether i is equal to N is used to determine whether there is a solution. The planning method returns at least one feasible result; if multiple results are returned, The time required for function calculation, the shortest time is the best result;

[0030] Where x0 is the square of the path velocity at the initial moment; is the initial node at step 0; for The jth node in the set; for The kth node in the set; To satisfy the impact constraint at step i, connect to the tree The sampling nodes in .

[0031] In step 6), the The jth node in the set is Function in Find the parent node with the lowest cost in the collection.

[0032] Among them, the The algorithm flow of the function includes the following steps:

[0033] 6-1) Input the i-th step The jth sampling point y in the set, and the i-1th step All parent nodes to be connected in the collection;

[0034] 6-2), use The function calculates the time between the sampling point and each parent node and puts it into Cost list In the collection;

[0035] 6-3) Cost list The elements in the collection are sorted from smallest to largest;

[0036] 6-4), use Function calculation y and Cost list Whether the parent node corresponding to the qth element satisfies the impact constraint;

[0037] 6-5) When the result of step 6-4) first appears to satisfy the impact constraint, the parent node is found and connected;

[0038] 6-6) When all the results of step 6-4) do not satisfy the impact constraint, there is no parent node and the sampling point is discarded.

[0039] In some embodiments, in step 6-2), the The function calculates the time from the parent node to the node after the sampling point is successfully connected. The calculation formula is:

[0040]

[0041] in, Indicates the time corresponding to sampling point j at step i; Indicates the time corresponding to the parent node k at the i-1th step.

[0042] In some embodiments, in step 6-4), the The function is used to further determine whether the requirements of the second-order and third-order constraints are met in step i to ensure that the final trajectory does not violate the constraints. The calculation formula is:

[0043] When i=1, the acceleration and impact constraints are

[0044]

[0045]

[0046] When i=2,...,N-1, the acceleration and impact constraints are

[0047]

[0048]

[0049] Among them, q s is the first-order derivative of q with respect to s, q ss is the second-order derivative of q with respect to s, q sss is the third-order derivative of q with respect to s; Δ0=s1-s0, Δ i-1 =s i -s i-1 ; is the path acceleration at step i-2.

[0050] In some embodiments, in step S3, the specific solution method for calculating the time required between two points when the connection is calculated in each step is:

[0051] For the function s(t) at t i and t i+1 Taylor first-order expansion

[0052]

[0053]

[0054] Subtracting the above two equations, we can get

[0055]

[0056] Therefore, the total time for trajectory planning is

[0057]

[0058] In some embodiments, in step S4, the algorithm flow for solving the system configuration of the system at each moment according to the sampling time for control is:

[0059] The s(t) function at each stage can be expressed as

[0060]

[0061] According to the system sampling time ts, the corresponding s(ts) is obtained, and the configuration of the system at each moment can be obtained by interpolation, thereby controlling the movement of the robot.

[0062] The present invention has the following beneficial effects:

[0063] The present invention provides a trajectory planning method based on sampling and path parameterization, which applies the sampling-based method to the optimal planning of the path parameterization, uses the TOPPRA algorithm to obtain the sampling range of the square of the path speed of each step, and then performs random sampling within the sampling range of the square of the path speed of each step to calculate whether it satisfies the third-order constraint, so as to ensure that the final trajectory will not violate various constraints, and then calculates the time required between two points in each step under the condition of being connectable, and finally calculates the trajectory formed by the points involved in the shortest time under the entire path, and solves the system configuration of the system at each moment according to the sampling time for control, so that each previous step can be adjusted so that each step can have a solution, so as to solve the problem that there is no solution in the optimization process in the time optimal trajectory planning with the third-order constraint, and bypasses the difficult problem of mapping the impact or torque rate to the third-order derivative of time, thereby effectively processing the impact constraint, improving the safety of the robot, and effectively protecting the hardware such as the robot joints from being damaged too quickly due to the rapid execution of tasks, thereby reducing the risk of frequent daily maintenance. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a flow chart of a trajectory planning method based on sampling and path parameterization according to an embodiment of the present invention.

[0065] Figure 2 Schematic diagram of a control set in an embodiment of the present invention.

[0066] Figure 3 Graph showing the TOPPRA path speed square in an embodiment of the present invention.

[0067] Figure 4 This is a first algorithm flow chart in an embodiment of the present invention.

[0068] Figure 5 4 is a flow chart of a second algorithm in an embodiment of the present invention.

[0069] Figure 6This is a flow chart of the third algorithm in an embodiment of the present invention.

[0070] Figures 7a to 7c Schematic diagram of the algorithm process of an embodiment of the present invention.

[0071] Figure 8 It is a schematic diagram of the switching point when the impact constraint is processed as linear correlation in the prior art of the present invention. DETAILED DESCRIPTION

[0072] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0073] The embodiment of the present invention provides a trajectory planning method based on sampling and path parameterization, which applies the sampling-based method to the optimal trajectory planning of path parameterization, mainly to solve the time optimal trajectory planning problem with third-order constraints. Using the TOPPRA algorithm, it is easy to obtain the upper and lower bounds of the path speed and path acceleration under the first-order and second-order constraints at each step. Based on this, random sampling is performed within the path speed range of each step to calculate whether it satisfies the third-order constraints, ensuring that the final trajectory does not violate each constraint. This method is inspired by the sampling-based path planning method, and the calculation of whether the third-order constraints are satisfied can be analogous to collision detection in path planning. Using this method, the nonlinear problem of dealing with third-order constraints can be easily avoided.

[0074] like Figure 1 As shown, the trajectory planning method based on sampling and path parameterization of an embodiment of the present invention includes the following steps:

[0075] S1. Use the TOPPRA algorithm to obtain the sampling range K of the square of the path speed for each step i (i=1,...,N).

[0076] Specifically, the specific algorithm flow of the TOPPRA algorithm is:

[0077] S1-1, from N-1 to N-2, ... to 0, reverse solution of the control set ; Where N is the path point;

[0078] S1-2, from 0 to 1..., to N-1, forward solve the maximum path acceleration u of each step;

[0079] S1-3. Get the sampling range K of the square of the path speed x for each step i (i=1,...,N).

[0080] S2, starting from i=1 to N, each step is in K i After sampling the range, calculate whether the third-order constraints are met to ensure that the final trajectory does not violate each constraint.

[0081] Specifically, from i=1 to N, each step is in K i After sampling the range, the algorithm flow for calculating whether the third-order constraints are met and ensuring that the final trajectory does not violate each constraint is as follows:

[0082] 1) Initialize the tree The root node is x0.

[0083] 2) Place the node of x0 into In collection.

[0084] 3) In the path speed square K i Samples(K i ) function to sample m points, denoted as Put in In collection.

[0085] 4) Traversal Each node in the collection

[0086] 5) Traversal Each node in the collection

[0087] 6) If Nodes in a collection and Nodes in a collection Satisfying the impact constraint, Add to In the collection, connected to the tree The sampling nodes in .

[0088] Further, The jth node in the set is Function in Find the parent node with the lowest cost in the collection. The algorithm flow of the function includes the following steps:

[0089] 6-1) Input the i-th step The jth sampling point y in the set, and the i-1th step All parent nodes in the collection to be connected.

[0090] 6-2), use The function calculates the time between the sampling point and each parent node and puts it into Costlist In collection.

[0091] Further, The function calculates the time from the parent node to the node after the sampling point is successfully connected. The calculation formula is:

[0092]

[0093] in, Indicates the time corresponding to sampling point j at step i; Indicates the time corresponding to the parent node k at the i-1th step.

[0094] 6-3) Cost list The elements in a collection are sorted from smallest to largest.

[0095] 6-4), use Function calculation y and Cost list Whether the parent node corresponding to the qth element satisfies the impact constraint.

[0096] Further, The function is used to further determine whether the requirements of the second-order and third-order constraints are met in step i to ensure that the final trajectory does not violate the constraints. The calculation formula is:

[0097] When i=1, the acceleration and impact constraints are

[0098]

[0099]

[0100] When i=2,...,N-1, the acceleration and impact constraints are

[0101]

[0102]

[0103] Among them, q s is the first-order derivative of q with respect to s, q ss is the second-order derivative of q with respect to s, q sss is the third-order derivative of q with respect to s; Δ0=s1-s0, Δ i-1 =s i -s i-1 ; is the path acceleration at step i-2.

[0104] 6-5) When the result of step 6-4) first appears to satisfy the impact constraint, the parent node is found and connected;

[0105] 6-6) When all the results of step 6-4) do not satisfy the impact constraint, there is no parent node and the sampling point is discarded.

[0106] 7) When the tree The depth reaches N, and whether i is equal to N is used to determine whether there is a solution. The planning method returns at least one feasible result; if multiple results are returned, The time required for function calculation, the one with the shortest time is the best result.

[0107] Furthermore, if multiple records are returned, The time required for function calculation, the shortest time is the best result; The function is based on the path from the initial node to the target node. The optimal solution is obtained by minimizing the sum of .

[0108] Where x0 is the square of the path velocity at the initial moment; is the initial node at step 0; for The jth node in the set; for The kth node in the set; To satisfy the impact constraint at step i, connect to the tree The sampling nodes in .

[0109] S3. In each step, the time required between two points is calculated if they can be connected, and finally the trajectory of the points involved in the shortest time on the entire path is calculated.

[0110] Specifically, the solution to calculate the time required between two points when the connection is calculated in each step is:

[0111] For the function s(t) at t i and t i+1 Taylor first-order expansion

[0112]

[0113]

[0114] Subtracting the above two equations, we can get

[0115]

[0116] Therefore, the total time for trajectory planning is

[0117]

[0118] S4. Solve the system configuration of the system at each moment according to the sampling time and perform control.

[0119] Specifically, the algorithm flow for solving the system configuration of the system at each moment for control according to the sampling time is:

[0120] The s(t) function at each stage can be expressed as

[0121]

[0122] According to the system sampling time ts, the corresponding s(ts) is obtained, and the configuration of the system at each moment can be obtained by interpolation, thereby controlling the movement of the robot.

[0123] The specific embodiments of the present invention are further described in detail below with reference to the accompanying drawings.

[0124] 1. Problem Description

[0125] Consider an n-dof (degree of freedom) robot system, whose configuration is composed of an n-dimensional vector Assume a path in the configuration space Use a function Indicates. Where s can be understood as the position of each path point in the complete path. For example: path There are 5 path points. Assuming s∈[0,1], the s corresponding to each path point can be s0=0, s1=0.25, s2=0.5, s3=0.75, s4=1. And s=s(t), that is, s is a function of time, and each s i corresponds to a time, so the solution of the time variable can be indirectly converted into the solution of the s variable. Finally, it can be expressed as q(s(t)) t∈[0,T] This method is the so-called path time parameterization.

[0126] Explanation of symbols:

[0127] represents the derivative with respect to time t, represents the derivative of s.

[0128] represents the lower bound, Indicates the upper bound.

[0129] path is a series of known path points, and the interpolated function q(s) can be derived to obtain q s ,q ss ,q sss ,q s represents the first-order derivative of q with respect to s, q ss represents the second-order derivative of q with respect to s, q sss represents the third derivative of q with respect to s.

[0130] The upper and lower bounds of the constraints are set based on the hardware of the robot system and other conditions, and are also known values. In general, they are constant at each stage in the path.

[0131] Speed ​​Constraint

[0132]

[0133] in, Indicates speed, represents the first derivative of s with respect to time t.

[0134] but

[0135]

[0136] Acceleration Constraint

[0137]

[0138] in, represents acceleration, represents the second derivative of s with respect to time t.

[0139] but

[0140]

[0141] Therefore, the constraints can be easily written as follows:

[0142]

[0143] Among them, a, b, and c represent parameters respectively, and a = q s , b = q ss , The transpose of .

[0144] Impact Constraint (TOPPRA not involved):

[0145]

[0146] in, Indicates impact, Represents the third derivative of s with respect to time t.

[0147] but

[0148]

[0149] 2. TOPPRA Algorithm

[0150] Set Path There are N+1 path points, let u i Indicates that in the interval [s i ,s i+1 ], x i Indicates in s i The square of the path speed x i+1 Indicates in s i+1 The square of the path speed Then the following relationship exists

[0151] x i+1 =x i +2Δ i u i , i=0...N-1

[0152] Where Δ i :=s i+1 -s i .

[0153] analyze:

[0154]

[0155]

[0156] Here, d is the derivative symbol.

[0157] 3. MPC reachability analysis:

[0158] 1. Definition of controllable set:

[0159] The expected final state set is known to be is the basic constraint for each stage x. The controllable set of stage i Refers to the state of satisfying constraints So that there is a state and a series of control inputs u i ,...,u N-1 Drive the system state from x to x N .

[0160] like Figure 2 As shown, is the final expected set, calculated from back to front When counting from the beginning to the end, arrive For example, from step N-1 to step N, there must exist (yes, it exists, not arbitrarily) a control input u that satisfies the constraint in step N-1. N-1 , so that x N Fall in collection Inside.

[0161] The purpose of calculating the control set is to further simplify the existing complex constraints into constraints for a certain variable through iterative rolling.

[0162] 2. Inverse solution of control set:

[0163] The control set solution problem can be described as solving the following two optimization problems to find the upper and lower bounds of the state. Because the state is a one-dimensional quantity, only two optimization problems are needed to solve.

[0164] Solve the upper bound of the state of stage i

[0165]

[0166] sta i u i +b i x i +c i ≤0

[0167]

[0168] Solve the state bound of stage i

[0169] x i :=min x i

[0170] sta i u i +b i x i +c i ≤0

[0171]

[0172] in, Represents x i+1 =x i +2Δ i u i x in i+1 is in the interval within.

[0173] therefore

[0174]

[0175] 3. Forward search for the optimal solution:

[0176] After going back from step N to step 0 to solve the control set, the next stage starts from the known initial state and iterates to solve the maximum path acceleration u that can be achieved at each step while satisfying the constraints. iThe optimization problem is

[0177] max u i

[0178]

[0179]

[0180] in, is a known value,

[0181] The curve of the square of the path velocity is as follows Figure 3 shown.

[0182] 4. Calculation time:

[0183] After obtaining the path speed and acceleration of each path point, we can solve the equation from s i to i+1 The time required is calculated as follows:

[0184] For the function s(t) at t i and t i+1 Taylor first-order expansion

[0185]

[0186]

[0187] Subtracting the above two equations, we can get

[0188]

[0189] Therefore, the total time for trajectory planning is

[0190]

[0191] 5. System time sampling synchronization:

[0192] The s(t) function at each stage can be expressed as

[0193]

[0194] According to the system sampling time ts, the corresponding s(ts) is obtained, and the configuration of the system at each moment can be obtained by interpolation, thereby controlling the movement of the robot.

[0195] 4. Analysis:

[0196] Based on the foregoing, the analysis of this embodiment is as follows:

[0197] Impact Constraint

[0198]

[0199] but

[0200]

[0201] As can be seen from the impact expression above, in addition to the additional variable In addition, It is nonlinear.

[0202] From the previous TOPPRA, we can see that

[0203]

[0204] The path shock is the first-order derivative of the path acceleration u with respect to time, that is is the rate of change of u with respect to time. According to s being a function of time t, we can get

[0205]

[0206] At the initial moment, both velocity and acceleration should be zero. i When the path velocity square is x1, and the path acceleration reaches u0, the path acceleration change rate at this time can be expressed as

[0207]

[0208] And so on

[0209]

[0210] The first algorithm flow chart used in this embodiment is as follows Figure 4 As shown in the figure, i is from 1 to N, let the i-th step, V open There are n elements in the set, V unvisited There are m elements in the collection; the k parameter is used to traverse V open (Step i-1) The nodes in the set, k = 0, 1, 2, ..., n, the j parameter is used to traverse V unvisited (Step i-1) Nodes in the set, j = 0, 1, 2, ..., m. The pseudo code of the first algorithm is as follows:

[0211] Algorithm pseudocode - brief

[0212]

[0213]

[0214] The specific description of the pseudo code of the first algorithm is as follows: the input of the algorithm includes the path speed square x0=0 at the initial moment, the path parameter q s ,q ss ,q sss , and the range K of the square of the path speed calculated by TOPPRA for each step i . It contains all nodes sampled in step i, and it has not yet been determined whether the impact constraint is satisfied. Contains the impact constraints that have been satisfied at step i-1, connected to the tree The sampling node in , which waits for the connection of the sampling node in the i-th step. Each node in is a parent node and needs to find a child node in step i. However, if a child node connection cannot be found in step i, it means that this path fails and no further expansion is required in subsequent iterations. The algorithm returns at least one feasible result when the depth reaches N. If it returns multiple results, the time required should be calculated, and the one with the shortest time is the final best result.

[0215] Specifically, the second algorithm flow chart used is as follows: Figure 5 As shown, the pseudo code of the second algorithm is as follows:

[0216] Algorithm pseudocode - detailed

[0217]

[0218]

[0219] The specific description of the pseudo code of the second algorithm is as follows: Samples(K i ) function is sampled at the i-th step. Secondly, because the purpose is to find the time optimal trajectory that satisfies the impact constraint, it is hoped that the final x of each step is as large as possible, so the sampling strategy can be set to sampling based on probability distribution.

[0220] The function is to find the parent node with the lowest cost for the new node. Its pseudo code is as follows. Because each step requires sampling n points, if all n points are connected to the connectable parent node, it is easy to have explosive growth of nodes. By only finding one parent node to connect, this problem can be effectively controlled. If you judge which parent nodes meet the impact constraint for each sampling point, it will consume a lot of computing time. Therefore, first use The function calculates the time between a sampling point and each parent node, and then sorts them from small to large. Then it traverses from the 0th position of Cost_list until the sampling point and a parent node meet the impact constraint. Because the ultimate goal is to minimize the time of the entire path, each step also hopes to be as short as possible. In this way, those parent nodes that do not need to detect impact constraints can be discarded, which can greatly reduce the calculation loss required for impact constraint judgment.

[0221] Furthermore, the third algorithm flow chart used is as follows Figure 6 As shown, the pseudo code of the third algorithm is as follows:

[0222] Algorithm pseudocode--FindParent

[0223]

[0224] The specific description of the pseudo code of the third algorithm is as follows: The function is used to further determine whether the requirements of the second-order and third-order constraints are met in the i-th step to ensure that the final trajectory does not violate the constraints.

[0225] When i=1, the acceleration and impact constraints are

[0226]

[0227]

[0228] When i=2,...,N-1, the acceleration and impact constraints are

[0229]

[0230]

[0231] The main purpose of this function is to connect parent and child nodes and add new nodes to the tree.

[0232] The function calculates the time from the parent node to the node after the sampling point is successfully connected. The calculation formula is:

[0233]

[0234] in, represents the time corresponding to sampling point j at step i, It indicates the time corresponding to the parent node k at the i-1th step. The time from the i-1th step to the i-th step is increasing. The i-1th step is the previous step, and the point above it is also a sampling point. Through connection, it becomes the parent node. The i-th step is the next step. To perform sampling, it is necessary to determine which parent node of the i-th step to connect with. The connection condition is determined by the shortest time interval.

[0235] The purpose of the function is to, when there are multiple solutions, The optimal solution is obtained by minimizing the sum of .

[0236] The algorithm process diagram is as follows Figures 7a to 7c shown.

[0237] In the embodiment of the present invention, based on the TOPPRA algorithm, if the impact constraint is treated as linearly dependent, the solution is often found in the same way at the switch point (the point where the maximum u suddenly changes to the minimum u, or the point where the minimum u suddenly changes to the maximum u). Figure 8 As shown. Because if the maximum path speed is taken for each step, when the path acceleration suddenly decreases, if the acceleration constraint is to be met, the path acceleration must be reduced by a large amount, and this amount just violates the impact constraint. At this time, it is necessary to adjust each step previously solved. But how to adjust is a big challenge. The method used in the embodiment of the present invention draws on the sampling-based method of path planning to solve. Just as the key of the sampling-based algorithm in path planning is to avoid directly constructing the configuration space (an extremely complex planning problem), the sampling method used in trajectory planning has a similar purpose, that is, adjusting each previous step so that each step can have a solution is a complex problem.

[0238] Although trajectory planning is essentially different from path planning in space, the research on path planning based on sampling methods is very complete, and many improvements can be borrowed from sampling-based trajectory methods. The present invention can open up a new path for trajectory planning to handle high-order constraints.

[0239] The above content is a further detailed description of the present invention in combination with specific / preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, without departing from the concept of the present invention, it can also make several substitutions or modifications to these described embodiments, and these substitutions or modifications should be regarded as belonging to the protection scope of the present invention. In the description of this specification, the description of the reference terms "an embodiment", "some embodiments", "preferred embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in combination with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily target the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In the absence of mutual contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples. Although the embodiments of the present invention and their advantages have been described in detail, it should be understood that various changes, substitutions and modifications can be made herein without departing from the scope of protection of the patent application.

Claims

1. A trajectory planning method based on sampling and path parameterization, characterized in that: The steps include: S1. Use the TOPPRA algorithm to obtain the sampling range K of the square of the path speed for each step i , i=1,...,N; S2, starting from i=1 to N, each step is in K i After sampling the range, calculate whether the third-order constraints are met to ensure that the final trajectory does not violate each constraint; S3, in each step, the time required between two points is calculated if they can be connected, and finally the trajectory of the points involved in the shortest time on the entire path is calculated; S4, solving the system configuration of the system at each moment according to the sampling time to perform control; In step S1, the specific algorithm flow of the TOPPRA algorithm is: S1-1, from N-1 to N-2, ... to 0, reverse solution of the control set Where N is the path point; S1-2, from 0 to 1..., to N-1, forward solve the maximum path acceleration u of each step; S1-3. Get the sampling range K of the square of the path speed x for each step i , i=1,...,N.

2. The trajectory planning method based on sampling and path parameterization according to claim 1, characterized in that: In step S2, starting from i=1 to N, each step is in K i After sampling the range, the algorithm flow for calculating whether the third-order constraints are met and ensuring that the final trajectory does not violate each constraint is as follows: 1) Initialize the tree The root node is x0; 2) Place the node of x0 into In the collection; 3) In the path speed square K i , i=1,...,N, the range is Samples(K i ) function to sample m points, denoted as Put in In the collection; 4) Traversal Each node in the collection 5) Traversal Each node in the collection 6) If Nodes in a collection and Nodes in a collection Satisfying the impact constraint, Add to In the collection, connected to the tree The sampling nodes in ; 7) When the tree The depth reaches N. Whether there is a solution is determined by whether i is equal to N. The planning method returns at least one feasible result. If multiple records are returned, The time required for function calculation, the shortest time is the best result; Where x0 is the square of the path velocity at the initial moment; is the initial node at step 0; for The jth node in the set; for The kth node in the set; To satisfy the impact constraint at step i, connect to the tree The sampling nodes in .

3. The trajectory planning method based on sampling and path parameterization as claimed in claim 2, characterized in that: In step 6), the The jth node in the set is Function in Find the parent node with the lowest cost in the collection.

4. The trajectory planning method based on sampling and path parameterization as claimed in claim 3, characterized in that: Said The algorithm flow of the function includes the following steps: 6-1) Input the i-th step The jth sampling point y in the set, and the i-1th step All parent nodes to be connected in the collection; 6-2), use The function calculates the time between the sampling point and each parent node and puts it into Cost list In the collection; 6-3) Cost list The elements in the collection are sorted from smallest to largest; 6-4), use Function calculation y and Cost list Whether the parent node corresponding to the qth element satisfies the impact constraint; 6-5) When the result of step 6-4) first appears to satisfy the impact constraint, the parent node is found and connected; 6-6) When all the results of step 6-4) do not satisfy the impact constraint, there is no parent node and the sampling point is discarded.

5. The trajectory planning method based on sampling and path parameterization as claimed in claim 4, characterized in that: In step 6-2), the The function calculates the time from the parent node to the node after the sampling point is successfully connected. The calculation formula is: in, Indicates the time corresponding to sampling point j at step i; Indicates the time corresponding to the parent node k at the i-1th step.

6. The trajectory planning method based on sampling and path parameterization as claimed in claim 4, characterized in that: In step 6-4), the The function is used to further determine whether the requirements of the second-order and third-order constraints are met in step i to ensure that the final trajectory does not violate the constraints. The calculation formula is: When i=1, the acceleration and impact constraints are When i=2,...,N-1, the acceleration and impact constraints are Among them, q s is the first-order derivative of q with respect to s, q ss is the second-order derivative of q with respect to s, q sss is the third-order derivative of q with respect to s; Δ0=s1-s0, Δ i-1 =s i -s i-1 ; is the path acceleration at step i-2.

7. The trajectory planning method based on sampling and path parameterization according to claim 1, characterized in that: In step S3, the specific solution method for calculating the time required between two points when the connection is calculated in each step is: For the function s(t) at t i and t i+1 Taylor first-order expansion Subtracting the above two equations, we can get Therefore, the total time for trajectory planning is 8. The trajectory planning method based on sampling and path parameterization according to claim 1, characterized in that: In step S4, the algorithm flow for solving the system configuration of the system at each moment according to the sampling time for control is: The s(t) function at each stage can be expressed as According to the system sampling time ts, the corresponding s(ts) is obtained, and the configuration of the system at each moment can be obtained by interpolation, thereby controlling the movement of the robot.

Images