Rapid edge searching method based on grid map

Abstract

The invention relates to a rapid edge search method based on a grid map, which comprises the following steps: acquiring starting point information and terminal point information of a path search task, and determining a starting point grid unit and a terminal point grid unit according to grid map data; the grid map data comprises a grid map, obstacle grids and passable grids; initializing a path search data structure by using the starting point grid unit to obtain an open list containing the starting point grid unit; searching a passable path from the starting point grid unit to the end point grid unit along the edge based on a greedy strategy, and updating the open list to obtain the open list containing a passable grid unit sequence; and obtaining a current optimal node grid unit of the open list, and when the current optimal node grid unit is the end point grid unit, generating an optimal path according to the open list. By adopting the method, the path from the starting point to the ending point can be quickly obtained, unnecessary search areas are reduced, and the search rate is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of path search, and in particular relates to a fast edge search method based on a grid map. Background Art

[0002] With the development of intelligent technologies such as robots, autonomous driving, and drones, path planning technology has emerged, which usually uses algorithms to search for feasible paths from the starting point to the end point in the map space.

[0003] Among many algorithms, the A algorithm uses a heuristic method to diffuse the search outward, combining the actual cost from the starting point to the current point with the estimated cost from the current point to the end point, so as to obtain the path from the starting point to the end point. This algorithm has the characteristic of being able to find the optimal solution in theory and performs well in a simple map environment. The JPS algorithm is improved based on the A algorithm. It only searches for jump points and improves the search efficiency by reducing the number of blank points in the search. It can quickly find the path in specific scenarios. The RRT* algorithm is a sampling-based search method. It diffuses outward to search for paths by generating random trees. It has the characteristic of being able to quickly explore in complex shape spaces.

[0004] However, among the above algorithms, algorithm A has a very slow search efficiency in large and complex environments because it needs to evaluate and expand a large number of nodes, and it is difficult to meet tasks with high real-time requirements, such as the rapid navigation of service robots in complex indoor environments. JPS also takes a long time to search in empty maps and cannot be applied to flexible scenarios such as topological maps. In addition, the recursive scheme in the implementation of this algorithm may cause unknown problems on large maps, affecting its stability and reliability. Since the RRT* algorithm is based on random sampling points, its search efficiency is very unstable, and it is difficult to obtain a feasible path for large and complex environments. Summary of the invention

[0005] Based on this, it is necessary to provide a fast edge search method based on a grid map that can efficiently generate an optimized performance path to address the above technical problems.

[0006] In a first aspect, the present application provides a fast edge search method based on a grid map, comprising:

[0007] Obtain the starting point information and the end point information of the path search task, and determine the starting point grid unit and the end point grid unit according to the grid map data; the grid map data includes the grid map, the obstacle grid and the passable grid;

[0008] Initializing a path search data structure using the starting grid cell to obtain an open list including the starting grid cell;

[0009] Based on the greedy strategy, a passable path from the starting grid cell to the end grid cell is searched along the edge, and the open list is updated to obtain an open list containing a passable grid cell sequence;

[0010] The current optimal node grid cell of the open list is obtained. When the current optimal node grid cell is the end grid cell, the optimal path is generated according to the open list.

[0011] In one embodiment, based on a greedy strategy, a traversable path from a starting grid cell to an end grid cell is searched along an edge, and an open list is updated to obtain an open list containing a traversable grid cell sequence, including:

[0012] Each time an updated open list is obtained, the node with the lowest cost is obtained from the open list as the current optimal node, and the current optimal node grid cell is obtained; the updated open list indicates that the content of the grid cell of the open list has changed;

[0013] Each time the current optimal node grid cell is obtained, a straight line extension direction from the current optimal node grid cell to the end point grid cell is calculated; the straight line extension direction includes multiple candidate grid cells;

[0014] According to the grid map data, it is determined whether there is an obstacle grid in the adjacent grid cells of the candidate grid cell, and the obstacle status result is obtained;

[0015] According to the obstacle status result, the open list is updated in a preset obstacle search mode along the edge to obtain an open list containing a passable grid unit sequence.

[0016] In one embodiment, updating the open list according to the obstacle status result in a preset obstacle search along the edge manner includes:

[0017] If the obstacle status result is that all adjacent grid cells are passable grids, determine that the obstacle search method along the edge is linear expansion, and update the open list according to the linear expansion result;

[0018] The straight line is extended to put the candidate grid cells into the open list to obtain an updated open list;

[0019] If the obstacle status result is that there is an obstacle grid in the adjacent grid unit, the obstacle search mode along the edge is determined to be obstacle processing, and the open list is updated according to the obstacle processing result;

[0020] Obstacle handling is achieved through the following steps:

[0021] Obtain the adjacent grid cells of the current optimal node grid cell;

[0022] Based on the relative position of the connection line between the current optimal node grid cell and the end point grid cell, the adjacent grid cells are divided into left grid cells and right grid cells;

[0023] Put the left grid cell and the right grid cell that are passable grids into the left grid cell open list and the right grid cell open list respectively;

[0024] The obstacle starting angle includes the obstacle starting angle of the left grid unit and the obstacle starting angle of the right grid unit;

[0025] The current optimal node with the minimum cost is obtained from the open list of the left grid unit as the current optimal node grid unit on the left, and the left edge search is performed based on the starting angle of the obstacle of the left grid unit, and the open list is updated;

[0026] The current optimal node of the node with the minimum cost is obtained from the open list of the right grid unit as the current optimal node grid unit on the right. The right side edge search is performed based on the starting angle of the obstacle of the right grid unit, and the open list is updated.

[0027] In one embodiment, performing a left edge search based on a starting angle of an obstacle in a left grid cell and updating an open list includes:

[0028] Each time a new left current optimal node grid cell is obtained, the adjacent grid cells of the left current optimal node grid cell are obtained;

[0029] When there is an obstacle grid in the adjacent grid cell, the left edge search includes:

