# A Lidar-Inertial Navigation System for UAVs in GNSS-Denied Environment with Spatial Grid Structures
## 2 Lidar-Inertial Navigation System for UAVs in GNSS-Denied Environment with Spatial Grid Structures
### Fast Feature Extraction Algorithm for Spatial Grid Structure  
An algorithm is proposed that can quickly and accurately extract grid features and shell features from point clouds collected by lidar in spatial grid structure environments. Assuming time \(t_{i}\), a frame of point cloud \(\hat{P}\) is obtained by lidar scanning. Because \(\hat{P}\) is usually the point cloud collected by the lidar in continuous time \([t_{i-1},t_{i}]\), it is necessary to first remove the original point cloud's motion distortion to unify all the points in the original point cloud \(\hat{P}\) to time \(t_{i}\). In this way, the measurement error of the sensor can be reduced when solving the pose at time \(t_{i}\) of the navigation system. Since an IMU is used in the navigation system, the method described in [23] is used instead of making the assumption of uniform motion when removing motion distortion:  
1. Calculate the pose prediction of IMU in the two point cloud frames time interval \([t_{i-1},t_{i}]\). 2. According to the scanning time of each point in the original point cloud \(\hat{P}\), interpolate the pose transformation of the point to the lidar frame \(L_{i}\) at time \(t_{i}\). 3. The points are projected to the lidar frame \(L_{i}\) at time \(t_{i}\) through their pose transformation. The lidar point cloud is recorded from which motion distortion has been removed as \(P^{L_{i}}\). Next, the features of the spatial grid structures are extracted from the point cloud after motion distortion removal. We first introduce the method of grid feature extraction. When the UAV is working in an environment full of spatial grid structures, the lidar will detect the environment's three-dimensional information from the building's interior. Generally, the space truss structure is closer to the lidar than the shell structure. In the same line of lidar scanning, the points falling on the grid structure and the adjacent points falling on the shell structure produce a \"V\" shape, as shown in Figure 3.  
Figure 3: Schematic diagram of the grid feature extraction algorithm. It is a horizontal section view of a spatial grid structure containing vertical truss sections. The blue dotted lines are the lights from the lidar scan points. The yellow lines are an illustration of the “V” shape. The \"V\" shape is used to screen the features of the grid on the single-line scanning of the lidar. Denote \(P_{k,j}^{L}\) as the position of the \(j\)th point in the lidar frame \(L\) of the \(k\)th line scan point of a certain frame of lidar. The distance from the point to the center of the lidar is measured as \(r_{k,j}\). The characteristic diameter of the steel frame of the spatial grid structure is set as \(d\). The horizontal angular resolution of the lidar is \(\alpha\). For each scan point \(P_{k,j}^{L}\), the criteria to be extracted as grid features are shown in Equation (1). \[\left\{ \begin{array}{l}L_{e\_th}\leq r_{k,j+m}-r_{k,j}\leq U_{e\_th}\\ L_{e\_th}\leq r_{k,j-m}-r_{k,j}\leq U_{e\_th}\end{array} \right. \tag{1}\]  
where \(L_{e\_th}\), \(U_{e\_th}\), and \(m\) are all threshold values related to the environment. The expression of m is shown in Equation (2). \[m=groudup\left(\frac{d}{\alpha r_{k,j}}\right) \tag{2}\]  
where \(groudup\) is an operator for upward forensic notation. It is ensured by m that the scan points used to extract the grid features have wholly contained the grid structure and have been irradiated on the shell structure behind. The shell feature extraction algorithm component in this paper is the same as the extraction method of plane features in the LOAM [16] algorithm. Calculate the average distance \(k_{k,j}^{L}\) from five points before and after a point \(P_{k,j}^{L}\) on each scan line to this point, as shown in Equation (3). \[k_{k,j}^{L}=\frac{1}{10\cdot\left\|P_{k,j}^{L}\right\|}\sum_{q\in[-5,5],d\in Z,d\
eq 0}\left\|P_{  
Figure 4: Schematic diagram of the shell feature extraction algorithm. (**a**) is a horizontal section view of a spatial grid structure containing vertical horizontal sections.