MultiRobot Coordinated Planning in Confined Environments
under Kinematic Constraints
Abstract
We investigate the problem of multirobot 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, fastdRRT (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 tradeoff between computational time and solution quality, especially in tight environments. The software implementation is available online at https://github.com/CMangette/FastdRRT.
I Introduction
Computationally efficient multirobot motion planning algorithms are highly sought after for their applications in industry. In a time when automotive manufacturers are quickly approaching the advent of selfdriving 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 singlerobot motion planning problems, making decoupled planners faster than centralized planners. However, this can compromise completeness and allow interrobot collisions [12]. Centralized planners, in comparison, can guarantee collisionfree motions and completeness, but at the cost of solution time and scaleability. 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 VehicletoInfrastructure (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].
The main challenge in centralized planning is doing so in a timeefficient 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.
Stateoftheart 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 multirobot planning.
This paper presents a variant to dRRT / dRRT* , which we call fastdRRT (fdRRT), that returns fast but suboptimal 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 collisionfree path connecting and . Gridbased methods such as Djikstra’s algorithm [2] and A* [6] were developed as a means of finding shortest paths between vertices on a graph. Samplingbased 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 sampledbased planning is provided in [4].
Extending motion planning to the multirobot 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 multirobot 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 collisionfree paths over an explicitly computed multirobot work space, but is limited in scope due to the memory required to build an explicitly defined road map. Using the principle of subdimensional expansion, Wagner et al. designed subdimensional RRT (sRRT) and subdimensional PRM (sPRM) to plan paths for multiple robots with integrator dynamics [20], the latter using M* to query a multirobot path.
Solovey et al. also use subdimensional expansion in discrete RRT (dRRT) [15]. The idea of dRRT is to build road maps of collisionfree 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 precomputed motions between configurations that have already been collision checked against environmental obstacles, dRRT can simply fetch the motions and check if any interrobot collisions occur, thus relieving the algorithm of significant computational burden. Collisionfree 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 stateoftheart algorithms in multirobot 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 preexisting dRRT algorithm to the domain of planning under motion constraints. The central planning algorithm, which we call fastdRRT (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 collisionfree 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 tradeoff 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 nonintersecting 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
(1) 
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 multirobot 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 replanning 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 timeparameterized 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 reparameterized to from the distance traveled over time, which can be tracked by a local controller using a technique such as purepursuit or nonlinear Model Predictive Control (MPC) [10].
Iva 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 singleintegrator 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 kinematicallyconstrained system, but are approximate solutions. Dubins’ paths adhere to kinematic constraints and yield minimal path length for carlike 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 bidirectional. Due to Dubins’ steering constraints and the nonholonomic constraints in (1), this is not necessarily true, and the existence of a collisionfree 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, kinematicallyconstrained PRM (KCPRM), 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).
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:

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

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 breadthfirst 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.
IvB 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 [15] and [14], whose purpose is to solve the multirobot 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 [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 collisionfree 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 collisionfree motion. In our algorithm, the lowest cost collisionfree 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 [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 tradeoff from adding is lower than the tradeoff 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 threelane 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 tradeoffs 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 tradeoff 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 carlike 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.
References
 [1] E. Ackerman, “Amazon uses 800 robots to run this warehouse,” Jun 2019. [Online]. Available: https://spectrum.ieee.org/automaton/robotics/industrialrobots/amazonintroducestwonewwarehouserobots
 [2] 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
 [3] 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
 [4] M. Elbanhawi and M. Simic, “Samplingbased robot motion planning: A review,” IEEE Access, vol. 2, pp. 56–77, 2014.
 [5] J. h. Jeon, S. Karaman, and E. Frazzoli, “Anytime computation of timeoptimal offroad vehicle maneuvers using the rrt*,” in 2011 50th IEEE Conference on Decision and Control and European Control Conference, Dec 2011, pp. 3276–3282.
 [6] 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.
 [7] L. E. Kavraki, P. Svestka, J. . Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in highdimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, Aug 1996.
 [8] J.T. Li and H.J. Liu, “Design optimization of amazon robotics,” 2016.
 [9] B. Liu and A. El Kamel, “V2xbased 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.
 [10] B. Paden, M. Cáp, S. Z. Yong, D. S. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for selfdriving urban vehicles,” CoRR, vol. abs/1604.07446, 2016. [Online]. Available: http://arxiv.org/abs/1604.07446
 [11] 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.
 [12] G. Sanchez and J. . Latombe, “Using a prm planner to compare centralized and decoupled planning for multirobot systems,” in Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292), vol. 2, May 2002, pp. 2112–2119 vol.2.
 [13] A. Scheuer and T. Fraichard, “Continuouscurvature path planning for carlike vehicles,” 10 1997, pp. 997 – 1003 vol.2.
 [14] R. Shome, K. Solovey, A. Dobson, D. Halperin, and K. E. Bekris, “drrt*: Scalable and informed asymptoticallyoptimal multirobot motion planning,” CoRR, vol. abs/1903.00994, 2019. [Online]. Available: http://arxiv.org/abs/1903.00994
 [15] K. Solovey, O. Salzman, and D. Halperin, “Finding a needle in an exponential haystack: Discrete RRT for exploration of implicit roadmaps in multirobot motion planning,” CoRR, vol. abs/1305.2889, 2013. [Online]. Available: http://arxiv.org/abs/1305.2889
 [16] 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
 [17] J. van den Berg, J. Snoeyink, M. Lin, and D. Manocha, “Centralized path planning for multiple robots: Optimal decoupling into sequential plans,” 06 2009.
 [18] 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
 [19] 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.
 [20] 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.
 [21] Z. Yan, N. Jouandeau, and A. Ali, “A survey and analysis of multirobot coordination,” International Journal of Advanced Robotic Systems, vol. 10, p. 1, 12 2013.
 [22] 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
 [23] 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.
 [24] 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.
 [25] 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