[0030] Obtain the obstacle angle of the left grid cell according to the connection angle between the current optimal node grid cell on the left and the end grid cell;

[0031] If the left grid cell obstacle angle is less than the left grid cell obstacle start angle, perform the following steps:

[0032] Assign the starting angle of the left grid unit obstacle to the left grid unit obstacle angle;

[0033] Get the first adjacent grid cell of the current optimal node grid cell on the left side in the direction of the end grid cell;

[0034] When the first adjacent grid cell is a passable grid, update the open list;

[0035] When the first adjacent grid cell is an obstacle grid, the node with the minimum cost is obtained from the open list of the left grid cells again as the current optimal node grid cell on the left;

[0036] If the obstacle angle of the left grid unit is greater than the starting angle of the obstacle of the left grid unit, the node with the minimum cost is directly obtained from the open list of the left grid unit as the current optimal node grid unit on the left.

[0037] In one embodiment, performing a right side edge search based on a starting angle of an obstacle in a right grid cell and updating an open list includes:

[0038] Each time a new right-side current optimal node grid cell is obtained, the adjacent grid cells of the right-side current optimal node grid cell are obtained;

[0039] If there is an obstacle grid in the adjacent grid cell, the right side edge search includes:

[0040] Obtain the obstacle angle of the right grid unit according to the line angle between the current optimal node grid unit and the end grid unit on the right;

[0041] If the right grid cell obstacle angle is greater than the right grid cell obstacle start angle, perform the following steps:

[0042] Assign the starting angle of the right grid unit obstacle to the right grid unit obstacle angle;

[0043] Get the second adjacent grid cell of the current optimal node grid cell on the right side in the direction of the end grid cell;

[0044] If the second adjacent grid cell is a passable grid, update the open list;

[0045] If the second adjacent grid cell is an obstacle grid, the node with the minimum cost is obtained from the open list of the right grid cells as the current optimal node grid cell on the right;

[0046] If the obstacle angle of the right grid unit is less than the starting angle of the obstacle of the right grid unit, the node with the minimum cost is directly obtained from the open list of the right grid unit as the current optimal node grid unit on the right.

[0047] In one embodiment, the method further comprises:

[0048] If the left current optimal node grid cell obtained from the left grid cell open list and the right current optimal node grid cell obtained from the right grid cell open list are the same grid cell, the path search task is directly exited and a no-path result is generated.

[0049] In one embodiment, when the current optimal node grid cell is the end grid cell, generating the optimal path according to the open list includes:

[0050] When the current optimal node grid cell obtained from the open list is the end grid cell, obtaining an open list containing a passable grid cell sequence;

[0051] The traversable grid cell sequence is sorted according to the reverse path from the end grid cell to the start grid cell to obtain a preliminary path;

[0052] Reconstruct the preliminary path using visibility detection to obtain the optimal path;

[0053] Output the optimal path for the path search task.

[0054] In one embodiment, the raster map data is obtained by:

[0055] Obtaining application environment map data, the application environment map data including obstacle information, traversable areas and terrain features;

[0056] rasterizing the application environment map data to obtain a raster map;

[0057] Mark obstacle grids on the grid map according to obstacle information;

[0058] Marking traversable grids on the grid map according to traversable areas;

[0059] The grid map, obstacle grid and passable grid are combined to obtain the grid map data.

[0060] In a second aspect, the present application also provides a fast edge search device based on a grid map, comprising:

[0061] An information acquisition module is used to obtain the starting point information and the end point information of the path search task;

[0062] A grid map module, used to determine a starting grid unit and an end grid unit according to grid map data;

[0063] An initialization module, used for initializing a path search data structure using a starting grid cell, and obtaining an open list including the starting grid cell;

[0064] A path search data module is used to search for a passable path from a starting grid cell to an end grid cell along an edge based on a greedy strategy, and to update an open list to obtain an open list containing a sequence of passable grid cells;

[0065] The path generation module is used to generate an optimal path according to an open list when the current optimal node grid cell is the end grid cell.

[0066] In a third aspect, the present application also provides a computer device including a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, it implements the steps of any of the above-mentioned fast edge search methods based on a raster map.

[0067] The above-mentioned fast edge search method based on raster map applies the fast edge search algorithm to the raster map, makes full use of the state information of each grid cell on the raster map, namely the obstacle grid and the passable grid to construct the search node, searches along the edge based on the greedy strategy, expands in a straight line when no obstacle is encountered, and expands along both sides when an obstacle is encountered, thereby reducing unnecessary search areas and improving the search rate. A path is generated by backtracking the parent node information of the node in the open list. The path is generated based on the nodes selected by the greedy strategy, which ensures the optimization of the path to a certain extent and improves the path quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the related technologies, the drawings required for use in the embodiments or the related technical descriptions are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0069] Figure 1 A schematic diagram of a process flow of a fast edge search method based on a grid map of the present invention;

[0070] Figure 2 is a schematic diagram of the step-by-step flow chart of step S103;

[0071] Figure 3 is a schematic diagram of the step-by-step flow chart of step S104;

[0072] Figure 4 It is a schematic diagram of the process flow of left edge search of the present invention;

[0073] Figure 5 A schematic diagram of a flow chart of right edge search of the present invention;

[0074] Figure 6 It is a structural diagram of the composition of the fast edge search device based on the grid map of the present invention. DETAILED DESCRIPTION

[0075] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0076] In one embodiment, Figure 1As shown, a fast edge search method based on a grid map is provided. This embodiment uses the method applied to a terminal as an example. It can be understood that the method can also be applied to a server, and can also be applied to a system including a terminal and a server, and is implemented through the interaction between the terminal and the server. In this embodiment, the method includes the following steps:

[0077] S101, obtaining the starting point information and the ending point information of the path search task, and determining the starting point grid unit and the ending point grid unit according to the grid map data, wherein the grid map data includes a grid map, an obstacle grid and a passable grid.

