Author: GPS World Staff

Airborne lidar/INS/GNSS: Algorithm Uses Fuzzy Controlled Scale Invariant Feature Transform

Sensor role reversal: Lidar with its superior performance can replace GNSS in the integration solution by providing fixes for the drifting inertial measurement unit (IMU). Tests show its potential for terrain-referenced navigation due to its high accuracy, resolution, update rate and anti-jamming abilities. A novel algorithm uses scanning lidar ranging data and a reference database to calculate the navigation solution of the platform and then further fuse with the inertial navigation system (INS) output data.

Recent rapid advances in laser-based remote sensing technologies, including pulsed linear, array and flash lidar systems, have fostered the development of integrated navigation algorithms for lidar and inertial sensors. In particular, trajectory recovery based on lidar point-cloud matching can provide valuable input to the navigation filter. Lidar/INS integrated navigation systems may provide continuous and fairly accurate navigation solutions in GNSS-challenged environments, on a variety of platforms, such as unmanned ground vehicles, mobile robot navigation and autonomous driving.

In the case of airborne lidar/INS applications, the free inertial navigation solution is used to create the point clouds, which are subsequently matched to a digital terrain elevation model (DEM). The results are fed back to the platform navigation filter, providing corrections to the free navigation solution. This solution may be used to recreate the point cloud to obtain better surface data.

However, depending on the lidar data acquisition parameters, INS drift during the time between the two epochs when point clouds are acquired could be significant. Besides the shift in platform position, the drift in attitude angles could more severely impact point-cloud generation, producing a less accurate point cloud and subsequently poor matching performance.

This article describes a new lidar positioning approach, where the scale-invariant feature transform (SIFT)-based lidar positioning algorithm is used to match between the lidar measured point cloud and the reference DEM. The matching process is aided with fuzzy control: SIFT-based lidar positioning algorithm with Fuzzy logic (SLPF), where the threshold for SIFT is adaptively controlled by the fuzzy logic system.

Based on the geometric distribution and the range difference variance of the matched point clouds, fuzzy logic is applied to calculate the threshold for the SIFT algorithm to extract feature points; thus the optimal matched point cloud is extracted in several iterations. When there are enough matched points in the final output of the SLPF, the platform position is calculated by using the least squares method (LSM). Next, for trajectory estimation, when applying the SLPF algorithm, frequent lidar updates can be used to correct small cumulative errors from the INS sensor measurements. A Kalman filter fuses the results of the SLPF algorithm with the INS system.

This integrated algorithm can handle situations when there are less than three matched feature points being extracted by the SLPF algorithm, and yet they could still contribute to obtain a better navigation solution. Simulation results show that, compared to the existing algorithms, the proposed lidar/INS integrated navigation algorithm not only improves the position, speed and attitude-determination accuracy, it also makes the lidar less dependent on INS, which makes the navigation system work longer without exceeding a particular drift threshold.

LIDAR ALGORITHM

To eliminate the influence of INS error on the lidar positioning system, instead of creating a measured DEM based on INS ortho-rectification, we directly map the range data measured by lidar to the local stored DEM data. If a successfully matched feature point can be obtained, it means that we can get a point with absolute position and relative range towards the platform, which is similar to the satellite in GNSS positioning. After scanning of one area by lidar, when three or more such matched feature points, if not on a line, can be obtained, then we are able to form a full rank equation with the unknown variables of the platform position x, y and z.

However, due to the effect of affine transformation, the standardized range dataset collected by lidar is significantly different from the elevation dataset belonging to the same area. Figure 1 shows an example of the large difference between the two datasets from the same area when the pitch angle of the platform is equal to 5° and the flying height is 2,000 m. In this situation, the traditional flooding algorithm or constellation feature point matching algorithm is incapable of extracting matched feature points from such different datasets.

In response, we introduce the SIFT algorithm to the elevation map-matching procedure. Designed for image matching, the SIFT algorithm is invariant to scale, rotation and translation, and it is robust to affine transformation and three-dimensional projection transformation to a certain extent. Although SIFT is often used in image matching, each pixel from the image is a numerical point, which, in fact, has no difference with elevation data point. Before applying the SIFT, some processing on the lidar measured range data must be done.

LIDAR RANGE DATA

The scanning information of the lidar measured points are (α, β, r), where α is the angle between the laser beam and the negative Z-axis of the platform body frame, β is the angle from the laser beam to the plane of axis and Z-axis in body frame, r is the range between the laser head and the measured target, as shown in the opening figure.

Due to the terrain relief, the lidar range data are irregularly spaced. Therefore, it is necessary to interpolate the collected data. Here we apply the Natural Neighbor Interpolation method.

SIFT Algorithm, Fuzzy Control. For the lidar positioning algorithm, which is based on the absolute position and relative range of the ground-matched feature points, a point cloud with sufficient number of points of good geometric distribution is needed. In practice, however, the terrain undulation and the attitude of the airplane will affect the quality of the point cloud and the accuracy in the matching process. In addition, the selected threshold in the SIFT algorithm plays an important role on the quality of the matched point cloud.

