Skip to main content

Über dieses Buch

Algorithms are a fundamental component of robotic systems. Robot algorithms process inputs from sensors that provide noisy and partial data, build geometric and physical models of the world, plan high-and low-level actions at different time horizons, and execute these actions on actuators with limited precision. The design and analysis of robot algorithms raise a unique combination of questions from many elds, including control theory, computational geometry and topology, geometrical and physical modeling, reasoning under uncertainty, probabilistic algorithms, game theory, and theoretical computer science.

The Workshop on Algorithmic Foundations of Robotics (WAFR) is a single-track meeting of leading researchers in the eld of robot algorithms. Since its inception in 1994, WAFR has been held every other year, and has provided one of the premiere venues for the publication of some of the eld's most important and lasting contributions.

This books contains the proceedings of the tenth WAFR, held on June 13{15 2012 at the Massachusetts Institute of Technology. The 37 papers included in this book cover a broad range of topics, from fundamental theoretical issues in robot motion planning, control, and perception, to novel applications.



The Minimum Constraint Removal Problem with Three Robotics Applications

This paper formulates a new Minimum Constraint Removal (MCR) motion planning problem in which the objective is to remove the fewest geometric constraints necessary to connect a start and goal state with a free path. I present a probabilistic roadmap motion planner for MCR in continuous configuration spaces. The planner operates by constructing increasingly refined roadmaps, and efficiently solves discrete MCR problems on these networks. A number of new theoretical results are given for discrete MCR, including a proof that it is NP-hard by reduction from SET-COVER, and I describe two search algorithms that perform well in practice. The motion planner is proven to produce the optimal MCR with probability approaching 1 as more time is spent, and its convergence rate is improved with various efficient sampling strategies. The planner is demonstrated on three example applications: generating human-interpretable excuses for failure, motion planning under uncertainty, and rearranging movable obstacles.

Kris Hauser

Hierarchical Decision Theoretic Planning for Navigation Among Movable Obstacles

In this paper we present the first decision theoretic planner for the problem of Navigation Among Movable Obstacles (NAMO). While efficient planners for NAMO exist, they are challenging to implement in practice due to the inherent uncertainty in both perception and control of real robots. Generalizing existing NAMO planners to nondeterministic domains is particularly difficult due to the sensitivity of MDP methods to task dimensionality. Our work addresses this challenge by combining ideas from Hierarchical Reinforcement Learning with Monte Carlo Tree Search, and results in an algorithm that can be used for fast online planning in uncertain environments. We evaluate our algorithm in simulation, and provide a theoretical argument for our results which suggest linear time complexity in the number of obstacles for typical environments.

Martin Levihn, Jonathan Scholz, Mike Stilman

Robust Complete Path Planning in the Plane

We present a complete path planning algorithm for a plane robot with three degrees of freedom and a static obstacle. The part boundaries consist of


linear and circular edges. The algorithm constructs and searches a combinatorial representation of the robot free space. Its computational complexity is O((



+ c


) log


) with







) the number of configurations with three simultaneous contacts between robot and obstacle edges. The algorithm is implemented robustly using our adaptive-precision controlled perturbation library. The program is fast and memory efficient, is provably accurate, and handles degenerate input.

Victor Milenkovic, Elisha Sacks, Steven Trac

Configurations and Path Planning of Convex Planar Polygonal Loops

Polygonal loops are interesting both as classical geometric objects and in modeling practical engineering systems,


, grasping systems with fingers having planar revolute joints. Convex loop configurations and path planning between them are important since many naturally occurring manipulation poses for human and robotic hands are convex or close to convex, and current collision-free path planning methods for polygonal loops use convex configurations in intermediate steps. We prove that, in a set of triangle-based parameters, the space


of convex configurations of a planar polygonal loop with fixed edge lengths and orientation, and one link pinned to the plane, is


with respect to an easily computed triangular configuration; with a further condition on edge lengths,


is actually a

convex polyhedron

. Thus reconfiguration between identically oriented convex configurations of a planar polygonal loop can be achieved by one or two straight-line motions within


. We conjecture that, in our parameter space, the straight-line motion joining any two such configurations passes through only non-self-intersecting configurations, although it may leave


. These results are substantially simpler and more efficient than prior work, and demonstrate the importance of suitable system parametrization.

