GIS-Based Topological Robot Localization through LIDAR CrossroadDetection

Andre Mueller, Michael Himmelsbach, Thorsten Luettel, Felix v. Hundelshausen and Hans-Joachim Wuensche

Abstract— While navigating in areas with weak or erroneousGPS signals such as forests or urban canyons, correct maplocalization is impeded by means of contradicting position hy-potheses. Thus, instead of just utilizing GPS positions improvedby the robot’s ego-motion, this paper’s approach tries to incor-porate crossroad measurements given by the robots perceptionsystem and topological informations associated to crossroadswithin a pre-defined road network into the localization process.We thus propose a new algorithm for crossroad detection inLIDAR data, that examines the free space between obstaclesin an occupancy grid in combination with a Kalman filter fordata association and tracking. Hence rather than correcting arobot’s position by just incorporating the robot’s ego-motion inthe absence of GPS signals, our method aims at data associationand correspondence finding by means of detected real worldstructures and their counterparts in predefined, maybe evenhandcrafted, digital maps.

I. INTRODUCTION

As stated in previous work [1], [2], [3], localizing arobot within its environment is considered to be a keyprerequisite for autonomous robot navigation. Hence simul-taneous localization and mapping (SLAM), which addressesthe problem of building an environment map by associatingsensor measurements to landmarks and localizing the robotwithin this environment, became quite popular. Localizing arobot within an unknown environment involves the correctmapping of the environment, by considering the noise proneego-motion- and sensor measurements as well as findingcorrespondences between sensor measurements and alreadyregistered landmarks. Instead of generating a map fromsensor measurements, this paper aims at robot localizationby identifying comparable features in both LIDAR measure-ments and human readable (maybe even handcrafted) digitalmaps. By stemming on an already mapped environment, theapproach’s challenge demands the association of structureswithin LIDAR range measurements to features within thedigital map. Especially the one to many association betweendetected structures within the sensor data and their corre-spondences within the map turns out to be demanding withinour problem formulation.By performing a topological analysis of the given map, com-parable features to real world structures are identified. It isdecomposed in crossroad acquisition by means of examiningthe map’s road intersections, extracting their topology (num-ber of branches and their angles) and evaluating the distancebetween adjacent crossroads. Second, crossroad hypotheses

All authors are with the Institute for Autonomous Systems Technology(TAS), University of the Bundeswehr Munich, 85577 Neubiberg, Germany.Contact author email: a.mueller@unibw.de

Fig. 1. A pane of the occupancy grid, provided by LIDAR rangemeasurements, with our robot MuCAR-3 in it’s center and two validcrossroads hypotheses (green structures). All figures are best viewed in color.

within the LIDAR range measurements are identified, thatenable a meaningful comparison. Our method is based on afree-space evaluation between obstacles within an occupancygrid. The evaluation’s result is a set of crossroad hypothesescomposed of a center point and branches fanning out towardsthe intersecting lanes. In that fashion we get topologicalcrossroad descriptions from LIDAR range measurements thatmatch the map’s extracted topological information and thusbuilds a means for robot localization.Our approach aims at robot position estimation in areas thattypically prevent correct GPS-localization due to error proneor even absent signals, e.g. while driving through a forestwith continuously wrong or missing GPS measurements,or navigating in areas with huge obstacles, disturbing orreflecting GPS signals.

A. Related Work

Following the idea of mapping topological features, Mor-ris et al. [4] built topological maps of abandoned mines byexploring mine corridors, their intersections and the distancebetween adjacent ones. Since external communication (suchas GPS) was impossible, the robot’s navigation and localiza-tion strongly depended on a correctly built map. Similar toour work, nodes represent the intersections (crossroads) andedges the paths between them. Intersections are extractedby triangulating a range scan according to Delaunay andfinding triangles with all three sides longer than a givendistance parameter. By matching hypotheses over multiple

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

