Route fusion planning method for traffic areas, robot and chip

Abstract

Provided are a route fusion planning method, robot and chip for a travel area, in which a robot searches in advance for fusionable candidate routes within a narrow passageway where many obstacles are distributed, and then uses the route fusion planning method to plan an overall navigation route by fusing a heuristic search algorithm with the candidate routes that meet the search conditions, and overcomes marking errors of the map grid, thereby solving the problem of planning a navigation route for a robot within a passable area where the passageway in the grid map is narrow and marking errors are likely to occur.

Description

[Technical field]

[0001] The present invention relates to the technical field of robot path planning, and more particularly to a method for path fusion planning in a traffic area, a robot, and a chip. [Background technology]

[0002] A mobile robot in the prior art is a robot that autonomously detects the surrounding environment using sensors, determines the movement of the device body using a controller, and realizes the movement using actuators such as wheels. In a home environment, cleaning robots in the prior art often move into narrow passages formed by restrictions between various furniture components, such as the four legs of a stool or the entrance of a living room table, or narrow passages formed by opening a room door. When a robot enters these narrow passages, the robot may slip, there may be accumulated errors in the positioning sensor, or there may be errors in the optimization of the visual map, so the robot is likely to mark these narrow passages as areas occupied by obstacles on the grid map constructed in real time, and as a result, the entrances of the narrow passages mapped on the grid map constructed by the robot are blocked, but are not really blocked in the actual movement scene. For this reason, based on the conventional path search algorithm, it is not possible to bypass the obstacle grid marked by the error of the map, which is disadvantageous for the robot to rely on navigation to enter and exit passable areas where marking errors are likely to occur due to the narrowness of the passages. Summary of the Invention [Problem to be solved by the invention]

[0003] The present invention provides a method for planning a route fusion in a travel area, a robot, and a chip. To solve the problem of a robot entering and exiting a travelable area in a grid map where the passage is narrow and marking errors are likely to occur, the robot searches in advance for candidate routes that can be fused in a narrow passage where many obstacles are distributed, and then uses the route fusion planning method to plan an overall navigation route by fusing a heuristic search algorithm with the candidate routes that meet the search conditions, and overcomes the marking errors of the grid of the map. [Means for solving the problem]

[0004] Specifically, the configuration is as follows. A method for planning route fusion in a travel area, comprising: step 1 of setting a navigation start point and a navigation end point in a lattice map and creating a traverse target node cache space; step 2 of setting the navigation start point as a current parent node and adding it to the traverse target node cache space; step 3 of performing a neighborhood search in the lattice map with the current parent node as a search center, in which eight lattice points adjacent to the current parent node are set as its child nodes; and step 4 of adding the other end point of the corresponding candidate route to the traverse target node cache space if the child node searched for in the neighborhood in step 3 is one end point of a corresponding candidate route in a candidate route coordinate set previously searched, and simultaneously setting all intermediate nodes between both end points of the corresponding candidate route and these end points as nodes that cannot be repeatedly searched, and adding the free lattice points searched for in step 3 that are not on the corresponding candidate route to the traverse target node cache space, and then proceeding to step 5, The method includes: step 4, in which the corresponding candidate route in the candidate route coordinate set is allowed to pass through the obstacle lattice points marked on the lattice map, and all nodes added to the traversal target node cache space have their parent node location information recorded for subsequent route node traceback; and step 5, in which a node with a minimum total path cost value is selected as a next parent node from the traversal target node cache space mentioned in step 4, and then judge whether the next parent node is the navigation end point, and if YES, plan a path from the navigation start point to the navigation end point by sequentially connecting child nodes and their parent nodes from the navigation end point, except for all intermediate nodes of the candidate route mentioned in step 4 and their corresponding one end point, until they are connected to the navigation start point, based on the recorded parent node location information; if NO, update the next parent node to the current parent node mentioned in step 3, and then return to step 3.

[0005] Compared with the prior art, this path planning scheme starts from the navigation start point, sequentially selects the node with the smallest total path cost value from the traversal target node cache space as the parent node to perform a neighborhood search, and adds all the nearby searched free lattice points that are not on the candidate route and one end point (start point or end point) on the candidate route with discriminatory value to the traversal target node cache space, and records the corresponding parent node as the path node retrospective information. At the same time, it eliminates the interference of the intermediate nodes on the candidate route (no matter which one is the navigation goal point suitable for these narrow passages) to the repeated search, and when it becomes the navigation end point as the parent node, it further connects the searched candidate route from the navigation end point according to the positional relationship between the child node and the parent node, and connects it backward to the navigation start point, so that the candidate route that meets the passage conditions is integrated into the mature path search algorithm to plan the navigation path, overcoming the narrowness of the narrow passage mapped on the grid map and prone to marking errors, allowing the robot to effectively enter and exit this narrow passage along the planned overall navigation path, and reducing the probability of path navigation failure.

[0006] Furthermore, step 4 further includes, if the child node searched for in the vicinity in step 3 is not an end point of all the candidate routes in the candidate route coordinate set, adding all the free lattice points searched for in the vicinity in step 3 to the traversal target node cache space, recording the position information of the parent node of the free lattice point searched for in the vicinity in step 3, and then proceeding to step 5. This not only expands the search range of the child node, but also contributes to expanding the search of the start point and end point of the route in the candidate route coordinate set in a wider range, making the planned route more practical and representative.

[0007] Furthermore, in step 4, if the child node searched for nearby in step 3 is the starting point of a corresponding candidate route in the candidate route coordinate set, the end point of the corresponding candidate route is added to the traversed node cache space, and the position information of the parent node of the end point of the corresponding candidate route is recorded so that each candidate route uses its end point as route identification information in the candidate route coordinate set, and at the same time, all intermediate nodes between both end points of the corresponding candidate route and the starting point of the same candidate route are all set as nodes that cannot be repeatedly searched, and here, along the path extension direction from the starting point of the corresponding candidate route toward its end point, the parent node of each node on the corresponding candidate route is located at the node position adjacent to that node.

[0008] Furthermore, in step 4, if the child node searched for nearby in step 3 is the end point of the corresponding candidate route in the candidate route coordinate set, the start point of the corresponding candidate route is added to the traversal target node cache space, and the position information of the parent node of the start point of the corresponding candidate route is recorded so that each candidate route uses its start point as route identification information in the candidate route coordinate set, and at the same time, all intermediate nodes between both end points of the corresponding candidate route and the end point of the same candidate route are all set as nodes that cannot be repeatedly searched, and here, along the path extension direction from the end point of the corresponding candidate route toward its start point, the parent node of each node on the corresponding candidate route is located at the node position adjacent to that node.

[0009] According to the two technical proposals corresponding to step 4, the starting point or the end point of the candidate route is configured as a node that can uniquely identify a candidate route, and is used as an identifier of the corresponding candidate route to record the corresponding parent node information, while the remaining nodes of the same candidate route are configured as repeatedly searched nodes, thereby reducing the amount of searching during route planning.

[0010] Furthermore, in step 5, sequentially connecting child nodes and their parent nodes from the navigation end point, except for all intermediate nodes and their corresponding one end point of the candidate route described in step 4, until they are connected to the navigation start point based on the position information of the recorded parent node, specifically includes step 51 of connecting the navigation end point and its parent node from the navigation end point based on the position information of the recorded parent node, and then proceeding to step 52; step 52 of treating the currently determined parent node as a child node, and further connecting the parent node of this child node based on the position information of the recorded parent node; step 52 is repeated until one end point of the corresponding candidate route described in step 4 is connected, and then from the other end point of the corresponding candidate route described in step 4, the other end point of the corresponding candidate route described in step 4 and its parent node, and then returning to step 52; and step 54 of obtaining a navigation route from the navigation start point to the navigation end point by reverse connection by repeating steps 52 and 53 until they are connected to the navigation start point.

[0011] Compared with the prior art, after planning the above navigation path, according to this technical solution, in the process of the robot passing through a narrow passage, when searching for a path using a heuristic search algorithm or an incremental heuristic search algorithm, when a narrow passage is detected, the robot will switch from entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm to entering the candidate route near the narrow passage; on the other hand, when it is detected that the robot has left the narrow passage, the robot will switch from entering the candidate route to entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm, thereby enabling the robot to pass through the narrow passage effectively, and improving the success rate of navigation to overcome the narrow passage.

[0012] Furthermore, in step 5, the total value of the path cost is obtained by adding or weighting the traversed path cost and the predicted path cost, where the traversed path cost is the cost from one specified node in the traversed node cache space to the navigation start point, and the predicted path cost is the cost from the same specified node in the traversed node cache space to the navigation end point, and the smaller the total value of the path cost is, the higher the traversal priority of the specified node in the traversed node cache space is set, where the moving cost between two adjacent nodes is expressed by Manhattan distance, diagonal distance, or Euclidean distance. It contributes to finding the shortest path with the lowest navigation cost.

