Coordinate alignment of the Lidar mapping system for tightly coupled distance measurements based on graph optimization ⋆ Pengcheng Zheng1,2†, Zhitian Li1,2† ,Wenhao Lei1,2∗,† and Xudong Zou1,2,3*,† 1 State Key Laboratory of Transducer Technology, Aerospace Information Research Institute, Chinese Academy of Sciences, Beijing 100190 2 School of Electronic, Electrical and Communication Engineering, University of Chinese Academy of Sciences, Beijing 100049, China 3 Qilu Aerospace Information Research Institute, Chinese Academy of Sciences, Jinan, Shandong 250000 Abstract In this paper, a tightly coupled UWB and LIDAR localization and map building framework is designed. This framework adopts the initialization of IMU and UWB fusion, so that the local coordinates are aligned with the global coordinates. This framework achieves consistent localization and mapping with higher accuracy and modeling of larger scenes. Keywords UWB, LIDAR, Tightly Coupled, Localization 1 1. 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. IPIN 2024: Proceedings of the Work-in-Progress Papers at the 14th International Conference on Indoor Positioning and Indoor Navigation (IPIN-WiP 2024), October 14–17, 2024, Hong Kong, China ∗ Corresponding author. † These authors contributed equally. zhengpengcheng20@mails.ucas.ac.cn (P. Zheng) 0000-0003-2918-1688 (P. Zheng); 0000-0002-0996-0011 (Z. Li); 0000-0002-5347-0124 (X. Zou) © 2024 Copyright for this paper by its authors. Use permitted under Creative Commons License Attribution 4.0 International (CC BY 4.0). CEUR ceur-ws.org Workshop ISSN 1613-0073 Proceedings 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. 2. Methods UWB UWB Optimization Coordinate NLOS Alignment Process IMU IMU Integration Initialization 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 U U r p stations as i , j , and the relative position of UWB tag j in the same coordinate system as j . The UWB ranging between these points can be calculated. The transformation relationship between {L} and {U } is represented by the rotation matrix UL R , and the translation vector U L 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).         P  (UL ) R, (UL )t Z1:t  P  (UL ) R= = , (UL )t U ri , j PUt / L  P  (UL ) R, (UL )t U ri , j  • P  ( L ) R, ( L )t PUt / L         =i 1:3, =j 1:4     =j 1:4  =i 1:3,    U  U    X UL   X UL   X UL   X UL  (1) 2.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. Coordinate alignment LIDAR UWB Tightly Back IMU initialization Optimization Pose UWB Map LIDAR LIDAR UWB Front LIDAR Feature Tracking Optimization Figure 2: System Initialization Process 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. e R Log(∆R i−+11，i RUI = i RUIj ) =ev R iIU ( vUI i +1 − vUI i − g∆ti +1,i ) − ∆v ij  i +1 i 1  =e p R iIU  pUI − pUI − vUI i ∆ti +1,i − gΔti2+1,i  − ∆pi +1,i  2  (2) UWB Anchors Anchors 00 Anchors 01 Anchors 02 Anchors 03 Tag 00 UWB Measurement Tag 01 IMU Integration Tag 02 UWB UWB Tags Tags Figure 3: System Initialization Factor Graph 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. dik, j − PUA i − (tUIk + RUIk * ∆tUIk +1,k + RUIk ∆RUIk +1,k * PBTj ) < 3σ (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: Ek +1,k = ∑ UWB i , j k e + ∑ UWB i , j k +1 e + IMU ek +1,k =i: 1:4, =j 1:3 =i: 1:4, =j 1:3 (4) 2.2. 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:  = e plane ( RUL k i plidar k +1 j k +1 , k + tUL − RUL plidar , k +1 − tUL ) k   •n plane • n plane  k +1 j k +1 =eedge ( RUL k i elidar ,k + tULk − RUL elidar ,k +1 − tUL )  k +1 j k +1 •n plane • n plane − ( RUL elidar ,k + tUL − RUL elidar ,k +1 − tUL ) k i k (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: E=f k , k +1 ∑ UWB i , j ke + ∑ UWB i , j e k +1 + ∑ e plane + ∑ eedge =i: 1:4, =j 1:3 =i: 1:4, =j 1:3 (6) UWB Tag0 UWB+IMU Prior UWB Tag Extrinsic UWB UWB position and LIDAR Measurement initial pose LIDAR LIDAR pose Prior Factor Measurement UWB Tag1 LIDAR Tk+1 UWB Tag2 Plane Feature Edge Feature UWB Anchors UWB Range Tk+1 LIDAR Tk UWB Range Tk UWB+IMU Prior initial pose Figure 4: Schematic diagram of the front-end positioning process 2.3. The Back-end UWB Tag Extrinsic UWB UWB UWB Tag0 position and LIDAR Measurement Front initial pose LIDAR LIDAR pose Prior Factor Measurement LIDAR Tk UWB Tag1 UWB Tag2 Plane Feature Edge Feature UWB Anchors UWB Range Tk Global map Figure 5: Schematic diagram of the back-end positioning process 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 odometry 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:  e plane ( RUL (7) k i , k + tUL − pU ) • n plane • n plane k j = plidar  eedge ( RUL elidar ,k + tUL − eU ) • n plane • n plane − ( RUL = k i k j k i k elidar ,k + tUL − eUj ) When an a priori factor for the initial value of the front-end odometry is added, the back-end optimized function is as follows: (8) Ekb = = ∑ i: 1:4, =j 1:3 e + ∑ e plane + ∑ eedge UWB i , j k 3. 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. 3.1. Numerical analysis validation 3.1.1. UWB Anchor DOP analysis Figure 6: Anchor Position UWBs, as typical gauges for spatial ranging, need to be analyzed for their spatial measurement validity and sources of error. Based on DOP (Dilution of precision), we analyze the sources of uncertainty in UWB 3D localization, aiming at describing the shortcomings of UWBs as localization information, and thus elucidating the implications of fusion. (a) DOP of Anchor Scene (c) DOP of Anchor Scene (b) DOP of Anchor Scene Figure 7: DOP of Anchor Position 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. 3.1.2. 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(a). The numerically analyzed positioning accuracy is shown in Fig.8(b)(c). In this section, the positioning error is divided into vertical and horizontal display in view of the gap between the horizontal and vertical positioning errors. (a) Anchor and Robot Trajectory (b)Horizontal Error Figure 8: Trajectory and FIM Analysis of Tag Number 3.1.3. 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. 3.2. 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 pseudo- ranging 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. Table 1 Comparison of ATE error KITTI 02 KITTI 05 KITTI 07 LIDAR (FLOAM) 2.096m 1.701m 1.115m UWB+LIDAR 1.529m 1.646m 0.678m (a)Horizontal (b)Vertical Error Error Figure 9: FIM Analysis of Tag Distance (a) KITTI_02 comparison (b) KITTI_05 comparison (c) KITTI_07 comparison Figure 10: KITTI position estimation comparison We show the results of global mapping based on KITTI 02/05, as shown in Fig11. The comparison of the positioning error is ATE (absolute trajectory error) and the comparison of the error is shown in the Tabel I. (a)KITTI_05 Point cloud Map (b)KITTI_02 Point cloud Map Figure 11: KITTI MAP Result 3.3. Real Scenario Test RTK UWB Intel NUC Computer 9 axis IMU VLP 16 LIDAR Figure 12: Device Our device is shown in Fig. 12. We conducted two tests of real-time localization and map building in a real scenario with the test environment shown in Fig.13,14. Positioning and seeing effects are shown in the figure. Our method of comparison remains FLOAM. One of the first tests using only LiDAR showed a significant localization failure, which proved the effectiveness of our system. (a) Test 01 (b) Test 02 Figure 13: Mapping and Coordination alignment (a)Test 01 (b)Test 02 Figure 14: Localization Test 4. Conclusion This framework achieves a high level of orientation and map building effectiveness. The next step of the framework needs to be extended to a multi-node localization and graph building system. [1] Shi Q, Zhao S, Cui X, et al. Anchor self-localization algorithm based on UWB ranging and inertial measurements[J]. Tsinghua Science and Technology, 2019, 24(6): 728-737. J. Clerk Maxwell, A Treatise on Electricity and Magnetism, 3rd ed., vol. 2. Oxford: Clarendon, 1892, pp.68–73. [2] Dubé R, Gawel A, Sommer H, et al. An online multi-robot SLAM system for 3D LiDARs[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017: 1004- 1011 [3] Ferrigno L, Miele G, Milano F, et al. A UWB-based localization system: analysis of the effect of anchor positions and robustness enhancement in indoor environments[C]//2021 IEEE International Instrumentation and Measurement Technology Conference (I2MTC). IEEE, 2021: 1-6.. [4] Zhou H, Yao Z, Lu M. Lidar/UWB fusion based SLAM with anti-degeneration capability[J]. IEEE Transactions on Vehicular Technology, 2020, 70(1): 820-830. [5] Wang X, Gao F, Huang J, et al. UWB/LiDAR Tightly Coupled Positioning Algorithm Based on ISSA Optimized Particle Filter[J]. IEEE Sensors Journal, 2024. [6] Queralta J P, Almansa C M, Schiano F, et al. Uwb-based system for uav localization in gnss- denied environments: Characterization and dataset[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020: 4521-4528. [7] Zhang J, Singh S. LOAM: Lidar odometry and mapping in real-time[C]//Robotics: Science and systems. 2014, 2(9): 1-9. [8] Wang H, Wang C, Chen C L, et al. F-loam: Fast lidar odometry and mapping[C]//2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2021: 4390-4396.