range scans a robust intersection detection was established.Based on the intersections and the distance and directionbetween them a topological map was built up.A more biologically inspired approach was presented byVasudevan et al.[5]. Its topological information did notrely on some abstract sensor data, but on spatial relationsbetween detected objects. Objects like doors, cups, tables,etc. are utilized to distinguish between different places.While moving within its environment, the robot examinesdetected objects for known features and relations. Detecteddoors cause the insertion of a new node into the topologicalgraph, but only if the room associated to this door is unknownin its object relations. Otherwise, gained object informationis used for rough self localization within the topologicalgraph, instead of determining the exact position within aroom. In that fashion a topological map was built up, thatdid not contain distances and directions between its nodes.But nodes were named after the place they represent (likekitchen, dining room, etc) while edges define the possibletransitions between these places.This paper’s approach shall combine portions off the afore-mentioned work to overcome some of their disadvantages. Inthe fashion of Vasudevan et al.[5] we also try to find objectsin the sensor data for topological mapping. Unlike Morris etal. [4] intersections are not represented by means of a simplecenter point, but as a center point with branches that definepossible driving directions. This enables robot localizationeven in case of missed crossroads, since the topology ofeach crossroad is incorporated into the localization processin addition to the distances and directions between them.Furthermore to localize the ”place” we are in, we do not haveto learn objects beforehand like in Vasudevan et al.[5], butrather match the object descriptions given by the map to theobject descriptions provided by the perception system. Ourapproach even enables a correct re-localization relative to agiven object, which was not covered in the work presentedso far, e.g. 5 meters behind crossroad X.The paper is composed as follows. In section II a shortoverview of the algorithms processing the raw LIDAR datais given, followed by a detailed explanation of the crossroaddetection mechanisms in section III. Mapping the detectedcrossroads to the given topological map is discussed insection IV, closing in section V with a short analysis ofthe achieved results and an outlook to future extensions andchallenges.

II. PERCEPTION

Equal to [6] we use a 2 12 -D ego-centered occupancy

grid of dimension 200m×200m, each cell covering a smallground patch of 0.25m×0.25m. Each cell stores a singlevalue expressing the degree of how occupied that cell is byan obstacle given the current scan only. For calculating theoccupancy values, we first inertially correct the LIDAR scan,taking the vehicle’s motion into account (exploiting IMU andinformation from odometry). This is done by simultaneouslymoving the coordinate system of the vehicle while transform-ing the local LIDAR measurements to global 3D space. After

Fig. 2. MuCAR-3 freely moving over the occupancy grid and accumulatingobstacles. Black indicates low obstacle probability, light yellow indicatinghigh obstacle probability. Note the large ochre area not hit by any beam,thus having obstacle probability of 0.5.

a frame is completed, all points are transformed back into thelast local coordinate system of the vehicle, simulating a scanas if all measurements were taken at a single point of timeinstead of the 0.1s time period of one LIDAR revolution.

Similar to Thrun et. al. [7], each cell’s occupancy valuecocc is then calculated to be the maximum absolute differencein z-coordinates of all points falling into the respective gridcell, cocc = |czmax − czmin |.

A. Accumulation of Obstacles

To accumulate sensor information from multiple scans inthe occupancy grid, we simply let the vehicle freely moveover the grid while keeping the grid at a fixed position.Unfortunately, it turns out that keeping records of extremez-coordinate measurements over multiple scans introducesmany spurious obstacle cells, i.e. ones with large absolutez-coordinate differences, into the grid. The reason is thataccumulating raw 3D point cloud data requires very highprecision in ego-motion estimation which the inertial naviga-tion system of the robot can not provide, especially in harshenvironments causing violent sensor pitch motions. Resortingto ICP-style algorithms [8] is also not a viable solution dueto the additional computational load involved.

A solution to this problem is to probabilistically accumu-late the obstacles detected at a cell instead of the raw pointmeasurements. We thus introduce an occupancy probabilitycPocc

with 0 ≤ cPocc≤ 1, where cPocc

= 0 denotes a freespace cell and cPocc

= 1 a cell being occupied. Whenever wemeasure an obstacle at a cell, we increment its probabilityof being occupied, while in contrary decrementing cPocc

if the measurements do not indicate an obstacle. If themeasurements do not tell us anything about there being anobstacle at the cell or not, we keep the occupancy probabilityas it is. Hence, apart from the occupancy of a cell based onmultiple scans, we can now tell the probability of a cell beingoccupied. Fig.2 shows a snapshot of the vehicle moving overthe occupancy grid while accumulating obstacles.

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

III. CROSSROAD DETECTION AND TRACKINGEven before the introduction of GPS, humanity was able to

determine the current position on a (maybe even handcrafted)map by exploring the environment and matching detectedobjects to features within a map. Following this principle,we thought of matching intuitive real world objects to dataprovided by a global map, since they depict severe decisionsregarding the steering direction: crossroads.

A. CROSSROAD DETECTION

By utilizing the accumulated grid’s data we perform anevaluation of the obstacle-free area, with obstacles definedby an obstacle probability cPocc

