Mobile robot fusion path planning method based on Voronoi skeleton
By using Voronoi skeleton diagram and global bootable DWA algorithm in mobile robot path planning, the problems of many inflection points and low security in path planning under raster maps are solved, and a smoother and more efficient path planning is achieved.
Patent Information
- Application Number
- CN202410874638.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-02
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-02
AI Technical Summary
The prior art path planning under grid maps has problems such as many path inflection points, unsmooth operation and low safety. Especially in complex and multi-obstruction environments, optimizing the path cannot ensure that it is far away from obstacles.
Using the fusion path planning method of mobile robots based on Voronoi skeleton, by converting the environment grid map into a skeleton map, using Delaunay triangulation and dual Voronoi graphs to construct the skeleton map, combining the A* algorithm to perform global optimization path planning, and guide it with global The fusion of DWA algorithms performs local path planning to ensure smooth and secure paths.
Reduce the path length and number of inflection points, making the robot's running path smoother and safer, improving navigation efficiency and the use of global paths, and ensuring rapid arrival near the target point.
Smart Images

Figure CN118915716A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the field of path planning, and in particular to a mobile robot fusion path planning method based on a Voronoi skeleton. Background Art
[0002] Path planning is a key step for mobile robots to complete navigation tasks. Mobile robots achieve map representation of the workspace through environmental modeling and complete global path planning based on the map. During the robot's navigation process, it uses its own sensors to perceive environmental obstacles in real time to perform obstacle avoidance and local path planning. At present, the most widely used global path planning algorithms include the A* algorithm and Dijkstra algorithm based on grid maps, and the Voronoi diagram algorithm based on topological maps. Local path planning algorithms include the artificial potential field method (APF) and the dynamic window approach (DWA).
[0003] When using A* or Dijkstra algorithm for path planning under grid map, a feasible path can be quickly planned by searching the grid, but the path has many turning points and is easy to approach obstacles, which is not conducive to the operation of the robot in complex environments. Using Voronoi diagram method under topological map can plan a safe path, but there are many turning points and the path is long, resulting in low efficiency of robot navigation. The above global path is integrated with dynamic window method to avoid collision with obstacles and improve the safety of robot operation.
[0004] The prior art discloses a mobile robot path planning method based on an improved A* fusion DWA algorithm (publication number: CN116817956A), which uses the A* algorithm to plan paths on a grid map, uses a folding optimization algorithm to optimize the optimized path, and inputs the optimized path nodes into the DWA algorithm as intermediate target points, so that the local path follows the optimized path. If in a complex multi-obstacle environment, the optimized path cannot guarantee to stay away from obstacles, and the DWA algorithm only uses path node information, and the fused path cannot guarantee refinement. Summary of the invention
[0005] In order to solve the technical defects existing in the prior art, the present invention proposes a mobile robot fusion path planning method based on Voronoi skeleton, which converts the environment grid map into a skeleton map, generates a global optimization prior path based on the skeleton map, and integrates the global guided DWA algorithm for real-time local planning. The method solves the problems of many path turning points, uneven operation, and low safety in path planning under grid maps in the prior art.
[0006] The present invention is achieved through the following technical solutions:
[0007] A mobile robot fusion path planning method based on Voronoi skeleton includes the following steps:
[0008] Step 1: Construct a grid map: Use the laser SLAM method to construct a grid map of the mobile robot's operating environment;
[0009] Step 2: Voronoi skeleton graph construction: Through the grid-skeleton graph construction method, a triangular mesh graph is obtained by extracting seed points and Delaunay triangulation on the grid map; a Voronoi graph is obtained by dual Voronoi construction on the triangular mesh graph; an adjacency matrix is created for the Voronoi graph, and the Voronoi vertices and edges related to the obstacle grid are processed through adjacency matrix transformation, and redundant vertices are deleted to obtain a skeleton graph of the mobile robot's operating environment;
[0010] Step 3: Global prior path planning: Before the mobile robot runs, the skeleton graph planning-key vertex optimization two-stage planning method is used to perform global path planning based on the skeleton vertices using the A* algorithm to obtain a preliminary global path; then the key vertices in the preliminary global path are selected according to the key vertex extraction method, and connected in sequence to obtain a global optimized prior path;
[0011] Step 4: Local path dynamic target point planning: According to the relative position relationship between the robot and the prior global path, the turning points in the global prior path are used as the dynamic target points of the local path planning, and then the global prior path is merged with the real-time local path;
[0012] Step 5: Local collision-free path planning: By optimizing the speed sampling space of the DWA algorithm, the global guided function term and the end point guided function term are introduced into the evaluation function to construct a global guided DWA algorithm; it is used as a local path planning algorithm, and the dynamic target point planned in step 4 is used as the algorithm input target. The local collision-free path planning is performed cyclically until the mobile robot reaches the final goal.
[0013] Furthermore, the grid-skeleton graph construction method comprises the following steps:
[0014] Step 1.1: Delaunay triangulation: extract the center point coordinates of the obstacle grid and the mapping boundary grid in the grid map, and construct a set of seed points; use the points in the seed point set as the starting points of the edges in the triangular mesh map, and generate a unique Delaunay triangular mesh map through the Delaunay triangulation method, in which all edges in the triangular mesh map do not intersect each other;
[0015] Step 1.2: Dual Voronoi construction: traverse all Delaunay triangles in the Delaunay triangular mesh diagram in turn. If two Delaunay triangles have a common edge, the centers of the circumscribed circles of the two Delaunay triangles are taken as Voronoi points, and the line connecting the two Voronoi points is taken as the Voronoi edge. The Delaunay triangular mesh diagram is converted to a Voronoi diagram through the dual Voronoi construction method.
[0016] Step 1.3: Construct the adjacency matrix of the Voronoi diagram: Traverse all the vertices of the Voronoi diagram in turn. If there is a connection relationship between vertex i and vertex j, then the adjacency matrix A vor (i,j)=dis ij , where dis ij is the Euclidean distance between vertex i and vertex j; if there is no connection between vertex i and vertex j, then the adjacency matrix A vor (i, j) = 0, thus constructing a symmetric adjacency matrix of size n×n, where n is the number of vertices in the Voronoi diagram;
[0017] Step 1.4: Adjacency matrix transformation: By determining the positional relationship between the Voronoi vertices, Voronoi edges, and the obstacle grid, the adjacency matrix generated in step 1.3 is transformed to remove the Voronoi vertices inside the obstacle, remove the Voronoi edges that pass through the obstacle, remove the redundant Voronoi vertices, and obtain the skeleton diagram of the Voronoi diagram.
[0018] Furthermore, the adjacency matrix transformation comprises the following steps:
[0019] Step 2.1: Eliminate the Voronoi vertices inside the obstacle: vi ,y vi ) is transformed according to formula (1) to obtain the transformed coordinates (x g_vi ,y g_vi ), where floor() is a floor rounding function, m_xl and m_yl are the minimum values of the environment map in the x and y directions respectively, and resolution is the grid resolution; if the converted coordinate is "0", the corresponding grid is an obstacle grid, and the row and column corresponding to the vertex are deleted in the adjacency matrix; if the converted coordinate is "1", it is not an obstacle grid and no processing is performed;
[0020]
[0021] Step 2.2: Eliminate Voronoi edges that pass through obstacles: Construct the minimum grid matrix M containing the Voronoi edges, and calculate the minimum distance d from the center of the grid occupied by the obstacle to the Voronoi edge in the minimum grid matrix M. obs ;d obs The calculation method is shown in formula (2), p 1 、p 2 are the coordinates of the two endpoints of the Voronoi edge, p i is the coordinate of the center point of the occupied grid, det() is the function for obtaining the matrix determinant, and norm() is the function for obtaining the vector Euclidean distance;
[0022]
[0023] if If the edge is equal to or greater than the grid edge length, the edge is considered to pass through the obstacle, and the corresponding matrix element value in the adjacency matrix is set to 0; otherwise, no processing is performed;
[0024] Step 2.3: Eliminate redundant Voronoi vertices inside the line: Traverse all vertices in the Voronoi diagram. If the Voronoi vertex V i If the degree is 2, then take the two adjacent Voronoi vertices of the point to determine whether the three points are on the same straight line. If the three points are on the same straight line, then the Voronoi vertex V i If it is a redundant vertex, delete the row and column corresponding to the vertex in the adjacency matrix; if the three points are not on the same line, no processing is done;
[0025] Step 2.4: Eliminate identical redundant Voronoi vertices: Traverse all vertices in the Voronoi diagram. If there are multiple identical Voronoi vertices at the same position, retain the connection relationship of the Voronoi vertices at that position, eliminate the duplicate Voronoi vertices, and delete the row and column corresponding to the vertex in the adjacency matrix.
[0026] Furthermore, the skeleton graph planning-key vertex optimization two-stage planning method comprises the following steps:
[0027] Step 3.1: Search for skeleton vertices at both ends of the path: Obtain the skeleton vertex V with the smallest Euclidean distance from the starting point S and the end point E through the nearest point search s 、V e ;
[0028] Step 3.2: Global planning based on skeleton vertices: Use the A* path planning algorithm, with skeleton vertices as the basic search unit, skeleton edges as the exploration relationship between search units, and the length of the skeleton edge as the actual cost g(n) between two vertices. Heuristic search starts from V s To V eThe path is obtained by adding the starting point and the end point to the path to obtain the preliminary planned path Apath;
[0029] Step 3.3: Extract key vertices to optimize the path: For the preliminary planned path Apath, extract the key vertices in the planned path according to the key vertex extraction method, eliminate redundant vertices, and obtain the global optimized prior path.
[0030] Furthermore, the key vertex extraction method comprises the following steps:
[0031] Step 4.1: Initialization: Initialize the global path key vertex set tpath to be empty, initialize the adjacent vertex sequence j = 1, start from the starting point S, and add the starting point S to the global path key vertex set tpath;
[0032] Step 4.2: Termination condition judgment: Find the next adjacent vertex v of the starting point S from the preliminary planned path Apath j , when v j If it is the end point, add the end point to the global path key vertex set tpath and go to step 4.6; when v j If it is not the end point, go to step 4.3;
[0033] Step 4.3: Determine obstacle avoidance requirements: Based on point S and v j The diagonal vertices form a rectangular area R o , if the rectangular area R o If there are obstacles in the rectangular area R, there is a need for spatial obstacle avoidance, and go to step 4.5; o If there is no obstacle inside, the current vertex is considered to be a redundant vertex and the process goes to step 4.4;
[0034] Step 4.4: Traverse adjacent vertices: j = j + 1, find a new adjacent vertex v in the initial planned path Apath j , return to step 4.3;
[0035] Step 4.5: Key vertex extraction: If the minimum distance d between the obstacle and the path min ≤ distance threshold d th , extract the key vertices, and convert the point v j-1 As the starting point S, return to step 4.2; otherwise, j = j + 1, return to step 4.3;
[0036] Step 4.6: Global optimization path generation: Connect the key vertices in the global path key vertex set tpath in sequence to form a global optimization path.
[0037] Furthermore, the global guided DWA algorithm comprises the following steps:
[0038] Step 5.1: Velocity vector space setting: According to the robot itself and the surrounding environment, the sampling space of the robot velocity vector (v, ω) is obtained by setting the robot parameters and the robot driving force constraints, as shown in formula (3):
[0039]
[0040] Among them, v max and v min represents the speed limit of the robot parameter constraints; ω max and ω min represents the angular velocity limit of the robot parameter constraint; v c ,ω c is the current speed and angular velocity of the robot; a max , α max They are respectively the linear acceleration and angular acceleration limits caused by the limited driving force of the robot;
[0041] Step 5.2: Vector space velocity sampling: In the velocity vector space, set the step size of the linear velocity and the angular velocity, and select the velocity vectors (v, ω) of the linear velocity and the angular velocity in sequence. The sequence principle includes: (1) starting from the minimum value of the linear velocity and the angular velocity, in the order of increasing the step size until the maximum value of the linear velocity and the angular velocity is reached; (2) starting from the maximum value of the linear velocity and the angular velocity, in the order of decreasing the step size until the minimum value of the linear velocity and the angular velocity is reached;
[0042] Step 5.3: Robot trajectory prediction: According to the robot kinematic model, the robot's trajectory Traj in the future time period is predicted for the velocity vectors (v, ω) of the linear velocity and angular velocity;
[0043] Step 5.4: Prediction trajectory evaluation: Calculate the heading score heading(v,ω), obstacle distance score Occdist(v,ω), speed score velocity(v,ω), global guidance item score Vorpath(v,ω) and endpoint convergence score Goaldist(v,ω) of the prediction trajectory Traj, perform numerical normalization on the above scores, and perform weighted summation as the evaluation score of the prediction trajectory, as shown in formula (4):
[0044]
[0045] Where k is the smoothing coefficient; α, β, γ, σ, η are the weighting coefficients of each sub-function;
[0046] Step 5.5: Velocity vector selection: For the predicted trajectories sampled from all velocity vectors (v, ω) in the velocity vector space, select the velocity vector corresponding to the predicted trajectory Tr with the highest score as the result of local path planning.
[0047] Furthermore, the global guide term function term Vorpath(v,ω) is as shown in formula (5):
[0048]
[0049] Among them, p 1 、p 2 are two adjacent nodes in the global path, p is the coordinate of the end of the predicted trajectory, det() is the function for obtaining the matrix determinant, and norm() is the function for obtaining the Euclidean norm of the vector.
[0050] Furthermore, the endpoint convergence term Goaldist(v,ω) is as shown in formula (6):
[0051]
[0052] Where, d min (traj,goal) is the shortest distance between the predicted trajectory and the target, d(r,goal) is the Euclidean distance between the robot and the target, and d sensor is the obstacle detection distance of the sensor.
[0053] Compared with the prior art, the present invention has at least the following beneficial effects or advantages:
[0054] The mobile robot fusion path planning method based on Voronoi skeleton described in the present invention designs a grid map-skeleton graph conversion method, and constructs a skeleton graph of the environment grid map through Delaunay triangulation, dual Voronoi graph construction and undirected graph adjacency matrix transformation processing. The global path planned by the two-stage planning method of skeleton graph planning-key vertex optimization retains the advantages of the Voronoi algorithm such as the characteristic of staying away from obstacles and high planning efficiency, while reducing the path length and the number of inflection points, making the robot's running path smoother and safer.
[0055] At the same time, at the local path planning level, in order to address the problems of low operating efficiency, easy to fall into local optimality, and lack of use of global paths in the traditional DWA algorithm, the speed sampling space is optimized, and the global guidance evaluation function items and end point convergence evaluation function items are added. While retaining the characteristics of obstacle avoidance, the robot's operating efficiency and the use of the global path are improved, while enabling the mobile robot to quickly reach the target point near the target point. BRIEF DESCRIPTION OF THE DRAWINGS
[0056] The present invention will be further described in detail below with reference to the accompanying drawings:
[0057] Figure 1The flowchart of the fusion path planning method for mobile robots based on Voronoi skeleton;
[0058] Figure 2 Construct Voronoi diagram for grid environment;
[0059] Figure 3 is an undirected graph and its adjacency matrix;
[0060] Figure 4 Extract schematic diagram for Voronoi skeleton;
[0061] Figure 5 This is a schematic diagram of global boot items.
[0062] Figure 6 This is a schematic diagram of the terminal convergence term. DETAILED DESCRIPTION
[0063] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0064] The mobile robot fusion path planning method based on Voronoi skeleton described in the present invention is as follows: Figure 1 The method specifically comprises the following steps:
[0065] Step 1: In the Robot Operating System (ROS), use the 2D lidar to create a grid map of the mobile robot's working environment using the cartographer method.
[0066] At this time, the map obtained is a grayscale raster map, and the boundary of the working environment in the actual environment is not smooth. Therefore, before constructing the skeleton map, the grayscale raster map needs to be binarized and image morphologically opened to obtain a binary raster map with smooth edges and clear distinction between obstacles and idle areas.
[0067] Step 2: Create a skeleton graph of the mobile robot's working environment through the grid-skeleton graph construction method.
[0068] Based on the binary grid map, the grid environment Voronoi diagram is obtained through Delaunay triangulation and dual Voronoi construction. An adjacency matrix is created for the Voronoi diagram, and the adjacency matrix is transformed and mapped to an undirected graph to obtain the skeleton diagram of the mobile robot's operating environment, which specifically includes the following steps:
[0069] Step 2.1: Extract the edge grid of the working environment in the grid map, and extract the obstacle grid inside the working environment, that is, the grid with image pixel value 0, and use it as a set of seed points. Use the Delaunay triangulation method to generate a unique Delaunay triangular mesh map for this set of points, where the starting point of any edge in the triangular mesh map is a point in the seed point set, and all edges do not intersect each other.
[0070] Step 2.2: Traverse all Delaunay triangles in the Delaunay triangular mesh in turn. If two Delaunay triangles have a common edge, the centers of the circumscribed circles of the two Delaunay triangles are taken as Voronoi points, and the line connecting the two Voronoi points is taken as Voronoi edge. The Delaunay triangular mesh is converted to a Voronoi diagram by the dual Voronoi construction method, as shown in Figure 2 shown.
[0071] Due to the continuity and closed loop of the obstacle grid, the Voronoi diagram generated by the above steps has problems such as Voronoi points inside the obstacle grid and Voronoi edges passing through obstacles. This diagram cannot be directly used for mobile robot path planning. In essence, the Voronoi diagram is an undirected graph with weighted edges. The adjacency matrix is often used to represent the graph relationship in the computer. Therefore, the undirected graph adjacency matrix transformation method is used to transform the Voronoi diagram into an environment skeleton graph.
[0072] Step 2.3: After obtaining the Voronoi diagram of the grid map, traverse all the vertices of the Voronoi diagram. If there is a connection relationship between vertex i and vertex j, the adjacency matrix A vor (i,j)=dis ij , where dis ij is the Euclidean distance between vertex i and vertex j; if there is no connection between vertex i and vertex j, then the adjacency matrix A vor (i, j) = 0, thus constructing a symmetric adjacency matrix of size n×n, where n is the number of vertices in the Voronoi diagram, such as Figure 3 shown.
[0073] Step 2.4: Change the Voronoi vertex coordinates (x vi ,y vi ) is transformed according to formula (1) to obtain the transformed coordinates (x g_vi ,y g_vi), where floor() is a floor function, m_xl and m_yl are the minimum values of the environment map in the x and y directions respectively, and resolution is the grid resolution; if the grid corresponding to the converted coordinates is an obstacle grid, the row and column corresponding to the vertex are deleted in the adjacency matrix; if it is not an obstacle grid, no processing is performed.
[0074]
[0075] Step 2.5: Construct the minimum grid matrix M containing the Voronoi edge, and calculate the minimum distance d between the center of the grid occupied by the obstacle and the Voronoi edge in M obs , d obs The calculation method is shown in formula (2), p 1 、p 2 are the coordinates of the two endpoints of the Voronoi edge, p i is the coordinate of the center point of the occupied grid, det() is the function for obtaining the matrix determinant, and norm() is the function for obtaining the vector Euclidean distance;
[0076]
[0077] if If the edge is equal to or greater than times the grid edge length, the edge is considered to pass through the obstacle, and the corresponding matrix element value in the adjacency matrix is set to 0; otherwise, no processing is performed.
[0078] Step 2.6: Traverse all vertices in the Voronoi diagram. If the Voronoi vertex V i If the degree is 2, then take the two adjacent Voronoi vertices of the point to determine whether the three points are on the same straight line. If the three points are on the same straight line, then the Voronoi vertex V i If it is a redundant vertex, delete the row and column corresponding to the vertex in the adjacency matrix; if the three points are not on the same line, no processing is done.
[0079] Step 2.7: Traverse all vertices in the Voronoi diagram. If there are multiple identical Voronoi vertices at the same position, keep the connection relationship of the Voronoi vertices at that position, remove the duplicate Voronoi vertices, and delete the row and column corresponding to the vertex in the adjacency matrix.
[0080] By deleting and modifying the adjacency matrix corresponding to the Voronoi diagram, we can finally get the environment skeleton diagram, such as Figure 4 shown.
[0081] Step 3: Before the mobile robot runs, a global optimization prior path is generated according to the two-stage planning method of skeleton graph planning-key vertex optimization.
[0082] The skeleton graph planning-key vertex optimization two-stage planning method comprises the following steps:
[0083] Step 3.1: Search the nearest point on the skeleton graph to obtain the skeleton vertex V with the smallest Euclidean distance from the starting point S and the end point E. s 、V e .
[0084] Step 3.2: Use the A* path planning algorithm, with skeleton vertices as the basic search units, skeleton edges as the exploration relationship between search units, and the length of the skeleton edge as the actual cost g(n) between two vertices. Heuristic search starts from V s To V e The path is obtained by adding the starting point and the end point to the path to obtain the preliminary planned path Apath.
[0085] At this time, the preliminary planned path Apath is attached to the skeleton graph, and has many turning points and is not smooth, so it needs to be further optimized.
[0086] Step 3.3: Initialize the global path key vertex set tpath to be empty, initialize the adjacent vertex sequence j = 1, start from the starting point S, and add the starting point S to the global path key vertex set tpath.
[0087] Step 3.4: Find the next adjacent vertex v of the starting point S from the preliminary planned path Apath j , when v j If it is the end point, add the end point to the global path key vertex set tpath and go to step 3.8; when v j If it is not the end point, proceed to step 3.5.
[0088] Step 3.5: Take points S and v j The diagonal vertices form a rectangular area R o , if the rectangular area R o If there are obstacles in the rectangular area R, there is a need for spatial obstacle avoidance, and go to step 4.5; o If there is no obstacle inside, the current vertex is considered to be a redundant vertex and the process goes to step 3.6;
[0089] Step 3.6: Traverse adjacent vertices: j = j + 1, find the new adjacent vertex v in the initial planning path Apath j , return to step 3.5;
[0090] Step 3.7: If the minimum distance d between the obstacle and the path min ≤ distance threshold d th , extract the key vertices, and convert v j-1 As the starting point S, return to step 3.4; otherwise, j = j + 1, return to step 3.5;
[0091] Step 3.8: Connect the key vertices in the global path key vertex set tpath in sequence to form a global optimization path.
[0092] Step 4: When the robot starts running, according to the relative position relationship between the robot and the prior global path, the key vertices in the global path key vertex set tpath are used as dynamic target points for local path planning in turn.
[0093] Step 5: When the mobile robot is running, it uses the airborne laser radar to detect the surrounding environment in real time, takes the dynamic target point planned in step 4 as the algorithm input target, and uses the global guided DWA algorithm for real-time planning until the mobile robot reaches the final target.
[0094] The global guided DWA algorithm comprises the following steps:
[0095] Step 5.1: In the local path planning process based on the DWA algorithm, according to the robot itself and the surrounding environment, the sampling space of the robot velocity vector (v, ω) is obtained by setting the robot parameters and the robot driving force constraints, as shown in formula (3):
[0096]
[0097] Among them, v max and v min represents the speed limit of the robot parameter constraints; ω max and ω min represents the angular velocity limit of the robot parameter constraint; v c ,ω c is the current speed and angular velocity of the robot; a max , α max They are respectively the linear acceleration and angular acceleration limits caused by the limited driving force of the robot;
[0098] Step 5.2: In the velocity vector space, set the step size of the linear velocity and the angular velocity, and select the velocity vectors (v, ω) of the linear velocity and the angular velocity in sequence according to the sequential principle. The sequential principle is to start from the minimum value of the linear velocity and the angular velocity, and increase the step size in sequence until the maximum value of the linear velocity and the angular velocity is reached.
[0099] Step 5.3: According to the robot kinematic model, predict the robot's trajectory Traj in the future time period for the velocity vectors (v, ω) of the linear velocity and angular velocity.
[0100] Step 5.4: Calculate the heading score heading(v,ω), obstacle distance score Occdist(v,ω), speed score velocity(v,ω), global guidance item score Vorpath(v,ω) and endpoint convergence score Goaldist(v,ω) of the predicted trajectory Traj, perform numerical normalization on the scores, and perform weighted summation as the evaluation score of the predicted trajectory, as shown in formula (4):
[0101]
[0102] Where k is the smoothing coefficient; α, β, γ, σ, η are the weighting coefficients of each sub-function.
[0103] Step 5.5: Select the velocity vector corresponding to the predicted trajectory Tr with the highest score as the result of local path planning.
[0104] The global guide term function term Vorpath(v,ω) is as follows: Figure 5 As shown in the figure, by calculating the shortest distance between the end of the predicted trajectory and the planned global path, the global path is introduced as a direction guide. When there are no new obstacles, the fit between the obstacle avoidance local path and the global optimization path is enhanced. The calculation method of this function term is shown in formula (5):
[0105]
[0106] Among them, p 1 、p 2 are two adjacent nodes in the global path, p is the coordinate of the end of the predicted trajectory, det() is the function for obtaining the matrix determinant, and norm() is the function for obtaining the Euclidean norm of the vector.
[0107] The endpoint convergence term Goaldist(v,ω) is as follows: Figure 6 As shown in the figure, by calculating the shortest distance between the predicted trajectory and the target, the trajectory close to the target point has a higher weight, thereby avoiding the robot from wandering near the target point and prompting it to quickly approach the target. The calculation method of this function term is shown in formula (6):
[0108]
[0109] Where, d min (traj,goal) is the shortest distance between the predicted trajectory and the target, d(r,goal) is the Euclidean distance between the robot and the target, and d sensor is the obstacle detection distance of the sensor.
[0110] The above description is only a preferred embodiment of the present invention. It should be pointed out that a person skilled in the art can make several improvements without departing from the principle of the present invention, and these improvements should also be regarded as within the protection scope of the present invention.
Claims
1. A mobile robot fusion path planning method based on Voronoi skeleton, characterized in that Including offline and online stages. The offline phase includes the following steps: Step 1: Construct a grid map: Use the laser SLAM method to construct a grid map of the mobile robot's operating environment; Step 2: Voronoi skeleton graph construction: Through the grid-skeleton graph construction method, a triangular mesh graph is obtained by extracting seed points and Delaunay triangulation on the grid map; a Voronoi graph is obtained by dual Voronoi construction on the triangular mesh graph; an adjacency matrix is created for the Voronoi graph, and the Voronoi vertices and edges related to the obstacle grid are processed through adjacency matrix transformation, and redundant vertices are deleted to obtain a skeleton graph of the mobile robot's operating environment; Step 3: Global prior path planning: Before the mobile robot runs, the skeleton graph planning-key vertex optimization two-stage planning method is used to perform global path planning based on the skeleton vertices using the A* algorithm to obtain a preliminary global path; then the key vertices in the preliminary global path are selected according to the key vertex extraction method, and the key vertices are connected in sequence to obtain a global optimized prior path; The online phase includes the following steps: Step 4: Local path dynamic target point planning: According to the relative position relationship between the robot and the global prior path, the turning points in the global prior path are used as the dynamic target points of the local path planning, and then the global prior path is merged with the real-time local path; Step 5: Local collision-free path planning: By optimizing the speed sampling space of the DWA algorithm, the global guided function term and the end point guided function term are introduced into the evaluation function to construct a global guided DWA algorithm; it is used as a local path planning algorithm, and the dynamic target point planned in step 4 is used as the algorithm input target. The local collision-free path planning is performed cyclically until the mobile robot reaches the final goal.
2. According to the Voronoi skeleton-based mobile robot fusion path planning method of claim 1, it is characterized in that: The grid-skeleton graph construction method in step 2 comprises the following steps: Step 1.1: Delaunay triangulation: extract the center point coordinates of the obstacle grid and the mapping boundary grid in the grid map, and construct a set of seed points; use the points in the seed point set as the starting points of the edges in the triangular mesh map, and generate a unique Delaunay triangular mesh map through the Delaunay triangulation method, in which all edges in the triangular mesh map do not intersect each other; Step 1.2: Dual Voronoi construction: traverse all Delaunay triangles in the Delaunay triangular mesh diagram in turn. If two Delaunay triangles have a common edge, the centers of the circumscribed circles of the two Delaunay triangles are taken as Voronoi points, and the line connecting the two Voronoi points is taken as the Voronoi edge. The Delaunay triangular mesh diagram is converted to a Voronoi diagram through the dual Voronoi construction method. Step 1.3: Construct the adjacency matrix of the Voronoi diagram: Traverse all the vertices of the Voronoi diagram in turn. If there is a connection relationship between vertex i and vertex j, then the adjacency matrix A vor (i,j)=dis ij , where dis ij is the Euclidean distance between vertex i and vertex j; if there is no connection between vertex i and vertex j, then the adjacency matrix A vor (i, j) = 0, thus constructing a symmetric adjacency matrix of size n×n, where n is the number of vertices in the Voronoi diagram; Step 1.4: Adjacency matrix transformation: By determining the positional relationship between the Voronoi vertices, Voronoi edges, and the obstacle grid, the adjacency matrix generated in step 1.3 is transformed to remove the Voronoi vertices inside the obstacle, remove the Voronoi edges that pass through the obstacle, remove the redundant Voronoi vertices, and obtain the skeleton diagram of the Voronoi diagram.
3. The mobile robot fusion path planning method based on Voronoi skeleton according to claim 2 is characterized in that: The adjacency matrix transformation comprises the following steps: Step 2.1: Eliminate the Voronoi vertices inside the obstacle: vi ,y vi ) is transformed according to formula (1) to obtain the transformed coordinates (x g_vi ,y g_vi ), where floor() is a floor rounding function, m_xl and m_yl are the minimum values of the environment map in the x and y directions respectively, and resolution is the grid resolution; if the converted coordinate is "0", the corresponding grid is an obstacle grid, and the row and column corresponding to the vertex are deleted in the adjacency matrix; if the converted coordinate is "1", it is not an obstacle grid and no processing is performed; Step 2.2: Eliminate Voronoi edges that pass through obstacles: Construct the minimum grid matrix M containing the Voronoi edges, and calculate the minimum distance d from the center of the grid occupied by the obstacle to the Voronoi edge in the minimum grid matrix M. obs ;d obs The calculation method is shown in formula (2), where p1 and p2 are the coordinates of the two endpoints of the Voronoi edge, and p i is the coordinate of the center point of the occupied grid, det() is the function for obtaining the matrix determinant, and norm() is the function for obtaining the vector Euclidean distance; if If the edge is equal to or greater than the grid edge length, the edge is considered to pass through the obstacle, and the corresponding matrix element value in the adjacency matrix is set to 0; otherwise, no processing is performed; Step 2.3: Eliminate redundant Voronoi vertices inside the line: Traverse all vertices in the Voronoi diagram. If the Voronoi vertex V i If the degree is 2, then take the two adjacent Voronoi vertices of the point to determine whether the three points are on the same straight line. If the three points are on the same straight line, then the Voronoi vertex V i If it is a redundant vertex, delete the row and column corresponding to the vertex in the adjacency matrix; if the three points are not on the same line, no processing is done; Step 2.4: Eliminate identical redundant Voronoi vertices: Traverse all vertices in the Voronoi diagram. If there are multiple identical Voronoi vertices at the same position, retain the connection relationship of the Voronoi vertices at that position, eliminate the duplicate Voronoi vertices, and delete the row and column corresponding to the vertex in the adjacency matrix.
4. The mobile robot fusion path planning method based on Voronoi skeleton according to claim 1 is characterized in that: The skeleton graph planning-key vertex optimization two-stage planning method comprises the following steps: Step 3.1: Search for skeleton vertices at both ends of the path: Obtain the skeleton vertex V with the smallest Euclidean distance from the starting point S and the end point E through the nearest point search s 、V e ; Step 3.2: Global planning based on skeleton vertices: Use the A* path planning algorithm, with skeleton vertices as the basic search unit, skeleton edges as the exploration relationship between search units, and the length of the skeleton edge as the actual cost g(n) between two vertices. Heuristic search starts from V s To V e The path is obtained by adding the starting point and the end point to the path to obtain the preliminary planned path Apath; Step 3.3: Extract key vertices to optimize the path: For the preliminary planned path Apath, extract the key vertices in the planned path according to the key vertex extraction method, eliminate redundant vertices, and obtain the global optimized prior path.
5. The mobile robot fusion path planning method based on Voronoi skeleton according to claim 4 is characterized in that: The key vertex extraction method comprises the following steps: Step 4.1: Initialization: Initialize the global path key vertex set tpath to be empty, initialize the adjacent vertex sequence j = 1, start from the starting point S, and add the starting point S to the global path key vertex set tpath; Step 4.2: Termination condition judgment: Find the next adjacent vertex v of the starting point S from the preliminary planned path Apath j , when v j If it is the end point, add the end point to the global path key vertex set tpath and go to step 4.6; when v j If it is not the end point, go to step 4.3; Step 4.3: Determine obstacle avoidance requirements: Based on point S and v j The diagonal vertices form a rectangular area R o , if the rectangular area R o If there are obstacles in the rectangular area R, there is a need for spatial obstacle avoidance, and go to step 4.5; o If there is no obstacle inside, the current vertex is considered to be a redundant vertex and the process goes to step 4.4; Step 4.4: Traverse adjacent vertices: j = j + 1, find a new adjacent vertex v in the initial planned path Apath j , return to step 4.3; Step 4.5: Key vertex extraction: If the obstacle is the minimum distance d from the path min ≤ distance threshold d th , extract the key vertices, and convert the point v j-1 As the starting point S, return to step 4.2; otherwise, j = j + 1, return to step 4.3; Step 4.6: Global optimization path generation: Connect the key vertices in the global path key vertex set tpath in sequence to form a global optimization path.
6. The mobile robot fusion path planning method based on Voronoi skeleton according to claim 1 is characterized in that The global guided DWA algorithm comprises the following steps: Step 5.1: Velocity vector space setting: According to the robot itself and the surrounding environment, the sampling space of the robot velocity vector (v, ω) is obtained by setting the robot parameters and the robot driving force constraints, as shown in formula (3): Among them, v max and v min represents the speed limit of the robot parameter constraints; ω max and ω min represents the angular velocity limit of the robot parameter constraint; v c ,ω c is the current speed and angular velocity of the robot; a max , α max They are respectively the linear acceleration and angular acceleration limits caused by the limited driving force of the robot; Step 5.2: Vector space velocity sampling: In the velocity vector space, set the step size of the linear velocity and the angular velocity, and select the velocity vectors (v, ω) of the linear velocity and the angular velocity in sequence. The sequence principle includes: (1) starting from the minimum value of the linear velocity and the angular velocity, in the order of increasing the step size until the maximum value of the linear velocity and the angular velocity is reached; (2) starting from the maximum value of the linear velocity and the angular velocity, in the order of decreasing the step size until the minimum value of the linear velocity and the angular velocity is reached; Step 5.3: Robot trajectory prediction: According to the robot kinematic model, the robot's trajectory Traj in the future time period is predicted for the velocity vectors (v, ω) of the linear velocity and angular velocity; Step 5.4: Prediction trajectory evaluation: Calculate the heading score heading(v,ω), obstacle distance score Occdist(v,ω), speed score velocity(v,ω), global guidance item score Vorpath(v,ω) and endpoint convergence score Goaldist(v,ω) of the prediction trajectory Traj, perform numerical normalization on the above scores, and perform weighted summation as the evaluation score of the prediction trajectory, as shown in formula (4): Where k is the smoothing coefficient; α, β, γ, σ, η are the weighting coefficients of each sub-function; Step 5.5: Velocity vector selection: For the predicted trajectories sampled from all velocity vectors (v, ω) in the velocity vector space, select the velocity vector corresponding to the predicted trajectory Tr with the highest score as the result of local path planning.
7. The mobile robot fusion path planning method based on Voronoi skeleton according to claim 6 is characterized in that The global guide term score Vorpath(v,ω) is shown in formula (5): Among them, p1 and p2 are two adjacent key vertices in the global path, p is the coordinate of the end of the predicted trajectory, det() is a function for obtaining the matrix determinant, and norm() is a function for obtaining the vector Euclidean distance.
8. The mobile robot fusion path planning method based on Voronoi skeleton according to claim 6 is characterized in that The endpoint convergence score Goaldist(v,ω) is shown in formula (6): Where, d min (traj,goal) is the shortest distance between the predicted trajectory and the target, d(r,goal) is the Euclidean distance between the robot and the target, and d sensor is the obstacle detection distance of the sensor.
Citation Information
Patent Citations
Mobile robot path planning method based on improved A star strategy
CN108549388A
Path planning method of mobile robot
CN109213169A
Inspection robot path planning method based on improved A-satellite fusion DWA optimization algorithm
CN115079705A
Path planning method and device of mobile robot, electronic equipment and storage medium
CN115930969A
Path planning method based on adaptive grid and improved A*-DWA fusion algorithm
CN116360457A
Cited By
Six-axis robot obstacle avoidance path planning method and system based on adam optimization algorithm
CN119458382A
Rescue robot multi-constraint path planning method based on Voronoi diagram
CN119469184A
TD3 map-free navigation method based on dynamic window method guidance
CN119472677A
Electromagnetic environment adaptive path planning sampling method and related equipment
CN119846967A
Transmission, distribution and transformation unmanned aerial vehicle path planning method based on combination of improved RRT algorithm and Voronoi diagram
CN119879920A