[0013] Furthermore, from the navigation start point, the center point of each traversed lattice is sequentially connected until it is connected to the specified node, and the length of the currently connected line is calculated from the length of the lattice side, and the length is set as the traversed path cost, and the specified node is set as the parent node, and all line connections from each child node corresponding to this parent node to the navigation end point are obtained, and the length of each connection line corresponding to each child node is calculated from the length of the lattice side, and the shortest length corresponding to each child node is selected as the corresponding predicted path cost, where the lattice point is represented by the center point of the lattice and is used to represent the position characteristics of the lattice. The method of calculating the path cost is simplified. Here, the cost value calculated in this technical solution is a measure of the cost of the robot's motion trajectory, and represents the cost of traveling from the start point to the specified node and then to the end point, and includes the path length.

[0014] In addition, the path fusion planning method further creates a traversed node cache space for storing the nodes that are set as non-searchable repeatedly in step 4, and the nodes existing in the traversed node cache space are prohibited from being added to the traversed node cache space. In the planning process, it is realized to identify the searched nodes or the nodes that are considered to be searched nodes.

[0015] Furthermore, step 4 further includes, if a free lattice point in the vicinity of the current parent node that is not located on the corresponding candidate route or all free lattice points in the vicinity of the current parent node are added to the traversed node cache space, deleting the current parent node from the traversed node cache space and then adding the current parent node to the traversed node cache space, thereby preventing the same search center from being repeatedly expanded.

[0016] Furthermore, before executing the route fusion planning method, the search method for the corresponding candidate route in the candidate route coordinate set includes, specifically, a step S1 of proceeding to step S2 when it is determined that the pre-search area satisfies a first preset circular area passage condition in the process of controlling the robot to move along a preset route, a step S2 of recording the current position of the robot as a first route node, creating a new predicted passage coordinate set, storing the first route node in the predicted passage coordinate set, and then proceeding to step S3, a step S3 of controlling the robot to continue moving along the preset route, and proceeding to step S4 when it is determined that the robot has moved to a position where the straight-line distance to the latest recorded route node is equal to or greater than the diameter of the robot's body, a step S4 of recording the current position of the robot as a second route node, and determining whether the second route node satisfies a second preset circular area passage condition, and if YES, proceeding to step S5, and if NO, proceeding to step S6, and determining that the robot is currently in the narrow passage. and step S5 of determining that the robot is not currently on the narrow path, storing the predicted traffic coordinate set in a same candidate route coordinate set according to the number of path nodes present in the predicted traffic coordinate set, connecting the path nodes stored in the predicted traffic coordinate set according to the order in which they were added to form a corresponding candidate route, and then returning to step S1. In this case, in the same candidate route coordinate set, a leading element and a trailing element in any of the predicted traffic coordinate sets are unique, and a leading element and a trailing element in the same predicted traffic coordinate set are not the same, in which the narrow path is a gap passage between two or more obstacles, and the gap passage is a gap corresponding to the narrowest part between two obstacles, and the width of the gap is equal to or greater than the diameter of the robot's body, in which the predetermined path is a path planned in advance by the robot, and the path nodes can be represented by lattice points.

[0017] Compared with the prior art, this technical solution is suitable for performing route searching when the robot enters a narrow passage, and firstly sets a first preset circular area passing condition as a narrow passage pre-judgment condition, provides a source of route nodes of the candidate route corresponding to the predicted passage coordinate set, and then continues to move the robot along the original route, and then sets a second preset circular area passing condition as a narrow passage refinement judgment condition, and continues to provide a source of grid points of the candidate route corresponding to the predicted passage coordinate set. Thus, the robot is only qualified to judge the narrow passage after moving a certain distance, and the candidate route for connecting grid points collected in the predicted passage coordinate set is more complete and more suitable for the error environment of the map grid in the narrow passage, and can provide the robot with a route that is actually passable without paying attention to the marking information of the corresponding grid in real time. Also, based on this, if the corresponding preset circular area passing condition is not met, stops searching for the predicted passage coordinate set, and determines that the route nodes in the single predicted passage coordinate set can be connected to form an independent candidate route. By repeatedly performing the above related steps, the robot searches for a set of route points that can eliminate the problem of grid impassability caused by map drift errors in normal working movement state, thereby improving the success rate of the robot in finding an effective driving path in a scene with a complex spatial layout of obstacles.

[0018] Furthermore, when it is detected that the robot has changed from a state in the narrow passage to a state in which it is not in the narrow passage, the execution of the path fusion planning method is started by executing the steps 1 to 5. By realizing the iterative processing of the steps 1 to 5, when the robot exits the narrow passage, the coordinate points of the searched candidate routes suitable for passing through the narrow passage can be merged with the use of a heuristic search algorithm at the current position to plan a navigation path, and since the navigation path planning does not rely only on the neighborhood search, the free lattice points searched in the narrow passage are easily marked as obstacle lattice points, which solves the problem that the narrow passage cannot be passed on the planned path.

[0019] Furthermore, the method of storing the predicted traffic coordinate sets in the same candidate route coordinate set according to the number of route nodes present in the predicted traffic coordinate set specifically includes: when the number of route nodes present in the predicted traffic coordinate set is less than two, deleting the predicted traffic coordinate set, and returning to step S1 to create a new predicted traffic coordinate set; when the number of route nodes present in the predicted traffic coordinate set is two or more, storing the predicted traffic coordinate set in the candidate route coordinate set, correspondingly storing a new candidate route, and returning to step S1 to create a new predicted traffic coordinate set, where the first route node and the second route node are added to the predicted traffic coordinate set according to the recorded order, so that the route nodes stored in the predicted traffic coordinate set are connected in a certain order to form the corresponding candidate route, where the route node corresponding to the head element in each predicted traffic coordinate set is the start point of the corresponding candidate route, and the route node corresponding to the tail element in each predicted traffic coordinate set is the end point of the corresponding candidate route. Thus, the route nodes in each predicted travel coordinate set are connected together to form one candidate route, which is predicted to allow the robot to travel through the corresponding area without any hindrance. In this technical solution, when the number of route nodes finally obtained in one predicted travel coordinate set is so small that it is difficult to connect them with a line, invalid route nodes can be deleted and reduced. The route nodes finally obtained in one predicted travel coordinate set are stored as a whole in the candidate route coordinate set for storing one candidate route, so that the storage and retrieval structure of the candidate route is rational and orderly.

[0020] Furthermore, the first preset circular area passage condition includes that the ratio of the area occupied by the first non-passable area in the preliminary search area is greater than a first passage evaluation value, where the first non-passable area is a grid area consisting of unknown grid points and obstacle grid points in a grid area corresponding to the first circular area, and the first passage evaluation value is a pre-judgment threshold value set to overcome the marking error of the free grid present in the constructed grid map, where the preliminary search area is a first circular area with the current position of the robot as the center of the circle and the diameter of the robot's body as the radius. This is a rough judgment condition for pre-judging whether the robot has started to enter a narrow passage, and the subsequent judgment depends on the robot making further judgments during continued movement. However, there is no need to consider whether the marking information of a single grid allows the robot to pass, which reduces the impact of the marking error of a single grid.

[0021] Furthermore, the second preset circular area passage condition includes that within a second circular area with the robot's current position as the center and the robot's body diameter as the radius, the proportion of the area occupied by the second impassable area is greater than a second passage evaluation value, where the second impassable area is a grid area consisting of unknown grid points and obstacle grid points within a grid area corresponding to the second circular area, and the second passage evaluation value is a judgment threshold set to overcome marking errors of free grids existing in the constructed grid map and is greater than the first passage evaluation value. According to this technical solution, based on the first circular area or the previously searched second circular area, after the robot moves to a location away from the latest recorded route node by the robot's body diameter, the proportion of the area occupied by the second impassable area is further judged, and there is no need to consider the influence of marker information of a single grid point, thereby improving the judgment accuracy of a narrow passage.

[0022] Furthermore, the second preset circular area travel condition includes searching for a path from the latest recorded second path node to the latest recorded first path node using a path search algorithm in a second circular area with the latest recorded first path node as the center of the circle and the preset multiple of the aircraft diameter as the radius, where the preset multiple of the aircraft diameter is set so that the second circular area does not intersect with other marked grid areas. Compared with the prior art, the technical solution reasonably sets the size of the search area of ​​the second circular area, thereby avoiding the robot being guided to other areas and not being guided to pass through the current narrow passage because it intersects with other known map areas and the planned path in the related area is contained. Meanwhile, further, by determining whether a complete navigation path from the start point to the end point can be planned in the currently searched second circular area, it can be proved that the passable area of ​​the second circular area is no longer affected by obstacles or is no longer affected by the marking position of the obstacle grid, thereby improving the judgment accuracy of the narrow passage.