≥ 0.5 for the followingtwo reasons. First, negative obstacles are considered. Asdiscussed in section II one cell’s occupancy probability isdetermined by examining the probabilities of being free oroccupied, resp. However, negative obstacles, like ditches orslopes, are not hit by any laser beam, information about theiroccupancy state can not be gained. But instead of explicitlydetecting negative obstacles, the phenomenon of not knowinga cell’s occupancy state is utilized. By definition, occupancyprobability cPocc

= 0.5 is assigned to each cell not coveredby any laser beam, which involves grid cells in the fardistance as well as cells that lie within a negative obstacleregion. I.e. each cell with probability cPocc = 0.5 is declaredto be an obstacle, which covers negative obstacles as well.Especially in terrain characterized by negative obstacles,such as off road pathways with ditches to the left and tothe right this obstacle definition gets useful. Second, gridcells that do not offer information regarding their occupancystate (especially cells far from the center) are declared to beoccupied, which reduces the computational demands withrespect to the free-space evaluation. As shown in Fig. 2,the ochre area is declared to be an obstacle whereby thefree space evaluation is reduced to a smaller patch close tothe vehicle, which has proved to be sufficient for crossroaddetection.According to each cell’s occupancy state the grid is nowbinarized to enable the application of the Distance Trans-form, as proposed by Borgefors [9]. We thus get the ap-proximate distance from every free-space cell to the closestobstacle cell. Since the distance transform’s outcome buildsthe foundation of our crossroad detection algorithm and isthus subject to a manifold of operations, we store it in a gridlike structure which is further referred to as distance gridG with each grid cell’s value v(c), c ∈ G representing thedistance to the closest obstacle.

1) Center Extraction: To determine a crossroad’s centeralongside it’s branches our method exploits the distancegrid’s property of monotonically increasing values towardsthe cell with the largest distance to all nearby obstacles.Interestingly, crossroads show salient characteristics in thisdata structure as shown in Fig. 3, with (a) representinga crossroad’s and (b) a road’s distance distribution, eachcornered by obstacles. The occurrence of a crossroad forcesthe appearance of one salient maxima within its center,while roads are characterized by an approximately uniform

(a) (b)

Fig. 3. The heights of two samples of the distance grid. Left: the sampleof a crossing with a salient maximum in its center. Right: the sample of astraight road with its multiple maxima distributed equally along the centerof the road.

distribution. To extract these maxima we introduce P (Occ |ΩDci

), the probability of a grid cell ci ∈ G being surroundedby grid cells that share the same distance towards an obstacle.For each obstacle distance v within G we first build up theset:

ΩV=v = ci : v(ci) = v (1)

,with ci denoting the grid’s i-th cell. For each elementwithin a set we then determine distance d·,· to all other cellswithin the same set by:

dci,cj = max| gxi − gxj |, | gyi − gyj | (2)

with (gxi, gyi) being the i-th and (gxj

, gyj ) the j-th cell’sgrid coordinates and | · | the absolute value, to yield the set:

ΩDci= dci,cj : ci, cj ∈ ΩV=v, 1 ≤ j ≤ |ΩV=v| , j 6= i.

(3)Each distance in ΩDci

then serves as evidence of howunique cell ci is with respect to it’s location. However, thesame distance may occur multiple times within ΩDci

, e.g. ifthree cells with equal distances share the same value v(c).To incorporate multiple appearances into the computation ofP (Occ | ΩDci

), we first define

P (Occ | D = d) =1

((2d+ 1)2 − (2d− 1)2)=

1

8d(4)

,which represents the probability that one cell is occupiedat a distance d. Finally we define P (Occ | ΩDci

) to be theprobability that a grid cell in the vicinity of ci is occupiedby the same value:

P (Occ | ΩDci) =

1

dm

|ΩDci|∑

j=1

P (Occ | D = dj), dj ∈ ΩDci

(5),with dm representing the maximal achievable distance, in

our case the occupancy grid’s halfway width.By applying equation 5 to all the distance grid’s cells we

get a local similarity distribution over all cells. We now usethis distribution to identify grid cells that are very likely tobe unique with respect to their location. I.e. cells having alow similarity value are considered to be crossroad centerpoints.

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

(a) (b)

Fig. 4. Results of successful branch extractions within the grid structure.While (a) shows a processed 3-way road junction, (b) presents the algorithmsresult with regard to a 4-way crossroad. The 4-way junction’s characteristicis recognizable with a center line (red colored cells) instead of a singlecenter point.

2) Branch Extraction: Extracted crossroad center hy-potheses are now utilized as a seed for branch extractionwithin an expansion algorithm. The method explores thedistance grid, starting at detected center hypotheses, alongthe level sets of maxima in a forward value iteration-stylealgorithm. I.e. it tries to find multiple paths with length don the distance transform, while certain paths may collapsein the process. For each path the procedure tries to chooseaction ui ∈ U(ci) which minimizes:

C∗d(ci) = minu1,...,ud−1

lI(ci) +d−1∑j=1

l(cj , uj)

(6)

,with lI(ci) = 0 for all center cells, lI(ci) = ∞ for anyother cell and l(xj , uj) defined by:

l(cj , uj) = 1 + δv(cj , uj) + d(cj , uj) +m(cj , uj). (7)

In this connection action u describes a move towards acell’s 1-neighborhood, i.e. to one of a cell’s edge neighbors.With δv(cj , uj), the difference between the two cell valuesv(cj) and v((cj , uj)) is expressed, which forces the expan-sion towards the neighboring cell that shares a similar valueand thus along a level set. The second term d(cj , uj) is ableto take two values either d(cj , uj) = 0 if the distance, asdefined in equation 3, to the center cell is kept or increases,or d(cj , uj) = ∞ otherwise, which causes the expansionaway from the center. With m(cj , uj) the difference betweenv((cj , uj)) and the cell having the largest value in the 1-neighborhood of cj is expressed. It aims to ascent towardsa level set by preferring cells with large values. The pathexpansion procedure stops if either distance d is reachedor only two paths remain throughout the expansion process.Latter even causes the algorithm to reject cell ci as crossroadhypotheses, since two resulting paths indicate the occurrenceof a road instead of a junction. Distance d is chosen to bed = v(ci) ∗ s with s weighting the value of a crossroad’scenter point and thus expresses how deep a crossroad shallbe explored depending on the crossroads dimension.

While a path is extended towards a new cell, the respectivecell may already be a part of another path, i.e. two paths mayexpand into the same direction. To omit multiple hypothesesdirecting towards the same branch, the occurrence of twopaths occupying the same cell, forces the path with the highercost to terminate it’s expansion. In short: The expansionprocedure forces the paths to run along the level sets ofmaxima in the distance transform, while paths having lowcosts are chosen over expensive paths if they meet at a certaingrid cell. A successful 3-way crossroad extraction is shownin 4(a).

While applying the described procedures to four-wayjunctions we learned that the characteristics of the centerpoint change. Instead of a single center point a center line,comprising a set of similar valued cells, occurs as shown in4(b). Interestingly this line’s two terminal points still satisfythe prerequisite’s to be crossroad center points. Hence, afterall the grid’s crossroads were extracted a further processrevises the grid cells associated to branches of differentcrossroads, while overlapping branches cause the crossroadsto merge. The merged crossroads center is defined by thecenter line’s mean cell.

The expansion algorithm’s result, a set of pathsp1, p2, . . . , pn, with n being the number of extractedbranches, each containing an ordered set of m grid cellspi = c1, c2, . . . , cm, is then evaluated to yield ΩB , a set ofangles expressing the different branch directions with regardto one crossroad. To compute ΩB we switch back from gridcoordinates (gx, gy) to the cartesian x, y by determining theposition of each cell ci in the robot’s coordinate frame with(xi, yi) = f(gxi

, gyi). For each path pi, with 1 ≤ i ≤ n, wethen determine mean angle:

bi =1

m

m∑j=1

atan2(yj − yc, xj − xc) (8)

,utilizing the vector between crossroad center cell co-ordinate (xc, yc) and the j-th cell’s cartesian coordinate(xj , yj), to yield ΩB = b1, . . . , bn with b1 ≤ b2 ≤· · · ≤ bn. Additionally each branch’s standard deviation siis determined, see equation 9, to yield ΩS = s1, . . . , sn,which gets especially meaningful in the context of crossroadcomparison.

si =1

m

m∑j=1

(atan2(yj − yc, xj − xc)− bi

)2(9)

Comprising crossroad center point pc = (xc, yc), theset of branches ΩB and the set of standard deviations ΩSour branch description is composed by the tuple: C =(pc,ΩB ,ΩS)

