Navigation path planning method based on connected graph and iterative search

Abstract

The present invention discloses a navigation path planning method based on a connected graph and iterative search, wherein the navigation path planning method searches for a path optimization starting point closest to the actual starting point and a path optimization end point closest to the actual end point in accordance with a reasonable path cost in a regional connected graph constructed by a robot in advance, and then completes the navigation path planning between the path optimization starting point and the path optimization end point by using the heuristic path cost and the matching search priority on the basis of the iterative search method. The search amount of the extended nodes is effectively reduced, the search speed of the path nodes in the regional connected graph is increased, the passability of the robot in the regional connected graph is improved, and the coverage of the planned path on the map is increased.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and relates to a navigation path planning method based on a connected graph and iterative search. Background Art

[0002] Traditional navigation path planning algorithms (such as Dijkstra, A*, D*) rely heavily on the accuracy and area of the grid map. As the accuracy of grid reading increases or the area of the map to be represented increases, the search cost increases exponentially, and the paths searched by traditional graph search algorithms consider less about the passability of the robot. Summary of the Invention

[0003] In order to solve the above technical problems, the technical solution of the present invention searches for a path optimization starting point closest to the actual starting point and a path optimization ending point closest to the actual ending point within the regional connected graph constructed by the robot's pre-walking, and then completes the navigation path planning between the path optimization starting point and the path optimization ending point in an iterative search manner. The specific technical solution is as follows:

[0004] Navigation path planning method based on a connected graph and iterative search, comprising: within a pre-constructed regional connected graph, using the corresponding cost value converted from the centrality value of a node on the movement trajectory segment to which it belongs, searching for a path optimization starting point that matches a pre-set navigation starting point and a path optimization ending point that matches a pre-set navigation ending point; wherein each node is located on a movement trajectory segment to which it belongs, and the movement trajectory segment is generated by the pre-movement of the robot; among all the nodes to be expanded in the regional connected graph, if the node to be expanded with the smallest sum of heuristic path costs is the path optimization ending point, according to the parent node information corresponding to the nodes in the regional connected graph, layer by layer, search for an optimized navigation path from the path optimization starting point to the path optimization ending point; then connect the two ends of the currently searched optimized navigation path to the navigation starting point and the navigation ending point respectively to complete the planning of the navigation path; among all the nodes to be expanded in the regional connected graph, if the node to be expanded with the smallest sum of heuristic path costs is not the path optimization ending point, then set the node to be expanded with the smallest sum of heuristic path costs as the current expansion node, and then set the unexpanded nodes among the neighborhood nodes of the current expansion node as new nodes to be expanded until the node to be expanded with the smallest sum of heuristic path costs currently searched is the path optimization ending point; wherein, the sum of the heuristic path costs consumed by any node in the regional connected graph is equal to the sum of the actual movement cost from the path optimization starting point to this node and the predicted path cost from this node to the path optimization ending point. Compared with the prior art, the technical solution of the present invention can effectively reduce the search volume of the expanded nodes, improve the search speed of the path nodes in the regional connected graph, improve the passability of the robot in the regional connected graph, and increase the coverage area of the planned path on the map. It also gives play to the path planning advantage of the heuristic search algorithm on the alternative navigation map of the connected graph.

[0005] Further, the method for searching for a path optimization start point matching a preset navigation start point and a path optimization end point matching a preset navigation end point by using the corresponding cost value converted from the centrality value of a node on the motion trajectory segment to which the node belongs includes: If there is a node to be judged whose connection line with the navigation start point does not pass through an obstacle grid point, it is determined that the node to be judged is a node directly leading to the navigation start point; then, the corresponding cost values converted from the centrality values of each node directly leading to the navigation start point on the motion trajectory segment to which it belongs are numerically sorted; then, the node directly leading to the navigation start point with the smallest cost value is set as the path optimization start point; If there is a node to be judged whose connection line with the navigation end point does not pass through an obstacle grid point, it is determined that the node to be judged is a node directly leading to the navigation end point; then, the corresponding cost values converted from the centrality values of each node directly leading to the navigation end point on the motion trajectory segment to which it belongs are numerically sorted; then, the node directly leading to the navigation end point with the smallest cost value is set as the path optimization end point; where the node to be judged belongs to the nodes in the pre-constructed regional connectivity graph; each node to be judged is located on a motion trajectory segment to which it belongs.

[0006] In the regional connectivity graph, this technical solution uses the centrality value of each node on the motion trajectory segment to which it belongs (indicating the degree of approaching the midpoint of the motion trajectory segment or indicating the degree of deviating from the endpoints of the motion trajectory segment) to search for the node that directly leads to the navigation point of the robot and has the smallest cost value, and uses it as the path optimization start point matching the distance to the navigation start point and the path optimization end point matching the distance to the navigation end point, and starts to plan a navigation path in the regional connectivity graph with the path optimization start point as the start point and the path optimization end point as the end point.

[0007] Further, it also includes: calculating the difference between the value 2 and the centrality value of the node to be judged on the motion trajectory segment to which it belongs as the deviation degree difference; calculating the Euclidean distance between the node to be judged and the navigation start point as the navigation start point matching distance; then multiplying the deviation degree difference, the navigation start point matching distance by the centrality influence coefficient, and the obtained product is the corresponding cost value converted from the centrality value of the node directly leading to the navigation start point on the motion trajectory segment to which it belongs; calculating the Euclidean distance between the node to be judged and the navigation end point as the navigation end point matching distance; then multiplying the deviation degree difference, the navigation end point matching distance by the centrality influence coefficient, and the obtained product is the corresponding cost value converted from the centrality value of the node directly leading to the navigation end point on the motion trajectory segment to which it belongs; where the centrality value is between 0 and 1; the centrality influence coefficient is preset and is used to adjust the distance of the path to be planned from the obstacle.

[0008] This technical solution obtains the converted cost value corresponding to the node to be judged according to the result of the product operation of the distance between the path to be planned and the obstacle, the Euclidean distance between the node to be judged and the navigation starting point or the navigation ending point, and the centrality value of the node to be judged on the movement trajectory segment to which it belongs. However, it is different from the actual movement cost from the navigation ending point or the navigation starting point to the node to be judged, and is used to evaluate the proximity and passing effect of the node to be judged to the navigation starting point or the navigation ending point.

[0009] Further, the actual movement cost from the path optimization starting point to the current expansion node is equal to the sum of the actual movement cost from the path optimization starting point to the parent node of the current expansion node and the actual movement cost from the parent node of the current expansion node to the current expansion node. It means that the actual movement cost from the path optimization starting point to the current expansion node is the cumulative result of the path cost, and controls the actual movement cost generated by the current expansion node to be equal to the sum of the actual movement cost accumulated by the previous expansion node and the actual movement cost generated by the current expansion node to its parent node.

[0010] Further, the calculation method of the actual movement cost from the parent node of the current expansion node to the current expansion node includes: calculating the difference between the value 2 and the centrality value of the current expansion node on the movement trajectory segment to which it belongs as the deviation degree difference; calculating the Euclidean distance between the current expansion node and its parent node as the neighborhood node matching distance; then multiplying the deviation degree difference, the neighborhood node matching distance and the centrality influence coefficient, and the obtained product is the actual movement cost from the parent node of the current expansion node to the current expansion node; where the centrality value is between 0 and 1; the centrality influence coefficient is preset and is used to adjust the distance between the path to be planned and the obstacle. So that the actual movement cost from the parent node of the current expansion node to the current expansion node can represent the degree of the path to be planned away from the obstacle, and improve the search success rate of the passable path between the parent node and the current expansion node of the current expansion node.

[0011] Furthermore, the calculation method of the actual movement cost from the path optimization starting point to the parent node of the current expanded node includes: Step a1, set the parent node of the current expanded node as the backtracking iteration node, and set the movement cost iteration sum to 0; then enter Step a2; Step a2, determine whether the backtracking iteration node is the path optimization starting point, if so, enter Step a3, otherwise enter Step a4; Step a3, set the actual movement cost from the path optimization starting point to the parent node of the current expanded node to be equal to the movement cost iteration sum; Step a4, calculate the difference between the value 2 and the centrality value of the backtracking iteration node on the corresponding movement trajectory segment as the deviation degree difference; calculate the Euclidean distance between the backtracking iteration node and its parent node as the neighborhood node matching distance; then multiply the deviation degree difference, the neighborhood node matching distance by the centrality influence coefficient, and the obtained product is the actual movement cost from the parent node of the backtracking iteration node to the backtracking iteration node, then add the actual movement cost from the parent node of the backtracking iteration node to the backtracking iteration node to the movement cost iteration sum, and then update the added sum value to the movement cost iteration sum; then enter Step a5; Step a5, set the parent node of the backtracking iteration node as the backtracking iteration node for the next calculation of the movement cost between the parent and child nodes, and then return to Step a2; where the centrality value is between 0 and 1; the centrality influence coefficient is preset and used to represent the distance of the path to be planned from the obstacle; where the backtracking iteration nodes before and after the update all belong to the nodes in the regional connectivity graph, and the backtracking iteration node and its parent node both belong to two adjacent nodes in the regional connectivity graph.

[0012] This technical solution accumulates the actual movement costs generated by the backtracking iteration node and all its associated parent nodes (nodes with an adjacent relationship) by repeatedly executing Step a2 to Step a5, so that the actual movement cost from the path optimization starting point to the parent node of the current expanded node represents: the sum of the movement costs generated by all adjacent two nodes existing between the path optimization starting point and the parent node of the current expanded node. It not only considers the currently obtained best selection node, but also comprehensively considers the path cost information corresponding to all traversed nodes from the path optimization starting point to the current expanded node.

