Algorithmic Foundations of Robotics IX

We present two path planning algorithms for mobile robots that are connected by cable to a fixed base. Our algorithms efficiently compute the shortest path and control strategy that lead the robot to the target location considering cable length and obstacle interactions. First, we focus on cable-obstacle collisions.We introduce and formally analyze algorithms that build and search an overlapped configuration space manifold. Next, we present an extension that considers cable-robot collisions. All algorithms are experimentally validated using a real robot.

We propose a novel enhancement to the task of collision-testing a set of local paths. Our approach circumvents expensive collision-tests, yet it declares a continuum of paths collision-free by exploiting both the structure of paths and the outcome of previous tests. We define a homotopy-like equivalence relation among local paths and provide algorithms to (1) classify paths based on equivalence, and (2) implicitly collision-test up to 90% of them. We then prove both correctness and completeness of these algorithms before providing experimental results showing a performance increase up to 300%.

For a mechanical system it often arises that its planned motion will need to be corrected either to refine an approximate plan or to deal with disturbances. This paper develops an algorithmic framework allowing for fast and elegant path correction for nonholonomic underactuated systems with Lie group symmetries, which operates without the explicit need for control strategies. These systems occur frequently in robotics, particularly in locomotion, be it ground, underwater, airborne, or surgical domains. Instead of reintegrating an entire trajectory, the method alters small segments of an initial trajectory in a consistent way so as to transform it via symmetry operations. This approach is demonstrated for the cases of a kinematic car and for flexible bevel tip needle steering, showing a prudent and simple, yet computationally tractable, trajectory correction.

As robots become more versatile, they are increasingly found to operate together in the same environment where they must coordinate their motion in a distributed manner. Such operation does not present problems if the motion is quasi-static and collisions can be easily avoided. However, when the robots follow second-order dynamics, the problem becomes challenging even for a known environment. The setup in this work considers that each robot replans its own trajectory for the next replanning cycle. The planning process must guarantee the robot’s safety by ensuring collision-free paths for the considered period and by not bringing the robot to states where collisions cannot be avoided in the future. This problem can be addressed through communication among the robots, but it becomes complicated when the replanning cycles of the different robots are not synchronized and the robots make planning decisions at different time instants. This paper shows how to guarantee the safe operation of multiple communicating second-order vehicles, whose replanning cycles do not coincide, through an asynchronous, distributed motion planning framework. The method is evaluated through simulations, where each robot is simulated on a different processor and communicates with its neighbors through message passing. The simulations confirm that the approach provides safety in scenarios with up to 48 robots with second-order dynamics in environments with obstacles, where collisions occur often without a safety framework.

Pursuit-evasion games have been used for modeling various forms of conflict arising between two agents modeled as dynamical systems. Although analytical solutions of some simple pursuit-evasion games are known, most interesting instances can only be solved using numerical methods requiring significant offline computation. In this paper, a novel incremental sampling-based algorithm is presented to compute optimal open-loop solutions for the evader, assuming worst-case behavior for the pursuer. It is shown that the algorithm has probabilistic completeness and soundness guarantees. As opposed to many other numerical methods tailored to solve pursuit-evasion games, incremental sampling-based algorithms offer anytime properties, which allow their real-time implementations in online settings.

We study a version of pursuit evasion where two or more pursuers are required to capture the evader because the evader is able to overpower a single defender. The pursuers must coordinate their moves to fortify their approach against the evader while the evader maneuvers to disable pursuers from their unprotected sides. We model this situation as a game of Kabaddi, a popular South Asian sport where two teams occupy opposite halves of a field and take turns sending an attacker into the other half, in order to win points by tagging or wrestling members of the opposing team, while holding his breath during the attack. The game involves team coordination and movement strategies, making it non-trivial to formally model and analyze, yet provides an elegant framework for the study of multiagent pursuitevasion, for instance, a team of robots attempting to capture a rogue agent. Our paper introduces a simple discrete (time and space) model for the game, offers analysis of winning strategies, and explores tradeoffs between maximum movement speed, number of pursuers, and locational constraints.

