Fast real-time localization with sparse digital maps for connected automated vehicles in urban areas

9th IFAC International Symposium on Advances in Automotive 9th IFAC 9th IFAC International International Symposium Symposium on on Advances Advances in in Automotive Automotive Control 9th IFAC International Symposium on Advances in Automotive Control Available online at www.sciencedirect.com Control Orléans, June 23-27, 9th IFAC France, International Control Orléans, France, June Symposium 23-27, 2019 2019 on Advances in Automotive Orléans, France, June 23-27, 2019 Control Orléans, France, June 23-27, 2019 Orléans, France, June 23-27, 2019

ScienceDirect

IFAC PapersOnLine 52-5 (2019) 366–371

Fast real-time localization with sparse Fast real-time localization with sparse Fast real-time localization with sparse digital maps for for connected with automated Fast real-time localization sparse digital maps connected automated digital maps for connected automated vehicles urban areas digital maps forin connected automated vehicles vehicles in in urban urban areas areas vehicles in urban areas Tobias Quack ∗∗∗ Frank-Josef Heßeler ∗∗∗ Dirk Abel ∗∗∗

Tobias Quack ∗ Frank-Josef Heßeler ∗ Dirk Abel ∗ Tobias Tobias Quack Quack Frank-Josef Frank-Josef Heßeler Heßeler Dirk Dirk Abel Abel ∗ ∗ ∗ ∗ Tobias Quack Frank-Josef Heßeler Dirk AbelAachen, ∗ f¨ u r Regelungstechnik, RWTH Aachen University, ∗ Institut Institut f¨ u r Regelungstechnik, RWTH Aachen University, Aachen, Institut f¨ u r Regelungstechnik, RWTH Aachen University, Aachen, ∗ Institut f¨ ur Regelungstechnik, RWTH Aachen University, Aachen, Germany (e-mail: Germany (e-mail: [email protected]) [email protected]) ∗ Germany (e-mail: Institut f¨ ur Regelungstechnik, RWTH Aachen University, Aachen, Germany (e-mail: [email protected]) [email protected]) Germany (e-mail: [email protected]) Abstract: For the of automation systems in accurate Abstract: For For the the realization realization of of advanced advanced automation automation systems systems in in road road vehicles, vehicles, accurate accurate Abstract: Abstract: For the realization realization of advanced advanced automation systems systems in road road vehicles, vehicles, accurate and robust localization is a crucial requirement. Satellite-based such as GPS are and robust localization is a crucial requirement. Satellite-based systems such as GPS are and robust localization is a crucial requirement. Satellite-based systems such as GPS are Abstract: For the realization of advanced automation systems in road vehicles, accurate and robust localization is a crucial requirement. Satellite-based systems such as GPS are generally able to provide geolocations, but their precision and robustness can be impaired generally able to provide geolocations, but their precision and robustness can be impaired generally able to provide geolocations, but their and can be impaired and robust localization asatellites crucial and requirement. Satellite-based systems such as GPS are generally able provideis their precision precision and robustness robustness cansurroundings. be impaired strongly due to shading of multipath effects, especially in urban strongly due due totoshading shading ofgeolocations, satellites and andbut multipath effects, especially especially in urban urban surroundings. strongly to of satellites multipath effects, in surroundings. generally able to provide geolocations, but their precision and robustness can be impaired strongly due to shading of satellites and multipath effects, especially in urban surroundings. Localization methods methods based based on on environment environment perception perception sensors sensors and and digital digital maps maps are are therefore therefore Localization Localization methods based on perception sensors and maps are therefore strongly duein tothe shading ofautonomous satellites and multipath effects, especially in urban Localization methods based on environment environment perception sensors and digital digital mapssurroundings. arecapability therefore widely used field of vehicles with accuracy, robustness, real-time widely used in the field of autonomous vehicles with accuracy, robustness, real-time capability widely used the field of vehicles with accuracy, robustness, real-time Localization methods on environment perception sensors andwedigital maps arecapability therefore widely used in in the fieldbased of autonomous autonomous withIn accuracy, robustness, real-time and sparseness of the maps being major objectives. this paper, present aa fast, real-time and sparseness sparseness of the the maps being major majorvehicles objectives. In this paper, paper, we present present fast,capability real-time and of maps being objectives. In this we a fast, real-time widely used in the field of autonomous vehicles with accuracy, robustness, real-time capability and sparseness of the maps being major objectives. In this paper, we present a fast, real-time capable implementation of a Monte Carlo Localization scheme which operates on a storage space capable implementation implementation of of aa Monte Monte Carlo Carlo Localization Localization scheme scheme which which operates operates on on aa storage storage space space capable and sparseness theand maps being objectives. Inscheme this paper, presenton a afast, real-time capable implementation ofis Montemajor Carlo Localization whichwe operates storage space efficient digital of map isatargeted targeted to provide provide precise localization in urban surroundings at aa efficient digital map and to precise localization in urban surroundings at efficient digital map and is targeted to provide precise localization in urban surroundings at capable implementation ofisatargeted Monte evaluation, Carlo Localization scheme which operates on a storage space efficient digital map and to provide precise localization in urban surroundings at aa rate of 50 Hz. For the experimental we use our test vehicle’s LiDAR sensor combined rate of 50 Hz. For the experimental evaluation, we use our test vehicle’s LiDAR sensor combined rate of 50 Hz. For the experimental evaluation, we use our test vehicle’s LiDAR sensor efficient map is targeted to provide precise localization in urban at a rate 50digital Hz. For theand experimental evaluation, we our testGPS. vehicle’s LiDARsurroundings sensor combined combined with wheel odometry, inertial measurements and aause low-cost with of wheel odometry, inertial measurements measurements and low-cost GPS. with wheel odometry, inertial and a low-cost GPS. rate of 50 Hz. For the experimental evaluation, we use our test vehicle’s LiDAR sensor combined with wheel odometry, inertial measurements and a low-cost GPS. © 2019, IFACodometry, (International Federation of Automatic Hosting by Elsevier Ltd. All rights reserved. with wheel inertial measurements andControl) a low-cost GPS. Keywords: Automated vehicles, Intelligent Transport Systems, Monte Carlo Localization, Keywords: Automated Automated vehicles, vehicles, Intelligent Intelligent Transport Transport Systems, Systems, Monte Monte Carlo Carlo Localization, Localization, Keywords: Keywords: Automated vehicles, Intelligent Transport Systems, Monte Carlo Localization, LiDAR, Sensor Fusion, Digital Maps LiDAR, Sensor Sensor Fusion, Fusion, Digital Digital Maps Maps LiDAR, Keywords: Automated Intelligent Transport Systems, Monte Carlo Localization, LiDAR, Sensor Fusion, vehicles, Digital Maps LiDAR, Sensor Fusion, Digital Maps 1. INTRODUCTION present real-world implementation of GPOM in which 1. INTRODUCTION INTRODUCTION present aaa real-world real-world implementation implementation of of GPOM GPOM in in which which 1. present 1. INTRODUCTION present a real-world implementation of GPOM in which road curbs and road markings are mapped. An adapted road curbs curbs and and road road markings markings are are mapped. mapped. An An adapted adapted road 1. INTRODUCTION present a real-world implementation of GPOM in3D which andis markings are mapped. Ana MCL variant applied for localization with LiAutomation is is one one of of the the key key areas areas in in current current automoautomo- road MCL curbs variant is road applied for localization localization with a adapted 3D LiLiMCL variant is applied for with 3D Automation road curbs and road markings arerepresentation mapped. Anaa adapted Automation is one of the key areas in current automoMCL variant is applied for localization with 3D LiDAR sensor. An alternative map suitable Automation is one of the key areas in current automotive research. Supporting and finally replacing the human DAR sensor. An alternative map representation suitable DAR sensor. An alternative map representation suitable tive research. Supporting and finally replacing the human variant is found applied for localization with where a suitable 3D the Litive research. research.isSupporting Supporting and finally replacing theautomohuman MCL DAR sensor. alternative map representation for MCL can be in (Rohde et al., 2016) Automation one of theand keyfinally areas in has current tive replacing human driver by intelligent automation systems the potential for MCL MCL can An be found found in (Rohde (Rohde et al., 2016) 2016) where where the for can be in et al., the driver by by intelligent intelligent automation automation systems has the thethe potential DAR sensor. An alternative map et representation suitable driver systems has potential for MCL can be found in (Rohde al., 2016) where the map consists of multiple 2D layers generated from LiDAR tive research. Supporting and finally replacing the human driver by intelligent automation systems has the potential to increase road safety by avoiding accidents, improve map consists consists of of multiple multiple 2D 2D layers layers generated generated from from LiDAR LiDAR map to increase increase road safety safety by avoiding avoiding accidents, improve for MCL can be found inthe (Rohde et al., 2016) where the to road by accidents, improve generated from data. In order to reduce memory required for storing driver by intelligent automation has efficient the potential to increase road safety by avoiding accidents, improve environmental sustainability duesystems to energy energy driv- map data.consists In order orderofto tomultiple reduce 2D thelayers memory required for LiDAR storing data. In reduce the memory required for storing environmental sustainability due to efficient drivmap consists of multiple 2D layers generated from LiDAR environmental sustainability due to energy efficient drivdata. In order to reduce the memory required for storing the map, Schiotka et al. (2017) propose a minimalistic to increase road safety bytravelers avoiding accidents, improve environmental sustainability due to of energy efficient driv- the ing strategies and relieve the tiring driving the map, map, Schiotka Schiotka et et al. al. (2017) (2017) propose propose aa minimalistic minimalistic ing strategies strategies and and relieve travelers travelers of the tiring tiring driving data. In order to reduce memory required for storing ing relieve of the driving the map, Schiotka etbased al.the (2017) propose a minimalistic localization scheme on aa sparse scan map which environmental sustainability dueIntoorder energy efficient driving strategies and relieve travelers of the tiring driving task, amongst other benefits. to achieve safe localization scheme based on sparse scan map which which localization scheme etbased based on aa sparse sparse scan map task, amongst amongst other other benefits. benefits. In In order order to to achieve achieve safe safe the map, Schiotka al. (2017) propose a minimalistic task, localization scheme on scan map which seeks to optimize localization accuracy while including a ing strategies and relieve travelers of the tiring driving task, amongst other benefits. In order to achieve safe operation of automated vehicles even in challenging surseeks to optimize localization accuracy while including seeks to optimize localization accuracy while including aa operation of of automated automated vehicles vehicles even even in in challenging challenging sursur- localization scheme based measurements. on accuracy a sparse while scan map which operation to optimize localization including a minimal number of LiDAR The problem of task, amongst other benefits. Ineven order tomajor achieve safe seeks operation of automated vehicles in challenging surroundings such as urban intersections, one requireminimal number number of of LiDAR LiDAR measurements. measurements. The The problem problem of of minimal roundings such such as as urban urban intersections, intersections, one one major major requirerequire- seeks to number optimize localization accuracyis while including roundings LiDAR measurements. The problemby ofa data reduction in the mapping process also addressed operation of automated vehicles even in challenging sur- minimal roundings such as urban intersections, oneCurrent major requirement is robust and accurate localization. vehicles data reduction reduction inof the mapping process is is also also addressed by data in the mapping process addressed by ment is robust and accurate localization. Current vehicles number LiDAR measurements. The problemby of ment is is robust robust and accurate localization. Current vehicles minimal data reduction inof the is also addressed TomTom (2017), who propose a 2D raster representation roundings such as urban intersections, one major (GNSS) requirement accurate localization. Current vehicles mainly rely on satellite-based navigation systems TomTom (2017), whomapping propose process 2D raster raster representation TomTom (2017), who propose aa 2D 2D representation mainly rely rely on on and satellite-based navigation systems systems (GNSS) data reduction in the mapping process is also addressed by mainly satellite-based navigation (GNSS) TomTom (2017), who propose a raster representation of 3D depth data as a map source for highly accurate ment is robust and accurate localization. Current vehicles mainly rely on satellite-based navigation systemsin which have significant shortcomings, especially urban of 3D 3D depth depth data data as as aa map map source source for for highly highly accurate accurate of which have have significant shortcomings, especially in(GNSS) urban TomTom (2017), a 2D raster representation which significant shortcomings, especially in urban of 3D depth datawho as propose a map source for highly accurate localization. mainly rely on satellite-based navigation systems (GNSS) which have significant shortcomings, especially in urban environments where shading of satellites and multipath localization. localization. environments where shading of satellites and multipath of 3D depth data as a map source for highly accurate environments where shading shading of satellites satellites and multipath multipath localization. which have significant shortcomings, especially in urban environments where effects significantly impair the localization accuracy and One general challenge of digital maps used for localizaeffects significantly significantly impair the theoflocalization localizationand accuracy and and localization. effects impair accuracy One general challenge challenge of of digital digital maps maps used used for for localizalocalizaOne general environments where shading of satellites and multipath effects significantly impair theunder localization accuracy and availability. In addition, even good conditions, the general challenge of of up-to-date digital maps used for localization is is the availability availability of up-to-date map information. In availability. In addition, addition, even under good conditions, conditions, the One availability. In even under good the tion the map information. In tion is the availability of up-to-date map information. In effects significantly impair the localization accuracy and availability. In addition,of under good conditions, the One localization uncertainty GNSS is still in the range of megeneral challenge of have digital maps used for localization is the availability of up-to-date map In (Quack et al., 2018), we proposed aainformation. framework for localization uncertainty uncertainty ofeven GNSS is still still in the the range of of memelocalization of GNSS is in range (Quack et al., 2018), we have proposed framework for (Quack et al., al., 2018), we we have proposed framework for for availability. In addition, under good conditions, the tion localization uncertainty ofeven GNSS still in the range of meters for widely-used receivers (see e.g. u-blox AG (2018)). (2018)). is the availability of have up-to-date mapaainformation. In (Quack et 2018), framework continuously updating maps on distributed infrastructure ters for for widely-used widely-used receivers (seeise.g. e.g. u-blox AG ters receivers (see u-blox AG (2018)). continuously updating maps onproposed distributed infrastructure continuously updating maps on distributed infrastructure localization uncertainty of GNSS ise.g. stillu-blox in theuse range of me- (Quack ters for widely-used receivers (see AG (2018)). Therefore, localization methods which make of highly et al., 2018), we have proposed a framework for continuously updating maps on distributed infrastructure computers in a so-called mobile edge computing (MEC) Therefore, localization methods which make use of highly Therefore, localization methods which make use use of(2018)). highly computers computers in in aa so-called so-called mobile mobile edge edge computing computing (MEC) (MEC) ters for widely-used receivers (see e.g. u-blox AGof Therefore, localization methods which highly accurate digital maps are considered as a fundamental tool continuously updating maps on distributed infrastructure inIn a this so-called mobile edge computing (MEC) architecture. In this context, vehicle to infrastructure infrastructure comaccurate digital digital maps are are considered as make a fundamental fundamental tool computers accurate maps considered as a tool architecture. context, vehicle to comarchitecture. In this context, vehicle to infrastructure comTherefore, localization methods which make use of highly accurate digitaldriving maps are considered a fundamental tool computers for automated automated driving (Seif and Hu, Hu, as 2016). in so-called mobile edge computing (MEC) architecture. Ina this context, vehicle toLTE infrastructure communication (V2I) based on 802.11p or communication for (Seif and 2016). for automated driving (Seif and Hu, 2016). munication (V2I) based on 802.11p or LTE communication munication (V2I) (V2I) based on 802.11p 802.11p ortoLTE LTE communication accurate digitaldriving maps are considered a fundamental tool architecture. for automated (Seif and Hu, as 2016). In this context, vehicle infrastructure communication based on or communication allows us to send the most recent map information to each A large number of localization schemes based on digital allows us us to to send send the the most recent map map information to each each allows most recent information to for automated driving (Seif and schemes Hu, 2016). A large large number of localization localization schemes based on on digital digital munication (V2I) based on 802.11p or LTE communication A number of based allows us to send the most recent map information to each traffic participant who enters the respective area. However, A large number of localization schemesStandard based onmethods digital traffic maps have been proposed proposed in literature. literature. Standard methods traffic participant participant who who enters enters the the respective respective area. area. However, However, maps have been in allows us togrowing send the most recent map information to each maps have been proposed proposed in literature. literature. Standard methods traffic who enters the area. networks, However, despite the bandwidth ofrespective current mobile networks, A large number of grid localization schemes based onmethods digital maps have been in Standard include occupancy mapping for the generation of despiteparticipant the growing growing bandwidth of current mobile despite the bandwidth of current mobile networks, include occupancy grid mapping mapping for the the generation of 222 traffic participant who enters the respective area. However, include occupancy grid for generation of despite the growing bandwidth of current mobile networks, the amount of data to be exchanged in a crowded urban maps have been proposed in literature. Standard methods include occupancy gridmonte mapping the generation dimensional maps and carlo localization (MCL) for the amount amount of of data data to to be be exchanged exchanged in in aa crowded crowded urban urban the dimensional maps and and monte carlofor localization (MCL)offor for2 despite the growing bandwidth of current mobile networks, dimensional maps monte carlo localization (MCL) the amount of data to be exchanged in a crowded urban scenario is still challenging. In addition, automated driving include occupancy grid mapping for the generation of 2 dimensional maps and in monte localization for scenario real-time localization the mapped area or landmarkscenario is is still still challenging. challenging. In In addition, addition, automated automated driving driving real-time localization localization in the carlo mapped area or or (MCL) landmarkthe amount of challenging. data to be position exchanged inautomated a crowded urban real-time in the mapped area landmarkstill In addition, driving functions require reliable updates at sufficiently dimensional maps (Thrun and in monte localization (MCL) for real-time localization the mapped area landmarkbased techniques techniques et carlo al., 2006). An orextension extension to scenario functionsisrequire require reliable position position updates at sufficiently sufficiently functions reliable updates at based (Thrun et al., 2006). An to scenario is still challenging. In addition, automated driving based techniques (Thrun et al., 2006). An extension to functions require reliable position updates at sufficiently high sample rates for robust operation. Therefore, requirereal-time localization in the mapped area orextension landmarkbased techniques (Thrun et al., 2006). An was to high conventional discrete occupancy grid maps proposed high sample sample rates rates for for robust robust operation. operation. Therefore, Therefore, requirerequireconventional discrete occupancy grid maps was proposed reliable position updates at suitable sufficiently conventional discrete occupancy grid maps was proposed high sample rates for robust operation. Therefore, requirements to aarequire map-based localization technique for based techniques (Thrun et (2011) al., grid 2006). An extension to functions conventional discrete occupancy maps was proposed by O’Callaghan and Ramos who introduce Gausments to map-based localization technique suitable for mentssample to aa map-based map-based localization technique suitable for by O’Callaghan O’Callaghan and and Ramos Ramos (2011) (2011) who who introduce introduce GausGaus- high rates for robust operation. Therefore, requireby ments to localization technique suitable for our framework include sparseness of the digital map maconventional discrete occupancy grid maps was proposed by O’Callaghan and Ramos (2011) who introduce Gaussian process occupancy maps (GPOM) as a continuous our framework include sparseness of the digital map maour framework include sparseness of the digital map masian process process occupancy occupancy maps maps (GPOM) (GPOM) as as a continuous continuous ments towell a map-based localization suitable for sian sparseness oftechnique theof map material as as fast real-time operation the localization by andofRamos (2011) introduce Gaus- our sian process occupancy (GPOM) as aainto continuous 2D O’Callaghan representation anmaps area whichwho takes into account terialframework as well well as as include fast real-time real-time operation ofdigital the localization localization terial as fast operation of the 2D representation of an an area which takes account our framework include sparseness of the digital map ma2D representation of area which takes into account terial as well as fast real-time operation of the localization algorithm. sian process occupancy as aetcontinuous 2D representation of neighboring anmaps area (GPOM) which dependencies between cells. Hata al. (2017) algorithm. algorithm. dependencies between neighboring cells.takes Hatainto et al. al.account (2017) terial as well as fast real-time operation of the localization dependencies between neighboring cells. Hata et (2017) algorithm. 2D representation of neighboring an area which takes into dependencies between cells. Hata et al.account (2017) algorithm. dependencies between neighboring cells. Hata et al. (2017) 2405-8963 © 2019, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved.

Copyright © 2019 IFAC 366 Copyright 2019 366 Copyright © under 2019 IFAC IFAC 366 Control. Peer review© responsibility of International Federation of Automatic Copyright © 2019 IFAC 366 10.1016/j.ifacol.2019.09.059 Copyright © 2019 IFAC 366

2019 IFAC AAC Orléans, France, June 23-27, 2019

Tobias Quack et al. / IFAC PapersOnLine 52-5 (2019) 366–371

This paper presents a map-based localization method which meets the aforementioned requirements. As many other contributions, we apply a monte carlo localization algorithm which is in our case optimized for fast position updates in real-time operation. Our algorithm runs on a test vehicle and exploits a large amount of data points from a 360◦ LiDAR sensor. The map definition we use is based on line segments and provides a storage efficient abstraction of the mapped geometry. The remainder of this paper is structured as follows: In section 2, we introduce our localization method while focusing specifically on the map update mechanism and the timing of the different processes. Section 3 provides an overview of the test facilities, the sensor setup and the vehicle we use for validation. Results of our experimental testing are presented and discussed in section 4 and section 5 provides a conclusion and outlook of our work.

367

50.9082

50.9081

50.9080

50.9079

2. REAL-TIME LOCALIZATION 50.9078 Monte carlo localization (MCL) is a popular method for map-based positioning due to its applicability to a large variety of sensor and map configurations and its ability to handle non-linear process models, non-Gaussian noise characteristics and multimodal probability distributions. The algorithm is based on particle filtering (Doucet and Johansen, 2011) and includes the following three major steps: The prediction step uses a model of the process – in our case the motion of the vehicle – to predict the future states. We define the three states position x, position y and orientation φ and employ a kinematic single track model (Heißing, 2007) to describe the vehicle movement. Side slip angles occurring at high lateral accelerations are neglected. As process inputs, we use the yaw rate and the rear wheel odometry of our vehicle. The prediction is calculated for each particle and includes error terms which are in our implementation sampled from Gaussian random distributions. The measurement update takes into account the information from the LiDAR sensor and the digital map to compute weights for each particle. Details on our implementation are given in section 2.2. The resampling step uses the results of the measurement update to remove or duplicate particles with the respective probability given by the particle weight. By doing so, the particle cloud is kept focused around the estimated position and particles with inferior localization hypotheses are removed. Since the general algorithm is well described in literature (see e.g. Thrun et al. (2006), Gustafsson et al. (2002)) we focus on the specific aspects of our implementation with sparseness of the map and fast real-time operation being major objectives. 2.1 Map definition The digital map we use in this contribution is targeted to include geometric objects which can be detected accurately and reproducibly by automotive LiDAR sensors. In urban environments, these objects comprise buildings, 367

6.2267

6.2269

6.2271

6.2273

Fig. 1. Test intersection with map elements referenced in WGS84 coordinates walls, street furniture and elements of the road or public transport infrastructure. We define a 2D horizontal layer in which the edges of these objects are represented as line segments. It is therefore sufficient to store a sequence of 2D georeferenced points which – when interconnected by straight lines – are an abstraction of the geometries detectable by a rangefinder. Figure 1 shows an aerial image of the test intersection we use for the validation of our algorithm. In this scenario, the map was generated manually from GPS data. The concrete walls of the building dummies on the test site are projected on the horizontal map plane. In the image, blue lines represent the respective line segments. The map information is defined in WGS84 coordinates (EPSG:4326) so that the monte carlo localization algorithm yields georeferenced coordinates. Results can there fore be fused with or compared against GNSS data. As a data format for the digital map, we use a lightweight XML document similar to the .gpx standard for GNSS tracks which avoids unnecessary overhead. In this format, the map of the test intersection requires less than 4 kB of storage space. 2.2 LiDAR based correction As introduced in section 2 one major step in monte carlo localization is the measurement update which combines information from the vehicle’s environment sensors with knowledge from the digital map in order to calculate weights for each particle of the filter. The weight wi of a particle i is defined as the conditional probability of obtaining the LiDAR measurement z given the pose hypothesis xi of the particle: wi = p(z|xi ). Assuming a Gaussian distribution of the noise on the LiDAR’s distance measurement, w can thus be calculated by comparing z to a virtual sensor measurement which is obtained by simulating the expected sensor output based on the pose hypothesis xi and the digital map. Since each scan of our

2019 IFAC AAC 368 Orléans, France, June 23-27, 2019

Tobias Quack et al. / IFAC PapersOnLine 52-5 (2019) 366–371

360◦ LiDAR rangefinder in the selected 2D plane contains up to 900 single distance measurements, each of which must be simulated for each particle in order to obtain the respective weight, a large number of rays must be processed during the measurement update. For efficient operation of the algorithm, it is therefore crucial to avoid unnecessary calculations in the resulting nested loops. In contrast to MCL variants that operate on occupancy grid maps and use computationally expensive ray tracing operations to obtain the virtual measurements for each particle, our map definition requires the calculation of the intersection point of the simulated LiDAR ray with the relevant line segment of the map. The relevant line segment is defined by two conditions: (a) The intersection point of the respective line with the simulated LiDAR ray must lie between the two end points p1 = (x1 , y1 ) and p2 = (x2 , y2 ) of the segment. (b) Among all line segments which fulfill (a), the one with the smallest distance between the intersection point and the position hypothesis of the current particle pp = (xp , yp ) must be chosen because it represents the first object in the line of sight of the LiDAR sensor. The virtual measurement dsim for a LiDAR ray with the angle φray is then given by equation 1. dsim =

(y1 − yp )(x2 − x1 ) + (xp − x1 )(y2 − y1 ) (x2 − x1 ) · sin(φray ) − (y2 − y1 ) · cos(φray )

(1)

In order to minimize the number of evaluations of equation 1 we implement two pre-check mechanisms. For each particle, the angles α1 from (xp , yp ) to (x1 , y1 ) and α2 from (xp , yp ) to (x2 , y2 ) are calculated in advance so that condition (a) can be tested quickly by checking if φray ∈ [α1 ; α2 ] for each LiDAR ray. Secondly, a lower limit dmin for the distance between (xp , yp ) and any point on the line segment is calculated as:    dmin = max 0, max (p1 − pp )2 , (p2 − pp )2  − (p2 − p1 )2

(2)

Assuming a maximum range rmax of the sensor, all line segments with dmin > rmax can then be neglected for all LiDAR ray simulations of the current particle. Despite the aforementioned optimizations the measurement update step in our localization framework still requires too much computation time to be run in real-time at a rate of 50 Hz if all data from the LiDAR scan is exploited and the number of particles is chosen sufficiently high for a robust and accurate localization solution. Therefore, in the following section 2.3, we introduce our method for fast real-time operation of the MCL.

u

u

p p p p p p p r p p p p p p p p p r t [s] preconditions for new merging measurement measurement update update results, step met resampling Fig. 2. Timing scheme of the MCL algorithm; p: prediction, u: measurement update, r: resampling particle. The prediction is also very accurate in short term, however in long term, the integration mechanism causes an increasing drift error – the problem of dead reckoning. The correction of this drift error is achieved by including globally referenced position information from the LiDAR and the digital map which requires a higher computational effort. However, at the same time, the measurement update mechanism must not be weighted too high in order to avoid an exceeding concentration of the particle cloud which does not reflect the true probability distribution. It can only provide new information to the weighting of the particles if the vehicle has moved significantly since the last correction so that new ranges are measured by the LiDAR sensor. Indeed, we define a minimum distance that the vehicle must have moved before a new measurement update can be started. These characteristics of the sensor setup favor a structure of the algorithm in which the prediction and the measurement update are executed independently. In our implementation, we use parallelization to define two main threads: The prediction thread is run at a fixed sample rate of 50 Hz and processes the fast IMU and odometry measurements to propagate the particle cloud. The measurement update thread runs independently and is only started if the precondition, i.e. the minimum distance traveled since the last correction, is met. Figure 2 depicts this parallel timing structure. It can be seen that once started, the measurement update thread runs in parallel to the prediction thread until all particle weights have been calculated. At this point, the weights are merged into the prediction thread and reprocessing is performed. Since in the prediction, no particles are added or removed, the delay between the start of correction and the reprocessing is not critical. While the correction is calculated, the filter propagates a valid cloud of particles, only the “cleaning up”, i.e. the removal or duplication of particles which ensures an effective concentration of the cloud, is carried out with a time delay. This parallel timing structure guarantees fast position updates at a fixed sample rate of 50 Hz while allowing for a complete exploitation of the LiDAR and map information at the same time.

2.3 Timing and Parallelization

2.4 Parametrization

In our MCL implementation, two main sources of sensor data are used to provide information about the vehicle pose: In the prediction step, the vehicle’s odometry including the inertial information is processed. This step is computationally inexpensive because it only involves the evaluation of the single-track model model for each

The performance of the algorithm strongly depends on the choice of multiple parameters. Table 1 shows an overview of the main parameters used in our implementation. The parameters have been tuned in a simulation environment in order to achieve accuracy, robustness, fast convergence and real-time operation.

368

2019 IFAC AAC Orléans, France, June 23-27, 2019

Tobias Quack et al. / IFAC PapersOnLine 52-5 (2019) 366–371

369

Table 1. MCL Parameters Parameter sample time number of particles minimum distance between two corrections standard deviation odometry error standard deviation yaw rate error standard deviation LiDAR distance error standard deviation random xy error

Value 0.02 s 1000 0.3 m 0.0025 m 0.001 rad s−1 1.2 m 0.01 m

While the Gaussian errors we assume for odometry and yaw rate have been identified from calibration measurements of the respective sensor systems, the standard deviation of the LiDAR distance error is chosen much higher than the real deviations of the sensor. This choice reduces the problem of sample impoverishment i.e. the strong concentration of particles in a small area which does not reflect the true density. Similarly, we add random noise to the x and y positions of the particles in order to improve the ability of the algorithm to recover from situations in which the filter temporarily converges against an inaccurate position, e.g. due to a mismatch between digital map and environment. In order to prevent LiDAR scans taken from a specific viewpoint with the vehicle standing still from being weighted too high, a minimum distance that the vehicle must travel between two measurement updates is defined. A suitable tradeoff between accuracy and computational load which enables real-time operation at 50 Hz on our embedded controller was found with a number of particles of 1000.

Fig. 3. Aerial view of the test intersection at Aldenhoven Testing Center

3. EXPERIMENTAL SETUP For the experimental implementation and validation we make use of a suitable test environment and a purposemade test vehicle which are introduced in the following sections. 3.1 Test facilities With the “CERMcity” urban validation area, the Aldenhoven Testing Center (ATC) of RWTH Aachen University features a test environment for the development and testing of vehicle automation systems in inner-city scenarios (ATC GmbH, 2017). Since the site is separated from the public traffic area, tests with prototypical vehicles and systems can be undertaken on site. In our work, we use the test intersection shown in Figure 3. Apart from the road infrastructure and building dummies, the test intersection also provides a communication network based on 802.11p and LTE mobile networks. Mobile edge computing technology enables infrastructure-based data storage and computation capacity which can be used for continuous learning and the distribution of digital maps (Quack et al., 2018). 3.2 Test vehicle The test vehicle we use for validation is the “IRT Buggy”, a model-size test platform for autonomous driving with a modular sensor and communication setup (Reiter et al., 2014). For the validation of the MCL, the sensor setup comprises a Velodyne VLP-16 LiDAR, an Analog Devices ADIS16405BMLZ inertial measurement unit, a uBlox 369

Fig. 4. IRT Buggy with mounted sensors and on board computer for localization NEO-6T low-cost GNSS receiver and wheel odometers based on the AMS AS5040 magnetic encoder on both rear wheels. Reference data for the geolocation of the vehicle is provided by a high-end Leica Viva GNSS GS10 RTK receiver with a localization accuracy of 8 mm + 1 ppm (rms). The MCL is run on a Neousys Nuvo-5108VTC embedded computer featuring an Intel i7-6700TE CPU with 4 cores and 3.4 GHz. The operating system is Xubuntu Linux and the software is programmed in C++11. Figure 4 shows the IRT Buggy fully equipped with the described sensors. 4. VALIDATION In our first test scenario, the vehicle approaches the test intersection using the low-cost GPS receiver only. Once in the mapped area, the MCL is activated and the particle cloud is initialized around the current GNSS position assuming a Gaussian distribution. The respective standard deviations are σxy = 4 m for the position and σφ = 0.3 rad for the orientation angle. Figure 5 depicts the resulting trajectory in a local east-north-up (ENU) coordinate frame together with the map elements used for localization. It also shows the particle cloud at the three time steps t = 0 s, t = 10 s and t = 20 s. The comparison with the high-end reference GNSS shows that the MCL tracks the vehicle trajectory well. In addition, it can be seen that the

2019 IFAC AAC 370 Orléans, France, June 23-27, 2019

ENU Orientation [rad]

Tobias Quack et al. / IFAC PapersOnLine 52-5 (2019) 366–371

20 10 Particle Cloud GPS Reference MCL Line Map

0 -10 -20

-30

-20

-10

0 10 20 30 ENU Position x [m]

40

-1.5 -2 -2.5

0

5

10

15 Time [s]

20

25

Fig. 7. Orientation of the test vehicle in MCL compared to the GNSS based reference 4 3 2 1 0 4 3 2 1 0 10 8 6 4 2 0 0 5 10 15 20 25 Time [s] σφ [ ◦ ]

Localization Error [m]

Fig. 5. Path of the test vehicle through the test intersection in local East North Up coordinate frame 1.5 1.25 1 0.75 0.5 0.25 0 0

GPS Reference MCL

-1

σx [m]

-30

-0.5

σy [m]

ENU Position y [m]

30

5

10

15 Time [s]

20

25

Fig. 8. Standard deviations of the particle filter states

Fig. 6. Euclidean localization error (x, y) of the MCL particle cloud converges from a broad initial distribution to a dense, focused distribution which represents the actual position. Quantitative information about the accuracy is given in Figure 6. Starting from the initial error of the low-cost GNSS of about 1.5 m, the MCL converges quickly towards the correct vehicle location. After about 3 s, the error is below 0.2 m and never exceeds that value again during tracking in the mapped area. Since our reference GNSS receiver does not measure the orientation of the vehicle directly, we obtain reference data by calculating the orientation between each two subsequent position measurements. Figure 7 shows a comparison of the reference orientation with the orientation calculated by the MCL algorithm. Similarly to the positioning solution, the orientation quickly converges towards the correct value and no additional errors occur during tracking. It is also notable that the MCL orientation is very smooth due to the processing of the high-frequency IMU yaw rate signal. Naturally, the reference orientation exhibits noise caused by the amplification of small positioning errors in the calculation method so it is not sensible to infer absolute values for the orientation errors. From the distribution of the particles over the three states position x, position y and orientation φ, a standard deviation can be calculated for each time step. As shown in Figure 8, the trends of the standard deviations represented by the particle filter match well with the actual positioning error. However, the absolute value of the esti370

mated standard deviations are generally higher than the real uncertainty. This is due to the deliberate widening of the particle cloud for improved robustness of the filtering algorithm (see section 2.4). The performance of the localization algorithm in a more confined environment with potential ambiguities in LiDAR scan matching is evaluated on a parking deck. Figure 9 shows the tracking results of the MCL in comparison with the reference GNSS together with the respective line map. Again, the MCL yields accurate positioning without requiring satellite information while tracking. The root mean square error of the localization is 0.116 m. In the parking scenario, comparative data from the vehicle’s lowcost GPS receiver was acquired which clearly emphasizes the gain in localization precision provided by the MCL. Information about the computational time required for different processing steps of the MCL algorithm is shown in Table 2. Due to the random sampling conducted in the MCL algorithm, the values are averaged over 5 test runs in the intersection scenario. It can be seen that with 7.07 ms, the average prediction time is safely below the limit of 20 ms for an operation at 50 Hz. The respective standard deviation is rather low because the computational complexity is independent of the current vehicle state. In contrast, the time required for calculating the measurement update has a stronger variation because the calculation of the weights depends on the number and geometry of map segments inside the LiDAR range and the number of echoes received by the LiDAR in the current scan. However, the average time of 133.27 ms required for

2019 IFAC AAC Orléans, France, June 23-27, 2019

Low-Cost GPS GPS Reference MCL Line Map

10 ENU Position y [m]

Tobias Quack et al. / IFAC PapersOnLine 52-5 (2019) 366–371

5 0 -5 -10 -15 -20 0

5

10 15 20 25 30 35 40 45 50 ENU Position x [m]

Fig. 9. Localization result for the parking scenario in local East North Up coordinate frame Table 2. Timing of the MCL processing steps average prediction time standard deviation prediction time average measurement update time standard deviation measurement update time

7.07 ms 3.36 ms 133.27 ms 34.67 ms

the measurement update guarantees a robust convergence and accurate tracking of the particle filter in combination with the parallelized timing scheme (see section 2.3). 5. CONCLUSION In this contribution, we have proposed an algorithm for fast real-time localization based on a sparse digital map and a variant of the monte carlo localization algorithm. Our localization scheme features specific adaptations targeted to guarantee position updates at a high sample rate of 50 Hz while exploiting all data points from the horizontal 2D layer of a 360◦ LiDAR sensor. For validation we have conducted experiments at an automotive test site for connected urban mobility and on a parking deck using a prototypical test vehicle. Our algorithm could be proven to achieve robust real-time operation, fast convergence and accurate localization with errors below 0.2 m while being independent of satellitebased positioning systems during tracking. Since the current test scenarios presented in this work use relatively clear, static outdoor environments, the next step is the adaptation for more cluttered, dynamic situations. In ongoing research, we implement a digital map which consists of several layers for both static and semi-dynamic objects such as parked vehicles, temporary road works etc. Infrastructure-based learning strategies are used to continuously update the digital map in order to enable reliable localization even under challenging conditions. REFERENCES ATC GmbH (2017). Aldenhoven Testing Center: Tracks. URL http://www.atc-aldenhoven.de/en/ tracks.html. Doucet, A. and Johansen, A.M. (2011). A tutorial on particle filtering and smoothing: Fifteen years later. In D. Crisan and B. Rozovsky (eds.), Nonlinear Filtering Handbook, 656–704. Oxford University Press. 371

371

Gustafsson, F., Gunnarsson, F., Bergman, N., Forssell, U., Jansson, J., Karlsson, R., and Nordlund, P.J. (2002). Particle filters for positioning, navigation, and tracking. IEEE Transactions on Signal Processing, 50(2), 425– 437. Hata, A.Y., Ramos, F.T., and Wolf, D.F. (2017). Monte Carlo Localization on Gaussian Process Occupancy Maps for Urban Environments. IEEE Transactions on Intelligent Transportation Systems, 1–10. doi:10.1109/ TITS.2017.2761774. Heißing, B. (2007). Fahrwerkhandbuch: Grundlagen, Fahrdynamik, Komponenten, Systeme, Mechatronik, Perspektiven. ATZ-MTZ-Fachbuch. Vieweg. O’Callaghan, S.T. and Ramos, F.T. (2011). Gaussian process occupancy maps. The International Journal of Robotics Research, 31(1), 42–62. doi:10.1177/ 0278364911421039. Quack, T., B¨osinger, M., Heßeler, F.J., and Abel, D. (2018). Infrastructure-based digital maps for connected autonomous vehicles. at - Automatisierungstechnik, 66(2), 183–191. doi:10.1515/auto-2017-0100. Reiter, M., Alrifaee, B., and Abel, D. (2014). Model Scale Experimental Vehicle as Test Platform For Autonomous Driving Applications. FISITA 2014 World Automotive Congress. Rohde, J., Jatzkowski, I., Mielenz, H., and Z¨ollner, J.M. (2016). Vehicle pose estimation in cluttered urban environments using multilayer adaptive Monte Carlo localization. In 2016 19th International Conference on Information Fusion (FUSION), 1774–1779. Schiotka, A., Suger, B., and Burgard, W. (2017). Robot localization with sparse scan-based maps. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 642–647. doi:10.1109/ IROS.2017.8202219. Seif, H.G. and Hu, X. (2016). Autonomous Driving in the iCity – HD Maps as a Key Challenge of the Automotive Industry. Engineering, 2(2), 159–162. doi:10.1016/J. ENG.2016.02.010. URL http://www.sciencedirect. com/science/article/pii/S2095809916309432. Thrun, S., Burgard, W., and Fox, D. (2006). Probabilistic robotics. Intelligent robotics and autonomous agents series. MIT Press, Cambridge Mass. TomTom (2017). ROADDNA: Robust and Scalable Localization Technology. URL https://automotive. tomtom.com/wordpress/wp-content/uploads/2017/ 01/RoadDNA-Product-Info-Sheet-1.pdf. u-blox AG (2018). NEO-M8 Data Sheet. URL https:// www.u-blox.com/de/product/neo-m8-series.