Scene reconstruction is an essential capability for underwater robots navigating in close proximity to structures. Monocular vision-based reconstruction methods are unreliable in turbid waters and lack depth scale information. Sonars are robust to turbid water and non-uniform lighting conditions, however, they have low resolution and elevation ambiguity. This work proposes a real-time opti-acoustic scene reconstruction method that is specially optimized to work in turbid water. Our strategy avoids having to identify point features in visual data and instead identifies regions of interest in the data. We then match relevant regions in the image to corresponding sonar data. A reconstruction is obtained by leveraging range data from the sonar and elevation data from the camera image. Experimental comparisons against other vision-based and sonar-based approaches at varying turbidity levels, and field tests conducted in marina environments, validate the effectiveness of the proposed approach. We have made our code open-source to facilitate reproducibility and encourage community engagement.

We present DRACo-SLAM2, a distributed SLAM framework for underwater robot teams equipped with multibeam imaging sonar. This framework improves upon the original DRACo-SLAM by introducing a novel representation of sonar maps as object graphs and utilizing object graph matching to achieve time-efficient inter-robot loop closure detection without relying on prior geometric information. To better-accommodate the needs and characteristics of underwater scan matching, we propose incremental Group-wise Consistent Measurement Set Maximization (GCM), a modification of Pairwise Consistent Measurement Set Maximization (PCM), which effectively handles scenarios where nearby inter-robot loop closures share similar registration errors. The proposed approach is validated through extensive comparative analyses on simulated and real-world datasets.

We present a novel multi-altitude camera pose estimation system, addressing the challenges of robust and accurate localization across varied altitudes when only considering sparse image input. The system effectively handles diverse environmental conditions and viewpoint variations by integrating the cross-view transformer, deep features, and structure-from-motion into a unified framework. To benchmark our method and foster further research, we introduce two newly collected datasets specifically tailored for multi-altitude camera pose estimation; datasets of this nature remain rare in the current literature. The proposed framework has been validated through extensive comparative analyses on these datasets, demonstrating that our system achieves superior performance in both accuracy and robustness for multi-altitude sparse pose estimation tasks compared to existing solutions, making it well suited for real-world robotic applications such as aerial navigation, search and rescue, and automated inspection.

This paper presents an approach for the localization of an Unmanned Underwater Vehicle (UUV) in a cooperative team with a tethered Unmanned Surface Vehicle (USV). For the localization, the UUV and the USV carry a camera and a sonar respectively to observe each other. The vehicle states are split between Extended Kalman Filter and grid-based estimators based on which sensors provide Gaussian or non-Gaussian observations of each state. Specifically, the horizontal position of the UUV is estimated using a grid-based method because the camera and sonar that observe these states provide non-Gaussian observations when they cannot detect their target. Additionally, the tether to the USV is treated as a non-Gaussian observation that prevents unbounded error growth. Validation of the technique was performed in simulations using sensor models developed based on testing in a lake and pool.

Reliable localization is an essential capability for marine robots navigating in GPS-denied environments. SLAM, commonly used to mitigate dead reckoning errors, still fails in feature-sparse environments or with limited-range sensors. Pose estimation can be improved by incorporating the uncertainty prediction of future poses into the planning process and choosing actions that reduce uncertainty. However, performing belief propagation is computationally costly, especially when operating in large-scale environments. This work proposes a computationally efficient planning under uncertainty framework suitable for large-scale, feature-sparse environments. Our strategy leverages SLAM graph and occupancy map data obtained from a prior exploration phase to create a virtual map, describing the uncertainty of each map cell using a multivariate Gaussian. The virtual map is then used as a cost map in the planning phase, and performing belief propagation at each step is avoided. A receding horizon planning strategy is implemented, managing a goal-reaching and uncertainty-reduction tradeoff. Simulation experiments in a realistic underwater environment validate this approach. Experimental comparisons against a full belief propagation approach and a standard shortest-distance approach are conducted.

We propose an autonomous exploration algorithm designed for decentralized multi-robot teams, which takes into account map and localization uncertainties of range-sensing mobile robots. Virtual landmarks are used to quantify the combined impact of process noise and sensor noise on map uncertainty. Additionally, we employ an iterative expectation-maximization inspired algorithm to assess the potential out-comes of both a local robot’s and its neighbors’ next-step actions. To evaluate the effectiveness of our framework, we conduct a comparative analysis with state-of-the-art algorithms. The results of our experiments show the proposed algorithm’s capacity to strike a balance between curbing map uncertainty and achieving efficient task allocation among robots.