3) Topology Optimization for 3-way-junctions: Fig. 5(a)shows the result of a 3-way crossroad extraction. Unfortu-nately, the detected center point does not represent the trueroad intersection point, which causes discrepancies withinthe topology extraction process, i.e. the extracted set ΩBdoes not represent the true topology. E.g. in the example

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

(a) (b)

Fig. 5. Within (a) an extracted crossroad topology is shown. The crossroad’scenter (green dot in the crossroad’s center) resides on the distance grid’slargest value with respect to the location, while its branches (green linesterminating at a specified distance, denoted by green dots) fan out towardsthe level set of maxima. (b) presents the forces (red arrows) that are usedto optimize the shape of a 3-way road junction.

of figure 5(a) the T-junction’s topology resembles more aY than a T. To get a more accurate crossroad topology aLevenberg-Marquardt optimizer tries to minimize:

g(pc, ε) =(d− v

(f−1(pc + ε−→a1 + ε−→a2 + ε−→a3)

) )2(10)

,with f−1(·) transforming cartesian coordinates into gridcoordinates, v(·) providing the value at a given grid co-ordinate, and d being the median of a crossroad’s set ofgrid cell values. The set is built up by incorporating allthe grid cells that define the branches of a crossroad. Vec-tors −→a1,

−→a2,−→a3 represent the angle bisectors of neighboring

crossroad branches with their magnitude equal to the angledifference as shown in Fig. 5(b). By applying a gradientdescent method, the crossroad’s center is now translated ina way that minimizes equation 10. While the center pointis moved, one sample point for each branch is retained thatrepresents the entry of this respective branch as shown inFig. 6(a).

B. CROSSROAD RECOGNITION AND TRACKING

To be declared valid, a crossroad needs to be detectedin at least two successive measurements. Hence, to asso-ciate crossroad measurements over several time steps, alldetected hypothesis are moved according to the vehicles ego-motion in the robot’s coordinate frame, utilizing an ExtendedKalman Filter. State xcr = (xc, yc,Ψc)

T is defined by thecrossroad’s center position (xc, yc) in the robot’s coordinateframe and yaw-angle Ψc, which defines the crossroad’srotation relative to the robot. Crossroads are moved withinthe Kalman Filter’s prediction step according to the robot’sego-motion in a constant velocity model [10]. System noisecovariance Qk and measurement noise covariance Rk arebuilt up and optimized by filter tuning.

To enable the comparison between generated hypothesesand current measurements, crossroad descriptions are evalu-ated by means of position and topology.

1) MEASURE OF CORRESPONDENCE: To comparetwo crossroads we define a measure of correspondence whichis composed as follows:

Fig. 6. presents the corrected topology from figure 5(a). Notably thecrossroad’s center point shifted slightly away from the distance grid’s largestvalue to enable a more realistic crossroad topology.

S(C1∧= C2) =(

1‖(c1,c2)‖+1 · S((ΩB1

,ΩS1)∧= (ΩB2

,ΩS2))).

(11)

The first term relates to the euclidean distance betweenthe crossroads C1 and C2, i.e. the more the two crossroadsare spatially separated the less their correspondence. Withoutreferring to their location, S((ΩB1 ,ΩS1)

∧= (ΩB2 ,ΩS2))

)evaluates the topology of both crossroads by utilizing theirbranch descriptions. The function basically tries to adaptparameter β that maximizes:

maxβ

(1n

(h(N (b11

+ β, s11),N (b12

, s12))

+ . . .

+h(N (bn1

+ β, sn1),N (bn2

, sn2)) )) (12)

with β being a rotation offset and h(·, ·) a function thatcomputes the convolution between two normal distributionsin the fashion of [11]. Input to h(·, ·) are the crossroads meanbranch angles (b11

, . . . , bn1∈ ΩB1

, b12, . . . , bn2

∈ ΩB2)

alongside their variances (s11, . . . , sn1

∈ ΩS1, s12

, . . . , sn2∈

ΩS2). In case |ΩB1

| 6= |ΩB2|, similarity function

S((ΩB1,ΩS1

)∧= (ΩB2

,ΩS2))

returns zero.For each hypothesis from time-step t − 1, processed by

the prediction step of the Kalman filter, the measurement attime-step t that maximizes equation 11 is searched. Validassociations are input to the Kalman innovation step toupdate the filter’s prediction. Also, within each time-stepthe crossroad’s probability of being valid is updated byevaluating the ratio between the number of time-steps ameasurement was associated and the number of time-stepsno measurement could be assigned since the first occurrence.

IV. TOPOLOGICAL MAPPING

