This paper addresses the problem of task planning for robots that must comply with operational manuals in real-world settings. Task planning under these constraints is essential for enabling autonomous robot operation in domains that require adherence to domain-specific knowledge. Current methods for generating robot goals and plans rely on common sense knowledge encoded in large language models. However, these models lack grounding of robot plans to domain-specific knowledge and are not easily transferable between multiple sites or customers with different compliance needs. In this work, we present SayComply, which enables grounding robotic task planning with operational compliance using retrievalbased language models. We design a hierarchical database of operational, environment, and robot embodiment manuals and procedures to enable efficient retrieval of the relevant context under the limited context length of the LLMs. We then design a task planner using a tree-based retrieval augmented generation (RAG) technique to generate robot tasks that follow user instructions while simultaneously complying with the domain knowledge in the database. We demonstrate the benefits of our approach through simulations and hardware experiments in real-world scenarios that require precise context retrieval across various types of context, outperforming the standard RAG method. Our approach bridges the gap in deploying robots that consistently adhere to operational protocols, offering a scalable and edge-deployable solution for ensuring compliance across varied and complex real-world environments. Project website: saycomply.github.io.

Autonomous exploration of unknown environments is fundamentally a problem of decision making under uncertainty where the agent must account for uncertainty in sensor measurements, localization, action execution, as well as many other factors. For large-scale exploration applications, autonomous systems must overcome the challenges of sequentially deciding which areas of the environment are valuable to explore while safely evaluating the risks associated with obstacles and hazardous terrain. In this work, we propose a risk-aware meta-level decision making framework to balance the tradeoffs associated with local and global exploration. Meta-level decision making builds upon classical hierarchical coverage planners by switching between local and global policies with the overall objective of selecting the policy that is most likely to maximize reward in a stochastic environment. We use information about the environment history, traversability risk, and kinodynamic constraints to reason about the probability of successful policy execution to switch between local and global policies. We have validated our solution in both simulation and on a variety of large-scale real world hardware tests. Our results show that by balancing local and global exploration we are able to significantly explore large-scale environments more efficiently.

Traversability estimation in rugged, unstructured environments remains a challenging problem in field robotics. Often, the need for precise, accurate traversability estimation is in direct opposition to the limited sensing and compute capability present on affordable, small-scale mobile robots. To address this issue, we present a novel method to learn [u]ncertainty-aware [n]avigation features from high-fidelity scans of [real]-world environments (UNRealNet). This network can be deployed on-robot to predict these high-fidelity features using input from lower-quality sensors. UNRealNet predicts dense, metric-space features directly from single-frame lidar scans, thus reducing the effects of occlusion and odometry error. Our approach is label-free, and is able to produce traversability estimates that are robot-agnostic. Additionally, we can leverage UNRealNet’s predictive uncertainty to both produce risk-aware traversability estimates, and refine our feature predictions over time. We find that our method outperforms traditional local mapping and inpainting baselines by up to 40%, and demonstrate its efficacy on multiple legged platforms.

This paper addresses the problem of autonomous robotic inspection in complex and unknown environments. This capability is crucial for efficient and precise inspections in various real-world scenarios, even when faced with perceptual uncertainty and lack of prior knowledge of the environment. Existing methods for real-world autonomous inspections typically rely on predefined targets and waypoints and often fail to adapt to dynamic or unknown settings. In this paper, we introduce the Semantic Belief Behavior Graph (SB2G) framework as a new approach to semantic-aware autonomous robot inspection. SB2G generates a control policy for the robot, using behavior nodes that encapsulate various semantic-based policies designed for inspecting different classes of objects. We design an active semantic search behavior to guide the robot in locating objects for inspection while reducing semantic information uncertainty. The edges in the SB2G encode transitions between these behaviors. We validate our approach through simulation and real-world urban inspections using a legged robotic platform. Our results show that SB2G enables a more efficient object inspection policy, exhibiting similar behaviors comparable to human-operated inspections.

This article surveys recent progress and discusses future opportunities for simultaneous localization and mapping (SLAM) in extreme underground environments. SLAM in subterranean environments, from tunnels, caves, and man-made underground structures on Earth, to lava tubes on Mars, is a key enabler for a range of applications, such as planetary exploration, search and rescue, disaster response, and automated mining, among others. SLAM in underground environments has recently received substantial attention, thanks to theDARPA Subterranean (SubT) Challenge, a global robotics competition aimed at assessing and pushing the state of the art in autonomous robotic exploration and mapping in complex underground environments. This article reports on the state of the art in underground SLAM by discussing different SLAM strategies and results across six teams that participated in the three-year-long SubT competition. In particular, the article has four main goals. First, we review the algorithms, architectures, and systems adopted by the teams; particular emphasis is put on light detection and ranging (LIDAR)-centric SLAM solutions (the go-to approach for virtually all teams in the competition), heterogeneous multirobot operation (including both aerial and ground robots), and real-world underground operation (from the presence of obscurants to the need to handle tight computational constraints). We do not shy away from discussing the “dirty details” behind the different SubT SLAM systems, which are often omitted from technical papers. Second, we discuss the maturity of the field by highlighting what is possible with the current SLAM systems and what we believe is within reach with some good systems engineering. Third, we outline what we believe are fundamental open problems, which are likely to require further research to break through. Finally, we provide a list of open-source SLAM implementations and datasets that have been produced during the SubT challenge and related efforts and constitute a useful resource for researchers and practitioners.

