Efficient Fleet Absolute Localization and Environment Re-Mapping*

9th IFAC Symposium on Intelligent Autonomous Vehicles 9th Intelligent Autonomous June 29 -Symposium July 1, 2016.on Messe Leipzig, Germany Vehicles 9th IFAC IFAC Symposium on Intelligent Autonomous Vehicles 9th IFAC on Intelligent Autonomous Vehicles June 29 --Symposium July 1, 2016. Messe Leipzig, Germanyonline Available at www.sciencedirect.com June 29 July 1, 2016. Messe Leipzig, June 29 - July 1, 2016. Messe Leipzig, Germany Germany

ScienceDirect IFAC-PapersOnLine 49-15 (2016) 236–241

Efficient Fleet Absolute Localization Efficient Fleet Absolute Localization  Efficient Fleet Absolute Localization Environment Re-Mapping Environment Re-Mapping 

and and and

Laurent DELOBEL ∗∗ Romuald AUFRERE ∗∗ Laurent AUFRERE Laurent DELOBEL Romuald AUFRERE∗∗ ∗∗ Roland DELOBEL CHAPUIS ∗∗∗ Romuald Thierry CHATEAU Laurent DELOBEL Romuald AUFRERE ∗ ∗ Roland CHAPUIS CHATEAU ∗ Thierry Roland CHAPUIS Thierry CHATEAU Roland CHAPUIS Thierry CHATEAU ∗ ∗ e Blaise PASCAL, Aubi`ere, 63178 ∗ Institut PASCAL - Universit´ ∗ Universit´ e Blaise PASCAL, Aubi`eere, 63178 ∗ Institut Institut PASCAL Universit´ FrancePASCAL (e-mail: [email protected]). Institut PASCAL Universit´ee Blaise Blaise PASCAL, PASCAL, Aubi` Aubi`ere, re, 63178 63178 France (e-mail: [email protected]). France France (e-mail: (e-mail: [email protected]). [email protected]).

