LIDAR AND INS FUSION IN PERIODS OF GPS OUTAGES FOR MOBILE LASER SCANNING MAPPING SYSTEMS

Transcription

1 LIDAR AND INS FUSION IN PRIODS OF GPS OUTAGS FOR MOBIL LASR SCANNING MAPPING SYSTMS Itzi Klei,, Sagi Fili Mappig ad Geo-Iformatio gieerig - Techio Israel Istitute of Techology, Haifa, Israel RAFAL, P.O.Box 5, Haifa, Israel (ilei, fili)@techio.ac.il KY WORDS: Iertial Navigatio Systems, lidar, Urba Navigatio ABSTRACT: Mobile laser scaig systems are becomig a icreasigly popular meas to obtai D coverage o a large scale. To perform the mappig, the exact positio of the vehicle must be ow throughout the trajectory. xact positio is achieved via itegratio of Global Positioig Systems (GPS) ad Iertial Navigatio Systems (INS). Yet, i urba eviromets, cases of complete or eve partial GPS outages may occur leavig the avigatio solutio to rely oly o the INS. The INS avigatio solutio degrades with time as the Iertial Measuremet Uit (IMU) measuremets cotais oise, which permeates ito the avigatio equatios. Degradatio of the positio determiatio leads to loss of data i such segmets. To circumvet such drift ad its effects, we propose fusig INS with lidar data by usig buildig edges. This detectio of edges is the traslated ito positio data, which is used as a aidig to the INS. It thereby eables the determiatio of the vehicle positio with a satisfactory level accuracy, sufficiet to perform the laser-scaig based mappig i those outage periods. INTRODUCTION Mobile laser-scaig mappig systems are becomig a icreasigly employed meas for gaiig accurate ad detailed D data o a large scale. Obtaiig this data, the vehicle positio must be ow accurately throughout the trajectory, maig it ecessary to obtai cotiuous ad accurate avigatio solutio. Typically, to meet this requiremet, cotiuous ad accurate avigatio Global Positioig Systems (GPS) ad Iertial Navigatio Systems (INS) are employed (Titterto ad Westo 4). GPS outages may occur however i urba eviromets alog urba cayos, or i other cases of sigal blocage, e.g., tuels or covered areas, leavig the avigatio solutio to rely o the INS stadaloe solutio. The INS solutio degrades however, with time as its sesors measuremets cotai oise, which permeates ito the avigatio equatios. Meas to reduce or boud the INS drift have bee addressed i the past. They ca be divided ito three categories. The first is based o icorporatio of exteral sesors ito the system, e.g., odometers or magetic sesors (Stephe ad Lachapelle, ; Godha et al., 5), ad fusio of the observatios ito the avigatio solutio. The secod category is based o utilizig vehicle costraits, which are based o traslatig a priori system owledge ito measuremets, followed by their icorporatio ito the estimator. Bradt et al. (998) ad Dissaayae et al. () utilize as a costrait the observatio that vehicles, ormally, do ot slip or jump off the groud. Usig this observatio, they derive a costrait o the vehicle s velocity. Klei et al. () itroduce costraits that feature groud vehicle dyamics, e.g., forward vehicles acceleratio, ad agular variatio oly i the yaw agle. Fially, the third approach proposes use of estimatio approaches other tha the Kalma filter. Amog them use of a secod order exteded Kalma filter samplig based filters such as Usceted Kalma Filter ad particle filters (Shi 5), ad artificial itelligece based methods such as adaptive eural fuzzy iformatio systems (l-sheimy et al. 4), have bee proposed. For exteral sesor fusio, laser (aa as light detectio ad ragig - lidar) scaers ca be regarded as a optio. Rage measuremets are usually based o time-of-flight measuremet priciple, ad whe itegrated with a scaig mechaism, they facilitate measuremets of objects i a certai agular field. Some of these objects may have ow positioal data. xamples INS/lidar fusio methods are foud i avigatio of autoomous groud or airbore platforms or for data collectio systems for mappig applicatios. For robotic localizatio, laser scaers are regularly used to perform ad improve curret simultaeous localizatio ad mappig (SLAM) methods, e.g. Pfister er al. (). For umaed air vehicle, Soloviev ad de Haag () use a laser scaer o-board to moitor slowly movig features to aid the INS. Additioally, Soloviev et al. (7) preseted a tight couplig approach betwee lidar ad INS for idoor ad outdoor urba eviromets. I this study, we propose mitigatig the INS drift i periods of GPS outages by usig the acquired laser scaig data ad groud plas. We focus o detectio of the crossig betwee objects (amely o their edges), which have positioal iformatio that ca be derived from the groud plas. We explore the case of avigatio i a urba eviromet. The sceario that is cosidered cosists of a vehicle equipped with a avigatio grade INS/GPS uit ad experieces complete GPS outages for short time periods. The aim is to fid meas to mitigate INS drift by employig laser scaer data. Use of the crossigs betwee buildigs as obtaied by the laser scaer is traslated it ito positio iformatio, ad is used