A new belief space planning algorithm, called covariance steering Belief RoadMap (CS-BRM), is introduced, analyzed, and numerically and experimentally tested. CS-BRM is a multi-query algorithm for motion planning for dynamical systems under simultaneous motion and observation uncertainties. CS-BRM extends the probabilistic roadmap (PRM) approach to belief spaces based on the recently developed theory of covariance steering (CS) that enables guaranteed satisfaction of terminal belief constraints in finite time. The nodes in the CS-BRM are sampled in the belief space and represent distributions of the system states. A covariance steering controller steers the system from one BRM node to another, thus acting as an edge controller of the corresponding belief graph that ensures belief constraint satisfaction. After the edge controller is computed, a specific edge cost is assigned to that edge. The CS-BRM algorithm allows the sampling of non-stationary belief nodes and thus is able to explore the velocity space and find much more efficient trajectories than previous BRM methods. The performance of CS-BRM is evaluated and compared to previous belief space planning approaches using several numerical examples and experimental demonstrations, illustrating the benefits of the proposed approach.

In active source seeking, a robot takes repeated measurements in order to locate a signal source in a cluttered and unknown environment. A key component of an active source seeking robot planner is a model that can produce estimates of the signal at unknown locations with uncertainty quantification. This model allows the robot to plan for future measurements in the environment. Traditionally, this model has been in the form of a Gaussian process, which has difficulty scaling and cannot represent obstacles. We propose a global and local factor graph model for active source seeking, which allows the model to scale to a large number of measurements and represent unknown obstacles in the environment. We combine this model with extensions to a highly scalable planner to form a system for large-scale active source seeking. We demonstrate that our approach outperforms baseline methods in both simulated and real robot experiments.

Modeling dynamics is often the first step to making a vehicle autonomous. While on-road autonomous vehicles have been extensively studied, off-road vehicles pose many challenging modeling problems. An off-road vehicle encounters highly complex and difficult-to-model terrain/vehicle interactions, as well as having complex vehicle dynamics of its own. These complexities can create challenges for effective high-speed control and planning. In this paper, we introduce a framework for multistep dynamics prediction that explicitly handles the accumulation of modeling error and remains scalable for sampling-based controllers. Our method uses a specially-initialized Long Short-Term Memory (LSTM) over a limited time horizon as the learned component in a hybrid model to predict the dynamics of a 4-person seating all-terrain vehicle (Polaris S4 1000 RZR) in two distinct environments. By only having the LSTM predict over a fixed time horizon, we negate the need for long term stability that is often a challenge when training recurrent neural networks. Our framework is flexible as it only requires odometry information for labels. Through extensive experimentation, we show that our method is able to predict millions of possible trajectories in real-time, with a time horizon of five seconds in challenging off road driving scenarios.

To achieve autonomy in unknown and unstruc-tured environments, we propose a method for semantic-based planning under perceptual uncertainty. This capability is cru-cial for safe and efficient robot navigation in environment with mobility-stressing elements that require terrain-specific locomotion policies. We propose the Semantic Belief Graph (SBG), a geometric- and semantic-based representation of a robot's probabilistic roadmap in the environment. The SBG nodes comprise of the robot geometric state and the semantic-knowledge of the terrains in the environment. The SBG edges represent local semantic-based controllers that drive the robot between the nodes or invoke an information gathering action to reduce semantic belief uncertainty. We formulate a semantic-based planning problem on SBG that produces a policy for the robot to safely navigate to the target location with min-imal traversal time. We analyze our method in simulation and present real-world results with a legged robotic platform navigating multi-level outdoor environments.

In this paper, we present a LiDAR Inertial Odometry (LIO) algorithm utilizing adaptive keyframe generation which achieves fast and accurate state estimation for aerial and ground robots. It is known that keyframe generation significantly affects the performance of Simultaneous Localization and Mapping (SLAM) algorithms. Unlike existing SLAM algorithms that generate keyframes based on fixed conditions, we propose to use adaptive keyframe generation conditions considering characteristics of surrounding environment using real-time LiDAR scans. When a keyframe is generated, the keyframe and the corresponding LiDAR measurements are stored in our novel data structure designed for efficient sub- map generation. The scan to sub-map matching module then uses the Generalized Iterative Closest Point (GICP) algorithm to adjust estimated states at a global scale, producing more accurate and globally consistent state estimation results even in large-scale underground environments. Experimental results from diverse types of underground environments show that the proposed method outperforms the existing state-of-the-art LIO algorithms in various metrics such as computational speed, CPU usage, and accuracy.