Collision avoidance algorithms for Autonomous Surface Vehicles (ASV) that follow the Convention on the International Regulations for Preventing Collisions at Sea (COLREGs) have been proposed in recent years. However, it may be difficult and unsafe to follow COLREGs in congested waters, where multiple ASVs are navigating in the presence of static obstacles and strong currents, due to the complex interactions. To address this problem, we propose a decentralized multi-ASV collision avoidance policy based on Distributional Reinforcement Learning, which considers the interactions among ASVs as well as with static obstacles and current flows. We evaluate the performance of the proposed Distributional RL based policy against a traditional RL-based policy and two classical methods, Artificial Potential Fields (APF) and Reciprocal Velocity Obstacles (RVO), in simulation experiments, which show that the proposed policy achieves superior performance in navigation safety, while requiring minimal travel time and energy. A variant of our framework that automatically adapts its risk sensitivity is also demonstrated to improve ASV safety in highly congested environments.

Recent work has shown impressive localization performance using only images of ground textures taken with a downward facing monocular camera. This provides a reliable navigation method that is robust to feature sparse environments and challenging lighting conditions. However, these localization methods require an existing map for comparison. Our work aims to relax the need for a map by introducing a full simultaneous localization and mapping (SLAM) system. By not requiring an existing map, setup times are minimized and the system is more robust to changing environments. This SLAM system uses a combination of several techniques to accomplish this. Image keypoints are identified and projected into the ground plane. These keypoints, visual bags of words, and several threshold parameters are then used to identify overlapping images and revisited areas. The system then uses robust Mestimators to estimate the transform between robot poses with overlapping images and revisited areas. These optimized estimates make up the map used for navigation. We show, through experimental data, that this system performs reliably on many ground textures, but not all.

Autonomous navigation of Unmanned Surface Vehicles (USV) in marine environments with current flows is challenging, and few prior works have addressed the sensor-based navigation problem in such environments under no prior knowledge of the current flow and obstacles. We propose a Distributional Reinforcement Learning (RL) based local path planner that learns return distributions which capture the uncertainty of action outcomes, and an adaptive algorithm that automatically tunes the level of sensitivity to the risk in the environment. The proposed planner achieves a more stable learning performance and converges to safer policies than a traditional RL based planner. Computational experiments demonstrate that comparing to a traditional RL based planner and classical local planning methods such as Artificial Potential Fields and the Bug Algorithm, the proposed planner is robust against environmental flows, and is able to plan trajectories that are superior in safety, time and energy consumption.

An essential task for a multi-robot system is generating a common understanding of the environment and relative poses between robots. Cooperative tasks can be executed only when a vehicle has knowledge of its own state and the states of the team members. However, this has primarily been achieved with direct rendezvous between underwater robots, via inter-robot ranging. We propose a novel distributed multi-robot simultaneous localization and mapping (SLAM) framework for underwater robots using imaging sonar-based perception. By passing only scene descriptors between robots, we do not need to pass raw sensor data unless there is a likelihood of inter-robot loop closure. We utilize pairwise consistent measurement set maximization (PCM), making our system robust to erroneous loop closures. The functionality of our system is demonstrated using two real-world datasets, one with three robots and another with two robots. We show that our system effectively estimates the trajectories of the multi-robot system and keeps the bandwidth requirements of inter-robot communication low. To our knowledge, this paper describes the first instance of multi-robot SLAM using real imaging sonar data (which we implement offline, using simulated communication). Code link: https://github.com/jake3991/DRACo-SLAM.