Li Han, Lee Rudolph, Michael Chou, Sean Corbett, Emily Eagle, Dylan Glotzer, Jake Kramer, Jonathan Moran, Christopher Pietras, Ammar Tareen, Matthew Valko

Equilibrium Configurations of a Kirchhoff Elastic Rod under Quasi-static Manipulation

Consider a thin, flexible wire of fixed length that is held at each end by a robotic gripper. The curve traced by this wire can be described as a local solution to a geometric optimal control problem, with boundary conditions that vary with the position and orientation of each gripper. The set of all local solutions to this problem is the configuration space of the wire under quasi-static manipulation. We will show that this configuration space is a smooth manifold of finite dimension that can be parameterized by a single chart. Working in this chart—rather than in the space of boundary conditions—makes the problem of manipulation planning very easy to solve. Examples in simulation illustrate our approach.

Timothy Bretl, Zoe McCarthy

From Discrete to Continuous Motion Planning

In this paper, we demonstrate an equivalence between a large class of discrete motion planning problems, and piano mover’s problems, which we refer to as ”continuous motion planning problems”. We first prove that under some assumptions, discrete motion planning in


dimensions can be transformed into continuous motion planning in 2


+ 1 dimensions. Then we prove a more specific, similar equivalence for which the number of dimensions of the configuration space does not necessarily have to be increased.We study two simple cases where this theorem applies, and show that it can lead to original and efficient motion planning algorithms, which could probably be applied to a wide range of multi-contact planning problems.We apply this equivalence to a simulation of legged locomotion planning for a hexapod robot.

Nicolas Perrin

Ray-Shooting Algorithms for Robotics

Ray shooting is a well-studied problem in computer graphics. It also occurs in robotics as a collision detection problem in 3-D object space or a contact force optimization problem in 6-D wrench space. However, the ray-shooting algorithms derived in computer graphics are limited to 3-D polyhedra and not suited for general convex sets in high-dimensional space. This paper discusses several general ray-shooting algorithms and their applications to these problems in robotics.

Yu Zheng, Katsu Yamane

Optimal Gap Navigation for a Disc Robot

This paper considers the problem of globally optimal navigation with respect to Euclidean distance for disc-shaped, differential-drive robot placed into an unknown, simply connected polygonal region. The robot is unable to build precise geometric maps of the environment. Most of the robot’s information comes from a gap sensor, which indicates depth discontinuities and allows the robot to move toward them. A motion strategy is presented that optimally navigates the robot to any landmark in the region. Optimality is proved and the method is illustrated in simulation.

Rigoberto Lopez-Padilla, Rafael Murrieta-Cid, Steven M. LaValle

Min-Max Latency Walks: Approximation Algorithms for Monitoring Vertex-Weighted Graphs

In this paper, we consider the problem of planning a path for a robot to monitor a known set of features of interest in an environment.We represent the environment as a vertex- and edge-weighted graph, where vertices represent features or regions of interest. The edge weights give travel times between regions, and the vertex weights give the importance of each region. If the robot repeatedly performs a closed walk on the graph, then we can define the latency of a vertex to be the maximum time between visits to that vertex, weighted by the importance (vertex weight) of that vertex. Our goal in this paper is to find the closed walk that minimizes the maximum weighted latency of any vertex. We show that there does not always exist an optimal walk of polynomial size. We then prove that for any graph there exist a constant approximation walk of size





), where


is the number of vertices. We provide two approximation algorithms; an




)-approximation and an




)-approximation, where


is the ratio between the maximum and minimum vertex weight. We provide simulation results which demonstrate that our algorithms can be applied to problems consisting of thousands of vertices.

Soroush Alamdari, Elaheh Fata, Stephen L. Smith

Multi-agent Path Planning and Network Flow

This paper connects multi-agent path planning on graphs (roadmaps) to network flow problems, showing that the former can be reduced to the latter, therefore enabling the application of combinatorial network flow algorithms, as well as general linear program techniques, tomulti-agent path planning problems on graphs. Exploiting this connection, we show that when the goals are permutation invariant, the problem always has a feasible solution path set with a longest finish time of no more than




- 1 steps, in which


is the number of agents and


is the number of vertices of the underlying graph.We then give a complete algorithm that finds such a solution in




) time, with


being the number of edges of the graph. Taking a further step, we study time and distance optimality of the feasible solutions, show that they have a pairwise Pareto optimal structure, and again provide efficient algorithms for optimizing two of these practical objectives.

