Simulation plays a key role in scaling robot learning and validating policies, but constructing simulations remains labor-intensive. In this paper, we introduce ReGen, a generative simulation framework that automates this process using inverse design. Given an agent's behavior (such as a motion trajectory or objective function) and its textual description, we infer the underlying scenarios and environments that could have caused the behavior. Our approach leverages large language models to construct and expand a graph that captures cause-and-effect relationships and relevant entities with properties in the environment, which is then processed to configure a robot simulation environment. Our approach supports (i) augmenting simulations based on ego-agent behaviors, (ii) controllable, counterfactual scenario generation, (iii) reasoning about agent cognition and mental states, and (iv) reasoning with distinct sensing modalities, such as braking due to faulty GPS signals. We demonstrate our method in autonomous driving and robot manipulation tasks, generating more diverse, complex simulated environments compared to existing simulations with high success rates, and enabling controllable generation for corner cases. This approach enhances the validation of robot policies and supports data or simulation augmentation, advancing scalable robot learning for improved generalization and robustness.

Commonly used image tokenizers produce a 2D grid of spatially arranged tokens. In contrast, so-called 1D image tokenizers represent images as highly compressed one-dimensional sequences of as few as 32 discrete tokens. We find that the high degree of compression achieved by a 1D tokenizer with vector quantization enables image editing and generative capabilities through heuristic manipulation of tokens, demonstrating that even very crude manipulations – such as copying and replacing tokens between latent representations of images – enable fine-grained image editing by transferring appearance and semantic attributes. Motivated by the expressivity of the 1D tokenizer’s latent space, we construct an image generation pipeline leveraging gradient-based test-time optimization of tokens with plug-and-play loss functions such as reconstruction or CLIP similarity. Our approach is demonstrated for inpainting and text-guided image editing use cases, and can generate diverse and realistic samples without requiring training of any generative model.

The deployment of autonomous vehicles controlled by machine learning techniques requires extensive testing in diverse real-world environments, robust handling of edge cases and out-of-distribution scenarios, and comprehensive safety validation to ensure that these systems can navigate safely and effectively under unpredictable conditions. Addressing Out-OfDistribution (OOD) driving scenarios is essential for enhancing safety, as OOD scenarios help validate the reliability of the models within the vehicle's autonomy stack. However, generating OOD scenarios is challenging due to their long-tailed distribution and rarity in urban driving datasets. Recently, Large Language Models (LLMs) have shown promise in autonomous driving, particularly for their zero-shot generalization and common-sense reasoning capabilities. In this paper, we leverage these LLM strengths to introduce a framework for generating diverse OOD driving scenarios. Our approach uses LLMs to construct a branching tree, where each branch represents a unique OOD scenario. These scenarios are then simulated in the CARLA simulator using an automated framework that aligns scene augmentation with the corresponding textual descriptions. We evaluate our framework through extensive simulations, and assess its performance via a diversity metric that measures the richness of the scenarios. Additionally, we introduce a new “OOD-ness” metric, which quantifies how much the generated scenarios deviate from typical urban driving conditions. Furthermore, we explore the capacity of modern Vision-Language Models (VLMs) to interpret and safely navigate through the simulated OOD scenarios. Our findings offer valuable insights into the reliability of language models in addressing OOD scenarios within the context of urban driving.

Diffusion models have been successfully applied to robotics problems such as manipulation and vehicle path planning. In this work, we explore their application to end-to-end navigation - including both perception and planning - by considering the problem of jointly performing global localization and path planning in known but arbitrary 2D environments. In particular, we introduce a diffusion model which produces collision-free paths in a global reference frame given an egocentric LIDAR scan, an arbitrary map, and a desired goal position. To this end, we implement diffusion in the space of paths in$\text{SE}(2)$, and describe how to condition the denoising process on both obstacles and sensor observations. In our evaluation, we show that the proposed conditioning techniques enable generalization to realistic maps of considerably different appearance than the training environment, demonstrate our model's ability to accurately describe ambiguous solutions, and run extensive simulation experiments showcasing our model's use as a real-time, end-to-end localization and planning stack.

This paper studies high-speed online planning in dynamic environments. The problem requires finding time-optimal trajectories that conform to system dynamics, meeting computational constraints for real-time adaptation, and accounting for uncertainty from environmental changes. To address these challenges, we propose a sampling-based online planning algorithm that leverages neural network inference to replace time-consuming nonlinear trajectory optimization, enabling rapid exploration of multiple trajectory options under uncertainty. The proposed method is applied to the drone interception problem, where a defense drone must intercept a target while avoiding collisions and handling imperfect target predictions. The algorithm efficiently generates trajectories toward multiple potential target drone positions in parallel. It then assesses trajectory reachability by comparing traversal times with the target drone's predicted arrival time, ultimately selecting the minimum-time reachable trajectory. Through extensive validation in both simulated and real-world environments, we demonstrate our method's capability for high-rate online planning and its adaptability to unpredictable movements in unstructured settings.

The paper considers the route planning problem for a vehicle with limited capacity operating in a road network. The vehicle is assigned a set of transportation requests that are more complex than traveling between two locations, may involve dependencies between their sub-tasks, and include deadlines and priorities. The requests arrive gradually over the deployment time-horizon, and thus replanning is needed for new requests. We address cases when not all requests can be serviced by their deadlines despite car sharing. We introduce multiple quality measures for plans that account for requests' delays with respect to deadlines and priorities. We formalize the problem as planning in a weighted transition system under syntactically co-safe LTL formulas. We develop an online planning and replanning algorithm based on the automata-based approach to least-violating plan synthesis and on translation to a Mixed Integer Linear Program (MILP). Furthermore, we show that the MILP reduces to graph search for a subclass of quality measures that satisfy a monotonicity property. We show the approach in simulations, including a case study on the mid-Manhattan road network over the span of 24 hours.

Efficiently navigating off-road environments presents a number of challenges arising from their unstructured nature. In the absence of high-fidelity maps, occlusions from obstacles and terrain lead to limited information available to inform planning decisions. Furthermore, resolution and latency limitations of real-world perception systems lead to potentially of degraded perception performance when traversing such environments at high speeds. We address these problems by proposing an algorithm which plans trajectories while anticipating future observations. In particular, we introduce a model which learns to predict the evolution of future riskmaps conditioned on the future path and speed profile of the vehicle. The model is trained in a self-supervised fashion using recordings of vehicle trajectories. We then present an algorithm which leverages a way to efficiently query the model along candidate paths and speed profiles to produce time-optimal trajectories while maintaining a bound on the future expected risk. We assess the predictive performance of our risk model through a comparison with real vehicle driving logs. Furthermore, our closed-loop simulations of several benchmark scenarios demonstrate how the behavior of our planner leads to qualitatively distinct trajectories, leading to improvements in both success rate and speed by up to 60%.

Rollout algorithms are renowned for their abilities to correct for the suboptimalities of offline-trained base policies. In the multiagent setting, performing online rollout can require an exponentially large number of optimizations with respect to the number of agents. One-agent-at-a-time algorithms offer computationally efficient approaches to guaranteed policy improvement; however, this improvement is with respect to a state value estimate derived from a potentially poor base policy. Monte Carlo tree search (MCTS) provably converges to the true state value estimates; however, the exponentially large search space often makes its online use limited. Here, we present the Multi-Level Action Tree Rollout (MLAT-R) algorithm. MLAT-R provides 1) provable improvement over a base policy, 2) policy improvement with respect to the true state value, 3) applicability to any number of agents, and 4) an action space that grows linearly with the number of agents rather than exponentially. In this paper, we outline the algorithm, sketch a proof of its improvement over a base policy, and evaluate its performance on a challenging problem for which the base policy cannot reach a terminal state. Despite the challenging experimental setup, our algorithm reached a terminal state in 86% of all experiments, compared to 31% for state-of-the-art one-agent-at-a-time algorithms. In experiments involving MCTS, MLAT-R reached a terminal state in 99% of experiments compared to 92% for MCTS. MLAT-R achieved these results while considering an exponentially smaller action space than MCTS.

Robots operating alongside humans often encounter unfamiliar environments that make autonomous task completion challenging. Though improving models and increasing dataset size can enhance a robot’s performance in unseen environments, data collection and model refinement may be impractical in every environment. Approaches that utilize human demonstrations through manual operation can aid in refinement and generalization, but often require significant data collection efforts to generate enough demonstration data to achieve satisfactory task performance. Interactive approaches allow for humans to provide correction to robot action in real time, but intervention policies are often based on explicit factors related to state and task understanding that may be difficult to generalize. Addressing these challenges, we train a lightweight interaction policy that allows robots to decide when to proceed autonomously or request expert assistance at estimated times of uncertainty. An implicit estimate of uncertainty is learned via evaluating the feature extraction capabilities of the robot’s visual navigation policy. By incorporating part-time human interaction, robots recover quickly from their mistakes, significantly improving the odds of task completion. Incorporating part-time interaction yields an increase in success of 0.38 with only a 0.3 expert interaction rate within the Habitat simulation environment using a simulated human expert. We further show success transferring this approach to a new domain with a real human expert, improving success from less than 0.1 with an autonomous agent to 0.92 with a 0.23 human interaction rate. This approach provides a practical means for robots to interact and learn from humans in real-world settings.

As autonomous driving technology matures, end-to-end methodologies have emerged as a leading strategy, promising seamless integration from perception to control via deep learning. However, existing systems grapple with challenges such as unexpected open set environments and the complexity of black-box models. At the same time, the evolution of deep learning introduces larger, multimodal foundational models, offering multi-modal visual and textual understanding. In this paper, we harness these multimodal foundation models to enhance the robustness and adaptability of autonomous driving systems. We introduce a method to extract nuanced spatial features from transformers and the incorporation of latent space simulation for improved training and policy debugging. We use pixel/patch-aligned feature descriptors to expand foundational model capabilities to create an end-to-end multimodal driving model, demonstrating unparalleled results in diverse tests. Our solution combines language with visual perception and achieves significantly greater robustness on out-of-distribution situations.

Modeling the energy consumption of a quadrotor involves complex electrical and physical dynamics, making it difficult to optimize. To address this challenge, this paper presents a multi-fidelity Gaussian process (MFGP) method that efficiently learns an accurate energy prediction model by combining many low-fidelity samples from a simple motor model with a few computationally expensive samples from a numerical battery simulation. We present extensive sample-efficiency experiments, demonstrating that a single-fidelity model often needs 10 times more high-fidelity data to match the accuracy achieved by the MFGP. The energy prediction model is then applied to a reinforcement learning (RL) agent, providing a reward signal to a minimum energy planning policy. The RL policy generates more energy efficient trajectories than those found by the minimum snap baseline method, achieving an average 3.6% energy reduction.

In recent years, Neural Radiance Fields (NeRF) have emerged as a powerful tool for 3D reconstruction and novel view synthesis. However, the computational cost of NeRF rendering and degradation in quality due to the presence of artifacts pose significant challenges for its application in real-time and robust robotic tasks, especially on embedded systems. This paper introduces a novel framework that integrates NeRF-derived localization information with Visual-Inertial Odometry (VIO) to provide a robust solution for real-time robotic navigation. By training an absolute pose regression network with augmented image data rendered from a NeRF and quantifying its uncertainty, our approach effectively counters positional drift and enhances system reliability. We also establish a mathematically sound foundation for combining visual inertial navigation with camera localization neural networks, considering uncertainty under a Bayesian framework. Experimental validation in a photorealistic simulation environment demonstrates significant improvements in accuracy compared to a conventional VIO approach.

In this work, we consider the problem of learning end to end perception to control for ground vehicles solely from aerial imagery. Photogrammetric simulators allow the synthesis of novel views through the transformation of pre-generated assets into novel views. However, they have a large setup cost, require careful collection of data and often human effort to create usable simulators. We use a Neural Radiance Field (NeRF) as an intermediate representation to synthesize novel views from the point of view of a ground vehicle. These novel viewpoints can then be used for several downstream autonomous navigation applications. In this work, we demonstrate the utility of novel view synthesis though the application of training a policy for end to end learning from images and depth data. In a traditional real to sim to real framework, the collected data would be transformed into a visual simulator which could then be used to generate novel views. In contrast, using a NeRF allows a compact representation and the ability to optimize over the parameters of the visual simulator as more data is gathered in the environment. We demonstrate the efficacy of our method in a custom built mini-city environment through the deployment of imitation policies on robotic cars. We additionally consider the task of place localization and demonstrate that our method is able to relocalize the car in the real world.

Generating varied scenarios through simulation is crucial for training and evaluating safety-critical systems, such as autonomous vehicles. Yet, the task of modeling the trajectories of other vehicles to simulate diverse and meaningful close interactions remains prohibitively costly. Adopting language descriptions to generate driving behaviors emerges as a promising strategy, offering a scalable and intuitive method for human operators to simulate a wide range of driving interactions. However, the scarcity of large-scale annotated language-trajectory data makes this approach challenging. To address this gap, we propose Text-to-Drive (T2D) to synthesize diverse driving behaviors via Large Language Models (LLMs). We introduce a knowledge-driven approach that operates in two stages. In the first stage, we employ the embedded knowledge of LLMs to generate diverse language descriptions of driving behaviors for a scene. Then, we leverage LLM’s reasoning capabilities to synthesize these behaviors in simulation. At its core, T2D employs an LLM to construct a state chart that maps low-level states to high-level abstractions. This strategy aids in downstream tasks such as summarizing low-level observations, assessing policy alignment with behavior description, and shaping the auxiliary reward, all without needing human supervision. With our knowledge-driven approach, we demonstrate that T2D generates more diverse trajectories compared to other baselines and offers a natural language interface that allows for interactive incorporation of human preference. Please check our website for more examples: here

Energy consumption of memory accesses dominates the compute energy in energy-constrained robots, which require a compact 3-D map of the environment to achieve autonomy. Recent mapping frameworks only focused on reducing the map size while incurring significant memory usage during map construction due to the multipass processing of each depth image. In this work, we present a memory-efficient continuous occupancy map, named GMMap, that accurately models the 3-D environment using a Gaussian mixture model (GMM). Memory-efficient GMMap construction is enabled by the single-pass compression of depth images into local GMMs, which are directly fused together into a globally-consistent map. By extending Gaussian Mixture Regression (GMR) to model unexplored regions, occupancy probability is directly computed from Gaussians. Using a low-power ARM Cortex A57 CPU, GMMap can be constructed in real time at up to 60 images/s. Compared with prior works, GMMap maintains high accuracy while reducing the map size by at least 56%, memory overhead by at least 88%, dynamic random-access memory (DRAM) access by at least 78%, and energy consumption by at least 69%. Thus, GMMap enables real-time 3-D mapping on energy-constrained robots.

Intelligent intersection managers can improve safety by detecting dangerous drivers or failure modes in autonomous vehicles, warning oncoming vehicles as they approach an intersection. In this work, we present FailureNet, a recurrent neural network trained end-to-end on trajectories of both nominal and reckless drivers in a scaled miniature city. FailureNet observes the poses of vehicles as they approach an intersection and detects whether a failure is present in the autonomy stack, warning cross-traffic of potentially dangerous drivers. FailureNet can accurately identify control failures, upstream perception errors, and speeding drivers, distinguishing them from nominal driving. The network is trained and deployed with autonomous vehicles in the MiniCity. Compared to speed or frequency-based predictors, FailureNet's recurrent neural network structure provides improved predictive power, yielding upwards of 84% accuracy when deployed on hardware.

Safety has been a key goal for autonomous driving since its inception, and we believe recognizing and responding to risk is a key component of safety. In this work, we aim to answer the question, “How can explainable risk representations be generated and used to produce risk-averse trajectories?” To answer this question, previous work uses risk metrics to formulate an optimization problem. In contrast, our work is based on research showing the usefulness of grids as a representation to generate image-based risk maps through a trained neural network. We propose a method of determining risk from a bird's eye view (BEV) of an autonomous vehicle's surroundings. Our method consists of (1) a risk map generator, which is trained to recognize risk associated with nearby agents and the map, (2) differentiable value iteration using the risk map to learn a policy, and (3) a trajectory sampler, which samples from this policy to generate a trajectory. We evaluate our planner in a close-loop manner and find improvements in its overall ability to mimic human driving while maintaining comparable safety statistics. Self-ablation also reveals the potential for fine-tuning the behavior of the planner given a designer's needs.

Semi-cooperative behaviors are intrinsic properties of human drivers and should be considered for autonomous driving. In addition, new autonomous planners can consider the social value orientation (SVO) of human drivers to generate socially-compliant trajectories. Yet the overall impact on traffic flow for this new class of planners remain to be understood. In this work, we present study of implicit semi-cooperative driving where agents deploy a game-theoretic version of iterative best response assuming knowledge of the SVOs of other agents. We simulate nominal traffic flow and investigate whether the proportion of prosocial agents on the road impact individual or system-wide driving performance. Experiments show that the proportion of prosocial agents has a minor impact on overall traffic flow and that benefits of semi-cooperation disproportionally affect egoistic and high-speed drivers.

This article proposes a novel algorithm for aerobatic trajectory generation for a vertical take-off and landing (VTOL) tailsitter flying wing aircraft. The algorithm differs from existing approaches for fixed-wing trajectory generation, as it considers a realistic six-degree-of-freedom (6-DOF) flight dynamics model, including aerodynamic equations. Using a global dynamics model enables the generation of aerobatics trajectories that exploit the entire flight envelope, allowing agile maneuvering through the stall regime, sideways uncoordinated flight, inverted flight, etc. The method uses the differential flatness property of the global tailsitter flying wing dynamics, which is derived in this work. By performing snap minimization in the differentially flat output space, a computationally efficient algorithm, suitable for online motion planning, is obtained. The algorithm is demonstrated in extensive flight experiments encompassing six aerobatic maneuvers, a time-optimal drone racing trajectory, and an airshowlike aerobatic sequence for three tailsitter aircraft.

Simulation has the potential to transform the development of robust algorithms for mobile agents deployed in safety-critical scenarios. However, the poor photorealism and lack of diverse sensor modalities of existing simulation engines remain key hurdles towards realizing this potential. Here, we present VISTA††Full code release for the VISTA data-driven simulation engine is available here: vista.csail.mit.edu., an open source, data-driven simulator that integrates multiple types of sensors for autonomous vehicles. Using high fidelity, real-world datasets, VISTA represents and simulates RGB cameras, 3D LiDAR, and event-based cameras, enabling the rapid generation of novel viewpoints in simulation and thereby enriching the data available for policy learning with corner cases that are difficult to capture in the physical world. Using VISTA, we demonstrate the ability to train and test perception-to-control policies across each of the sensor types and showcase the power of this approach via deployment on a full scale autonomous vehicle. The policies learned in VISTA exhibit sim-to-real transfer without modification and greater robustness than those trained exclusively on real-world data.

Temporal patterns (how vehicles behave in our observed past) underline our reasoning of how people drive on the road, and can explain why we make certain predictions about interactions among road agents. In this paper we propose the ConceptNet trajectory predictor - a novel prediction framework that is able to incorporate agent interactions as explicit edges in a temporal knowledge graph. We demonstrate the sample efficiency and the overall accuracy of the proposed approach, and show that using the graphical structure to explicitly model interactions enables better detection of agent interactions and improved trajectory predictions on a large real-world driving dataset.

