Multi-Robot Motion Planning with Dynamics Guided by Multi-Agent Search

1 Introduction

Robotic teams provide a viable venue to enhance automation, increase productivity, and reduce operational costs in transportation, exploration, inspection, surveillance, search-andrescue, and many other applications. In these settings, robots are often required to move to different locations while avoiding collisions with obstacles and other robots.

Multi-robot motion planning is challenging since it is no longer sufficient to plan the motions of the robots independently as the plans of one robot can interfere with the plans of another robot or even prevent it from reaching the goal. The planned motions should also obey the underlying dynamics of each robot, which impose differential constraints on the velocity, acceleration, direction, and turning radius. This is essential to ensure that the planned motions can be executed by the robots in the physical world.

Related work has considered various settings for the multirobot motion-planning problem. In a discrete setting, i.e., multi-agent pathfinding, each robot is a point with no dynamics that in one step can move to an adjacent vertex in a graph abstraction of the world. To solve this NP-hard problem [Yu and LaValle, 2013], decoupled approaches often plan the paths for each agent one at a time, ensuring that agent i avoids conflicts with the plans of agents 1, . . . , i - 1 [Silver, 2005; Sturtevant and Buro, 2006; Barer et al., 2014]. Decoupled approaches are fast but suboptimal. To provide optimality, centralized approaches based on A* [Standley and Korf, 2011; Goldenberg et al., 2014; Svancara and Surynek, 2017], M* [Wagner and Choset, 2015], increasing-cost tree search [Sharon et al., 2013], and conflict-based search [Sharon et al., 2015] operate over the composite space of all the agents [Felner et al., 2017]. Solvers have also used auctions [Amir et al., 2015], answer-set programming [Erdem et al., 2013], or rules [Khorshid et al., 2011; Botea and Surynek, 2015].

∗This paper is an abridged version of the paper "Cooperative Multi-Robot Sampling-Based Motion Planning with Dynamics" that won the Best Robotics Paper award at ICAPS, 2017.

In a continuous setting, sampling-based motion planning has often been used. To account for dynamics, RRT [LaValle and Kuffner, 2001], KPIECE [S¸ucan and Kavraki, 2012], GUST [Plaku, 2015], and others expand a motion tree by adding collision-free and dynamically-feasible trajectories as branches. For multi-robot problems, sampling-based motion planners can be used in a decoupled or centralized framework. Decoupled approaches plan the motions one robot at a time, treating robots 1, . . . , i - 1 as moving obstacles when planning the motions of robot i. Decoupled approaches work well in open environments but have difficulty finding solutions in cluttered environments where coordination among robots becomes necessary since the motions planned for robots 1, . . . , i-1 could make it impossible for robot i to reach its goal. Centralized approaches treat the robots as one system and operate over the resulting composite state space. They are probabilistically complete, but do not scale well due to the high dimensionality of the composite state space.

To develop an effective multi-robot motion planner, we present a framework that couples the ability of samplingbased motion planning to handle the complexity arising from geometric and differential constraints imposed by the obstacles and underlying robot dynamics with the ability of multiagent pathfinding to find non-conflicting routes over discrete abstractions. The underlying idea is to use multi-agent search as a heuristic to guide sampling-based motion planning as it expands a motion tree in the composite state space of all the robots. The expansion in the composite state space allows the approach to obtain probabilistic completeness, which ensures that a solution will be found, when it exists, with probability approaching one. Scalability and efficiency are obtained by using multi-agent search as a heuristic to guide the motiontree expansion. As with any heuristic, a critical aspect is the design of an appropriate relaxed problem setting. We obtain the discrete abstraction by ignoring the vehicle dynamics and using probabilistic roadmaps to create a network of roads that captures the connectivity of the environment, making it easy to reach each goal from any location.

Multi-agent search and sampling-based motion planning work in tandem. At each iteration of a core loop, multiagent search is first invoked to find non-conflicting routes over the discrete abstraction. At a second step, samplingbased motion planning seeks to expand the motion tree along the routes. When little or no progress is made, the routes are penalized and the multi-agent search is invoked again to find alternative routes. This synergistic combination makes it possible to effectively plan collision-free and dynamicallyfeasible motions that enable each robot to reach its goal. Experiments using vehicle models with nonlinear dynamics operating in unstructured, obstacle-rich, environments demonstrate the scalability and efficiency of the approach, yielding significant speedups over related work.

