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.

Helmet-mounted wearable positioning systems are crucial for enhancing safety and facilitating coordination in industrial, construction, and emergency rescue environments. These systems, including LiDAR-Inertial Odometry (LIO) and Visual-Inertial Odometry (VIO), often face challenges in localization due to adverse environmental conditions such as dust, smoke, and limited visual features. To address these limitations, we propose a novel head-mounted Inertial Measurement Unit (IMU) dataset with ground truth, aimed at advancing data-driven IMU pose estimation. Our dataset captures human head motion patterns using a helmet-mounted system, with data from ten participants performing various activities. We explore the application of neural networks, specifically Long Short-Term Memory (LSTM) and Transformer networks, to correct IMU biases and improve localization accuracy. Additionally, we evaluate the performance of these methods across different IMU data window dimensions, motion patterns, and sensor types. We release a publicly available dataset, demonstrate the feasibility of advanced neural network approaches for helmet-based localization, and provide evaluation metrics to establish a baseline for future studies in this field. Data and code can be found at https://lqiutong.github.io/HelmetPoser.github.io/.

While UWB-based methods can achieve high localization accuracy in small-scale areas, their accuracy and reliability are significantly challenged in large-scale environments. In this paper, we propose a learning-based framework named ULOC for Ultra-Wideband (UWB) based localization in such complex, large-scale environments. First, anchors are deployed in the environment without knowledge of their actual position. Then, UWB observations are collected when the vehicle travels in the environment. At the same time, map-consistent pose estimates are developed from registering onboard self-localization data (from VIO, LIO, and other SLAM methods) with the prior map to provide the training labels. We then propose a network based on MAMBA that learns the ranging patterns of UWBs over a complex, large-scale environment. The experiment demonstrates that our solution can ensure high localization accuracy on a large scale compared to the state-of-the-art. We release our source code to benefit the community at https://github.com/brytsknguyen/uloc.

Ultra-wideband (UWB) is gaining popularity with devices like AirTags for precise home item localization but faces significant challenges when scaled to large environments like seaports. The main challenges are calibration and localization under obstructed conditions, which are common in logistics environments. Traditional calibration methods, dependent on line-of-sight (LoS), are slow, costly, and unreliable in seaports and warehouses, making large-scale localization a significant pain point in the industry. To overcome these challenges, we propose a one-shot calibration and localization framework based on UWB-LiDAR fusion. Our method uses Gaussian processes to estimate the anchor position from continuous-time LiDAR Inertial Odometry with sampled UWB ranges. This approach ensures accurate and reliable calibration with only one round of sampling in large-scale areas, i.e.,$600 \times 450 ~\mathrm{m}^{2}$. With LoS issues, UWB-only localization can be problematic, even when anchor positions are known. We demonstrate that by applying a UWB-range filter, the search range for LiDAR loop closure descriptors is significantly reduced, improving both accuracy and speed. This concept can be applied to other loop closure detection methods, enabling cost-effective localization in large-scale warehouses and seaports. It significantly improves precision in challenging environments where the UWB-only and LiDAR-Inertial methods fail, as shown in the video https://https://youtu.be/oY8jQKdM7lU. We will open-source our datasets and calibration codes for community use.

Commercial UAVs are an emerging security threat as they are capable of carrying hazardous payloads or disrupting air traffic. To counter UAVs, we introduce an autonomous 3D target encirclement and interception strategy. Unlike traditional ground-guided systems, this strategy employs autonomous drones to track and engage non-cooperative hostile UAVs, which is effective in non-line-of-sight conditions, GPS denial, and radar jamming, where conventional detection and neutralization from ground guidance fail. Using two noisy real-time distances measured by drones, guardian drones estimate the relative position from their own to the target using observation and velocity compensation methods, based on anti-synchronization (AS) and an X−Y circular motion combined with vertical jitter. An encirclement control mechanism is proposed to enable UAVs to adaptively transition from encircling and protecting a target to encircling and monitoring a hostile target. Upon breaching a warning threshold, the UAVs may even employ a suicide attack to neutralize the hostile target. We validate this strategy through real-world UAV experiments and simulated analysis in MATLAB, demonstrating its effectiveness in detecting, encircling, and intercepting hostile drones. More details: https://youtu.be/5eHW56lPVto.

