We present an algorithm that, given a representation of a road network in lane-level detail, computes a route that minimizes the expected cost to reach a given destination. In doing so, our algorithm allows us to solve for the complex trade-offs encountered when trying to decide not just which roads to follow, but also when to change between the lanes making up these roads, in order to-for example-reduce the likelihood of missing a left exit while not unnecessarily driving in the leftmost lane. This routing problem can naturally be formulated as a Markov Decision Process (MDP), in which lane change actions have stochastic outcomes. However, MDPs are known to be time-consuming to solve in general. In this paper, we show that-under reasonable assumptions-we can use a Dijkstra-like approach to solve this stochastic problem, and benefit from its efficient O(n log n) running time. This enables an autonomous vehicle to exhibit lane-selection behavior as it efficiently plans an optimal route to its destination.

Robotic manipulators designed for home assistance and new surgical procedures often have significant uncertainty in their actuation due to compliance requirements, cost constraints, and size limits. We introduce a new integrated motion planning and control algorithm for robotic manipulators that makes safety a priority by explicitly considering the probability of unwanted collisions. We first present a fast method for estimating the probability of collision of a motion plan for a robotic manipulator under the assumptions of Gaussian motion and sensing uncertainty. Our approach quickly computes distances to obstacles in the workspace and appropriately transforms this information into the configuration space using a Newton method to estimate the most relevant collision points in configuration space. We then present a sampling-based motion planner based on executing multiple independent rapidly exploring random trees that returns a plan that, under reasonable assumptions, asymptotically converges to a plan that minimizes the estimated collision probability. We demonstrate the speed and safety of our plans in simulation for (1) a 3-D manipulator with 6 DOF, and (2) a concentric tube robot, a tentacle-like robot designed for surgical applications.

We present Extended LQR, a novel approach for locally-optimal control for robots with non-linear dynamics and non-quadratic cost functions. Our formulation is conceptually different from existing approaches, and is based on the novel concept of LQR-smoothing, which is an LQR-analogue of Kalman smoothing. Our approach iteratively performs both a backward Extended LQR pass, which computes approximate cost-to-go functions, and a forward Extended LQR pass, which computes approximate cost-to-come functions. The states at which the sum of these functions is minimal provide an approximately optimal sequence of states for the control problem, and we use these points to linearize the dynamics and quadratize the cost functions in the subsequent iteration. Our results indicate that Extended LQR converges quickly and reliably to a locally-optimal solution of the non-linear, non-quadratic optimal control problem. In addition, we show that our approach is easily extended to include temporal optimization, in which the duration of a trajectory is optimized as part of the control problem. We demonstrate the potential of our approach on two illustrative non-linear control problems involving simulated and physical differential-drive robots and simulated quadrotor helicopters.

We consider the problem of computing a minimum uncertainty path for an autonomous vehicle from a start to a destination location in the presence intermittent sensing, modeled as a stochastic process, in addition to process and measurement noise. We introduce the use of a novel bound on the maximum eigenvalue of the estimation error covariance matrix as the cost function for belief space planning. Our main contributions are three-fold. We first derive an analytic bound on the performance of a state estimator under sensor misdetection (intermittency) occurring stochastically over time. Second, we use this bound as a proxy for the expected maximum eigenvalue evolution in a sample-based path planning algorithm to produce a path that trades off accuracy and robustness. This extends the recent body of work on planning under uncertainty to include the fact that sensors may not provide any measurement owing to misdetection. Computational results demonstrate the benefit of the approach and comparisons are made with the state of the art in path planning in belief space. Third, and finally, we establish theoretically that the proposed algorithm possesses the optimal substructure property, i.e. the algorithm returns an optimal path relative to the bound treated as a proxy for the expected maximum eigenvalue evolution.

