LOFF: LiDAR and Optical Flow Fusion Odometry

Abstract

Simultaneous Location and Mapping (SLAM) is a common algorithm for position estimation in GNSS-denied environments. However, the high structural consistency and low lighting conditions in tunnel environments pose challenges for traditional visual SLAM and LiDAR SLAM. To this end, this paper presents LiDAR and optical flow fusion odometry (LOFF), which uses a direction-separated data fusion method to fuse optical flow odometry into the degenerate direction of the LiDAR SLAM without sacrificing the accuracy. Moreover, LOFF incorporates detectors and a compensator, allowing for a smooth transition between general environments and degeneracy environments. This capability facilitates the stable flight of unmanned aerial vehicles (UAVs) in GNSS-denied tunnel environments, including corners and long-distance consistency. Through real-world experiments conducted in a GNSS-denied pedestrian tunnel, we demonstrate the superior position accuracy and trajectory smoothness of LOFF compared to state-of-the-art visual SLAM and LiDAR SLAM.

1. Introduction

With their flexible altitude and immunity to ground obstacles [1], unmanned aerial vehicles (UAV) are currently widely used in inspection [2], search missions [3], and cargo measurements [4]. In most of these applications, UAVs use GNSS as the prime source of positioning information. However, underground or indoor tunnels represent GNSS-denied environments, and their low lighting, featurelessness, and high consistency also make UAV positioning more challenging [5]. Large hydroelectric power stations and air raid shelters often contain extensive long-distance tunnels (as shown in Figure 1) and drainage pipes that require regular inspection to ensure that cracks, water seepage, landslides, etc., can be detected immediately. Considering the presence of stairs and potential deep-water accumulation in these tunnels, UAVs have more application advantages than unmanned ground vehicles (UGVs) and quadruped robots for these inspection tasks. Currently, the positioning methods in the GNSS-denied environment can be briefly classified into two categories: (1) positioning by external landmarks with known location information; (2) positioning by on-board sensors.

For external landmarks, Wi-Fi [6], RF backscatter [7], and LED light [8] are useful sources for UAVs to obtain position estimates in low-lighting environments. However, because of the limited range, these landmarks are only suitable for use in small spaces. Ultra-wideband (UWB) is more suitable for positioning in large-scale GNSS-denied scenes [9], but the positioning accuracy will be affected after the signals pass through walls. In addition, laying a large number of UWB landmarks in long tunnels also creates a high cost. Besides static landmarks, Wang et al. [10] and Chen et al. [11] obtained the global position of the UAV by positioning it relative to a vehicle with known position information, but, in this context, the flight area of the UAV will be limited to the area accessible to the vehicle.

For positioning via the UAV’s own on-board sensors, visual SLAM and LiDAR SLAM are the most common localization algorithms for UAVs in GNSS-denied environments. Visual SLAMs, such as ORB-SLAM3 [12] and VINS-Fusion [13], estimate the position based on tracking the key points in the image. However, due to the low light and high grey consistency in the tunnel environment, the quality of the key points is poor, resulting in missing tracking or the incorrect matching of the key points. Moreover the performance of visual SLAMs will be significantly impacted by the absence of reliable key points. On the other hand, the existing excellent LiDAR SLAMs, such as Faster-Lio [14] and DLIO [15], typically only use LiDAR and IMU for position estimation, which is generally accurate in most environments. However, tunnel-like environments also represent a degeneracy environment for LiDAR SLAM, because the point clouds are highly consistent in each part of the long tunnel.

To this end, we approach a multi-sensor fusion method by adding an optical flow sensor to solve the problem whereby the above traditional SLAMs cannot obtain an accurate position estimate in tunnel-like degeneracy environments (Figure 1). Our contributions are as follows:

• We present a direction-separated data fusion approach that is capable of fusing optical flow odometry in the degeneracy direction of LiDAR SLAM only, ensuring the accuracy of the position estimates of the whole system;
• We present a low-computation failure and recovery detector that enables our data fusion approach to perform fast switching between degeneracy environments and general environments, as well as an offset module to guarantee smoothness during switching;
• The performance of our approach is verified through real-world UAV experiments against the state of the art, as shown in the video https://youtu.be/ITytYCW8y5w (accessed on 21 July 2024).

For benefit of the community, we have provided our code in open-source format.

2. Related Work

Since only one camera is needed as a hardware device, the optical flow method has been widely used in unmanned vehicles. However, due to the optical flow method estimating the distance of movement at the image scale rather than the real-world scale, it is mostly used for target recognition and depth estimation, rather than the vehicle or position estimation of the vehicle itself. For example, OF-VO [16] used LiDAR for position estimation and the optical flow method for pedestrian detection and vehicle estimation to generate an obstacle avoidance navigation path. Jiang et al. [17] and Liu et al. [18] used the optical flow method to generate a pseudo-LiDAR point cloud. In terms of restoring the real-world scale and using the optical flow method for position estimation, this can be achieved by adding it to the network together with LiDAR. FuseMODNet [19] proposes a CNN architecture for the fusion of RGB images, RGB flows, and LiDAR flows to estimate the velocity of dynamic objects. Pandya et al. [20] fused mmWave FMCW radar and optical flow data for velocity estimation. However, due to the limitations of UAVs in terms of payload, it is not suitable to deploy a network that places significant computing costs on UAVs. Therefore, estimating the velocity directly without relying on the network is more suitable for UAV use. Zheng et al. [21] used UAV attitude data and altitude data with optical flow for velocity estimation, but the altitude data were estimated from visual odometry, which is unsuitable for the low-lighting tunnel-like environment. Yun et al. [22] used LiDAR to estimate the distance and flatness of the ground, which was used to restore the position estimate via the optical flow to the real-world scale. Inspired by these approaches, we use the attitude data of a UAV, distance sensor, and camera with an optical flow method for position estimation in the degeneracy direction.

Filters, such as the Kalman filter, are a common tool for multi-sensor data fusion. Premachandra et al. [9] used an extended Kalman filter (EKF) to fuse UWB and radar. Du et al. [23] also used an EKF to fuse multiple heterogeneous sensors to solve the problem of position estimation in multi-environments. Zhen et al. [24] used a Gaussian particle filter to obtain position measurements from UWB distance measurements and fused them with LiDAR through an error state Kalman Filter (ESKF). However, these approaches do not enable targeted fusion in the degeneracy direction of prime sensors, because the sensors are assumed to be uniformly reliable in all directions. Therefore, for a tunnel-like environment, Kim et al. [25] used a point landmark map to locate the lane information of the vehicle and constrained the position estimates of other sensors so as not to deviate from the lane in the lateral direction. In addition, Zhang et al. [26] defined a degeneracy factor to separate degenerate directions in the state space. After this, a nonlinear optimization method was used to solve the problem with the sensor data in well-conditioned directions, avoiding being affected by the errors introduced by the sensor data in the degenerate direction. Hinduja et al. [27] improved this approach and applied it to underwater SLAM with sonar sensors. XICP [28] proposes a fine-grained localizability detection and localizability-aware constrained ICP optimization module for an underground mine-like LiDAR degeneracy environment. However, the code is not available in open-source format yet. Inspired by these approaches, we propose a degenerate direction-separated data fusion method with lower computing costs that is more suitable for tunnel-like degeneracy environments.

3. Method

3.1. System Overview

LOFF is an odometry method that combines LIDAR SLAM and optical flow odometry with the intention of improving the performance of traditional LiDAR SLAM in tunnel-like degeneracy environments. Our approach comprises three modules, as shown in Figure 2. The first module is a low-computation and fast-response failure detector designed to separate the erroneous mutation data produced by LiDAR SLAM in the degeneracy environment to guarantee the flight stability of the UAV (Section 3.3). The second is an odometry fusion module (Section 3.2), which replaces the failed direction of LiDAR SLAM with a single-direction optical flow odometry. Additionally, this module encompasses a direction detector to identify the degeneracy direction. The third module is a recovery detector, used to detect whether the UAV has exited the degeneracy environment so that LiDAR SLAM can be re-used as the primary output of the state. The offset recorder in this module will guarantee UAV stability during switching (Section 3.3).

Our experimental platform is an assembled quadrotor UAV with Pixhawk’s 6C flight control unit, as shown in Figure 3. The LiDAR at the top of it is the LIVOX MID-360, which provides 360-degree non-repetitive scanning point cloud data for LiDAR SLAM. The optical flow module, at the bottom of the UAV, consists of a Benewake TFmini Plus distance sensor, a monocular camera, and an LED light. Depending on the requirements of the experimental environment, this module can be installed either at the bottom or on the side of the UAV. To compare with the visual SLAM, a RealSense T265 was installed at the front of the UAV. Notably, it did not contribute data to our approach. Inside the UAV, we used an Intel i7 computer, running Ubuntu 20.04 LTS and ROS, to process the above sensor data and deploy our approach.

3.2. Direction-Separated Data Fusion

Due to the consistency problem of point clouds along the axial direction in tunnel-like degeneracy environments, the iterative closest point (ICP)-based LiDAR SLAM will eliminate one degree of freedom [28]. Therefore, in contrast to traditional data fusion algorithms (such as the extended Kalman filter [29], graph optimization [30], etc.), our approach directly isolates the odometry data in the degeneracy direction from LiDAR SLAM.

Firstly, a detector identifies the degeneracy direction (the axial direction of the tunnel) so that the position data along this direction can be removed in the next step, as shown in Figure 4. To minimize the computation time, we employ a small detection box centred on the UAV. After this, each LiDAR point within the detection box forms a surface with its K-nearest neighbors (KNN), and their properties (normal and curvature of the surface) are estimated [31]. To mitigate errors introduced by obstacles on the wall, we estimate the normal of the tunnel wall by averaging the normal vectors of the 30 flattest surfaces. The handing deviation angle $\alpha$ (in radians) is computed as follows:

$$\alpha =\pi -arccos\left(\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]·\overrightarrow{n}\right),$$

(1)

where $\overrightarrow{n}$ represents the unit vector in the normal direction of the wall in the UAV body frame. Since the walls are generally perpendicular in tunnels and the pitch and roll angles of our UAV are small during low-speed flight, we omit the projection process $\overrightarrow{n}$ onto the UAV X-Y plane in practical applications. Subsequently, the degeneracy direction can be calculated via

$$q_d=q_b\otimes \left[\begin{array}{c}cos(\alpha /2)\\ 0\\ 0\\ 1\ast sin(\alpha /2)\end{array}\right],$$

(2)

where $q_b$ denotes the quaternion representing the UAV body frame with respect to the world frame, and we define the quaternion $q_d$ as the “degeneration frame” with respect to the world frame. ⊗ is the quaternion multiplication operator, described in [32].

Secondly, we propose a direction-separated data fusion approach to fuse optical flow odometry with LiDAR SLAM. The degeneracy environments of LiDAR SLAM are usually divided into three situations: translational degeneracy, rotational degeneracy, and combined degeneracy [28]. Tunnel-like environments belong to the first type. In such environments, the point clouds, received from LiDAR, are almost the same when the sensor moves along the axial direction. Consequently, both point-to-point ICP [33], and plane-to-plane GICP [34]-based LiDAR SLAM lose accuracy in this direction, as shown in Figure 5. Therefore, unlike traditional data fusion, we consider LiDAR SLAM positioning in the degeneracy direction as interference. The fused position estimate ${\mathbf{P}}_{\mathbf{f}}$ can be calculated by replacing the degeneracy direction $q_d$ from (2) of LiDAR odometry with optical flow odometry via

$${\mathbf{P}}_{\mathbf{f}}=q_{\mathrm{d}}\otimes (({q_{\mathrm{d}}}^{-1}\otimes P_{Li})·\left[\begin{array}{c}1\\ 0\\ 1\\ 1\end{array}\right]+\sum (P_{of}·\left[\begin{array}{c}1\\ 1\\ 0\\ 0\end{array}\right])),$$

(3)

where the homogeneous vectors of optical flow odometry $P_{of}$ in the degeneration frame and LiDAR SLAM odometry $P_{Li}$ in the world frame are defined as

$$\begin{array}{cc}\hfill P_{of}=& {q_d}^{-1}\otimes q_b\otimes \left[\begin{array}{c}1\\ \Delta x_{of}\\ \Delta y_{of}\\ 0\end{array}\right],\hfill \\ \hfill P_{Li}=& \left[\begin{array}{c}1\\ x_{Li}\\ y_{Li}\\ z_{Li}\end{array}\right],\hfill \end{array}$$

(4)

