3.1. VoxelGrid

LiDAR collects environmental data to create a point cloud and evaluates suitable points by specifying distance criteria. This process involves multiple iterations of the adaptive VoxelGrid to minimize the number of point clouds while preserving the features. However, despite these efforts, noise points may persist owing to factors such as line-of-sight occlusion and the surface materials of obstacles.

3.2. Scan matching

The robot can obtain an accurate position when traveling slowly without violent rotation. However, laboratory environments are complex and unpredictable. Figure 1 shows the mapping results for common laboratories with map offsets in red boxes.

Figure 1.

Defect maps. (a) denotes 9.0 m $\times$ 5.3 m rotated small map, the rotation angle is over $180^\circ$ (i.e., $A$ , $B$ , $C$ , $D$ ) while (i.e., $E$ , $F$ ) is over $90^\circ$ ; (b): 21.2 m $\times$ 8.1 m symmetrical middle map, (i.e., $A$ , $B$ , $C$ , $D$ ) share the same point cloud characteristics; (c): 65 m $\times$ 42 m big unfeatured map. Images a and b with local SLAM while c with global SLAM(back-end).

Figure 1(a) depicts multiple violent rotations, where the robot is prone to losing its position during rotation, leading to an offset in the map, which can be corrected through loop closure. In the symmetric structure environment shown in Figure 1(b), if we turn on the back-end, the map shows erroneous matching. Figure 1(c) illustrates a corridor with a degraded structure. Owing to the lack of features, even with loop closure, an error of 2 ${\rm m}$ $\times$ 2.5 m occurs in the red box. The back-end benefits the accuracy of Cartographer’s maps, but it has limitations in a symmetric structural environment.

3.3. PoseExtrapolator

Table I illustrates the hierarchical algorithm used to prioritize angular and linear velocity data during position renewing. W-Odom is not utilized due to its inferior performance, while high-frequency IMU data will consume substantial computation. The Pose estimation signifies the position data following the last scan matching, and we can also predict the current position by using the data from the preceding frame. However, the position accuracy decreases. If none of these is reachable, Cartographer will use a uniform rectilinear model to estimate.

4.1. Improved framework

We optimize the front-end to enhance the accuracy of robot localization and mapping in unknown environment.

Figure 2.

Framework overview of HT-Carto. In “Sensors,” HT-Carto provides two LiDARs to be selected, they need to match the IMU to get suitable frequency. In “Data preprocessing,” we use a new approach to handle the point cloud. No alterations were made to the local or global SLAM component. In contrast, we developed IMU pre-integration and LiDAR odometry factors to facilitate IMU odometry acquisition through a factor graph. This odometry is then fed into the PoseExtrapolator for the current position updates.

The overall framework of HT-Carto is shown in Figure 2 and comprises two main components. The first component involves a hybrid filter of the point cloud, the second initializes the IMU data statically and dynamically with reference to D-LIOM [Reference Wang, Zhang, Shen and Zhou21]. In theory, the IMU has no bias, and the integral of the angular velocity is ideally equal to the value estimated by the LiDAR odometer. However, in practical application, the external noise and internal bias of the IMU cannot be ignored. To address this problem, the system acquires timestamps from two consecutive LiDAR frames and uses scan matching as observation to constrain the IMU noise and bias.

4.2. Hybrid filter

Preprocessing of the point cloud can reduce the quantity and noise, which improves the quality of the point cloud and reduces the effect of noise in the subsequent scan matching. In the original algorithm, Cartographer employs distance judgment, VoxelGrid, and Adaptive VoxelGrid. VoxelGrid can preserve most environmental characteristics, but its indoor environment is complex. The point cloud emanating from the interstices between the tables and chairs is not conducive to map construction, and is thus classified as a noise point. A RadiusOutlierRemoval was used to remove these points. A hybrid filter was designed, as shown in Figure 2, based on PassThrough to remove a specific range of points. As shown in Algorithm.1 line 3, VoxelGrid achieves down sampling to preserve the geometric features of the point cloud, while RadiusOutlierRemoval filters outlier points.