2 as iformatio to aid the INS. Testig this approach shows that the aided INS solutio provides vehicle positio with a satisfactory level of accuracy, sufficiet to perform mobile laser mappig scaig projects whe o GPS iformatio is available.. INS INS AND LIDAR FUSION The avigatio frame is defied as the oe where the x-axis poits towards the geodetic orth, the z-axis is o the local vertical poitig dow, ad the y-axis completes a righthaded orthogoal frame. Positio i the avigatio frame is expressed by curviliear coordiates T r h where, is the latitude, is the logitude ad h is the height above the arth surface. Motio equatios i the -frame are give by (Titterto ad Westo 4): r D v b b v T f g ie e v b T b b T b () D M h N h cos where v v v v T is the vehicle velocity; N D () b T b ad are the trasformatio matrices from the body frame (The x-axis is parallel to the vehicle logitudial axis of symmetry, poitig forward, the z-axis poits dow ad the y- axis completes a right-haded orthogoal frame) to the - b frame ad vice-versa, respectively; f is the measured specific force; ie is the arth tur rate expressed i the -frame; e is the tur rate of the -frame with respect to the arth; g is the local gravity vector, M ad N are the radii of curvature i the meridia ad prime vertical respectively; ad is the sew-symmetric form of the body rate with respect to the - frame give by: b b b b ib ie e T () The INS mechaizatio equatios provide o iformatio about errors i the system states as they process raw data from the Iertial Measuremet Uit (IMU) to estimate avigatio parameters. The IMU outputs cotai additioal errors that caot be compesated for. To improve the INS performace, it is ecessary to develop a error model, which describes how the IMU sesor errors propagate ito avigatio errors through the motio equatio. These avigatio errors are the corrected for i order to obtai a improved avigatio b b solutio. Several models (e.g. Titterto ad Westo 4; Jeeli ) were developed to describe the time-depedet behaviour of these errors. The classic approach is perturbatio aalysis, i which avigatio parameters are perturbed with respect to the true avigatio frame. Perturbatio is implemeted via a first-order Taylor series expasio of the states. A complete derivatio of this model ca be foud i Brittig (97) ad Shi (). The error state vector model 5 is defied as x r v b b, x ad cosists of positio error, velocity error, attitude errors, ad accelerometer ad gyro bias/drift. The state-space model is give by: x F x G (4) where Frr Frv b Frv Fvv F T b Fer Fev i T F a g, T G I I b b T (5) A detailed descriptio of the parameters i q. (4) is give i Appedix A.. Fusio For the fusio of the INS ad lidar data, a Kalma filter is implemeted. The Kalma filter algorithm ivolves: i) predictio of the state based o the system model, ad ii) update of the state based o the measuremets. A short descriptio of the Kalma Filter is give i Appedix B. Several approaches for fusig INS with lidar have bee cosidered. Here, we propose a fusio methodology, which is based o detectio of buildig corers from the lidar data for buildigs with a ow positio (e.g., groud pla iformatio). With such iformatio at had, a approximatio of the uow actual vehicle positio may be obtaied ad utilized as a positio aidig to the INS. We cosider a urba avigatio sceario. There, usually the elevatio of the trajectory is costat, ad so, the assumptio is that the vehicle is travelig at a costat height. Figure illustrates such a typical sceario. There, actual positio of the vehicle is mared by a blue rectagle, INS vehicle positio is mared by red circle, ad the buildig corer that has bee crossed is mared by a gree triagle. a g T

