Method for constructing region connection graph based on robot motion trajectory

Abstract

The present invention discloses a method for constructing a regional connectivity graph based on a robot's motion trajectory. Step 1: Along each motion trajectory segment that is pre-marked and parallel to each other by the robot, nodes with passability are stepwise extracted from the midpoint to both ends. 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 in the tree structure; wherein, the navigation starting point is navigation information pre-configured into the robot. This enables the robot to 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.

Description

Technical Field

[0001] The present invention relates to the technical field of robot map construction, and relates to a method for constructing a regional connectivity graph based on the motion trajectory of a robot. Background Art

[0002] Graph search algorithms (A*, D*) are traditional path planning algorithms that rely heavily on grid maps. As the reading accuracy of the grid increases or the map area increases, their computational complexity increases exponentially with the increase of the map, and the search volume shows an explosive growth.

[0003] Currently, the mainstream probabilistic roadmap algorithms, although solving the problem of explosive growth of the search volume, have poor completeness of the constructed grid map, and it is not easy for the robot to search for a feasible path. Summary of the Invention

[0004] In order to solve the above technical problems, the present invention proposes a method for constructing a regional connectivity graph based on the motion trajectory of a robot, which screens out a suitable node set from the cleaning routes formed by the robot's traversal, and finally uses a traditional graph search algorithm to search for the screened node set to form a complete regional connectivity graph. The specific technical solutions are as follows:

[0005] The method for constructing a regional connectivity graph based on the motion trajectory of a robot includes: Step 1, in each of the mutually parallel motion trajectory line segments pre-marked by the robot, extract passable nodes step by step from the midpoint of the motion trajectory line segment to both ends along each of the mutually parallel motion trajectory line segments pre-marked by the robot; Step 2, first construct a tree structure based on the passable nodes extracted in Step 1, and then combine the moving cost of the specified node from the navigation start point, and search for a regional connectivity graph from the reachable neighborhood of the nodes of the tree structure; wherein, the navigation start point is the navigation information pre-configured in the robot. Compared with the prior art, this technical solution searches for nodes step by step from the midpoint of the cleaning trajectory to both ends, then uses the searched nodes to construct a tree data structure and performs a traditional graph search algorithm 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 computation amount, and at the same time can also connect more feasible paths and improve the completeness of regional reachability.

[0006] Furthermore, the specific method of step 2 includes: Step 21, configuring the specified node with the minimum movement cost from the navigation starting point as the search starting point; among them, the first node configured as the specified node is any traversable node extracted in step 1. Then enter step 22; Step 22, among the nodes of the tree structure constructed by the traversable nodes extracted in step 1, screen out the unvisited adjacent nodes within 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 steps 22 to 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 constraining 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 node set obtained in the last time has the best completeness and is interconnected, and these node sets form the regional connectivity graph.

[0007] Furthermore, 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 where any two nodes are interconnected; among them, the search starting point and its corresponding adjacent nodes screened in step 22 support the construction of a traversable route; among them, the regional connectivity graph is a graphical structure space and belongs to a data structure.

[0008] Further, in step 2, it further includes: calculating the movement cost of each passable node extracted in 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 stored priority in the priority queue space is first screened out and placed into the graph structure space to form the regional connectivity graph; where the priority queue space and the graph structure space are both data structures used to describe the connection between nodes; where the designated node belongs to the passable nodes extracted in step 1 and also belongs to the nodes on the tree structure. This technical solution uses a data structure to store the associated 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 searches; on the basis of the above technical solution, this technical solution is based on each passable node that can construct a tree structure extracted in 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 stored priority 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.

[0009] 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.

[0010] Further, when the designated node with the highest current stored priority 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 step 22 currently executed is set as the designated node and stored into the priority queue space, and at the same time, the designated node currently configured as the search starting point is stored into the traversed node set structure; where the node screened out in step 22 refers to the adjacent node in 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 exist in the priority queue space in advance. To avoid reconfiguring the previously configured search starting point searched in the reachable neighborhood of the currently configured search starting point as the new search starting point, resulting in repeated searches of nodes.

