Terrain Based GPS Independent Lane-Level Vehicle Localization using Particle Filter and Dead Reckoning

Transcription

1 Terrain Based GPS Independen -Level Vehicle Localizaion using Paricle Filer and Dead Reckoning Hamad Ahmed and Muhammad Tahir Deparmen of Elecrical Engineering, Lahore Universiy of Managemen Sciences, Lahore, Pakisan {hamad.ahmed, ahir Absrac The need of accurae and reliable posiioning in various locaion-aware safey criical ransporaion applicaions is increasing day by day. The Global Posiioning Sysem (GPS) is no able o provide lane-level vehicle localizaion wihou he aid of differenial correcions. I also suffers from signal ouages in urban areas resuling in a complee loss of locaion informaion. Therefore, GPS independen localizaion mehods are now being developed. In his domain, inerial sensors along wih a errain map have been successfully deployed o achieve submeer level accuracy in he longiudinal direcion of he vehicle in an urban environmen. However, laeral localizaion of he vehicle wih good accuracy and compuaional efficiency remains a challenging opic. Exising algorihms are compuaionally inensive, and do no provide locaion informaion during he process of lane change by he vehicle. This informaion is very crucial as he risk of poenial conflic wih nearby vehicles is higher during lane changes. In his paper, we presen a compuaionally efficien mehod for achieving lane-level localizaion in a muli-lane scenario by combining he paricle filer wih dead-reckoning. The paricle filer provides he locaion informaion abou a single lane while locaion informaion during he lane change maneuvers is provided by dead-reckoning. -change maneuvers are deeced by consanly observing he yaw rae of he vehicle. Developing a compuaionally efficien algorihm enables he GPS independen localizaion algorihm o be run on low cos micro-conrollers making is deploymen feasible for packaged devices. Experimens performed on an insrumened vehicle show he superioriy of he proposed algorihm on he exising ones. I. INTRODUCTION Numerous driving relaed applicaions developed in he pas couple of years have been focused on making driving as smar, safe and auomaed as possible. These developed soluions have reduced he risk of accidens hrough lane keeping and acive seering sysems, increased road hroughpu by flee managemen & lane allocaion, and caused los of convenience o he drivers by auomaed oll collecion and ickeing sysems. Accurae and reliable vehicle locaion informaion is a key elemen of all hese applicaions. The accuracy requiremen on he vehicle locaion informaion has also increased wih he increasing sophisicaion level of hese applicaions. Now, lane-level localizaion is a mus requiremen for all he applicaions menioned above. The Global Posiioning Sysem (GPS) has been he sandard for providing vehicle localizaion for decades. The visibiliy of GPS saellies in urban environmens is very poor due o urban canyons and unnels which degrade he qualiy of locaion informaion from GPS. If unobsruced GPS signal is somehow available, differenial correcions provided by Differenial-GPS (DGPS) are required o increase he accuracy o lane-level. Unforunaely, DGPS subscripion is only available in a very few counries, and is cosly. Therefore he effors have been focused owards developing soluions ha are eiher independen of GPS or can overcome he visibiliy and low accuracy issues of GPS for lane-level posiioning. Camera and LIDAR based soluions have been exensively proposed and sudied in he lieraure [] []. They rely on deecing he lane markings by image processing o esimae he lane of ravel. However he camera suffers from visibiliy problem in poor ligh condiions and he LIDAR is inaccurae due o road reflecions. Furher, boh of hese sensors are cosly and require hefy image/signal processing o exrac lane informaion which increases he on-board processing requiremens. A simple and cos-effecive soluion is o inegrae an Inerial Measuremen Uni (IMU) wih GPS receiver for aiding []. GPS/IMU inegraion no only increases he accuracy bu also provides locaion during signal ouages. However, he IMU based locaion begins o drif if he GPS ouage lass for a long ime, as he case wih unnels. To resolve hese predicamens, recen research effors have developed soluions which provide locaion informaion compleely independen of GPS. In his caegory, he use of errain map of a road segmen (road elevaion and bank angles) along wih an IMU sensor (gyroscope & acceleromeer) inside a Paricle Filer have been successfully demonsraed for achieving he accuracy a decimeer levels []. However, in his work, here has been an inheren assumpion ha he vehicle never changes is lane of moion. For laeral localizaion and lane change esimaion wo approaches, namely Bayesian Belief (BB) algorihm and Laeral Paricle Filer (LPF), were proposed in a laer work [5]. Boh of hese approaches increase he overall compuaions of he sysem hus prohibiing i o funcion on low end microprocessors. Anoher problem wih hese approaches is ha hey only provide locaion while he vehicle is raveling in a single lane. They do no provide locaion during he lane change. As one can observe ha he even of lane changing is he mos crucial during which he risk of poenial collisions is he highes. Hence, a locaion esimae during his even is umos desired for safey criical applicaions. Also, he work described in [], [5] assumes he use of highly accurae IMU sensors which are quie expensive. We have been working on enabling he IMU based errain aided localizaion wih low cos MEMS-IMU sensors suied for low cos processors which will enable a low cos finished device for commercial use on large scale. Recenly, we proposed a novel Kalman filer o exrac he accurae roll angle of he vehicle which is subsequenly used inside a Paricle Filer for longiudinal vehicle localizaion. The proposed soluion achieves good accuracy using low cos MEMS- IMU sensors [6]. In his work, we exend our previous sudy o develop a new algorihm which esimaes he lane of ravel wihou using any sensor oher han he IMU and wihou increasing he compuaional complexiy of he algorihm so ha i is sill able o funcion on a low cos microprocessor. We monior he yaw rae of he vehicle o deec he lane change maneuver. The algorihm is only riggered in he evens of lane change, hus keeping he compuaional cos low. We also inroduce dead reckoning ino our algorihm so ha i also provides soluion during he maneuvers of lane changing. Hence we achieve laeral localizaion along wih longiudinal localizaion o complee he soluion. The res of his paper is organized as follows. Secion II briefly describes he single lane longiudinal localizaion algorihm using

2 Paricle filer algorihm. Secion III presens he proposed approach for lane-level localizaion in he presence of lane change maneuvers. Secion IV describes he experimens ha were performed o es, validae and compare he algorihm. Finally, secion V concludes he work based on he discussion carried ou in previous secion. II. SINGLE LANE LONGITUDINAL LOCALIZATION Before describing he proposed algorihm, an explanaion of he longiudinal localizaion algorihm is in order. The road errain consiss of boh road elevaion and bank angles. The elevaion angle affecs he vehicle pich and he bank angle affecs he vehicle roll. In order o localize a vehicle using road errain, informaion regarding any one of hese angles is required. As we have shown in our previous work, obaining he roll angle from low cos sensors is compuaionally efficien as compared o pich angle. Hence we used only he roll angle in our localizaion algorihm [6]. The given errain map of a single road segmen consiss of longiude and laiude values of he road wih he bank (or roll) angle of he road a ha locaion. This readings are spaially spread a every.m disance of he road segmen. This map is generaed using exremely accurae IMU sensors and DGPS so ha hey can be considered as a ground ruh and serve as an accurae localizaion reference. The in-vehicle roll sensor which is a low cos MEMS IMU, provides he roll angle afer fusing he measuremens from a gyroscope and acceleromeer hrough a Kalman filer as described in [6]. For maching his roll angle wih he already available errain map, we used he paricle filer. Noe ha a he sar of he drive, he vehicle can be presen anywhere along he map hence he probabiliy disribuion describing he iniial locaion of vehicle is uniform (non Gaussian). Any probabilisic echnique ha assumes a Gaussian disribuion canno be used here. Paricle filers are sequenial Mone- Carlo mehods wih he abiliy o work on non-gaussian densiies. The paricle filer used here is he hird algorihm described in [7]. Following are main seps of his algorihm. A. Drawing he paricles A he sar of he algorihm, N paricles are disribued uniformly across he enire map due o uniform disribuion a he beginning of he drive. Each paricle i =...N has a locaion X, a roll angle value φ p,i and a weigh q i. The weigh of a paricle is an indicaor of he mach beween is roll angle value and he value measured by he in-vehicle sensor. The range of his weigh is from o. A he beginning, all paricles are assigned an equal weigh /N. B. Sae Updae The odomeery of he vehicle is also available hrough he CAN bus and is used o updae he locaion of each paricle a every ime insan k. The locaion of each paricle is projeced forward according o he speed of he vehicle v from he odomeer reading. A variance equal o he variance of he odomeery is added o each paricle s posiion. X k = X k + dx + do () where dx = v is he forward projeced disance, is he sampling ime and do is he added variance. C. Measuremen Updae For each paricle i, is roll angle φ p,i is compared wih he roll measured by he IMU sensor φ a and he paricle s weigh q i is calculaed using q i = exp( R.(φa φp,i) ) Σ N j= (exp( R.(φa φp,j) )) () where R is he variance of he roll measured by he IMU sensor. The paricles wih roll similar o ha measured by he sensor will ge a higher weigh from his funcion. D. Resampling A each sep, he number of effecive paricles is calculaed by N eff = Σ N i= (qi) () When number of effecive paricles are below a defined hreshold N T, he paricles are re-sampled according o he re-sampling algorihm given in [7]. The resampling algorihm is presened below in Algorihm. The paricles wih roll very differen from he Algorihm. Algorihm for Paricle Resampling c = cumsum(q i) u = rand().n i = for j =...N do u j = u + (j ).N while u j > c i do i = i + end while X j = X i q j = N end for sensor measuremens ge a lower weigh and he paricle populaion evenually becomes less effecive. The resampling sep draws he paricles again from he cumulaive densiy resuling in more paricles being drawn from he neighborhood of he higher weighed paricles. This causes he filer o converge when he variance of all he paricle s locaion becomes less han a hreshold. The mean locaion of he paricles is he locaion of he vehicle. So, given ha he vehicle ravels in he same lane on he road segmen, he above algorihm no only finds is locaion in he mapped segmen bu also coninues o rack i. The complexiy of he problem however, increases if lane changing is aken ino accoun because each lane will have is own errain map and he paricle filer will need o operae on he correc map for localizing and racking he vehicle. Therefore whenever he vehicle changes is lane, he correc errain map should be swiched auomaically. III. PROPOSED METHODOLOGY The proposed approach exends he Paricle Filer based single lane localizaion as described in he previous secion o muli-lane localizaion by incorporaing a lane change deecion and deadreckoning modules. Suppose he vehicle is raveling on a road wih wo lanes L and L. A he sar of he drive, he Paricle Filer algorihm essenially has wo maps o localize he vehicle as he vehicle could be presen anywhere on eiher lanes. Hence he oal number of paricles are spread over boh he lanes. Evenually he filer converges o he righ lane providing longiudinal posiion of he vehicle. Now, given ha vehicle has been iniially localized by he Paricle Filer in he correc lane, Fig. shows he nex seps of he proposed approach which are described below. change deecion : When he vehicle is changing lanes, he gyroscope aligned wih he yaw axis of he vehicle measures he yaw rae and hence i can be used as an indicaion ha he vehicle is in he process of changing lanes. The yaw rae observed during a ypical

3 Yaw Rae.5 YES Dead Reckoning Yaw Rae Speed Projec Forward from Previous Locaion Longiudinal Laeral Deeced? NO Paricle Filer Algorihm IMU/ Speed Roll Weigh Paricles Resample Longiudinal Locaion Informaion (Longiudinal posiion, Laeral Posiion) Terrain Map of Curren Paricle Populaion Projec Forward Fig.. Block diagram describing he proposed approach lane change is shown in Fig.. I is eviden ha he yaw rae exhibis a specific behavior due o lane changing. By aking he variance of he gyroscope measuremens over a pre-defined window lengh M, we can ge an indicaion whenever he car changes lanes. The choice of window lengh M is ineresing. There is a rade-off beween accuracy and he delay o lane change deecion which depends on he window lengh M. If a large window is chosen, here will be a sufficien delay in he deecion process. If a small window is chosen, hen he signal may remain buried in he noise floor. We colleced daa for a lane change and recorded he variance of yaw rae over differen window sizes. Fig. a indicaes ha he window size of.s is oo small o deec he lane change. On increasing i o.5s, here is a visible peak in he variance (Fig. b) which can be used as an indicaion ha he vehicle is changing is lane. Furher increase in he window sizes will only increase he delay in he deecion process. The opimal value of hreshold o rigger he lane change deecion can be se via a machine learning classifier which can be rained o separae he wo cases of driving i.e. sraigh lane drive and lane changing maneuver. However for simpliciy, here we se his value manually by observing he variance over several lane change drives. change aid o localizaion module : Suppose he vehicle was iniially occupying L. The paricle filer will be racking he vehicle in L using he errain map of L. When he yaw rae monior senses a lane change, i noifies he localizaion module which swiches o dead reckoning insead of Paricle Filer racking. This is due o he reason ha he vehicle is no longer in L and he paricle filer will coninue o rack i in L causing laeral locaion error. Dead-Reckoning during lane change : In dead reckoning, he x and y coordinaes of he vehicle a ime are updaed from he coordinaes a ime by using he speed v and yaw rae ψ by he equaion [ x y ] = [ x y ] + ([ cos(ψ +. ψ ] sin(ψ +. ψ.v ) () Yaw Rae Variance (dps) /s Yaw Rae (deg/s) Fig.. Yaw rae during a ypical lane change maneuver 5 5 (a) Yaw Rae Variance (dps) /s (b) Yaw Rae Variance (dps) /s Fig.. Variances of vehicle yaw rae over differen window sizes (a).s (b).5s (c) s I is eviden ha he accuracy of his locaion esimae will only hold for a shor duraion and he error will grow because he inegraion process will accumulae any noise presen in he sensors. However, lane changing is a shor maneuver lasing for a few seconds, in which he accuracy of he dead reckoning does no deeriorae. Hence we ge a locaion esimae even during he process of lane changing. Paricle Filer afer lane change : When he variance of yaw rae goes below he hreshold, he vehicle is assumed o have shifed o he desired lane and is no maneuvering anymore. Now he errain map for he appropriae lane should be used for furher racking he vehicle. We choose he errain map of ha lane, in which he dead reckoning placed he vehicle. The paricle filer sars racking he vehicle on his new map. A. Experimens IV. RESULTS AND DISCUSSION STMicroelecronics -axis MEMS acceleromeer LSMDHLC and gyroscope LGD were mouned on he es vehicle. Experimen was performed in he viciniy of he Lahore Universiy of Managemen Sciences. The errain map of boh he lanes of he road was available, and is shown in Fig. 5. The vehicle was driven for some ime in he firs lane and hen shifed o he second lane. The roll measured by he vehicle is also shown in Fig. 5 for he es drive. I can be seen ha he vehicle ravels in he firs lane for abou 6m and hen sars o shif lanes. A abou m, he vehicle has enered lane and he measured roll angle now resembles he errain map of he second lane. (c)

4 Reference Map ( ) Reference Map ( ) (a) (b) Map (c) Map (d) Map.5 Map.5 Map.5 Map (e) (f) (g) (h) Fig.. Paricle disribuion over he wo lanes for differen disances raveled (a)-(b) Sar (c)-(d) m (e)-(f) 5m (g)-(h) 5m Roll Angle (deg) Real Drive Longiudinal Locaion Error (m) Disance Travelled (m) Fig. 5. Terrain map of boh lanes and he roll measured in a es drive Disance Travelled (m) Fig. 6. Error in posiion esimae wih disance raveled B. Vehicle localizaion using he proposed approach For he proposed approach, he paricle filer was implemened wih N = paricles. The window size for monioring yaw rae was se o.5s. Half of he paricles were spread in lane (Fig. a) and half in lane (Fig. b) as iniially he vehicle can be presen in any lane. Afer only m of ravel, all he paricles are converged o lane (Fig. c-d) which indicaes ha he vehicle has been localized in lane. The longiudinal localizaion was achieved afer 5m of ravel (Fig. e-f) where he error was wihin m which can be observed from Fig. 6. A 65m, he vehicle iniiaes a lane change. The yaw rae moniors deecs i a 68 meers. A his poin, he dead reckoning akes over and keeps racking he vehicle unil he variance goes below he hreshold. The new lane map is decided based on he lane in which he vehicle was localized by he dead reckoning soluion. As shown in Fig. g-h, he paricles now have shifed o lane because he dead reckoning soluion localized he vehicle in lane as shown in Fig. 7a. Fig. 6 indicaes ha he proposed approach was able o achieve sub-meer level accuracy during he complee drive of he vehicle. C. Comparison wih he BB algorihm To compare he proposed echniques, we also implemened BB algorihm as well from [5]. Oher algorihm described in [5] i.e. LPF algorihm is infeasible for a low cos packaged device as i requires an exra sensor o measure he vehicle yaw. Hence, i was no considered for comparison. The BB algorihm erms he vehicle occupying any lane as a sae. An assumpion on he likelihood of changing lanes is assumed, and he beliefs of each sae are propagaed a periodic ime inervals. These beliefs are hen updaed in nex sep using he

5 Y Coordinaes (m) Y Coordinaes(m) X Coordinaes (m) (a) Proposed approach X Coordinaes (m) (b) BB algorihm Fig. 7. Vehicle localizaion during he es drive REFERENCES [] Amol Borkar, Monson Hayes, and Mark T Smih. A novel lane deecion sysem wih efficien ground ruh generaion. Inelligen Transporaion Sysems, IEEE Transacions on, ():65 7,. [] Chrisopher Rose, Jordan Bri, John Allen, and David Bevly. An inegraed vehicle navigaion sysem uilizing lane-deecion and laeral posiion esimaion sysems in difficul environmens for gps. Inelligen Transporaion Sysems, IEEE Transacions on, 5(6):65 69,. [] Rafael Toledo Moreo, B Úbeda, A Skarmea, Miguel A Zamora Izquierdo, e al. High inegriy imm-ekf based road vehicle navigaion wih low cos gps/ins. 7. [] A Dean, R Marini, and S Brennan. Terrain-based road vehicle localisaion using paricle filers. Vehicle Sysem Dynamics, 9(8):9,. [5] Adam J Dean and Sean N Brennan. Terrain-based road vehicle localizaion on muli-lane highways. In American Conrol Conference, 9. ACC 9., pages IEEE, 9. [6] Hamad Ahmed and M. Tahir. Terrain-based vehicle localizaion using low cos mems imu sensors. In 8rd Vehicular Technology Conference. IEEE, 6. [7] M Sanjeev Arulampalam, Simon Maskell, Neil Gordon, and Tim Clapp. A uorial on paricle filers for online nonlinear/non-gaussian bayesian racking. Signal Processing, IEEE Transacions on, 5():7 88,. measured roll value. The sae wih higher belief is ermed as he lane of ravel of he vehicle. Fig. 7a & 7b show he localizaion resuls of he proposed approach and BB algorihm during he same es drive. In he case of BB algorihm, i can be seen ha he locaion informaion during he lane changing is meaningless which indicaes he inabiliy of he algorihm o localize during lane change maneuvers. anoher problem wih he BB algorihm is ha i esimaes incorrec lane indices during cerain inervals e.g. beween 6 9m and 5m. The reason is ha he errain map of boh lanes is quie similar in hese regions (clear from Fig. 5) which confuses he BB algorihm. Also, he BB algorihm has o make assumpions on lane change likelihood. In realiy, his assumpion is very hard o make and calculae. The proposed approach does no make any assumpion of his kind. D. Compuaional complexiy comparison The BB algorihm propagaes and updaes he sae beliefs a periodic ime inervals in addiion o running he expensive Paricle Filer. Also, he number of beliefs propagaed and updaed a each ime insance is equal o he number of lanes. Hence he compuaional complexiy increases wih he number of lanes. In he proposed mehod, only he paricle filer is running, and an accompanying yaw rae monior which is simply a variance esimaor. The complexiy of he proposed approach does no scale wih he increase in number of lanes. Hence, he proposed approach is compuaionally efficien as compared o he BB algorihm. V. CONCLUSION In his work, we have exended he errain based localizaion algorihm using low cos IMU sensors o deec lane changes and localize he vehicle in he laeral direcion as well, hus compleing he localizaion soluion. The proposed mehod also provides locaion informaion during lane change maneuvers which were missing in previous algorihms. I is also compuaionally efficien making i feasible for implemenaion on low cos microprocessors. The mehod was experimenally verified and compared wih an exising algorihm in he lieraure. Fuure works aims o exend his work for incorporaion of curved and geomerically complex road neworks.