[0013] Furthermore, the centrality value of the foregoing corresponding node on the corresponding movement trajectory segment is: the ratio of the straight-line distance from the node to the nearest endpoint on the movement trajectory segment to half of the length of the movement trajectory segment. It is used to represent the degree of deviation of the foregoing corresponding node from the midpoint on the corresponding movement trajectory segment, so that: when the corresponding node is located at the midpoint of the corresponding movement trajectory segment, the corresponding centrality value is 1; when the corresponding node is located at the endpoint of the corresponding movement trajectory segment, the corresponding centrality value is 0.

[0014] Furthermore, the predicted path cost from the current expansion node to the path optimization end point is represented by the Euclidean distance, Manhattan distance, or diagonal distance between the current expansion node and the path optimization end point. It is used to represent the estimated cost from a specified point to the target end point.

[0015] Furthermore, the method for layer-by-layer searching for an optimized navigation path from the path optimization start point to the path optimization end point according to the parent node information corresponding to the nodes in the regional connectivity graph includes: among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the minimum sum of heuristic path costs is the path optimization end point, then starting from the path optimization end point, according to the parent node position information recorded in the corresponding neighborhood of the path optimization end point in the regional connectivity graph, connect the parent nodes and the parent nodes of these parent nodes in sequence until the path optimization start point is connected in the regional connectivity graph, so as to layer-by-layer search out an optimized navigation path from the path optimization start point to the path optimization end point between the navigation start point and the navigation end point. This technical solution selects, among the sums of heuristic path costs generated corresponding to all nodes to be expanded, the node with the minimum value of the sum of heuristic path costs and belonging to the path optimization end point as the path expansion start point, and obtains the navigation path from the navigation start point to the navigation end point by connecting in reverse through the way of interconnecting adjacent nodes one by one.

[0016] Furthermore, among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the minimum sum of heuristic path costs is not the path optimization end point, then setting the node to be expanded with the minimum sum of heuristic path costs as the current expansion node, and then setting the unexpanded nodes among the neighborhood nodes of the current expansion node as new nodes to be expanded, until the node to be expanded with the minimum sum of heuristic path costs currently searched out is the path optimization end point, the method includes: Step b1, among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the minimum sum of heuristic path costs is not the path optimization end point, then setting the node to be expanded with the minimum sum of heuristic path costs as the current expansion node, then searching out the unexpanded neighborhood nodes of the current expansion node in the regional connectivity graph and setting them as new nodes to be expanded, at the same time, recording the parent node of the newly set node to be expanded as the currently set current expansion node, then updating the current expansion node to an expanded node, and then entering Step b2; Step b2, among all the currently set nodes to be expanded, judging whether the node to be expanded with the minimum heuristic path cost is the path optimization end point, if so, executing the method for layer-by-layer searching for an optimized navigation path from the path optimization start point to the path optimization end point according to the parent node information corresponding to the nodes in the regional connectivity graph; otherwise, returning to Step b1.

[0017] This technical solution uses the current expanded node as the search starting point for the node to be expanded (spare path node), expands the search scope of the node to be expanded through neighborhood search, and differentiates the expanded nodes (nodes that have been traversed and marked as the current expanded node) and unexpanded nodes (nodes that have not been visited) during the search process, reducing the repeated search rate and also facilitating the rapid search for the path optimization end point within a larger range, making the planned path have better passability and higher area coverage.

[0018] Further, during the execution of the navigation path planning method by the robot, a priority queue is created to store the nodes to be expanded; when the node to be expanded with the highest priority stored in the priority queue dequeues, the node to be expanded with the highest priority is updated as the current expanded node; after searching for the unexpanded neighborhood nodes of the current expanded node in the regional connectivity graph, the current expanded node is marked as an expanded node to distinguish it from the unexpanded nodes, and the searched unexpanded neighborhood nodes of the current expanded node are added to the priority queue and updated as nodes to be expanded; among them, the unexpanded neighborhood nodes belong to the unexpanded nodes, and there is no node that has been previously configured as a node to be expanded among the unexpanded nodes, so that the unexpanded neighborhood nodes of the current expanded node do not pre-exist in the priority queue, and the unexpanded neighborhood nodes of the current expanded node do not exist in the expanded nodes; among them, before the priority queue starts to dequeue a node to be expanded for a round of node expansion operations, the priority queue has already stored the path optimization starting point to facilitate guiding the expansion from the path optimization starting point to the path optimization end point through the neighborhood; among them, the smaller the heuristic path cost consumed by the node to be expanded, the higher the search priority of the node to be expanded in the priority queue is configured. This technical solution creates a priority queue for storing the nodes to be expanded, realizes the orderly caching, reading and writing operations of the nodes to be expanded and the expanded nodes, and is conducive to accelerating the expansion of the shortest path with the lowest navigation cost.

[0019] Further, the method for constructing the regional connectivity graph includes: Step 1, in each of the pre-marked parallel motion trajectory segments of the robot, along each of the pre-marked parallel motion trajectory segments of the robot, stepwise extract passable nodes from the midpoint of the motion trajectory segment towards both ends; Step 2, first construct a tree structure based on the passable nodes extracted in Step 1, and then, in combination with the movement cost of the specified node from the navigation starting point, search for a regional connectivity graph from the reachable neighborhood of the nodes of the tree structure; wherein, the navigation starting point is the navigation information pre-configured in the robot. Compared with the prior art, in this technical solution, nodes are stepwise searched from the midpoint of the cleaning trajectory towards both ends, and then the searched nodes are used to construct a tree data structure and a traditional graph search algorithm is performed on the tree data structure to search for the regional connectivity graph, so that the robot can use the regional connectivity graph to replace the traditional grid map during navigation, which can greatly reduce the search operation amount, and at the same time can also connect more feasible paths and improve the completeness of regional reachability.

[0020] Further, the specific method of Step 2 includes: Step 21, configure the specified node with the minimum movement cost from the navigation starting point as the search starting point; wherein, the first node configured as the specified node is any one of the passable nodes extracted in Step 1. Then enter Step 22; Step 22, in the nodes of the tree structure constructed by the passable nodes extracted in Step 1, screen out the unvisited adjacent nodes belonging to the reachable neighborhood of the currently configured search starting point and configure all the currently screened nodes as specified nodes, and then enter Step 23; Step 23, form the regional connectivity graph with the currently configured search starting point and the adjacent nodes screened in Step 22, configure the currently configured search starting point as a visited node so that it is no longer the specified node; then, among the currently configured specified nodes, configure the specified node with the minimum movement cost from the navigation starting point as the search starting point for screening new neighborhood nodes next time, and then return to Step 22; Step 24, repeat Step 22 to Step 23 until there is no specified node configured as a new search starting point, and determine that a regional connectivity graph is constructed. Thus, by restricting a reachable neighborhood to search for unvisited adjacent nodes and updating the shortest path distance information in continuous iterative processing, it is ensured that the finally obtained is a node set with the best completeness and interconnection, and these node sets form the regional connectivity graph.

[0021] Further, the currently configured search starting point and the adjacent nodes screened in Step 22 are both configured as key values to construct the regional connectivity graph in which any two nodes are interconnected; wherein, the search starting point and its corresponding adjacent nodes screened in Step 22 support constructing a passable route; wherein, the regional connectivity graph is a graphical structure space and belongs to a data structure.

[0022] Further, in the step 2, it further includes: calculating the movement cost of each passable node extracted in the step 1 from the navigation starting point respectively, as the movement cost from the navigation starting point to the corresponding designated node; then configuring the priority of the designated node by using the calculated movement cost corresponding to the designated node, and then storing the designated node with the configured priority into the priority queue space, so that the designated node with the highest current priority stored in the priority queue space is first screened out and placed into the graph structure space to form the regional connectivity graph; wherein, both the priority queue space and the graph structure space are data structures used to describe the connection between nodes; wherein, the designated node belongs to the passable nodes extracted in the step 1 and also belongs to the nodes on the tree structure. This technical solution uses a data structure to store the relevant information of the designated node, including the coordinate information of the node, the route information formed by the nodes, and the information of the graph, and can distinguish whether it belongs to the traversed nodes to avoid repeated search; on the basis of the foregoing technical solution, this technical solution is based on each passable node that can construct a tree structure extracted in the step 1, combines the neighbor relationship of each node in the tree, calculates the movement cost between nodes and uses it to configure the priority of the designated node, and then sorts the designated nodes according to the priority to facilitate dequeueing from the priority queue where it is located, so that the designated node with the highest current priority stored in the priority queue space is first screened out and placed into the graph structure space. It is convenient to access node information in order and improves the acquisition efficiency of connectable nodes.

[0023] Further, the method for configuring the priority includes: if the movement cost of the designated node from the navigation starting point is greater, the priority of the designated node is lower; if the movement cost of the designated node from the navigation starting point is smaller, the priority of the designated node is higher; so that the designated node with the smallest movement cost from the navigation starting point is first screened out from the priority queue space.

[0024] Further, when the specified node with the highest priority currently stored in the priority queue space is first screened out and configured as the search starting point, the specified node currently configured as the search starting point is removed from the priority queue space. Then, the node screened out in step 22 currently being executed is set as the specified node and stored in the priority queue space. Meanwhile, the specified node currently configured as the search starting point is stored in the traversed node set structure. Among them, the node screened out in step 22 refers to an adjacent node within the reachable neighborhood of the currently configured search starting point that has not been previously stored in the traversed node set structure and does not pre-exist in the priority queue space, so as to avoid reconfiguring the previously configured search starting point searched within the reachable neighborhood of the currently configured search starting point as the new search starting point, resulting in duplicate search of nodes.

[0025] Further, the adjacent nodes within the reachable neighborhood of the currently configured search starting point simultaneously satisfy the following conditions: the straight-line distance between the currently configured search starting point and its adjacent nodes is less than or equal to the body diameter of the robot; the connection line between the currently configured search starting point and its adjacent nodes does not pass through obstacle grid points and does not pass through unknown grid points. Compared with the traditional grid map construction method, this technical solution focuses on expanding adjacent nodes within a two-dimensional plane with stronger connectivity, making the passable routes that can be formed by the searched nodes more complete and the coverage rate of the passable area higher.

