Large-scale 3D Outdoor Mapping and On-line Localization using 3D-2D Matching

Transcription

1 Large-scale 3D Oudoor Mapping and On-line Localizaion using 3D-D Maching Takahiro Sakai, Kenji Koide, Jun Miura, and Shuji Oishi Absrac Map-based oudoor navigaion is an acive research area in mobile robos and auonomous driving. By preparing a precise map of an environmen or roadside, a robo or a vehicle can localize iself based on a maching beween he map and a sequence of sensor inpus. This paper describes a campus-wide mapping and localizaion of a mobile robo wih D and 3D LIDARs (Laser Imaging Deecion and Ranging). For mapping, we use a 3D daa acquisiion sysem wih a D LIDAR and a roaion mechanism and akes a sequence of poin clouds. We adop an NDT (Normal Disribuion Transform)- based ego-moion esimaion mehod for pose graph generaion and opimizaion for loop closing. For localizaion, we propose o use a D LIDAR on a robo for being mached wih a 3D map for a fas and low-cos localizaion. The mapping and he localizaion mehod are validaed hrough he experimens in our campus. I. INTRODUCTION There is an increasing demand for mobile services robos which suppor human life in various ways, and he operaional areas of such robos are expeced o exend from indoor o oudoor environmens. Auonomous navigaion capabiliy is essenial for mobile robos, and accurae localizaion is herefore a key funcion. Localizaion using pre-regisered informaion can be divided ino map-based (e.g., [1], []) and view-based (e.g., [3], [4]). The former uilizes a maching beween a geomeric map and inpu sensory daa, while he laer is based on an image-o-image maching. This paper pursues he former because using a 3D geomeric map is more general and suiable for a large-scale mapping and localizaion in oudoor. SLAM (Simulaneous Localizaion And Mapping) echnologies [5] are usually used for auonomous map making by a mobile robo. For a large-scale mapping, loop closing [6] is a key o cope wih accumulaed pose errors. Recen progress in opimizaion mehods (e.g., [7]) makes i possible o generae a large-scale map efficienly. Once we have a 3D map of he environmen, we can adop 3D scan maching mehods like ICP [8] or NDT [9] for robo pose esimaion, probably combined wih saisical filering echniques [10]. 3D LIDARs such as Velodyne scanners have been gaining populariy due o heir abiliy of obaining very rich 3D informaion and i is very suiable for 3D mapping and localizaion in oudoor environmens where various objecs including naural ones exis. I is, however, cosly o use such a high-definiion 3D LIDAR if i should be insalled on all auonomous robos/vehicles. Therefore his paper proposes o use a more cos-effecive D LIDAR as a sensor on a The auhors are wih he Deparmen of Compuer Science and Engineering, Toyohashi Universiy of Technology, Toyohashi, Japan. robo. We experimenally validae he effeciveness of using a D LIDAR as he sensor on he robo as long as we have a reasonably accurae 3D map of he environmen. Noe ha 3D sensing is necessary only for he mapping sage which does no have o be carried ou in real-ime. The conribuions of he paper is wofold. One is o experimenally evaluae a 3D LIDAR-based large-scale oudoor mapping in our campus including large loops. The oher is show he effeciveness of on-line localizaion using 3D-D maching by off-line and on-line experimens. The res of he paper is organized as follows. Secion II describes relaed work. Secion III describes a 3D mapping wih loop closure and shows he resuls for our campus. Secion IV describes a localizaion mehod based on a 3D map-d scan maching and experimenal evaluaions. Secion V concludes he paper and discusses fuure work. II. RELATED WORK A. 3D mapping wih loop closure 3D mapping is done by inegraing a sequence of 3D scans using he esimaed robo poses. When a robo ravels by a long disance wih loops, i may suffer from accumulaed ego-moion esimaion errors hereby reducing he accuracy of pose esimaion. Therefore loop closing is he mos imporan sep especially in large-scale mapping [6], [11], [7]. Many image-based loop closure deecion mehods [1], [13], [14] have been proposed. They characerize locaions wih an image feaure such as BOVW (bag of visual words) [15] and compare he curren image wih pas ones o find loop closures. Inegraion of D image and 3D shape or poin feaures are also proposed [16], [17]. Calculaion of image feaures is relaively cosly and image-based maching could suffer from a sensiiviy o illuminaion changes. B. Map-based localizaion Given a map of he environmen, a robo can localize iself by maching he curren inpu wih he map. In probabilisic localizaion approaches (i.e., Markov localizaion [5]) using a grid or voxel map, he similariy beween he map and he curren scan is used as he likelihood of observaion [18]. If we have a precise 3D map, localizaion is o regiser he curren scan wih he map. In his regisraion, many scan maching approaches can be adoped such as ICP (ieraive closes poin) [8], GICP (Generalized ICP) [19], NDT (Normal Disribuion Transform) [9]). For so-called 6D localizaion, which esimaes full six degrees of freedom of