where $x_{Li}$, $y_{Li}$, and $z_{Li}$ represent the position estimates of LiDAR odometry, and $\Delta x_{of}$ and $\Delta y_{of}$ denote the increment in optical flow odometry between samples. As shown in Figure 6, $P_{of}$ is calculated by projecting the optical flow odometry in the degeneration direction. In practical applications, according to the high sampling time and low flight speeds, the trajectory of the UAV can be approximated as the sum of multiple straight lines.

The final section of this module records the offset between the position estimates of LiDAR SLAM and optical flow odometry. LiDAR SLAM and optical flow odometry form a loosely coupled system that operates independently, with their position estimations not interfering with each other. Therefore, because of the different positioning accuracies, even when UAVs follow the same path, their position estimates will have different results, leading to an abrupt change in the position estimate of LOFF when switching the sources of position data. Therefore, the last position estimate of LIDAR SLAM before failure $P_{Li}^{last}$ and the offset between the state estimates of UAV and LiDAR SLAM $\delta P$ are introduced to maintain the smoothness of the UAV’s state estimation during flight. By incorporating these into (3), the position estimate $\mathbf{P}$ can be written as

$$\mathbf{P}=\left\{\begin{array}{cc}{P}_{Li}+\delta P,\hfill & \mathrm{in}\mathrm{a}\mathrm{general}\mathrm{environment}\hfill \\ P_f+\delta P,\hfill & \mathrm{in}\mathrm{a}\mathrm{degeneracy}\mathrm{environment}\hfill \end{array}\right.$$

(5)

where

$$\begin{array}{cc}\hfill P_f=& q_d\otimes (({q_d}^{-1}\otimes P_{Li})·\left[\begin{array}{c}1\\ 0\\ 1\\ 1\end{array}\right]\hfill \\ \hfill +& ({q_d}^{-1}\otimes P_{Li}^{last})·\left[\begin{array}{c}1\\ 1\\ 0\\ 0\end{array}\right]+\sum (P_{of}·\left[\begin{array}{c}1\\ 1\\ 0\\ 0\end{array}\right])).\hfill \end{array}$$

(6)

3.3. LiDAR Odometry Failure and Recovery Detector

Quadrotor UAVs exhibit high dynamic responsiveness and are simultaneously intolerant to collisions; therefore, an accurate and real-time state estimate, especially a velocity and attitude estimate, is important; otherwise, a crash may occur. As mentioned in Figure 5, ICP-based LiDAR SLAMs may produce inaccurate position estimates in degeneracy environments, leading to the loss of control over the UAV’s speed. Hence, a low-computation and fast-response LiDAR odometry failure detector was designed, and the optical flow odometry will dominate the position estimation in the degeneracy direction until a “recovery” signal is received from the recovery detector.

The failure detector operates by assessing the error between the LiDAR odometry and the integration of the IMU. Upon surpassing a predetermined threshold, the detector issues a signal. Considering the error accumulation effect of the IMU [35], we compute the error $P_e$ between two LiDAR odometry frames via

$$P_e=∥P_{Li}^{k+1}-P_{Li}^k-P_{IMU}∥,$$

(7)

where $P_{Li}^{k+1}$ and $P_{Li}^k$ denote the position estimates of LiDAR odometry in the $k+1$ and k frame, respectively. $p_{IMU}$ represents the position integration of IMU measurements between these two frames via [15]

$$\begin{array}{cc}\hfill P_i=& P_{i-1}+V_{i-1}\Delta t+{\displaystyle \frac{1}{2}}{q}_{i-1}a\Delta t^2,\hfill \\ \hfill V_i=& V_{i-1}+q_{i-1}\otimes a\Delta t,\hfill \\ \hfill q_i=& q_{i-1}+{\displaystyle \frac{1}{2}}(q_{i-1}\otimes \omega )\Delta t,\hfill \end{array}$$

(8)

where the quaternion q denotes the IMU body frame with respect to the world frame, while a and $\omega$ represent the linear acceleration and angular velocity measurements of the IMU, respectively. Since the IMU sampling frequency exceeds that of LIDAR odometry, $i=1,\dots ,N$ for N number of IMU measurements between two LiDAR odometry frames. We also experimented with the use of optical flow odometry and the covariance of LiDAR point clouds as the failure detector. However, neither proved as accurate or responsive as the current detector.

To ensure the UAV’s flight safety, we only generate the recovery signal after completely exiting the degeneracy environment. In tunnel-like environments, LiDAR odometry fails due to the absence of walls or obstacles in the degeneracy direction for position estimation. Therefore, we calculate the surface normal vectors of the point clouds [31] both in front of and behind the UAV. After this, if there are enough of those vectors perpendicular to the degeneracy direction (calculated in (2)), the recovery signal is activated.

To summarize, by combining (5) and the detectors mentioned above, the complete procedure of our LiDAR and optical flow odometry fusion approach can be described as in the pseudo-code in Algorithm 1.   

Algorithm 1: LiDAR and optical flow odometry fusion approach

3.4. Optical Flow Odometry

In our approach, optical flow odometry is utilized primarily to estimate the position in the degenerate direction. Tunnel-like environments, characterized by low light conditions and featureless surroundings, represent degeneracy environments for visual SLAM as well (the performance is shown in Section 4.3). Therefore, we use optical flow odometry instead, which can estimate the velocity of UAVs when even a few key points are available. Optical flow odometry employs the Lucas–Kanade method to obtain the pixel motion distances of key points in the image and then utilizes a distance sensor to measure the distance d between the UAV and the wall, as depicted in Figure 7. The position estimate at a real-world scale is given by multiplying the pixel motion distance, distance, and magnification factor:

$$\left[\begin{array}{c}{x}_{of}\\ y_{of}\end{array}\right]=avg(q_{cam}\otimes (d·f·\left[\begin{array}{c}u\\ v\\ 0\end{array}\right])),$$

(9)

where the quaternion $q_{cam}$ represents the UAV body frame with respect to the camera frame, and the magnification factor f can be obtained through calibration. To reduce the interference of noise, a smoothing filter is used in the optical flow estimation of images. In addition, the position estimate in (9) is passed as an observation to the Kalman filter and combined with the IMU data to obtain a more accurate optical flow odometry position estimate.

4. Experiments and Results

This section presents the results of the real-world experiments to verify the feasibility of each module and evaluate the overall performance of LOFF. This work focuses on the feasibility of a UAV flying in GNSS-denied, low-light, and degeneracy tunnel-like environments. However, to the best of our knowledge, there is no public dataset for such environments. Therefore, we conduct experiments and compare them with state-of-the-art SLAM in the real world, rather than relying on open-source datasets. First, we verify the feasibility of the data fusion algorithm in an indoor manual switching experiment. Following this, failure detection experiments are used to evaluate the responsiveness and feasibility of our odometry switching algorithm. Finally, we evaluate the performance of LOFF and compare it with state-of-the-art SLAM algorithms in a long tunnel flight experiment.

4.1. Indoor Manual Switching Experiment

This experiment aims to demonstrate the continuity and smoothness of the trajectory estimated by our approach. To evaluate the performance of our approach under repeated switching between the use of LiDAR odometry only and fused with optical flow odometry, we manually trigger failure and recovery signals (the detectors in Section 3.3 were not operational in this experiment). The trajectory of this experiment is shown in Figure 8. We switch to fused odometry during the straight-line flight of the UAV, whereas, during turns, we rely solely on LiDAR odometry. Although the orientation of the UAV changes several times, our direction-separated data fusion (Section 3.2) can still fuse two odometry instances in the correct direction, and the fused odometry maintains a consistent motion trend with the ground truth. Due to the error accumulation effect and the featureless environment, the accuracy of optical flow odometry is lower than that of LiDAR odometry, which is similar to the ground truth in this experiment. Nevertheless, there are no abrupt changes in the position estimate during the switching process (see in Figure 8) owing to the offset recorder (as detailed in Section 3.2).

4.2. Failure Detection Experiment

To evaluate the performance of the odometry switching algorithm (as detailed in Section 3.3) in a tunnel-like LiDAR degeneracy environment, an experiment was conducted with the UAV in a tunnel that was over 200 m long and free of obstacles. In this experiment, the UAV took off from one end of the tunnel, maneuvered manually using our odometry as a position estimate throughout the flight, and eventually landed after entering the LiDAR degeneracy environment. As shown in Figure 9, our approach uses LiDAR odometry only because the LiDAR can receive the point cloud of the wall, located at the end of the tunnel, and the position estimate was accurate during this situation. However, as the UAV flew further, the point cloud of the wall became sparser, increasing the likelihood of LiDAR odometry position estimation failures in the degenerate direction. At 83.15 s, the failure detector detected an instability in the LiDAR odometry, prompting our approach to switch to fused optical flow odometry and LiDAR odometry. This switch helped to avoid a sudden increase in the position estimate between 83 and 85 s and subsequent fluctuations after 105 s.

4.3. Tunnel Flight Experiment

To comprehensively assess the performance of our odometry and compare it with the state-of-the-art LiDAR SLAM and visual SLAM techniques, we conducted experiments in a real-world long straight pedestrian tunnel located within a hydropower station (in Figure 10B). Due to the lack of a motion capture system or real-time kinematic positioning (RTK), we used landmarks marked at one-meter intervals to obtain the ground truth data, as shown in Figure 10B. The trajectory of the UAV (the green line), positioning with our odometry, in the forward experiment is shown in Figure 10A. The UAV flew through two right-angle corners before entering a straight tunnel over 200 m in length. For the LiDAR SLAM, it flew in a general environment and then gradually transitioned to a degeneracy environment. For the visual SLAM, it flew in intervals between the general environment and the degeneracy environment as the light changed between light and dark (as shown in the lighting conditions in Figure 10A). Comparing the LiDAR SLAM DLIO [15], the LiDAR SLAM Faster-Lio [14], and the Intel RealSense T265 [36], the results of position estimation in the degeneracy direction in the forward experiment and return experiment are shown in Figure 10C,D, respectively.

In the forward experiment, the position estimates of Faster-Lio [14], DLIO [15], and our odometry were close to the ground truth in the initial 75 s. However, the position of visual SLAM [36] was estimated to fail when it first entered the dark environment (25 s). When the UAV flew into the LiDAR degeneracy environment, the position estimate of Faster-Lio [14] involved staying in place or occasionally moving forward by a small distance, because the point cloud data received by the LiDAR were almost the same in each frame. In contrast, the position estimate of DLIO [15] involved continuing to move forward with fluctuations, because it was tightly coupled to the IMU. It is worth noting that the position estimate of our odometry involved moving forward smoothly with the ground truth, because it switched to the fusion of LiDAR odometry and optical flow odometry in the degeneracy environment. At the time of landing (170 s), our odometry had an error of 5 m, compared to 14, 43, and 70 m for DLIO [15], Faster-Lio [14], and visual SLAM [36].

The return experiment was performed in the same pedestrian tunnel but in the opposite direction. Thus, LiDAR SLAM was first in the degeneracy environment and then transitioned to the general environment. Visual SLAM [36] had no position estimate for the first 65 s because it failed to initialize in the dark environment. Similarly, the position estimate of Faster-Lio [14] involved remaining immobile after the UAV operator’s point cloud disappeared and returned to normal after receiving enough point clouds of the wall (98 s). The position estimate of DLIO still involved moving forward with fluctuations until receiving the point cloud of the wall (74 s). Unlike these, our odometry smoothly followed the ground truth as it started with the fused odometry and switched to the LiDAR odometry only later. In the return experiment, the errors of our odometry, DLIO [15], Faster-Lio [14], and visual SLAM [36] were 3, 6, 49, and 60 m, respectively. In these experiments, our odometry outperformed all other algorithms that were compared. Moreover, through the trajectory (in Figure 10A) and the actual flight, it is also proven that our odometry has good performance in straight tunnels and those with corners in the tunnel-like LiDAR degeneracy environment.

5. Conclusions

To enable UAVs to achieve position estimation and stable flight in tunnel-like degeneracy environments, this work presents a LiDAR and optical flow fusion approach (LOFF). The key innovation is that our approach uses a direction-separated data fusion method to compensate for the position estimation of LIDAR SLAM in the degenerate direction with optical flow odometry. In addition, equipped with our detectors and offset recorder, UAVs can stably explore and fly in tunnel-like GNSS-denied environments that include both general and degeneracy environments. We demonstrate the reliability of our approach through real-world flight experiments.

In future work, we are interested in replacing optical flow odometry with visual SLAM, offering greater available information. By harnessing the synergies between visual and LiDAR data, we anticipate achieving enhanced position estimation capabilities, particularly in more intricate environments.