System design tools are often only available as input–output blackboxes: for a given design as input, they compute an output representing system behavior. Blackboxes are intended to be run in the forward direction. This article presents a new method of solving the “inverse design problem,” namely, given requirements or constraints on output, find an input that also optimizes an objective function. This problem is challenging for several reasons. First, blackboxes are not designed to be run in reverse. Second, inputs and outputs can be discrete and continuous. Third, finding designs concurrently satisfying a set of requirements is hard because designs satisfying individual requirements may conflict with each other. Fourth, blackbox evaluations can be expensive. Finally, evaluations can sometimes fail to produce an output due to nonconvergence of underlying numerical algorithms. This article presents CNMA, a new method of solving the inverse problem that overcomes these challenges. CNMA tries to sample only the part of the design space relevant to solving the inverse problem, leveraging the power of neural networks, mixed-integer linear programs, and a new learning-from-failure feedback loop. This article also presents a parallel version of CNMA that improves the efficiency and quality of solutions over the sequential version and tries to steer it away from local optima. CNMA’s performance is evaluated against conventional optimization methods for seven nonlinear design problems of 8 (two problems), 10, 15, 36 and 60 real-valued dimensions and one with 186 binary dimensions. Conventional methods evaluated are stable, off-the-shelf implementations of the Bayesian optimization with the Gaussian Processes, Nelder–Mead, and Random Search. The first two do not even produce a solution for problems that are high dimensional, having both discrete and continuous variables or whose blackboxes fail to return values for some inputs. CNMA produces solutions for all problems. When conventional methods do produce solutions, CNMA improves upon their performance by 1%–87%.

This paper studies the problem of autonomous exploration under localization uncertainty for a mobile robot with 3D range sensing. We present a framework for self-learning a high-performance exploration policy in a single simulation environment, and transferring it to other environments, which may be physical or virtual. Recent work in transfer learning achieves encouraging performance by domain adaptation and domain randomization to expose an agent to scenarios that fill the inherent gaps in sim2sim and sim2real approaches. However, it is inefficient to train an agent in environments with randomized conditions to learn the important features of its current state. An agent can use domain knowledge provided by human experts to learn efficiently. We propose a novel approach that uses graph neural networks in conjunction with deep reinforcement learning, enabling decision-making over graphs containing relevant exploration information provided by human experts to predict a robot's optimal sensing action in belief space. The policy, which is trained only in a single simulation environment, offers a real-time, scalable, and transferable decision-making strategy, resulting in zero-shot transfer to other simulation environments and even real-world environments.

Recent work has achieved dense 3D reconstruction with wide-aperture imaging sonar using a stereo pair of orthogonally oriented sonars. This allows each sonar to observe a spatial dimension that the other is missing, without requiring any prior assumptions about scene geometry. However, this is achieved only in a small region with overlapping fields-of-view, leaving large regions of sonar image observations with an unknown elevation angle. Our work aims to achieve large-scale 3D reconstruction more efficiently using this sensor arrangement. We propose dividing the world into semantic classes to exploit the presence of repeating structures in the subsea environment. We use a Bayesian inference framework to build an understanding of each object class’s geometry when 3D information is available from the orthogonal sonar fusion system, and when the elevation angle of our returns is unknown, our framework is used to infer unknown 3D structure. We quantitatively validate our method in a simulation and use data collected from a real outdoor littoral environment to demonstrate the efficacy of our framework in the field.

We propose a methodology for robust, real-time place recognition using an imaging lidar, which yields image-quality high-resolution 3D point clouds. Utilizing the intensity readings of an imaging lidar, we project the point cloud and obtain an intensity image. ORB feature descriptors are extracted from the image and encoded into a bag-of-words vector. The vector, used to identify the point cloud, is inserted into a database that is maintained by DBoW for fast place recognition queries. The returned candidate is further validated by matching visual feature descriptors. To reject matching outliers, we apply PnP, which minimizes the reprojection error of visual features’ positions in Euclidean space with their correspondences in 2D image space, using RANSAC. Combining the advantages from both camera and lidar-based place recognition approaches, our method is truly rotation-invariant, and can tackle reverse revisiting and upside down revisiting. The proposed method is evaluated on datasets gathered from a variety of platforms over different scales and environments. Our implementation and datasets are available at https://git.io/image-lidar.

We propose a framework for tightly-coupled lidar-visual-inertial odometry via smoothing and mapping, LVI-SAM, that achieves real-time state estimation and map-building with high accuracy and robustness. LVI-SAM is built atop a factor graph and is composed of two sub-systems: a visual-inertial system (VIS) and a lidar-inertial system (LIS). The two sub-systems are designed in a tightly-coupled manner, in which the VIS leverages LIS estimation to facilitate initialization. The accuracy of the VIS is improved by extracting depth information for visual features using lidar measurements. In turn, the LIS utilizes VIS estimation for initial guesses to support scan-matching. Loop closures are first identified by the VIS and further refined by the LIS. LVI-SAM can also function when one of the two sub-systems fails, which increases its robustness in both texture-less and feature-less environments. LVI-SAM is extensively evaluated on datasets gathered from several platforms over a variety of scales and environments. Our implementation is available at https://git.io/lvi-sam.