This article presents a 3D point cloud map-merging framework for egocentric heterogeneous multi-robot exploration, based on overlap detection and alignment, that is independent of a manual initial guess or prior knowledge of the robots' poses. The novel proposed solution utilizes state-of-the-art place recognition learned descriptors, that through the framework's main pipeline, offer a fast and robust region overlap estimation, hence eliminating the need for the time-consuming global feature extraction and feature matching process that is typically used in 3D map integration. The region overlap estimation provides a homogeneous rigid transform that is applied as an initial condition in the point cloud registration algorithm Fast-GICP, which provides the final and refined alignment. The efficacy of the proposed framework is experimentally evaluated based on multiple field multi-robot exploration missions in underground environments, where both ground and aerial robots are deployed, with different sensor configurations.

Robust mission planning is an essential component for mission autonomy to perform complicated tasks in extreme environments. In this paper, we are interested in the role of semantic abstractions for guiding autonomous mission planning. In particular, we focus on how semantics can be leveraged to transition, at the mission level, in between individually robust task plans. We present a mission autonomy framework wherein a task plan adaptation policy leverages up-to-date semantics information in order to adapt to changes that occur during run-time, which endows the robot with better resiliency to unexpected events and improves the overall efficiency of mission operations. Under this new perspective, we provide a concrete and challenging application of autonomous exploration and radio source seeking in a complex multi-level building environment. Experimental results over simulations and real hardware tests demonstrate that the presented semantics-aware mission adaptation more effectively completes the mission with better qualitative results compared to a non-adaptive baseline.

Experimental design of autonomous systems involves defining experimental inputs to maximize the experimenter’s information gained, minimize costs, and balance risk. This effectively leads to improved understanding and trustworthiness, which are necessary for deployment in realworld settings. Since experimental design is inherently a human-in-the-loop, sequential decision making problem, and decisions are being made about complex systems, an investigation into decision-making quality and decision-supporting methods is warranted. In this work, we investigate a decision support system (DSS) to augment the human’s experimental design decision making abilities, and conduct an exploratory user study to investigate the potential for decision support. Our findings show that experimenters, including experienced field roboticists, make suboptimal decisions and mistakes during the experimental design process, which suggests robotics research could benefit from DSSs. Our proposed DSS shows promise in some select aspects of experimental design, including helping to reduce suboptimal decisions, and participants in the user study reported favorable opinions of using such a system, including a sense of usefulness and lack of burden. The broader implication of this work is the identification of decision support in experimental design as one way to help bridge the gap between academia and industry by way of accelerated, informative experimentation and increased system explainability.

Geometric methods for solving open-world off-road navigation tasks, by learning occupancy and metric maps, provide good generalization but can be brittle in outdoor environments that violate their assumptions (e.g., tall grass). Learning-based methods can directly learn collision-free behavior from raw observations, but are difficult to integrate with standard geometry-based pipelines. This creates an unfortunate conflict – either use learning and lose out on well-understood geometric navigational components, or do not use it, in favor of extensively hand-tuned geometry-based cost maps. In this work, we reject this dichotomy by designing the learning and non-learning-based components in a way such that they can be effectively combined in a self-supervised manner. Both components contribute to a planning criterion: the learned component contributes predicted traversability as rewards, while the geometric component contributes obstacle cost information. We instantiate and comparatively evaluate our system in both in-distribution and out-of-distribution environments, showing that this approach inherits complementary gains from the learned and geometric components and significantly outperforms either of them.

A new belief space planning algorithm, called covariance steering Belief RoadMap (CS-BRM), is introduced, which is a multi-query algorithm for motion planning of dynamical systems under simultaneous motion and observation uncertainties. CS-BRM extends the probabilistic roadmap (PRM) approach to belief spaces and is based on the recently developed theory of covariance steering (CS) that enables guaranteed satisfaction of terminal belief constraints in finitetime. The CS-BRM algorithm allows the sampling of non-stationary belief nodes, and thus is able to explore the velocity space and find efficient motion plans. We evaluate CS-BRM in different planning problems and demonstrate the benefits of the proposed approach.

We present a method for solving the coverage problem with the objective of autonomously exploring an unknown environment under mission time constraints. Here, the robot is tasked with planning a path over a horizon such that the accumulated area swept out by its sensor footprint is maximized. Because this problem exhibits a diminishing returns property known as submodularity, we choose to formulate it as a tree-based sequential decision making process. This formulation allows us to evaluate the effects of the robot's actions on future world coverage states, while simultaneously accounting for traversability risk and the dynamic constraints of the robot. To quickly find near-optimal solutions, we propose an effective approximation to the coverage sensor model which adapts to the local environment. Our method was extensively tested across various complex environments and served as the local exploration algorithm for a competing entry in the DARPA Subterranean Challenge.

