In this paper, we present STALC, a hierarchical planning approach for multi-robot coordination in real-world environments with significant inter-robot spatial and temporal dependencies. At its core, STALC consists of a multi-robot graph-based planner which combines a topological graph with a novel, computationally efficient mixed-integer programming formulation to generate highly-coupled multi-robot plans in seconds. To enable autonomous planning across different spatial and temporal scales, we construct our graphs so that they capture connectivity between free-space regions and other problem-specific features, such as traversability or risk. We then use receding-horizon planners to achieve local collision avoidance and formation control. To evaluate our approach, we consider a multi-robot reconnaissance scenario where robots must autonomously coordinate to navigate through an environment while minimizing the risk of detection by observers. Through simulation-based experiments, we show that our approach is able to scale to address complex multi-robot planning scenarios. Through hardware experiments, we demonstrate our ability to generate graphs from real-world data and successfully plan across the entire hierarchy to achieve shared objectives.

Ground-based positioning solutions are critical in featureless, Global Positioning System or GPS-denied environments, where traditional infrastructure-based methods are unavailable. This paper introduces PIONEER a novel ground-based positioning solution that enables the targets to collaboratively determine their positions without reliance on external infrastructure. PIONEER operates by utilizing the received signal strength among targets to minimize their positioning errors and reduce the pseudorange measurement errors, thus enhancing the overall positioning accuracy. PIONEER models the targets' interactions as a potential game with a proven Pure Nash Equilibrium (PNE), guiding the optimal transmission power levels and the peer-targets selection. Two learning-based distributed algorithms, Best Response and Better Reply Dynamics, are introduced to determine the PNE based on the exploitation and exploration processes, respectively. Experimental results conducted in featureless terrains demonstrate the PIONEER's operational advantages, outperforming existing alternatives in reducing the positioning errors and proving its real-world applicability.

Teams of drones have been proposed for many monitoring and data collection applications, including forest fire monitoring, search and rescue, disaster response, and infrastructure inspection. However, robot systems can be stochastic, and uncertainty arises when operating environments are dynamic or hostile. This paper investigates the problem of assigning drones to tasks where the probability that a given group of drones can cooperatively complete a task follows a Poisson-Binomial distribution. We show how to determine if a solution exists and how to calculate an upper bound on the optimal solution. We present a variation of the branch-and-bound algorithm – termed Branch-and-Match – that is tailored to our problem and always finds an optimal solution at the cost of computation time. For a more tractable approach, we present a heuristics-based algorithm – termed M+ILS – that turns the problem into a balanced matching problem to find an initial solution then runs a variation of the Iterated Local Search (ILS) algorithm. Our M+ILS algorithm is applicable to distributed scenarios but finds suboptimal solutions. We evaluate these various algorithms in a simulated forest fire monitoring scenario based on the characteristics of a fleet of real drones. Our empirical results show that the M+ILS algorithm finds solutions with an average performance gap of 2.68% compared to the optimal solution found using the Branch-and-Match algorithm.

Coordinated multi-robot navigation is an essential ability for a team of robots operating in diverse environments. Robot teams often need to maintain specific formations, such as wedge formations, to enhance visibility, positioning, and efficiency during fast movement. However, complex environments such as narrow corridors challenge rigid team formations, which makes effective formation control difficult in real-world environments. To address this challenge, we introduce a novel Adaptive Formation with Oscillation Reduction (AFOR) approach to improve coordinated multi-robot navigation. We develop AFOR under the theoretical framework of hierarchical learning and integrate a spring-damper model with hierarchical learning to enable both team coordination and individual robot control. At the upper level, a graph neural network facilitates formation adaptation and information sharing among the robots. At the lower level, reinforcement learning enables each robot to navigate and avoid obstacles while maintaining the formations. We conducted extensive experiments using Gazebo in the Robot Operating System (ROS), a high-fidelity Unity3D simulator with ROS, and real robot teams. Results demonstrate that AFOR enables smooth navigation with formation adaptation in complex scenarios and outperforms previous methods. More details of this work are provided on the project website: https://hcrlab.gitlab.io/project/afor.

