Abstract
Autonomous construction machines operating in complex, "dirty" environments face significant challenges in maintaining precise localization due to frequent GNSS signal degradation. This paper presents a robust localization system for autonomous excavators that achieves centimeter-level accuracy and low-latency state estimation. By employing a novel dual-graph factor graph architecture, the system seamlessly transitions between global satellite positioning and local lidar-based odometry, ensuring consistent performance even in GNSS-denied scenarios.
Introduction
Autonomous construction robots, such as walking excavators, are increasingly deployed in environments characterized by tall buildings, dense vegetation, or deep trenches. These conditions frequently block or degrade Global Navigation Satellite System (GNSS) signals. For safe and effective operation, these robots require high-frequency, low-latency updates for real-time control (e.g., chassis balancing) alongside long-term global consistency to prevent mapping drift. Traditional methods like Extended Kalman Filters (EKF) often struggle with the non-linearities and delayed measurements inherent in lidar and GNSS data, while standard factor graph optimization can be computationally prohibitive for high-speed control loops.
Methodology: The Dual-Graph Architecture
The proposed system utilizes a multi-threaded, graph-based prediction-update loop that fuses data from an Inertial Measurement Unit (IMU) at 100Hz, GNSS at 20Hz, and 3D Lidar at 5Hz. The core innovation lies in its Dual-Graph Design:
- Main Graph: This primary graph fuses all available sensor data (IMU, Lidar, and GNSS) to maintain global accuracy.
- Fallback Graph: When GNSS quality drops below a specific covariance threshold, the system activates a fallback graph. This graph utilizes Lidar measurements as "pseudo-global" unary factors, effectively anchoring the robot's position to a local map frame. This prevents the estimated position from "jumping" or drifting significantly when GNSS signals are lost and subsequently regained.
To address latency, the system decouples IMU propagation from the optimization process. An IMU thread provides immediate state estimates for the controller, while a background optimization thread solves the factor graph to periodically correct IMU bias and state.
Experimental Results
The system was validated using real-world data from Menzi Muck walking excavators in two distinct scenarios: a construction task involving earth-moving and a navigation task through urban environments with significant tree cover. Evaluation was performed against high-precision ground truth from Leica RTK-GNSS and RTC360 scanners.
Accuracy and Robustness
In the construction task, the proposed method achieved a mean relative position error of just 0.18 cm. As shown in the vertical translation analysis, the proposed system tracks the ground truth significantly more accurately than baseline methods like MSF or Compslam, particularly during complex maneuvers.
Latency
Despite the computational demands of optimization-based approaches, the system achieved a state-estimation latency of only 41µs. This performance is significantly faster than the Two-State Implicit Filter (102µs) and remains competitive with high-end EKF implementations, making it suitable for high-speed robotic control.
Conclusion and Future Work
The dual-graph approach successfully bridges the gap between high-frequency local control and globally consistent navigation for heavy machinery. By treating lidar as a temporary global anchor during GNSS outages, the system ensures smooth recovery and prevents operational jerks. Future research will focus on online calibration to estimate sensor extrinsics and time-offsets during motion, as well as the integration of joint encoders to improve dead-reckoning in featureless or high-dust environments.
Summary updated