Dynamic multilayer route planning

Abstract

The invention generally relates to the field of route planning of unmanned vehicles, more specifically, but not exclusively, to a comprehensive system for pathfinding that enables consideration of any number of map layers simultaneously, and that is equipped to adapt in response to updates in the environment and movement of the vehicle.

Description

[0001] DYNAMIC MULTILAYER ROUTE PLANNING

[0002] TECHNOLOGICAL FIELD

[0003] The present disclosure relates to the field of route planning of unmanned vehicles, more specifically, but not exclusively, to a comprehensive system for pathfinding that enables consideration of any number of map layers simultaneously, and that is equipped to adapt in response to updates in the environment and movement of the vehicle.

[0004] BACKGROUND OF THE INVENTION

[0005] Various algorithms are used for route planning in unmapped terrain having obstacles. One such algorithm is called RRT. RRT stands for “Rapidly Exploring Random Tree” and refers to an exploring algorithm that constructs a space-filling tree. The tree is constructed so as to expand incrementally from an initial node (“root”) until a target point (“goal”) is reached. The target point is eventually reached, but typically through a random and non-ideal route.

[0006] RRT* is a variation of RRT that is designed to achieve the “lowest cost” route. In RRT*, each new node is connected to the tree via a “parent node” exhibiting the “lowest cost” in terms of steps needed to get back to the root. For example, in FIG. 1 A, new node 9 is being added to the tree. Although the lowest cost single-step connection of node 9 to the tree is via node 6, the total cost of reaching node 9 from the root is lowest when 9 is connected to 5. Thus, node 5 is selected as the “parent” of node 9. In addition, once a new node is connected to the tree, the tree is rewired so that each node is always on a route which is the lowest cost to the initial node. In the example of FIG. IB, node 6 was initially wired to node 4. However, once node 9 is added to the tree, the total cost of reaching node 6 is lowered when node 6 is connected to node 9. Thus, the connection between node 4 and node 6 is erased, and a new connection between nodes 6 and 9 is added.

[0007] An unmanned ground vehicle (UGV) is a vehicle that operates while in contact with the ground and without an onboard human presence. UGVs are often required to navigate in off-road terrain, without a pre-loaded map, and with numerous static and moving obstacles. A UGV is one example of an “agent” that may be required to determine a route from a root to a goal . Classically, RRT* (and its many variants) are useful for mapping routes in fixed two-dimensional spaces. In order to make RRT* useful for autonomous navigation of UGVs, it is necessary' to adapt the algorithm for handling both fixed and moving obstacles. Some attempts have been made to adapt the algorithm to three-dimensional spaces. However, such adaptations are generally computationally complex, are not easily adaptable in response to changing conditions or new goals and are not fully suited to the challenges of three-dimensional pathfinding.

[0008] SUMMARY OF THE INVENTION

[0009] The present disclosure introduces a novel system and method for pathfinding in unmapped terrain, or unmarked, and particularly in off-road terrain, which improves upon prior efforts in efficiency and adaptability. In particular, the present system and method include the following features, which transcend classic limitations of pathfinding algorithms in multiple ways.

[0010] Multiple Map Layers Derived from Sensor Data. Traditionally, path making algorithms consider only a single obstacle map relevant to traversing from point to point. The present system, by contrast, utilizes multiple map layers, each corresponding to the same spatial coordinates. These map layers are derived from sensor data and relate to various aspects of difficulty in traversing from place to place within the environment. For example, there may be a layer related to occupancy costs for being in a certain cell; a semantic layer including textual identifications of a region of the cell, and a gradient layer for identifying slope of movement between different cells. Each layer may derive its data from one or more sensors. Any combination of layers may be utilized at any point in order to update the tree and in order to serve as a basis for the path making. So long as map layers are encoded into the system, they may be considered or bypassed during the route planning process. As a result, the route planning algorithm is adaptable to the degree of complexity or sensitivity required by the user.

[0011] The map layers are maintained independently but considered together. Unlike some algorithms, in which the data points considered above are merged into a single analysis layer, in the present algorithm, the map layers are maintained independently. Advantageously, it is possible to add and subtract layers at will, as desired in order to adapt the route-making algorithm to particular conditions. For example, as will be described further herein, one map layer may include preferred “waypoints” which have been previously identified as being on a safe path of travel. The layer of waypoints may be added when desired to limit the travel to such points, and may be removed when it is desired for the agent to explore or travel on areas which have not been previously identified.

[0012] Permanence of Tree Irrespective of Path Making. Classically, RRT* and its variants are used to build a single path from a single starting point (root) to a single end point (goal). Prior to identification of the root and goal, there is no tree. Conversely, once a route is generated, the tree is discarded. Every time that it is desired to generate a new path, the tree is constructed anew. By contrast, according to the disclosed system and method, a tree is generated prior to initiating any pathfinding request. This tree - representing permissible movements between nodes in the environment - is retained by the agent and is available to the agent for construction of any new paths. When path making is required, the system identifies the root and the goal of the tree, locates the root and goal in relation to the preexisting tree, and devises the most efficient path that links up to the existing tree.

[0013] Dynamic Updates to Path According to Agent Movement. As discussed above, traditional trees are single-use, and accordingly they are fixed as soon as they are generated. There is no ability to update the tree as the agent moves. Often, however, in the use cases relevant to the present disclosure, the path as devised initially is no longer relevant after a few steps or movements of the agent. One reason why this may be the case is that the map may be generated in real time, before exploration of the environment is complete. Thus, the route may be initially constructed based on incomplete data. Another reason why this might be the case is that the data based on which the route is planned may be outdated. Thus, a new tree might have been planted, or a new barrier might have been built, which is not accounted for in the initial route-making.

[0014] In traditional paths, the only option available to the agent is continuing on the initially devised path. By contrast, in system and method of the present disclosure, because the “tree” exists prior to any particular path, and the “route” is constructed anew upon request, the same process may be used to reevaluate the route every single time the agent moves. The root of the tree is redefined as the agent’s new position. The branches of the tree are reconnected accordingly, based on their ability to be connected to the new root. As a result, the system is always assured to deliver the best route based on up-to- date information. As is evident in view of the foregoing, the system is also equally capable of updating the path in response to identification of a new goal.

[0015] Dynamic Updates to Tree and Path Following Receipt of Updated Sensor Data. The system and method are dynamic in response to other types of changes. Specifically, the system may be adaptable to changes in sensor data (which, in turn, give rise to changes in map data). Every time that there are changes in the map data that are determined to be significant, each of the connections between nodes is evaluated. Connections between nodes that are no longer valid are discarded. When discarding of a connection results in severing the node from its parent on the tree (“orphaning” the node), the node is reattached to the tree, if possible (“adopted”), or discarded from the tree. In parallel, when a node is updated, each path containing the node is updated.

[0016] Incorporation of Waypoints into the Route-Making Algorithm. Classically, the systems and methods described herein are applicable for route making in off-road terrains in which navigable tracks have not yet been identified. In some instances, however, certain areas in the region may be pre-marked as comprising navigable tracks. Locations within such tracks may be identified as “waypoints.” The term “waypoints,” as used herein, refers to specific points within the environment that the agent is encouraged to pass through. The algorithm for adding nodes to the tree and for building routes between added nodes may be biased such that the waypoints are preferentially selected. In especially advantageous embodiments, the waypoints are considered together with semantic mapping. For example, the semantic mapping may identify terrain on which the agent may travel as a dirt road, rocky terrain, or forested terrain. The semantic mapping may further include semantic edges defining the boundaries of each area. The waypoints are integrated with the semantic edges, to enable calculation of the safest route of travel. When it is not desired to use waypoints, the map layer including the waypoints may be excluded from consideration.

[0017] The path finding system and method described herein is highly robust and adaptable. It enables efficient path making upon demand, and real-time adaptation of the path in response to changing circumstances.

[0018] In particular, the system and method described herein are characterized by the following unique attributes. First, the method focuses on utilizing a large number of map layers, each treated separately. These may include Occupancy Cost Maps, Semantic Maps, Height Maps, Gradient Maps, and Waypoint Maps. This is opposed to other path- finding methods, which utilize a single, complex map that incorporates multiple inputs of various types. Second, the method allows for a comprehensive, versatile, and adaptable approach to route planning. The planning is easily able to adapt to changes in the environment and movement of the agent. The tree is also preserved so that when a new goal is identified, existing edges in the tree may be utilized to generate a new route.

