In this paper, we propose a third-order, i.e., white-noise-on-jerk, Gaussian Process (GP) Trajectory Representation (TR) framework for continuous-time (CT) motion estimation (ME) tasks. Our framework features a unified trajectory representation that encapsulates the kinematic models of both SO(3)$times$R3and SE(3) pose representations. This encapsulation strategy allows users to use the same implementation of measurement-based factors for either choice of pose representation, which facilitates experimentation and comparison to make a better choice for the ME task. In addition, unique to our framework, we derive the kinematic models with theclosed-form temporal derivatives of the local variables ofSO(3) and SE(3), which so far has only been approximated based on Taylor expansion in the literature. Our experiments show that these kinematic models can improve the estimation accuracy in high-speed scenarios. All analytical Jacobians of the interpolated states with respect to the support states of the trajectory representation, as well as the motion prior factors, are also provided for accelerated Gauss-Newton (GN) optimization. Our experiments demonstrate the efficacy and efficiency of the framework in various motion estimation tasks such as localization, calibration, and odometry, facilitating fast prototyping for ME researchers. We release the source code for the benefit of the community. Our project is available athttps://github.com/brytsknguyen/gptr.

We present a new method to combine several rigidly connected but physically separated IMUs through a weighted average into a single virtual IMU (VIMU). This has the benefits of (i) reducing process noise through averaging, and (ii) allowing for tuning the location of the VIMU. The VIMU can be placed to be coincident with, for example, a camera frame or GNSS frame, thereby offering a quality-of-life improvement for users. Specifically, our VIMU removes the need to consider any lever-arm terms in the propagation model. We also present a quadratic programming method for selecting the weights to minimize the noise of the VIMU while still selecting the placement of its reference frame. We tested our method in simulation and validated it on a real dataset. The results show that our averaging technique works for IMUs with large separation and performance gain is observed in both the simulation and the real experiment compared to using only a single IMU.

We present closed-form expressions for marginalizing and conditioning Gaussians onto linear manifolds, and demonstrate how to apply these expressions to smooth non-linear manifolds through linearization. Although marginalization and conditioning onto axis-aligned manifolds are well-established procedures, doing so onto non-axis-aligned manifolds is not as well understood. We demonstrate the utility of our expressions through three applications: 1) approximation of the projected normal distribution, where the quality of our linearized approximation increases as problem non-linearity decreases; 2) covariance extraction in Koopman SLAM, where our covariances are shown to be consistent on a real-world dataset; and 3) covariance extraction in constrained GTSAM, where our covariances are shown to be consistent in simulation.

Frequency-modulated continuous-wave (FMCW) scanning radar has emerged as an alternative to spinning LiDAR for state estimation on mobile robots. Radar's longer wavelength is less affected by small particulates, providing operational advantages in challenging environments such as dust, smoke, and fog. This paper presents Radar Teach and Repeat (RT&R): a full-stack radar system for long-term off-road robot autonomy. RT&R can drive routes reliably in off-road cluttered areas without any GPS. We benchmark the radar system's closed-loop path-tracking performance and compare it to its 3D LiDAR counterpart. 11.8 km of autonomous driving was completed without interventions using only radar and gyro for navigation. RT&R was evaluated on four different routes with progressively less structured scene geometry. RT&R achieved lateral path-tracking root mean squared errors (RMSE) of 5.6 cm, 7.5 cm, and 12.1 cm as the routes became more challenging. These RMSE values are less than half of the width of one tire (24 cm) on our robot testing platform. These same routes have worst-case errors of 21.7 cm, 24.0 cm, and 43.8 cm. We conclude that radar is a viable alternative to LiDAR for long-term autonomy in challenging off-road scenarios. The implementation of RT&R is open-source and available at: https://github.com/utiasASRL/vtr3.

This paper presents a novel method for assessing the resilience of the iterative closest point (ICP) algorithm via learning-based, worst-case attacks on lidar point clouds. For safety-critical applications such as autonomous navigation, ensuring the resilience of algorithms before deployments is crucial. The ICP algorithm is the standard for lidar-based localization, but its accuracy can be greatly affected by corrupted measurements from various sources, including occlusions, adverse weather, or mechanical sensor issues. Unfortunately, the complex and iterative nature of ICP makes assessing its resilience to corruption challenging. While there have been efforts to create challenging datasets and develop simulations to evaluate the resilience of ICP, our method focuses on finding the maximum possible ICP error that can arise from corrupted measurements at a location. We demonstrate that our perturbation-based adversarial attacks can be used pre-deployment to identify locations on a map where ICP is particularly vulnerable to corruptions in the measurements. With such information, autonomous robots can take safer paths when deployed, to mitigate against their measurements being corrupted. The proposed attack outperforms baselines more than 88% of the time across a wide range of scenarios.

For several tasks, ranging from manipulation to inspection, it is beneficial for robots to localize a target object in their surroundings. In this paper, we propose an approach that utilizes coarse point clouds obtained from miniaturized VL53L5CX Time-of-Flight (ToF) sensors (tiny LiDARs) to localize a target object in the robot’s workspace. We first conduct an experimental campaign to calibrate the dependency of sensor readings on relative range and orientation to targets. We then propose a probabilistic sensor model, which we validate in an object pose estimation task using a Particle Filter (PF). The results show that the proposed sensor model improves the performance of the localization of the target object with respect to two baselines: one that assumes measurements are free from uncertainty and one in which the confidence is provided by the sensor datasheet.

This paper presents Virtual Teach and Repeat (VirT&R): an extension of the Teach and Repeat (T&R) framework that enables GPS-denied, zero-shot autonomous ground vehicle navigation in untraversed environments. VirT&R leverages aerial imagery captured for a target environment to train a Neural Radiance Field (NeRF) model so that dense point clouds and photo-textured meshes can be extracted. The NeRF mesh is used to create a high-fidelity simulation of the environment for piloting an unmanned ground vehicle (UGV) to virtually define a desired path. The mission can then be executed in the actual target environment by using NeRF-generated point cloud submaps associated along the path and an existing LiDAR Teach and Repeat (LT&R) framework. We benchmark the repeatability of VirT&R on over 12 km of autonomous driving data using physical markings that allow a sim-to-real lateral path-tracking error to be obtained and compared with LT&R. VirT&R achieved measured root mean squared errors (RMSE) of 19.5 cm and 18.4 cm in two different environments, which are slightly less than one tire width (24 cm) on the robot used for testing, and respective maximum errors were 39.4 cm and 47.6 cm. This was done using only the NeRF-derived teach map, demonstrating that VirT&R has similar closed-loop path-tracking performance to LT&R but does not require a human to manually teach the path to the UGV in the actual environment.

This paper presents a sound source localization strategy that relies on a microphone array embedded in an unmanned ground vehicle and an asynchronous close-talking microphone near the operator. A signal coarse alignment strategy is combined with a time-domain acoustic echo cancellation algorithm to estimate a time-frequency ideal ratio mask to isolate the target speech from interferences and environmental noise. This allows selective sound source localization, and provides the robot with the direction of arrival of sound from the active operator, which enables rich interaction in noisy scenarios. Results demonstrate an average angle error of 4 degrees and an accuracy within 5 degrees of 95% at a signal-to-noise ratio of 1dB, which is significantly superior to the state-of-the-art localization methods.

In this work, we demonstrate continuous-time radar-inertial and lidar-inertial odometry using a Gaussian process motion prior. Using a sparse prior, we demonstrate improved computational complexity during preintegration and interpolation. We use a white-noise-on-acceleration motion prior and treat the gyroscope as a direct measurement of the state while preintegrating accelerometer measurements to form relative velocity factors. Our odometry is implemented using sliding-window batch trajectory estimation. To our knowledge, our work is the first to demonstrate radar-inertial odometry with a spinning mechanical radar using both gyroscope and accelerometer measurements. We improve the performance of our radar odometry by 43% by incorporating an inertial measurement unit. Our approach is efficient and we demonstrate real-time performance. Code for this article can be found at:https://github.com/utiasASRL/steam_icp.