[0023] A chip, storing a program code for executing a path fusion planning method according to any of the above technical solutions, which plans a navigation path in a narrow passage with many obstacles by integrating a heuristic search algorithm and a candidate route that meets a search condition, thereby eliminating the narrow passage in the map grid that is prone to marking errors, allowing the robot to effectively enter and exit the narrow passage along the planned overall navigation path, and reducing the probability of path navigation failure in a scene with a complex layout of the obstacle space.

[0024] A robot includes the chip for controlling the robot to execute the path fusion planning method, and in the process of the robot passing through a narrow passage according to a navigation path planned by the path fusion planning method, when the robot searches for a path using a heuristic search algorithm or an incremental heuristic search algorithm, if a narrow passage is detected, the robot switches from entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm to entering the candidate route near the narrow passage, while if the robot detects that it has left the narrow passage, the robot switches from entering the candidate route to entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm, thereby enabling the robot to effectively pass through the narrow passage and increasing the success rate of navigation to overcome the narrow passage. [Brief description of the drawings]

[0025] [Figure 1] 2 is a flowchart of a traffic area route merging planning method according to an embodiment of the present invention. [Diagram 2] 4 is a flowchart of a method for searching for corresponding candidate routes in a set of candidate route coordinates according to another embodiment of the present invention; DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0026] Hereinafter, specific embodiments of the present invention will be further described with reference to the drawings.

[0027] It will be understood by those skilled in the art that the lattice map is marked with environmental information around the robot's current position, and the lattices in the map area constructed by the robot are marked with three states: free, occupied, and unknown, that these lattices are represented by lattice points which are the center points of the lattices in this embodiment, that a lattice point in a free state refers to a lattice that is not occupied by an obstacle, is a lattice position point that the robot can reach, is a free lattice point, and can constitute an unoccupied area, that a lattice point in an occupied state refers to a lattice that is occupied by an obstacle, is an obstacle lattice point, and can constitute an occupied area, and that an unknown lattice point refers to a lattice area whose specific situation is unclear during the robot's map construction, and the position point is often blocked by an obstacle, and can constitute an unknown area.

[0028] In addition, when solving a problem using a search algorithm, it is necessary to construct a data structure that represents the relationship between the state characteristics of the own position and the states of other positions, and this data structure is called a node. Different problems need to be described with different data structures. Depending on the conditions given in the search problem, one or more new nodes can be generated starting from one node, and this process is usually called expansion. The relationship between nodes can generally be expressed 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 a node that matches the goal state.

[0029] In addition, the intelligent cleaning robot of the prior art often moves through narrow passages formed by the restrictions between various furniture components in a home environment, such as the four legs of a stool or the entrance of a living room table, or the narrow passage formed by an open door of a room, but it should be noted that the narrow passage is a gap passage of each obstacle, and the gap passage of each obstacle is a gap corresponding to the narrowest part between two obstacles, and the width of this gap is larger than the diameter of the robot's body, allowing the robot to pass through. Because the robot may slip, because there may be accumulated errors in the positioning sensor, and because errors may also occur in the optimization of the visual map, the robot is likely to mark the narrow passage as an area occupied by an obstacle on the grid map constructed by real-time scanning, i.e., by mistakenly marking the free grid point where the narrow passage was originally mapped as an area occupied by an obstacle, the entrance of the narrow passage mapped on the grid map constructed by the robot is blocked (but is not really blocked in the actual motion scene), so the robot cannot complete a navigation path that passes through the narrow passage using a mature path search algorithm, for example, a heuristic search algorithm such as the A* algorithm or an incremental heuristic search algorithm such as the D* algorithm.

[0030] For this purpose, the present invention discloses a method for planning a route fusion in a travel area. In the present invention, the robot is first controlled to traverse narrow passages with many obstacles in advance in the original normal navigation operation state to search for candidate routes that can be fused, and then the route fusion planning method is used to plan an overall navigation route by fusing a heuristic search algorithm (mainly the search idea of ​​the route node of this type of search algorithm) with the candidate routes that meet the search conditions, and overcome the marking error of the map grid. This solves the problem of planning a navigation route for the robot in a passable area where the passages in the grid map are narrow and marking errors are likely to occur.

[0031] As an embodiment, the present invention discloses a route fusion planning method for a traffic area, which can be performed after a robot completely leaves the narrow passage or immediately before leaving the current narrow passage and entering a new narrow passage. As shown in FIG. 1, the route fusion planning method includes the following steps S101 to S111. In step S101, a navigation start point and a navigation end point are set in a lattice map, a traversal target node cache space is created, and then the process proceeds to step S102. Here, the lattice map is a local map including a start point and an end point constructed by a robot, and the lattice map also includes obstacle information of various layouts, and includes a sufficient spatial area for the robot to plan a path trajectory according to the navigation start point and the navigation end point. The navigation start point and the navigation end point can be represented by the lattice coordinates of the lattice map, the lattice center point coordinates, or other types of navigation data, and are not limited thereto, and any of them can be converted for the lattice map to participate in the route planning. On the other hand, in step S101, a traversal target node cache space is created and initialized to be empty, and the priority of its internal elements is related to the route cost, and the priority of the elements can be the traversal priority and the planning priority.

[0032] In step S102, the navigation start point is set to the current parent node, specifically, the lattice or lattice point where the navigation start point exists is set to the current parent node, and added to the traversed node cache space, and then proceeds to step S103. In one optional embodiment, before adding the navigation start point to the traversed node cache space, the initial traversed node cache space may be empty, in which case the navigation start point becomes the first node added to the traversed node cache space. Preferably, the parent node located at the origin of the robot coordinate system (origin of the local coordinate system) is adjacent to any of the child nodes expanded and searched within the four quadrant areas.

[0033] Here, the traversal target node cache space is regarded as a set of candidate path nodes used in the path planning of this embodiment, and path nodes that the robot may pass through are added to it for subsequent screening processing. The traversal target node cache space can record the position of each path node therein and the state in which the robot moves to the corresponding path node. In this exemplary embodiment, for a path node in a lattice map, a state parameter represents the state when the robot moves and reaches the path node. The state parameter of a path node includes at least a time, i.e., the time when the robot reaches this path node. In addition, the state parameter may further include a direction when the robot reaches the path node, a posture including a steering angle (i.e., whether the robot itself is in a straight state or a steering state, and the steering angle), and a speed, i.e., a movement speed when the robot reaches the path node. When starting path planning, the state parameters of the navigation start point can be determined first, where the current time or zero time is the time of the navigation start point, and the current robot motion state can be the attitude and velocity corresponding to the start point, for example, if the robot is initially in a stationary state, the steering angle and velocity corresponding to the navigation start point are both 0. In one exemplary embodiment, the traversal target node cache space is a data structure of a priority queue into which corresponding candidate path nodes are added in a first-in-first-out storage order, and is stored in the robot's memory space.

[0034] In step S103, a neighborhood search is performed in the lattice map with the current parent node at the center. Here, the eight lattice points adjacent to the current parent node are each regarded as a child node. Then, the process proceeds to step S104. Here, the process of searching for child nodes in the vicinity of the parent node (the current parent node described in step S103) can be called "expansion." In one expansion process, it is possible to search for child nodes that meet the association conditions by searching for lattice points one by one in a clockwise direction in the vicinity of the current parent node (neighborhood search) or searching for lattice points one by one in a counterclockwise direction in the vicinity of the current parent node (neighborhood search).

[0035] In step S104, it is determined whether the child node searched for nearby in step S103 is one end point of the corresponding candidate route in the candidate route coordinate set searched in advance, and if YES, the process proceeds to step S105, and if NO, the process proceeds to step S106. That is, by determining whether the child node searched for nearby in step S103 is the start point or end point of a representative candidate route in the candidate route coordinate set searched in advance, the planned navigation route can be connected to the searched candidate route and used for actual navigation travel without considering that an error in the marking information of the grid points detected in real time affects the navigation route planned by the conventional mature route search algorithm, in particular, that a free grid point is erroneously marked as an obstacle grid point, and therefore the erroneously marked obstacle grid point cannot be passed through via the navigation route planned by the conventional mature route search algorithm, which is disadvantageous for the robot to pass through the narrow passage. Here, when one end point of the candidate route is the starting point, the other end point of the candidate route becomes the end point, and when one end point of the candidate route is the end point, the other end point of the candidate route becomes the starting point.