Large-scale LiDAR Bundle Adjustment (LBA) to refine sensor orientation and point cloud accuracy simultaneously for building navigation maps is a fundamental task in logistics, intelligent transportation, and robotics. In the context of autonomous delivery and smart mobility, the 3D map obtained by accurate and robust LBA plays a pivotal role in enabling reliable localization and navigation across complex, large-scale urban environments. Unlike pose-graph-based methods that rely solely on pairwise relationships between LiDAR frames, LBA leverages raw LiDAR correspondences to achieve more precise results, especially when initial pose estimates are unreliable for low-cost sensors. However, existing LBA methods face challenges such as simplistic planar correspondences, extensive observations, and dense normal matrices in the least-squares problem, which limit robustness, efficiency, and scalability. To address these issues, we propose a Graph Optimality-aware Stochastic Optimization scheme with Progressive Spatial Smoothing, namely PSS-GOSO, to achieverobust,efficient, andscalableLBA. The Progressive Spatial Smoothing (PSS) module extractsrobustLiDAR feature association exploiting the prior structure information obtained by the polynomial smooth kernel. The Graph Optimality-aware Stochastic Optimization (GOSO) module first sparsifies the graph according to optimality for anefficientoptimization. GOSO then utilizes stochastic clustering and graph marginalization to solve the large-scale state estimation problem for ascalableLBA. We validate PSS-GOSO across diverse scenes captured by various platforms, demonstrating its superior performance compared to existing methods. Moreover, the resulting point cloud maps are used for automatic last-mile delivery in large-scale complex scenes, showcasing the practical benefits of our method in modern intelligent transportation systems. The project page can be found at:https://kafeiyin00.github.io/PSS-GOSO/

Perception plays a crucial role in various robot applications. However, existing well-annotated datasets are biased towards autonomous driving scenarios, while unlabelled SLAM datasets are quickly over-fitted, and often lack environment and domain variations. To expand the frontier of these fields, we introduce a comprehensive dataset named MCD (Multi-Campus Dataset), featuring a wide range of sensing modalities, high-accuracy ground truth, and diverse challenging environments across three Eurasian university campuses. MCD comprises both CCS (Classical Cylindrical Spinning) and NRE (Non-Repetitive Epicyclic) lidars, high-quality IMUs (Inertial Measurement Units), cameras, and UWB (Ultra-WideBand) sensors. Further-more, in a pioneering effort, we introduce semantic annotations of 29 classes over 59k sparse NRE lidar scans across three domains, thus providing a novel challenge to existing semantic segmentation research upon this largely unexplored modality. Finally, we propose, for the first time to the best of our knowledge, continuous-time ground truth based on optimization-based registration of lidar-inertial data on three survey-grade prior maps, each several times larger than the next largest publicly available ones. We conduct a rigorous evaluation of numerous state-of-the-art algorithms on MCD, report their performance, and highlight the challenges awaiting solutions from the research community.

One-shot LiDAR localization refers to the ability to estimate the robot pose from one single point cloud, which yields significant advantages in initialization and relocalization processes. In the point cloud domain, the topic has been extensively studied as a global descriptor retrieval (i.e., loop closure detection) and pose refinement (i.e., point cloud registration) problem both in isolation or combined. However, few have explicitly considered the relationship between candidate retrieval and correspondence generation in pose estimation, leaving them brittle to substructure ambiguities. To this end, we propose a hierarchical one-shot localization algorithm called Outram that leverages substructures of 3D scene graphs for locally consistent correspondence searching and global substructure-wise outlier pruning. Such a hierarchical process couples the feature retrieval and the correspondence extraction to resolve the substructure ambiguities by conducting a local-to-global consistency refinement. We demonstrate the capability of Outram in a variety of scenarios in multiple large-scale outdoor datasets. Our implementation is open-sourced: https://github.com/Pamphlett/Outram.

In response to the evolving challenges posed by small unmanned aerial vehicles (UAVs), which possess the potential to transport harmful payloads or independently cause damage, we introduce MMAUD: a comprehensive Multi-Modal Anti-UAV Dataset. MMAUD addresses a critical gap in contemporary threat detection methodologies by focusing on drone detection, UAV-type classification, and trajectory estimation. MMAUD stands out by combining diverse sensory inputs, including stereo vision, various Lidars, Radars, and audio arrays. It offers a unique overhead aerial detection vital for addressing real-world scenarios with higher fidelity than datasets captured on specific vantage points using thermal and RGB. Additionally, MMAUD provides accurate Leica-generated ground truth data, enhancing credibility and enabling confident refinement of algorithms and models, which has never been seen in other datasets. Most existing works do not disclose their datasets, making MMAUD an invaluable resource for developing accurate and efficient solutions. Our proposed modalities are cost-effective and highly adaptable, allowing users to experiment and implement new UAV threat detection tools. Our dataset closely simulates real-world scenarios by incorporating ambient heavy machinery sounds. This approach enhances the dataset’s applicability, capturing the exact challenges faced during proximate vehicular operations. It is expected that MMAUD can play a pivotal role in advancing UAV threat detection, classification, trajectory estimation capabilities, and beyond. Our dataset, codes, and designs will be available in https://ntu-aris.github.io/MMAUD.