2 Fig. 1. Fig.. A 3D scanning sysem. (a) scan 1. (b) scan. NDT maching of 3D scans. (c) NDT maching resul. robo pose, 3D LIDARs are usually used bu hey suffer from a high cos and a relaively long compuaion. III. 3D MAPPING WITH LOOP CLOSURE A. 3D scanning We use a combinaion of a D LIDAR (Laser Imaging Deecion and Ranging, LMS151 by SICK) and a pan-il (p/) uni (PTU47 by FLIR, using is one axis) o acquire 3D scans (see Fig. 1). We roae he p/ uni wih a fixed speed while aking daa by he LIDAR. Using he sysem, we can acquire he scans as shown in Fig. (a)(b). LMS151 can acquire 1081 poin daa per scan for a 70 field of view wih 0.5 angular resoluion. We roae PTU47 by 180 in abou weny seconds for geing 357 scans. As a resul, he sysem can acquire 385,917 poin daa by one observaion. B. 3D mapping using NDT and pose graph opimizaion The edges of a pose graph are usually generaed by he following wo ways. One is for a pair of consecuive poses and given by an ego-moion esimaion beween hem. The oher is by loop closure which finds a revisi of he (almos) same locaion. 1) Ego-moion esimaion using NDT: We esimae egomoions by comparing wo consecuive 3D scans. Among various scan maching approaches, we compared hree mehods, namely, ICP, GICP, and NDT. We used PCL (poin cloud library) [0] implemenaions and compared for a roue of abou 1.km in our campus. The roue includes various scenes such as building-rich regions and ree-rich ones, and NDT shows he mos robus performance and efficiency. We herefore selec NDT for he scan maching for ego-moion esimaion. In NDT, a relaive pose beween wo poin clouds (we call hem a reference poin cloud and an inpu poin cloud) is calculaed as follows. (1) The reference poin cloud is divided ino voxels and he poin disribuion in each voxel is approximaed by a Gaussian. () A maching score is calculaed by: score= N exp { (x k µ k ) T Σ 1 k=1 k (x k µ k ) }, (1) where N is he number of inpu poins, µ k and Σ k are he mean and he covariance marix for he voxel which includes he kh inpu poin x k ransformed by he curren relaive pose. (3) Updae he relaive pose using a Quasi-Newon mehod. (4) Repea seps () and (3) unil convergence. ) Loop closure deecion: We use wo ways of deecing loop closures, wih and wihou GPS readings. Afer candidaes pairs of loop closing poses, hey are verified using he NDT-based maching. a) Loop closure candidae deecion based on GPS daa: The firs one uses GPS daa for deecing pairs of poses which are sufficienly near o each oher, and hen verifies hem wih calculaing relaive poses using NDT. The candidae deecion sep requires a disance calculaion beween poses. Following Hubeny s formula [1], he disance D beween wo poses is given by: D = (Mdφ) +(N cos(φ)dλ), () M = ( sin (Φ) ), (3) 3 N = , (4) sin (Φ) where M and N are he radius of he meridian and he prime verical circle, respecively, Φ is he average of wo laiude values, dφ and dλ are he difference of laiude and longiude of he wo locaions, respecively. We use GPS PahfinderPro by Tremble as a GPS receiver, and se he hreshold for he disance o 5m. If he disance beween wo poses is less han he disance hreshold, hey are judged as a loop closure candidae. b) Loop closure candidae deecion based on a shape signaure and he accumulaed posiion esimaes: The second way of deecing loop closures does no use GPS daa bu uses a shape signaure and he absolue posiion esimaion hrough he accumulaion of ego-moion esimaes. We use he classificaion of each voxel by [9] and describe a scene (i.e., 3D scan a a pose) by a hisogram of he following hree classes. Le he eigenvalues of he disribuion of a voxel be λ 1 λ λ 3 and classify i as follows: The disribuion is linear if λ /λ

