We introduce a novel optimization-based motion planner, Stochastic Extended LQR (SELQR), which computes a trajectory and associated linear control policy with the objective of minimizing the expected value of a user-defined cost function. SELQR applies to robotic systems that have stochastic non-linear dynamics with motion uncertainty modeled by Gaussian distributions that can be state- and control-dependent. In each iteration, SELQR uses a combination of forward and backward value iteration to estimate the cost-to-come and the cost-to-go for each state along a trajectory. SELQR then locally optimizes each state along the trajectory at each iteration to minimize the expected total cost, which results in smoothed states that are used for dynamics linearization and cost function quadratization. SELQR progressively improves the approximation of the expected total cost, resulting in higher quality plans. For applications with imperfect sensing, we extend SELQR to plan in the robot's belief space. We show that our iterative approach achieves fast and reliable convergence to high-quality plans in multiple simulated scenarios involving a car-like robot, a quadrotor, and a medical steerable needle performing a liver biopsy procedure.

Computing grasps for an object is challenging when the object geometry is not known precisely. In this paper, we explore the use of Gaussian process implicit surfaces (GPISs) to represent shape uncertainty from RGBD point cloud observations of objects. We study the use of GPIS representations to select grasps on previously unknown objects, measuring grasp quality by the probability of force closure. Our main contribution is GP-GPIS-OPT, an algorithm for computing grasps for parallel-jaw grippers on 2D GPIS object representations. Specifically, our method optimizes an approximation to the probability of force closure subject to antipodal constraints on the parallel jaws using Sequential Convex Programming (SCP). We also introduce GPIS-Blur, a method for visualizing 2D GPIS models based on blending shape samples from a GPIS. We test the algorithm on a set of 8 planar objects with transparency, translucency, and specularity. Our experiments suggest that GP-GPIS-OPT computes grasps with higher probability of force closure than a planner that does not consider shape uncertainty on our test objects and may converge to a grasp plan up to 5.7×faster than using Monte-Carlo integration, a common method for grasp planning under shape uncertainty. Furthermore, initial experiments on the Willow Garage PR2 robot suggest that grasps selected with GP-GPIS-OPT are up to 90% more successful than those planned assuming a deterministic shape. Our dataset, code, and videos of our experiments are available at http://rll.berkeley.edu/icra2015grasping/.

We present an approach for asymptotically optimal motion planning for kinodynamic systems with arbitrary nonlinear dynamics amid obstacles. Optimal sampling-based planners like RRT*, FMT*, and BIT* when applied to kinodynamic systems require solving a two-point boundary value problem (BVP) to perform exact connections between nodes in the tree. Two-point BVPs are non-trivial to solve, hence the prevalence of alternative approaches that focus on specific instances of kinodynamic systems, use approximate solutions to the two-point BVP, or use random propagation of controls. In this work, we explore the feasibility of exploiting recent advances in numerical optimal control and optimization to solve these two-point BVPs for arbitrary kinodynamic systems and how they can be integrated with existing optimal planning algorithms. We combine BIT* with a two-point BVP solver that uses sequential quadratic programming (SQP). We consider the problem of computing minimum-time trajectories. Since the duration of trajectories is not known a-priori, we include the time-step as part of the optimization to allow SQP to optimize over the duration of the trajectory while keeping the number of discrete steps fixed for every connection attempted. Our experiments indicate that using a two-point BVP solver in the inner-loop of BIT* is competitive with the state-of-the-art in sampling-based optimal planning that explicitly avoids the use of two-point BVP solvers.

This paper presents a stochastic approach for automatic collision avoidance for tele-operated unmanned aerial vehicles (UAVs). Collision detection and mitigation in the presence of uncertainty is an important problem to address because on-board sensing and state estimation uncertainties are inherent in real-world systems. A feedforward-based algorithm is described that continually extrapolates the future trajectory of the vehicle given the current operator control input for collision avoidance. If the predicted probability of a collision is greater than a user-defined confidence bound, the algorithm overrides the operator control input with the nearest, safe command signal to steer the robot away from obstacles, while maintaining user intent. The algorithm is implemented on a simulated quadrotor helicopter (quadcopter) with varying amounts of artificial uncertainty. Simulation results show that for a given confidence bound, the aerial robot is able to avoid collisions, even in a situation where the operator is deliberately attempting to crash the vehicle.