[0019] The beneficial elements of the system and method described herein are applicable regardless of the specific pathfinding algorithm that is utilized. Relevant algorithms include A*, 9*, and SMAC. In preferred embodiments, however, the system and method utilize the RRT* pathfinding algorithm. Because the RRT* algorithm is programmed to select the most efficient overall path from the root to the node from among multiple options, it is well-suited to the benefits of the presently disclosed system, which also enables consideration of multiple paths from the root to the goal and selection of the optimal path.

[0020] Additional considerations favor selection of the RRT* algorithm as a routefinding algorithm for implementing the methods described herein. First, RRT* operates in continuous space rather than discrete map space. This characteristic significantly facilitates integration of multiple map layers, as the system functions in a continuous environment. An algorithm that operates in discrete map space would necessitate merging all the maps into a single super map. By contrast, since RRT* operates in continuous and global space, each map layer may be handled independently, without combining them. Second, speed is a crucial factor. Sampling algorithms, such as RRT*, generally perform faster than other search methods, especially in high-dimensional space. The speed of the sampling algorithm, and the responsiveness to dynamic changes, makes it a preferred algorithm for performing the calculations described herein. Accordingly, the balance of the present disclosure will present the route-finding algorithm as a new and improved implementation or variation of the RRT* algorithm.

[0021] As used in the present disclosure, and in view of the above, the terms “tree” and “tree making” have a slightly different meaning than is typically used in the field. Typically, a “tree” is generated only in response to a request to generate a route from a root to a goal. In the absence of such a request, no tree is generated. By contrast, in the present disclosure, the term “tree” is used to refer to a permanent set of interrelationships between nodes within an environment, which exist independent of any root, goal, or path therebetween. Accordingly, when it is desired to refer to a mapping of travel between a root and a goal, the term “path” or “route” will be employed.

[0022] According to a first aspect, a method of route planning for a mobile agent is disclosed. The method includes: determining valid node connections within a plurality of map layers in an environment, maintained independently but considered together, to thereby generate a tree defined by connections between nodes, wherein validity of a node connection within each layer is defined based on costs of traversing from one cell to a different cell, and wherein a root of the tree is a first location of the agent; calculating least cost connections between a goal and nodes existing on the tree; and determining the route with the least cost between the root and the goal, optionally by applying a path finding algorithm, wherein, following the determining step, the tree remains available for determination of additional routes by the agent within the environment.

[0023] The method for route planning for a mobile agent, may comprise: generating cost maps on multiple map layers in an environment, wherein the cost map comprises costs of traversing from one cell to a different cell within each layer; selecting map layers to consider for route planning; setting rules for valid costs of node connections within each selected map layer; determining valid node connections within the selected map layers, considered together, to thereby generate a tree defined by connections between nodes, wherein a root of the tree is a first location of the agent; defining a goal within the environment; calculating least cost connections between the goal location and nodes existing on the tree; and applying a path finding algorithm to determine the route with the least cost between the root and the goal.

[0024] The multiple or plurality of map layers may include one or more of: an occupancy cost map, which measures the cost of occupying a given cell; a semantic map, wherein the semantic map includes descriptions of a type of object on the ground or a type of ground; a height map, wherein the height map includes information regarding elevation of every point in a given space, and thus may be used to determine energy needed to move within the environment based on height of the terrain; and a gradient map, wherein the gradient map measures side slopes within the environment. Each of these map layers may be considered, or any combination thereof, or any additional map layers not listed here. In addition or in the alternative, the plurality of map layers includes a layer of waypoints. The waypoints are specific points within the environment that the agent is encouraged to pass through. The waypoints may be weighed so that a cost of traversing via the waypoints is lower than a cost of traversing via nodes which have the same characteristics (e.g., occupancy cost, height, and side slope) but are not waypoints. Optionally, in such embodiments, the plurality of map layers includes a semantic layer including semantic edges defining boundaries of different areas within the environment, wherein the layer of waypoints is integrated with the semantic edges. Thus, for example, there may be an additional cost associated with exiting the area that has been semantically identified as a “dirt path,” which further improves the safety of the route.

[0025] Optionally, the method further includes, upon movement of the agent to a second location which is further than a predetermined distance from the first location, resetting the root of the tree as the agent’s second location, and recalculating the route on the tree. The method utilizes the same tree, which was previously created, but updates the tree from the new vantage point of the agent. Advantageously, this enables the agent to consider new information not previously considered (e.g., because certain areas of the environment became known to the agent only after the agent moved), to take into account changes in the environment, and to update the route in accordance with these updates and changes.

[0026] The method may further include repeating the resetting and recalculating steps whenever the agent moves a predetermined distance from the previous root, until the agent reaches the goal.

[0027] The method may further include rewiring the tree following movement of the agent further than the predetermined distance, and performing the recalculating step on the rewired tree. Optionally, the rewiring step comprises adding one or more new nodes to the tree. The step of adding one or more new nodes to the tree includes: selecting one or more potential new nodes to be added in a random process; for each potential new node, generating a list of near nodes, evaluating costs of connection to each near node, and forming a connection between a new node and a respective near node if said connection is permissible within cost constraints of all map layers and represents the lowest cost to the root.

[0028] The rewiring step may also include changing connections between existing nodes on the tree. In such embodiments, the step of changing connections between existing nodes on the tree may include evaluating, for each node on the tree, whether a cumulative cost of travel to the reset root is lowered with a different parent node, and, if yes, revising the connection of the node to a different parent node. The method may further include, if the rewiring step results in orphaning of one or more child nodes of the rewired node, seeking an alternative parent node for each of the orphaned child nodes, and if an alternative parent node is found, rewiring the child node to the alternative parent node, and if no alternative parent node is found, removing the child node from the tree.

[0029] The method may further include updating the map layers based on changes in environmental factors. This type of update may proceed based on, for example, environmental data that is acquired following movement of the agent, or changes to the environment generated by movement of other objects.

[0030] Optionally, in order to speed up the algorithm for evaluating environmental changes, a clustering algorithm may be employed to identify groups of nodes that have changed within the environment. The method further includes selecting nodes to consider for rewiring based on results of the clustering algorithm.

[0031] Optionally, the cluster may take the shape of an ellipse. The method may further include identifying dominant vectors of each cluster; identifying edges of the tree that intersect with the dominant vectors; and selecting the nodes to consider for rewiring based on which nodes have edges that intersect with the dominant vectors. In this manner, the changes of the environment are translated onto the map in an efficient manner.

[0032] The method may further include rewiring the tree in view of the updates in the map layers. The rewiring step includes: breaking connections between nodes that are no longer valid in view of the changes within the environment; and when a node is left without any valid connections, determining whether a new parent may be assigned for that node consistent with the rules for valid costs of node connections; if a new parent may be assigned, then adopting the node with the new parent; and if a new parent may not be assigned, then removing the node from the map.

[0033] The foregoing changes may be implemented together. The full versatility of the method described herein is evident when considering that the planned route may be adapted in response to changes in the environment, and in response to movement of the agent, essentially continuously, throughout the movement of the agent from the root to the goal. A cost of travel between an initial node Ni and a given node Nk in a map Nj may be defined according to the following equation:

[0034] Cost (Ni, Nj, Q^k, Agent) = D (Ni, Nk) + Poccupancy (Ni, Nj. Agent) + Enirect movement (Ni, Nk, Agent) + ERotationai movement (Ni, Nk, Agent) + Pfau (Ni, Nk, 9, Agent), wherein: D (Ni, Nk) refers to an absolute distance between nodes Ni and Nk; Poccupancy (Ni, Nj, Agent) refers to the cost of occupancy of the agent in node Ni in map Nj; Eoirect movement refers to an energy cost in direct movement of the agent from node Ni to node Nk, the energy cost of direct movement being accumulated based on each individual energy cost incurred during movement of the agent from node to node; ERotationai movement refers to an energy cost in rotational movement of the agent from node Ni to node Nk , the energy cost of rotation being accumulated based on each individual rotation between adjacent edges, incurred during movement of the agent from node to node, and Pfau refers to a probability that the agent will fall when traversing between node Ni and node Nk.

