Multi-Robot Coordinated Planning in Confined Environments under Kinematic Constraints

We investigate the problem of multi-robot coordinated planning in environments where the robots may have to operate in close proximity to each other. We seek computationally efficient planners that ensure safe paths and adherence to kinematic constraints. We extend the central planner dRRT* with our variant, fast-dRRT (fdRRT), with the intention being to use in tight environments that lead to a high degree of coupling between robots. Our algorithm is empirically shown to achieve the trade-off between computational time and solution quality, especially in tight environments.



There are no comments yet.


page 1


Path Planning with Kinematic Constraints for Robot Groups

Path planning for multiple robots is well studied in the AI and robotics...

Robot Safe Interaction System for Intelligent Industrial Co-Robots

Human-robot interactions have been recognized to be a key element of fut...

Prioritized SIPP for Multi-Agent Path Finding With Kinematic Constraints

Multi-Agent Path Finding (MAPF) is a long-standing problem in Robotics a...

Lifelong Path Planning with Kinematic Constraints for Multi-Agent Pickup and Delivery

The Multi-Agent Pickup and Delivery (MAPD) problem models applications w...

Planning Jerk-Optimized Trajectory with Discrete-Time Constraints for Redundant Robots

We present a method for effectively planning the motion trajectory of ro...

Evaluation of Skid-Steering Kinematic Models for Subarctic Environments

In subarctic and arctic areas, large and heavy skid-steered robots are p...

Augmenting GRIPS with Heuristic Sampling for Planning Feasible Trajectories of a Car-Like Robot

Kinodynamic motion planning for non-holomonic mobile robots is a challen...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Computationally efficient multi-robot motion planning algorithms are highly sought after for their applications in industry. In a time when automotive manufacturers are quickly approaching the advent of self-driving cars, centralized motion planners in lieu of traditional traffic control structures open the possibility of increased traffic flow in busy urban environments, with studies in [9] and [22] supporting this. With an increase in automation in warehouses by companies like Amazon [1], efficient path planning of robots designed to move inventory in place of human workers has become another important use case. Beyond ground vehicles, traffic management of Unmanned Aerial Vehicles (UAVs) is identified by NASA as an important area of research to ensure safe integration of aerial drones into the air space [11].
In each of the aforementioned applications, the algorithms used must be robust to planning in tight, confined environments while still ensuring that robots do not collide with one another. In the case of automated driving, urban traffic structures such as intersections and highway merging ramps constrain vehicles to a narrow set of paths. Similarly, warehouses limit robot paths due to shelving and storage units occupying the space. While not subject to high clutter, high volume air traffic can artificially restrict paths for UAVs.

The planning algorithms available for such problems can be classified as centralized or decoupled. Centralized algorithms plan in the joint space of all robots whereas decoupled approaches only consider the space for each individual robot

[12]. Decoupling interactions between robots that don’t directly interact can simplify the original planning problem into a number of single-robot motion planning problems, making decoupled planners faster than centralized planners. However, this can compromise completeness and allow inter-robot collisions [12]. Centralized planners, in comparison, can guarantee collision-free motions and completeness, but at the cost of solution time and scale-ability. If a decoupled planner considers a space of dimension for robots, then a centralized algorithm plans over a joint space .
For our targeted applications, safety is of the utmost importance, so a centralized algorithm is better suited than a decoupled algorithm. Furthermore, centralized frameworks already exists in each use case. The intersection manager in [18] is a hypothetical replacement to traffic lights that can control when autonomous vehicles enter an intersection via Vehicle-to-Infrastructure (V2I) communications. A task allocation and path planning system in [8] demonstrates how to automate warehouse stock movement with kiva robots. The Unmanned Aerial System (UAS) Traffic Management (UTM) in development uses a centralized service supplier to manage requests and conflicts between UAVs operating within the same space [11].

Fig. 1: Targeted use cases: UAV coordination (left), traffic intersections (center), and warehouse motion planning (right).