[0026] Further, the specific method of step 1 includes: first, configuring the midpoint of each parallel motion trajectory segment as the node with passability; then, starting from the midpoint of each motion trajectory segment, respectively along both ends of a corresponding motion trajectory segment, extracting a node every sampling step length of one body diameter, and configuring the extracted node as the node with passability until sampling reaches both ends of a corresponding motion trajectory segment. Thus, the passability of each motion trajectory segment is detected with the body diameter as the step length, improving the extraction efficiency of nodes with passability and increasing the construction accuracy of the tree structure.

[0027] Further, step 1 also includes: if the distance between a node extracted on a corresponding motion trajectory segment according to a sampling step length of one body diameter and one end point of the same motion trajectory segment is less than one body diameter, then configuring this end point as the node with passability. This improves the redundancy of node extraction and improves the completeness of the regional connectivity graph.

[0028] Further, the tree structure is a Kd - tree constructed on a two - dimensional data set, and the Kd - tree represents a partitioning of the two - dimensional coordinate space formed by the two - dimensional data set; wherein, the two - dimensional data set is constructed by the passable nodes and includes node coordinate values and marker information of the motion trajectory segments to which they belong. This technical solution arranges the disordered passable nodes in an orderly manner according to the specific recursive order specified by the Kd - tree or performs segmentation processing on the two - dimensional plane where the passable nodes are located, realizing neighborhood search within the passable nodes. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 FIG. is a flowchart of a navigation path planning method based on a connected graph and iterative search disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0030] The following further describes the specific embodiments of the present invention with reference to the accompanying drawings. It should be understood that when used in this specification and the appended claims, the terms "comprises" and "comprising" indicate the presence of the described features, wholes, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, wholes, steps, operations, elements, components, and / or their combinations.

[0031] It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in this specification of the present invention and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an", and "the" are intended to include the plural forms.

[0032] It should be further understood that the term "and / or" used in this specification of the present invention and the appended claims refers to any combination and all possible combinations of one or more of the related listed items, and includes these combinations.

[0033] As used in this specification and the appended claims, the term "if" can be interpreted as "when", "once", "in response to determining", or "in response to detecting" depending on the context. Similarly, the phrase "if determined" or "if detected [the described condition or event]" can be interpreted as meaning "once determined", "in response to determining", "once detected [the described condition or event]", or "in response to detecting [the described condition or event]" depending on the context.

[0034] It should be noted that for those skilled in the art, it is understandable that the grid map marks the environmental information around the current position of the robot. The grids within the map area constructed by the robot include three states: free, occupied, and unknown. These grids are represented by grid points in this embodiment, that is, the center points of the grids. The grid points in the free state refer to the grids not occupied by obstacles, which are the grid positions that the robot can reach and are free grid points, and can form an unoccupied area. The grid points in the occupied state refer to the grids occupied by obstacles, which are obstacle grid points and can form an occupied area. The unknown grid points refer to the grid areas where the specific situation is not clear during the process of the robot constructing the map. The positions of these points are often blocked by obstacles and can form an unknown area.

[0035] It should be noted that when using a search algorithm to solve a problem, it is necessary to construct a data structure that indicates the state characteristics of its own position and the relationship between the states of different positions. This data structure is called a node. Different problems need to be described by different data structures. According to the conditions given by the search problem, starting from a node, one or more new nodes can be generated. This process is usually called expansion. The relationship between nodes can generally be represented as adjacent parent nodes and child nodes. The search process of the search algorithm is actually a process of constructing a path according to the initial conditions and expansion rules to find the node that meets the target state and connecting the shortest path.

[0036] In order to overcome the problems that traditional navigation path planning algorithms (such as Dijkstra, A*, D*) rely heavily on the accuracy and area of the grid map and the search cost increases exponentially with the increase of the accuracy and area of the grid map, an embodiment of the present invention discloses a navigation path planning method based on a connected graph and iterative search, as Figure 1 shown, which includes the following steps:

[0037] Step S1. In the pre-constructed region connection graph, use the centrality value of a node on its affiliated movement trajectory segment to convert the corresponding cost value, and then use the converted corresponding cost value to search for a path optimization starting point that matches the pre-set navigation starting point and a path optimization ending point that matches the pre-set navigation ending point. In this embodiment, the "matching" refers to matching in terms of distance, including the closest distance, or the distance or movement cost being within a reasonable range, so as to set a reasonable path starting point and path ending point in the region connection graph and reduce the search space; then proceed to Step S2. Among them, each node is located on an affiliated movement trajectory segment, and the movement trajectory segment is generated by the robot's pre-movement, so that the planning of the navigation path has practical significance. The nodes in Step S1 include, but are not limited to, the currently expanded nodes, unexpanded nodes, and nodes to be expanded (candidate expanded nodes) commonly used or newly used in the search algorithm of this embodiment.

[0038] Step S2. Among all the nodes to be expanded in the region connection graph, determine whether the node with the minimum sum of heuristic path costs is the path optimization ending point. If so, proceed to Step S3; otherwise, proceed to Step S4. Among them, the sum of the heuristic path costs consumed by the node to be expanded is equal to the sum of the actual movement cost from the path optimization starting point to the node to be expanded and the predicted path cost from the node to be expanded to the path optimization ending point. Step S2 selects the node to be expanded with the minimum sum of heuristic path costs (the candidate expanded node with the minimum distance cost) to determine whether it is the path optimization ending point, which is equivalent to determining whether the node with the minimum corresponding movement cost sum is the target end point of the navigation path to be planned in the region connection graph, facilitating the search for target points within a reasonable distance range. In this embodiment, the path with the lowest navigation cost of the robot is planned with the end point as the guide, reducing the overall navigation cost.

[0039] Step S3. According to the parent node information corresponding to the nodes in the region connection graph, search layer by layer for the optimized navigation path from the path optimization starting point to the path optimization ending point, and then proceed to Step S5. In this embodiment, for the region connection graph constructed on the grid map, the corresponding implementation method of "searching layer by layer" is essentially a process of continuously expanding from the path optimization starting point to reach the path optimization ending point, including completing the marking and update of the parent node according to the principle of the least cost from the starting point to this node, and then starting from the path optimization ending point, connecting the adjacent path nodes expanded in sequence to form the optimized navigation path between the path optimization starting point and the path optimization ending point.

[0040] Step S4: Set the unexpanded node with the minimum heuristic path cost as the current expanded node. Then, search for the unexpanded neighborhood nodes of the current expanded node within the regional connectivity graph and set them as new unexpanded nodes. At the same time, update the currently set current expanded node to an expanded node, and then return to Step S2. In Step S4, in this embodiment, the unexpanded node with the minimum heuristic path cost is set as the current expanded node. At this time, the newly set current expanded node is no longer the previously set unexpanded node. Preferably, the unexpanded node with the minimum heuristic path cost is removed from the original preset storage space and transformed into the current expanded node. Then, set the unexpanded nodes among the neighborhood nodes of the current expanded node as new unexpanded nodes. Among them, the unexpanded nodes among the neighborhood nodes of the current expanded node neither belong to the type of nodes of the current expanded node nor belong to the type of unexpanded nodes. Preferably, add the newly set unexpanded nodes to the original preset storage space and transform them into unexpanded nodes. Repeat Steps S2 to S4 until the currently searched unexpanded node with the minimum heuristic path cost is the path optimization end point. In this embodiment, during the search process among the neighborhood nodes of the current expanded node, the search is centered on the current expanded node in any direction, including but not limited to an eight-neighborhood search. This is beneficial for searching out the shortest path with the lowest navigation cost.

[0041] In Step S4, search for the unexpanded neighborhood nodes of the current expanded node within the regional connectivity graph, allowing the robot to search in any direction. The expansion step or search step can be represented by the Euclidean distance. Each execution of Step S4 is an expansion / search, enabling the robot to start from the current parent node and reach the unexpanded neighborhood nodes that can be reached after traveling for a preset interval time as child nodes. In short, each expansion / search represents the robot "taking a step" and corresponds to crossing a grid in the map. It should be noted that the preset interval time is the periodic time for each expansion, representing a relatively small time unit. For example, it can be 5 seconds, 10 seconds, etc. The shorter the preset interval time, the finer the planned navigation path. Therefore, the preset interval time can be determined according to actual needs.

[0042] Step S5: Connect the two ends of the currently searched optimized navigation path to the navigation start point and the navigation end point respectively to form a complete navigation path, and determine that the planning of the navigation path is completed. Compared with the prior art, the embodiments described in the foregoing steps can effectively reduce the search volume of expanded nodes, improve the search speed of path nodes within the regional connectivity graph, improve the passability of the robot within the regional connectivity graph, and increase the coverage of the planned path on the map. It also gives full play to the path planning advantages of heuristic search algorithms (including the A* algorithm) on the alternative navigation map of the connectivity graph.

[0043] As an embodiment, the method for searching for a path optimization start point that matches a pre-set navigation start point and a path optimization end point that matches a pre-set navigation end point by using the corresponding cost value converted from the centrality value of a node on its corresponding motion trajectory segment includes:

[0044] If the connection line between a node to be judged and the navigation start point does not pass through an obstacle grid point, that is, a node whose connection line with the navigation start point does not pass through an obstacle is searched in the regional connectivity graph, then it is determined that the node to be judged is a node directly leading to the navigation start point, where the node to be judged is any node in the regional connectivity graph; then, the corresponding cost values converted from the centrality values of each node directly leading to the navigation start point on its corresponding motion trajectory segment are numerically sorted. Generally, the corresponding cost values converted from all nodes directly leading to the navigation start point are sorted from small to large; then, the node directly leading to the navigation start point with the smallest corresponding cost value is set as the path optimization start point, and it is determined that a directly connected node that matches the distance to the navigation start point has been searched, which is understood as the most accessible node with the closest distance, and it plays the role of representing the navigation start point in the process of planning the navigation path.