2 ProblemFormulation

Each robot model Mi = 〈Pi, Si, Ai, fi〉 is defined by its geometric shape Pi, state space Si, action space Ai, and underlying dynamics fi expressed as a set of differential equations

As an example, a vehicle model defines the state in terms of the position (x, y), orientation θ, steering angle ψ, and velocity v. The vehicle is controlled by setting the acceleration acc and steering rate ω. The motion equations are defined as

where L is the distance from the back to the front wheels.

The new state snew ∈ S obtained by applying a control action a ∈ Ai to a state s ∈ Si is computed by a function

which numerically integrates fi for one time step dt. Starting from s ∈ S and applying actions 〈a1, . . . , aℓ-1〉 in succession gives rise to a dynamically-feasible trajectory ζ : {1, . . . , ℓ} → Si, where ζ(1) ← s and ∀j ∈ {2, . . . , ℓ}:

The multi-robot motion-planning problem is defined by

The world W contains obstacles O = {O1, . . . ,Om} and goals G = {G1, . . . , Gn}. The objective is to compute trajectories ζ1, . . . , ζn, one for each robot, so that ζi : {1, . . . , ℓ} → Si starts at the initial state sinit i ∈ Si, reaches the goal Gi, and avoids collisions with the obstacles and the other robots.

3 Method

The approach has several components: (i) a discrete abstraction based on a relaxed problem setting; (ii) multi-agent search over the discrete abstraction to guide sampling-based motion planning; (iii) expansion of a motion tree in the composite state space of all the robots; (iv) using the discrete abstraction to partition the motion tree into equivalence classes; and (v) interplay of sampling-based motion planning and multi-agent search to effectively expand the motion tree along routes obtained by the multi-agent search. A schematic representation is shown in Fig. 1.

3.1 Discrete Abstraction via Roadmaps

The discrete abstraction provides a relaxed problem representation, obtained by ignoring the robot dynamics. Specifically, the robot state is reduced to just the position and orientation (referred to as configuration) and each robot is allowed to translate and rotate in any direction. To solve the multi-robot motion-planning problem in this relaxed setting, a roadmap Ri = (VRi ,ERi ) is constructed for each robot Mi, seeking to make it easy to reach each goal from any location in the environment. The roadmap is initially populated by adding collision-free configurations from each goal G1, . . . , Gn. Leveraging PRM [Kavraki et al., 1996], the roadmap is further populated by sampling collision-free configurations and connecting neighboring configurations with collision-free paths whenever possible, as shown in Fig. 2. Dense roadmaps are preferred in order to provide alternative routes along which the robots can move to reach their goals. Robots that have the same shape can use the same roadmap.

3.2 Multi-Agent Search over the Discrete Abstraction

MULTIAGENTSEARCH(R1, . . . ,Rn, c1, . . . , cn, G1, . . . , Gn) searches over the roadmaps R1, . . . ,Rn to compute nonconflicting routes σ1, . . . , σn for each robot such that σi is over Ri, starts at ci, and ends at a roadmap vertex associated with the goal Gi. Since in multi-agent search, each robot moves from one vertex to the next in one step, each roadmap edge is divided into several parts to ensure that the distance from one vertex to the next is no more than a user-specified step size. The edge division is done only once after each roadmap is constructed. Note that the roadmap construction guarantees that robots will not collide with the obstacles as they move from one roadmap vertex to the next. Thus, the multi-agent search has to avoid only robot-robot conflicts. The overall approach is agnostic to the inner workings of the multi-agent search, so it can be used in conjunction with any available method that operates over graphs.

3.3 Motion Tree in the Composite State Space

Note that solutions over the relaxed problem setting do not necessarily correspond to dynamically-feasible trajectories since the discrete abstraction ignores the robot dynamics. As a result, geometric and differential constraints imposed by the obstacles and the underlying dynamics may make it difficult or impossible for a robot to follow its route σi.