This paper presents a new method for efficient and exact collision-checking of linear motions of 3-D rigid bodies. 3-D rigid bodies have 6-D configuration spaces (three degrees of freedom for position and three for orientation), and constitute an important subclass of motion planning problems. Our method can be used with any collision-checker that is capable of performing linear transformations and distance computations on 3-D geometry. As previous work has shown, computing the distance between the rigid body in some configuration and the workspace obstacles immediately determines the collision-status of surrounding configurations. Using a recursive procedure one can then determine exactly whether an entire motion of the rigid body is collision-free. In this paper, we will show that by performing an optimally selected linear transformation on the workspace, the collision-status of rigid body motions can be determined using significantly fewer (costly) distance computations. Since collision-checking is often the computational bottleneck in sampling-based motion planning, our approach allows for significant performance improvements of algorithms such as PRM and RRT when planning for 3-D rigid bodies. We demonstrate the benefit of our approach when used in combination with RRT to construct a planning tree in an illustrative benchmark motion planning scenario.

In this paper we present an approach that aids the human operator of unmanned aerial vehicles by automatically performing collision avoidance with obstacles in the environment so that the operator can focus on the global direction of motion of the vehicle. As opposed to systems that override operator control as a last resort in order to avoid collisions (such as those found in modern automobiles), our approach is designed such that the operator can rely on the automatic collision avoidance, enabling intuitive and safe operator control of vehicles that may otherwise be difficult to control. Our approach continually extrapolates the future flight path of the vehicle given the current operator control input. If an imminent collision is predicted our algorithm will override the operator's control input with the nearest control input that will actually let the vehicle avoid collisions with obstacles. This ensures safe flight while simultaneously maintaining the intent of the human operator as closely as possible. We successfully implemented our approach on a physical quadrotor system in a laboratory environment. In all experiments the human operator failed to crash the vehicle into floors, walls, ceilings, or obstacles, even when deliberately attempting to do so.

An accurate dynamics model of a robot is an important ingredient of many algorithms used to solve robotics problems, including motion planning, control, localization, and mapping. Models derived from first principles often contain parameters (e.g. mass, moment of inertia, arm lengths, etc.) for which values are unknown. Those which cannot be easily measured must be estimated from the observed behavior of the robot. A good approach to address this problem is to plan control policies for the robot that elicit maximal amounts of information about the parameters of the system, while still achieving other objectives specified for the robot. In case of parameters subject to drift, this must be done continuously over the lifetime of the robot if costly re-calibrations are to be avoided. In this paper, we introduce a new method that formulates the parameter estimation problem as a continuous partially-observable Markov decision process (POMDP), which plans control policies that optimally trade-off the effort spent on learning parameters and effort spent on achieving regular robot objectives (exploration vs. exploitation), and allow for online, continual parameter estimation. While POMDPs have, until recently, been mostly of theoretical interest due to their inherent complexity, we build on recent advances that allow continuous, Gaussian POMDPs to be approximately-optimally solved in near-real-time rates. We show that the computed control policies lead to improved convergence of the belief of the parameters compared to system identification approaches based on applying random controls.

In this paper we present a formal approach to reciprocal collision avoidance for multiple mobile robots sharing a common 2-D or 3-D workspace whose dynamics are subject to linear differential constraints. Our approach defines a protocol for robots to select their control input independently (i.e. without coordination with other robots) while guaranteeing collision-free motion for all robots, assuming the robots can perfectly observe each other's state. To this end, we extend the concept of LQR-Obstacles (which is a generalization of Velocity Obstacles to robots with dynamics for collision avoidance among static obstacles) for reciprocal collision avoidance among multiple robots. We implemented and tested our approach in 3-D simulation environments for reciprocal collision avoidance of quadrotor helicopters, which have complex dynamics in 16-D state spaces. Our results show that our approach enables collision avoidance among over a hundred quadrotors in tight workspaces at real-time computation rates.