The main challenge in centralized planning is doing so in a time-efficient manner. A secondary challenge is extending planning to robots with kinematic constraints, which complicates local path construction. This paper attempts solving both of these concerns by designing a framework for centralized planning in spaces with tight corridors with multiple kinematically constrained robots.
State-of-the-art planners have progressed towards algorithms that increase efficiency while preserving completeness. Recognizing the shortcomings of previous algorithms that rely on explicit computation of the composite planning space, discrete RRT (dRRT) [15] and its optimal variant dRRT* [14] improve computational efficiency by offloading computations to offline tasks when possible and relying on implicit representations on the planning space. These algorithms do not encode steering constraints, but provide a general framework for fast multi-robot planning.
This paper presents a variant to dRRT / dRRT* , which we call fast-dRRT (fdRRT), that returns fast but sub-optimal trajectories in tight environments requiring significant coordination between robots. We also extend these planners to account for robots with kinematic constraints. dRRT* and our algorithm are tested across multiple environments (Figure 1) and demonstrate fdRRT’s increased computational efficiency in confined spaces.

Ii Related Work

Motion planning has been studied as one of the fundamental problems in robotics. In the standard planning framework, a robot within a work space begins with a starting point and goal point , and the solution to the planning problem is to find a collision-free path connecting and . Grid-based methods such as Djikstra’s algorithm [2] and A* [6] were developed as a means of finding shortest paths between vertices on a graph. Sampling-based motion planners became popular for their adaptability to different kinematic models and low cost by sampling points instead of searching exhaustively over the work space. A detailed review of sampled-based planning is provided in [4].
Extending motion planning to the multi-robot domain has been challenging due an increase in search space size and has led to a variety of approaches. Strategies are categorized in [21] to use cell decomposition, potential field navigation, roadmaps to plan efficient paths. Cell decomposition methods to path planning rely on discrete maps of the planning space to determine optimal paths. A sequential process in [23] splits the problem into local path planning using D* and coordination between robots to avoid entering collision regions simultaneously. Instead of handling spatial and velocity planning separately, Wagner and Choset developed M* , a multi-robot analogue to A* that resolves local path collisions by coupling paths only when they are found to overlap [19]. Although M* can plan paths for up to 100 robots, its performance suffers when high degrees of coupling between robots arise at choke points in the planning space. Yu and Lavalle optimize paths on a graph across various objectives and demonstrate the scaleability of their algorithm, but do not consider kinematic contraints in their models [24].
Roadmap strategies, in contrast, iteratively explore the work space instead of searching exhaustively. Van den Berg et al. provide a general framework for planning in a roadmap a sequential path planner that determines a sequential ordering for each robot to execute its path [17]. It relies on a coupled motion planner for handling local connections between conflicting agents, so run time performance is dependent on the degree of coupling between robots. The coordinated path planner in [25] searches collision-free paths over an explicitly computed multi-robot work space, but is limited in scope due to the memory required to build an explicitly defined road map. Using the principle of sub-dimensional expansion, Wagner et al. designed sub-dimensional RRT (sRRT) and sub-dimensional PRM (sPRM) to plan paths for multiple robots with integrator dynamics [20], the latter using M* to query a multi-robot path.
Solovey et al. also use sub-dimensional expansion in discrete RRT (dRRT) [15]. The idea of dRRT is to build road maps of collision-free motions for each robot, and then use them to build a search tree implicitely embedded in . dRRT draws samples from each road map and combines them into a composite sample, , to which is extended towards by selecting a composite neighboring vertex . Because relies on pre-computed motions between configurations that have already been collision checked against environmental obstacles, dRRT can simply fetch the motions and check if any inter-robot collisions occur, thus relieving the algorithm of significant computational burden. Collision-free composite motions are added as vertices to until a goal is reached.
The optimal variant of dRRT, dRRT*, improves upon computation time further by carefully choosing neighbors to expand towards the goal state [14]. In addition to

, a path heuristic,

, is computed to identify configurations with short paths to . This improves both solution quality and computational efficiency, making dRRT* the one of the state-of-the-art algorithms in multi-robot planning.
This paper presents a centralized planning strategy for kinematically constrained robots in tight environments. We demonstrate the feasibility of our kinematically constrained PRM algorithm in extending the pre-existing dRRT algorithm to the domain of planning under motion constraints. The central planning algorithm, which we call fast-dRRT (fdRRT), is designed to switch between randomly exploring the state space and driving greedily towards the goal state in a manner similar to dRRT*. The difference in our algorithm is how expansion failures due to collisions are adjudicated. Instead of reporting an expansion failure if no collision-free connection can be established to a new node, fdRRT forces a connection by commanding some robots to stay in their previous configurations while permitting others to move forward. In practice, this makes fdRRT faster than dRRT* in tight work spaces, but at the cost of solution quality. Unlike dRRT*, our algorithm makes no guarantee of minimal path length, thus imposing an trade-off between solution efficiency and quality when choosing between the two algorithms. Additionally, fdRRT’s incorporation of kinematic constraints makes it a more flexible planner that can be used in different systems.