[0045] Similarly, if the connection line between a node to be judged and the navigation end point does not pass through an obstacle grid point, that is, a node whose connection line with the navigation end point does not pass through an obstacle is searched in the regional connectivity graph, then it is determined that the node to be judged is a node directly leading to the navigation end point, where the node to be judged is any node in the regional connectivity graph that participates in judging the path optimization end point; then, the corresponding cost values converted from the centrality values of each node directly leading to the navigation end point on its corresponding motion trajectory segment are numerically sorted. Generally, the corresponding cost values converted from all nodes directly leading to the navigation end point are sorted from small to large; then, the node directly leading to the navigation end point with the smallest corresponding cost value is set as the path optimization end point, and it is determined that a directly connected node that matches the distance to the navigation end point has been searched, which is understood as the most accessible node with the closest distance to the end point, and it plays the role of representing the navigation end point in the process of planning the navigation path.

[0046] It should be noted that the node to be judged belongs to the nodes in the pre-constructed regional connectivity graph; each node to be judged is located on a corresponding motion trajectory segment and has practical traffic significance.

[0047] In this embodiment, within the regional connectivity graph, by using the centrality value of each node on its respective movement trajectory segment (indicating the degree of approaching the midpoint of the movement trajectory segment, or indicating the degree of deviating from the endpoints of the movement trajectory segment), a node that leads directly to the navigation starting point and has the minimum cost value is searched for, and used as the path optimization starting point that matches the distance from the navigation starting point, as well as the path optimization ending point that matches the distance from the navigation ending point, and then a navigation path starting from the path optimization starting point and ending at the path optimization ending point is planned within the regional connectivity graph.

[0048] Based on the above embodiment, the method for calculating the corresponding cost value converted from the centrality value of the foregoing corresponding node on its respective movement trajectory segment includes:

[0049] Calculate the difference between the value 2 and the centrality value of the node to be judged on its respective movement trajectory segment as the deviation degree difference; wherein, the centrality value is between 0 and 1; specifically, if the centrality value is closer to 1, the node to be judged is closer to the midpoint of its respective movement trajectory segment; if the centrality value is closer to 0, the node to be judged is closer to one of the endpoints of its respective movement trajectory segment; the centrality value of the node to be judged on its respective movement trajectory segment is the ratio of the straight-line distance from the node to be judged to the nearest endpoint on the movement trajectory segment to half of the length of the movement trajectory segment, such that when the node to be judged is located at the midpoint of its respective movement trajectory segment, the corresponding centrality value is 1; when the node to be judged is located at the endpoint of its respective movement trajectory segment, the corresponding centrality value is 0.

[0050] Then calculate the Euclidean distance between the node to be judged and the navigation starting point as the navigation starting point matching distance; then multiply the deviation degree difference, the navigation starting point matching distance by the centrality influence coefficient, and the product obtained is the corresponding cost value converted from the centrality value of the node leading directly to the navigation starting point on its respective movement trajectory segment; wherein, the centrality influence coefficient is preset and used to adjust the distance of the path to be planned from the obstacle; in this embodiment, the centrality influence coefficient is set to be greater than or equal to 1. When the centrality influence coefficient is set larger, the influence weight of the centrality value in the foregoing obtained product is increased, and the influence degree of the centrality value on the path to be planned is enhanced, so that the path to be planned is far from the obstacle and the passability of the path to be planned is ensured. Further, the closer the centrality value is to 1, the smaller the corresponding cost value converted, which means the node to be judged is closer to the navigation starting point.

[0051] Similarly, calculate the Euclidean distance between the node to be judged and the navigation end point as the navigation end point matching distance; then multiply the deviation degree difference, the navigation end point matching distance by the centering degree influence coefficient, and the obtained product is the corresponding cost value converted from the centering degree value of the node directly leading to the navigation end point on the corresponding motion trajectory segment; among them, the centering degree influence coefficient is preset and used to adjust the distance between the path to be planned and the obstacle; in this embodiment, the centering degree influence coefficient is set to be greater than or equal to 1. When the centering degree influence coefficient is set larger, the weight of the centering degree value in the above-obtained product is increased, and the influence degree of the centering degree value on the path to be planned is enhanced, so that the path to be planned is farther away from the obstacle, ensuring the effect that the path to be planned leads to the end point without obstacles. Further, the closer the centering degree value is to 1, the smaller the corresponding cost value converted, indicating that the node to be judged is closer to the navigation end point.

[0052] Therefore, in this embodiment, according to the result of the product operation of the distance between the path to be planned and the obstacle, the Euclidean distance between the node to be judged and the navigation start point or the navigation end point, and the centering degree value of the node to be judged on the corresponding motion trajectory segment, the corresponding cost value converted from the node to be judged is obtained, which is different from the actual movement cost from the navigation end point or the navigation start point to the node to be judged, and is used to evaluate the proximity and passing effect of the node to be judged to the navigation start point or the navigation end point.

[0053] As an embodiment, the actual movement cost from the path optimization start point to the current expansion node is equal to the sum of the actual movement cost from the path optimization start point to the parent node of the current expansion node and the actual movement cost from the parent node of the current expansion node to the current expansion node. The actual movement cost corresponding to the current expansion node disclosed in this embodiment indicates that the actual movement cost from the path optimization start point to the current expansion node is the cumulative result of the path cost of the corresponding parent node, and controls the actual movement cost generated by the current expansion node to be equal to the sum of the actual movement cost accumulated by the previous expansion node and the actual movement cost generated by the current expansion node to its parent node.

[0054] Specifically, the calculation method of the actual movement cost from the parent node of the current expansion node to the current expansion node includes: calculating the difference between the value 2 and the centrality value of the current expansion node on the movement trajectory segment to which it belongs as the deviation degree difference; the centrality value of the current expansion node on the movement trajectory segment to which it belongs is the ratio of the straight-line distance from the current expansion node to the nearest endpoint on the movement trajectory segment to half of the length of the movement trajectory segment, so that: the closer the centrality value is to 1, the closer the current expansion node is to the midpoint of the movement trajectory segment to which it belongs, indicating that the current expansion node tends to be far from the obstacle; the closer the centrality value is to 0, the closer the current expansion node is to one endpoint of the movement trajectory segment to which it belongs, indicating that the current expansion node tends to be close to the obstacle. Then, calculate the Euclidean distance between the current expansion node and its parent node as the neighborhood node matching distance, where the current expansion node and its previously recorded parent node are two adjacent nodes in the regional connectivity graph; then multiply the deviation degree difference, the neighborhood node matching distance by the centrality influence coefficient, and the product obtained is the actual movement cost from the parent node of the current expansion node to the current expansion node; where the centrality value is between 0 and 1; the centrality influence coefficient is preset and used to represent the distance of the path to be planned from the obstacle. The centrality influence coefficient is greater than or equal to 1. When the centrality influence coefficient is set larger, the product obtained above is also larger, enhancing the influence degree of the centrality value on the path to be planned, making the path to be planned farther from the obstacle and ensuring the passability of the path to be planned. Further, the closer the centrality value is to 1, the smaller the actual movement cost from the parent node of the current expansion node to the current expansion node, indicating that the movement cost from the parent node of the current expansion node to the current expansion node is lower. The actual movement cost from the parent node of the current expansion node to the current expansion node can represent the degree of the path to be planned away from the obstacle, improving the search success rate of the passable path between the parent node of the current expansion node and the current expansion node.

[0055] Specifically, the calculation method of the actual movement cost from the path optimization starting point to the parent node of the current expansion node includes:

[0056] Step a1: Set the parent node of the current expansion node as the backtracking iteration node, and set the movement cost iteration sum to 0 as the accumulated cost sum value; then enter step a2.

[0057] Step a2: Determine whether the backtracking iteration node is the path optimization starting point. If so, enter step a3; otherwise, enter step a4.

[0058] Step a3: Set the actual movement cost from the path optimization starting point to the parent node of the current expansion node to be equal to the movement cost iteration sum. Specifically, if step a2 is executed for the first time, determine that the actual movement cost from the path optimization starting point to the parent node of the current expansion node is equal to 0; if step a2 is not executed for the first time, set the actual movement cost from the path optimization starting point to the parent node of the current expansion node to be equal to the movement cost iteration sum.

[0059] Step a4: Calculate the difference between the value 2 and the centering degree value of the backtracking iteration node on the movement trajectory segment to which it belongs as the deviation degree difference; the centering degree value of the backtracking iteration node on the movement trajectory segment to which it belongs is the ratio of the straight-line distance from the backtracking iteration node to the closest endpoint on the movement trajectory segment to half of the length of the movement trajectory segment, so that: the closer the centering degree value is to 1, the closer the backtracking iteration node is to the midpoint of the movement trajectory segment to which it belongs; the closer the centering degree value is to 0, the closer the backtracking iteration node is to one endpoint of the movement trajectory segment to which it belongs. It should be emphasized that the backtracking iteration node belongs to the nodes within the region connection graph and is from the movement trajectory segment generated by the robot's pre-movement. Then, calculate the Euclidean distance between the backtracking iteration node and its parent node as the neighborhood node matching distance; then multiply the deviation degree difference, the neighborhood node matching distance by the centering degree influence coefficient, and the obtained product is the actual movement cost from the parent node of the backtracking iteration node to the backtracking iteration node, which is equal to the cost value corresponding to one backtracking iteration node. Then, add the actual movement cost from the parent node of the backtracking iteration node to the backtracking iteration node to the movement cost iteration sum, and update the sum value obtained by the addition as the movement cost iteration sum; then enter step a5.