Reconfiguring chain-type modular robots has been considered a difficult problem scaling poorly with increasing numbers of modules. We address the reconfiguration problem for robots in 2D by presenting centralized and decentralized algorithms based on the Carpenter’s Rule Theorem [4]. The theorem guarantees the existence of instantaneous collision-free unfolding motions which monotonically increase the distance between all joint pairs until an open chain is straightened or a closed chain is convexified. The motions can be found by solving a convex program. Compared to the centralized version, the decentralized algorithm utilizes local proximity sensing and limited communications between subsets of nearby modules. Because the decentralized version reduces the number of joint pairs considered in each convex optimization, it is a practical solution for large number of modules.

Self-reconfigurable robots have an intriguingly flexible design, composing a single robot with many small modules that can autonomously move to transform the robot’s shape and structure. Scaling to a large number of modules is necessary to achieve great flexibility, so each module may only have limited processing and memory resources. This paper introduces a novel distributed locomotion algorithm for lattice-style self-reconfigurable robots which uses constant memory per module with constant computation and communication for each attempted module movement. Our algorithm also guarantees physical stability in the presence of gravity. By utilizing some robot modules to create a static support structure, other modules are able to move freely through the interior of this structure with minimal path planning and without fear of causing instabilities or losing connectivity. This approach also permits the robot’s locomotion speed to remain nearly constant even as the number of modules in the robot grows very large. Additionally,we have developed methods to overcome dropped messages between modules or delays in module computation or movement. Empirical results from our simulation are also presented to demonstrate the scalability and locomotion speed advantages of this approach.

Replanning is a powerful mechanism for controlling robot motion under hard constraints and unpredictable disturbances, but it involves an inherent tradeoff between the planner’s power (e.g., a planning horizon or time cutoff) and its responsiveness to disturbances. We present a real-time replanning technique that uses adaptive time stepping to learn the amount of time needed for a sample-based motion planner to make monotonic progress toward the goal. The technique is robust to the typically high variance exhibited by planning queries, and we prove that it is asymptotically complete for a deterministic environment and a static objective. For unpredictable environments, we present an adaptive time stepping contingency planning algorithm that achieves simultaneous safety-seeking and goal-seeking motion. These techniques generate responsive and safe motion in simulated scenarios across a range of difficulties, including applications to pursuit-evasion and aggressive collision-free teleoperation of an industrial robot arm in a cluttered environment.

We present a novel data structure, the Bayes tree, that provides an algorithmic foundation enabling a better understanding of existing graphical model inference algorithms and their connection to sparse matrix factorization methods. Similar to a clique tree, a Bayes tree encodes a factored probability density, but unlike the clique tree it is directed and maps more naturally to the square root information matrix of the simultaneous localization and mapping (SLAM) problem. In this paper, we highlight three insights provided by our new data structure. First, the Bayes tree provides a better understanding of batch matrix factorization in terms of probability densities. Second, we show how the fairly abstract updates to a matrix factorization translate to a simple editing of the Bayes tree and its conditional densities. Third, we apply the Bayes tree to obtain a completely novel algorithm for sparse nonlinear incremental optimization, that combines incremental updates with fluid relinearization of a reduced set of variables for efficiency, combined with fast convergence to the exact solution. We also present a novel strategy for incremental variable reordering to retain sparsity.We evaluate our algorithmon standard datasets in both landmark and pose SLAM settings.

Partially observable Markov decision processes (POMDPs) have been successfully applied to various robot motion planning tasks under uncertainty. However, most existing POMDP algorithms assume a discrete state space, while the natural state space of a robot is often continuous. This paper presents Monte Carlo Value Iteration (MCVI) for continuous-state POMDPs. MCVI samples both a robot’s state space and the corresponding belief space, and avoids inefficient a priori discretization of the state space as a grid. Both theoretical results and preliminary experimental results indicate that MCVI is a promising new approach for robot motion planning under uncertainty.

We present a sample-based replanning strategy for driving partially-observable, high-dimensional robotic systems to a desired goal. At each time step, it uses forward simulation of randomly-sampled open-loop controls to construct a belief-space search tree rooted at its current belief state. Then, it executes the action at the root that leads to the best node in the tree. As a node quality metric we use Monte Carlo simulation to estimate the likelihood of success under the QMDP belief-space feedback policy, which encourages the robot to take information-gathering actions as needed to reach the goal. The technique is demonstrated on target-finding and localization examples in up to 5D state spacess.

