Developing software for large scale off-road robot applications is challenging and tedious due to cost, logistics, and rigor of field testing. High-fidelity sensor-realistic simulation can speed up the development process for perception and state estimation algorithms. We focus on Lidar simulation for robots operating in off-road environments. Lidars are integral sensors for robots, and Lidar simulation for off-road environments is particularly challenging due to the way Lidar rays interact with natural terrain such as vegetation. A hybrid geometric terrain representation has been shown to model Lidar observations well [1]. However, previous work has only been able to simulate a single, fixed scene, and the entire scene had to be precisely surveyed. In this work, we add semantic information to the hybrid geometric model. This allows us to extract terrain primitives, such as trees and shrubs, from data logs. Our approach uses these primitives to compose arbitrary scenes for Lidar simulation. We evaluate our simulator on a real-world environment of interest, and show that primitives derived using our approach generalize to new scenes.

Finding meaningful, structured representations of 3D point cloud data (PCD) has become a core task for spatial perception applications. In this paper we introduce a method for constructing compact generative representations of PCD at multiple levels of detail. As opposed to deterministic structures such as voxel grids or octrees, we propose probabilistic subdivisions of the data through local mixture modeling, and show how these subdivisions can provide a maximum likelihood segmentation of the data. The final representation is hierarchical, compact, parametric, and statistically derived, facilitating run-time occupancy calculations through stochastic sampling. Unlike traditional deterministic spatial subdivision methods, our technique enables dynamic creation of voxel grids according the application's best needs. In contrast to other generative models for PCD, we explicitly enforce sparsity among points and mixtures, a technique which we call expectation sparsification. This leads to a highly parallel hierarchical Expectation Maximization (EM) algorithm well-suited for the GPU and real-time execution. We explore the trade-offs between model fidelity and model size at various levels of detail, our tests showing favorable performance when compared to octree and NDT-based methods.

Traditional control and planning algorithms for wheeled mobile robots (WMR) either totally ignore or make simplifying assumptions about the effects of wheel slip on the motion. While this approach works reasonably well in practice on benign terrain, it fails very quickly when the WMR is deployed in terrain that induces significant wheel slip. We contribute a novel control framework that predictively corrects for the wheel slip to effectively minimize path following errors. Our framework, the Receding Horizon Model Predictive Path Follower (RHMPPF), specifically addresses the problem of path following in challenging environments where the wheel slip substantially affects the vehicle mobility. We formulate the solution to the problem as an optimal controller that utilizes a slip-aware model predictive component to effectively correct the controls generated by a strictly geometric pure-pursuit path follower. We present extensive experimental validation of our approach using a simulated 6-wheel skid-steered robot in a high-fidelity data-driven simulator, and on a real 4-wheel skid-steered robot. Our results show substantial improvement in the path following performance in both simulation and real world experiments.

There is growing interest in applying machine learning to motion planning. Potential applications are predicting an initial seed for trajectory optimization, predicting an effective heuristic for search based planning, and even predicting a planning algorithm for adaptive motion planning systems. In these situations, providing only a single prediction is unsatisfactory. It leads to many scenarios where the prediction suffers a high loss. In this paper, we advocate list prediction. Each predictor in a list focusses on different regions in the space of environments. This overcomes the shortcoming of a single predictor, and improves overall performance. A framework for list prediction, ConseqOpt, already exists. Our contribution is an extensive domain-specific treatment. We provide a rigorous and clear exposition of the procedure for training a list of predictors. We provide experimental results on a spectrum of motion planning applications. Each application contributes to understanding the behavior of list prediction. We observe that the benefit of list prediction over a single prediction is significant, irrespective of the application.