[0036] In step S105, the other end point of the candidate route to which the child node found in step S104 belongs is added to the traversal target node cache space, i.e., the other end point of the candidate route to which the child node belongs is also stored in the traversal target node cache space created in step S101, and the free lattice points found in step S103 that are not on the corresponding candidate route in step S104 are added to the same traversal target node cache space created in step S101 to form a set of candidate nodes to be used in subsequent route planning, and at the same time, the position information of the parent node of the route node added to the traversal target node cache space is recorded, and the position information of the route node is stored in the traversal target node cache space together with the position information of the corresponding parent node. Preferably, other information such as the time, attitude, and speed of the child node and its parent node are also recorded in the traversal target node cache space. Among the route nodes added to the traverse target node cache space in the currently executed step S105, the parent nodes of the remaining route nodes excluding the other end point of the candidate route to which it belongs are all the current parent node described in step S103. Preferably, the parent node of the other end point of the candidate route to which it belongs is an adjacent node on the candidate route to which it belongs or an adjacent node other than the candidate route to which it belongs. This allows the location distribution information of the candidate route to be traced back directly via the parent node when searching for this end point, thereby speeding up route planning.

[0037] Here, the reason why one of the candidate routes is added to the traversal target node cache space, instead of both end points and intermediate nodes between these end points, is that the candidate route is previously searched, and it is known that before the path merging planning method is executed, a set of related coordinate points is searched and sequentially connected to become a path segment, and at this time, only one of the candidate routes can function as an identification of the corresponding route or an index of the corresponding starting point on this route, so in this step, only another end point (equivalent to a path node that has not been searched) on the candidate route to which one of the nearby searched end points belongs is added to the traversal target node cache space, and subsequently, it is possible to continue searching for a new path node that is meaningful to pass and is related to this candidate route, centered on the end point added to the traversal target node cache space. The free grid points that are not on the corresponding candidate route in step S104 and belong to the vicinity of the current parent node are used to provide a free grid point having the shortest path.

[0038] For this reason, during the execution of step S107, all intermediate nodes between both ends of the candidate route to which the child node searched for in step S104 belongs and all end points searched for in step S104 are set as nodes that cannot be searched for repeatedly. In this embodiment, a traversed node cache space is specially created for this purpose and used to store nodes that are set as not searchable repeatedly. Note that nodes that exist in the traversed node cache space are prohibited from being added to the traversed node cache space, thereby realizing identification of nodes that are considered to be traversed or searched for in the planning process, and avoiding repeated search processing. Here, steps S105 and S107 may be executed simultaneously. When the execution of step S107 is completed, the process proceeds to the execution of step S108.

[0039] Preferably, if a free lattice point in the vicinity of the current parent node that is not located on the corresponding candidate route or all free lattice points in the vicinity of the current parent node have been added to the traversed node cache space, the current parent node is deleted from the traversed node cache space and then added to the traversed node cache space, thereby preventing the same search center from being repeatedly expanded, since the current parent node has been expanded once in step S103. The traversed node cache space can exist inside the robot in the form of a data storage structure called a list.

[0040] It should be noted that the corresponding candidate route in the candidate route coordinate set is allowed to pass through the obstacle grid point marked on the grid map, regardless of whether the obstacle grid point is marked due to map error or not.

[0041] In this embodiment, the grid marking error is formed by errors such as sensor errors, map drift errors, or obstacle movement being reflected in the grid map.

[0042] In step S106, if the child node searched for in the vicinity in step S103 is not an end point of any of the candidate routes in the candidate route coordinate set, all free lattice points searched for in the vicinity in step S103 are added to the traverse target node cache space, and the position information of the parent node of the free lattice point searched for in the vicinity in step S103 is recorded, and then the process proceeds to step S108. This not only expands the search range of the child node being searched for in the vicinity, but also contributes to expanding the search of the start point and end point of the route in the candidate route coordinate set to a wider range, making the planned route more practical and representative.

[0043] As an embodiment of step S105, if the child node searched nearby in step S103 is the starting point of the corresponding candidate route in the candidate route coordinate set, the end point of the corresponding candidate route is added to the traversal target node cache space, and the position information of the parent node of the end point of the corresponding candidate route is recorded in the candidate route coordinate set so that each candidate route uses its end point as route identification information, and at the same time, all intermediate nodes between both ends of the corresponding candidate route and the starting point of the same candidate route are set as nodes that cannot be repeatedly searched. Along the path extension direction from the starting point of the corresponding candidate route toward its end point, the parent node of each node on the corresponding candidate route is located at the node position adjacent to that node. Specifically, in the candidate route coordinate set, starting from the starting point of the corresponding candidate route, the position information of the parent node of each node of the candidate route is recorded in a first route extension direction from the starting point of the candidate route to its end point, until the position information of the parent node of the end point of the candidate route is recorded, where the parent node of each node of the candidate route is an adjacent node in the first route extension direction of the candidate route to which it belongs. Therefore, the parent node of the end point of the candidate route is located outside the candidate route to which it belongs and adjacent to the end point of the candidate route, and the child node of the end point of the candidate route is located on the candidate route to which it belongs and adjacent to the end point of the candidate route. The parent node of the start point of the candidate route is located on the candidate route to which it belongs and adjacent to the starting point of the candidate route, and the child node of the start point of the candidate route is located on the candidate route to which it belongs and adjacent to the starting point of the candidate route.

[0044] As another embodiment of step S105, when the child node searched nearby in step S103 is the end point of the corresponding candidate route in the candidate route coordinate set, the start point of the corresponding candidate route is added to the traversal target node cache space, and the position information of the parent node of the start point of the corresponding candidate route is recorded in the candidate route coordinate set so that each candidate route uses its start point as route identification information, and at the same time, all intermediate nodes between both end points of the corresponding candidate route and the end point of the same candidate route are all set as nodes that cannot be repeatedly searched. Here, along the path extension direction from the end point of the corresponding candidate route toward its start point, the parent node of each node on the corresponding candidate route is located at the node position adjacent to that node. Specifically, within the candidate route coordinate set, along a second route extension direction starting from the end point of the candidate route and heading from the end point of the corresponding candidate route to its start point, position information of the parent node of each node of the candidate route is recorded in sequence until position information of the parent node of the start point of the candidate route is recorded, where the parent node of each node of the candidate route is an adjacent node in the second route extension direction of the candidate route to which it belongs. Therefore, the parent node of the start point of the candidate route is located outside the candidate route to which it belongs and adjacent to the start point of the candidate route, and the child node of the start point of the candidate route is located on the candidate route to which it belongs and adjacent to the start point of the candidate route.

[0045] In the two embodiments based on step S105 described above, the starting point or the end point of the candidate route is configured to uniquely identify one candidate route, and is used as an identifier of the corresponding candidate route to record the corresponding parent node information, while the remaining nodes of the same candidate route are configured as repeatedly searched nodes, thereby reducing the amount of searching during route planning and accelerating the tracing back of matching candidate routes.

[0046] In step S108, the node with the smallest total path cost is selected as the next parent node from within the traversed node cache space, and the process proceeds to step S109. In this embodiment, the path costs of all nodes within the traversed node cache space are compared to select a better node as the next parent node. Here, the smaller the total path cost, the higher the priority of the node. The node with the smallest total path cost (highest priority) is selected as the next parent node, and a new neighborhood search is started. The search is continued with the next parent node as the search center, and a new neighborhood lattice point (distinguished from the node already existing in the traversed node cache space) is added.

[0047] In this embodiment, the total value of the path cost is obtained by adding or weighting the traversed path cost and the predicted path cost. Here, the traversed path cost is the cost from one specified node in the traversed node cache space to the navigation start point, and the predicted path cost is the cost from the same specified node in the traversed node cache space to the navigation end point, and the definition of these path costs is from the heuristic function of the A* algorithm. The smaller the total value of the path costs is, the higher the traverse priority of the specified node in the traversed node cache space is set. In this embodiment, when calculating the movement cost between two adjacent nodes based on the A* heuristic function, it is expressed using Manhattan distance, diagonal distance, or Euclidean distance. Here, if the robot is only allowed to move in four directions, up, down, left, and right, in the map area where the robot is currently located, Manhattan distance can be used. If the robot is allowed to move in eight directions in the map area where the robot is currently located, diagonal distance can be used. If the robot is allowed to move in any direction in the map area where the robot is currently located, Euclidean distance can be used. It contributes to finding the shortest route with the lowest navigation cost.