Iii Problem Formulation

The input to our problem is the set of start and goal positions for robots. The two goals are to construct a local map for each robot encompassing feasible paths connecting a robot’s local start and goal position, and to use these maps to construct trajectories for each robot that respect kinematic constraints and do not intersect other trajectories.
Formally, given a set of initial configurations, and final configurations, , we would like to find a set of trajectories , such that all trajectories in are non-intersecting with obstacles and other robots. Time is not explicitly is part of the configuration space, but we assume that each instance of a configuration is uniformly discretized. Each robot is kinematically constrained by the motion model


For simplicity, we assume that each vehicle can only move forward. The dynamics in (1) are an extension of Dubins’ steering constraints [3] that add curvature constraints. The sum of path lengths of the multi-robot trajectory is the cost metric chosen for evaluation.

Iv Algorithm Overview

Our system is illustrated in Figure 2. A local roadmap is constructed for each robot by the local planner that runs offline. The local roadmap is defined as a directed graph containing configurations within the robot’s local configuration space and paths connecting configurations.
The central planner receives path queries in the form of initial and final configurations and local roadmaps from the robots entering the planning space. To avoid re-planning due to new requests, the central planner accepts requests until a deadline and relegate new requests to the next planning cycle. Given the local roadmaps and initial and final configurations of each robot, the central planner returns composite path that guarantees collision free trajectories between robots. Each local trajectory is sent to its corresponding robot as a list of time-parameterized waypoints and connecting paths .
The local controller on each robot determines the speed profile to follow from and the distance travelled between consecutive waypoints. is re-parameterized to from the distance traveled over time, which can be tracked by a local controller using a technique such as pure-pursuit or nonlinear Model Predictive Control (MPC) [10].

Fig. 2: The central planner returns collision-free path queries by referencing pre-computed roadmaps from a local planner.

Iv-a Local Roadmaps

Solovey et al. suggest using probabilistic roadmaps (PRMs) as approximations to local configuration spaces [15]. The original PRM algorithm builds a roadmap as an undirected graph , with each vertex being a unique configuration and each edge a path in free space connecting two adjacent vertices and [7]. Configurations are randomly sampled in the configuration space and connected to any vertices in within a connection distance , . Construction of continues for iterations, after which paths between configurations are found during a query.
This framework presents numerous challenges to adapting to a robot with kinematic constraints. Connections in [7] are line segments, which are sufficient under the assumption of single-integrator dynamics, but not for the dynamics in Equation (1). Numerical methods used in [5] capture both kinematic and dynamic constraints to connect two configurations in a kinematically-constrained system, but are approximate solutions. Dubins’ paths adhere to kinematic constraints and yield minimal path length for car-like robots [3], but require sharp changes in steering curvature that are not achievable in a real system. Scheuer and Fraichard extend Dubins’ paths to continuous curvature paths using clothoids to transition between changes in curvature that, while less computationally tractable than Dubins’ paths, are a feasible connection method [13].
Additionally, in [7] is an undirected graph, implying that motions between connected vertices are bi-directional. Due to Dubins’ steering constraints and the non-holonomic constraints in (1), this is not necessarily true, and the existence of a collision-free path connecting two vertices to does not guarantee the reverse. To address this, Svestka and Overmars demonstrate that making a directed graph is sufficient to impose this restriction [16].
Our local planner, kinematically-constrained PRM (KC-PRM), is similar to the Probabilistic Path Planner (PPP) in [16] with additional sampling and connection constraints to build a road map biased towards the optimal path that discriminates against unnecessary connections (Algorithm 1).

2 ;
3 while   do
4       ;
5       for   do
6             ;
7             if  then
8                   if  then
9                         ;
11                   end if
12                  ;
14             end if
16       end for
18 end while
20 ;
return G , H
Algorithm 1 LocalPlanner()