We introduce a new concept; meso-scale planning in real-time multi-agent navigation. Whereas many traditional approaches to multi-agent navigation typically consist of two-levels — a macro-scale level providing agents with a global direction of motion around (large) static obstacles, and a micro-scale level in which agents seek to avoid collision with other agents — our approach adds a meso-scale level to give agents realistic behavior in scenarios where groups of other agents (e.g. families or crowds in a virtual world) form coherent entities. Rather than moving straight through such groups, our approach lets agents move around them. Our formulation considers each agent as an individual that may perceive sets of other agents as a group, and plans its motion accordingly. We base our approach on the velocity obstacle concept, and we show using simulation results that our method dramatically improves the quality of the trajectories computed for the agents.

We present Kinodynamic RRT*, an incremental sampling-based approach for asymptotically optimal motion planning for robots with linear dynamics. Our approach extends RRT*, which was introduced for holonomic robots [10], by using a fixed-final-state-free-final-time controller that optimally connects any pair of states, where the cost function is expressed as a trade-off between the duration of a trajectory and the expended control effort. Our approach generalizes earlier work on RRT* for kinodynamic systems, as it guarantees asymptotic optimality for any system with controllable linear dynamics, in state spaces of any dimension. In addition, we show that for the rich subclass of systems with a nilpotent dynamics matrix, closed-form solutions for optimal trajectories can be derived, which keeps the computational overhead of our algorithm compared to traditional RRT* at a minimum. We demonstrate the potential of our approach by computing asymptotically optimal trajectories in three challenging motion planning scenarios: (i) a planar robot with a 4-D state space and double integrator dynamics, (ii) an aerial vehicle with a 10-D state space and linearized quadrotor dynamics, and (iii) a car-like robot with a 5-D state space and non-linear dynamics.

In many home and service applications, an emerging class of articulated robots such as the Raven and Baxter trade off precision in actuation and sensing to reduce costs and to reduce the potential for injury to humans in their workspaces. For planning and control of such robots, planning in belief ssigma hullpace, i.e., modeling such problems as POMDPs, has shown great promise but existing belief space planning methods have primarily been applied to cases where robots can be approximated as points or spheres. In this paper, we extend the belief space framework to treat articulated robots where the linkage can be decomposed into convex components. To allow planning and collision avoidance in Gaussian belief spaces, we introduce the concept of sigma hulls: convex hulls of robot links transformed according to the sigma standard deviation boundary points generated by the Unscented Kalman filter (UKF). We characterize the signed distances between sigma hulls and obstacles in the workspace to formulate efficient collision avoidance constraints compatible with the Gilbert-Johnson-Keerthi (GKJ) and Expanding Polytope Algorithms (EPA) within an optimization-based planning framework. We report results in simulation for planning motions for a 4-DOF planar robot and a 7-DOF articulated robot with imprecise actuation and inaccurate sensors. These experiments suggest that the sigma hull framework can significantly reduce the probability of collision and is computationally efficient enough to permit iterative re-planning for model predictive control.

We introduce a highly efficient method for solving continuous partially-observable Markov decision processes (POMDPs) in which beliefs can be modeled using Gaussian distributions over the state space. Our method enables fast solutions to sequential decision making under uncertainty for a variety of problems involving noisy or incomplete observations and stochastic actions. We present an efficient approach to compute locally-valid approximations to the value function over continuous spaces in time polynomial (O[n^4]) in the dimension n of the state space. To directly tackle the intractability of solving general POMDPs, we leverage the assumption that beliefs are Gaussian distributions over the state space, approximate the belief update using an extended Kalman filter (EKF), and represent the value function by a function that is quadratic in the mean and linear in the variance of the belief. Our approach iterates towards a linear control policy over the state space that is locally-optimal with respect to a user defined cost function, and is approximately valid in the vicinity of a nominal trajectory through belief space. We demonstrate the scalability and potential of our approach on problems inspired by robot navigation under uncertainty for state spaces of up to 128 dimensions.