[0060] Step a5: Set the parent node of the backtracking iteration node as the backtracking iteration node for the next calculation of the movement cost between the parent and child nodes, and then return to step a2; where the backtracking iteration nodes before and after the update both belong to the nodes within the region connection graph, and the backtracking iteration node and its parent node both belong to two adjacent nodes within the region connection graph.

[0061] In this embodiment, by repeatedly executing step a2 to step a5, the actual movement cost generated by the backtracking iteration node and all its associated parent nodes (nodes with an adjacency relationship) is accumulated, so that the actual movement cost from the path optimization starting point to the parent node of the current expansion node represents the sum of the movement costs generated by all adjacent two nodes between the path optimization starting point and the parent node of the current expansion node. This enables this embodiment to not only consider the currently obtained best selection node, but also comprehensively consider the path cost information corresponding to all traversed nodes from the path optimization starting point to the current expansion node within the regional connectivity graph.

[0062] In the foregoing embodiment, the predicted path cost from the current expansion node to the path optimization end point is represented by using the Euclidean distance, Manhattan distance, or diagonal distance between the current expansion node and the path optimization end point. If the robot can only move in four directions (up, down, left, and right) in the current connectivity graph, the Manhattan distance can be used; if the robot can move in eight directions in the current connectivity graph, the diagonal distance can be used; if the robot can move in any direction in the current connectivity graph, the Euclidean distance can be used. This is beneficial for searching out the shortest navigation path with the lowest navigation cost. It should be noted that for each node, its cost value can be calculated. The cost value is a measure of the cost of the robot's movement trajectory, representing the cost of moving from the starting point to this node and then to the end point, including factors such as path length, required time, whether a collision occurs, and whether the speed direction is frequently switched.

[0063] As an embodiment, the method for layer-by-layer searching out the optimized navigation path from the path optimization starting point to the path optimization end point according to the parent node information corresponding to the nodes in the regional connectivity graph includes: among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the minimum sum of heuristic path costs is the path optimization end point, then starting from the path optimization end point, according to the parent node position information recorded in the corresponding neighborhood of the path optimization end point in the regional connectivity graph, connect the parent node and the parent node of this parent node in sequence until the path optimization end point is connected in the regional connectivity graph, thereby realizing layer-by-layer searching out the optimized navigation path from the path optimization starting point to the path optimization end point between the navigation starting point and the navigation end point; wherein, when the sum of the heuristic path costs consumed by a node to be expanded is the smallest among all the nodes to be expanded currently configured in the regional connectivity graph, then this node to be expanded is configured as the node to be expanded / search first.

[0064] The method of connecting the recorded parent node position information in the corresponding neighborhood of the path optimization end point in the regional connectivity graph according to the path, and connecting the parent node and the parent node of the parent node in sequence is specifically as follows: in the to-be-expanded nodes configured currently, when the to-be-expanded node with the minimum heuristic path cost sum is the path optimization end point, the to-be-expanded node with the minimum heuristic path cost sum is used as the child node, and then its parent node is traced back according to the pre-recorded parent node position information and connected in sequence; then the traced-back parent node is used as the next child node, and its parent node is continuously traced back according to the pre-recorded parent node position information and connected in sequence. Repeat this process until reaching the path optimization start point, where the path optimization start point also belongs to the to-be-expanded nodes; thus, in this embodiment, among the heuristic path cost sums corresponding to all to-be-expanded nodes, the node with the minimum heuristic path cost sum value and belonging to the path optimization end point is selected as the path expansion start point, and the navigation path from the navigation start point to the navigation end point is reversely connected by the way of interconnecting adjacent nodes one by one.

[0065] As another embodiment, in all to-be-expanded nodes in the regional connectivity graph, when the to-be-expanded node with the minimum heuristic path cost sum is not the path optimization end point, the method of setting the to-be-expanded node with the minimum heuristic path cost sum as the current expansion node, and then setting the unexpanded nodes in the neighborhood nodes of the current expansion node as new to-be-expanded nodes until the to-be-expanded node with the minimum heuristic path cost sum searched currently is the path optimization end point includes:

[0066] Step b1: In all to-be-expanded nodes in the regional connectivity graph, specifically in the to-be-expanded nodes configured currently, when the to-be-expanded node with the minimum heuristic path cost sum obtained by comparing the heuristic path cost sums consumed by each to-be-expanded node is not the path optimization end point, the to-be-expanded node with the minimum heuristic path cost sum is set as the current expansion node. Then, in the regional connectivity graph, with the current expansion node as the search center, the unexpanded neighborhood nodes in its neighborhood are searched out and set as new to-be-expanded nodes. At the same time, the current expansion node set in step b1 is used as the currently expanded parent node and the parent node of the currently searched unexpanded neighborhood nodes; then the current expansion node is updated to an expanded node, and it is determined to set the current expansion node as a node that cannot be repeatedly expanded or a node that cannot be repeatedly searched, excluding the possibilities of this type of node as the to-be-expanded node and this type of node as the current expansion node; then step b2 is entered.

[0067] Step b2: Among all the nodes to be expanded currently marked, determine whether the node to be expanded with the minimum heuristic path cost is the path optimization end point. If so, according to the method of the optimized navigation path from the path optimization start point to the path optimization end point disclosed in the foregoing embodiment, start from the path optimization end point and search layer by layer for the optimized navigation path from the path optimization start point to the path optimization end point; otherwise, return to step b1. In the newly configured nodes to be expanded, continue to compare the heuristic path costs consumed by each node to be expanded. When the node to be expanded with the minimum obtained heuristic path cost is not the path optimization end point, set the node to be expanded with the minimum heuristic path cost as the current expansion node. Then, in the region connection graph, with the current expansion node as the next search center, continue to search for the unexpanded neighborhood nodes in its neighborhood and set them as the next batch of nodes to be expanded, and update the next search center to the next parent node; iterate in this way until the unexpanded neighborhood node of the current expansion node searched in the region connection graph is the path optimization end point. Among them, there are no nodes to be expanded and expanded nodes among the unexpanded neighborhood nodes of the foregoing current expansion node. In this embodiment, the current expansion node is used as the search start point of the nodes to be expanded (alternative path nodes), and the search range of the nodes to be expanded is expanded by means of neighborhood search. Moreover, during the search process, the expanded nodes (already traversed and marked as the current expansion node) and the unexpanded nodes (nodes not visited) are distinguished, reducing the repeated search rate, which is beneficial to expanding the path optimization end point to a larger range, making the planned path have better passability and higher regional coverage.

[0068] On the basis of the foregoing embodiment, during the process of the robot executing the navigation path planning method, a priority queue is created to store the foregoing nodes to be expanded; when the node to be expanded with the highest priority stored in the priority queue dequeues, update the node to be expanded with the highest priority as the current expansion node. Among them, the smaller the heuristic path cost consumed by the node to be expanded, the higher the search priority of the node to be expanded is configured. Then, the node to be expanded with the highest search priority is the node to be expanded with the minimum heuristic path cost. Those skilled in the art can understand that the node to be expanded with the highest current priority stored in the priority queue and its associated node information are output first. Preferably, other information such as the position, attitude, and speed of the parent node of the node to be expanded is also recorded in the priority queue.

[0069] After searching for the unexpanded neighborhood nodes of the current expansion node in the regional connectivity graph, the current expansion node is marked as an expanded node to distinguish it from unexpanded nodes. In this embodiment, a cache space for traversed nodes is specifically created to store expanded nodes; at this time, the expanded nodes have been removed from the priority queue and then moved into the cache space for traversed nodes; it should be noted that nodes existing in the cache space for traversed nodes are not allowed to be added to the priority queue, so as to identify the expanded nodes or the nodes regarded as searched out during the planning process and prevent the phenomenon of repeated expansion of the same search center; preferably, the cache space for traversed nodes can exist in the robot in the form of a list data storage structure. At the same time, the unexpanded neighborhood nodes of the currently searched expansion node are added to the priority queue and updated to be nodes to be expanded. As the set of path nodes for subsequent path planning, they also all originate from the nodes that make up the regional connectivity graph; at the same time, record the position information of the parent nodes of the newly added neighborhood nodes in the priority queue. Preferably, the position information of these neighborhood nodes and the corresponding position information of the parent nodes are stored in the priority queue together, and the position information of these parent nodes also all originates from the regional connectivity graph and is from the motion trajectory segments generated by the robot's pre-movement; so that when the path optimization end point is searched, the path between the start point and the end point can be found step by step directly through the backtracking method of its parent node, accelerating the speed of path planning. In this embodiment, a priority queue for storing nodes to be expanded is created to realize the orderly caching, reading and writing operations of nodes to be expanded and expanded nodes, which is beneficial to accelerating the expansion of the shortest path with the lowest navigation cost.

[0070] It should be noted that unexpanded neighborhood nodes belong to unexpanded nodes, and there are no nodes that have been previously configured as nodes to be expanded among the unexpanded nodes, so that the unexpanded neighborhood nodes of the current expansion node do not pre-exist in the priority queue, and the unexpanded neighborhood nodes of the current expansion node do not exist in the expanded nodes; among them, before the priority queue dequeues a node to be expanded for a round of node expansion operations, the priority queue has stored the path optimization start point, so as to facilitate guiding the expansion from the path optimization start point to the path optimization end point through the neighborhood. Among them, the nodes to be expanded inside the priority queue are all configured with search priorities to form an orderly dequeue order. Among them, the nodes to be expanded inside the priority queue respectively have the identification information of the motion trajectory segments, because they all originate from the motion trajectory segments generated by the robot's pre-movement.

