Path planning method based on multi-map fusion, terminal equipment and medium

Abstract

The invention relates to a path planning method based on multi-map fusion, terminal equipment and a medium, and the method comprises the steps: collecting a plurality of sub-maps of a to-be-researched region under different obstacle distribution conditions, carrying out the meshing of each sub-map, and marking whether there is an obstacle in each grid obtained through the meshing of each sub-map; performing weighted stacking on the grid values of all the sub-maps, and generating a global probability map based on the global obstacle probability of each grid obtained by weighted stacking; path planning of a to-be-researched area is carried out through an improved Theta * algorithm, global obstacle probability and an obstacle probability threshold are introduced into application of a heuristic function and sight line detection, and a global path from a starting point to an ending point is obtained; and performing local correction on the global path according to the actual sub-map under the actual obstacle distribution condition of the to-be-researched area to obtain a final path. According to the method, the robustness, the safety and the real-time performance of path planning are improved.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and particularly to a path planning method, a terminal device, and a medium based on multi-map fusion. Background Art

[0002] Path planning is a core technology for robot navigation, autonomous driving, and intelligent decision-making systems, aiming to find a safe and efficient moving path from a starting point to an ending point. Traditional path planning algorithms (such as A*, Dijkstra, etc.) perform well in a deterministic environment, but generally face many challenges in obstacle avoidance in an uncertain environment, such as limited detection capabilities, uncertain distribution positions and influence ranges of obstacles. In actual scenarios, the rescue environment often exhibits highly unstructured characteristics, the distribution of obstacles has significant spatio-temporal uncertainty, there are multi-source information breaks between rescue prior information (such as historical maps) and posterior perception data (such as real-time sensor feedback), and the assumption of a single deterministic map is difficult to adapt to multi-probability obstacle scenarios. In addition, traditional algorithms do not consider the impact of obstacle probability on path planning, and the heuristic function cannot take into account both path safety and planning efficiency, resulting in problems such as insufficient risk quantification and noise sensitivity. Summary of the Invention

[0003] To solve the above problems, the present invention proposes a path planning method, a terminal device, and a medium based on multi-map fusion.

[0004] The specific solutions are as follows:

[0005] A path planning method based on multi-map fusion, comprising the following steps:

[0006] S1: Collect multiple sub-maps of the area to be studied under different obstacle distributions, grid each sub-map, mark whether there is an obstacle in each grid obtained by gridifying each sub-map, and use the probability of the obstacle appearing in each grid as the grid value of the grid;

[0007] S2: Perform weighted superposition on the grid values of all sub-maps, and generate a global probability map based on the global obstacle probability of each grid obtained by weighted superposition;

[0008] S3: Perform path planning for the area to be studied through an improved Theta* algorithm to obtain a global path from the starting point to the ending point;

[0009] The improved Theta* algorithm is improved in the following two aspects on the basis of the traditional Theta* algorithm:

[0010] (1) Modify the calculation formula of the heuristic function to:

[0011] h(n) = h0(n) · σ(n)

[0012]

[0013] Among them, h(n) represents the heuristic cost of node n; h0(n) represents the Euclidean distance from node n to the end point; σ(n) represents the weight function, which is non-linearly mapped from p(n) through the Sigmoid function; p(n) represents the global obstacle probability of the grid where node n is located; p b represents the obstacle probability threshold; both α and β represent adjustable parameters, which are used to control the risk weight intensity and adjust the slope of the Sigmoid function respectively;

[0014] (2) Set the obstacle probability threshold p b during the search stage of line-of-sight detection. Only when the global obstacle probability corresponding to the grid passed by the path exceeds this obstacle probability threshold p b , it is determined that the line of sight is blocked;

[0015] S4: According to the actual sub-map under the actual obstacle distribution in the area to be studied, locally correct the global path on this actual sub-map to obtain the final path.

[0016] Furthermore, the calculation formula for the global obstacle probability of each grid is:

[0017]

[0018] Among them, N represents the number of sub-maps; i represents the serial number of the sub-map; M global (x, y) represents the global obstacle probability corresponding to the position (x, y); M i (x, y) represents the grid value corresponding to the position (x, y) in the i-th sub-map, and ω i represents the appearance probability of the i-th sub-map.

[0019] Furthermore, the specific implementation process of step S4 includes:

[0020] S401: Traverse all nodes in the global path, and judge whether the grid where each node is located in the actual sub-map is an obstacle grid. If so, delete this node from the global path; update the path after traversing all nodes to the corrected path;

[0021] S402: Judge whether the number of nodes included in the corrected path is less than 2. If so, determine that the corrected path is unavailable, and re-plan the path from the starting point to the end point of the actual sub-map to obtain the final path; otherwise, enter S403;

