PMLIO: Panoramic Tightly-Coupled Multi-LiDAR-Inertial Odometry and Mapping

: The limited field of view (FoV) of single LiDAR poses challenges for robots to achieve comprehensive environmental perception. Incorporating multiple LiDAR sensors can effectively broaden the FoV of robots, providing abundant measurements to facilitate simultaneous localization and mapping (SLAM). In this paper, we propose a panoramic tightly-coupled multi-LiDAR-inertial odometry and mapping framework, which fully leverages the properties of solid-state LiDAR and spinning LiDAR. The key of the proposed framework lies in the effective completion of multi-LiDAR spatial-temporal fusion. Additionally, we employ the iterated extended Kalman filter to achieve tightly-coupled inertial odometry and mapping with IMU data. PMLIO showcases competitive performance on multiple scenarios data, compared with state-of-the-art single LiDAR-inertial SLAM algorithms, and reaches a noteworthy improvement of 27.1% and 12.9% in max and median of absolute pose error (APE) respectively.


INTRODUCTION
Robots utilize SLAM to construct environmental maps and achieving self-localization (Durrant-Whyte & Bailey, 2006) by own equipped sensors in complex environments such as GNSSdenied environments, while providing a foundation for robot exploration and planning (Cadena et al., 2016).With the advancement of robot technology, SLAM has been widely applied in underground space exploration (Chang et al., 2022) and autonomous driving ( Li et al., 2020).The mainstream SLAM technology commonly relies on sensors such as cameras, LiDARs, and inertial measurement units (IMU).Visual SLAM based on cameras (Campos et al., 2021;Forster et al., 2017) is prone to failure in conditions of low light, rain, and snow.Conversely, LiDAR SLAM (Cong et al., 2022(Cong et al., , 2023;;Zhang & Singh, 2014) relies on long-distance observation and stronger robustness in conditions such as poor lighting and harsh environments.It can not only provide high-precision 6 DOF state estimation, but also obtain high-resolution environmental perception maps.
The evolution of LiDAR SLAM is closely intertwined with the hardware support provided by various types of LiDARs.As LiDAR technology has advanced, the solid-state LiDAR that employs a non-repeating scan mode to generate dense 3D point clouds abstracts more attention , in addition to traditional mechanically spinning LiDAR.(Li et al., 2021;Lin & Zhang, 2020) specifically propose new feature extraction methods for the irregular scanning patterns of solid-state LiDARs.In particular, the omnidirectional non-repetitive Livox Mid-360 LiDAR has a 360-degree horizontal FOV, which is a significant improvement over traditional solid-state LiDAR with narrow horizontal FOVs.Moreover, it is more affordable, making it more suitable for mapping large scenes.
Despite the extensive research on LiDAR-inertial odometry and mapping (Shan et al., 2020;Xu et al., 2022), traditional single * Corresponding author LiDAR SLAM system is susceptible to limitations in terms of perspective, resulting in blind spots and sparse data.In contrast, multi-LiDAR systems have garnered increasing interest due to their ability to overcome the limitations of single LiDAR sensor, providing a more comprehensive and dense mapping solution.(Chen et al., 2021) proposes a backpack-style LiDAR system with two spinning LiDARs.It extracts features from three categories: edges, corners, and planes, which are used for subsequent ground segmentation.(Jiao et al., 2021) proposes a system that achieves online extrinsic calibration, odometry, and mapping for multiple LiDARs simultaneously.(Nguyen et al., 2021) applies angle-complementary solid-state LiDAR and spinning LiDAR in a feature-based tightly coupled LiDARinertial odometry and mapping system.However, prior methods for multi-LiDAR odometry and mapping may not effectively leverage the distinctive capabilities of solid-state and spinning LiDARs, or may be constrained by limitations such as the narrow field-of-view.
To address the problem of limited observation information caused by the solitary perspective, we propose a panoramic tightly-coupled multi-LiDAR-inertial odometry and mapping framework.As depicted in the Figure 1, our method synergistically leverages the perspectives of the Mid-360 LiDAR and Ouster LiDAR while also incorporating the distinctive characteristics of solid-state and spinning LiDAR technologies.This integration empowers the robot to generate a more comprehensive and denser point cloud map.The proposed system first performs the multi-LiDAR spatial-temporal fusion, and it's worth noting that we have performed hardware time synchronization for the two LiDARs beforehand.To compensate for distortion caused by motion, we utilize the high-frequency observation of acceleration and angular velocity from IMU.Subsequently, the LiDAR and IMU measurements are fused using a tightly-coupled iterated extended Kalman filter to achieve high-precision state estimation.
To fully validate our algorithm, we construct a UGV platform (DCSI-LuoJia Explorer) equipped with multiple LiDAR sensors, as shown in Figure 1, and collect diverse experimental data for verification, our algorithm demonstrate competitive performance over state-of-the-art single LiDAR SLAM algorithms.
The main contributions of our work can be summarized as follows: 1.
To overcome the limited perspective of single LiDAR sensor, we propose a panoramic tightly-coupled multi-LiDAR-inertial odometry and mapping framework, which is the framework that combines the Livox mid-360 LiDAR and spinning LiDAR.Through the fusion of complementary characteristics of different LiDARs and IMU, our framework achieves highly accurate and robust odometry estimation.2.
We have validated our algorithm by collecting data on DCSI-LuoJia Explorer equipped with multiple LiDARs, which has demonstrated its superiority over single LiDAR algorithms in terms of enabling more comprehensive and detailed mapping.Moreover, in comparison to the Fast-LIO-Mid, our method has shown a remarkable improvement of 27.1% and 12.9% in max and median of APE respectively.

METHODOLOGY
This section presents a comprehensive explanation of our algorithm, as illustrated in Figure 2.

Overview
To address the limitation of the solitary perspective of single LiDAR, we propose a panoramic tightly coupled multi-LiDARinertial odometry and mapping framework that leverages the characteristics of the Mid-360 solid-state LiDAR with a 360degree horizontal FOV and the spinning LiDAR.Meanwhile, basic hardware time synchronization has been achieved among all hardware components.
Our method starts with the input of multimodal data, and unifies the spatial and temporal reference of multi-LiDAR information.The pre-integrated IMU measurements are then utilized to compensate for the motion during the acquisition.
1 https://github.com/Livox-SDK/livox_ros_driverMoreover, the IMU and fused LiDAR information are incorporated into the iterated extended Kalman filter for state estimation.The following sections will provide detailed descriptions of these modules.

Notations and Definitions：
The following are definitions of the formula symbols involved in this paper.We use ⊟ to parameterize the state error on manifold ℳ.For specific explanations, please refer to Fast-LIO2 (Xu et al., 2022).Additionally, Table 1

Spatial-Temporal Fusion
Since multiple LiDARs are utilized and the LiDAR Points are collected via the point-by-point scanning method, spatialtemporal fusion of multiple LiDARs is required, which consists of four modules: Offline LiDAR Calibration, Temporal Unification, Spatial Unification, and Points Accumulation.

Offline LiDAR Calibration:
In order to unify the spatial reference between LiDARs, external parameter    need to be available, which can be calculated based on the overlapping view of LiDARs in our method.LiDARs acquire maps of the same scene independently and simultaneously.Afterward, fast-gicp algorithm (Koide et al., 2021) is employed to obtain the external parameter    with the overlapped region.To enhance the reliability of the external parameter, the scene should contain abundant features.

Temporal and Spatial Unification:
Due to the inherent hardware variations and differences in data acquisition methods among multiple LiDARs, the collected data may contain temporal offsets.Temporal unification is necessary to achieve accurate data synchronization.
We assume that     = {

Multi-modal LiDAR Pose Estimation
After completing Section 2.2, we obtain the merged point clouds from the LiDAR sensors.Subsequently, we need to integrate the fused LiDAR observations and IMU measurements to achieve tightly-coupled state estimation.This process involves four distinct modules, as shown in Figure 2: Forward propagation, Backward propagation, Residual calculation, and Iterated Update.
We assume that the hardware time synchronization between the LiDAR and IMU has been completed in advance, and that the external parameters between the LiDAR and IMU are known.Based on the definition in Table 1, we denote the first IMU frame as the global frame ( )  .

Forward propagation:
When each IMU constraint   is received, as shown in Figure 3, we perform forward propagation to update the state  � +1 and covariance  � +1 . �  ,  �  represent the propagated state and covariance until    .

Backward propagation and motion distortion:
Since the LiDAR sensor collects points one by one at high frequency, rather than all points at the same time, backward propagation and motion distortion need to be performed on the collected point cloud, as depicted in Figure 3.This can be achieved using the method proposed in (Xu & Zhang, 2021) , which projects all points of the scan to the end time of the scan.

Residual calculation:
After is the raw LiDAR measurement noise,   is a covariance,   is the total measurement noise with   .

Iterated Update:
The propagated state and covariance  �  ,  �  obtained by forward propagation, which inflict the prior distribution on   are as follows: ⊟  �  ∼ (,  �  ) (4) By combining Equations 2 and 4, we can obtain the maximum a posteriori estimate (MAP) for the state   as shown in Equation 5, the solution to which can be found in reference (Xu et al., 2022). (5)

EXPERIMENTS
This section presents the high-quality experimental results of our method in various datasets.

Sensor Configuration and Implementation
To validate our algorithm, reliable and comprehensive experimental data have been collected by our DCSI-LuoJia Explorer system in Figure 1 , which comprises three layers: the bottom layer includes power supplies and computers, while the middle and upper layers contain various sensors.The hardware equipment and their parameters are listed in the Table 2.
In our experiment, we employ two LiDAR sensors: the Ouster1 128 LiDAR, which is a 128-channel spinning LiDAR with a vertical FoV of 45°(± 22.5°), and the Livox mid-360 LiDAR, which is a solid-state LiDAR with a wide horizontal FoV of 360°. Figure 1 shows the positions where the LiDARs are mounted, with both oriented towards the front.Due to the Livox mid-360 LiDAR's larger vertical FOV and solid-state nonrepetitive scanning capability, we mount it above the Ouster1 LiDAR.Compared to traditional single LiDAR system, our approach integrates the characteristics of multimodal LiDAR to achieve a complementary viewpoint and more uniform scanning coverage of the environment.Additionally, both the Ouster1 and Livox mid-360 LiDARs are equipped with a 6-axis IMU.When implementing our proposed algorithm, we use the IMU built into the Ouster LiDAR.The relative external parameters between the IMUs and LiDARs can be obtained through factory settings.The external parameter calibration method between the LiDARs is described in Section 2.2.
To facilitate real-time storage and processing of all data, a portable computer (Intel NUC) is installed on the UGV.Additionally, to assess the accuracy of our algorithm, we equip the UGV with a high-precision GNSS UB482 board and an HG4930 MEMS inertial navigation system.Ground truth is obtained through post-processing using a GNSS/INS fusion method.Moreover, we ensure the reliability of the experimental data by synchronizing all hardware time at the hardware level.

Data Acquisition
We have collected data from two scenarios in the campus to verify our algorithm, and the trajectories are shown in Figure 4.The specific parameters of each scenario are listed in Table 3. Due to obstructions such as buildings and trees during data collection, we only obtain ground truth for Campus1.Therefore, in the experiment, we use Campus1 to evaluate the accuracy of the trajectories, while using data from all scenarios to qualitatively evaluate the mapping results.
We implement the proposed method in C++ with the robot operating system (ROS) on Ubuntu 18.04.In experiment, we don't make use of the GPU hardware of the computer, which is equipped with an Intel(R) i7-11800H @ 2.30GHz CPU and 16GB of memory.We evaluate the accuracy of the experimental trajectory using the evo tool3 .To assess the trajectory accuracy, we cite the absolute pose error (APE) as quantifiable metric.

Evaluation Results
The purpose of this section is to compare the advantages of our algorithm over traditional single LiDAR SLAM in terms of mapping and trajectory accuracy.We modify the state-of-the-art tightly coupled LiDAR inertial odometry algorithm, Fast-lio (Xu et al., 2022), as the single LiDAR SLAM algorithm, which is highly compatible and can be applied to both types of LiDAR.We utilize the IMU built into the LiDARs.In the following analysis, we refer to the use of Fast-lio for mapping with Ouster LiDAR and Mid360 LiDAR as Fast-lio-OS and Fast-lio-Mid, respectively.

Mapping Quality Comparison
The superiority of our method is substantiated by conducting experiments in diverse scenarios, wherein all frames have been meticulously saved without employing any map sampling techniques to ensure the utmost fairness of the experiments.Figure 5 presents the map results, highlighting the differences between each method in detail for each scenario.
In Campus 1, the limited field-of-view of the Ouster LiDAR results in one side of the building facade being unobserved.The map generated by Fast-lio-Mid displays sparse building facades and ground points, with unremarkable step lines.Our proposed method addresses the missing building facade in Fast-lio-OS and adds significant ground details to the map.In Campus 2, Fast-lio-OS exhibits incomplete reconstruction of the building façade.Fast-lio-Mid presents fewer details of the building facade in its map.In contrast, our method significantly enhances the reconstruction of the building facade information.
Through the comparison of map results, it is evident in practical scenarios that the Ouster LiDAR often misses or cannot observe complete building facade information due to its small vertical FOV.Meanwhile, although the Mid360 LiDAR obtains more facade information, its ground points are sparse and cannot represent details near the ground effectively.Our method successfully combines the characteristics of solid-state LiDAR and spinning LiDAR to address the limitations caused by single perspective.

3.2.2
Quantitative Experiment In this section, we conduct a quantitative assessment to compare the performance of our method with single LiDAR-Inertial SLAM.We have selected Campus 1 to evaluate the trajectory accuracy and use APE as the evaluation metric.
Table 4 presents the root mean square error (RMSE), max ,median, and mean of APE for the three methods.Figure 6 illustrates the comparison between estimated trajectories and ground truth in terms of XYZ coordinates , as well as APE.
Our method improves the max and median value of APE by 27.1% and 12.9% respectively, compared to the Fast-LIO-Mid.

Dataset
Campus  4. The results of APE (RMSE/Max/Median/Mean, Meters) Based on the trajectory accuracy, we conclude that our method offers improvements over the single LiDAR approach.Furthermore, map comparisons confirm that our method not only effectively integrates information from multiple LiDARs but also ensures trajectory accuracy.

CONCLUSION
In this paper, we propose a novel tightly-coupled panoramic multi-LiDAR-inertial odometry and mapping framework, which is the framework that combines the Livox mid-360 LiDAR and spinning LiDAR.Our approach effectively utilizes the characteristics of multi-modal LiDARs, significantly expanding the robot's perception FoV.Through a diverse range of experiments, we have demonstrated that our method is capable of generating more comprehensive and densely populated maps with a wider FoV when compared to the single LiDAR approach.In the future, we will focus on feature extraction from multi-LiDAR data to reduce data redundancy and online calibration to ensure precise alignment among multiple sensors.

Figure 1 .
Figure 1.The exhibition for our DCSI-LuoJia Explorer system.Left: the system is collecting data.Right: schematic diagram of hardware composition

Figure 2 .
Figure 2. The pipeline of proposed framework.The overall system consists of data input module, spatial-temporal fusion module and state estimation module.Prior to merging, the point cloud     should be projected onto ( )  by the external parameter    , resulting in the point cloud     .Next, the two segments of point cloud     (distinguished by different colors in the Figure 3 ) are merged into point cloud     , which shares a common time domain.After spatial-temporal fusion, we obtain the merged point cloud      = � 1   ,  2   , …     �.

Figure 4 .Figure 5 .
Figure 4. Experiment sites shown in Google Image

Figure 6 .
Figure 6.The comparison of the XYZ and APE in campus1 .

Table 1 .
defines the coordinate system expressions and other symbols involved in this paper.Notations and Definitions I ,   ∈ (3) The extrinsic matrix of Ouster LiDAR frame w.r.t IMU frame, and Mid360 LiDAR frame w.r.t Ouster LiDAR frame ,  �,  � The ground truth, propagated and updated estimation of state   �,  �  The error between the ground truth  and estimation  � , the estimate of state  in the -th iteration of iKF.  ,   State  at the -th IMU sample time and at the -th LiDAR scan end time achieving the forward and backward propagations, state  �  ,covariance  �  and undistorted point cloud      = Moreover, we utilize the ikd-tree algorithm(Xu et al., 2022) to search five nearest points of  �    in the map to fit a plane, then calculate the normal vector    and centroid    .Furthermore, the LiDAR measurement from the  -th point    can be represented as follow:  =   �  , � ≃   � �   , � +     �   +   =    +     �   +   (2) Where  �   =   ⊟  �   ,    is the Jacobian matrix of   �  ,    � with respect to  �   = 0 .Residual    can be summarized as follow: