Multi-Agent Path Finding (MAPF) is the problem of finding a set of collision-free paths for a team of agents. Although several MAPF methods that solve full-horizon MAPF have completeness guarantees, very few MAPF methods that plan partial paths have completeness guarantees. Recent work introduced the Windowed Complete MAPF (WinC-MAPF) framework, which shows how windowed optimal MAPF solvers (e.g., SS-CBS) can use heuristic updates and disjoint agent groups to maintain completeness even when planning partial paths. A core limitation of WinC-MAPF is that it requires optimal MAPF solvers. Our main contribution is to extend WinC-MAPF by showing how we can use a bounded suboptimal solver while maintaining completeness. In particular, we design Dynamic Agent Grouping ECBS (DAG-ECBS) which dynamically creates and plans agent groups while maintaining that each agent group solution is bounded suboptimal. We prove how DAG-ECBS can maintain completeness in the WinC-MAPF framework and can improve scalability compared to windowed ECBS which does not have completeness guarantees. More broadly, our work serves as a blueprint for designing more MAPF methods that can use the WinC-MAPF framework

Traditional multi-agent path finding (MAPF) methods try to compute entire collision free start-goal paths, with several algorithms offering completeness guarantees. However, computing partial paths offers significant advantages including faster planning, adaptability to changes, and enabling decentralized planning. Methods that compute partial paths employ a "windowed" approach and only try to find collision free paths for a limited timestep horizon. While this improves flexibility, this adaptation introduces incompleteness; all existing windowed approaches can become stuck in deadlock or livelock. Our main contribution is to introduce our framework, WinC-MAPF, for Windowed MAPF that enables completeness. Our framework leverages heuristic update insights from single-agent real-time heuristic search algorithms and agent independence ideas from MAPF algorithms. We also develop Single-Step Conflict Based Search (SS-CBS), an instantiation of this framework using a novel modification to CBS. We show how SS-CBS, which only plans a single step and updates heuristics, can effectively solve tough scenarios where existing windowed approaches fail.

Diffusion models have recently been successfully applied to a wide range of robotics applications for learning complex multi-modal behaviors from data. However, prior works have mostly been confined to single-robot and small-scale environments due to the high sample complexity of learning multi-robot diffusion models. In this paper, we propose a method for generating collision-free multi-robot trajectories that conform to underlying data distributions while using only single-robot data. Our algorithm, Multi-robot Multi-model planning Diffusion (MMD), does so by combining learned diffusion models with classical search-based techniques---generating data-driven motions under collision constraints. Scaling further, we show how to compose multiple diffusion models to plan in large environments where a single diffusion model fails to generalize well. We demonstrate the effectiveness of our approach in planning for dozens of robots in a variety of simulated scenarios motivated by logistics environments.

Multi-Agent Path Finding (MAPF) is the problem of effectively finding efficient collision-free paths for a group of agents in a shared workspace. The MAPF community has largely focused on developing high-performance heuristic search methods. Recently, several works have applied various machine learning (ML) techniques to solve MAPF, usually involving sophisticated architectures, reinforcement learning techniques, and set-ups, but none using large amounts of high-quality supervised data. Our initial objective in this work was to show how simple large-scale imitation learning of high-quality heuristic search methods can lead to state-of-the-art ML MAPF performance. However, we find that, at least with our model architecture, simple large-scale (700k examples with hundreds of agents per example) imitation learning does not produce impressive results. Instead, we find that by using prior work that post-processes MAPF model predictions to resolve 1-step collisions (CS-PIBT), we can train a simple ML MAPF policy in minutes that dramatically outperforms existing ML MAPF policies. This has serious implications for all future ML MAPF policies (with local communication) which currently struggle to scale. In particular, this finding implies that future learnt policies should always (1) use smart 1-step collision shields (e.g, CS-PIBT) and (2) include the collision shield with greedy actions as a baseline (e.g. PIBT), as well as (3) motivates future models to focus on longer horizon / more complex planning as 1-step collisions can be efficiently resolved.

Best-first search algorithms such as A* and Weighted A* are widely used tools. However, their high memory requirements often make them impractical for memory-constrained applications, such as on-board planning for interplanetary rovers, drones, and embedded systems. One popular strategy among memory-efficient approaches developed to address this challenge is to eliminate or sparsify the Closed list, a structure that tracks states explored by the search. However, such methods often incur substantial overhead in runtime, requiring recursive searches for solution reconstruction. In this work, we propose Attractor-based Closed List Search (ACLS), a novel framework that sparsely represents the Closed list using a small subset of states, termed attractors. ACLS intelligently identifies attractor states in a way that enables efficient solution reconstruction while preserving theoretical guarantees on the quality of the solution. Furthermore, we also introduce a lazy variant, Lazy-ACLS, which defers the computation of attractor states until necessary, substantially improving planning speed. We demonstrate the efficacy of ACLS used in conjunction with A*, Weighted A*, and Dijkstra’s searches across multiple domains including 2D and 3D navigation, Sliding Tiles, and Towers of Hanoi. Our experimental results demonstrate that ACLS significantly reduces memory usage, maintaining only 9% of the states typically stored in a Closed list, while achieving comparable planning times and outperforming state-of-the-art approaches. Source code can be found at github.com/alvin-ruihua-zou/ACLS.

Model-based planners and controllers are commonly used to solve complex manipulation problems as they can efficiently optimize diverse objectives and generalize to long horizon tasks. However, they often fail during deployment due to noisy actuation, partial observability and imperfect models. To enable a robot to recover from such failures, we propose to use hierarchical reinforcement learning to learn a recovery policy. The recovery policy is triggered when a failure is detected based on sensory observations and seeks to take the robot to a state from which it can complete the task using the nominal model-based controllers. Our approach, called RecoveryChaining, uses a hybrid action space, where the model-based controllers are provided as additional nominal options which allows the recovery policy to decide how to recover, when to switch to a nominal controller and which controller to switch to even with sparse rewards. We evaluate our approach in three multi-step manipulation tasks with sparse rewards, where it learns significantly more robust recovery policies than those learned by baselines. We successfully transfer recovery policies learned in simulation to a physical robot to demonstrate the feasibility of sim-to-real transfer with our method.

The vast majority of Multi-Agent Path Finding (MAPF) methods with completeness guarantees require planning full-horizon paths. However, planning full-horizon paths can take too long and be impractical in real-world applications. Instead, real-time planning and execution, which only allows the planner a finite amount of time before executing and replanning, is more practical for real-world multi-agent systems. Several methods utilize real-time planning schemes but none are provably complete, which leads to livelock or deadlock. Our main contribution is Real-Time LaCAM, the first Real-Time MAPF method with provable completeness guarantees. We do this by leveraging LaCAM in an incremental fashion. Our results show how we can iteratively plan for congested environments with a cutoff time of milliseconds while still maintaining the same success rate as full-horizon LaCAM. We also show how it can be used with a single-step learned MAPF policy.

This frontmatter introduces the proceedings of the Eighteenth International Symposium on Combinatorial Search (SoCS 2025), held from August 12–15, 2025, in Scotland, United Kingdom. It includes a preface by the conference co-chairs—Maxim Likhachev, Hana Rudová, and Enrico Scala—along with details on the Best Paper Awards, the organizing and program committees, and the sponsors of this edition.

Heuristic search solvers like RTDP-Bel and LAO* have proven effective for computing optimal and bounded sub-optimal solutions for Partially Observable Markov Decision Processes (POMDPs), which are typically formulated as belief MDPs. A belief represents a probability distribution over possible system states. Given a parent belief and an action, computing belief state transitions involves Bayesian updates that combine the transition and observation models of the POMDP to determine successor beliefs and their transition probabilities. However, there is a class of problems, specifically in robotics, where computing these transitions can be prohibitively expensive due to costly physics simulations, raycasting, or expensive collision checks required by the underlying transition and observation models, leading to long planning times. To address this challenge, we propose Lazy RTDP-Bel and Lazy LAO*, which defer computing expensive belief state transitions by leveraging Q-value estimation, significantly reducing planning time. These algorithms are specific instantiations of the broader idea of lazy search for POMDPs. We demonstrate the superior performance of the proposed lazy planners in domains such as contact-rich manipulation for pose estimation, outdoor navigation in rough terrain, and indoor navigation with a 1-D Lidar sensor. Additionally, we discuss practical Q-value estimation techniques for commonly encountered problem classes that our lazy planners can leverage. Our results show that lazy heuristic search methods dramatically improve planning speed by postponing expensive belief transition evaluations while maintaining solution quality.

An exciting frontier in robotic manipulation is the use of multiple arms at once. However, planning concurrent motions is a challenging task using current methods. The high-dimensional composite state space renders many well-known motion planning algorithms intractable. Recently, Multi-Agent Path Finding (MAPF) algorithms have shown promise in discrete 2D domains, providing rigorous guarantees. However, widely used conflict-based methods in MAPF assume an efficient single-agent motion planner. This poses challenges in adapting them to manipulation cases where this assumption does not hold, due to the high dimensionality of configuration spaces and the computational bottlenecks associated with collision checking. To this end, we propose an approach for accelerating conflict-based search algorithms by leveraging their repetitive and incremental nature -- making them tractable for use in complex scenarios involving multi-arm coordination in obstacle-laden environments. We show that our method preserves completeness and bounded sub-optimality guarantees, and demonstrate its practical efficacy through a set of experiments with up to 10 robotic arms.

Multi-agent path finding (MAPF) is the problem of finding collision-free paths for a team of agents to reach their goal locations. State-of-the-art classical MAPF solvers typically employ heuristic search to find solutions for hundreds of agents but are typically centralized and can struggle to scale when run with short timeouts. Machine learning (ML) approaches that learn policies for each agent are appealing as these could enable decentralized systems and scale well while maintaining good solution quality. Current ML approaches to MAPF have proposed methods that have started to scratch the surface of this potential. However, state-of-the-art ML approaches produce ``local" policies that only plan for a single timestep and have poor success rates and scalability. Our main idea is that we can improve a ML local policy by using heuristic search methods on the output probability distribution to resolve deadlocks and enable full horizon planning. We show several model-agnostic ways to use heuristic search with learnt policies that significantly improve the policies' success rates and scalability. To our best knowledge, we demonstrate the first time ML-based MAPF approaches have scaled to high congestion scenarios (e.g. 20% agent density).

Recent works have made significant progress in multi-agent path finding (MAPF), with modern methods being able to scale to hundreds of agents, handle unexpected delays, work in groups, etc. The vast majority of these methods have focused on 2D "grid world" domains. However, modern warehouses often utilize multi-agent robotic systems that can move in 3D, enabling dense storage but resulting in a more complex multi-agent planning problem. Motivated by this, we introduce and experimentally analyze the application of MAPF to 3D warehouse management, and release the first (see http://mapf.info/index.php/Main/Benchmarks) open-source 3D MAPF dataset. We benchmark two state-of-the-art MAPF methods, EECBS and MAPF-LNS2, and show how different hyper-parameters affect these methods across various 3D MAPF problems. We also investigate how the warehouse structure itself affects MAPF performance. Based on our experimental analysis, we find that a fast low-level search is critical for 3D MAPF, EECBS's suboptimality significantly changes the effect of certain CBS techniques, and certain warehouse designs can noticeably influence MAPF scalability and speed. An additional important observation is that, overall, the tested 2D MAPF techniques scaled well to 3D warehouses and demonstrate how the MAPF community's progress in 2D can generalize to 3D warehouses.

Robotic manipulators are essential for future autonomous systems, yet limited trust in their autonomy has confined them to rigid, task-specific systems. The intricate configuration space of manipulators, coupled with the challenges of obstacle avoidance and constraint satisfaction, often makes motion planning the bottleneck for achieving reliable and adaptable autonomy. Recently, a class of constant-time motion planners (CTMP) was introduced. These planners employ a preprocessing phase to compute data structures that enable online planning provably guarantee the ability to generate motion plans, potentially sub-optimal, within a user defined time bound. This framework has been demonstrated to be effective in a number of time-critical tasks. However, robotic systems often have more time allotted for planning than the online portion of CTMP requires, time that can be used to improve the solution. To this end, we propose an anytime refinement approach that works in combination with CTMP algorithms. Our proposed framework, as it operates as a constant time algorithm, rapidly generates an initial solution within a user-defined time threshold. Furthermore, functioning as an anytime algorithm, it iteratively refines the solution’s quality within the allocated time budget. This enables our approach to strike a balance between guaranteed fast plan generation and the pursuit of optimization over time. We support our approach by elucidating its analytical properties, showing the convergence of the anytime component towards optimal solutions. Additionally, we provide empirical validation through simulation and real-world demonstrations on a 6 degree-of-freedom robot manipulator, applied to an assembly domain.

We are interested in studying sports with robots and starting with the problem of intercepting a projectile moving toward a robot manipulator equipped with a shield. To successfully perform this task, the robot needs to (i) detect the incoming projectile, (ii) predict the projectile’s future motion, (iii) plan a minimum-time rapid trajectory that can evade obstacles and intercept the projectile, and (iv) execute the planned trajectory. These four steps must be performed under the manipulator’s dynamic limits and extreme time constraints (≤ 350ms in our setting) to successfully intercept the projectile. In addition, we want these trajectories to be smooth to reduce the robot’s joint torques and the impulse on the platform on which it is mounted. To this end, we propose a kinodynamic motion planning framework that preprocesses smooth trajectories offline to allow real-time collision-free executions online. We present an end-to-end pipeline along with our planning framework, including perception, prediction, and execution modules. We evaluate our framework experimentally in simulation and show that it has a higher blocking success rate than the baselines. Further, we deploy our pipeline on a robotic system comprising an industrial arm (ABB IRB-1600) and an onboard stereo camera (ZED 2i), which achieves a 78% success rate in projectile interceptions.

We are interested in enabling robots to solve difficult pick-and-place manipulation tasks in cluttered and constrained environments. If the robot does not have collision-free access to the object-of-interest (OoI) which it intends to grasp and extract from the workspace, it must reason about which movable objects to rearrange, where to move them, and how it may do so. In recent work [1] we introduced E-M4M, a graph search-based solver for solving such Manipulation tasks Among Movable Objects (MAMO). In this paper we make several improvements to E-M4M – we introduce the use of prehensile or pick-and-place rearrangement actions in addition to pushes; we show that by running it as a depth-first search improves performance; we show how the search can be run "eagerly lazily" to only simulate actions in a physics-based simulator when necessary; finally we relax the assumption that we require perfect knowledge of the physical properties of objects (mass and coefficient of friction in particular). The improved version of E-M4M presented in this paper, I-M4M, is a faster and more versatile MAMO solver with a rich action space. We discuss the impact of the improvements we make in an extensive simulation study and show previously unachievable results on a real-world PR2 robot.

Trajectory optimization is a widely used technique in robot motion planning for letting the dynamics of the system shape and synthesize complex behaviors. Several previous works have shown its benefits in high-dimensional continuous state spaces and under differential constraints. However, long time horizons and planning around obstacles in non-convex spaces pose challenges in guaranteeing convergence or finding optimal solutions. As a result, discrete graph search planners and sampling-based planers are preferred when facing obstacle-cluttered environments. A recently developed algorithm called INSAT effectively combines graph search in the low-dimensional subspace and trajectory optimization in the full-dimensional space for global kinodynamic planning over long horizons. Although INSAT successfully reasoned about and solved complex planning problems, the numerous expensive calls to an optimizer resulted in large planning times, thereby limiting its practical use. Inspired by the recent work on edge-based parallel graph search, we present PINSAT, which introduces systematic parallelization in INSAT to achieve lower planning times and higher success rates, while maintaining significantly lower costs over relevant baselines. We demonstrate PINSAT by evaluating it on 6 DoF kinodynamic manipulation planning with obstacles. We demonstrate PINSAT by evaluating it on two kinodynamic manipulation planning scenarios: (i) a single ball blocking task among obstacles using a 6 DoF ABB arm, and (ii) a multi-ball blocking task where the balls are separated by short time intervals using a 7 DoF KUKA LBR iiwa arm with obstacles.

Multi-Robot-Arm Motion Planning (M-RAMP) is a challenging problem featuring complex single-agent planning and multi-agent coordination. Recent advancements in extending the popular Conflict-Based Search (CBS) algorithm have made large strides in solving Multi-Agent Path Finding (MAPF) problems. However, fundamental challenges remain in applying CBS to M-RAMP. A core challenge is the existing reliance of the CBS framework on conservative "complete" constraints. These constraints ensure solution guarantees but often result in slow pruning of the search space -- causing repeated expensive single-agent planning calls. Therefore, even though it is possible to leverage domain knowledge and design incomplete M-RAMP-specific CBS constraints to more efficiently prune the search, using these constraints would render the algorithm itself incomplete. This forces practitioners to choose between efficiency and completeness. In light of these challenges, we propose a novel algorithm, Generalized ECBS, aimed at removing the burden of choice between completeness and efficiency in MAPF algorithms. Our approach enables the use of arbitrary constraints in conflict-based algorithms while preserving completeness and bounding sub-optimality. This enables practitioners to capitalize on the benefits of arbitrary constraints and opens a new space for constraint design in MAPF that has not been explored. We provide a theoretical analysis of our algorithms, propose new "incomplete" constraints, and demonstrate their effectiveness through experiments in M-RAMP.

With the advent of machine learning, there have been several recent attempts to learn effective and generalizable heuristics. Local Heuristic A* (LoHA*) is one recent method that instead of learning the entire heuristic estimate, learns a "local" residual heuristic that estimates the cost to escape a region. LoHA*, like other supervised learning methods, collects a dataset of target values by querying an oracle on many planning problems (in this case, local planning problems). This data collection process can become slow as the size of the local region increases or if the domain requires expensive collision checks. Our main insight is that when an A* search solves a start-goal planning problem it inherently ends up solving multiple local planning problems. We exploit this observation to propose an efficient data collection framework that does

The majority of multi-agent path finding (MAPF) methods compute collision-free space-time paths which require agents to be at a specific location at a specific discretized timestep. However, executing these space-time paths directly on robotic systems is infeasible due to real-time execution differences (e.g. delays) which can lead to collisions. To combat this, current methods translate the space-time paths into a temporal plan graph (TPG) that only requires that agents observe the order in which they navigate through locations where their paths cross. However, planning space-time paths and then post-processing them into a TPG does not reduce the required agent-to-agent coordination, which is fixed once the space-time paths are computed. To that end, we propose a novel algorithm Space-Order CBS that can directly plan a TPG and explicitly minimize coordination. Our main theoretical insight is our novel perspective on viewing a TPG as a set of space-visitation order paths where agents visit locations in relative orders (e.g. 1st vs 2nd) as opposed to specific timesteps. We redefine unique conflicts and constraints for adapting CBS for space-order planning. We experimentally validate how Space-Order CBS can return TPGs which significantly reduce coordination, thus subsequently reducing the amount of agent-agent communication and leading to more robustness to delays during execution.

Conflict-Based Search (CBS) is a popular multi-agent path finding (MAPF) solver that employs a low-level single agent planner and a high-level constraint tree to resolve conflicts. The vast majority of modern MAPF solvers focus on improving CBS by reducing the size of this tree through various strategies with few methods modifying the low level planner. Typically low level planners in existing CBS methods use an unweighted cost-to-go heuristic, with suboptimal CBS methods also using a conflict heuristic to help the high level search. In this paper, we show that, contrary to prevailing CBS beliefs, a weighted cost-to-go heuristic can be used effectively alongside the conflict heuristic in two possible variants. In particular, one of these variants can obtain large speedups, 2-100x, across several scenarios and suboptimal CBS methods. Importantly, we discover that performance is related not to the weighted cost-to-go heuristic but rather to the relative conflict heuristic weight's ability to effectively balance low-level and high-level work. Additionally, to the best of our knowledge, we show the first theoretical relation of prioritized planning and bounded suboptimal CBS and demonstrate that our methods are their natural generalization.

Robots often have to perform manipulation tasks in close proximity to people (Fig 1). As such, it is desirable to use a robot arm that has limited joint torques so as to not injure the nearby person. Unfortunately, these limited torques then limit the payload capability of the arm. By using contact with the environment, robots can expand their reachable workspace that, otherwise, would be inaccessible due to exceeding actuator torque limits. We adapt our recently developed INSAT algorithm [1] to tackle the problem of torque-limited whole arm manipulation planning through contact. INSAT requires no prior over contact mode sequence and no initial template or seed for trajectory optimization. INSAT achieves this by interleaving graph search to explore the manipulator joint configuration space with incremental trajectory optimizations seeded by neighborhood solutions to find a dynamically feasible trajectory through contact. We demonstrate our results on a variety of manipulators and scenarios in simulation. We also experimentally show our planner exploiting robot-environment contact for the pick and place of a payload using a Kinova Gen3 robot. In comparison to the same trajectory running in free space, we experimentally show that the utilization of bracing contacts reduces the overall torque required to execute the trajectory.

Typical robotic systems rely on models for planning. Therefore, the quality of the robot's behavior is heavily dependent on how accurately the model can predict the outcome of the robot's actions in the environment. A challenge, however, is that no model is perfect; moreover, we often do not know where discrepancies between the model's prediction and the actual outcome occur prior to observing executions in the real-world. One way to address this is to bias the planner away from these discrepancies by inflating the cost of states and actions where we previously observed the model to be inaccurate. Making such decisions about where and how to bias purely at the planning-level, however, neglects valuable information from the control-level, which gives a more fine-grained understanding of where and how the model went wrong during execution. Based on this observation, our key idea is to first infer a statistical model over discrepancies in the control-level's model. Then, we translate this model to the planning-level, where we use it to more informatively bias the planner away from states and actions where the model's predicted outcome is likely to be inaccurate. We demonstrate that our framework enables a robot to complete tasks, despite an inaccurate planning model, with greater efficiency than existing approaches. We do so through an experimental evaluation in simulation and real-robot experiments on NASA's Astrobee free-flyer.

Real-world manipulation problems in heavy clutter require robots to reason about potential contacts with objects in the environment. We focus on pick-and-place style tasks to retrieve a target object from a shelf where some ‘movable’ objects must be rearranged in order to solve the task. In particular, our motivation is to allow the robot to reason over and consider non-prehensile rearrangement actions that lead to complex robot-object and object-object interactions where multiple objects might be moved by the robot simultaneously, and objects might tilt, lean on each other, or topple. To support this, we query a physics-based simulator to forward simulate these interaction dynamics which makes action evaluation during planning computationally very expensive. To make the planner tractable, we establish a connection between the domain of Manipulation Among Movable Objects and Multi-Agent Pathfinding that lets us decompose the problem into two phases our M4M algorithm iterates over. First we solve a multi-agent planning problem that reasons about the configurations of movable objects but does not forward simulate a physics model. Next, an arm motion planning problem is solved that uses a physics-based simulator but does not search over possible configurations of movable objects. We run simulated and real-world experiments with the PR2 robot and compare against relevant baseline algorithms. Our results highlight that M4M generates complex 3D interactions, and solves at least twice as many problems as the baselines with competitive performance.

Operating under real world conditions is challenging due to the possibility of a wide range of failures induced by execution errors and state uncertainty. In relatively benign settings, such failures can be overcome by retrying or executing one of a small number of hand-engineered recovery strategies. By contrast, contact-rich sequential manipulation tasks, like opening doors and assembling furniture, are not amenable to exhaustive hand-engineering. To address this issue, we present a general approach for robustifying manipulation strategies in a sample-efficient manner. Our approach incrementally improves robustness by first discovering the failure modes of the current strategy via exploration in simulation and then learning additional recovery skills to handle these failures. To ensure efficient learning, we propose an online algorithm called Meta-Reasoning for Skill Learning (MetaReSkill) that monitors the progress of all recovery policies during training and allocates training resources to recoveries that are likely to improve the task performance the most. We use our approach to learn recovery skills for door-opening and evaluate them both in simulation and on a real robot with little fine-tuning. Compared to open-loop execution, our experiments show that even a limited amount of recovery learning improves task success substantially from 71% to 92.4% in simulation and from 75% to 90% on a real robot.

Parallel search algorithms have been shown to improve planning speed by harnessing the multithreading capability of modern processors. One such algorithm PA*SE achieves this by parallelizing state expansions, whereas another algorithm ePA*SE achieves this by effectively parallelizing edge evaluations. ePA*SE targets domains in which the action space comprises actions with expensive but similar evaluation times. However, in a number of robotics domains, the action space is heterogenous in the computational effort required to evaluate the cost of an action and its outcome. Motivated by this, we introduce GePA*SE: Generalized Edge-based Parallel A* for Slow Evaluations, which generalizes the key ideas of PA*SE and ePA*SE, i.e., parallelization of state expansions and edge evaluations, respectively. This extends its applicability to domains that have actions requiring varying computational effort to evaluate them. The open-source code for GePA*SE, along with the baselines, is available here: https://github.com/shohinm/parallel_search

Anytime search algorithms are useful for planning problems where a solution is desired under a limited time budget. Anytime algorithms first aim to provide a feasible solution quickly and then attempt to improve it until the time budget expires. On the other hand, parallel search algorithms utilize the multithreading capability of modern processors to speed up the search. One such algorithm, ePA*SE (Edge-Based Parallel A* for Slow Evaluations), parallelizes edge evaluations to achieve faster planning and is especially useful in domains with expensive-to-compute edges. In this work, we propose an extension that brings the anytime property to ePA*SE, resulting in A-ePA*SE. We evaluate A-ePA*SE experimentally and show that it is significantly more efficient than other anytime search methods. The open-source code for A-ePA*SE, along with the baselines, is available here: https://github.com/shohinm/parallel_search

Search-based techniques have shown great success in motion planning problems such as robotic navigation by discretizing the state space and precomputing motion primitives. However in domains with complex dynamic constraints, constructing motion primitives in a discretized state space is non-trivial. This requires operating in continuous space which can be challenging for search-based planners as they can get stuck in local minima regions. Previous work [1] on planning in continuous spaces introduced soft duplicate detection which requires search to compute the duplicity of a state with respect to previously seen states to avoid exploring states that are likely to be duplicates, especially in local minima regions. They propose a simple metric utilizing the Euclidean distance between states, and proximity to obstacles to compute the duplicity. In this paper, we improve upon this metric by introducing a kinodynamically informed metric, subtree overlap, between two states as the similarity between their successors that can be reached within a fixed time horizon using kinodynamic motion primitives. This captures the intuition that, due to robot dynamics, duplicate states can be far in Euclidean distance and result in very similar successor states, while non-duplicate states can be close and result in widely different successors. Our approach computes the new metric offline for a given robot dynamics, and stores the subtree overlap value for all possible relative state configurations. During search, the planner uses these precomputed values to speed up duplicity computation, and achieves fast planning times in continuous spaces in addition to completeness and sub-optimality guarantees. Empirically, we show that our improved metric for soft duplicity detection in search-based planning outperforms previous approaches in terms of planning time, by a factor of 1.5 to 2× on 3D and 5D planning domains with highly constrained dynamics.

Heuristic search-based motion planning algorithms typically discretise the search space in order to solve the shortest path problem. Their performance is closely related to this discretisation. A fine discretisation allows for better approximations of the continuous search space, but makes the search for a solution more computationally costly. A coarser resolution might allow the algorithms to find solutions quickly at the expense of quality. For large state spaces, it can be beneficial to search for solutions across multiple resolutions even though defining the discretisations is challenging. The recently proposed algorithm Multi-Resolution A* (MRA*) searches over multiple resolutions. It traverses large areas of obstacle-free space and escapes local minima at a coarse resolution. It can also navigate so-called narrow passageways at a finer resolution. In this work, we develop AMRA*, an anytime version of MRA*, AMRA* tries to find a solution quickly using the coarse resolution as much as possible. It then refines the solution by relying on the fine resolution to discover better paths that may not have been available at the coarse resolution. In addition to being anytime, AMRA* can also leverage information sharing between multiple heuristics. We prove that AMRA* is complete and optimal (in-the-limit of time) with respect to the finest resolution. We show its performance on 2D grid navigation and 4D kinodynamic planning problems.

We consider the problem of completing a set of$n$tasks with a human-robot team using minimum effort. In many domains, teaching a robot to be fully autonomous can be counterproductive if there are finitely many tasks to be done. Rather, the optimal strategy is to weigh the cost of teaching a robot and its benefit- how many new tasks it allows the robot to solve autonomously. We formulate this as a planning problem where the goal is to decide what tasks the robot should do autonomously (act), what tasks should be delegated to a human (delegate) and what tasks the robot should be taught (learn) so as to complete all the given tasks with minimum effort. This planning problem results in a search tree that grows expo-nentially with$n$- making standard graph search algorithms intractable. We address this by converting the problem into a mixed integer program that can be solved efficiently using off-the-shelf solvers with bounds on solution quality. To predict the benefit of learning, we use an approximate simulation model of the tasks to train a precondition model that is parameterized by the training task. Finally, we evaluate our approach on peg insertion and Lego stacking tasks- both in simulation and real-world, showing substantial savings in human effort.

RACOD is an algorithm/hardware co-design for mobile robot path planning. It consists of two main components: CODAcc, a hardware accelerator for collision detection; and RASExp, an algorithm extension for runahead path exploration. CODAcc uses a novel MapReduce-style hardware computational model and massively parallelizes individual collision checks. RASExp predicts future path explorations and proactively computes its collision status ahead of time, thereby overlapping multiple collision detections. By affording multiple cheap CODAcc accelerators and overlapping collision detections using RASExp, RACOD significantly accelerates planning for mobile robots operating in arbitrary environments. Evaluations of popular benchmarks show up to 41.4× (self-driving cars) and 34.3× (pilotless drones) speedup with less than 0.3% area overhead.

The emergence of “robotics in the wild” has triggered a wave of recent research in hardware and software to boost robots’ compute capabilities. Nevertheless, research in this area is hindered by the lack of a comprehensive benchmark suite.In this paper, we present RTRBench, a benchmark suite for robotic kernels. RTRBench includes 16 kernels, spanning the entire software pipeline of a wide swath of robots, all implemented in C++ for fast execution.Together with the suite, we conduct an evaluation of the workloads at the architecture level. We pinpoint the sources of inefficiencies in a modern robotic processor when executing the robotic kernels, along with the opportunities for improvements.The source code of the benchmark suite is available in https://cmu-roboarch.github.io/rtrbench/.

Off-road and unstructured environments often contain complex patches of various types of terrain, rough elevation changes, deformable objects, etc. An autonomous ground vehicle traversing such environments experiences physical interactions that are extremely hard to model at scale and thus very hard to predict. Nevertheless, planning a safely traversable path through such an environment requires the ability to predict the outcomes of these interactions instead of avoiding them. One approach to doing this is to learn the interaction model offline based on collected data. Unfortunately, though, this requires large amounts of data and can often be brittle. Alternatively, models using physics-based simulators can generate large data and provide a reliable prediction. However, they are very slow to query online within the planning loop. This work proposes an algorithmic framework that utilizes the combination of a learned model and a physics-based simulation model for fast planning. Specifically, it uses the learned model as much as possible to accelerate planning while sparsely using the physics-based simulator to verify the feasibility of the planned path. We provide a theoretical analysis of the algorithm and its empirical evaluation showing a significant reduction in planning times.

Parallel search algorithms harness the multithreading capability of modern processors to achieve faster planning. One such algorithm is PA*SE (Parallel A* for Slow Expansions), which parallelizes state expansions to achieve faster planning in domains where state expansions are slow. In this work, we propose ePA*SE (Edge-Based Parallel A* for Slow Evaluations) that improves on PA*SE by parallelizing edge evaluations instead of state expansions. This makes ePA*SE more efficient in domains where edge evaluations are expensive and need varying amounts of computational effort, which is often the case in robotics. On the theoretical front, we show that ePA*SE provides rigorous optimality guarantees. In addition, ePA*SE can be trivially extended to handle an inflation weight on the heuristic resulting in a bounded suboptimal algorithm w-ePA*SE (Weighted ePA*SE) that trades off optimality for faster planning. On the experimental front, we validate the proposed algorithm in two different planning domains: 1) motion planning for 3D humanoid navigation and 2) task and motion planning for a dual-arm robotic assembly task. We show that ePA*SE can be significantly more efficient than PA*SE and other alternatives. The open-source code for ePA*SE along with the baselines is available here: https://github.com/shohinm/parallel_search

This work addresses a Multi-Objective Shortest Path Problem (MO-SPP) on a graph where the goal is to find a set of Pareto-optimal solutions from a start node to a destination in the graph. A family of approaches based on MOA* have been developed to solve MO-SPP in the literature. Typically, these approaches maintain a "frontier" set at each node during the search process to keep track of the non-dominated, partial paths to reach that node. This search process becomes computationally expensive when the number of objectives increases as the number of Pareto-optimal solutions becomes large. In this work, we introduce a new method to efficiently maintain these frontiers for multiple objectives by incrementally constructing balanced binary search trees within the MOA* search framework. We first show that our approach correctly finds the Pareto-optimal front, and then provide extensive simulation results for problems with three, four and five objectives to show that our method runs faster than existing techniques by up to an order of magnitude.

Conflict-Based Search (CBS) is a popular multi-agent path finding (MAPF) solver that employs a low-level single agent planner and a high-level constraint tree to resolve conflicts. The majority of modern MAPF solvers focus on improving CBS by reducing the size of this tree through various strategies with few methods modifying the low level planner. All low level planners in existing CBS methods use an unweighted cost-to-go heuristic, with suboptimal CBS methods also using a conflict heuristic to help the high level search. Contrary to prevailing beliefs, we show that the cost-to-go heuristic can be used significantly more effectively by weighting it in a specific manner alongside the conflict heuristic. We introduce two variants of doing so and demonstrate that this change can lead to 2-100x speedups in certain scenarios. Additionally, we show the first theoretical relation of prioritized planning and bounded suboptimal CBS and demonstrate that our methods are their natural generalization.

Given access to accurate dynamical models, modern planning approaches are effective in computing feasible and optimal plans for repetitive robotic tasks. However, it is difficult to model the true dynamics of the real world before execution, especially for tasks requiring interactions with objects whose parameters are unknown. A recent planning approach, CMAX, tackles this problem by adapting the planner online during execution to bias the resulting plans away from inaccurately modeled regions. CMAX, while being provably guaranteed to reach the goal, requires strong assumptions on the accuracy of the model used for planning and fails to improve the quality of the solution over repetitions of the same task. In this paper we propose CMAX++, an approach that leverages real-world experience to improve the quality of resulting plans over successive repetitions of a robotic task. CMAX++ achieves this by integrating model-free learning using acquired experience with model-based planning using the potentially inaccurate model. We provide provable guarantees on the completeness and asymptotic convergence of CMAX++ to the optimal path cost as the number of repetitions increases. CMAX++ is also shown to outperform baselines in simulated robotic tasks including 3D mobile robot navigation where the track friction is incorrectly modeled, and a 7D pick-and-place task where the mass of the object is unknown leading to discrepancy between true and modeled dynamics.

In many applications, including logistics and manufacturing, robot manipulators operate in semi-structured environments alongside humans or other robots. These environments are largely static, but they may contain some movable obstacles that the robot must avoid. Manipulation tasks in these applications are often highly repetitive, but require fast and reliable motion planning capabilities, often under strict time constraints. Existing preprocessing-based approaches are beneficial when the environments are highly-structured, but their performance degrades in the presence of movable obstacles, since these are not modelled a priori. We propose a novel preprocessing-based method called Alternative Paths Planner (APP) that provides provably fixed-time planning guarantees in semi-structured environments. APP plans a set of alternative paths offline such that, for any configuration of the movable obstacles, at least one of the paths from this set is collision-free. During online execution, a collision-free path can be looked up efficiently within a few microseconds. We evaluate APP on a 7 DoF robot arm in semi-structured domains of varying complexity and demonstrate that APP is several orders of magnitude faster than state-of-the-art motion planners for each domain. We further validate this approach with real-time experiments on a robotic manipulator.