A recent set of techniques in the robotics community, known ascertifiably correct methods, frames robotics problems aspolynomial optimization problems(POPs) and applies convex, semidefinite programming (SDP) relaxations to either find or certify their global optima. In parallel,differentiable optimizationallows optimization problems to be embedded into end-to-end learning frameworks and has received considerable attention in the robotics community. In this paper, we consider the ill effect of convergence to spurious local minima in the context of learning frameworks that use differentiable optimization. We present SDPRLayers, an approach that seeks to address this issue by combining convex relaxations with implicit differentiation techniques to providecertifiably correct solutions and gradientsthroughout the training process. We provide theoretical results that outline conditions for the correctness of these gradients and provide efficient means for their computation. Our approach is first applied to two simple-but-demonstrative simulated examples, which expose the potential pitfalls of reliance on local optimization in existing, state-of-the-art, differentiable optimization methods. We then apply our method in a real-world application: we train a deep neural network to detect image keypoints for robot localization in challenging lighting conditions. We provide our open-source, PyTorch implementation of SDPRLayers and our differentiable localization pipeline.

In contrast to conventional robots, accurately modeling the kinematics and statics of continuum robots is challenging due to partially unknown material properties, parasitic effects, or unknown forces acting on the continuous body. Consequentially, state estimation approaches that utilize additional sensor information to predict the shape of continuum robots have garnered significant interest. This article presents a novel approach to state estimation for systems with multiple coupled continuum robots, which allows estimating the shape and strain variables of multiple continuum robots in an arbitrary coupled topology. Simulations and experiments demonstrate the capabilities and versatility of the proposed method, while achieving accurate and continuous estimates for the state of such systems, resulting in average end-effector errors of 3.3 mm and 5.02$^\circ$depending on the sensor setup. It is further shown, that the approach offers fast computation times of below 10 ms, enabling its utilization in quasi-static real-time scenarios with average update rates of 100–200 Hz. An open-source C++ implementation of the proposed state estimation method is made publicly available to the community.

Accurate, efficient, and robust state estimation is more important than ever in robotics as the variety of platforms and complexity of tasks continue to grow. Historically, discrete-time filters and smoothers have been the dominant approach, in which the estimated variables are states at discrete sample times. The paradigm of continuous-time state estimation proposes an alternative strategy by estimating variables that express the state as a continuous function of time, which can be evaluated at any query time. Not only can this benefit downstream tasks such as planning and control, but it also significantly increases estimator performance and flexibility, as well as reduces sensor preprocessing and interfacing complexity. Despite this, continuous-time methods remain underutilized, potentially because they are less well-known within robotics. To remedy this, this work presents a unifying formulation of these methods and the most exhaustive literature review to date, systematically categorizing prior work by methodology, application, state variables, historical context, and theoretical contribution to the field. By surveying splines and Gaussian process together and contextualizing works from other research domains, this work identifies and analyzes open problems in continuous-time state estimation and suggests new research directions.

In the field of deep point cloud understanding, KP-Conv is a unique architecture that uses kernel points to locate convolutional weights in space, instead of relying on Multi-Layer Perceptron (MLP) encodings. While it initially achieved success, it has since been surpassed by recent MLP networks that employ updated designs and training strategies. Building upon the kernel point principle, we present two novel designs: KPConvD (depthwise KP-Conv), a lighter design that enables the use of deeper architectures, and KPConvX, an innovative design that scales the depthwise convolutional weights of KPConvD with kernel attention values. Using KPConvX with a modern architecture and training strategy, we are able to outperform current state-of-the-art approaches on the ScanObjectNN, Scannetv2, and S3DIS datasets. We validate our design choices through ablation studies and release our code and models.

In recent years, semidefinite relaxations of common optimization problems in robotics have attracted growing attention due to their ability to provide globally optimal solutions. In many cases, it was shown that specific handcrafted redundant constraints are required to obtain tight relaxations, and thus global optimality. These constraints are formulation-dependent and typically identified through a lengthy manual process. Instead, the present article suggests an automatic method to find a set of sufficient redundant constraints to obtain tightness, if they exist. We first propose an efficient feasibility check to determine if a given set of variables can lead to a tight formulation. Second, we show how to scale the method to problems of bigger size. At no point of the process do we have to find redundant constraints manually. We showcase the effectiveness of the approach, in simulation and on real datasets, for range-based localization and stereo-based pose estimation. We also reproduce semidefinite relaxations presented in recent literature and show that our automatic method always finds a smaller set of constraints sufficient for tightness than previously considered.

In this article, we present a framework for model-free batch localization and simultaneous localization and mapping (SLAM). We use lifting functions to map a control-affine system into a high-dimensional space, where both the process model and the measurement model are rendered bilinear. During training, we solve a least-squares problem using groundtruth data to compute the high-dimensional model matrices associated with the lifted system purely from data. At inference time, we solve for the unknown robot trajectory and landmarks through an optimization problem, where constraints are introduced to keep the solution on the manifold of the lifting functions. The problem is efficiently solved using a sequential quadratic program (SQP), where the complexity of an SQP iteration scales linearly with the number of timesteps. Our algorithms, called reduced constrained Koopman linearization localization (RCKL-Loc) and reduced constrained Koopman linearization SLAM (RCKL-SLAM), are validated experimentally in simulation and on two datasets: one with an indoor mobile robot equipped with a laser rangefinder that measures range to cylindrical landmarks, and one on a golf cart equipped with radio-frequency identification (RFID) range sensors. We compare RCKL-Loc and RCKL-SLAM with classic model-based nonlinear batch estimation. While RCKL-Loc and RCKL-SLAM have a similar performance compared to their model-based counterparts, they outperform the model-based approaches when the prior model is imperfect, showing the potential benefit of the proposed data-driven technique.

We survey the current state of millimeter-wave (mmWave) radar applications in robotics with a focus on unique capabilities, and discuss future opportunities based on the state of the art. Frequency modulated continuous wave mmWave radars operating in the 76–81 GHz range are an appealing alternative to lidars, cameras, and other sensors operating in the near-visual spectrum. Radar has been made more widely available in new packaging classes, more convenient for robotics and its longer wavelengths have the ability to bypass visual clutter, such as fog, dust, and smoke. We begin by covering radar principles as they relate to robotics. We then review the relevant new research across a broad spectrum of robotics applications beginning with motion estimation, localization, and mapping. We then cover object detection and classification, and then close with an analysis of current datasets and calibration techniques that provide entry points into radar research.

In recent years, there has been remarkable progress in the development of so-calledcertifiable perceptionmethods, which leverage semidefinite, convex relaxations to findglobal optimaof perception problems in robotics. However, many of these relaxations rely on simplifying assumptions that facilitate the problem formulation, such as anisotropicmeasurement noise distribution. In this article, we explore the tightness of the semidefinite relaxations ofmatrix-weighted(anisotropic) state-estimation problems and reveal the limitations lurking therein: matrix-weighted factors can cause convex relaxations to lose tightness. In particular, we show that the semidefinite relaxations of localization problems with matrix weights may be tight only for low noise levels. To better understand this issue, we introduce a theoretical connection between the posterior uncertainty of the state estimate and the certificate matrix obtained via convex relaxation. With this connection in mind, we empirically explore the factors that contribute to this loss of tightness and demonstrate thatredundant constraintscan be used to regain it. As a second technical contribution of this article, we show that the state-of-the-art relaxation of scalar-weighted simultaneous localization and mapping cannot be used when matrix weights are considered. We provide an alternate formulation and show that its semidefinite program relaxation is not tight (even for very low noise levels) unless specificredundant constraintsare used. We demonstrate the tightness of our formulations on both simulated and real-world data.