[0022] S403: Traverse adjacent node pairs in the corrected path, and use the line-of-sight detection method to determine whether there are obstacles between each pair of nodes. If not, skip this pair of nodes; if so, use the improved Theta* algorithm to perform path planning with the two nodes in this pair of nodes as the starting point and the ending point respectively, obtain the sub-path corresponding to this pair of nodes, and insert the obtained sub-path into the corresponding position of the corrected path; update the corrected path after traversing all adjacent node pairs as the spliced path.

[0023] S404: After de-duplicating the spliced path, use it as the final path.

[0024] A path planning terminal device based on multi-map fusion includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the method in the above embodiments of the present invention.

[0025] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the steps of the method in the above embodiments of the present invention.

[0026] By adopting the above technical solutions, the present invention significantly improves the robustness, safety, and real-time performance of path planning by fusing multi-map probability obstacle information, designing a probability-sensitive heuristic function, and a line-of-sight (LOS) path optimization mechanism and a two-stage architecture of global-first and local-second, providing an efficient solution to the path planning problem in complex dynamic environments. Description of the Drawings

[0027] Figure 1 The flowchart of the method in the first embodiment of the present invention is shown.

[0028] Figure 2 The schematic diagrams of three sub-maps collected in this embodiment are shown.

[0029] Figure 3 The schematic diagram of the global probability map in this embodiment is shown.

[0030] Figure 4 The schematic diagram of the global path planning result in this embodiment is shown.

[0031] Figure 5 The schematic diagram of the final path with complete obstacle avoidance obtained based on the first sub-map in this embodiment is shown. Detailed Embodiments

[0032] To further illustrate the embodiments, the present invention provides accompanying drawings. These drawings are part of the disclosure of the present invention, mainly used to illustrate the embodiments and can explain the operating principles of the embodiments in conjunction with the relevant descriptions in the specification. With reference to these contents, those of ordinary skill in the art should be able to understand other possible implementation manners and the advantages of the present invention.

[0033] The present invention will be further described below in conjunction with the accompanying drawings and specific implementation manners.

[0034] Embodiment 1:

[0035] The embodiment of the present invention provides a path planning method based on multi-map fusion, as Figure 1 shown, the method includes the following steps:

[0036] S1: Collect multiple sub-maps of the area to be studied under different obstacle distributions, grid each sub-map, and mark whether there is an obstacle in each grid obtained by gridifying each sub-map. Take the probability of the obstacle appearing in each grid as the grid value of the grid.

[0037] In this embodiment, the area to be path-planned is used as the area to be studied, and one (a kind of) sub-map corresponds to one obstacle distribution situation.

[0038] The method for obtaining the sub-maps under different obstacle distributions in this embodiment is: collect different obstacle distribution situations that may occur in the area to be studied, and obtain multiple typical situations of different obstacle appearance probabilities through clustering and reduction. Take these typical situations as sub-maps. Three sub-maps collected in this embodiment are as Figure 2 shown.

[0039] When marking whether there is an obstacle in the grid of the sub-map in this embodiment, if a grid is blank, mark the probability of the existence of an obstacle as 0; if a grid has an obstacle, mark the probability of the existence of an obstacle as 1.

[0040] S2: Perform weighted superposition on the grid values of all sub-maps, and generate a global probability map based on the global obstacle probability of each grid obtained by weighted superposition.

[0041] The global obstacle probability is to perform probability weighted superposition on the probabilities of the existence of obstacles in each grid of each sub-map, and take the map marked with the global obstacle probability of each grid as the global probability map. By superimposing multiple sub-maps into a global probability map according to probability, a unified environmental model is provided for path planning.

[0042] The calculation formula for the global obstacle probability of each grid in this embodiment is:

[0043]

[0044] Among them, N represents the number of sub - maps; i represents the serial number of the sub - map; M global (x, y) represents the global obstacle probability corresponding to the position (x, y), which can be any probability value between [0, 1]. For example, 0 represents no obstacle, 1 represents a complete obstacle, and 0.35 represents that the probability of an obstacle appearing in this grid is 35%; M i (x, y) represents the grid value corresponding to the position (x, y) in the i - th sub - map, ω i represents the appearance probability (weight) of the i - th sub - map.

[0045] In this embodiment, the statistical method for the appearance probability of the sub - map is as follows: Based on prior knowledge and experience summary, experts cluster and reduce the possible obstacle distributions into several typical scenarios, construct the corresponding sub - maps, and assign the corresponding appearance probabilities to the sub - maps.