Path planning for robotic coverage is the task of determining a collision-free robot trajectory that observes all points of interest in an environment. Robots employed for such tasks are often capable of exercising active control over onboard observational sensors during navigation. We address the problem of planning robot and sensor trajectories that maximize information gain in such tasks, where the robot needs to cover points of interest with its sensor footprint. Search-based planners in general guarantee completeness and provable bounds on sub-optimality with respect to an underlying graph discretization. However, searching for kinodynamically feasible paths in the joint space of robot and sensor state variables with standard search is computationally expensive. We propose two alternative search-based approaches to this problem. The first solves for robot and sensor trajectories independently in decoupled state spaces while maintaining a history of sensor headings during the search. The second is a two-step approach that first quickly computes a solution in decoupled state spaces and then refines it by searching its local neighborhood in the joint space for a better solution. We evaluate our approaches in simulation with a kinodynamically constrained unmanned aerial vehicle performing coverage over a 2D environment and show their benefits.

Robot manipulation in cluttered scenes often requires contact-rich interactions with objects. It can be more economical to interact via non-prehensile actions, for example, push through other objects to get to the desired grasp pose, instead of deliberate prehensile rearrangement of the scene. For each object in a scene, depending on its properties, the robot may or may not be allowed to make contact with, tilt, or topple it. To ensure that these constraints are satisfied during non-prehensile interactions, a planner can query a physics-based simulator to evaluate the complex multi-body interactions caused by robot actions. Unfortunately, it is infeasible to query the simulator for thousands of actions that need to be evaluated in a typical planning problem as each simulation is time-consuming. In this work, we show that (i) manipulation tasks (specifically pick-and-place style tasks from a tabletop or a refrigerator) can often be solved by restricting robot-object interactions to adaptive motion primitives in a plan, (ii) these actions can be incorporated as subgoals within a multi-heuristic search framework, and (iii) limiting interactions to these actions can help reduce the time spent querying the simulator during planning by up to 40× in comparison to baseline algorithms. Our algorithm is evaluated in simulation and in the real-world on a PR2 robot using PyBullet as our physics-based simulator. Supplementary video: https://youtu.be/ABQc7JbeJPM.

Agent control among pedestrians is often approached in one of the three following ways: using predefined behaviors for agent navigation, learning navigation behaviors from data, or search-based planning on a graph where each edge is a feasible action chosen from a set of predefined actions. While the first approach often produces natural looking motions and the second learns and utilizes complex interactions with pedestrians, both lack global reasoning about how to sequence these behaviors to achieve the overall goal. The third approach, namely search-based planning, does incorporate global reasoning but relies on predefined actions that do not involve any interactions with pedestrians or assume predefined interactions that cannot model complex interactions. This is a significant drawback since many situations such as going through a doorway blocked by other people require complex interactions in order to avoid highly suboptimal behaviors or not being able to get to the goal at all. To this end, we propose a search-based planning framework that constructs and searches a graph wherein each edge can be either a predefined action or a learned behavior. We further extend it to deal with the uncertainty arising from introducing learned behaviors. We present the algorithm, go over its theoretical analysis, and present experimental results.

Path planning in the presence of dynamic obstacles is a fundamental problem in robotics with widespread applications. A typical approach to such problems is that a robot predicts the trajectories of dynamic obstacles, and plans its path while avoiding them. Such a formulation becomes limiting though for scenarios where an agent cannot complete its task efficiently, without disrupting the movement of dynamic obstacles. For example, when merging in heavy traffic or navigating through crowded corridors. In this paper, we propose a paradigm for planning in dynamic environments, called Disruption-Limited Planning (DLP), that allows a robot to disrupt the motions of dynamic obstacles in order to accomplish its task. DLP builds on the premise that while a robot may have to disrupt others’ trajectories to achieve its goals, it should try to limit the disruption. DLP assumes that it can estimate others’ response to its own actions/plans, and plans its own path while ensuring that no other agents’ disrupted trajectory cost gets worse than w-times their initial trajectory costs. While our formulation is motivated by the Stackelberg competitions, we show that DLP can be both more expressive and computationally more efficient compared to a Stackelberg planner. We present DLP paradigm, develop its efficient implementation based on A*, analyze its theoretical properties, and apply it to multiple planning in dynamic environment problems, including x,y,time planning, planning for self-driving, and planning for arm manipulation. We compare DLP with purely altruistic, purely egocentric, and optimal Stackelberg planners, demonstrating the efficacy of DLP over these alternatives.

Zero-shot execution of unseen robotic tasks is important to allowing robots to perform a wide variety of tasks in human environments, but collecting the amounts of data necessary to train end-to-end policies in the real-world is often infeasible. We describe an approach for sim-to-real training that can accomplish unseen robotic tasks using models learned in simulation to ground components of a simple task planner. We learn a library of parameterized skills, along with a set of predicates-based preconditions and termination conditions, entirely in simulation. We explore a block-stacking task because it has a clear structure, where multiple skills must be chained together, but our methods are applicable to a wide range of other problems and domains, and can transfer from simulation to the real-world with no fine tuning. The system is able to recognize failures and accomplish long-horizon tasks from perceptual input, which is critical for real-world execution. We evaluate our proposed approach in both simulation and in the real-world, showing an increase in success rate from 91.6% to 98% in simulation and from 10% to 80% success rate in the real-world as compared with naive baselines. For experiment videos including both real-world and simulation, see: https://www.youtube.com/playlist?list=PL-oD0xHUngeLfQmpngYkGFZarstfPOXqX

In this work we tackle the path planning problem for a 21-dimensional snake robot-like manipulator, navigating a cluttered gas turbine for the purposes of inspection. Heuristic search based approaches are effective planning strategies for common manipulation domains. However, their performance on high dimensional systems is heavily reliant on the effectiveness of the action space and the heuristics chosen. The complex nature of our system, reachability constraints, and highly cluttered turbine environment renders naive choices of action spaces and heuristics ineffective. To this extent we have developed i) a methodology for dynamically generating actions based on online optimization that help the robot navigate narrow spaces, ii) a technique for lazily generating these computationally expensive optimization actions to effectively utilize resources, and iii) heuristics that reason about the homotopy classes induced by the blades of the turbine in the robot workspace and a Multi-Heuristic framework which guides the search along the relevant classes. The impact of our contributions is presented through an experimental study in simulation, where the 21 DOF manipulator navigates towards regions of inspection within a turbine.

In the real-world, robots must often plan despite the environment being partially known. This frequently necessitates planning under uncertainty over missing information about the environment. Unfortunately, the computational expense of such planning often precludes its scalability to real-world problems. The Probabilistic Planning with Clear Preferences (PPCP) framework focuses on a specific subset of such planning problems wherein there exist clear preferences over the actual values of missing information (Likhachev and Stenz 2009). PPCP exploits the existence and knowledge of these preferences to perform provably optimal planning via a series of deterministic A*-like searches over particular instantiations of the environment. Such decomposition leads to much better scalability with respect to both the size of a problem and the amount of missing information in it. The run-time of PPCP however is a function of the number of searches it has to run until convergence. In this paper, we make a key observation that the number of searches PPCP has to run can be dramatically decreased if each search computes a plan that minimizes the amount of missing information it relies upon. To that end, we introduce Fast-PPCP, a novel planning algorithm that computes a provably bounded suboptimal policy using significantly lesser number of searches than that required to find an optimal policy. We present Fast-PPCP with its theoretical analysis, compare with common alternative approaches to planning under uncertainty over missing information, and experimentally show that Fast-PPCP provides substantial gain in runtime over other approaches while incurring little loss in solution quality.

We consider the task of autonomously unloading boxes from trucks using an industrial manipulator robot. There are multiple challenges that arise: (1) real-time motion planning for a complex robotic system carrying two articulated mechanisms, an arm and a scooper, (2) decision-making in terms of what action to execute next given imperfect information about boxes such as their masses, (3) accounting for the sequential nature of the problem where current actions affect future state of the boxes, and (4) real-time execution that interleaves high-level decision-making with lower level motion planning. In this work, we propose a planning, learning, and reasoning framework to tackle these challenges, and describe its components including motion planning, belief space planning for offline learning, online decision-making based on offline learning, and an execution module to combine decision-making with motion planning. We analyze the performance of the framework on real-world scenarios. In particular, motion planning and execution modules are evaluated in simulation and on a real robot, while offline learning and online decision-making are evaluated in simulated real-world scenarios.

Fully autonomous vehicles are expected to share the road with less advanced vehicles for a significant period of time. Furthermore, an increasing number of vehicles on the road are equipped with a variety of low-fidelity sensors which provide some perception and localization data, but not at a high enough quality for full autonomy. In this paper, we develop a perception and localization system that allows a vehicle with low-fidelity sensors to incorporate high-fidelity observations from a vehicle in front of it, allowing both vehicles to operate with full autonomy. The resulting system generates perception and localization information that is both low-noise in regions covered by high-fidelity sensors and avoids false negatives in areas only observed by low-fidelity sensors, while dealing with latency and dropout of the communication link between the two vehicles. At its core, the system uses a set of Extended Kalman filters which incorporate observations from both vehicles’ sensors and extrapolate them using information about the road geometry. The perception and localization algorithms are evaluated both in simulation and on real vehicles as part of a full cooperative driving system.

Use of physics-based simulation as a planning model enables a planner to reason and generate plans that involve non-trivial interactions with the world. For example, grasping a milk container out of a cluttered refrigerator may involve moving a robot manipulator in between other objects, pushing away the ones that are moveable and avoiding interactions with certain fragile containers. A physics-based simulator allows a planner to reason about the effects of interactions with these objects and to generate a plan that grasps the milk container successfully. The use of physics-based simulation for planning however is underutilized. One of the reasons for it being that physics-based simulations are typically way too slow for being used within a planning loop that typically requires tens of thousands of actions to be evaluated within a matter of a second or two. In this work, we develop a planning algorithm that tries to address this challenge. In particular, it builds on the observation that only a small number of actions actually need to be simulated using physics, and the remaining set of actions, such as moving an arm around obstacles, can be evaluated using a much simpler internal planning model, e.g., a simple collision-checking model. Motivated by this, we develop an algorithm called Planning with Selective Physics-based Simulation that automatically discovers what should be simulated with physics and what can utilize an internal planning model for pick-and-place tasks.

Traditional planning and control methods could fail to find a feasible trajectory for an autonomous vehicle to execute amongst dense traffic on roads. This is because the obstacle-free volume in spacetime is very small in these scenarios for the vehicle to drive through. However, that does not mean the task is infeasible since human drivers are known to be able to drive amongst dense traffic by leveraging the cooperativeness of other drivers to open a gap. The traditional methods fail to take into account the fact that the actions taken by an agent affect the behaviour of other vehicles on the road. In this work, we rely on the ability of deep reinforcement learning to implicitly model such interactions and learn a continuous control policy over the action space of an autonomous vehicle. The application we consider requires our agent to negotiate and open a gap in the road in order to successfully merge or change lanes. Our policy learns to repeatedly probe into the target road lane while trying to find a safe spot to move in to. We compare against two model-predictive control-based algorithms and show that our policy outperforms them in simulation. As part of this work, we introduce a benchmark for driving in dense traffic for use by the community.

Pose estimation of known objects is fundamental to tasks such as robotic grasping and manipulation. The need for reliable grasping imposes stringent accuracy requirements on pose estimation in cluttered, occluded scenes in dynamic environments. Modern methods employ large sets of training data to learn features in order to find correspondence between 3D models and observed data. However these methods require extensive annotation of ground truth poses. An alternative is to use algorithms that search for the best explanation of the observed scene in a space of possible rendered scenes. A recently developed algorithm, PERCH (PErception Via SeaRCH) does so by using depth data to converge to a globally optimum solution using a search over a specially constructed tree. While PERCH offers strong guarantees on accuracy, the current formulation suffers from low scalability owing to its high runtime. In addition, the sole reliance on depth data for pose estimation restricts the algorithm to scenes where no two objects have the same shape. In this work, we propose PERCH 2.0, a novel perception via search strategy that takes advantage of GPU acceleration and RGB data. We show that our approach can achieve a speedup of 100x over PERCH, as well as better accuracy than the state-of-the-art data-driven approaches on 6-DoF pose estimation without the need for annotating ground truth poses in the training data. Our code and video are available at https://sbpl-cruz.github.io/perception/.

Heuristic-based graph search algorithms like A* are frequently used to solve motion planning problems in many domains. For most practical applications, it is infeasible and unnecessary to pre-compute the graph representing the whole search space. Instead, these algorithms generate the graph incrementally by applying a fixed set of actions (frequently called motion primitives) to find the successors of every node that they need to evaluate. In many domains, it is possible to define actions (called adaptive motion primitives) that are not pre-computed but generated on the fly. The generation and validation of these adaptive motion primitives is usually quite expensive compared to pre-computed motion primitives. However, they have been shown to drastically speed up search if used judiciously. In prior work, ad hoc techniques like fixed thresholds have been used to limit unsuccessful evaluations of these actions. In this paper, we propose a learning-based approach to make more intelligent decisions about when to evaluate them. We do a thorough empirical evaluation of our model on a 3 degree-of-freedom (dof) motion planning problem for navigation using the Reeds-Shepp path as an adaptive motion primitive. Our experiments show that using our approach in conjunction with search algorithms leads to over 2x speedup in planning time.

Heuristic search-based planning techniques are commonly used for motion planning on discretized spaces. The performance of these algorithms is heavily affected by the resolution at which the search space is discretized. Typically a fixed resolution is chosen for a given domain. While a finer resolution allows better maneuverability, it exponentially increases the size of the state space, and hence demands more search efforts. On the contrary, a coarser resolution gives a fast exploratory behavior but compromises on maneuverability and the completeness of the search. To effectively leverage the advantages of both high and low resolution discretizations, we propose Multi-Resolution A* (MRA*) algorithm, that runs multiple weighted-A*(WA*) searches with different resolution levels simultaneously and combines the strengths of all of them. In addition to these searches, MRA* uses one anchor search to control expansions of other searches. We show that MRA* is bounded suboptimal with respect to the anchor resolution search space and resolution complete. We performed experiments on several motion planning domains including 2D, 3D grid planning and 7 DOF manipulation planning and compared our approach with several search-based and sampling-based baselines.

Sampling-based approaches are often favored in robotics for high-dimensional motion planning for their fast exploration of the search space. However, at best they offer asymptotic guarantees on solution quality due to their inherent stochasticity. While planning, the majority of effort is often spent near the start and goal configurations with a large amount of free space in between. Bidirectional approaches such as RRT-Connect exploit this fact by greedily extending and connecting search frontiers that simultaneously propagate from the start and goal configurations of a planning problem. In this work, we use such an extend operator for bidirectional heuristic search-based planners, which typically struggle with high-dimensionality. In doing so, we address the difficulty that these bidirectional planners face with connecting frontiers of both search efforts while providing suboptimality bounds on solution quality. We validate our simple approach on high-dimensional manipulation tasks, demonstrating significantly reduced search effort when compared against other popular bidirectional algorithms, both search-based and sampling. Our algorithm maintains theoretical guarantees on suboptimality and completeness for a given resolution. In addition, the solutions found by our planner are of higher quality compared to those found by the other baseline algorithms.

Search-based planning for relatively low-dimensional motion-planning problems such as for autonomous navigation and autonomous flight has been shown to be very successful. Such framework relies on laying a grid over a state-space and constructing a set of actions (motion primitives) that connect the centers of cells. However, in some cases such as kinodynamic motion planning, planning for bipedal robots with high balance requirements, computing these actions can be highly non-trivial and often impossible depending on the dynamic constraints. In this paper, we explore a soft version of discretization, wherein the state-space remains to be continuous but the search tries to avoid exploring states that are likely to be duplicates of states that have already been explored. We refer to this property of the search as soft duplicate detection and view it as a relaxation of the standard notion of duplicate detection. Empirically, we show that the search can efficiently compute paths in highly-constrained settings and outperforms alternatives on several domains.

We are interested in the problem of providing intuitive instructions for human agents to enable reliable navigation in unknown environments. Since the advent of GPS and digital maps, a common approach is to visually provide a planned path on a digital map defined in terms of actions to take at specific junctions. However, this approach relies on the agent to constantly and accurately localize itself. Furthermore, it comes in stark contrast to the way humans provide instructions—by leveraging known landmarks in the environment to both augment the description of the planned path as well as to allow to detect when the agent deviated from the planned path. Hence, there is need for assurable means of localization, an intuitive way of compactly conveying directions to agents and a systematic approach to account for human errors. To this end, our key insight is to employ known landmarks in the environment to overcome these challenges. We formally model this intuitive way to use landmarks for conveying instructions and for creating contingency plans. We present experiments demonstrating the efficacy of our approach both on synthetic environments as well as on realworld maps, computed using a smart-phone iOS application that we developed.

Designing good heuristic functions for graph search requires adequate domain knowledge. It is often easy to design heuristics that perform well and correlate with the underlying true cost-to-go values in certain parts of the search space but these may not be admissible throughout the domain thereby affecting the optimality guarantees of the search. Bounded suboptimal search using several of such partially good but inadmissible heuristics was developed in Multi-Heuristic A* (MHA*). Although MHA* leverages multiple inadmissible heuristics to potentially generate a faster suboptimal solution, the original version does not improve the solution over time. It is an one shot algorithm that requires careful setting of inflation factors to obtain a desired one time solution. In this work, we tackle this issue by extending MHA* to an anytime version that finds a feasible suboptimal solution quickly and continually improve it until time runs out. Our work is inspired from the Anytime Repairing A* (ARA*) algorithm. We prove that our precise adaptation of ARA* concepts in the MHA* framework preserves the original suboptimal and completeness guarantees and enhances MHA* to perform in an anytime fashion. Furthermore, we report the performance of A-MHA* in 3-D path planning domain and sliding tiles puzzle and compare against MHA* and other anytime algorithms.