[0011] Furthermore, the adjacent nodes within the reachable neighborhood of the search starting point belonging to the current configuration simultaneously satisfy the following conditions: the straight-line distance between the search starting point of the current configuration and its adjacent nodes is less than or equal to the body diameter of the robot; the connection line between the search starting point of the current configuration 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.

[0012] Furthermore, the specific method of step 1 includes: first, configure the midpoint of each parallel motion trajectory segment as the node with passability; then, starting from the midpoint of each motion trajectory segment, extract a node at intervals of one body diameter along both ends of the corresponding motion trajectory segment, and configure the extracted node as the node with passability until sampling reaches the two endpoints of the corresponding motion trajectory segment. Thus, the passability of each motion trajectory segment is detected with the body diameter as the step size, improving the extraction efficiency of nodes with passability and increasing the construction accuracy of the tree structure.

[0013] Furthermore, step 1 also includes: if the distance between a node extracted on a corresponding motion trajectory segment at a sampling step size of one body diameter and one endpoint of the same motion trajectory segment is less than one body diameter, then configure this endpoint as the node with passability. This improves the redundancy of node extraction and improves the completeness of the regional connectivity graph.

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

[0015] Figure 1 is a flowchart of a method for constructing a regional connectivity graph based on a robot motion trajectory disclosed in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0016] The following further describes the specific embodiments of the present invention with reference to the accompanying drawings.

[0017] As is known to those skilled in the art, the bow-shaped cleaning method is the mainstream method for floor cleaning robots to complete coverage cleaning. In this embodiment, the cleaning trajectory generated by the cleaning robot in the cleaned area is a bow-shaped cleaning route. Specifically, the cleaning trajectory generated by the cleaning robot in the cleaned area is composed of multiple parallel moving trajectory segments. Among them, the vertical distance between two adjacent parallel moving trajectory segments (adjacent in position) is less than the body diameter of the floor cleaning robot. There is an endpoint of two adjacent parallel moving trajectory segments connected by a bent line or a short line segment (shorter than the parallel moving trajectory segment). Those skilled in the art can understand that these parallel moving trajectory segments belong to the long-side cleaning route of the bow-shaped cleaning route, and the aforementioned bent line or short line segment is the short-side cleaning route between two adjacent moving trajectory segments, so that these parallel moving trajectory segments cover the reachable area of the cleaning robot. At the same time, the cleaning robot marks path nodes along the moving trajectory segments and maps them to the grid map. The aforementioned short line segment can be set perpendicular to the moving trajectory segment or the initial cleaning direction. It should be noted that in addition to being equipped with specific cleaning devices and used for performing cleaning operations, other types of robots can also automatically move along the bow-shaped cleaning route.

[0018] It should be noted that for those skilled in the art, it is understandable that the grid map constructed in real time is marked with environmental information around the current position of the robot. The grids in the map area constructed by the robot include three states: free, occupied, and unknown; these grids are represented by grid points in the moving trajectory segments of 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 position points that the robot can reach, and are free grid points, which 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 position points where the specific situation is not clear during the process of the robot constructing the map, and are often blocked by obstacles, and can form an unknown area.