Algorithm 1:

Hybrid point-cloud filtering for HT-Carto

4.3. IMU pre-integration

An IMU typically comprises a gyroscope and accelerometer, each containing a three-axis angular velocity and three-axis acceleration, respectively. Theoretically, the robot’s position can be determined by integrating the angular velocity and acceleration; however, bias and noise impair the accuracy. Moreover, long-term integration not only accumulates errors but also increases computational resources.

Assuming that the IMU coordinate system is coincident with the robot system $Body(b)$ , the world coordinate system is $World(w)$ , and the z-axis is aligned with the direction of gravity in the $w$ . The physical measurement model of the IMU is as follows [Reference Qin, Li and Shen4]:

(1) \begin{equation} \begin{array}{l} {\widetilde w^b}(t) = {w^b}(t) + {b^g}(t) + {\eta ^g}(t)\\ {\widetilde a^b}(t) = {(R_b^w)^T}(t)\left [ {{a^w}(t) - {g^w}} \right ] + {b^a}(t) + {\eta ^a}(t) \end{array} \end{equation}

In Eq. (1), ${\widetilde w^b}(t)$ and $w^b(t)$ denote the measurement and actual values of the angular velocity in the $b$ system at moment $t$ , ${\widetilde a^b}(t)$ is the measured value of acceleration in the $b$ system, while ${a^w}(t)$ is the true value in the $w$ system. $({R_b^w})^T$ refers to the rotational attitude matrix under the $b$ system relative to the $w$ system. ${b^g}(t)$ and ${b^a}(t)$ are the bias of accelerometer and gyroscope measurements. $g^w$ indicates the gravity of the current area. ${\eta ^g}(t)$ and ${\eta ^a}(t)$ are measurement noises at the time $t$ .

(2) \begin{equation} \begin{array}{l} \mathop {R_b^w}\limits ^ \bullet = R_b^w{({\omega ^b})^ \wedge }\\ \mathop {{v^w}}\limits ^ \bullet = {a^w}\\ \mathop {{p^w}}\limits ^ \bullet = {v^w} \end{array} \end{equation}

Combined with the different form of the kinematic equation in Eq. (2) for further derivation. First, the forward Euler method is applied to obtain the discrete form of kinematics. The discretized integral recursion is then performed on the attitude ( $R$ ), velocity ( $v$ ), and position ( $p$ ) after the $\Delta t$ transformation. To simplify the equations, sign substitution like the Eq. (3) was applied to the $w$ system, and Eq. (4) can be obtained. Where $ {\eta ^{ad}},{\eta ^{gd}}$ denote discrete Gaussian noise. Knowledge of Lie Groups and Lie Algebra [Reference Hashim and Eltoukhy23] was used to derive the formula.

(3) \begin{equation} R(t) \buildrel \Delta \over = R_b^w(t), v(t) \buildrel \Delta \over = {v^w}(t), p(t) \buildrel \Delta \over = {p^w}(t) \end{equation}

(4) \begin{equation} \begin{array}{l} {R_{k + 1}} = {R_k}Exp\left [ {\left ( {{{\tilde \omega }_k} - b_k^g - \eta _k^{gd}} \right )\Delta t} \right ]\\[3pt] {v_{k + 1}} = {v_k} + {R_k}\left ( {{{\tilde a}^k} - b_k^a - \eta _k^{ad}} \right )\Delta t + g\Delta t\\[3pt] {p_{k + 1}} = {p_k} + {v_k}\Delta t + \frac {1}{2}g\Delta {t^2} + \frac {1}{2}{R_k}\left ( {{{\tilde a}^k} - b_k^a - \eta _k^{ad}} \right )\Delta {t^2} \end{array} \end{equation}