To account for the dynamics, a motion tree T = (VT , ET ) is rooted at the initial state 〈sinit 1 , . . . , sinit n 〉 and is incrementally expanded in the composite state space S1 × . . . × Sn. Thus, each vertex v ∈ VT corresponds to a composite state, denoted by STATE(v), where STATEi(v) represents the state associated with the i-th robot. By construction, v is added to VT only if STATE(v) is collision-free, i.e., when the robots are placed according to STATE(v), there is no robotrobot or robot-obstacle collision. To generate a successor v′ from v, some control actions 〈a1, . . . , an〉 are applied component-wise to STATE(v) and the motion equations of each robot are integrated for one time step dt, i.e., ∀i ∈ {1, . . . , n} : STATEi(v′ ) ← SIMULATE(STATEi(v), ai, fi, dt). When STATE(v′ ) is not in collision, v′ and the edge (v, v′ ) are added to the motion tree T .

The motion tree T is expanded until a vertex vnew is added where each robot has reached its goal, i.e., ∀i ∈ {1, . . . , n} : STATEi(vnew) ∈ Gi. The path 〈v1, . . . , vℓ〉, with v1 = vinit and vℓ = vnew, from the root of T to vnew is retrieved and the solution trajectory for each robot i is constructed as ζi = 〈STATEi(v1), . . . , STATEi(vℓ)〉.

3.4 Partitioning the Motion Tree into Equivalence Classes Based on the Discrete Abstraction

The motion tree T could be expanded in an uninformed way by selecting at each iteration a vertex v ∈ VT at random and applying controls to generate a successor v′ from v. Such an expansion, however, would have difficulty finding solutions.

In contrast, we introduce multi-agent search to guide the motion-tree expansion. Since multi-agent search operates over the discrete abstraction, a mapping from the composite state space to the discrete abstraction must be first defined. As the discrete abstraction is obtained via roadmaps, a robot state si ∈ Si is mapped to the nearest configuration in the roadmap Ri. In this way, a composite state 〈s1, . . . , sn〉 ∈ S1 × . . . × Sn is mapped component-wise to a composite configuration 〈c1, . . . , cn〉, i.e.,

where ci = NEAREST(Ri, si).

The approach leverages this mapping to partition the motion tree T into equivalence classes. The premise is that vertices in T that provide the same discrete information should belong to the same equivalence class. In other words, vertices v, v′ ∈ T belong to the same equivalence class Γ〈c1,...,cn〉 if and only if MAP(STATE(v)) = MAP(STATE(v′ )) = 〈c1, . . . , cn〉. So, an equivalence class Γ〈c1,...,cn〉 contains all the vertices in T that map to 〈c1, . . . , cn〉.

The significance of an equivalence class Γ〈c1,...,cn〉 is that it makes it possible to leverage multi-agent search to compute non-conflicting routes σ1, . . . , σn, where each σi starts at ci and reaches the goal Gi. These routes then serve to guide the motion planner as it seeks to expand the motion tree from vertices associated with the equivalance class Γ〈c1,...,cn〉 along σ1, . . . , σn.

3.5 Putting it all Together: UsingMulti-Agent Search to Guide the Motion-Tree Expansion

The overall approach starts by creating the discrete abstraction, rooting the motion tree T at the initial state, and creating the first equivalence class ΓSTATE(vinit), which contains the root vertex vinit. Afterwards, the approach enters a core loop, where each iteration consists of the following:

• select an equivalence class Γ〈c1,...,cn〉 from Γ;
• use multi-agent search over the discrete abstraction to obtain non-conflicting routes σ1, . . . , σn that reach the goals starting from c1, . . . , cn; and
• expand the motion tree T from vertices in Γ〈c1,...,cn〉 along the routes σ1, . . . , σn.

MULTIAGENTSEARCH(R1, . . . ,Rn, c1, . . . , cn, G1, . . . , Gn) searches the discrete abstraction to obtain non-conflicting routes 〈σ1, . . . , σn〉, where σi starts at ci and reaches the goal Gi. To save computation time, the routes are stored with Γ〈c1,...,cn〉 and retrieved when the multi-agent search is invoked again for Γ〈c1,...,cn〉. During the selection of an equivalence class, priority is given to equivalence classes associated with short routes to promote rapid expansions toward the goals. The expansion adds new vertices to T , which could result in creating new equivalence classes. When the expansion from Γ〈c1,...,cn〉 fails or has difficulty following the routes σ1, . . . , σn, which could happen due to the geometric and differential constraints imposed by the obstacles and the underlying robot dynamics, the approach penalizes Γ〈c1,...,cn〉 in order to promote expansions from other equivalence classes. This process of selecting and expanding from an equivalence class is repeated until a solution is found or a runtime limit is reached.

Selecting an Equivalence Class

