In this paper, we propose Monte-Carlo Policy-Tree Decision Making (MCPTDM), an uncertainty-aware framework for high-variance planning problems with multiple dynamic agents. Planning when surrounded by multiple uncertain dynamic agents is hard because we cannot be certain of either the initial states or the future actions of those agents, leading to an exponential explosion in possible futures. Many important real-world problems, such as autonomous driving, fit this model. To address these difficulties, we combine Multi-policy Decision Making (MPDM) and Monte Carlo tree search (MCTS) and perform policy tree search with marginal action cost (MAC) estimation and repeated belief particles. We first design a synthetic experiment to evaluate these novel improvements in isolation. Then we evaluate the complete framework in a self-driving car simulation experiment and compare it against MPDM and Efficient Uncertainty-aware Decision Making (EUDM) methods. We release our complete source code for replicating our experiments and results.

Factor graph chains– the special case of a factor graph in which there are no potentials connecting non-adjacent nodes– arise naturally in many robotics problems. Importantly, they are often part of an inner loop in trajectory optimization and estimation problems, and so applications can be very sensitive to the performance of a solver.Of course, it is well-known that factor graph chains have an O(N) solution, but an actual solution is often left as "an exercise to the reader"… with the inevitable consequence that few (if any) efficient solutions are readily available.In this paper, we carefully derive the solution while keeping track of the specific block structure that arises, we work through a number of practical implementation challenges, and we highlight additional optimizations that are not at first apparent. An easy-to-use and self-contained solver is provided in C, which outperforms the AprilSAM general-purpose sparse matrix factorization library by a factor of 7.3x even without specialized block operations.The name AXLE reflects the names of the key matrices involved (the approach here solves the linear problem AX = E by factoring A as LLT), while also reflecting its key application in kino-dynamic trajectory estimation of vehicles with axles.

Robots working collaboratively can share observations with others to improve team performance, but communication bandwidth is limited. Recognizing this, an agent must decide which observations to communicate to best serve the team. Accurately estimating the value of a single communication is expensive; finding an optimal combination of observations to put in the message is intractable. In this paper, we present OCBC, an algorithm for Optimizing Communication under Bandwidth Constraints. OCBC uses forward simulation to evaluate communications and applies a bandit-based combinatorial optimization algorithm to select what to include in a message. We evaluate OCBC’s performance in a simulated multi-robot navigation task. We show that OCBC achieves better task performance than a state-of-the-art method while communicating up to an order of magnitude less.

In this paper, we propose a flexible mapping scheme that uses a masking function (mask) to focus the attention of a pose graph SLAM (Simultaneous Localization and Mapping) system. The masking function takes the robot’s observations and returns true if the robot is in an important location. State-of-the-art methods in SLAM generate dense metric lidar maps, creating precise maps at a high computational cost by storing lidar scans for each pose node and continually attempting to close loops. In many cases, trying to always make loop closures is unnecessary for localization and even risky because of perceptual aliasing and false positives. By masking out these less useful positions, our method can create more accurate maps despite performing far fewer scan matches. We evaluate our system with three simple mask functions on a 2.5 km trajectory with significant angular drift. We compare the number of scan matches performed under each mask as well as the accuracy of the loop closures.

—Multi-Policy Decision Making (MPDM) is a planning framework in which an agent dynamically switches be-tween a set of policies by predicting the performance of those policies using forward simulation. But in virtually all MPDM approaches, the set of policies are created by domain experts. In this paper, we learn these policy sets off-line. We use an evolutionary algorithm approach, which allows us to directly optimize the performance of the policy set, rather than some proxy objective. We also propose the use of Terminal, an online strategy game, as an evaluation domain for planning algorithms. Like many real-world robotics problems, Terminal requires multi-agent planning, coping with uncertainty, and practical limits on computational complexity. We describe how we used our approach to generate an agent which is ranked in the top 10 in a global online competition.

Multi-Policy Decision Making (MPDM) has been shown to be an effective method for single-agent navigation tasks. In this paper, we extend MDPM to long-horizon multirobot planning tasks with uncertain communication. We constrain each team member to choose the best of several simple policies through forward simulation in a decentralized fashion. We demonstrate this algorithm on both a coverage task as well as a challenging adversarial target search scenario, with uncertain communication for both. We also show that our algorithm can generalize to scenarios it was not tuned for.

System, methods, and other embodiments described herein relate to identifying changes between models of a locality. In one embodiment, a method includes, in response to determining that a location model is available for a present environment of a vehicle, generating a current model of the present environment using at least one sensor of the vehicle. The method also includes isolating dynamic objects in the current model as a function of the location model. The method includes providing the dynamic objects to be identified and labeled.

We propose the idea of flexible tag layouts for visual fiducial systems. In contrast to traditional square tags, flexible tag layouts allow circular, annular, or other shapes as desired. Layout flexibility also allows the data density of square shaped tags to be increased at the cost of a minor decrease in detection distance and pose estimation accuracy by decreasing the size of the detection border. In addition, we describe a detector that is faster and has higher recall than either the AprilTag 2 or the ArUco detectors.

System, methods, and other embodiments described herein relate to identifying surface properties of objects using a light detection and ranging (LIDAR) sensor. In one embodiment, a method includes, in response to scanning a surface of an object using the LIDAR sensor, receiving a reflected waveform as a function of attributes of the surface. The method includes analyzing the reflected waveform according to a surface property model to produce an estimate of the attributes. The surface property model characterizes relationships between reflected waveforms and different surface properties. The method includes providing the estimate as an indication of the surface of the scanned object.