Accurate and consistent construction of point clouds from LiDAR scanning data is fundamental for 3D modeling applications. Current solutions, such as multiview point cloud registration and LiDAR bundle adjustment, predominantly depend on the local plane assumption, which may be inadequate in complex environments lacking of planar geometries or substantial initial pose errors. To mitigate this problem, this paper presents a LiDAR bundle adjustment with progressive spatial smoothing, which is suitable for complex environments and exhibits improved convergence capabilities. The proposed method consists of a spatial smoothing module and a pose adjustment module, which combines the benefits of local consistency and global accuracy. With the spatial smoothing module, we can obtain robust and rich surface constraints employing smoothing kernels across various scales. Then the pose adjustment module corrects all poses utilizing the novel surface constraints. Ultimately, the proposed method simultaneously achieves fine poses and parametric surfaces that can be directly employed for high-quality point cloud reconstruction. The effectiveness and robustness of our proposed approach have been validated on both simulation and real-world datasets. The experimental results demonstrate that the proposed method outperforms the existing methods and achieves better accuracy in complex environments with low planar structures.

LiDAR odometry is a pivotal technology in the fields of autonomous driving and autonomous mobile robotics. However, most of the current works focus on nonlinear optimization methods, and still existing many challenges in using the traditional Iterative Extended Kalman Filter (IEKF) framework to tackle the problem: IEKF only iterates over the observation equation, relying on a rough estimate of the initial state, which is insufficient to fully eliminate motion distortion in the input point cloud; the system process noise is difficult to be determined during state estimation of the complex motions; and the varying motion models across different sensor carriers. To address these issues, we propose the Dual-Iteration Extended Kalman Filter (I2EKF) and the LiDAR odometry based on I2EKF (I2EKF-LO). This approach not only iterates over the observation equation but also leverages state updates to iteratively mitigate motion distortion in LiDAR point clouds. Moreover, it dynamically adjusts process noise based on the confidence level of prior predictions during state estimation and establishes motion models for different sensor carriers to achieve accurate and efficient state estimation. Comprehensive experiments demonstrate that I2EKF-LO achieves outstanding levels of accuracy and computational efficiency in the realm of LiDAR odometry. Additionally, to foster community development, our code is open-sourced.1

Despite recent progress in trajectory planning for multiple robots and a single tethered robot, trajectory planning for multiple tethered robots to reach their individual targets without entanglements remains a challenging problem. In this article, a complete approach is presented to address this problem. First, a multirobot tether-aware representation of homotopy is proposed to efficiently evaluate the feasibility and safety of a potential path in terms of 1) the cable length required to reach a target following the path, and 2) the risk of entanglements with the cables of other robots. Then the proposed representation is applied in a decentralized and online planning framework, which includes a graph-based kinodynamic trajectory finder and an optimization-based trajectory refinement, to generate entanglement-free, collision-free, and dynamically feasible trajectories. The efficiency of the proposed homotopy representation is compared against the existing single and multiple tethered robot planning approaches. Simulations with up to eight UAVs show the effectiveness of the approach in entanglement prevention and its real-time capabilities. Flight experiments using three tethered UAVs verify the practicality of the presented approach. The software implementation is publicly available online.1

In recent years, onboard self-localization (OSL) methods based on cameras or lidar have achieved many significant progresses. However, some issues such as estimation drift and robustness in low-texture environment still remain inherent challenges for OSL methods. On the other hand, infrastructure-based methods can generally overcome these issues, but at the expense of some installation cost. This poses an interesting problem of how to effectively combine these methods, so as to achieve localization with long-term consistency as well as flexibility compared to any single method. To this end, we propose a comprehensive optimization-based estimator for the 15-D state of an unmanned aerial vehicle (UAV), fusing data from an extensive set of sensors: inertial measurement unit (IMU), ultrawideband (UWB) ranging sensors, and multiple onboard visual-inertial and lidar odometry subsystems. In essence, a sliding window is used to formulate a sequence of robot poses, where relative rotational and translational constraints between these poses are observed in the IMU preintegration and OSL observations, while orientation and position are coupled in thebody-offsetUWB range observations. An optimization-based approach is developed to estimate the trajectory of the robot in this sliding window. We evaluate the performance of the proposed scheme in multiple scenarios, including experiments on public datasets, high-fidelity graphical-physical simulation, and field-collected data from UAV flight tests. The result demonstrates that our integrated localization method can effectively resolve the drift issue, while incurring minimal installation requirements.