[0046] S3: Perform path planning for the area to be studied through an improved Theta* algorithm to obtain the global path from the starting point to the ending point. The specific planning process is as follows:

[0047] Step 1: Initialization.

[0048] Define the coordinates of the starting point and the ending point of the path planning, and initialize the open list and the closed list. The open list is used to store the nodes to be explored, and the closed list is used to store the nodes that have been explored. Add the starting point to the open list, set its actual cost g to 0, the heuristic cost h to the Euclidean distance from the starting point to the ending point, and the parent node points to itself.

[0049] Step 2: Enter the main loop to perform node selection and expansion.

[0050] First, it is node selection. If the open list is not empty, select the node with the minimum combined cost f(n) from the open list as the current node to prepare for further expanding the neighborhood. If the selected current node is the ending point, end the loop; otherwise, move the current node from the open list to the closed list and record that this node has been expanded.

[0051] Next, perform neighborhood expansion. Traverse the 8 neighborhood nodes of the current node (including the up, down, left, right, and diagonal directions), process each neighborhood node one by one, calculate and record the actual path cumulative cost g(n), the heuristic estimated cost h(n), and the combined cost f(n) of each neighborhood node. If the neighborhood node exceeds the map range or the corresponding grid value exceeds the preset obstacle threshold, skip this node; check for duplicate nodes. If the neighborhood node already exists in the closed list, skip it.

[0052] The calculation formula for the combined cost function f(n) is as follows:

[0053] f(n) = g(n) + h(n)

[0054] Among them, g(n) represents the total path cost from the starting point to this node. For the neighboring nodes of the current node, this value represents the total cost from the starting point to the current node plus the Euclidean distance between the current node and this node, and the total cost from the starting point to the current node has been updated after the previous loop.

[0055]

[0056] Incorporate the obstacle probability into the heuristic cost calculation. Higher probability obstacle areas are assigned higher heuristic costs to guide the algorithm to generate low-risk paths.

[0057] The calculation formula for the heuristic cost h(n) of neighboring nodes is as follows:

[0058] h(n) = h0(n) · σ(n)

[0059] Among them, h0(n) represents the Euclidean distance from this node n to the end point, and σ(n) represents the weight function, which is obtained by non-linearly mapping p(n) through the Sigmoid function.

[0060]

[0061] Among them, α and β are adjustable parameters, which are used to control the risk weight intensity and adjust the slope of the Sigmoid function respectively; p(n) represents the global obstacle probability of the grid where node n is located; p b represents the obstacle probability threshold, which is used to identify the probability critical value of the existence of obstacles, that is, if it is greater than this threshold, it is determined that the line of sight is blocked. Through this design, the heuristic cost of high-probability obstacle areas can be significantly increased to guide the path to bypass high-risk areas.

[0062] Finally, perform real-time line-of-sight detection and parent node update. Trace back the parent node of the current node. If there is a parent node, call the line-of-sight detection function based on the Bresenham algorithm to check whether there is an obstacle-free straight-line path between the parent node and the selected next expanded neighboring node. If the line of sight exists, that is, the global obstacle probability corresponding to each grid passed by this straight-line path is less than the obstacle probability threshold p b , then directly update the actual cost g(n) and the parent node of the selected neighboring node through the parent node. If there is no parent node, it is still updated through the current node. After the update, add the neighboring node to the open list and wait for subsequent exploration, that is, enter the next loop.

[0063] Repeat the loop process until the end point is found or the open list is empty.

[0064] Step 3: Optimize and output the results.

[0065] Starting from the end point, trace back to the starting point level by level through the parent node pointer, and post-process and optimize the generated path. Check whether there is still a straight path without obstacles among the intermediate nodes of the path. If so, delete the intermediate nodes to further reduce path redundancy. Finally, output the optimized path coordinates from the starting point to the end point (i.e., the global path) and the total actual movement cost of the path.

[0066] S4: According to the actual sub-map under the actual obstacle distribution in the area to be studied, locally correct the global path to obtain the final path.

[0067] In the above global path generation, although the probability distribution of obstacles in multiple maps has been considered, the actual execution needs to adapt to the complete obstacle avoidance requirements of the specific sub-map. For this reason, this embodiment further proposes local correction, and realizes the rapid adaptation of the path by dynamically deleting the obstacle nodes and re-planning between adjacent nodes.

[0068] After obtaining the global path based on the global path planning of multiple maps, when a certain sub-map is matched in the actual scene, the global path needs to be locally corrected to ensure complete obstacle avoidance. The process of local correction includes the following steps.

[0069] S401: Traverse all the nodes in the global path, and judge whether the grid where each node is located in the actual sub-map is an obstacle grid (i.e., there are obstacles in the grid). If so, delete the node from the global path; update the path after traversing all the nodes to the corrected path new_path.