Computing consumes a significant portion of energy in many robotics applications, especially the ones involving energy-constrained robots. In addition, memory access accounts for a significant portion of the computing energy. For mapping a 3D environment, prior approaches reduce the map size while incurring a large memory overhead used for storing sensor measurements and temporary variables during computation. In this work, we present a memory-efficient algorithm, named Single-Pass Gaussian Fitting (SPGF), that accurately constructs a compact Gaussian Mixture Model (GMM) which approximates measurements from a depthmap generated from a depth camera. By incrementally constructing the GMM one pixel at a time in a single pass through the depthmap, SPGF achieves higher throughput and orders-of-magnitude lower memory overhead than prior multipass approaches. By processing the depthmap row-by-row, SPGF exploits intrinsic properties of the camera to efficiently and accurately infer surface geometries, which leads to higher precision than prior approaches while maintaining the same compactness of the GMM. Using a low-power ARM Cortex-A57 CPU on the NVIDIA Jetson TX2 platform, SPGF operates at 32fps, requires 43KB of memory overhead, and consumes only 0.11J per frame (depthmap). Thus, SPGF enables real-time mapping of large 3D environments on energy-constrained robots.

Deployment of deep neural networks (DNNs) for monocular depth estimation in safety-critical scenarios on resource-constrained platforms requires well-calibrated and efficient uncertainty estimates. However, many popular uncertainty estimation techniques, including state-of-the-art ensembles and popular sampling-based methods, require multiple inferences per input, making them difficult to deploy in latency-constrained or energy-constrained scenarios. We propose a new algorithm, called Uncertainty from Motion (UfM), that requires only one inference per input. UfM exploits the temporal redundancy in video inputs by merging incrementally the per-pixel depth prediction and per-pixel aleatoric uncertainty prediction of points that are seen in multiple views in the video sequence. When UfM is applied to ensembles, we show that UfM can retain the uncertainty quality of ensembles at a fraction of the energy by running only a single ensemble member at each frame and fusing the uncertainty over the sequence of frames. In a set of representative experiments using FCDenseNet and eight indistribution and out-of-distribution video sequences, UfM offers comparable uncertainty quality to an ensemble of size 10 while consuming only 11.3% of the ensemble's energy and running 6.4× faster on a single Nvidia RTX 2080 Ti GPU, enabling near ensemble uncertainty quality for resource-constrained, real-time scenarios.

Data-driven simulators promise high data-efficiency for driving policy learning. When used for modelling interactions, this data-efficiency becomes a bottleneck: small underlying datasets often lack interesting and challenging edge cases for learning interactive driving. We address this challenge by proposing a data-driven simulation engine† that uses inpainted ado vehicles for learning robust driving policies. Thus, our approach can be used to learn policies that involve multi-agent interactions and allows for training via state-of-the-art policy learning methods. We evaluate the approach for learning standard interaction scenarios in driving. In extensive experiments, our work demonstrates that the resulting policies can be directly transferred to a full-scale autonomous vehicle without making use of any traditional sim-to-real transfer techniques such as domain randomization.

In this paper, we present a motion planning framework for multi-modal vehicle dynamics. Our proposed algorithm employs transcription of the optimization objective function, vehicle dynamics, and state and control constraints into sparse factor graphs, which—combined with mode transition constraints—constitute a composite pose graph. By formulating the multi-modal motion planning problem in composite pose graph form, we enable utilization of efficient techniques for optimization on sparse graphs, such as those widely applied in dual estimation problems, e.g., simultaneous localization and mapping (SLAM). The resulting motion planning algorithm optimizes the multi-modal trajectory, including the location of mode transitions, and is guided by the pose graph optimization process to eliminate unnecessary transitions, enabling efficient discovery of optimized mode sequences from rough initial guesses. We demonstrate multi-modal trajectory optimization in both simulation and real-world experiments for vehicles with various dynamics models, such as an airplane with taxi and flight modes, and a vertical take-off and landing (VTOL) fixed-wing aircraft that transitions between hover and horizontal flight modes.

This work addresses the problem of 6-DoF pose estimation under heavy occlusion. While previous work demonstrates reasonable results in unoccluded situations, robust and efficient pose estimation is still challenging in heavily occluded and low-texture scenarios which are ubiquitous in many applications. To this end, we propose a novel end-to-end deep neural network model recovering object poses from depth measurements. The proposed model enforces pairwise consistency of 3D geometric features by applying spectral convolutions on a pairwise compatibility graph. We achieve comparable accuracy as the state-of-the-art graph matching solver while being much faster. Our approach outperforms state-of-the-art 6-DoF pose estimation methods on LineMOD and Occlusion LineMOD and runs in reasonable time (~5.9 Hz). We additionally verify this method on a synthetic dataset with large affine changes.

Deep learning has been used to demonstrate end-to-end neural network learning for autonomous vehicle control from raw sensory input. While LiDAR sensors provide reliably accurate information, existing end-to-end driving solutions are mainly based on cameras since processing 3D data requires a large memory footprint and computation cost. On the other hand, increasing the robustness of these systems is also critical; however, even estimating the model’s uncertainty is very challenging due to the cost of sampling-based methods. In this paper, we present an efficient and robust LiDAR-based end-to-end navigation framework. We first introduce Fast-LiDARNet that is based on sparse convolution kernel optimization and hardware-aware model design. We then propose Hybrid Evidential Fusion that directly estimates the uncertainty of the prediction from only a single forward pass and then fuses the control predictions intelligently. We evaluate our system on a full-scale vehicle and demonstrate lane-stable as well as navigation capabilities. In the presence of out-of-distribution events (e.g., sensor failures), our system significantly improves robustness and reduces the number of takeovers in the real world.

Autonomous control of an emergency vehicle will save lives through faster transport and shorter response. Towards this goal, it must overcome the challenge of inter- acting with existing human drivers on the road. We present a game-theoretic approach for semi-cooperative control of an autonomous emergency vehicle that can interact efficiently with humans on the road. We model the interactions between autonomous and human driven cars with Social Value Orientation, a metric from social psychology, that allows the controller to leverage their influence on the trajectories of neighboring human drivers. In addition, by using a modified version of iterative best response, we direct the algorithm to converge to Nash equilibria that are cooperative. We demonstrate the efficacy of our algorithm in simulations of drivers in traffic, with a variety of traffic densities and driver personalities. In simulations of prosocial human drivers, our algorithm provides an 8% improvement in distance-traveled compared to egoistic human drivers.

Exploration tasks are essential to many emerging robotics applications, ranging from search and rescue to space exploration. The planning problem for exploration requires determining the best locations for future measurements that will enhance the fidelity of the map, for example, by reducing its total entropy. A widely-studied technique involves computing the Mutual Information (MI) between the current map and future measurements, and utilizing this MI metric to decide the locations for future measurements. However, computing MI for reasonably-sized maps is slow and power hungry, which has been a bottleneck towards fast and efficient robotic exploration. In this paper, we introduce a new hardware accelerator architecture for MI computation that features a low-latency, energy-efficient MI compute core and an optimized memory subsystem that provides sufficient bandwidth to keep the cores fully utilized. The core employs interleaving to counter the recursive algorithm, and workload balancing and numerical approximations to reduce latency and energy consumption. We demonstrate this optimized architecture with a Field-Programmable Gate Array (FPGA) implementation, which can compute MI for all cells in an entire 201-by-201 occupancy grid (e.g., representing a 20.1m-by-20.1m map at 0.1m resolution) in 1.55 ms while consuming 1.7 mJ of energy, thus finally rendering MI computation for the whole map real time and at a fraction of the energy cost of traditional compute platforms. For comparison, this particular FPGA implementation running on the Xilinx Zynq-7000 platform is two orders of magnitude faster and consumes three orders of magnitude less energy per MI map compute, when compared to a baseline GPU implementation running on an NVIDIA GeForce GTX 980 platform. The improvements are more pronounced when compared to CPU implementations of equivalent algorithms.

Information gathering while interacting with other agents under sensing and motion uncertainty is critical in domains such as driving, service robots, racing, or surveillance. The interests of agents may be at odds with others, resulting in a stochastic noncooperative dynamic game. Agents must predict others’ future actions without communication, incorporate their actions into these predictions, account for uncertainty and noise in information gathering, and consider what information their actions reveal. Our solution uses local iterative dynamic programming in Gaussian belief space to solve a game-theoretic continuous POMDP. Solving a quadratic game in the backward pass of a game-theoretic belief-space variant of iterative linear-quadratic Gaussian control (iLQG) achieves a runtime polynomial in the number of agents and linear in the planning horizon. Our algorithm yields linear feedback policies for our robot, and predicted feedback policies for other agents. We present three applications: Active surveillance, guiding eyes for a blind agent, and autonomous racing. Agents with game-theoretic belief-space planning win 44% more races than without game theory and 34% more than without belief-space planning.

The safety of an autonomous vehicle not only depends on its own perception of the world around it, but also on the perception and recognition from other vehicles. If an ego vehicle considers the uncertainty other vehicles have about itself, then by reducing the estimated uncertainty it can increase its safety. In this paper, we focus on how an ego vehicle plans its trajectories through the blind spots of other vehicles. We create visibility-aware planning, where the ego vehicle chooses its trajectories such that it reduces the perceived uncertainty other vehicles may have about the state of the ego vehicle. We present simulations of traffic and highway environments, where an ego vehicle must pass another vehicle, make a lane change, or traverse a partially-occluded intersection. Emergent behavior shows that when using visibility-aware planning, the ego vehicle spends less time in a blind spot, and may slow down before entering the blind spot so as to increase the likelihood other vehicles perceive the ego vehicle.