Legged robots can traverse a wide variety of terrains, some of which may be challenging for wheeled robots, such as stairs or highly uneven surfaces. However, quadruped robots face stability challenges on slippery surfaces. This can be resolved by adjusting the robot's locomotion by switching to more conservative and stable locomotion modes, such as crawl mode (where three feet are in contact with the ground always) or amble mode (where one foot touches down at a time) to prevent potential falls. To tackle these challenges, we propose an approach to learn a model from past robot experience for predictive detection of potential failures. Accordingly, we trigger gait switching merely based on proprioceptive sensory information. To learn this predictive model, we propose a semi-supervised process for detecting and annotating ground truth slip events in two stages: We first detect abnormal occurrences in the time series sequences of the gait data using an unsupervised anomaly detector, and then, the anomalies are verified with expert human knowledge in a replay simulation to assert the event of a slip. These annotated slip events are then used as ground truth examples to train an ensemble decision learner for predicting slip probabilities across terrains for traversability. We analyze our model on data recorded by a legged robot on multiple sites with slippery terrain. We demonstrate that a potential slip event can be predicted up to 720 ms ahead of a potential fall with an average precision greater than 0.95 and an average F-score of 0.82. Finally, we validate our approach in real-time by deploying it on a legged robot and switching its gait mode based on slip event detection.

To achieve autonomy in complex real-world exploration missions, we consider deployment strategies for a team of robots with heterogeneous capabilities. We formulate a multi-robot exploration mission and compute an operation policy to maintain robot team productivity and maximize mission success. The environment description, robot capability, and mission outcome are modeled as a Markov decision process (MDP). We also include constraints, such as sensor failures, limited communication coverage, and mobility-stressing elements. The proposed operation model is applied to the DARPA Subterranean (SubT) Challenge. The deployment policy is also compared against the human-based operation strategy in the final competition of the SubT Challenge.

We present a method for autonomous exploration of large-scale unknown environments under mission time con-straints. We start by proposing the Frontloaded Information Gain Orienteering Problem (FIG-OP) - a generalization of the traditional orienteering problem where the assumption of a reliable environmental model no longer holds. The FIG-OP ad-dresses model uncertainty by frontloading expected information gain through the addition of a greedy incentive, effectively expe-diting the moment in which new area is uncovered. In order to reason across multi-kilometer environments, we solve FIG-OP over an information-efficient world representation, constructed through the aggregation of information from a topological and metric map. Our method was extensively tested and field-hardened across various complex environments, ranging from subway systems to mines. In comparative simulations, we observe that the FIG-OP solution exhibits improved coverage efficiency over solutions generated by greedy and traditional orienteering-based approaches (i.e. severe and minimal model uncertainty assumptions, respectively).

Navigating off-road with a fast autonomous vehicle depends on a robust perception system that differentiates traversable from non-traversable terrain. Typically, this depends on a semantic understanding which is based on supervised learning from images annotated by a human expert. This requires a significant investment in human time, assumes correct expert classification, and small details can lead to misclassification. To address these challenges, we propose a method for predicting high- and low-risk terrains from only past vehicle experience in a self-supervised fashion. First, we develop a tool that projects the vehicle trajectory into the front camera image. Second, occlusions in the 3D representation of the terrain are filtered out. Third, an autoencoder trained on masked vehicle trajectory regions identifies low- and high-risk terrains based on the reconstruction error. We evaluated our approach with two models and different bottleneck sizes with two different training and testing sites with a four-wheeled off-road vehicle. Comparison with two independent test sets of semantic labels from similar terrain as training sites demonstrates the ability to separate the ground as low-risk and the vegetation as high-risk with 81.1% and 85.1% accuracy.

In recent years, unsupervised deep learning approaches have received significant attention to estimating the depth and visual odometry (VO) from unlabelled monocular image sequences. However, their performance is limited in challenging environments due to perceptual degradation, occlusions, and rapid motions. Moreover, the existing unsupervised methods suffer from the lack of scale-consistency constraints across frames, which causes that the VO estimators fail to provide persistent trajectories over long sequences. In this study, we propose an unsupervised monocular deep VO framework that predicts a six-degrees-of-freedom pose camera motion and depth map of the scene from unlabelled RGB image sequences. We provide detailed quantitative and qualitative evaluations of the proposed framework on a) a challenging dataset collected during the DARPA Subterranean challenge1; and b) the benchmark KITTI and Cityscapes datasets. The proposed approach significantly outperforms state-of-the-art unsupervised deep VO and depth prediction methods under perceptually degraded conditions providing better results for both pose estimation and depth recovery. Furthermore, it achieves state-of-the-art results in most of the VO and depth metrics on benchmark datasets. The presented approach is part of the solution used by the COSTAR team participating in the DARPA Subterranean Challenge.