Abstract: Abstract: Abstract: As robots leave the simple and static environments to more complex and dynamic ones, they will Abstract: As robots leave the simple and static environments to more and dynamic ones, theydata. will As leave simple and environments to complex and ones, have to improve their localisation abilities and to deal withcomplex heterogeneous and imprecise As robots robots leave the the simple and static static environments to more more complex and dynamic dynamic ones, they they will will have to improve their localisation abilities and to deal with heterogeneous and imprecise data. have to improve their localisation abilities and to deal with heterogeneous and imprecise data. In this paper, we present a general cooperative framework designed to localize in an absolute have to improve their localisation abilities and to deal with heterogeneous and imprecise data. In this paper, we present a general designed to localize in an absolute In paper, we cooperative framework designed to in way a fleet of heterogeneous vehicles.cooperative Depending framework on the sensors it embeds, each vehicle localize In this this paper, we present present a a general general cooperative framework designed to localize localize in an an absolute absolute way a fleet of heterogeneous vehicles. Depending on the sensors it embeds, each vehicle localize way a ausing fleet aof ofGNSS heterogeneous vehicles. GPS), Depending on the the sensors sensors it embeds, embeds, each vehicle localize itself system (typically an orientation system (a compass forvehicle instance), the way fleet heterogeneous vehicles. Depending on it each localize itself using a GNSS system (typically GPS), an orientation system (a compass for instance), the itself using a GNSS system (typically GPS), an orientation system (a compass for instance), the detection others robots in the neighbourhood (typically with(a a LIDAR) the detection itself usingofathe GNSS system (typically GPS), an orientation system compass and for instance), the detection of the others robots in the neighbourhood with aa LIDAR) the detection detection of in neighbourhood (typically with and the of visible geo-referenced features in the map (eg. wall,(typically poles, etc...). map and features are often detection of the the others others robots robots in the the neighbourhood (typically with These a LIDAR) LIDAR) and the detection detection of visible geo-referenced features in the map (eg. wall, poles, etc...). These map features are often of visible geo-referenced features in the map (eg. wall, poles, etc...). These map features imprecise (as is typically the case with collaborative public maps such as OpenStreetMap). Our of visible geo-referenced features in the map (eg. wall, poles, etc...). These map features are are often often imprecise (as is typically the case with collaborative public maps such as OpenStreetMap). imprecise allows (as is is typically typically the casefeatures with collaborative collaborative public mapsframework. such as as OpenStreetMap). OpenStreetMap). Our approach to update these positions inpublic the same We first presentOur the imprecise (as the case with maps such Our approach allows towe update these features positions in the same framework. We first present the approach allows these features positions in same We present the filtering developed solve the classical over-convergence problem using the SCI approachapproach allows to to update update these to features positions in the the same framework. framework. We first first present the filtering approach we developedfilter. to solve the classical over-convergence problem using the SCI filtering approach we the classical over-convergence problem using (Split Covariance Map relative detection being simultaneously the filtering approach Intersection) we developed developed to to solve solve thefeature classical over-convergence problem using the the SCI SCI (Split Covariance Intersection) filter. Map feature relative detection being simultaneously the (Split Covariance Intersection) filter. Map feature relative detection being simultaneously the main information source as well as compute-time expensive, we show how in the same framework (Split Covariance Intersection) filter. Map feature relative detection being simultaneously the main information source as as we how in same main information source as well well as compute-time compute-time expensive, we show show howwhich in the the avoids same framework framework we optimize resource usage thanks to an entropyexpensive, optimization strategy all sensor main information source as well as compute-time expensive, we show how in the same framework we optimize resource usage thanks to an entropy optimization strategy which avoids sensor we optimize resource usage thanks to an entropy optimization strategy which avoids all data fusion and instead selects the best at each time step. strategy which avoids all we optimize resource usage thanks to anone entropy optimization all sensor sensor data fusion and instead selects the best one at each time step. data fusion and instead selects the best one at each time step. data fusion and instead selects the best one at each time step. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Absolute localization, cooperative localization, environment mapping, Keywords: Absolute Absolute localization, cooperative localization, environment Keywords: localization, cooperative localization, environment mapping, over-convergence, entropy optimization, covariance intersection filter. mapping, Keywords: Absolute localization, cooperative localization, environment mapping, over-convergence, entropy entropy optimization, optimization, covariance covariance intersection intersection filter. filter. over-convergence, over-convergence, entropy optimization, covariance intersection filter. 1. INTRODUCTION (2006), a simple full Kalman filter which state contains 1. INTRODUCTION INTRODUCTION (2006), full Kalman filter which stateis contains 1. (2006), simple full filter which the stateaaa simple of all robots and which the full 1. INTRODUCTION (2006), simple full Kalman Kalman filter covariance which state state contains contains state of all robots and which covariance is the Presently, robotic applications develop and gain widespread the the state of all robots and which covariance is the full covariance of the whole state is developed. This kindfull of the state of all robots and which covariance is the full Presently, robotic applications develop and gain widespread Presently, robotic applications developautonomous and gain gain widespread widespread of the whole state is developed. This kind of use (autonomous driving, convoy driving, covariance Presently, robotic car applications develop and covariance of the whole state is developed. This kind filter takes all potential correlations between vehicles into covariance of the whole state is developed. This kind of of use (autonomous (autonomous car car driving, convoy autonomous driving, use driving, convoy autonomous driving, filter takes all potential correlations between vehicles into semi-autonomous search and rescue robots to name a use (autonomous car driving, convoy autonomous driving, filter filter takes takesHowever, all potential potential correlations between vehicles into account. it fails to scale between up, sincevehicles for a fleet all correlations into semi-autonomous search and rescue robots to name a semi-autonomous and robots to aa account. However, it of fails tocovariance scale up, up, since since for for a fleet fleet 2 few). In order to search make these applications it is semi-autonomous search and rescue rescue robotsfeasible, to name name However, it a of N robots, the size theto is O(N ). account. However, it fails fails to scale scale up, matrix since for a fleet 2 few). In In order to make make these applications applications feasible, it is is account. 2 few). to these feasible, it N robots, the size of the covariance matrix is O(N necessary to have a localization. In this paper, we focus 2 ). few). In order order to make these applications feasible, it is of of N robots, the size of the covariance matrix is O(N ). Thus matrix operations are slower in term of CPU usage of N robots, the size of the covariance matrix is O(N ). necessary to have a localization. In this paper, we focus necessary to localization. this we matrix operations are slower in term of CPU usage on the localization a fleet of In vehicles. It is Thus necessary to have have aa of localization. Inrobotic this paper, paper, we focus focus Thus matrix operations are slower in term of CPU usage (inversion of a big matrix is costly). Also, it is difficult to Thus matrix operations are slower in term of CPU usage on the localization of a fleet of robotic vehicles. It is on localization of aa fleet robotic is of matrix Also, is well known that using measurements like inIt on the the localization of relative fleet of of robotic vehicles. vehicles. Itthe is (inversion (inversion of aaa big big matrix is acostly). costly). Also,init it the is difficult difficult to share between robots suchis big object case of to a of big matrix is costly). Also, it is difficult to well known known thatcase using relative measurements like in in the (inversion well like share between robots such a big object in the case of of a a aforementioned willrelative lead tomeasurements the rumour phenomenon well known that that using using relative measurements like in the the share between robots such a big object in the case large fleet. share between robots such a big object in the case of a aforementioned case will will lead leadato to the rumour rumour phenomenon aforementioned case the fleet. and to overconvergence Kalman filter, phenomenon like in Julier large aforementioned case willwith lead to the rumour phenomenon large fleet. large fleet. and to overconvergence with a Kalman filter, like in Julier and overconvergence with Kalman filter, like in Uhlmann (2001a). Also, methods pre- Using separate state with a Kalman filter is however and to to overconvergence with aasome Kalman filter,can likeuse in aJulier Julier Using separate stateetwith with Kalman filter is the however and Uhlmann Uhlmann (2001a). Also, some methods can use aa preprestate aaa Kalman is feasibleseparate (see Karam al. (2006)) but filter only in case and (2001a). Also, some methods can use existing map as the way to localize like in Parsley and Using Using separate state with Kalman filter is however however and Uhlmann (2001a). Also, some methods can use a prefeasible (see Karam et al. (2006)) but only in the case existing map as the way to localize like in Parsley and feasible (see Karam et al. (2006)) but only in the case where the vehicle never communicates its estimated existing map as way like and Julier (Aynaud al., 2014a). this paper, (see Karam et al. (2006)) but only in the state case existing(2010) map and as the the way to toetlocalize localize like in inInParsley Parsley and feasible where the vehicle never communicates its estimated state Julier (2010) and (Aynaud et al., 2014a). In this paper, where the vehicle never communicates its estimated state to other vehicle, and maintains an estimate for each of the Julier (2010) and (Aynaud et al., 2014a). In this paper, we take(2010) into account a publicetprior and imprecise as where the vehicle never communicates its estimated state Julier and (Aynaud al., 2014a). In this map paper, other vehicle, and maintains an estimate for each of the take into into account a public public prior and and Therefore, imprecise map map as to to other vehicle, and maintains an estimate for each of the other vehicles. we take account a prior imprecise as awe partial source for vehicle localization. in this to other vehicle, and maintains an estimate for each of the we take into account a public prior and imprecise map as other vehicles. a partial source for vehicle localization. Therefore, in this other vehicles. aintroduction, localization. Therefore, in wefor willvehicle present first the problems related to other vehicles. a partial partial source source for vehicle localization. Therefore, in this this et al. (2013), a solution to this problem introduction, weofwill will present first the the problems problems related to to In Carrillo-Arce introduction, we present first related the localization a fleet of vehicles, the re-precision Carrillo-Arce et al. aa solution to problem introduction, we will present first thethen problems related to In In Carrillo-Arce et al. (2013), (2013),and solution to this thisalgorithm problem is presented. The cooperative decentralized In Carrillo-Arce et al. (2013), a solution to this problem thea localization localization of aa finally fleet of of vehicles, vehicles, then the re-precision re-precision the of the of map topic, and thethen organization of this is is presented. The cooperative and decentralized algorithm the localization of a fleet fleet ofpresent vehicles, then the re-precision presented. The cooperative and decentralized algorithm that is used The is based on a Covariance Intersection (CI) is presented. cooperative and decentralized algorithm of a map topic, and finally present the organization of this of paper. is used is on aa Covariance Intersection (CI) of aa map map topic, topic, and and finally finally present present the the organization organization of of this this that that is used is based on Covariance Intersection (CI) filter in order to based take into account for possible correlations that is used is based on a Covariance Intersection (CI) paper. paper. filter in order to take into account for possible correlations paper. filter in in order order to state take into into account for possible possible correlations between robot andaccount measurement. This leads to filter to take for correlations 1.1 Fleet localization between and measurement. This between robot robot state andcooperative measurement. This leads leads to to consistent resultsstate for the localization. between robot state and measurement. This leads to 1.1 Fleet Fleet localization 1.1 consistent results for the cooperative localization. 1.1 Fleet localization localization consistent results for the cooperative localization. In order to be able to correctly localize a fleet of robotic consistent results for the cooperative localization. In order order to be able to correctly correctly localize fleet of of robotic robotic 1.2 Re-mapping problem In to localize aa devices, it be is able necessary to take into account In order to to be able to correctly localize a fleet fleet of possible robotic devices, it is necessary to take into account possible problem devices, necessary to possible 1.2 Re-mapping Re-mapping correlations without over-convergence. Mourikis et al. 1.2 devices, it it is is necessary to take take into into Inaccount account possible Re-mapping problem problem correlations without without over-convergence. over-convergence. In In Mourikis Mourikis et et al. al. 1.2 correlations this section, we deal with the re-mapping problem, since  correlations without over-convergence. In Mourikis et al. In This work was supported by the French program ”Investissements In this section, we deal with the re-mapping problem, since  In section, deal with re-mapping problem, since in wewe start pre-existing map. Pre-existing This work was supported the French program  In this work, section, we dealwith witha the the re-mapping problem, since d’Avenir” grant byby Research”Investissements Agency (ANR), This was supported by the French ”Investissements  This work work wasmanaged supported bythe theNational French program program ”Investissements in this work, we start with a pre-existing map. Pre-existing in this work, we start with a pre-existing map. Pre-existing d’Avenir” grant managed by the National Research Agency (ANR), maps solves efficiently the problem of the initial estimate. in this work, we start with a pre-existing map. Pre-existing the European Fund (ERDF) and (ANR), Region d’Avenir” grantRegional managed Development by the the National National Research Agency (ANR), d’Avenir” grant managed by Research Agency maps the of estimate. the European Development Fund (ERDF) and Region maps solves solves efficiently the problem problem of the the initial initial estimate.a Some vehicleefficiently localizations can be based on exploiting Auvergne, in theRegional framework of the IMobS3 Laboratory Excellence the Regional Development Fund (ERDF) Region maps solves efficiently the problem of the initial estimate. the European European Regional Development Fund (ERDF) ofand and Region Some vehicle localizations can be based on exploiting a Auvergne, in the framework of the IMobS3 Laboratory of Excellence Some vehicle localizations can be based on exploiting a (ANR-10-LABX-16-01). prior public However,can sincebeobserving map and Auvergne, in in the the framework framework of of the the IMobS3 IMobS3 Laboratory Laboratory of of Excellence Excellence Some vehiclemap. localizations based onthis exploiting a Auvergne, (ANR-10-LABX-16-01). prior public map. However, since observing this map and (ANR-10-LABX-16-01). prior public map. However, since observing this map and (ANR-10-LABX-16-01). prior public map. However, since observing this map and Copyright © 2016, 2016 IFAC 236Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright 2016 IFAC 236 Peer review© of International Federation of Automatic Copyright © 2016 IFAC 236 Copyright ©under 2016 responsibility IFAC 236Control. 10.1016/j.ifacol.2016.07.741

IFAC IAV 2016 June 29 - July 1, 2016. Messe Leipzig, Germany Laurent Delobel et al. / IFAC-PapersOnLine 49-15 (2016) 236–241

improve its precision with vehicles that also uses it to selflocalize may lead to strong correlations, using a Kalman filter may lead to inconsistent results. Thus, like for the previous section, the framework should be able to cope with this problem. In Hentschel and Wagner (2010), a semi-autonomous vehicle is able to travel in an OpenStreetMap mapped environment, and the trajectory is reconstructed out of the OpenStreetMap cartography. Despite being interesting for the feasibility, this paper doesn’t address the problems related to inacurate mapping and landmark ambiguity. In Parra et al. (2011), the map-related localization is addressed by considering each vehicle moving parallel to the road on which it is located, unless being in an intersection. This method is interesting only when we are sure to be on a road and for human navigation, but is too imprecise for localization and autonomous guidance. In Floros et al. (2013), the structure of the city map is analyzed and 3D reconstructed. A visual SLAM process then performs a relative localization and is able to match the streets. This method is based on absolute landmarks (being the road position), but the SLAM used is physically incremental, so that the localization method is not fully absolute. Since we need in order to perform autonomous vehicle navigation a localization accuracy around 10 cm, despite being robust this method seem difficult to be safely use on an autonomous robot. 1.3 Coping with correlations In Julier and Uhlmann (2001b), a pure bayesian-based SLAM is developed, based on the Split Covariance Intersection (SCI) variant of the Kalman filter. This work has been taken as reference as of taking into account correlations between different measurement. This update scheme is able to efficiently cope with the overconvergence problem that is so common for Kalman-based SLAM. This filtering technique could be used as well for cooperative localization, like in Karam et al. (2006). In Li et al. (2013), the sensor fusion process is based on the split covariance intersection variant of the basic covariance intersection framework developed in 1997 by Jullier and Uhlmann. This framework seems to us to ally the covariance intersection robustness to correlations as well as the optimality from the Kalman filter. In this paper, we aim at comparing these three filtering techniques against the cooperative map-building process. 1.4 Real-Time strategy Our team has been developing a focalizing approach that leads to real-time localization operation on vehicles. This focalization strategy permits an efficient robot localization selecting only the best informative landmark (Aynaud et al. (2014a)), leading to lower detection time (only a part of the sensor data is analyzed) and information versus time best trade-off. This optimization process is based on the maximization of an objective function (also called criterion). This technique also permits to take into account ambiguity (scoring low for ambiguous landmarks and high for non-ambiguous ones). In the present work, we use the principle of taking only the best informative measurement 237

237

not only for real-time execution, but also for network bandwidth usage considerations. To summarize, in this paper we show a cooperative and absolute simultaneous fleet localization and map-refinement method based on an a-priori map avoiding the overconvergence problem that exist in classical Kalman filtering fleet management. In order to make this method potentially real-time, we also test a focalizing approach that takes at each time step the best source of information instead of taking all possible measurements. After presenting the general framework in section 2, we develop simulations in section 3 to ensure that the chosen update scheme is immune to over-convergence problems. Then in section 4, we focus on the focalizing approach for refining the map as well as improving vehicle localization, and demonstrate the possibility to simultaneously localize the environment’s landmarks as well as the vehicle position with good accuracy and without over-convergence problems. 2. GENERAL FRAMEWORK DESCRIPTION 2.1 Framework description In this section, we present the general principle by which we aim at simultaneously refine the environment map based on an initial given map, and localize the ego-vehicle in this map. We first define the map M at time t as : Mt = Dt ∪ Lt where D is the set of detecting elements  at time t :  Dt = Dt1 , Dt2 , · · · DtN   Di i Dti = XD t , Ct

containing detecting elements Di (like an autonomous vehicle equipped with LIDAR sensors, or an infrastructure i camera) and consists of a state vector XD referring t i to variables representing only D (like its position and i heading), and its associated covariance matrix CD t . L is the set of detectable landmarks at time t :   Lt = L1t , L2t , · · · LM t   j Lj Ljt = XL , C t t Likewise, Lj represents a visible element to observe (landmark like a pole, a wall, or an autonomous vehicle, which state is given by its position). The first map M0 is an apriori given and consistent map. Here, the landmarks and detecting elements can be modeled by completely different state vectors, and, especially in split covariance intersection, by several covariances. In this framework, we decide to implement the Split Covariance Intersection (Julier and Uhlmann (2001b)) update strategy and to compare it with simple Covariance Intersection (Uhlmann et al. (1997)) and Kalman filtering.

Here, we focus on relative measurements on different elements of the map, because this is the main potential source for map non-integrity. Of course, absolute measurements like GPS can be taken into account in this method (but withour being related to another element of the map). For each measurement from Di to Lj at time t, let’s define an observation function with observation Y : i j i j i Lj YtD L = hD L (XD (1) t , Xt ) + w with w being a zero-centered gaussian noise random

IFAC IAV 2016 238 Laurent Delobel et al. / IFAC-PapersOnLine 49-15 (2016) 236–241 June 29 - July 1, 2016. Messe Leipzig, Germany

variable with covariance Cw . This function lets us make updates for both Di and Lj , through the h observation function. At each time step, we have a choice to use different available measurements. One way is to use them all, which is described in section 3. The other way in order to limit network traffic is to use for each Di only one measurement and taking the best measurement available. This is described in section 4. It is to be noticed that in this general framework vehicles can use measurements at any given time (i.e. not periodic) without loss of generality. So the choice in our simulations to rate all available measurements at a fixed rate is purely arbitrary. 2.2 State estimation filters Covariance Intersection (CI) filter is a Bayes filter that removes duplicate information (correlations) from measurement and state covariance, during the update process. Split Covariance Intersection (SCI) filter is a kind of mix between CI filter and Kalman filter, that ”splits” its covariance in an independent and a correlated part. As was previously mentioned, the state estimation in our different simulations can be chosen as Kalman filtering, CI and SCI, that will be compared over in section 3. For Kalman filtering, only one covariance is used (denoted ”ind” for independent). For CI and SCI, two covariances are used, the independent and the dependent (denoted ”dep”). Table 1 sums up all covariance used for the update process, with Di → Lj representing the landmark update covariance, i j and Di ← Lj the vehicle update covariance. HD and HL i j are the jacobian of hD L versus respectively detecting element and landmark states. Table 1. Relative measurement covariances Covariance Kalman CtD CI CtD CI SCI SCI Kalman CI CI SCI SCI

i

→Lj ind

i

→Lj ind

i j CtD →L i j CtD →L i j CtD →L i j CtD ←L i j D ←L Ct D i ←Lj Ct i j CtD ←L i j CtD ←L

dep ind dep ind ind dep ind dep

Value i

i

iT

i

iT

D Cw + H D · C D t ·H D Cw + H D · C D t ·H i

D · CD t ·H j

iT

j

jT

j

jT

L C w + HL · C L t ·H

0

j

L C w + HL · C L t ·H

Cw

j HL

j

L · CL t ·H

Otk ∈Ot

f (measurement) = H(af ter update) − H(bef ore measurement) (4) The criterion in 4 selects the best landmark or measurement based on the available information before the detection and update process is performed. H is the entropy in the sense of information theory, that is a direct measure of the uncertainty quantity. We finally obtain the expressions 5 and 6 : f (absolute measurement) = − pkt obs log(pkt obs )

− (1 − pkt obs ) log(1 − pkt obs )    i  1    Di  e + pkt obs log 2π e CD − log C  2π t u t p 2

(5)

f (relative measurement) =

− (1 − pkt obs ) log(1 − pkt obs )    i  1    Di  + pkt obs log 2π e CD t u  − log 2π e Ct p  2    j  1    k Lj  e − log C (6) + pt obs log 2π e CL  2π t u t p 2 X awhere CX t p and Ct u are respectivelyi a-priori and Lj and C are posteriori covariance matrices, and CD t X t X respectively vehicle and landmark covariances.

Cw

i HD

with pkt obs the probability to make a successfull detection or that the corresponding measurement is feasible, and hkt the observation function. Let’s associate to each measurement Otk at time t a score function f based on the information improvement that this measurement can give. Let Ochosen be the best informative measurement as given in 3. Otchosen = arg min f (Otk ) (3)

− pkt obs log(pkt obs )

0

i

take the list L of all possible observable landmarks for Di and add to it the set of absolute measurements A (GPS...). We define by observation the set O of relative and absolute observations (measurements) : O =L∪A   Ot = Ot1 , Ot2 , · · · OtP    (2) i hkt (XD ) for A t k k t Ot = pt obs , i Lj hkt (XD t , Xt ) for Lt

jT

3. SIMULATIONS

2.3 RealTime operation A naive implementation of our method with all available measurements would cause problems in a real application. First, since each vehicle would need to transfer at each time step its state to each other vehicle, we would even for moderately sized fleet encounter a bandwidth problem. This would be a O(N 2 ) bandwidth usage, with N being the number of vehicles. Second, if all vehicles have to detect each element they can possibly see, and since in robotics, detection is quite heavy in CPU time, this would lead to a huge computing time. The idea here is that in human life, very often only one detectable element can give sufficient information in order to localize and to improve environment knowledge. Let’s 238

Here, we describe the results of simulations with one and two vehicles. All simulations use four poles, with close to 1 m position uncertainty. First we analyze overconvergence behaviour, then we look at measurement selection. Finally, we conclude. 3.1 Simulation Principle In this section, we describe the simulation model used, as well as the different modeled elements. Simulations were ran thanks to our LibK2 -LibCI bayesian filtering C++ template library. The LibCI module was added to LibK2 in order to implement the different versions of covariance intersection, thus allowing to compare the fundamental properties of different filters based on the same C++ code structure. All simulated noises are gaussian.

IFAC IAV 2016 June 29 - July 1, 2016. Messe Leipzig, Germany Laurent Delobel et al. / IFAC-PapersOnLine 49-15 (2016) 236–241

There are two elements in the simulation : • moving Vehicles which are prone to detect and being detected ( ∈ L ∪ D), • motion-less Poles, which are prone to be detected (∈ L).

Each of these elements is given an initial state with initial covariance. The vehicle model is a two-degree of freedom planar model (2D), using vehicle state Xv = [xv , yv , θv ]T . Vehicle process noise is given by a constant covariance matrix. In order to simplify modelling, vehicles are moving at constant speed on a circular trajectory. Vehicle odometry is given by an uncorrelated curvilinear rate as well as a rotation rate, with Uv = [s, ˙ ω] ˙ T. Depending on their configuration, vehicles can measure GPS position, compass orientation, which are absolute measurements. The GPS noise model is a 2D order 1 AutoRegressive non-white gaussian noise model. Vehicle to Vehicle and Vehicle to Pole LIDAR relative measurements are performed with Ym = [ρm , θm ], the LIDAR range and azimuth readings. Poles are simply given with a planar position state Xp = [xp , yp ]T . Initial position uncertainty is around 1 m. Pole state can only be updated by a vehicle’s measurement. Initial estimates and covariance are also given in the configuration file. 3.2 Overconvergence behaviour First, let’s simulate one vehicle vs two vehicles with all possible LIDAR updates : • GPS position measurement, • vehicle to all poles measurements for updating vehicle state, • vehicle to all poles measurements for updating pole state. Simulation time is 1000 seconds, and complete update cycle with all possible measurements occur every 100 milliseconds. Results shown below are averages. For LIDAR measurements, the same measurement is used for both updating vehicle state and pole state. Analysis results for 1-vehicle simulation are shown in table 2, and 2-vehicle simulation in table 3.

Simulation kind Kalman

CI

SCI

Item Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1

Euclidean error (m) 0.005 0.015 0.038 0.012 0.016 0.020 0.023 0.033 0.074 0.022 0.015 0.021 0.019 0.036 0.018

Vehicle 1 is equipped with LIDAR (σρ ≈ 13 cm, σθ ≈ 4◦ ) and RTK-GPS (σ ≈ 5 cm), whereas vehicle 2 uses EGNOS-GPS (σ ≈ 40 cm), heading (σθ ≈ 0.7◦ ) and LIDAR measurements (σρ ≈ 8 cm, σθ ≈ 0.1◦ ). On table 2, we observe that despite the Kalman filter results having a little higher Mahalanobis distance for pole 3, the results seems consistent. When comparing with table 3, we see that the Kalman filter Mahalanobis distance results are completely unrealistic. We observe the effect of cross-correlation between vehicle 1 and vehicle 2 spreads to all poles and vehicle 2. This is due to vehicle 2 updating itself on vehicle 1 LIDAR measurement, propagating it to all poles, and propagating from all poles to vehicle 2. σ-number for vehicle 1 remains within limits since regularly, vehicle 1 corrects its accumulated bias from low correlation GPS absolute position measurements. Thus, in the presence of correlation in the map, standard Kalman filter is completely unusable. Now, comparing SCI to CI for the two vehicle simulation, we observe that : • for poles, euclidean error is 2 to 4 times lower for SCI than for CI, • for vehicles, vehicle 2 is almost as precise as vehicle 1 with SCI (whereas twice worse with CI), • Mahalanobis distances remain low for both SCI and CI. The conclusion is that SCI and CI seem to be correlationproof, with a better error management for SCI compared with CI. A simulation with four vehicles with all LIDAR vehicle to vehicle and vehicle to pole updates shows that the SCI ans CI remains fully consistent, although Kalman is clearly inconsistent, and SCI showing much better error results compared to CI. This proves the validity of the bilateral LIDAR update (consisting of measurement to observing vehicle update followed by measurement to observed item update) and SCI for being immune to overconvergence. 3.3 Scalability Let’s now take into account a more realistic case based on this simulation. Let’s use a IEEE802.11p wireless Table 3. Two-vehicles simulation results Simulation kind Kalman

Table 2. One-vehicles simulation results Mahalanobis error 0.49 0.85 1.86 0.66 0.91 0.10 0.11 0.13 0.43 0.41 0.24 0.38 0.26 0.47 0.77

CI

SCI

239

239

Item Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Vehicle 2 Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Vehicle 2 Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Vehicle 2

Euclidean error (m) 0.034 0.027 0.036 0.042 0.013 0.028 0.021 0.049 0.054 0.041 0.022 0.039 0.005 0.010 0.012 0.006 0.018 0.020

Mahalanobis error 170 31 39 59 1.29 11 0.15 0.32 0.32 0.26 0.42 0.38 0.15 0.17 0.18 0.18 0.78 0.55

IFAC IAV 2016 240 Laurent Delobel et al. / IFAC-PapersOnLine 49-15 (2016) 236–241 June 29 - July 1, 2016. Messe Leipzig, Germany

medium with a 500 kB · s−1 data rate. Let’s assume we are using a naive local centralizing node that holds the map. Of course, more involved stategies exist, for example in (Carrillo-Arce et al., 2013), but let’s take this as a basic example. At each time step of 100 ms and with N being the number of vehicles, we transfer into this local map the following information : SizeB = N · (N − 1)(DF · 2 + (DF · 4) · 2) vehicle + N · (DF · 2 + (DF · 4) · 2) GPS + N · (DF + DF · 2) heading (7) where DF represents the size of a double floating point real number (8 bytes). For each line, the first DF term corresponds to the size of the measurement vector, and the second DF term, the size of the two SCI covariance matrices associated to the measurement. Let’s take the example of a road traffic situation where N = 30. The bandwidth usage would therefore be around 730kB · s−1 .

Now, let’s focus to detection time, related to vehiclevehicle LIDAR measurement, and let’s also use N = 30. Every 100 ms, a vehicle attempts to detect 29 other vehicles; assuming detection time is 10 ms, we end up with a detection time of 290 ms per 100 ms time slice. Detection time and bandwidth usage are both clearly unmanageable.

with a low position uncertainty, since pole 4 (closest to vehicle 3) was mapped by vehicle 1 at the start. Vehicle 4 on the other hand keeps its localization integrity but evolves in an unmapped environment (poles 2 and 3). When vehicle 1 enters this area, vehicle 4 localizes itself based on vehicle 1, and vehicle 1 maps the environment. Vehicle 2 never uses its GPS because other measurements give a much better precision. 4.3 Simulation comparison with criterion In this section, we compare CI with SCI in terms of map integrity and precision. We can see on table 4 the different results obtained with a 1000 steps of 100 ms simulation. Results are averaged over the complete simulation. Here, A means taking fully all possible measurements, and B means using the best measurement in the sense of section 2.3. Table 4. Simulation with criterion Filtering type

Item

CI

Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Vehicle 2 Vehicle 3 Vehicle 4 Pole 1 Pole 2 Pole 3 Pole 4 Vehicle 1 Vehicle 2 Vehicle 3 Vehicle 4

4. MAP OPTIMIZATION WITH FOCALIZATION As was explained in 2.3, we implement here a focalizing approach which selects only one measurement source per vehicle at each time step. SCI

4.1 Observation probability To mimic the behavior of the criterion in Aynaud et al. (2014b), we use here an observation probability, which is : • 1 for GPS, • 1 for heading (given by a compass measurement), • pobs for LIDAR measurements.

where pobs is the probability for the landmark to be observed within LIDAR range. We could take into account GPS-screening and canyoning effect for example by changing GPS probability so that it represents the probability to be in direct satellite sight. 4.2 Four-vehicle simulation Although not mentioned in this paper, for the sake of paper length, simulations performed with one, two and three vehicles show as good performance as the fourvehicle simulation we are going to describe. This last simulation involves one vehicle with RTK-GPS, a compass and LIDAR (vehicle 1), and another with an EGNOS-GPS, a compass too, and a LIDAR (vehicle 2), like in section 3 and with the same measurement covariances. Two other vehicles (vehicle 3 and vehicle 4) are added. These last vehicles do not have any absolute sensors (i.e. no GPS nor compass), only LIDAR sensor (σρ ≈ 13 cm for both vehicles, σθ ≈ 3.6◦ for vehicle 3 and σθ ≈ 1.8◦ for vehicle 4). They can measure poles as well as other vehicles. These vehicles can localize as long as the environment is mapped. In this simulation, we observe a quick localization for vehicles 1 and 2. Vehicle 3 also starts to map correctly 240

Euclidean error (m) all best A B 0.027 0.070 0.045 0.050 0.061 0.052 0.027 0.119 0.022 0.025 0.038 0.047 0.027 0.089 0.030 0.070 0.007 0.040 0.008 0.069 0.010 0.057 0.006 0.060 0.018 0.019 0.020 0.032 0.019 0.049 0.020 0.061

Mahalanobis error all best A B 0.19 0.49 0.30 0.27 0.37 0.26 0.20 0.57 0.41 0.47 0.36 0.42 0.21 0.41 0.25 0.36 0.17 0.43 0.18 0.40 0.15 0.44 0.15 0.89 0.77 0.78 0.54 0.65 0.53 0.63 0.53 0.62

Let’s compare the results obtained by SCI to CI taking the best available measurement. We observe that vehicle 1 is in both cases the best localized vehicle (with a position error of around 2 cm). Since this vehicle can use RTKGPS readings, this seems normal. Vehicle 2 is a bit better localized using SCI scheme than with CI (3 cm vs 4.5 cm). Vehicles 3 and 4 use only LIDAR measurements, so they heavily rely on map building by other vehicles. We observe that with SCI, localization accuracy is twice better for than for CI. Since vehicle 3 is close to pole 4, and that for SCI, pole 4 is better localized, this looks normal. Vehicle 4 localizes fairly good although it is close to almost no pole nor vehicle. A comparison of vehicle 4 simulation results can be seen on figure 1. Looking at pole mapping, we observe that except for pole 2, all poles are equal or better localized using SCI update scheme, especially for pole 4. Also, covariance ellipses for SCI are smaller than for CI, although keeping the map with full integrity (Mahalanobis distance lower than 1). We conclude that for map building, SCI is much more efficient than CI. Looking a the simulation video, we observe that vehicle 3 and 4 rely on vehicles 1 and 2 as being relative landmarks. So, every vehicle uses all possible and available information in the surroundings to localize and help build the map.

IFAC IAV 2016 June 29 - July 1, 2016. Messe Leipzig, Germany Laurent Delobel et al. / IFAC-PapersOnLine 49-15 (2016) 236–241

REFERENCES

Vehicle 4 localization error (LIDAR only) 0.45

Covariance Intersection Split Covariance Intersection

Position error (meters)

0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0

0

20

40

60

241

80

100

Simulation time (seconds)

Fig. 1. Simulation of localization precision for vehicle 4 When comparing the results between using all possible measurements and only the best available measurement, we observe that for CI, pole absolute errors are of the same order of magnitude, except for pole 4 which is much worse. This remark is equaly true for vehicles 1 and 2. Since vehicle 3 and 4 rely on relative measurements, their error is almost twice with the best measurement compared with all measurements, while still remaining below 10 cm. This last remark for the four vehicles is also true for SCI, but there, all errors are in average half of the CI case. Poles are much better localized with all measurements for SCI scheme, but the results taking the best measurement are the same order of all the CI results and no bigger than 7 cm. In conclusion, the SCI filter with the realtime entropy-optimal selection criterion performs well while being potentially realtime. 4.4 Scalability Following section 3.3, and using the focalizing method, the bandwidth usage would go to 24 kB · s−1 and would be O(N ) instead of O(N 2 ). Second, detection time would be close to 10 ms per 100 ms time step which would be totally feasible. 5. CONCLUSION In this paper, we have proven the feasibility of managing and updating consistently a global map containing a fleet of vehicles and visible geo-referenced features. We have shown that in order to keep integrity in an absolute map built using several vehicles, it is necessary to take into account the correlation to other measurements. We have proven that Kalman filter leads to overconvergence, whereas Covariance Intersection and Split Covariance intersection do not, with Split Covariance Intersection leading to the best performance. In order to make map building feasible, we have limited each vehicle to use only one measurement per time step. An entropy-based criterion is used to select the best informative measurement, based on detection probability and covariance improvement after update. SCI performs especially well using this criterion and the map is efficiently built this way. Further work will involve ambiguity management using a Bayes network like the one from Aynaud et al. (2014b) and porting this technique to a fleet of real vehicles managing a global map. 241

Aynaud, C., Bernay-Angeletti, C., Chapuis, R., Aufrere, R., Debain, C., and Karam, N. (2014a). Real-time vehicle localization by using a top-down process. In Information Fusion (FUSION), 2014 17th International Conference on, 1–6. IEEE. Aynaud, C., Bernay-Angeletti, C., Chapuis, R., Auirere, R., and Debain, C. (2014b). Vehicle localization by using a multi-modality top down approach. In Control Automation Robotics & Vision (ICARCV), 2014 13th International Conference on, 1415–1420. IEEE. Carrillo-Arce, L.C., Nerurkar, E.D., Gordillo, J.L., Roumeliotis, S., et al. (2013). Decentralized multi-robot cooperative localization using covariance intersection. In Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on, 1412–1417. IEEE. Floros, G., van der Zander, B., and Leibe, B. (2013). Openstreetslam: Global vehicle localization using openstreetmaps. In Robotics and Automation (ICRA), 2013 IEEE International Conference on, 1054–1059. IEEE. Hentschel, M. and Wagner, B. (2010). Autonomous robot navigation based on openstreetmap geodata. In Intelligent Transportation Systems (ITSC), 2010 13th International IEEE Conference on, 1645–1650. IEEE. Julier, S.J. and Uhlmann, J.K. (2001a). A counter example to the theory of simultaneous localization and map building. In Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEE International Conference on, volume 4, 4238–4243. IEEE. Julier, S.J. and Uhlmann, J.K. (2001b). Simultaneous localisation and map building using split covariance intersection. In Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ International Conference on, volume 3, 1257–1262. IEEE. Karam, N., Chausse, F., Aufr`ere, R., and Chapuis, R. (2006). Collective localization of communicant vehicles applied to collision avoidance. In Intelligent Transportation Systems Conference, 2006. ITSC’06. IEEE, 442– 449. IEEE. Li, H., Nashashibi, F., Lefaudeux, B., and Pollard, E. (2013). Track-to-track fusion using split covariance intersection filter-information matrix filter (scif-imf) for vehicle surrounding environment perception. In Intelligent Transportation Systems-(ITSC), 2013 16th International IEEE Conference on, 1430–1435. IEEE. Mourikis, A., Roumeliotis, S., et al. (2006). Performance analysis of multirobot cooperative localization. Robotics, IEEE Transactions on, 22(4), 666–681. Parra, I., Sotelo, M.A., Llorca, D.F., Fern´andez, C., Llamazares, A., Hern´andez, N., et al. (2011). Visual odometry and map fusion for gps navigation assistance. In Industrial Electronics (ISIE), 2011 IEEE International Symposium on, 832–837. IEEE. Parsley, M.P. and Julier, S.J. (2010). Towards the exploitation of prior information in slam. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, 2991–2996. IEEE. Uhlmann, J.K., Julier, S.J., and Csorba, M. (1997). Nondivergent simultaneous map building and localization using covariance intersection. In AeroSense’97, 2–11. International Society for Optics and Photonics.