Exploration of unknown environments is embedded and essential in many robotics applications. Traditional algorithms, that decide where to explore by computing the expected information gain of an incomplete map from future sensor measurements, are limited to very powerful computational platforms. In this paper, we describe a novel approach for computing this expected information gain efficiently, as principally derived via mutual information. The key idea behind the proposed approach is a continuous occupancy map framework and the recursive structure it reveals. This structure makes it possible to compute the expected information gain of sensor measurements across an entire map much faster than computing each measurements’ expected gain independently. Specifically, for an occupancy map composed of |M| cells and a range sensor that emits |Θ| measurement beams, the algorithm (titled FCMI) computes the information gain corresponding to measurements made at each cell in O(|Θ||M|) steps. To the best of our knowledge, this complexity bound is better than all existing methods for computing information gain. In our experiments, we observe that this novel, continuous approach is two orders of magnitude faster than the state-of-the-art FSMI algorithm.

This paper introduces the Weighted Buffered Voronoi tessellation, which allows us to define distributed, semicooperative multi-agent navigation policies with guarantees on collision avoidance. We generate the Voronoi cells with dynamic weights that bias the boundary towards the agent with the lower relative weight while always maintaining a buffered distance between two agents. By incorporating agent weights, we can encode selfish or prioritized behavior among agents, where a more selfish agent will have a larger relative cell over less selfish agents. We consider this semi-cooperative since agents do not cooperate in symmetric ways. Furthermore, when all agents start in a collision-free configuration and plan their control actions within their cells, we prove that no agents will collide. Simulations demonstrate the performance of our algorithm for agents navigating to goal locations in a position-swapping game. We observe that agents with more egoistic weights consistently travel shorter paths to their goal than more altruistic agents.

The increasing popularity of quadrotors has given rise to a class of predominantly vision-driven vehicles. This paper addresses the problem of perception-aware time optimal path parametrization for quadrotors. Although many different choices of perceptual modalities are available, the low weight and power budgets of quadrotor systems makes a camera ideal for on-board navigation and estimation algorithms. However, this does come with a set of challenges. The limited field of view of the camera can restrict the visibility of salient regions in the environment, which dictates the necessity to consider perception and planning jointly. The main contribution of this paper is an efficient time optimal path parametrization algorithm for quadrotors with limited field of view constraints. We show in a simulation study that a state-of-the-art controller can track planned trajectories, and we validate the proposed algorithm on a quadrotor platform in experiments.

We study a novel class of motion planning problems, inspired by emerging low-energy robotic vehicles, such as insect-size flyers, chip-size satellites, and high-endurance autonomous blimps, for which the energy consumed by computing hardware during planning a path can be as large as the energy consumed by actuation hardware during the execution of the same path. We propose a new algorithm, called Compute Energy Included Motion Planning (CEIMP). CEIMP operates similarly to any other anytime planning algorithm, except it stops when it estimates further computing will require more computing energy than potential savings in actuation energy. We show that CEIMP has the same asymptotic computational complexity as existing sampling-based motion planning algorithms, such as PRM*. We also show that CEIMP outperforms the average baseline of using maximum computing resources in realistic computational experiments involving 10 floor plans from MIT buildings. In one representative experiment, CEIMP outperforms the average baseline 90.6% of the time when energy to compute one more second is equal to the energy to move one more meter, and 99.7% of the time when energy to compute one more second is equal to or greater than the energy to move 3 more meters.

We study a problem in vision-aided navigation in which an autonomous agent has to traverse a specified path in minimal time while ensuring extraction of a steady stream of visual percepts with low latency. Vision-aided robots extract motion estimates from the sequence of images of their on-board cameras by registering the change in bearing to landmarks in their environment. The computational burden of the latter procedure grows with the range of apparent motion undertaken by the projections of the landmarks, incurring a lag in pose estimates that should be minimized while navigating at high speeds. This paper addresses the problem of selecting a desired number of landmarks in the environment, together with the time parametrization of the path, to allow the agent execute it in minimal time while both (i) ensuring the computational burden of extracting motion estimates stays below a set threshold and (ii) respecting the actuation constraints of the agent. We provide two efficient approximation algorithms for addressing the aforementioned problem. Also, we show how it can be reduced to a mixed integer linear program for which there exist well-developed optimization packages. Ultimately, we illustrate the performance of our algorithms in experiments using a quadrotor.

Futuristic communication network formed by autonomously operated, unmanned aerial vehicles, has piqued researchers interests in highly mobile wireless networks. Exchanging safety critical information, with low latency and high throughput, in such systems is of paramount importance. We study the broadcast capacity and minimum delay scaling laws for such highly mobile wireless networks, in which each node has to disseminate packets to all other nodes in the network. In particular, we consider a cell partitioned network under an IID mobility model, in which each node chooses a new position at random, every time slot. We derive scaling laws for broadcast capacity and minimum delay as a function of the network size. We propose a simple first-come-first-serve flooding scheme, which nearly achieve both capacity and minimum delay scaling. Thus, in contrast to what has been speculated in the literature, we show that there is nearly no tradeoff between capacity and delay. Our results also show that high mobility does not improve broadcast capacity. Our analysis makes use of the theory of Markov Evolving Graphs (MEGs), and develops two new bounds on flooding time in MEGs by relaxing the previously required expander property assumption. Simulation results verify our analysis, and throw up interesting open problems.

Age of information (AoI) is a recently proposed metric for measuring information freshness. AoI measures the time that elapsed since the last received update was generated. We consider the problem of minimizing average and peak AoI in a wireless networks, consisting of a set of source-destination links, under general interference constraints. When fresh information is always available for transmission, we show that a stationary scheduling policy is peak age optimal. We also prove that this policy achieves average age that is within a factor of two of the optimal average age. In the case where fresh information is not always available, and packet/information generation rate has to be controlled along with scheduling links for transmission, we prove an important separation principle: the optimal scheduling policy can be designed assuming fresh information, and independently, the packet generation rate control can be done by ignoring interference. Peak and average AoI for discrete time G/Ber/1 queue is analyzed for the first time, which may be of independent interest.

Age of information (AoI), defined as the time that elapsed since the last received update was generated, is a newly proposed metric to measure the timeliness of information updates in a network. We consider AoI minimization problem for a network with general interference constraints, and time varying channels. We propose two policies, namely, virtual-queue based policy and age-based policy when the channel state is available to the network scheduler at each time step. We prove that the virtual-queue based policy is nearly optimal, up to a constant additive factor, and the age-based policy is at-most a factor of 4 away from optimality. Comparison with previous work, which derived age optimal policies when channel state information is not available to the scheduler, demonstrates significant improvement in age due to the availability of channel state information. Our analysis relies on the age conservation law and age-square conservation law developed in this paper, which hold more generally and may be of independent interest.

Deep learning has revolutionized the ability to learn “end-to-end” autonomous vehicle control directly from raw sensory data. While there have been recent extensions to handle forms of navigation instruction, these works are unable to capture the full distribution of possible actions that could be taken and to reason about localization of the robot within the environment. In this paper, we extend end-to-end driving networks with the ability to perform point-to-point navigation as well as probabilistic localization using only noisy GPS data. We define a novel variational network capable of learning from raw camera data of the environment as well as higher level roadmaps to predict (1) a full probability distribution over the possible control commands; and (2) a deterministic control command capable of navigating on the route specified within the map. Additionally, we formulate how our model can be used to localize the robot according to correspondences between the map and the observed visual road topology, inspired by the rough localization that human drivers can perform. We test our algorithms on real-world driving data that the vehicle has never driven through before, and integrate our point-topoint navigation algorithms onboard a full-scale autonomous vehicle for real-time performance. Our localization algorithm is also evaluated over a new set of roads and intersections to demonstrates rough pose localization even in situations without any GPS prior.

Depth completion, the technique of estimating a dense depth image from sparse depth measurements, has a variety of applications in robotics and autonomous driving. However, depth completion faces 3 main challenges: the irregularly spaced pattern in the sparse depth input, the difficulty in handling multiple sensor modalities (when color images are available), as well as the lack of dense, pixel-level ground truth depth labels for training. In this work, we address all these challenges. Specifically, we develop a deep regression model to learn a direct mapping from sparse depth (and color images) input to dense depth prediction. We also propose a self-supervised training framework that requires only sequences of color and sparse depth images, without the need for dense depth labels. Our experiments demonstrate that the self-supervised framework outperforms a number of existing solutions trained with semi-dense annotations. Furthermore, when trained with semi-dense annotations, our network attains state-of-the-art accuracy and is the winning approach on the KITTI depth completion benchmark at the time of submission.

In this paper, we examine the problem of navigating cluttered environments without explicit object detection and tracking. We introduce the dynamic risk density to map the congestion density and spatial flow of the environment to a cost function for the agent to determine risk when navigating that environment. We build upon our prior work, wherein the agent maps the density and motion of objects to an occupancy risk, then navigate the environment over a specified risk level set. Here, the agent does not need to identify objects to compute the occupancy risk, and instead computes this cost function using the occupancy density and velocity fields around them. Simulations show how this dynamic risk density encodes movement information for the ego agent and closely models the object-based congestion cost. We implement our dynamic risk density on an autonomous wheelchair and show how it can be used for navigating unstructured, crowded and cluttered environments.

Depth sensing is a critical function for robotic tasks such as localization, mapping and obstacle detection. There has been a significant and growing interest in depth estimation from a single RGB image, due to the relatively low cost and size of monocular cameras. However, state-of-the-art single-view depth estimation algorithms are based on fairly complex deep neural networks that are too slow for real-time inference on an embedded platform, for instance, mounted on a micro aerial vehicle. In this paper, we address the problem of fast depth estimation on embedded systems. We propose an efficient and lightweight encoder-decoder network architecture and apply network pruning to further reduce computational complexity and latency. In particular, we focus on the design of a low-latency decoder. Our methodology demonstrates that it is possible to achieve similar accuracy as prior work on depth estimation, but at inference speeds that are an order of magnitude faster. Our proposed network, FastDepth, runs at 178 fps on an NVIDIA Jetson TX2 GPU and at 27 fps when using only the TX2 CPU, with active power consumption under 10 W. FastDepth achieves close to state-of-the-art accuracy on the NYU Depth v2 dataset. To the best of the authors' knowledge, this paper demonstrates real-time monocular depth estimation using a deep neural network with the lowest latency and highest throughput on an embedded platform that can be carried by a micro aerial vehicle.