Monocular depth inference has gained tremendous attention from researchers in recent years and remains as a promising replacement for expensive time-of-flight sensors, but issues with scale acquisition and implementation overhead still plague these systems. To this end, this work presents an unsupervised learning framework that is able to predict at-scale depth maps and egomotion, in addition to camera intrinsics, from a sequence of monocular images via a single network. Our method incorporates both spatial and temporal geometric constraints to resolve depth and pose scale factors, which are enforced within the supervisory reconstruction loss functions at training time. Only unlabeled stereo sequences are required for training the weights of our single-network architecture, which reduces overall implementation overhead as compared to previous methods. Our results demonstrate strong performance when compared to the current state-of-the-art on multiple sequences of the KITTI driving dataset and can provide faster training times with its reduced network complexity.

This article establishes the Exploration-RRT algorithm: A novel general-purpose combined exploration and path planning algorithm, based on a multi-goal Rapidly-Exploring Random Trees (RRT) framework. Exploration-RRT (ERRT) has been specifically designed for utilization in 3D exploration missions, with partially or completely unknown and unstructured environments. The novel proposed ERRT is based on a multi-objective optimization framework and it is able to take under consideration the potential information gain, the distance travelled, and the actuation costs, along trajectories to pseudorandom goals, generated from considering the on-board sensor model and the non-linear model of the utilized platform. In this article, the algorithmic pipeline of the ERRT will be established and the overall applicability and efficiency of the proposed scheme will be presented on an application with an Unmanned Aerial Vehicle (UAV) model, equipped with a 3D lidar, in a simulated operating environment, with the goal of exploring a completely unknown area as efficiently and quickly as possible.

Simultaneous Localization and Mapping (SLAM) in large-scale, unknown, and complex subterranean environments is a challenging problem. Sensors must operate in off-nominal conditions; uneven and slippery terrains make wheel odometry inaccurate, while long corridors without salient features make exteroceptive sensing ambiguous and prone to drift; finally, spurious loop closures that are frequent in environments with repetitive appearance, such as tunnels and mines, could result in a significant distortion of the entire map. These challenges are in stark contrast with the need to build highly-accurate 3D maps to support a wide variety of applications, ranging from disaster response to the exploration of underground extraterrestrial worlds. This paper reports on the implementation and testing of a lidar-based multi-robot SLAM system developed in the context of the DARPA Subterranean Challenge. We present a system architecture to enhance subterranean operation, including an accurate lidar-based front-end, and a flexible and robust back-end that automatically rejects outlying loop closures. We present an extensive evaluation in large-scale, challenging subterranean environments, including the results obtained in the Tunnel Circuit of the DARPA Subterranean Challenge. Finally, we discuss potential improvements, limitations of the state of the art, and future research directions.

Deep learning has enjoyed much recent success, and applying state-of-the-art model learning methods to controls is an exciting prospect. However, there is a strong reluctance to use these methods on safety-critical systems, which have constraints on safety, stability, and real-time performance. We propose a framework which satisfies these constraints while allowing the use of deep neural networks for learning model uncertainties. Central to our method is the use of Bayesian model learning, which provides an avenue for maintaining appropriate degrees of caution in the face of the unknown. In the proposed approach, we develop an adaptive control framework leveraging the theory of stochastic CLFs (Control Lyapunov Functions) and stochastic CBFs (Control Barrier Functions) along with tractable Bayesian model learning via Gaussian Processes or Bayesian neural networks. Under reasonable assumptions, we guarantee stability and safety while adapting to unknown dynamics with probability 1. We demonstrate this architecture for high-speed terrestrial mobility targeting potential applications in safety-critical high-speed Mars rover missions.

We present an approach for estimating the body-frame velocity of a mobile robot. We combine measurements from a millimeter-wave radar-on-a-chip sensor and an inertial measurement unit (IMU) in a batch optimization over a sliding window of recent measurements. The sensor suite employed is lightweight, low-power, and is invariant to ambient lighting conditions. This makes the proposed approach an attractive solution for platforms with limitations around payload and longevity, such as aerial vehicles conducting autonomous exploration in perceptually degraded operating conditions, including subterranean environments. We compare our radar-inertial velocity estimates to those from a visual-inertial (VI) approach. We show the accuracy of our method is comparable to VI in conditions favorable to VI, and far exceeds the accuracy of VI when conditions deteriorate.

This paper serves as one of the first efforts to enable large-scale and long-duration autonomy using the Boston Dynamics Spot robot. Motivated by exploring extreme environments, particularly those involved in the DARPA Subterranean Challenge, this paper pushes the boundaries of the state-of-practice in enabling legged robotic systems to accomplish real-world complex missions in relevant scenarios. In particular, we discuss the behaviors and capabilities which emerge from the integration of the autonomy architecture NeBula (Networked Belief-aware Perceptual Autonomy) with next-generation mobility systems. We will discuss the hardware and software challenges, and solutions in mobility, perception, autonomy, and very briefly, wireless networking, as well as lessons learned and future directions. We demonstrate the performance of the proposed solutions on physical systems in real-world scenarios.3The proposed solution contributed to winning 1st-place in the 2020 DARPA Subterranean Challenge, Urban Circuit.4