The problem of planning the actions of several robots (pursuers) who are searching for another agent (an evader) has been frequently studied, with most methods focusing on finding strategies that guarantee capture of even a worst-case evader. However, in many real-world situations, the environment may be too complex or the pursuers too few in number to ensure capture. In such cases, the best that the pursuers can do is select actions that maximize the probability of capture for a given type of evader. In this paper, we propose Probabilistic Adversarial Target Search (PATS), which computes joint search actions that approximately maximize capture probability against an evader with perfect knowledge but finite speed. PATS uses Monte Carlo tree search (MCTS) to compute pursuit plans given the pursuers’ probabilistic belief about the evader’s location. PATS then evolves this belief forward in time based on the expected actions of the evader, which are obtained from the search tree’s empirical statistics. We show that PATS outperforms an existing probabilistic search method in a simulated search setting for which guaranteed search is impossible.

ICRA 2019 Paper Submission Code and DatasetsWe propose an ultra-wideband-based (UWB) localization system that achieves high accuracy through non-parametric estimation of measurement probability densities and explicit modeling of antenna delays. This problem is difficult because non-line-of-sight conditions give rise to multimodal errors, which make linear estimation methods ineffective. The primary contribution in this paper is an approach for both characterizing these errors in situ and an optimization framework that recovers both positions and antenna delays. We evaluate our system with a network of 8 nodes based on the DecaWave DWM1000 and achieve accuracies from 3 cm RMSE in line-of-sight conditions to 30 cm RMSE in non-line-of-sight conditions. Collecting measurements and localizing the network in this manner requires less than a minute, after which the realized network may be used for dynamic real-time tracking.

We propose an ultra-wideband-based (UWB) localization system that achieves high accuracy through non-parametric estimation of measurement probability densities and explicit modeling of antenna delays. This problem is difficult because non-line-of-sight conditions give rise to multimodal errors, which make linear estimation methods ineffective. The primary contribution in this paper is an approach for both characterizing these errors in situ and an optimization framework that recovers both positions and antenna delays. We evaluate our system with a network of 8 nodes based on the DecaWave DWM1000 and achieve accuracies from 3 cm RMSE in line-of-sight conditions to 30 cm RMSE in non-line-of-sight conditions. Collecting measurements and localizing the network in this manner requires less than a minute, after which the realized network may be used for dynamic real-time tracking.

In this paper, we describe a method for global localization in a previously unvisited environment using only a schematic floor plan as a prior map. The floor plan need not be a precision map - it can be the sort of image found in buildings to guide people or aid evacuation. The core idea is to identify features that are stable across both a drawn floor plan and robot point-of-view LIDAR data, for example wall intersections, which appear as corners from overhead and as vertical lines from the ground. We introduce a factor graph-based global localization method that uses these features as landmarks. The detections of such descriptorless features are noisy and often ambiguous. We therefore propose robust data association based on a pairwise measurement consistency check and max-mixtures error model. We evaluate the resulting system in a real-world indoor environment, demonstrating performance comparable to a baseline system that uses a conventional LIDAR-based prior map.

Fiducials are artificial features with a variety of uses in computer vision such as object tracking and localization. We propose the idea of flexible tag layouts for visual fiducial systems. In contrast to traditional square tags, flexible tag layouts allow circular, annular, or other shapes as desired. One use of layout flexibility is to increase the data density of standard square shaped tags. In addition, we describe a detector that is faster and has higher recall than both the AprilTag 2 and ArUco detectors while maintaining precision.

For online robots, incremental SLAM algorithms offer huge potential computational savings over batch algorithms. The dominant incremental algorithms are iSAM and iSAM2 which offer radically different approaches to computing incremental updates, balancing issues like 1) the need to relinearize, 2) changes in the desirable variable marginalization order, and 3) the underlying conceptual approach (i.e. the "matrix" story versus the "factor graph" story). In this paper, we propose a new incremental algorithm that computes solutions with lower absolute error and generally provides lower error solutions for a fixed computational budget than either iSAM or iSAM2. Key to AprilSAM's performance are a new dynamic variable reordering algorithm for fast incremental Cholesky factorizations, a method for reducing the work involved in backsubstitutions, and a new algorithm for deciding between incremental and batch updates.

This chapter presents multi-policy decision-making (MPDM): a novel approach to navigating in dynamic multi-agent environments. Rather than planning the trajectory of the robot explicitly, the planning process selects one of a set of closed-loop behaviors whose utility can be predicted through forward simulation that captures the complex interactions between the actions of these agents. These polices capture different high-level behavior and intentions, such as driving along a lane, turning at an intersection, or following pedestrians. We present two different scenarios where MPDM has been applied successfully: an autonomous driving environment models vehicle behavior for both our vehicle and nearby vehicles and a social environment, where multiple agents or pedestrians configure a dynamic environment for autonomous robot navigation. We present extensive validation for MPDM on both scenarios, using simulated and real-world experiments.

Risk-aware Multi-Policy Decision Making (MPDM) is a powerful framework for reliable navigation in a dynamic social environment where rather than evaluating individual trajectories, a "library" of policies (reactive controllers) is evaluated by anticipating potentially dangerous future outcomes using an on-line forward roll-out process. There is a core tension in Multi-Policy Decision Making (MPDM) systems - it is desirable to add more policies to the system for flexibility in finding good policies, however, this increases computational cost. As a result, MPDM was limited to small (perhaps 5-10) discrete policies - a significant performance bottleneck. In this paper, we radically enhance the expressivity of MPDM by allowing policies to have continuous-valued parameters, while simultaneously satisfying real-time constraints by quickly discovering promising policy parameters through a novel iterative gradient-based algorithm. Our evaluation includes results from extensive simulation and real-world experiments in semi-crowded environments.