Information-based mapping algorithms are critical to robot exploration tasks in several applications ranging from disaster response to space exploration. Unfortunately, most existing information-based mapping algorithms are plagued by the computational difficulty of evaluating the Shannon mutual information between potential future sensor measurements and the map. This has lead researchers to develop approximate methods, such as Cauchy-Schwarz Quadratic Mutual Information (CSQMI). In this paper, we propose a new algorithm, called Fast Shannon Mutual Information (FSMI), which is significantly faster than existing methods at computing the exact Shannon mutual information. The key insight behind FSMI is recognizing that the integral over the sensor beam can be evaluated analytically, removing an expensive numerical integration. In addition, we provide a number of approximation techniques for FSMI, which significantly improve computation time. Equipped with these approximation techniques, the FSMI algorithm is more than three orders of magnitude faster than the existing computation for Shannon mutual information; it also outperforms the CSQMI algorithm significantly, being roughly twice as fast, in our experiments.

Current methods for autonomous management use strict first-come, first-serve (FCFS) ordering to manage incoming autonomous vehicles at an intersection. In this work, we present a coordination policy that swaps agent ordering to increase the system-wide performance while ensuring that the swaps are socially compliant. By considering an agent's Social Value Orientation (SVO), a social psychology metric for their willingness to help another vehicle, the central coordinator can reduce system delays while ensuring each individual vehicle increases their own utility. The FCFS-SVO algorithm is both computationally tractable and accounts for a variety of real-world agent types, such as human drivers and a variety of social orientations. Simulation results show that average vehicle delays decrease with swapping by enabling cooperation between agents. In addition, we show that the proportion of human drivers, as well as, the distribution of prosocial and egoistic vehicles in the system can have a prominent effect on the performance of the system.

FlightGoggles is a photorealistic sensor simulator for perception-driven robotic vehicles. The key contributions of FlightGoggles are twofold. First, FlightGoggles provides photorealistic exteroceptive sensor simulation using graphics assets generated with photogrammetry. Second, it provides the ability to combine (i) synthetic exteroceptive measurements generated in silico in real time and (ii) vehicle dynamics and proprioceptive measurements generated in motio by vehicle(s) in flight in a motion-capture facility. FlightGoggles is capable of simulating a virtual-reality environment around autonomous vehicle(s) in flight. While a vehicle is in flight in the Flight-Goggles virtual reality environment, exteroceptive sensors are rendered synthetically in real time while all complex dynamics are generated organically through natural interactions of the vehicle. The FlightGoggles framework allows for researchers to accelerate development by circumventing the need to estimate complex and hard-to-model interactions such as aerodynamics, motor mechanics, battery electrochemistry, and behavior of other agents. The ability to perform vehicle-in-the-loop experiments with photorealistic exteroceptive sensor simulation facilitates novel research directions involving, e.g., fast and agile autonomous flight in obstacle-rich environments, safe human interaction, and flexible sensor selection. FlightGoggles has been utilized as the main test for selecting nine teams that will advance in the AlphaPilot autonomous drone racing challenge. We survey approaches and results from the top AlphaPilot teams, which may be of independent interest. FlightGoggles is distributed as open-source software along with the photorealistic graphics assets for several simulation environments, under the MIT license at http://flightgoggles.mit.edu.

Current perception systems mostly require direct line of sight to anticipate and ultimately prevent potential collisions at intersections with other road users. We present a fully integrated autonomous system capable of detecting shadows or weak illumination changes on the ground caused by a dynamic obstacle in NLoS scenarios. This additional virtual sensor “ShadowCam” extends the signal range utilized so far by computer-vision ADASs. We show that (1) our algorithm maintains the mean classification accuracy of around 70% even when it doesn't rely on infrastructure - such as AprilTags - as an image registration method. We validate (2) in real-world experiments that our autonomous car driving in night time conditions detects a hidden approaching car earlier with our virtual sensor than with the front facing 2-D LiDAR.

Age-of-information (AoI) is a newly proposed performance metric of information freshness. It differs from the traditional delay metric, because it is destination centric and measures the time that elapsed since the last received fresh information update was generated at the source. We show that AoI and packet delay differ in a fundamental way in certain systems, i.e. minimizing one can imply maximizing the other. We consider two queueing systems, namely a single server last come first serve queue with preemptive service (LCFSp) and G/G/∞ queue, and show that a heavy tailed service distribution, that results in the worst case packet delay or variance in packet delay, respectively, minimizes AoI. For the specific case of M/G/1 LCFSp and G/G/∞ queue, we also prove that deterministic service, that minimizes packet delay and variance in packet delay, respectively, results in the worst case AoI.

This paper examines how vehicles can quickly quantify the level of congestion in their environment for planning. We use risk level sets to define a metric of congestion for the vehicles. Using this metric, we can quickly identify distributions of environment and driver features, such as velocities and number of neighbors, based on risk within human driving data sets. We use the NGSIM and highD data sets to study how risk influences behaviors in city and highway driving. From these data sets, we learn common risk thresholds for classifying low, medium, and high-risk situations. Using these thresholds, we develop simulations of an autonomous vehicle driving along a highway, and demonstrate how the chosen risk threshold influences the autonomous vehicle behavior.

Deep learning has been successfully applied to “end-to-end” learning of the autonomous driving task, where a deep neural network learns to predict steering control commands from camera data input. However, the learned representations do not support higher-level decision making required for autonomous navigation, nor the uncertainty estimates required for parallel autonomy, where vehicle control is shared between human and robot. This paper tackles the problem of learning a representation to predict a continuous control probability distribution, and thus steering control options and bounds for those options, which can be used for autonomous navigation. Each mode of the distribution encodes a possible macro-action that the system could execute at that instant, and the covariances of the modes place bounds on safe steering control values. Our approach has the added advantage of being trained on unlabeled data collected from inexpensive cameras. The deep neural network based algorithm generates a probability distribution over the space of steering angles, from which we leverage Variational Bayesian methods to extract a mixture model and compute the different possible actions in the environment. A bound, which the autonomous vehicle must respect in our parallel autonomy setting, is then computed for each of these actions. We evaluate our approach on a challenging dataset containing a wide variety of driving conditions, and show that our algorithm is capable of parameterizing Gaussian Mixture Models for possible actions, and extract steering bounds with a mean error of only 2 degrees. Additionally, we demonstrate our system working on a full scale autonomous vehicle and evaluate its ability to successful handle various different parallel autonomy situations.

Autonomous driving requires 3D perception of vehicles and other objects in the in environment. Much of the current methods support 2D vehicle detection. This paper proposes a flexible pipeline to adopt any 2D detection network and fuse it with a 3D point cloud to generate 3D information with minimum changes of the 2D detection networks. To identify the 3D box, an effective model fitting algorithm is developed based on generalised car models and score maps. A two-stage convolutional neural network (CNN) is proposed to refine the detected 3D box. This pipeline is tested on the KITTI dataset using two different 2D detection networks. The 3D detection results based on these two networks are similar, demonstrating the flexibility of the proposed pipeline. The results rank second among the 3D detection algorithms, indicating its competencies in 3D detection.

In this paper we consider a fleet of self-driving cars operating in a road network governed by rules of the road, such as the Vienna Convention on Road Traffic, providing rides to customers to serve their demands with desired deadlines. We focus on the associated motion planning problem that trades-off the demands' delays and level of violation of the rules of the road to achieve social optimum among the vehicles. Due to operating in the same environment, the interaction between the cars must be taken into account, and can induce further delays. We propose an integrated route and motion planning approach that achieves scalability with respect to the number of cars by resolving potential collision situations locally within so-called bubble spaces enclosing the conflict. The algorithms leverage the road geometries, and perform joint planning only for lead vehicles in the conflict and use queue scheduling for the remaining cars. Furthermore, a framework for storing previously resolved conflict situations is proposed, which can be use for quick querying of joint motion plans. We show the mobility-on-demand setup and effectiveness of the proposed approach in simulated case studies involving up to 10 self-driving vehicles.

We consider the problem of dense depth prediction from a sparse set of depth measurements and a single RGB image. Since depth estimation from monocular images alone is inherently ambiguous and unreliable, to attain a higher level of robustness and accuracy, we introduce additional sparse depth samples, which are either acquired with a low-resolution depth sensor or computed via visual Simultaneous Localization and Mapping (SLAM) algorithms. We propose the use of a single deep regression network to learn directly from the RGB-D raw data, and explore the impact of number of depth samples on prediction accuracy. Our experiments show that, compared to using only RGB images, the addition of 100 spatially random depth samples reduces the prediction root-mean-square error by 50% on the NYU-Depth-v2 indoor dataset. It also boosts the percentage of reliable prediction from 59 % to 92 % on the KITTI dataset. We demonstrate two applications of the proposed algorithm: a plug-in module in SLAM to convert sparse maps to dense maps, and super-resolution for LiDARs. Software22https://github.com/fangchangma/sparse-to-dense and video demonstration33https://www.youtube.com/watch?v=vNIIT_M7×7Y are publicly available.

In this paper, we address the problem of navigating in a cluttered environment by introducing a congestion cost that maps the density and motion of objects to an occupancy risk. We propose that an agent can choose a “risk level set” from this cost function and construct a planning space from this set. In choosing different levels of risk, the agent adjusts its interactions with the other agents. From the assumption that agents are self-preserving, we show that any agent planning within their risk level set will avoid collisions with other agents. We then present an application of planning with risk level sets in the framework of an autonomous vehicle driving along a highway. Using the risk level sets, the agent can determine safe zones when planning a sequence of lane changes. Through simulations in Matlab, we demonstrate how the choice of risk threshold manifests as aggressive or conservative behavior.