[0071] As an embodiment, a method for constructing a regional connectivity graph described in the foregoing embodiments is disclosed, which specifically includes: Step 1: Along each movement trajectory line segment that is pre-marked by the robot and parallel to each other, nodes with passability are stepwise extracted from the midpoint of each movement trajectory line segment to both ends, that is, with a specific step size as a sampling interval (understood as stepwise), nodes with passability are extracted; after extracting nodes with passability on all movement trajectory line segments in the area already covered by the robot's work, proceed to execute Step 2; Step 2: Based on the nodes with passability extracted in Step 1, a tree structure is constructed, and then in combination with the movement cost of the specified node from the navigation starting point, a regional connectivity graph is searched from the reachable neighborhood of the nodes of the tree structure; where the navigation starting point is navigation information pre-configured into the robot, and the specified node has a traversal priority and serves as a candidate path node, so that the nodes selected from the reachable neighborhood of the nodes of the tree structure form a regional connectivity graph with stronger completeness. Compared with the existing random roadmap algorithm and graph search algorithm, the regional connectivity graph searched in Step 2 covers a better-connected area, supports the construction of more feasible paths, and relatively expands the range of the robot's movement coverage. Compared with the existing technology, in this embodiment, nodes are stepwise searched from the midpoint of the cleaning trajectory to both ends, and then the searched nodes are used to construct a tree data structure and a traditional graph search algorithm is performed on the tree data structure to search out the regional connectivity graph, so that the robot can use the regional connectivity graph to replace the traditional grid map during navigation, which can greatly reduce the search computation amount, and at the same time can also connect more feasible paths and improve the completeness of regional reachability.

[0072] As an embodiment, the specific method of Step 2 includes:

[0073] Step 21: Configure the specified node with the minimum movement cost from the navigation starting point as the search starting point; then proceed to Step 22; where the navigation starting point is navigation information pre-configured into the robot and belongs to the path starting point pre-set by the traditional graph search algorithm (including but not limited to A* algorithm, D* algorithm, Dijkstra algorithm) for iterative calculation of the movement cost (shortest path); the search starting point is the currently traversed node set in the current iterative calculation and supports being updated in the next iterative calculation. Among them, the node first configured as the specified node is any node with passability extracted in Step 1.

[0074] Step 22: Among the nodes of the tree structure constructed by the nodes with passability extracted in Step 1, filter out the unvisited adjacent nodes within the reachable neighborhood of the currently configured search starting point, and configure all the currently filtered nodes as specified nodes. This is equivalent to searching for the unvisited adjacent nodes within the reachable neighborhood of the search starting point from the tree data structure constructed in Step 1, including adjacent nodes that have not been configured as specified nodes, and then configuring the adjacent nodes that have not been configured as specified nodes as new specified nodes; then proceed to Step 23. Therefore, in this embodiment, there are no pre-configured specified nodes among the unvisited neighborhood nodes filtered out in Step 22, and the nodes that have been configured as specified nodes do not belong to the unvisited neighborhood nodes filtered out in Step 22. It should be noted that the adjacent nodes are relative to the reachable neighborhood of the search starting point and are adjacent nodes within the neighborhood of a node (such as an eight-neighborhood, four-neighborhood, etc. neighborhood range), equivalent to the child nodes of a parent node.

[0075] It should be noted that in Step 22, when the currently configured search starting point is a parent node, all the unvisited neighborhood nodes within the reachable neighborhood of the currently configured search starting point belong to the child nodes of this parent node.

[0076] Step 23: Mark the currently configured search starting point and the adjacent nodes filtered out in Step 22 as node elements within the route node set. Then these node elements are stored in a graph structure to form the regional connectivity graph, that is, the route node set forms the regional connectivity graph in the form of a graph structure, which belongs to a data structure. At the same time, mark the currently configured search starting point as a traversed node so that it is no longer the specified node, which is equivalent to making the traversed specified node not be reconfigured as the search starting point subsequently; at the same time, the adjacent nodes filtered out in Step 22 cannot be searched repeatedly to form the regional connectivity graph, nor can they be searched repeatedly and used as the search starting point for configuring new adjacent nodes in the next screening. Among the adjacent nodes searched within the reachable neighborhood of the currently configured search starting point, there may be the search starting point configured in the previous time. Then configure the unvisited specified node with the minimum movement cost from the navigation starting point as the search starting point for screening new adjacent nodes in the next time, and then return to Step 22. Thus, the currently executed Step 23, on the basis of excluding the interference of the search starting point configured in the previous time, including excluding the unvisited adjacent nodes filtered out in the previous Step 22, selects the specified node with the minimum movement cost from the navigation starting point among all the newly configured specified nodes as the search starting point for screening new adjacent nodes in the next time, realizes updating the currently configured search starting point in Step 21, obtains the search starting point for constructing the passable route next time, and then returns to Step 22.

[0077] Step 24. Repeat Step 22 to Step 23 until there is no specified node configured as a new search starting point, that is, until all passable nodes (i.e., specified nodes) corresponding to the movement costs of all values are traversed. The corresponding configured search path (a path abstracted for the foregoing iterative process, not a passable route that can be formed by the nodes in the regional connectivity graph) becomes empty, stop the search, and determine that the search starting point and its adjacent nodes within the reachable neighborhood form a regional connectivity graph, and determine that a regional connectivity graph is constructed.

[0078] The embodiments described in the foregoing Step 21 to Step 24 start from the navigation starting point and adopt the strategy of the greedy algorithm. Each time, the adjacent nodes within a specific neighborhood of the search starting point (vertex) that is closest to the navigation starting point and has not been traversed are traversed until there is no suitable node as a new search starting point. Thus, by restricting a reachable neighborhood to search for unvisited adjacent nodes and updating the shortest path distance information in continuous iterative processing, it is ensured that the most complete and interconnected roadmap (equivalent to the regional connectivity graph) is obtained in the last iteration.

[0079] It should be added that the Dijkstra algorithm is based on the idea of the greedy algorithm. The so-called greedy algorithm always keeps the current iterative solution as the current optimal solution. That is to say, under the known conditions or all the conditions currently available, the optimal solution is guaranteed. If a better solution is generated due to the addition of new conditions in subsequent iterations, it will replace the previous optimal solution. By continuously iterating and always ensuring that the result of each iteration is the current optimal solution, the global optimal solution will be obtained when the last iteration is reached.

[0080] In the foregoing embodiments, the currently configured search starting point and the adjacent nodes screened in Step 22 are both configured as key values to construct a regional connectivity graph in which any two nodes are interconnected; the search starting point and its adjacent nodes screened in Step 22 support the construction of a passable route, realizing the construction of a passable route from the search starting point to its adjacent nodes screened in Step 22. This satisfies the technical effect of constructing a passable route for any iterative operation of configuring and searching for neighborhood nodes within the reachable neighborhood. It should be noted that the graphical structure is a data structure in which the relationship between elements is arbitrary. In the graphical structure, the number of predecessor nodes and successor nodes of each node can be arbitrary, and the relationship between node elements is arbitrary. Other data structures (such as trees, linear lists, etc.) have clear conditional restrictions, and any two node elements in the graphical structure can be connected. This is conducive to realizing the construction of a regional connectivity graph in which any two points are connected and reachable between the navigation starting point and the navigation end point.

[0081] As an embodiment, in Step 2, it further includes:

[0082] Calculate the movement cost of each passable node extracted in Step 1 from the navigation starting point as the movement cost from the navigation starting point to the corresponding specified node, that is, obtain the movement cost of each specified node from the same navigation starting point. The movement cost includes, but is not limited to, the Euclidean distance and Manhattan distance from the navigation starting point to the specified node. Then, configure the priority of the specified node using the calculated movement cost corresponding to the specified node to facilitate the sequential access of node information and improve the acquisition efficiency of connectable nodes. Specifically, the method for configuring the priority includes: if the movement cost of the specified node from the navigation starting point is greater, then configure the priority of the specified node to be lower; if the movement cost of the specified node from the navigation starting point is smaller, then configure the priority of the specified node to be higher. So that the specified node with the smallest movement cost from the navigation starting point is the first to be output from the priority queue space.

[0083] Then store the specified node with configured priority into the priority queue space. It should be noted that when starting to execute Step 2, this specified node with configured priority is any node on the tree structure constructed by the passable nodes extracted in Step 1. Among them, the specified node with the highest priority currently stored in the priority queue space is the first to be screened out (since it is a priority queue, it can be understood as the first to be output from the queue) and placed into the graph structure space to construct the regional connectivity graph; in the priority queue space, the elements can only be specified nodes and all are assigned priorities, and the element with the highest priority is preferentially removed from the priority queue space, so that the specified node with the smallest movement cost from the navigation starting point is the first to be screened out from the priority queue space (since it is a priority queue, it can be understood as the first to be output from the queue). Specifically, during the process of repeatedly executing Step 22 to Step 23, if it is detected that the priority queue space is empty, it means that all the specified nodes that can be configured as new search starting points have been removed from the priority queue space to the graph structure space, and it is determined that a regional connectivity graph has been constructed.

[0084] When the specified node with the highest current priority stored in the priority queue space is first screened out (since it is a priority queue, it can be understood as the first to be output from the queue) and configured as the search starting point, the specified node currently configured as the search starting point is removed from the priority queue space, and then the specified node currently configured as the search starting point is stored in the traversed node set structure, so as to further identify the specified node currently configured as the search starting point as a node that cannot be repeatedly searched, that is, a traversed node. Then, the node screened out in step 22 currently executed is configured as the specified node and stored in the priority queue space. In this embodiment, the node screened out in step 22 refers to an adjacent node within the reachable neighborhood of the currently configured search starting point that has not been previously stored in the traversed node set structure and does not pre-exist in the priority queue space, so that the node already configured as the search starting point among the screened adjacent nodes is excluded, that is, the node already stored in the traversed node set structure is excluded, to avoid reconfiguring the previously configured search starting point searched within the reachable neighborhood of the currently configured search starting point as a new search starting point, resulting in repeated search of nodes.

[0085] Those skilled in the art can easily understand that: the traversed node set structure, the priority queue space, and the graph structure space are all data structures used to describe the connections between nodes and can store the nodes that need to be backtracked. Among them, the priority queue space is a data structure used to describe the order of entry and exit between nodes with different priority levels.