[0078] Schematically, the grid map divides the actual environment space into grid units of the same size, and each grid unit can represent different states. The obstacle grid indicates that there are obstacles at the location, such as walls, furniture, etc. The passable grid indicates that there are no obstacles at the location and it is passable. The grid map data can be represented by two-dimensional coordinates and is the basic data structure for path search. The starting point information and the end point information can be derived from user input, sensor data, etc. For example, in the robot navigation scenario, the starting point may be the current location of the robot, which is provided by the positioning sensor carried by the robot itself. The starting point information and the end point information can be the position coordinates of the original map in the path search task. The grid units where the starting point and the end point are located are determined according to the grid map mapping. Furthermore, the starting point grid unit and the end point grid unit are both passable grids, and the starting point grid unit and the end point grid unit are not in the same grid unit.

[0079] S102: Initialize a path search data structure using the starting grid cell to obtain an open list including the starting grid cell.

[0080] In the path search algorithm, data structure is an important tool for storing and managing relevant information during the search process. Open List is a commonly used data structure used to store nodes to be explored, i.e. grid cells. Through the initialization operation, the starting grid cell is placed in the open list as the starting point of the path search. The subsequent algorithm will expand the search outward from this starting point and gradually find the path to the end point. Furthermore, after initializing the open list, in order to maintain the stability of the algorithm, it is necessary to check whether the open list is not empty to avoid the failure of the starting grid cell to be successfully placed in the open list due to insufficient memory, data structure initialization failure, custom logic interference, etc. If the open list is empty, that is, the open list does not contain the starting grid cell, an error should be reported and the path search task should be exited to avoid the subsequent algorithm from continuing to execute and producing unexpected results.

[0081] S103 , searching for a passable path from the starting grid cell to the end grid cell along the edge based on a greedy strategy, and updating an open list to obtain an open list containing a passable grid cell sequence.

[0082] The greedy strategy will give priority to exploring nodes closer to the end point each time it expands a node. It usually evaluates whether a node is the best node by calculating a certain distance measure from the node to the end point, and selects the node with the smallest distance as the next node to be explored. When an obstacle is encountered during the search process, since the obstacle will block the path directly to the end point, the algorithm will search along the edge of the obstacle to find a node that can bypass the obstacle and continue to move toward the end point. Specifically, when searching along the edge, the traversable grid cells adjacent to the obstacle will be added to the search range, and the appropriate node will be selected according to the greedy strategy to continue exploring. During the search process, whenever a new traversable grid cell is found, it will be added to the open list. At the same time, in order to ensure the efficiency of the search, the open list will be sorted according to the evaluation value of the node, such as the distance from the node to the end point, so that each node taken out of the open list is the node that currently appears to be the best. When a node is explored, it will be removed from the open list.

[0083] S104: Obtain the current optimal node grid unit of the open list. When the current optimal node grid unit is the end point grid unit, generate the optimal path according to the open list.

[0084] The algorithm will evaluate the nodes in the open list according to the set rules to find the node that currently appears to be most likely to lead to the optimal path, that is, the current optimal node grid cell. When this current optimal node grid cell happens to be the end point grid cell, it means that a path from the starting point to the end point has been found.

[0085] In one embodiment, if Figure 2 As shown, based on the greedy strategy, a passable path from the starting grid cell to the end grid cell is searched along the edge, and the open list is updated to obtain an open list containing a passable grid cell sequence, including:

[0086] S201. Each time an updated open list is obtained, a node with the minimum cost is obtained from the open list as the current optimal node, and a current optimal node grid cell is obtained. The updated open list indicates that the grid cell content of the open list has changed.

[0087] During the path search process, as the search progresses, new traversable grid cells are constantly discovered. When new grid cells are added to the open list, or the status of some nodes in the open list changes, the open list is updated.

[0088] The cost is used to guide the search node to obtain the optimal path, avoiding the algorithm from blindly searching and arbitrarily expanding nodes, thereby increasing the search time and complexity. The node with the lowest cost indicates the best direction from the end grid unit. In order to select the current optimal node from the open list, a cost function needs to be defined for each node. Exemplarily, the cost function uses the Manhattan distance formula: S = |x1-x2| + |y1-y2|, where S represents the distance from the current node (x1, y1) to the end grid unit (x2, y2). When the open list is updated, the algorithm will traverse all nodes in the open list, calculate the cost of each node according to the cost function, and find the node with the lowest cost. The node with the lowest cost is determined as the current optimal node. Once the current optimal node is determined, the grid unit corresponding to the node can be determined, that is, the current optimal node grid unit. The current optimal node grid unit will become the starting point for the algorithm to explore the next step.

[0089] S202 , each time a current optimal node grid cell is obtained, a straight line extension direction from the current optimal node grid cell to the end point grid cell is calculated, where the straight line extension direction includes a plurality of candidate grid cells.

[0090] After the current optimal node grid cell is determined, the linear extension direction to the end grid cell is calculated to clarify the possible search area for the next step, thereby improving the search efficiency and avoiding blindly searching the entire map. For example, in a two-dimensional grid map, the direction vector can be determined by calculating the coordinate difference between the current optimal node grid cell and the end grid cell. Specifically, the coordinates of the current optimal node grid cell are (x1, y1), and the coordinates of the end grid cell are (x2, y2). Then the direction vector Normalize the direction vector to balance the step size and avoid biasing to the horizontal or vertical direction. Obtain continuous grid cells along the direction vector to determine multiple candidate grid cells in the linear extension direction. Furthermore, since the grid coordinates are integers, the Bresenham (Bresenham line algorithm) or DDA (Digital Differential Analyzer, based on the principle of numerical differentiation) line algorithm can be used to further optimize the continuous grid cells of the direction vector and obtain integer grid coordinates as multiple candidate grid cells. For example, starting from the current optimal node (x1, y1), follow the direction vector The candidate grid cells (x1+Δx, y1+Δy), (x1+2Δx, y1+2Δy), etc. are obtained in turn.