The development of fast, agile micro Unmanned Aerial Vehicles (UAVs) has been limited by (i) on-board computing hardware restrictions, (ii) the lack of sophisticated vision-based perception and vision-in-the-loop control algorithms, and (iii) the absence of development environments where such systems and algorithms can be rapidly and easily designed, implemented, and validated. Here, we first present a new micro UAV platform that integrates high-rate cameras, inertial sensors, and an NVIDIA Jetson Tegra X1 system-on-chip compute module that boasts 256 GPU cores. The UAV mechanics and electronics were designed and built in house, and are described in detail. Second, we present a novel “virtual reality” development environment, in which photorealistically-rendered synthetic on-board camera images are generated in real time while the UAV is in flight. This development environment allows us to rapidly prototype computing and sensing hardware as well as perception and control algorithms, using real physics, real interoceptive sensor data (e.g., from the on-board inertial measurement unit), and synthetic exteroceptive sensor data (e.g., from synthetic cameras). Third, we demonstrate repeated agile maneuvering with closed-loop vision-based perception and control algorithms, which we have developed using this environment.

Localization is an essential component for autonomous robots. A well-established localization approach combines ray casting with a particle filter, leading to a computationally expensive algorithm that is difficult to run on resource-constrained mobile robots. We present a novel data structure called the Compressed Directional Distance Transform for accelerating ray casting in two dimensional occupancy grid maps. Our approach allows online map updates, and near constant time ray casting performance for a fixed size map, in contrast with other methods exhibit poor worst case performance. Our experimental results show that the proposed algorithm approximates the performance characteristics of reading from a three dimensional lookup table of ray cast solutions while requiring two orders of magnitude less memory and precomputation. This results in a particle filter algorithm which can maintain 2500 particles with 61 ray casts per particle at 40Hz, using a single CPU thread onboard a mobile robot.

This paper introduces a new method for end-to-end training of deep neural networks (DNNs) and evaluates it in the context of autonomous driving. DNN training has been shown to result in high accuracy for perception to action learning given sufficient training data. However, the trained models may fail without warning in situations with insufficient or biased training data. In this paper, we propose and evaluate a novel architecture for self-supervised learning of latent variables to detect the insufficiently trained situations. Our method also addresses training data imbalance, by learning a set of underlying latent variables that characterize the training data and evaluate potential biases. We show how these latent distributions can be leveraged to adapt and accelerate the training pipeline by training on only a fraction of the total dataset. We evaluate our approach on a challenging dataset for driving. The data is collected from a full-scale autonomous vehicle. Our method provides qualitative explanation for the latent variables learned in the model. Finally, we show how our model can be additionally trained as an end-to-end controller, directly outputting a steering control command for an autonomous vehicle.

We consider pursuit-evasion games in which the pursuer is tasked with intercepting the evader using only partial measurements. Motivated by the utilization of visual sensing on board the pursuer, we focus on the case when only bearing measurements are available to the pursuer. The resulting partially-observable interception problem is computationally challenging, and the separation principle does not hold in general. In this paper, we identify a set of maneuvers that improve observability, and we propose an algorithm that utilizes these maneuvers to move the pursuer so that the expected payoff of the differential game is maximized. The algorithm uses in-the-loop uncertainty propagation based on linear covariance analysis to assess the effect of the maneuvers. We evaluate the resulting guidance law in experiments involving a quadcopter in flight representing the pursuer, and a simulated evader.

Most existing motion planning algorithms assume that a map (of some quality) is fully determined prior to generating a motion plan. In many emerging applications of robotics, e.g., fast-moving agile aerial robots with constrained embedded computational platforms and visual sensors, dense maps of the world are not immediately available, and they are computationally expensive to construct. We propose a new algorithm for generating plan graphs which couples the perception and motion planning processes for computational efficiency. In a nutshell, the proposed algorithm iteratively switches between the planning sub-problem and the mapping sub-problem, each updating based on the other until a valid trajectory is found. The resulting trajectory retains a provable property of providing an optimal trajectory with respect to the full (unmapped) environment, while utilizing only a fraction of the sensing data in computational experiments.

Age of information (AoI) is a recently proposed metric that measures the time elapsed since the generation of the last received information update. We consider the problem of AoI minimization for a network under general interference constraints, and time varying channel. We study the case where the channel statistics are known, but the current channel state is unknown. We propose two scheduling policies, namely, the virtual queue based policy and age-based policy. In the virtual queue based policy, the scheduler schedules links with maximum weighted sum of the virtual queue lengths, while in the age-based policy, the scheduler schedules links with maximum weighted sum of a function of link AoI. We prove that the virtual queue based policy is peak age optimal, up to an additive constant, while the age-based policy is at most factor 4 away from the optimal age. Numerical results suggest that both the proposed policies are, in fact, very close to the optimal.

In this work, we present new theoretical results on convolutional generative neural networks, in particular their invertibility (i.e., the recovery of input latent code given the network output). The study of network inversion problem is motivated by image inpainting and the mode collapse problem in training GAN. Network inversion is highly non-convex, and thus is typically computationally intractable and without optimality guarantees. However, we rigorously prove that, under some mild technical assumptions, the input of a two-layer convolutional generative network can be deduced from the network output efficiently using simple gradient descent. This new theoretical finding implies that the mapping from the low- dimensional latent space to the high-dimensional image space is bijective (i.e., one-to-one). In addition, the same conclusion holds even when the network output is only partially observed (i.e., with missing pixels). Our theorems hold for 2-layer convolutional generative network with ReLU as the activation function, but we demonstrate empirically that the same conclusion extends to multi-layer networks and networks with other activation functions, including the leaky ReLU, sigmoid and tanh.

Age of information (AoI), defined as the time elapsed since the last received update was generated, is a newly proposed metric to measure the timeliness of information updates in a network. We consider AoI minimization problem for a network with general interference constraints, and time varying channels. We propose two policies, namely, virtual-queue based policy and age-based policy when the channel state is available to the network scheduler at each time step. We prove that the virtual-queue based policy is nearly optimal, up to a constant additive factor, and the age-based policy is at-most factor 4 away from optimality. Comparison with previous work, which derived age optimal policies when channel state information is not available to the scheduler, demonstrates a 4 fold improvement in age due to the availability of channel state information.

High-end vehicles are already equipped with safety systems, such as assistive braking and automatic lane following, enhancing vehicle safety. Yet, these current solutions can only help in low-complexity driving situations. In this paper, we introduce a parallel autonomy, or shared control, framework that computes safe trajectories for an automated vehicle, based on human inputs. We minimize the deviation from the human inputs while ensuring safety via a set of collision avoidance constraints. Our method achieves safe motion even in complex driving scenarios, such as those commonly encountered in an urban setting. We introduce a receding horizon planner formulated as nonlinear model predictive control (NMPC), which includes the analytic descriptions of road boundaries and the configuration and future uncertainties of other road participants. The NMPC operates over both steering and acceleration simultaneously. We introduce a nonslip model suitable for handling complex environments with dynamic obstacles, and a nonlinear combined slip vehicle model including normal load transfer capable of handling static environments. We validate the proposed approach in two complex driving scenarios. First, in an urban environment that includes a left-turn across traffic and passing on a busy street. And second, under snow conditions on a race track with sharp turns and under complex dynamic constraints. We evaluate the performance of the method with various human driving styles. We consequently observe that the method successfully avoids collisions and generates motions with minimal intervention for parallel autonomy. We note that the method can also be applied to generate safe motion for fully autonomous vehicles.

Visual attention is the cognitive process that allows humans to parse a large amount of sensory data by selecting relevant information and filtering out irrelevant stimuli. This papers develops a computational approach for visual attention in robots. We consider a Visual-Inertial Navigation (VIN) problem in which a robot needs to estimate its state using an on-board camera and an inertial sensor. The robot can allocate limited resources to VIN, due to time and energy constraints. Therefore, we answer the following question: under limited resources, what are the most relevant visual cues to maximize the performance of visual-inertial navigation? Our approach has four key features. First, it is task-driven, in that the selection of the visual cues is guided by a metric quantifying the task performance. Second, it exploits the notion of anticipation, since it uses a simplified model for forward-simulation of robot dynamics, predicting the utility of a set of visual cues over a time horizon. Third, it is efficient and easy to implement, since it leads to a greedy algorithm for the selection of the most relevant visual cues. Fourth, it provides formal performance guarantees: we leverage submodularity to prove that the greedy selection cannot be far from the optimal (combinatorial) selection. Simulations and real experiments on agile micro aerial vehicles show that our approach leads to dramatic improvements in the VIN performance. In the easy scenarios, our approach outperforms the state of the art in terms of localization errors. In the most challenging scenarios, it enables accurate visual-inertial navigation while the state of the art fails to track robot's motion during aggressive maneuvers.

Duckietown is an open, inexpensive and flexible platform for autonomy education and research. The platform comprises small autonomous vehicles (“Duckiebots”) built from off-the-shelf components, and cities (“Duckietowns”) complete with roads, signage, traffic lights, obstacles, and citizens (duckies) in need of transportation. The Duckietown platform offers a wide range of functionalities at a low cost. Duckiebots sense the world with only one monocular camera and perform all processing onboard with a Raspberry Pi 2, yet are able to: follow lanes while avoiding obstacles, pedestrians (duckies) and other Duckiebots, localize within a global map, navigate a city, and coordinate with other Duckiebots to avoid collisions. Duckietown is a useful tool since educators and researchers can save money and time by not having to develop all of the necessary supporting infrastructure and capabilities. All materials are available as open source, and the hope is that others in the community will adopt the platform for education and research.