[0070] By traversing all the nodes in the global path, the deletion of all nodes located on the obstacle grid is realized.

[0071] S402: Judge whether the corrected path is empty or the number of nodes is less than 2. If so, it is determined that the corrected path is unavailable (indicating that the path is completely infeasible or broken), and re-plan the path from the starting point to the end point of the actual sub-map (trigger the exception handling mechanism) to obtain the final path; otherwise, enter S403.

[0072] S403: Traverse the adjacent node pairs in the corrected path, and judge whether there are obstacles between each node pair through the line-of-sight detection method. If not, skip this pair of nodes; if so, use the improved Theta* algorithm to plan the path with the two nodes in this node pair as the starting point and the end point respectively (realize local obstacle avoidance), obtain the sub-path corresponding to this node pair, and insert the obtained sub-path into the corresponding position of the corrected path; update the corrected path after traversing all the adjacent node pairs to the spliced path.

[0073] S404: After de-duplicating the spliced path, it is used as the final path.

[0074] The duplicate removal operation is to delete consecutive duplicate nodes to reduce path redundancy.

[0075] The following uses an example to introduce the process of local correction.

[0076] Suppose the obtained global path contains the node sequence A→B→C→D, where node B is located on an obstacle in the actual sub-map. The local correction process is as follows:

[0077] 1. Remove the obstacle node: Delete B to generate a temporary path A→C→D.

[0078] 2. Local replanning:

[0079] Plan an obstacle avoidance sub-path for A→C (such as A→E→C).

[0080] Plan an obstacle avoidance sub-path for C→D (such as C→F→D).

[0081] 3. Path splicing: Generate the final path A→E→C→F→D.

[0082] The following uses a specific case to illustrate the method of this embodiment.

[0083] Suppose there are three sub-maps map1, map2, map3 (see Figure 2 ), the occurrence probabilities of the three sub-maps are 0.3, 0.3, and 0.4 respectively, the adjustment factor α of the heuristic function is 1, the overall obstacle threshold is 0 (when expanding nodes, if it is greater than 0, it is judged as an obstacle and cannot be selected), and the line-of-sight detection uses the obstacle probability threshold p b is 0.3 (when performing line-of-sight detection, if it is greater than 0.3, the line of sight is considered not to pass). The global probability maps obtained for the three sub-maps are as Figure 3 shown. The probability of obstacles appearing in each grid can be intuitively reflected by the color depth of each grid. The global path obtained by global planning is as Figure 4 shown. It can be seen from the figure that the global path passes through some areas with a relatively small probability of obstacle appearance, indicating that the global path can ensure high safety and efficiency on most sub-maps. After Figure 2 performing local correction on the first sub-map in Figure 5 the final path shown realizes complete obstacle avoidance on the actual sub-map.

[0084] In the embodiments of the present invention, through the collaborative design of global path and local correction, the generalized environmental representation ability of the global probability map is retained, and the complete feasibility of the path is ensured through the binary constraint of the sub-map, thus solving the path planning contradiction in a dynamic and uncertain environment. Compared with the traditional single-stage path planning method, the hierarchical design adopted in this embodiment unifies the multi-map probabilistic obstacle modeling and the actual specific obstacle avoidance requirements into the same framework for the first time through the progressive process of probability fusion → global optimization → local correction.

[0085] Embodiment 2:

[0086] The present invention also provides a path planning terminal device based on multi-map fusion, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps in the above method embodiment of Embodiment 1 of the present invention are implemented.

[0087] Furthermore, as an executable solution, the path planning terminal device based on multi-map fusion may be a computing device such as a desktop computer, a notebook, a palm computer, or a cloud server. The path planning terminal device based on multi-map fusion may include, but is not limited to, a processor and a memory. Those skilled in the art can understand that the composition structure of the above path planning terminal device based on multi-map fusion is only an example of the path planning terminal device based on multi-map fusion, and does not constitute a limitation on the path planning terminal device based on multi-map fusion. It may include more or fewer components than the above, or combine some components, or different components. For example, the path planning terminal device based on multi-map fusion may also include input / output devices, network access devices, buses, etc. The embodiments of the present invention do not make limitations on this.

[0088] Furthermore, as an executable solution, the so-called processor may be a Central Processing Unit (CPU), or may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, etc. The general-purpose processor may be a microprocessor, or the processor may also be any conventional processor, etc. The processor is the control center of the path planning terminal device based on multi-map fusion, and connects various parts of the entire path planning terminal device based on multi-map fusion through various interfaces and lines.