In this work, we present an approach to planning for humanoid mobility. Humanoid mobility is a challenging problem, as the configuration space for a humanoid robot is intractably large, especially if the robot is capable of performing many types of locomotion. For example, a humanoid robot may be able to perform such tasks as bipedal walking, crawling, and climbing. Our approach is to plan for all these tasks within a single search process. This allows the search to reason about all the capabilities of the robot at any point, and to derive the complete solution such that the plan is guaranteed to be feasible. A key observation is that we often can roughly decompose a mobility task into a sequence of smaller tasks, and focus planning efforts to reason over much smaller search spaces. To this end, we leverage the results of a recently developed framework for planning with adaptive dimensionality, and incorporate the capabilities of available controllers directly into the planning process. The resulting planner can also be run in an interleaved fashion alongside execution so that time spent idle is much reduced.

We consider the problem of planning a collision-free path for a high-dimensional robot. Specifically, we suggest a planning framework where a motion-planning algorithm can obtain guidance from a user. In contrast to existing approaches that try to speed up planning by incorporating experiences or demonstrations ahead of planning, we suggest to seek user guidance only when the planner identifies that it ceases to make significant progress towards the goal. Guidance is provided in the form of an intermediate configuration q^, which is used to bias the planner to go through q^. We demonstrate our approach for the case where the planning algorithm is Multi-Heuristic A* (MHA*) and the robot is a 34-DOF humanoid. We show that our approach allows to compute highly-constrained paths with little domain knowledge. Without our approach, solving such problems requires carefully-crafted domain-dependent heuristics.

Both Optimal Control and Search-based Planning are used extensively for path planning and have their own set of advantages and disadvantages. In this paper, we propose an algorithm FOCS (Fusion of Optimal Control and Search) that combines these two classes of approaches together. FOCS finds a path exploiting the advantages of both approaches while providing a bound on the sub-optimality of its solution. The returned path is a concatenation of the path found in the implicit graph constructed by search and the path generated by following the negative gradient of the value function obtained as a solution of the Hamilton-Jacobi-Bellman equation. We analyze the algorithm and illustrate its effectiveness in finding a minimum-time path for a car-like vehicle in different environments.

Intervention Autonomous Underwater Vehicle or I-AUV has recently started to grab researchers attention in the last 20 years. Only three I-AUVs have demonstrated autonomous manipulation skills: ALIVE, SAUVIM and GIRONA 500. While prior systems rely on variations of the task-priority redundancy control framework, our recent research showed preliminary results using motion planning for floating-based intervention in the presence of obstacles. With the increasing need for autonomously performing more complex manipulation tasks, two main challenges need to be addressed: the high-dimensionality of the system, and the motion coordination between the mobile base and the working arm. The latter challenge is of high importance if accurate execution is required, especially considering the floating nature of the AUV and the control challenges that come with it. Our approach relies on exploiting the loose coupling between the AUV and the arm. In particular we present an approach based on MR-MHA * (Multi-Representation, Multi-Heuristic A*), and we show how it can generate efficient trajectories by exploiting decoupling. We show for the first time the use of a search-based planner on a high-dimensional underwater manipulator. In addition, we support our claims with experimental analysis of the generated trajectories with respect to various metrics in different environments. Furthermore, we demonstrate the ability of our approach to conduct a full intervention mission in a realistic simulated underwater intervention environment.

Parts assembly, in a broad sense, is to make multiple objects to be in specific relative poses in contact with each other. One of the major reasons that make it difficult is uncertainty. Because parts assembly involves physical contact between objects, it requires higher precision than other manipulation tasks like collision avoidance. The key idea of this paper is to use simulation-aided physical reasoning while planning with the goal of finding a robust motion plan for parts assembly. Specifically, in the proposed approach, a) uncertainty between object poses is represented as a distribution of particles, b) the motion planner estimates the transition of particles for unit actions (motion primitives) through physics-based simulation, and c) the performance of the planner is sped up using Multi-Heuristic A* (MHA*) search that utilizes multiple inadmissible heuristics that lead to fast uncertainty reduction. To demonstrate the benefits of our framework, motion planning and physical robot experiments for several parts assembly tasks are provided.

A fundamental robot perception task is that of identifying and estimating the poses of objects with known 3D models in RGB-D data. While feature-based and discriminative approaches have been traditionally used for this task, recent work on deliberative approaches such as PERCH and D2P have shown improved robustness in handling scenes with severe inter-object occlusions. These deliberative approaches work by treating multi-object pose estimation as a combinatorial search over the space of possible rendered scenes of the objects, thereby inherently being able to predict and account for occlusions. However, these methods have so far been restricted to scenes comprising only of known objects, and have been unable to handle extraneous clutter — a common occurrence in many real-world settings. This work significantly increases the practical relevance of deliberative perception methods by developing a formulation that: i) accounts for extraneous unmodeled clutter in scenes, and ii) provides object pose uncertainty estimates. Our algorithm is complete and provides bounded suboptimality guarantees for the cost function chosen to be optimized. Empirically, we demonstrate successful object recognition and uncertainty-aware localization in challenging scenes with unmodeled clutter, where previous deliberative methods perform unsatisfactorily. In addition, this work was used as part of the perception system by Carnegie Mellon University's Team HARP in the 2016 Amazon Picking Challenge.

The efficiency of heuristic search depends dramatically on the quality of the heuristic function. For an optimal heuristic search, heuristics that estimate cost-to-goal better typically lead to faster searches. For a sub-optimal heuristic search such as weighted A*, the search speed depends more on the correlation between the heuristic and the true cost-to-goal. In this extended abstract, we discuss our preliminary work on computing heuristic functions that exploit this fact. In particular, we introduce a many-to-one mapping from an original search space to a conservative abstract space. Edges in the abstract space capture reachability among all corresponding nodes in the original space. We compute a heuristic in the conservative abstract space which when used by the search in the original space reduces the number of searched nodes. Our preliminary results on 3D navigation show that in more complex scenarios the speedup can be dramatic.

Algorithms for fixed-wing unmanned aerial systems (UAS) must integrate on-board sensor capabilities and vehicle maneuver constraints to reliably satisfy the objectives of persistent surveillance, path planning, and trajectory management. In many cases, the characteristic dimensions of sensor fields of view are comparable with the turning radius of the UAS platform. Consequently, when persistent, full area, time-critical coverage is required and the number of assets is limited, the complexity of path planning is increased, as the turn radius becomes comparable with or exceeds the sensor footprint. A technique is developed to integrate persistent surveillance mission requirements with sensor resolution and field-of-view to facilitate efficient path planning. Graph search techniques and spline-based methods are combined to develop computationally simple algorithms that converge to feasible paths with $G^{2}$ continuity. ( $G^{2}$ continuity in this context means the path, which is constructed from a concatenated series of $C^{2}$ curve segments, and its derivatives are continuous through two derivatives, but the magnitudes of second derivatives may differ at the boundaries, where these curve segments are joined to form the entire path. Curvature is continuous throughout.) Two example cases are provided. In the first, the turning radius is small in comparison with the sensor footprint, while in the second, the minimum turn radius is a critical parameter in the determination of a feasible path.

Robots are increasingly being used in situations such as search and rescue that require robust navigation capabilities, potentially in areas with little or no GPS or other high-quality localization information. As more robots are used in these scenarios, it becomes viable to collaborate between heterogeneous types of robots to leverage their individual strengths while minimizing their weaknesses. More specifically, in a scenario involving unmanned ground and aerial vehicles (UGV, UAV), the ground robot can contribute its high payload capacity to provide computational resources and high accuracy sensors while the aerial robot can bring its high mobility and capability to traverse obstacles to the team. However, in order for the team to benefit from these capabilities, it must be capable of generating a plan for both robots that allows them to collaboratively localize when necessary. Our approach to this problem is to combine a recently developed state lattice planner using controller-based motion primitives (SLC) with planning using adaptive dimensionality (PAD). The SLC planner allows for robust navigation using a wide variety of sensors including in areas with no or limited high-quality localization information while the PAD planner allows us to expand beyond a single robot and generate plans for a team of robots operating in a high dimensional space. We present our results to this combined approach for a UGV/UAV team operating indoors in areas with limited visual features.

The benefits of bidirectional planning over the unidirectional version are well established for motion planning in high-dimensional configuration spaces. While bidirectional approaches have been employed with great success in the context of sampling-based planners such as in RRT-Connect, they have not enjoyed popularity amongst search-based methods such as A*. The systematic nature of search-based algorithms, which often leads to consistent and high-quality paths, also enforces strict conditions for the connection of forward and backward searches. Admissible heuristics for the connection of forward and backward searches have been developed, but their computational complexity is a deterrent. In this work, we leverage recent advances in search with inadmissible heuristics to develop an algorithm called A*-Connect, much in the spirit of RRT-Connect. A*-Connect uses a fast approximation of the classic front-to-front heuristic from literature to lead the forward and backward searches towards each other, while retaining theoretical guarantees on completeness and bounded suboptimality. We validate A*-Connect on manipulation as well as navigation domains, comparing with popular sampling-based methods as well as state-of-the-art bidirectional search algorithms. Our results indicate that A*-Connect can provide several times speedup over unidirectional search while maintaining high solution quality.

In a cluttered scene, an object is often occluded by other objects, and a robot cannot figure out what the object is and perceive its pose exactly. We assume that the robot is equipped with a depth sensor and given a database of 3D object models and their grasping poses, but yet there is uncertainty about object's class and pose. In this paper, we study the problem of how to predict the class and pose of an occluded object by carefully taking a sequence of observations. To find the best sequence of viewpoints by the robot, we construct hypotheses of the states of the target and occluding objects, and update our belief state as new observations come in. Every time selecting the next robot pose, we greedily choose the one that is expected to reduce the uncertainty the most. Based on the theoretical analysis of adaptive submodular maximization problems, this process is guaranteed to find a near-optimal sequence of robot poses in terms of observation and traverse costs. To validate the proposed method, we present simulation and robot experiments using a PR2.

In many robotic domains such as flexible automated manufacturing or personal assistance, a fundamental perception task is that of identifying and localizing objects whose 3D models are known. Canonical approaches to this problem include discriminative methods that find correspondences between feature descriptors computed over the model and observed data. While these methods have been employed successfully, they can be unreliable when the feature descriptors fail to capture variations in observed data; a classic cause being occlusion. As a step towards deliberative reasoning, we present PERCH: PErception via SeaRCH, an algorithm that seeks to find the best explanation of the observed sensor data by hypothesizing possible scenes in a generative fashion. Our contributions are: i) formulating the multi-object recognition and localization task as an optimization problem over the space of hypothesized scenes, ii) exploiting structure in the optimization to cast it as a combinatorial search problem on what we call the Monotone Scene Generation Tree, and iii) leveraging parallelization and recent advances in multi-heuristic search in making combinatorial search tractable. We prove that our system can guaranteedly produce the best explanation of the scene under the chosen cost function, and validate our claims on real world RGB-D test data. Our experimental results show that we can identify and localize objects under heavy occlusion— cases where state-of-the-art methods struggle.

Many robot applications involve lifelong planning in relatively static environments e.g. assembling objects or sorting mail in an office building. In these types of scenarios, the robot performs many tasks over a long period of time. Thus, the time required for computing a motion plan becomes a significant concern, prompting the need for a fast and efficient motion planner. Since these environments remain similar in between planning requests, planning from scratch is wasteful. Recently, Experience Graphs (E-Graphs) were proposed to accelerate the planning process by reusing parts of previously computed paths to solve new motion planning queries more efficiently. This work describes a method to improve planning times with E-Graphs given changes in the environment by lazily evaluating the validity of past experiences during the planning process. We show the improvements with our method in a single-arm manipulation domain with simulations on the PR2 robot.

Many motion planning problems in robotics are high dimensional planning problems. While sampling-based motion planning algorithms handle the high dimensionality very well, the solution qualities are often hard to control due to the inherent randomization. In addition, they suffer severely when the configuration space has several ‘narrow passages’. Search-based planners on the other hand typically provide good solution qualities and are not affected by narrow passages. However, in the absence of a good heuristic or when there are deep local minima in the heuristic, they suffer from the curse of dimensionality. In this work, our primary contribution is a method for dynamically generating heuristics, in addition to the original heuristic(s) used, to guide the search out of local minima. With the ability to escape local minima easily, the effect of dimensionality becomes less pronounced. On the theoretical side, we provide guarantees on completeness and bounds on suboptimality of the solution found. We compare our proposed method with the recently published Multi-Heuristic A* search, and the popular RRT-Connect in a full-body mobile manipulation domain for the PR2 robot, and show its benefits over these approaches.

Personal robots need to manipulate a variety of articulated mechanisms as part of day-to-day tasks. These tasks are often specific, goal-driven, and permit very little bootstrap time for learning the articulation type. In this work, we address the problem of purposefully manipulating an articulated object, with uncertainty in the type of articulation. To this end, we provide two primary contributions: first, an efficient planning algorithm that, given a set of candidate articulation models, is able to correctly identify the underlying model and simultaneously complete a task; and second, a representation for articulated objects called the Generalized Kinematic Graph (GK-Graph), that allows for modeling complex mechanisms whose articulation varies as a function of the state space. Finally, we provide a practical method to auto-generate candidate articulation models from RGB-D data and present extensive results on the PR2 robot to demonstrate the utility of our representation and the efficiency of our planner.

Experience Graphs have been shown to accelerate motion planning using parts of previous paths in an A* framework. Experience Graphs work by computing a new heuristic for weighted A* search on top of the domain's original heuristic and the edges in an Experience Graph. The new heuristic biases the search toward relevant prior experience and uses the original heuristic for guidance otherwise. In previous work, Experience Graphs were always built on top of domain heuristics which were computed by dynamic programming (a lower dimensional version of the original planning problem). When the original heuristic is computed this way the Experience Graph heuristic can be computed very efficiently. However, there are many commonly used heuristics in planning that are not computed in this fashion, such as euclidean distance. While the Experience Graph heuristic can be computed using these heuristics, it is not efficient, and in many cases the heuristic computation takes much of the planning time. In this work, we present a more efficient way to use these heuristics for motion planning problems by making use of popular nearest neighbor algorithms. Experimentally, we show an average 8 times reduction in heuristic computation time, resulting in overall planning time being reduced by 66%. with no change in the expanded states or resulting path.

Learning from demonstration (LfD) is a common technique applied to many problems in robotics, such as populating grasp databases, training for reinforcement learning of high-level skill sets and bootstrapping motion planners. While such approaches are generally highly valued, they rely on the often time-consuming process of gathering user demonstrations, and hence it becomes difficult to attain a sizeable dataset. In this paper, we present a tool capable of recording large numbers of high-dimensional demonstrations of mobile manipulation tasks provided by non-experts in the field. Our tool accomplishes this via a web interface that requires no additional software to be installed beyond a web browser, as well as a scalable architecture that is capable of supporting 10 concurrent demonstrators on a single server. Our architecture employs a lightweight simulation environment to reduce unnecessary computations and improve performance. Furthermore, we show how our tool can be used to gather a large set of demonstrations of a mobile manipulation task by leveraging existing crowdsource platforms. The data set collected has been made available to the robotics community. We also present experiments in which we apply demonstrations collected through our infrastructure to teach a robot how to grasp, to teach a robot how to perform dexterous manipulation tasks such as scooping and to accelerate motion planning for full-body manipulation tasks.

Follow-the-leader based approaches have been popular for the control of multi-robot teams for their ability to drive many with few. Typically, in these methods you select a single leader, generate a plan for it, while all other agents follow this leader using their individual controllers. However, there are many scenarios where this approach can lead to highly suboptimal behavior or even failure in the presence of clutter. In this work, we present a planning approach that automatically figures out when to switch leaders on the way to the goal while minimizing a given cost function that penalizes leader switching and deviations from the desired formation. To deal with the increased dimensionality of the problem we show how a recently developed algorithm, MHA* (multi-heuristic A*) can be extended to support planning for a team of robots. We also provide explicit cost minimization and guarantee that paths found are within a user- chosen factor of optimality with respect to the graph modeling the planning problem. Experimentally, we found that allowing for dynamic leader-switching leads to a significant increase in finding feasible plans for multi-robot teams ranging up to 21 robots.

Exploration of unknown environments is an important aspect to fielding teams of robots. Without the ability to determine on their own where to go in the environment, the full potential of robotic teams is limited to the abilities of human operators to deploy them for search and rescue, mapping, or other tasks that are predicated on gaining knowledge from the environment. This is of particular importance in real-world 3-Dimensional (3-D) environments where simple planar assumptions can lead to incomplete exploration, for example, real-world environments have areas underneath overhangs or inside caves. As an additional challenge, when the teams of robots have vastly different capabilities, the planning system must take those into account to efficiently utilize the available assets. In this paper, we present a combined air-ground system for conducting 3-D exploration in cluttered environments. We first describe the hardware and software components of the system. We then present our algorithm for planning 3-D goal locations for a heterogeneous team of robots to efficiently explore a previously unknown environment and demonstrate its applicability in real-world experiments.