3 The disribuion is planar if hey are non-linear and λ1 /λ 0.1. Oherwise, he disribuion is spherical. The voxel is 5m cube and use poins wihin 0m from he robo are used for hisogram calculaion. If wo poses is wihin a cerain disance using he accumulaed posiion esimaes and he hisogram is similar enough, hey are judged as a loop closure candidae. This signaure is very simple o calculae bu effecive as shown in he mapping resuls below. c) Candidae verificaion by NDT: We apply NDT o deeced candidaes o calculae he relaive pose and he degree of maching (i.e., he roo mean squared error (RMSE)). The RMSE is calculaed by: N 1 dxk + dyk + dzk, (5) ermse = N (a) The roue for acquiring 3D scans. (b) The graph afer loop closure deecion. k=1 where dk is he posiional difference beween an inpu poin and he corresponding neares-neighbor reference poin in each axis, and N is he number of inpu poins. If his error is less han a cerain hreshold (currenly, 1m), we add a new edge beween he poses wih he esimaed relaive pose. 3) Opimizaion: Pose graph opimizaion is o calculae he opimal se of poses by solving he non-linear opimizaion problem in he following form [7]: e(xi, xj, zij )T Ωij e(xi, xj, zij ), (6) F (x) = (c) The map before loop closing. i,j x = arg min F (x), x (7) where x = (xt1,..., xtn )T is a se of pose parameers and xi is he robo pose a ih observaion; Ωij is he informaion marix represening a consrain beween ih and jh pose, and zij is heir relaive pose given by he ego-moion esimaion; zij is represened by a concaenaion of relaive ranslaion ij and relaive roaion q ij in a quaernion form; e(xi, xj, zij ) is he error funcion evaluaing he difference beween he esimaed and he observed relaive pose. We use go library [7] for pose graph opimizaion. The informaion marix is relaed o he cerainy of relaive pose esimaion using NDT. We hus define Ωij as follows: pos Ωij 0 Ωij =, (8) 0 Ωro ij Ωpos ij = I/(σpos eij ), (9) Ωro ij = I/(σro eij ), (10) where I is an idenify marix, eij is given by ermse in eq. (5), and σpos and σro are he variance of posiional and roaional errors (currenly boh are se o 1.0). Fig. 3 shows he resul and he effec of loop closing. Fig. 3(a) is a roue for acquiring 3D scans (b) shows he final pose graph wih markers on he added edges by he loop closing. Fig. 3(c) and (d) compare he map before and afer he pose graph opimizaion. Inconsisencies of he map a many places have been solved in (d). The final graph has 183 nodes and 6 edges and he opimizaion ook [sec.] (d) The map afer loop closing. Fig. 3. Resul of pose graph opimizaion. The colors in he map indicae he heigh. using a PC wih Core i7-6700k and 16GB memory. C. Large-scale mapping resul Fig. 4 shows he resul of generaing a larger map. Fig. 4(a) shows he roue for acquiring 83 scans and Fig. 4(b) shows he mapping resul. Fig. 4(c) shows a 3D view of a par of he map. We can see a good map is obained. IV. O N - LINE L OCALIZATION WITH 3D-D M ATCHING Map-based localizaion is performed by maching beween a map and he curren sensor readings. To cope wih abrup sensing errors and/or feaure-scarce environmens, filering echniques are usually used [5]. In he case of oudoor localizaion using a 3D map, a usual way is o use a 3D sensor for 3D-3D maching. However, i is cosly o equip a 3D LIDAR for every robo jus for localizaion. We herefore pursue an approach o localizaion from a combinaion of a 3D map and a D LIDAR. 1) UKF-based localizaion: We use Unscened Kalman filer (UKF) [] for localizaion. The sae vecor x is defined as: T x = (p, v, q, bgyro ), (11)