[0035] In some implementations, the agent is an unmanned ground vehicle, and the environment is an uneven terrain. In such implementations, the unmanned ground vehicle includes a plurality of sensors, and the method further comprises obtaining information about the environment with the sensors.

[0036] The path-finding algorithm may be a random sampling algorithm that operates in continuous space. This type of algorithm facilitates integration of multiple map layers, as the system functions in a continuous environment. In one example, the path-finding algorithm is the RRT* algorithm.

[0037] According to a second aspect, a system for route planning by a mobile agent is disclosed. The system comprises a non-transitory computer-readable medium storing instructions, that, when implemented by a processor, cause the performance of the following steps: determining valid node connections within a plurality of map layers in an environment, considered together, to thereby generate a tree defined by connections between nodes, wherein validity of a node connection within each layer is defined based on costs of traversing from one cell to a different cell, and wherein a root of the tree is a first location of the agent; calculating least cost connections between a goal and nodes existing on the tree; and applying a path finding algorithm to determine the route with the least cost between the root and the goal. In another implementation according to the second aspect, the method further includes one or more sensors configured to obtain information about the environment, wherein the sensor information is deliverable to the processor as a map layer.

[0038] Optionally, the sensors comprise one or more of a visual camera, an infrared camera, a radar sensor, and a LIDAR sensor.

[0039] Optionally, the mobile agent is an unmanned ground vehicle.

[0040] Thus, the invention provides:

[0041] A method of route planning for a mobile agent, comprising: determining valid node connections within a plurality of map layers in an environment, considered together, to thereby generate a tree defined by connections between nodes, wherein validity of a node connection within each layer is defined based on costs of traversing from one cell to a different cell, and wherein a root of the tree is a first location of the agent; calculating least cost connections between a goal and nodes existing on the tree; and determining a route with the least cost between the root and the goal, optionally by utilizing or applying a path finding algorithm.

[0042] In some cases, the step of determining a route comprises applying a path finding algorithm to determine a route with the least cost between the root and the goal.

[0043] In some configurations of the method, the plurality of map layers comprise one or more of: an occupancy cost map, which measures a cost of occupying a given cell; a semantic map, wherein the semantic map includes descriptions of a type of object on the ground or a type of ground; a height map, wherein the height map describes energy needed to move within the environment based on height of the terrain; and a gradient map, wherein the gradient map measures side slopes within the environment.

[0044] In some configurations of the method, the method further comprising: upon movement of the agent to a second location which is further than a predetermined distance from the first location, resetting the root of the tree as the agent’s second location, and recalculating the route on the tree. In some configurations of the method, the method further comprising repeating the resetting and recalculating steps whenever the agent moves a predetermined distance from the previous root, until the agent reaches the goal.

[0045] In some configurations of the method, the method further comprising rewiring the tree following movement of the agent further than the predetermined distance, and performing the recalculating step on the rewired tree.

[0046] In some configurations of the method, the rewiring step comprises adding one or more new nodes to the tree.

[0047] In some configurations of the method, the step of adding one or more new nodes to the tree comprises: selecting one or more potential new nodes to be added in a random process; and, for each potential new node, generating a list of near nodes, evaluating costs of connection to each near node, and forming a connection between a new node and a respective near node if said connection is permissible within cost constraints of all map layers and represents a lowest cost to the root.

[0048] In some configurations of the method, the rewiring step comprises changing connections between existing nodes on the tree.

[0049] In some configurations of the method, the step of changing connections between existing nodes on the tree comprises evaluating, for each node on the tree, whether a cumulative cost of travel to the reset root is lowered with a different parent node, and, if yes, revising the connection of the node to a different parent node.

[0050] In some configurations of the method, the method further further comprising, if the rewiring step results in orphaning of one or more child nodes of the rewired node, seeking an alternative parent node for each of the orphaned child nodes, and if an alternative parent node is found, rewiring the child node to the alternative parent node, and if no alternative parent node is found, removing the child node from the tree.

[0051] In some configurations of the method, the method further comprising updating the map layers based on changes in environmental factors.

[0052] In some configurations of the method, the method further comprising utilizing a clustering algorithm to identify groups of nodes that have changed within the environment, and selecting nodes to consider for rewiring based on results of the clustering algorithm. In some configurations of the method, the method further comprising: identifying dominant vectors of each cluster; identifying edges of the tree that intersect with the dominant vectors; and selecting nodes to consider for rewiring based on which nodes have edges that intersect with the dominant vectors.

[0053] In some configurations of the method, the method further comprising rewiring the tree in view of updates in the map layers.

[0054] In some configurations of the method, rewiring step comprises: breaking connections between nodes that are no longer valid in view of the changes within the environment; and when a node is left without any valid connections, determining whether a new parent may be assigned for that node consistent with set rules for valid costs of node connections; if a new parent may be assigned, then adopting the node with the new parent; and if a new parent may not be assigned, then removing the node from the map.

[0055] In some configurations of the method, a cost of travel between an initial node Ni and a given node Nk in a map Nj is defined according to the following equation:

[0056] Cost (Ni, Nj, Q^k, Agent) = D (Ni, Nk) + Poccupancy (Ni, Nj. Agent) + Euirect movement (Ni, Nk, Agent) + ERotationai movement (Ni, Nk, Agent) + Pfau (Ni, Nk, 9, Agent), wherein:

[0057] D (Ni, Nk) refers to an absolute distance between nodes Ni and Nk;

[0058] Poccupancy (Ni, Nj, Agent) refers to the cost of occupancy of the agent in node Ni in map Nj,

[0059] Euirect movement refers to an energy cost in direct movement of the agent from node Ni to node Nk, the energy cost of direct movement being accumulated based on each individual energy cost incurred during movement of the agent from node to node; and,

[0060] ERotationai movement refers to an energy cost in rotational movement of the agent from node Ni to node Nk , the energy cost of rotation being accumulated based on each individual rotation between adjacent edges, incurred during movement of the agent from node to node, and Pfau refers to a probability that the agent will fall when traversing between node Ni and node Nk.

[0061] In some configurations of the method, the agent is an unmanned ground vehicle, and the environment is an uneven terrain. In some configurations of the method, the unmanned ground vehicle includes a plurality of sensors, and the method further comprises obtaining information about the environment with the sensors.

[0062] In some configurations of the method, the path-finding algorithm is a random sampling algorithm that operates in continuous space.

[0063] In some configurations of the method, the path-finding algorithm is the RRT* algorithm.

[0064] In some configurations of the method, the plurality of map layers includes a layer of waypoints, wherein the cost of traversing through waypoints is set as lower than a cost of traversing through nodes having the same characteristics but which are not waypoints.

[0065] In some configurations of the method, the plurality of map layers includes a semantic layer including semantic edges defining boundaries of different areas within the environment, wherein the layer of waypoints is integrated with the semantic edges.

[0066] A system is provided for route planning by a mobile agent, comprising a non- transitory computer-readable medium storing instructions, that, when implemented by a processor, cause the performance of the following steps: determining valid node connections within a plurality of map layers in an environment, considered together, to thereby generate a tree defined by connections between nodes, wherein validity of a node connection within each layer is defined based on costs of traversing from one cell to a different cell, and wherein a root of the tree is a first location of the agent; calculating least cost connections between a goal and nodes existing on the tree; and applying a path finding algorithm to determine the route with the least cost between the root and the goal.

[0067] In some configurations of the system, the system further comprising one or more sensors configured to obtain information about the environment, wherein the information is deliverable to the processor as a map layer.

[0068] In some configurations of the system, the sensors comprise one or more of a visual camera, an infrared camera, a radar sensor, and a LIDAR sensor.

[0069] In some configurations of the system, the mobile agent is an unmanned ground vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] In order to better understand the subject matter that is disclosed herein and to exemplify how it may be carried out in practice, embodiments will now be described, by way of non-limiting example only, with reference to the accompanying drawings, in which:

[0071] FIG. 1A illustrates a prior art process of adding of a new node in the RRT* algorithm; FIG. IB illustrates a prior art process of rewiring a node in the RRT* algorithm;