The IMU operates at a frequency range of 100–400 Hz. If each data need to be integrated during motion solving, the resulting nodes that are inserted into the factor graph require extensive CPU time. Additionally, the new data are highly correlated with the historical data, leading to repeated calculations. The LiDAR data between two frames (moments $i$ and $j$ ) are selected as observations, and the pre-integration model is utilized to determine the zero bias of the IMU. The variation is subsequently applied to update the IMU data in the next frame, with the aim of reducing the computation. Furthermore, if the radar data are deemed unreliable, IMU pre-integration will provide the initial position of the subsequent scan.

Equation (4) determines the changes in $R,v,p$ over two intervals. However, each calculation must start from its initial value, which requires the construction of the pre-integration term in Eq. (5).

(5) \begin{equation} \begin{array}{l} \Delta {R_{ij}} = {R_i}^T{R_j} = \prod \limits _{k = i}^{j - 1} {Exp\left [ {\left ( {{{\tilde \omega }_k} - b_k^g - \eta _k^{gd}} \right )\Delta t} \right ]} \\ \Delta {v_{ij}} = {R_i}^T\left ( {{v_j} - {v_i} - g\Delta {t_{ij}}} \right ) = \sum \limits _{k = i}^{j - 1} {\Delta {R_{ik}}} \left ( {{{\tilde a}^k} - b_k^a - \eta _k^{ad}} \right )\Delta t\\ \Delta {p_{ij}} = {R_i}^T\left ( {{p_j} - {p_i} - {v_i}\Delta {t_{ij}} - \frac {1}{2}g\Delta t_{ij}^2} \right )\\ = \sum \limits _{k = i}^{j - 1} {\left [ {\Delta {v_{ik}}\Delta t + \frac {1}{2}\Delta {R_{ik}}\left ( {{{\tilde a}^k} - b_k^a - \eta _k^{ad}} \right )\Delta {t^2}} \right ]} \end{array} \end{equation}

For $b$ (bias) and $\eta$ (white noise) in the pre-integration, the value of each individual moment is theoretically unequal. Consequently, the recursion of noise, as described in Eq. (5), is necessary to determine the residuals of $R,v,p$ .

4.4. Factor graph

A factor graph is used for model estimation by transforming the problems involved in solving a joint probability density function and can make full use of the sensor’s information from historical times [Reference Lyu, Wang, Lai, Bai, Liu and Yu24]. Variable nodes represent the quantity to be optimized, such as the robot’s position, whereas factor nodes represent observation constraints, including IMU pre-integration factors and position factors from scan matching. The process involves solving for the optimal value of the objective function, as shown in Eq. (6), which consists of the residuals of each item in the system, and determines the optimal value of the objective function through nonlinear optimization.

(6) \begin{equation} \mathop {\min }\limits _X \left \{{\sum {||{r_L}\left ( {L_k^{k + 1},X} \right )|{|^2} + \sum {||{r_B}\left ( {B_k^{k + 1},X} \right )|{|^2}} } } \right \} \end{equation}

Where $r_L$ denotes the residuals from LiDAR odometry and $r_B$ denotes the residuals after IMU pre-integration.

4.5. Tightly coupled LiDAR/IMU

Both IMU pre-integration and factor graphs are utilized to obtain IMU Odometry. As indicated in Algorithm 2, the data acquired from GTSAM are employed to update the PoseExtrapolator. Initially, through trimming to reduce the data in the cache, the first and last data points in the queue are utilized to calculate the linear and angular velocities. Subsequently, the rotation and translation values in the PoseExtrapolator are updated.

Algorithm 2:

Tightly coupled LiDAR/IMU framework for HT-Carto

5.1. Experimental protocols

(1) Implements:

This study adds two external modules to improve the performance of the Cartographer. All modules in HT-Carto are implemented in C++, which runs in the robot operating system [Reference Carpio, Potena, Maiolini, Ulivi, Rosselló, Garone and Gasparri25] (ROS 1.0) in Ubuntu Melodic. The configurations of the two LiDARs are presented in Table II, and the corresponding experimental scenarios are illustrated in Figure 3 (right). To ensure the replicability of the experiments, numerous datasets are recorded by the Jeston Nano 4 g vehicle platform. These datasets are subsequently transferred to an Intel i7-12 64 g to test HT-Carto, and the experimental results are graphically represented using Matplotlib.