A weight is defined for each equivalence class and the equivalence class with the maximum weight is selected for expansion. Specifically, the weight for Γ〈c1,...,cn〉 is defined as

where σ1, . . . , σn denote the routes associated with Γ〈c1,...,cn〉, 0 < α < 1, and NRSEL(Γ〈c1,...,cn〉) denotes the number of times Γ〈c1,...,cn〉 has been previously selected.

In this way, to promote expansions towards the goals, preference is given to equivalence classes associated with short routes. This constitutes the greedy aspect of the selection process. To mitigate the potential pitfalls of a greedy selection, a penalty factor α (0 < α < 1) is introduced, which reduces the weight of an equivalence class each time it is selected. In fact, repeatedly selecting Γ〈c1,...,cn〉 will continue to reduce its weight so that eventually some other equivalence class will end up having a greater weight and thus be selected for expansion. This is essential to ensure probabilistic completeness and avoid becoming stuck when the motion-tree expansion from Γ〈c1,...,cn〉 repeatedly fails due to the constraints imposed by the obstacles and the underlying robot dynamics.

Expanding the Motion Tree from the Equivalence Class

After selecting an equivalence class Γ〈c1,...,cn〉, the objective becomes to expand the motion tree T from a vertex v ∈ Γ〈c1,...,cn〉 along the routes σ1, . . . , σn associated with Γ〈c1,...,cn〉. The vertex v could be selected at random to promote expansions from different vertices. Attempts are then made to expand T from STATE(v) so that each robot i moves from STATEi(v) toward the next configuration in σi. We can use proportional-integrative-derivative (PID) controllers [Spong et al., 2005] to select controls that would steer each robot toward its target, e.g., by turning the wheels and then moving toward the target. The expansion from v continues for several steps until the target is reached or a robot collides with an obstacle or another robot. If a new state is in collision, the trajectory generation stops and the approach goes back to selecting an equivalence class. Otherwise, the new state is added to tree T and the partition Γ is updated accordingly. If in the new state each robot has reached its goal, then a solution is found and the search terminates successfully.

4 Experiments and Results

Experiments are conducted using vehicle models with nonlinear dynamics operating in complex environments, as shown in Fig. 2 and 3. In many of these scenarios, the robots are required to cooperate in order to reach their goals.

The approach is compared to a centralized and a prioritized version of RRT [LaValle and Kuffner, 2001], denoted by cRRT and pRRT, as RRT is one of the most popular sampling-based motion planners. The approach is also compared to a prioritized version of GUST [Plaku, 2015], denoted by pGUST, as GUST has been shown to be significantly faster than RRT and other sampling-based motion planners.

Results in Fig. 4 show significant speedups over cRRT, pRRT, and pGUST. As RRT-based planners lack guidance, they have difficulty finding solutions. pGUST is faster than cRRT and pRRT since it uses discrete search to guide the motion-tree expansion. In open scenes (scene 2), where cooperation among the robots is not essential, pGUST is even faster than our approach since the robot trajectories do not interfere with each other. In scenes where cooperation is important, pGUST, as any prioritized approach, has difficulty finding solutions, even timing out in scenes 4, 5, 6. In contrast, due to the guidance by multi-agent search, our approach is able to find solutions in a matter of 1-3s.

5 Discussion

This paper presented an effective multi-robot motion planner that enables each robot to reach its desired location while avoiding collisions. The approach also took into account the dynamics of each robot and generated dynamically-feasible trajectories that can be executed in the physical world.

The crux of the approach was coupling the ability of sampling-based motion planning to handle the complexity arising from obstacles and robot dynamics with the ability of multi-agent pathfinding to find non-conflicting routes over discrete abstractions. The underlying idea was to use multiagent search over the discrete abstraction as a heuristic to guide sampling-based motion planning as it expands a motion tree in the composite state space of all the robots.

This work opens up several research directions. Advances in multi-agent search can significantly improve the efficiency and scalability of the framework by reducing the runtime to compute the routes. Moreover, the quality of the routes can also be improved, for example, by requiring multi-agent search to compute non-conflicting routes that maximize the separation among the robots and the clearance from the obstacles. Such routes would be easier to follow, which would reduce the runtime for the motion-tree expansion. Another direction is to enable the team of robots to perform complex tasks given by PDDL or some other high-level formalism.

Acknowledgments

This work is supported by NSF IIS1548406.