In this paper, we solve the path planning problem for a tethered mobile robot, which is connected to a fixed base by a cable of length L. The reachable space of the robot is restricted by the length of the cable and obstacles. The reachable space of the tethered robot can be computed by considering the topology class of the cable. However, it is computationally too expensive to compute this space a-priori. Instead, in this paper, we show how we can plan using a recently-developed variant of A* search, called Multi-Heuristic A*. Normally, the Multi-Heuristic A* algorithm takes in a fixed set of heuristic functions. In our problem, however, the heuristics represent length of paths to the goal along different topology classes, and there can be too many of them and not all the topology classes are useful. To deal with this, we adapt Multi-Heuristic A* to work with a dynamically generated set of heuristic functions. It starts out as a normal weighted A*. Whenever the search gets trapped in a local minimum, we find the proper topology class of the path to escape from it and add the corresponding new heuristic function into the set of heuristic functions considered by the search. We present experimental analysis comparing our approach with weighted A* on planning for a tethered robot in simulation.

Learning from experience can significantly improve the performance of search based planners, especially for challenging problems like high-dimensional planning. Experience Graph (E-Graph) is a recently developed framework that encodes experiences, obtained from solving instances in the past, into a single bounded-admissible heuristic, and uses it to guide the search. While the E-Graph approach was shown to be very useful for repetitive problems, it suffers from two issues. First, computing the E-Graph heuristic is time consuming as it maintains the bounded admissibility constraints. Second, a single heuristic can get stuck in a local minimum, and thereby, degrade the performance. In this work, we present an alternative approach to improving the runtime of search from experience, based on a recently developed search algorithm Multi-heuristic A* (MHA*). This framework provides an improvement over the E-Graph planner for two reasons: a) MHA* uses multiple heuristics simultaneously to explore the search space, which reduces the probability of getting stuck in a local minimum, and b) the heuristics in MHA* can be arbitrarily inadmissible, which makes it very easy to compute them. The paper describes the framework, explains how to compute these (inadmissible) heuristics through offline and online processing and presents experimental analysis on two domains, motion planning for a 6D planar arm and large sliding tile puzzles.

Many robotic systems are comprised of two or more arms. Such systems range from dual-arm household manipulators to factory floors populated with a multitude of industrial robotic arms. While the use of multiple arms increases the productivity of the system and extends dramatically its workspace, it also introduces a number of challenges. One such challenge is planning the motion of the arm(s) required to relocate an object from one location to another. This problem is challenging because it requires reasoning over which arms and in which order should manipulate the object, finding a sequence of valid handoff locations between the consecutive arms and finally choosing the grasps that allow for successful handoffs. In this paper, we show how to exploit the characteristics of this problem in order to construct a planner that can solve it effectively. We analyze our approach experimentally on a number of simulated examples ranging from a 2-arm system operating at a table to a 3-arm system working at a bar and to a 4-arm system in a factory setting.

Recently, several researchers have brought forth the benefits of searching with multiple (and possibly inadmissible) heuristics, arguing how different heuristics could be independently useful in different parts of the state space. However, algorithms that use inadmissible heuristics in the traditional best-first sense, such as the recently developed Multi-Heuristic A* (MHA*), are subject to a crippling calibration problem: they prioritize nodes for expansion by additively combining the cost-to-come and the inadmissible heuristics even if those heuristics have no connection with the cost-to-go (e.g., the heuristics are uncalibrated) . For instance, if the inadmissible heuristic were an order of magnitude greater than the perfect heuristic, an algorithm like MHA* would simply reduce to a weighted A* search with one consistent heuristic. In this work, we introduce a general multi-heuristic search framework that solves the calibration problem and as a result a) facilitates the effective use of multiple uncalibrated inadmissible heuristics, and b) provides significantly better performance than MHA* whenever tighter sub-optimality bounds on solution quality are desired. Experimental evaluations on a complex full-body robotics motion planning problem and large sliding tile puzzles demonstrate the benefits of our framework.

Advanced modern humanoid robots often have complex manipulators with a large number of degrees of freedom. Thus, motion planning for such manipulators is a very computationally challenging problem. However, often robotic manipulators allow the wrist degrees of freedom to be controlled independently from the configuration of the rest of the arm. In this paper we show how to split the high dimensional planning problem into two lower-dimensional sub-problems - planning for the main arm joints and planning for the wrist joints, without losing guarantees on completeness. This approach is an extension of our previously developed framework for planning with adaptive dimensionality. Experimentally, we show that this approach is very effective in speeding up planning for robotic arms on Willow Garage's PR2 platform. We compare our algorithm with several popular alternative approaches for performing motion planning for robotic arms. The results we observe illustrate that our algorithm provides a good balance between planning time, planning success rate, path consistency, and path quality.

Kinematic planning for robotic arms has been shown to be capable of planning for robust manipulation of static objects. However, this approach falls short when manipulating moving objects such as picking up a jar off of a conveyor belt at a bottling plant. The challenge in carefully picking up moving objects is that these actions require motions that do not involve large decelerations, to avoid jerking the object, as well as figuring out the proper time in which the object can be picked up. We present a search-based kinodynamic motion planning algorithm that generates a time-parameterized trajectory for both the arm and end-effector, capable of carefully picking up the object at the earliest feasible point in its trajectory. To combat the high-dimensionality of the time-parameterized kinodynamic planning problem, our approach employs informative heuristics and adaptive dynamic motion primitives. To validate our approach, we used a 7DOF manipulator on Willow Garage's PR2 robot to pickup objects off of a conveyor belt. We also provide a detailed set of results that demonstrate the planner's ability to generate consistent, low cost trajectories for manipulation.

State lattice-based planning has been used in navigation for ground, water, aerial and space robots. State lattices are typically constructed of simple motion primitives connecting one state to another. There are situations where these metric motions may not be available, such as in GPS-denied areas. In many of these cases, however, the robot may have some additional sensing capability that is not being fully utilized by the planner. For example, if the robot has a camera it may be able to use simple visual servoing techniques to navigate through a GPS-denied region. Likewise, a LIDAR may allow the robot to skirt along an environmental feature even if there is not enough information to generate an accurate pose estimate. In this paper we present an expansion of the state lattice framework that allows us to incorporate controller-based motion primitives and external perceptual triggers directly into the planning process. We provide a formal description of our method of constructing the search graph in these cases as well as presenting real-world and simulated testing data showing the practical application of this approach.

When one uses a hand-held tool, the fingers often make the tool to be in contact with the palm in the form of multi-contact manipulation. Multi-contact manipulation is useful for object-environment interaction tasks because it can provide both powerful grasping of the object body and dexterous manipulation of the object end-effector. However, dealing with the internal link contact with the object is not trivial. In this paper, we propose Reactional Internal Contact Hypothesis that regards the internal contact force as a reaction force so that the desired finger force can be reduced. By taking a handwriting task as an example, optimal configuration search and grasping force computation problems are addressed based on this hypothesis and validated via dynamic simulation.

Crowd activities are often randomized to create the appearance of heterogeneity. However, the parameters that control randomization are frequently hard to tune because it is unclear how changes at the character level affect the high-level appearance of the crowd. We propose a method for computing randomization parameters that supports direct animator control. Given details about the environment, available activities, timing information and the desired high-level appearance of the crowd, we model the problem as a graph, formulate a convex optimization problem, and solve for a set of stochastic transition rates which satisfy the constraints. Unlike the use of heuristics for adding randomness to crowd activities, our approach provides guarantees on convergence to the desired result, allows for decentralized simulation, and supports a variety of constraints. In addition, because the rates can be pre-computed, no additional runtime processing is needed during simulation.

We present a novel heuristic search framework, called Multi-Heuristic A* (MHA*), that simultaneously uses multiple, arbitrarily inadmissible heuristic functions and one consistent heuristic to search for complete and bounded suboptimal solutions. This simplifies the de- sign of heuristics and enables the search to effectively combine the guiding powers of different heuristic func- tions. We support these claims with experimental results on full-body manipulation for PR2 robots.

Incremental graph search methods reuse information from previous searches in order to minimize redundant computation and to find solutions to series of similar search queries much faster than it is possible by solving each query from scratch. In this work, we present a simple, but very effective, technique for performing incremental weighted A* graph search in an anytime fashion. On the theoretical side, we show that our anytime incremental algorithm preserves the strong theoretical guarantees provided by the weighted A* algorithm, such as completeness and bounds on solution cost sub-optimality. We also show that our algorithm can handle a variety of changes to the underlying graph, such as both increasing and decreasing edge costs, and changes in the heuristic. On the experimental side, we demonstrate the effectiveness of our algorithm in the context of (x, y, z, yaw) navigation planning for an unmanned aerial vehicle and compare our algorithm to popular incremental and anytime graph search algorithms.

Incremental heuristic searches try to reuse their previous search efforts whenever these are available. As a result, they can often solve a sequence of similar planning problems much faster than planning from scratch. State-of-the-art incremental heuristic searches such as LPA*, D* and D* Lite all work by propagating cost changes to all the states on the search tree whose g-values (the costs of computed paths from the start) are no longer optimal. While such a complete propagation of cost changes is required to ensure optimality, the propagations can be stopped much earlier if we are looking for solutions within a given suboptimality bound. We present a framework called Truncated Incremental Search that builds on this observation, and uses a target suboptimality bound to efficiently restrict the cost propagations. Using this framework, we develop two algorithms, Truncated LPA* (TLPA*) and Truncated D* Lite (TD* Lite). We discuss their analytical properties and present experimental results for 2D and 3D (x, y, heading) path planning that show significant improvement in runtime over existing incremental heuristic searches when searching for close-to-optimal solutions. In addition, unlike typical incremental searches, Truncated Incremental Search is much less dependent on the proximity of the cost changes to the goal of the search due to the early termination of the cost change propagation.

Opening and navigating through doors remains a challenging problem, particularly in cluttered environments and for spring-loaded doors. Passing through doors, especially spring-loaded doors, requires making and breaking contacts with the door and preventing the door from closing while passing through. In this work, we present a planning framework that handles non-spring and spring-loaded doors, in cluttered or confined workspaces, planning the approach to the door, pushing or pulling it open, and passing through. Because the problem is solved in a combined search space, the planner yields an overall least-cost path. The planner is able to insert a transition between robot-door contacts at any point along the plan. We utilize a compact graph-based representation of the problem to keep planning times low. We precompute the force workspace of the end-effectors to eliminate checks against joint torque limits at plan time. We have validated our solution in both simulation and real-world experiments on the PR2 mobile manipulation platform; the robot is able to successfully open a variety of spring-loaded and non-spring-loaded doors by pushing and pulling.

Operating micro aerial vehicles (MAVs) outside of the bounds of a rigidly controlled lab environment, specifically one that is unstructured and contains unknown obstacles, poses a number of challenges. One of these challenges is that of quickly determining an optimal (or nearly so) path from the MAVs current position to a designated goal state. Past work in this area using full-size unmanned aerial vehicles (UAVs) has predominantly been performed in benign environments. However, due to their small size, MAVs are capable of operating in indoor environments which are more cluttered. This requires planners to account for the vehicle heading in addition to its spatial position in order to successfully navigate. In addition, due to the short flight times of MAVs along with the inherent hazards of operating in close proximity to obstacles, we desire the trajectories to be as cost-optimal as possible. Our approach uses an anytime planner based on A* that performs a graph search on a four-dimensional (4-D) (x,y,z, heading) lattice. This allows for the generation of close-to-optimal trajectories based on a set of precomputed motion primitives along with the capability to provide trajectories in real-time allowing for on-the-fly re-planning as new sensor data is received. We also account for arbitrary vehicle shapes, permitting the use of a non-circular footprint during the planning process. By not using the overly conservative circumscribed circle for collision checking, we are capable of successfully finding optimal paths through cluttered environments including those with narrow hallways. Analytically, we show that our planner provides bounds on the sub-optimality of the solution it finds. Experimentally, we show that the planner can operate in real-time in both a simulated and real-world cluttered environments.

We present a framework based on graph search for navigation in the plane with a variety of topological constraints. The method is based on modifying a standard graph-based navigation approach to keep an additional state variable that encodes topological information about the path. The topological information is represented by a sequence of virtual sensor beam crossings. By considering classes of beam crossing sequences to be equivalent under certain equivalence relations, we obtain a general method for planning with topological constraints that subsumes existing approaches while admitting more favorable representational characteristics. We provide experimental results that validate the approach and show how the planner can be used to find loop paths for autonomous surveillance problems, simultaneously satisfying minimum-cost objectives and in dynamic environments. As an additional application, we demonstrate the use of our planner on the PR2 robot for automated building of 3D object models.

This paper addresses the problem of planning in the presence of humans modeled as dynamic obstacles with multiple hypotheses on their trajectories and actions which can disambiguate between the hypotheses. To solve this problem, we develop and analyze a generalization to the PPCP (Probabilistic Planning with Clear Preferences) algorithm that allows us to efficiently solve problems with approximate preferences on missing information. The approach finds policies with bounded suboptimal expected cost and scales well with the number of people, only disambiguating between the trajectories of people when necessary. We present simulated results as well as experiments on two different physical robots demonstrating the capability of this planner.

Robots operating in real world environments need to find motion plans quickly. Robot motion should also be efficient and, when operating among people, predictable. Minimizing a cost function, e.g. path length, can produce short, reasonable paths. Anytime planners are ideal for this since they find an initial solution quickly and then improve solution quality as time permits. In previous work, we introduced the concept of Experience Graphs, which allow search-based planners to find paths with bounded sub-optimality quickly by reusing parts of previous paths where relevant. Here we extend planning with Experience Graphs to work in an anytime fashion so a first solution is found quickly using prior experience. As time allows, the dependence on this experience is reduced in order to produce closer to optimal solutions. We also demonstrate how Experience Graphs provide a new way of approaching incremental planning as they naturally reuse information when the environment, the starting configuration of the robot or the goal configuration change. Experimentally, we demonstrate the anytime and incremental properties of our algorithm on mobile manipulation tasks in both simulation and on a real PR2 robot.

Mobile manipulators have brought a new level of flexibility to traditional automation tasks such as tabletop manipulation, but are not yet capable of the same speed and reliability as industrial automation. We present approaches to 3D perception and manipulator motion planning that enable a general purpose robotic platform to recognize and manipulate a variety of objects at a rate of one pick-and-place operation every 6.7 s, and work with a conveyor belt carrying objects at a speed of 33cm/s.

We are interested in the multiple robot surveillance problem where robots must allocate waypoints to be visited among themselves and plan paths through different waypoints while avoiding obstacles. Furthermore, the robots are allocated specific times to reach their respective goal locations and as a result they have to decide which robots have to visit which waypoints. Such a problem has the challenge of computing the allocation of waypoints across robots, ordering for these waypoints and dynamical feasibility of the paths between waypoints. We present an algorithm that runs a series of graph searches to solve the problem and provide theoretical analysis that our approach yields an optimal solution. We present simulated results as well as experiments on two UAVs that validate the capability of our algorithm. For a single robot, we can solve instances having 10–15 waypoints and for multiple robots, instances having five robots and 10 waypoints can be solved.

Incremental heuristic searches reuse their previous search efforts to speed up the current search. Anytime search algorithms iteratively tune the solutions based on available search time. Anytime D* (AD*) is an incremental anytime search algorithm that combines these two approaches. AD* uses an inflated heuristic to produce bounded suboptimal solutions and improves the solution by iteratively decreasing the inflation factor. If the environment changes, AD* recomputes a new solution by propagating the new costs. Recently, a different approach to speed up replanning (TLPA*/TD* Lite) was proposed that relies on selective truncation of cost propagations instead of heuristic inflation. In this work, we present an algorithm called Anytime Truncated D* (ATD*) that combines heuristic inflation with truncation in an anytime fashion. We develop truncation rules that can work with an inflated heuristic without violating the completeness/suboptimality guarantees, and show how these rules can be applied in conjunction with heuristic inflation to iteratively refine the replanning solutions with minimal reexpansions. We explain ATD*, discuss its analytical properties and present experimental results for 2D and 3D (x, y, heading) path planning demonstrating its efficacy for anytime replanning.