We describe a new approach for managing aleatoric uncertainty in the Reinforcement Learning (RL) paradigm. Instead of selecting actions according to a single statistic, we propose a distributional method based on the second-order stochastic dominance (SSD) relation. This compares the inherent dispersion of random returns induced by actions, producing a comprehensive evaluation of the environment’s uncertainty. The necessary conditions for SSD require estimators to predict accurate second moments. To accommodate this, we map the distributional RL problem to a Wasserstein gradient flow, treating the distributional Bellman residual as a potential energy functional. We propose a particle-based algorithm for which we prove optimality and convergence. Our experiments characterize the algorithm’s performance and demonstrate how uncertainty and performance are better balanced using an SSD policy than with other risk measures.

We consider an autonomous exploration problem in which a range-sensing mobile robot is tasked with accurately mapping the landmarks in an a priori unknown environment efficiently in real-time; it must choose sensing actions that both curb localization uncertainty and achieve information gain. For this problem, belief space planning methods that forward- simulate robot sensing and estimation may often fail in real-time implementation, scaling poorly with increasing size of the state, belief and action spaces. We propose a novel approach that uses graph neural networks (GNNs) in conjunction with deep reinforcement learning (DRL), enabling decision-making over graphs containing exploration information to predict a robot's optimal sensing action in belief space. The policy, which is trained in different random environments without human intervention, offers a real-time, scalable decision-making process whose high-performance exploratory sensing actions yield accurate maps and high rates of information gain.

The ability to infer map variables and estimate pose is crucial to the operation of autonomous mobile robots. In most cases the shared dependency between these variables is modeled through a multivariate Gaussian distribution, but there are many situations where that assumption is unrealistic. Our paper shows how it is possible to relax this assumption and perform simultaneous localization and mapping (SLAM) with a larger class of distributions, whose multivariate dependency is represented with a copula model. We integrate the distribution model with copulas into a Sequential Monte Carlo estimator and show how unknown model parameters can be learned through gradient-based optimization. We demonstrate our approach is effective in settings where Gaussian assumptions are clearly violated, such as environments with uncertain data association and nonlinear transition models.

We propose a novel approach to handling the ambiguity in elevation angle associated with the observations of a forward looking multi-beam imaging sonar, and the challenges it poses for performing an accurate 3D reconstruction. We utilize a pair of sonars with orthogonal axes of uncertainty to independently observe the same points in the environment from two different perspectives, and associate these observations. Using these concurrent observations, we can create a dense, fully defined point cloud at every time-step to aid in reconstructing the 3D geometry of underwater scenes. We will evaluate our method in the context of the current state of the art, for which strong assumptions on object geometry limit applicability to generalized 3D scenes. We will discuss results from laboratory tests that quantitatively benchmark our algorithm's reconstruction capabilities, and results from a real-world, tidal river basin which qualitatively demonstrate our ability to reconstruct a cluttered field of underwater objects.

We propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO-SAM, that achieves highly accurate, real-time mobile robot trajectory estimation and map-building. LIO-SAM formulates lidar-inertial odometry atop a factor graph, allowing a multitude of relative and absolute measurements, including loop closures, to be incorporated from different sources as factors into the system. The estimated motion from inertial measurement unit (IMU) pre-integration de-skews point clouds and produces an initial guess for lidar odometry optimization. The obtained lidar odometry solution is used to estimate the bias of the IMU. To ensure high performance in real-time, we marginalize old lidar scans for pose optimization, rather than matching lidar scans to a global map. Scan-matching at a local scale instead of a global scale significantly improves the real-time performance of the system, as does the selective introduction of keyframes, and an efficient sliding window approach that registers a new keyframe to a fixed-size set of prior "sub-keyframes." The proposed method is extensively evaluated on datasets gathered from three platforms over various scales and environments.

