Introduction

Accurate localization of robots in both indoor and outdoor environments is crucial for their automation and intelligence. In many scenarios, GNSS signals can be obstructed, making localization and mapping technologies in GNSS-denied environments significantly valuable. Simultaneous Localization and Mapping (SLAM) is one of the key technologies for addressing localization in environments where GNSS signals are limited, yet it has constraints in global observability. Particularly, when initiated at different positions, SLAM can result in inconsistencies in localization. Furthermore, Ultra-Wideband (UWB) technology, especially systems based on UWB stations, provides consistent observational coordinates and is a vital radio technology for localization [1]. Against this backdrop, this paper proposes a tightly coupled localization and mapping system integrating UWB and LIDAR technologies, leveraging their strengths to achieve more accurate and reliable localization [2] [3]. Under conditions of sufficient computational power, the LIDAR SLAM system demonstrates high stability. Compared to camera-based SLAM systems, LIDAR systems are unaffected by lighting conditions and can extract more robust three-dimensional geometric features. LIDAR SLAM systems typically utilize Iterative Closest Point (ICP) or Normal Distribution Transform (NDT) algorithms to solve for position and orientation. To accelerate the solution speed and enhance the system's robustness to LIDAR point cloud noise, geometric features are commonly extracted based on planes and edges. Moreover, LIDAR SLAM systems typically employ graph optimization or Extended Kalman Filter (EKF) for pose estimation. Under conditions of sufficient computational power, graph optimization can utilize more comprehensive measurement data at various moments, thereby theoretically providing more reliable pose estimates. Accordingly, this paper adopts the graph optimization approach for pose estimation.

UWB or other global observational data can effectively overcome the global unobservability issues inherent in SLAM systems [4]. TOA-based UWB ranging and localization systems have been extensively researched and applied, and the integration of UWB with other measurement data is widely applicable [5] [6]. Specifically, nonlinear optimization of UWB fused with Inertial Measurement Units (IMUs) can utilize IMU measurements to circumvent UWB's Non-Line-of-Sight (NLOS) errors. Therefore, this paper adopts a fusion approach of UWB with IMU to avoid the NLOS issues associated with UWB. Additionally, the tight coupling of LIDAR with UWB can compensate for NLOS issues at the level of feature measurement, thus enabling globally consistent localization and mapping.

The main work of this paper is the development of a tightly coupled localization system integrating UWB ranging data and LIDAR point clouds. The key contributions are as follows:

• First, a coordinate alignment method based on the fusion of LO (Laser Inertial Navigation) and UIO (Ultrawideband Inertial Navigation) measurements is proposed.

• Second, an external parameter alignment combining DOP (Dilution of Precision) and LIDAR features is utilized to maximize the effectiveness of range space measurements.

• Finally, a tightly coupled strategy using multiple UWB tags with LIDAR point clouds leverages spatiotemporal information for global optimization and explores the effectiveness of deploying multiple UWB tags.

Methods

Figure 1: System Overview This paper presents a tightly coupled localization and mapping framework integrating UWB ranging information with LIDAR point cloud data, as shown in Fig. 1. The system employs a soft synchronization method for temporal filtering of UWB and LIDAR data. The proposed system mainly consists of an initialization positioning module combining UWB and IMU, a synchronous front-end processing for UWB and LIDAR, and a fusion positioning and mapping backend that integrates Range with Submap. Additionally, the system has been extended to incorporate a scan-to-map mapping approach.

At time t, a specific point in the point cloud frame collected by the LIDAR SLAM system is represented in the LIDAR coordinate system{L} , with the origin at the start point of the LIDAR SLAM. The UWB measurements are conducted in a point-to-point manner between fixed UWB stations and a mobile UWB tag, requiring at least three pairs of UWB measurements for positional solution. Let's denote the position of UWB tag j in the coordinate system { } U formed by UWB stations as , U i j r , and the relative position of UWB tag j in the same coordinate system as

U j

p . The UWB ranging between these points can be calculated. The transformation relationship between {L} and { } U is represented by the rotation matrix L U R , and the translation vector L U t , known as the external parameters, describing the transformation from coordinate system {L} to { } U . The problem studied in this paper can be represented by equation eq (1).

1: , / , / 1:3, 1:4 1:3, 1:4 ( ) ,( ) ( ) ,( ) ( ) ,( ) ( ) ,( ) L L L L U U U U U t U t t i j U L i j U U L L L L L L L L L U U U U U U i j i j U X X X X P Z P r P P r P R t R t R t R P t = = = =                 = = •                                (1)

Initialization

