Path planning algorithm based on dynamic scene structure expansion perception
Through a path planning algorithm based on dynamic scene structure expansion perception, a sparse topological structure diagram and a two-way fast search random tree algorithm are used, combined with an adaptive expansion model and a motion model, the real-time and flexibility of the robot's path planning in dynamic scenes is solved, and rapid obstacle avoidance navigation is achieved.
Patent Information
- Application Number
- CN202211066028.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-09-01
- Publication Date
- 2025-08-08
- Estimated Expiration
- 2042-09-01
AI Technical Summary
The existing local path planning algorithms are difficult to respond quickly and re-plan the path when facing dynamic scenarios, resulting in the robot being unable to effectively deal with emergencies in an indoor environment.
The path planning algorithm based on dynamic scene structure expansion perception is adopted, and the path planning of the robot in dynamic scenes is realized through the sparse topological structure diagram of the raster map and the improved two-way fast search random tree algorithm, combined with the adaptive expansion model and motion model.
It greatly shortens the path planning time, improves the real-time and flexibility of obstacle avoidance navigation in dynamic scenarios, and can effectively deal with changes in dynamic obstacles.
Smart Images
Figure CN116009527B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of robot automatic path planning, and in particular to a path planning algorithm based on dynamic scene structure expansion perception. Background Art
[0002] The application of intelligent robots is attracting increasing attention. To complete specific tasks in complex scenarios, mobile robots need to be able to navigate autonomously. Path planning, one of the core technologies for autonomous navigation, aims to enable mobile robots to move quickly and collision-free from a starting position to a target position in environments with obstacles. In today's production and daily life, scenes often contain moving obstacles. When faced with dynamic scenarios, existing local path planning algorithms often struggle to react quickly and replan paths, and their adaptability to environmental changes is low, making it impossible to judge and handle sudden indoor situations. Summary of the Invention
[0003] The purpose of this invention is to address the shortcomings of existing technologies by providing a path planning algorithm based on dynamic scene structure expansion perception. First, the algorithm uses the sparse topological structure of the grid map to make the expansion of the random tree more directional. On this basis, the algorithm uses an improved bidirectional fast search random tree algorithm to obtain the initial path, reducing the pathfinding time of the mobile robot. Second, the algorithm uses adaptive operators to establish an expansion model for the robot based on the size and number of dynamic obstacles, the shortest distance between static obstacles, and the size of the map, allowing it to pre-collision with moving obstacles in dynamic scenes. Finally, the algorithm constructs a motion model of the mobile robot based on the planned path characteristics obtained by the random sampling algorithm.
[0004] The present invention is achieved through the following technical solutions:
[0005] A path planning algorithm based on dynamic scene structure expansion perception includes the following steps:
[0006] (1) A two-dimensional rectangular coordinate system XOY is established for the actual scene, and the scene is divided into an M×N two-dimensional grid map, where M represents the number of horizontal grids in the grid map, and N represents the number of vertical grids in the grid map.
[0007] (2) Harris corner detection algorithm is used to obtain feature points in the contour skeleton, and redundant feature points are eliminated through adaptive thresholding. The remaining feature points become sparse topological nodes. The obtained corner points are adaptively selected using the following formula according to the size of the grid map:
[0008]
[0009] Where a is the height of the simulated grid map, b is the width of the simulated grid map, and ε is the order of magnitude of the map size. With the current feature point as the center and Threshold as the radius, the feature points outside the center of the circle are removed.
[0010] (3) Construct a sparse topology graph through sparse topology nodes. On this basis, the initial path is obtained through the improved bidirectional fast search random tree algorithm. The specific method is as follows:
[0011] (3.1): All closed loops are generated into a tree;
[0012] (3.2): Define the parent nodes of sparse topology nodes on the cross branch as parent1, parent2, ..., parent n , a tree is generated in this way;
[0013] (3.3): Count all feasible paths consisting of sparse topological nodes, compare them, and select the shortest one as the initial path;
[0014] (3.4): When the line connecting the starting point and the child node of the nearest sparse topology node passes through an obstacle, set the starting point as the parent node of the nearest sparse topology node and then insert the starting point into the initial tree. If the line connecting the starting point and the nearest sparse topology node still passes through an obstacle, set the starting point as the parent node of the parent node of the sparse topology node, and repeat this process until the line does not pass through an obstacle, and set the starting point as the parent node of that node.
[0015] (3.5): When the line connecting the starting point and the child node of the nearest sparse topology node does not pass through an obstacle, let the starting point be the parent node of the child node of the nearest sparse topology node, and also insert the starting point into the tree.
[0016] (4) Set the adaptive expansion model and motion model of the mobile robot to simulate movement in dynamic scenes and perform obstacle perception in advance. First, use the adaptive expansion radius to perform expansion perception around the object to predict obstacles in advance. After the obstacle is perceived, the path is immediately replanned. The specific formula is as follows:
[0017]
[0018] Where k is an adaptive parameter that dynamically adjusts the expansion radius. R represents the adaptive expansion radius, and argmin is a mathematical function that represents the variable value when the objective function f(x) takes the minimum value. Make sure that the expansion interval does not contain dynamic obstacles, min / 2 is to allow the expansion area of the mobile robot to move normally between static obstacles, and to prevent it from being too large and causing invalid path replanning.
[0019] Next, the motion model of the mobile robot is set. Dynamic scenes often require a better motion model to adapt to ever-changing obstacles. The present invention proposes the following formula:
[0020] X pos =node current .x+(node next .x-node current .x) / k
[0021] Y pos =node current .y+(node next .y-node current .y) / k
[0022] where node current Represents the position of the current node, node next Represents the position of the next node adjacent to the planned path. Xpos and Ypos contain the position information and direction information in the robot motion model. x and y are the coordinate values of the robot, such as node current .x represents the X-axis coordinate value of the current robot node, node next .x represents the X-axis coordinate value of the next node, and y, similarly, represents the Y-axis coordinate value. The k value adaptively adjusts the robot's speed: larger k values result in slower movement, while smaller k values result in faster movement. In densely populated areas, the path formed by these nodes is less smooth, resulting in slower movement. Meanwhile, in sparsely populated areas, the path formed by these nodes is smoother, resulting in faster movement.
[0023] At the same time, in order to improve the efficiency of path planning in dynamic scenes, the mobile robot needs to replan when encountering dynamic obstacles. This chapter sets the end point of replanning as exploring W nodes backward (the nodes obtained by backward exploration should be included in the path node. If exceeded, the value of W is decremented by one until the explored node is included in the path node), and judges whether the node is within the obstacle. If the node is within the obstacle, continue to explore one node backward until the explored node is not included in the obstacle, and let the explored node be the end point of re-path planning. If the node is not within the obstacle, let the node be the end point of re-path planning. The new path obtained in this way is combined with the originally planned path to improve the efficiency of path re-planning.
[0024] (5) When no dynamic obstacle is sensed, the mobile robot continues to move along the planned path in step (3).
[0025] (6) When a dynamic obstacle is perceived, the mobile robot replans its path, with the starting point being the current node and the end point being W nodes backward from the current node. If the path is composed of a sparse topological structure graph, then it is W sparse topological nodes.
[0026] (7) When replanning the path, a new path is quickly planned based on the starting and ending points of step (6) and the optimal bidirectional rapid search random tree method. The present invention sets dynamic obstacles to 1 and static obstacles to 0. When the mobile robot is tracking and navigating on the replanned path, if the expansion model encounters a static obstacle again, the mobile robot does not replan, which is beneficial for it to pass through a narrow and long space. If a dynamic obstacle is encountered, replanning is required again.
[0027] (8) The new path is effectively merged with the original path, and the mobile robot continues to move on the merged path while repeating step (7).
[0028] The advantages of the present invention are:
[0029] 1. This invention uses the Harris corner detection algorithm to obtain feature points from the contour skeleton and uses an adaptive threshold to eliminate redundant feature points, retaining sparse topological nodes. This sparse topological node structure is then used to construct a sparse topological structure graph, and a bidirectional fast search random tree algorithm is used to rapidly determine the initial path. This effectively guides the direction of path planning and significantly shortens path planning time, effectively resolving the issues of long robot path planning times in indoor environments and the inability to quickly plan paths in narrow and long spaces.
[0030] 2. The present invention uses an adaptive expansion model and motion model to adjust the robot's expansion radius and motion speed according to the map size, the number of obstacles, etc., has strong real-time performance and flexibility, and can effectively perform obstacle avoidance navigation in dynamic scenes.
[0031] 3. This invention combines a bidirectional fast-search random tree algorithm with a robot's adaptive expansion model and motion model. This allows the algorithm to rapidly calculate an initial path when there are no dynamic obstacles. It also uses the adaptive expansion model and motion model to rapidly avoid and replan paths when obstacles are present. This proposed method, which allows the mobile robot to only explore W nodes backward as the replanning endpoint during path replanning, improves the algorithm's efficiency and ensures the real-time performance of its planned paths. BRIEF DESCRIPTION OF THE DRAWINGS
[0032] Figure 1 Flow chart of the method of the present invention;
[0033] Figure 2 A grid map scene graph with dynamic obstacles is established;
[0034] Figure 3 is a sparse topological structure graph;
[0035] Figure 4 Graph of the optimization process for bidirectional fast exploration of random trees;
[0036] Figure 5 Schematic diagram for setting the expansion radius;
[0037] Figure 6 This is a simulation result diagram of the present invention. DETAILED DESCRIPTION
[0038] The present invention is further described below with reference to the accompanying drawings and examples.
[0039] In this example, taking an indoor robot as an example, the overall process is as follows to implement the path planning algorithm based on dynamic scene structure expansion perception: Figure 1 As shown:
[0040] 1. Create a grid map based on the actual scenario;
[0041] 2. Use the Harris corner detection algorithm to obtain the feature points in the contour skeleton, and use the adaptive threshold to eliminate redundant feature points to form a topological structure diagram;
[0042] 3. Use the bidirectional fast search random tree algorithm to calculate the initial path of the topology graph;
[0043] 4. Adaptive expansion model and motion model enable movement simulation in dynamic scenes and early obstacle perception;
[0044] 5. Replan the route until reaching the destination.
[0045] In summary, when a mobile robot is performing path-following navigation, the path planning algorithm based on dynamic scene structure expansion perception uses an expansion model to enable the mobile robot to perceive obstacles in advance. After sensing an obstacle, the robot replans its path using a bidirectional fast search random tree algorithm. While ensuring the algorithm's effectiveness, and to further improve its real-time performance in dynamic scenes, when replanning its path, the mobile robot sets the starting point as the current node and the end point as the current node, exploring W nodes backward, rather than the initially set target point. The resulting replanned path is then integrated into the originally planned path to create a new obstacle-avoiding path. This process reduces the number of algorithm iterations compared to when the end point is the target point, improving its efficiency.
[0046] The specific process of this example is as follows:
[0047] Step 1: If Figure 2 , using a simulated indoor environment as the experimental map data, and adding 7 dynamic obstacles. The experiment uses a square expansion model. In order to better perform collision detection, the experimental map data needs to be preprocessed before the simulation experiment. The circular obstacles in the simulated indoor environment are expanded into square obstacles according to their radius. The map size is 734×402. According to the size of the map, the size of the dynamic obstacles is adjusted accordingly, and the adjusted size is 18×18. At the same time, a two-dimensional rectangular coordinate system XOY is established, and the scene is divided into a two-dimensional grid map of M×N, where M represents the number of horizontal grids in the grid map, and N represents the number of vertical grids in the grid map.
[0048] Step 2: If Figure 3 , Harris corner detection algorithm is used to obtain feature points in the contour skeleton, and redundant feature points are eliminated through adaptive thresholding. The remaining feature points become sparse topological nodes. The obtained corner points are adaptively selected using the following formula according to the size of the grid map:
[0049]
[0050] Where a is the height of the simulated grid map, b is the width of the simulated grid map, and ε is the order of magnitude of the map size. With the current feature point as the center and Threshold as the radius, the feature points outside the center of the circle are removed.
[0051] Step 3: Use an improved bidirectional fast search random tree algorithm to obtain the initial path for the sparse topology node graph. This paper proposes an improved sampling strategy that ensures that the sampled nodes follow a normal distribution. That is, there is a 50% probability that the sampling points will be sampled along the line between the closest nodes in the two random trees, and another 50% probability that the sampling points will be sampled throughout the state space.
[0052] Since the present invention is theoretically asymptotically optimal, the optimal path can be found as the number of iterations increases. In addition, the algorithm is expanded from the starting point and the end point at the same time, which reduces the exploration time of path planning. At the same time, when the algorithm performs the two processes of selecting the parent node with the lowest cost for the new node and finding the child node for the new node, the present invention changes the judgment function from Euclidean distance to Manhattan distance to improve the operation efficiency of the algorithm, as shown in the following formula, where node represents the current node, node i Represents all nodes in a circle with node as the center and sampling radius r (the size of the sampling radius is determined by the following formula).
[0053] dis=|node.x-node i .x|+|node.y-node i .y|
[0054] The main process of the algorithm is as follows: First, two fast-expanding random trees are established with the starting point and the target point as the initial nodes. The two randomly generated trees are T1 and T2. When the T1 tree is in the space map without obstacles in the grid map, free Randomly sample a node node r , extend from the nearest vertex in the T1 tree to it to get a new node node new . Secondly, build a new node node new Sampling a circle with a radius of r as the center, and obtaining the set of all nodes in the circle Node n Finally, in the collection Node n Find the node in the node and make it match the new node node new The path cost is the lowest, and the new node new Perform rewiring.
[0055] After the T1 tree completes the above process, the asymptotically optimal bidirectional rapid exploration random tree algorithm attempts to move the T2 tree to the result vertex node in the T1 tree. new The nearest vertex node connect Next, the process of T2 tree path optimization is described as follows: First, two nodes node1 and node2 are selected as input in T2 tree, and the iterative process and RRT in T2 tree are * The iterative process of the algorithm is the same, where node2 plays the role of node r The algorithm constructs a circle from the T2 tree with node 1 as the center and a radius of a specified length. It calculates the path cost between each node in the circle and node 2 and stores the path costs in a linked list. The algorithm then traverses and sorts the linked list based on the total path cost, attempting to connect to vertices in another tree only if the resulting solution has a lower cost than the current best solution. Finally, the algorithm updates the current best solution, pruning and swapping tree extensions using a branch-and-bound algorithm.
[0056] Figure 4 The optimization process of the T2 tree is shown, where the red nodes are T2 trees and the black nodes are T1 trees. From the figure we can see that before node2 (node a) is selected, the planned path will follow the node ecd to reach node goal, its current path cost is 27. After adding node2, its path cost is recalculated. From the figure, we can see that the path cost along node eca becomes smaller, with a cost of 22. Therefore, it updates the current solution, that is, replacing node ecd with node eca to reduce the cost of the planned path.
[0057] Step 4: Set the adaptive expansion model and motion model of the mobile robot to enable it to perform movement simulation in dynamic scenes and perceive obstacles in advance.
[0058] When planning a path in a dynamic scene, dynamic obstacles will also move as the robot moves from the starting point to the target point. Therefore, the originally planned path may not be applicable to the scene after the obstacles move. Therefore, we use an adaptive expansion radius to expand the perception around the object to predict obstacles in advance, such as Figure 5 , after sensing an obstacle, the path is replanned immediately. The expansion radius is mainly determined by the following parameters: the shortest distance between static obstacles l min The number of dynamic obstacles q is related to the size of the map a, b, and the area of the dynamic obstacle s, where a is the width of the grid map and b is the length of the grid map. The specific formula is as follows:
[0059]
[0060] Where k is an adaptive parameter that dynamically adjusts the expansion radius. Make sure that the expansion interval does not contain dynamic obstacles, min / 2 is to allow the expansion area of the mobile robot to move normally between static obstacles, and to prevent it from being too large and causing invalid path replanning.
[0061] Next, the motion model of the mobile robot is set. Dynamic scenes often require a better motion model to adapt to ever-changing obstacles. The present invention proposes the following formula:
[0062] X pos =node current .x+(node next .x-node current .x) / k
[0063] Y pos =node current .y+(node next .y-node current .y) / k
[0064] where node current Represents the position of the current node, node nextRepresents the location of the next adjacent node on the planned path. The k value adaptively adjusts the mobile robot's speed: larger k values result in slower movement, while smaller k values result in faster movement. In densely populated areas, the path formed by these nodes is less smooth, resulting in slower movement. Meanwhile, in sparsely populated areas, the path formed by these nodes is smoother, resulting in faster movement. Therefore, the constructed mobile robot motion model is well suited for path navigation in dynamic scenarios.
[0065] At the same time, in order to improve the efficiency of path planning in dynamic scenes, the mobile robot needs to replan when encountering a dynamic obstacle. The present invention sets the end point of replanning to explore W nodes backward (the node obtained by backward exploration should be included in the path node. If exceeded, the value of W is subtracted by one until the explored node is included in the path node), and judges whether the node is within the obstacle. If the node is within the obstacle, continue to explore one node backward until the explored node is not included by the obstacle, and let the explored node be the end point of re-path planning. If the node is not within the obstacle, let the node be the end point of re-path planning. The new path obtained in this way is combined with the originally planned path to improve the efficiency of path replanning.
[0066] Step 5: When no dynamic obstacle is sensed, the mobile robot continues to move along the planned path in step 3.
[0067] Step 6: When a dynamic obstacle is sensed, the mobile robot replans its path, starting from the current node and ending at W nodes backward from the current node. If the path is composed of a sparse topology graph, then it will be W sparse topology nodes.
[0068] Step 7: When replanning the path, a new path is quickly planned based on the starting and ending points of step 6 and the optimal two-way fast search random tree method. The present invention sets dynamic obstacles to 1 and static obstacles to 0. When the mobile robot is tracking and navigating on the replanned path, if the expansion model encounters a static obstacle again, the mobile robot does not replan, which is beneficial for it to pass through a narrow and long space. If a dynamic obstacle is encountered, replanning is required again; if Figure 6 If the mobile robot encounters an obstacle again while following the replanned path, it will use its current location as the starting point and the end point as the replanned destination. The green path left at the end is the robot's collision-free planned path.
[0069] Step 8: Effectively merge the new path with the original path, and the mobile robot continues to move on the fused path while repeating step 7.
Claims
1. A path planning algorithm based on dynamic scene structure expansion perception, characterized by: The specific steps include: (1) Establish a grid map based on the actual scene; (2) Harris corner detection algorithm is used to obtain feature points in the contour skeleton, and redundant feature points are eliminated through adaptive thresholding. The remaining feature points become sparse topological nodes. (3) Construct a sparse topology graph through sparse topology nodes, and calculate the initial path of the topology graph through a bidirectional fast search random tree algorithm; (4) Setting the adaptive expansion model and motion model of the mobile robot to enable it to simulate movement in dynamic scenes and perceive obstacles in advance; (5) When no dynamic obstacle is sensed, the mobile robot continues to move along the planned path in step (3); (6) When a dynamic obstacle is perceived, the mobile robot replans its path, starting from the current node and ending at W nodes backward from the current node. If the path is composed of a sparse topological structure graph, then it is W sparse topological nodes; (7) When replanning the path, a new path is quickly planned based on the starting and ending points of step (6) and the optimal bidirectional fast search random tree method; (8) The new path is merged with the original path, and the mobile robot continues to move on the merged path while repeating step (7); The setting of the adaptive expansion model and motion model of the mobile robot in step (4) enables it to perform movement simulation and early obstacle perception in a dynamic scene, as follows: First, an adaptive expansion radius is used to expand the area around the object to predict obstacles in advance. After the obstacle is detected, the path is immediately replanned. The specific formula is as follows: Where k is an adaptive parameter that dynamically adjusts the expansion radius, l min is the shortest distance between static obstacles, q is the number of dynamic obstacles, s is the area of dynamic obstacles, R is the adaptive expansion radius, and argmin is a mathematical function that represents the variable value when the objective function f(x) takes the minimum value; Then set the mobile robot motion model and propose the following formula: X pos =node current .x+(node next .x-node current .x) / k THE pos =node current .y+(node next .y-node current .y) / k where node current Represents the position of the current node, node next It represents the position of the next node adjacent to the planned path. Xpos and Ypos contain the position information and direction information in the robot motion model. x and y are the coordinate values of the robot. The k value adaptively adjusts the speed of the mobile robot. The larger the k value, the slower the moving speed, and the smaller the k value, the faster the moving speed. In places where the nodes are dense, the path formed by the nodes is less smooth, which is specifically reflected in the slower moving speed. At the same time, in places where the nodes are sparse, the path formed by the nodes is more smooth, which is specifically reflected in the faster moving speed.
2. The path planning algorithm based on dynamic scene structure expansion perception according to claim 1, characterized in that: The step (1) of establishing a grid map according to the actual scene is as follows: a two-dimensional rectangular coordinate system XOY is established for the actual scene, and the actual scene is divided into an M×N two-dimensional grid map, where M represents the number of horizontal grids in the grid map, and N represents the number of vertical grids in the grid map.
3. The path planning algorithm based on dynamic scene structure expansion perception according to claim 1, characterized in that: In step (2), the Harris corner detection algorithm is used to obtain feature points in the contour skeleton, and redundant feature points are eliminated through adaptive thresholding. The remaining feature points become sparse topological nodes. Specifically, the following formula is used to adaptively select the obtained corner points according to the size of the grid map: Where a is the height of the simulated grid map, b is the width of the simulated grid map, ε is the order of magnitude of the map size, and the feature points outside the center of the circle are removed from the circle formed by the current feature point as the center and Threshold as the radius.
4. The path planning algorithm based on dynamic scene structure expansion perception according to claim 1, characterized in that: The sparse topology structure graph described in step (3) is constructed by sparse topology nodes, and the initial path of the topology structure graph is calculated by a bidirectional fast search random tree algorithm. The specific method is as follows: (3.1): All closed loops are generated into a tree; (3.2): Define the parent nodes of sparse topology nodes on the cross branch as parent1, parent2, ..., parent n , a tree is generated in this way; (3.3): Count all feasible paths consisting of sparse topological nodes, compare them, and select the shortest one as the initial path; (3.4): When the line connecting the starting point and the child node of the nearest sparse topology node passes through an obstacle, let the starting point be the parent node of the nearest sparse topology node, and then insert the starting point into the initial tree. If the line connecting the starting point and the nearest sparse topology node still passes through the obstacle, then another starting point is set as the parent node of the parent node of the sparse topology node, and so on until the line does not pass through the obstacle, and let the starting point be the parent node of the node; (3.5): When the line connecting the starting point and the child node of the nearest sparse topology node does not pass through an obstacle, let the starting point be the parent node of the child node of the nearest sparse topology node, and also insert the starting point into the tree.
5. The path planning algorithm based on dynamic scene structure expansion perception according to claim 1, characterized in that: When replanning the path in step (7), the dynamic obstacle is set to 1 and the static obstacle is set to 0. When the mobile robot is navigating along the replanned path, if the expansion model encounters a static obstacle again, the mobile robot does not replan, which is beneficial for it to pass through a narrow space. If it encounters a dynamic obstacle, the path is replanned again.