In this paper, we propose a novel approach for underwater simultaneous localization and mapping using a multibeam imaging sonar for 3D terrain mapping tasks. The high levels of noise and the absence of elevation angle information in sonar images present major challenges for data association and accurate 3D mapping. Instead of repeatedly projecting extracted features into Euclidean space, we apply optical flow within bearing-range images for tracking extracted features. To deal with degenerate cases, such as when tracking is interrupted by noise, we model the subsea terrain as a Gaussian Process random field on a Chow-Liu tree. Terrain factors are incorporated into the factor graph, aimed at smoothing the terrain elevation estimate. We demonstrate the performance of our proposed algorithm in a simulated environment, which shows that terrain factors effectively reduce estimation error. We also show ROV experiments performed in a variable-elevation tank environment, where we are able to construct a descriptive and smooth height estimate of the tank bottom.

We consider the problem of autonomous mobile robot exploration in an unknown environment taking into account the robot's mapping rate, map uncertainty, and state estimation uncertainty. This paper presents an exploration framework built upon segment-aided pose SLAM adapted for better active localization. We build on our previous work on expectation maximization (EM) exploration, which explicitly models unknown landmarks as latent variables and predicts their expected uncertainty, to resolve the lack of landmark state in denser instances of SLAM. The proposed system comprises path generation, place recognition forecasting, belief propagation and utility evaluation using a virtual map. We analyze the performance in simulated experiments, showing that our algorithm maintains higher coverage speed in exploration as well as lower mapping and localization error. The real-time applicability is demonstrated on an unmanned ground vehicle.

In this paper, we consider the problem of building descriptive three-dimensional (3-D) maps from sparse and noisy range sensor data. We expand our previously proposed method leveraging Bayesian kernel inference for prediction of occupancy in locations not directly observed by a range sensor. In this paper, we show that our kernel inference approach generalizes previous “counting sensor model” approaches from discrete occupancy grids to continuous maps. Our approach enables prediction about occupancy in regions unobserved by the range sensor based on local measurements, and smoothly transitions to a prior in regions lacking sufficient data for reliable inference. Furthermore, we demonstrate quantitatively using simulated data that the mapping performance of our method can be improved by considering rays as continuous observations, rather than sampling discrete free-space point observations along rays. Though the maps produced by our method are in principle continuous, discretizing space affords us several computational advantages, including the ability to apply recursive Bayesian updates, that allow us to perform inference very efficiently, even on large datasets. To demonstrate this advantage, we present experimental results applying this method to large-scale lidar data collected with a ground robot, showing real-time performance. Other field robotics applications, including underwater 3-D mapping with sonar, are explored qualitatively.

We propose a lightweight and ground-optimized lidar odometry and mapping method, LeGO-LOAM, for realtime six degree-of-freedom pose estimation with ground vehicles. LeGO-LOAM is lightweight, as it can achieve realtime pose estimation on a low-power embedded system. LeGO-LOAM is ground-optimized, as it leverages the presence of a ground plane in its segmentation and optimization steps. We first apply point cloud segmentation to filter out noise, and feature extraction to obtain distinctive planar and edge features. A two-step Levenberg-Marquardt optimization method then uses the planar and edge features to solve different components of the six degree-of-freedom transformation across consecutive scans. We compare the performance of LeGO-LOAM with a state-of-the-art method, LOAM, using datasets gathered from variable-terrain environments with ground vehicles, and show that LeGO-LOAM achieves similar or better accuracy with reduced computational expense. We also integrate LeGO-LOAM into a SLAM framework to eliminate the pose estimation error caused by drift, which is tested using the KITTI dataset.

We study the ambiguous data association problem confronting simultaneous localization and mapping (SLAM), specifically for the autonomous exploration of environments lacking rich features. In such environments, a single false positive assignment might lead to catastrophic failure, which even robust back-ends may be unable to resolve. Inspired by multiple hypothesis tracking, we present a novel approach to effectively manage multiple hypotheses (MH) of data association inherited from traditional joint compatibility branch and bound (JCBB), which entails the generation, ordering and elimination of hypotheses. We analyze the performance of MHJCBB in two particular situations, one applying it to SLAM over a predefined trajectory and the other showing its applicability in exploring unknown environments. Statistical results demonstrate that MHJCBB's maintenance of diverse hypotheses under ambiguous conditions significantly improves map accuracy.