This paper presents LQG-Obstacles, a new concept that combines linear-quadratic feedback control of mobile robots with guaranteed avoidance of collisions with obstacles. Our approach generalizes the concept of Velocity Obstacles [3] to any robotic system with a linear Gaussian dynamics model. We integrate a Kalman filter for state estimation and an LQR feedback controller into a closed-loop dynamics model of which a higher-level control objective is the “control input”. We then define the LQG-Obstacle as the set of control objectives that result in a collision with high probability. Selecting a control objective outside the LQG-Obstacle then produces collision-free motion. We demonstrate the potential of LQG-Obstacles by safely and smoothly navigating a simulated quadrotor helicopter with complex non-linear dynamics and motion and sensing uncertainty through three-dimensional environments with obstacles and narrow passages.

We present a fast, analytical method for estimating the probability of collision of a motion plan for a mobile robot operating under the assumptions of Gaussian motion and sensing uncertainty. Estimating the probability of collision is an integral step in many algorithms for motion planning under uncertainty and is crucial for characterizing the safety of motion plans. Our method is computationally fast, enabling its use in online motion planning, and provides conservative estimates to promote safety. To improve accuracy, we use a novel method to truncate estimated a priori state distributions to account for the fact that the probability of collision at each stage along a plan is conditioned on the previous stages being collision free. Our method can be directly applied within a variety of existing motion planners to improve their performance and the quality of computed plans. We apply our method to a car-like mobile robot with second order dynamics and to a steerable medical needle in 3D and demonstrate that our method for estimating the probability of collision is orders of magnitude faster than naïve Monte Carlo sampling methods and reduces estimation error by more than 25% compared to prior methods.

We present an information-theoretic method to measure the similarity between a given set of observed, real-world data and visual simulation technique for aggregate crowd motions of a complex system consisting of many individual agents. This metric uses a two-step process to quantify a simulator's ability to reproduce the collective behaviors of the whole system, as observed in the recorded real-world data. First, Bayesian inference is used to estimate the simulation states which best correspond to the observed data, then a maximum likelihood estimator is used to approximate the prediction errors. This process is iterated using the EM-algorithm to produce a robust, statistical estimate of the magnitude of the prediction error as measured by its entropy (smaller is better). This metric serves as a simulator-to-data similarity measurement. We evaluated the metric in terms of robustness to sensor noise, consistency across different datasets and simulation methods, and correlation to perceptual metrics.

Anytime variants of Dijkstra's and A* shortest path algorithms quickly produce a suboptimal solution and then improve it over time. For example, ARA* introduces a weighting value "epsilon" to rapidly find an initial suboptimal path and then reduces "epsilon" to improve path quality over time. In ARA*, "epsilon" is based on a linear trajectory with ad-hoc parameters chosen by each user. We propose a new Anytime A* algorithm, Anytime Nonparametric A* (ANA*), that does not require ad-hoc parameters, and adaptively reduces varepsilon to expand the most promising node per iteration, adapting the greediness of the search as path quality improves. We prove that each node expanded by ANA* provides an upper bound on the suboptimality of the current-best solution. We evaluate the performance of ANA* with experiments in the domains of robot motion planning, gridworld planning, and multiple sequence alignment. The results suggest that ANA* is as efficient as ARA* and in most cases: (1) ANA* finds an initial solution faster, (2) ANA* spends less time between solution improvements, (3) ANA* decreases the suboptimality bound of the current-best solution more gradually, and (4) ANA* finds the optimal solution faster. ANA* is freely available from Maxim Likhachev's Search-based Planning Library (SBPL).

