Rescue robot multi-constraint path planning method based on Voronoi diagram

Abstract

The invention belongs to the field of path planning, and particularly relates to a rescue robot multi-constraint path planning method based on a Voronoi diagram. Comprising the following steps: constructing a Voronoi graph: constructing the Voronoi graph according to a known global two-dimensional grid map; initial path / trajectory searching: constructing an initial feasible path from a starting point to an end point in the Voronoi diagram by using an A * algorithm, and fitting the path by using a cubic spline curve to obtain an initial trajectory; and constructing an optimization function: constructing a proper trajectory optimization function according to task requirements. And trajectory optimization: optimizing the initial trajectory by taking a control point of the initial trajectory as a variable to be optimized and taking a minimum optimization function as an optimization target. The advantages of the Voronoi diagram and the optimization algorithm are fused, and compared with a traditional global path planning method, the method has the advantages of being high in time efficiency, robustness and flexibility, and is a solution suitable for a global path planning task in a large-scale and complex environment.

Description

Technical Field

[0001] The invention belongs to the field of path planning, in particular to a multi-constraint path planning method for a rescue robot based on a Voronoi diagram. Background Art

[0002] Mobile robots are widely used in fire scene rescue, earthquake relief and other fields due to their high efficiency and intelligence, and global path planning algorithms are an important part of mobile robots. Commonly used global path planning algorithms include algorithms based on graph search, algorithms based on artificial potential fields, and algorithms based on random sampling.

[0003] The algorithm based on graph search abstracts the environment into a graph. The nodes of the graph represent the locations that the robot can reach, and the edges of the graph represent the paths that the robot can move. Then, a graph search algorithm is used to find a path from the starting point to the end point in the graph. Representative algorithms include Dijkstra algorithm, A* algorithm and D* algorithm. Dijkstra algorithm uses a priority queue to store the nodes to be expanded, and each time selects the node closest to the starting point for expansion until the end point is found or all nodes are traversed. A* algorithm is based on Dijkstra algorithm, introduces a heuristic function, estimates the distance from the current node to the end point, and preferentially selects nodes with a shorter estimated distance for expansion. D* algorithm can handle dynamic environments and can quickly update the path when the environment changes. This type of algorithm has a sound theoretical basis, strong scalability, and has been widely used in reality. However, it also has the problem that as the scale of the environment increases, the computational complexity of the graph search algorithm will increase significantly, resulting in a long algorithm running time and difficulty in meeting real-time requirements. In addition, the path planned by this type of algorithm is generally not smooth, resulting in discontinuity in the robot's movement.

[0004] The algorithm based on artificial potential field abstracts the environment into a potential field. The target point generates attraction to the robot, and the obstacle generates repulsion to the robot. The robot moves along the gradient direction of the potential field and eventually reaches the target point. Its representative algorithm is the artificial potential field method. This type of algorithm is generally simple, easy to implement, and can quickly find a path. However, it is also easy to fall into a local minimum, which leads to path planning failure.

[0005] The random sampling-based algorithm randomly samples nodes in the environment, connects these nodes into a tree or graph, and then uses a graph search algorithm to find a path from the start point to the end point in the tree or graph. Representative algorithms include the rapidly expanding random tree (RRT) algorithm and the probabilistic roadmap (PRM) algorithm. During the expansion process, the RRT algorithm will give priority to nodes that are closer to the target point for expansion until the tree covers the target point. The PRM algorithm first randomly samples nodes in the environment, then connects these nodes to form a path graph, and finally uses a graph search algorithm to find a path from the start point to the end point in the path graph. This type of algorithm can find paths in complex environments and is suitable for high-dimensional spaces, but it also has problems such as long search time, unstable path quality, and possible jitter. Summary of the invention

[0006] The purpose of the present invention is to design a global path planning algorithm suitable for large-scale and complex environments, which has the characteristics of high time efficiency, strong robustness and strong flexibility.

[0007] The technical solution adopted by the present invention to achieve the above-mentioned purpose is:

[0008] The multi-constraint path planning method for rescue robots based on Voronoi diagram includes the following steps:

[0009] 1) Construct a Voronoi diagram using images collected by the sensor;

[0010] 2) Using the A* algorithm and cubic spline fitting in the Voronoi diagram, the initial trajectory is obtained;

