Maps are essential for diverse applications, such as vehicle navigation and autonomous robotics. Both require spatial models for effective route planning and localization. This paper addresses the challenge of road graph construction for autonomous vehicles. Despite recent advances, creating a road graph remains labor-intensive and has yet to achieve full automation. The goal of this paper is to generate such graphs automatically and accurately. Modern cars are equipped with onboard sensors used for today's advanced driver assistance systems like lane keeping. We propose using global navigation satellite system (GNSS) traces and basic image data acquired from these standard sensors in consumer vehicles to estimate road-level maps with minimal effort. We exploit the spatial information in the data by framing the problem as a road centerline semantic segmentation task using a convolutional neural network. We also utilize the data’s time series nature to refine the neural network’s output by using map matching. We implemented and evaluated our method using a fleet of real consumer vehicles, only using the deployed onboard sensors. Our evaluation demonstrates that our approach not only matches existing methods on simpler road configurations but also significantly outperforms them on more complex road geometries and topologies. This work received the 2023 Woven by Toyota Invention Award.

This paper develops a novel slip estimator using the invariant observer design theory and Disturbance Observer (DOB). The proposed state estimator for mobile robots is fully proprioceptive and combines data from an inertial measurement unit and body velocity within a Right Invariant Extended Kalman Filter (RI-EKF). By embedding the slip velocity into SE3(3) matrix Lie group, the developed DOB-based RI-EKF provides real-time velocity and slip velocity estimates on different terrains. Experimental results using a Husky wheeled robot confirm the mathematical derivations and effectiveness of the proposed method in estimating the observable state variables. Open-source software is available for download and reproducing the presented results.

In this article, we present a novel and flexible multitask multilayer Bayesian mapping framework with readily extendable attribute layers. The proposed framework goes beyond modern metric-semantic maps to provide even richer environmental information for robots in a single mapping formalism while exploiting intralayer and interlayer correlations. It removes the need for a robot to access and process information from many separate maps when performing a complex task, advancing the way robots interact with their environments. To this end, we design a multitask deep neural network with attention mechanisms as our front-end to provide heterogeneous observations for multiple map layers simultaneously. Our back-end runs a scalable closed-form Bayesian inference with only logarithmic time complexity. We apply the framework to build a dense robotic map, including metric-semantic occupancy and traversability layers. Traversability ground truth labels are automatically generated from exteroceptive sensory data in a self-supervised manner. We present extensive experimental results on publicly available datasets and data collected by a three-dimensional bipedal robot platform and show reliable mapping performance in different environments. Finally, we also discuss how the current framework can be extended to incorporate more information, such as friction, signal strength, temperature, and physical quantity concentration using Gaussian map layers. The software for reproducing the presented results or running on customized data is made publicly available.

Lane-level maps provide crucial detail to autonomous vehicle perception and decision making systems. Many common HD maps require human annotators to painstakingly label attributes such as lane geometry, connectivity, and speed limit. In this paper, we propose a method to derive these attributes automatically. Our method uses lane features and previous vehicle experience to generate potentials in a rectified raster map. We then infer the driving lanes within a Markov random field. Moreover, the lane topologies are extracted and represented by parsing the roadway into different segments based on the number of driving lanes and generating lane transitions between adjacent road segments. Lane attributes, such as speed limit and stop locations, are inferred from statistical analysis of previous vehicle experiences. Our approach is supported by experiments demonstrating effective automatic lane map generation on real-world roads.

Determining the rigid-body transformation be-tween 2D image data and 3D point cloud data has applications for mobile robotics including sensor calibration and localizing into a prior map. Common approaches to 2D-3D registration use least-squares solvers assuming known associations often provided by heuristic front-ends, or iterative nearest-neighbor. We present a linear line-based 2D-3D registration algorithm formulated as a mixed-integer program to simultaneously solve for the correct transformation and data association. Our formulation is explicitly formulated to handle outliers, by modeling associations as integer variables. Additionally, we can constrain the registration to SE(2) to improve runtime and accuracy. We evaluate this search over multiple real-world data sets demonstrating adaptability to scene variation.