In recent years, thanks to the continuously reduced cost and weight of 3D lidar, the applications of this type of sensor in the community have become increasingly popular. Despite many progresses, estimation drift and tracking loss are still prevalent concerns associated with these systems. However, in theory these issues can be resolved with the use of some observations to fixed landmarks in the operation environments. This motivates us to investigate a sensor fusion scheme of lidar and inertia measurements with Ultra-Wideband (UWB) range measurements to such landmarks, which can be easily deployed in the environments with minimal cost and time. Hence, data from IMU, lidar and UWB are tightly-coupled with the robot's states on a sliding window based on their timestamps. Then, we construct a cost function comprising of factors from UWB, lidar and IMU preintegration measurements. Finally an optimization process is carried out to estimate the robot's position and orientation. It is demonstrated through some real world experiments that the method can effectively resolve the drift issue, while only requiring two or three anchors deployed in the environment.

In this article, we propose a general graph optimization-based framework for localization, which can accommodate different types of measurements with varying measurement time intervals. Special emphasis will be on range-based localization. Range and trajectory smoothness constraints are constructed in a position graph, then the robot trajectory over a sliding window is estimated by a graph-based optimization algorithm. Moreover, convergence analysis of the algorithm is provided, and the effects of the number of iterations and window size in the optimization on the localization accuracy are analyzed. Extensive experiments on quadcopter under a variety of scenarios verify the effectiveness of the proposed algorithm and demonstrate a much higher localization accuracy than the existing range-based localization methods, especially in the altitude direction.

In this work, we propose a tightly-coupled odometry framework, which combines monocular visual feature observations with distance measurements provided by a single ultra-wideband (UWB) anchor with an initial guess for its location. Firstly, the scale factor and the anchor position in the vision frame will be simultaneously estimated using a variant of Levenberg-Marquardt non-linear least squares optimization scheme. Once the scale factor is obtained, the map of visual features is updated with the new scale. Subsequent ranging errors in a sliding window are continuously monitored and the estimation procedure will be reinitialized to refine the estimates. Lastly, range measurements and anchor position estimates are fused when needed into a pose-graph optimization scheme to minimize both the landmark reprojection errors and ranging errors, thus reducing the visual drift and improving the system robustness. The proposed method is implemented in Robot Operating System (ROS) and can function in real-time. The performance is validated on both public datasets and real-life experiments and compared with state-of-the-art methods.

In this article, we investigate the problem of controlling a multirobot team to follow a leader in formation, supported by a relative position estimate derived from distance and self-displacement measurements, thus waiving the need of external localization infrastructure. The main challenge of the problem, which is to simultaneously fulfill both relative localization and control tasks, is efficiently and novelly resolved by embedding a distance-displacement-based persistently excited adaptive relative localization technique into a time-varying formation with bounded control input (PEARL-TVF). By assuming that the leader is globally reachable and by selecting proper parameters, it is shown that the PEARL-TVF ensures exponentially convergent localization, which leads to exponentially convergent formation when the leader's behavior is deterministic, and bounded formation error for a nondeterministic leader. Numerical simulations and experiments on quadcopters are provided to verify the theoretical findings.

Though vision-based techniques have become quite popular for autonomous docking of Unmanned Aerial Vehicles (UAVs), due to limited field of view (FOV), the UAV must rely on other methods to detect and approach the target before vision can be used. In this paper we propose a method combining Ultra-wideband (UWB) ranging sensor with vision-based techniques to achieve both autonomous approaching and landing capabilities in GPS-denied environments. In the approaching phase, a robust and efficient recursive least-square optimization algorithm is proposed to estimate the position of the UAV relative to the target by using the distance and relative displacement measurements. Using this estimate, UAV is able to approach the target until the landing pad is detected by an onboard vision system, then UWB measurements and vision-derived poses are fused with onboard sensor of UAV to facilitate an accurate landing maneuver. Real-world experiments are conducted to demonstrate the efficiency of our method.