3 Positio RMS rror [m] Iertial Measuremet Uit (IMU) with a classical implemetatio of a triad of accelerometers ad gyros. It is assumed that the accelerometer ad gyro measuremets cotai oly of white oise with a std. of 5[ g] ad.[deg hr]. Acc Gyro To evaluate the cotributio of the proposed approach, positio ad velocity error measures are examied. To that ed, the followig error measure is utilized: ( t) q ( t) q ( t) () q aidig omial Figure : A corer detectio sceario It is assumed that the agle betwee the lidar measuremet ad the calculated INS rage is small. Thus, employig the law of cosies, the distace betwee the INS ad the true positio, may be give by: (6) lidar INS lidar INS where the calculated INS rage defied from the INS positio to the ow detected corer is: x x y y INS INS GIS INS GIS Next, we fid, the uow positio of the vehicle x y by solvig the followig two algebraic equatios for the lidar measured rage x x y y (8) lidar t GIS t GIS ad the distace betwee the INS ad the true positio x x y y (9) t INS t INS The, the computed positio p x y t t t t t (7) is used as a positio aidig to the INS. That is, the differece betwee the estimated INS positio, p, ad the above-metioed INS calculated positio is used as the residual measuremet, z p p, i the Kalma Filter. INS t This relatively simple strategy eables turig the laser scaig related iformatio ito a referece/cotrol iformatio that is added ito the avigatio solutio. ANALYSIS AND DISCUSSION To evaluate the proposed approach a simulated test of a vehicle s trajectory travelig i a costat velocity of 5 m/h for 6 secods is studied. The elevatio is set to a costat value throughout the trajectory. Laser ragig oise is cosidered havig a std. of.5[ m]. The INS cotais a L where () t is the error for state q, q () t is the state q aidig history obtaied from the aidig of vehicle costraits ad q t is the omial state history. The positio ad () omial velocity errors are obtaied from pos lat log h vel ( t) v( t) ve( t) vd ( t) where t, t ad t h lat log the logitude errors respectively ad vd t () () are the height, latitude ad t, t v ad are the orth, east ad dow velocity errors, respectively. Performace of the fusio depeds o the time differece betwee two successive crossig detectios. These detectios are traslated ito two successive measuremets. We cosider the time betwee two successive measuremets to be 5 secods. The filter fuctios the i a predictio mode for the ext 5 secods util the followig measuremet is provided. For simplicity, the time betwee two cosecutive measuremets remais costat throughout the trajectory. The results of such aidig are preseted i Figure for positio error ad i Figure for velocity errors. I both cases the results are compared to the drift whe usig the stadaloe INS solutio Time betwee Measuremets: 5 [sec] LIDAR Aidig Stadaloe INS Time [sec] Figure : Positio error for lidar aidig versus stadaloe INS ve

4 Mea elcoity rror [m/s] Mea Positio rror [m] elocity RMS rror [m/s].5.5 Time betwee Measuremets: 5 [sec] LIDAR Aidig Stadaloe INS as aidig to the INS. Simulatio results of such fusio showed improvemets relative to the stadaloe INS performace i estimatig the positio ad velocity compoets. Other tha applyig the proposed methodology o actual data, future research, will also address cases of partial GPS availability ad utilize this iformatio ito the proposed methodology..5 9 LIDAR Aidig Cotiuous GPS Aidig Time [sec] Figure : elocity error for lidar aidig versus stadaloe INS As Figure shows, the stadaloe INS positio solutio drifts, ad after 6 secods has a error of about 8 meters. Whe applyig laser scaig data as a aidig, at a 5 secods iterval, the INS drift drops dramatically ad the combied solutio is bouded by a maximal error which is lower tha.4 meter, ad eve lower o average. Notice that this performace was obtaied with oly twelve measuremets (at a 5 secods iterval). The same behavior occurs with the velocity error, where the stadaloe INS drifts ad has a error of. m/s after 6 secods, while the lidar aidig solutio has a bouded error of.4 m/s ad a average error of.5 m/s after 6 secods. It follows that such aidig greatly improves the stadaloe INS solutio ad reaches a bouded solutio for both positio ad velocity vectors. To examie the proposed approach further, we use the same trajectory but with differet time itervals betwee two measuremets. The itervals vary from.5 secods (equalig the INS samplig rate), to 4 secods. The umber of measuremets throughout the trajectory varies accordigly from (4 secods) to (.5 secods). For each period, the aidig positio ad velocity errors alog the trajectory were obtaied. For presetatio purposes, we average of the positio ad velocity errors, leadig to two values that describe the aidig performace. Graphs of these two mea values as a fuctio of the time iterval are show i Figures 4-5. As ca be see, the higher the umber of measuremets (small time betwee two detectios) the better fusio performace is. A compariso that is made for the performace of the aided INS to the fusio betwee GPS/INS while assumig GPS is cotiuously available is also show i both figures. It is show that whe the time iterval is of 5 secods, the performace is almost equivalet to the oe achieved by the GPS/INS itegratio. Thus, availability o groud plas as ladmars ad the derivatio of equivalet iformatio (e.g., buildig corers, but other objects as well) from the laser scaig data ca cotribute to securig the accuracy-level of the mappig data i such zoes. 4 CONCLUSIONS This paper explored the possibility of usig lidar measuremets as aidig by usig detected crossig betwee buildigs. mployig the proposed methodology, crossig data are traslated ito positioal iformatio, which are the used Time betwee Measuremets[sec] Figure 4: Mea positio error for lidar aidig versus stadaloe INS LIDAR Aidig Cotiuous GPS Aidig Time betwee Measuremets[sec] Figure 5: Mea velocity error for lidar aidig versus stadaloe INS 5 RFRNCS Bradit, A. ad Garder, J., F., Costraied avigatio algorithm for strapdow iertial avigatio systems with reduced set of sesors, Proceedigs of the America cotrol coferece pp , Philadelphia PA., 998. Dissaayae, G., Suarieh, S., Nebot,. ad Durrat-Whyte, H., The aidig of a low cost strapdow iertial measuremet uit usig vehicle model costraits for lad vehicle applicatios, I trasactios o robotics ad automatio, 7 (5), pp ,. l-sheimy, N., Adel-Hamid, W. ad Lachaplle, G., A adaptive euro fuzzy model for bridgig GPS outages i MMS-IMU/GPS lad vehicle avigatio, i Proceedig of the ION GNSS, pp , Log Beach, Califoria, 4. Godha, S., M., Petovello, G. ad Lachapelle, G., Performace aalysis of MMS IMU/HSGPS/magetic sesor