Jingjin Yu, Steven M. LaValle

Trajectory Planning and Assignment in Multirobot Systems

In this paper, we consider the problem of tasking large numbers of homogenous robots to move to a set of specified goal locations, addressing both the assignment and trajectory planning subproblems concurrently. This is related to the standard linear Euclidean assignment problem except that the solution to the trajectory generation subproblem must result in time-parameterized trajectories and guarantee collision avoidance.We begin with a centralized approach and derive an optimal centralized solution and study the computational complexity. The main contribution of this paper, however, is a decentralized algorithm with limited communication between neighbors that guarantees collision-avoidance and overcomes the computational challenges of the centralized method at the cost of suboptimal solutions. We demonstrate the performance of the algorithm as the number of robots is increased to tens of robots and the resulting increase in communication across neighbors required for safe execution.

Matthew Turpin, Nathan Michael, Vijay Kumar

k-Color Multi-robot Motion Planning

We present a simple and natural extension of the

multi-robot motion planning

problem where the robots are partitioned into groups (colors), such that in each group the robots are interchangeable. Every robot is no longer required to move to a specific target, but rather to some target placement that is assigned to its group. We call this problem

k-color multi-robot motion planning

and provide a sampling-based algorithm specifically designed for solving it. At the heart of the algorithm is a novel technique where the


-color problem is reduced to several discrete multi-robot motion planning problems. These reductions amplify basic samples into massive collections of free placements and paths for the robots. We demonstrate the performance of the algorithm by an implementation for the case of disc robots in the plane and show that it successfully and efficiently copes with a variety of challenging scenarios, involving many robots, while a straightforward extension of prevalent sampling-based algorithms for the


-color case, fails even on simple scenarios. Interestingly, our algorithm outperforms a state-of-the- art implementation for the standard multi-robot problem, in which each robot has a distinct color.

Kiril Solovey, Dan Halperin

Distributed Construction of Truss Structures

We address the construction of truss structures with standardized parts and simple fasteners using multiple aerial robots. Robotic construction, unlike assembly in industrial settings, often occurs in unstructured environments where it may be difficult to manipulate objects to a high level of precision. This is particularly true with aerial assembly. There are challenges in positioning aerial robots and in deploying complicated manipulators or material handling devices in constrained environments characteristic of partially-built structures.We consider a scaled-down version of the problem with quadrotors, lightweight truss elements, and magnetic fasteners. Specifically, we present a provably-correct distributed construction algorithm that allows multiple robots to assemble structures according to a specified blue print with only local knowledge. In addition, we describe several heuristics for choosing the order in which parts are placed to improve performance measures like completion time. We illustrate the performance of the algorithm and the heuristics with simulations of our multi-quadrotor testbed.

Quentin Lindsey, Vijay Kumar

Space-Time Group Motion Planning

We present a novel approach for planning and directing heterogeneous groups of virtual agents based on techniques from linear programming. Our method efficiently identifies the most promising paths in both time and space and provides an optimal distribution of the groups’ members over these paths such that their average traveling time is minimized. The computed space-time plan is combined with an agent-based steering method to handle collisions and generate the final motions of the agents. Our overall solution is applicable to a variety of virtual environment applications, such as computer games and crowd simulators. We highlight its potential on different scenarios and evaluate the results from our simulations using a number of quantitative quality metrics. In practice, our system runs at interactive rates and can solve complex planning problems involving one or multiple groups.

Ioannis Karamouzas, Roland Geraerts, A. Frank van der Stappen

Multi-robot Coverage and Exploration in Non-Euclidean Metric Spaces

Multi-robot coverage and exploration is a fundamental problem in robotics. A widely-used, efficient and distributable algorithm for achieving coverage of a convex environment with Euclidean metric is that proposed by Cortes,

et al.

, which is based on the discrete-time Lloyd’s algorithm. It is significantly difficult to achieve the same in non-convex environments and with non-Euclidean metrics. In this paper we generalize the control law based on minimization of the

coverage functional

to spaces that are inherently non-Euclidean and are punctured by obstacles. We also propose a practical discrete implementation based on standard graph search-based algorithms. We demonstrate the applicability of the proposed algorithm by solving efficient coverage problems on a sphere and exploration problems in highly non-convex indoor environments.

Subhrajit Bhattacharya, Robert Ghrist, Vijay Kumar