Accurate and robust vehicle localization in highly urbanized areas is challenging. Sensors are often corrupted in those complicated and large-scale environments. This article introducesgnssFGO, a global and online trajectory estimator that fuses global navigation satellite systems (GNSS) observations alongside multiple sensor measurements for robust vehicle localization. IngnssFGO, we fuse asynchronous sensor measurements into the graph with a continuous-time trajectory representation using Gaussian process (GP) regression. This enables querying states at arbitrary timestamps without strict state and measurement synchronization. Thus, the proposed method presents a generalized factor graph for multisensor fusion. To evaluate and study different GNSS fusion strategies, we fuse GNSS measurements in loose and tight coupling with a speed sensor, inertial measurement unit, and LiDAR-odometry. We employed datasets from measurement campaigns in Aachen, Düsseldorf, and Cologne and presented comprehensive discussions on sensor observations, smoother types, and hyperparameter tuning. Our results show that the proposed approach enables robust trajectory estimation in dense urban areas where a classic multisensor fusion method fails due to sensor degradation. In a test sequence containing a 17-km route through Aachen, the proposed method results in a mean 2-D positioning error 0.48 m while fusing raw GNSS observations with LiDAR odometry in a tight coupling

Autonomous surface vessels (ASV) represent a promising technology to automate water-quality monitoring of lakes. In this work, we use satellite images as a coarse map and plan sampling routes for the robot. However, inconsistency between the satellite images and the actual lake, as well as environmental disturbances such as wind, aquatic vegetation, and changing water levels can make it difficult for robots to visit places suggested by the prior map. This paper presents a robust route-planning algorithm that minimizes the expected total travel distance given these environmental disturbances, which induce uncertainties in the map. We verify the efficacy of our algorithm in simulations of over a thousand Canadian lakes and demonstrate an application of our algorithm in a 3.7 km-long real-world robot experiment on a lake in Northern Ontario, Canada.

In this paper, we present an algorithm for learning time-correlated measurement covariances for application in batch state estimation. We parameterize the inverse measurement covariance matrix to be block-banded, which conveniently factorizes and results in a computationally efficient approach for correlating measurements across the entire trajectory. We train our covariance model through supervised learning using the groundtruth trajectory. In applications where the measurements are time-correlated, we demonstrate improved performance in both the mean posterior estimate and the covariance (i.e., improved estimator consistency). We use an experimental dataset collected using a mobile robot equipped with a laser rangefinder to demonstrate the improvement in performance. We also verify estimator consistency in a controlled simulation using a statistical test over several trials.

Long-term visual localization is an essential problem in robotics and computer vision, but remains challenging due to the environmental appearance changes caused by lighting and seasons. While many existing works have attempted to solve it by directly learning invariant sparse keypoints and descriptors to match scenes, these approaches still struggle with adverse appearance changes. Recent developments in image transformations such as neural style transfer have emerged as an alternative to address such appearance gaps. In this work, we propose to combine an image transformation network and a feature-learning network to improve long-term localization performance. Given night-to-day image pairs, the image transformation network transforms the night images into day-like conditions prior to feature matching; the feature network learns to detect keypoint locations with their associated descriptor values, which can be passed to a classical pose estimator to compute the relative poses. We conducted various experiments to examine the effectiveness of combining style transfer and feature learning and its training strategy, showing that such a combination greatly improves long-term localization performance.

In this paper, we present a fast, lightweight odometry method that uses the Doppler velocity measurements from a Frequency-Modulated Continuous-Wave (FMCW) lidar without data association. FMCW lidar is a recently emerging technology that enables per-return relative radial velocity measurements via the Doppler effect. Since the Doppler measurement model is linear with respect to the 6-degrees-of-freedom (DOF) vehicle velocity, we can formulate a linear continuous-time estimation problem for the velocity and numerically integrate for the 6-DOF pose estimate afterward. The caveat is that angular velocity is not observable with a single FMCW lidar. We address this limitation by also incorporating the angular velocity measurements from a gyroscope. This results in an extremely efficient odometry method that processes lidar frames at an average wall-clock time of 5.64ms on a single thread, well below the 10Hz operating rate of the lidar we tested. We show experimental results on real-world driving sequences and compare against state-of-the-art Iterative Closest Point (ICP)-based odometry methods, presenting a compelling tradeoff between accuracy and computation. We also present an algebraic observability study, where we demonstrate in theory that the Doppler measurements from multiple FMCW lidars are capable of observing all 6 degrees of freedom (translational and angular velocity).

We present a method for generating, predicting, and using spatiotemporal occupancy grid maps (SOGM), which embed future semantic information of real dynamic scenes. We present an autolabeling process that creates SOGMs from noisy real navigation data. We use a 3-D–2-D feedforward architecture, trained to predict the future time steps of SOGMs, given 3-D Lidar frames as input. Our pipeline is entirely self-supervised, thus enabling lifelong learning for real robots. The network is composed of a 3-D back-end that extracts rich features and enables the semantic segmentation of the lidar frames, and a 2-D front-end that predicts the future information embedded in the SOGM representation, potentially capturing the complexities and uncertainties of real-world multiagent interactions. We also design a navigation system that uses these predicted SOGMs within planning, after they have been transformed into spatiotemporal risk maps. We verify our navigation system's abilities in simulation, validate it on a real robot, study SOGM predictions on real data in various circumstances, and provide a novel indoor 3-D lidar dataset, collected during our experiments, which includes our automated annotations.

We present a novel method for generating, predicting, and using Spatiotemporal Occupancy Grid Maps (SOGM), which embed future information of dynamic scenes. Our au-tomated generation process creates groundtruth SOGMs from previous navigation data. We build on prior work to annotate lidar points based on their dynamic properties, which are then projected on time-stamped 2D grids: SOGMs. We design a 3D-2D feedforward architecture, trained to predict the future time steps of SOGMs, given 3D lidar frames as input. Our pipeline is entirely self-supervised, thus enabling lifelong learning for robots. The network is composed of a 3D back-end that extracts rich features and enables the semantic segmentation of the lidar frames, and a 2D front-end that predicts the future information embedded in the SOGMs within planning. We also design a navigation pipeline that uses these predicted SOGMs. We provide both quantitative and qualitative insights into the predictions and validate our choices of network design with a comparison to the state of the art and ablation studies.

Accurate and reliable state estimation is becoming increasingly important as robots venture into the real world. Gaussian variational inference (GVI) is a promising alternative for nonlinear state estimation, which estimates a full probability density for the posterior instead of a point estimate as in maximum a posteriori (MAP)-based approaches. GVI works by optimizing for the parameters of a multivariate Gaussian (MVG) that best agree with the observed data. However, such an optimization procedure must ensure the parameter constraints of a MVG are satisfied; in particular, the inverse covariance matrix must be positive definite. In this work, we propose a tractable algorithm for performing state estimation using GVI that guarantees that the inverse covariance matrix remains positive definite and is well-conditioned throughout the optimization procedure. We evaluate our method extensively in both simulation and real-world experiments for range-only localization. Our results show GVI is consistent on this problem, while MAP is over-confident.

Occupancy grids have been widely used for mapping with mobile robots for several decades. Occupancy grids discretize the analog environment and seek to determine the occupancy probability of each cell. More recent occupancy grid mapping algorithms have shown the advantage of capturing cell correlations in the measurement model and the posterior. By estimating the probability of a given map as opposed to a cell, these algorithms have been able to better capture the occupancy probability of cells in the map. The advantage of incorporating data-driven prior probabilities in occupancy grid mapping is explored. A form of Markov Chain Monte Carlo (MCMC) known as Gibbs sampling allows us to sample maps from the full posterior. Previous research has sampled the occupancy probability of each cell, but this paper extends that work to sample a larger patch of cells and highlights the benefit of obtaining the prior for each patch from real maps.