4 Fig. 5. (a) The roue for acquiring 3D scans in a larger region. (b) The mapping resul. Colors indicae he heighs of he poins. (c) 3D view of a par of he map. Fig. 4. Resul of mapping of a larger region. where p is he posiion, v is he ranslaional velociy, q is is he bias for he orienaion in he quaernion form; bgyro he gyroscope. The predicion sep of he UKF is described as: T x = p 1 + v 1 Δ, q 1 Δq, v 1, bgyro, (1) 1 where Δ is he duraion beween and 1 and Δq is he roaion during Δ caused by he bias-compensaed angular velociy from he gyroscope, also in he quaernion form. The correcion sep uses he measuremen of he robo pose, defined as: obs T. (13) z = pobs, q Tes roue 1 for evaluaing he localizaion accuracy. The ranslaion pobs and he roaion q obs par calculaed by maching he curren D scan wih he 3D map using he NDT represenaion of he map. Noe ha he procedure for he 3D-3D maching for mapping and ha for he 3D-D maching for localizaion are he same wih only difference in he number of 3D poins in he curren inpu poin cloud. We use he resul of he predicion sep as an iniial esimae of he robo pose, and search a fixed range of voxels for he mached voxel of each inpu 3D poin. The range is specified as a cubic se of voxels wih he edge lengh being fifeen. The uncerainy is calculaed as he inverse of he informaion marix in eq (8). ) Experimens: a) Off-line experimens: We firs evaluae he accuracy of he proposed localizaion mehod. Since i is difficul o obain he ground ruh for he robo pose, we used a 3D LIDAR (HDL-3e by Velodyne) o ake full 3D scans as he robo moves, and assume is localizaion resuls as he rue values. In addiion, for simulaing a D LIDAR, we exrac only he horizonal scan ou of 3 scans of he sensor and use i for localizaion. Fig. 5 indicaes he firs roue used for evaluaion, which is he loop a he op-lef par of he environmen (see Fig. 4(a)); he roue is almos fla. The lenghs of he longer and he shorer edge are approximaely 130m and 75m, respecively. We performed five experimenal runs and calculaed he RMSE values for all of six degrees of freedom. We also simulae various limiaions in he range measuremens (i.e., maximum measurable range); we see how accuracy changes as he limiaion changes. Fig. 6 shows he RMSE error in he ranslaional and he roaional elemens of he localizaion (i.e., he robo pose esimaion) for various maximum measurable ranges. Table I summarizes he accuracy for wo maximum ranges. In his experimen, when he maximum measurable range is equal o or larger han 30m, he proposed localizaion mehod can provide a sufficien accuracy for auonomous navigaion. In he daa for he 60m maximum range, he roll and he pich value happen o have larger errors han he 30m case; however, hese values are no very imporan for he robo moving on a road-like, almos fla surface and, a he same ime, he yaw accuracy, which is more imporan for navigaion, is improved for he 60m range case. Fig. 7 indicaes he second roue used for evaluaion, which has a variaion in he road heigh. The region around he