State-of-the-art robotic perception systems have achieved sufficiently good performance using Inertial Measurement Units (IMUs), cameras, and nonlinear optimization techniques, that they are now being deployed as technologies. However, many of these methods rely significantly on vision and often fail when visual tracking is lost due to lighting or scarcity of features. This paper presents a state-estimation technique for legged robots that takes into account the robot's kinematic model as well as its contact with the environment. We introduce forward kinematic factors and preintegrated contact factors into a factor graph framework that can be incrementally solved in real-time. The forward kinematic factor relates the robot's base pose to a contact frame through noisy encoder measurements. The preintegrated contact factor provides odometry measurements of this contact frame while accounting for possible foot slippage. Together, the two developed factors constrain the graph optimization problem allowing the robot's trajectory to be estimated. The paper evaluates the method using simulated and real sensory IMU and kinematic data from experiments with a Cassie-series robot designed by Agility Robotics. These preliminary experiments show that using the proposed method in addition to IMU decreases drift and improves localization accuracy, suggesting that its use can enable successful recovery from a loss of visual tracking.

Sparse feature-based maps provide a compact representation of the environment that admit efficient algorithms, for example simultaneous localization and mapping. These representations typically assume a static world and therefore contain static map features. However, since the world contains dynamic elements, determining when map features no longer correspond to the environment is essential for long-term utility. This work develops a feature-based model of the environment which evolves over time through feature persistence. Moreover, we augment the state-of-the-art sparse mapping model with a correlative structure that captures spatio-temporal properties, e.g. that nearby features frequently have similar persistence. We show that such relationships, typically addressed through an ad hoc formalism focusing only on feature repeatability, are crucial to evaluate through a probabilistically principled approach. The joint posterior over feature persistence can be computed efficiently and used to improve online data association decisions for localization. The proposed algorithms are validated in numerical simulation and using publicly available data sets.

Many autonomous systems require the ability to perceive and understand motion in a dynamic environment. We present a novel algorithm that estimates this motion from raw LIDAR data in real-time without the need for segmentation or model-based tracking. The sensor data is first used to construct an occupancy grid. The foreground is then extracted via a learned background filter. Using the filtered occupancy grid, raw scene flow between successive scans is computed. Finally, we incorporate these measurements in a filtering framework to estimate temporal scene flow. We evaluate our method on the KITTI dataset.

This paper reports on an underwater cooperative localization algorithm for faulty low-bandwidth communication channels based on a factor graph estimation framework. Vehicles measure the one-way-travel-time (OWTT) of acoustic broadcasts to obtain a relative range observation to the transmitting vehicle. We present a method to robustly share locally observed sensor data across the network by exploiting odometry factor composition. Our algorithm calls on approximate marginalization techniques to compute a compact set of informative factors that enable local navigation data to be shared efficiently. We provide results from a real-time implementation of our algorithm using two autonomous underwater vehicles and a surface vehicle.

This paper reports on a Gaussian belief-space planning formulation for mobile robots that includes random measurement acquisition variables that model whether or not each measurement is actually acquired. We show that maintaining the stochasticity of these variables in the planning formulation leads to a random belief covariance matrix, allowing us to consider the risk associated with the acquisition in the objective function. Inspired by modern portfolio theory and utility optimization, we design objective functions that are risk-averse, and show that risk-averse planning leads to decisions made by the robot that are desirable when operating under uncertainty. We show the benefit of this approach using simulations of a planar robot traversing an uncertain environment and of an underwater robot searching for loop-closure actions while performing visual SLAM.

This paper reports on the inclusion of a probabilistic channel model within a cooperative localization planning framework. Underwater cooperative localization reduces positioning errors by sharing sensor data across a team of underwater vehicles. Relative range constraints between vehicles are measured by the one-way-travel-time of successfully received acoustic communication broadcasts. The quality of the navigation solution is intimately linked to the geometry of the network and, therefore, can benefit from planning informative relative trajectories. We cast this planning problem as an instance of belief space planning. In order to weight packet loss over the acoustic channel, we introduce a probabilistic channel model into the planning framework. We propose an optimization algorithm that allows us to plan open-loop control actions and, by extension, closed-loop parameterized trajectories.

We report a decentralized extended information filter (DEIF) algorithm designed for single-beacon cooperative acoustic navigation of one or more client underwater vehicles. In single-beacon cooperative acoustic navigation, ranges and state information from a single reference source (the server) are used to improve localization and navigation of an underwater vehicle (the client). The ranges and state information are obtained using underwater acoustic modems and a synchronous-clock time-of-flight paradigm. Apart from the server's acoustic data broadcasts, the client has no access to the server's position or sensor measurements. We show that at the instance of each range measurement update, the DEIF algorithm yields identical results for the current vehicle state estimate as the corresponding centralized extended information filter (CEIF), which fully tracks the joint probability distribution between the server and client. We compare the state estimation results of the DEIF algorithm with that of a CEIF and three other filters reported in the literature. The evaluation is performed using both simulated data and an experimental dataset comprised of one surface craft and two autonomous underwater vehicles.