Hybrid ground and aerial vehicles can possess distinct advantages over ground-only or flight-only designs in terms of energy savings and increased mobility. In this work we outline our unified framework for controls, planning, and autonomy of hybrid ground/air vehicles. Our contribution is three-fold: 1) We develop a control scheme for the control of passive two-wheeled hybrid ground/aerial vehicles. 2) We present a unified planner for both rolling and flying by leveraging differential flatness mappings. 3) We conduct experiments leveraging mapping and global planning for hybrid mobility in unknown environments, showing that hybrid mobility uses up to five times less energy than flying only1.1Video at https://youtu.be/nlGfYehTLpg.

Simultaneous localization and planning (SLAP) is a crucial ability for an autonomous robot operating under uncertainty. In its most general form, SLAP induces a continuous partially observable Markov decision process (POMDP), which needs to be repeatedly solved online. This paper addresses this problem and proposes a dynamic replanning scheme in belief space. The underlying POMDP, which is continuous in state, action, and observation space, is approximated offline via sampling-based methods, but operates in a replanning loop online to admit local improvements to the coarse offline policy. This construct enables the proposed method to combat changing environments and large localization errors, even when the change alters the homotopy class of the optimal trajectory. It further outperforms the state-of-the-art Feedback-based Information RoadMap (FIRM) method by eliminating unnecessary stabilization steps. Applying belief space planning to physical systems brings with it a plethora of challenges. A key focus of this paper is to implement the proposed planner on a physical robot and show the SLAP solution performance under uncertainty, in changing environments and in the presence of large disturbances, such as a kidnapped robot situation.

In this paper we present the PUMP (Parallel Uncertainty-aware Multiobjective Planning) algorithm for addressing the stochastic kinodynamic motion planning problem, whereby one seeks a low-cost, dynamically-feasible motion plan subject to a constraint on collision probability (CP). To ensure exhaustive evaluation of candidate motion plans (as needed to tradeoff the competing objectives of performance and safety), PUMP incrementally builds the Pareto front of the problem, accounting for the optimization objective and an approximation of CP. This is performed by a massively parallel multiobjective search, here implemented with a focus on GPUs. Upon termination of the exploration phase, PUMP searches the Pareto set of motion plans to identify the lowest cost solution that is certified to satisfy the CP constraint (according to an asymptotically exact estimator). We introduce a novel particle-based CP approximation scheme, designed for efficient GPU implementation, which accounts for dependencies over the history of a trajectory execution. We present numerical experiments for quadrotor planning wherein PUMP identifies solutions in ~100 ms, evaluating over one hundred thousand partial plans through the course of its exploration phase. The results show that this multiobjective search achieves a lower motion plan cost, for the same CP constraint, compared to a safety buffer-based search heuristic and repeated RRT trials.

Planning safe, high-speed trajectories in unknown environments remains a major roadblock on the way toward achieving fast autonomous flight. Current state-of-the-art planning approaches use sampling-based methods or trajectory optimization to obtain fast trajectories, whose safety is evaluated by taking into account the current state estimate of the environment. In unknown environments, however, this leads to numerous stops caused by the need for re-planning the trajectory due to unexpected obstacles. In this paper, we propose to use an active perception paradigm for planning. We predict the future uncertainty of the map and optimize trajectories to minimize re-planning risk. This leads to faster and safer trajectories. We evaluate the proposed planning approach in a series of simulation experiments, which show that we are able to achieve safer trajectories with a smaller number of re-planning stops and faster speeds.

The operation of mobile robots in unknown environments typically requires building maps during exploration. As the exploration time and environment size increase, the amount of data collected and the number of variables required to represent these maps both grow, which is problematic since all real robots have finite resources. The solution proposed in this paper is to only retain the variables and measurements that are most important to achieve the robot's task. The variable and measurement selection approach is demonstrated on the task of navigation with a low risk of collision. Our approach has two stages: first, a subset of the variables is selected that is most useful for minimizing the uncertainty of navigation (termed the “focused variables”). And second, a task-agnostic method is used to select a subset of the measurements that maximizes the information over these focused variables (“focused inference”). Detailed simulations and hardware experiments show that the two-stage approach constrains the number of variables and measurements. It can generate much sparser maps than existing approaches in the literature, while still achieving a better task performance-in this case (fewer collisions). An incremental and iterative approach is further presented, in which the two-stage procedure is performed on subsets of the data, and thus, avoids the necessity of performing a resource-intensive batch selection on large datasets.