Topological maps represent the environment as a combi-natorial structure that minors the importance of geometricinformation, such as distances, directions or scale, but keepsthe relationship between distinct points. In that manner infor-mation is simplified with unnecessary details removed, whilethe vital information required for navigation still remains.These maps are represented by means of graphs that con-sist of vertices, denoting different places or landmarks andedges connecting adjacent nodes. Topological approaches in

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

Fig. 7. Raw GIS data, represented as a set of point sequences. In the topright corner a pane of the complete network is cut out.

general determine the position of the robot relative to themodel. These models are primarily based on landmarks ordistinct, momentary sensor features. Since the resolution oftopological maps corresponds directly to the complexity ofthe environment their compactness can be distinguished tobe a key advantage. Due to their compactness they permitfast planning, facilitate interfacing to symbolic planners andproblem-solvers and provide more natural interfaces forhuman instructions (such as: go to place X).With respect to our localization method, they provide anappropriate environment description, since crossroad descrip-tions, including their number of branches, relative branchangles and their sequence of occurrence is maintained.

A. GIS DATA

Within our system, GIS (Geo Information System) dataprovides a means to build the necessary predefined map,since a rich set of environment features, including infor-mations with regard to roads is given. It is composedby vector data including positions (highly precise GPS orUTM coordinates, provided by land surveying offices) andtheir description. The vector data can define any kind ofcontinuous entity, such as roads, contour lines, borders, etc.and is divided into different layers. A layer is composed offeatures (line sequences, stand alone points, etc.) and refersto a specific information, such as “highways” or “dirt road”.Figure 7 presents an example of raw GIS data.

B. MAP FEATURE EXTRACTION

Feature extraction is carried out by identifying crossroads,their topology and the distances between adjacent ones inthe GIS data by combining GIS-features within the “road”-network layer. A feature in the context of a “road” layeris defined to be a point sequence that connects either twocrossroads or one crossroad and a dead end. Hence, featuressharing equal endpoints have to be identified to detectpossible crossroads within the network. These crossroadhypotheses are then analyzed with respect to their topology.Each crossroad’s center point alongside its outgoing branchesis extracted while distances to adjacent crossroads are deter-mined by moving along the outgoing paths. Since GIS dataprovides unidirectional network data only, the extracted datahas to be duplicated to yield a bidirectional road network

Fig. 8. The resulting road network after extracting road and crossroadfeatures from raw GIS data. In the top right corner crossroads and theirtopological information is visualized. (black lines represent the topology ofthe associated crossroad).

that allows movement in both road directions. A pane of anextracted road network is shown in figure 8

C. POSITION ESTIMATION

Crossroad hypotheses from LIDAR data are now utilizedwithin a particle filter to estimate the robot’s position withinthe extracted road network. The particle state xp = (x, y,Ψ)is composed by the particle’s global position (x, y) and itsrotation Ψ within the map. The first occurrence of a validcrossroad hypothesis CH = (cH ,ΩBH

,ΩSH) causes the

particle filter’s initialization. With ΩN being the set of all thenetwork’s crossroads, the initialization routine tries to find allcrossroads CN = (cN ,ΩBN

,ΩSN) ∈ ΩN that satisfy:

S((ΩBH,ΩSH

)∧= (ΩBN

,ΩSN)) > α. (13)

I.e. with respect to their topology, all the road network’scrossroads are compared to the LIDAR crossroad hypothesis,while their degree of similarity has to exceed a predefinedparameter α. Furthermore the number of initialized particlesNCN

with regard to a certain crossroad is determined byutilizing the value of S((ΩBH

,ΩSH)∧= (ΩBN

,ΩSN))

in thefashion of equation 14.

NCN=

S((ΩBH,ΩSH

)∧=(ΩBN

,ΩSN))∑CN∈ΩN S((ΩBH

,ΩSH)∧=(ΩBN

,ΩSN))· n (14)

In this connection n denotes the maximum number of par-ticles that is available for initialization. Each crossroad thatsatisfies equation 13 is further processed to yield the robot’sposition that explains the initial crossroad measurement thebest. At distance

√x2cH + y2

cH , which denotes the distanceto the measured crossroad’s center, a robot pose hypothesispi is placed at every branch of a crossroad. Each hypothesis’slocal “view” of the crossroad Cpi is then compared to themeasured LIDAR hypothesis CH to maximize:

argmaxCpi

(S(CH

∧= Cpi) ·

(1−

∣∣∣βπ ∣∣∣)) (15)