Sensor models, which specify the distribution of sensor observations, are a widely used and integral part of robotics algorithms. Observation distributions are commonly approximated by parametric models, which are limited in their expressiveness, and may require careful design to suit an application. In this paper, we propose nonparametric distribution regression as a procedure to model sensors. It is a data-driven procedure to predict distributions that makes few assumptions. We apply the procedure to model raw distributions from real sensors, and also demonstrate its utility to a mobile robot state estimation task. We show that nonparametric distribution regression adapts to characteristics in the training data, leading to realistic predictions. The same procedure competes favorably with baseline parametric models across applications. The results also help develop intuition for different sensor modeling situations. Our procedure is useful when distributions are inherently noisy, and sufficient data is available.

Advances in hardware design have made wheeled mobile robots (WMRs) exceptionally mobile. To fully exploit this mobility, we present a novel dynamic model formulation for use in WMR planning, control, and estimation systems. The formulation is high fidelity, general, modular, and fast. It builds on our prior work on recursive methods for 3-D kinematics derivation and constrained motion prediction using differential algebraic equations. It is stable, even for large integration steps, and can enforce realistic nonlinear models of wheel-terrain interaction. Simulation tests show our dynamic models to be more functional, stable, and efficient than common alternatives. Simulations can run over 1K × faster than real time on an ordinary PC. Experimental results on multiple platforms and terrain types show that, once calibrated, our models predict motion accurately. To facilitate their use, we have released open-source MATLAB and C++libraries implementing our modeling/simulation methods.

Registration of Point Cloud Data (PCD) forms a core component of many 3D vision algorithms such as object matching and environment reconstruction. In this paper, we introduce a PCD registration algorithm that utilizes Gaussian Mixture Models (GMM) and a novel dual-mode parameter optimization technique which we call mixture decoupling. We show how this decoupling technique facilitates both faster and more robust registration by first optimizing over the mixture parameters (decoupling the mixture weights, means, and co variances from the points) before optimizing over the 6 DOF registration parameters. Furthermore, we frame both the decoupling and registration process inside a unified, dual-mode Expectation Maximization (EM) framework, for which we derive a Maximum Likelihood Estimation (MLE) solution along with a parallel implementation on the GPU. We evaluate our MLE-based mixture decoupling (MLMD) registration method over both synthetic and real data, showing better convergence for a wider range of initial conditions and higher speeds than previous state of the art methods.

We study the problem of building a sensor model for the purpose of simulation. Our work is motivated by the potential impact of realistic simulators on the development cycle of software for real robots. The case is made for building models from approximate state information, relieving the burden of ground truth. Unlike calibration, where the goal is to identify and remove error from a signal, our aim to reproduce the signal in its entirety, including its error properties. Instead of physically modeling sensor behavior, a data-driven approach is taken. The implementation of our approach to simulate a simple but noisy laser rangefinder is described. Finally, approaches to validate the simulator are discussed. We compare not only raw sensor predictions, but also overall performance of algorithms on simulated versus real data.

We formulate the problem of wheeled mobile robot (WMR) modeling under wheel slip, while moving over rough terrain, as the integral of a differential algebraic equation (DAE) of either first or second order. That is, the elements of the model include both differential equations and kinematic constraints. While such a formulation is typical of Lagrangian dynamics in augmented form, we show how identical principles apply to kinematic models of motion. In effect, we show in this paper that the very high fidelity models traditionally used in offline simulation can be readily adapted for use in real time for both control and estimation of the motion of arbitrary WMRs over arbitrary rolling terrain, while slipping. While such models are more correct and general than the naive methods that are typically used, we also show that they are orders of magnitude more efficient than such naive methods for equivalent accuracy.

For purposes of real-time 3D sensing, it is important to be able to quickly register together incoming point cloud data. In this paper, we devise a method to quickly and robustly decompose large point clouds into a relatively small number of meaningful surface patches from which we register new data points. The surface patch representation sidesteps the costly problem of matching points to points since incoming data only need to be compared with the patches. The chosen parametrization of the patches (as Gaussians) leads to a smooth data likelihood function with a well-defined gradient. This representation thus forms the basis for a robust and efficient registration algorithm using a parallelized gradient descent implemented on a GPU using CUDA. We use a modified Gaussian Mixture Model (GMM) formulation solved by Expectation Maximization (EM) to segment the point cloud and an annealing gradient descent method to find the 6-DOF rigid transformation between the incoming point cloud and the segmented set of surface patches. We test our algorithm, Robust EM Segmentation (REM-Seg), against other GPU-accelerated registration algorithms on simulated and real data and show that our method scales well to large numbers of points, has a wide range of convergence, and is suitably accurate for 3D registration.