[0089] The memory can be used to store the computer programs and / or modules. By running or executing the computer programs and / or modules stored in the memory, and invoking the data stored in the memory, the processor realizes various functions of the path planning terminal device based on multi-map fusion. The memory mainly includes a program storage area and a data storage area. Among them, the program storage area can store the operating system and application programs required for at least one function; the data storage area can store data created according to the use of the mobile phone, etc. In addition, the memory can include high-speed random access memory, and can also include non-volatile memory, such as hard disks, memory, plug-in hard disks, smart media cards (SMC), secure digital (SD) cards, flash cards, at least one magnetic disk storage device, flash memory device, or other volatile solid-state storage devices.

[0090] The present invention also provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, the steps of the above method in the embodiments of the present invention are realized.

[0091] If the modules / units integrated in the path planning terminal device based on multi-map fusion are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, to implement all or part of the processes in the above method embodiments of the present invention, it can also be completed by instructing relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps of the above various method embodiments can be realized. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), and software distribution medium, etc.

[0092] Although the present invention is specifically shown and described in conjunction with the preferred embodiments, those skilled in the art should understand that various changes can be made to the present invention in form and detail without departing from the spirit and scope of the present invention defined by the appended claims, and all of them are within the protection scope of the present invention.

Claims

1. A path planning method based on multi-map fusion, characterized in that It includes the following steps: S1: Collect multiple sub - maps of the area to be studied under different obstacle distributions, grid each sub - map, mark whether there is an obstacle in each grid obtained by grid - meshing each sub - map, and take the probability of obstacle occurrence corresponding to each grid as the grid value of the grid; S2: Perform weighted superposition on the grid values of all sub - maps, and generate a global probability map based on the global obstacle probabilities of each grid obtained by weighted superposition; S3: Perform path planning for the area to be studied through an improved Theta* algorithm to obtain a global path from the starting point to the ending point; The improved Theta* algorithm is improved in the following two aspects based on the traditional Theta* algorithm: (1) Modify the calculation formula of the heuristic function to: h(n) = h0(n)·σ(n) Among them, h(n) represents the heuristic cost of node n; h0(n) represents the Euclidean distance from node n to the end point; σ(n) represents the weight function, which is obtained by non-linearly mapping p(n) through the Sigmoid function; p(n) represents the global obstacle probability of the grid where node n is located; p b represents the obstacle probability threshold; α and β both represent adjustable parameters, which are used to control the risk weight intensity and adjust the slope of the Sigmoid function respectively; (2) Set the obstacle probability threshold p during the search stage of line-of-sight detection b , and only when the global obstacle probability corresponding to the grid passed by the path exceeds the obstacle probability threshold p b , it is determined that the line of sight is blocked; S4: According to the actual sub - map under the actual obstacle distribution of the area to be studied, perform local correction on the global path on this actual sub - map to obtain the final path.

2. The path planning method based on multi-map fusion according to claim 1, wherein: The calculation formula for the global obstacle probability of each grid is: Among them, N represents the number of sub-maps; i represents the serial number of the sub-map; M global (x, y) represents the global obstacle probability corresponding to the position (x, y); M i (x, y) represents the grid value corresponding to the position (x, y) in the i-th sub-map, ω i represents the occurrence probability of the i-th sub-map.

3. The path planning method based on multi-map fusion according to claim 1, wherein: The specific implementation process of step S4 includes: S401: Traverse all nodes in the global path, determine whether the grid where each node is located in the actual sub - map is an obstacle grid. If so, delete the node from the global path; update the path after traversing all nodes as the corrected path; S402: Determine whether the number of nodes included in the corrected path is less than 2. If so, determine that the corrected path is unavailable, and re - perform path planning from the starting point to the ending point of the actual sub - map to obtain the final path; otherwise, proceed to S403; S403: Traverse adjacent node pairs in the corrected path, and determine whether there is an obstacle between each pair of nodes through the line - of - sight detection method. If not, skip this pair of nodes; if there is, perform path planning through the improved Theta* algorithm with the two nodes in this pair of nodes as the starting point and the ending point respectively to obtain the sub - path corresponding to this pair of nodes, and insert the obtained sub - path at the corresponding position in the corrected path; update the corrected path after traversing all adjacent node pairs as the spliced path; S404: Perform a duplicate - removal operation on the spliced path as the final path.

4. A path planning terminal device based on multi-map fusion, characterized in that: It includes a processor, a memory, and a computer program stored in the memory and running on the processor. When the processor executes the computer program, it implements the steps of the method described in any one of claims 1 - 3.

5. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by the processor, it implements the steps of the method described in any one of claims 1 - 3.

Images