By exploiting rotation offset β provided by equation 12,equation 15 basically chooses Cpi and thus pose hypothesispi that provides the smallest value of β. Finally, at posehypothesis pi the number NCN

of particles is initialized.

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

Fig. 9. Possibilites of a particle (2D-coordinate systems, with the red arrowdenoting the forward direction) to move within a 4-way junction. With eachchoice a different movement behavior and thus a different progress of theyaw rate is measurable.

Once initialized, a particle’s movement is coupled to thedirection of the closest lane within the road-network. Position(xt, yt) at time-step t is updated according to the constantvelocity model [10], while the particle’s rotation Ψ is definedto be the closest lane’s tangent angle ΨL. I.e. each particlemoves in parallel to the closest lane with its rotation alignedto the lane’s direction.

With each new crossroad measurement every particle’sweight wi is recomputed by:

w(i) = S(CH∧= Cpi) ·

(1−

∣∣∣βπ ∣∣∣) · P (xp | ZΨ) (16)

With CH being the crossroad measurement and Cpi theparticle’s closest crossroad, equation 16 exploits correspon-dence of equation 11 in combination with rotation offsetβ provided by equation 12. Hence particles that declarethe current crossroad measurement the best are weightedthe highest. Furthermore P (xp | ZΨ), the probability ofobserving state xp given the robot’s past yaw rates ZΨ,provided by robot’s INS (Inertial Navigation System), isincorporated to sustain particles that carry out a movementsimilar to the robot’s. As mentioned, a particle’s movementis coupled to its closest lane. Basically, a particle’s yaw rateis close to zero, while moving on a lane. But within junctionseach particle moves towards another branch, which results indifferent yaw rates as shown in figure 9. Thus each particlepossesses a queue QΨ that contains the former yaw rates ofthe particle. With regard to the i-th of m particle’s queueQΨi

we thus get:

P (xpi | ZΨ) =

∑0

j=

∣∣∣∣QΨi

∣∣∣∣(qj−zj)2

∑mi=1

∑0

j=

∣∣∣∣QΨi

∣∣∣∣(qj−zj)2 (17)

, with qj ∈ QΨi, z ∈ ZΨ, and the queues size

∣∣QΨi

∣∣restricted to a defined length. I.e. equation 17 supportsparticles that have a similar progress of the the yaw rateto the yaw rate of the robot. Especially in the vicinity of4-way crossroads, the equation gets useful, since the robot’smotion provides a hint towards the driven branch.

Fig. 10. Complete road network for robot localization. The small cutoutshows the area in which the robot has operated. The results of one run in atimbered area (driving direction from top to the bottom) is shown with thered line denoting the path of the best particles.

(a) (b)

Fig. 11. Figure (a) presents the particle filter’s initialization at the firstdetected crossroad in the vicinity of the robot’s GPS position (white vehiclemodel). At the initialization, the occurrence of a crossroad forces thedistribution of the whole particle set over the complete map. Hence onlyfew particles are initialized close to the GPS position. Figure (b) shows theparticle distribution after the occurrence of the second crossroad. Since acrossroad’s topology is integrated into the localization process, the majorityof particles is located at the true position, even though only two crossroadsappeared.

(a) (b)

Fig. 12. Figure (a) presents the particle distribution after a long periodwithout crossroad measurements, before a 4-way-junction, while (b) showsthe distribution after passing the crossroad. Notably the robot’s motionsupported the choice of the correct particles.

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011

V. CONCLUSIONS AND FUTURE WORKS

A. Results

Throughout a test on a forest track of about 4.2 km length,see figure 10, 8 out of 10 real world crossroads were detectedcorrectly. The forest track itself contained more crossroadsthan represented in the GIS-data. Possibly the GIS-data wasnot up to date or some junctions just not significant enough torepresent them in the GIS-data. At 6 crossroads in the roadnetwork a correct re-localization was established. To alignthe particle weight onto the robot’s motion greatly improvedthe resampling. In the vicinity of complicated crossroads,like e.g. 4-way road junctions with neighboring branchesthat were arranged orthogonally, a hint towards the drivenbranch was provided when the junction was passed. Asfigures 11(a) and 11(b) show, only a small subset of particlesis required to localize the robot already after the secondcrossroad that occurred. Even after a long period withoutcrossroad measurements, as presented in figure 12(a), withthe particles wide spread, the chosen particle weightingfunction supported these particles the most that carried outthe same motion as the robot, see figure 12(b).

