Hierarchical Constraint Fusion for Robust 3D Rigid Body Localization Using UWB Networks

Ultra-wideband rigid body localization (RBL) represents a pivotal technology for achieving high-precision indoor localization. However, it encounters significant challenges, particularly non-line-of-sight (NLOS) propagation errors and reduced vertical observability in complex environments, both of which considerably undermine the robustness of existing three-dimensional(3D) RBL techniques. To mitigate these challenges, this paper introduces a novel, robust 3D-RBL methodology based on hierarchical constraint fusion, referred to as HC-RBL. This approach enhances performance with a three-tier optimization framework: 1) at the sensor layer, the Particle Swarm Optimization algorithm is used to derive the globally optimal geometric configuration of sensor anchor nodes and tag nodes, thereby maximizing spatial observability; 2) at the signal layer, a hybrid robust M-estimation coupled with multiple outlier detection techniques is leveraged to effectively suppress the localization error; 3) at the trajectory layer, the Rauch-Tung-Striebel smoothing algorithm is applied, incorporating rigid-body kinematic constraints to ensure the physical consistency of 3D motion trajectory. The dataset was obtained from indoor wheeled robot experiments, covering approximately 500 m of trajectory over 16 min. In indoor NLOS environments, with a ranging accuracy of 0.31 m, experimental results from multiple wheeled robot localization tests demonstrate that HC-RBL significantly outperforms conventional RBL methods, achieving an 76% reduction in root mean square error to 0.103 m and an 80% reduction in the 95% cumulative error to 0.182 m. The proposed HC-RBL method exhibits remarkable robustness in complex indoor environments against pronounced NLOS effects.