Current state-of-the-art vehicle safety systems, such as assistive braking or automatic lane following, are still only able to help in relatively simple driving situations. We introduce a Parallel Autonomy shared-control framework that produces safe trajectories based on human inputs even in much more complex driving scenarios, such as those commonly encountered in an urban setting. We minimize the deviation from the human inputs while ensuring safety via a set of collision avoidance constraints. We develop a receding horizon planner formulated as a Non-linear Model Predictive Control (NMPC) including analytic descriptions of road boundaries, and the configurations and future uncertainties of other traffic participants, and directly supplying them to the optimizer without linearization. The NMPC operates over both steering and acceleration simultaneously. Furthermore, the proposed receding horizon planner also applies to fully autonomous vehicles. We validate the proposed approach through simulations in a wide variety of complex driving scenarios such as left-turns across traffic, passing on busy streets, and under dynamic constraints in sharp turns on a race track.

This work focuses on integrated routing and motion planning for an autonomous vehicle in a road network. We consider a problem in which customer demands need to be met within desired deadlines, and the rules of the road need to be satisfied. The vehicle might not, however, be able to satisfy these two goals at the same time. We propose a systematic way to compromise between delaying the satisfaction of the given demand and violating the road rules. We utilize scLTL formulas to specify desired behavior and develop a receding horizon approach including a periodically interacting routing algorithm and a RRT*-based motion planner. The proposed solution yields a provably minimum-violation trajectory. An illustrative case study is included.

Sampling-based methods have advanced the state of the art in robotic motion planning and control across complex, high-dimensional domains. With few exceptions, such approaches only admit simple constraints and objectives, such as collision-avoidance and reaching a goal state. In this work we leverage the best of two worlds: the scalability of sampling-based motion planning and the precise formal guarantees of temporal logic. We present an incremental sampling-based algorithm that synthesizes a motion control policy satisfying a bounded Signal Temporal Logic formula over properties of a given environment. Our key insight is that we can bias the selection of samples using a quantitative measure of how well the best path in the current tree of samples satisfies the specification. This allows us both to converge to a path that satisfies the specification, and to improve upon an existing path, i.e. to satisfy the specification with maximum robustness. We illustrate the performance of our method in several case studies.

Recent advances in autonomous driving have raised the problem of safety to the forefront and incentivized research into establishing safety guarantees. In this paper, we propose a safety verification framework as a safety standard for driving controllers with full or shared autonomy based on compositional and contract-based principles. Our framework enables us to synthesize safety guarantees over entire road networks by first building a library of locally verified models, and then composing local models together to verify the entire network. Composition is achieved using assume-guarantee contracts that are synthesized concurrently during verification. Thus, we can reuse local models within and across networks, add additional models to cover local road geometries without re-verifying the entire library, and perform all computations in a parallel and distributed way, which enables computational tractability. Furthermore, we employ controller contracts such that any controller satisfying them can be certified safe. We demonstrate the practical effectiveness of our framework by certifying controllers over parts of the Manhattan road network.

We present the development of a full-scale “parallel autonomy” research platform including software and hardware. In the parallel autonomy paradigm, the control of the vehicle is shared; the human is still in control of the vehicle, but the autonomy system is always running in the background to prevent accidents. Our holistic approach includes: (1) a drive-by-wire conversion method only based on reverse engineering mounting of relatively inexpensive sensors onto the vehicle implementation of a localization and mapping system, (4) obstacle detection and (5) a shared controller as well as (6) integration with an advanced autonomy simulation system (Drake) for rapid development and testing. The system can operate in three modes: (a) manual driving, (b) full autonomy, where the system is in complete control of the vehicle and (c) parallel autonomy, where the shared controller is implemented. We present results from extensive testing of a full-scale vehicle on closed tracks that demonstrate these capabilities.

We study broadcast capacity and minimum delay scaling laws for highly mobile wireless networks, in which each node has to disseminate or broadcast packets to all other nodes in the network. In particular, we consider a cell partitioned network under the simplified independent and identically distributed (IID) mobility model, in which each node chooses a new cell at random every time slot. We derive scaling laws for broadcast capacity and minimum delay as a function of the cell size. We propose a simple first-come-first-serve (FCFS) flooding scheme that nearly achieves both capacity and minimum delay scaling. Our results show that high mobility does not improve broadcast capacity, and that both capacity and delay improve with increasing cell sizes. In contrast to what has been speculated in the literature we show that there is (nearly) no tradeoff between capacity and delay. Our analysis makes use of the theory of Markov Evolving Graphs (MEGs) and develops two new bounds on flooding time in MEGs by relaxing the previously required expander property assumption.

In the classic Traveling Salesman Problem (TSP), the objective is to find the shortest path that visits a set of target locations. This problem is embedded and essential in many planning problems that arise in robotics, particularly in the domains of exploration, monitoring, surveillance, and reconnaissance. In this paper we consider the Stochastic TSP for Dynamical Systems, where a vehicle with complex dynamics is tasked with visiting n random target locations. By borrowing techniques from the applied probability literature, which were used to study the related stochastic Orienteering problem (where the vehicle has to visit as many of the n points as possible with a path of fixed length), we simplify and extend the existing results for both the TSP and the stochastic Orienteering problems to cases where the target points can be picked up only when the vehicle is in a certain configuration (i.e. it is not enough simply to be on the target point). Specifically, we show that there is a special parameter γ of the dynamics of the vehicle, which governs the length of the TSP tour. The length of the shortest path will then be Θ(n(γ-1)/γ) with very high probability. For stochastic Orienteering, if the path must have length at most λ, the vehicle can pick up Θ(λn1/γ) with very high probability. We also provide simple and efficient path planning algorithms which achieve these bounds, and are therefore within a constant factor of the length of the optimal path with very high probability.

We address the following question: is it possible to reconstruct the geometry of an unknown environment using sparse and incomplete depth measurements? This problem is relevant for a resource-constrained robot that has to navigate and map an environment, but does not have enough on-board power or payload to carry a traditional depth sensor (e.g., a 3D lidar) and can only acquire few (point-wise) depth measurements. In general, reconstruction from incomplete data is not possible, but when the robot operates in man-made environments, the depth exhibits some regularity (e.g., many planar surfaces with few edges); we leverage this regularity to infer depth from incomplete measurements. Our formulation bridges robotic perception with the compressive sensing literature in signal processing. We exploit this connection to provide formal results on exact depth recovery in 2D and 3D problems. Taking advantage of our specific sensing modality, we also prove novel and more powerful results to completely characterize the geometry of the signals that we can reconstruct. Our results directly translate to practical algorithms for depth reconstruction; these algorithms are simple (they reduce to solving a linear program), and robust to noise. We test our algorithms on real and simulated data, and show that they enable accurate depth reconstruction from a handful of measurements, and perform well even when the assumption of structured environment is violated.

The RRT* algorithm has efficiently extended Rapidly-exploring Random Trees (RRTs) to endow it with asymptotic optimality. We propose Goal-Rooted Feedback Motion Trees (GR-FMTs) that honor state/input constraints and generate collision-free feedback policies. Given analytic solutions for optimal local steering, GR-FMTs obtain and realize safe, dynamically feasible, and asymptotically optimal trajectories toward goals. Second, for controllable linear systems with linear state/input constraints, we propose a fast method for local steering, based on polynomial basis functions and segmentation. GR-FMTs with the method obtain and realize trajectories that are collision-free, dynamically feasible under constraints, and asymptotically optimal within a set we define. The formulation includes linear or quadratic programming of small sizes, where constraints are identified by root-finding in low or medium order of polynomials and added progressively.

We study the problem in which a mobile sensing robot is tasked to travel among and gather intelligence at a set of spatially distributed points-of-interest (POIs). The quality of the information collected at a POI is characterized by some sensory (reward) function of time. With limited fuel, the robot must balance between spending time traveling to more POIs and performing time-consuming sensing activities at POIs to maximize the overall reward. In a dual formulation, the robot is required to acquire a minimum amount of reward with the least amount of time. We propose an anytime planning algorithm for solving these two NP-hard problems to arbitrary precision for arbitrary reward functions. The algorithm is effective on large instances with tens to hundreds of POIs, as demonstrated with an extensive set of computational experiments. Besides mobile sensor scheduling, our algorithm also applies to automation scenarios such as intelligent and optimal itinerary planning.

This paper introduces a new mobile sensor scheduling problem involving a single robot tasked to monitor several events of interest that are occurring at different locations (stations). Of particular interest is the monitoring of transient events of a stochastic nature, with applications ranging from natural phenomena (e.g., monitoring abnormal seismic activity around a volcano using a ground robot) to urban activities (e.g., monitoring early formations of traffic congestion using an aerial robot). Motivated by examples like these, this paper focuses on problems in which the precise occurrence times of the events are unknown apriori, but statistics for their interarrival times are available. In monitoring such events, the robot seeks to: (1) maximize the number of events observed and (2) minimize the delay between two consecutive observations of events occurring at the same location. This paper considers the case when a robot is tasked with optimizing the event observations in a balanced manner, following a cyclic patrolling route. To tackle this problem, first, assuming that the cyclic ordering of stations is known, we prove the existence and uniqueness of the optimal solution and show that the solution has desirable convergence rate and robustness. Our constructive proof also yields an efficient algorithm for computing the unique optimal solution with O(n) time complexity, in which n is the number of stations, with O(log n) time complexity for incrementally adding or removing stations. Except for the algorithm, our analysis remains valid when the cyclic order is unknown. We then provide a polynomial-time approximation scheme that computes for any ε > 0 a (1 + ε)-optimal solution for this more general, NP-hard problem.

This paper is concerned with a novel mobile sensor scheduling problem, involving a single robot tasked with monitoring several events of interest that occur at different locations. Of particular interest is the monitoring of events that can not be easily forecast. Prominent examples range from natural phenomena (e.g., monitoring abnormal seismic activity around a volcano using a ground robot) to urban activities (e.g., monitoring early formations of traffic congestion in the Boston area using an aerial robot). Motivated by these examples, this paper focuses on problems where the precise occurrence time of the events is not known a priori, but some statistics for their inter-arrival times are available from past observations. The robot's task is to monitor the events to optimize the following two objectives: (i) maximize the number of events observed and (ii) minimize the delay between two consecutive observations of events occurring at the same location. Provided with only one robot, it is crucial to optimize these objectives in a balanced way, so that they are optimized at each station simultaneously. Our main theoretical result is that this complex mobile sensor scheduling problem can be reduced to a quasi-convex program, which can be solved in polynomial time. In other words, a globally optimal solution can be computed in time that is polynomial in the number of locations. We also provide computational experiments that validate our theoretical results.

