=Paper=
{{Paper
|id=Vol-3919/short4
|storemode=property
|title=Coordinate Alignment of the Lidar Mapping System for Tightly Coupled Distance Measurements Based on Graph Optimization
|pdfUrl=https://ceur-ws.org/Vol-3919/short4.pdf
|volume=Vol-3919
|authors=Pengcheng Zheng,Zhitian Li,Wenhao Lei,Xudong Zou
|dblpUrl=https://dblp.org/rec/conf/ipin/ZhengLLZ24
}}
==Coordinate Alignment of the Lidar Mapping System for Tightly Coupled Distance Measurements Based on Graph Optimization==
https://ceur-ws.org/Vol-3919/short4.pdf
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.