[0048] When calculating the path cost, the center point of each traversed lattice is connected from the navigation start point until it is connected to the specified node, and the length of the currently connected line is calculated from the length of the side of the lattice, and this length is set as the traversed path cost. When calculating the path cost, the specified node is set as the parent node, and all line connections from each child node corresponding to this parent node to the navigation end point are obtained, and the length of each connection line corresponding to each child node is calculated from the length of the side of the lattice, and the shortest length corresponding to each child node is selected and set as the corresponding predicted path cost. Here, the aforementioned lattice point is represented by the center point of the lattice and is used to represent the position characteristics of the lattice. The cost value calculated in this embodiment is a measure of the cost of the robot's motion trajectory, and represents the cost of traveling from the start point to the specified node and then to the end point, and includes the path length.

[0049] For each node, a cost value can be calculated. The cost value is a measure of the cost of the robot's motion trajectory, representing the cost of moving from the start point to this node and then to the end point, and includes factors such as the path length, the time required, whether collisions have occurred, and whether speed and direction are frequently changed.

[0050] In step S109, it is determined whether or not the next parent node is the navigation end point. If YES, the process proceeds to step S110, and if NO, the process proceeds to step S111.

[0051] In step S111, the next parent node is updated to the current parent node mentioned in step S103, and the process returns to step S103.

[0052] In the case of a grid map, the embodiment corresponding to the above steps is essentially a process of starting from a navigation start point and constantly expanding to reach a navigation end point, and the expanded route node and the candidate route are connected to form a final planned navigation path. Therefore, step S111 is a trigger step that is repeatedly executed, and each time it returns to step S103, it becomes one expansion, and the node that the robot can reach by starting from the current parent node and traveling for a preset interval time becomes a child node. In simple terms, each expansion means that the robot "takes one step forward", and corresponds to crossing a grid once in the map. Note that the preset interval time is the periodic time of each expansion, and represents a short time unit such as 5 seconds, 10 seconds, etc., and the shorter the preset interval time, the more precise the planned navigation path, so the preset interval time may be determined according to actual needs.

[0053] In this embodiment, in the process of expanding the search center to the next parent node, if the current parent node is searched for nearby as a child node of the next parent node, there is no need to add the current parent node to the traversal target node cache space.

[0054] In step S110, when it is determined that the navigation end point exists within the traversal target node cache space, i.e., the navigation end point has been searched for (or extended to) in the above-mentioned neighborhood search process, based on the position information of the parent node recorded in the above-mentioned step, child nodes and their parent nodes are sequentially connected from the navigation end point, except for all intermediate nodes and their corresponding one end point of the candidate route described in step S104, until they are connected to the navigation start point, and a route from the navigation start point to the navigation end point is planned.

[0055] Compared with the prior art, in the above embodiment, the robot is controlled to start from the navigation start point at the same position, sequentially select the node with the smallest total path cost value from the traversal target node cache space as the parent node to perform a neighborhood search, and add all the nearby searched free lattice points that are not on the candidate route and one end point (start point or end point) on the candidate route with discriminatory value to the traversal target node cache space, and record the corresponding parent node as the path node retroactive information. At the same time, it eliminates the interference of the intermediate nodes on the candidate route (no matter which one is the navigation goal point suitable for these narrow passages) to the repeated search, and when it becomes the navigation end point as the parent node, it further connects the searched candidate route from the navigation end point according to the positional relationship between the child node and the parent node, and connects it backward to the navigation start point, so that the candidate route that meets the passage conditions is integrated into the heuristic search algorithm to plan the navigation path, overcoming the narrow passage in the map grid that is prone to marking errors, and allows the robot to effectively enter and exit this narrow passage along the planned overall navigation path, reducing the probability of path navigation failure.

[0056] Specifically, when it is determined that the next parent node is the navigation end point, sequentially connecting child nodes and their parent nodes from the navigation end point, except for all intermediate nodes of the candidate route described in step 4 and their corresponding one end point, until they are connected to the navigation start point based on the recorded position information of the parent node, specifically includes the following steps 51 to 54. In step 51, the navigation end point and its parent node are connected from the navigation end point based on the already recorded position information of the parent node or the position information of the parent node and the child node stored in the traverse target node cache space, and then proceed to step 52. This starts the connection in the reverse direction for each node.

[0057] In step 52, the currently determined parent node is made a child node, i.e., the parent node determined in step 51 is updated to a child node including its location information, and further, the parent node of this child node is connected based on the recorded location information of the parent node, and then the process proceeds to step 53.

[0058] In step 53, step 52 is repeated until one end point of the corresponding candidate route definitely found in step S105 is connected, and then the other end point of this candidate route is connected to the other end point of the corresponding candidate route and its parent node, and thereafter, the process returns to step 52. Here, the parent node of the other end point of the corresponding candidate route is a node that is adjacent to the other end point of the corresponding candidate route in the route extension direction from the end point definitely found in step S105 toward the other end point of the candidate route to which it belongs, and that is located outside this candidate route.

[0059] As one embodiment, in step 53, when a connection is made to the starting point of the corresponding candidate route certainly searched in step S105, as can be seen from the above embodiment, the starting point of the corresponding candidate route is the parent node of the child node updated in step 52, i.e., the starting point of the corresponding candidate route is the parent node of the adjacent node in the opposite direction to the route extension direction from the starting point of the corresponding candidate route to its end point. From this, it is found that the parent node of each node on the corresponding candidate route is located at the node position adjacent to the node along the route extension direction from the starting point of the corresponding candidate route to its end point, and at the same time, it is found that both the starting point and the intermediate node of the candidate route are nodes that cannot be repeatedly searched, i.e., traversed route nodes. Therefore, in step 53, the end point of the candidate route is connected to the end point of the candidate route and its parent node, because the parent node of the end point of the candidate route is outside the candidate route so that it is possible to return to step 52 and trace back to the outside area of ​​the candidate route.

[0060] As another example, in step 53, when the end point of the corresponding candidate route definitely searched in step S105 is connected, as can be seen from the above example, the end point of the corresponding candidate route is the parent node of the child node updated in step 52, i.e., the end point of the corresponding candidate route is the parent node of the adjacent node in the route extension direction from the start point of the corresponding candidate route to the end point. From this, it is found that the parent node of each node on the corresponding candidate route is located at the node position adjacent to the node along the route extension direction from the end point of the corresponding candidate route to the start point, and at the same time, it is found that the start point and intermediate node of the candidate route are both nodes that cannot be repeatedly searched, i.e., traversed route nodes. Therefore, in step 53, the start point of the candidate route is connected to the start point of the candidate route and its parent node, because the parent node of the start point of the candidate route is outside the candidate route so that it is possible to return to step 52 and trace back to the outside area of ​​the candidate route.

[0061] In step 54, the parent nodes searched in the above steps S101 to S111 are connected in the reverse direction one by one based on the position information of the parent nodes recorded as above by repeating the above step 52 and the above step 53 until they are connected to the navigation start point, thereby obtaining a navigation route from the navigation start point to the navigation end point. In the process of executing steps 51 to 54, the process of sequentially connecting the parent nodes of this child node (including the end point of the candidate route) and further making this parent node a child node can be called "reverse expansion", which is essentially a retroactive process of the route node starting from the navigation end point, constantly expanding and connecting to the start point or end point of the candidate route, and reaching the navigation start point, and the trajectory of the reverse expansion is such that a navigation route connecting the navigation start point, the candidate route that can overcome the obstacle lattice point, and the navigation end point is obtained.

[0062] Compared with the prior art, after planning the above navigation path through the above steps, according to this embodiment, when the robot searches for a path using a heuristic search algorithm or an incremental heuristic search algorithm in the process of passing through a narrow passage, if a narrow passage is detected, the robot will switch from entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm to entering the candidate route near the narrow passage; on the other hand, if the robot detects that it has left the narrow passage, the robot will switch from entering the candidate route to entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm, thereby enabling the robot to pass through the narrow passage effectively and increasing the success rate of navigation in overcoming the narrow passage.

[0063] Preferably, the state parameters of each node can also be marked on the navigation path, and the state parameters of each node include the time of each node, i.e., the time when the robot moves to each node according to the plan. Thus, state parameters can be added to the navigation path, for example, the time of each node can be marked on the navigation path, and when the narrow passage exists in the map, it can be estimated whether the robot will be trapped in the narrow passage for a long time, thereby realizing effective path planning in complex obstacle layout scenes and meeting practical needs.

[0064] It should be noted that if there are no new child nodes to add to the traversed node cache space and it cannot continue to expand in the vicinity, this indicates that there may be obstacles in the map that are difficult to overcome, for example, the passage formed between the obstacles is smaller than the width of the aircraft.

[0065] Furthermore, if it is not possible to connect to the navigation start point even after steps 52-53 have been repeated a certain number of times, this indicates that an abnormality may have occurred in the processing, and this certain number may be related to the distance between the navigation start point and the navigation end point.

