A hierarchical path planning method for topology-grid-metric hybrid maps
By performing hierarchical path planning on topology-raster-metric hybrid maps, combined with improved DDPG algorithms and value classification experience playback pools, the problem of time-consuming and poor obstacle avoidance in complex large-scenario environments is solved. Achieve efficient and safe robot movement.
Patent Information
- Application Number
- CN202210340211.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-01
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2042-04-01
AI Technical Summary
When facing some unknown complex large-scenario environments, the existing path planning methods have large search space, time-consuming planning, and poor real-time obstacle avoidance, making it difficult to ensure the safe and efficient movement of the robot in complex environments.
A hierarchical path planning method for topology-raster-metric mixed map is proposed. By applying different path planning methods on topology, raster, and metric maps, the path planning is split into global preset paths and local path planning, and the path planning is used. Improved DDPG algorithm and value classification experience playback pool for local path planning.
It effectively reduces the scale of path planning problems in large-scale scenarios, improves search efficiency, improves the quality of path planning and the robot's obstacle avoidance ability in dynamic unknown environments.
Smart Images

Figure CN114740846B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of path planning, and in particular to a hierarchical path planning method for a topology-grid-metric hybrid map. Background Art
[0002] Path planning is the basis of navigation. Its purpose is to search for a collision-free optimal or suboptimal path from the starting point to the end point based on one or more performance indicators such as safety, shortest distance, and optimal time. There have been a large number of relatively mature research results on path planning for known environment maps, but most scenarios in practical applications are relatively complex, highly random, and belong to partially unknown environments. Most of the existing path planning methods rely more on the exact environment. For path planning in partially unknown environments, obstacle avoidance safety and real-time performance are difficult to guarantee. In addition, the tasks of mobile robots are becoming more and more complex, requiring their working scope to continue to expand. For example, sweeping robots need to have the ability to clean every corner of the whole house, and AGVs in factories involve cross-workshop distribution. The expansion of the working scope not only brings challenges to indoor positioning technology, but also the widely used intelligent search methods take a long time to find a solution due to the increase in search space, and even fail to search for the optimal solution.
[0003] The prior art discloses a path planning method based on an improved A* algorithm (publication number: CN113359768A), which combines the improved A* algorithm with the artificial potential field method to design a hybrid path planning algorithm, optimizes the global initial path and takes into account the unknown environment. However, when planning the global path, A* and ant colony algorithm are directly applied on the grid map. If faced with a large scene environment, the algorithm search space expands rapidly and the path search efficiency decreases. At the same time, the artificial potential field method may have frequent swinging problems when dealing with sudden obstacles, which is not conducive to the robot motion control.
[0004] Faced with partially unknown complex large-scale environments, the path planning algorithm using a single map is not yet ideal, mainly reflected in the high requirements for the completeness of environmental information, large search space, time-consuming planning, and low real-time obstacle avoidance. Summary of the invention
[0005] In order to solve the technical defects existing in the prior art, the present invention proposes a hierarchical path planning method for topology-grid-metric hybrid maps, which splits the path planning into several sub-processes. By applying different path planning methods on topology, grid, and metric maps, the problems of the prior art in facing partially unknown complex large-scene environments, such as large search space, time-consuming planning, and low real-time obstacle avoidance, are solved.
[0006] The present invention is achieved through the following technical solutions:
[0007] A hierarchical path planning method for topology-grid-metric hybrid maps includes the following steps:
[0008] Step 1: Based on the topology-grid-metric hybrid map construction method, a hybrid map of the mobile robot's operating environment is created, including a topology map for abstract representation, a grid map for concrete representation, and a metric map for refined representation;
[0009] Step 2: Before the mobile robot runs, a global preset path with overall optimization is generated according to the topology-grid hierarchical planning method;
[0010] Step 3: When the mobile robot runs along the global preset path, it detects the surrounding environment in real time to determine whether the distance to the nearest obstacle to the robot is less than the obstacle avoidance threshold. If so, it proceeds to step 4; if not, it proceeds to step 5;
[0011] Step 4: Use the improved Deep Deterministic Policy Gradient (DDPG) algorithm to perform local path planning on the metric map: first build a value classification experience replay pool, and store high-value samples, ordinary samples, and recent samples in three sub-replay pools of the value classification experience replay pool respectively; then use a stratified sampling strategy to randomly extract an appropriate amount of samples from the three sample sub-replay pools for the DDPG algorithm to learn and train local path planning; and introduce a continuous reward function including arrival reward, collision penalty, and process reward to reward or punish the local path planned by the DDPG algorithm, train the DDPG algorithm to plan a better local path, and return to the global preset path after completing obstacle avoidance through local path planning;
[0012] Step 5: Determine whether the mobile robot has reached the target position. If so, the algorithm ends; if not, proceed to step 3.
[0013] Furthermore, the topology-grid-metric hybrid map construction method comprises the following steps:
[0014] Step 1.1: Rasterize the overall working environment and construct a global raster map;
[0015] Step 1.2: Divide the global grid map into several sub-grid maps, set key nodes in each sub-grid map, and use the hierarchical graph to describe the working environment as a hierarchical map representation with sub-maps as tree nodes. In the k-th layer map, the k+1-th layer sub-map is represented by key nodes, and the key nodes at the same level are connected by offline prior paths;
[0016] Step 1.3: A thinning algorithm is used on the grid map to generate an offline prior path with a high degree of safety, and a topological map corresponding to each level of sub-grid map is constructed through a topological feature extraction method;
[0017] Step 1.4: The mobile robot maintains and updates the local metric map using the onboard sensor information it carries during operation.
[0018] Furthermore, the granularity of the sub-map should not be too fine, and it is generally sufficient to divide it into a single room dimension.
[0019] Furthermore, when the thinning algorithm processes a two-dimensional grid map, a free grid with a value of "0" is regarded as "1" in the image, and an obstacle grid with a value of "1" is regarded as "0" in the image, and finally an offline path of the idle area is generated.
[0020] Furthermore, the topological feature extraction method uses the bifurcation points of the offline prior paths as ordinary nodes in the topological map, manually selects regional entrance grids or other representative grids to represent the key nodes of the sub-map, and counts the offline prior path lengths between the key nodes as the weights of the edges, where the path length is the number of grids contained in the path.
[0021] Furthermore, the topology-grid hierarchical planning method comprises the following steps:
[0022] Step 2.1: Determine whether the starting grid S and the target grid D are in the same sub-map. If so, proceed to step 2.2; if not, proceed to step 2.3;
[0023] Step 2.2: For the starting point S to the end point D, an improved A* algorithm is used to plan an overall optimized global preset path, and the algorithm ends;
[0024] Step 2.3: Get the depth L of the starting point S and the end point D in the hierarchical graph architecture respectively s , L D and the key node C of the sub-grid map where it is located S , C D , if L s <L D , then from C D Start by searching from bottom to top in the hierarchical graph architecture until L s-1 Find C in the topology diagram corresponding to the layer S ; Otherwise, from C S Start searching upwards until L D-1 Find C in the topology diagram corresponding to the layer D Stop; During the search process, the Floyd algorithm is used to plan the optimal node sequence on each level of topological map, and the offline prior paths between nodes are saved as the mid-segment preset paths;
[0025] Step 2.4: Starting from the starting point S, the key node C SAs the end point, the improved A* algorithm is used to search for a local path on the sub-grid map where the starting point S is located and save it as the preset path for the previous section; similarly, the key node C D Set it as the starting point and the end point D as the end point. Use the improved A* algorithm to search for another local path on the sub-grid map where the starting point S is located and save it as the preset path for the next section. If no feasible solution can be found, the path planning fails and the algorithm ends.
[0026] Step 2.5: Merge the middle preset path generated in step 2.3 with the front preset path and the back preset path searched in step 2.4 to obtain the overall optimized global preset path, and the algorithm ends.
[0027] Furthermore, the improved A* algorithm introduces an extension point screening strategy, a bidirectional search mechanism, and a path redundant point elimination technology into the standard A* algorithm.
[0028] The extension point screening strategy has a certain probability of ignoring nodes with weak directionality during expansion, and the steps are:
[0029] Use the area of the rectangle formed by the current extension point m, the starting point S and the end point D To reflect the degree of conformity between m and the expected path, The smaller it is, the more consistent the node m is with the expected path, and the more likely it is to be added to the Open table and continue to expand.
[0030]
[0031] In formula (1), (x s ,y S )、(x D ,y D )、(x m ,y m ) are the grid coordinates of the starting point S, the end point D and the current extension point m respectively;
[0032] Set area threshold when When m is directly added to the Open table that is used to store nodes that have been detected but not yet visited; when When , a random number p(m) between [0,1] is generated, and it is determined whether it is greater than the trust threshold probability p0. If it is, node m is ignored, otherwise it is added to the Open table.
[0033] The bidirectional search mechanism simultaneously performs forward and reverse searches from the starting point and the end point. The forward search and the reverse search respectively use the latest node obtained by the other party as the target node for alternating extended searches, and continuously move closer to the other party until the target nodes of the forward search and the reverse search meet, and finally combine the paths generated by the forward search and the reverse search, including the following steps:
[0034] Step 3.1: Initialize the Open1, Open2, Close1, Close2 lists, and add the starting point S to Open1 and the end point D to Open2;
[0035] Step 3.2: Take out the nodes n1 and n2 with the smallest cost value from the Open1 and Open2 lists respectively, and move them to Close1 and Close2 respectively;
[0036] Step 3.3: Determine whether n1 and n2 are the current search nodes in the relative search direction. If so, the algorithm ends and the paths generated in the forward and reverse directions are combined to form the final optimized path. If not, proceed to step 3.4.
[0037] Step 3.4: Forward search starts from S, ends at n2, and expands to the surrounding 8 neighborhoods with n1 as the center, and updates Open1 according to the rules for selecting the extension points;
[0038] Step 3.5: The reverse search starts from D, ends at n1, and expands to the surrounding 8 neighborhoods with n2 as the center, and updates Open2 according to the rules for filtering the extension points;
[0039] Step 3.6: Determine whether Open1 and Open2 are empty. If so, the algorithm ends and no valid path can be found. Otherwise, go to step 3.2.
[0040] The path redundant point elimination technology specifically includes the following steps:
[0041] Step 4.1: Insert a new node at the point where the distance between each pair of adjacent nodes on the original path is halved;
[0042] Step 4.2: Let the starting node be P1, the next node be P2, and so on until the end point P k , and each node sets its previous node as its parent node;
[0043] Step 4.3: Let P n To optimize the benchmark node, P c is the node being explored, n is initialized to k, and c is initialized to k-2;
[0044] Step 4.4: Determine P n With P c The connection L nc Whether it has passed the obstacle, if it has passed, it means P c+1 Cannot be deleted, P n Jump to P c+1 ; Otherwise delete P c+1 , and P c Set to P n The parent node of Pc Jump to its parent node P c-1 ;
[0045] Step 4.5: Repeat step 4.4 until P n =P1 or P c =P1;
[0046] Step 4.6: Connect the remaining nodes by their parent node pointers.
[0047] Furthermore, the improved DDPG algorithm introduces a value classification experience replay pool and a stratified sampling strategy, and designs a continuous reward function that integrates target distance, safety, motion, and number of steps.
[0048] The value classification experience replay pool consists of three queues, including: a sub-replay pool B1 for storing high-value samples, a sub-replay pool B2 for storing recent samples, and a sub-replay pool B3 for storing ordinary samples. Each sub-replay pool stores experience samples in the following five-tuple form:
[0049] (S t , a t , r t , S t+1 ,done) (2)
[0050] Formula (2) indicates that the agent is in state S t Execute action a t After reaching the new state S t+1 , and get the reward r given by the environment t , where done represents S t+1 Whether it is in the terminated state.
[0051] The specific steps for constructing the value classification experience replay pool are as follows:
[0052] During initialization, set the average value V of all sample values in the two sub-replay pools B1 and B3 m is 0, and the weight coefficient α of the reward value is set to 1. During the training process, an experience sample is generated at each time step. The generated sample is first added to the tail of the B2 queue. If B2 reaches the capacity limit, a sample is popped out from the head of the queue, and the value of the sample is calculated according to the sample value measurement standard. If it is higher than V m If yes, the sample is added to B1, otherwise it is stored in B3. After storage, update V m and α, and repeat the above process until the training is completed.
[0053] Taking the sample generated at time step t as an example, the sample value measurement standard is calculated as follows:
[0054] V t =α*|r t|+(1-α)*|δ t | (3)
[0055] δ t =r t +γQ′(S t+1 ,a t+1 θ Q′ )-Q(S t ,a t θ Q ) (4)
[0056] In formula (3), V t is the value of the sample, |r t | is the absolute value of the sample reward value, α is |r t The weight coefficient of |δ t | is the absolute value of the sample time difference error, (1-α) is |δ t |weight;
[0057] In formula (4), γ is the reward value attenuation coefficient, Q′ is the target value network state action value, θ Q′ is the target value network parameter, Q is the online value network state action value, θ Q is the online value network parameter, S t is the state space at time step t, a t is the action value decided in this state space, S t+1 For the agent in S t Execute a t The new state reached after t+1 is the action value in the new state;
[0058] The stratified sampling strategy randomly extracts appropriate amount of samples from the B1, B2 and B3 sub-replay pools for training the network.
[0059] The continuous reward function includes the arrival reward r a , collision penalty r c , process reward n Three parts, reward function r t Defined as:
[0060]
[0061] r n =r d +r s +r m +r p (6)
[0062] During navigation, the robot obtains the process reward r for executing actions n Divided into 4 parts: Distance Rewardd , security penalty s , sports punishment m and step penalty r p , as shown in formula (6).
[0063] r d It is the most important part of the reward in the navigation process, guiding the robot to approach the target point, and is defined as:
[0064] rd=k1*(Dg(St -1 )-Dg(St)) (7)
[0065] In formula (7), k1 is the distance reward coefficient, D g (S t-1 ) represents the distance between the robot and the target point in the previous state, D g (S t ) represents the distance between the robot and the target point in the current state.
[0066] r s Used to warn the robot to stay away from obstacles as much as possible and choose a safer path. It is defined as:
[0067]
[0068] In formula (8), k2 is the security penalty coefficient, D o Indicates the distance between the robot and the nearest obstacle around it, d s is the safety distance threshold; s r The safety level of the performance path is defined as:
[0069]
[0070] r m Penalize the robot's frequent swings and simplify the robot's motion control, which is defined as:
[0071]
[0072] In formula (10), k3 is the motion penalty coefficient; w t Indicates the angular velocity of the current state output, w t-1 Indicates the angular velocity of the previous state output.
[0073] In order to prevent the robot from blindly pursuing the maximum reward and deliberately not going straight to the goal, r p Prevent the robot from blindly exploring and indirectly stimulate the robot to find a better path, which is defined as:
[0074] r p =k4 (11)
[0075] In formula (11), k4 is the step penalty coefficient.
[0076] Compared with the prior art, the present invention has at least the following beneficial effects or advantages:
[0077] The hierarchical path planning method for topology-grid-metric hybrid maps described in the present invention divides the overall operating environment into sub-maps of different levels, applies different path planning methods to each sub-map to generate a path, and finally splices the path. This hierarchical idea avoids a detailed description of the overall environment, only expands key point information when necessary, limits the search space of the path planning algorithm to the sub-map, reduces the scale of the path planning problem in large-scale scenarios, and improves the search efficiency.
[0078] At the same time, in terms of global path planning, the global optimization initial path generated based on the topological map and grid map has both safety and smoothness, which improves the quality of path planning. In terms of local path planning, in order to address the problems of long training time of the DDPG algorithm and low utilization rate of experience samples, a value classification experience replay pool, stratified sampling strategy and continuous reward function are designed to accelerate the training process and guide the network to converge to a more reasonable obstacle avoidance strategy, helping the robot to better respond to emergencies and adapt to dynamic unknown environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0079] Figure 1 A flow chart of a hierarchical path planning method for a topology-grid-metric hybrid map of the present invention;
[0080] Figure 2 Diagram of filtering policies for extension points
[0081] Figure 3 Schematic diagram of topology-raster-metric hybrid map
[0082] Figure 4 Hierarchical diagram DETAILED DESCRIPTION
[0083] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments completed by the inventor according to the technical solution provided by the present invention. However, this embodiment is not intended to limit the present invention. All similar methods and similar variations of the present invention should be included in the protection scope of the present invention.
[0084] The hybrid map hierarchical path planning method for topology-grid-metric map described in the present invention is as follows: Figure 1 As shown, the specific steps include:
[0085] Step 1: Create a hybrid map of the mobile robot's operating environment based on the topology-grid-metric hybrid map construction method.
[0086] The topology-raster-metric hybrid map uses topology maps, raster maps, and metric maps to represent the work environment in an abstract, concrete, and refined manner from multiple dimensions. Figure 3 The topology-grid-metric hybrid map construction method comprises the following steps:
[0087] Step 1.1: Rasterize the overall working environment and construct a global raster map.
[0088] Raster maps are not difficult to obtain. By using sonar sensors or tools such as the gmapping function package in the Robot Operating System (ROS) to completely scan the surrounding environment, a raster representation of the global environment can be obtained.
[0089] Step 1.2: Divide the global grid map into several sub-grid maps. Set key nodes in each sub-grid map. Use a hierarchical graph to describe the working environment as a hierarchical map representation with sub-maps as tree nodes. In the k-th layer map, the k+1-th layer sub-map is represented by key nodes, and key nodes at the same level are connected by offline paths.
[0090] The hierarchical graph is expressed using a multi-branch tree, which contains multiple abstract levels, and the degree of abstraction decreases from top to bottom, such as Figure 4 The sub-map granularity should not be too fine, generally divided into 10-30m 2 The room dimensions can be.
[0091] Step 1.3: Use a thinning algorithm on the grid map to generate an offline prior path with a higher degree of security, and construct a topological map corresponding to each level through a topological feature extraction method.
[0092] The thinning algorithm only processes areas with a pixel value of 1. Therefore, when processing a two-dimensional grid map, a free grid with a value of "0" is regarded as "1" in the image, and an obstacle grid with a value of "1" is regarded as "0" in the image, and an offline path of the idle area is finally generated. On this basis, the topological features are extracted, and the bifurcation points of the offline prior path are used as ordinary nodes in the topological map. The regional entrance grid or other representative grids are manually selected to represent the key nodes of the sub-map. The offline prior path length between the key nodes is counted as the weight of the edge. The path length is the number of grids contained in the path, and finally the topological map corresponding to each level is constructed.
[0093] Step 1.4: The mobile robot maintains and updates the local metric map using the onboard sensor information it carries during operation.
[0094] Through the inertial navigation system equipped on the mobile robot and sensors for detecting the surrounding environment, such as lidar, a refined position representation is achieved and a local metric map is maintained.
[0095] Step 2: Before the mobile robot runs, an overall optimized global preset path is generated according to the topology-grid hierarchical planning method.
[0096] The topology-grid hierarchical planning method comprises the following steps:
[0097] Step 2.1: Determine whether the starting grid S and the target grid D are in the same sub-map. If so, proceed to step 2.2; if not, proceed to step 2.3;
[0098] Step 2.2: For the starting point S to the end point D, an improved A* algorithm is used to plan an overall optimized global preset path, and the algorithm ends;
[0099] Step 2.3: Get the depth L of the starting point S and the end point D in the hierarchical graph architecture respectively s , L D and the key node C of the sub-grid map where it is located S , C D ,; if L s <L D , then from C D Start by searching from bottom to top in the hierarchical graph architecture until L s-1 Find C in the layer S . ; Otherwise, from C S Start searching upwards until L D-1 Find C in the layer D Stop; During the search process, the Floyd algorithm is used to plan the optimal node sequence on each level of topological map, and the offline prior paths between nodes are saved as the mid-segment preset paths;
[0100] Step 2.4: Starting from the starting point S, the key node C S As the end point, use the improved A* algorithm to search for a local path on the sub-grid map where the starting point S is located and save it as the previous preset path. D Set as the starting point, and the end point D as the end point, use the improved A* algorithm to search for another local path on the sub-grid map where the starting point S is located and save the preset path for the latter part. If a feasible solution cannot be found, the path planning fails and the algorithm ends;
[0101] Step 2.5: Merge the middle preset path generated in step 2.3 with the front preset path and the back preset path searched in step 2.4 to obtain the overall optimized global preset path, and the algorithm ends.
[0102] Step 3: When the mobile robot runs along the global preset path, it detects the surrounding environment in real time and determines whether the distance to the nearest obstacle of the robot is less than the obstacle avoidance threshold. If so, it proceeds to step 4; if not, it proceeds to step 5.
[0103] The laser radar equipped on the robot can obtain multiple point cloud data in one scan. These point cloud data reflect the distance of the surrounding obstacles from the robot. When the minimum value of the point cloud data is less than the obstacle avoidance threshold, it means that the robot is close to the obstacle and needs to avoid it.
[0104] Step 4: Use the improved deep deterministic policy gradient algorithm to perform local path planning on the metric map, and return to the global preset path after completing obstacle avoidance.
[0105] The metric map can easily obtain the relative position of two points, which is conducive to the state space representation. By splicing the distance point cloud data of the lidar, the current position of the robot, and the target position into the state space and inputting it into the trained obstacle avoidance strategy network, the linear velocity and angular velocity are output end-to-end to control the robot movement. When the robot detects an obstacle, it selects a sub-target point behind the obstacle on the initial path according to the obstacle's position and plans the obstacle avoidance route by itself.
[0106] Step 5: Determine whether the mobile robot has reached the target position. If so, the algorithm ends; if not, proceed to step 3.
[0107] The improved A* algorithm introduces an extension point screening strategy, a bidirectional search mechanism, and a path redundant point elimination technology into the standard A* algorithm.
[0108] (1) Extension point screening strategy
[0109] The extension point screening strategy has a certain probability of ignoring nodes with weak directionality when expanding. The method is as follows:
[0110] Path planning expects the path length to be as short as possible, that is, the path direction is as close as possible to the direction from the starting point to the end point. Use the area of the rectangle enclosed by the current extension point m, the starting point S and the end point D To reflect the degree of conformity between m and the expected path, such as Figure 2 Shown in the green rectangle. The smaller it is, the more consistent the node m is with the expected path, and the more likely it is to be added to the Open table and continue to expand.
[0111]
[0112] In formula (1), (x s ,y S )、(x D ,y D )、(x m ,y m ) are the grid coordinates of the starting point S, the end point D and the current extension point m respectively;
[0113] Set area threshold when When m is added directly to the Open table; when When , a random number p(m) is generated and it is determined whether it is greater than the trust threshold probability p0. If it is, node m is ignored, otherwise it is added to the Open table.
[0114] (2) Bidirectional search mechanism
[0115] The bidirectional search mechanism starts forward search and reverse search from the starting point and the end point at the same time. The forward search and the reverse search use the latest node obtained by the other party as the target node for alternating expansion search, and continue to move closer to each other until the target nodes of the forward search and the reverse search meet. Finally, the paths generated by the forward search and the reverse search are combined, including the following steps:
[0116] Step 3.1: Initialize the Open1, Open2, Close1, Close2 lists, and add the starting point S to Open1 and the end point D to Open2;
[0117] Step 3.2: Take out the nodes n1 and n2 with the smallest cost value F from the Open1 and Open2 lists respectively, and move them to Close1 and Close2 respectively;
[0118] Furthermore, the cost value F is calculated as follows:
[0119] F(n)=g(n)+h(n) (2)
[0120]
[0121] In formula (2), F(n) is the estimated cost of the starting point s to reach the target point via node n; g(n) is the actual cost of the starting point s to reach node n; h(n) is the actual cost of the node n to reach the target d.
[0122] In formula (3), (x n ,y n )、(x s ,y S ) are the coordinates of node n and starting point s respectively;
[0123] In formula (4), (x d ,y d ) is the coordinate of the end point d;
[0124] Step 3.3: Determine whether n1 and n2 are the current search nodes in the relative search direction. If so, the algorithm ends and the paths generated in the forward and reverse directions are combined to form the final optimized path. If not, proceed to step 3.4.
[0125] Step 3.4: Forward search starts from S, ends at n2, and expands to the surrounding 8 neighborhoods with n1 as the center, and updates Open1 according to the rules for selecting the extension points;
[0126] Step 3.5: The reverse search starts from D, ends at n1, and expands to the surrounding 8 neighborhoods with n2 as the center, and updates Open2 according to the rules for filtering the extension points;
[0127] Step 3.6: Determine whether Open1 and Open2 are empty. If so, the algorithm ends and no valid path can be found. Otherwise, go to step 3.2.
[0128] It should be noted that when the bidirectional search mechanism is extended to the middle, it follows the extension point screening strategy.
[0129] (3) Path redundant point elimination technology
[0130] On the basis of the original path generated by bidirectional search, the redundant point elimination technology of the path is further applied, which specifically includes the following steps:
[0131] Step 4.1: Insert a new node at the point where the distance between each pair of adjacent nodes on the original path is halved;
[0132] Step 4.2: Let the starting node be P1, the next node be P2, and so on until the end point P k , and each node sets its previous node as its parent node;
[0133] Step 4.3: P n is the currently optimized node, P c is the node being explored, n is initialized to k, and c is initialized to k-2;
[0134] Step 4.4: Determine P n With P c The connection line L nc Whether it has passed the obstacle, if it has passed, it means P c+1 Cannot be deleted, P n Jump to P c+1 ; Otherwise delete P c+1 , and P c Set to P n The parent node of P c Jump to its parent node P c-1 ;
[0135] Step 4.5: Repeat step 3.4 until P n =P1 or P c =P1;
[0136] Step 4.6: Connect the remaining nodes by their parent node pointers.
[0137] The improved DDPG algorithm introduces a value classification experience replay pool and a stratified sampling strategy, and designs a continuous reward function that integrates target distance, safety, motion, and number of steps.
[0138] (1) Value Classification Experience Replay Pool
[0139] The value classification experience replay pool consists of a sub-replay pool B1 for storing high-value samples, a sub-replay pool B2 for storing recent samples, and a sub-replay pool B3 for storing ordinary samples. Each sub-replay pool stores experience samples in the following five-tuple form:
[0140] (S t , a t , r t , S t+1 ,done) (5)
[0141] Formula (5) indicates that the agent is in state S t Execute action a t After reaching the new state S t+1 , and get the reward r given by the environment t , where done represents S t+1 Whether it is in the terminated state.
[0142] The specific steps for constructing the value classification experience replay pool are as follows:
[0143] During initialization, set the average value V of all sample values in the two sub-replay pools B1 and B3 m is 0, and the weight coefficient α of the reward value is set to 1. During the training process, an experience sample is generated at each time step. The generated sample is first added to the tail of the B2 queue. If B2 reaches the capacity limit, a sample is popped out from the head of the queue, and the value of the sample is calculated according to the sample value measurement standard. If it is higher than V m If yes, the sample is added to B1, otherwise it is stored in B3. After storage, update V m and α, and repeat the above process until the training is completed.
[0144] Taking the sample generated at time step t as an example, the sample value measurement standard is calculated as follows:
[0145] V t =α*|r t |+(1-α)*|δ t | (6)
[0146] δ t =r t +γQ′(S t+1 ,a t+1 θ Q′ )-Q(S t ,at θ Q ) (7)
[0147] In formula (6), V t is the value of the sample, |r t | is the absolute value of the sample reward value, α is |r t The weight coefficient of |δ t | is the absolute value of the sample time difference error, (1-α) is |δ t |weight;
[0148] In formula (7), γ is the reward value attenuation coefficient, Q′ is the target value network state action value, θ Q′ is the target value network parameter, Q is the online value network state action value, θ Q is the online value network parameter, S t is the state space at time step t, a t is the action value decided in this state space, S t+1 For the agent in S t Execute a t The new state reached after t+1 is the action value in the new state;
[0149] (2) Stratified sampling strategy
[0150] The stratified sampling strategy randomly extracts appropriate samples from the B1, B2, and B3 sub-replay pools for training the network. The total number of small batch samples is N, the number of samples from the sub-replay pool B1 is N1, the number of samples from the sub-replay pool B2 is N2, and the number of samples from the sub-replay pool B3 is N3.
[0151] N=N1+N2+N3 (8)
[0152] N2 is a fixed value, ensuring that each small batch sampling contains recent samples. Such samples are time-sensitive and help to adjust the network convergence direction in a timely manner. In the early stage of training, N1 takes a larger value to promote the network to converge in the right direction by selecting a large number of high-quality samples. As training progresses and the policy network matures, the robot performs better and better, and the frequency of high-value samples increases. To avoid overfitting, N1 is appropriately reduced and N3 is increased.
[0153] (3) Continuous Reward Function
[0154] The continuous reward function includes three parts: arrival reward, collision penalty, and process reward. As shown in formula (9), when the distance between the robot center and the target position is less than the arrival threshold, it is judged to have reached the target point and obtains a reward value of 300; when the minimum value of the laser radar point cloud data equipped by the robot is less than the collision threshold, it is judged to have collided and obtains a reward value of -500; the process reward r is integrated with the target distance, safety, movement, and step number. n Defined as formula (10):
[0155]
[0156] r n =r d +r s +r m +r p (10)
[0157] During navigation, the robot obtains the process reward r for executing actions n Divided into 4 parts: Distance Reward d , security penalty s , sports punishment m and step penalty r p .
[0158] r d It is the most important part of the reward in the navigation process, guiding the robot to approach the target point, and is defined as:
[0159] r d =k1*(D g (S t-1 )-D g (S t )) (11)
[0160] In formula (11), k1 is the distance reward coefficient. In this example, k1=20. g (S t-1 ) represents the distance between the robot and the target point in the previous state, D g (S t ) represents the distance between the robot and the target point in the current state.
[0161] r s Used to warn the robot to stay away from obstacles as much as possible and choose a safer path. It is defined as:
[0162]
[0163] In formula (12), k2 is the security penalty coefficient. In this example, k2=-1, D o is the distance between the robot and the nearest obstacle, d s is the safety distance threshold; sr The safety level of the performance path is defined as:
[0164]
[0165] r m Penalize the robot's frequent swings and simplify the robot's motion control, which is defined as:
[0166]
[0167] In formula (14), k3 is the motion penalty coefficient, and in this example, k3=-0.2; w t Indicates the angular velocity of the current state output, w t-1 Indicates the angular velocity of the previous state output.
[0168] In order to prevent the robot from blindly pursuing the maximum reward and deliberately not going directly to the goal, r4 is introduced to prevent the robot from blindly exploring and indirectly stimulate the robot to find a better path, which is defined as:
[0169] r p =k4 (15)
[0170] In formula (15), k4 is the step penalty coefficient, and in this example, k4=-0.05.
[0171] It should be noted that DDPG is a deep reinforcement learning algorithm, so the design of the value classification experience replay pool and the continuous reward function is to accelerate the training process and ensure that a high-quality obstacle avoidance strategy is finally learned. The training process can be carried out in a simulation environment, and after the training is completed, it can be migrated to the real environment by loading the network parameters.
[0172] 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 hierarchical path planning method for topology-grid-metric hybrid maps, characterized in that: Includes steps: Step 1: According to the topology-grid-metric hybrid map construction method, a hybrid map of the mobile robot's operating environment is created, including a topology map for abstract representation, a grid map for concrete representation, and a metric map for refined representation. The specific steps are as follows: Step 1.1: Rasterize the overall working environment and construct a global raster map; Step 1.2: Divide the global grid map into several sub-grid maps, set key nodes in each sub-grid map, and use the hierarchical graph to describe the working environment as a hierarchical map representation with sub-maps as tree nodes. In the k-th layer map, the k+1-th layer sub-map is represented by key nodes, and the key nodes at the same level are connected by offline prior paths; Step 1.3: A thinning algorithm is used on the grid map to generate an offline prior path with a high degree of security, and a topological map corresponding to each level of sub-grid map is constructed by a topological feature extraction method; the topological feature extraction method uses the bifurcation points of the offline prior path on the grid map as ordinary nodes in the topological map, manually selects the regional entrance grid or other representative grids as the key nodes of the sub-grid map, and counts the offline prior path lengths between the key nodes as the weight of the edge, and the path length is the number of grids contained in the path; Step 1.4: The mobile robot uses the onboard sensor information it carries to maintain and update the local metric map during operation; Step 2: Before the mobile robot runs, an overall optimized global preset path is generated according to the topology-grid hierarchical planning method, including the following steps: Step 2.1: Determine whether the starting grid S and the target grid D are in the same sub-map. If so, proceed to step 2.2; if not, proceed to step 2.3; Step 2.2: For the starting point S to the end point D, an improved A* algorithm is used to plan an overall optimized global preset path, and step 2 ends; Step 2.3: Get the depth L of the starting point S and the end point D in the hierarchical graph architecture respectively s , L D and the key node C of the sub-grid map where it is located S , C D , if L s <L D , then from C D Start by searching from bottom to top in the hierarchical graph architecture until L s-1 Find C in the topology diagram corresponding to the layer S ; Otherwise, from C S Start searching upwards until L D-1 Find C in the topology diagram corresponding to the layer D Stop; During the search process, the Floyd algorithm is used to plan the optimal node sequence on each level of topological map, and the offline prior paths between nodes are saved as the mid-segment preset paths; Step 2.4: Starting from S, key node C S As the end point, the improved A* algorithm is used to search for a local path on the sub-grid map where the starting point S is located and save it as the preset path for the previous section; the key node C D Set it as the starting point, and the end point D as the end point. Use the improved A* algorithm to search for another local path on the sub-grid map where the starting point S is located and save it as the preset path for the next section. If no feasible solution can be found, the path planning fails and step 2 ends. Step 2.5: merge the middle preset path generated in step 2.3 with the front preset path and the back preset path searched in step 2.4 to obtain the overall optimized global preset path, and step 2 ends; Step 3: When the mobile robot runs along the global preset path, it detects the surrounding environment in real time to determine whether the distance to the nearest obstacle to the robot is less than the obstacle avoidance threshold. If so, it proceeds to step 4; if not, it proceeds to step 5; Step 4: Use the improved Deep Deterministic Policy Gradient (DDPG) algorithm to perform local path planning on the metric map: first build a value classification experience replay pool, and store high-value samples, ordinary samples, and recent samples in three sample sub-replay pools of the value classification experience replay pool respectively; then use a stratified sampling strategy to randomly extract an appropriate amount of samples from the three sample sub-replay pools for the DDPG algorithm to perform learning and training on local path planning; and introduce a continuous reward function including arrival reward, collision penalty, and process reward to reward or penalize the local path planned by the DDPG algorithm, train the DDPG algorithm to plan a better local path, and return to the global preset path after completing obstacle avoidance through local path planning; Step 5: Determine whether the mobile robot has reached the target position. If so, the algorithm ends; if not, proceed to step 3.
2. The hierarchical path planning method for topology-grid-metric hybrid maps according to claim 1 is characterized in that: The improved A* algorithm introduces a bidirectional search mechanism into the standard A* algorithm, and performs forward search and reverse search from the starting point and the end point at the same time, including the following steps: Step 3.1: Initialize the Open1, Open2, Close1, Close2 lists, and add the starting point S to Open1 and the end point D to Open2; Step 3.2: Take out the nodes n1 and n2 with the smallest cost value from the Open1 and Open2 lists respectively, and move them to Close1 and Close2 respectively; Step 3.3: Determine whether n1 and n2 are the current search nodes in the relative search direction. If so, the algorithm ends and the paths generated in the forward and reverse directions are combined to form the final optimized path. If not, proceed to step 3.
4. Step 3.4: Forward search starts from S, ends at n2, and expands to the surrounding 8 neighborhoods with n1 as the center, and updates Open1 according to the rules for selecting the extension points; Step 3.5: The reverse search starts from D, ends at n1, and expands to the surrounding 8 neighborhoods with n2 as the center, and updates Open2 according to the rules for filtering the extension points; Step 3.6: Determine whether Open1 and Open2 are empty. If so, the algorithm ends and no valid path can be found. Otherwise, go to step 3.
2.
3. The hierarchical path planning method for topology-grid-metric hybrid maps according to claim 1 is characterized in that: The improved A* algorithm introduces a path redundant point elimination technique into the standard A* algorithm, which specifically includes the following steps: Step 4.1: Insert a new node at the point where the distance between each pair of adjacent nodes on the original path is halved; Step 4.2: Let the starting node be P1, the next node be P2, and so on until the end point P k , and each node sets its previous node as its parent node; Step 4.3: Let P n To optimize the benchmark node, P c is the node currently being explored, n is initialized to k, and c is initialized to k-2; Step 4.4: Determine P n With P c The connection line L nc Whether it passes through an obstacle. If it passes through an obstacle, it means P c+1 Cannot be deleted, P n Jump to P c+1 ; Otherwise delete P c+1 , and P c Set to P n The parent node of P c Jump to its parent node P c-1 ; Step 4.5: Repeat step 4.4 until P n =P1 or P c =P1; Step 4.6: Connect the remaining nodes by their parent node pointers.
4. The hierarchical path planning method for topology-grid-metric hybrid maps according to claim 1, characterized in that: The value classification experience replay pool consists of three queues, including a sub-replay pool B1 for storing high-value samples, a sub-replay pool B2 for storing recent samples, and a sub-replay pool B3 for storing ordinary samples. Each sub-replay pool stores experience samples in the following five-tuple form: (S t ,a t ,r t ,S t+1 ,done) (2) Formula (2) indicates that the agent is in state S t Execute action a t After reaching the new state S t+1 , and get the reward r given by the environment t , where done represents S t+1 Whether it is in the terminated state; The specific steps for constructing the value classification experience replay pool are as follows: During initialization, set the average value V of all sample values in the two sub-replay pools B1 and B3 m is 0, and the weight coefficient α of the reward value is set to 1; During the training process, an experience sample is generated at each time step. The generated sample is first added to the tail of the B2 queue. If B2 reaches the capacity limit, a sample is popped out from the head of the queue and the value of the sample is calculated according to the sample value measurement standard. If it is higher than V m, Then add the sample to B1, otherwise store it in B3; after storage, update V m and α, repeat the above process until the training is completed; Taking the sample generated at time step t as an example, the sample value measurement standard is calculated as follows: V t =α*|r t |+(1-a)*|d t | (3) δ t =r t +γQ′(S t+1 ,a t+1 ;θ Q′ )-Q(S t ,a t ;θ Q ) (4) In formula (3), V t is the value of the sample, |r t | is the absolute value of the sample reward value, α is |r t The weight coefficient of |δ t | is the absolute value of the sample time difference error, (1-α) is |δ t |weight; In formula (4), γ is the reward value attenuation coefficient, Q′ is the target value network state action value, θ Q′ is the target value network parameter, Q is the online value network state action value, θ Q is the online value network parameter, S t is the state space at time step t, a t is the action value decided in this state space, S t+1 For the agent in S t Execute a t The new state reached after t+1 is the action value in the new state; The stratified sampling strategy randomly extracts appropriate amount of samples from the B1, B2 and B3 sub-replay pools for training the network.
5. The hierarchical path planning method for topology-grid-metric hybrid maps according to claim 1, characterized in that: The continuous reward function includes the arrival reward r a , collision penalty r c , process reward n Three parts, reward function r t Defined as: During navigation, the robot obtains the process reward r for executing actions n Divided into 4 parts: Distance Reward d , safety penalty s , sports punishment m and step penalty r p , as shown in formula (6): r n =r d +r s +r m +r p (6) In formula (6), the distance reward r d for r d =k1*(D g (S t-1 )-D g (S t )) (7) Among them, k1 is the distance reward coefficient, D g (S t-1 ) represents the distance between the robot and the target point in the previous state, D g (S t ) represents the distance between the robot and the target point in the current state; In formula (6), the security penalty r s for: Among them, k2 is the security penalty coefficient, D o Indicates the distance between the robot and the nearest obstacle around it, d s is the safety distance threshold; s r The safety level of the performance path is defined as follows: In formula (6), the motion penalty r m for: Among them, k3 is the motion penalty coefficient; w t Indicates the angular velocity of the current state output, w t-1 Indicates the angular velocity of the previous state output; In formula (6), the step penalty r p for: r p =k4 (11) Among them, k4 is the step penalty coefficient.
Citation Information
Patent Citations
Path planning method based on improved A* algorithm
CN113359768A