Lion and Man with Visibility in Monotone Polygons

In the original version of the lion and man game, a lion tries to capture a man who is trying to escape in a circular arena. The players have equal speeds. They can observe each other at all times. We study a new variant of the game in which the lion has only line-of-sight visibility. Hence, it can observe the man’s position only if the line segment connecting them does not intersect the boundary.We show that despite this limitation the lion can capture the man in any monotone polygon in finite time.

Narges Noori, Volkan Isler

Sparse Roadmap Spanners

Asymptotically optimal planners like


* guarantee that solutions approach optimal as iterations increase. Roadmaps with this property, however, may grow too large. If optimality is relaxed, asymptotically near-optimal solutions produce sparser graphs by not including all edges. The idea stems from graph spanners, which produce sparse subgraphs that guarantee near-optimal paths. Existing asymptotically optimal and near-optimal planners, however, include all sampled configurations as roadmap nodes, meaning only infinite graphs have the desired properties. This work proposes an approach, called


, that provides the following asymptotic properties: (a) completeness, (b) near-optimality and (c) the probability of adding nodes to the spanner converges to zero as iterations increase, which suggests that finite-size data structures may exist that have near-optimality properties. The method brings together ideas from various planners but deviates from existing integrations of


* with graph spanners. Simulations for rigid bodies show that


indeed provides small roadmaps and results in faster query resolution. The rate of node addition is shown to decrease over time and the quality of solutions is even better than the theoretical bounds.

Andrew Dobson, Athanasios Krontiris, Kostas E. Bekris

Toggle PRM: A Coordinated Mapping of C-Free and C-Obstacle in Arbitrary Dimension

Motion planning has received much attention over the past 40 years. More than 15 years have passed since the introduction of the successful sampling-based approach known as the Probabilistic RoadMap Method (PRM). PRM and its many variants have demonstrated great success for some high-dimensional problems, but they all have some level of difficulty in the presence of narrow passages. Recently, an approach called Toggle PRM has been introduced whose performance does not degrade for 2-dimensional problems with narrow passages. In Toggle PRM, a simultaneous, coordinated mapping of both C


and C


is performed and every connection attempt augments one of the maps – either validating an edge in the current space or adding a configuration ’witnessing’ the connection failure to the other space. In this paper, we generalize Toggle PRM to


-dimensions and show that the benefits of mapping both C


and C


continue to hold in higher dimensions. In particular, we introduce a new narrow passage characterization,




-separable narrow passages, which describes the types of passages that can be successfully mapped by Toggle PRM. Intuitively,




-separable narrow passages are arbitrarily narrow regions of C


that separate regions of C


, at least locally, such as hallways in an office building. We experimentally compare Toggle PRM with other methods in a variety of scenarios with different types of narrow passages and robots with up to 16



Jory Denny, Nancy M. Amatoo

On the Power of Manifold Samples in Exploring Configuration Spaces and the Dimensionality of Narrow Passages

We extend our study of Motion Planning via Manifold Samples (MMS), a general algorithmic framework that combines geometric methods for the exact and complete analysis of low-dimensional configuration spaces with sampling-based approaches that are appropriate for higher dimensions. The framework explores the configuration space by taking samples that are

low-dimensional manifolds of the configuration space

capturing its connectivity much better than isolated point samples. The contributions of this paper are as follows: (i) We present a recursive application of MMS in a sixdimensional configuration space, enabling the coordination of two polygonal robots translating and rotating amidst polygonal obstacles. In the adduced experiments for the more demanding test cases MMS clearly outperforms PRM, with over 20-fold speedup in a


setting. (ii) A probabilistic completeness proof for the most prevalent case, namely MMS with samples that are affine subspaces. (iii) A closer examination of the test cases reveals that MMS has, in comparison to standard sampling-based algorithms, a significant advantage in scenarios containing

high-dimensional narrow passages

. This provokes a novel characterization of narrow passages which attempts to capture their dimensionality, an attribute that had been (to a large extent) unattended in previous definitions.

Oren Salzman, Michael Hemmer, Dan Halperin

Sampling Extremal Trajectories for Planar Rigid Bodies

This paper presents an approach to finding the time-optimal trajectories for a simple rigid-body model of a mobile robot in an obstacle-free plane. Previous work has used Pontryagin’s Principle to find strong necessary conditions on time-optimal trajectories of the rigid body; trajectories satisfying these conditions are called