[0011] 3) Taking the control points of the initial trajectory as the variables to be optimized and minimizing the optimization function as the optimization objective, the initial trajectory is optimized.

[0012] The step 1) comprises the following steps:

[0013] 1.1) Build a global map of the environment based on the images obtained by the sensor, and then build a global two-dimensional grid map of the environment;

[0014] 1.2) Generate Voronoi diagram through global two-dimensional grid map.

[0015] The step 1.2) comprises the following steps:

[0016] 1.2.1) Construct a Delaunay triangulation network, number the discrete points and the formed triangles, and record each triangle and the three discrete points that constitute the triangle;

[0017] 1.2.2) Calculate the center of the circumscribed circle of each triangle and construct a triangle linked list;

[0018] 1.2.3) Traverse the triangle list and find the triangle that matches the current triangle Tri P Adjacent triangles Tri with three common sides A 、Tri B and Tri C ;

[0019] 1.2.4) If Tri P If there is an adjacent triangle on one side of the triangle, then the circumcenter of the found triangle is connected to Tri P The circumcenters are connected and stored in the Voronoi edge list; if Tri P If there is no adjacent triangle on one side of , the perpendicular bisector ray of the side is stored in the Voronoi edge list;

[0020] 1.2.5) After the traversal is completed, a Voronoi diagram is drawn based on the edges in the Voronoi edge list, and Voronoi edges whose vertices are contained in obstacles or intersecting obstacles are eliminated. The remaining Voronoi edges are used to construct a set of drivable paths that avoid all obstacles.

[0021] The step 2) comprises the following steps:

[0022] 2.1) Convert the Voronoi edge into a graph structure, and associate the starting and ending positions of the path planning with the Voronoi edge closest to them;

[0023] 2.2) Use the A* algorithm to generate a safe initial path from the starting point to the end point;

[0024] 2.3) Fit the safe initial path using a cubic spline curve to obtain the initial trajectory.

[0025] The step 2.2) comprises the following steps:

[0026] 2.2.1) Create two empty sets, Open List and Closed List, and put the start node into Open List;

[0027] 2.2.2) Use the A* algorithm to select the best node from the Open List and put it into the Closed List, and put its successor node into the Open List;

[0028] 2.2.3) Repeat step 2.2.2) to select the best node until the best node selected is the target node, or the Open List is empty;

[0029] 2.2.4) In the Open List, search the last starting node in reverse order according to the previous nodes contained in the target node, and use the reverse order of the link where the node is located as the safe initial path.

[0030] The step 2.3) comprises the following steps:

[0031] 2.3.1) Trajectory segment division: Assume that the maximum moving speed of the robot is V max , the total length of the initial trajectory is L, and the number of trajectory segments N after division is:

[0032] N=L / (n*V max ), n=1,2,3......

[0033] Where n is the time required for the robot to complete each segment of the trajectory. If N is an integer, the number of segmented trajectories is N; if N is not an integer, the number of segmented trajectories is N+1.

[0034] 2.3.2) Cubic spline fitting: For a given N+1 point P i , i=0,1......N, contains N segments of trajectory, defines the i-th segment of trajectory S i for:

[0035] S i =a i +b i s+c i s 2 +d i s 3 ,s∈[0,1],i=1,2......N

[0036] a i =x i

[0037] b i =D i

[0038] c i =3(x i+1 -x i )-2D i -D i+1

[0039] d i =2(x i -x i+1 )+D i +D i+1

[0040] Among them, a i 、b i 、c i d i are the coefficients of the polynomial of the ith trajectory at each order, s is the ratio of the distance from the current point to the starting point of the ith trajectory to the total length of the ith trajectory, and x iis the starting point of the i-th trajectory, D i is the velocity at the starting point of the i-th trajectory;

[0041] 2.3.3) Construct the initial trajectory: Construct the trajectory S of all segments i Add together to get the initial trajectory.

[0042] The step 3) comprises the following steps:

[0043] 3.1) Construct optimization function;

[0044] 3.2) Obtain the variables to be optimized;

[0045] 3.3) Taking minimization of the optimization function as the optimization objective, the initial trajectory is optimized using the L-BFGS optimizer.

[0046] In step 3.1), the optimization function F opt for:

[0047] F opt =λ1C smooth +λ2C energy +λ3C safety +λ4C dynamics +λ5C others

