Pose-Graph-Based UWB SLAM with NLOS Factor Estimation

In indoor environments where global navigation satellite system (GNSS) is not available, the pose estimation algorithm based on ultrawide-band (UWB) signals has advantages, especially in fire or earthquake disaster situation. However, the UWB range distance measurement can be increased due to a non-line-of-sight (NLOS) condition between the UWB anchor and the UWB tag. In this paper, we propose the pose-graph-based UWB SLAM algorithm which is robust in NLOS environments. Adjacent nodes of the 3D pose-graph are connected with relative poses obtained by the IMU pre-integration. The location of the UWB anchor is estimated and designated as a landmark. The constraint between nodes and landmarks are set as the UWB range between the UWB anchor and the robot. The NLOS factor of UWB range is calculated to correct and mitigate the UWB range error. Finally, the pose of the robot is estimated through graph optimization. In simulation experiments, the absolute trajectory error compared with the ground truth is about 0.1 m on average.