A Monte Carlo simulation, shown in FIGURE 2, illustrates the impact of the threshold on the number of successful matched points (normalized) and mismatched rate. For obtaining better matched point clouds, we have introduced a SIFT terrain matching algorithm assisted by fuzzy control, as shown in FIGURE 3.

The algorithm mainly consists of two fuzzy logic controllers. Controller 1 calculates the initial threshold for the SIFT algorithm according to the gridded SR data terrain undulation degree λ, and the angle Θ between Z-axis in body-frame and Z-axis in navigation frame.

Controller 2, which is responsible to adaptively changing the threshold at each epoch, has two inputs. The first one is the Normalized Points Area (NPA), which represent the geometric condition of the matched point cloud. The other one is the Relative Range Difference Variance, which indicates if a mismatch has happened. When the final matched feature point cloud is obtained, and the number of points is greater than or equal to 3, then the LSM is used to calculate the position of the platform.

INS/LIDAR NAVIGATION

Loosely and tightly coupled integration are the most common methods in navigation systems. Given the characteristics of the proposed positioning algorithm, the classical integrated navigation algorithm needs to be modified. In the loosely coupled approach, the lidar is unable to aid INS when flying through a flat region and/or flying with a large tilt angle, because the proposed lidar positioning method may have difficulty in extracting enough matched points to calculate a position.

In the tightly coupled method, as the output frequency of matched point cloud is low and the geometry of the matched feature points is relatively poor, the integrated system may be extremely unstable. Here we propose a combined loosely and tightly (CLT) integrated navigation algorithm that when the lidar positioning algorithm can extract enough matched points for a navigation solution, the lidar-calculated navigation solution is used as the main observation.

However, when the matched points are not sufficient to obtain a navigation solution, the baseline vector of the matched point that is closer to the projection of the platform center to the surface will be utilized as the observation. In this solution, lidar can still provide a certain degree of aid to the INS, once extracting matched feature points, even if less than 3.

SIMULATION ANALYSIS

In the simulation experiment, the 3D DEM data of 0.5-meter resolution is obtained from an open source named EOWEB. Then the DEM data is resampled to a higher resolution of 0.1 meter, which is used to generate the simulated, irregularly spaced, measured range data. On the basis of the original DEM (0.5 meter resolution), the proposed lidar positioning algorithm and lidar/INS integrated navigation algorithm are verified and compared with the traditional methods.

Simulation of Lidar Algorithm. As shown above, the successfully matched points rate is very important for positioning, as once a mismatched point occurs, it may lead to a faulty navigation solution. In the simulation, the proposed SLPF is simulated under the condition of different aircraft tilt angle ϴ, from 0° to 10° with a step of 1° , at 5,000 different positions, which is the same simulation condition as in Figure 2. Comparison is made with the traditional constellation feature matching based lidar positioning algorithm (CLP) and the SIFT based lidar positioning algorithm without fuzzy control (SLP). The successfully matched points rate and the NPA value are shown in Figure 4.

As can be seen from the figure, along with the increasing platform attitude angle, the successfully matched points rate of all the three algorithms has declined. However, compared to the CLP, both SIFT-based algorithms have a higher success matching rate due to the more stringent feature-point extraction approach. And due to the adjustable threshold mechanism, the SLPF could remove some of the mismatched points by raising the threshold; thus it is superior to the common SIFT algorithm in performance. The NPA values of the extracted point cloud from the three algorithms are shown in Figure 4(b). With the increased attitude angle, the NPA value of the matching feature point cloud decreases in all three algorithms. The CLP algorithm, however, is more sensitive to the projected range data, which makes the number of successful matching points drop sharply, and further affect geometric distribution of the point cloud. The gap between the SLPF and SLP shows that the fuzzy control module can help improve the geometric structure of the feature point cloud.

Figure 5 shows the positioning error when applying the three different matching algorithms at 5,000 different areas. The SLPF algorithm is better than the other two algorithms in all directions. When the platform’s attitude angle reaches about 10 degrees, the north and east positioning accuracy of SLPF algorithm is still about 8 meters, and the height positioning accuracy is about 0.2 meters. The reason that the height positioning error is far less than the north and east positioning error is because of the matching point cloud distribution. Due to the airborne lidar scanning mechanism, the matched point cloud is all located in a relative small area at the bottom of the platform, resulting in the great component value in the height direction of each matched feature point baseline vector in the G matrix, and then affect the final positioning accuracy.

Table 1 shows some detailed information as average number of matched points (ANMP) and matched points position error (MPPE) using the three methods. The MPPE is calculated in 3D space. It can be seen that when the tilt attitude is small, comparing to the CLP method, although the number of matched points extracted by SLPF is less, the matched points position accuracy is still much better, leading to a better localization result. Moreover, with the increasing platform tilt attitude, CLP and SLP have more difficulty in maintaining the number and accuracy of the matched points.