[0048] Among them, λ1~λ5 are the weights of each cost function, C smooth is the smoothness cost, C energy is the time / energy cost, C safety For security cost, C dynamics is the dynamic cost, C others It is a custom price, specifically:

[0049]

[0050] error=[x i-1 +x i+1 -2*x i y i-1 +y i+1 -2*y i ]

[0051]

[0052]

[0053] Among them, x i-1 ,y i-1 is the x and y values ​​of the previous node, x i+1 ,y i+1 is the x and y value of the next node, x i ,yi is the x and y value of the current node, error is the offset between the current point and the midpoint of the adjacent points before and after, c i d i is the parameter of the cubic spline trajectory, s is the ratio of the distance from the current point to the starting point of the i-th trajectory to the total length of the i-th trajectory, s = (x i ) / (x i+1 -x i ), dis threshold is the safety threshold, dis is the actual distance from the control point to the obstacle, a i and v i is the acceleration and velocity of the robot at a certain point, a max and v max It is the maximum value that the robot's acceleration and speed can reach.

[0054] In step 3.2), the variable to be optimized is the starting point / control point P of each trajectory segment of the cubic spline. i =(x i ,y i ), when the number of trajectory segments is n, the number of control points is n+1, and the optimization variable X is specifically:

[0055] X=[x0,x1......x n-1 ,x n ,y0,y1......y n-1 ,y n ].

[0056] The present invention has the following beneficial effects and advantages:

[0057] 1. The present invention uses the Delaunay triangulation algorithm to generate a Voronoi diagram based on a two-dimensional grid map, and uses the A* algorithm to generate a feasible initial path from the starting point to the end point and passing through the Voronoi edge. Compared with traditional algorithms, the search space is greatly reduced and the search efficiency is improved.

[0058] 2. The present invention integrates the optimization algorithm and comprehensively considers factors such as smoothness, safety, and dynamic feasibility when generating the final optimized trajectory. Although there is a certain probability that the final trajectory obtained is longer than the trajectory obtained by the traditional method, it is still the optimal trajectory overall.

[0059] 3. The present invention is flexible in configuration and has few restrictions on use. As long as a two-dimensional grid map of the relevant environment can be obtained, the present invention can provide a global path planning capability for the robot, including but not limited to ground unmanned vehicles, aerial drones, etc. In addition, users can also customize the cost function C according to their own needs. others , making it suitable for more application scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 The main flow chart of the present invention;

[0061] Figure 2 Flowchart of generating Voronoi edges using Delaunay triangulation algorithm;

[0062] Figure 3 Flowchart for generating initial path / trajectory;

[0063] Figure 4 Flowchart of trajectory optimization. DETAILED DESCRIPTION

[0064] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0065] Figure 1 It is the main flow chart of the technical solution of the present invention, comprising the following steps:

[0066] (1) Obtaining a two-dimensional grid map: Using sensors such as lidar in conjunction with SLAM technology or other methods, a global map of the environment is constructed, and then a global two-dimensional grid map of the environment is constructed.

[0067] (2) Constructing Voronoi diagram: Constructing Voronoi diagram based on the known global two-dimensional grid map.

[0068] (3) Initial path / trajectory search: Use the A* algorithm in the Voronoi diagram to construct an initial feasible path from the start point to the end point, and fit the path using a cubic spline curve to obtain the initial trajectory.

[0069] (4) Trajectory optimization: The control points of the initial trajectory are used as the variables to be optimized, and the minimization of the optimization function is used as the optimization objective to optimize the initial trajectory. If the optimization fails within the optimization number limit, re-optimization is performed until the optimization succeeds; otherwise, the optimization fails, an error message is returned, and the process ends.

[0070] 1. Construct the Voronoi diagram

[0071] Figure 2 The flowchart for constructing the Voronoi diagram using the Delaunay triangulation algorithm is as follows:

[0072] (1) Discrete points automatically construct triangulated networks, that is, construct Delaunay triangulation networks. Number the discrete points and the triangles formed, and record which three discrete points each triangle is composed of.

[0073] (2) Calculate the center of the circumscribed circle of each triangle and record it.

[0074] (3) Traverse the triangle list and find the triangle that matches the current triangle Tri P Adjacent triangles Tri with three common sides A 、Tri B and Tri C .