[0019] Currently, mainstream random roadmap algorithms (such as the PRM algorithm) randomly generate a sufficient number of free grid points in an instantaneously constructed grid map. Secondly, a certain strategy is adopted to directly connect the generated random free grid points into feasible edges to form a graph, which is the generated roadmap or connected graph. These random free grid points are all free nodes that are evenly distributed with equal probability within the grid map, but this leads to the disadvantage of poor completeness in generating the roadmap. It should be noted that the difference between the random roadmap algorithm and traditional graph search algorithms is that the roadmap is not constructed in a deterministic manner in the planning space, but is constructed based on probability theory using a method of a certain random probability. Among them, traditional graph search algorithms include but are not limited to the A* algorithm, D* algorithm, and Dijkstra algorithm. Further, after the random roadmap algorithm generates the roadmap, traditional graph search algorithms such as A* are used to perform path planning on the roadmap, such as path planning or querying for the optimal path, so that the random roadmap algorithm can find a navigation path with relatively few random sampling points in an environment with fewer obstacles. However, when there are many obstacles, too few sampling nodes, the distribution of nodes in the corresponding area is unreasonable, or in other specific motion environments, the nodes sampled by the random roadmap algorithm may fail in path planning. Therefore, the routes planned by the random roadmap algorithm become incomplete.

[0020] To solve the foregoing technical problems and obtain a roadmap with higher accuracy and higher completeness in the application scenario of the actual work of the robot, an embodiment of the present invention discloses a method for constructing a regional connected graph based on the motion trajectory of the robot, as Figure 1As shown in the figure, it includes: Step 1, respectively along each pre-marked parallel motion trajectory line segment of the robot, step-by-step extract passable nodes from the midpoint of each motion trajectory line segment to both ends, that is, taking a specific step length as a sampling interval (understood as step-by-step), and extract passable nodes; after extracting all the passable nodes on all the motion trajectory line segments in the area covered by the robot's work, enter Step 2; Step 2, based on the passable nodes extracted in Step 1, construct a tree structure, and then combine the movement cost of the specified node from the navigation starting point, and search for a region connectivity graph from the reachable neighborhood of the nodes of the tree structure; among them, the navigation starting point is the navigation information pre-configured in the robot; it should be noted that the specified node belongs to the node to be traversed, and the specified node has the priority of traversal, and is used as a candidate path node, so that the nodes selected from the reachable neighborhood of the nodes of the tree structure can form a more complete region connectivity graph. Compared with the existing random roadmap algorithm and graph search algorithm, the region connectivity graph searched in Step 2 has better connectivity in the covered area, supports the construction of more feasible paths, and relatively expands the mobile coverage range of the robot. Compared with the existing technology, in this embodiment, nodes are searched step-by-step from the midpoint of the cleaning trajectory to both ends, and then a tree data structure is constructed using the searched nodes and a traditional graph search algorithm is performed on the tree data structure to search for a region connectivity graph, so that the robot can use the region 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.

[0021] It should be noted that all the motion trajectory line segments mentioned in this embodiment are a group of parallel line segments. Only the parallel motion trajectory line segments formed by the movement of the robot are allowed to extract the passable nodes. In one embodiment, the parallel long side line segments of the bow-shaped cleaning route executed by the robot are the effective motion trajectory line segments.

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

[0023] Step 21, configure the specified node with the minimum movement cost from the navigation starting point as the search starting point; then enter Step 22; among them, the navigation starting point is the navigation information pre-configured in the robot, and it is the path starting point pre-set for the traditional graph search algorithm (including but not limited to A* algorithm, D* algorithm, Dijkstra algorithm) to iteratively calculate the movement cost (shortest path); the search starting point is the currently traversed node set in the current iteration calculation, and supports being updated in the next iteration calculation. Among them, the node first configured as the specified node is any passable node extracted in Step 1.

[0024] 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 search starting point of the current configuration, and configure all the currently filtered-out 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 the 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 the adjacent nodes within the neighborhood of a node (such as the eight-neighborhood, four-neighborhood, etc. neighborhood range), which are equivalent to the child nodes of a parent node.

[0025] Step 23: Mark the currently configured search starting point and the 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. This 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 out repeatedly to form the regional connectivity graph, nor can they be searched out repeatedly and used to configure the search starting point for filtering out new adjacent nodes next time. Among the adjacent nodes searched out within the reachable neighborhood of the currently configured search starting point, there may be the search starting point configured last time. Then, configure the unvisited specified node with the minimum movement cost from the navigation starting point as the search starting point for filtering out new adjacent nodes 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 last time, including excluding the nodes filtered out in Step 22 last time, selects the specified node with the minimum movement cost from the navigation starting point among all the newly configured specified nodes and configures it as the search starting point for filtering out new adjacent nodes next time, so as to update the search starting point currently configured in Step 21, obtain the search starting point for constructing the passable route next time, and then return to Step 22.