We consider the problem of automatic control strategy synthesis, for discrete models of robotic systems, to fulfill a task that requires reaching a goal state while obeying a given set of safety rules. In this paper, we focus on the case when the said task is not feasible without temporarily violating some of the rules. We propose an algorithm that {synthesizes} a motion which violates only lowest priority rules for the shortest amount of time. Although the proposed algorithm can be applied in a variety of control problems, throughout the paper, we motivate this problem with an autonomous car navigating in an urban environment while abiding by the rules of the road, such as "always stay in the right lane" and "do not enter the sidewalk." We evaluate the algorithm on a case study with several illustrative scenarios.

Sampling-based motion planning algorithms, such as the Probabilistic RoadMap (PRM) and the Rapidly-exploring Random Tree (RRT), have received a large and growing amount of attention during the past decade. Most recently, sampling-based algorithms, such as the PRM* and RRT*, that guarantee asymptotic optimality, i.e., almost-sure convergence towards optimal solutions, have been proposed. Despite the experimental success of asymptotically-optimal sampling-based algorithms, their extensions to handle complex non-holonomic dynamical systems remains largely an open problem. In this paper, with the help of results from differential geometry, we extend the RRT* algorithm to handle a large class of non-holonomic dynamical systems. We demonstrate the performance of the algorithm in computational experiments involving the Dubins' car dynamics.

In this paper we examine the correlation between the information content and the spatial realization of range measurements taken by a mapping robot. To do so, we consider the task of constructing an occupancy grid map with a binary Bayesian filter. Using a narrow beam-based sensor model (versus an additive white Gaussian noise model), we prove that any controller tasked to maximize a mutual information reward function is eventually attracted to unexplored space. This intuitive behavior is derived solely from the geometric dependencies of the occupancy grid mapping algorithm and the monotonie properties of mutual information. Since it is a function of both the robot's position and the uncertainty of the surrounding cells, mutual information encodes geometric relationships that are fundamental to robot control, thus yielding geometrically relevant reward surfaces on which the robot can navigate. Lastly, we present the results of two experiments employing an omnidirectional ground robot equipped with a laser rangefinder.

In this paper, we consider a class of continuous-time, continuous-space stochastic optimal control problems. Building upon recent advances in Markov chain approximation methods and sampling-based algorithms for deterministic path planning, we propose a novel algorithm called the incremental Markov Decision Process (iMDP) to compute incrementally control policies that approximate arbitrarily well an optimal policy in terms of the expected cost. The main idea behind the algorithm is to generate a sequence of finite discretizations of the original problem through random sampling of the state space. At each iteration, the discretized problem is a Markov Decision Process that serves as an incrementally refined model of the original problem. We show that with probability one, (i) the sequence of the optimal value functions for each of the discretized problems converges uniformly to the optimal value function of the original stochastic optimal control problem, and (ii) the original optimal value function can be computed efficiently in an incremental manner using asynchronous value iterations. Thus, the proposed algorithm provides an anytime approach to the computation of optimal control policies of the continuous problem. The effectiveness of the proposed approach is demonstrated on motion planning and control problems in cluttered environments in the presence of process noise.

Inspired by birds flying through cluttered environments such as dense forests, this paper studies the theoretical foundations of high-speed motion through a randomly-generated obstacle field. Assuming that the locations and the sizes of the trees are determined by an ergodic point process, and under mild technical conditions on the dynamics of the bird, it is shown that the existence of an infinite collision-free trajectory through the forest exhibits a phase transition. In other words, if the bird flies faster than a certain critical speed, there is no infinite collision-free trajectory, with probability one, i.e., the bird will eventually collide with some tree, almost surely, regardless of the planning algorithm governing its motion. On the other hand, if the bird flies slower than this critical speed, then there exists at least one infinite collision-free trajectory, almost surely. Lower and upper bounds on the critical speed are derived for the special case of a Poisson forest considering a simple model for the bird's dynamics. Moreover, results from an extensive Monte-Carlo simulation study are presented. This paper also establishes novel connections between robot motion planning and statistical physics through ergodic theory and the theory of percolation, which may be of independent interest.

A genetic algorithm that utilizes process algebra for coding of solution chromosomes and for defining evolutionary based operators is presented. The algorithm is applicable to mission planning and optimization problems. As an example the high level mission planning for a cooperative group of uninhabited aerial vehicles is investigated. The mission planning problem is cast as an assignment problem, and solutions to the assignment problem are given in the form of chromosomes that are manipulated by evolutionary operators. The evolutionary operators of crossover and mutation are formally defined using the process algebra methodology, along with specific algorithms needed for their execution. The viability of the approach is investigated using simulations and the effectiveness of the algorithm is shown in small, medium, and large scale problems.

The Rapidly-exploring Random Tree (RRT) algorithm, based on incremental sampling, efficiently computes motion plans. Although the RRT algorithm quickly produces candidate feasible solutions, it tends to converge to a solution that is far from optimal. Practical applications favor "anytime" algorithms that quickly identify an initial feasible plan, then, given more computation time available during plan execution, improve the plan toward an optimal solution. This paper describes an anytime algorithm based on the RRT* which (like the RRT) finds an initial feasible solution quickly, but (unlike the RRT) almost surely converges to an optimal solution. We present two key extensions to the RRT% committed trajectories and branch-and-bound tree adaptation, that together enable the algorithm to make more efficient use of computation time online, resulting in an anytime algorithm for real-time implementation. We evaluate the method using a series of Monte Carlo runs in a high-fidelity simulation environment, and compare the operation of the RRT and RRT* methods. We also demonstrate experimental results for an outdoor wheeled robotic vehicle.

In recent years, the growth of the computational power available in the Central Processing Units (CPUs) of consumer computers has tapered significantly. At the same time, growth in the computational power available in the Graphics Processing Units (GPUs) has remained strong. Algorithms that can be implemented on GPUs today are not only limited to graphics processing, but include scientific computation and beyond. This paper is concerned with massively parallel implementations of incremental sampling-based robot motion planning algorithms, namely the widely-used Rapidly-exploring Random Tree (RRT) algorithm and its asymptotically-optimal counterpart called RRT*. We demonstrate an example implementation of RRT and RRT* motion-planning algorithm for a high-dimensional robotic manipulator that takes advantage of an NVidia CUDA-enabled GPU. We focus on parallelizing the collision-checking procedure, which is generally recognized as the computationally expensive component of sampling-based motion planning algorithms. Our experimental results indicate significant speedup when compared to CPU implementations, leading to practical algorithms for optimal motion planning in high-dimensional configuration spaces.

A desirable property of path planning for robotic manipulation is the ability to identify solutions in a sufficiently short amount of time to be usable. This is particularly challenging for the manipulation problem due to the need to plan over high-dimensional configuration spaces and to perform computationally expensive collision checking procedures. Consequently, existing planners take steps to achieve desired solution times at the cost of low quality solutions. This paper presents a planning algorithm that overcomes these difficulties by augmenting the asymptotically-optimal RRT* with a sparse sampling procedure. With the addition of a collision checking procedure that leverages memoization, this approach has the benefit that it quickly identifies low-cost feasible trajectories and takes advantage of subsequent computation time to refine the solution towards an optimal one. We evaluate the algorithm through a series of Monte Carlo simulations of seven, twelve, and fourteen degree of freedom manipulation planning problems in a realistic simulation environment. The results indicate that the proposed approach provides significant improvements in the quality of both the initial solution and the final path, while incurring almost no computational overhead compared to the RRT algorithm. We conclude with a demonstration of our algorithm for single-arm and dual-arm planning on Willow Garage's PR2 robot.

One long-standing challenge in robotics is the realization of mobile autonomous robots able to operate safely in existing human workplaces in a way that their presence is accepted by the human occupants. We describe the development of a multi-ton robotic forklift intended to operate alongside human personnel, handling palletized materials within existing, busy, semi-structured outdoor storage facilities. The system has three principal novel characteristics. The first is a multimodal tablet that enables human supervisors to use speech and pen-based gestures to assign tasks to the forklift, including manipulation, transport, and placement of palletized cargo. Second, the robot operates in minimally-prepared, semi-structured environments, in which the forklift handles variable palletized cargo using only local sensing (and no reliance on GPS), and transports it while interacting with other moving vehicles. Third, the robot operates in close proximity to people, including its human supervisor, other pedestrians who may cross or block its path, and forklift operators who may climb inside the robot and operate it manually. This is made possible by novel interaction mechanisms that facilitate safe, effective operation around people. We describe the architecture and implementation of the system, indicating how real-world operational requirements motivated the development of the key subsystems, and provide qualitative and quantitative descriptions of the robot operating in real settings.

This paper addresses the problem of autonomous manipulation of a priori unknown palletized cargo with a robotic lift truck (forklift). Specifically, we describe coupled perception and control algorithms that enable the vehicle to engage and place loaded pallets relative to locations on the ground or truck beds. Having little prior knowledge of the objects with which the vehicle is to interact, we present an estimation framework that utilizes a series of classifiers to infer the objects' structure and pose from individual LIDAR scans. The classifiers share a low-level shape estimation algorithm that uses linear programming to robustly segment input data into sets of weak candidate features. We present and analyze the performance of the segmentation method, and subsequently describe its role in our estimation algorithm. We then evaluate the performance of a motion controller that, given an estimate of a pallet's pose, is employed to safely engage each pallet. We conclude with a validation of our algorithms for a set of real-world pallet and truck interactions.