We consider the problem of building accurate and descriptive 3D occupancy maps of an environment from sparse and noisy range sensor data. We seek to accomplish this task by constructing a predictive model online and inferring the occupancy probability of regions we have not directly observed. We propose a novel algorithm leveraging recent advances in data structures for mapping, sparse kernels, and Bayesian nonparametric inference. The resulting inference model has several desirable properties in comparison to existing methods, including speed of computation, the ability to be recursively updated without approximation, and consistency between batch and online inference. The method also reverts to the use of a specified prior state when insufficient relevant training data exist to predict the occupancy probability of a query point, a property which is attractive for motion planning and exploration applications with mobile robots.

We present a novel approach to perform underwater simultaneous localization and mapping (SLAM) using a small inspection-class remotely operated vehicle (ROV) equipped with a single-beam scanning sonar, amidst high levels of noise present in the sonar data, and in the absence of inertial/odometry measurements. Features are extracted from hierarchically grouped clusters of sonar returns, data association is performed via the iterative joint compatibility test, and the vehicle's trajectory and map are estimated using incremental smoothing and mapping (iSAM). The resulting point clouds derived from the ROV's sonar are used to produce Gaussian process occupancy maps, which interpolate among gaps in the acoustic range data to produce descriptive 3D maps of submerged structures. The proposed localization and mapping approach is demonstrated using data gathered in two harbor environments in close proximity to piers and seawalls.

We consider an autonomous mapping and exploration problem in which a range-sensing mobile robot is guided by an information-based controller through an a priori unknown environment, choosing to collect its next measurement at the location estimated to yield the maximum information gain within its current field of view. We propose a novel and time-efficient approach to predict the most informative sensing action using a deep neural network. After training the deep neural network on a series of thousands of randomly-generated “dungeon maps”, the predicted optimal sensing action can be computed in constant time, with prospects for appealing scalability in the testing phase to higher dimensional systems. We evaluated the performance of deep neural networks on the autonomous exploration of two-dimensional workspaces, comparing several different neural networks that were selected due to their success in recent ImageNet challenges. Our computational results demonstrate that the proposed method provides high efficiency as well as accuracy in selecting informative sensing actions that support autonomous mobile robot exploration.

We characterize and propose advances in the technique of Belief Roadmap Search (BRMS), the process of searching a roadmap in belief space for robot motion planning under localization uncertainty. We discuss the conditions required for optimal substructure in the single-source search of a roadmap in belief space, demonstrating that there are several desirable cost functions for which this property cannot be achieved. Practical performance issues of BRMS are discussed, including the implications of a commonly-used anti-cycling rule, and the computational complexity realized in practical applications of the technique. We propose a best-first implementation of BRMS, in contrast to the standard breadth-first implementation, which we show to improve the computational cost of search by up to 49% by eliminating unnecessary node expansions - the mechanics of both approaches are compared in detail. A variety of motion planning examples are explored.

We present a novel formulation of Hilbert mapping in which we construct a global occupancy map by incrementally fusing local overlapping Hilbert maps. Rather than maintain a single supervised learning model for the entire map, a new model is trained with each of a robot's range scans, and queried at all points within the robot's perceptual field. We treat the probabilistic output of the classifier as a sensor, employing sensor fusion to merge local maps. This formulation allows Hilbert mapping to be used incrementally in real-world mapping scenarios with overlap between sensor observations. The methodology is applied to three-dimensional map-building, and evaluated using real and simulated 3D range data.

We present a novel algorithm to produce descriptive online 3D occupancy maps using Gaussian processes (GPs). GP regression and classification have met with recent success in their application to robot mapping, as GPs are capable of expressing rich correlation among map cells and sensor data. However, the cubic computational complexity has limited its application to large-scale mapping and online use. In this paper we address this issue first by proposing test-data octrees, octrees within blocks of the map that prune away nodes of the same state, condensing the number of test data used in a regression, in addition to allowing fast data retrieval. We also propose a nested Bayesian committee machine which, after new sensor data is partitioned among several GP regressions, fuses the result and updates the map with greatly reduced complexity. Finally, by adjusting the range of influence of the training data and tuning a variance threshold implemented in our method's binary classification step, we are able to control the richness of inference achieved by GPs - and its tradeoff with classification accuracy. The performance of the proposed approach is evaluated with both simulated and real data, demonstrating that the method may serve both as an improved-accuracy classifier, and as a predictive tool to support autonomous navigation.