One of the most ubiquitous AI applications is vehicle route planning. While state-of-the-art systems take into account current traffic conditions or historic traffic data, current planning approaches ignore the impact of their own plans on the future traffic conditions. We present a novel algorithm for self-aware route planning that uses the routes it plans for current vehicle traffic to more accurately predict future traffic conditions for subsequent cars. Our planner uses a roadmap with stochastic, time-varying traffic densities that are defined by a combination of historical data and the densities predicted by the planned routes for the cars ahead of the current traffic. We have applied our algorithm to large-scale traffic route planning, and demonstrated that our self-aware route planner can more accurately predict future traffic conditions, which results in a reduction of the travel time for those vehicles that use our algorithm.

We present an approach for collision avoidance for mobile robots that takes into account acceleration constraints. We discuss both the case of navigating a single robot among moving obstacles, and the case of multiple robots reciprocally avoiding collisions with each other while navigating a common workspace. Inspired by the concept of velocity obstacles [3], we introduce the acceleration-velocity obstacle (AVO) to let a robot avoid collisions with moving obstacles while obeying acceleration constraints. AVO characterizes the set of new velocities the robot can safely reach and adopt using proportional control of the acceleration. We extend this concept to reciprocal collision avoidance for multi-robot settings, by letting each robot take half of the responsibility of avoiding pairwise collisions. Our formulation guarantees collision-free navigation even as the robots act independently and simultaneously, without coordination. Our approach is designed for holonomic robots, but can also be applied to kinematically constrained non-holonomic robots such as cars. We have implemented our approach, and we show simulation results in challenging environments with large numbers of robots and obstacles.

Existing sampling-based robot motion planning methods are often inefficient at finding trajectories for kinodynamic systems, especially in the presence of narrow passages between obstacles and uncertainty in control and sensing. To address this, we propose EG-RRT, an Environment-Guided variant of RRT designed for kinodynamic robot systems that combines elements from several prior approaches and may incorporate a cost model based on the LQG-MP framework to estimate the probability of collision under uncertainty in control and sensing. We compare the performance of EG-RRT with several prior approaches on challenging sample problems. Results suggest that EG-RRT offers significant improvements in performance.

We present the hybrid reciprocal velocity obstacle for collision-free and oscillation-free navigation of multiple mobile robots or virtual agents. Each robot senses its surroundings and acts independently without central coordination or communication with other robots. Our approach uses both the current position and the velocity of other robots to compute their future trajectories in order to avoid collisions. Moreover, our approach is reciprocal and avoids oscillations by explicitly taking into account that the other robots sense their surroundings as well and change their trajectories accordingly. We apply hybrid reciprocal velocity obstacles to iRobot Create mobile robots and demonstrate direct, collision-free, and oscillation-free navigation.

We present a novel approach to direct and control virtual crowds using navigation fields. Our method guides one or more agents toward desired goals based on guidance fields. The system allows the user to specify these fields by either sketching paths directly in the scene via an intuitive authoring interface or by importing motion flow fields extracted from crowd video footage. We propose a novel formulation to blend input guidance fields to create singularity-free, goal-directed navigation fields. Our method can be easily combined with the most current local collision avoidance methods and we use two such methods as examples to highlight the potential of our approach. We illustrate its performance on several simulation scenarios.

We present a novel concept, Virtualized Traffic, to reconstruct and visualize continuous traffic flows from discrete spatiotemporal data provided by traffic sensors or generated artificially to enhance a sense of immersion in a dynamic virtual world. Given the positions of each car at two recorded locations on a highway and the corresponding time instances, our approach can reconstruct the traffic flows (i.e., the dynamic motions of multiple cars over time) between the two locations along the highway for immersive visualization of virtual cities or other environments. Our algorithm is applicable to high-density traffic on highways with an arbitrary number of lanes and takes into account the geometric, kinematic, and dynamic constraints on the cars. Our method reconstructs the car motion that automatically minimizes the number of lane changes, respects safety distance to other cars, and computes the acceleration necessary to obtain a smooth traffic flow subject to the given constraints. Furthermore, our framework can process a continuous stream of input data in real time, enabling the users to view virtualized traffic events in a virtual world as they occur. We demonstrate our reconstruction technique with both synthetic and real-world input.