extremal trajectories

. The main contribution of this paper is a method for sampling the extremal trajectories sufficiently densely to guarantee that for any pair of start and goal configurations, a trajectory can be found that (provably) approximately reaches the goal, approximately optimally; the quality of the approximation is only limited by the availability of computational resources.

Weifu Wang, Devin Balkcom

Convex Hull Asymptotic Shape Evolution

The asymptotic properties of Rapidly exploring Random Tree (RRT) growth in large spaces is studied both in simulation and analysis. The main phenomenon is that the convex hull of the RRT reliably evolves into an equilateral triangle when grown in a symmetric planar region (a disk). To characterize this and related phenomena from flocking and swarming, a family of dynamical systems based on incremental evolution in the space of shapes is introduced. Basins of attraction over the shape space explain why the number of hull vertices tends to reduce and the shape stabilizes to a regular polygon with no more than four vertices.

Maxim Arnold, Yuliy Baryshnikov, Steven M. LaValle

Efficient Collision Checking in Sampling-Based Motion Planning

Collision checking is generally considered to be the primary computational bottleneck in sampling-based motion planning algorithms.We show that this does not have to be the case. More specifically, we introduce a novel way of implementing collision checking in the context of sampling-based motion planning, such that the amortized complexity of collision checking is negligible with respect to that of the other components of sampling-based motion planning algorithms. Our method works by storing a lower bound on the distance to the nearest obstacle of each normally collision-checked point. New samples may immediately be determined collision free—without a call to the collision-checking procedure—if they are closer to a previously collision-checked point than the latter is to an obstacle. A similar criterion can also be used to detect points inside of obstacles (i.e., points that are in collision with obstacles). Analysis proves that the expected fraction of points that require a call to the normal (expensive) collision-checking procedure approaches zero as the total number of points increases. Experiments, in which the proposed idea is used in conjunction with the RRT and RRT


path planning algorithms, also validate that our method enables significant benefits in practice.

Joshua Bialkowski, Sertac Karaman, Michael Otte, Emilio Frazzoli

Faster Sample-Based Motion Planning Using Instance-Based Learning

We present a novel approach to improve the performance of sample-based motion planners by learning from prior instances. Our formulation stores the results of prior collision and local planning queries. This information is used to accelerate the performance of planners based on probabilistic collision checking, select new local paths in free space, and compute an efficient order to perform queries along a search path in a graph. We present fast and novel algorithms to perform


-NN (


-nearest neighbor) queries in high dimensional configuration spaces based on locality-sensitive hashing and derive tight bounds on their accuracy. The


-NN queries are used to perform instance-based learning and have a sub-linear time complexity. Our approach is general, makes no assumption about the sampling scheme, and can be used with various sample-based motion planners, including PRM, Lazy-PRM, RRT and RRT


, by making small changes to these planners.We observe up to 100% improvement in the performance of various planners on rigid and articulated robots.

Jia Pan, Sachin Chitta, Dinesh Manocha

Scale-Free Coordinates for Multi-robot Systems with Bearing-Only Sensors

We propose

scale-free coordinates

as an alternative coordinate system for multi-robot systems with large robot populations. Scale-free coordinates allow each robot to know, up to scaling, the relative position and orientation of other robots in the network.We consider a weak sensing model where each robot is only capable of measuring the angle, relative to its own heading, to each of its neighbors. Our contributions are three-fold. First, we derive a precise mathematical characterization of the computability of scale-free coordinates using only bearing measurements, and we describe an efficient algorithm to obtain them. Second, through simulations we show that even in graphs with low average vertex degree, most robots are able to compute the scale-free coordinates of their neighbors using only 2-hop bearing measurements. Finally, we present an algorithm to compute scale-free coordinates that is tailored to low-cost systems with limited communication bandwidth and sensor resolution. Our algorithm mitigates the impact of sensing errors through a simple yet effective noise sensitivity model. We validate our implementation with real-world robot experiments using static accuracy measurements and a simple scale-free motion controller.

Alejandro Cornejo, Andrew J. Lynch, Elizabeth Fudge, Siegfried Bilstein, Majid Khabbazian, James McLurkin

Mapping Polygons with Agents That Measure Angles