[0091] S203: Determine whether there is an obstacle grid in the adjacent grid cells of the candidate grid cell according to the grid map data, and obtain an obstacle status result.

[0092] Schematically, in a grid map, adjacent grid cells can usually be considered as grids in the four neighborhoods of up, down, left, and right as adjacent grid cells of the current candidate grid cell. To determine whether there is an obstacle grid in the adjacent grid cell, the numerical representation of the corresponding grid cell in the grid map can be directly queried. For example, 0 represents a passable grid and 1 represents an obstacle grid. By traversing the adjacent grid cells of the candidate grid cell, it is determined whether it is an obstacle grid according to the value of the corresponding element in the grid map data. If there is an element with a value of 1, it means that there is an obstacle grid in the adjacent grid cell. If the values ​​of all adjacent grid cells are 0, it means that the adjacent grid cells are all passable grids.

[0093] S204: Update the open list according to the obstacle state result in a preset obstacle search along the edge manner to obtain an open list containing a passable grid unit sequence.

[0094] When the obstacle status result shows that all adjacent grid cells are passable grids, it means that the current direction can be passed smoothly. The preset search method is usually linear expansion, that is, the candidate grid cells are directly placed in the open list. When the obstacle status result shows that there are obstacle grids in the adjacent grid cells, it is necessary to use the edge search method, that is, to search for passable grids around both sides of the obstacle to update the open list.

[0095] In one embodiment, updating the open list according to the obstacle status result in a preset obstacle search along the edge manner includes:

[0096] S310: If the obstacle status result is that all adjacent grid cells are passable grids, determine that the obstacle search method along the edge is linear expansion, and update the open list according to the linear expansion result.

[0097] The straight line is extended to put the candidate grid cells into the open list to obtain an updated open list.

[0098] When it is determined that all adjacent grid cells are passable grids, it means that there are no obstacles in the current search direction that hinder the extension of the path. The corresponding candidate grid cells are placed in the open list, that is, the new possible path nodes are included in the subsequent search range, and better nodes are selected in the subsequent steps for further exploration.

[0099] S320: If the obstacle status result is that there is an obstacle grid in the adjacent grid unit, determine that the obstacle search mode along the edge is obstacle processing, and update the open list according to the obstacle processing result.

[0100] When an obstacle grid is detected in the adjacent grid cells of the candidate grid cell, it means that the path cannot be extended directly along a straight line, so obstacle handling along the edge of the obstacle is required. Obstacle handling explores both sides of the obstacle to find a path to bypass the obstacle and finally reach the end point.

[0101] Obstacle handling is achieved through the following steps:

[0102] S321. Obtain adjacent grid cells of the current optimal node grid cell.

[0103] Specifically, the two-dimensional coordinate data of adjacent grid cells is obtained.

[0104] S322 . Based on the relative position of the line connecting the current optimal node grid cell and the end point grid cell, divide the adjacent grid cells into left grid cells and right grid cells.

[0105] Determine the coordinates of the current optimal node grid cell and the end point grid cell, connect them to form a straight line, and divide the adjacent grid cells into left grid cells and right grid cells based on the relative position relationship of the straight line. Exemplarily, a vector cross multiplication operation is used, the coordinates of the current optimal node grid cell are (x1, y1), the coordinates of the end point grid cell are (x2, y2), and the coordinates of the adjacent grid cell are (x0, y0). The relative position is determined by calculating the value of (x2-x1)(y0-y1)-(y2-y1)(x0-x1). If the result is greater than 0, the adjacent grid cell is on the left side of the connecting line and is classified as a left grid cell. If the result is less than 0, it is on the right side of the connecting line and is classified as a right grid cell. In particular, the result equal to 0 is classified as a left grid cell.

[0106] S323, putting the left grid cell and the right grid cell that are passable grids into the left grid cell open list and the right grid cell open list respectively.

[0107] The passable left and right grid cells are placed in the corresponding open lists to manage and explore the passable areas on both sides of the obstacle in a more orderly manner. The algorithm can search and evaluate the areas on the left and right sides independently to avoid confusion and improve search efficiency. The subsequent algorithm will select the node with the lowest cost from these open lists as the current optimal node and continue to search along the edge.

[0108] S324. Obtain an obstacle starting angle according to a line angle between the current optimal node grid unit and the end point grid unit, where the obstacle starting angle includes an obstacle starting angle of a left grid unit and an obstacle starting angle of a right grid unit.

[0109] Based on the line between the current optimal node grid unit and the end grid unit, the angle of view of the obstacle formed by the current optimal node grid unit as the viewpoint is determined, wherein the angle of view is the angle formed by two rays tangent to the edge of the obstacle starting from the current optimal node grid unit. According to the uniqueness theorem of angle of view, the angle of view is unique, and with the continuous expansion of the edge of the obstacle, the angle of view also changes continuously. Further, the tangent point position corresponding to the maximum value or critical value reached during the expansion of the angle of view is the ideal entry point to bypass the obstacle, and the angle formed by its tangent line is used as the starting angle of the obstacle. Specifically, the reference angle formed by the continuous connection of the current optimal node grid unit and the end grid unit is calculated, and the rays tangent to the obstacle around the current optimal node are tracked until the critical change point of the angle of view is detected, thereby determining the starting angle of the obstacle, and assigning it to the obstacle starting angle of the left grid unit and the obstacle starting angle of the right grid unit as the basis for subsequent judgment of whether to get out of the obstacle and resume linear expansion. For example, the current optimal node grid unit is A, there is an obstacle B at a distance l from point A, and the distance that obstacle B expands to both sides is dx, then the tangent length is According to the inverse trigonometric function, the viewing angle is the starting angle of the obstacle. In particular, in the process of searching for the maximum viewing angle, if the search point is within the viewing angle, that is, the point is located in the area sandwiched by two rays tangent to the obstacle emitted from the search point, since the tangent point is a point on the boundary of the viewing angle, the point within the viewing angle cannot be the tangent point of the line of sight, and there is no need to check whether it is a tangent point, thereby reducing unnecessary calculation and checking steps.