[0075] (4) If found, compare the circumcenter of the found triangle with Tri P The circumcenters of the edges are connected and stored in the Voronoi edge list. If it is not found, the perpendicular bisector ray of the outermost edge is calculated and stored in the Voronoi edge list.

[0076] (5) After the traversal is completed, all Voronoi edges are found and the Voronoi diagram is drawn based on the edges. After the Voronoi construction is completed, the Voronoi edges whose vertices are contained in obstacles or intersecting obstacles are eliminated, and the remaining Voronoi edges constitute a set of drivable paths that avoid all obstacles.

[0077] 2. Initial path / trajectory search

[0078] Initial path / trajectory search:

[0079] Convert the Voronoi edge into a graph structure, associate the starting and ending positions of the path planning with the closest Voronoi edge, and then use the A* algorithm to generate a safe initial path from the starting point to the end point. Fit the path with a cubic spline curve, specifically:

[0080] (1) Trajectory segment division: Assume that the maximum moving speed of the robot is V max , the total length of the initial trajectory is L, then the number of trajectory segments after division can be obtained as

[0081] N=L / (n*V max ), n=1,2,3......

[0082] If N is an integer, the number of segmented trajectories is N; if N is not an integer, the number of segmented trajectories is N + 1. In addition, n represents the time required for the robot to complete each segment of the trajectory (generally excluding the last segment).

[0083] (2) Cubic spline fitting: For a given N+1 point P i ,i=0,1......N, contains N segments of trajectory, defines the i-th segment of trajectory

[0084] S i =a i +b i s+c i s 2 +di s 3 ,s∈[0,1],i=1,2......N

[0085] According to the first-order and second-order continuity between trajectories and boundary conditions, we can get

[0086] a i =x i

[0087] b i =D i

[0088] c i =3(x i+1 -x i )-2D i -D i+1

[0089] d i =2(x i -x i+1 )+D i +D i+1

[0090] Figure 3 The flowchart of the shortest feasible path connecting the starting point and the end point obtained by searching on the Voronoi edge using the A* algorithm is shown. The detailed steps are as follows:

[0091] (1) Create two empty sets, Open List and Closed List, and put the start node in the Open List.

[0092] (2) Select the best node from the Open List and put it in the Closed List, then put its successor node into the Open List, and then repeat the operation to select the best node until the best node is the target node or the Open List is empty. If the best node is the target node, it means that an optimal trajectory has been found. If the best node is not the target node until the Open List is empty, it means that the search has failed.

[0093] (3) In the Open List, search in reverse order based on the predecessor nodes contained in the target node to finally reach the starting node. The reverse order of this link is the optimal path.

[0094] 3. Trajectory Optimization

[0095] Figure 4 The flowchart of trajectory optimization using the optimizer is shown, with the detailed steps as follows:

[0096] (1) Define the optimization objective function

[0097] F opt =λ1C smooth +λ2C energy +λ3C safety +λ4C dynamics +λ5C others

[0098] Among them, λ1~λ5 are the weights of each cost function, The cost functions are:

[0099] Smoothness cost C smooth : This cost describes the smoothness of the trajectory. The smaller the value, the smoother the trajectory.

[0100]

[0101] error=[x i-1 +x i+1 -2*x i y i-1 +y i+1 -2*y i ]

[0102] Among them, x i-1 ,y i-1 Represents the x and y values ​​of the previous node, x i+1 ,y i+1 Represents the x and y values ​​of the next node, x i ,y i Represents the x and y values ​​of the current node.

[0103] Time / Energy Cost C energy :This cost describes the time / energy cost from the starting point to the end point. The smaller the value, the less time / energy is consumed. When the robot speed has an upper limit, it reflects the length of the trajectory. For N segments of trajectory, it is specifically

[0104]

[0105] Among them, c i d i is the parameter of the cubic spline trajectory, s = xx i .

[0106] Security Cost C safety : This cost describes the distance between the trajectory and the obstacle. The smaller the value, the farther away from the obstacle, the smaller the probability of collision, and the safer the robot. Specifically,

[0107]

[0108] Among them, dis thresholdis the safety threshold, and dis is the actual distance from the control point to the obstacle.