[0072] FIG. 2A illustrates an exemplary environment in which a pathfinding task is to be implemented, according to embodiments of the present disclosure; FIG. 2B illustrates inputs for different map layers for consideration within the system of the present disclosure;

[0073] FIG. 3 illustrates a high-level overview of the process of tree-making and path finding, according to methods of the present disclosure;

[0074] FIG. 4A illustrates steps in a method of determining costs of movement between different nodes in an environment; FIG. 4B illustrates exemplary steps in a method for evaluating tree expansion to new nodes, according to embodiments of the present disclosure;

[0075] FIG. 5A illustrates exemplary steps in a method for evaluating whether addition of a new node should result in rewiring of existing nodes; FIG. 5B illustrates exemplary steps in a method of rewiring between existing nodes, according to embodiments of the present disclosure; FIG. 5C illustrates exemplary steps in a method for adopting nodes following updating of cost maps for connections between nodes, according to embodiments of the present disclosure;

[0076] FIG. 6A illustrates steps in a method of updating a map following changes in an environment, according to embodiments of the present disclosure; FIG. 6B illustrates steps in a method of updating a map following changes in the odometry of the agent, according to embodiments of the present disclosure; FIGS. 6C and 6D illustrate steps in a method of determining a path on the map, according to embodiments of the present disclosure;

[0077] FIGS. 7A-7H illustrate exemplary environments and maps for carrying out the processes of FIGS. 4A-6D; and FIGS. 8A-8B illustrate the use of waypoints and semantic mapping in determining a route.

[0078] DETAILED DESCRIPTION OF EMBODIMENTS

[0079] The present disclosure relates to the field of route planning of unmanned vehicles, more specifically, but not exclusively, to a comprehensive system for pathfinding that enables consideration of any number of map layers simultaneously, and that is equipped to adapt in response to updates in the environment and movement of the vehicle.

[0080] Before explaining at least one embodiment of the invention in detail, it is to be understood that the invention is not necessarily limited in its application to the details of construction and the arrangement of the components and / or methods set forth in the following description and / or illustrated in the drawings and / or the Examples. The invention is capable of other embodiments or of being practiced or carried out in various ways.

[0081] Environment

[0082] FIG. 2A illustrates an exemplary environment 20, in which an agent is situated. As used in the present disclosure, an “agent” is a computing device situated on a robot that is configured to move within the environment. The computing device includes a processor and a memory, which is a non-transitory computer-readable medium for storing instructions for execution by the processor. The agent may be equipped with capabilities of remote communication with a central computer, and may upload data to and receive instructions from said central computer. The agent may also include various sensors, such as a camera (visual and / or infrared ranges), a radar sensor, and a LIDAR sensor. The sensors are configured to scan the environment and communicate information about the environment to the on-board computer.

[0083] When the agent is first placed in an environment, the on-board computer may be pre-loaded with information about the environment. This information may include, for example, aerial photographs, maps, and textual identifications of the different features of the environment. In the context of unmanned ground vehicles, these features may include rocks, trees, paths, bodies of water, buildings, people, animals, and vehicles. The agent may also obtain information about the environment from the on-board sensors. Locations within environment 20 may be assigned coordinates. These coordinates may be universal planner coordinates, e.g. GPS coordinates. The coordinates may also be arbitrary grid coordinates assigned to the environment. Each point within the coordinates defines a “cell.”

[0084] Environment 20, as depicted, includes different regions with respect to allowable movement of the agent. These regions are demarcated based on information about the environment that was pre-loaded into the computer and / or learned via the sensors, and in view of tolerances which may be set by an administrator of the system. Region 22 is a low-cost region in which travel is acceptable. Movement of the agent within region 22 complies with energy and safety constraints. Region 24 is a higher-cost region, in which travel is not allowed within the constraints of the system. Region 26 is an unknown area, with regard to which the ability to travel is not yet defined.

[0085] The illustrated depiction of environment 20 represents one map “layer.” A “layer,” as used in the present disclosure, refers to a set of data associated with each node within the environment.

[0086] In exemplary embodiments, there are at least four types of permanent map layers associated with the same physical location in the environment. The permanent layers are:

[0087] • Occupancy Cost maps. An occupancy cost map measures the cost of occupying a particular cell. Each cell is given a certain number based on the probability that that cell itself is an obstacle. This probability may be derived from any of the inputs mentioned above.

[0088] • Semantic maps. Semantic maps indicate the type of ground that is being traversed (flat, rocky, muddy, watery, etc.). Each type of ground may be assigned a different difficulty value for traversing between locations within the environment. The semantic layer is particularly useful for mission-oriented planning, for avoiding obstacles such as mud and water, and for enhancing maneuverability.

[0089] • Height maps. A “height” map, or “obstacle” map, includes information regarding the elevation of every point in a given space. The height map is used to determine whether movement between different nodes is flat, or requires climbing or descending. This is relevant insofar as climbing or descending requires more energy than moving on level ground, and also requires additional travel time. Thus, the height map measures the energy needed for moving within the environment. This energy value may differ depending on the type of terrain and other factors. • Gradient maps. A “gradient” map measures the contour, or side slope, of the ground in any particular location in the environment. The gradient is one component of the cost needed for moving between locations on the map. The gradient map details the angles between different terrain cells surrounding the agent. This information is used to prevent falls caused by excessively steep side slopes. The side slope may be computed and updated using fast depth image analysis combined with recursive Bayesian updates.

[0090] • Dynamic Layer. In parallel to the layers described above, the maps also include a dynamic layer. The dynamic layer includes all objects that are subject to change within the environment. Examples include vehicles, animals, or persons. These objects may be identified and located through techniques such as image processing and SLAM-3 D tracking.

[0091] • Waypoints. In preferred embodiments, there is an additional high-level layer that includes identified waypoints. The waypoints are specific points within the environment that the agent is encouraged to pass through, for example, because they have been identified as safe for passage. The waypoints may be weighted so that a cost of traversing via the waypoints is lower than a cost of traversing via nodes which have the same characteristics (e.g., occupancy cost, height, and side slope) but are not waypoints. Optionally, in such embodiments, the semantic layer includ semantic edges defining boundaries of different areas within the environment, wherein the layer of waypoints is integrated with the semantic edges. Thus, for example, there may be an additional cost associated with exiting the area that has been semantically identified as a “dirt path,” which further improves the safety of the route.

[0092] In different map layers, the region of permitted travel and the overall pictorial representation as illustrated in FIG. 2 may be different.

[0093] FIG. 2B illustrates schematically how different sensory inputs are used to create the different map layers. On the left side of the Figure, different types of sensor inputs are listed. These inputs include semantic segmentation, depth images obtained from stereo cameras (i.e., two or cameras on the agent capturing the same spot from multiple vantage points), and other types of sensors which are not listed here. In the middle of the Figure, each of these inputs is utilized to create a map layer. The map layers illustrated herein include a semantic layer, a height or obstacle layer, and a side-slope layer (same as the “gradient layer” discussed above). Each of the cost maps derived from these layers is fed to the processor for tree-making and route planning, as will be discussed further herein. Any number of the map layers may be considered for any particular path-making task.

[0094] As is apparent from the foregoing description, the pathfinding system and method described herein are predicated on exploration of the environment, even before any specific request is entered to plan a route. This initial exploration is relied upon whenever the system is called upon to perform a specific pathfinding task. In addition, continuous exploration is performed to update the data upon which the analysis is predicated.

[0095] Still referring to FIG. 2A, in order to commence a mapping operation, the agent identifies various “cells” within the environment, between which the agent may travel. The term “cell” is generally used to refer to a two-dimensional point within an environment. Cells within an environment are eligible to become “nodes.” The term “node” refers to a point which has been added to the “tree” (in the sense used in the present disclosure) and which can be used within a route.

[0096] As noted above, the terms “tree” and “tree making” have a slightly different meaning than is typically used in the field. Typically, a “tree” is defined between one initial root, which remains fixed, and a goal. In the present disclosure, the term “tree” is used to refer to a permanent set of interrelationships between nodes within an environment, which exist independent of any one root, goal, or specific path therebetween. To avoid confusion, when it is desired to refer to a mapping of travel between one specific root and one specific goal, the term “path” or “route” will be employed.