B. Conclusions

In this paper we presented work in the area of crossroaddetection and topological mapping. The proposed cross-road detection algorithm has proven to be sufficient todetect crossroads in obstacle (positive and negative) richenvironments. The particle filter approach to localize therobot worked really well, especially the incorporation ofthe robot’s motion increased its reliability. Furthermore, theinteresting fact of describing pathways verbally arose fromthe topological approach. This offers a new perspective, sinceroad descriptions do not have to be provided by means ofcoordinates and angles, but by verbal descriptions such as”take the next crossroad’s right branch, then after 500 meterstake the next crossroad’s left branch” and thus may makenavigation more human like.

C. Future Works

While the particle filter, to localize the robot within thegiven GIS data, worked very well, improvements have tobe made with regard to crossroad detection. The approachrestricts the robot’s movement to obstacle rich areas, likeforests or inner city environments, while flat areas, like grass-lands or fields, are not covered. We thus want to extend thecurrent approach with visual features, that describe obstacles,e.g. green surfaces (trees, grassland, etc.). Furthermore, theoutput and application of human readable plans is desired,that may lead to a more human like navigation.

VI. ACKNOWLEDGEMENTS

The authors gratefully acknowledge funding by Germancluster of excellence COTESYS, Cognition for TechnicalSystems (also see http://www.cotesys.org).

REFERENCES

[1] M. Bosse, P. Newman, J. Leonard, M. Soika, W. Feiten, and S. Teller,“An atlas framework for scalable mapping,” in IEEE InternationalConference on Robotics and Automation, 2003. Proceedings. ICRA’03., vol. 2, Sept. 2003, pp. 1899–1906 vol.2.

[2] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “Fastslam:A factored solution to the simultaneous localization and mappingproblem,” in National Conference on Artificial Intelligence, September2002.

[3] M. Montemerlo and S. Thrun, “Simultaneous localization and mappingwith unknown data association using fastslam,” in IEEE InternationalConference on Robotics and Automation, 2003. Proceedings. ICRA’03., vol. 2, Sept. 2003, pp. 1985–1991.

[4] A. Morris, D. Silver, D. Ferguson, and S. Thayer, “Towards topologicalexploration of abandoned mines,” in Proceedings of the 2005 IEEEInternational Conference on Robotics and Automation, 2005. ICRA2005., April 2005, pp. 2117–2123.

[5] S. Vasudevan, S. Gaechter, V. Nguyen, and R. Siegwart, “Cognitivemaps for mobile robots–an object based approach,” Robotics andAutonomous Systems, vol. 55, no. 5, pp. 359 – 371, 2007, from Sensorsto Human Spatial Concepts.

[6] M. Himmelsbach, T. Luettel, F. Hecker, F. von Hundelshausen, andH.-J. Wuensche, “Autonomous Off-Road Navigation for MuCAR-3– Improving the Tentacles Approach: Integral Structures for Sensingand Motion,” Kuenstliche Intelligenz, vol. Special Issue on Off-Road-Robotics, 2011, accepted for publication.

[7] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron,J. Diebel, P. Fong, J. Gale, M. Halpenny, G. Hoffmann, K. Lau,C. Oakley, M. Palatucci, V. Pratt, P. Stang, S. Strohband, C. Dupont,L.-E. Jendrossek, C. Koelen, C. Markey, C. Rummel, J. van Niek-erk, E. Jensen, P. Alessandrini, G. Bradski, B. Davies, S. Ettinger,A. Kaehler, A. Nefian, and P. Mahoney, “Stanley: The robot that wonthe darpa grand challenge: Research articles,” J. Robot. Syst., vol. 23,no. 9, pp. 661–692, 2006.

[8] P. J. Besl and N. D. McKay, “A method for registration of 3-d shapes,”IEEE Transactions on Pattern Analysis and Machine Intelligence,vol. 14, no. 2, pp. 239–256, 1992.

[9] G. Borgefors, “Distance transformations in digital images,” in Com-puter Vision, Graphics and Image Processing, vol. 34, 1986, pp. 344–371.

[10] Y. Bar-Shalom, X.-R. Li, and T. Kirubarajan, Estimation with Appli-cations to Tracking and Navigation. John Wiley and Sons, Ltd.,2002.

[11] R. Bracewell, ”Convolution” and ”Two-Dimensional Convolution.”.McGraw-Hill, 1965.

accepted for publication at International IEEE Conference on Intelligent Transportation Systems, 2011