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. 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[
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.
UWB IMU
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 p stations as ri, 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 ULR , and the translation vector ULt , 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).
Z1:t =)RX,UL(UL)t P (UL U ri, j i=1:3, j=1:4 PUt /L
=(UL)t P (UL )R, U ri, j i=1:3, j=1:4 • P (UL )R, (UL)t PUt /L XUL XUL (1)
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.
IMU
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.
eR =Log(∆Ri−+11，i RUiI RUjI ) ev
=RiIU ( vUi+I1 − vUiI − g∆ti+1,i ) − ∆vij e p
1 =RiIU pUi+I1 − pUiI − vUiI ∆ti+1,i − 2 gΔti2+1,i − ∆pi+1,i UWB Anchors
Anchors00
Anchors01
Anchors02
Anchors03
UWB Measurement
IMU Integration
Tag00 Tag01 Tag02 UWB Tags
UWB Tags
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 − PUiA − (tUkI + RUkI * ∆tUkI+1,k + RUkI ∆RUkI+1,k * PBjT ) < 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 =
∑ i:=1:4, j=1:3
UWBeki, j +
∑ i:=1:4, j=1:3
UWBeki,+j1 + IMU ek+1,k
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
(2)
(4)
eplane =(RUkL pliidar,k + tUkL − RUkL+1 plijdar,k+1 − tUkL+1)
•nplane • nplane
i lidar,k+1 − tUkL+1)
eedge =(RUkLelidar,k + tUkL − RUkL+1e j
•nplane • nplane − (RUkLeliidar,k + tUkL − RUkL+1e j
lidar,k+1 − tUkL+1)
performed according to the LOAM[
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:
UWB Tag0 UWB Tag1 UWB Tag2 UWB Tag0 UWB Tag1 UWB Tag2
LUpIDWoAsBiRtiTopanogse PErxaitonrirdnFsLaiIccDtUoAWrRB MMeeaasULsuIWuDrerABemRmeenntt Plane Feature
Edge Feature UWB+IMU Prior initial pose
LUpIDWoAsBiRtiTopanogse PErxaitonrirdnFsLaiIccDtUoAWrRB MMeeaasULsuIWuDrerABemRmeenntt Plane Feature
Edge Feature
LIDAR Tk+1
LIDAR Tk UWB+IMU Prior initial pose Frontinitial pose epilane =(RUkL pliidar,k + tUkL − pUj ) • nplane • ni plane eedge =(RUkLelidar,k + tUkL − eUj ) • nplane • nplane − (RUkLelidar,k + tUkL − 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: i:=1:4, j=1:3
UWBeki, j + ∑ eplane + ∑ eedge (8)
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.
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
The ranging site for the UWB here is a square row with a length of 40 at a horizontal height of 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.
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
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.
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[
Error Figure 9: FIM Analysis of Tag Distance (a)Horizontal (b)Vertical Error (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
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.
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. [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: 10041011