5 itegrated system i urba cayos, i Proceedigs of ION GPS, Log Beach, CA, U. S. Istitute of Navigatio, Fairfax A, September 5, pp Klei I, Fili S ad Toledo T, Pseudo-measuremets as aidig to INS durig GPS outages, Navigatio: Joural of the Istitute of Navigatio, 57(), pp 5-4,. Maybec P. S., Stochastic models, estimatio ad cotrol, olume, Navtech Boo & Software store, 994. Pfister S., Roumeliotis S., Burdic J., Weighted Lie Fittig Algorithms for Mobile Robot Map Buildig ad fficiet Data Represetatio. i Proceedigs of the I Iteratioal Coferece o Robotics ad Automatio,. Shi,.-H., stimatio Techiques for low-cost iertial avigatio, UCG reports umber 9, the Uiversity of Calgary, Calgary, Alberta, Caada, 5. Soloviev, A., Bates, D., va Graas, F, Tight Couplig of Laser Scaer ad Iertial Measuremets for a Fully Autoomous Relative Navigatio Solutio", NAIGATION, ol. 54, No., Fall pp. 89-5, 7. Soloviev, A. ad de Haag M. U., Three-dimesioal avigatio with scaig LiDARs: cocept & iitial verificatio, I trasactios o Aerospace ad lectroic System, ol. 6, No., pp. 4-,. Stephe, J. ad Lachapelle, G., Developmet ad testig of a GPS-augmeted multi-sesor vehicle avigatio system. The joural of avigatio, ol. 54(), pp. 97-9,. Titterto, D., H. ad Westo, J., L., Strapdow iertial avigatio techology secod editio, The America Istitute of Aeroautics ad Astroautics ad the istitutio of electrical egieers, 4. Zarcha, P. ad Musoff, H., Fudametals of Kalma filterig: a practical approach secod editio, The America Istitute of Aeroautics ad Astroautics, 5. ta ecos N hcos M h N h ta Fvr e N cos D si N hcos N h N h esi N D N N D N N N h M h R h (A.) N ta D e si M h N h M h ta D N ta Fvv e si e cos N h N h N h N e cos M h N h (A.4) F r F v e si N h N M h ta e cos N hcos N h (A.5) N h M h ta N h (A.6) 6 APPNDIX A The followig matrixes are associated with the INS state space error model q. (4) N M h si si Frr N hcos N h cos M h Frv N hcos (A.) (A.) F where ta N N h M h e si ta esi ecos N h N h ecos cos e N hcos N h (A.7) T v vn v v D is the velocity vector i the -frame ad the rest of the parameters were defied i the text. 7 APPNDIX B For the fusio of the INS ad lidar data, a Kalma filter is proposed. The Kalma filter algorithm ivolves: i) predictio of the state based o the system model, ad ii) update of the state based o the measuremets. The covariace associated with the predictio step is give by (Zarcha ad Musoff 5):

6 F() t t xˆ ˆ x, e (B.) P P Q T (B.) where the superscripts ad + represet the predicted ad updated quatities (before ad after the measuremet update, respectively); x ad P are the system state ad the associated error covariace matrices respectively; is the state trasitio matrix from time to time +; Ft () is the system dyamics matrix; ad Q is the process-oise covariacematrix (Maybec 994) give by: T T T Q Gt Qt G t Gt Qt G t t (B.) where, Gt () is the shapig matrix, ad t is the time step. The update is implemeted by: T K P H H P H R T xˆ xˆ K z H xˆ P I K H P where K is the Kalma gai; (B.4) (B.5) (B.6) H is the measuremet matrix; R is the measuremet oise covariace matrix; ad measuremet. z is the