This paper introduces a probabilistic algorithm for multi-robot decision-making under uncertainty, which can be posed as a Decentralized Partially Observable Markov Decision Process (Dec-POMDP). Dec-POMDPs are inherently synchronous decision-making frameworks which require significant computational resources to be solved, making them infeasible for many real-world robotics applications. The Decentralized Partially Observable Semi-Markov Decision Process (Dec-POSMDP) was recently introduced as an extension of the Dec-POMDP that uses high-level macro-actions to allow large-scale, asynchronous decision-making. However, existing Dec-POSMDP solution methods have limited scalability or perform poorly as the problem size grows. This paper proposes a cross-entropy based Dec-POSMDP algorithm motivated by the combinatorial optimization literature. The algorithm is applied to a constrained package delivery domain, where it significantly outperforms existing Dec-POSMDP solution methods.

The focus of this paper is on solving multi-robot planning problems in continuous spaces with partial observability. Decentralized Partially Observable Markov Decision Processes (Dec-POMDPs) are general models for multi-robot coordination problems, but representing and solving Dec-POMDPs is often intractable for large problems. To allow for a high-level representation that is natural for multi-robot problems and scalable to large discrete and continuous problems, this paper extends the Dec-POMDP model to the Decentralized Partially Observable Semi-Markov Decision Process (Dec-POSMDP). The Dec-POSMDP formulation allows asynchronous decision-making by the robots, which is crucial in multi-robot domains. We also present an algorithm for solving this Dec-POSMDP which is much more scalable than previous methods since it can incorporate closed-loop belief space macro-actions in planning. These macro-actions are automatically constructed to produce robust solutions. The proposed method's performance is evaluated on a complex multi-robot package delivery problem under uncertainty, showing that our approach can naturally represent multi-robot problems and provide high-quality solutions for large-scale problems.

Many robotic missions require online estimation of the unknown state transition models associated with uncertainty that stems from mission dynamics. The learning problem is usually distributed among agents in multiagent scenarios, either due to the absence of a centralized processing unit or because of the large size of the joint learning problem. This paper addresses the problem of multiagent learning in the likely scenario that agents estimate different models from their measured data, but they can share information by communicating model parameters. Previous approaches either consider homogeneous scenarios or perform model transfer in an open-loop manner, which hinders the convergence rate. We develop a closed-loop multiagent learning algorithm, Collaborative Filtering-Decentralized Incremental Feature Dependency Discovery (CF-Dec-iFDD), which enables agents to learn and share models in heterogeneous scenarios. Each agent learns a linear function approximation of the actual model, and the number of features is increased incrementally to adjust model complexity based on the observed data. The agents obtain feedback from other agents on the model error reduction associated with the communicated features. Although this increases the communication cost of exchanging features, it improves the quality/utility of what is being exchanged, leading to improved convergence rate. The approach is demonstrated in indoor hardware flight tests on a forest fire management scenario for which agents must learn the transition model of the fire spread depending on external factors such as wind and vegetation. It is shown that CF-Dec-iFDD has superior convergence rate compared to the alternative approaches.

Learning from demonstration provides an attractive solution to the problem of teaching autonomous systems how to perform complex tasks. Reward learning from demonstration is a promising method of inferring a rich and transferable representation of the demonstrator's intents, but current algorithms suffer from intractability and inefficiency in large domains due to the assumption that the demonstrator is maximizing a single reward function throughout the whole task. This paper takes a different perspective by assuming that the reward function behind an unsegmented demonstration is actually composed of several distinct subtasks chained together. Leveraging this assumption, a Bayesian nonparametric reward-learning framework is presented that infers multiple subgoals and reward functions within a single unsegmented demonstration. The new framework is developed for discrete state spaces and also general continuous demonstration domains using Gaussian process reward representations. The algorithm is shown to have both performance and computational advantages over existing inverse reinforcement learning methods. Experimental results are given in both cases, demonstrating the ability to learn challenging maneuvers from demonstration on a quadrotor and a remote-controlled car.

Motion planning in belief space (under motion and sensing uncertainty) is a challenging problem due to the computational intractability of its exact solution. The Feedback-based Information RoadMap (FIRM) framework made an important theoretical step toward enabling roadmap-based planning in belief space and provided a computationally tractable version of belief space planning. However, there are still challenges in applying belief space planners to physical systems, such as the discrepancy between computational models and real physical models. In this paper, we propose a dynamic replanning scheme in belief space to address such challenges. Moreover, we present techniques to cope with changes in the environment (e.g., changes in the obstacle map), as well as unforeseen large deviations in the robot's location (e.g., the kidnapped robot problem). We then utilize these techniques to implement the first online replanning scheme in belief space on a physical mobile robot that is robust to changes in the environment and large disturbances. This method demonstrates that belief space planning is a practical tool for robot motion planning.