We present parallel algorithms to accelerate collision queries for sample-based motion planning. Our approach is designed for current many-core GPUs and exploits the data-parallelism and multi-threaded capabilities. In order to take advantage of high number of cores, we present a clustering scheme and collision-packet traversal to perform efficient collision queries on multiple configurations simultaneously. Furthermore, we present a hierarchical traversal scheme that performs workload balancing for high parallel efficiency. We have implemented our algorithms on commodity NVIDIA GPUs using CUDA and can perform 500,000 collision queries/second on our benchmarks, which is 10X faster than prior GPU-based techniques. Moreover, we can compute collision-free paths for rigid and articulated models in less than 100 milliseconds for many benchmarks, almost 50-100X faster than current CPU-based planners.

We introduce a novel proximity query, called connection collision query (CCQ), and use it for efficient and exact local planning in sampling-based motion planners. Given two collision-free configurations, CCQ checks whether these configurations can be connected by a given continuous path that either lies completely in the free space or penetrates any obstacle by at most ε , a given threshold. Our approach is general, robust, and can handle different continuous path formulations. We have integrated the CCQ algorithm with sampling-based motion planners and can perform reliable local planning queries with little performance degradation, as compared to prior methods. Moreover, the CCQ-based exact local planner is about an order of magnitude faster than prior exact local planning algorithms.

We present a unified treatment for modeling Coulomb and viscous friction within multi-rigid body simulation using the principle of maximum dissipation. This principle is used to build two different methods—an event-driven impulse-based method and a time stepping method—for modeling contact. The same principle is used to effect joint friction in articulated mechanisms. Experiments show that the contact models are able to be solved faster and more robustly than alternative models. Experiments on the joint friction model show that it is as accurate as a standard model while permitting much larger simulation step sizes to be employed.

This paper studies modeling of tangential compliance as two rigid bodies collide in the space. Stronge’s spring-based contact structure [13, pp. 95-96] is extended to three dimensions. Slip or stick is indicated by the tangential motion of a massless particle connected to the contact point (viewed as an infinitesimal region) on one body via three orthogonal springs. We show that the effect of tangential compliance can be analyzed using normal impulse rather than time, contrary to a previous claim by Stronge. This is primarily due to the ability of updating the elastic energies of the three springs without knowledge of their stiffnesses or length changes. The change rates, nevertheless, are computable. So are sliding velocity and tangential impulse. The latter is then integrated into impact equations and contact kinematics, making the whole system driven by normal impulse alone. Examples include a ball and a pencil bouncing on a table, and a massé billiard shot. The theory has potential impact on impulsive robotic manipulation in which the ability to deal with friction and compliance is vital for skillful maneuvers.

Sampling-based motion planners are a central tool for solving motion-planning problems in a variety of domains, but the theoretical understanding of their behavior remains limited, in particular with respect to the quality of the paths they generate (in terms of path length, clearance, etc.). In this paper we prove, for a simple family of obstacle settings, that the popular dual-tree planner Bi-RRT may produce low-quality paths that are arbitrarily worse than optimal with modest but significant probability, and overlook higher-quality paths even when such paths are easy to produce. At the core of our analysis are probabilistic automata designed to reach an accepting state when a path of significantly low quality has been generated. Complementary experiments suggest that our theoretical bounds are conservative and could be further improved. To the best of our knowledge, this is the first work to study the attainability of high-quality paths that occupy a significant (non-negligible) portion of the space of all paths. The formalism presented in this work can be generalized to other algorithms and other motion-planning problems by defining appropriate predicates, and pave the way to deeper understanding of hallmark planning algorithms.

This paper addresses theoretical foundations of motion planning with differential constraints in the presence of obstacles. We establish general conditions for the existence of resolution complete planning algorithms by introducing a functional analysis framework and reducing algorithm existence to a simple topological property. First, we establish metric spaces over the control function space and the trajectory space. Second, using these metrics and assuming that the control system is Lipschitz continuous, we show that the mapping between open-loop controls and corresponding trajectories is continuous. Next, we prove that the set of all paths connecting the initial state to the goal set is open. Therefore, the set of open-loop controls, corresponding to solution trajectories, must be open. This leads to a simple algorithm that searches for a solution by sampling a control space directly, without building a reachability graph. A dense sample set is given by a discrete-time model. Convergence of the algorithm is proven in the metric of a trajectory space. The results provide some insights into the design of more effective planning algorithms and motion primitives.