[0110] S325, obtaining the current optimal node of the node with the minimum cost from the open list of the left grid unit as the current optimal node grid unit on the left, performing a left side edge search based on the starting angle of the obstacle of the left grid unit, and updating the open list.

[0111] In the left side search process, the algorithm will explore the adjacent grid cells of the current optimal node grid cell on the left side, based on the starting angle of the obstacle of the left grid cell. Indicatively, in the process of bypassing the obstacle, it will gradually approach the obstacle, which is equivalent to the obstacle expanding, and the viewing angle is also expanding. Based on this, the viewing angle expansion critical value, that is, the maximum viewing angle, is calculated to pass through the tangent point of the obstacle edge, and continue to search from the tangent position to the end grid cell. Further, each time the current optimal node grid cell on the left is obtained, the new connection angle between the current node and the end point is calculated and compared with the starting angle of the obstacle of the left grid cell. If the new angle is less than the starting angle, it means that the search along the current direction is closer to the ideal tangent point, the starting angle is updated and the new adjacent grid cells are explored along the current direction to decide whether to update the open list to restore the linear expansion; if the new angle is greater than the starting angle, it is necessary to re-select other nodes with the lowest cost from the open list of the left grid cell and try different search directions.

[0112] S326, obtaining the current optimal node of the node with the minimum cost from the right grid unit open list as the right current optimal node grid unit, performing a right side edge search based on the starting angle of the obstacle of the right grid unit, and updating the open list.

[0113] Similarly, in the right edge exploration process, the process is basically the same as the left edge exploration. Based on the left-right symmetry relationship, the judgment logic is opposite. When new passable grid cells are found, they will be added to the open list. The left edge search and the right edge search are performed in parallel, which can reduce the search time.

[0114] In one embodiment, if Figure 3 As shown, based on the starting angle of the obstacle in the left grid cell, the left edge search is performed and the open list is updated, including:

[0115] S301 . Each time a new left-side current optimal node grid cell is obtained, an adjacent grid cell of the left-side current optimal node grid cell is obtained.

[0116] S302: When there is an obstacle grid in the adjacent grid unit, the left side edge search includes:

[0117] S303 , obtaining the obstacle angle of the left grid unit according to the connection angle between the current optimal node grid unit on the left and the end point grid unit.

[0118] S304: If the left grid unit obstacle angle is smaller than the left grid unit obstacle starting angle, perform the following steps:

[0119] S305: assign the starting angle of the left grid unit obstacle to the left grid unit obstacle angle.

[0120] S306 , obtaining the first adjacent grid cell in the direction from the current optimal node grid cell on the left to the end grid cell.

[0121] S307: When the first adjacent grid unit is a passable grid, update the open list.

[0122] S308: When the first adjacent grid cell is an obstacle grid, the node with the minimum cost is obtained from the open list of the left grid cells again as the current optimal node grid cell on the left.

[0123] If the left grid unit obstacle angle is less than the left grid unit obstacle starting angle, it means that the current search direction is more biased towards the best obstacle tangent point, which is a better direction. At this time, update the left grid unit obstacle starting angle, and continue to explore with this angle as the new reference direction. Get the first adjacent grid unit of the current optimal node grid unit on the left in the direction of the end point, and perform different processing according to its traffic conditions: if it is passable, add it to the open list, exit the left edge search, repeat the straight line direction expansion to continue the search; if it is an obstacle, re-select the node with the lowest cost from the open list and try other directions.

[0124] S309: If the obstacle angle of the left grid unit is greater than the starting angle of the obstacle of the left grid unit, directly re-acquire the node with the minimum cost from the open list of the left grid unit as the current optimal node grid unit on the left.

[0125] If the obstacle angle of the left grid cell is greater than the starting angle of the obstacle of the left grid cell, it means that the current search direction has deviated from the optimal obstacle tangent point, and it is necessary to reselect the node with the lowest cost from the open list of the left grid cell and change the search direction.

[0126] In one embodiment, if Figure 4 As shown, based on the starting angle of the obstacle in the right grid cell, the right edge search is performed and the open list is updated, including:

[0127] S401, each time a new right-side current optimal node grid cell is obtained, the adjacent grid cells of the right-side current optimal node grid cell are obtained;

[0128] If there is an obstacle grid in the adjacent grid cell, the right side edge search includes:

[0129] S402, obtaining the obstacle angle of the right grid unit according to the connection angle between the current optimal node grid unit on the right and the end grid unit;

[0130] S403: If the right grid unit obstacle angle is greater than the right grid unit obstacle starting angle, perform the following steps:

[0131] S404, assigning the right grid unit obstacle starting angle to the right grid unit obstacle angle;

[0132] S405, obtaining the second adjacent grid cell of the current optimal node grid cell on the right side in the direction toward the end grid cell;

[0133] S406, if the second adjacent grid unit is a passable grid, update the open list;

[0134] S407, if the second adjacent grid cell is an obstacle grid, reacquire the node with the minimum cost from the right grid cell open list as the right current optimal node grid cell;

[0135] S408: If the right grid unit obstacle angle is smaller than the right grid unit obstacle starting angle, directly reacquire the node with the minimum cost from the right grid unit open list as the right current optimal node grid unit.

[0136] Similarly, if the second adjacent grid cell is a passable grid, the second adjacent grid cell is put into the open list, the right edge search loop is exited, and the operation after the open list receives the update is followed.

[0137] In one embodiment, the method further comprises:

[0138] If the left current optimal node grid cell obtained from the left grid cell open list and the right current optimal node grid cell obtained from the right grid cell open list are the same grid cell, the path search task is directly exited and a no-path result is generated.

[0139] For example, from the perspective of spatial layout, when the current optimal node grid cells on the left and right sides are the same grid cell, it means that the traversable area around the obstacle has been fully explored, and there is no path that can bypass the obstacle and continue to the end point. On the contrary, if there is a feasible path, the search on the left and right sides should be in different directions to bypass the obstacle, and will not converge at the same node halfway. When this happens, it can be reasonably inferred that under the current map environment and search strategy, there is no feasible path from the starting point to the end point.

[0140] In one embodiment, if Figure 5 As shown, when the current optimal node grid cell is the end grid cell, the optimal path is generated according to the open list, including:

[0141] S501: When the current optimal node grid cell obtained from the open list is the end grid cell, obtain an open list containing a passable grid cell sequence.

[0142] When the current optimal node grid cell obtained from the open list is the same as the end point grid cell, it means that a possible path from the start point to the end point has been found.

[0143] S502 , sorting a sequence of passable grid cells according to a reverse path from the end grid cell to the start grid cell to obtain a preliminary path.

[0144] During the search process, nodes are added to the open list in a certain order, and each node records its parent node information. The sequence of passable grid cells is sorted according to the reverse path from the end grid cell to the starting grid cell, which is backtracked using the parent-child relationship of the node. Starting from the end point, by continuously tracing the parent node until the starting point, the path obtained is the order from the end point to the starting point. After reversing it, the preliminary path from the starting point to the end point is obtained. This preliminary path is generated based on the nodes passed during the search process, but there may be some circuitous or unnecessary nodes.

[0145] S503: Reconstruct the preliminary path using visibility detection to obtain an optimal path.

[0146] The purpose of reconstructing the preliminary path using visibility detection is to optimize the path and make it shorter and more reasonable. Visibility detection is to check whether the adjacent nodes on the path are visible, that is, there are no obstacles between the two points. If two adjacent nodes are visible, then the intermediate node between them can be removed, thereby shortening the path. By performing such detection and processing on the entire preliminary path, unnecessary turns and detours can be removed to obtain a better path, that is, the optimal path.

[0147] S504: Output the optimal path of the path search task.

[0148] Output the optimal path of the path search task, and provide the shortest and most reasonable path from the starting point to the end point to the user or subsequent application program to complete the goal of the path search task.

[0149] In one embodiment, the raster map data is obtained by:

[0150] S61. Acquire application environment map data, where the application environment map data includes obstacle information, traversable areas, and terrain features.

[0151] By applying the algorithm to service robots, sweeping robots, autonomous driving, drones, etc. and the corresponding actual application environment maps, application environment map data is obtained.

[0152] S62: rasterize the application environment map data to obtain a raster map.

[0153] Rasterizing the application environment map data is to discretize the continuous map space into regular grid units. Through rasterization, complex map information is simplified into discrete and easy-to-manage units, providing a basic data structure for subsequent path search algorithms.

[0154] S63: Mark obstacle grids on the grid map according to the obstacle information.

[0155] According to the acquired obstacle information, the corresponding obstacle grid is marked on the grid map. For example, if the obstacle information indicates that a certain area is a building, then on the grid map, the grid unit corresponding to the area will be marked as an obstacle grid.

[0156] S64. Mark the passable grids on the grid map according to the passable areas.

[0157] The traversable grid defines the area that can be explored by the path search algorithm, which searches for a path from the start point to the end point within these grid cells.

[0158] S65: merging the grid map, the obstacle grid and the passable grid to obtain grid map data.

[0159] The raster map data contains all the map-related information required for path search. The subsequent path search algorithm will determine the starting point and end point, search for the path, and optimize the path based on this data.

[0160] Path search based on raster map data is highly adaptable to map types and environments. Whether it is a uniform raster map or a map with complex terrain and obstacle distribution, it can effectively search for paths and has stronger versatility, which can meet the needs of more practical application scenarios, such as path planning for autonomous driving in complex road topology structures.

[0161] The fast edge search method based on the grid map of the present application searches along the edge based on the greedy strategy, expands in a straight line when no obstacle is encountered, and expands along both sides when an obstacle is encountered, thereby reducing unnecessary search areas. As shown in Table 1, Obs (Obstacle) is an obstacle, and RES is the fast edge search algorithm based on the grid map of the present application. Taking the same 2000×2000 complex map as an example, the search of the present application only takes more than 20 milliseconds, which is about 200 times the search speed of the A* algorithm and about 20 times the search speed of the JPS algorithm. It greatly improves the search efficiency and can better meet the tasks with high real-time requirements, such as the rapid navigation of service robots in complex indoor environments.

[0162] Table 1. Comparison of average time consumption (ms) of each algorithm in 1000 experiments on a 2000×2000 grid map

[0163] Obs: 1 Obs: 20 Obs: 40 Obs: 60 Obs: 80 A* 53.015 1668.81 2413.31 2878.89 3228.55 LazyTheta* 22.122 3273.02 4143.96 4889.82 5650.74 JPS 301.257 366.012 418.904 443.426 470.514 RES 1.968 3.835 6.724 10.51 15.978 RES+Rebuild 83.2353 81.6347 104.289 112.647 126.419

[0164] To further illustrate the solution of the embodiments of the present application, a specific example is given below for explanation.

[0165] Step 1: Put the starting point into the open_list (open list) and start searching in the direction of the end point.

[0166] Step 2: If the open_list is empty, the search fails and the search exits. If it is not empty, take out the node with the minimum cost as the cur_node (current optimal node).

[0167] Step 3: If the node cur_node is the end point, exit; otherwise, continue to execute.