We present a self-supervised learning approach for the semantic segmentation of lidar frames. Our method is used to train a deep point cloud segmentation architecture without any human annotation. The annotation process is automated with the combination of simultaneous localization and mapping (SLAM) and ray-tracing algorithms. By performing multiple navigation sessions in the same environment, we are able to identify permanent structures, such as walls, and disentangle short-term and long-term movable objects, such as people and tables, respectively. New sessions can then be performed using a network trained to predict these semantic labels. We demonstrate the ability of our approach to improve itself over time, from one session to the next. With semantically filtered point clouds, our robot can navigate through more complex scenarios, which, when added to the training pool, help to improve our network predictions. We provide insights into our network predictions and show that our approach can also improve the performances of common localization techniques.

Vision-based path following allows robots to autonomously repeat manually taught paths. Stereo Visual Teach and Repeat (VT&R) [1] accomplishes accurate and robust long-range path following in unstructured outdoor environments across changing lighting, weather, and seasons by relying on colour-constant imaging [2] and multi-experience localization [3]. We leverage multi-experience VT&R together with two datasets of outdoor driving on two separate paths spanning different times of day, weather, and seasons to teach a deep neural network to predict relative pose for visual odometry (VO) and for localization with respect to a path. In this paper we run experiments exclusively on datasets to study how the network generalizes across environmental conditions. Based on the results we believe that our system achieves relative pose estimates sufficiently accurate for in-the-loop path following and that it is able to localize radically different conditions against each other directly (i.e. winter to spring and day to night), a capability that our hand-engineered system does not have.

We estimate the global pose of a multirotor UAV by visually localizing images captured during a flight with Google Earth images pre-rendered from known poses. We metrically localize real images with georeferenced rendered images using a dense mutual information technique to allow accurate global pose estimation in outdoor GPS-denied environments. We show the ability to consistently localize throughout a sunny summer day despite major lighting changes while demonstrating that a typical feature-based localizer struggles under the same conditions. Successful image registrations are used as measurements in a filtering framework to apply corrections to the pose estimated by a gimballed visual odometry pipeline. We achieve less than 1 m and 1° RMSE on a 303 m flight and less than 3 m and 3° RMSE on six 1132 m flights as low as 36 m above ground level conducted at different times of the day from sunrise to sunset.

The stochastic Canadian Traveler Problem (CTP), which finds application in robot route selection under uncertainty, aims to find the traversal policy with the minimum expected cost. This paper extends the CTP to what we call the Robust Canadian Traveler Problem (RCTP), in which the variability of the policy cost is also part of the evaluation criteria. An optimal (offline) algorithm and an approximate (online) algorithm are then proposed to compute the policy that has a good balance of both mean and variation of the traversal cost. The benefit of the proposed framework versus traditional approaches is shown by doing simulations in randomly generated worlds as well as on a map of 5 km of paths built from robot field trials. Specifically, the RCTP framework is able to search for sub-optimal policy alternatives with significantly lower worst-case cost and less computational time compared to the optimal policy, but with little sacrifice on the expected cost.

Operating in rough, unstructured terrain is an essential requirement for any truly field-deployable ground robot. Search-and-rescue, border patrol and agricultural work all require operation in environments with little established infrastructure for easy navigation. This presents challenges for sensor-based navigation such as vision, where erratic motion and feature-poor environments test feature tracking and hinder the performance of repeat matching of point features. For vision-based route-following methods such as Visual Teach and Repeat (VT&R), maintaining similar visual perspective of salient point features is critical for reliable odometry and accurate localisation over long periods. In this paper, we investigate a potential solution to these challenges by integrating a gimbaled camera with VT&R on a Grizzly Robotic Utility Vehicle (RUV) for testing at high speeds and in visually challenging environments. We examine the benefits and drawbacks of using an actively gimbaled camera to attenuate image motion and control viewpoint. We compare the use of a gimbaled camera to our traditional fixed stereo configuration and demonstrate cases of improved performance in Visual Odometry (VO), localisation and path following in several sets of outdoor experiments.

Vision-based navigation is extremely susceptible to natural scene changes. This can result in localization failures in less than a few hours after map creation. To combat short-term illumination changes as well as long-term seasonal variations, we propose using a place-and-time-dependent binary descriptor that adapts to different scenarios in an online fashion. This is achieved by extending the GRIEF [6] evolution algorithm in two ways: correspondence generation using a known pose change and the inclusion of LATCH triplets in addition to BRIEF comparisons for descriptor generation. We show the adaptive descriptor outperforms a single descriptor scheme for localization within a single-experience Visual Teach and Repeat (VT&R) system while maintaining the efficiency of binary descriptors. By adapting the description function to different environmental conditions, it allows the system to operate for a longer period before a new experience is required. In the presence of extreme illumination changes from day to night, we obtain 40% more inlier matches compared to SURF. In the case of seasonal variations, a 70% increase is demonstrated. The increased correspondences result in more localizable sections along the paths, amounting to a 25% and 150% increase in the lighting and seasonal cases, respectively.

Anytime almost-surely asymptotically optimal planners, such as RRT*, incrementally find paths to every state in the search domain. This is inefficient once an initial solution is found as then only states that can provide a better solution need to be considered. Exact knowledge of these states requires solving the problem but can be approximated with heuristics. This paper formally defines these sets of states and demonstrates how they can be used to analyze arbitrary planning problems. It uses the well-known $L^2$ norm (i.e., Euclidean distance) to analyze minimum-path-length problems and shows that existing approaches decrease in effectiveness factorially (i.e., faster than exponentially) with state dimension. It presents a method to address this curse of dimensionality by directly sampling the prolate hyperspheroids (i.e., symmetric $n$-dimensional ellipses) that define the $L^2$ informed set. The importance of this direct informed sampling technique is demonstrated with Informed RRT*. This extension of RRT* has less theoretical dependence on state dimension and problem size than existing techniques and allows for linear convergence on some problems. It is shown experimentally to find better solutions faster than existing techniques on both abstract planning problems and HERB, a two-arm manipulation robot.

Our work builds upon Visual Teach & Repeat 2 (VT&R2): a vision-in-the-loop autonomous navigation system that enables the rapid construction of route networks, safely built through operator-controlled driving. Added routes can be followed autonomously using visual localization. To enable long-term operation that is robust to appearance change, its Multi-Experience Localization (MEL) leverages many previously driven experiences when localizing to the manually taught network. While this multi-experience method is effective across appearance change, the computation becomes intractable as the number of experiences grows into the tens and hundreds. This paper introduces an algorithm that prioritizes experiences most relevant to live operation, limiting the number of experiences required for localization. The proposed algorithm uses a visual Bag-of-Words description of the live view to select relevant experiences based on what the vehicle is seeing right now, without having to factor in all possible environmental influences on scene appearance. This system runs in the loop, in real time, does not require bootstrapping, can be applied to any pointfeature MEL paradigm, and eliminates the need for visual training using an online, local visual vocabulary. By picking a subset of visually similar experiences to the live view, we demonstrate safe, vision-in-the-loop route following over a 31 hour period, despite appearance as different as night and day.

This paper describes visual route following for a cliff-climbing, tethered mobile robot for the purpose of autonomously traversing extreme terrain in the presence of obstacles. When the robot's tether contacts an obstacle, an intermediate anchor is formed. In order to detach from intermediate anchors and avoid entanglement, the robot must backtrack along its outgoing trajectory. We use the Visual Teach & Repeat (VT&R) algorithm to autonomously repeat a manually taught path. However, our problem is complicated by the fact that the robot's tether must (i) remain taut regardless of inclination, (ii) allow the robot to drive freely, and (iii) provide motion assistance when wheel traction is reduced on steep slopes. To enable visual route following over varied terrain, we have developed a novel tether controller that selects a safe, steady-state tension based on the robot's inclination while also accounting for vehicle motion. Experiments are performed on our Tethered Robotic Explorer (TReX), which autonomously repeats paths while tethered in both flat-indoor and steep-outdoor environments in the presence of obstacles.