Homotopy classes of trajectories, arising due to the presence of obstacles, are defined as sets of trajectories that can be transformed into each other by gradual bending and stretching without colliding with obstacles. The problem of exploring/finding the different homotopy classes in an environment and the problem of finding least-cost paths restricted to a specific homotopy class (or not belonging to certain homotopy classes) arises frequently in such applications as predicting paths for unpredictable entities and deployment of multiple agents for efficient exploration of an environment. In [Bhattacharya, Kumar, Likhachev, AAAI 2010] we have shown how homotopy classes of trajectories on a two-dimensional plane with obstacles can be classified and identified using the Cauchy Integral Theorem and the Residue Theorem from Complex Analysis. In more recent work [Bhattacharya, Likhachev, Kumar, RSS 2011] we extended this representation to three-dimensional spaces by exploiting certain laws from the Theory of Electromagnetism (Biot-Savart law and Ampere's Law) for representing and identifying homotopy classes in three dimensions in an efficient way. Using such a representation, we showed that homotopy class constraints can be seamlessly weaved into graph search techniques for determining optimal path constrained to certain homotopy classes or forbidden from others, as well as for exploring different homotopy classes in an environment. (This is a condensed, non-technical overview of work previously published in the proceedings of Robotics: Science and Systems, 2011 conference [Bhattacharya, Likhachev, Kumar, RSS 2011].)

In looking back on the last five to ten years of work in heuristic search a few trends emerge. First, there has been a broadening of research topics studied. Second, there has been a deepened understanding of the theoretical foundations of search. Third, and finally, there have been increased connections with work in other fields. This paper, corresponding to a AAAI 2012 invited talk on recent work in heuristic search, highlights these trends in a number of areas of heuristic search. It is our opinion that the sum of these trends reflects the growth in the field and the fact that heuristic search has come of age.

Dual-arm manipulation is an increasingly important skill for robots operating in home, retail and industrial environments. Dual-arm manipulation is especially essential for tasks involving large objects which are harder to grasp and manipulate using a single arm. In this work, we address dual-arm manipulation of objects in indoor environments. We are particularly focused on tasks that involve an upright orientation constraint on the grasped object. Such constraints are often present in human environments, e.g. when manipulating a tray of food or a container with fluids. In this paper, we present a search-based approach that is capable of planning dual-arm motions, often within one second, in cluttered environments while adhering to the orientation constraints. Our approach systematically constructs a graph in task space and generates motions that are consistent across runs with similar start/goal configurations and are low-cost. These motions come with guarantees on completeness and bounds on the suboptimality with respect to the graph that encodes the planning problem. For many problems, the consistency of the generated motions is important as it helps make the actions of the robot more predictable for a human interacting with the robot.

Mobile manipulation planning is a hard problem composed of multiple challenging sub-problems, some of which require searching through large high-dimensional state-spaces. The focus of this work is on computing a trajectory to safely maneuver an object through an environment, given the start and goal configurations. In this work we present a heuristic search-based deterministic mobile manipulation planner, based on our recently-developed algorithm for planning with adaptive dimensionality. Our planner demonstrates reasonable performance, while also providing strong guarantees on completeness and suboptimality bounds with respect to the graph representing the problem.

Path planning in dynamic environments with moving obstacles is computationally complex since it requires modeling time as an additional dimension. While in other domains there are state dominance relationships that can significantly reduce the complexity of the search, in dynamic environments such relationships do not exist. This paper presents a novel state dominance relationship tailored specifically for dynamic environments, and presents a planner that uses that property to plan paths over ten times faster than without using state dominance.

Collision-free navigation in cluttered environments is essential for any mobile manipulation system. Traditional navigation systems have relied on a 2D grid map projected from a 3D representation for efficiency. This approach, however, prevents navigation close to objects in situations where projected 3D configurations are in collision within the 2D grid map even if actually no collision occurs in the 3D environment. Accordingly, when using such a 2D representation for planning paths of a mobile manipulation robot, the number of planning problems which can be solved is limited and suboptimal robot paths may result. We present a fast, integrated approach to solve path planning in 3D using a combination of an efficient octree-based representation of the 3D world and an anytime search-based motion planner. Our approach utilizes a combination of multi-layered 2D and 3D representations to improve planning speed, allowing the generation of almost real-time plans with bounded sub-optimality. We present extensive experimental results with the two-armed mobile manipulation robot PR2 carrying large objects in a highly cluttered environment. Using our approach, the robot is able to efficiently plan and execute trajectories while transporting objects, thereby often moving through demanding, narrow passageways.

Planning with kinodynamic constraints is often required for mobile robots operating in cluttered, complex environments. A common approach is to use a two-dimensional (2-D) global planner for long range planning, and a short range higher dimensional planner or controller capable of satisfying all of the constraints on motion. However, this approach is incomplete and can result in oscillations and the inability to find a path to the goal. In this paper we present an approach to solving this problem by combining the global and local path planning problem into a single search using a combined 2-D and higher dimensional state-space.

Path planning in dynamic environments is significantly more difficult than navigation in static spaces due to the increased dimensionality of the problem, as well as the importance of returning good paths under time constraints. Anytime planners are ideal for these types of problems as they find an initial solution quickly and then improve it as time allows. In this paper, we develop an anytime planner that builds off of Safe Interval Path Planning (SIPP), which is a fast A*-variant for planning in dynamic environments that uses intervals instead of timesteps to represent the time dimension of the problem. In addition, we introduce an optional time-horizon after which the planner drops time as a dimension. On the theoretical side, we show that in the absence of time-horizon our planner can provide guarantees on completeness as well as bounds on the sub-optimality of the solution with respect to the original space-time graph. We also provide simulation experiments for planning for a UAV among 50 dynamic obstacles, where we can provide safe paths for the next 15 seconds of execution within 0.05 seconds. Our results provide a strong evidence for our planner working under real-time constraints.

In this paper, we develop an online motion planning approach which learns from its planning episodes (experiences) a graph, an Experience Graph. On the theoretical side, we show that planning with Experience graphs is complete and provides bounds on suboptimality with respect to the graph that represents the original planning problem. Experimentally, we show in simulations and on a physical robot that our approach is particularly suitable for higher-dimensional motion planning tasks such as planning for two armed mobile manipulation.

We present a method to efficiently find winding-constrained loops in the plane that are optimal with respect to a minimum- cost objective and in the presence of obstacles. Our approach is similar to a typical graph-based search for an optimal path in the plane, but with an additional state variable that encodes information about path homotopy. Upon finding a loop, the value of this state corresponds to a line integral over the loop that indicates how many times it winds around each obstacle, enabling us to reduce the problem of finding paths satisfying winding constraints to that of searching for paths to suitable states in this augmented state space. We give an intuitive in- terpretation of the method based on fluid mechanics and show how this yields a way to perform the necessary calculations efficiently. Results are given in which we use our method to find optimal routes for autonomous surveillance and intruder containment.

In a number of graph search-based planning problems, the value of the cost function that is being minimized also affects the set of possible actions at some or all the states in the graph. For example, in path planning for a robot with a limited battery power, a common cost function is energy consumption, whereas the level of remaining energy affects the navigational capabilities of the robot. Similarly, in path planning for a robot navigating dynamic environments, a total traversal time is a common cost function whereas the timestep affects whether a particular transition is valid. In such planning problems, the cost function typically becomes one of the state variables thereby increasing the dimensionality of the planning problem, and consequently the size of the graph that represents the problem. In this paper, we show how to avoid this increase in the dimensionality for the planning problems whenever the availability of the actions is monotonically non-increasing with the increase in the cost function. We present three variants of A* search for dealing with such planning problems: a provably optimal version, a suboptimal version that scales to larger problems while maintaining a bound on suboptimality, and finally a version that relaxes our assumption on the relationship between the cost function and action space. Our experimental analysis on several domains shows that the presented algorithms achieve up to several orders of magnitude speed up over the alternative approaches to planning.

In this paper, we propose a long-term motion model for visual object tracking. In crowded street scenes, persistent occlusions are a frequent challenge for tracking algorithm and a robust, long-term motion model could help in these situations. Motivated by progresses in robot motion planning, we propose to construct a set of `plausible' plans for each person, which are composed of multiple long-term motion prediction hypotheses that do not include redundancies, unnecessary loops or collisions with other objects. Constructing plausible plan is the key step in utilizing motion planning in object tracking, which has not been fully investigate in robot motion planning. We propose a novel method of efficiently constructing disjoint plans in different homotopy classes, based on winding numbers and winding angles of planned paths around all obstacles. As the goals can be specified by winding numbers and winding angles, we can avoid redundant plans in the same homotopy class and multiple whirls or loops around a single obstacle. We test our algorithm on a challenging, real-world dataset, and compare our algorithm with Linear Trajectory Avoidance and a simplified linear planning model. We find that our algorithm outperforms both algorithms in most sequences.

In this paper, we present a search-based motion planning algorithm for manipulation that handles the high dimensionality of the problem and minimizes the limitations associated with employing a strict set of pre-defined actions. Our approach employs a set of adaptive motion primitives comprised of static motions with variable dimensionality and on-the-fly motions generated by two analytical solvers. This method results in a slimmer, multi-dimensional lattice and offers the ability to satisfy goal constraints with precision. To validate our approach, we used a 7DOF manipulator to perform experiments on a real mobile manipulation platform (Willow Garage's PR2). Our results demonstrate the effectiveness of the planner in efficiently navigating cluttered spaces; the method generates consistent, low-cost motion trajectories, and guarantees the search is complete with bounds on the suboptimality of the solution.

Robotic path planning in static environments is a thoroughly studied problem that can typically be solved very efficiently. However, planning in the presence of dynamic obstacles is still computationally challenging because it requires adding time as an additional dimension to the search-space explored by the planner. In order to avoid the increase in the dimensionality of the planning problem, most real-time approaches to path planning treat dynamic obstacles as static and constantly re-plan as dynamic obstacles move. Although gaining efficiency, these approaches sacrifice optimality and even completeness. In this paper, we develop a planner that builds on the observation that while the number of safe timesteps in any configuration may be unbounded, the number of safe time intervals in a configuration is finite and generally very small. A safe interval is a time period for a configuration with no collisions and if it were extended one timestep in either direction, it would then be in collision. The planner exploits this observation and constructs a search-space with states defined by their configuration and safe interval, resulting in a graph that generally only has a few states per configuration. On the theoretical side, we show that our planner can provide the same optimality and completeness guarantees as planning with time as an additional dimension. On the experimental side, in simulation tests with up to 200 dynamic obstacles, we show that our planner is significantly faster, making it feasible to use in real-time on robots operating in large dynamic environments. We also ran several real robot trials on the PR2, a mobile manipulation platform.

Robust navigation in cluttered environments has been well addressed for mobile robotic platforms, but the problem of navigating with a moveable object like a cart has not been widely examined. In this work, we present a planning and control approach to navigation of a humanoid robot while pushing a cart. We show how immediate information about the environment can be integrated into this approach to achieve safer navigation in the presence of dynamic obstacles. We demonstrate the robustness of our approach through long-running experiments with the PR2 mobile manipulation robot in a typical indoor office environment, where the robot faced narrow and high-traffic passageways with very limited clearance.

With the continued improvements in portable computing power and sensing systems it is becoming more common for groups of robots to cooperate to achieve a goal. When the robots are operating in an initially unknown environment, the most natural form of cooperation is multi-robot exploration. For many years frontier based approaches have been commonly used to assign target points for each of the robots in the group based on expected information gain and distance to travel. In this paper we present an expansion to these approaches allowing for the incorporation of multiple objective utility functions that allow adjustment of the exploration priorities both for the individual robots and the group as a whole. In addition, we discuss real world results of our algorithm including our first place finish at the Old Ram Shed Challenge and second place at the MAGIC2010 main competition.

In the aerial supply delivery problem, an unmanned aircraft needs to deliver supplies as close as possible to the desired location. This involves choosing, flying to, sensing, and landing at a safe landing site that is most accessible from the goal. The problem is often complicated by the fact that the availability of these landing sites may be unknown before the mission begins. Therefore, the aircraft needs to compute a sequence of actions that will minimize the expected value of the objective function. The problem of computing this sequence corresponds to planning under uncertainty in the environment. In this paper, we show how it can be solved efficiently via a recently developed probabilistic planning framework, called Probabilistic Planning with Clear Preferences (PPCP). We show that the problem satisfies the Clear Preferences assumption required by PPCP, and therefore all the theoretical guarantees continue to hold. The experimental results in simulation demonstrate that our approach can solve large-scale problems in realtime while experiments on our custom quad-rotor helicopter provide a proof of concept for the planner.

Path planning quickly becomes computationally hard as the dimensionality of the state-space increases. In this paper, we present a planning algorithm intended to speed up path planning for high-dimensional state-spaces such as robotic arms. The idea behind this work is that while planning in a high-dimensional state-space is often necessary to ensure the feasibilityof the resulting path, large portions of the path have a lower-dimensional structure. Based on this observation, our algorithm iteratively constructs a state-space of an adaptive dimensionality--a state-space that is high-dimensional only where the higher dimensionality is absolutely necessary for finding a feasible path. This often reduces drastically the size of the state-space, and as a result, the planning time and memory requirements. Analytically, we show that our method is complete and is guaranteed to find a solution if one exists, within a specified suboptimality bound. Experimentally, we apply the approach to 3D vehicle navigation (x, y, heading), and to a 7 DOF robotic arm on the Willow Garage’s PR2 robot. The results from our experiments suggest that ourmethod can be substantially faster than some of the state-of-the-art planning algorithms optimized for those tasks.

Search-based planning is widely used for mobile robot motion planning because of its guarantees of optimality and completeness. In continuous state-spaces, however, most existing approaches have significant limitations in terms of optimality and completeness because of the underlying grid used. We propose an approach that eliminates the dependency on grids by using more general equivalence classes to quickly find an initial solution and instead of pruning states that fall within an equivalence class and have higher cost, we use an inflated heuristic to lower the priority of these states in the search. In further iterations, we reduce the inflated heuristic in a principled way, thus providing fast solutions with provable suboptimality bounds that can be improved as time allows. The proposed approach produces smooth paths with the resolution dictated by the action set. Finer action sets produce higher resolution paths that are more computationally intensive to calculate and coarser action sets produce lower resolution paths that are faster to compute. To the best of our knowledge, this is the first algorithm that is able to plan in continuous state-spaces with provable guarantees on completeness and bounds on suboptimality for a given action set. Experimental results on 3D (x,y,theta) path planning show that, on average, this approach is able to find paths in less than two seconds that are within 2% of the optimal path cost in worlds of up to 1000x1000 m with a minimum step size of one meter.

In the aerial supply delivery problem, an un-manned aircraft needs to deliver supplies as close as possible to the desired goal location. This involves choosing and landing at a landing site that is closest to or most accessible from the desired goal location. The problem is complicated by the fact that the status of candidate landing sites is unknown before the mission begins, and instead the aircraft needs to compute a sequence according to which it flies and senses the candidate landing sites in order to land as quickly as possible. The problem of computing this sequence corresponds to planning under uncertainty about environment. In this paper, we show how it can be solved efficiently via a recently developed probabilistic planning framework, called Probabilistic Planning with Clear Preferences (PPCP). We show that the problem satisfies the Clear Preferences assumption required by PPCP,and therefore all the theoretical guarantees continue to hold. The experimental results in simulation show that our approachcan solve large-scale problems in real-time while experiments on a physical quad-rotor provide proof of concept.

In a number of graph search-based planning problems, the value of the cost function that is being minimized also affects the set of possible actions at some or all the states in the graph. In such planning problems, the cost function typically becomes one of the state variables thereby increasing the dimensionality of the planning problem, and consequently the size of the graph that represents the problem. In this paper, we show how to avoid this increase in the dimensionality for weighted search (with bounded suboptimality) whenever the availability of the actions is monotonically non-increasing with the increase in the cost function.

Abstract — The DPC algorithm developed in our previous work is an efficient way of computing optimal trajectories for multiple robots in a distributed fashion with timeparameterized constraints on the distances between pairs of robots. In the present work we extend DPC to the problem of multiple task execution. While this extended problem inherits all the objectives, complexities and constraints of the basic DPC algorithm, each robot is also given an unordered set of tasks that it has to execute before it reaches its goal. There is no specific order imposed on the tasks assigned to a particular robot. The algorithm decides the order of execution of the tasks such that an optimal solution is attained while the time-parametrized distance constraints are satisfied along with successful execution of the tasks. We solve this problem by designing a “State-task Graph ” that represents a product of the state-space graph and the task graph. We then develop an efficient heuristic function for performing searches in this graph. I.

Computing a motion that enables a mobile manipulator to open a door is challenging because it requires tight coordination between the motions of the arm and the base. Hard-coding the motion, on the other hand, is infeasible since doors vary widely in their sizes and types, some doors are opened by pulling and others by pushing, and indoor spaces often contain obstacles that limit the freedom of the mobile manipulator and the degree to which the doors open up. In this paper, we show how to overcome the high-dimensionality of the planning problem by identifying a graph-based representation that is small enough for efficient planning yet rich enough to contain feasible motions that open doors. The use of graph search-based motion planning enables us to handle consistently the wide variance of conditions under which doors need to be open. We demonstrate our approach on the PR2 robot - a mobile manipulator with an omnidirectional base and a 7 degree of freedom arm. The robot was successful in opening a variety of doors both by pulling and pushing.

Heuristic searches such as A* search are highly popular means of finding least-cost plans due to their generality, strong theoretical guarantees on completeness and optimality and simplicity in the implementation. In planning for robotic manipulation however, these techniques are commonly thought of as impractical due to the high-dimensionality of the planning problem. In this paper, we present a heuristic search-based manipulation planner that does deal effectively with the high-dimensionality of the problem. The planner achieves the required efficiency due to the following three factors: (a) its use of informative yet fast-to-compute heuristics; (b) its use of basic (small) motion primitives as atomic actions; and (c) its use of ARA* search which is an anytime heuristic search with provable bounds on solution suboptimality. Our experimental analysis on a real mobile manipulation platform with a 7-DOF robotic manipulator shows the ability of the planner to solve manipulation in cluttered spaces by generating consistent, low-cost motion trajectories while providing guarantees on completeness and bounds on suboptimality.

Optimal heuristic searches such as A* search are commonly used for low-dimensional planning such as 2D path finding. These algorithms however, typically do not scale well to high-dimensional planning problems such as motion planning for robotic arms, computing motion trajectories for non-holonomic robotic vehicles and motion synthesis for humanoid characters. A recently developed randomized version of A* search, called R* search, scales to higher-dimensional planning problems by trading off deterministic optimality guarantees of A* for probabilistic sub-optimality guarantees. In this paper, we show that in addition to its scalability, R* lends itself well to a parallel implementation. In particular, we demonstrate how R* can be implemented on the GPU. On the theoretical side, the GPU version of R*, called R*GPU, preserves all the theoretical properties of R* including its probabilistic bounds on sub-optimality. On the experimental side, we show that R*GPU consistently produces lower cost solutions, scales better in terms of memory, and runs faster than R*. These results hold for both motion planning for a 6DOF robot arm planar as well as 2D path finding.

Goal-directed path planning is one of the basic and widely studied problems in the field of mobile robotics. Homotopy classes of trajectories, arising due to the presence of obstacles, are defined as sets of trajectories that can be transformed into each other by gradual bending and stretching without colliding with obstacles. The problem of finding least-cost paths restricted to a specific homotopy class or finding least-cost paths that do not belong to certain homotopy classes arises frequently in such applications as predicting paths for dynamic entities and computing heuristics for path planning with dynamic constraints. In the present work, we develop a compact way of representing homotopy classes and propose an efficient method of graph search-based optimal path planning with constraints on homotopy classes. The method is based on representing the environment of the robot as a complex plane and making use of the Cauchy Integral Theorem. We prove optimality of the method and show its efficiency experimentally.

For vehicles navigating initially unknown cluttered environments, current state-of-the-art planning algorithms are able to plan and re-plan dynamically-feasible paths efficiently and robustly. It is still a challenge, however, to deal well with the surroundings that are both cluttered and highly dynamic. Planning under these conditions is more difficult for two reasons. First, tracking and predicting the trajectories of moving objects (i.e., cars, humans) is very noisy. Second, the planning process is computationally more expensive because of the increased dimensionality of the state-space, with time as an additional variable. Moreover, re-planning needs to be invoked more often since the trajectories of moving obstacles need to be constantly re-estimated. In this paper, we develop a path planning algorithm that addresses these challenges. First, we choose a representation of dynamic obstacles that efficiently models their predicted trajectories and the uncertainty associated with the predictions. Second, to provide real-time guarantees on the performance of planning with dynamic obstacles, we propose to utilize a novel data structure for planning - a time-bounded lattice - that merges together short-term planning in time with longterm planning without time. We demonstrate the effectiveness of the approach in both simulations with up to 30 dynamic obstacles and on real robots.

We present a search-based planning approach for controlling a quadrupedal robot over rough terrain. Given a start and goal position, we consider the problem of generating a complete joint trajectory that will result in the legged robot successfully moving from the start to the goal. We decompose the problem into two main phases: an initial global planning phase, which results in a footstep trajectory; and an execution phase, which dynamically generates a joint trajectory to best execute the footstep trajectory. We show how R* search can be employed to generate high-quality global plans in the high-dimensional space of footstep trajectories. Results show that the global plans coupled with the joint controller result in a system robust enough to deal with a variety of terrains.

For a robot with a circular footprint, obstacles in a map can be inflated by the radius of the footprint, and planning can treat the robot as a point robot. Many robotic vehicles however have non-circular footprints. When operating in cluttered spaces it therefore becomes important to evaluate the footprint of these robots against a cost map. This evaluation is one of the major computational burdens in planning for robots whose footprints can not be assumed to be circular. In this paper, we propose an efficient method for evaluating a footprint of the robot against a cost map. Our method involves a transformation of the set of points covered by the footprint of the robot into two sets of points: points that should be evaluated against the cost map with inflated obstacles, and points that should be evaluated against the original cost map. The cumulative number of these points is much smaller than the number of points in the original footprint of the robot. Moreover, the method automatically reduces the robot to a single point when its footprint is circular. On the theoretical side, the paper proves the correctness of our method. On the experimental side, the paper shows that the method results in a significant speedup.

In the path clearance problem the robot needs to reach its goal as quickly as possible without being detected by enemies. The robot does not know the precise locations of enemies, but has a list of their possible locations. These locations can be sensed, and the robot can go through them if no enemy is present or has to take a detour otherwise. We have previously developed an approach to the path clearance problem when the robot itself had to sense possible enemy locations. In this paper we investigate the problem of path clearance when the robot can use multiple scout robots to sense the possible enemy locations. This becomes a high-dimensional planning under uncertainty problem. We propose an efficient and scalable approach to it. While the approach requires centralized planning, it can scale to very large environments and to a large number of scouts and allows the scouts to be heterogenous. The experimental results show the benefits of using our approach when multiple scout robots are available.

We present the motion planning framework for an autonomous vehicle navigating through urban environments. Such environments present a number of motion planning challenges, including ultra-reliability, high-speed operation, complex inter-vehicle interaction, parking in large unstructured lots, and constrained maneuvers. Our approach combines a model-predictive trajectory generation algorithm for computing dynamically-feasible actions with two higher-level planners for generating long range plans in both on-road and unstructured areas of the environment. In this Part I of a two-part paper, we describe the underlying trajectory generator and the on-road planning component of this system. We provide examples and results from ldquoBossrdquo, an autonomous SUV that has driven itself over 3000 kilometers and competed in, and won, the Urban Challenge.

We present the motion planning framework for an autonomous vehicle navigating through urban environments. Such environments present a number of motion planning challenges, including ultra-reliability, high-speed operation, complex inter-vehicle interaction, parking in large unstructured lots, and constrained maneuvers. Our approach combines a model-predictive trajectory generation algorithm for computing dynamically-feasible actions with two higher-level planners for generating long range plans in both on-road and unstructured areas of the environment. In this Part II of a two-part paper, we describe the unstructured planning component of this system used for navigating through parking lots and recovering from anomalous on-road scenarios. We provide examples and results from ldquoBossrdquo, an autonomous SUV that has driven itself over 3000 kilometers and competed in, and won, the Urban Challenge.

This paper addresses the problem of planning for goal directed navigation in the environment that contains a number of possible adversary locations. It first shows that commonly used approaches such as assumptive planning can result in very long and costly robot traverses. It then shows how one can solve the same problem using a general probabilistic planner we have recently developed called PPCP (Probabilistic Planning with Clear Preferences). The paper also introduces two optimizations to the PPCP algorithm that make it run up to five times faster for our domain. The experimental results show that solving the problem with PPCP can substantially reduce the expected execution cost as compared to assumptive planning.

MDPs are an attractive formalization for planning, but realistic problems often have intractably large state spaces. When we only need a partial policy to get from a fixed start state to a goal, restricting computation to states relevant to this task can make much larger problems tractable. We introduce a new algorithm, Bounded RTDP, which can produce partial policies with strong performance guarantees while only touching a fraction of the state space, even on problems where other algorithms would have to visit the full state space. To do so, Bounded RTDP maintains both upper and lower bounds on the optimal value function. The performance of Bounded RTDP is greatly aided by the introduction of a new technique to efficiently find suitable upper bounds; this technique can also be used to provide informed initialization to a wide range of other planning algorithms.

Mobile robots often operate in domains that are only incompletely known, for example, when they have to move from given start coordinates to given goal coordinates in unknown terrain. In this case, they need to be able to replan quickly as their knowledge of the terrain changes. Stentz' Focussed Dynamic A/sup */ (D/sup */) is a heuristic search method that repeatedly determines a shortest path from the current robot coordinates to the goal coordinates while the robot moves along the path. It is able to replan faster than planning from scratch since it modifies its previous search results locally. Consequently, it has been extensively used in mobile robotics. In this article, we introduce an alternative to D/sup */ that determines the same paths and thus moves the robot in the same way but is algorithmically different. D/sup */ Lite is simple, can be rigorously analyzed, extendible in multiple ways, and is at least as efficient as D/sup */. We believe that our results will make D/sup */-like replanning methods even more popular and enable robotics researchers to adapt them to additional applications.

Planning algorithms designed for deterministic worlds, such as A* search, usually run much faster than algorithms designed for worlds with uncertain action outcomes, such as value iteration. Real-world planning problems often exhibit uncertainty, which forces us to use the slower algorithms to solve them. Many real-world planning problems exhibit sparse uncertainty: there are long sequences of deterministic actions which accomplish tasks like moving sensor platforms into place, inter- spersed with a small number of sensing actions which have uncertain out- comes. In this paper we describe a new planning algorithm, called MCP (short for MDP Compression Planning), which combines A* search with value iteration for solving Stochastic Shortest Path problem in MDPs with sparse stochasticity. We present experiments which show that MCP can run substantially faster than competing planners in domains with sparse uncertainty; these experiments are based on a simulation of a ground robot cooperating with a helicopter to fill in a partial map and move to a goal location. In deterministic planning problems, optimal paths are acyclic: no state is visited more than once. Because of this property, algorithms like A* search can guarantee that they visit each state in the state space no more than once. By visiting the states in an appropriate order, it is possible to ensure that we know the exact value of all of a state's possible successors before we visit that state; so, the first time we visit a state we can compute its correct value. By contrast, if actions have uncertain outcomes, optimal paths may contain cycles: some states will be visited two or more times with positive probability. Because of these cycles, there is no way to order states so that we determine the values of a state's successors before we visit the state itself. Instead, the only way to compute state values is to solve a set of simultaneous equations. In problems with sparse stochasticity, only a small fraction of all states have uncertain outcomes. It is these few states that cause all of the cycles: while a deterministic state s may participate in a cycle, the only way it can do so is if one of its successors has an action with a stochastic outcome (and only if this stochastic action can lead to a predecessor of s). In such problems, we would like to build a smaller MDP which contains only states which are related to stochastic actions. We will call such an MDP a compressed MDP, and we will call its states distinguished states. We could then run fast algorithms like A* search to plan paths between distinguished states, and reserve slower algorithms like value iteration for deciding how to deal with stochastic outcomes. (a) Segbot (b) Robotic helicopter (d) Planning map (e) Execution simulation (c) 3D Map Figure 1: Robot-Helicopter Coordination There are two problems with such a strategy. First, there can be a large number of states which are related to stochastic actions, and so it may be impractical to enumerate all of them and make them all distinguished states; we would prefer instead to distinguish only states which are likely to be encountered while executing some policy which we are considering. Second, there can be a large number of ways to get from one distinguished state to another: edges in the compressed MDP correspond to sequences of actions in the original MDP. If we knew the values of all of the distinguished states exactly, then we could use A* search to generate optimal paths between them, but since we do not we cannot. In this paper, we will describe an algorithm which incrementally builds a compressed MDP using a sequence of deterministic searches. It adds states and edges to the compressed MDP only by encountering them along trajectories; so, it never adds irrelevant states or edges to the compressed MDP. Trajectories are generated by deterministic search, and so undistinguished states are treated only with fast algorithms. Bellman errors in the values for distinguished states show us where to try additional trajectories, and help us build the relevant parts of the compressed MDP as quickly as possible. 1 Robot-Helicopter Coordination Problem The motivation for our research was the problem of coordinating a ground robot and a helicopter. The ground robot needs to plan a path from its current location to a goal, but has only partial knowledge of the surrounding terrain. The helicopter can aid the ground robot by flying to and sensing places in the map. Figure 1(a) shows our ground robot, a converted Segway with a SICK laser rangefinder. Figure 1(b) shows the helicopter, also with a SICK. Figure 1(c) shows a 3D map of the environment in which the robot operates. The 3D map is post-processed to produce a discretized 2D environment (Figure 1(d)). Several places in the map are unknown, either because the robot has not visited them or because their status may have changed (e.g, a car may occupy a driveway). Such places are shown in Figure 1(d) as white squares. The elevation of each white square is proportional to the probability that there is an obstacle there; we assume independence between unknown squares. The robot must take the unknown locations into account when planning for its route. It may plan a path through these locations, but it risks having to turn back if its way is blocked. Alternately, the robot can ask the helicopter to fly to any of these places and sense them. We assign a cost to running the robot, and a somewhat higher cost to running the helicopter. The planning task is to minimize the expected overall cost of running the robot and the helicopter while getting the robot to its destination and the helicopter back to its home base. Figure 1(e) shows a snapshot of the robot and helicopter executing a policy. Designing a good policy for the robot and helicopter is a POMDP planning problem; unfortunately POMDPs are in general difficult to solve (PSPACE-complete [7]). In the POMDP representation, a state is the position of the robot, the current location of the helicopter (a point on a line segment from one of the unknown places to another unknown place or the home base), and the true status of each unknown location. The positions of the robot and the helicopter are observable, so that the only hidden variables are whether each unknown place is occupied. The number of states is (# of robot locations)(# of helicopter locations)2# of unknown places. So, the number of states is exponential in the number of unknown places and therefore quickly becomes very large. We approach the problem by planning in the belief state space, that is, the space of probability distributions over world states. This problem is a continuous-state MDP; in this belief MDP, our state consists of the ground robot's location, the helicopter's location, and a probability of occupancy for each unknown location. We will discretize the continuous probability variables by breaking the interval [0, 1] into several chunks; so, the number of belief states is exponential in the number of unknown places, and classical algorithms such as value iteration are infeasible even on small problems. If sensors are perfect, this domain is acyclic: after we sense a square we know its true state forever after. On the other hand, imperfect sensors can lead to cycles: new sensor data can contradict older sensor data and lead to increased uncertainty. With or without sensor noise, our belief state MDP differs from general MDPs because its stochastic transitions are sparse: large portions of the policy (while the robot and helicopter are traveling be- tween unknown locations) are deterministic. The algorithm we propose in this paper takes advantage of this property of the problem, as we explain in the next section. 2 The Algorithm Our algorithm can be broken into two levels. At a high level, it constructs a compressed MDP, denoted M c, which contains only the start, the goal, and some states which are out- comes of stochastic actions. At a lower level, it repeatedly runs deterministic searches to find new information to put into M c. This information includes newly-discovered stochas- tic actions and their outcomes; better deterministic paths from one place to another; and more accurate value estimates similar to Bellman backups. The deterministic searches can use an admissible heuristic h to focus their effort, so we can often avoid putting many irrelevant actions into M c. Because M c will often be much smaller than M , we can afford to run stochastic plan- ning algorithms like value iteration on it. On the other hand, the information we get by planning in M c will improve the heuristic values that we use in our deterministic searches; so, the deterministic searches will tend to visit only relevant portions of the state space. 2.1 Constructing and Solving a Compressed MDP Each action in the compressed MDP represents several consecutive actions in M : if we see a sequence of states and actions s1, a1, s2, a2, . . . , sk, ak where a1 through ak-1 are deterministic but ak is stochastic, then we can represent it in M c with a single action a, available at s1, whose outcome distribution is P (s | sk, ak) and whose cost is k-1 c(s1, a, s ) = c(si, ai, si+1) + c(sk, ak, s ) i=1 (See Figure 2(a) for an example of such a compressed action.) In addition, if we see a se- quence of deterministic actions ending in sgoal, say s1, a1, s2, a2, . . . , sk, ak, sk+1 = sgoal, we can define a compressed action which goes from s1 to sgoal at cost k c(s i=1 i, ai, si+1). We can label each compressed action that starts at s with (s, s , a) (where a = null if s = sgoal). Among all compressed actions starting at s and ending at (s , a) there is (at least) one with lowest expected cost; we will call such an action an optimal compression of (s, s , a). Write Astoch for the set of all pairs (s, a) such that action a when taken from state s has more than one possible outcome, and include as well (sgoal, null). Write Sstoch for the states which are possible outcomes of the actions in Astoch, and include sstart as well. If we include in our compressed MDP an optimal compression of (s, s , a) for every s Sstoch and every (s , a) Astoch, the result is what we call the full compressed MDP; an example is in Figure 2(b). If we solve the full compressed MDP, the value of each state will be the same as the value of the corresponding state in M . However, we do not need to do that much work: (a) action compression (b) full MDP compression (c) incremental MDP compression Figure 2: MDP compression Main() 01 initialize M c with sstart and sgoal and set their v-values to 0; 02 while (s M c s.t. RHS(s) - v(s) > and s belongs to the current greedy policy) 03 select spivot to be any such state s; 04 [v; vlim] = Search(spivot); 05 v(spivot) = v; 06 set the cost c(spivot, a, sgoal) of the limit action a from spivot to vlim; 07 optionally run some algorithm satisfying req. A for a bounded amount of time to improve the value function in M c; Figure 3: MCP main loop many states and actions in the full compressed MDP are irrelevant since they do not appear in the optimal policy from sstart to sgoal. So, the goal of the MCP algorithm will be to construct only the relevant part of the compressed MDP by building M c incrementally. Figure 2(c) shows the incremental construction of a compressed MDP which contains all of the stochastic states and actions along an optimal policy in M . The pseudocode for MCP is given in Figure 3. It begins by initializing M c to contain only sstart and sgoal, and it sets v(sstart) = v(sgoal) = 0. It maintains the invariant that 0 v(s) v(s) for all s. On each iteration, MCP looks at the Bellman error of each of the states in M c. The Bellman error is v(s) - RHS(s), where RHS(s) = min RHS(s, a) RHS(s, a) = Es succ(s,a)(c(s, a, s ) + v(s )) aA(s) By convention the min of an empty set is , so an s which does not have any compressed actions yet is considered to have infinite RHS. MCP selects a state with negative Bellman error, spivot, and starts a search at that state. (We note that there exist many possible ways to select spivot; for example, we can choose the state with the largest negative Bellman error, or the largest error when weighted by state visitation probabilities in the best policy in M c.) The goal of this search is to find a new compressed action a such that its RHS-value can provide a new lower bound on v(spivot). This action can either decrease the current RHS(spivot) (if a seems to be a better action in terms of the current v-values of action outcomes) or prove that the current RHS(spivot) is valid. Since v(s ) v(s ), one way to guarantee that RHS(spivot, a) v(spivot) is to compute an optimal compression of (spivot, s, a) for all s, a, then choose the one with the smallest RHS. A more sophisticated strategy is to use an A search with appropriate safeguards to make sure we never overestimate the value of a stochastic action. MCP, however, uses a modified A search which we will describe in the next section. As the search finds new compressed actions, it adds them and their outcomes to M c. It is allowed to initialize newly-added states to any admissible values. When the search returns, MCP sets v(spivot) to the returned value. This value is at least as large as RHS(spivot). Consequently, Bellman error for spivot becomes non-negative. In addition to the compressed action and the updated value, the search algorithm returns a "limit value" vlim(spivot). These limit values allow MCP to run a standard MDP planning algorithm on M c to improve its v(s) estimates. MCP can use any planning algorithm which guarantees that, for any s, it will not lower v(s) and will not increase v(s) beyond the smaller of vlim(s) and RHS(s) (Requirement A). For example, we could insert a fake "limit action" into M c which goes directly from spivot to sgoal at cost vlim(spivot) (as we do on line 06 in Figure 3), then run value iteration for a fixed amount of time, selecting for each backup a state with negative Bellman error. After updating M c from the result of the search and any optional planning, MCP begins again by looking for another state with a negative Bellman error. It repeats this process until there are no negative Bellman errors larger than . For small enough , this property guarantees that we will be able to find a good policy (see section 2.3). 2.2 Searching the MDP Efficiently The top level algorithm (Figure 3) repeatedly invokes a search method for finding trajec- tories from spivot to sgoal. In order for the overall algorithm to work correctly, there are several properties that the search must satisfy. First, the estimate v that search returns for the expected cost of spivot should always be admissible. That is, 0 v v(spivot) (Property 1). Second, the estimate v should be no less than the one-step lookahead value of spivot in M c. That is, v RHS(spivot) (Property 2). This property ensures that search either increases the value of spivot or finds additional (or improved) compressed actions. The third and final property is for the vlim value, and it is only important if MCP uses its optional planning step (line 07). The property is that v vlim v(spivot) (Property 3). Here v(spivot) denotes the minimum expected cost of starting at spivot, picking a com- pressed action not in M c, and acting optimally from then on. (Note that v can be larger than v if the optimal compressed action is already part of M c.) Property 3 uses v rather than v since the latter is not known while it is possible to compute a lower bound on the former efficiently (see below). One could adapt A* search to satisfy at least Properties 1 and 2 by assuming that we can control the outcome of stochastic actions. However, this sort of search is highly optimistic and can bias the search towards improbable trajectories. Also, it can only use heuristics which are even more optimistic than it is: that is, h must be admissible with respect to the optimistic assumption of controlled outcomes. We therefore present a version of A*, called MCP-search (Figure 4), that is more effi- cient for our purposes. MCP-search finds the correct expected value for the first stochas- tic action it encounters on any given trajectory, and is therefore far less optimistic. And, MCP-search only requires heuristic values to be admissible with respect to v values, h(s) v(s). Finally, MCP-search speeds up repetitive searches by improving heuris- tic values based on previous searches. A* maintains a priority queue, OPEN, of states which it plans to expand. The OPEN queue is sorted by f (s) = g(s)+h(s), so that A* always expands next a state which appears to be on the shortest path from start to goal. During each expansion a state s is removed from OPEN and all the g-values of s's successors are updated; if g(s ) is decreased for some state s , A* inserts s into OPEN. A* terminates as soon as the goal state is expanded. We use the variant of A* with pathmax [5] to use efficiently heuristics that do not satisfy the triangle inequality. MCP is similar to A, but the OPEN list can also contain state-action pairs {s, a} where a is a stochastic action (line 31). Plain states are represented in OPEN as {s, null}. Just ImproveHeuristic(s) 01 if s M c then h(s) = max(h(s), v(s)); 02 improve heuristic h(s) further if possible using f best and g(s) from previous iterations; procedure fvalue({s, a}) 03 if s = null return ; 04 else if a = null return g(s) + h(s); 05 else return g(s) + max(h(s), E {c(s, a, s ) + h(s )}) s Succ(s,a) ; CheckInitialize(s) 06 if s was accessed last in some previous search iteration 07 ImproveHeuristic(s); 08 if s was not yet initialized in the current search iteration 09 g(s) = ; InsertUpdateCompAction(spivot, s, a) 10 reconstruct the path from spivot to s; 11 insert compressed action (spivot, s, a) into A(spivot) (or update the cost if a cheaper path was found) 12 for each outcome u of a that was not in M c previously 13 set v(u) to h(u) or any other value less than or equal to v(u); 14 set the cost c(u, a, sgoal) of the limit action a from u to v(u); procedure Search(spivot) 15 CheckInitialize(sgoal), CheckInitialize(spivot); 16 g(spivot) = 0; 17 OPEN = {{spivot, null}}; 18 {sbest, abest} = {null, null}, f best = ; 19 while(g(sgoal) > min{s,a}OPEN(fvalue({s, a})) AND f best + > min{s,a}OPEN(fvalue({s, a}))) 20 remove {s, a} with the smallest fvalue({s, a}) from OPEN breaking ties towards the pairs with a = null; 21 if a = null //expand state s 22 for each s Succ(s) 23 CheckInitialize(s ); 24 for each deterministic a A(s) 25 s = Succ(s, a ); 26 h(s ) = max(h(s ), h(s) - c(s, a , s )); 27 if g(s ) > g(s) + c(s, a , s ) 28 g(s ) = g(s) + c(s, a , s ); 29 insert/update {s , null} into OPEN with fvalue({s , null}); 30 for each stochastic a A(s) 31 insert/update {s, a } into OPEN with fvalue({s, a }); 32 else //encode stochastic action a from state s as a compressed action from spivot 33 InsertUpdateCompAction(spivot, s, a); 34 if f best > fvalue({s, a}) then {sbest, abest} = {s, a}, f best = fvalue({s, a}); 35 if (g(sgoal) min{s,a}OPEN(fvalue({s, a})) AND OPEN = ) 36 reconstruct the path from spivot to sgoal; 37 update/insert into A(spivot) a deterministic action a leading to sgoal; 38 if f best g(sgoal) then {sbest, abest} = {sgoal, null}, f best = g(sgoal); 39 return [f best; min{s,a}OPEN(fvalue({s, a}))]; Figure 4: MCP-search Algorithm like A*, MCP-search expands elements in the order of increasing f -values, but it breaks ties towards elements encoding plain states (line 20). The f -value of {s, a} is defined as g(s) + max[h(s), Es Succ(s,a)(c(s, a, s ) + h(s ))] (line 05). This f -value is a lower bound on the cost of a policy that goes from sstart to sgoal by first executing a series of deterministic actions until action a is executed from state s. This bound is as tight as possible given our heuristic values. State expansion (lines 21-31) is very similar to A. When the search removes from OPEN a state-action pair {s, a} with a = null, it adds a compressed action to M c (line 33). It also adds a compressed action if there is an optimal deterministic path to sgoal (line 37). f best tracks the minimum f -value of all the compressed actions found. As a result, f best v(spivot) and is used as a new estimate for v(spivot). The limit value vlim(spivot) is obtained by continuing the search until the minimum f -value of elements in OPEN approaches f best + for some 0 (line 19). This minimum f -value then provides a lower bound on v(spivot). To speed up repetitive searches, MCP-search improves the heuristic of every state that it encounters for the first time in the current search iteration (lines 01 and 02). Line 01 uses the fact that v(s) from M c is a lower bound on v(s). Line 02 uses the fact that f best - g(s) is a lower bound on v(s) at the end of each previous call to Search; for more details see [4]. 2.3 Theoretical Properties of the Algorithm We now present several theorems about our algorithm. The proofs of these and other theo- rems can be found in [4]. The first theorem states the main properties of MCP-search. Theorem 1 The search function terminates and the following holds for the values it re- turns: (a) if sbest = null then v(spivot) f best E{c(spivot, abest, s ) + v(s )} (b) if sbest = null then v(spivot) = f best = (c) f best min{s,a}OPEN(fvalue({s, a})) v(spivot). If neither sgoal nor any state-action pairs were expanded, then sbest = null and (b) says that there is no policy from spivot that has a finite expected cost. Using the above theorem it is easy to show that MCP-search satisfies Properties 1, 2 and 3, considering that f best is returned as variable v and min{s,a}OPEN(fvalue({s, a})) is returned as variable vlim in the main loop of the MCP algorithm (Figure 3). Property 1 follows directly from (a) and (b) and the fact that costs are strictly positive and v-values are non-negative. Property 2 also follows trivially from (a) and (b). Property 3 follows from (c). Given these properties the next theorem states the correctness of the outer MCP algorithm (in the theorem cgreedy denotes a greedy policy that always chooses an action that looks best based on its cost and the v-values of its immediate successors). Theorem 2 Given a deterministic search algorithm which satisfies Properties 13, the MCP algorithm will terminate. Upon termination, for every state s M c c we greedy have RHS(s) - v(s) v(s). Given the above theorem one can show that for 0 < cmin (where cmin is the smallest expected action cost in our MDP) the expected cost of executing c from greedy sstart is at most cmin v(s c start). Picking cmin is not guaranteed to result in a proper min - policy, even though Theorem 2 continues to hold. 3 Experimental Study We have evaluated the MCP algorithm on the robot-helicopter coordination problem de- scribed in section 1. To obtain an admissible heuristic, we first compute a value function for every possible configuration of obstacles. Then we weight the value functions by the probabilities of their obstacle configurations, sum them, and add the cost of moving the helicopter back to its base if it is not already there. This procedure results in optimistic cost estimates because it pretends that the robot will find out the obstacle locations immediately instead of having to wait to observe them. The results of our experiments are shown in Figure 5. We have compared MCP against three algorithms: RTDP [1], LAO* [2] and value iteration on reachable states (VI). RTDP can cope with large size MDPs by focussing its planning efforts along simulated execu- tion trajectories. LAO* uses heuristics to prune away irrelevant states, then repeatedly performs dynamic programming on the states in its current partial policy. We have im- plemented LAO* so that it reduces to AO* [6] when environments are acyclic (e.g., the robot-helicopter problem with perfect sensing). VI was only able to run on the problems with perfect sensing since the number of reachable states was too large for the others. The results support the claim that MCP can solve large problems with sparse stochas- ticity. For the problem with perfect sensing, on average MCP was able to plan 9.5 times faster than LAO*, 7.5 times faster than RTDP, and 8.5 times faster than VI. On average for these problems, MCP computed values for 58633 states while M c grew to 396 states, and MCP encountered 3740 stochastic transitions (to give a sense of the degree of stochastic- ity). The main cost of MCP was in its deterministic search subroutine; this fact suggests that we might benefit from anytime search techniques such as ARA* [3]. The results for the problems with imperfect sensing show that, as the number and den- sity of uncertain outcomes increases, the advantage of MCP decreases. For these problems MCP was able to solve environments 10.2 times faster than LAO* but only 2.2 times faster than RTDP. On average MCP computed values for 127,442 states, while the size of M c was 3,713 states, and 24,052 stochastic transitions were encountered. Figure 5: Experimental results. The top row: the robot-helicopter coordination problem with perfect sensors. The bottom row: the robot-helicopter coordination problem with sensor noise. Left column: running times (in secs) for each algorithm grouped by environments. Middle column: the number of backups for each algorithm grouped by environments. Right column: an estimate of the expected cost of an optimal policy (v(sstart)) vs. running time (in secs) for experiment (k) in the top row and experiment (e) in the bottom row. Algorithms in the bar plots (left to right): MCP, LAO*, RTDP and VI (VI is only shown for problems with perfect sensing). The characteristics of the environments are given in the second and third rows under each of the bar plot. The second row indicates how many cells the 2D plane is discretized into, and the third row indicates the number of initially unknown cells in the environment. 4 Discussion The MCP algorithm incrementally builds a compressed MDP using a sequence of deter- ministic searches. Our experimental results suggest that MCP is advantageous for problems with sparse stochasticity. In particular, MCP has allowed us to scale to larger environments than were otherwise possible for the robot-helicopter coordination problem. Acknowledgements This research was supported by DARPA's MARS program. All conclusions are our own.

In real world planning problems, time for deliberation is often limited. Anytime planners are well suited for these problems: they find a feasi- ble solution quickly and then continually work on improving it until time runs out. In this paper we propose an anytime heuristic search, ARA*, which tunes its performance bound based on available search time. It starts by finding a suboptimal solution quickly using a loose bound, then tightens the bound progressively as time allows. Given enough time it finds a provably optimal solution. While improving its bound, ARA* reuses previous search efforts and, as a result, is significantly more effi- cient than other anytime search methods. In addition to our theoretical analysis, we demonstrate the practical utility of ARA* with experiments on a simulated robot kinematic arm and a dynamic path planning prob- lem for an outdoor rover.

Mobile robots often operate in domains that are only incompletely known, for example, when they have to move from given start coordinates to given goal coordinates in unknown terrain. In this case, they need to be able to replan quickly as their knowledge of the terrain changes. Stentz' Focussed Dynamic A* is a heuristic search method that repeatedly determines a shortest path from the current robot coordinates to the goal coordinates while the robot moves along the path. It is able to replan one to two orders of magnitudes faster than planning from scratch since it modifies previous search results locally. Consequently, it has been extensively used in mobile robotics. In this paper, we introduce an alternative to Focussed Dynamic A* that implements the same navigation strategy but is algorithmically different. Focussed Dynamic A* Lite is simpler, easier to understand, easier to analyze and easier to extend than Focussed Dynamic A*, yet is more efficient. We believe that our results will make D*-like replanning algorithms even more popular and enable robotics researchers to adapt them to additional applications.

This paper studies the effects of the integration of two learning algorithms, case-base reasoning (CBR) and learning momentum (LM), for the selection of behavioral parameters in real-time for robotic navigational tasks. Use of CBR methodology in the selection of behavioral parameters has already shown significant improvement in robot performance as measured by mission completion time and success rate. It has also made unnecessary the manual configuration of behavioral parameters from a user. However, the choice of the library of CBR cases does affect the robot performance, and choosing the right library sometimes is a difficult task especially when working with a real robot. In contrast, learning momentum does not depend on any prior information such as cases and searches for the "right" parameters in real-time. This results in high mission success rates and requires no manual configuration of parameters, but it shows no improvement in mission completion time. This work combines the two approaches so that CBR discontinuously switches behavioral parameters based on given cases whereas LM uses these parameters as a starting point for the real-time search for the "right" parameters. The integrated system was extensively evaluated on both simulated and physical robots. The tests showed that on simulated robots the integrated system performed as well as the CBR only system and outperformed the LM only system, whereas on real robots it significantly outperformed both CBR only and LM only systems.

This paper presents an approach to learning an optimal behavioral parameterization in the framework of a case-based reasoning methodology for autonomous navigation tasks. It is based on our previous work on a behavior-based robotic system that also employed spatio-temporal case-based reasoning in the selection of behavioral parameters but was not capable of learning new parameterizations. The present method extends the case-based reasoning module by making it capable of learning new and optimizing the existing cases where each case is a set of behavioral parameters. The learning process can either be a separate training process or be part of the mission execution. In either case, the robot learns an optimal parameterization of its behavior for different environments it encounters. The goal of this research is not only to automatically optimize the performance of the robot but also to avoid the manual configuration of behavioral parameters and the initial configuration of a case library, both of which require the user to possess good knowledge of robot behavior and the performance of numerous experiments. The presented method was integrated within a hybrid robot architecture and evaluated in extensive computer simulations, showing a significant increase in the performance over a nonadaptive system and a performance comparable to a non-learning CBR system that uses a hand-coded case library.

Incremental heuristic search methods can often replan paths much faster than incremental or heuristic search methods individually, yet are simple to use. So far, they have only been used in mobile robotics to move a robot to given goal coordinates in unknown terrain. As far as we know, incremental heuristic search methods have not yet been applied to the problem of mapping unknown terrain. In this paper we therefore describe how to apply our incremental heuristic search method D* Lite, that combines ideas from Lifelong Planning A* and Focussed D*, to mapping unknown terrain, which is rather nontrivial. We then compare its runtime against that of incremental search and heuristic search alone, demonstrating the computational benefits of their combination. By demonstrating the versatility and computational benefits of incremental heuristic search, we hope that this underexploited technique will be used more often in mobile robotics.

In this paper, we introduce an efficient replanning algorithm for nonde- terministic domains, namely what we believe to be the first incremental heuristic minimax search algorithm. We apply it to the dynamic dis- cretization of continuous domains, resulting in an efficient implemen- tation of the parti-game reinforcement-learning algorithm for control in high-dimensional domains.

Presents the application of a case-based reasoning approach to the selection and modification of behavioral assemblage parameters. The goal of this research is to achieve an optimal parameterization of robotic behaviors in run-time. This increases robot performance and makes a manual configuration of parameters unnecessary. The case-based reasoning module selects a set of parameters for an active behavioral assemblage in real-time. This set of parameters fits the environment better than hand-coded ones, and its performance is monitored providing feedback for a possible reselection of the parameters. The paper places a significant emphasis on the technical details of the case-based reasoning module and how it is integrated within a schema-based reactive navigation system. The paper also presents the results and evaluation of the system in both in simulation and real world robotic experiments.

Incremental search techniques find optimal solutions to series of similar search tasks much faster than is possible by solving each search task from scratch. While researchers have developed incremental versions of uninformed search methods, we develop an incremental version of A*. The first search of Lifelong Planning A* is the same as that of A* but all subsequent searches are much faster because it reuses those parts of the previous search tree that are identical to the new search tree. We then present experimental results that demonstrate the advantages of Lifelong Planning A* for simple route planning tasks. 1 Overview Artificial intelligence has investigated knowledge-based search techniques that allow one to solve search tasks in large domains. Most of the research on these methods has studied how to solve one-shot search problems. However, search is often a repetitive process, where one needs to solve a series of similar search tasks, for example, because the actual situation turns out to be slightly different from the one initially assumed or because the situation changes over time. An example for route planning tasks are changing traffic conditions. Thus, one needs to replan for the new situation, for example if one always wants to display the least time-consuming route from the airport to the conference center on a web page. In these situations, most search methods replan from scratch, that is, solve the search problems independently. Incremental search techniques share with case-based planning, plan adaptation, repair-based planning, and learning search-control knowledge the property that they find solutions to series of similar search tasks much faster than is possible by solving each search task from scratch. Incremental search techniques, however, differ from the other techniques in that the quality of their solutions is guaranteed to be as good as the quality of the solutions obtained by replanning from scratch. Although incremental search methods are not widely known in artificial intelligence and control, different researchers have developed incremental search versions of uninformed search methods in the algorithms literature. An overview can be found in [FMSN00]. We, on the other hand, develop an incremental version of A*, thus combining ideas from the algorithms literature and the artificial intelligence literature. We call the algorithm Lifelong Planning A* (LPA*), in analogy to “lifelong learning” [Thr98], because it reuses  We thank Anthony Stentz for his support. The Intelligent Decision-Making Group is partly supported by NSF awards under contracts IIS- 9984827, IIS-0098807, and ITR/AP-0113881. The views and conclusions contained in this document are those of the authors and should not be interpreted as representing the official policies, either expressed or implied, of the sponsoring organizations and agencies or the U.S. government.  information from previous searches. LPA* uses heuristics to focus the search and always finds a shortest path for the current edge costs. The first search of LPA* is the same as that of A* but all subsequent searches are much faster. LPA* produces at least the search tree that A* builds. However, it achieves a substantial speedup over A* because it reuses those parts of the previous search tree that are identical to the new search tree.