We present an approach to reciprocal collision avoidance, where multiple mobile agents must avoid collisions with each other while moving in a common workspace. Each agent acts fully independently, and does not communicate with others. Yet our approach guarantees that all agents will be collision-free for at least a fixed amount of time. Our approach provides a sufficient condition for collision-free motion. Given the agent's objective, the optimal collision-free action can be computed very efficiently, as it is the solution to a two-dimensional linear program. We show our approach on dense and complex simulation scenarios involving thousands of agents at fast real-time running times.

In the future, robotic surgical assistants may assist surgeons by performing specific subtasks such as retraction and suturing to reduce surgeon tedium and reduce the duration of some operations. We propose an apprenticeship learning approach that has potential to allow robotic surgical assistants to autonomously execute specific trajectories with superhuman performance in terms of speed and smoothness. In the first step, we record a set of trajectories using human-guided backdriven motions of the robot. These are then analyzed to extract a smooth reference trajectory, which we execute at gradually increasing speeds using a variant of iterative learning control. We evaluate this approach on two representative tasks using the Berkeley Surgical Robots: a figure eight trajectory and a two handed knot-tie, a tedious suturing sub-task required in many surgical procedures. Results suggest that the approach enables (i) rapid learning of trajectories, (ii) smoother trajectories than the human-guided trajectories, and (iii) trajectories that are 7 to 10 times faster than the best human-guided trajectories.

We present a method for smooth and collision-free navigation for multiple independent robots under differential-drive constraints. Our algorithm is based on the optimal reciprocal collision avoidance formulation and guarantees both smoothness in the trajectories of the robots and locally collision-free paths. We provide proofs of these guarantees and demonstrate the effectiveness of our method in experimental scenarios using iRobot Create mobile robots navigating amongst each other.

We propose an approach for acquiring geometric 3D models using cameras mounted on autonomous vehicles and robots. Our method uses structure from motion techniques from computer vision to obtain the geometric structure of the scene. To achieve an efficient goal-driven resource deployment, we develop an incremental approach, which alternates between an accuracy-driven next best view determination and recursive path planning. The next best view is determined by a novel cost function that quantifies the expected contribution of future viewing configurations. A sensing path for robot motion towards the next best view is then achieved by a cost-driven recursive search of intermediate viewing configurations. We discuss some of the properties of our view cost function in the context of an iterative view planning process and present experimental results on a synthetic environment.

We present an approach for smooth and collision-free navigation of multiple mobile robots amongst each other. Each robot senses its surroundings and acts independently without central coordination or communication with other robots. Our approach uses both the current position and the velocity of other robots to predict their future trajectory in order to avoid collisions. Moreover, our approach is reciprocal and avoids oscillations by explicitly taking into account that the other robots also sense their surroundings and change their trajectories accordingly. We build on prior work related to velocity obstacles and reciprocal velocity obstacles and introduce the concept of hybrid reciprocal velocity obstacles for collision avoidance that takes into account the kinematics of the robots and uncertainty in sensor data. We apply our approach to a set of iRobot Create robots using centralized sensing and show natural, direct, and collision-free navigation in several challenging scenarios.

We address the problem of real-time navigation in dynamic environments for car-like robots. We present an approach to identify controls that will lead to a collision with a moving obstacle at some point in the future. Our approach generalizes the concept of velocity obstacles, which have been used for navigation among dynamic obstacles, and takes into account the constraints of a car-like robot. We use this formulation to find controls that will allow collision free navigation in dynamic environments. Finally, we demonstrate the performance of our algorithm on a simulated car-like robot among moving obstacles.

We present a novel concept, Virtualized Traffic, to reconstruct and visualize continuous traffic flows from discrete spatio-temporal data provided by traffic sensors or generated artificially to enhance a sense of immersion in a dynamic virtual world. Given the positions of each car at two recorded locations on a highway and the corresponding time instances, our approach can reconstruct the traffic flows (i.e. the dynamic motions of multiple cars over time) in between the two locations along the highway for immersive visualization of virtual cities or other environments. Our algorithm is applicable to high-density traffic on highways with an arbitrary number of lanes and takes into account the geometric, kinematic, and dynamic constraints on the cars. Our method reconstructs the car motion that automatically minimizes the number of lane changes, respects safety distance to other cars, and computes the acceleration necessary to obtain a smooth traffic flow subject to the given constraints. Furthermore, our framework can process a continuous stream of input data in real time, enabling the users to view virtualized traffic events in a virtual world as they occur.

