Planning on a (Risk) Budget: Safe Non-Conservative Planning in Probabilistic Dynamic Environments

Planning in environments with other agents whose future actions are uncertain often requires compromise between safety and performance. Here our goal is to design efficient planning algorithms with guaranteed bounds on the probability of safety violation, which nonetheless achieve non-conservative performance. To quantify a system's risk, we define a natural criterion called interval risk bounds (IRBs), which provide a parametric upper bound on the probability of safety violation over a given time interval or task. We present a novel receding horizon algorithm, and prove that it can satisfy a desired IRB. Our algorithm maintains a dynamic risk budget which constrains the allowable risk at each iteration, and guarantees recursive feasibility by requiring a safe set to be reachable by a contingency plan within the budget. We empirically demonstrate that our algorithm is both safer and less conservative than strong baselines in two simulated autonomous driving experiments in scenarios involving collision avoidance with other vehicles, and additionally demonstrate our algorithm running on an autonomous class 8 truck.

Map Matching Algorithm for Large-scale Datasets

GPS receivers embedded in cell phones and connected vehicles generate a series of location measurements that can be used for various analytical purposes. A common pre-processing step of this data is the so-called map matching. The goal of map matching is to infer the trajectory that the device followed in a road network from a potentially sparse series of noisy location measurements. Although accurate and robust map matching algorithms based on probabilistic models exist, they are computationally heavy and thus impractical for processing of large datasets. In this paper, we present a scalable map-matching algorithm based on Dijkstra shortest path method, that is both accurate and applicable to large datasets. Our experiments on a publicly-available dataset showed that the proposed method achieves accuracy that is comparable to that of the existing map matching methods using only a fraction of computational resources. In result, our algorithm can be used to efficiently process large datasets of noisy and potentially sparse location data that would be unexploitable using existing techniques due to their high computational requirements.

Provably Safe and Deadlock-Free Execution of Multi-Robot Plans under Delaying Disturbances

One of the standing challenges in multi-robot systems is the ability to reliably coordinate motions of multiple robots in environments where the robots are subject to disturbances. We consider disturbances that force the robot to temporarily stop and delay its advancement along its planned trajectory which can be used to model, e.g., passing-by humans for whom the robots have to yield. Although reactive collision-avoidance methods are often used in this context, they may lead to deadlocks between robots. We design a multi-robot control strategy for executing coordinated trajectories computed by a multi-robot trajectory planner and give a proof that the strategy is safe and deadlock-free even when robots are subject to delaying disturbances. Our simulations show that the proposed strategy scales significantly better with the intensity of disturbances than the naive liveness-preserving approach. The empirical results further confirm that the proposed approach is more reliable and also more efficient than state-of-the-art reactive techniques.

Complete Decentralized Method for On-Line Multi-Robot Trajectory Planning in Valid Infrastructures

We consider a system consisting of multiple mobile robots in which the user can at any time issue relocation tasks ordering one of the robots to move from its current location to a given destination location. In this paper, we deal with the problem of finding a trajectory for each such relocation task that avoids collisions with other robots. The chosen robot plans its trajectory so as to avoid collision with other robots executing tasks that were issued earlier. We prove that if all possible destinations of the relocation tasks satisfy so-called valid infrastructure property, then this mechanism is guaranteed to always succeed and provide a trajectory for the robot that reaches the destination without colliding with any other robot. The time-complexity of the approach on a fixed space-time discretization is only quadratic in the number of robots. We demonstrate the applicability of the presented method on several real-world maps and compare its performance against a popular reactive approach that attempts to solve the collisions locally. Besides being dead-lock free, the presented approach generates trajectories that are significantly faster (up to 48% improvement) than the trajectories resulting from local collision avoidance.

Finding Near-optimal Solutions in Multi-robot Path Planning

We deal with the problem of planning collision-free trajectories for robots operating in a shared space. Given the start and destination position for each of the robots, the task is to find trajectories for all robots that reach their destinations with minimum total cost such that the robots will not collide when following the found trajectories. Our approach starts from individually optimal trajectory for each robot, which are then penalized for being in collision with other robots. The penalty is gradually increased and the individual trajectories are iteratively replanned to account for the increased penalty until a collision-free solution is found. Using extensive experimental evaluation, we find that such a penalty method constructs trajectories with near-optimal cost on the instances where the optimum is known and otherwise with 4-10 % lower cost than the trajectories generated by prioritized planning and up to 40 % cheaper than trajectories generated by local collision avoidance techniques, such as ORCA.

Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots

An important capability of autonomous multi-robot systems is to prevent collision among the individual robots. One approach to this problem is to plan conflict-free trajectories and let each of the robots follow its pre-planned trajectory. A widely used practical method for multi-robot trajectory planning is prioritized planning, which has been shown to be effective in practice, but is in general incomplete. Formal analysis of instances that are provably solvable by prioritized planning is still missing. Moreover, prioritized planning is a centralized algorithm, which may be in many situations undesirable. In this paper we a) propose a revised version of prioritized planning and characterize the class of instances that are provably solvable by the algorithm and b) propose an asynchronous decentralized variant of prioritized planning, which maintains the desirable properties of the centralized version and in the same time exploits the distributed computational power of the individual robots, which in most situations allows to find the joint trajectories faster. The experimental evaluation performed on real-world indoor maps shows that a) the revised version of prioritized planning reliably solves a wide class of instances on which both classical prioritized planning and popular reactive technique ORCA fail and b) the asynchronous decentralized algorithm provides solution faster than the previously proposed synchronized decentralized algorithm.

Finding Coordinated Paths for Multiple Holonomic Agents in 2-d Polygonal Environment

Avoiding collisions is one of the vital tasks for systems of autonomous mobile agents. We focus on the problem of finding continuous coordinated paths for multiple mobile disc agents in a 2-d environment with polygonal obstacles. The problem is PSPACE-hard, with the state space growing exponentially in the number of agents. Therefore, the state of the art methods include mainly reactive techniques and sampling-based iterative algorithms. We compare the performance of a widely-used reactive method ORCA with three variants of a popular planning algorithm RRT* applied to multi-agent path planning and find that an algorithm combining reactive collision avoidance and RRT* planning, which we call ORCA-RRT* can be used to solve instances that are out of the reach of either of the techniques. We experimentally show that: 1) the reactive part of the algorithm can efficiently solve many multi-agent path finding problems involving large number of agents, for which RRT* algorithm is often unable to find a solution in limited time and 2) the planning component of the algorithm is able to solve many instances containing local minima, where reactive techniques typically fail.

Multi-agent RRT*: Sampling-based Cooperative Pathfinding (Extended Abstract)

Cooperative pathfinding is a problem of finding a set of non-conflicting trajectories for a number of mobile agents. Its applications include planning for teams of mobile robots, such as autonomous aircrafts, cars, or underwater vehicles. The state-of-the-art algorithms for cooperative pathfinding typically rely on some heuristic forward-search pathfinding technique, where A* is often the algorithm of choice. Here, we propose MA-RRT*, a novel algorithm for multi-agent path planning that builds upon a recently proposed asymptotically-optimal sampling-based algorithm for finding single-agent shortest path called RRT*. We experimentally evaluate the performance of the algorithm and show that the sampling-based approach offers better scalability than the classical forward-search approach in relatively large, but sparse environments, which are typical in real-world applications such as multi-aircraft collision avoidance.

Asynchronous Decentralized Algorithm for Space-Time Cooperative Pathfinding

Cooperative pathfinding is a multi-agent path planning problem where a group of vehicles searches for a corresponding set of non-conflicting space-time trajectories. Many of the practical methods for centralized solving of cooperative pathfinding problems are based on the prioritized planning strategy. However, in some domains (e.g., multi-robot teams of unmanned aerial vehicles, autonomous underwater vehicles, or unmanned ground vehicles) a decentralized approach may be more desirable than a centralized one due to communication limitations imposed by the domain and/or privacy concerns. In this paper we present an asynchronous decentralized variant of prioritized planning ADPP and its interruptible version IADPP. The algorithm exploits the inherent parallelism of distributed systems and allows for a speed up of the computation process. Unlike the synchronized planning approaches, the algorithm allows an agent to react to updates about other agents' paths immediately and invoke its local spatio-temporal path planner to find the best trajectory, as response to the other agents' choices. We provide a proof of correctness of the algorithms and experimentally evaluate them on synthetic domains.