In persistent missions, taking system's health and capability degradation into account is an essential factor to predict and avoid failures. The state space in health-aware planning problems is often a mixture of continuous vehicle-level and discrete mission-level states. This in particular poses a challenge when the mission domain is partially observable and restricts the use of computationally expensive forward search methods. This paper presents a method that exploits a structure that exists in many health-aware planning problems and performs a two-layer planning scheme. The lower layer exploits the local linearization and Gaussian distribution assumption over vehicle-level states while the higher layer maintains a non-Gaussian distribution over discrete mission-level variables. This two-layer planning scheme allows us to limit the expensive online forward search to the mission-level states, and thus predict system's behavior over longer horizons in the future. We demonstrate the performance of the method on a long duration package delivery mission using a quadrotor in a partially-observable domain in the presence of constraints and health/capability degradation.

This paper extends the concept of “probabilistic completeness” defined for motion planners in state space (or configuration space) to the concept of “probabilistic completeness under uncertainty” for motion planners in belief space. Accordingly, an approach is proposed to verify the probabilistic completeness of the sampling-based planners in belief space. Finally, through the proposed approach, it is shown that under mild conditions the sampling-based methods constructed based on the abstract framework of FIRM (Feedback-based Information Roadmap Method) are probabilistically complete under uncertainty.

In roadmap-based methods, such as the Probabilistic Roadmap Method (PRM) in deterministic environments or the Feedback-based Information RoadMap (FIRM) in partially observable probabilistic environments, a stabilizing controller is needed to guarantee node reachability in state or belief space. In belief space, it has been shown that belief-node reachability can be achieved using stationary Linear Quadratic Gaussian (LQG) controllers, for linearly controllable systems. However, for nonholonomic systems such as a unicycle model, belief reachability is a challenge. In this paper, we construct a roadmap in information space, where the local planners in partially-observable space are constructed by utilizing a Kalman filter as an estimator along with a Dynamic Feedback Linearization-based (DFL-based) controller as the belief controller. As a consequence, the task of belief stabilization to pre-defined nodes in belief space is accomplished even for nonholonomic systems. Therefore, a query-independent roadmap is generated in belief space that preserves the “principle of optimality”, required in dynamic programming solvers. This method serves as an offline POMDP solver for motion planning in belief space, which can seamlessly take obstacles into account. Experimental results show the efficiency of both individual local planners and the overall planner over the information graph for a nonholonomic model.

We report a method for the detection and recognition of a large planar mirror based on the images captured by a monocular camera. We start with deriving a mirror transformation matrix in a homogeneous coordinate and geometric constraints for corresponding real and virtual feature points in the image. We find that existing feature detection methods are not reflection invariant. We introduce a secondary artificial reflection to virtual features to generate secondary features which are proven to share a rigid body motion relationship with the original feature set. We propose an iterative strategy to adjust the secondary mirror configuration so that existing feature matching methods can be used. The combined method yields a robust mirror detection algorithm which has been verified in physical experiments.

Direct transformation of sampling-based motion planning methods to the Information-state (belief) space is a challenge. The main bottleneck for roadmap-based techniques in belief space is that the incurred costs on different edges of the graph are not independent of each other. In this paper, we generalize the Probabilistic RoadMap (PRM) framework to obtain a Feedback controller-based Information-state RoadMap (FIRM) that takes into account motion and sensing uncertainty in planning. The FIRM nodes and edges lie in belief space and the crucial feature of FIRM is that the costs associated with different edges of FIRM are independent of each other. Therefore, this construct essentially breaks the “curse of history” in the original Partially Observable Markov Decision Process (POMDP), which models the planning problem. Further, we show how obstacles can be rigorously incorporated into planning on FIRM. All these properties stem from utilizing feedback controllers in the construction of FIRM.

In this paper a new strategy for handling the observation information of a bearing-range sensor throughout the filtering process of EKF-SLAM is proposed. This new strategy is advised based on a thorough consistency analysis and aims to improve the process consistency while reducing the computational cost. At first, three different possible observation models are introduced for the EKF-SLAM solution for a robot equipped with a bearing-range sensor. General form of the covariance matrix and the level of inconsistency in the robot orientation estimate is then calculated for these variants, and based on the numerical comparison of the estimation results, it is proposed to use the bearing and range information of a feature in the initialization step of EKF-SLAM. However, it is recommended to use only the bearing information to perform other iteration steps. The simulation observations verify that the new strategy yields to more consistent estimates both for the robot and the features. Moreover, through the proposed consistency analysis, it is shown that since the source of consistency improvement is independent from the choice of the motion model, it gives us an advantage over other existing methods that assume a specific motion models for consistency improvement.

This paper proposes a method for augmenting the information of a monocular camera and a range finder. This method is a valuable step towards solving the SLAM problem in unstructured environments free from problems of using encoderspsila data. Proposed algorithm causes the robot to benefit from a feature-based map for filtering purposes, while it exploits an accurate motion model, based on point-wise raw range scan matching rather than unreliable feature-based range scan matching, in unstructured environments. Moreover, robust loop closure detection procedure is the other consequence of this method. Experiments with a low-cost IEEE 1394 webcam and a range finder illustrate the effectiveness of the proposed method in drift-free SLAM at loop closing motions in unstructured environments.