[0066] As another embodiment, the present invention further discloses a method for searching for a corresponding candidate route in the candidate route coordinate set, and the searching method is performed before executing the route fusion planning method. As shown in FIG. 2, specifically, the method includes the following steps S201 to S207. In step S201, in the process of controlling the robot to move along a preset route, it is determined in real time whether the pre-search area satisfies a first preset circular area passage condition, and if YES, the process proceeds to step S202, and if NO, the robot is controlled to continue moving along the preset route. Here, the preset route is a route that the robot has planned in advance. The route nodes can be represented by lattice points. The preset route is a normal work route or navigation route of the robot, and when the robot is a cleaning robot, the preset route may be a planned cleaning route such as a bow-shaped movement route, an edge-along travel route, a "circle"-shaped route, etc. The robot moving along the preset route can search for a route coordinate point or route suitable for overcoming the narrow passage during normal work, or perform a corresponding route search in the process of entering the narrow passage. The pre-search area is a first circular area with the robot's current position as its center and the robot's body diameter as its radius, and covers the minimum passable area around the robot.

[0067] In step S202, the current position of the robot is recorded as a first route node, a new predicted travel coordinate set is created, and the first route node is stored in the predicted travel coordinate set, and then the process proceeds to step S203. In this embodiment, the elements in the predicted travel coordinate set are a set of route nodes configured to store nodes in order to form one route, and the first element or the last element in the predicted travel coordinate set is unique, so that the first element or the last element in the predicted travel coordinate set becomes unique identification information of the route it represents, and can function as an index node in route planning or as a marked node of a traceback route. And the first element and the last element in the same predicted travel coordinate set are not the same, because if the first element and the last element in the same predicted travel coordinate set are the same, the elements in the predicted travel coordinate set form a closed figure according to the recorded order, forming a route that has no value for navigation.

[0068] In this embodiment, when executing step S201, if the robot determines that the pre-search area satisfies the first predetermined circular area passage condition at its current position, the current position of the robot is recorded as a new first route node and added to the predicted passage coordinate set, so that it can be configured as the starting point of the corresponding candidate route, which is equivalent to a navigation starting point.

[0069] In this embodiment, the first predetermined circular area passage condition includes that the ratio of the area occupied by the first impassable area in the pre-search area is greater than a first passage evaluation value. Here, the first impassable area is a lattice area consisting of unknown lattice points and obstacle lattice points in a lattice area corresponding to the first circular area. The first passage evaluation value is a pre-judgment threshold set to overcome marking errors of free lattices present in the constructed lattice map, and is a result of repeated experiments in the narrow passage. The first passage evaluation value is preferably 50%. When the robot judges that the ratio of the area occupied by the first impassable area in the lattice area corresponding to the first circular area is 50% or less, the robot is controlled to maintain movement along the preset path until the robot judges that the ratio of the area occupied by the first impassable area in the lattice area corresponding to the first circular area is greater than 50%. Therefore, the first preset circular area passage condition is a rough judgment condition for preliminary judging whether the robot has started to enter a narrow passage, and subsequently, it depends on the robot to make further judgments during its continued movement. However, there is no need to consider whether the marking information of a single grid allows the robot to pass, which reduces the influence of the marking error of a single grid.

[0070] In step S203, the robot is controlled to continue moving along the preset route, and it is determined in real time whether the robot has moved to a position where the straight-line distance to the most recently recorded route node is equal to or greater than the diameter of the robot's body. If the result is YES, the process proceeds to step S204. If the result is NO, the robot is controlled to continue moving along the preset route. In this step S203, a straight-line distance from the robot to the previously recorded route node, including a straight-line distance from the robot's current position to the first route node, is detected. Here, the robot detects the straight-line distance from the current position to the previously recorded route node in real time. In addition, distance sampling detection may be performed at regular detection intervals, and the route node detected in real time may be recorded when a certain passage condition is satisfied.

[0071] In step S204, if it is detected that the straight-line distance between the robot's current position and the most recently recorded route node is equal to or greater than the robot's body diameter, the robot's current position is recorded as the second route node, and then the process proceeds to step S205.

[0072] In step S205, it is determined whether the second route node satisfies a second preset circular area travel condition. If the result is YES, the process proceeds to step S206. If the result is NO, the process proceeds to step S207. Therefore, the route node recorded most recently in the above-mentioned step S204 includes the first route node recorded last time or the second route node recorded last time. Here, the second route node recorded in step S204 is a new second route node for the second route node recorded corresponding to the previous execution of step S204, or a new route node for the first route node recorded corresponding to step S202. Specifically, when steps S202 to S203 are executed, the most recently recorded route node described in step S203 is the first route node recorded in step S202, and at this time, the predicted travel coordinate set to which the currently recorded first route node and second route node are added is not the predicted travel coordinate set created in the previously executed step S202. If step S202 is omitted and step S203 is repeated once (returning from a subsequent step and repeating), the most recently recorded route node mentioned in step S203 is the second route node recorded during the previous execution of step S204, and all of these second route nodes obtained by the repeated processing are stored sequentially within the same predicted travel coordinate set.

[0073] As one embodiment of the second preset circular area travel condition, the second preset circular area travel condition includes that the ratio of the area occupied by the second impassable area in a second circular area having the robot's current position (i.e., the most recently recorded second route node) as the center of the circle and the robot's body diameter as the radius is greater than a second travel evaluation value. Here, the second impassable area is a lattice area consisting of unknown lattice points and obstacle lattice points in a lattice area corresponding to the second circular area. The second travel evaluation value is a judgment threshold set to overcome marking errors of free lattices present in the constructed lattice map, and is greater than the first travel evaluation value to improve judgment accuracy. The second travel evaluation value is preferably 75%. If the robot judges that the ratio of the area occupied by the second impassable area in the lattice area corresponding to the second circular area is 75% or less, the process proceeds to step S207. Here, the area covered by the second circular area is different from the area covered by the first circular area, because the position of the robot changes. Therefore, based on the first circular area or the previously searched second circular area, the robot moves to a location away from the most recently recorded route node by the robot's body diameter, and then the ratio of the area occupied by the second impassable area is determined, so that there is no need to consider the influence of the marker information of a single grid point, and the accuracy of determining a narrow passage is improved. In this embodiment, the search radius of the second circular area is simply set to the robot's body diameter, rather than a larger value, thereby avoiding irrelevant (outside the detection range) grid areas from being involved in the calculation, and reducing time overhead.

[0074] As another embodiment of the second preset circular area travel condition, the second preset circular area travel condition includes: searching for a path from the latest recorded second path node to the latest recorded first path node using a path search algorithm in a second circular area with the latest recorded first path node as the center of the circle and the preset multiple of the aircraft diameter as the radius, and proving that the path search in the current second circular area is no longer affected by obstacles or no longer affected by erroneous marks in the obstacle grid. Since the second circular area may be near a narrow passage, there may be obstacles between the latest recorded second path node and the latest recorded first path node, and in this embodiment, the nodes that may collide are called invalid nodes or incorrect nodes, and these nodes need to be avoided when planning a path, so a mature and stable path search algorithm needs to be used to verify the passability of the area. In this embodiment, the A* algorithm is used as a path search algorithm to efficiently and quickly search for a navigation path from the latest recorded second path node to the latest recorded first path node in the second circular area. Note that in this embodiment, the first circular area or the second circular area can be regarded as a cleaning area. Here, the preset multiple of the body diameter is set so that the second circular area does not intersect with other marked grid areas, thus avoiding the already planned path in the other marked grid areas from being involved in the judgment, and further avoiding erroneous judgment. The preset multiple of the body diameter is set to two-thirds of the body diameter so that the second circular area formed is larger than the first circular area. Compared with the prior art, in this embodiment, the size of the search area, the second circular area, is set reasonably to avoid the robot being guided to other areas and not being guided to pass through the current narrow passage as a result of intersecting with other known map areas and containing the planned path in the related area.Meanwhile, by further determining whether a complete navigation path from the start point to the end point can be planned within the second circular area currently being searched, it is possible to prove that the passable area of ​​this second circular area is no longer affected by obstacles or the marking positions of the obstacle grid, thereby improving the accuracy of judging the narrow passage.