Autonomous ground robots navigating unstructured off-road environments face perceptual challenges, such as sensor obscuration or failure, which can lead to inaccurate perception or navigation failures. While robot adaptation has recently gained increasing attention, self-reflective robot adaptation, where robots understand and adjust to their own sensor limitations, remains under-explored. This paper proposes a novel approach for self-reflective perceptual adaptation in order to enhance robust off-road navigation. Our approach enables a robot to identify its own perceptual difficulties and dynamically adapt in challenging environments. The key novelty is learning a modality-invariant perceptual representation that encodes shared sensor data into a compact feature space. Within this representation space, the robot's dynamics model is also learned, which enables accurate prediction of future navigation paths. Extensive experiments in off-road environments with sensor obstructions and failures demonstrate that our method significantly improves adaptive capabilities and outperforms baseline and state-of-the-art approaches. More details of this work are provided on the project website: https://hcrlab.gitlab.io/project/srpa.

Factor graphs are a ubiquitous tool for multisource inference in robotics and multi-sensor networks. They allow for heterogeneous measurements from many sources to be concurrently represented as factors in the state posterior distribution, so that inference can be conducted via sparse graphical methods. Adding measurements from many sources can supply robustness to state estimation, as seen in distributed pose graph optimization. However, adding excessive measurements to a factor graph can also quickly degrade their performance as more cycles are added to the graph. In both situations, the relevant quality is the redundancy of information. Drawing on recent work in information theory on partial information decomposition (PID), we articulate two potential definitions of redundancy in factor graphs, both within a common axiomatic framework for redundancy in factor graphs. This is the first application of PID to factor graphs, and only one of a few quantitative measures of redundancy.

Estimating terrain traversability in off-road environments requires reasoning about complex interaction dynamics between the robot and these terrains. However, it is challenging to create informative labels to learn a model in a supervised manner for these interactions. We propose a method that learns to predict traversability costmaps by combining exteroceptive environmental information with proprioceptive terrain interaction feedback in a self-supervised manner. Additionally, we propose a novel way of incorporating robot velocity into the costmap prediction pipeline. We validate our method in multiple short and large-scale navigation tasks on challenging off-road terrains using two different large, all-terrain robots. Our short-scale navigation results show that using our learned costmaps leads to overall smoother navigation, and provides the robot with a more fine-grained understanding of the robot-terrain interactions. Our large-scale navigation trials show that we can reduce the number of interventions by up to 57% compared to an occupancy-based navigation baseline in challenging off-road courses ranging from 400 m to 3150 m. Appendix and full experiment videos can be found in our website: https://mateoguaman.github.io/hdif.

This paper presents a method for robust optimization for online incremental Simultaneous Localization and Mapping (SLAM). Due to the NP-Hardness of data association in the presence of perceptual aliasing, tractable (approximate) approaches to data association will produce erroneous measurements. We require SLAM back-ends that can converge to accurate solutions in the presence of outlier measurements while meeting online efficiency constraints. Existing robust SLAM methods either remain sensitive to outliers, become increasingly sensitive to initialization, or fail to provide online efficiency. We present the robust incremental Smoothing and Mapping (riSAM) algorithm, a robust back-end optimizer for incremental SLAM based on Graduated Non-Convexity. We demonstrate on benchmarking datasets that our algorithm achieves online efficiency, outperforms existing online approaches, and matches or improves the performance of existing offline methods.

Using robots to collect data is an effective way to obtain information from the environment and communicate it to a static base station. Furthermore, robots have the capability to communicate with one another, potentially decreasing the time for data to reach the base station. We present a Mixed Integer Linear Program that reasons about discrete routing choices, continuous robot paths, and their effect on the latency of the data collection task. We analyze our formulation, discuss optimization challenges inherent to the data collection problem, and propose a factored formulation that finds optimal answers more efficiently. Our work is able to find paths that reduce latency by up to 101% compared to treating all robots independently in our tested scenarios.

Imagery collected from outdoor visual environments is often degraded due to the presence of dense smoke or haze. A key challenge for research in scene understanding in these degraded visual environments (DVE) is the lack of representative benchmark datasets. These datasets are required to evaluate state-of-the-art object recognition and other computer vision algorithms in degraded settings. In this paper, we address some of these limitations by introducing the first realistic haze image benchmark, from both aerial and ground view, with paired haze-free images, and in-situ haze density measurements. This dataset was produced in a controlled environment with professional smoke generating machines that covered the entire scene, and consists of images captured from the perspective of both an unmanned aerial vehicle (UAV) and an unmanned ground vehicle (UGV). We also evaluate a set of representative state-of-the-art dehazing approaches as well as object detectors on the dataset. The full dataset presented in this paper, including the ground truth object classification bounding boxes and haze density measurements, is provided for the community to evaluate their algorithms at: https://a2i2-archangel.vision. A subset of this dataset has been used for the "Object Detection in Haze" Track of CVPR UG2 2022 challenge at https://cvpr2022.ug2challenge.org/track1.html.