A model-free localization method with noisy distance measurement is proposed for estimating a moving robot in 3D space. Considering that the traditional filter-based sensor network localization algorithms can not provide acceptable estimation accuracy in altitude in 3D space, the proposed method utilizes not only current measurements but also previous measurements to localize a robot. This character adds more constraints to localization to avoid local minimum. In addition, different from the traditional filter-based localization methods which need kinetic model for localization, our proposed method is model-free and converts the localization problem to graph optimization problem. The advantage is that we avoid the possible estimation error caused by inaccurate or simplified kinetic model. Considering that the communication limitation in application makes many graph optimization theories such as distributed localization theory and trilateration difficult to be realized, our method proposes to add constrained equation between adjacent positions to solve this problem. Experiments under a variety of scenarios verify the stability of this method and show that the algorithm achieves better localization accuracy than filter-based methods.

As recalling an Unmanned Aerial Vehicle (UAV) after completing a mission requires quite a lot of attention and skill from its operator, in this paper we propose a method to empower UAV with the capability to autonomously return to base and perform precision landing after completing a mission. The main challenge being tackled in this work is that while the vision-based landing technique is already mature, due to GPS error, UAV can only return to within several meters of home position after completing a mission and may fail to detect the visual marker. To resolve this problem, we employ Ultra-wideband (UWB) ranging measurements to localize and approach the home station. Once the UAV detects the visual marker, both UWB and visual tracking information are fused with onboard sensor to achieve even more accurate positioning. Real-life experiment is used to demonstrate the efficacy of the proposed scheme.

In this paper we propose a method to achieve relative positioning and tracking of a target by a quadcopter using Ultra-wideband (UWB) ranging sensors, which are strategically installed to help retrieve both relative position and bearing between the quadcopter and target. To achieve robust localization for autonomous flight even with uncertainty in the speed of the target, two main features are developed. First, an estimator based on Extended Kalman Filter (EKF) is developed to fuse UWB ranging measurements with data from onboard sensors including inertial measurement unit (IMU), altimeters and optical flow. Second, to properly handle the coupling of the target's orientation with the range measurements, UWB based communication capability is utilized to transfer the target's orientation to the quadcopter. Experiments results demonstrate the ability of the quadcopter to control its position relative to the target autonomously in both cases when the target is static and moving.

Robust velocity and position estimation is crucial for autonomous robot navigation. The optical flow based methods for autonomous navigation have been receiving increasing attentions in tandem with the development of micro unmanned aerial vehicles. This paper proposes a kernel cross-correlator (KCC) based algorithm to determine optical flow using a monocular camera, which is named as correlation flow (CF). Correlation flow is able to provide reliable and accurate velocity estimation and is robust to motion blur. In addition, it can also estimate the altitude velocity and yaw rate, which are not available by traditional methods. Autonomous flight tests on a quadcopter show that correlation flow can provide robust trajectory estimation with very low processing power. The source codes are released based on the ROS framework.

In this paper we study the distance-based docking problem of unmanned aerial vehicles (UAVs) by using a single landmark placed at an arbitrarily unknown position. To solve the problem, we propose an integrated estimation-control scheme to simultaneously achieve the relative localization and navigation tasks for discrete-time integrators under bounded velocity: a nonlinear adaptive estimation scheme to estimate the relative position to the landmark, and a delicate control scheme to ensure both the convergence of the estimation and the asymptotic docking at the given landmark. A rigorous proof of convergence is provided by invoking the discrete-time LaSalle's invariance principle, and we also validate our theoretical findings on quadcopters equipped with ultra-wideband ranging sensors and optical flow sensors in a GPS-less environment.

This paper proposes an ultra-wideband (UWB) aided localization and mapping system that leverages on inertial sensor and depth camera. Inspired by the fact that visual odometry (VO) system, regardless of its accuracy in the short term, still faces challenges with accumulated errors in the long run or under unfavourable environments, the UWB ranging measurements are fused to remove the visual drift and improve the robustness. A general framework is developed which consists of three parallel threads, two of which carry out the visualinertial odometry (VIO) and UWB localization respectively. The other mapping thread integrates visual tracking constraints into a pose graph with the proposed smooth and virtual range constraints, such that a bundle adjustment is performed to provide robust trajectory estimation. Experiments show that the proposed system is able to create dense drift-free maps in real-time even running on an ultra-low power processor in featureless environments.