5 Fig. 7. Tes roue for evaluaing he localizaion accuracy. (a) Accuracy in he posiion elemens. (a) Accuracy in he posiion elemens. (b) Accuracy in he orienaion elemens. Fig. 6. Accuracy of he pose esimaion for various limiaions of range measuremen for es roue 1. TABLE I ACCURACY IN THE LIMITATIONS OF 30m AND 60m IN THE RANGE MEASUREMENTS FOR TEST ROUTE 1. elemens maximum range is 30m maximum range is 60m x[m] y[m] z[m] roll [deg] pich [deg] yaw [deg] (b) Accuracy in he orienaion elemens. Fig. 8. Accuracy of he pose esimaion for various limiaions of range measuremen for es roue. firs corner is lower han he oher places on he roue. Daa collecion and analyses have been conduced similarly o he firs roue case. Fig. 8 shows he RMSE error in he ranslaional and he roaional elemens of he localizaion (i.e., he robo pose esimaion) for various maximum measurable ranges. Table II summarizes he accuracy for wo maximum ranges. Since he average disance o objecs (i.e. buildings) are smaller han in he roue 1 case, he necessary measurable range of LIDAR looks smaller and he overall accuracy is beer. Also in his experimen, exending he measurable range of he LIDAR basically increases he accuracy. b) On-line experimens: We performed on-line experimens using he LMS151 as he D LIDAR. The processing speed of one cycle including localizaion and robo conrol is abou 10fps, which is sufficienly fas for auonomous navigaion. Fig. 9(a) shows he rajecory when he robo moved on he firs wo edges of he roue in Fig. 5. Fig. 9(b) shows a snapsho of localizaion; red poins in he 3D map and a whie sphere indicae he daa obained by he curren D scan and he esimaed robo posiion, respecively. V. CONCLUSIONS AND FUTURE WORK This paper describes a large-scale 3D oudoor mapping mehod and a 6D localizaion mehod. 3D mapping is realized by combining an NDT-based ego-moion esimaion using 3D scans and a robus loop closure deecion using eiher GPS signals or a shape signaure followed by verifica- TABLE II ACCURACY IN THE LIMITATIONS OF 30m AND 60m IN THE RANGE MEASUREMENTS FOR TEST ROUTE. elemens maximum range is 30m maximum range is 60m x[m] y[m] z[m] roll [deg] pich [deg] yaw [deg]