[0086] This embodiment uses a data structure to store the associated information of the specified node, including the coordinate information of the node, the route information formed by the nodes connected, and the information of the graph, and can distinguish whether it belongs to a traversed node to avoid repeated search; on the basis of the foregoing embodiment, this embodiment combines the neighbor relationships of the nodes in the tree for each passable node that can construct a tree structure extracted in step 1 to calculate the movement cost between the nodes and use it to configure the priority of the specified node, and then sort the specified nodes according to the priority to facilitate dequeueing from the priority queue where they are located, so that the specified node with the highest current priority stored in the priority queue space is first screened out (since it is a priority queue, it can be understood as the first to be output from the queue) and placed into the graph structure space. It is convenient to access the node information in order and improves the acquisition efficiency of connectable nodes.

[0087] In the foregoing embodiment, the adjacent nodes within the reachable neighborhood of the currently configured search starting point simultaneously satisfy the following conditions:

[0088] (1) The straight-line distance between the current configured search starting point and its adjacent nodes is less than or equal to the body diameter of the robot, indicating that the distance dimension between the current configured search starting point and the currently screened adjacent nodes is within the range of one body diameter, enabling the robot located at the current configured search starting point to cover the passable adjacent nodes or detect the corresponding adjacent nodes, and also increasing the distribution density of the nodes in the regional connectivity graph that meet the aforementioned passing conditions.

[0089] (2) The connection line between the current configured search starting point and its adjacent nodes does not pass through the obstacle grid points and does not pass through the unknown grid points, enabling the robot to move from the current configured search starting point to the currently screened adjacent nodes without obstacles. Otherwise, the robot cannot move along the route between the current configured search starting point and its adjacent nodes.

[0090] Among them, the vertical distance between two adjacent parallel motion trajectory line segments pre-marked by the robot is less than or equal to the body diameter of the robot, enabling the robot to repeatedly clean the same area during the process of sweeping in a zigzag pattern along the motion trajectory line segments.

[0091] As an embodiment, the specific method of step 1 includes:

[0092] First, configure the midpoint of each of the mutually parallel motion trajectory line segments as the passable node. Then, starting from the midpoint of each motion trajectory line segment, along the two ends of the corresponding motion trajectory line segment respectively, that is, in the direction from the midpoint of the corresponding motion trajectory line segment to the two ends of the same motion trajectory line segment, extract a node every sampling step length equal to one body diameter, and configure the extracted nodes as the passable nodes, so as to extract the passable nodes step by step or at specific distance intervals from the midpoint of the corresponding motion trajectory line segment to both sides until sampling reaches the two ends of the corresponding motion trajectory line segment. Thus, the passability of each motion trajectory line segment is detected step by step with the body diameter as the step length, improving the extraction efficiency of the passable nodes and increasing the construction accuracy of the tree structure. Among them, the sampling step length belongs to the sampling distance with a step length of one body diameter.

[0093] Preferably, each of the mutually parallel motion trajectory line segments marked during the movement of the robot is stored in a pre-set zigzag trajectory line set. Then, each motion trajectory line segment stored in the zigzag trajectory line set is marked with an id number, and the coordinates of the two ends of any motion trajectory line segment are stored to establish a node data structure.

[0094] For step 1, in order to reduce the search space of the iterative algorithm described in steps 21 to 24, in this embodiment, a step-by-step search is performed from the midpoint of the line connecting two points to both ends, and the distance of each search is one fuselage diameter. This can not only compress the search volume but also ensure the passage significance between nodes. Since similar search operations are performed on each moving trajectory segment with different coverage areas, the diversity of the routes that can be constructed by the area connectivity graph will not be reduced, and the algorithm will not fail to find a feasible path.

[0095] Preferably, step 1 further includes: if the distance between a node extracted on a corresponding moving trajectory segment with a sampling step size of one fuselage diameter and one end point of the same moving trajectory segment is less than one fuselage diameter, then this end point is also configured as the node with passability. This can improve the redundancy of node extraction and the completeness of the area connectivity graph.

[0096] Preferably, the tree structure is a Kd-tree constructed on a two-dimensional data set. The Kd-tree represents a partition of the two-dimensional coordinate space formed by the two-dimensional data set, specifically, a segmentation process of the two-dimensional map plane. Among them, the two-dimensional data set is constructed by the nodes with passability and includes the node coordinate values and the marker information of the moving trajectory segments to which they belong. As for how the nodes with passability construct the Kd-tree, those skilled in the field of path planning can master it, and it will not be elaborated in detail here. In short, in this embodiment, the disordered nodes with passability are sorted in an orderly manner according to the specific recursive order specified by the Kd-tree or the two-dimensional plane where the nodes with passability are located is segmented, so as to realize neighborhood search within the nodes with passability, including octal neighborhood search with any one of the nodes with passability as the search center.

[0097] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0098] The above are only the preferred embodiments of the present invention, and are not intended to limit the present invention in any other form. Any person skilled in the relevant art may use the technical content disclosed above to make changes or modifications into equivalent embodiments with equivalent changes. However, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the technical content of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims

1. A navigation path planning method based on a connected graph and iterative search, characterized in that: include: In the pre-constructed regional connectivity graph, the corresponding cost value converted by the centering degree value of the node on the corresponding motion trajectory segment is used to search for the path optimization starting point matching the pre-set navigation starting point and the path optimization end point matching the pre-set navigation end point; wherein each node is located on a corresponding motion trajectory segment, and the motion trajectory segment is generated by the robot's pre-movement; Among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the smallest heuristic path cost is the path optimization end point, the optimized navigation path from the path optimization starting point to the path optimization end point is searched layer by layer according to the parent node information corresponding to the node in the regional connectivity graph; and then the two ends of the currently searched optimized navigation path are respectively connected to the navigation starting point and the navigation end point to complete the planning of the navigation path; Among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the smallest heuristic path cost is not the end point of the path optimization, the node to be expanded with the smallest heuristic path cost is set as the current expansion node, and then the unexpanded nodes in the neighborhood nodes of the current expansion node are set as new nodes to be expanded, until the node to be expanded with the smallest heuristic path cost currently searched out is the end point of the path optimization; The sum of the heuristic path costs consumed by any node in the regional connectivity graph is equal to the sum of the actual moving cost from the path optimization starting point to the node and the predicted path cost from the node to the path optimization end point; The method of searching for a path optimization starting point that matches a preset navigation starting point and a path optimization end point that matches a preset navigation end point by using a corresponding cost value converted from a centering degree value of a node on a corresponding motion trajectory segment includes: If there is a line between the node to be determined and the navigation starting point that does not pass through the obstacle grid point, the node to be determined is determined to be a node directly connected to the navigation starting point; then the corresponding cost values ​​converted from the centering degree values ​​of each node directly connected to the navigation starting point on the motion trajectory line segment to which it belongs are numerically sorted; and then the node directly connected to the navigation starting point with the smallest cost value is set as the path optimization starting point; If there is a line between the node to be determined and the navigation end point that does not pass through the obstacle grid point, the node to be determined is determined to be a node that directly connects to the navigation end point; then the corresponding cost values ​​converted from the centering degree values ​​of each node directly connecting to the navigation end point on the motion trajectory line segment to which it belongs are numerically sorted; and then the node directly connecting to the navigation end point with the smallest cost value is set as the path optimization end point; The nodes to be determined belong to nodes in a pre-constructed regional connectivity graph; each of the nodes to be determined is located on a motion trajectory segment to which it belongs; Also includes: Calculate the difference between 2 and the centering degree value of the node to be determined on the motion trajectory segment to which it belongs, as the deviation degree difference; Calculate the Euclidean distance between the node to be determined and the navigation starting point as the navigation starting point matching distance; then multiply the deviation degree difference, the navigation starting point matching distance and the centering degree influence coefficient, and the product obtained is the corresponding cost value converted from the centering degree value of the node directly connected to the navigation starting point on the motion trajectory segment to which it belongs; Calculate the Euclidean distance between the node to be determined and the navigation endpoint as the navigation endpoint matching distance; then multiply the deviation degree difference, the navigation endpoint matching distance and the centering degree influence coefficient, and the product obtained is the corresponding cost value converted from the centering degree value of the node directly connected to the navigation endpoint on the corresponding motion trajectory segment; The centering degree value is between 0 and 1; the centering degree influence coefficient is preset and is used to adjust the distance of the path to be planned from the obstacle.

2. The navigation path planning method according to claim 1, characterized in that: The actual moving cost from the path optimization starting point to the current extended node is equal to the sum of the actual moving cost from the path optimization starting point to the parent node of the current extended node and the actual moving cost from the parent node of the current extended node to the current extended node.

3. The navigation path planning method according to claim 2, characterized in that: The calculation method of the actual moving cost from the parent node of the current expansion node to the current expansion node includes: Calculate the difference between 2 and the centering degree value of the current extended node on the motion trajectory segment to which it belongs as the deviation degree difference; calculate the Euclidean distance between the current extended node and its parent node as the neighborhood node matching distance; then multiply the deviation degree difference, the neighborhood node matching distance and the centering degree influence coefficient, and the product obtained is the actual moving cost from the parent node of the current extended node to the current extended node; The centering degree value is between 0 and 1; the centering degree influence coefficient is preset and is used to adjust the distance of the path to be planned from the obstacle.

