TL;DR
This paper introduces a flexible graph optimization framework for robot localization that can handle various sensor measurements with different timing. Focusing on range-based localization, it achieves significantly higher accuracy, especially in the altitude direction, compared to existing methods, by jointly optimizing range and trajectory smoothness constraints within a sliding window.
What Problem Does This Paper Address?
Accurate and reliable robot localization is crucial for many applications, from formation flying to target search. Traditional methods like vision-based SLAM can suffer from drift and high computational costs, while WiFi localization offers low accuracy. Optical motion capture systems are expensive and limited in space. Ultra-wideband (UWB) technology offers a promising alternative due to its robustness, but existing UWB localization algorithms have limitations: they often require synchronized multi-channel measurements, accurate kinematic models, or struggle with accuracy in the altitude dimension. This paper aims to address these limitations by proposing a more general and robust range-based localization approach.
Methods
The core of the proposed solution is a general graph optimization framework. This framework represents the robot's state and measurements as a graph, where nodes are robot poses and edges are constraints derived from sensor measurements. The key innovation lies in how it handles range-based localization:
- Graph Representation: The robot's trajectory is modeled as a graph where nodes represent poses (position and orientation) and edges represent constraints.
- Range and Trajectory Smoothness Constraints: The framework explicitly incorporates two types of constraints:
- Range Constraints: These are derived from UWB distance measurements between the robot and fixed anchors. The paper uses a pseudo-Huber loss function to handle potential outliers.
- Trajectory Smoothness Constraints: These enforce that consecutive robot poses should be physically plausible, preventing abrupt jumps. This is achieved by relating adjacent poses using a constraint based on maximum velocity and time intervals, also employing a pseudo-Huber loss.
- Sliding Window Optimization: To ensure real-time performance, the optimization is performed over a sliding window of recent poses rather than the entire trajectory. This limits the computational burden.
- Optimization in Euclidean Space: For range-only localization, the cost function is optimized in Euclidean space using the Levenberg-Marquardt algorithm.
- Optimization in Non-Euclidean Space: For range-orientation based localization, the framework extends to non-Euclidean spaces to handle rotations, mapping poses to their corresponding vector representations.
- Outlier Rejection: A mechanism is included to identify and reject range measurements that are likely outliers, further improving robustness.
Datasets
The experiments were conducted using a quadrotor UAV equipped with a UWB module, IMU sensor, and Pixhawk Autopilot. The ground truth for localization accuracy was provided by a VICON system, which offers millimeter-level accuracy. Experiments were performed in both indoor (6m x 6m x 3m) and outdoor (6m x 8m x 5m) environments. The paper does not introduce new datasets but rather uses these controlled environments to evaluate the proposed algorithm against existing methods.
Key Insights and Ideas
- Unified Framework: The graph optimization approach provides a flexible way to integrate various sensor types and measurement timings, making it adaptable to different localization scenarios.
- Altitude Accuracy Breakthrough: A significant finding is the dramatically improved localization accuracy in the altitude (z) direction, a common challenge for existing range-based methods. This is achieved without requiring additional altitude sensors or special anchor placements.
- Reduced Dependencies: The method effectively removes the need for accurate kinematic models and concurrent multi-channel range measurements, simplifying system requirements.
- Real-time Viability: The combination of sliding window optimization and efficient algorithms makes the approach suitable for low-power processors and real-time applications.
- Robustness to Outliers: The inclusion of outlier rejection mechanisms and the use of robust loss functions contribute to stable performance even in challenging environments.
Key Results
- High Accuracy: The proposed range-only and range-orientation based localization methods achieved significantly higher accuracy compared to existing methods in both 2D and 3D space.
- In 2D, the mean error was 3.1 cm and RMSE was 3.3 cm.
- In 3D, the mean error was 8.3 cm and RMSE was 8.1 cm.
- Specifically, the accuracy in the z-direction was around 7.7 cm, a notable improvement.
- Comparative Performance: Tables III and IV show that the proposed method outperforms existing EKF, MHE, MDS, and machine learning-based methods by a considerable margin, often by more than 3 times.
- Computational Efficiency: The algorithm can run in real-time on low-power processors like Intel Atom x7-Z8750 and ARM Cortex-A53, with running times well within the UWB sensor's frequency limits.
- Parameter Sensitivity: Experiments showed that good localization accuracy can be obtained with a window size N > 5 and a product of iterations and window size (M*N) < 2000.
Limitations and Future Work
- Non-Convexity: The cost function is non-convex, meaning there's a possibility of converging to a local minimum. The authors suggest that a larger window size can help mitigate this.
- Ground Truth Dependency: The evaluation relies on a VICON system for ground truth, which is limited in its operational area.
- Future Work: The paper implicitly suggests that the general framework can be extended to fuse other sensor modalities beyond orientation, and further exploration of parameter tuning for optimal performance across various scenarios would be beneficial. The robustness and smoothness demonstrated in the experiments also open avenues for more complex autonomous tasks.
Summary updated