6 (a) The esimaed rajecory (blue line) by he on-line localizaion. (b) Snapsho of on-line localizaion. Red poins: curren D scan; whie sphere: esimaed robo posiion. Fig. 9. Resul of on-line localizaion. ion and relaive pose esimaion using NDT. We have shown he mehod can generae a campus-wide 3D map reasonably accuraely. The localizaion mehod is based on a novel idea of maching a 3D map wih D scans. Use of a D LIDAR on a robo has advanages of a low-cos and a compuaional efficiency. We esed he mehod in wo locaions in our campus o compare he accuracy beween a usual 3D-3D maching-based mehod and ours. The comparison resuls show ha he proposed mehod has a sufficien accuracy for auonomous navigaion. The generaed map looks reasonably good qualiaively bu has no been evaluaed quaniaively. Comparison wih he resuls wih more accurae sysems such as FARO Focus3D and/or evaluaion using publicly-available daases such as KITTI daase would be preferable. Combining he curren geomeric map wih semanic informaion such as building names and locaion caegories will be necessary o communicae wih or acceping commands from people, using objec recogniion and caegorizaion echniques. Using such a semanically-enhanced map, we are planning o make a robo call sysem which can guide people or carry iems auonomously wih a human-friendly inerface. [] K. Irie, T. Yoshida, and M. Tomono. Mobile Robo Localizaion Using Sereo Vision in Oudoor Environmens under Various Illuminaion Condiions. In Proceedings of 010 IEEE/RSJ In. Conf. on Inelligen Robos and Sysems, pp , 010. [3] M.J. Milford and G.F. Wyeh. SeqSLAM: Visual Roue-Based Navigaion for Sunny Summer Days and Sormy Winer Nighs. In Proceedings 01 IEEE In. Conf. on Roboics and Auomaion, 01. [4] Y. Inoue, J. Miura, and S. Oishi. Oudoor Robo Navigaion Based on View-based Global Localizaion and Local Navigaion. In Proceedings of he 14h In. Conf. on Inelligen Auonomous Sysems, 016. [5] S. Thrun, W. Burgard, and D. Fox. Probabilisic Roboics. TheMIT Press, 005. [6] S. Thrun and M. Monemerlo. The Graph SLAM Algorihm wih Applicaions o Large-Scale Mapping of Urban Srucures. In. J. of Roboics Research, Vol. 5, No. 5-6, pp , 006. [7] R. Kümmerle, G. Grisei, H. Trasda, K. Konolige, and W. Burgard. g o: A General Framework for Graph Opimizaion. In Proceedings of 011 IEEE In. Conf. on Roboics and Auomaion, pp , 011. [8] P. Besl and N. McKay. A Mehod for Regisraion of 3-D Shapes. IEEE Trans. on Paern Analysis and Machine Inelligence, Vol. 14, No., pp , 199. [9] M. Magnusson, A. Lilienhal, and T. Ducke. Scan Regisraion for Auonomous Mining Vehicles using 3D NDT. J. of Field Roboics, Vol. 4, No. 10, pp , 007. [10] D. Fox, W. Burgard, and S. Thrun. Markov Localizaion for Mobile Robos in Dynamic Environmens. J. of Arificial Inelligence Research, Vol. 11, pp , [11] E. Olson, J. Leonard, and S. Teller. Fas Ieraive Alignmen of Pose Graphs wih Poor Iniial Esimaes. In Proceedings of 006 IEEE In. Conf. on Roboics and Auomaion, pp. 6 69, 006. [1] P. Newman, D. Cole, and K. Ho. Oudoor SLAM using visual appearance and laser ranging. In Proceedings 006 IEEE In. Conf. on Roboics and Auomaion, pp , 006. [13] M. Cummins and P. Newman. FAB-MAP: Probabilisic localizaion and mapping in he space of appearance. In. J. of Roboics Research, Vol. 7, No. 6, pp , 008. [14] T. Boerill, S. Mills, and R. Green. Bag-of-Words-driven, Single- Camera Simulaneous Localizaion and Mapping. J. of Field Roboics, Vol. 8, No., pp. 04 6, 011. [15] J. Sivic and A. Zisserman. Video Google: A Tex Rerieval Approach o Objec Maching in Videos. In Proceedings of 013 IEEE In. Conf. on Compuer Vision, pp , 003. [16] Y. Bok, Y. Jeong, and D.-G. Choi adn I.S. Kweon. Capuring Villagelevel Heriages wih a Hand-held Camera-Laser Fusion Sensor. In. J. of Compuer Vision, Vol. 94, No. 1, pp , 011. [17] C. Brand, M.J. Schuser, H. Hirschmüler, and M. Suppa. Submap Maching for Sereo-Vision Based Indoor/Oudoor SLAM. In Proceedings of 015 IEEE/RSJ In. Conf. on Inelligen Robos and Sysems, pp , 015. [18] K. Konolige. Markov Localizaion using Correlaion. In Proceedings of 16h In. Join Conf. on Arificial Inelligence, Vol., pp , [19] S. Segal, D. Hähnel, and S. Thrun. Generalized-ICP. In Roboics: Science and Sysems, 009. [0] Poin Cloud Library. hp://poinclouds.org/. [1] Hubeny s disance calulcaion formula. hp://vldb.gsi. go.jp/sokuchi/surveycalc/algorihm/ellipse/ ellipse.hml. accessed on 1/15/017. [] S.J. Julier and J.K. Uhlmann. Unscened Filering and NonLinear Esimaion. Proceedings of IEEE, Vol. 9, No. 3, pp , 004. REFERENCES [1] H. Andreasson and A.J. Lilienhal. 6D Scan Regisraion using Deph- Inerpolaed Local Image Feaures. Roboics and Auonomous Sysems, Vol. 58, No., pp , 010.