[0026] Step 24. Repeat steps 22 to 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 processing, rather than 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.

[0027] In the foregoing embodiments of steps 21 to 24, starting from the navigation starting point, adopting the strategy of the greedy algorithm, each time traverse 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 until there is no suitable node as a new search starting point. Thus, by restricting a reachable neighborhood to search for un-traversed adjacent nodes and updating the shortest path distance information in continuous iterative processing, to ensure that the best completeness and interconnected roadmap (equivalent to the regional connectivity graph) is obtained in the last iteration.

[0028] It should be added that Dijkstra's 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, ensure the optimal solution under the known conditions or all the conditions currently available. If a better solution is generated due to the addition of new conditions in subsequent iterations, then replace the previous optimal solution. By continuously iterating to ensure that the result of each iteration is the current optimal solution, then when iterating to the last round, the global optimal solution will be obtained.

[0029] In the foregoing embodiments, the currently configured search starting point and the 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 the nodes screened in step 22 corresponding thereto. 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 graphic structure is a data structure in which the relationship between elements is arbitrary. In the graphic structure, the number of precursor 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 graphic structure can be connected. It is beneficial to realize the construction of a regional connectivity graph in which any two points are connected and reachable between the navigation starting point and the navigation ending point.

[0030] As an embodiment, in step 2, it further includes:

[0031] Calculate the movement cost of each passable node extracted in Step 1 from the navigation starting point respectively, 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, which is convenient for accessing node information in order and improves 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.

[0032] Then store the specified nodes with configured priorities 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 first screened out 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. Specifically, during the repeated execution of 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.

[0033] When the specified node with the highest current priority 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, 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 searched repeatedly, 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 previously 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, so as 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.

[0034] Those skilled in the art easily know 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 entry and exit order relationship between nodes with different priority levels.

[0035] This embodiment uses a data structure to store the relevant information of the specified 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 node to avoid repeated search; on the basis of the foregoing embodiment, this embodiment combines the neighbor relationship of each node in the tree to calculate the movement cost between nodes and use it to configure the priority of the specified node based on each traversable node that can construct a tree structure extracted in step 1 above, and then sort the specified nodes according to the priority to facilitate dequeueing from the priority queue where it is located, so that the specified 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 the node information in order and improves the acquisition efficiency of the connectable nodes.

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

[0037] (1) The straight-line distance between the search starting point of the current configuration and its adjacent nodes is less than or equal to the body diameter of the robot, indicating that the distance dimension between the search starting point of the current configuration and the currently screened adjacent nodes is within the range of one body diameter, enabling the robot located at the search starting point of the current configuration 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.

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

[0039] Among them, the vertical distance between two adjacent motion trajectory line segments that are parallel to each other and 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.

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

[0041] First, configure the midpoint of each of the mutually parallel motion trajectory line segments as the node with passability. 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 of one body diameter, and configure the extracted nodes as the nodes with passability, so as to extract the nodes with passability 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, detect the passability of each motion trajectory line segment with the body diameter as the step, improve the extraction efficiency of the nodes with passability, and increase the construction accuracy of the tree structure. Among them, the sampling step belongs to the sampling distance with a step of one body diameter.

[0042] 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.

[0043] 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 kept as 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 regional connectivity graph will not be reduced, and the algorithm will not fail to find a feasible path.

[0044] 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 regional connectivity graph.

[0045] 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 from 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 art 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 ordered 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.

[0046] 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 take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take 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.

[0047] As described above, it is only the preferred embodiment of the present invention, and it is not intended to limit the present invention in other forms. Any person skilled in the 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 solution content of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims

1. A method for constructing a region connection graph based on a robot's motion trajectory, characterized in that Including: Step 1: Along each pre-marked parallel motion trajectory segment of the robot, stepwise extract passable nodes from the midpoint of the motion trajectory segment towards both ends; Step 2: Based on the passable nodes extracted in Step 1, construct a tree structure, and then, in combination with the movement cost of the specified node from the navigation starting point, search for a regional connectivity graph within the reachable neighborhood of the nodes of the tree structure; wherein, the navigation starting point is navigation information pre-configured in the robot; 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; then enter Step 22; wherein, the node first configured as the specified node is any one of the passable nodes extracted in Step 1; Step 22: Among the nodes of the tree structure constructed by the passable nodes extracted in Step 1, screen out the unvisited adjacent nodes within the reachable neighborhood of the currently configured search starting point and configure all the currently screened out nodes as specified nodes, then enter Step 23; wherein, there is no pre-configured specified node among the unvisited neighborhood nodes screened out in Step 22; Step 23: Combine the currently configured search starting point and the nodes screened out in Step 22 to form the regional connectivity graph, 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.

2. The method for constructing a region connection graph according to claim 1, characterized in that The currently configured search starting point and the adjacent nodes screened out in Step 22 are both configured as key values to construct the regional connectivity graph where any two nodes are interconnected; Wherein, the search starting point and its corresponding adjacent nodes screened out in Step 22 support constructing a passable route; Wherein, the regional connectivity graph is a graphical structure space.

3. The method for constructing a region connection graph according to claim 2, wherein In Step 2, it further includes: Calculate the movement cost of each passable node extracted in Step 1 from the navigation starting point as the movement cost of the navigation starting point to the corresponding specified node; Then configure the priority of the specified node using the calculated movement cost corresponding to the specified node, and then store the specified node with the configured priority into the priority queue space. Among them, the specified node with the highest current priority stored in the priority queue space is first screened out and placed into the graphical structure space to form the regional connectivity graph; Wherein, the priority queue space is a data structure used to describe the entry and exit sequence relationship between nodes with different priority levels.

4. The method for constructing a region-connected graph according to claim 3, wherein The method for configuring priorities includes: if the movement cost of a specified node from the navigation starting point is greater, the priority of this specified node is lower; if the movement cost of a specified node from the navigation starting point is smaller, the priority of this specified node is higher; so that the specified node with the smallest movement cost from the navigation starting point is the first to be selected from the priority queue space.

5. The method for constructing a region connection graph according to claim 4, wherein When the specified node with the highest current priority stored in the priority queue space is first selected 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 selected in step 22 currently being executed is set as the specified node and stored in the priority queue space, and at the same time, the specified node currently configured as the search starting point is stored in the traversed node set structure; Among them, the node selected 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.

6. The method for constructing a region connection graph according to any one of claims 1 to 5, characterized in that, The adjacent nodes belonging to 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.

7. The method for constructing a region-connected graph according to claim 6, wherein The specific method of step 1 includes: First, the midpoint of each parallel movement trajectory line segment is configured as the passable node; Then, starting from the midpoint of each movement trajectory line segment, nodes are extracted at intervals of one body diameter sampling step along both ends of the corresponding movement trajectory line segment, and the extracted nodes are configured as the passable nodes until the two endpoints of the corresponding movement trajectory line segment are sampled.

8. The method for constructing a region connection graph according to claim 7, wherein Step 1 also includes: If the distance between a node extracted on a corresponding movement trajectory line segment at a sampling step of one body diameter and one endpoint of the same movement trajectory line segment is less than one body diameter, then this endpoint is also configured as the passable node.

9. The method for constructing a region connectivity graph according to any one of claims 1 to 5, characterized in that, 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; Among them, the two-dimensional data set is constructed by the passable nodes, including node coordinate values and marker information of the movement trajectory line segment to which they belong.

Images