STITCHER: Real-Time Trajectory Planning with Motion Primitive Search

Autonomous high-speed navigation through large, complex environments requires real-time generation of agile trajectories that are dynamically feasible, collision-free, and satisfy state or actuator constraints. Most modern trajectory planning techniques rely on numerical optimization because high-quality, expressive trajectories that satisfy various constraints can be systematically computed. However, meeting computation time constraints and the potential for numerical instabilities can limit the use of optimization-based planners in safety-critical scenarios. This work presents an optimization-free planning framework that stitches short trajectory segments together with graph search to compute long range, expressive, and near-optimal trajectories in real-time. Our STITCHER algorithm is shown to outperform modern optimization-based planners through our innovative planning architecture and several algorithmic developments that make real-time planning possible. Extensive simulation testing is conducted to analyze the algorithmic components that make up STITCHER, and a thorough comparison with two state-of-the-art optimization planners is performed. It is shown STITCHER can generate trajectories through complex environments over long distances (tens of meters) with low computation times (milliseconds).

Quaternion Sliding Variables in Manipulator Control

We present two quaternion-based sliding variables for controlling the orientation of a manipulator's end-effector. Both sliding variables are free of singularities and represent global exponentially convergent error dynamics that do not exhibit unwinding when used in feedback. The choice of sliding variable is dictated by whether the end-effector's angular velocity vector is expressed in a local or global frame, and is a matter of convenience. Using quaternions allows the end-effector to move in its full operational envelope, which is not possible with other representations, e.g., Euler angles, that introduce representation-specific singularities. Further, the presented stability results are global rather than almost global, where the latter is often the best one can achieve when using rotation matrices to represent orientation.

Submodular Optimization for Keyframe Selection & Usage in SLAM

Keyframes are LiDAR scans saved for future reference in Simultaneous Localization And Mapping (SLAM), but despite their central importance most algorithms leave choices of which scans to save and how to use them to wasteful heuristics. This work proposes two novel keyframe selection strategies for localization and map summarization, as well as a novel approach to submap generation which selects keyframes that best constrain localization. Our results show that online keyframe selection and submap generation reduce the number of saved keyframes and improve per scan computation time without compromising localization performance. We also present a map summarization feature for quickly capturing environments under strict map size constraints.

LiDAR Inertial Odometry And Mapping Using Learned Registration-Relevant Features

SLAM is an important capability for many autonomous systems, and modern LiDAR-based methods offer promising performance. However, for long duration missions, existing works that either operate directly the full pointclouds or on extracted features face key tradeoffs in accuracy and computational efficiency (e.g., memory consumption). To address these issues, this paper presents DFLIOM with several key innovations. Unlike previous methods that rely on handcrafted heuristics and hand-tuned parameters for feature extraction, we propose a learning-based approach that select points relevant to LiDAR SLAM pointcloud registration. Furthermore, we extend our prior work DLIOM with the learned feature extractor and observe our method enables similar or even better localization performance using only about 20\% of the points in the dense point clouds. We demonstrate that DFLIOM performs well on multiple public benchmarks, achieving a 2.4\% decrease in localization error and 57.5\% decrease in memory usage compared to state-of-the-art methods (DLIOM). Although extracting features with the proposed network requires extra time, it is offset by the faster processing time downstream, thus maintaining real-time performance using 20Hz LiDAR on our hardware setup. The effectiveness of our learning-based feature extraction module is further demonstrated through comparison with several handcrafted feature extractors.

Dynamic Adaptation Gains for Nonlinear Systems with Unmatched Uncertainties

We present a new direct adaptive control approach for nonlinear systems with unmatched and matched uncertainties. The method relies on adjusting the adaptation gains of individual unmatched parameters whose adaptation transients would otherwise destabilize the closed-loop system. The approach also guarantees the restoration of the adaptation gains to their nominal values and can readily incorporate direct adaptation laws for matched uncertainties. The proposed framework is general as it only requires stabilizability for all possible models.

Multi-Robot Task Assignment and Path Finding for Time-Sensitive Missions with Online Task Generation