When robots operate in real-world off-road environments with unstructured terrains, the ability to adapt their navigational policy is critical for effective and safe navigation. However, off-road terrains introduce several challenges to robot navigation, including dynamic obstacles and terrain uncertainty, leading to inefficient traversal or navigation failures. To address these challenges, we introduce a novel approach for adaptation by negotiation that enables a ground robot to adjust its navigational behaviors through a negotiation process. Our approach first learns prediction models for various navigational policies to function as a terrain-aware joint local controller and planner. Then, through a new negotiation process, our approach learns from various policies' interactions with the environment to agree on the optimal combination of policies in an online fashion to adapt robot navigation to unstructured off-road terrains on the fly. Additionally, we implement a new optimization algorithm that offers the optimal solution for robot negotiation in real-time during execution. Experimental results have validated that our method for adaptation by negotiation outperforms previous methods for robot navigation, especially over unseen and uncertain dynamic terrains.

Traditional imitation learning provides a set of methods and algorithms to learn a reward function or policy from expert demonstrations. Learning from demonstration has been shown to be advantageous for navigation tasks as it allows for machine learning non-experts to quickly provide information needed to learn complex traversal behaviors. However, a minimal set of demonstrations is unlikely to capture all relevant information needed to achieve the desired behavior in every possible future operational environment. Due to distributional shift among environments, a robot may encounter features that were rarely or never observed during training for which the appropriate reward value is uncertain, leading to undesired outcomes. This paper proposes a Bayesian technique which quantifies uncertainty over the weights of a linear reward function given a dataset of minimal human demonstrations to operate safely in dynamic environments. This uncertainty is quantified and incorporated into a risk averse set of weights used to generate cost maps for planning. Experiments in a 3-D environment with a simulated robot show that our proposed algorithm enables a robot to avoid dangerous terrain completely in two out of three test scenarios and accumulates a lower amount of risk than related approaches in all scenarios without requiring any additional demonstrations.

Exploring robots may fail due to environmental hazards. Thus, robots need to account for the possibility of failure to plan the best exploration paths. Optimizing expected utility enables robots to find plans that balance achievable reward with the inherent risks of exploration. Moreover, when robots rendezvous and communicate to exchange observations, they increase the probability that at least one robot is able to return with the map. Optimal exploration is NP-hard, so we apply a constraint-based approach to enable highly-engineered solution techniques. We model exploration under the possibility of robot failure and communication constraints as an integer, linear program and a generalization of the Vehicle Routing Problem. Empirically, we show that for several scenarios, this formulation produces paths within 50% of a theoretical optimum and achieves twice as much reward as a baseline greedy approach.

This paper presents an approach and introduces new open-source tools that can be used to evaluate robotic mapping algorithms. Also described is an extensive subterranean mine rescue dataset based upon the DARPA Subterranean (SubT) challenge including professionally surveyed ground truth. Finally, some commonly available approaches are evaluated using this metric.

As robotic platforms have become more capable and autonomous, they have increasingly been utilized in time sensitive applications such as search and rescue. To that end, we have developed a system for teams of robots to efficiently explore an environment while taking sensor measurements. The system utilizes an information seeking algorithm that generates high priority points of interest based on the highest expected information gained per distance travelled. In order to coordinate multiple robots, the system partitions the area into different regions according to the effort needed to explore each region. Robots are assigned different regions to measure in order to minimize repetition of work and reduce interference between each robot.We present an information rate adaptive sampling approach for tasking robots within an environment to gather sensor measurements. We evaluated our approach within a simulation environment with one to four robots. Multiple robots are coordinated through our region segmentation approach. The data shows efficiency gains through the use of adaptive information gain rate tasking above a naïve closest point approach. We also see positive results from using the region segmentation technique. We further the experimentation by testing the algorithm on real world robots and verify the results in real world experimentation.