In this paper, we propose a new concept - the "Reciprocal Velocity Obstacle"- for real-time multi-agent navigation. We consider the case in which each agent navigates independently without explicit communication with other agents. Our formulation is an extension of the Velocity Obstacle concept [3], which was introduced for navigation among (passively) moving obstacles. Our approach takes into account the reactive behavior of the other agents by implicitly assuming that the other agents make a similar collision-avoidance reasoning. We show that this method guarantees safe and oscillation- free motions for each of the agents. We apply our concept to navigation of hundreds of agents in densely populated environments containing both static and moving obstacles, and we show that real-time and scalable performance is achieved in such challenging scenarios.

We present a novel approach for interactive navigation and planning of multiple agents in crowded scenes with moving obstacles. Our formulation uses a precomputed roadmap that provides macroscopic, global connectivity for wayfinding and combines it with fast and localized navigation for each agent. At runtime, each agent senses the environment independently and computes a collision-free path based on an extended "Velocity Obstacles" concept. Furthermore, our algorithm ensures that each agent exhibits no oscillatory behaviors. We have tested the performance of our algorithm in several challenging scenarios with a high density of virtual agents. In practice, the algorithm performance scales almost linearly with the number of agents and can run at interactive rates on multi-core processors.

In this paper we present a new method for kinodynamic motion planning in environments that contain both static and moving obstacles. We present an efficient two-stage approach: in the preprocessing phase, it constructs a roadmap that is collision-free with respect to the static obstacles and encodes the kinematic constraints on the robot. In the query phase, it plans a time-optimal path on the roadmap that obeys the dynamic constraints (bounded acceleration, curvature derivative) on the robot and avoids collisions with any of the moving obstacles. We do not put any constraints on the motions of the moving obstacles, but we assume that they are completely known when a query is performed. We implemented our method, and experiments confirm its good performance.

This paper addresses the problem of path planning in environments in which some of the obstacles can change their positions. It uses the popular PRM method for navigating a robot through an environment. One of the key features of PRM is that it moves the major part of the calculations involved in the path planning process to the preprocessing phase. After that, paths can be extracted very quickly (in a query phase) usually without any noticeable delay. While very successful in many applications, doing most of the work in a preprocessing phase restricts the environment to be static i.e. obstacles are not allowed to change their configurations after the preprocessing phase. In this paper we describe and evaluate an algorithm based on PRM that does allow obstacles to change their configuration after preprocessing while still allowing for a quick query phase.

We present an efficient, anytime method for path planning in dynamic environments. Current approaches to planning in such domains either assume that the environment is static and replan when changes are observed, or assume that the dynamics of the environment are perfectly known a priori. Our approach takes into account all prior information about both the static and dynamic elements of the environment, and efficiently updates the solution when changes to either are observed. As a result, it is well suited to robotic path planning in known or unknown environments in which there are mobile objects, agents or adversaries

We introduce a new type of diagram called the VV(c)-diagram (the Visibility--Voronoi diagram for clearance c), which is a hybrid between the visibility graph and the Voronoi diagram of polygons in the plane. It evolves from the visibility graph to the Voronoi diagram as the parameter c grows from 0 to ∞. This diagram can be used for planning natural-looking paths for a robot translating amidst polygonal obstacles in the plane. A natural-looking path is short, smooth, and keeps --- where possible --- an amount of clearance c from the obstacles. The VV(c)-diagram contains such paths. We also propose an algorithm that is capable of preprocessing a scene of configuration-space polygonal obstacles and constructs a data structure called the VV(c)-complex. The VV(c)-complex can be used to efficiently plan motion paths for any start and goal configuration and any clearance value c, without having to explicitly construct the VV(c)-diagram for that c-value. The preprocessing time is O(n2 log n), where n is the total number of obstacle vertices, and the data structure can be queried directly for any c-value by merely performing a Dijkstra search. We have implemented a Cgal-based software package for computing the VV(c)-diagram in an exact manner for a given clearance value, and used it to plan natural-looking paths in various applications.