We consider an autonomous exploration problem in which a mobile robot is guided by an information-based controller through an a priori unknown environment, choosing to collect its next measurement at the location estimated to be most informative within its current field of view. We propose a novel approach to predict mutual information (MI) using Bayesian optimization. Over several iterations, candidate sensing actions are suggested by Bayesian optimization and added to a committee that repeatedly trains a Gaussian process (GP). The GP estimates MI throughout the robot's action space, serving as the basis for an acquisition function used to select the next candidate. The best sensing action in the committee is executed by the robot. This approach is compared over several environments with two batch methods, one which chooses the most informative action from a set of pseudo-random samples whose MI is explicitly evaluated, and one that applies GP regression to this sample set. Our computational results demonstrate that the proposed method provides not only computational efficiency and rapid map entropy reduction, but also robustness in comparison with competing approaches.

We present a novel path planning algorithm that, starting from a probabilistic roadmap, efficiently constructs a product graph used to search for a near optimal solution of a multiobjective optimization problem. The goal is to find paths that minimize a primary cost, such as the path length from start to goal, subject to a bound on a secondary cost such as the state estimation error covariance. The proposed algorithm is efficient as it relies on a scalar metric, related to the largest eigenvalue of the error covariance, and adaptively quantizes the secondary cost, yielding a product graph whose number of vertices and edges provides a good tradeoff between optimality and computational complexity. We further show how our approach can be extended to handle constraints on the probability of collision avoidance specified at every vertex along the path. Numerical examples show 1) how the computed paths change as a function of the specified bound on the secondary costs, and 2) the tradeoff between accuracy and computational efficiency of the proposed approach compared with methods where the product graph is built by quantizing the secondary cost uniformly.

This paper considers the problem of planning a path for an autonomous vehicle from a start to a goal location in presence of sensor intermittency modeled as a stochastic process, in addition to process and measurement noise. The aim is to plan a path that minimizes the localizational uncertainty for the vehicle upon arriving at the goal location. The main contribution of this paper is two-fold. We first show that it is possible to obtain an analytical bound on the performance of a state estimator under sensor misdetection (intermittency) occurring stochastically over time. We then use this bound in a sample-based path planning algorithm to produce a path that trades off accuracy and robustness. This extends the recent body of work on planning under uncertainty to include the fact that sensors may not provide any measurement owing to misdetection. This is caused either by adverse environmental conditions that prevent the sensors from making measurements or by the fundamental limitations of the sensors. Examples include RF-based ranging devices that intermittently do not receive the signal from beacons because of obstacles or the misdetection of features by a camera system in detrimental lighting conditions. Computational results demonstrate the benefit of the approach and comparisons are made with the state of the art in path planning in belief space.

We propose a hierarchical planning framework for mission planning and execution in uncertain and dynamic environments. We consider missions that involve motion planning in large, cluttered environments, trading off mission objectives while satisfying logical/spatial/temporal constraints. Our framework enables the decomposition of the planning problem across different layers, leveraging the difference in spatial and temporal scales of the mission objectives. We show that this framework facilitates contingency management under unanticipated events. Interaction between the various layers requires consistent model abstractions and common message semantics. To satisfy these requirements, we adopt a generic knowledge-based architecture that is independent from a specific application domain. We show a specific instance of our framework using a Constrained Markov Decision Process (CMDP) planner at the higher level and a Multi-Objective Probabilistic Roadmap (MO-PRM) planner at the lower level. The resulting planning system is tested in a realistic scenario where an agent is tasked with a mission in a large urban threat rich environment under dynamic uncertain conditions. The mission specification includes a Linear Temporal Logic (LTL) formula that defines the desired behaviors, a list of metrics to be optimized and a list of constraints on time, resources and probability of mission success.

We discuss the problem of inspecting an underwater structure, such as a submerged ship hull, with an autonomous underwater vehicle (AUV). In such scenarios, the goal is to construct an accurate 3D model of the structure and to detect any anomalies (e.g., foreign objects or deformations). We propose a method for constructing 3D meshes from sonar-derived point clouds that provides watertight surfaces, and we introduce uncertainty modeling through non-parametric Bayesian regression. Uncertainty modeling provides novel cost functions for planning the path of the AUV to minimize a metric of inspection performance. We draw connections between the resulting cost functions and submodular optimization, which provides insight into the formal properties of active perception problems. In addition, we present experimental trials that utilize profiling sonar data from ship hull inspection.