[0168] Step 4: If the cur_node has no obstacle neighbors, put the neighbor with the minimum cost into the open_list and return to Step 2; if the current node has obstacle neighbors, then put the neighbor node in the left area adjacent to the obstacle and on the line connecting the current node and the end point into the left_open_list (left grid cell), put the node in the right area adjacent to the obstacle into the right_open_list (right grid cell), and calculate the angle between the current node and the end point as theta (starting angle of the obstacle), and initialize theta_left (starting angle of the obstacle in the left grid cell) = theta, theta_right (starting angle of the obstacle in the right grid cell) = theta.

[0169] Step 5: Search along the left side of the obstacle. If the left_open_list is not empty, take out the node with the minimum straight-line distance to the end point from the left_open_list as the current node, calculate the angle between the current node and the end point as theta2 (angle of the obstacle in the left grid cell). If theta2 < theta_left, then record the minimum left angle theta_left = theta2, and determine whether there is an obstacle in the neighbor node facing the end point direction of the current point. If not, put the neighbor point into the open_list and exit the left loop; if so, continue to loop and take out nodes from the left_open_list for search until meeting the right direction.

[0170] Step 6: Search along the right side of the obstacle. If right_open_list is not empty, take the node with the shortest straight-line distance to the end point from right_open_list as the current node, calculate the angle between the current node and the end point as theta3 (the obstacle angle of the right grid unit), if theta3>theta_right, then record the maximum angle on the right side theta_right=theta3, and determine whether there is an obstacle in the neighboring node of the current point facing the end point. If not, put the neighboring node into open_list and exit the right loop; if so, continue to loop and take out nodes from right_open_list for searching until it meets the left direction;

[0171] If the left search node meets the right search node, it is considered that the end point is surrounded by obstacles and there is no feasible path, so the search is exited; otherwise, steps 1 to 4 are looped until the search reaches the end point or open_list is empty, and the search is exited.

[0172] Reconstruct the path. After searching for the path, post-processing can be performed through visibility judgment to reconstruct the path, and finally a shorter and relatively smoother path can be obtained.

[0173] It should be understood that, although the various steps in the flowcharts involved in the above-mentioned embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-mentioned embodiments can include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0174] Based on the same inventive concept, the embodiment of the present application also provides a fast edge search device based on a grid map for implementing the fast edge search method based on a grid map. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more embodiments of the fast edge search device based on a grid map provided below can refer to the limitations of the fast edge search method based on a grid map above, and will not be repeated here.

[0175] In an exemplary embodiment, Figure 6 As shown, a fast edge search device based on a grid map is provided, comprising:

[0176] An information acquisition module is used to obtain the starting point information and the end point information of the path search task;

[0177] A grid map module, used to determine a starting grid unit and an end grid unit according to grid map data;

[0178] An initialization module, used for initializing a path search data structure using a starting grid cell, and obtaining an open list including the starting grid cell;

[0179] A path search data module is used to search for a passable path from a starting grid cell to an end grid cell along an edge based on a greedy strategy and update an open list to obtain an open list containing a sequence of passable grid cells;

[0180] The path generation module is used to generate an optimal path according to an open list when the current optimal node grid cell is the end grid cell.

[0181] In one embodiment, the path search data module is further used to obtain the node with the lowest cost from the open list as the current optimal node each time the updated open list is obtained, and obtain the current optimal node grid unit, and each time the current optimal node grid unit is obtained, calculate the straight line extension direction from the current optimal node grid unit to the end point grid unit, and update the open list according to the obstacle state result according to the preset obstacle along the edge search method to obtain the open list containing the passable grid unit sequence;

[0182] The grid map module is also used to determine whether there is an obstacle grid in the adjacent grid cells of the candidate grid cell according to the grid map data, and obtain an obstacle status result.

[0183] In one of the embodiments, the path search data module is further used to obtain the obstacle starting angle according to the connection angle between the current optimal node grid unit and the end point grid unit.

[0184] In one embodiment, the grid map module determines the obstacle status of the first adjacent grid cell.

[0185] In one embodiment, the grid map module determines the obstacle status of the second adjacent grid cell.

[0186] In one embodiment, the path generation module is further used to sort the passable grid cell sequence according to the reverse path from the end grid cell to the start grid cell to obtain a preliminary path, and reconstruct the preliminary path using visibility detection to obtain an optimal path.

[0187] In one embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of a fast edge search method based on a grid map as described above when executing the computer program.

[0188] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps in the above-mentioned embodiments of the fast edge search method based on a grid map are implemented.

[0189] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can refer to the partial description of the method embodiments. The device embodiments described above are only schematic, wherein the components described as separate parts may or may not be physically separated, and the parts displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the disclosed solution. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0190] The above-mentioned embodiments only express several implementation methods of the embodiments of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the patent of the embodiments of the present application. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the embodiments of the present application, and these all belong to the protection scope of the embodiments of the present application.

Claims

1. A fast edge search method based on a grid map, characterized in that: The method comprises: Obtaining the starting point information and the end point information of the path search task, and determining the starting point grid unit and the end point grid unit according to the grid map data; the grid map data includes a grid map, an obstacle grid and a passable grid; Initializing a path search data structure using the starting grid cell to obtain an open list including the starting grid cell; Searching for a traversable path from the starting grid cell to the end grid cell along the edge based on a greedy strategy, and updating the open list to obtain the open list containing a traversable grid cell sequence; The current optimal node grid unit of the open list is obtained, and when the current optimal node grid unit is the end point grid unit, an optimal path is generated according to the open list.

2. The method according to claim 1, characterized in that The greedy strategy based edge search for a traversable path from the starting grid cell to the end grid cell and updating the open list to obtain the open list containing a traversable grid cell sequence includes: Each time the updated open list is obtained, the node with the minimum cost is obtained from the open list as the current optimal node, and the current optimal node grid unit is obtained; the updated open list indicates that the content of the grid unit of the open list has changed; Each time a current optimal node grid cell is obtained, a straight line extension direction from the current optimal node grid cell to the end point grid cell is calculated; the straight line extension direction includes a plurality of candidate grid cells; Determine whether the obstacle grid exists in the adjacent grid cells of the candidate grid cell according to the grid map data, and obtain an obstacle status result; The open list is updated according to the obstacle state result in a preset obstacle search along the edge manner to obtain the open list containing a passable grid unit sequence.