We present an approach to motion planning under motion and sensing un-certainty, formally described as a continuous partially-observable Markov decision process (POMDP). Our approach is designed for non-linear dynamics and observation models, and follows the general POMDP solution framework in which we represent beliefs by Gaussian distributions, approximate the belief dynamics using an extended Kalman filter (EKF), and represent the value function by a quadratic function that is valid in the vicinity of a nominal trajectory through belief space. Using a variant of differential dynamic programming, our approach iterates with second-order convergence towards a linear control policy over the belief space that is locally-optimal with respect to a user-defined cost function. Unlike previous work, our approach does not assume maximum-likelihood observations, does not assume fixed estimator or control gains, takes into account obstacles in the environment, and does not require discretization of the belief space. The running time of the algorithm is polynomial in the dimension of the state space. We demonstrate the potential of our approach in several continuous partially-observable planning domains with obstacles for robots with non-linear dynamics and observation models.

This paper studies the application of automatic collision avoidance algorithms to help pilots improve their maneuvering of unmanned aerial vehicles (UAVs). Automatic collision avoidance technology can help reduce the cognitive workload of a pilot, especially when flying UAVs through cluttered and complex unstructured environments. The feedforward-based algorithm reviewed herein exploits the dynamics of the aerial robot and if a collision is predicted, the algorithm modifies the operator's input to avoid a collision. The algorithm has recently been implemented on a quadcopter UAV with on-board computation and sensing. To quantify the improvement in pilot performance compared to other methods, human-subject studies were conducted using a simulated quadcopter UAV running the collision avoidance algorithms. Specifically, a comparison is made between the feedforward-based algorithm, the basic risk field algorithm (a variant on potential field), and full manual control. Experimental results show that the feedforward-based algorithm performs significantly better than manual control by lowering the number of collisions and increasing the UAV's average speed, both of which are extremely vital, for example, for UAV-assisted search-and-rescue applications. Compared to the potential-field based algorithm, the feedforward algorithm enabled the pilot to operate the UAV with significantly higher average speeds without drastically affecting the number of collisions.

For applications such as warehouse order fulfillment, robot grasps must be robust to uncertainty arising from sensing, mechanics, and control. One way to achieve robustness is to evaluate the performance of candidate grasps by sampling perturbations in shape, pose, and gripper approach and to compute the probability of force closure for each candidate to identify a grasp with the highest expected quality. Since evaluating the quality of each grasp is computationally demanding, prior work has turned to cloud computing. To improve computational efficiency and to extend this work, we consider how Multi-Armed Bandit (MAB) models for optimizing decisions can be applied in this context. We formulate robust grasp planning as a MAB problem and evaluate convergence times towards an optimal grasp candidate using 100 object shapes from the Brown Vision 2D Lab Dataset with 1000 grasp candidates per object. We consider the case where shape uncertainty is represented as a Gaussian process implicit surface (GPIS) with Gaussian uncertainty in pose, gripper approach angle, and coefficient of friction. We find that Thompson Sampling and the Gittins index MAB methods converged to within 3% of the optimal grasp up to 10x faster than uniform allocation and 5x faster than iterative pruning.

Aspects of the disclosure relate to an autonomous vehicle that may detected other nearby vehicles and identify them as parked or unparked. This identification may be based on visual indicia displayed by the detected vehicles as well as traffic control factors relating to the detected vehicles. Detected vehicles that are in a known parking spot may automatically be identified as parked. In addition, detected vehicles that satisfy conditions that are indications of being parked may also be identified as parked. The autonomous vehicle may then base its control strategy on whether or not a vehicle has been identified as parked or not.

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.

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/.

Reciprocal collision avoidance has become a popular area of research over recent years. Approaches have been developed for a variety of dynamic systems ranging from single integrators to car-like, differential-drive, and arbitrary, linear equations of motion. In this paper, we present two contributions. First, we provide a unification of these previous approaches under a single, generalized representation using control obstacles. In particular, we show how velocity obstacles, acceleration velocity obstacles, continuous control obstacles, and LQR-obstacles are special instances of our generalized framework. Secondly, we present an extension of control obstacles to general reciprocal collision avoidance for non-linear, non-homogeneous systems where the robots may have different state spaces and different non-linear equations of motion from one another. Previous approaches to reciprocal collision avoidance could not be applied to such systems, as they use a relative formulation of the equations of motion and can, therefore, only apply to homogeneous, linear systems where all robots have the same linear equations of motion. Our approach allows for general mobile robots to independently select new control inputs while avoiding collisions with each other. We implemented our approach in simulation for a variety of mobile robots with non-linear equations of motion: differential-drive, differential-drive with a trailer, car-like, and hovercrafts. We also performed physical experiments with a combination of differential-drive, differential-drive with a trailer, and car-like robots. Our results show that our approach is capable of letting a non-homogeneous group of robots with non-linear equations of motion safely avoid collisions at real-time computation rates.