is initialized with an initial configuration (Line 1). A base path is computed as the ideal path to follow from to and is used when sampling configurations (Line 2). expands to size by sampling random configurations , attempting connections to vertices (Lines 6 – 7), and adding connections to when attempts are successful (Lines 8 – 11). Details are provide below.
RandomConfig: Random configurations are uniformly sampled along the sample path, with additive Gaussian noise, to allow for variation in . The motivation behind sampling along instead of the entire space is that one of the primary use cases is autonomous driving in urban environments. The space of locations that an autonomous vehicle can sample without violating traffic norms such as staying within one’s own lane is confined to the center of the lane with some allowable deviation, so is restricted appropriately.
Steer: Connections between adjacent vertices are attempted using the procedure described in [13]. Although more time consuming to compute than a Dubins’ curve, this is an offline procedure, so efficiency is not a concern.
IsReachable: A configuration is defined to be reachable from if two conditions are met:

  1. the length of , the path connecting to , is within the connection radius.

  2. is in front of . The planner in [5] imposes a similar condition by checking if is in the half space of

    . We check this condition by computing the normalized distance vector between

    and , , the tangent vector at , , and check if the angle between these two vectors is less than 90 degrees.

The purpose of this check is to only allow movements that would be feasible in traffic. Vehicle motions must move forward along the road in the direction of traffic, but this constraint isn’t encoded into Steer. Thus, the reachability check enforces this behavior.
CostToGoal:The cost to go from each vertex in to is stored in to be used as a heuristic in the central planner. Our implementation uses a breadth-first search.
PruneDeadNodes: Due to being a directed graph and the reachability constraints, some sampled nodes will not have a path to . These ”dead” nodes in are removed to avoid running into dead ends in the central planning stage.

Iv-B Central Planner

2 ;
3 while  do
4       ;
5       if  then
6             ;
7             return
8       end if
10 end while
Algorithm 2 fdRRT()

The algorithm structure from dRRT* (Algorithm 1) is preserved with the initialization of with (Line 1). The algorithm then expands, while keeping track of the most recent expansion node to determine how it expands in the next iteration (Line 4). FindPath queries for a path to and returns a composite path if successful (Lines 5 – 6). A notable difference in fdRRT is the omission of a local connector present in [15] and [14], whose purpose is to solve the multi-robot coordination problem when sufficiently close to . We found this to be unnecessary in practice due to the structure of our environments. In the case of a traffic intersection, once all vehicles have passed through the physical intersection of the two roads, tends to expand greedily towards . A similar subroutine is utilized in resolving path conflict by forcing some robots to hold their positions while others move forward.

1if   then
2       ;
3       ;
5 else
6       ;
7       ;
9 end if
11 ;
12 ;
13 if  then
14       ;
15       if  then
16             return
17       else
18             ;
19             return ;
21       end if
23 else
24       ;
25       return ;
27 end if
Algorithm 3 Expand()

Expand: Expansion of begins with selecting a node to expand from. If a vertex was added during the previous call, then a new expansion vertex is chosen by selecting a neighbor of (Lines 2–3). Otherwise, the closest neighbor of a random configuration is chosen (Lines 5 – 6). The direction oracle subroutine selects an expansion node based on the success of the previous expansion (Line 8). If , is chosen as the tuple of individual vertices that are neighbors to and have the lowest path cost to , and is otherwise chosen as a tuple of randomly selected neighbors to . We refer to [14] for a detailed explanation.
All composite parents to that have already been added to are expansion candidates to connect to (Line 9). Each candidate is evaluated base on whether the composite path between and results in a collision-free motion and the composite path cost. Our algorithm differs from [14] when choosing the parent node to , . dRRT* chooses as the lowest cost vertex that is also a collision-free motion. In our algorithm, the lowest cost collision-free node, , and the lowest cost node are selected. If no such exists, the subroutine ForceConnect attempts forcing to expand by creating a new hybrid node, , that restricts some individual nodes to hold their position at , and allows others to move forward towards . While forcing some vehicles to stop increases traffic delays for individual vehicles, ForceConnect increases computational efficiency in practice by restricting random sampling to a last resort.