We study the problem of mapping an initially unknown environment with autonomous mobile robots. More precisely, we consider simplistic agents that move from vertex to vertex along the boundary of a polygon and measure angles at each vertex. We show that such agents are already capable of drawing a map of any polygon in the sense that they can infer the exact geometry up to similarity. Often, such tasks require the agent to have some prior bound on the size of the environment. In this paper, we provide an efficient reconstruction algorithm that

does not

need any a priori knowledge about the total number of vertices.

Yann Disser, Matúš Mihalák, Peter Widmayer

Counting Moving Bodies Using Sparse Sensor Beams

This paper examines the problem of determining the distribution of a number of indistinguishable moving bodies located in regions separated by sensor beams that can detect whether a body moves across them. We characterize the conditions under which an exact distribution of bodies can be determined, and compute bounds on the expected number of sensor observations required to determine this exact distribution for a certain movement model of the bodies.

Lawrence H. Erickson, Jingjin Yu, Yaonan Huang, Steven M. LaValle

Convex Receding Horizon Control in Non-Gaussian Belief Space

One of the main challenges in solving partially observable control problems is planning in high-dimensional belief spaces. Essentially, it is necessary to plan in the parameter space of all relevant

probability distributions

over the state space. The literature has explored different planning technologies including trajectory optimization [8, 6] and roadmap methods [12, 4]. Unfortunately, these methods are hard to use in a receding horizon control context where it is potentially necessary to replan on every time step. Trajectory optimization is not guaranteed to find globally optimal solutions and roadmap methods can have long planning times. This paper identifies a non-trivial instance of the belief space planning problem that is convex and can therefore be solved quickly and optimally even for high dimensional problems. We prove that the resulting control strategy will ultimately reach a goal region in belief space under mild assumptions. Since the space of convex belief space planning problem is somewhat limited, we extend the approach using mixed integer programming. We propose to solve the integer part of the problem in advance so that only convex problems need be solved during receding horizon control.

Robert Platt

Planar Uncertainty Propagation and a Probabilistic Algorithm for Interception

Many robotics applications involve motion planning with uncertainty. In this paper, we focus on path planning for planar systems by optimizing the probability of successfully arriving at a goal. We approach this problem with a modified version of the Path-of-Probability (POP) algorithm.We extend the POP algorithm to allow for a moving target and to optimize the number of steps to reach the goal. One tool that we develop in this paper to increase efficiency of the POP algorithm is a second order closed-form uncertainty propagation formula. This formula is utilized to quickly propagate the mean and covariance of nonparametrized distributions for planar systems. The modified POP algorithm is demonstrated on a simple rolling disc example with a moving goal.

Andrew W. Long, Kevin C. Wolfe, Gregory S. Chirikjian

Intention-Aware Motion Planning

As robots venture into new application domains as autonomous vehicles on the road or as domestic helpers at home, they must recognize human intentions and behaviors in order to operate effectively. This paper investigates a new class of motion planning problems with uncertainty in human intention. We propose a method for constructing a practical model by assuming a finite set of unknown intentions. We first construct a motion model for each intention in the set and then combine these models together into a single Mixed Observability Markov Decision Process (MOMDP), which is a structured variant of the more common Partially Observable Markov Decision Process (POMDP). By leveraging the latest advances in POMDP/MOMDP approximation algorithms, we can construct and solve moderately complex models for interesting robotic tasks. Experiments in simulation and with an autonomous vehicle show that the proposed method outperforms common alternatives because of its ability in recognizing intentions and using the information effectively for decision making.

Tirthankar Bandyopadhyay, Kok Sung Won, Emilio Frazzoli, David Hsu, Wee Sun Lee, Daniela Rus

Point-Based Policy Transformation: Adapting Policy to Changing POMDP Models

Motion planning under uncertainty that can efficiently take into account changes in the environment is critical for robots to operate reliably in our living spaces. Partially Observable Markov Decision Process (POMDP) provides a systematic and general framework for motion planning under uncertainty. Point-based POMDP has advanced POMDP planning tremendously over the past few years, enabling POMDP planning to be practical for many simple to moderately difficult robotics problems. However, when environmental changes alter the POMDP model, most existing POMDP planners recompute the solution from scratch, often wasting significant computational resources that have been spent for solving the original problem. In this paper, we propose a novel algorithm, called

Point-Based Policy Transformation (PBPT)

, that solves the altered POMDP problem by


