Toward Underground Localization: Lidar Inertial Odometry Enabled Aerial Robot Navigation
Jiun Fatt Chow, Basaran Bahadir Kocer, John Henawy, Gerald Seet, Zhengguo Li, Wei Yun Yau, Mahardhika Pratama
TThis paper has been accepted for publication at IROS 2019 workshop ”Challenges in Vision-based Drones Navigation”. @IEEE
Toward Underground Localization: Lidar Inertial Odometry EnabledAerial Robot Navigation
Jiun Fatt Chow , Basaran Bahadir Kocer , John Henawy , Gerald Seet , Zhengguo Li , Wei Yun Yau ,Mahardhika Pratama Abstract — Localization can be achieved by different sensorsand techniques such as a global positioning system (GPS),wifi, ultrasonic sensors, and cameras. In this paper, we focuson the laser-based localization method for unmanned aerialvehicle (UAV) applications in a GPS denied environment suchas a deep tunnel system. Other than a low-cost 2D LiDARfor the planar axes, a single axis Lidar for the vertical axisas well as an inertial measurement unit (IMU) device is usedto increase the reliability and accuracy of the localizationperformance. We present a comparative analysis of the threeselected laser-based simultaneous localization and mapping(SLAM) approaches:(i) Hector SLAM; (ii) Gmapping; and(iii) Cartographer. These algorithms have been implementedand tested through real-world experiments. The results arecompared with the ground truth data and the experiments areavailable at https://youtu.be/kQc3mJjw_mw . Keywords – Aerial robot, UAV, indoor localization, sensorinstrumentation, Lidar, laser-based SLAM.
I. I
NTRODUCTION
UAVs have become a valuable platform for specific taskssuch as inspection, mobile manipulation, surveillance, aerialphotography and mapping due to their dexterity while flying[1], [2]. In 2017, the Land Transport Authority (LTA) inSingapore has researched intensively on the use of UAVtechnology for more efficient and flexible tunnel inspections[3]. To perform these tasks, it is possible to leverage model-based [4] and/or model-free control approaches [5]. However,the localization plays a more crucial role in determining thecurrent position and the orientation of the aerial robot withrespect to a reference frame. In recent years, various algorith-mic approaches have been presented for the mobile robots’localization, combining different sensor configurations, suchas visual-based localization such as a stereo camera andoptical flow sensor, laser-based localization and GPS [6].Outdoor localization can be easily achieved using GPS whileindoor localization mainly relies on a stereo camera, ultra-wideband technology, radio waves or beacons configura-tion [7], [8]. However, these methods have difficulties for The authors are with the School of Mechanical and Aerospace Engi-neering, Nanyang Technological University, 50 Nanyang Avenue, Singapore. { jchow011,koce0001,mglseet } @ntu.edu.sg John Henawy is with the School of Mechanical and Aerospace Engi-neering, Nanyang Technological University, Singapore, 639798 and Institutefor Infocomm Research, A-STAR, Singapore, 138632 and is a recipient ofthe A*STAR SINGA scholarship. [email protected] Zhengguo Li and Wei Yun Yau are with the Insti-tute for Infocomm Research, A-STAR, Singapore, 138632 { ezgli,wyyau } @i2r.a-star.edu.sg Mahardhika Pratama is with the School of Computer Science and Engi-neering, Nanyang Technological University, 50 Nanyang Avenue, Singapore. [email protected] navigation in a tunnel environment due to the followingreasons: poor light condition, featureless environment, echointerference, and GPS-denied.For a potential localization in a tunnel environment, simul-taneous localization and mapping (SLAM) technique can bea viable option. Some available approaches aim to explorethe use of thermal and visual camera information [9]. With anonboard illumination and semi-autonomous setting, the useof laser scanners together with cameras are used in [10]. In asimilar setting, the combination of stereo cameras and laserscanners is proposed recently in [11] for a chimney inspec-tion. Specifically, SLAM using cameras is referred to visual-based SLAM (vSLAM) which is based on visual informationonly while SLAM using the LIDAR sensor is referred to aslaser-based SLAM which relies on laser scan information[12]. The laser-based SLAM technique might be superior tovSLAM in an indoor environment (e.g. deep tunnel system)where the ambient light condition is not optimum. Hence,this paper presents a comparison of potential laser-basedSLAM techniques considering our desired tolerance of 20cmwith a two-dimensional (2D) LIDAR sensor as the mainperception input. For the vertical axis, the system is endowedby TFMini, which is a 1D Lidar sensor. An onboard IMU isused to further improve both reliability and accuracy of poseestimation [13] by eliminating unusable laser scan (causedby rolling and pitching).Three potential approaches, namely Hector SLAM, Gmap-ping, and Cartographer, are implemented and configured tobe tested on the UAV platform. The mapping and localizationperformances are then compared with ground truth data inthe motion capture lab, and the pose estimation error ofboth approaches are evaluated and discussed in the paper.The evaluations from this study might potentially determinewhich approach is more suitable for aerial robot localizationin an indoor environment such as a deep tunnel system.II. P
RELIMINARIES
A. Localization and Mapping
Localization of a mobile robot is required to determinethe pose information with various sensor configurationswithin an environment based on an algorithm. For example,LIDARs, ultrasonic sensors, stereo cameras are commonconfigurations for the localization. For autonomous systems,SLAM is introduced as the most widely researched topic.It can be useful for creating and updating maps withinan unknown environment, while keep tracking the posi-tion of the mobile robot instantaneously. Therefore, there a r X i v : . [ c s . R O ] O c t odel Range (m) FoV (deg)
Frequency (Hz)RPLidar A1 [0.15, 12] 360 10RPLidar A2 [0.20, 12] 360 10LDS-01 [0.12, 3.50] 360 5Hokuyo URG-04LX [0.06, 4] 240 10Hokuyo UTM-30LX [0.10, 30] 270 36TABLE I: Specifications of Selected Lidars. has been extensive research into SLAM algorithms, withreliably working solutions for typical indoor and outdoorenvironments using particle filters as in Gmapping. Most ofthem are being available as open software for individual andcollaborative development [14]–[19].
B. LIDAR Selection
LIDAR is a remote sensing method that uses light in theform of a pulsed laser beam to measure ranges (variabledistance). The capabilities of the LIDAR sensor are criticalin our project since it serves as the main sensing unitfor the localization algorithm. Therefore, some potentialLIDAR sensors are shortlisted in Table I, presenting theircharacteristics including detecting range, the field of view(FoV), and scanning frequency.In this study, an omnidirectional laser scan is desired todetect the features in a tunnel environment. The detectingrange of the sensor must be able to reach the width of thesewerage tunnel with 6m wide, similar to the case in [20].After some comparisons and considerations, RPLidar A1 wasselected because it is a low-cost 360-degree laser scannerwith a scanning rate of 10Hz.III. P
OTENTIAL
2D SLAM T
ECHNIQUES
This section discusses the characteristic of chosen SLAMtechniques, the configuration and fine-tuning of them toperform seamlessly with our LIDAR platform. To accomplishthis goal, a personal laptop is used to perform softwareimplementation, with the following specifications: (i) IntelCore i5-4210U [email protected] quad-core; (ii) 8GB RAM;and (iii) NVIDIA 840M GPU.
A. Hector SLAM
Hector SLAM incorporates with 2D LIDAR sensor togenerate a map from the laser scan. In contrast to otherSLAM techniques (e.g. Gmapping), Hector SLAM does notrequire any auxiliary odometry sensor (e.g. wheel encoders)which directly measures the travel distance of a land-basedrobot, but only relies on the information from the laserscan matching approaches. Therefore, the Hector SLAM ismore suitable for aerial vehicles. The Hector SLAM takesadvantage of the low distance measurement noise and highsampling rates of LIDAR for a fast scan-matching method[16]. Another advantage of the Hector SLAM is its capabilityto generate multi-resolution grid maps to avoid singularityduring scan matching. A map can be generated by the Hector SLAM accordingto the endpoints of the laser beams hit onto the walls. Then,the transformation of the current scan is determined by theGauss-Newton approach, which finds the best alignment ofthe current scan to the map generated previously.
B. Gmapping
Gmapping is a laser-based SLAM algorithm, which usesa Rao-Blackwellized Particle Filter SLAM approach. It isone of the most widely used SLAM methods in robotics,especially for land-based mobile robots. In general, theparticle filter family of the algorithm requires high samplingparticles to obtain accurate results, therefore it might haverelatively increased computational complexity. Also, the de-pletion problem associated with this method decreases the al-gorithm accuracy. This arises when the elimination of a largenumber of particles from a sample set during the resamplingstep. In this context, an adaptive resampling technique hasbeen developed to minimize the depletion problem since theresampling process is only performed when it is required.Moreover, this approach takes into account not only themovement of the mobile robot but also the most recent sensorobservation with odometry information; therefore decreasingthe uncertainty for the robots pose in the particle filter’sprediction step. As a result, the number of particles requiredis significantly reduced since the uncertainty is lower, dueto the quality of the laser scan matching process. In ourexperiment, the number of particles used is set to the defaultvalue of 30. C. Cartographer
Cartographer is an active approach that provides real-timeSLAM in 2D and 3D across multiple platforms and sensorconfigurations. It is an open-source library, developed byGoogle since 2016, which is also a state of art algorithm.Worth to mention, Google Cartographer does not requirea particle filter algorithm for mapping. It overcomes theissue of error accumulation during long iterations by poseestimation against a recent submap.In 2D SLAM, the Cartographer supports running thecorrelative scan matcher, which is used for finding loopclosure constraints with a submap (at the best-estimatedposition) referred to as frames. In detail, scan matchingoccurs at a recent submap, therefore it only depends onthe recent scans. After each submap is finished, there areno longer new scans that could be inserted; it automaticallychecks all submaps and scans for the loop closure. A scanmatcher starts to find the scan in the submap if the scansand the submaps are close enough based on the current poseestimates [21].The conversion process from a scan into a submap is givenin [22]. The generated submaps are presented in the formof a probability grid point which contains all the endpointsof beams that are closest to that grid point. Whenever ascan is inserted into the probability grid; the hits and misses re computed. Cartographer uses the Ceres scan matchingapproach to increase the accuracy of the scan pose in thesubmap. IV. R ESULTS AND D ISCUSSION
All three potential SLAM techniques are implemented andtested through offline experiments. A hand-held experimentalplatform of the LIDAR sensor and IMU device is designedand the data collection is conducted to obtain the results foreach approach. The indoor experiments are conducted in theMotion Analysis Laboratory which equipped with OptiTrackMotion Capture (Mocap) systems that allow vehicles tonavigate with a less than centimeter accuracy. Mocap uses8 off-board cameras to identify vehicle pose information(position and attitude) in a 3D space. It is an external systemthat tracks the position of reflective markers and providesits pose at 240 Hz. The ground truth data are received andrecorded for the comparison.There are two trajectories with two different speeds (nor-mal walking speed and fast walking speed), with the startingpoint set as the origin of the coordinates system (centerlocation), as described below.
1) Straight line trajectories:
Starting from the origin,moving straight and then follow the rectangular path untilit ends at the starting point. The heading of the LIDARsensor is purposely remained facing forward (X-direction),thus avoid the potential noise due to the large yawing angle.
2) 8-shape trajectories:
Starting from the origin, movingalong figure 8 with the heading aligned with the trajectory,ending at the starting point.
A. Performance Analysis of Selected SLAM Approaches
A group of typical localization results in the indoorexperiment is illustrated from the following aspects: (i) robotpath; and (ii) yaw angle.
1) Normal moving speed:
Firstly, the results from the firstcase are illustrated in Fig. 1 and Fig. 2. All trajectories areplotted on the same graph to have a clearer comparison ofthe selected algorithms. In Fig. 1, the localization resultsfrom the Hector SLAM and Gmapping are comparablysmoother than the outcome of the Cartographer which showsfluctuation and jerkiness. This result is possibly due to thecharacteristics of the Cartographer that fuses multiple sensordata, but there are only LiDAR sensors in our case. On theother hand, it is difficult to conclude that either the HectorSLAM or the Gmapping is more accurate only by this test.According to our observation, the localization performanceof the laser-based algorithm is affected by three variables,namely: the performance of the LIDAR sensor, the featureextraction within the environment and the matching algo-rithm.From Fig. 2, a significant time delay is observed in theGmapping algorithm, which also exhibits slightly discretemovement. This behavior is mainly because of the slowerupdating frequency of the Gmapping algorithm as comparedto others. On the other hand, both Hector SLAM and Cartographer give reliable positioning results, but the latteralgorithm shows fluctuation during each peak.In the second case, the circular path is used to testthe robustness of each SLAM technique when facing largerotating in the yaw angle. The results are shown in Fig. 3and Fig. 4. Once again, Hector SLAM had better resultsas compared to the Cartographer which is wavy and theGmapping which has a high time delay. Notably, the timeshift of the Gmapping could be larger when the simulationtime increases. We can also observe that there are two suddenpeaks of the yaw estimation from both the Gmapping andthe Cartographer packages.
2) Fast moving speed:
A similar experiment is conductedwith a faster speed to test the robustness of each algorithm.As can be seen in Fig. 5, the Gmapping is not functioningproperly. Hector SLAM has the best pose estimation whereelse the Cartographer generated a wobbly path. In Fig.6, the ground-truth data update stops at some instants inthe Mocap system due to the communication problems. -2 -1 0 1 2-1.5-1-0.500.51 Ground truthHector SLAM GmappingCartographer
Fig. 1: UAV path comparison: straight lines in nominal velocity trajectories.
Fig. 2: Yaw angle comparison in straight lines for nominal velocity trajec-tories.
Fig. 3: UAV path comparison: an 8-shape in nominal velocity trajectories.
Fig. 4: Yaw angle comparison in an 8-shape for nominal velocity trajectories. -1 0 1 2-2-1012 Ground truthHector SLAM GmappingCartographer
Fig. 5: UAV path comparison: straight lines in faster velocity trajectories.
During the faster circular trajectory shown in Fig. 7, theGmapping has failed to perform SLAM properly while the
Fig. 6: Yaw angle comparison in straight lines for faster velocity trajectories. -0.5 0 0.5 1 1.5-3-2-101 Ground truthHector SLAM GmappingCartographer
Fig. 7: UAV path comparison: an 8-shape in faster velocity trajectories.
Cartographer generated a fluctuated data. Only the HectorSLAMs trajectory has the closest trajectory as compared tothe ground truth values.Notably, the Cartographer experienced a sudden spiketwice in determining the heading of our robot when thesystem rotates -120 degrees of yaw, shown in Fig. 8 (greenline). There are also some stationary instants from ground-truth value during the experiment, this might be due to theblockade of reflective markers during the operation.
B. Further Analysis of SLAM Performance
To further analyze the results, we use the root mean squareformula (Eq. 1) to determine the accuracy of each approachcompared to the ground truth.
RMSE = (cid:114) n Σ ni = (cid:16) d t − d e (cid:17) ( ) where d t is the true displacement and d e is the estimateddisplacement. Fig. 8: Yaw angle comparison in an 8-shape for faster velocity trajectories.
To calculate the displacement between the true pose andthe estimated pose: d = (cid:114)(cid:16) x t − x e (cid:17) + (cid:16) y t − y e (cid:17) ( ) where the subscript t denotes the truth data, and the subscript e denotes the estimated data. Henceforth, some conclusionson each technique are shown in Table II. Firstly, the HectorSLAM algorithm relies only upon the laser scan matchingwithout the use of odometry, which could be an advantagefor our aerial robot platform. Apart from that, the HectorSLAM also provides accurate pose estimation, with anaverage error of 15.09cm in translation after taking averageRMSE of all scenarios. On the other hand, the Gmappingshows its robustness only in slow motion situations, but thetime delay accumulated over time. Most importantly, theGmapping is malfunctioning in faster movements. Lastly, theCartographer achieves the fastest computation time and had adecent average accuracy of 18.04cm in translation. Worth tomention, the Cartographer is designed for multiple sensorsplatform, therefore it is expected to obtain more accurateresults in a multi-sensor based system. C. On-board Experiments
From the previous off-board experiments, the HectorSLAM is selected as an optimum SLAM algorithm consid-ering our limitations for the sensor instrumentation. Exceptfor 2D Lidar and onboard IMU, an additional distance sensoris needed to provide information for the altitude. Since it istiny, low cost and consumes low power with a detecting range
Approach LinearNominal CircularNominal LinearFast CircularFast
Hector SLAM 9.39 14.83 11.47 24.69Gmapping 40.10 42.60 133.95 196.31Cartographer 16.70 14.94 15.95 24.56TABLE II: Comparison of Different Approaches: RMSE Values in cm. Fig. 9: Aerial robot platform. of 0.30m - 12m, TFmini Lidar is selected. It is configuredwith QGroundControl. Our UAV system is shown in Fig.9, where Intel NUC serves as an on-board processing unitwhich receives laser scan data and utilizes the Hector SLAMalgorithm meanwhile fusing the altitude (from TFmini) togenerate real-time 6 DoF position information.During real-time experiments, 4 scenarios are carried out: • Normal/fast speed circle path; • Normal/fast speed 8 path.The aerial robot is controlled via ROS for the desiredtrajectories. All the necessary nodes are running onboard.The recorded data are extracted and the displacement iscalculated in 3D space, followed by the RMSE formula. D = (cid:114)(cid:16) x t − x e (cid:17) + (cid:16) y t − y e (cid:17) + (cid:16) z t − z e (cid:17) ( ) A summary of RMS errors in 3D translations for all casesis shown in Table III. Since the generated paths are similarto each other, only the 3D visualization of the normal speedcircle path is given in Fig. 10.Notably, the detecting range of TFmini is only from 0.30 to12m, therefore any distance below 30cm will be consideredas a minimum value of 0.30m. Since the placement ofTFmini was 8cm offset below the CG of the drone, theeffective detecting range of altitude is 0.38m to 12.08m.Henceforth, the limitation of this fusion method is the blindzone when altitude is under 38cm.In summary, the 3D pose information is obtained withthe fusion of the Hector SLAM and the TFmini sensor.According to the ground truth data, the RMSE has stayedbelow 20cm for all the cases.
Trajectories RMSE (cm)Circular Nominal 19.08Circular Fast 18.858-shape Nominal 17.278-shape Fast 17.69TABLE III: Hector SLAM: Comparison of Different Trajectories.
10 1 -0.50.5 00 0.5-0.5 1-10.51 Ground truth Hector SLAM
Fig. 10: 3D pose estimation of Hector SLAM with the ground truthcomparison.
V. C
ONCLUSION
In this work, three selected approaches were implementedon the aerial robot system endowed by a 2D Lidar forhorizontal axes and 1D Lidar for the vertical axis. A setof offline experiments were conducted in different veloci-ties and conditions to measure their tracking and mappingperformances. From the results, it was concluded that theHector SLAM package obtained a reasonable localizationperformance. Also, the on-board experiments showed thatit was achieved to keep the RMSE below 20cm in 3Dtranslation. At the same time, the Cartographer packageis also preferable due to its potential with fusing differentperception units.In our future work, we intend to carry out the experimentsin a real tunnel environment to obtain the actual environ-mental conditions. Some difficulties can be expected, forexample, the rough surface and the humidity within a deeptunnel might not be optimum for a laser sensor. As a solution,the robustness of the laser-based algorithm can be improvedby fusing multiple SLAM algorithms such as a visual-basedSLAM that using a stereo camera to detect the featureswithin an environment. This would potentially achieve thegoal of autonomous UAV inspection in a deep tunnel systemwithout human intervention. Moreover, different lidars (e.g.,Velodyne and Hokuyo) are considered to be used in ourresearch for further improvement.R
EFERENCES[1] B. B. Kocer, T. Tjahjowidodo, M. Pratama, et al. , “Inspection-while-flying: An autonomous contact-based nondestructive test using uav-tools,”
Automation in Construction , vol. 106, p. 102895, 2019.[2] O. E. Mahmoud, M. R. Roman, and J. F. Nasry, “Linear and nonlinearstabilizing control of quadrotor uav,” in et al. , “Aerial robot control inclose proximity to ceiling: A force estimation-based nonlinear mpc,” arXiv preprint arXiv:1907.13594 , 2019.[5] M. A. Hady, B. B. Kocer, H. Kandath, et al. , “Real-time uav complexmissions leveraging self-adaptive controller with elastic structure,” arXiv preprint arXiv:1907.08619 , 2019.[6] I. Skog, “Sensing and Perception: Localization and positioning,”2016. [Online]. Available: http://wasp-sweden.org/custom/uploads/2016/10/michael-felsberg-overview-20161005.pdf[7] S. Hening, C. A. Ippolito, K. S. Krishnakumar, et al. , “3d lidar slamintegration with gps/ins for uavs in urban gps-degraded environments,”in
AIAA Information Systems-AIAA Infotech@ Aerospace , 2017, p.0448.[8] W. Zhen and S. Scherer, “Estimating the localizability in tunnel-likeenvironments using lidar and uwb,” in . IEEE, 2019, pp. 4903–4908.[9] S. Khattak, C. Papachristos, and K. Alexis, “Visual-thermal landmarksand inertial fusion for navigation in degraded visual environments,” in . IEEE, 2019, pp. 1–9.[10] T. ¨Ozaslan, G. Loianno, J. Keller, et al. , “Autonomous navigation andmapping for inspection of penstocks and tunnels with mavs,”
IEEERobotics and Automation Letters , vol. 2, no. 3, pp. 1740–1747, 2017.[11] J. Quenzel, M. Nieuwenhuisen, D. Droeschel, et al. , “Autonomousmav-based indoor chimney inspection with 3d laser localization andtextured surface reconstruction,”
Journal of Intelligent & RoboticSystems , vol. 93, no. 1-2, pp. 317–335, 2019.[12] T. Taketomi, H. Uchiyama, and S. Ikeda, “Visual SLAM algorithms:a survey from 2010 to 2016,”
IPSJ Transactions on Computer Visionand Applications , vol. 9, no. 1, p. 16, dec 2017. [Online]. Available:http://ipsjcva.springeropen.com/articles/10.1186/s41074-017-0027-2[13] J. Henawy, Z. Li, W. Y. Yau, et al. , “Accurate imu preintegrationusing switched linear systems for autonomous systems,” arXiv preprintarXiv:1907.08434 , 2019.[14] J. M. Santos, D. Portugal, and R. P. Rocha, “An evaluation of 2DSLAM techniques available in Robot Operating System,” , 2013.[15] R. Li, J. Liu, L. Zhang, et al. , “LIDAR/MEMS IMU integratednavigation (SLAM) method for a small UAV in indoor environments,”in . IEEE, sep 2014,pp. 1–15. [Online]. Available: http://ieeexplore.ieee.org/document/7049479/[16] S. Kohlbrecher, O. von Stryk, J. Meyer, et al. , “A flexible andscalable SLAM system with full 3D motion estimation,” in . IEEE, nov 2011, pp. 155–160. [Online]. Available:http://ieeexplore.ieee.org/document/6106777/[17] D. Tardioli and J. L. Villarroel, “Odometry-less localization in tunnel-like environments,” , vol. 32100,pp. 65–72, 2014.[18] I. Nikolov and C. Madsen, “LiDAR-based 2D Localization andMapping System using Elliptical Distance Correction Modelsfor UAV Wind Turbine Blade Inspection,”
Proceedings of the12th International Joint Conference on Computer Vision, Imagingand Computer Graphics Theory and Applications et al. , “Localisation of a mobilerobot for bridge bearing inspection,”
Automation in Construction ,vol. 94, no. April 2017, pp. 244–256, 2018. [Online]. Available:https://doi.org/10.1016/j.autcon.2018.07.003[20] C. Tan, M. Ng, D. Shaiful, et al. , “A smart unmanned aerial vehicle(uav) based imaging system for inspection of deep hazardous tunnels,”
Water Practice and Technology , vol. 13, no. 4, pp. 991–1000, 2018.[21] R. Yagfarov, M. Ivanou, and I. Afanasyev, “Map Comparison of Lidar-based 2D SLAM Algorithms Using Precise Ground Truth,” , pp. 1979–1983, 2018.[22] W. Hess, D. Kohler, H. Rapp, et al. , “Real-time loop closure in 2DLIDAR SLAM,”