Research in autonomous driving has benefited from a number of visual datasets collected from mobile platforms, leading to improved visual perception, greater scene understanding, and ultimately higher intelligence. However, this set of existing data collectively represents only highly structured, urban environments. Operation in unstructured environments, e.g., humanitarian assistance and disaster relief or off-road navigation, bears little resemblance to these existing data. To address this gap, we introduce the Robot Unstructured Ground Driving (RUGD) dataset with video sequences captured from a small, unmanned mobile robot traversing in unstructured environments. Most notably, this data differs from existing autonomous driving benchmark data in that it contains significantly more terrain types, irregular class boundaries, minimal structured markings, and presents challenging visual properties often experienced in off road navigation, e.g., blurred frames. Over 7, 000 frames of pixel-wise annotation are included with this dataset, and we perform an initial benchmark using state-of-the-art semantic segmentation architectures to demonstrate the unique challenges this data introduces as it relates to navigation tasks.

When working alongside human collaborators in dynamic environments such as a disaster recovery, an unmanned ground vehicle (UGV) may require fast field adaptation to perform its duties or learn novel tasks. In disaster recovery situations, personnel and equipment are constrained, so training must be accomplished with minimal human supervision. In this paper, we introduce a novel framework which uses learned visual perception and inverse optimal control trained with minimal human supervisory examples. This approach is used to learn to mimic navigation behavior and is demonstrated through extensive evaluation in a real-world environment. Finally, we demonstrate the ability to learn an additional behavior with minimal human demonstration in the field.

We introduce an unsupervised semantic scene labeling approach that continuously learns and adapts semantic models discovered within a data stream. While closely related to unsupervised video segmentation, our algorithm is not designed to be an early video processing strategy that produces coherent over-segmentations, but instead, to directly learn higher-level semantic concepts. This is achieved with an ensemble-based approach, where each learner clusters data from a local window in the data stream. Overlapping local windows are processed and encoded in a graph structure to create a label mapping across windows and reconcile the labelings to reduce unsupervised learning noise. Additionally, we iteratively learn a merging threshold criteria from observed data similarities to automatically determine the number of learned labels without human provided parameters. Experiments show that our approach semantically labels video streams with a high degree of accuracy, and achieves a better balance of under and over-segmentation entropy than existing video segmentation algorithms given similar numbers of label outputs.

Autonomous underwater vehicles (AUVs) are cost- and time-efficient systems for environmental sampling. Informative adaptive sampling has been shown to be an effective method of sampling a lake or ocean for environmental modeling. In this paper, we focus on multi-robot coordination for informative adaptive sampling. We use a dynamic Voronoi partitioning approach whereby the vehicles, in a decentralized fashion, repeatedly calculate weighted Voronoi partitions for the space. Each vehicle then runs informative adaptive sampling within their partition. The vehicles can request surfacing events to share data between vehicles. Simulation results show that the addition of the coordination with dynamic Voronoi partitioning results in obtaining higher quality models faster. Thus we created a decentralized, multi-robot coordination approach for informative, adaptive sampling of unknown environments.

We propose a distributed algorithm to estimate the 3D trajectories of multiple cooperative robots from relative pose measurements. Our approach leverages recent results [1] which show that the maximum likelihood trajectory is well approximated by a sequence of two quadratic subproblems. The main contribution of the present work is to show that these subproblems can be solved in a distributed manner, using the distributed Gauss-Seidel (DGS) algorithm. Our approach has several advantages. It requires minimal information exchange, which is beneficial in presence of communication and privacy constraints. It has an anytime flavor: after few iterations the trajectory estimates are already accurate, and they asymptotically convergence to the centralized estimate. The DGS approach scales well to large teams, and it has a straightforward implementation. We test the approach in simulations and field tests, demonstrating its advantages over related techniques.

Multi-concept visual classification is emerging as a common environment perception technique, with applications in autonomous mobile robot navigation. Supervised visual classifiers are typically trained with large sets of images, hand annotated by humans with region boundary outlines followed by label assignment. This annotation is time consuming, and unfortunately, a change in environment requires new or additional labeling to adapt visual perception. The time is takes for a human to label new data is what we call adaptation latency. High adaptation latency is not simply undesirable but may be infeasible for scenarios with limited labeling time and resources. In this paper, we introduce a labeling framework to the environment perception domain that significantly reduces adaptation latency using unsupervised learning in exchange for a small amount of label noise. Using two real-world datasets we demonstrate the speed of our labeling framework, and its ability to collect environment labels that train high performing multi-concept classifiers. Finally, we demonstrate the relevance of this label collection process for visual perception as it applies to navigation in outdoor environments.