4. The navigation path planning method according to claim 2, characterized in that: The calculation method of the actual moving cost from the path optimization starting point to the parent node of the current expansion node includes: Step a1, set the parent node of the current expansion node as the backtracking iteration node, and set the movement cost iteration sum to 0; then go to step a2; Step a2, determining whether the backtracking iteration node is the starting point of the path optimization, if yes, proceeding to step a3, otherwise proceeding to step a4; Step a3, setting the actual moving cost from the path optimization starting point to the parent node of the current expansion node to be equal to the iterative sum of the moving costs; Step a4, calculate the difference between 2 and the centering degree value of the backtracking iteration node on the corresponding motion trajectory segment as the deviation degree difference; calculate the Euclidean distance between the backtracking iteration node and its parent node as the neighborhood node matching distance; then multiply the deviation degree difference, the neighborhood node matching distance and the centering degree influence coefficient, and the product obtained is the actual moving cost from the parent node of the backtracking iteration node to the backtracking iteration node, and then add the actual moving cost from the parent node of the backtracking iteration node to the backtracking iteration node to the moving cost iterative sum, and then update the sum obtained by adding as the moving cost iterative sum; then proceed to step a5; Step a5, setting the parent node of the backtracking iteration node as the backtracking iteration node for calculating the movement cost between the parent and child nodes next time, and then returning to step a2; The centering degree value is between 0 and 1; the centering degree influence coefficient is preset and is used to indicate the degree to which the path to be planned deviates from the obstacle; The backtracking iteration nodes before and after the update are both nodes in the regional connectivity graph, and the backtracking iteration node and its parent node are both two adjacent nodes in the regional connectivity graph.

5. The navigation path planning method according to any one of claims 1 to 4, characterized in that: The centrality value of a node on the motion trajectory segment to which it belongs is: the ratio of the straight-line distance from the node to the nearest endpoint on the motion trajectory segment to half the length of the motion trajectory segment.

6. The navigation path planning method according to claim 1, characterized in that: The predicted path cost from the current extension node to the path optimization endpoint is represented by the Euclidean distance, Manhattan distance, or diagonal distance between the current extension node and the path optimization endpoint.

7. The navigation path planning method according to claim 1, characterized in that: The method of searching for the optimized navigation path from the path optimization starting point to the path optimization end point layer by layer according to the parent node information corresponding to the nodes in the regional connectivity graph comprises: Among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the smallest heuristic path cost is the path optimization end point, starting from the path optimization end point, according to the parent node position information recorded by the path optimization end point in the corresponding neighborhood of the regional connectivity graph, the parent node and the parent node of the parent node are connected in sequence until it is connected to the path optimization starting point in the regional connectivity graph.

8. The navigation path planning method according to claim 7, characterized in that: The method of, among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the smallest heuristic path cost and the smallest one is not the end point of the path optimization, setting the node to be expanded with the smallest heuristic path cost as the current expansion node, and then setting the unexpanded nodes in the neighborhood nodes of the current expansion node as new nodes to be expanded, until the currently searched out heuristic path cost and the smallest node to be expanded is the end point of the path optimization, comprises: Step b1, among all the nodes to be expanded in the regional connectivity graph, if the node to be expanded with the smallest heuristic path cost is not the end point of the path optimization, the node to be expanded with the smallest heuristic path cost is set as the current expansion node, and then the unexpanded neighboring node of the current expansion node is searched out in the regional connectivity graph and set as the new node to be expanded, and at the same time, the parent node of the new node to be expanded currently set is recorded as the current expansion node currently set, and then the current expansion node is updated to the expanded node, and then the process proceeds to step b2; Step b2: among all the nodes to be expanded currently set, determine whether the node to be expanded with the smallest heuristic path cost is the end point of the path optimization. If so, execute the method of searching for the optimized navigation path from the path optimization starting point to the path optimization end point layer by layer according to the parent node information corresponding to the node in the regional connectivity graph; otherwise, return to step b1.

9. The navigation path planning method according to claim 7 or 8, characterized in that: In the process of executing the navigation path planning method, the robot creates a priority queue for storing nodes to be expanded; when the node to be expanded with the highest priority stored in the priority queue is dequeued, the node to be expanded with the highest priority is updated as the current expanded node; after searching for the unexpanded neighboring nodes of the current expanded node in the regional connectivity graph, the current expanded node is marked as an expanded node to distinguish it from the unexpanded node, and the unexpanded neighboring nodes of the current expanded node that are searched out are added to the priority queue and are all updated as nodes to be expanded; The unexpanded neighboring nodes are unexpanded nodes, and there is no node previously configured as a to-be-expanded node among the unexpanded nodes, so that the unexpanded neighboring nodes of the current expanded node do not exist in the priority queue in advance, and the unexpanded neighboring nodes of the current expanded node do not exist in the expanded node; Before the priority queue starts to dequeue a node to be expanded to perform a round of node expansion operation, the priority queue has stored the path optimization starting point, so as to guide the expansion from the path optimization starting point through the neighborhood to the path optimization end point; The smaller the sum of the heuristic path costs consumed by the node to be expanded is, the higher the search priority of the node to be expanded in the priority queue is configured.

10. The navigation path planning method according to claim 1, characterized in that: The method for constructing the regional connectivity graph includes: Step 1: In each parallel motion trajectory segment pre-marked by the robot, nodes with passability are extracted step by step from the midpoint of the motion trajectory segment to both sides thereof; Step 2: Based on the tree structure constructed by the nodes with accessibility extracted in step 1, combined with the movement cost of the specified node from the navigation starting point, a regional connectivity graph is searched from the reachable neighborhood of the nodes of the tree structure; wherein the navigation starting point is the navigation information pre-configured in the robot.

11. The navigation path planning method according to claim 10, characterized in that: The specific method of step 2 includes: Step 21, configuring the designated node with the smallest moving cost from the navigation starting point as the search starting point; then proceeding to step 22; wherein the node first configured as the designated node is any node with accessibility extracted in step 1; Step 22, among the nodes of the tree structure constructed by the nodes with accessibility extracted in step 1, filter out the untraversed neighboring nodes within the reachable neighborhood of the currently configured search starting point and configure all the currently filtered nodes as designated nodes, and then proceed to step 23; wherein, when the currently configured search starting point is a parent node, the untraversed neighboring nodes within the reachable neighborhood of the currently configured search starting point are all child nodes of the parent node; there is no previously configured designated node among the untraversed neighboring nodes filtered out in step 22; Step 23, the currently configured search starting point and the untraversed neighboring nodes screened out in step 22 form the regional connectivity graph, the currently configured search starting point is configured as a traversed node so that it is no longer the designated node, and the untraversed neighboring nodes screened out in step 22 are configured as new designated nodes; then, among the currently configured designated nodes, the designated node with the smallest moving cost from the navigation starting point is configured as the search starting point for the next screening of new neighboring nodes, and then returns to step 22; Step 24, repeating steps 22 to 23 until there is no designated node configured as a new search starting point, and determining that a regional connectivity graph is constructed.

12. The navigation path planning method according to claim 11, characterized in that: The currently configured search starting point and the neighboring nodes screened out in step 22 are configured as key values ​​to construct the regional connectivity graph in which any two nodes are interconnected; The search starting point and its corresponding neighboring nodes selected in step 22 support the construction of a traversable route; The region connectivity graph is a graph structure space and belongs to a data structure.

13. The navigation path planning method according to claim 12, characterized in that: In step 2, it also includes: Calculate the movement cost of each traversable node extracted in step 1 from the navigation starting point as the movement cost from the navigation starting point to the corresponding designated node; Then, the priority of the designated node is configured using the calculated movement cost corresponding to the designated node, and then the designated node with the configured priority is stored in the priority queue space, wherein the designated node with the highest priority currently stored in the priority queue space is first screened out and placed in the graph structure space to form the regional connectivity graph; Among them, the priority queue space is a data structure used to describe the order of entry and exit between designated nodes with different priorities.

14. The navigation path planning method according to claim 13, characterized in that: The priority configuration method includes: if the moving cost of the designated node from the navigation starting point is greater, the priority of the designated node is lower; if the moving cost of the designated node from the navigation starting point is smaller, the priority of the designated node is higher; so that the designated node with the smallest moving cost from the navigation starting point is output first from the priority queue space.

15. The navigation path planning method according to claim 14, characterized in that: When the designated node with the highest priority currently stored in the priority queue space is first screened out and configured as the search starting point, the designated node currently configured as the search starting point is removed from the priority queue space, and then the node screened out in the currently executed step 22 is set as the designated node and stored in the priority queue space, and at the same time, the designated node currently configured as the search starting point is stored in the traversed node set structure; The nodes screened out in step 22 are neighborhood nodes that are not pre-stored in the traversed node set structure, nor pre-exist in the priority queue space, and are within the reachable neighborhood of the currently configured search starting point.

16. The navigation path planning method according to any one of claims 11 to 15, characterized in that: The neighboring nodes within the reachable neighborhood of the currently configured search starting point satisfy the following conditions at the same time: The straight-line distance between the currently configured search starting point and its neighboring nodes is less than or equal to the robot's body diameter; The line between the currently configured search starting point and its neighboring nodes does not pass through obstacle grid points and does not pass through unknown grid points.

17. The navigation path planning method according to claim 16, characterized in that: The specific method of step 1 includes: Firstly, the midpoint of each of the mutually parallel motion trajectory segments is configured as the passable node; Then, starting from the midpoint of each motion trajectory segment, along the two ends of the corresponding motion trajectory segment, a node is extracted every sampling step of the fuselage diameter, and the extracted nodes are configured as the nodes with accessibility until the two endpoints of the corresponding motion trajectory segment are sampled.

18. The navigation path planning method according to claim 17, characterized in that: The step 1 also includes: If the distance between a node extracted on a corresponding motion trajectory segment according to a sampling step of the fuselage diameter and an end point of the same motion trajectory segment is less than one of the fuselage diameters, then the end point is also configured as the node with accessibility.

19. The navigation path planning method according to any one of claims 11 to 15, characterized in that: The tree structure is a Kd tree constructed on a two-dimensional data set, and the Kd tree represents a division of a two-dimensional coordinate space formed by the two-dimensional data set; The two-dimensional data set is constructed by the nodes with accessibility, including node coordinate values ​​and marking information of the corresponding motion trajectory segments.

Images