2 ;
3 ;
4 for  do
5       for  do
6             ;
8       end for
10 end for
12 for  do
13       if  then
14             ;
16       else if  then
17             if  then
18                   ;
20             end if
22       end if
24 end for
26 return ;
Algorithm 4

ForceConnect: When forcing a connection between two composite nodes and , the robot either holds its position at or moves forward towards . Three sets are initialized for each robot : , the set of robots with higher local priority than , , the set of robots with lower priority than , and , the set of robots that conflict with but have no local priority assigned. Each interaction is checked and are populated by LocalPriority. The local priority of with respect to is assigned according to the rules, which originate from the local connector logic in [15] and [17]:

  • If blocks , then robot is given priority

  • If blocks the path of , then robot is given priority

  • If and do not overlap, then there is no interaction and a priority is not assigned

  • Otherwise, the local priority can not be determined. This occurs when and overlap, but the starting positions of robots and do not block each other’s paths. Either robot can be given priority, but the decision is deferred.

A solution set is then initialized to pick robots that should move forward (Line 9). Each robot is added to or rejected from based on its own sets. For a robot , if no other robots have a higher local priority and no robots have an undetermined priority, then is added to . if any vehicles have a higher priority, then is rejected from . If no robots have a higher priority, but some have undetermined priorities, then the cost of adding is assessed. In this context, the cost refers to number of vehicles that would be excluded from if was added to . The cost of adding is compared to the cost of adding any of and will be added to if the trade-off from adding is lower than the trade-off from adding any other member of . After all robots are either added to or rejected from , a hybrid node is constructed (Line 19).

V Simulations and Results

Fig. 3: Performance comparison in traffic intersection.
Fig. 4: Performance comparison in a warehouse space.
Fig. 5: Performance comparison in UAV environment.

Our algorithm was implemented and tested in MATLAB. Three environments are considered for validation: a three-lane traffic intersection, a cluttered rectangular space akin to a warehouse, and a crowded space of UAVs (Figure 1). We assume each vehicle in the first environment is rectangular with length and , while the robots in the second and third environments disks with radius

, respectively. 1000 test cases were run for each combination of vehicles in each environment. Average search tree size, solution time, and path lengths are evaluation metrics to illustrate the trade-offs between dRRT* and fdRRT (Figures

From the test results, fdRRT performs better than dRRT* in computation efficiency in the intersection and warehouse spaces. In test cases with maximum traffic, fdRRT returned solutions 57% faster in the traffic intersection and around 2000% faster in the warehouse. However, dRRT* is 12% faster in the UAV environment. This may be due to the lack of clutter within the UAV space, and thus reduced number of choke points. Under these conditions, the added computational time in fdRRT when forcing connections may degrade performance.
Solution quality metrics show the opposite trend. As more robots are added to each environment, the path quality in fdRRT degrades, with paths being 22% and 54% longer in the intersection and warehouse spaces, respectively. Paths in the UAV space are nearly identical with a 0.2% discrepancy.
The trends in solution times and path lengths across the scenarios can be attributed the amount of clutter in each space. The warehouse space has more obstacles distributed across its environment, and thus more possible choke points and corridors, the traffic intersection funnels all vehicles into a single, albeit large, choke point. The UAV space, in contrast, has no obstacles and thus allows the most movement. We conclude that there’s a trade-off between the two algorithms; fdRRT will generally return trajectories faster, but dRRT* will have lower cost solutions.

Vi Conclusion

We have developed a PRM planner for car-like robots to create trajectories that adhere to traffic standards, making it well suited for motion planning on roadway environments. That planning strategy was used in a central planning algorithm based on the previously published dRRT / dRRT* algorithm. Our implementation has demonstrated its advantage in computational time over dRRT* when planning in confined environments, at the cost of solution quality.
The results from this study are promising, but several challenges remain. Testing the feasibility of fdRRT in a real system is one goal we would like to reach. We also plan to explore extending the planner to incorporate vehicle dynamics in addition to vehicle kinematics. In its current form, we only consider sampling configurations and ignore constraints on vehicle speed and acceleration. Adding constraints on vehicle dynamics makes connecting between configurations more difficult, but carries the benefit of ensuring that all paths are feasible for robots with both kinematic and dynamic constraints.