[0097] Within the context of route-making, the route begins with an initial node, called a “root,” proceeds through intermediate nodes, and ends at a final node, called a “goal.” Each intermediate node has a parent node, which is the node from which that node branches, and one or more child nodes, which branch therefrom. A straight-line connection between two different nodes is referred to as an “edge.”

[0098] In the example of FIG. 2A, the agent has begun a mapping process from initial node 30 to one or more nodes 32. The path proceeds along edges 34. In addition, in the illustrated example, node 36 is a parent of node 38, and node 38 is a child of node 36. This is because a path extending from the initial node 30 reaches node 38 via node 36. FIGS. 8 A and 8B illustrate the use of waypoints as part of a route-making process. In FIG. 8A, an environment is depicted with various areas indicated thereon. These include: A dirt road; and off-road portion of trees; and an off-road portion of rocks. A path is defined from the “start” point (described above as the “root”) to the goal. As can be seen, the agent is routed not on the shortest possible path in terms of net distance, but on a path that maximizes the time spent on the waypoints (which are located on the dirt path). FIG. 8B illustrates how the path-making process may be integrated with a semantic map. As seen at the bottom left of the Figure, initially, a Start and a Goal may be identified, and a Global Planner may be used to evaluate paths for the agent to traverse from the Start to the Goal, as will be set forth in the remainder of this Disclosure. A Global Trajectory is output from the global planner, without consideration of semantic segmentation. In parallel, a semantic segmentation process may be performed on an orthophoto map, thereby generating a semantic map of the same environment. The semantic map may include identification of dirt pathways, and thus may be used to provide a source of waypoints; alternatively, the waypoints may be identified through an independent analysis. The global trajectory is integrated with the semantic map, so the global trajectory is enriched with semantic data, indicating what type of terrain is expected to be seen on the chosen path. In the final stage of the analysis, this combined global trajectory with the waypoints and semantic edges is fed to the low-level local planner, which incorporates the different layers described above and the analytical techniques described below, in planning each particular step or movement of the agent.

[0099] The remainder of this disclosure describes an architecture and flow charts for pathfinding in environments such as environment 20.

[0100] General Architecture

[0101] FIG. 3 illustrates a high-level overview of a system 300 for path planning. The system 300 generally consists of three modules, housed within central processor 304.

[0102] Module 301 performs processes of tree expansion and rewiring. These processes relate to evaluating the feasibility of cells within the environment to become nodes on the tree, adding potential nodes to the tree, and evaluating permissible edges or routes between different nodes. These are preliminary steps, antecedent to any specific route planning. These processes are described in detail herein in connection with FIGS. 4A-4B and 5A-5C. Module 302 considers dynamic processes, including changes in the map and movement of the agent. The dynamic processes are described in detail in connection with FIGS. 6A-6B.

[0103] Module 303 considers the path-making process itself. This is described in detail in connection with FIGS. 6C-6D.

[0104] Tree Expansion and Rewiring

[0105] The tree expansion process refers to a process of assessing the different possible connections between different cells in the environment, adding cells as nodes of a tree, and establishing paths or edges between the different node locations.

[0106] FIG. 7A illustrates an exemplary environment 70, containing cells 71-80. At the present stage, there is no tree. In subsequent Figures, when the cells are part of trees, and in reference to algorithms and calculations described herein for route making and path finding along a tree, the cells are alternatively referred to as “nodes.”

[0107] Referring to FIG. 4A, in order to evaluate whether a cell should be added to the tree, the costs of travel between different cells within the environment are calculated. The costs of such travel are organized in a table known as a “Connections Data List.” The term “connections data list” refers to a list including all identified nodes within an environment (node i) and costs and validations or connections between each individual node i and the other nodes in the environment. The term “costs” refers to the cost of taking a path, based on factors such as energy and safety, in which a lower cost signifies that the path is better. The term “validations” signifies that the path is valid, i.e., that it forms connections only between cells that are permissible to traverse. This is as opposed to a non-validation route, which is a route that goes through a cell which is defined as not possible to pass. For example, with reference to FIG. 7A, the table includes the costs of travel between cell 71 and each of cells 72-80, and also includes a list of validations of the various cells. Although FIG. 7A schematically illustrates only one “map layer,” such a table is generated for each of the map layers.

