Underground Positioning Algorithm Logic Based on the Fusion of Inertial Navigation and Odometry with Path Correction

In environments such as underground tunnels where GNSS signals are completely unavailable, a standalone Inertial Navigation System (INS) suffers from positioning errors that grow cubically over time, while a standalone odometer is prone to proportional drift caused by wheel-ground interactions like slipping or spinning. Fusing these two modalities—complemented by a path-correction mechanism—forms the core of the autonomous navigation algorithm for underground equipment. This process is not merely a simple aggregation of data; rather, it establishes a closed-loop filtering and inference framework based on a "predict-observe-correct-backtrack" cycle.

A tightly coupled INS/odometer filtering model serves as the cornerstone of the entire algorithm. Moving beyond the traditional "loosely coupled" approach—where each system independently calculates its pose before fusion—this model enables interaction at the level of raw sensor data. Specifically, velocity and heading changes measured by the odometer are treated as external observations and fed into a Kalman filter centered on the INS mechanization. This allows for the real-time estimation and compensation of the gyroscope’s zero-bias and the accelerometer’s constant drift. This tight coupling ensures that even if the odometer fails momentarily due to wheel slip, the system can maintain short-term dead reckoning via the INS; once wheel-speed data becomes reliable again, the system can rapidly correct any accumulated deviation, thereby significantly enhancing positioning continuity.

A path-correction mechanism based on map topology acts as a critical defense against long-term drift. Movement within underground tunnels is not unconstrained; rather, it is strictly limited to a network of tunnel centerlines. Leveraging this prior constraint, the algorithm implements a "virtual track" correction strategy. When the fused positioning result indicates that the equipment has deviated from a known tunnel centerline beyond a predefined threshold, the system does not simply force the coordinates back onto the path. Instead, it employs a gradual approach—combining "heading adjustment" with "lateral constraint"—by recalibrating the heading reference for subsequent INS dead reckoning while incorporating the lateral error component into the filter’s innovation. This allows the estimated pose to converge progressively back onto the correct path. Furthermore, for areas with topological ambiguity—such as circular tunnels or intersections—the algorithm incorporates a confidence assessment for map matching. Lateral trajectory correction is activated only when the matching reliability exceeds a predefined threshold, thereby preventing localization divergence caused by erroneous matches.

A synergistic strategy combining Zero-Velocity Update (ZUPT) and loop-closure detection provides the system with absolute reference anchors. In operational scenarios involving periodic stops, the algorithm automatically identifies stationary intervals and applies ZUPT, resetting velocity observations to zero to suppress the accumulation of velocity errors. For operations involving long-distance round trips or repeated paths, the system leverages the high short-term precision of inertial navigation to construct "trajectory fingerprints." When the current trajectory is spatially identified as closing a loop with a previously recorded path, a loop-closure detection is triggered; a global pose-graph optimization is then employed to perform a comprehensive, smoothed correction of the cumulative errors across the entire path. This mechanism relies exclusively on the intrinsic data characteristics of the inertial navigation system—without requiring any external signals—making it particularly well-suited for deep underground environments.

In summary, the logic behind this underground localization algorithm—which integrates inertial odometry with path correction—addresses the dual challenge of "inevitable divergence in continuous dead reckoning" and "unreliability of external aids." It achieves this through three layers: suppressing sensor-level noise via tightly-coupled filtering, eliminating structural drift through topological constraints, and establishing a long-term reference via self-observed motion. This hierarchical, progressive architecture—relying on sensor fusion for the short term, constraints for the medium term, and loop-closure detection for the long term—collectively ensures the availability and reliability of localization for underground equipment, even under demanding operational conditions.

оставить сообщение

оставить сообщение
Если вас заинтересовала наша продукция и вы хотите узнать больше подробностей, пожалуйста, оставьте сообщение здесь, мы ответим вам как можно скорее.

Дом

Продукты

whatsApp

контакт

КОРЗИНА
item.name
[[ item.product_name ]]
[[ line.text ]]
[[ item.delivery_text ]]
* [[ item.qty ]]
Your Cart Is Empty!