Figure 3.

Experimental environment. The left panel shows the Ackermann car with LiDAR(YDLIDAR X4, RICHBEAM 1). Right is the test environment, which consists a corridor and three symmetry structure laboratories. (a) With the size of 63.2 m $\times$ 2.1 m; (b) 15.8 m $\times$ 7.9 m; (c) 15.7 m $\times$ 7.9 m; (d) 15.6 m $\times$ 7.9 m.

(2) Datasets:

In the Hybrid Filter of HT-Carto test, as shown in Figure 3(b), two lidars were used to record the datasets for a few minutes while the robot remained stationary.

For the localization and mapping of HT-Carto, we used the fourth floor of the laboratory building as the experimental site, as shown in Figure 3(right). To obtain a suitable match between the LiDAR and IMU frequencies and verify the feasibility in a strongly rotating environment, an additional small map is added, as shown in Figure 1(a).

As a uniform note, the parameters in HT-Carto were consistently maintained, and the back-end was disabled. YDLIDAR and RICHBEAM were used as acronyms for their respective radar carts. “Cartographer,” “Loop Closure” marking the mapping results of the local SLAM and global SLAM. The result of Loop Closure was used as a reference trajectory if it could be opened. Verification of HT-Carto’s characteristics was accomplished by examining the similarity and size of trajectories and maps. First, the similarity between Cartographer, HT-Carto, GMapping, Hector, Karto, and Loop Closure trajectories were assessed using dynamic time warping (DTW) [Reference Li, He, Liang, Yang and Han26]. Subsequently, the EVO trajectory evaluation tool [Reference Zhou, Wang, Poiesi, Qin and Wan27] was employed to conduct a comparative analysis of the absolute trajectory error (ATE) and relative pose error (RPE) between trajectories. Finally, several distinct points were marked on each map, the discrepancies between the map and reality were evaluated by comparing the field measurements and utilizing the measurement tool in the RVIZ [Reference Fasiolo, Scalera and Maset28].

5.2. Hybrid filter of HT-Carto

Figure 4 demonstrates the filtering effects of the two LiDARs in diverse environments. Gray serves as the initial point cloud, while green and red represent the refined outcomes. A comparison of the colors demonstrates that the number of individual points is diminished following the Hybrid Filter. As shown in Table III, the point cloud was obtained from different areas: one was small, and the other was large. The filtering results are shown in Figure 4, after several thousand scans of point clouds were tested on both HT-Carto and Cartographer, the number was reduced by 4%, whereas the time was increased by only a few hundred microseconds.

Figure 4.

Hybrid filter results. The horizontal pictures are the same feature environment as shown in Figure 3(b), while the vertical images are the same LiDAR. The left pictures are RPLIDAR X4 and the right are RICHBEAM 1.

5.3. Localization and mapping of HT-Carto

5.3.1. Strongly rotating environment

Table IV lists the experimental findings for the two LiDARs shown in Figure 3(b). The trajectories and maps from the Loop Closure were utilized as reference values for comparison with HT-Carto and others. Five different sensor combinations with varying frequencies were selected for the experiment to evaluate the plane errors and DTW values between trajectories. A smaller DTW value signifies a higher degree of similarity between the trajectories.

The cross-sectional analysis indicates that the errors between HT-Carto and Loop Closure are smaller and more similar than those of Cartographer across all levels, as shown in Table IV. However, the magnitude of the error cannot be used as a criterion for directly selecting the frequency. From a longitudinal comparison of the data, the similarity of the trajectories between HT-Carto and Loop Closure was higher at frequencies of “8 + 100” and “30 + 100”. These two sets of frequencies are selected for further testing. Table V lists the root mean square error (RMSE) results of the ATE and RPE from GMapping, Hector, Cartographer, and HT-Carto which compared to Loop Closure respectively. Except for GMapping, the remaining algorithms exhibited minimal differences and demonstrated excellent performance in map construction within small-scale environments.