[0109] Dynamic cost C dynamics :This cost describes the degree of consistency between the dynamic parameters (such as speed, acceleration, etc.) required to execute the trajectory and the actual dynamic performance of the robot. That is, the required dynamic parameters must be consistent with the actual situation. The smaller the value, the better the robot can keep up with the trajectory changes when performing trajectory tracking tasks. Specifically,

[0110]

[0111] Among them, a i and v i is the acceleration and velocity of the robot at a certain point, a max and v max It is the maximum value that the robot's acceleration and speed can reach.

[0112] Custom Price C others : This cost is other cost defined by the user.

[0113] (2) Obtaining the optimization variable is the object to be processed by the optimizer, that is, the starting point / control point P of each trajectory segment of the cubic spline i =(x i ,y i ). When the number of trajectory segments is n, the number of control points is n+1. At this time, the optimization variable X is specifically

[0114] X=[x0,x1......x n-1 ,x n ,y0,y1......y n-1 ,y n ]

[0115] (3) Use the optimizer to optimize the optimization variable. If the optimization is within the optimization number limit, re-optimize after the optimization fails until the optimization succeeds. Otherwise, it means that the optimization has failed, and an error message is returned and the process ends.

[0116] Optimizer selection L-BFGS optimizer: L-BFGS (Limited-memory BFGS) is a quasi-Newton method for solving large-scale optimization problems. It is a variant of the BFGS algorithm that aims to reduce memory usage, making it suitable for problems where there is not enough memory to store the entire Hessian matrix or its inverse matrix. The Newton method is a method for finding the minimum of a function in multidimensional space that relies on the first-order derivative (gradient) and second-order derivative (Hessian matrix) of the objective function. The quasi-Newton method is an improvement on the Newton method. It does not directly calculate the Hessian matrix, but approximates the inverse of the Hessian matrix through the information accumulated during the iteration process.

[0117] Unlike the BFGS algorithm, L-BFGS does not store the entire Hessian matrix, but only stores information from the most recent few iteration steps. This allows L-BFGS to be applied to large-scale problems. In addition, L-BFGS uses a set of vectors to approximate the inverse of the Hessian matrix, which are extracted from previous iteration steps. In each iteration, L-BFGS uses these vectors to update the search direction, which is usually done by solving a system of linear equations. Its algorithm steps generally include:

[0118] ① Initialization: Select an initial point and set some parameters, such as memory size.

[0119] ② Iteration: Calculate the gradient of the current point; use the stored vector to approximate the inverse of the Hessian matrix and calculate the search direction; perform a line search to find the next iteration point; update the stored vector.

[0120] ③ Termination: If a preset convergence criterion is reached (for example, the norm of the gradient is less than a certain threshold), the iteration is stopped.

Claims

1. A multi-constraint path planning method for rescue robots based on Voronoi diagram, characterized in that: The following steps are involved: 1) Construct a Voronoi diagram using images collected by the sensor; 2) Using the A* algorithm and cubic spline fitting in the Voronoi diagram, the initial trajectory is obtained; 3) Taking the control points of the initial trajectory as the variables to be optimized and minimizing the optimization function as the optimization objective, the initial trajectory is optimized.

2. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 1, characterized in that: The step 1) comprises the following steps: 1.1) Build a global map of the environment based on the images obtained by the sensor, and then build a global two-dimensional grid map of the environment; 1.2) Generate Voronoi diagram through global two-dimensional grid map.

3. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 2, characterized in that: The step 1.2) comprises the following steps: 1.2.1) Construct a Delaunay triangulation network, number the discrete points and the formed triangles, and record each triangle and the three discrete points that constitute the triangle; 1.2.2) Calculate the center of the circumscribed circle of each triangle and construct a triangle linked list; 1.2.3) Traverse the triangle list and find the triangle that matches the current triangle Tri P Adjacent triangles Tri with three common sides A 、Tri B and Tri C ; 1.2.4) If Tri P If there is an adjacent triangle on one side of the triangle, then the circumcenter of the found triangle is connected to Tri P The circumcenters are connected and stored in the Voronoi edge list; if Tri P If there is no adjacent triangle on one side of , the perpendicular bisector ray of the side is stored in the Voronoi edge list; 1.2.5) After the traversal is completed, a Voronoi diagram is drawn based on the edges in the Voronoi edge list, and Voronoi edges whose vertices are contained in obstacles or intersecting obstacles are eliminated. The remaining Voronoi edges are used to construct a set of drivable paths that avoid all obstacles.

4. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 1, characterized in that: The step 2) comprises the following steps: 2.1) Convert the Voronoi edge into a graph structure, and associate the starting and ending positions of the path planning with the Voronoi edge closest to them; 2.2) Use the A* algorithm to generate a safe initial path from the starting point to the end point; 2.3) Fit the safe initial path using a cubic spline curve to obtain the initial trajectory.

5. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 4, characterized in that: Step 2.2) The following steps are involved: 2.2.1) Create two empty sets, Open List and Closed List, and put the start node into Open List; 2.2.2) Use the A* algorithm to select the best node from the Open List and put it into the Closed List, and put its successor node into the Open List; 2.2.3) Repeat step 2.2.2) to select the optimal node until the selected optimal node is the target node, or OpenList is empty; 2.2.4) In the Open List, search the last starting node in reverse order according to the previous nodes contained in the target node, and use the reverse order of the link where the node is located as the safe initial path.

6. The multi-constraint path planning method for rescue robots based on Voronoi diagram according to claim 4, characterized in that: The step 2.3) comprises the following steps: 2.3.1) Trajectory segment division: Assume that the maximum moving speed of the robot is V max , the total length of the initial trajectory is L, and the number of trajectory segments N after division is: N=L / (n*V max ),n=1,2,3...... Where n is the time required for the robot to complete each segment of the trajectory. If N is an integer, the number of segmented trajectories is N; if N is not an integer, the number of segmented trajectories is N+1. 2.3.2) Cubic spline fitting: For a given N+1 point P i , i=0,1......N, contains N segments of trajectory, defines the i-th segment of trajectory S i for: S i =a i +b i s+c i s 2 +d i s 3 ,s∈[0,1],i=1,2......N a i =x i b i =D i c i =3(x i+1 -x i )-2D i -D i+1 d i =2(x i -x i+1 )+D i +D i+1 Among them, a i 、b i 、c i ,d i are the coefficients of the polynomial of the ith trajectory at each order, s is the ratio of the distance from the current point to the starting point of the ith trajectory to the total length of the ith trajectory, and x i is the starting point of the i-th trajectory, D i is the velocity at the starting point of the i-th trajectory; 2.3.3) Construct the initial trajectory: Construct the trajectory S of all segments i Add together to get the initial trajectory.

7. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 1, characterized in that: The step 3) comprises the following steps: 3.1) Construct optimization function; 3.2) Obtain the variables to be optimized; 3.3) Taking minimization of the optimization function as the optimization objective, the initial trajectory is optimized using the L-BFGS optimizer.

8. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 7, characterized in that: In step 3.1), the optimization function F opt for: F opt =λ1C smooth +λ2C energy +λ3C safety +λ4C dynamics +λ5C others Among them, λ1~λ5 are the weights of each cost function, C smooth is the smoothness cost, C energy is the time / energy cost, C safety For security cost, C dynamics is the dynamic cost, C others It is a custom price, specifically: error=[x i-1 +x i+1 -2*x i and i-1 +y i+1 -2*y i ] Among them, x i-1 ,y i-1 is the x and y values ​​of the previous node, x i+1 ,y i+1 is the x and y value of the next node, x i ,y i is the x and y value of the current node, error is the offset between the current point and the midpoint of the adjacent points before and after, c i ,d i is the parameter of the cubic spline trajectory, s is the ratio of the distance from the current point to the starting point of the i-th trajectory to the total length of the i-th trajectory, s = (x i ) / (x i+1 -x i ), dis threshold is the safety threshold, dis is the actual distance from the control point to the obstacle, a i and v i is the acceleration and velocity of the robot at a certain point, a max and v max The maximum value that the robot's acceleration and speed can reach.

9. The multi-constraint path planning method for a rescue robot based on a Voronoi diagram according to claim 7, characterized in that: In step 3.2), the variable to be optimized is the starting point / control point P of each trajectory segment of the cubic spline. i =(x i ,y i ), when the number of trajectory segments is n, the number of control points is n+1, and the optimization variable X is specifically: X=[x0,x1......x n-1 ,x n ,y0,y1......y n-1 ,y n ]。

Images