Executing time-sensitive multi-robot missions involves two distinct problems: Multi-Robot Task Assignment (MRTA) and Multi-Agent Path Finding (MAPF). Computing safe paths that complete every task and minimize the time to mission completion, or makespan, is a significant computational challenge even for small teams. In many missions, tasks can be generated during execution which is typically handled by either recomputing task assignments and paths from scratch, or by modifying existing plans using approximate approaches. While performing task reassignment and path finding from scratch produces theoretically optimal results, the computational load makes it too expensive for online implementation. In this work, we present Time-Sensitive Online Task Assignment and Navigation (TSOTAN), a framework which can quickly incorporate online generated tasks while guaranteeing bounded suboptimal task assignment makespans. It does this by assessing the quality of partial task reassignments and only performing a complete reoptimization when the makespan exceeds a user specified suboptimality bound. Through experiments in 2D environments we demonstrate TSOTAN's ability to produce quality solutions with computation times suitable for online implementation.

Direct LiDAR-Inertial Odometry and Mapping: Perceptive and Connective SLAM

This paper presents Direct LiDAR-Inertial Odometry and Mapping (DLIOM), a robust SLAM algorithm with an explicit focus on computational efficiency, operational reliability, and real-world efficacy. DLIOM contains several key algorithmic innovations in both the front-end and back-end subsystems to design a resilient LiDAR-inertial architecture that is perceptive to the environment and produces accurate localization and high-fidelity 3D mapping for autonomous robotic platforms. Our ideas spawned after a deep investigation into modern LiDAR SLAM systems and their inabilities to generalize across different operating environments, in which we address several common algorithmic failure points by means of proactive safe-guards to provide long-term operational reliability in the unstructured real world. We detail several important innovations to localization accuracy and mapping resiliency distributed throughout a typical LiDAR SLAM pipeline to comprehensively increase algorithmic speed, accuracy, and robustness. In addition, we discuss insights gained from our ground-up approach while implementing such a complex system for real-time state estimation on resource-constrained systems, and we experimentally show the increased performance of our method as compared to the current state-of-the-art on both public benchmark and self-collected datasets.

Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation

Aligning a robot's trajectory or map to the inertial frame is a critical capability that is often difficult to do accurately even though inertial measurement units (IMUs) can observe absolute roll and pitch with respect to gravity. Accelerometer biases and scale factor errors from the IMU's initial calibration are often the major source of inaccuracies when aligning the robot's odometry frame with the inertial frame, especially for low-grade IMUs. Practically, one would simultaneously estimate the true gravity vector, accelerometer biases, and scale factor to improve measurement quality but these quantities are not observable unless the IMU is sufficiently excited. While several methods estimate accelerometer bias and gravity, they do not explicitly address the observability issue nor do they estimate scale factor. We present a fixed-lag factor-graph-based estimator to address both of these issues. In addition to estimating accelerometer scale factor, our method mitigates limited observability by optimizing over a time window an order of magnitude larger than existing methods with significantly lower computational burden. The proposed method, which estimates accelerometer intrinsics and gravity separately from the other states, is enabled by a novel, velocity-agnostic measurement model for intrinsics and gravity, as well as a new method for gravity vector optimization on S2. Accurate IMU state prediction, gravity-alignment, and roll/pitch drift correction are experimentally demonstrated on public and self-collected datasets in diverse environments.

A Contracting Hierarchical Observer for Pose-Inertial Fusion

This work presents a contracting hierarchical observer that fuses position and orientation measurements with an IMU to generate smooth position, linear velocity, orientation, and IMU bias estimates that are guaranteed to converge to their true values. The proposed approach is composed of two contracting observers. The first is a quaternion-based orientation observer that also estimates gyroscope bias. The output of the orientation observer serves as an input for another contracting observer that estimates position, linear velocity, and accelerometer bias thus forming a hierarchy. We show that the proposed observer guarantees all state estimates converge to their true values. Simulation results confirm the theoretical performance guarantees.

Unmatched Control Barrier Functions: Certainty Equivalence Adaptive Safety

This work applies universal adaptive control to control barrier functions to achieve forward invariance of a safe set despite the presence of unmatched parametric uncertainties. The approach combines two ideas. The first is to construct a family of control barrier functions that ensures the system is safe for all possible models. The second is to use online parameter adaptation to methodically select a control barrier function and corresponding safety controller from the allowable set. While such a combination does not necessarily yield forward invariance without additional requirements on the barrier function, we show that such invariance can be established by simply adjusting the adaptation gain online. It is also shown that the developed method is applicable to systems with safety constraints that have a relative degree greater than one. This work thus represents the first adaptive safety approach that successfully employs the certainty equivalence principle for general state constraints without sacrificing safety guarantees.