Assessing terrain ahead of a robot when repeating previously driven safe paths can be accomplished by looking for geometric changes (e.g., due to the appearance of humans or other obstacles). Previous work has shown that the incorporation of data-driven learning and place-dependence are useful aspects of making terrain classification viable in challenging terrain. This paper presents a learning, place-dependent (LPD) terrain classifier that uses a probabilistic model of the terrain to improve detection of small obstacles in uncluttered terrain while avoiding false positives in more challenging environments. Specifically, a Gaussian mixture model is used to account for multi-height terrain cells that arise in heavily vegetated areas (where both a ground plane and overhanging vegetation can occupy the same cell). A variational Bayesian technique is used to automatically determine the number of components required for each cell using a Dirichlet prior on mixing proportions and a Normal-Inverse-Wishart prior on the means and covariances of the components. The probabilistic nature of the model allows for the detection of much smaller obstacles in regions that exhibit low variance in the terrain surface, whilst still avoiding false positives in regions where the terrain is highly cluttered (e.g., vegetation). The algorithm is tested on almost 10 km of autonomous traverse and is shown to be able to classify a wider range of obstacles than two baseline change-detection algorithms based on absolute geometric differences.

Sampling-based optimal planners, such as RRT*, almost-surely converge asymptotically to the optimal solution, but have provably slow convergence rates in high dimensions. This is because their commitment to finding the global optimum compels them to prioritize exploration of the entire problem domain even as its size grows exponentially. Optimization techniques, such as CHOMP, have fast convergence on these problems but only to local optima. This is because they are exploitative, prioritizing the immediate improvement of a path even though this may not find the global optimum of nonconvex cost functions.

Mobile robots supported by an electromechanical tether can safely explore extremely rugged terrain in resource-limited environments. While a tether provides power, wired communication, and support on steep surfaces, it also reduces maneuverability; in cluttered environments the tether will contact obstacles, forming intermediate anchor points. In order for the robot to avoid tether entanglement, it must localize itself with respect to any added anchor points. Accordingly, we present a first approach towards nonvisual localization and mapping that utilizes tether measurements and wheel odometry to jointly estimate vehicle trajectory and tether-to-obstacle contact points. The proposed method is inspired by FastSLAM, where instead of updating a map of landmarks, tether length and bearing measurements are used to update sequential lists of anchor points for every particle representing a belief of the robot's trajectory. Results from both simulation and experiment using our Tethered Robotic eXplorer (TReX) demonstrate that (i) our method is more accurate than odometry alone, and (ii) we are able to map intermediate anchor points nonvisually.

This paper presents a learned, place-dependent terrain-assessment classifier that improves over time. Whereas typical methods aim to assess all of the terrain in a given environment, we exploit the fact that many robotic navigation tasks are well-suited to visual-teach-and-repeat navigation where robot motion is restricted to previously driven paths. In such scenarios, we argue that general terrain assessment is not required, and we can instead solve the much easier problem of detecting changes along the path; we sacrifice the ability to generalize off the path in favour of improved performance on the path. Terrain along a pretaught path is compared to terrain seen at the same location during previous, human-supervised traverses, and any significant differences cause that location to be labelled as unsafe. By storing all of our previous experiences, we are able to continuously improve our estimates as we revisit the same locations multiple times. We tested our method on two datasets collected at the University of Toronto and show that we improve over existing place-independent (both learned and not) methods, enabling nearly full autonomy in challenging, varied terrain using only a stereo camera.

Vision-based, route-following algorithms enable autonomous robots to repeat manually taught paths over long distances using inexpensive vision sensors. However, these methods struggle with long-term, outdoor operation due to the challenges of environmental appearance change caused by lighting, weather, and seasons. While techniques exist to address appearance change by using multiple experiences over different environmental conditions, they either provide topological-only localization, require several manually taught experiences in different conditions, or require extensive offline mapping to produce metric localization. For real-world use, we would like to localize metrically to a single manually taught route and gather additional visual experiences during autonomous operations. Accordingly, we propose a novel multi-experience localization (MEL) algorithm developed specifically for route-following applications; it provides continuous, six-degree-of-freedom (6DoF) localization with relative uncertainty to a privileged (manually taught) path using several experiences simultaneously. We validate our algorithm through two experiments: i) an offline performance analysis on a 9km subset of a challenging 27km route-traversal dataset and ii) an online field trial where we demonstrate autonomy on a small 250m loop over the course of a sunny day. Both exhibit significant appearance change due to lighting variation. Through these experiments we show that safe localization can be achieved by bridging the appearance gap.

This paper presents an approach to learning robot terrain assessment from human demonstration. An operator drives a robot for a short period of time, supervising the gathering of traversable and untraversable terrain data. After this initial training period, the robot can then predict the traversability of new terrain based on its experiences. We improve on current methods in two ways: first, we maintain a richer (higher-dimensional) representation of the terrain that is better able to distinguish between different training examples. Second, we use a Gaussian-process classifier for terrain assessment due to its superior introspective abilities (leading to better uncertainty estimates) when compared to other classifier methods in the literature. Our method is tested on real data and shown to outperform current methods both in classification accuracy and uncertainty estimation.

In this paper, we present Batch Informed Trees (BIT*), a planning algorithm based on unifying graph- and sampling-based planning techniques. By recognizing that a set of samples describes an implicit random geometric graph (RGG), we are able to combine the efficient ordered nature of graph-based techniques, such as A*, with the anytime scalability of sampling-based algorithms, such as Rapidly-exploring Random Trees (RRT).

Robust control maintains stability and performance for a fixed amount of model uncertainty but can be conservative since the model is not updated online. Learning-based control, on the other hand, uses data to improve the model over time but is not typically guaranteed to be robust throughout the process. This paper proposes a novel combination of both ideas: a robust Min-Max Learning-Based Nonlinear Model Predictive Control (MM-LB-NMPC) algorithm. Based on an existing LB-NMPC algorithm, we present an efficient and robust extension, altering the NMPC performance objective to optimize for the worst-case scenario. The algorithm uses a simple a priori vehicle model and a learned disturbance model. Disturbances are modelled as a Gaussian Process (GP) based on experience collected during previous trials as a function of system state, input, and other relevant variables. Nominal state sequences are predicted using an Unscented Transform and worst-case scenarios are defined as sequences bounding the 3σ confidence region. Localization for the controller is provided by an on-board, vision-based mapping and navigation system enabling operation in large-scale, GPS-denied environments. The paper presents experimental results from testing on a 50 kg skid-steered robot executing a path-tracking task. The results show reductions in maximum lateral and heading path-tracking errors by up to 30% and a clear transition from robust control when the model uncertainty is high to optimal control when model uncertainty is reduced.

Stereo Visual Teach & Repeat (VT&R) is a system for long-range, autonomous route following in unstructured 3D environments. As this system relies on a passive sensor to localize, it is highly susceptible to changes in lighting conditions. Recent work in the optics community has provided a method to transform images collected from a three-channel passive sensor into color-constant images that are resistant to changes in outdoor lighting conditions. This paper presents a lighting-resistant VT&R system that uses experimentally trained color-constant images to autonomously navigate difficult outdoor terrain despite changes in lighting. We show through an extensive field trial that our algorithm is capable of autonomously following a 1km outdoor route spanning sandy/rocky terrain, grassland, and wooded areas. Using a single visual map created at midday, the route was autonomously repeated 26 times over a period of four days, from sunrise to sunset with an autonomy rate (by distance) of over 99.9%. These experiments show that a simple image transformation can extend the operation of VT&R from a few hours to multiple days.

This paper shows how to carry out batch continuous-time trajectory estimation for bodies translating and rotating in three-dimensional (3D) space, using a very efficient form of Gaussian-process (GP) regression. The method is fast, singularity-free, uses a physically motivated prior (the mean is constant body-centric velocity), and permits trajectory queries at arbitrary times through GP interpolation. Landmark estimation can be folded in to allow for simultaneous trajectory estimation and mapping (STEAM), a variant of SLAM.