Simultaneous Localization and Mapping (SLAM) is not a problem with a one-size-fits-all solution. The literature includes a variety of SLAM approaches targeted at different environments, platforms, sensors, CPU budgets, and applications. We propose OmniMapper, a modular multimodal framework and toolbox for solving SLAM problems. The system can be used to generate pose graphs, do feature-based SLAM, and also includes tools for semantic mapping. Multiple measurement types from different sensors can be combined for multimodal mapping. It is open with standard interfaces to allow easy integration of new sensors and feature types. We present a detailed description of the mapping approach, as well as a software framework that implements this, and present detailed descriptions of its applications to several domains including mapping with a service robot in an indoor environment, large-scale mapping on a PackBot, and mapping with a handheld RGBD camera.

Context is an important factor for domestic service robots to consider when interpreting their environments to perform tasks. In people's homes, rooms are laid out in a specific arrangement to enable comfortable and efficient living; for example, the living room is central to the house, and the dining room is adjacent to the kitchen. The identity of the objects in a room are a strong cue for determining that room's purpose. This paper will present a planner for an autonomous mobile robot system which uses room connectivity topology and object understanding as context for an object search task in a domestic environment.

Place categorization and object recognition are competencies needed by robots to perform a variety of service tasks in the home, such as fetch-and-carry, retrieval, cleaning, meal preparation, and companionship. Context is a powerful cue for place categorization and object recognition; rooms are laid out in a specific fashion to enable comfortable and efficient living, and objects are used within rooms for tasks specific to that room. This paper will present a technique which leverages contextual cues for joint reasoning about object and room classification via a conditional random field model.

We present an extension to our feature based mapping technique that allows for the use of planar surfaces such as walls, tables, counters, or other planar surfaces as landmarks in our mapper. These planar surfaces are measured both in 3D point clouds, as well as 2D laser scans. These sensing modalities compliment each other well, as they differ significantly in their measurable fields of view and maximum ranges. We present experiments to evaluate the contributions of each type of sensor.

Complex and structured landmarks like objects have many advantages over low-level image features for semantic mapping. Low level features such as image corners suffer from occlusion boundaries, ambiguous data association, imaging artifacts, and viewpoint dependance. Artificial landmarks are an unsatisfactory alternative because they must be placed in the environment solely for the robot's benefit. Human environments contain many objects which can serve as suitable landmarks for robot navigation such as signs, objects, and furniture. Maps based on high level features which are identified by a learned classifier could better inform tasks such as semantic mapping and mobile manipulation. In this paper we present a technique for recognizing door signs using a learned classifier as one example of this approach, and demonstrate their use in a graphical SLAM framework with data association provided by reasoning about the semantic meaning of the sign.

Simultaneous Localization and Mapping (SLAM) aims to estimate the maximum likelihood map and robot pose based on a robot's control and sensor measurements. In structured environments, such as human environments, we might have additional domain knowledge that could be applied to produce higher quality mapping results. We present a method for using virtual measurements, which are measurements between two features in our map. To demonstrate this, we present a system that uses such virtual measurements to relate visually detected points to walls detected with a laser scanner.

Classification of spatial regions based on semantic information in an indoor environment enables robot tasks such as navigation or mobile manipulation to be spatially aware. The availability of contextual information can significantly simplify operation of a mobile platform. We present methods for automated recognition and classification of spaces into separate semantic regions and use of such information for generation of a topological map of an environment. The association of semantic labels with spatial regions is based on Human Augmented Mapping. The methods presented in this paper are evaluated both in simulation and on real data acquired from an office environment.

The goal of simultaneous localization and mapping (SLAM) is to compute the posterior distribution over landmark poses. Typically, this is made possible through the static world assumption - the landmarks remain in the same location throughout the mapping procedure. Some prior work has addressed this assumption by splitting maps into static and dynamic sets, or by recognizing moving landmarks and tracking them. In contrast to previous work, we apply an Expectation Maximization technique to a graph based SLAM approach and allow landmarks to be dynamic. The batch nature of this operation enables us to detect moveable landmarks and factor them out of the map. We demonstrate the performance of this algorithm with a series of experiments with moveable landmarks in a structured environment.

Simultaneous Localization and Mapping (SLAM) suffers from a quadratic space and time complexity per update step. Recent advancements have been made in approximating the posterior by forcing the information matrix to remain sparse as well as exact techniques for generating the posterior in the full SLAM solution to both the trajectory and the map. Current approximate techniques for maintaining an online estimate of the map for a robot to use while exploring make capacity-based decisions about when to split into sub-maps. This paper will describe an alternative partitioning strategy for online approximate real-time SLAM which makes use of normalized graph cuts to remove less information from the full map.