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. The software implementation is available online at https://github.com/CMangette/Fast-dRRT.
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  and  supporting this. With an increase in automation in warehouses by companies like Amazon , 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 .
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 . 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 . 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  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  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 .
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)  and its optimal variant dRRT*  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  and A*  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 .
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  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  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 . 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 .
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 . 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  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 , the latter using M* to query a multi-robot path.
Solovey et al. also use sub-dimensional expansion in discrete RRT (dRRT) . 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 . 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  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) .
Iv-a Local Roadmaps
Solovey et al. suggest using probabilistic roadmaps (PRMs) as approximations to local configuration spaces . 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 . 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  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  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 , 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 .
Additionally, in  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 .
Our local planner, kinematically-constrained PRM (KC-PRM), is similar to the Probabilistic Path Planner (PPP) in  with additional sampling and connection constraints to build a road map biased towards the optimal path that discriminates against unnecessary connections (Algorithm 1).
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 . 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:
the length of , the path connecting to , is within the connection radius.
is in front of . The planner in  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
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  and , 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.
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  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  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.
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  and :
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
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 3 – 5).
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.
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.
-  E. Ackerman, “Amazon uses 800 robots to run this warehouse,” Jun 2019. [Online]. Available: https://spectrum.ieee.org/automaton/robotics/industrial-robots/amazon-introduces-two-new-warehouse-robots
-  E. W. Dijkstra, “A note on two problems in connexion with graphs,” Numerische Mathematik, vol. 1, no. 1, pp. 269–271, Dec 1959. [Online]. Available: https://doi.org/10.1007/BF01386390
-  L. E. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents,” American Journal of Mathematics, vol. 79, no. 3, p. 497, jul 1957. [Online]. Available: https://doi.org/10.2307
-  M. Elbanhawi and M. Simic, “Sampling-based robot motion planning: A review,” IEEE Access, vol. 2, pp. 56–77, 2014.
-  J. h. Jeon, S. Karaman, and E. Frazzoli, “Anytime computation of time-optimal off-road vehicle maneuvers using the rrt*,” in 2011 50th IEEE Conference on Decision and Control and European Control Conference, Dec 2011, pp. 3276–3282.
-  P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,” IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, July 1968.
-  L. E. Kavraki, P. Svestka, J. . Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, Aug 1996.
-  J.-T. Li and H.-J. Liu, “Design optimization of amazon robotics,” 2016.
-  B. Liu and A. El Kamel, “V2x-based decentralized cooperative adaptive cruise control in the vicinity of intersections,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 3, pp. 644–658, March 2016.
-  B. Paden, M. Cáp, S. Z. Yong, D. S. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” CoRR, vol. abs/1604.07446, 2016. [Online]. Available: http://arxiv.org/abs/1604.07446
-  J. L. Rios, L. Martin, and J. Mercer, “Use of UAS Reports (UREPs) during TCL3 Field Testing,” National Aeronautics and Space Administration,, Tech. Rep., 07 2017.
-  G. Sanchez and J. . Latombe, “Using a prm planner to compare centralized and decoupled planning for multi-robot systems,” in Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292), vol. 2, May 2002, pp. 2112–2119 vol.2.
-  A. Scheuer and T. Fraichard, “Continuous-curvature path planning for car-like vehicles,” 10 1997, pp. 997 – 1003 vol.2.
-  R. Shome, K. Solovey, A. Dobson, D. Halperin, and K. E. Bekris, “drrt*: Scalable and informed asymptotically-optimal multi-robot motion planning,” CoRR, vol. abs/1903.00994, 2019. [Online]. Available: http://arxiv.org/abs/1903.00994
-  K. Solovey, O. Salzman, and D. Halperin, “Finding a needle in an exponential haystack: Discrete RRT for exploration of implicit roadmaps in multi-robot motion planning,” CoRR, vol. abs/1305.2889, 2013. [Online]. Available: http://arxiv.org/abs/1305.2889
-  P. Svestka and M. H. Overmars, “Motion planning for carlike robots using a probabilistic learning approach,” The International Journal of Robotics Research, vol. 16, no. 2, pp. 119–143, 1997. [Online]. Available: https://doi.org/10.1177/027836499701600201
-  J. van den Berg, J. Snoeyink, M. Lin, and D. Manocha, “Centralized path planning for multiple robots: Optimal decoupling into sequential plans,” 06 2009.
-  J. J. B. Vial, W. E. Devanny, D. Eppstein, and M. T. Goodrich, “Scheduling autonomous vehicle platoons through an unregulated intersection,” CoRR, vol. abs/1609.04512, 2016. [Online]. Available: http://arxiv.org/abs/1609.04512
-  G. Wagner and H. Choset, “M*: A complete multirobot path planning algorithm with performance bounds,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, Sep. 2011, pp. 3260–3267.
-  G. Wagner, Minsu Kang, and H. Choset, “Probabilistic path planning for multiple robots with subdimensional expansion,” in 2012 IEEE International Conference on Robotics and Automation, May 2012, pp. 2886–2892.
-  Z. Yan, N. Jouandeau, and A. Ali, “A survey and analysis of multi-robot coordination,” International Journal of Advanced Robotic Systems, vol. 10, p. 1, 12 2013.
-  L. Ye and T. Yamamoto, “Modeling connected and autonomous vehicles in heterogeneous traffic flow,” Physica A: Statistical Mechanics and its Applications, vol. 490, pp. 269 – 277, 2018. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0378437117307392
-  Yi Guo and L. E. Parker, “A distributed and optimal motion planning approach for multiple mobile robots,” in Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292), vol. 3, May 2002, pp. 2612–2619 vol.3.
-  J. Yu and S. M. LaValle, “Optimal multirobot path planning on graphs: Complete algorithms and effective heuristics,” IEEE Transactions on Robotics, vol. 32, no. 5, pp. 1163–1177, Oct 2016.
-  P. Švestka and M. H. Overmars, “Coordinated path planning for multiple robots,” Robotics and Autonomous Systems, vol. 23, no. 3, pp. 125 – 152, 1998. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S092188909700033X