the solution of the original problem to accommodate changes in the problem. PBPT uses the point-based POMDP approach. It transforms the original solution by modifying the set of sampled beliefs that represents the belief space


, and then uses this new set of sampled beliefs to revise the original solution. Preliminary results indicate that PBPT generates a good policy for the altered POMDP model in a matter of minutes, while recomputing the policy using the fastest offline POMDP planner today fails to find a policy with similar quality after two hours of planning time, even when the policy for the original problem is reused as an initial policy.

Hanna Kurniawati, Nicholas M. Patrikalakis

From Formal Methods to Algorithmic Implementation of Human Inspired Control on Bipedal Robots

This paper presents the process of translating formal theory and methods to efficient algorithms in the context of human-inspired control of bipedal robots, with the end result being experimentally realized robust and efficient robotic walking with AMBER. We begin by considering human walking data and find outputs (or virtual constraints) that, when calculated from the human data, are described by simple functions of time (termed canonical walking functions). Formally, we construct a torque controller, through model inversion, that drives the outputs of the robot to the outputs of the human as represented by the canonical walking function; while these functions fit the human data well, they do not apriori guarantee robotic walking (due to do the physical differences between humans and robots). An optimization problem is presented that determines the best fit of the canonical walking function to the human data, while guaranteeing walking for a specific bipedal robot; in addition, constraints can be added that guarantee physically realizable walking. We consider a physical bipedal robot AMBER and define a simple voltage based control law—utilizing only the human outputs and canonical walking function with parameters obtained from the optimization—for which we obtain walking in simulation. Since this controller does not require model inversion, it can be implemented efficiently in software. Moreover, applying this methodology to AMBER experimentally results in robust and efficient ”human-like” robotic walking.

Shishir Nadubettu Yadukumar, Murali Pasupuleti, Aaron D. Ames

Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact

Direct methods for trajectory optimization are widely used for planning locally optimal trajectories of robotic systems. Most state-of-the-art techniques treat the discontinuous dynamics of contact as discrete modes and restrict the search for a complete path to a specified sequence through these modes. Here we present a novel method for trajectory planning through contact that eliminates the requirement for an

a priori

mode ordering. Motivated by the formulation of multi-contact dynamics as a Linear Complementarity Problem (LCP) for forward simulation, the proposed algorithm leverages Sequential Quadratic Programming (SQP) to naturally resolve contact constraint forces while simultaneously optimizing a trajectory and satisfying nonlinear complementarity constraints. The method scales well to high dimensional systems with large numbers of possible modes.We demonstrate the approach using three increasingly complex systems: rotating a pinned object with a finger, planar walking with the Spring Flamingo robot, and high speed bipedal running on the FastRunner platform.

Michael Posa, Russ Tedrake

Robust Online Motion Planning with Regions of Finite Time Invariance

In this paper we consider the problem of generating motion plans for a nonlinear dynamical system that are guaranteed to succeed despite uncertainty in the environment, parametric model uncertainty, disturbances, and/or errors in state estimation. Furthermore, we consider the case where these plans must be generated online, because constraints such as obstacles in the environment may not be known until they are perceived (with a noisy sensor) at runtime. Previous work on feedback motion planning for nonlinear systems was limited to offline planning due to the computational cost of safety verification. Here we take a

trajectory library

approach by designing controllers that stabilize the nominal trajectories in the library and precomputing regions of finite time invariance (”funnels”) for the resulting closed loop system. We leverage sums-of-squares programming in order to efficiently compute funnels which take into account bounded disturbances and uncertainty. The resulting

funnel library

is then used to

sequentially compose

motion plans at runtime while ensuring the safety of the robot. A major advantage of the work presented here is that by explicitly taking into account the effect of uncertainty, the robot can evaluate motion plans based on how vulnerable they are to disturbances.We demonstrate our method on a simulation of a plane flying through a two dimensional forest of polygonal trees with parametric uncertainty and disturbances in the form of a bounded ”cross-wind”.

Anirudha Majumdar, Russ Tedrake

Towards Consistent Vision-Aided Inertial Navigation

In this paper, we study estimator inconsistency in Vision-aided Inertial Navigation Systems (VINS) from a standpoint of system observability. We postulate that a leading cause of inconsistency is the gain of spurious information along unobservable directions, resulting in smaller uncertainties, larger estimation errors, and possibly even divergence.We develop an Observability-Constrained VINS (OC-VINS), which explicitly enforces the unobservable directions of the system, hence preventing spurious information gain and reducing inconsistency. Our analysis, along with the proposed method for reducing inconsistency, are extensively validated with simulation trials and real-world experiments.