[0075] In step S206, it is determined that the robot is currently in the narrow passage, and the second route node is added to the predicted travel coordinate set mentioned in step S202, and the process returns to step S203. This is because the robot continues to move along the preset route in the narrow passage after recognizing that its current position is in the narrow passage and then returns to step S203. Therefore, in this embodiment, the robot determines whether the current position or the current search area of ​​the robot satisfies the second preset circular area travel condition each time the robot moves a linear distance of the diameter of the body after searching the first route node. Thereafter, if it is determined that the robot is currently in the narrow passage according to the second preset circular area travel condition, the robot continues to move through the narrow passage along the preset route, continues to search for a new second route node, and connects and forms a route for passing through the narrow passage. Note that the narrow passage is a gap passage of two or more obstacles, and the gap passage is a gap corresponding to the narrowest part between two obstacles, and the width of the gap is equal to or greater than the diameter of the body of the robot.

[0076] In step S207, it is determined that the robot is not currently located in the narrow passage, and the predicted travel coordinate sets are stored in the same candidate route coordinate set according to the number of route nodes present in the predicted travel coordinate set, and the route nodes stored in the predicted travel coordinate set are connected in the order in which they were added to form a corresponding candidate route, so that one predicted travel coordinate set becomes a set of points representing one candidate route in the candidate route coordinate set. Then, the process returns to step S201, and a new predicted travel coordinate set for describing a new candidate route is created in the candidate route coordinate set. Here, within the same candidate route coordinate set, there are multiple predicted travel coordinate sets each representing a different candidate route. This is because within the same candidate route coordinate set, the first element and the last element of any of the predicted travel coordinate sets are unique.

[0077] Specifically, the method of storing the predicted travel coordinate sets in the same candidate route coordinate set according to the number of route nodes present within the predicted travel coordinate set specifically includes the following: when the number of route nodes present within the predicted travel coordinate set is less than two, the robot detects that there is an obstacle that is difficult to overcome or that an abnormal situation such as being trapped has occurred, indicating that the route nodes present within the predicted travel coordinate set may be invalid nodes, deleting the predicted travel coordinate set, and returning to step S201 to create a new predicted travel coordinate set. Therefore, in this embodiment, when the number of route nodes finally obtained by one predicted travel coordinate set is so small that it is difficult to connect them with a line, the related sets may be deleted to reduce the number of invalid route nodes.

[0078] If the number of route nodes present in the predicted traffic coordinate set is two or more, the predicted traffic coordinate set is used to represent a single candidate route, and a route search algorithm is called, for example, a heuristic search algorithm is used, and the predicted traffic coordinate set is stored in the candidate route coordinate set, and then the process returns to step S201 to create a new predicted traffic coordinate set. In this embodiment, the route nodes finally obtained by one predicted traffic coordinate set can be connected to store the entire route in the candidate route coordinate set for storing one candidate route, so that the storage and retrieval structure of the candidate route is rational and orderly. Here, the first route node and the second route node are added to the predicted traffic coordinate set according to the recorded order, so that the route nodes stored in the predicted traffic coordinate set are connected in the recorded order to form the corresponding candidate route. Note that the route node corresponding to the head element in each predicted traffic coordinate set is the start point of the corresponding candidate route, and the route node corresponding to the tail element in each predicted traffic coordinate set is the end point of the corresponding candidate route. As a result, the route nodes in each predicted travel coordinate set are connected to form one candidate route, forming a candidate route that is predicted to allow the robot to travel through the corresponding area without any hindrance. Compared with the conventional technology, the method of searching for a candidate route described in the above steps S201 to S207 is suitable for a case where a route search is performed with the robot in a narrow passage, and first, a first preset circular area travel condition is set as a narrow passage advance judgment condition, a source of route nodes of the candidate route corresponding to the predicted travel coordinate set is provided, and the robot continues to move along the original route, and then a second preset circular area travel condition is set as a narrow passage refinement judgment condition, and a source of grid points of the candidate route corresponding to the predicted travel coordinate set is provided. As a result, the robot is only qualified to judge a narrow passage after moving a certain distance, and the candidate routes connecting the grid points collected in the predicted passage coordinate set are more complete and more suitable for the error environment of the map grid in the narrow passage, so that the robot can be provided with an actually passable route without paying attention to the marking information of the corresponding grid in real time.Based on this, if the corresponding preset circular area travel condition is not met, stop searching for the predicted travel coordinate set, and determine that the route nodes in the single predicted travel coordinate set can be connected to form an independent candidate route. By repeatedly performing the above related steps, the robot searches for a set of route points that can solve the problem of grid impassability caused by map drift error in a normal work movement state. Increase the success rate of the robot in finding an effective driving path in a scene with a complex spatial layout of obstacles.

[0079] In particular, it should be noted that an obstacle grid point may be included in the predicted travel coordinate set, indicating that the obstacle grid point may exist on the corresponding candidate route. As can be seen from steps S201 to S207, when the robot itself moves to a specific route node, for example, moves to the corresponding robot current position when step S202 is executed during one iteration (initial state condition is satisfied) or when step S206 is executed during one iteration, and it is determined that the first preset circular area travel condition or the second preset circular area travel condition is satisfied on this route node, this route node is added to the predicted travel coordinate set as a route node connected to the corresponding candidate route. However, due to sensor detection errors, map drift errors, etc., the grid information marked on the grid map for this route node may not be in a free state, but may be erroneously marked as an obstacle-occupied state, i.e., as the obstacle grid point. Although this route node is marked as an obstacle grid point on the grid map, it is added to the predicted travel coordinate set and becomes a route node on the corresponding candidate route. It is then proven that the robot can actually move to this route node, and this route node is passable or connectable.

[0080] Based on the above embodiment, when the robot detects that the current state has changed from being in the narrow passage to not being in the narrow passage, it starts to execute the path fusion planning method, i.e., it starts to repeatedly execute the above steps S101 to S111, thereby planning a navigation path that allows the robot to freely enter and exit the narrow passage, thereby overcoming map drift errors. In this way, when the robot is exiting the narrow passage, or just before leaving the current narrow passage and entering a new narrow passage, the coordinate points of the searched candidate route suitable for passing through the narrow passage can be merged with the use of a heuristic search algorithm at the current position to plan a navigation path, and since the navigation path planning does not rely only on neighborhood search, the problem that the free grid points searched in the narrow passage are easily marked as obstacle grid points, and the narrow passage cannot be passed on the planned path is solved.

[0081] A chip, storing a program code for executing the path fusion planning method according to any of the above technical solutions, which plans a navigation path in a narrow passage with many obstacles by integrating a heuristic search algorithm and a candidate route that meets a search condition, overcomes the narrow passage in the map grid that is prone to marking errors, enables the robot to effectively enter and exit the narrow passage along the planned overall navigation path, and reduces the probability of path navigation failure in a scene with a complex layout of the obstacle space.

[0082] A robot includes the chip for controlling the robot to execute the path fusion planning method, and in the process of the robot passing through a narrow passage according to a navigation path planned by the path fusion planning method, when the robot searches for a path using a heuristic search algorithm or an incremental heuristic search algorithm, if a narrow passage is detected, the robot switches from entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm to entering the candidate route near the narrow passage, while if the robot detects that it has left the narrow passage, the robot switches from entering the navigation path searched by the heuristic search algorithm or the incremental heuristic search algorithm from the candidate route, thereby enabling the robot to effectively pass through the narrow passage along the navigation path connected to the corresponding candidate routes, thereby increasing the success rate of navigation to overcome the narrow passage.

Claims