We introduce a novel optimization-based motion planner, the stochastic extended linear quadratic regulator (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 nonlinear 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.

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.

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.

Sampling perturbations in shape, state, and control can facilitate grasp planning in the presence of uncertainty arising from noise, occlusions, and surface properties such as transparency and specularities. Monte-Carlo sampling is computationally demanding, even for planar models. We consider an alternative based on the multi-armed bandit (MAB) model for making sequential decisions, which can apply to a variety of uncertainty models. We formulate grasp planning as a “budgeted multi-armed bandit model” (BMAB) with finite stopping time to minimize “simple regret”, the difference between the expected quality of the best grasp and the expected quality of the grasp evaluated at the stopping time. To evaluate MABbased sampling, we compare it with Monte-Carlo sampling for grasping an uncertain planar object defined by a Gaussian process implicit surface (GPIS), but the method is applicable to other models of uncertainty. We derive distributions on contact points, surface normal, and center of mass and use these solve the associated MAB model, finding that it computes grasps of similar quality and can reduce computation time by an order of magnitude. This suggests a number of new research questions about how MAB can be applied to other models of uncertainty and how different MAB solution techniques can be applied to further reduce computation.

This paper presents two new search algorithms: Potential Search (PTS) and Anytime Potential Search/Anytime Non-Parametric A⁎ (APTS/ANA⁎). Both algorithms are based on a new evaluation function that is easy to implement and does not require user-tuned parameters. PTS is designed to solve bounded-cost search problems, which are problems where the task is to find as fast as possible a solution under a given cost bound. APTS/ANA⁎ is a non-parametric anytime search algorithm discovered independently by two research groups via two very different derivations. In this paper, co-authored by researchers from both groups, we present these derivations: as a sequence of calls to PTS and as a non-parametric greedy variant of Anytime Repairing A⁎.

In this paper, we present an implementation of 3-D reciprocal collision avoidance on real quadrotor helicopters where each quadrotor senses the relative position and velocity of other quadrotors using an on-board camera. We show that using our approach, quadrotors are able to successfully avoid pairwise collisions in GPS and motion-capture denied environments, without communication between the quadrotors, and even when human operators deliberately attempt to induce collisions. To our knowledge, this is the first time that reciprocal collision avoidance has been successfully implemented on real robots where each agent independently observes the others using on-board sensors. We theoretically analyze the response of the collision-avoidance algorithm to the violated assumptions by the use of real robots. We quantitatively analyze our experimental results. A particularly striking observation is that at times the quadrotors exhibit "reciprocal dance" behavior, which is also observed when humans move past each other in constrained environments. This seems to be the result of sensing uncertainty, which causes both robots involved to have a different belief about the relative positions and velocities and, as a result, choose the same side on which to pass.

This paper introduces the novel concept of LQR smoothing, which analogous to Kalman smoothing consists of both a backward pass and a forward pass. In the backward pass the cost-to-go function is computed using the standard LQR Riccati equation that runs backward in time, and in the forward pass the cost-to-come function is computed using a Riccati equation that runs forward in time. The sum of the cost-to-go and the cost-to-come function gives the total-cost function, and we will show that the states for which the total-cost function is minimal constitute the minimum-cost trajectory for the linear-quadratic optimal control problem. This insight is used to construct a fast-converging iterative procedure to compute a locally-optimal feedback control policy for systems with non-linear dynamics and non-quadratic cost, where in each iteration the current minimal-total-cost states provide natural points about which the dynamics can be linearized and the cost quadratized. We demonstrate the potential of our approach on two illustrative non-linear control problems involving physical differential-drive robots and simulated quadrotor helicopters in environments with obstacles, and show that our approach converges in only about a third of the number of iterations required by existing approaches such as Iterative LQR.