Joel A. Hesch, Dimitrios G. Kottas, Sean L. Bowman, Stergios I. Roumeliotis

Learning to Segment and Track in RGBD

We consider the problem of segmenting and tracking deformable objects in color video with depth (RGBD) data available from commodity sensors such as the Kinect. We frame this problem with very few assumptions - no prior object model, no stationary sensor, no prior 3D map - thus making a solution potentially useful for a large number of applications, including semi-supervised learning, 3D model capture, and object recognition. Our approach makes use of a rich feature set, including local image appearance, depth discontinuities, optical flow, and surface normals to inform the segmentation decision in a conditional random field model. In contrast to previous work, the proposed method


how to best make use of these features from ground-truth segmented sequences. We provide qualitative and quantitative analyses which demonstrate substantial improvement over the state of the art.

Alex Teichman, Sebastian Thrun

The Path Inference Filter: Model-Based Low-Latency Map Matching of Probe Vehicle Data

We consider the problem of reconstructing vehicle trajectories from sparse sequences of GPS points, for which the sampling interval ranges between 10 seconds and 2 minutes. We introduce a new class of algorithms, collectively called

path inference filter

(PIF), that maps streaming GPS data in real-time, with a high throughput. We present an efficient Expectation Maximization algorithm to train the filter on new data without ground truth observations. The path inference filter is evaluated on a large San Francisco taxi dataset. It is deployed at an industrial scale inside the

Mobile Millennium

traffic information system, and is used to map fleets of vehicles in San Francisco, Sacramento, Stockholm and Porto.

Timothy Hunter, Pieter Abbeel, Alexandre M. Bayen

Predicting Pedestrian Trajectories Using Velocity-Space Reasoning

We introduce a novel method to predict pedestrian trajectories using agent-based velocity-space reasoning for improved human-robot interaction. This formulation models the trajectory of each moving pedestrian in a robot’s environment using velocity obstacles and learns the simulation parameters based on tracked data. The resulting motion model for each agent is computed using statistical inferencing techniques from noisy data. This includes the combination of Ensemble Kalman filters and maximum likelihood estimation algorithm to learn individual motion parameters at interactive rates. We highlight the performance of our motion model in real-world crowded scenarios. We also compare its performance with prior techniques and demonstrate improved accuracy in the predicted trajectories.

Sujeong Kim, Stephen J. Guy, Wenxi Liu, Rynson W. H. Lau, Ming C. Lin, Dinesh Manocha

Erratum: Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact

The Authors, Editors, and Publisher wish to include a reference to a body of work that was announced to us following publication of this article that independently produced similar results, and was in fact published before our manuscript (1-3). This work formulated nonlinear programs (NLP) in the mathematical programming with equilibrium constraints (MPEC) framework, and included formulations for autonomous and non-autonomous discontinuous transitions with or without unilateral constraints, for both elastic and inelastic collisions, and for time-stepping-based shooting methods.

These methods are the first we know of to remove the combinatorial complexity of hybrid mode scheduling and produce a polynomial time NLP. Our manuscript presents a different but similar framework, which differs by emphasizing sparseness in the program and solution techniques used by modern sequential quadratic programming solvers to scale the solution techniques to large systems (up to 22 discrete variables and over 4 million resulting modes).

1) Yunt, Kerim; Glocker, Christoph. Trajectory optimization of mechanical hybrid systems using SUMT. In: Advanced Motion Control, 2006. 9th IEEE International Workshop on. IEEE, 2005. S. 665-671.

2) Yunt, Kerim; Glocker, Christoph. A combined continuation and penalty method for the determination of optimal hybrid mechanical trajectories. In: IUTAM Symposium on Dynamics and Control of Nonlinear Systems with Uncertainty. Springer Netherlands, 2007. S. 187-196.

3) Yunt, Kerim; An Augmented Lagrangian-based Shooting Method for the Optimal Trajectory Generation of Switching Lagrangian Systems. Dynamics of Continuous, Discrete and Impulsive Systems Series B: Applications & Algorithms 18 (2011) 615-645, 2011 Watam Press.

Michael Posa, Russ Tedrake


Weitere Informationen