Lidar/INS Algorithm. To validate the feasibility of the proposed integrated navigation algorithm, firstly, the motion trajectory of the platform must be simulated. As shown in Figure 6, the red line is the simulated platform true trajectory, which lasts for 1,400 seconds. During the trajectory, the platform undertakes the different motion states as acceleration, deceleration, climbing, turning and descent. Then the INS output data based on the true trajectory with the frequency of 100 Hz is generated. To verify the calibration performance on the INS in the integrated navigation algorithm, accelerometer and gyroscope drift noise is added to the INS output data. The green line shown in Figure 6 is the INS output data trajectory solution. At the end of simulation, the error to the east direction reaches 500 meters, and the north direction error reaches to more than 2,200 meters.

At the same time of the INS outputting navigation solution, lidar also scans and calculates the position of the platform with 1-Hz frequency. Note that the speed of the aircraft is from 70 m/s to 100 m/s, and the maximum lidar scanning angle α_max is 20°. Figure 7 and Figure 8 show the number of matched points and the positioning error for each scanned terrain using SLFP. When the platform maintains smooth flying, the number of matched points can reach an average of 10, and the positioning accuracy is relatively high, less than 3 meters. Note, during the period, only in a few epochs are the number of matched points less than five. However, when the platform is climbing or changing flight direction, the number of matched points is obviously decreased due to the large tilt angle of the platform, and so does the number of successful positioning times. In this case, the position error is also increased dramatically, reaching about 10 meters error in east and north, and 0.2 meters error in height. Especially in the course of changing the direction of the flight, shown in Figure 7, during the periods of 720s–800s and 920s–1,000s, due to the larger roll angle, the SLPF could hardly be able to calculate the position through the LSM. During this period the lidar would occasionally output 1 or 2 matched feature points.

During the simulation, the CLT and LC methods are used for data fusion and trajectory estimation comparisons. TC method is not added to the comparison because of slow convergence. The data fusion results are shown in Figure 9. It illustrates that the LC method and the CLT method have close positioning accuracy in the case of sufficient matched feature points. As can be seen in conjunction with Figure 8, when lacking matched points, the CLT method is superior to LC on positioning accuracy, especially in the height direction. In addition, the CLT integrated algorithm shows some improvement on the accuracy of estimating speed and attitude.

Figure 10 shows the position error distribution when using four different lidar/INS integrated navigation methods for data fusion under the condition of different simulation trajectories. In the simulation, 50 1,400-second-long different trajectories, with flat areas, are generated with different platform attitude, velocity or acceleration. As can be seen from the figure, compared to other integrated navigation methods, the CLT method greatly improves the accuracy of navigation.

During 84.26% of the simulation period, CLT could maintain the position error less than 3 meters; the rate with error that is larger than 15 meters is 1.2%. For the TC method, due to the frequent divergence of the data fusion filter, most of the position estimates are not available. In addition, after flying above a flat area, the voting-based constellation integrated method has poor matched point accuracy and successfully matched rate due to large INS drift error, which makes lidar unable to calibrate the INS. When using the constellation-based method, during only 32.35% of the simulation period, the error is maintained in 3 meters and most of the period, 54.9%, the position error is between 3 to 15 meters.

CONCLUSION

We propose a new lidar matching algorithm based on SIFT, which does not rely on the INS output data to generate measured DEM data, and can adaptively change the threshold of the SIFT algorithm to generate optimal matching between the point cloud and the DEM. Through verification of simulation, the algorithm is compared with traditional lidar/INS integrated navigation methods based on comparing achieved accuracies in estimating position, speed and attitude. Simulation results show that the SLPF algorithm has better reliability for feature points matching and robustness against the platform attitude than the traditional algorithms. The CLT method improves trajectory estimation accuracy, especially when flying over moderately undulating terrain or flying with large roll or pitch angles.

ACKNOWLEDGMENT

This article is based on a paper presented at the ION International Technical Meeting, January 2017. This research used an open-source GNSS/INS simulator based on Matlab, developed by Gongmin Yan of Northwestern Polytechnical University, China.

---

Haowei Xu is a Ph.D. student at Northwestern Polytechnical University, where he received an M.Sc in Information and Communication Engineering. He is a visiting scholar at The Ohio State University.

Baowang Lian is a professor at Northwestern Polytechnical University where he is also director of the Texas Instruments DSPs Laboratory.

Charles K. Toth is a senior research scientist at the Ohio State University Center for Mapping. He received a Ph.D. in electrical engineering and geo-information sciences from the Technical University of Budapest, Hungary.

Dorota A. Brzezinska is a professor in geodetic science, and director of the Satellite Positioning and Inertial Navigation (SPIN) Laboratory at The Ohio State University.