We present a hybrid algorithm that plans feasible paths for 100% sensor coverage of complex 3D structures. The structures to be inspected are segmented to isolate planar areas, and back-and-forth sweep paths are generated to view as much of these planar areas as possible while avoiding collision. A randomized planning procedure fills in the remaining gaps in coverage. The problem of selecting an order to traverse the elements of the inspection is solved by reduction to the traveling salesman problem. We present results of the planning algorithm for an autonomous underwater vehicle inspecting the in-water portion of a ship hull. The randomized configurations succeed in observing confined and occluded areas, while the 2D sweep paths succeed in covering the open areas.

Conventional methods for rotation angle estimation are not very robust to variations in object shape or intensity. However in real object recognition scenarios like in underwater sonar images, the object seldom retains the same appearance in different test cases. Object representation using Zernike moments allows to capture these variabilities in a way that makes it robust in the context of rotation angle estimation. This paper presents a novel way to exploit the phase information of Zernike moments to infer the object orientation. This is achieved via a compact directional representation that describes the variation in object shape along different directions. Results yielded on the DIDSON sonar imageset collected by CSAIL at MIT show that the method can robustly infer the relative orientation between objects.

A new algorithm for solving multi-goal planning problems in the presence of obstacles is introduced. We extend ant colony optimization (ACO) from its well-known application, the traveling salesman problem (TSP), to that of multi-goal feasible path planning for inspection and surveillance applications. Specifically, the ant colony framework is combined with a sampling-based point-to-point planning algorithm; this is compared with two successful sampling-based multi-goal planning algorithms in an obstacle-filled two-dimensional environment. Total mission time, a function of computational cost and the duration of the planned mission, is used as a basis for comparison. In our application of interest, autonomous underwater inspections, the ACO algorithm is found to be the best-equipped for planning in minimum mission time, offering an interior point in the tradeoff between computational complexity and optimality.

We introduce an algorithm to achieve complete sensor coverage of complex, three-dimensional structures surveyed by an autonomous agent with multiple degrees of freedom. Motivated by the application of an ocean vehicle performing an autonomous ship hull inspection, we consider a planning problem for a fully-actuated, six degree-of-freedom hovering AUV using a bathymetry sonar to inspect the complex structures underneath a ship hull. We consider a discrete model of the structure to be inspected, requiring only that the model be provided in the form of a closed triangular mesh. A dense graph of feasible paths is constructed in the robot's configuration space until the set of edges in the graph allows complete coverage of the structure. Then, we approximate the minimum-cost closed walk along the graph which observes 100% of the structure. We emphasize the embedding of observations within the edges of the graph as a means of utilizing all available sensor data in planning the inspection.

In this paper we address the problem of drift-free navigation for underwater vehicles performing harbor surveillance and ship hull inspection. Maintaining accurate localization for the duration of a mission is important for a variety of tasks, such as planning the vehicle trajectory and ensuring coverage of the area to be inspected. Our approach only uses onboard sensors in a simultaneous localization and mapping setting and removes the need for any external infrastructure like acoustic beacons. We extract dense features from a forward-looking imaging sonar and apply pair-wise registration between sonar frames. The registrations are combined with onboard velocity, attitude and acceleration sensors to obtain an improved estimate of the vehicle trajectory. We show results from several experiments that demonstrate drift-free navigation in various underwater environments.

Our aim is to explore the fundamental stability issues of a robotic vehicle carrying out localization, mapping, and feedback control in a perturbation-filled environment. Motivated by the application of an ocean vehicle performing an autonomous ship hull inspection, our planar vehicle model performs localization using point features from a given map. Cases in which the agent must update the map are also considered. The stability of the controller and estimator duo is investigated using a pair of theorems requiring boundedness and convergence of the transition matrix Euclidean norm. These theorems yield a stability test for the feedback controller. Perturbations are then considered using a theorem on the convergence on the perturbed system transition matrix, yielding a robustness test for the estimator. Together, these tests form a set of tools which can be used in planning and evaluating the robustness of marine vehicle survey trajectories, which is demonstrated through experiment.