Patent Application
20210342715
The present application claims the benefit under 35 U.S.C. § 119 of EP 20172025.7 filed on Apr. 29, 2020, which is expressly incorporated herein by reference in its entirety.
The present invention is related to the field of motion path planning for multi-agent systems considering the presence of multiple objects and/or individuals.
In general, path planning approaches for multi-agent systems distinguish between decoupled path planning and the centralized path planning. In the decoupled path planning approaches, each agent computes its path by avoiding conflicts. Due to the parallel computation, decoupled path planning can be performed relatively fast, but optimality and even completeness are not always guaranteed.
In contrast thereto, coupled path planning approaches searches the entire state space considering all the agents' states, such as known from an A*-based algorithm.
While most path planning algorithms consider the presence of an object or individual as an obstacle for the motion path, the presence of groups of individuals is more difficult to consider, as also motion paths between individuals of a single group should be avoided.
According to the present invention, a method is provided for motion path planning of multiple agents in a multi-agent system, as well as a device, an agent, and a multi-agent system.
Example embodiments of the present invention are disclosed herein.
According to a first aspect a computer-implemented method for planning a motion path for multiple agents is provided. In accordance with an example embodiment of the present invention, the method, comprises the following steps:
The above method is related to a multi-agent path finding problem according to a coupled path planning approach which is performed in a central unit being in communication with multiple agents. The central unit obtains all information about poses of the multiple agents, objects and groups of objects and determines motion paths for all the multiple agents. Conflicts of one agent with other agents and with obstacles, objects, individuals and groups of objects/individuals are avoided by the path planning algorithm carried out in the central unit.
One common multi-agent path planning algorithm is called the conflict-based search (CBS) as described in G. Sharon et al., “Conflict-based search for optimal multi-agent path finding”, Proceedings of the 26th AAAE Conference on Artificial Intelligence, p. 563 to 569.
Particularly, the conflict-based motion planning may include considering a constraint tree built with nodes defining constraints for at least one of the agents.
Basically, conflict-based search is a two-level algorithm wherein the high-level search is performed in a constraint tree whose nodes include constraints on time and location for a single agent. At each node in the constraint tree, a low-level search is performed to find paths for all of the multiple agents under the constraints given by the high-level node. In the high-level algorithm part, the constraint tree is searched. Each node in the constraint tree contains a set of constraints imposed on each agent, a single consistent solution including one path for each agent that is consistent with the set of constraints given for the respective node, and the total costs of the current solution.
Once a consistent path has been found by any suitable path finding algorithms for each agent, the found paths have to be checked with respect to the paths of each other agent by simulating the movement of the agents along their planned paths. If all agents reach their goal without any conflicts, e.g. no collision of two agents occurred, the node of the constraint tree is declared as the goal node and the solution can be returned. If, however, while performing the validation, a conflict is found for two or more agents, the validation is stopped, and the node is declared as non-goal.
A main feature of the conflict-based search algorithm is to grow a set of constraints for each agent and find paths that are consistent with these constraints. If these paths still have conflicts and are thus invalid, the conflicts are resolved by adding new constraints. The high-level tree path (building the constraint tree) therefore basically is for finding and adding constraints.
Given a non-goal node of the constraint tree, whose solution includes a conflict, it is known that in any valid solution at most one of the conflicting agents may occupy the vertex v at time t. Therefore, at least one of the constraints for at least one of the agents must hold. Consequently, the algorithm generates two new nodes for the constraint tree as children of the non-goal node each adding one of these constraints to the previous set of constraints. For each new constraint tree node, the low-level search algorithm part is only activated for a single agent for which the new constraint was added. The low-level search is invoked for individual agents to determine an optimal path that is consistent with the individual constraints associated with the given node of the constraint tree (which also includes all constraints of the parent nodes up to the root node).
The constraint tree is searched for goal nodes using a focal search wherein the costs of the constraint tree node is determined. In the low-level path finding algorithm, the focal search is applied to find a consistent single path for an agent wherein the cost functions depend on a conflict heuristic.
Any optimal single agent path finding algorithm (low-level algorithm part) can be applied for the low-level algorithm part of the conflict-based search. In general, multi-agent path finding (MAPF) is the problem of finding a set of feasible paths for a set of agents with specific individual start and goal poses. The path is developed along vertices in a state space map. Possible path finding algorithms are A*, RRT*, BIT* and the like.
According to the above method, the conflict heuristic shall be based on conflicts with moving objects/individuals and their group affiliations. Therefore, based on agent sensors, individuals and/or groups of individuals are detected and current poses and velocities of objects/individuals and groups of objects/individuals moving within the environment in which the agents shall move are detected by a sensor system.
The path planning is made depending on interaction costs. While generally, time to goal is one important cost factor for motion planning, in an environment with dynamic objects further cost factors can be considered, which allows to reduce the disturbance of dynamic objects/individuals and/or groups of objects/individuals as low as possible.
So the interaction costs may include agent costs which depend on at least one of obstacle repulsive costs indicating costs for moving with respect to a static obstacle, interactive costs between the agent depending on a distance between the considered agent with each of the other agents, and the acceleration costs indicating the acceleration of the considered agent.
Moreover, a detection may be made for groups of at least two individual objects having a distance of less than a given group distance threshold, wherein the interaction costs may further include group costs which depend on at least one of group movement costs that measures how much an individual belonging to the group moves forwards to the center of the group, attraction costs indicating a measure of how the considered agent is attracted to the center of the group of objects and repulsive costs by which the considered agent repulses from overlapping with another agent.
Therefore, an individual-based conflict heuristic is proposed which is based on a group and a social force-field computation. The force field is a means to define an impact on the agent which increases or decreases the movement costs as the “energy consumption” to move against the force increases and the “energy consumption” to move with the force decreases.
The force field is based on the poses and velocities of the objects/individuals and the group of objects/individuals. To each of the object/individual I a force is computed as
with fi being the goal force, fwall being an obstacle repulsive force, fij being the interactive force between the considered agent and another agent, figroup being the force related to belonging to a group of objects/individuals and
being the acceleration of the considered agent.
The group force figroup can be expressed as
f
i
group
=f
i
vis
+f
i
att
+f
i
rep,
fivis corresponds to a force that measures how much an object/individual belonging to the group moves forward to the center of the group. This is based on the assumption that people belonging to the group stay as close as possible and interact with each other. fiatt corresponds to a force by which the considered agent is attracted to the center of the group of individuals. This is based on the assumption that people belonging to the group stay as close as possible and interact to each other. firep corresponds to a force by which the considered agent repulses from overlapping or a collision with another agent.
The force field is computed for each object/individual and each group of objects/individuals detected. Once the force field is computed, the force field is used as an additional cost term in an enhanced conflict-based search planning approach.
Adding the additional cost term in the heuristic function allows to consider the presence of individuals in the path planning, which allows the agents to move smoothly without conflicts around individuals and group of individuals.
Example embodiments of the present invention are described in more detail in conjunction with the figures.
The free space S may include obstacles as objects which can be stationary or dynamic. Dynamic obstacles may include an object/individual I or a group of objects/individuals G. Objects form a group of objects if the distance between each two of the objects within the group is lower than a given distance.
A central unit 5 is provided which is in communication with each of the agents 2.
Each agent 2 has a sensor system 21 and an actuation system 22. The sensor system 21 may be configured to detect the environment and may also be configured to detect their own pose, i.e. the position and the orientation, and velocities. Furthermore, the sensor system 21 may be configured to track the objects I and groups of objects G being or moving through the free space S. This information may be communicated to the central unit 5.
By means of the actuation system 22, the agent 2 may move along a motion path (trajectory) which is communicated by the central unit to each of the agents 2. Control of communication with the central unit 5 and effecting the movement according to instructions of a motion path is made by means of a control unit 23 of each agent 2. Such a sensor system can be based on radar and/or Lidar sensors that could use the input from other sensor modalities, such as monocular, stereovision or RGB-D sensors. The readings of these agent sensor systems 21 or global sensors are given as input to a person/group tracker which could be a single monolithic component or a separate module that communicates with the central unit.
In the central unit 5, a pose and a velocity of each agent 2 are obtained either by a global sensor system 52 overviewing the environment E and locating the poses and motion of the agents 2 in the environment. Alternatively, the poses and velocities of the agents 2 can be received from the agents 2 so that the central unit 5 has available knowledge about the poses of the agents 2 for each computational time step at which the motion paths of the agents 2 are updated.
Furthermore, the central unit 5 has available the goal positions of each of the agents 2.
By means of the agent sensors 21 or the global sensor system 52, poses and velocities of individuals I and a group of individuals G in the environment can be detected. The poses of the individuals and the groups of individuals can be determined by a tracking module 51 that obtains the sensor data of the agent sensor system 21 and/or of the global sensor system 52 so that the poses of the individuals are communicated to the central unit 5. In other embodiments, the central unit 5 can include the tracking module 51 or implement a tracking algorithm.
The central unit 5 performs a method to compute motion paths of each of the agents 2 according to the process as described in more detail in conjunction with the flowchart of
In step S1, the poses of the agents 2 are obtained. This can be performed by the global sensors or the agent sensors which are then communicated to the central unit 5 as described above.
In step S2, the sensor data of the sensor system 21 of the agents 2 and the global sensor system 52 are used to also obtain poses and velocities of individual objects I and/or groups of objects G which may be derived from at least one of the agent sensor system 21 or the global sensor system 52.
In step S3, a social force field is computed which is part of a cost function used for the conflict-based search as described below. The social force field is based on the positions and velocities of the individual objects I and the group of objects G and is calculated for each individual object detected. The force field is calculated as follows:
where fi is the goal force depending on the distance between the agent and the respective object/individual, fwall is an obstacle repulsive force depending on a distance to a stationary obstacle, fij the interactive force between two concurring agents which is depending on a distance particularly considered exponentially, figroup is a force related to belonging to a group of objects and
the acceleration of the agent Ai.
The group force figroup can be expressed as
f
i
group
=f
i
vis
+f
i
att
+f
i
rep
fivis corresponds to a force that measures how much an object/individual belonging to the group moves forwards to the center of the group. This is based on the assumption, that people belonging to the group stay as close as possible and interact to each other. fiatt corresponds to a force by which the considered agent is attracted to the center of the group G of individuals. This is based on the assumption, that people belonging to the group stay as close as possible and interact to each other. firep corresponds to a force by which the considered agent repulses from overlapping or a collision with another agent. The center of the group of objects is calculated based on the positions of the agents of the group.
The forces may correspond or may indicate costs for a cost function on which the motion path planning refers to.
Once the force field is computed, the force field is used as a cost term in an enhanced conflict-based search planning approach as performed in step S4.
In step S4, a multi-agent path planning algorithm is carried out.
One common multi-agent path planning algorithm is called conflict-based search CBS. However, for the present invention, any coupled path planning algorithm for multiple agents can be applied, which avoids conflicts and minimize overall agent movement costs.
Conflict-based search is a two-level algorithm wherein the high-level search is performed in a constraint tree (CT) whose nodes include constraints on time and location for a single agent. Such a constraint for a given agent Ai may e.g. be indicated as a tuple (Ai, v, t), defining that the agent Ai is prohibited from occupying vertex v at time step t. At each node in the constraint tree, a low-level search is performed to find paths for all of the agents under the constraints given by the high-level node. In the high-level algorithm part, the constraint tree is searched. Each node in the constraint tree contains a set of constraints imposed on each agent, a single consistent solution including one path for each agent that is consistent with the set of constraints given for the respective node, and the total cost of the current solution. The root of the constraint tree contains an empty set of constraints while a successor of a node in the constraint tree inherits the constraints of the current and adds a single new constraint for each of the agents.
The goal node is defined when the solution of the node is valid, i.e. the set of paths for all agents have no conflicts. A conflict may be defined as the tuple (Ai, Aj, v, t), the agent Ai and agent Aj occupy vertex v at time point t. A node is declared as a non-goal node if a conflict occurred.
Furthermore, a consistent solution can be invalid despite of the fact that the paths are consistent with their individual agent constraints so that these paths still have conflicts.
A main feature of the conflict-based search algorithm is to grow a set of constraints of each of the agents and find paths that are consistent with these constraints. If these paths still have conflicts and are thus invalid, the conflicts are resolved by adding new constraints. The high-level algorithm path (building the constraint tree) therefore basically is for finding and adding constraints.
Given a node of the constraint tree, the low-level search is invoked for individual agents to determine an optimal path that is consistent with the individual constraints associated with the given node of the constraint tree. Any optimal single agent path finding algorithm (low-level algorithm part) can be applied for the low-level algorithm part of the conflict-based search. Possible path finding algorithms are A*, RRT*, BIT* and the like. In general, multi-agent path finding (MAPF) is the problem of finding a set of feasible paths for a set of agents with specific individual start and goal poses. The path is developed along vertices in a state space map.
In general, the path finding algorithms consider costs of moving an agent along a considered motion path. Here, apart from considering the conventional time to goal costs or the movement costs of the specific agent, the costs defined by a social force field are additionally considered. The determination of this additional interaction costs is explained below.
Once consistent paths have been found by any suitable path finding algorithms for each agent, the found paths have to be checked with respect to the paths of each other agent by simulating the movement of the agents along their planned paths. If all agents reach their goal without any conflicts, e.g. no collision of two agents occurred, the node of the constraint tree is declared as the goal node and the solution and be returned. If, however, while performing the validation, a conflict is found for two or more agents, the validation is stopped and the node is declared as non-goal. Given a non-goal node of the constraint tree, whose solution includes a conflict, it is known that in any valid solution at most one of the conflicting agents may occupy the vertex v at time t. Therefore, at least one of the constraints for at least one of the agents must hold.
Consequently, the algorithm generates at least two new nodes for the constraint tree as children of the non-goal node each adding one of these constraints to the previous set of constraints. For each constraint tree node, it may be provided that the low-level search algorithm part is only activated for a single agent for which the new constraint was added.
The constraint tree is searched for goal nodes using a focal search wherein the costs of the constraint tree node are determined. In the low-level path finding algorithm, the focal search is applied to find a consistent single path for an agent wherein the cost functions depend on a conflict heuristic as described above.
For each agent we have a defined single goal to reach. Once all the goals (or some agents may fail to find it) have been reached, the building of the constraint tree is stopped.
According to the above method, the conflict heuristic shall be based on potential conflicts with moving objects/individuals and their group affiliations.
So, the conflict-based search algorithm makes use of a cost term depending on a force field as described above. The costs are considered over each of the objects/individuals in the environment. The interaction costs consider the sum of all interaction forces applied by each object/individual and/or each group of objects/individuals within the environment. The interaction forces have an impact on the movement costs as they act against or with the agent's motion paths.
In step S5, the agents 2 are controlled according to the determined motion path of the selected goal node.
The process is cyclically performed by a return to step S1.
In
The root node is added to the open node list which includes the tree nodes of the constraint tree to be built (line 4). For each of the entries in the open node list (line 5), it is performed the following steps:
The best node which has the lowest solution costs is selected from the open node list (line 6). The solutions of the motion paths for each of the agents is validated for the best node P to determine if a conflict occurs (line 7). If there is no conflict (line 8), the node P and its solutions are returned to indicate that the node P is a goal node.
If a conflict occurred, each agent which is involved in the conflict triggers the generation of a new node with a set of constraints (line 13) to which the determined conflict is added (line 12). A solution is calculated using the low-level algorithm (line 14), so the motion paths for each agent can be associated to the respective node. Furthermore, the costs for the determined solution is further associated to the respective node (line 16).
The newly generated nodes are added to the open node list OPEN (line 17).
This method is continued until no conflict can be determined for each of the nodes of the so-built constraint tree.
| Number | Date | Country | Kind |
|---|---|---|---|
| 20172025.7 | Apr 2020 | EP | regional |
