Lidar/UWB Fusion Based SLAM With Anti-Degeneration Capability

This paper presents a Lidar/ultra-wideband (UWB) fusion algorithm for simultaneous localization and mapping (SLAM) with anti-degeneration capability. In the proposed algorithm, the sensor states are estimated by minimizing the sum of the Mahalanobis norm of all measurement residuals associated with the Lidar sensor and the UWB sensors. Additionally, considering that a poor geometric distribution of the sensor measurements will introduce large estimation uncertainties in the fusion algorithm, a trade-off parameter with its self-adjustment strategy is introduced in the fusion algorithm to adjust the ratio of the contributions provided by the Lidar measurement residuals and the UWB measurement residuals in the state estimation according to their degeneration degrees. A method that can detect and determine the degeneration degree of the sensor measurements is also presented in the paper. In order to evaluate the performance of the proposed fusion algorithm, two experiments were carried out. Results of the experiments show that the proposed Lidar/UWB fusion algorithm can automatically adjust the trade-off parameter according to the degeneration degrees of the sensor measurements and mitigate the effects of the degeneration.