This paper proposes using hierarchical wavelets as a basis in parametric continuous-time batch estimation. The need for a continuous-time robot pose in the simultaneous localization and mapping (SLAM) problem has arisen as state-of-the-art batch SLAM algorithms attempt to handle more challenging hardware; specifically, the continuous-time framework is particularly beneficial when using high-rate sensors, multiple unsynchronized sensors, or scanning sensors, such as lidar and rolling-shutter cameras, during motion. Although the traditional discrete-time SLAM formulation can be adapted by using temporal pose interpolation, approaches using the continuous-time framework are able to generate smooth robot trajectories with less state variables. In this paper, we focus on the parametric approach using temporal basis functions to develop a finite-element representation of the continuous-time robot trajectory. While the majority of current implementations have utilized a uniformly spaced B-spline basis, we note that trajectory richness is often quite variable; in this paper, we show how a hierarchical system of wavelet basis functions can be used to increase the resolution of the solution only in the temporally local regions of the trajectory that require additional detail. We validate our approach by contrasting uniform B-splines and wavelets in a six-dimensional pose-graph SLAM experiment, using both simulated and real data.

Occupancy grids have been a popular mapping technique in mobile robotics for nearly 30 years. Occupancy grids offer a discrete representation of the world and seek to determine the occupancy probability of each cell. Traditional occupancy grid mapping methods make two assumptions for computational efficiency and it has been shown that the full posterior is computationally intractable for real-world mapping applications without these assumptions. The two assumptions result in tuning parameters that control the information gained from each distance measurement. In this paper, several tuning parameters found in the literature are optimized against the full posterior in 1D. In addition, this paper presents a new parameterization of the update function that outperforms existing methods in terms of capturing residual uncertainty. Capturing the residual uncertainty better estimates the position of obstacles and prevents under- and over-confidence in both the occupied and unoccupied cells. The paper concludes by showing that the new update function better captures the residual uncertainty in each cell when compared to an offline mapping method for realistic 2D simulations.

This paper presents a Learning-based Nonlinear Model Predictive Control (LB-NMPC) algorithm for an autonomous mobile robot to reduce path-tracking errors over repeated traverses along a reference path. The LB-NMPC algorithm uses a simple a priori vehicle model and a learned disturbance model. Disturbances are modelled as a Gaussian Process (GP) based on experience collected during previous traversals as a function of system state, input and other relevant variables. Modelling the disturbance as a GP enables interpolation and extrapolation of learned disturbances, a key feature of this algorithm. Localization for the controller is provided by an on-board, vision-based mapping and navigation system enabling operation in large-scale, GPS-denied environments. The paper presents experimental results including over 1.8 km of travel by a four-wheeled, 50 kg robot travelling through challenging terrain (including steep, uneven hills) and by a six-wheeled, 160 kg robot learning disturbances caused by unmodelled dynamics at speeds ranging from 0.35 m/s to 1.0 m/s. The speed is scheduled to balance trial time, path-tracking errors, and localization reliability based on previous experience. The results show that the system can start from a generic a priori vehicle model and subsequently learn to reduce vehicle- and trajectory-specific path-tracking errors based on experience.

Rapidly-exploring random trees (RRTs) are popular in motion planning because they find solutions efficiently to single-query problems. Optimal RRTs (RRT*s) extend RRTs to the problem of finding the optimal solution, but in doing so asymptotically find the optimal path from the initial state to every state in the planning domain. This behaviour is not only inefficient but also inconsistent with their single-query nature.

In this paper, we provide specific and practical approaches to associate uncertainty with 4 ×4 transformation matrices, which is a common representation for pose variables in 3-D space. We show constraint-sensitive means of perturbing transformation matrices using their associated exponential-map generators and demonstrate these tools on three simple-yet-important estimation problems: 1) propagating uncertainty through a compound pose change, 2) fusing multiple measurements of a pose (e.g., for use in pose-graph relaxation), and 3) propagating uncertainty on poses (and landmarks) through a nonlinear camera model. The contribution of the paper is the presentation of the theoretical tools, which can be applied in the analysis of many problems involving 3-D pose and point variables.

Appearance-based batch nonlinear optimization techniques for simultaneous localization and mapping (SLAM) have been highly successful in assisting robot motion estimation. Traditionally, these techniques are applied in a single privileged coordinate frame, which can become computationally expensive over long distances, particularly when a loop closure requires the adjustment of many pose variables. Recent approaches to the problem have shown that a completely relative coordinate framework can be used to incrementally find a close approximation of the full maximum likelihood solution in constant time. However, due to the nature of these discrete-time techniques, the state size becomes intractable when challenged with high-rate sensors. We propose moving the relative coordinate formulation of SLAM into continuous time by estimating the velocity profile of the robot. We derive the relative formulation of the continuous-time robot trajectory and formulate an estimator for the SLAM problem using temporal basis functions. Although we do not yet take advantage of large-scale loop closures, we intentionally use a relative formulation to set the stage for future work that will close loops in constant time. We show how the estimator can be used in a window-style filter to incrementally find the batch solution in constant time. The estimator is validated on a set of appearance-based feature measurements acquired using a two-axis scanning laser rangefinder over a 1.1km trajectory.

Accurate pose estimation relies on high-quality sensor measurements. Due to manufacturing tolerance, every sensor (camera or lidar) needs to be individually calibrated. Feature-based techniques using simple calibration targets (e.g., a checkerboard pattern) have become the dominant approach to camera sensor calibration. Existing lidar calibration methods require a controlled environment (e.g., a space of known dimension) or specific configurations of supporting hardware (e.g., coupled with GPS/IMU). Leveraging recent state estimation developments based on lidar intensity imagery, this paper presents a calibration procedure for a two-axis scanning lidar using only an inexpensive checkerboard calibration target. In addition, the proposed method generalizes a two-axis scanning lidar as an idealized spherical camera with additive measurement distortions. Conceptually, this is not unlike normal camera calibration in which an arbitrary camera is modelled as an idealized projective (pinhole) camera with tangential and radial distortions. The resulting calibration method, we believe, can be readily applied to a variety of two-axis scanning lidars. We present the measurement improvement quantitatively, as well as the impact of calibration on a 1.1-km visual odometry estimate.

Occupancy grids have been widely used for mapping with mobile robots for nearly 30 years. Occupancy grids discretize the analog environment and seek to determine the occupancy probability of each cell. Traditional occupancy grid mapping methods make two assumptions for computational efficiency and it has been shown that the full posterior is computationally intractable without these assumptions. This paper employs a form of Markov Chain Monte Carlo (MCMC) known as Gibbs sampling to sample from the full posterior. By drawing many samples, we are able to capture the full posterior, which more accurately represents the uncertainty in the map due to sensor measurement error. The MCMC method is shown to compute the full posterior in a 1D toy example, and it is shown to be computationally tractable, though not online, for realistic 2D simulations.

In this paper, we present a method for obtaining Visual Odometry (VO) estimates using a scanning laser rangefinder. Though common VO implementations utilize stereo camera imagery, cameras are dependent on ambient light. In contrast, actively-illuminated sensors such as laser rangefinders work in a variety of lighting conditions, including full darkness. We leverage previous successes by applying sparse appearance-based methods to laser intensity images, and address the issue of motion distortion by considering the estimation problem in continuous time. This is facilitated by Gaussian Process Gauss-Newton (GPGN), an algorithm for non-parametric, continuous-time, nonlinear, batch state estimation. We include a concise derivation of GPGN, along with details on the extension to three-dimensions (3D). Validation of the 3D laser-based VO framework is provided using 1.1km of experimental data, which was gathered by a field robot equipped with a two-axis scanning lidar.

Visual odometry (VO) is a highly efficient and powerful 6D motion estimation technique; state-of-the-art bundle adjustment algorithms now optimize over several frames of temporally tracked, appearance-based features in real time. It is well known that the temporal feature correspondence process is highly prone to mismatches. The standard technique used for outlier rejection in this process is random sample consensus (RANSAC), which is an iterative and non-deterministic process used to find the parameters of a mathematical model that best describe a likely set of inliers. The traditional model used for RANSAC in the visual odometry pipeline is a rigid transformation between two camera poses; this model has long assumed the use of an imaging sensor with a global shutter. In order to use imaging sensors that do not operate with a global shutter, it is proposed that the RANSAC algorithm be modified to use a constant-camera-velocity model. Specifically, this paper investigates the use of a two-axis scanning lidar in the visual-odometry pipeline. Images are formed using lidar intensity data, and due to the scanning-while-moving nature of the lidar, the behaviour of the sensor resembles that of a slow rolling-shutter camera. We formulate a Motion-Compensated RANSAC algorithm that uses a constant-velocity model and the individual timestamp of each extracted feature. The algorithm is validated using 6880 lidar frames with a resolution of 480 × 360, captured at 2 Hz, over a 1.1 km traversal. Our results show that the new algorithm results in far more inlying feature tracks for rolling-shutter-type images and ultimately higher-accuracy VO results.

This paper presents a path-repeating, mobile robot controller that combines a feedforward, proportional Iterative Learning Control (ILC) algorithm with a feedback-linearized path-tracking controller to reduce path-tracking errors over repeated traverses along a reference path. Localization for the controller is provided by an on-board, vision-based mapping and navigation system enabling operation in large-scale, GPS-denied, extreme environments. The paper presents experimental results including over 600 m of travel by a four-wheeled, 50 kg robot travelling through challenging terrain including steep hills and sandy turns and by a six-wheeled, 160 kg robot at gradually-increased speeds up to three times faster than the nominal, safe speed. In the absence of a global localization system, ILC is demonstrated to reduce path-tracking errors caused by unmodelled robot dynamics and terrain challenges.

Roboticists often formulate estimation problems in discrete time for the practical reason of keeping the state size tractable. However, the discrete-time approach does not scale well for use with high-rate sensors, such as inertial measurement units or sweeping laser imaging sensors. The difficulty lies in the fact that a pose variable is typically included for every time at which a measurement is acquired, rendering the dimension of the state impractically large for large numbers of measurements. This issue is exacerbated for the simultaneous localization and mapping (SLAM) problem, which further augments the state to include landmark variables. To address this tractability issue, we propose to move the full maximum likelihood estimation (MLE) problem into continuous time and use temporal basis functions to keep the state size manageable. We present a full probabilistic derivation of the continuous-time estimation problem, derive an estimator based on the assumption that the densities and processes involved are Gaussian, and show how coefficients of a relatively small number of basis functions can form the state to be estimated, making the solution efficient. Our derivation is presented in steps of increasingly specific assumptions, opening the door to the development of other novel continuous-time estimation algorithms through the application of different assumptions at any point. We use the SLAM problem as our motivation throughout the paper, although the approach is not specific to this application. Results from a self-calibration experiment involving a camera and a high-rate inertial measurement unit are provided to validate the approach.

Visual Teach and Repeat (VT&R) has proven to be an effective method to allow a vehicle to autonomously repeat any previously driven route without the need for a global positioning system. One of the major challenges for a method that relies on visual input to recognize previously visited places is lighting change, as this can make the appearance of a scene look drastically different. For this reason, passive sensors, such as cameras, are not ideal for outdoor environments with inconsistent/inadequate light. However, camera-based systems have been very successful for localization and mapping in outdoor, unstructured terrain, which can be largely attributed to the use of sparse, appearance-based computer vision techniques. Thus, in an effort to achieve lighting invariance and to continue to exploit the heritage of the appearance-based vision techniques traditionally used with cameras, this paper presents the first VT&R system that uses appearance-based techniques with laser scanners for motion estimation. The system has been field tested in a planetary analogue environment for an entire diurnal cycle, covering more than 11km with an autonomy rate of 99.7% of the distance traveled.

Mobile robots have been using two-dimensional discrete occupancy grid maps for more than 25 years to represent a continuous environment. Occupancy grids discretize the environment into a grid of cells and seek to determine the occupancy of each cell. These maps are generally used to determine the degree to which each cell is occupied.

This paper presents a simultaneous localization and mapping (SLAM) algorithm that allows a recursive state estimation process to be both distributed and decentralized in a sparse robot network that is never guaranteed to be fully connected (communication-wise). In such a sparse network, a robot may not always have the latest odometry and measurements from other robots. Our approach allows robots to obtain a temporary (localization and map) estimate at the current timestep using information available locally, but we also ensure that the centralized-equivalent estimate can always be recovered by all robots at a later time; we do not require a robot to keep track of what other robots know when it applies the Markov property to discard past information. Our method is validated through a hardware SLAM experiment where we distribute data association hypotheses amongst a team of robots. Estimate errors are shown to validate the performance of our approach. We also discuss the trade-offs and show comparisons between our distributed approach versus a non-distributed one.

Cameras have emerged as the dominant sensor modality for localization and mapping in three-dimensional, unstructured terrain, largely due to the success of sparse, appearance-based techniques, such as visual odometry. However, the Achilles' heel for all camera-based systems is their dependence on consistent ambient lighting, which poses a serious problem in outdoor environments that lack adequate or consistent light, such as the Moon. Actively illuminated sensors on the other hand, such as a light detection and ranging (lidar) device, use their own light source to illuminate the scene, making them a favourable alternative in light-denied environments. The purpose of this paper is to demonstrate that the largely successful appearance-based methods traditionally used with cameras can be applied to laser-based sensors, such as a lidar. We present two experiments that are vital to understanding and enabling appearance-based methods for lidar sensors. In the first experiment, we explore the stability of a representative keypoint detection and description algorithm on both camera images and lidar intensity images collected over a 24 hour period. In the second experiment, we validate our approach by implementing visual odometry based on sparse bundle adjustment on a sequence of lidar intensity images.

In this paper, the problem of outliers in a batch alignment problem (given heterogeneous measurements and sparse features) is considered. The conventional approach from the field of computer vision, pairwise RANSAC, is shown to be inappropriate for this scenario, which motivates the need for a new method. To address this problem, the heterogeneous measurements are compared in a common currency using their respective scaled measurement innovations. Furthermore, a family of three algorithms for classifying outliers given a hypothesis model are presented, each having its own balance between speed and accuracy. These classification criteria are then incorporated through iterative reclassification in a batch alignment framework, providing a robust estimate for localization and mapping. Lastly, statistical validation is obtained through a large set of simulated trials.

In this paper, we present an infrastructure-based ground-truth localization system suitable for deployment in large worksite environments. In particular, the system is low cost, simple-to-deploy, and is able to provide full six-degree of-freedom relative localization for three-dimensional laser scanners with centimetre-level accuracy in translation, and half degree accuracy in orientation. This system utilizes common laser scanner hardware, and exploits the fact that retroreflective material is easily identified based on the return intensity. This enables the use of simple rectangular signs placed around the scene as landmarks. An uncertainty model is presented that accounts for the shape of the landmarks, and a batch alignment algorithm is formulated that efficiently considers the structure of the problem. Lastly, characterization of the accuracy of the system is provided through small-scale testing in an indoor lab, and examples for a large-scale setup.

In this paper, we present a robust framework suitable for conducting three-dimensional Simultaneous Localization and Mapping (3D SLAM) in a planetary worksite environment. By utilizing a laser rangefinder mounted on a rover platform, we have demonstrated an approach that is able to create globally consistent maps of natural, unstructured 3D terrain. The framework presented in this paper utilizes a sparse-feature-based approach, and conducts data association using a hybrid combination of feature constellations and dense data. To maintain global consistency, the measurements are resolved using a batch alignment algorithm, which is reinforced with batch outlier rejection to improve its robustness. Finally, a map is created from the alignment estimates and the dense data. Validation is provided using data gathered at two different planetary analogue facilities.