In this paper we introduce a method based on the probabilistic roadmap (PRM) planner to construct robust roadmaps for motion planning in changing environments. PRM's are usually aimed at static environments. In reality though, many environments are not static, but contain moving obstacles as well. Often the motion of these obstacles is not unconstrained, but is restricted to some confined area, e.g. a door that can be open or closed or a chair which is bounded to a room. We exploit this observation by assuming that a moving obstacle has a predefined set of potential placements. We present a variant of PRM that is robust against placement changes of obstacles. Our method creates a roadmap that is guaranteed to contain a path for any feasible query when time goes to infinity, i.e. the method is probabilistically complete. Our implementation shows that after a roadmap is created in the preprocessing phase, queries can be solved instantaneously, thus allowing for on-the-fly replanning to anticipate changes in the environment.

In this paper we address the problem of motion planning for multiple robots. We introduce a prioritized method, based on a powerful method for motion planning in dynamic environments, recently developed by the authors. Our approach is generically applicable: there is no limitation on the number of degrees of freedom of each of the robots, and robots of various types - for instance free-flying robots and articulated robots - can be used simultaneously. Results show that high-quality paths can be produced in less than a second of computation time, even in confined environments involving many robots. We examine three issues in particular in this paper: the assignment of priorities to the robots, the performance of prioritized planning versus coordinated planning, and the influence of the extent by which the robot motions are constrained on the performance of the method. Results are reported in terms of both running time and the quality of the paths produced.

In this paper, a new method is presented for motion planning in dynamic environments, that is, finding a trajectory for a robot in a scene consisting of both static and dynamic, moving obstacles. We propose a practical algorithm based on a roadmap that is created for the static part of the scene. On this roadmap, an approximately time-optimal trajectory from a start to a goal configuration is computed, such that the robot does not collide with any moving obstacle. The trajectory is found by performing a two-level search for a shortest path. On the local level, trajectories on single edges of the roadmap are found using a depth-first search on an implicit grid in state-time space. On the global level, these local trajectories are coordinated using an A/sup */-search to find a global trajectory to the goal configuration. The approach is applicable to any robot type in configuration spaces with any dimension, and the motions of the dynamic obstacles are unconstrained, as long as they are known beforehand. The approach has been implemented for both free-flying and articulated robots in three-dimensional workspaces, and it has been applied to multirobot motion planning, as well. Experiments show that the method achieves interactive performance in complex environments.

The probabilistic roadmap (PRM) planner is a popular method for robot motion planning problems with many degrees of freedom. However, it has been shown that the method performs less well in situations where the robot has to pass through a narrow passage in the scene. This is mainly due to the uniformity of the sampling used in the planner; it places many samples in large open regions and too few in tight passages. A technique based on a robot independent cell decomposition of the free workspace is proposed to guide the probabilistic sampling, such that the distribution of samples tends more toward the interesting regions in the scene. It is shown that this leads to improved performance on difficult planning problems in 2D and 3D workspaces.

In this paper a new method is presented for motion planning in dynamic environments, that is, finding a trajectory for a robot in a scene consisting of both static and dynamic, moving obstacles. We propose a practical algorithm based on a roadmap that is created for the static part of the scene. On this roadmap an approximate time-optimal trajectory from a start to a goal configuration is computed, such that the robot does not collide with any moving obstacle. The trajectory is found by performing a search for a shortest path on an implicit grid in state-time space. The approach is applicable to any robot type in configuration spaces with any dimension, and the motions of the dynamic obstacles are unconstrained, as long as they are known beforehand. The approach has been implemented for a free-flying robot in a three-dimensional workspace and experiments show that the method achieves interactive performance in complex environments.