This paper presents an online approach to calibrating vehicle model parameters that uses the integrated dynamics of the system. Specifically, we describe the identification of the time constant and delay in a first-order model of the vehicle powertrain, as well as parameters required for pose estimation (including position offsets for the inertial measurement unit, steer angle sensor parameters, and wheel radius). Our approach does not require differentiation of state measurements like classical techniques; making it ideal when only low-frequency measurements are available. Experimental results on the LandTamer and Zoë rover platforms show online calibration using integrated dynamics to be fast and more accurate than both manual and classical calibration methods.

When GPS, or other absolute positioning, is un-available, terrain-relative velocity is crucial for dead reckoning and the vehicle's pose estimate. Unfortunately, the position-denied accuracy of the inertial navigation system (INS) is governed by the performance of the linear velocity aiding sources, such as wheel odometry, which are typically corrupted by large systematic errors due to wheel slip. As a result, affordable terrestrial inertial navigation is ineffective in estimating position when denied position fixes for an extended period of time. For mobile robots, the mapping between inputs and resultant behavior depends critically on terrain conditions which vary significantly over time and space which cannot be pre-programmed. Past work has used Integrated Perturbative Dynamics (IPD) to identify successively systematic and stochastic models of wheel slip, but treated the pose filter only as input without improving the odometry measurements used for vehicle navigation. We present a unique approach of a predictive vehicle slip model in a delayed state extended Kalman filter. The relative pose difference between the current state and delayed state is used as a measurement update to the vehicle slip model. These results create an opportunity to compensate for wheel slip effects in terrestrial inertial navigation. This paper presents the design, calibration, and verification of such a system and concludes that the position-denied performance of the compensated system is far superior.

Exploring planetary surfaces typically involves traversing challenging and unknown terrain and acquiring insitu measurements at designated locations using arm-mounted instruments. We present field results for a new implementation of an autonomous capability that enables a rover to traverse and precisely place an arm-mounted instrument on remote targets. Using point-and-click mouse commands, a scientist designates targets in the initial imagery acquired from the rover's mast cameras. The rover then autonomously traverses the rocky terrain for a distance of 10 - 15 m, tracks the target(s) of interest during the traverse, positions itself for approaching the target, and then precisely places an arm-mounted instrument within 2-3 cm from the originally designated target. The rover proceeds to acquire science measurements with the instrument. This work advances what has been previously developed and integrated on the Mars Exploration Rovers by using algorithms that are capable of traversing more rock-dense terrains, enabling tight thread-the-needle maneuvers. We integrated these algorithms on the newly refurbished Athena Mars research rover and fielded them in the JPL Mars Yard. We conducted 43 runs with targets at distances ranging from 5 m to 15 m and achieved a success rate of 93% for placement of the instrument within 2-3 cm.

This paper presents a type of motion primitives that can be used for building efficient kinodynamic motion planners. The primitives are pre-computed to meet two objective: to capture the mobility constraints of the robot as well as possible and to establish a state sampling policy that is conductive to efficient search. The first objective allows encoding mobility constraints into primitives, thereby enabling fast unconstrained search to produce feasible solutions. The second objective enables high quality (lattice) sampling of state space, further speeding up exploration during search. We further discuss several novel results enabled by using such motion primitives for kinodynamic planning, including incremental search, efficient bi-directional search and incremental sampling.