[0108] At step 401, a loop (a sequence of instruction s that is continually repeated until a certain condition is reached) is started over a list of nodes and a list of maps. The node list includes an initial node Ni (selected arbitrarily and a list of given nodes Nk. The maps list includes an initial map Ni (also selected arbitrarily) and a list of given maps Nj. The purpose of this step is to create a list containing information about the connections between a selected node and a list of other nodes. The initial cost for each connection starts at zero. Then, as the tree expands, the price from each map is added incrementally. Thus, at the outset of this inquiry, a “total cost” to reach each node from the initial node is given as “0.”

[0109] At step 402, an evaluation is performed for each pair of nodes. The evaluation addresses the question - is a connection between the initial node Ni and given node Nk invalid within any map j? Stated differently, within any map j, does a connection between a pair of nodes exceed the maximum permitted value (as defined by an administrator) for that map?

[0110] The cost of traveling between an initial node Ni and a given node Nk may be expressed as the sum of various cost elements. The cost elements are accumulated as the agent plans a route from the root Ni, via various nodes, until reaching node Nk. In one example, the cost is defined according to the following equation:

[0111] Cost (Ni, Nj, Q^k, Agent) = D (Ni, Nk) + Poccupancy (Ni, Nj. Agent) + EDirect movement (Ni, Nk, Agent) + ERotationai movement (Ni, Nk, Agent) + Pfau (Ni, Nk, 9, Agent) wherein: D (Ni, Nk) refers to an absolute distance between nodes Ni and Nk. Poccupancy (Ni, Nj, Agent) refers to the cost of occupancy of the agent in node Ni in map Nj, Eoirect movement refers to an energy cost in direct movement of the agent from node Ni to node Nk (the energy cost of direct movement being accumulated based on each individual energy cost incurred during movement of the agent from node to node) ERotationai movement refers to an energy cost in rotational movement of the agent from node Ni to node Nk(the energy cost of rotation being accumulated based on each individual rotation between adjacent edges, incurred during movement of the agent from node to node), and Pfaii refers to a probability that the agent will fall when traversing between node Ni and node Nk.

[0112] Taking the illustration of FIG. 7A as an example, each of the cells 71-80 may be paired, and the costs for traveling between each pair of cells may be defined. Some pairs of cells may exceed the limit because they are too far from each other, such as cells 71 and 79. Other pairs of cells may exceed the limit because of obstacles in between them, which would raise the probability of the agent falling, such as nodes 72 and 73 which have obstacle 91 between them. Still other pairs might have a high cost when some factors are considered, but a lower cost when other factors are considered, and thus travel between the nodes is allowed. For example, cells 76 and 77 have a smaller obstacle 92 in between them, and are relatively close together. The connection between these cells may be deemed acceptable. One of the cells, such as cell 71, may be designated as the root, and the tree may be built in relation to that cell.

[0113] Returning to FIG. 4 A, at 403, if the cost does exceed the defined limit, then the cell is not added to the tree at that point. The cell may be added to the tree at a subsequent point (e.g., cell 73 may not be added as a connection to cell 72, but may be added as a connection to node 76.). At 404, the next cell Nk+i is considered within that map, and the process continues.

[0114] At 405, if the cost does not exceed the defined limit, then the cell is added to the tree. In the present disclosure, once the cell is added to the tree, it is referred to as a “node.” At 406, the total cost of traveling from Ni to Nk is obtained within the map J that is being considered, and, at 407, the total cost from the initial cell to cell Nk is calculated, for all of the maps J.

[0115] At 408, an evaluation is carried out regarding whether all maps j have been checked. If false, at 409, then the process is repeated on the next map j + 1. If true, at 411, then the connection data is added between Ni and Nk .

[0116] Once a given node Nkis added, at 412, the process is repeated for all cells in Nnear (meaning, all cells in the environment within a certain distance from the initial cell). If all cells have not been checked, at 413, then the process proceeds anew with the next cell. If all cells have been checked, at 414, then the list of nodes is set. The connections data list may be sorted by cost, at 415, to provide the cost of travel between any two nodes in the environment. For example, the cost of travel between node 71 to node 72 may be assigned “1,” between node 71 and node 74 may be assigned “3,” and between node 71 and node 75 may be assigned “4.” The costs may be calculated for any two nodes that are on the tree, including nodes that are more than one edge apart from each other. The values of the costs may be dimensionless, as they express relative costs of travel. At 416, this concludes the process of creating the costs and validations list.

[0117] After a connections data list is in place for an environment, new nodes may be added to the tree. This process is illustrated within FIG. 4B, which depicts a flow chart for a method of tree expansion and rewiring 420, and FIG. 7B, which illustrates the new point being added to the tree. A new node may be added to the tree within the boundaries of the existing tree. That is, a tree may be constructed without fully exhausting all cells that may be included on the tree. At some subsequent point, additional cells may be added as nodes. In the alternative, the agent may move locations within the environment, thereby exposing the agent to areas that were not previously mapped, and accordingly necessitating consideration of how to add the new nodes to the tree. This process is depicted in FIGS. 4B and 7B. In advantageous embodiments, this process is commenced with a random search. The random search increases the chances of finding connection points and helps to expand the tree effectively in space.

[0118] Referring to FIG. 4B, at step 421, a random point (Nrand) is generated from the environment. The random point is a location that has not previously been identified as a node on the tree. In FIG. 7B, cell Nrand is indicated. At 422, from among the existing nodes on the tree, the nearest node to Nrand (referred to as Nearest) is selected. In the illustration of FIG. 7B, node 78 is the nearest node. At 423, another cell and potential node, Nnew, is then selected. Nnew is a point on the line between Nrand and Nearest. In exemplary embodiments, Nrand being selected randomly, may have a relatively large distance from Nearest. Nnew is on a vector between Nnew and Nearest within a predetermined range from Nearest. This range is chosen so that the edge between the existing tree and the new node is of a desired length that is typical for the tree. At 424, a list of nodes that are already on the tree that are closest to Nnew is generated. In the example, these may be nodes 75, 78, and 80. This list constitutes the Nnear list for the new potential node. At 425, the process of method 400 is then performed, in order to determine the cost of movement from each of the nodes in the Nnear list to the proposed new node. The next step, at 426, depends on whether there is any valid connection to the tree. If not, then, at 427, the process ends, and new node Nnew is not added. If yes, then, at 428, new node Nnew is added to the tree. At 429, the node may be subsequently rewired to a new node based on changes in the tree (e.g. addition of other new nodes which lower the overall cost to the root), as will be discussed further herein.

[0119] Figs. 5A, 5B, and 5C depict various algorithms that are utilized in the processes of optimizing the tree and rewiring between nodes of the tree.

[0120] Fig. 5 A depicts a process of rewiring a tree via adding a new node. At step 501 of 5 A, a recursive loop is started. A given node Ni, which is already on the tree, is selected to begin the loop. The current cumulative cost of reaching Ni from the root is the sum of each of the costs of the links from Nito the root. For example, considering FIG. 7C, a lowest-cumulative-cost path between root 71 and node 80 is indicated in a thickened line.

[0121] In FIG. 7C, all the nodes in the tree are indicated. Edges are drawn for connections that form the tree, representing the lowest cumulative cost to the root. Costs for certain connections which are not part of the tree, because they do not represent the lowest cumulative cost from the root, are indicated in phantom and without edge lines between them.

[0122] At step 502, a value Tend(Ni)Cost is defined as the cost of rerouting Nito the root via Nx. For example, if Nxis inserted between Ni and the root, then the total cost of reaching Ni is the sum of the costs from the root to Nxand from Nxto Ni. For example, in FIG. 7C, a new node Nxis inserted onto the tree. Based on the cost and data valuations which were previously assigned in the Connections Data List (see discussion in reference to FIGS. 4A and 7A), the cost of routing via that node is added. At step 503, an evaluation is performed regarding whether the revised cost obtained by rerouting through Nxis lower than the current cost. If false, then at 504, then that particular rerouting is discarded. The analysis is repeated with respect to another node in the Connections Data List. If true, then, at step 505, node Ni is disconnected from its parent and connected instead to Nx. For example, as seen in FIG. 7C, the cost of reaching node 80 is indeed less via node Nxas compared to node 75. Thus, the connection between node 80 and node 75 is severed, and the connection is instead introduced between node 80 and node Nx.

[0123] This analysis is repeated for each node in the environment Nnear (meaning, all potential nodes within a certain distance from the selected node), indicated at 506. Until all nodes have been checked, the next node in the connections data list is considered, at 504. If all nodes have been checked, then, at step 507, the process ends.

[0124] FIG. 5B depicts a similar analysis for evaluating rewiring of nodes in a tree, except that in FIG. 5B, the proposed new nodes are already on the tree. This process has many parallels to the process detailed in FIG. 5A. However, because the nodes that are being rewired are already on the tree, this step requires an additional evaluation of whether the rewiring causes the node to be severed from the tree.

[0125] At step 511, a recursive loop is begun. At 512, the current cost of reaching Ni is defined as the cumulative cost of reaching Ni from the root. For example, considering FIG. 7D, suppose an initial route from node 71 to node 80 proceeded via nodes 74 and 78. The cumulative cost is 11. The revised cost of reaching Ni is defined as the cost of reaching Ni via existing node Nj (also referred to herein as Tend_Ni_Cost). In this example, the cost of reaching node 80 via node 74 and node 75 is 8, which is lower than the initial cost.

[0126] If the revised cost is not less than the current cost, at step 513, then the proposed revised node is not used. At 514, if all nodes eligible for serving as a new connection have already been checked, then the rewiring is determined to be unsuccessful (step 515), and the process ends (step 516). This means that the existing tree is more optimal than rewiring through the proposed node. If some nodes remain for checking, then the process is repeated with respect to those nodes (step 517).

[0127] If, on the other hand, the revised cost is lower than the existing cost, as in the example of FIG. 7D, a second analysis is conducted to determine whether the new connection will disconnect the node Ni from the root (step 518). This might occur, for example, if more than one node is deleted at or around the same time. If the new connection will indeed disconnect the goal from the root, the proposed node is rejected, and the flow chart returns to step 514. If the rewiring will not disconnect the node from the root, then, at step 519, the rewiring is successful. Thus, in the example, node 80 is disconnected from node 78 and is instead connected to node 75. At step 520, the process ends.

[0128] Referring to FIG. 5C, sometimes, due to changes in a map or environment, or due to a changed location of the agent, a route which had previously been mapped is no longer valid, because (for example) the costs for traversing between different nodes on the route exceed tolerances, or because cumulative costs of traversing between the node and the root exceeds a particular tolerance. As a result, a parent node for a particular node may be removed from the tree. Accordingly, it is appropriate to evaluate whether the child node may be retained on the tree through rerouting via an existing node.

[0129] This process 530 is shown in FIG. 5C and in FIG. 7E. Process 530 is similar to process 510 of FIG. 5B, except that in FIG. 5B, the rewiring takes place while the node that is rewired is still on the tree, and thus serves only to optimize the route, whereas in FIG. 5C, the rewiring takes place after the node has been provisionally removed from the tree, and thus serves to save the route through that node from being cancelled.

[0130] Referring to FIG. 7E, suppose that node 72 has been severed from node 71. For example, an obstacle 93 has entered the space between nodes 71 and 72, rendering traversing therebetween overly costly. Nodes 76, 73, 77, and 79 are children only of node 72, such that if node 72 is removed from the tree, the other nodes would also be removed from the tree. Node 72 could be restored to the tree if it were “adopted” by another node which could serve as a parent node

[0131] At step 531, the analysis is initiated for a given node Ni to adopt (here, node 72). At 532, a list of near nodes to node 72 that are candidates for serving as new parent nodes is generated and designated as Nnear. In this example, the nodes that are near to node 72 may include nodes 74 and 75. Using the connections data list, costs of connections to each of the nodes in Nnear are collected. At 534, the new nodes are evaluated to determine whether there is a valid connection between the orphaned node and the new node.

[0132] If there is no valid connection, then, at step 535, adoption is deemed to have failed. At 536, the node is removed from the tree. In addition, at 537, all nodes that are children of the node that is removed are also added to a list of nodes to adopt, so that the same process may be performed for them (if, for example, a node exists for rewiring of the child but not for rewiring of the node that is currently being examined.

[0133] If there is a valid connection, then, at 538, process 510 for rewiring with an old node is performed. This process ensures that the rewiring does not result in removal of the node from the root of the tree, as discussed. If that process is unsuccessful, at 539, then the adoption is false, and the process proceeds at step 535 with removing the node from the tree, as discussed. If the process is successful, the rewiring is set. In the illustration of FIG. 7E, node 72 is rewired to a new parent 75. Regardless of the outcome of the adoption for each node, the process proceeds at 540 with consideration of other nodes that need adoption, and movement to consideration of the next node to adopt, at 541. When all nodes have been considered, at 542, then the process ends.

[0134] Changes in Map Information

[0135] Up until now, the processes that have been considered relate to the route mapping and tree building processes. The following charts relate to the specific manners in which dynamic updates may be implemented and reflected in the trees.

[0136] Referring to FIG. 6A, a process of evaluating changes in a map is disclosed. At 601, the process is started. At step 602, the term Mapaiff is defined as the differences between the new map and the old map. The difference may be expressed as a matrix, with a numerical value assigned to the differences in each cell. For purpose of evaluating differences in the map, all cells are considered, whether or not a tree node is contained within them. For example, in the map of FIG. 7F, obstacles 93, 94, 95, 96, and 97 have entered the map. At 603, a sensitivity threshold may be assigned for considering whether differences are significant. At step 604, an evaluation is performed to determine whether any cells in the map have changed in a manner that exceeds the sensitivity threshold. If no, then, at step 605, the process ends, and no further action is necessary.

[0137] If the changes that have occurred do exceed the sensitivity threshold, then these changes must be implemented in the tree, which had previously been constructed. To do so, at step 606, a process of finding tree nodes whose edges intersect with the changes in the map is begun. The specific manner that this process of identifying the relevant tree nodes is described in the following steps 607-614. To be clear, the process described herein does not actually involve updating the tree, as this is performed in different processes. Rather, in the process described here, the nodes that are potentially affected by the update in the map (those whose edges intersect with the map changes) are identified, for each map layer. This, in turn, enables consideration of the need for changes in the tree only on a completed list of nodes. For each new map layer that is considered, nodes whose edges intersect with map changes are added to the general list of nodes to update. If a node is found to intersect changes in multiple map layers, it is added to the list only once to avoid redundant checks. During the dynamic update process, connections to the node are updated based on the changes in the various maps. This approach is more efficient because most changes occur across multiple layers simultaneously. By first collecting all the changes and the intersecting edges, and then performing the update, which is computationally intensive, the speed of the process is improved.

[0138] At step 607, a process is begun to represent the position of map cells that have changed, in planner coordinates. Planner coordinates are continuous coordinates with a fixed origin. The origin, for example, may be the tree root. The planner coordinates are used instead of map coordinates, which are discrete coordinates and do not necessarily have a fixed origin, as opposed to relative coordinates which may change based on the display of a screen. At 608, the planner coordinates are transposed onto the map, by taking the distance between the changed map cell and the center of the map, and multiplying by the map resolution.

[0139] When a map has changed, it may be computationally intensive to consider all of the changes on their own. Accordingly, at 609, a clustering algorithm may be performed in order to group the changes and identify the areas of significant change. The clustering algorithm may be any suitable algorithm known to those of skill in the art.

[0140] This process is schematically illustrated in FIG. 7H. In FIG. 7H, at a first step, a tree is illustrated with root 61 and edges 62a, 62b, and 62c. In the second step, changes 63a-f are added to the map. In the third step, the various changes are clustered into two clusters 64a, 64b.

[0141] Referring back to FIG. 6A, at 610, the mean and covariance is calculated for each cluster. This is illustrated at the fourth step of FIG. 7H, with ellipses 65a, 65b, being drawn around each cluster. At 611, a PCA process is performed to identify the two dominant vectors. The two dominant vectors are the diagonals of the ellipse derived from the covariance matrix calculated in step 610 for each cluster group. This is illustrated at the fifth step of FIG. 7H, with the diagonals 66 being identified. The vectors of the diagonals 66 are positioned so that their tails are at the mean of the distribution. For the diagonals 66 of the ellipse so identified, intersects are found with all tree edges, at 612. This is illustrated at the sixth step of FIG. 7H, with intersecting points 67a and 67b marked on the map. Intersecting points 67a and 67b are locations where existing edges cross the diagonals of the ellipse derived from the cluster. From this process, at 613, a list of nodes on the existing tree is selected for potential rewiring. These are the nodes whose associated edge (or edges) intersects with changes in the dominant vectors. In this manner, only the nodes that are directly affected by the change in the map are updated.

[0142] This process is repeated for each map layer. Then, at step 614, the list of nodes to update in each map is added to a general node to update list. This list is utilized later in the dynamic update process. At 615, the process ends.

[0143] FIG. 6B illustrates a process 620 of updating the tree in response to new odometry of the agent. As used in the present context, the term “odometry” refers to the calculation of change in position over time. Such changes may be detected with sensor data.

[0144] The first step, at 621, is to update the pose of the agent, meaning to determine the present location of the agent. At 622, the distance that the agent has moved since the last tree root computation is recorded, and is stored as a value called “diff pose.” At 623, this change in distance is evaluated in order to determine whether the difference in the pose of the agent is significant, meaning whether it exceeds a minimum tolerance for updating the tree. At 624, if the minimum difference is not exceeded, no update is performed, and the process ends. At 625, if the difference is determined to be significant, the tree root is replaced. The new root is defined as the current pose of the agent. At 626, the cost to each node in the tree that connects to the root is updated, based on the changed location of the agent. At 627, when the changed cost results in the cost exceeding the permitted cost of traveling to a particular node, the connection to that node is severed, and a process of adopting that node is performed, as discussed above. At 628-634, a process of rewiring is also performed for nodes that are near the new root. For these nodes, it may be more efficient to connect directly to the new root, as opposed to indirectly. Thus, for each of the nodes within a certain radius to the new root, the connections data list is consulted to determine if there is a valid direct connection to the new root; if there is, then the node is rewired to connect to the new root.

[0145] The foregoing description applies to handling of localized changes within a single map. In the event that it is desired to add or subtract an entire map (e.g ., a semantic map, gradient map, or height map, as discussed above), a full update of the entire tree is required, as every pixel on the new map represents a change. Therefore, a comprehensive update of the entire tree is performed, using the data from the new map.

[0146] Determining a Path to a Goal

[0147] FIGS. 6C and 6D illustrate a process 650 of determining a path to a goal, according to embodiments of the present disclosure. The method is performed within an environment in which the cost maps for travel between different regions and the nodes in a tree have already been identified, as discussed in connection with the previous Figures. The method is illustrated in connection with the previously described environments, in FIG. 7G.

[0148] At step 651, the process of finding a path from a goal to a tree root is commenced. To do so, the goal location is identified. The goal may be identified based on human input, for example, or through any sort of machine learning process. The goal location may already be a node on the tree or may be in the environment without being on the tree. In FIG. 7G, the goal is identified as point 99.

[0149] At step 652, a list of nodes that are already on the tree and that are near the goal is assembled. This list is referred to as Nnear. In FIG. 7G, this list may include, for example, nodes 74, 75, 78, and 80. At step 653, the process of the “connections data list” described above is performed. In this process, the cost of connection between the goal 99 and each of the nodes in Nnear is identified. At step 654, the processor determines if there is any valid connection. If not, at 655, the process stops, with the end 656 of the method, as it is not possible to reach this target. If yes, at 657, a process begins of generating a list of paths from the goal to the individual nodes within Nnear. The process utilizes the data from the connections data list, as discussed above. For example, in FIG. 7G, paths may be formed from the goal 99 to nodes 75, 74, 78, and 80.

[0150] Once the goal is connected to one or more nodes of the tree, a recursive loop is used to get the path from each such node to the root of the tree. This loop is discussed in the following steps. At step 658, the loop is started within the connections data list. A node is selected from the group of near nodes and given an identifier Node i. At 659, path i is defined as the path from the goal to node i. For example, in FIG. 7G, the initially selected node may be node 75, and a path 82 may be drawn between goal 99 and node 75. At 661, an analysis is performed - is the parent of the current node the tree root? In this case, the parent of node 75 is not the tree root, as the tree root is node 71. Thus, at 668, a path is added to the tree to the next node, until the current node is the node parent, at 669. At that point, at 662, the end segment is added to the path, the end segment being the last segment from the current node to the root. At 663, this completes the process of adding one path (path i) to a list of paths to the root.

[0151] The same process may be repeated multiple times, so that multiple paths are drawn from the goal back to the root, along the tree. At 664, verification is performed to make sure that all nodes in the connections data list are checked. If this is not yet the case, then at 665, the next node in the connections data list (i + 1) in relation to the goal is considered. The process is continued until each node in the connections data list that is an eligible node relative to the goal node is considered.

[0152] Once all nodes are considered and a list of potential paths is generated, at 666, a process of path filtering on all paths to the root is started. For example, considering the environment of FIG. 7G, a route may be defined via nodes 78, 74, and 71, or via nodes 75, 74 and 71, or via nodes 75 and 72. All of these routes may be eligible routes insofar as the route may be drawn on existing nodes of the tree without violating the rules of the tree or engaging in higher costs than allowed. From among all of these routes, at 667, the final route that is selected is the path which has the minimum cost to the root from all paths. At 668, the path to the goal is set, and this is the end of the process In sum, the processes described herein illustrate a system and method for defining a tree relative to an environment, based on multiple map layers; optimizing the tree; updating the tree in response to changed conditions and movement of the agent, and selecting an ideal path between an agent and any goal within the environment. As may be recognized by those of skill in the art, certain of the steps herein may be performed with basic modifications without departing from the scope of the present disclosure.

Claims

CLAIMS:

1. A method of route planning for a mobile agent, comprising: determining valid node connections within a plurality of map layers in an environment, maintained independently but considered together, to thereby generate a tree defined by connections between the nodes, wherein validity of a node connection within each layer is defined based on costs of traversing from one cell to a different cell, and wherein a root of the tree is a first location of the agent; calculating least cost connections between a goal and nodes existing on the tree; and determining a route with the least cost between the root and the goal, optionally by applying a path finding algorithm, wherein, following the determining step, the tree remains available for determination of additional routes by the agent within the environment.

2. The method of claim 1, wherein the plurality of map layers comprises one or more of: an occupancy cost map, which measures a cost of occupying a given cell; a semantic map, wherein the semantic map includes descriptions of a type of object on the ground or a type of ground; a height map, wherein the height map includes information regarding an elevation of every point in a given space; and a gradient map, wherein the gradient map measures side slopes within the environment.

3. The method of claim 1, wherein the plurality of map layers includes a layer of waypoints, wherein the cost of traversing through waypoints is set as lower than a cost of traversing through nodes having the same characteristics but which are not waypoints.

4. The method of claim 3, wherein the plurality of map layers includes a semantic layer including semantic edges defining boundaries of different areas within the environment, wherein the layer of waypoints is integrated with the semantic edges.

5. The method of claim 1, further comprising: upon movement of the agent to a second location which is further than a predetermined distance from the first location, resetting the root of the tree as the agent’s second location, and recalculating the route on the tree.

6. The method of claim 5, further comprising repeating the resetting and recalculating steps whenever the agent moves a predetermined distance from the previous root, until the agent reaches the goal.

7. The method of claim 6, further comprising rewiring the tree following movement of the agent further than the predetermined distance, and performing the recalculating step on the rewired tree.

8. The method of claim 7, wherein the rewiring step comprises adding one or more new nodes to the tree.

9. The method of claim 8, wherein the step of adding one or more new nodes to the tree comprises: selecting one or more potential new nodes to be added in a random process; and, for each potential new node, generating a list of near nodes, evaluating costs of connection to each near node, and forming a connection between a new node and a respective near node if said connection is permissible within cost constraints of all map layers and represents a lowest cost to the root.

10. The method of claim 7, wherein the rewiring step comprises changing connections between existing nodes on the tree.

11. The method of claim 10, wherein the step of changing connections between existing nodes on the tree comprises evaluating, for each node on the tree, whether a cumulative cost of travel to the reset root is lowered with a different parent node, and, if yes, revising the connection of the node to a different parent node.

12. The method of claim 11, further comprising, if the rewiring step results in orphaning of one or more child nodes of the rewired node, seeking an alternative parent node for each of the orphaned child nodes, and if an alternative parent node is found, rewiring the child node to the alternative parent node, and if no alternative parent node is found, removing the child node from the tree.

13. The method of claim 1, further comprising updating the map layers based on changes in environmental factors.

14. The method of claim 13, further comprising utilizing a clustering algorithm to identify groups of nodes that have changed within the environment, and selecting nodes to consider for rewiring based on results of the clustering algorithm.

15. The method of claim 14, further comprising: identifying dominant vectors of each cluster; identifying edges of the tree that intersect with the dominant vectors; and selectingnodes to consider for rewiring based on which nodes have edges that intersect with the dominant vectors.

16. The method of claim 13, further comprising rewiring the tree in view of updates in the map layers.

17. The method of claim 15, wherein the rewiring step comprises: breaking connections between nodes that are no longer valid in view of the changes within the environment; and when a node is left without any valid connections, determining whether a new parent may be assigned for that node consistent with set rules for valid costs of node connections; if a new parent may be assigned, then adopting the node with the new parent; and if a new parent may not be assigned, then removing the node from the map.

18. The method of claim 1, wherein a cost of travel between an initial node Ni and a given node Nk in a map Nj is defined according to the following equation:Cost (Ni, Nj, Q^k, Agent) = D (Ni, Nk) + Poccupancy (Ni, Nj. Agent) + Euirect movement (Ni, Nk, Agent) + ERotationai movement (Ni, Nk, Agent) + Pfau (Ni, Nk, 9, Agent), wherein:D (Ni, Nk) refers to an absolute distance between nodes Ni and Nk;Poccupancy (Ni, Nj, Agent) refers to the cost of occupancy of the agent in node Ni in map Nj,Euirect movement refers to an energy cost in direct movement of the agent from node Ni to node Nk, the energy cost of direct movement being accumulated based on each individual energy cost incurred during movement of the agent from node to node; and,ERotationai movement refers to an energy cost in rotational movement of the agent from node Ni to node Nk , the energy cost of rotation being accumulated based on each individual rotation between adjacent edges, incurred during movement of the agent from node to node, and Pfau refers to a probability that the agent will fall when traversing between node Ni and node Nk.

19. The method of claim 1, wherein the agent is an unmanned ground vehicle, and the environment is an uneven terrain.

20. The method of claim 19, wherein the unmanned ground vehicle includes a plurality of sensors, and the method further comprises obtaining information about the environment with the sensors.

21. The method of claim 1, wherein the pathfinding algorithm is a random sampling algorithm that operates in continuous space.

22. The method of claim 21, wherein the pathfinding algorithm is the RRT* algorithm.

23. A system for route planning by a mobile agent, comprising a non-transitory computer-readable medium storing instructions, that, when implemented by a processor, cause the performance of the following steps: determining valid node connections within a plurality of map layers in an environment, maintained independently but considered together, to thereby generate a tree defined by connections between nodes, wherein validity of a node connection within each layer is defined based on costs of traversing from one cell to a different cell, and wherein a root of the tree is a first location of the agent; calculating least cost connections between a goal and nodes existing on the tree; and determining the route with the least cost between the root and the goal, optionally by applying a path finding algorithm, wherein following the determining step, the tree remains available for determination of additional routes by the agent within the environment.

24. The system of claim 23, further comprising one or more sensors configured to obtain information about the environment, wherein the information is deliverable to the processor as a map layer.

25. The system of claim 24, wherein the sensors comprise one or more of a visual camera, an infrared camera, a radar sensor, and a LIDAR sensor.

26. The system of claim 23, wherein the mobile agent is an unmanned ground vehicle.

Images