3. The method according to claim 2, characterized in that The updating of the open list according to the obstacle status result in a preset obstacle search along the edge manner includes: If the obstacle status result is that all the adjacent grid cells are the passable grids, determining that the obstacle search method along the edge is linear expansion, and updating the open list according to the linear expansion result; wherein the straight line expansion is to put the candidate grid cell into the open list to obtain an updated open list; If the obstacle state result is that the obstacle grid exists in the adjacent grid unit, determining that the obstacle search mode along the edge is obstacle processing, and updating the open list according to the obstacle processing result; The obstacle handling is achieved by the following steps: Acquire the adjacent grid cells of the current optimal node grid cell; Based on the relative position of the line connecting the current optimal node grid cell and the end point grid cell, the adjacent grid cells are divided into left grid cells and right grid cells; Put the left grid cell and the right grid cell, which are the passable grids, into a left grid cell open list and a right grid cell open list respectively; Obtaining an obstacle starting angle according to the connection angle between the current optimal node grid unit and the end point grid unit; the obstacle starting angle includes an obstacle starting angle of a left grid unit and an obstacle starting angle of a right grid unit; The current optimal node of the node with the minimum cost is obtained from the open list of the left grid unit as the current optimal node grid unit on the left, and a left side edge search is performed based on the starting angle of the obstacle of the left grid unit, and the open list is updated; The current optimal node of the node with the minimum cost is obtained from the right grid unit open list as the right current optimal node grid unit, and a right side edge search is performed based on the starting angle of the obstacle of the right grid unit to update the open list.

4. The method according to claim 3, characterized in that The step of performing a left side edge search based on the starting angle of the left grid unit obstacle and updating the open list comprises: Each time a new left-side current optimal node grid unit is obtained, the adjacent grid unit of the left-side current optimal node grid unit is obtained; When there is an obstacle grid in the adjacent grid unit, the left side edge search includes: Obtaining the obstacle angle of the left grid unit according to the connection angle between the left current optimal node grid unit and the end point grid unit; If the left grid unit obstacle angle is less than the left grid unit obstacle starting angle, perform the following steps: Assigning the left grid unit obstacle starting angle to the left grid unit obstacle angle; Acquire the first adjacent grid cell of the left current optimal node grid cell in the direction of the end grid cell; When the first adjacent grid unit is the passable grid, updating the open list; When the first adjacent grid unit is the obstacle grid, reacquire the node with the minimum cost from the open list of the left grid unit as the current optimal node grid unit on the left; If the left grid unit obstacle angle is greater than the left grid unit obstacle starting angle, the node with the minimum cost is directly obtained from the left grid unit open list again as the left current optimal node grid unit.

5. The method according to claim 3, characterized in that: The performing right side edge search based on the starting angle of the right grid unit obstacle and updating the open list comprises: Each time a new right-side current optimal node grid unit is obtained, the adjacent grid unit of the right-side current optimal node grid unit is obtained; If there is an obstacle grid in the adjacent grid unit, the right side edge search includes: Obtaining the obstacle angle of the right grid unit according to the connection angle between the right current optimal node grid unit and the end point grid unit; If the right grid unit obstacle angle is greater than the right grid unit obstacle starting angle, perform the following steps: Assigning the right grid unit obstacle starting angle to the right grid unit obstacle angle; Acquire the second adjacent grid cell of the right current optimal node grid cell in the direction of the end grid cell; If the second adjacent grid cell is the passable grid, updating the open list; If the second adjacent grid unit is the obstacle grid, reacquire the node with the minimum cost from the open list of the right grid unit as the current optimal node grid unit on the right; If the right grid unit obstacle angle is smaller than the right grid unit obstacle starting angle, the node with the minimum cost is directly obtained from the right grid unit open list again as the right current optimal node grid unit.

6. The method according to any one of claims 3 to 5, characterized in that: The method further comprises: If the left current optimal node grid cell obtained from the left grid cell open list and the right current optimal node grid cell obtained from the right grid cell open list are the same grid cell, the path search task is directly exited to generate a no-path result.

7. The method according to claim 1, characterized in that When the current optimal node grid unit is the end point grid unit, generating an optimal path according to the open list includes: When the current optimal node grid cell obtained from the open list is the end grid cell, obtaining the open list containing a passable grid cell sequence; Sort the passable grid cell sequence according to the reverse path from the end grid cell to the start grid cell to obtain a preliminary path; Reconstructing the preliminary path by using visibility detection to obtain an optimal path; Output the optimal path of the path search task.

8. The method according to claim 1, characterized in that The grid map data is obtained in the following way: Acquiring application environment map data, wherein the application environment map data includes obstacle information, traversable areas, and terrain features; Performing rasterization processing on the application environment map data to obtain a raster map; Marking an obstacle grid on the grid map according to the obstacle information; Marking a passable grid on the grid map according to the passable area; The grid map, the obstacle grid and the passable grid are combined to obtain the grid map data.

9. A fast edge search device based on a grid map, characterized in that: The device comprises: An information acquisition module is used to obtain the starting point information and the end point information of the path search task; A grid map module, used to determine a starting grid unit and an end grid unit according to grid map data; An initialization module, used for initializing a path search data structure using the starting grid cell to obtain an open list including the starting grid cell; A path search data module, used to search for a passable path from the starting grid cell to the end grid cell along the edge based on a greedy strategy, and update the open list to obtain the open list containing a passable grid cell sequence; The path generation module is used to generate an optimal path according to the open list when the current optimal node grid unit is the terminal grid unit.

10. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

Images