This paper presents an approach to differentially constrained robot motion planning and efficient re-planning. Satisfaction of differential constraints is guaranteed by the search space which consists of motions that satisfy the constraints by construction. Any systematic replanning algorithm, e.g. D*, can be utilized to search the state lattice to find a motion plan that satisfies the differential constraints, and to repair it efficiently in the event of a change in the environment. Further efficiency is obtained by varying the fidelity of representation of the planning problem. High fidelity is utilized where it matters most, while it is lowered in the areas that do not affect the quality of the plan significantly. The paper presents a method of modifying the fidelity between replans, thereby enabling dynamic flexibility of the search space, while maintaining its compatibility with replanning algorithms. The approach is especially suited for mobile robotics applications in unknown challenging environments. We successfully applied the motion planner on a real robot: the planner featured 10Hz average replan rate on minimal computing hardware, while satisfying the car-like differential constraints.

We combine a visual odometry system with an aided inertial navigation filter to produce a precise and robust navigation system that does not rely on external infrastructure. Incremental structure from motion with sparse bundle adjustment using a stereo camera provides real-time highly accurate pose estimates of the sensor which are combined with six degree-of-freedom inertial measurements in an Extended Kalman Filter. The filter is structured to neatly handle the incremental and local nature of the visual odometry measurements and to handle uncertainties in the system in a principled manner. We present accurate results from data acquired in rural and urban scenes on a tractor and a passenger car travelling distances of several kilometers.

This paper presents an approach to differentially constrained robot motion planning and efficient re-planning. Satisfaction of differential constraints is guaranteed by the state lattice, a search space which consists of motions that satisfy the constraints by construction. Any systematic replanning algorithm, e.g. D*, can be utilized to search the state lattice to find a motion plan that satisfies the differential constraints, and to repair it efficiently in the event of a change in the environment. Further efficiency is obtained by varying the fidelity of representation of the planning problem. High fidelity is utilized where it matters most, while it is lowered in the areas that do not affect the quality of the plan significantly. The paper presents a method to modify the fidelity between replans, thereby enabling dynamic flexibility of the search space, while maintaining its compatibility with replanning algorithms. The approach is especially suited for mobile robotics applications in unknown challenging environments. In this setting, we applied the planner successfully to the navigation of research prototype rovers in JPL Mars Yard.

We present a method for trajectory generation for all-wheel steering mobile robots which can account for rough terrain and predictable vehicle dynamics and apply it to the problem of generating optimal motion splines. There has been little work in trajectory generation for vehicles with all-wheel steering capability compared to the Ackermann, differential-drive, or omnidirectional mobility system models. The presented method linearizes and inverts forward models of propulsion, suspension, and motion to minimize boundary state error given a parameterized set of controls. Our method for generating efficient motion splines between a series of state boundary constraints optimizes the free path heading boundary constraint while meeting position and orientation state constraints. We demonstrate this algorithm on the Rocky 8 rover platform, where parameterized linear velocity, curvature, and path heading controls are generated which satisfy position, orientation, and path heading constraints in rough terrain

This paper presents a solution to the problem of finding an effective yet admissible heuristic function for A* by precomputing a look-up table of solutions. This is necessary because traditional heuristic functions such as Euclidean distance often produce poor performance for certain problems. Here, the technique is applied to the state lattice, which is used for full state space motion planning. However, the approach is applicable to many applications of heuristic search algorithms. The look-up table is demonstrated to be feasible to generate and store. A principled technique is presented for selecting which queries belong in the table. Finally, the results are validated through testing on a variety of path planning problems

We propose a principled method to create a search space for constrained motion planning, which efficiently encodes only feasible motion plans. The space of possible paths is encoded implicitly in the connections between states, but only feasible and only local connections are allowed. Furthermore, we propose a systematic method to generate a near-minimal set of spatially distinct motion alternatives. This set of motion primitives preserves the connectivity of the representation while eliminating redundancy - leading to a very efficient structure for motion planning at the chosen resolution.