1. A method for planning route merging in a traffic area, comprising: Step 1: setting a navigation start point and a navigation end point in a grid map and creating a traversal target node cache space; Step 2: setting the navigation start point to the current parent node and adding it to the traversal target node cache space; Step 3 of performing a neighborhood search in a grid map with a current parent node as a search center, in which eight grid points adjacent to the current parent node are set as its child nodes; Step 4, in which if the child node searched nearby in step 3 is one end point of the corresponding candidate route in the candidate route coordinate set previously searched, the other end point of the corresponding candidate route is added to the traversal target node cache space, and at the same time, all intermediate nodes between both end points of the corresponding candidate route and the end point are all set as nodes that cannot be repeatedly searched, and the free lattice points searched in step 3 that are not on the corresponding candidate route are added to the traversal target node cache space, and then proceed to step 5; Step 4, in which the corresponding candidate route in the candidate route coordinate set is allowed to pass through the obstacle lattice points marked on the lattice map, and all nodes added to the traversal target node cache space have their parent node position information recorded for subsequent route node traceback; Step 5 includes: selecting a node with the smallest total path cost value as a next parent node from the traversal target node cache space mentioned in step 4; then judging whether the next parent node is the navigation end point; if yes, based on the recorded parent node position information, from the navigation end point, except for all intermediate nodes of the candidate route mentioned in step 4 and their corresponding one end point, sequentially connecting child nodes and their parent nodes until they are connected to the navigation start point, thereby planning a path from the navigation start point to the navigation end point; if no, updating the next parent node to the current parent node mentioned in step 3, and returning to step 3; Before executing the path fusion planning method, In the process of controlling the robot to move along a preset route, it is determined in real time whether or not the pre-search area satisfies a first preset circular area passage condition, and if the result is YES, the process proceeds to step S202, and if the result is NO, the process controls the robot to continue moving along the preset route in step S201; Step S202 of recording the current position of the robot as a first route node, creating a new predicted travel coordinate set, storing the first route node in the predicted travel coordinate set, and then proceeding to step S203; In the process of the robot moving along the preset path, it is determined in real time whether the robot has moved to a position where the straight-line distance to the most recently recorded path node is equal to or greater than the diameter of the robot's body, and if the result is YES, the process proceeds to step S204, and if the result is NO, the process controls the robot to continue moving along the preset path in step S203; step S204 of recording the current position of the robot as a second path node and then proceeding to step S205; Step S205: determining whether the second route node satisfies a second preset circular area travel condition, and if the result is YES, the process proceeds to step S206, and if the result is NO, the process proceeds to step S207; step S206 of determining that the robot is currently in a narrow passage, adding the second route node into the predicted travel coordinate set, and returning to step S203; and step S207 of determining that the robot is not currently located in the narrow passage, storing the predicted travel coordinate set in a same candidate route coordinate set according to the number of route nodes present in the predicted travel coordinate set, connecting the route nodes stored in the predicted travel coordinate set according to the order in which they were added to form a corresponding candidate route, and then returning to step S201. The narrow passage is a gap passage between two or more obstacles, the gap passage being a gap corresponding to the narrowest part between two obstacles, and the width of the gap is equal to or larger than a diameter of the body of the robot; the first predetermined circular area passage condition includes that a ratio of an area occupied by a first impassable area in the pre-search area is greater than a first passage evaluation value, the first impassable area is a grid area consisting of unknown grid points and obstacle grid points in a grid area corresponding to the first circular area, the first passage evaluation value is a pre-judgment threshold value set to overcome a marking error of a free grid present in a constructed grid map, the pre-search area is the first circular area having a current position of the robot as a center of a circle and a body diameter of the robot as a radius, The second predetermined circular area passage condition includes that within a second circular area having a center of the current position of the robot and a radius of the body diameter of the robot, a proportion of an area occupied by a second impassable area is greater than a second passage evaluation value, the second impassable area being a grid area consisting of unknown grid points and obstacle grid points within a grid area corresponding to the second circular area, the second passage evaluation value being a judgment threshold value set to overcome marking errors of free grids present in the constructed grid map and being greater than the first passage evaluation value, or The second predetermined circular area travel condition includes searching for a route from the most recently recorded second route node to the most recently recorded first route node using a route search algorithm within a second circular area having a center of the circle at the most recently recorded first route node and a radius of a preset multiple of the aircraft diameter, and the preset multiple of the aircraft diameter is set so that the second circular area does not intersect with other marked grid areas.

2. Step 4 is:

2. The method for route fusion planning according to claim 1, further comprising: if the child node searched for in the vicinity in step 3 is not an end point of any of the candidate routes in the candidate route coordinate set, adding all the free lattice points searched for in the vicinity in step 3 to the traversal target node cache space, recording position information of the parent node of the free lattice point searched for in the vicinity in step 3, and then proceeding to step 5.

3. In step 4, if the child node searched for nearby in step 3 is the starting point of a corresponding candidate route in the candidate route coordinate set, add the end point of the corresponding candidate route to the traversal target node cache space, record the position information of the parent node of the end point of the corresponding candidate route in the candidate route coordinate set so that each candidate route uses its end point as route identification information, and at the same time, set all intermediate nodes between both end points of the corresponding candidate route and the starting point of the same candidate route as nodes that cannot be repeatedly searched; 2. The route merging planning method according to claim 1, wherein a parent node of each node on the corresponding candidate routes is located at a node position adjacent to the node along a route extension direction from a start point of the corresponding candidate routes toward an end point of the corresponding candidate routes.

4. In step 4, if the child node searched for nearby in step 3 is the end point of the corresponding candidate route in the candidate route coordinate set, add the start point of the corresponding candidate route to the traversal target node cache space, record the position information of the parent node of the start point of the corresponding candidate route in the candidate route coordinate set so that each candidate route uses its start point as route identification information, and at the same time, set all intermediate nodes between both end points of the corresponding candidate route and the end point of the same candidate route as nodes that cannot be repeatedly searched; 2. The route merging planning method according to claim 1, wherein a parent node of each node on the corresponding candidate route is located at a node position adjacent to the node along a route extension direction from an end point of the corresponding candidate route toward a start point of the corresponding candidate route.

5. In step 5, based on the position information of the recorded parent node, sequentially connecting child nodes and their parent nodes from the navigation end point, except for all intermediate nodes and their corresponding one end point of the candidate route mentioned in step 4, until they are connected to the navigation start point, specifically, Step 51, connecting the navigation end point and its parent node from the navigation end point based on the recorded parent node position information, and then proceeding to step 52; Step 52: taking the currently determined parent node as a child node and connecting the parent node of this child node based on the recorded parent node position information; Step 52 is repeated until one end point of the corresponding candidate route described in step 4 is connected, and then the other end point of the corresponding candidate route described in step 4 and its parent node are connected from the other end point of the corresponding candidate route described in step 4, and then the step 53 returns to step 52; The route fusion planning method according to claim 3 or 4, further comprising a step 54 of obtaining a navigation route from the navigation start point to the navigation end point by a reverse connection by repeating steps 52 and 53 until the navigation start point is connected.

6. In step 5, the total value of the path costs is obtained by adding or weighting the traversed path costs and the predicted path costs, where the traversed path costs are the costs from one specified node in the traversal target node cache space to the navigation start point, and the predicted path costs are the costs from the same specified node in the traversal target node cache space to the navigation end point, and the smaller the total value of the path costs is, the higher the traverse priority of the specified node in the traversal target node cache space is set; 6. The method of claim 5, wherein the travel cost between two adjacent nodes is expressed by Manhattan distance, diagonal distance, or Euclidean distance.

7. From the navigation start point, sequentially connect the center points of each traversed grid until they are connected to the specified node, and further, calculate the length of the currently connected line from the length of the grid side, and set the length as the traversed path cost; The specified node is set as a parent node, and all line connections from each child node corresponding to the parent node to the navigation end point are obtained. Furthermore, the length of each line connection corresponding to each child node is calculated from the length of the lattice side, and the shortest length corresponding to each child node is selected as the corresponding predicted route cost. The method for path fusion planning according to claim 6, wherein the grid point is represented by a center point of the grid and is used to represent a position feature of the grid.

8. In the path fusion planning method, a traversed node cache space is further created for storing the nodes that are set as not repetitively searchable in the step 4; 2. The path merging planning method according to claim 1, wherein a node existing in the traversed node cache space is prohibited from being added to the traversed node cache space.

9. Step 4 includes:

9. The method for path fusion planning according to claim 8, further comprising: if a free lattice point in the vicinity of a current parent node that is not located on the corresponding candidate route, or all free lattice points in the vicinity of a current parent node, have been added to the traversed node cache space, then deleting the current parent node from the traversed node cache space and then adding the current parent node to the traversed node cache space.

10. 2. The path fusion planning method according to claim 1, further comprising the steps of: executing steps 1 to 5 when it is detected that the robot has changed from a state in which it is currently in the narrow passage to a state in which it is not in the narrow passage, thereby starting execution of the path fusion planning method.

11. Specifically, the method of storing the predicted travel coordinate set in the same candidate route coordinate set according to the number of route nodes present in the predicted travel coordinate set is as follows: If the number of route nodes present in the predicted travel coordinate set is less than two, the predicted travel coordinate set is deleted, and the process returns to step S1 to create a new predicted travel coordinate set; If the number of route nodes present in the predicted traffic coordinate set is two or more, the predicted traffic coordinate set is stored in the candidate route coordinate set, a new candidate route is correspondingly stored, and then, returning to step S1, a new predicted traffic coordinate set is generated; wherein the first route node and the second route node are added to the predicted travel coordinate set in accordance with the order in which they were recorded, so that the route nodes stored in the predicted travel coordinate set are connected in a certain order to form a corresponding candidate route; 2. The route fusion planning method according to claim 1, wherein a route node corresponding to a first element in each predicted traffic coordinate set is a start point of a corresponding candidate route, and a route node corresponding to a last element in each predicted traffic coordinate set is an end point of the corresponding candidate route.

12. A chip storing a program code for executing the path fusion planning method according to any one of claims 1 to 11.

13. A robot, A robot incorporating the chip according to claim 12 for controlling the robot to execute the path fusion planning method according to any one of claims 1 to 11.

Images