Table I.

Velocity selected rules in poseExtrapolator.

Figure 5.

Trajectories and maps (Figure 3b) of the two LiDARs. The red line represents the trajectory of Cartographer which closes the back-end. The blue line is HT-Carto, whereas the yellow line is Cartographer with loop closure, which is close to the true path. (a) notes YDLIDAR, (b) notes RICHBEAM, (c) and (d) represents the Partial Zoom from the left’s dashed circle boxes.

Our findings indicate that the Cartographer and HT-Carto demonstrated satisfactory performance in the trajectory comparison. Furthermore, it is imperative to conduct a comparative analysis of their mapping efficacy. The beginning of trajectories in Figure 5 are highly overlapped and the errors accumulated with the robot’s motion. The deviation of trajectories would be larger if there is no back-end, whereas the high overlap between HT-Carto and Loop Closure indicates that HT-Carto is closer to the real path.

Then, a map size comparison is carried out; six distances are marked in Figure 5, and Table VI shows the results of the real and test values under the two algorithms. Refs. A and B illustrate the full map’s size, which is important than the others.

5.3.2. Symmetry environment

The preceding section demonstrated the effectiveness of HT-Carto in optimizing the front-end. As shown in Figure 3(b), the environment is symmetric; therefore, Loop Closure can not be utilized. Figure 6 depicts the results of the map construction, and the red arrows indicate the three optional marker distances. In a comparison of the true values, HT-Carto reduced the map size error by 4.5 cm in Figure 6(left) and 2.4 cm in Figure 6(right) compared to the Cartographer.

Table II.

Parameters of LiDAR.

Table III.

Point cloud comparison.

Table IV.

Trajectory comparison.

¹ Freq represents the combination of the LiDAR + IMU frequency.

² Standard deviation in the difference of $x$ , $y$ directions, compare trajectory differences between Cartographer (without back-end), HT-Carto and Loop Closure.

Table V.

EVO comparison.

Table VI.

Comparation of the map size.

¹ All units in the table are in cm. Ref: A-F note the red marks from Figure 5b. Assume Optimal=(“Cartographer”- “True”)- (“HT-Carto”- “True”);

² Cartographer, the widths of the table columns are too small.

³ The discrepancy was less than 1 cm and can be considered negligible.

Table VII.

Trajectory comparison of the dynamic environment.

Figure 6.

Symmetry environment mapping results in HT-Carto.

5.3.3. Dynamic environment

The corridor is illustrated in Figure 3(a). Without sufficient feature points, a robot is susceptible to losing its current positional attitude. Consequently, a sensor combination of 30 and 100 Hz from Table IV was selected for testing, which enabled HT-Carto to reduce the error by 10 cm.

To further simulate the authentic laboratory environment, we incorporate a dynamic scenario experiment by placing books on the floor to represent items left behind by students and positioning four persons to ambulate randomly within the environment. The Cartographer drifts significantly in the red box, as shown in Figure 7(a), and HT-Carto performs better in Figure 7(b). Figure 7(c) displays a map generated from the Loop Closure, and HT-Carto clearly performs better on the trajectory. Figure 7(d) and (e) show the details of the map; Loop Closure leads to map overlaps, whereas HT-Carto is more complete in the structural symmetry scene. Table VII illustrates the comparative outcomes of trajectory analysis for the four algorithms. As the environmental area expanded, GMapping became impractical, and the Hector exhibited excessive reliance on the laser data. Cartographer(without back-end) and Karto demonstrated loosely coupled sensor data.

Figure 7.

Dynamic mapping results. The left panel includes three images representing the results of the Cartographer, HT-Carto, and Loop Closure. The red letter $A$ is a corridor and (i.e., $B$ , $C$ , $D$ ) are a symmetry laboratory. Right is the Partial Zoom like the $C$ . The last line shows the maps for both Hector and Karto.