Current rover localization techniques such as visual odometry have proven to be very effective on short to medium-length traverses (e.g., up to a few kilometres). This paper deals with the problem of long-range rover localization (e.g., 10km and up). An autonomous method to globally localize a rover is proposed by matching features detected from a 3D orbital elevation map and rover-based 3D lidar scans. The accuracy and efficiency of the algorithm is enhanced with visual odometry, and inclinometer/sun-sensor orientation measurements. The methodology was tested with real data, including 37 lidar scans of terrain from a Mars-Moon analogue site on Devon Island, Nunavut. When a scan contained a sufficient number of good topographic features, localization produced position errors of no more than 100m, and as low as a few metres in many cases. On a 10km traverse, the developed algorithm's localization estimates were shown to significantly outperform visual odometry estimates. It is believed that this architecture could be used to accurately and autonomously localize a rover on long-range traverses.

This paper describes the design and testing of a technique to enable long-range autonomous navigation using a stereo camera as the only sensor. During a learning phase, the rover is piloted along a route capturing stereo images. The images are processed into a manifold map of topologically-connected submaps that may be used for localization during an autonomous repeat traverse. Path following in non-planar terrain is handled by moving from localization in three dimensions, to path following in two dimensions using a local ground plane associated with each submap. The use of small submaps decouples the computational complexity of route repeating from the length of the path. We validate the algorithm by demonstrating its performance on a difficult three-dimensional route. Using this technique, a rover may autonomously traverse a multi-kilometer route in unstructured, three-dimensional terrain, without an accurate global reconstruction.

Visual teach-and-repeat navigation enables long-range rover autonomy without solving the simultaneous localization and mapping problem or requiring an accurate global reconstruction. During a learning phase, the rover is piloted along a route, logging images. After post-processing, the rover is able to repeat the route in either direction any number of times. This paper describes and evaluates the localization algorithm at the core of a teach-and-repeat system that has been tested on over 32 kilometers of autonomous driving in an urban environment and at a planetary analog site in the High Arctic. We show how a stereo visual odometry pipeline can be extended to become a mapping and localization system, then evaluate the performance of the algorithm with respect to accuracy, robustness to path-tracking error, and the effects of lighting.

Global localization of a planetary-exploration rover in the absence of a satellite-based global positioning system (GPS) is still an open problem. Although a satellite network is not available for localization around any near-term exploration targets, topographic maps derived from satellite imagery are available. This has spurred the development of several algorithms that perform global localization by matching data collected from onboard sensors to a global digital elevation map (DEM). This paper reviews two of these algorithms-Multiple-frame Odometry-compensated Global Alignment (MOGA) and VIsual Position Estimation for Rovers (VIPER)-and compares their performance on a common dataset, collected in a planetary analog environment. The comparison demonstrates the common factors limiting the performance of these algorithms, but also highlights the benefits and drawbacks of each method. Overall, the MOGA algorithm performed significantly better; however, running both algorithms is seen to be the best option as the computational cost of VIPER is low and it may succeed in some situations wherein MOGA will fail.

Communication among robots is key to performance in cooperative multi-robot systems. In practice, communication connections for information exchange between all robots are not always guaranteed, which adds difficulty to state estimation. This paper examines the decentralized cooperative simultaneous localization and mapping (SLAM) problem under a sparsely-communicating and dynamic network. We mathematically prove how the centralized-equivalent estimate can be obtained by all robots in the network in a decentralized manner. Furthermore, a robot only needs to consider its own knowledge of the network topology to detect when the centralized-equivalent estimate is obtainable. Our approach is validated through more than 250 minutes of experiments using a team of real robots, with accurate groundtruth data of all robots and landmark features.

Terrain assessment and path planning are intrinsically linked. There exist a variety of terrain-assessment algorithms and these methods follow the trend of low-fidelity at low-cost and high-fidelity at high-cost. We present a modular path-planning algorithm that uses a hierarchy of terrain-assessment methods; from low-fidelity to high-fidelity. Using all the available sensor data, the visible terrain is assessed with the low-fidelity, low-cost method. The decision to assess a piece of terrain with the high-fidelity, high-cost method is made considering potential path benefits and the cost of assessment. The result is a lower combined cost of the path and terrain assessment that exploits the capabilities of the robot chassis where prudent. We demonstrate the technique on a large number of simulated path-planning problems using fractal terrain, as well as provide preliminary results from an experimental field test carried out on Devon Island, Canada.

Finite-range sensing and communication are factors in the connectivity of a dynamic mobile-robot network. State estimation becomes a difficult problem when communication connections allowing information exchange between all robots are not guaranteed. This paper presents a decentralized state-estimation algorithm guaranteed to work in dynamic robot networks without connectivity requirements. We prove that a robot only needs to consider its own knowledge of network topology in order to produce an estimate equivalent to the centralized state estimate whenever possible while ensuring that the same can be performed by all other robots in the network. We prove certain properties of our technique and then it is validated through simulations. We present a comprehensive set of results, indicating the performance benefit in different network connectivity settings, as well as the scalability of our approach.

Finite-range sensing and communication are factors in the connectivity of a dynamic mobile robot network. State estimation becomes a difficult problem when communication connections for information exchange between all robots are not guaranteed. This paper presents a decentralized state estimation algorithm guaranteed to work in dynamic networks without connectivity requirements. We show that a robot only needs to consider its own knowledge of network topology in order to produce an estimate equivalent to the centralized state estimate whenever possible, while ensuring the same can be performed by all other robots in the network. Our technique is validated through simulations.

This paper describes a technique to estimate the 3D motion of a vehicle using odometric sensors and a stereo camera. The algorithm falls into the category of simultaneous localization and mapping as a large database of visual landmarks is created. The algorithm has been field tested online on a rover traversing loose terrain in the presence of obstacles. The resulting position estimation errors are between 0.5% and 4% of distance travelled, a significant improvement over odometry alone.

The ability to predict rover locomotion performance is critical during the design, validation and operational phases of a planetary robotic mission. Predicting locomotion performance depends on the ability to accurately characterize the wheel-soil interactions. In this research, wheel-soil interaction experiments were carried out on a single-wheel testbed and the results were compared with a single-wheel dynamic computer simulator which was developed in Matlab and Simulink's SimMechanics toolbox using a commercially-available wheel-soil interaction computer model called AESCO Soft Soil Tire Model (AS/sup 2/TM). Two different tire treads were used and compared in this study. There is good agreement between experimental and simulation results for wheel sinkage as a function of slip ratio; however, more investigation is needed to understand the differences observed for the drawbar pull and motor torque results.

This work presents an approach to optimizing entire trajectories for a group of mobile robots that use one another as localization beacons. The cost function we seek to optimize is a measure of localization uncertainty (as opposed to distance travelled or time). Our initial findings show that, for example, it is possible to improve on the intuitive equilateral-triangle formation for three robots.

We discuss our experiences in integrating a commercial off-the-shelf ground-penetrating radar unit with an all-terrain rover. Straight-line subsurface surveys were generated in a fully autonomous manner using odometry and a simple visual servoing technique. Survey results for various terrains are presented. We discuss the configuration of the integrated system and make recommendations for both Martian and terrestrial applications.

A method of planning paths for formations of mobile robots with nonholonomic constraints is presented. The kinematics equations presented in this paper allow a general geometrical formation of mobile robots to be maintained while the group as a whole travels an arbitrary path. It is possible to represent a formation of mobile robots by a single entity with the same type of nonholonomic constraint as the individual members. Thus, any path-planner or control method may be used with the formation as would be applied to an individual robot. Equations are developed for changing the geometrical formation and hardware results are presented from the Stanford MARS Testbed.

An approach to evolving globally coordinated behaviours in groups of autonomous mobile robots is presented. The control system in each robot is identical and consists of a cellular automaton which serves to arbitrate between a number of fixed basis behaviours. Genetic algorithms search for cellular automata whose arbitration results in success on a predefined task. Heap formation is presented as an example of a task requiring global coordination. Simulation results are provided.