This paper introduces a principle to guide the design of finger form: invariance of contact geometry over some continuum of varying shape and/or pose of the grasped object in the plane. Specific applications of this principle include scale-invariant and pose-invariant grasps. Under specific conditions, the principle gives rise to spiral shaped fingers, including logarithmic spirals and straight lines as special cases. The paper presents a general technique to solve for finger form, given a continuum of shape or pose variation and a property to be held invariant. We apply the technique to derive scale-invariant and pose-invariant grasps for disks, and we also explore the principle’s application to many common devices from jar wrenches to rock-climbing cams.

Despite the significant advances in path planning methods, problems involving highly constrained spaces are still challenging. In particular, in many situations the configuration space is a non-parametrizable variety implicitly defined by constraints, which complicates the successful generalization of sampling-based path planners. In this paper, we present a new path planning algorithm specially tailored for highly constrained systems. It builds on recently developed tools for Higher-dimensional Continuation, which provide numerical procedures to describe an implicitly defined variety using a set of local charts. We propose to extend these methods to obtain an efficient path planner on varieties, handling highly constrained problems. The advantage of this planner comes from that it directly operates into the configuration space and not into the higher-dimensional ambient space, as most of the existing methods do.

Developing robust and automated protein structure determination algorithms using nuclear magnetic resonance (NMR) data is an important goal in computational structural biology. Algorithms based on global orientational restraints from residual dipolar couplings (RDCs) promise to be quicker and more accurate than approaches that use only distance restraints. Recent development of analytic expressions for the roots of RDC equations together with protein kinematics has enabled exact, linear-time algorithms, highly desirable over earlier stochastic methods. In addition to providing guarantees on the number and quality of solutions, exact algorithms require a minimal amount of NMR data, thereby reducing the number of NMR experiments. Implementations of these methods determine the solution structures by explicitly computing the intersections of algebraic curves representing discrete RDC values. However, if additional RDC data can be measured, the algebraic curves no longer generically intersect. We address this situation in the paper and show that globally optimal structures can still be computed analytically as points closest to all of the algebraic curves representing the RDCs. We present new algorithms that expand the types and number of RDCs from which analytic solutions are computed. We evaluate the performance of our algorithms on NMR data for four proteins: human ubiquitin, DNA-damage-inducible protein I (DinI), the Z domain of staphylococcal protein A (SpA), and the third IgG-binding domain of Protein G (GB3). The results show that our algorithms are able to determine high-resolution backbone structures from a limited amount of NMR data.

This paper presents a technique for planning and controlling bevel-tip steerable needles towards a target location in 3-D anatomy under the guidance of partial, noisy sensor feedback. Our approach minimizes the probability that the needle intersects obstacles such as bones and sensitive organs by (1) explicitly taking into account motion uncertainty and sensor types, and (2) allowing for efficient optimization of sensor placement.We allow for needle trajectories of arbitrary curvature through duty-cycled spinning of the needle, which is conjectured to make a needle path small-time locally “trackable” [13]. This enables us to use LQG control to guide the needle along the path. For a given path and sensor placement, we show that a priori probability distributions of the needle state can be estimated in advance. Our approach then plans a set of candidate paths and sensor placements and selects the pair for which the estimated uncertainty is least likely to cause intersections with obstacles. We demonstrate the performance of our approach in a modeled prostate cancer treatment environment.

This paper introduces a problem of validating the claimed behavior of an autonomous agent (human or robot) in an indoor environment containing one or more agents, against the observation history from a sparse network of simple, stationary sensors deployed in the same environment. Following principles of dynamic programming, we partition the decision problem into incremental search over a sequence of connectivity subgraphs induced by sensor recordings, which yields efficient algorithms for both single and multiple agent cases. In addition to immediate applicability towards security and forensics problems, the idea of behavior validation using external sensors complements design time model verification.

We consider how the tedious chore of folding clothes can be performed by a robot. At the core of our approach is the definition of a cloth model that allows us to reason about the geometry rather than the physics of the cloth in significant parts of the state space. We present an algorithm that, given the geometry of the cloth, computes how many grippers are needed and what the motion of these grippers are to achieve a final configuration specified as a sequence of g − folds—folds that can be achieved while staying in the subset of the state space to which the geometric model applies. G-folds are easy to specify and are sufficiently rich to capture most common cloth folding procedures.We consider folds involving single and stacked layers of material and describe experiments folding towels, shirts, sweaters, and slacks with a Willow Garage PR2 robot. Experiments based on the planner had success rates varying between 5/9 and 9/9 for different clothing articles.