The initialization of coordinate alignment is conducted through UIO (UWB+IMU combination), solving for the initial position of the robot's body coordinate system within the UWB Anchor coordinate system. The purpose is to unify the spatial representation of LIDAR measurement data with UWB measurement data. The factor graph involved in this initialization process is illustrated in the Fig. 2. The initialization process and the localization and mapping system are loosely coupled. The initialization procedure primarily involves using the Time of Arrival (TOA) from UWB measurements at multiple moments for initialization, along with the use of IMU pre-integration to obtain the measurement model. The handling of NLOS errors primarily involves statistical consistency checks. The initial pose transformations provided by the IMU between two UWB measurements are trustworthy over short periods, and so are the results of their integrations, as shown in Fig. 3. Therefore, erroneous UWB data are filtered out based on consistency checks, and these incorrect data are not used in the initialization or subsequent front-end and back-end processes.

( )1 1 1, 2 1, 1 1 1 1, , Log( ) 1 2i j R i UI i i v IU UI UI i i ij i i i p IU UI UI UI i i i i i i UI i i t t t + + − + + + + + = ∆ = − − ∆ − ∆   = − − ∆ − − ∆     e R R R e R v v g v e R p p v gΔ p ，i(2)1, 1, , ( * * ) 3 k i k k k k k k k j i j UA UI UI UI UI UI BT d P t R t R R P σ + + − − + ∆ + ∆ <(3)

The constraints used for initialization mainly include: the IMU pre-integration factor and the UWB ranging constraints, and the optimization function is as follows:

, , 1, 1 1, : 1:4, 1:3 : 1:4, 1:3 UWB i j UWB i j IMU k k k k k k i j i j E e e e + + + = = = = = + + ∑ ∑ (4)

The Front-end

Based on the results of the coordinate alignment obtained from the initialization, the front-end carries out the measurement fusion of the UWB data with the LIDAR point cloud, and the LIDAR point cloud information is feature-associated by the two frames of the point cloud at two adjacent moments. The handsome selection and association of edge features and planar features are performed according to the LOAM [7] selection method, as shown in Fig. 4. The LIDAR measurement factors are as follows:

1 1 , ,1 1 1 , , 1 1 1 , , 1 ( ) ( ) (+ +  = + − −  • •   = + − −   • • − + − −  (5)

Thus the factors that make up the front-end process are the initialized coordinate alignment a priori factor and the LIDAR feature correlation factor and the UWB ranging factor, and the optimization function is as follows: The initial values of the odometry processed by the front-end process are used by the back-end process to optimize the final results of the and the coherent map building, as shown in Fig. 4. In addition, the measurement information involved in consistent localization and mapping still includes the corresponding UWB measurement information. Therefore, the factors involved in the back-end optimization mainly include the single-frame LiDAR point cloud and the LiDAR factor for matching the map point cloud, as well as the UWB ranging factor:

•   = + − • • − + −   (7)

When an a priori factor for the initial value of the front-end odometry is added, the back-end optimized function is as follows:

Experiments

The experimental validation part of this paper mainly includes numerical analysis and validation for the validity of UWB ranging information, simulation and comparison validation of datasets, and real-time localization and map building test for real scenarios. The ranging site for the UWB here is a square row with a length of 40 𝑚𝑚 at a horizontal height of 1𝑚𝑚 as shown in Fig. 6. In Fig. 7, the distribution of DOPs based on such settings is shown in Fig. 7(a). The error in ranging in this analysis is, and the numerical results show that the main error comes from the horizontal error approximating the height of the UWB. In Fig7(b)(c), the results show that the main source of error comes from the horizontal dissipation of localization information.

Numerical analysis validation

UWB Anchor DOP analysis

UWB Tag number FIM analysis

This part mainly verifies the effect of the number of UWB tags on the positioning accuracy. Without loss of generality, the simulation trajectory adopts the uniform circular motion and the base station arrangement as above. In Fig. 8, the number of UWB tags are 2, 3 and 4, the spacing of tags is 1 meter, and the relative measurement accuracy of the odometers used to connect the two moments is 0.1m and 1°.With this arrangement, the improvement in positioning accuracy when the number of tags exceeds three has little effect, and this subsequent test provides a basis for this. The trajectory of the simulation and the Anchor arrangement are shown in Fig. 8

UWB Tag distance FIM analysis

The distance of UWB tags is also a factor to be explored, the number of UWB tags is 3 the number of base stations is 4 and the base station rows are the same as described above when the distances of UWB tags are 0.5m, 1m, and 2m respectively. the experimental trajectories and the errors of localization are shown in Fig. 9.

The results show that the enhancement for localization is no longer significant at distances greater than one meter for UWB tags.

KITTI Dataset Simulation

The KITTI dataset contains a VELODYNE 64 LIDAR, and we added pseudo-ranging labels to the dataset with the locations (0,0,0),(0,200,0),(20,0,0), (200,200,0). The output frequency of the pseudoranging is the same as that of the lidar. We tested this on KITTI Odometry 02/05/07 and the comparison was FLOAM [8] (only LIDAR). The experimental trajectories and the errors of localization are shown in Fig. 10.

Conclusion