Mobile robot localization from large-scale appearance mosaics has been showing increasing promise as a low-cost, high-performance and infrastructure-free solution to vehicle guidance in man-made environments. The feasibility of this technique relies on the construction of a locally smooth and globally consistent high-resolution mosaic of the vehicle environment, efficiently done using observations that have low spatial and temporal persistence. The problem of loop closure in cyclic environments that plagues this process is one that is commonly encountered in all map-building procedures, and its solution is often computationally expensive. The paper presents a method that reliably generates consistent maps at low computational cost, while fully exploiting the topology of the observations. Extensions to a real-time implementation are discussed along with results using simulated data and those from real indoor environments.

Mobile robot localization from large-scale appearance mosaics has been showing increasing promise as a low-cost, high-performance and infrastructure free solution to vehicle-guidance in man-made environments. The generation of the globally consistent high-resolution mosaics crucial to this procedure suffers from the same problem of loop-closure in cyclic environments that is commonly encountered in all map-building procedures. This paper presents a batch solution to the problem of reliably generating globally consistent mosaics at low computational cost, that simultaneously exploits the topological constraints among the observations and minimizes the total residual in observed features. An extension to a general scalable framework that facilitates an incremental online mapping strategy is also presented, along with results using simulated data and from real indoor environments.

Vehicle odometry is a nonlinear dynamical system in echelon form. Accordingly, a general solution can be written by solving the nonlinear equations in the correct order. Another implication of this structure is that a completely general solution to the linearized (perturbative) dynamics exists. The associated vector convolution integral is the general relationship between the output error and both the input error and reference trajectory. Solutions for errors in individual coordinates are in the form of line integrals in state space. Response to initial conditions and translational scale errors, among others, is path independent and vanishes on all closed trajectories. Response to other errors is path dependent and can be reduced to expressions in error moments of the reference trajectory. These path dependent errors vanish on closed symmetric paths, among others. These theoretical results and the underlying error expressions have many uses in design, calibration, and evaluation of odometry systems.

For autonomously navigating vehicles, the automatic generation of dense geometric models of the environment is a computationally expensive process. Using first principles, it is possible to quantify the relationship between the raw throughput required of the perception system and the maximum safely achievable speed of the vehicle. We show that terrain mapping perception is of polynomial complexity in the response distance. To the degree that geometric perception consumes time, it also degrades real-time response characteristics. Given this relationship, several strategies of adaptive geometric perception arise which are practical for autonomous vehicles.

A basic requirement of autonomous vehicles is that of guaranteeing the safety of the vehicle by avoiding hazardous situations. This paper analyses this requirement in general terms of real-time response, and throughput. Several nondimensional expressions emerge which characterize requirements in canonical form.

A basic requirement of autonomous vehicles is that of guaranteeing the safety of the vehicle by avoiding hazardous situations. This paper analyses this requirement in general terms of the resolution and accuracy of sensors and computations. Several nondimensional expressions emerge which characterize requirements in canonical form.

For autonomously navigating vehicles, the automatic generation of dense geometric models of the environment is a computationally expensive process. Yet, analysis suggests that some approaches to mapping the environment in mobility scenarios can waste significant computational resources. This paper proposes a relatively simple method of approaching the minimum required perceptual throughput in a terrain mapping system, and hence the fastest possible update of the environmental model. We accomplish this by exploiting the constraints of typical mobility scenarios. The technique proposed will be applicable to any application that models the environment with a terrain map or other 2-1/2 D representation.

Several researchers in robotics and artificial intelligence have found that the commonly used method of planning in a state (configuration) space is intractable in certain domains. This may be because the C-space has very high dimensionality, the "C-space obstacles" are too difficult to compute, or because a mapping between desired states and actions is not straightforward. Instead of using an inverse model that relates a desired state to an action to be executed by a robot, we have used a methodology that selects between the feasible actions that a robot might execute, in effect, circumventing many of the problems faced by configuration space planners. In this paper we discuss the implications of such a method and present two examples of working systems that employ this methodology. One system drives an autonomous cross-country vehicle while the other controls a robotic excavator performing a trenching operation.