Feasibility Grids for Localization and Mapping in Crowded Urban Scenes

(1)

Feasibility Grids for Localization and Mapping in Crowded Urban Scenes

Shao-Wen Yang and Chieh-Chih Wang

Abstract— Localization and mapping are fundamental tasks in mobile robotics. State-of-the-arts often rely on the static world assumption using the occupancy grids. However, the real environment is typically dynamic. We propose the feasibility grids to facilitate the representation of both the static scene and the moving objects. The dual sensor models are introduced to discriminate between stationary and moving objects in mobile robot localization. Instead of estimating the occupancy states, the feasibility grids maintain the stochastic estimates of the feasibility (crossability) states of the environment. Given that an observation can be decomposed into stationary objects and moving objects, incorporating the feasibility grids in localization yields performance improvements over the occupancy grids, particularly in highly dynamic environments. Our approach is extensively evaluated using real data acquired with a planar laser range finder. The experimental results show that the feasibility grid is capable of rapid convergence and robust performance in mobile robot localization by taking into account moving object information. A root mean squares accuracy of within 50cm is achieved, without the aid of GPS, which is sufficient for autonomous navigation in crowded urban scenes. The empirical results suggest that the performance of localization can be improved when handling the changing environment explicitly.

I. INTRODUCTION

Mapping is widely considered the most important problem in robotics whose progress can impact a wide range of perceptual problems, such as localization, navigation, explo- ration and so forth. A most representative mapping method is the occupancy grid mapping algorithm [1] which represents maps by a collection of fine-grained grid cells that model the occupancy states of the environment. Occupancy grid mapping representation has been used in a great number of robotic algorithms [2–4]. However, most existing robotic mapping algorithms assume the world is static and maintain the states of the obstacles in the environment [5]. The techniques used in conjunction with these mapping algorithms can only use the static parts of the environment, wherein the dynamic parts of the environment are considered noise and filtered out [2, 6]. Localization in consequence becomes less accurate and can diverge while navigating a highly dynamic environment. Specifically, algorithms presuming a static world assumption can be unreliable in the real environment, which is typically dynamic. Instead of modeling only the static part of the environment and filtering out the

This work was supported in part by the Taiwan National Science Council under Grant 99-2221-E-002-192, and in part by the Intel-NTU Connected Context Computing Center.

Shao-Wen Yang and Chieh-Chih Wang are with the Department of Computer Science and Information Engineering, and the Graduate Institute of Networking and Multimedia, National Taiwan University, Taiwan.

E-mail: [email protected], [email protected]

counterpart, taking into account moving object information is critical for mobile robots to act autonomously in the real environment.

Moving object detection is the problem of determining if an object is moving, which is a subset of the detection and tracking of moving objects (DATMO) problem that also estimates object’s motion states. While the DATMO problem has been extensively studied [7], mapping and localization can also take advantage of these techniques to improve upon the state-of-the-arts. Comparing to the conventional approaches focusing on the space that is occupied by an obstacle, we take a different perspective on the mapping problem by considering the feasibility (crossability) of the space. Given the a priori moving object detection based on our previous work [8, 9], we propose to represent the map using the feasibility states of the environment. Specifically, the stationary parts and the moving parts of the environment are assumed to be separated given the sensory data. The objective is that the probabilistic sensor models for stationary objects and moving objects are complementary in both mapping and localization. The higher the probability that a grid cell is feasible (crossable), the more probable that the location is free space and temporarily occupied by a moving object. On the other hand, the lower the probability that a grid cell is feasible, the more probable that the location is occupied by a stationary object.

The basic idea is to model the feasibility of the environment as a collection of grid cells, just like the occupancy grid mapping algorithm, by making use of both moving object information and stationary object information. Dual probabilistic sensor models are applied for moving objects and stationary objects, respectively. In mapping, a grid cell is feasible only if it was ever occupied by a non-stationary object. On the other hand, a grid cell is infeasible if it was occupied by a stationary obstacle. In localization, moving objects should probably be seen at grid cells which are feasible. In contrast, stationary objects should be observed at infeasible grid cells. In addition to modeling explicitly the stationary environment and the dynamic objects, the use of feasibility grids permits a straightforward use of the sensor model in localization and results in improvement, particularly in dynamic urban scenes.

II. RELATEDWORK

The last decade has seen many developments towards mapping and localization in dynamic environments. We draw on the work from two major bodies of researches – redesign- ing the sensor (or observation) model for localization, and

(2)

TABLE I

RANGE DATA INTERPRETATION

Occupancy Feasibility

Description Moving Stationary

Preceding Free Unknown Unknown Seeing through cells does not implies their feasibility. e.g., out-of-plane motion, uneven terrain Corresponding Occupied Feasible Infeasible The feasibility state depends on the observed. i.e., moving–feasible, stationary–infeasible Succeeding Unknown Unknown Unknown The unobserved cells remain unknown. c.f., analogous to the occupancy grid

mapping environments temporally.

The occupancy grid map representation has demonstrated its powerfulness in mobile robot localization. One of the key challenge in applying sequential Monte Carlo methods, such as Monte Carlo localization (MCL), lies in the design of the sensor model which can lead to the so-called particle depletion or particle deprivation problem [10]. Thrun et al.

[5] addressed the side effect of using an over-peaked sensor model. Limketkai et al. [11] derived conditional random field filters (CRF-filters) to learn the motion model and the sensor model for Monte Carlo localization. Levinson and Thrun [12] proposed the use of a probabilistic map to improve upon GPS/IMU systems by taking into account the infrared reflectivity of the observations. These approaches essentially based on the occupancy grid cannot take into account the moving object information and could fail in dynamic environments.

While the occupancy grid has been widely used in robotic perception, mapping dynamic environments was rarely addressed. Arbuckle et al. [13] proposed to maintain collections of occupancy grids representing the occupancy states of the environment among different time intervals. Mitsou and Tzafestas [14] proposed to store all occupancy probabilities of each grid cell and arrange them for each grid cell with a B+ tree. However, these approaches modeling the environment as collections of grid cells are computational demanding. On the other hand, they are tailored towards representing environments in distinct time scales using so- phisticated data structures, which can be difficult to integrate into the state-of-the-art localization algorithms.

Our approach estimating the feasibility states of the environment is practicable in modeling dynamic environments and can subsequently be used in a Bayesian estimation framework, such as Monte Carlo localization. The robustness of probabilistic localization approaches can be improved through the use of moving object information, particularly in highly dynamic environments.

III. FEASIBILITYGRIDS

This section firstly establishes the basic mathematical notation and derives the feasibility grid in the way analogous to the occupancy grid mapping algorithm [1]. Let m = {m_x,y}_x,y be the feasibility grid map where m_x,y denotes the feasibility probability of the grid cell with index (x, y).

Let {z1, z2, · · · , zT} be the collection of observations up to time T where ztdenotes the observation at time t. Just as the occupancy grid, the feasibility grid is modeled as a Markov random field (MRF) and the probability of each grid cell can be estimated independently. Mapping is thus the problem of

determining the probability of feasibility of each grid cell m_x,y given the collection of observations Z_t up to some time t, which can be expressed as

p(mx,y|z1, z2, · · · , zt)

= p(mx,y|zt) p(zt) p(mx,y|z1, z2, · · · , zt−1) p(m_x,y) p(z_t|z1, z₂, · · · , z_t−1) (1) The log odds ratio commonly applied for computational efficiency is calculated instead of estimating the posterior directly, which can be defined and led to a recursive expres- sion as

l_x,y^t = log p(mx,y|zt)

1 − p(m_x,y|z_t)+ log1 − p(mx,y)

p(m_x,y) + l^t−1_x,y (2) where ¯mx,y denotes the complement of mx,y.

The occupancy grid map representation provides a straightforward approach to sensor integration. Regarding feasibility, there are de facto two sensory modalities available for updating the states of the grid cells: sensor update – the perceptual sensors, and trivial update – the robot itself.

Specifically, feasible grids are probably crossed over by the robot or some moving object. Stationary obstacles are supposed to appear in infeasible grids. On the other hand, the robot itself acts just as a proximity, specifically touch, sensor identifying the feasibility of the ground plane crossed over. The feasibility grid involves summing the evidence for each grid cell from multiple sensory information. Let z1, z2, · · · , zT in general denote the collection of compound observations up to time T where ztis a composition of sensor readings ∨izⁱ_t at time t. The formulation of the feasibility grid following from Eq. 1 can be expressed as

p(m_x,y|z₁, z₂, · · · , z_t)

= Y

i,z_tⁱ∈z_t

p(m_x,y|zⁱ_t) p(zⁱ_t) p(mx,y)

p(m_x,y|z1, z₂, · · · , z_t−1) p(zt|z1, z2, · · · , zt−1) The log odds ratio at each time t estimated recursively via Bayes rule, following from Eq. 2, can be derived as

l^t_x,y=X

i

logp(mx,y|zⁱ_t)

p( ¯m_x,y|zⁱ_t)+ N logp( ¯mx,y)

p(m_x,y)+ l_x,y^t−1 (3) where N is the number of sensors.

Note that the vehicle pose is implicit in the observation in the above derivation of the mapping. In this work, there are two sensory modalities available at each time t: zt– the laser range scan, and z_t^r – the dummy observations obtained from crossing over grid cells. The formulation following

(3)

0 r

Probability Density Function (PDF)

(a) Feasibility grids – stationary object model z^s_t

0 r

(b) Feasibility grids – moving object model z_t^m

0 r

(c) Occupancy grids

0 0

0

(d) Feasibility grids – vehicle model

Fig. 1. Sensor model for mapping. (a) Sensor model of the feasibility grid for stationary objects. (b) Sensor model of the feasibility grid for moving objects. (c) Sensor model of the occupancy grid. (d) Vehicle model of the feasibility grid. In (a)–(c), r denotes the range reading. In (d), the robot is at the origin of the x-y plane.

from Eq. 3 can be subsequently rewritten as l^t_x,y = log p(mx,y|zt) − log[1 − p(mx,y|zt)]

| {z }

Sensor Update

+ log p(mx,y|z^r_t) − log[1 − p(mx,y|z^r_t)]

| {z }

Trivial Update

+η + l^t−1_x,y

where η is a constant depending on the environment.

A. Sensor Update

The sensor model p(mx,y|zt) is used to represent the likelihood of having the laser range scan ztgiven the map. To interpret the sensor reading obtained, a probabilistic density function relating the range reading is used to determine the feasibility probabilities, just as the occupancy grid mapping algorithm.

Tab. I describes the feasibility profile derived and com- pares to the occupancy grid. Given a range measurement from a moving object, the corresponding grid cell has the highest feasibility probability. On the contrary, the corresponding grid cell has the lowest feasibility probability if the range measurement comes from a stationary object. The succeeding grid cells whose distance to the sensor is greater than the range reading are unknown as they are unobserved.

However, comparing to the occupancy grid deducing that the preceding grid cells are free (with a low occupancy probability), we cannot infer much about the feasibility of the grid cells which are not ever crossed over. A feasibility probability slightly higher than 50% is assigned to these grid cells. In other words, feasibility infers unoccupancy, but on the contrary it does not necessarily hold. The feasibility grid also eliminates the side effect of the occupancy grid that commonly happens in the presence of the robot’s pitch or roll motion. Specifically, once the sensor sees through obstacles due to the out-of-plane motion, the information of the nearer obstacles can subsequently be lost upon updating the occupancy states for the measurements of the farer obstacles, given that the sensor is located above ground.

The sensor model p(mx,y|zt) of the feasibility grid can be expressed as

p(m_x,y|zt) = p(m_x,y|z_t^s) p(m_x,y|z_t^m)

where zt = z^s_t∨ z_t^m in which z^s_t ⊆ z_t and z_t^m ⊆ z_t are the stationary part and the moving part, respectively, of the observation z_t. The sensor models for stationary objects and moving objects are illustrated in Fig. 1(a) and 1(b), respectively, whereas the sensor model for occupancy grids is depicted in Fig. 1(c).

B. Trivial Update

A vehicle model p(m_x,y|z^r_t) is additionally used to describe the effect of the robot’s pose with respect to the feasibility grids. The feasibility of the grid cells can be inferred subsequently when crossed over by the robot. The shape of the robot can be easily taken into account in the vehicle model. In this paper, we assume a circular robot body in the experiments. The vehicle model is illustrated in Fig. 1(d).

C. Implementation

In order to construct the map of the environment, a non- linear least squares approach [15] is used for solving the SLAM problem, also known as GraphSLAM [16]. A graph representation is used to optimize an objective function wherein the vertices denote to robot poses and the edges corresponds to the non-linear constraints extracted from the odometric data and the observations. In brief, robot poses are linked to their estimated location estimated using scan matching [17], and consecutive robot poses are linked by the odometric data. In the preprocessing stage, the motions of the robot, and the moving object detection of the observations are estimated [8, 9]. Given the displacements between the observations and the stationary parts of the observations, the globally consistent robot poses are estimated and used to create the map of the environment of which the grid size is 20cm×20cm.

Fig. 2(a) shows the occupancy grid map of the environment, parts of which are enlarged in Fig. 2(b) and 2(c), whereas Fig. 2(d) shows the feasibility grid map of the environment, parts of which are enlarged in Fig. 2(e) and 2(f). As can be seen in Fig. 2(f), the boundaries of the sidewalk and the building at different heights above ground are both seen by the robot. The robot can frequently see objects at different heights due to the slight pitch or roll motion caused by the slippage of the vehicle or the uneven terrain.

(4)

meter

0 100 200 300 400 500 600 700 800

0 100 200 300 400 500 600

(a) Occupancy grid map

meter

90 100 110 120 130 140 150 160 170 180

−10 0 10 20 30 40 50 60

(b) Occupancy grid map

meter

380 400 420 440 460 480 500

260 270 280 290 300 310 320 330 340 350 360

(c) Occupancy grid map

meter

0 100 200 300 400 500 600 700 800

0 100 200 300 400 500 600

(d) Feasibility grid map

meter

90 100 110 120 130 140 150 160 170 180

−10 0 10 20 30 40 50 60

(e) Feasibility grid map

meter

380 400 420 440 460 480 500

260 270 280 290 300 310 320 330 340 350 360

(f) Feasibility grid map

Fig. 2. Grid maps. Note that the color codes are different. (a) Occupancy grid map. The darker the color a grid cell, the higher its occupancy probability.

(b) The enlargement of the lower left rectangle in (a). (c) The enlargement of the upper right rectangle in (a). (d) Feasibility grid map. The lighter the color of a grid cell, the higher its feasibility probability. (e) The enlargement of the lower left rectangle in (d). (f) The enlargement of the upper right rectangle in (d).

In the occupancy grid mapping framework, the sidewalk boundary can be omitted while updating the grid cells for the measurements from the farer boundary of the building.

Some details of the environment are consequently neglected, as shown in Fig. 2(c). While maintaining the feasibility states of the environment, our approach can also retain finer details of the static scene, as depicted in Fig. 2(f).

IV. LOCALIZATION WITHFEASIBILITYGRIDS

The feasibility grids describe the dynamic environment facilitating the representation of both the stationary object and moving object information. In this section, localization with the feasibility grid is described based on sequential Bayesian estimation. Both the static scene and the moving objects are subsequently used to localize the robot, given a feasibility grid map.

A. Bayesian Filters

Localization is the problem of estimating the pose x_t of a robot relative to a given map, which can be expressed as p(x_0:t|z1:t, u_0:t−1, m)

Gaussian filters, such as extended Kalman filters (EKFs), can be limited since they requires Gaussian mixture models to deal with multi-modality or ambiguity in the location

of the robot [18]. Relying on the design of the likelihood function of the sensor model, sampling-based approaches, such as particle filters [10], handle multi-modality by approx- imating the posterior probability density with a collection of particles and weights. Monte Carlo localization [3] is a specialization of particle filtering tailored towards mobile robot localization. In this work, the Monte Carlo localization algorithm is used to evaluate the robustness of our approach, though it can pose no problems to integrate with Gaussian filters, such as EKF localization.

B. Likelihood Models

In Monte Carlo localization with occupancy grids, the sensor model is used to correct the weights of the particles by a likelihood function whereas the motion model draws for particles with probabilities proportional to their weights.

The likelihood p(zt|xt, m) of the observation zt given the location xtand the map m is generally expressed as

p(z_t|xt, m) = Y

i,zⁱ_t∈zt

p(z_tⁱ|xt, m)

where z_tⁱdenotes the i-th range reading of the observation z_t, assuming conditional independence between the individual range readings.

Typical likelihood models can be categorized into beam models [2] and likelihood fields [4]. Beam models calculate the likelihoods by simulating the way rays of light travel through the environment. The likelihood function is deter- mined relying on the ray casting operation which is closely related to the physics of the sensor but suffers from lack

(5)

of smoothness and high computational expense. In addition, the beam-based sensor models excluding the seeing through problem described in Sec. III-A can degrade the effectiveness of the feasibility grids. The likelihood fields on the contrary measuring the correlation between the observation and the map overcome the limitations and work well in practice [16]. The likelihood function is derived by projecting the end points of an observation into the Cartesian coordinates of the map. While the likelihood field sensor model lacks a physical explanation in the occupancy grids, it is appropriate and works ideally for the feasibility grids.

C. Dual Sensor Models

The sensor model for feasibility grids is designated to make use of the stationary and moving object information in the map and the sensor observations. The idea is that moving objects are supposed to traverse in crossable space, such as traffic lanes and sidewalks. On the other hand, stationary obstacles are presumed to appear in non-crossable space, such as buildings, bushes and traffic islands. Given that the observation ztis decomposed into stationary objects z^s_t and moving objects z_t^m, we at first define the sensor models for stationary objects and moving objects respectively based on the likelihood fields sensor model, which can be expressed as

p(z_t|x_t, m) = Y

i,zⁱ_t∈z^m_t

p_m(zⁱ_t|x_t, m) Y

i,zⁱ_t∈z^s_t

p_s(zⁱ_t|x_t, m)

where pm(·) and ps(·) denotes the likelihood function for moving objects and stationary object, respectively. As the feasibility grids represent the crossability states of the environment, the likelihood fields of the feasibility grids are ideally adequate for deriving the likelihood function for moving objects, just as the likelihood fields of the occupancy grids are used to obtain the likelihood function for stationary objects. On the other hands, the complements of the feasibility grids are used to obtain the likelihood function for stationary objects. The sensor model for stationary objects can then be expressed as the dual function of the sensor model for moving objects, which can be written as

ps(z_tⁱ|xt, m) = 1 − pm(z_tⁱ|xt, ¯m)

where ¯m representing the non-feasibility states is the complement of the feasibility grid map m. By substituting the term ps(·), the sensor model for feasibility grids can be expanded as

p(z_t|xt, m) = Y

i,z_tⁱ∈z_t^m

p(zⁱ_t|xt, m) Y

i,zⁱ_t∈z^s_t

1 − p(z_tⁱ|xt, ¯m) (4) where p(·) ≡ p_m(·) is the likelihood field sensor model directly derived from the feasibility grids.

In comparison with the conventional localization algorithms considering only the free space and the occupied space based on the occupancy grid, our approach further takes into account the crossable space wherein moving objects travel through. This allows our approach to further make use of

the moving object information, instead of filtering out or neglecting the moving object information. It is particularly crucial when the robot navigates a highly dynamic scene.

V. EXPERIMENTALRESULTS

We quantitatively evaluate our approach to mobile robot localization using real data [19] acquired in a crowded urban scene. The workspace about 600m×800m in size is highly dynamic comprising stationary obstacles, vehicles, trailers, pedestrians, etc. A loop of approximately 3km is traversed twice, and the data can thus be used to cross- validate the comparative approaches. In addition, the ground truth moving objects were manually annotated over the entire data set so that an accurate, globally consistent trajectory of the vehicle can be obtained for evaluation by eliminating the non-stationary parts of the data. In the data set, 16.13% of the data are dynamic with a standard deviation of 15.4%. The moving objects inhabit 93.91% of the measurements in the most dynamic case. While it can be a common configuration of data acquired in a crowded urban scene, the performances of comparative approaches are tested using the data set.

The robustness against non-stationary objects can also be evaluated by inspecting the performance with respect to the dynamics of the data.

Our approach is evaluated against Monte Carlo localization with occupancy grids in terms of convergence and accuracy. The convergence is firstly experimented under global localization in which the initial pose of the robot is unknown. The presence of dynamic objects can result in serious problem in convergence. We show that our approach achieves improvements by explicitly considering the moving object information. Secondly, the accuracy is assessed by comparing the root mean squares errors (RMSEs) under position tracking assuming that the initial robot pose is known. Specifically, we compared the performance of the following approaches.

OG Occupancy grids. Mapping and localization using entire observations containing stationary and moving objects. Specifically, moving objects are neglected and treated as stationary objects.

OGS Occupancy grids. Mapping with only stationary object information, and localization using entire observations. Specifically, moving objects are filtered out in mapping.

OGSS Occupancy grids. Mapping and localization using only stationary object information. Specifically, moving objects are filtered out in mapping and localization.

OGSD Occupancy grids presuming free space is crossable.

Mapping with only stationary objects, and localization using entire observations in which the dual sensor model of occupancy grids is applied for range readings from moving objects. Specifically, moving objects are filtered out in mapping, and discriminated in localization.

FG Feasibility grid. Mapping and localization using entire observations. Specifically, moving objects are

(6)

1000 3000 5000 0.6

0.65 0.7 0.75 0.8 0.85 0.9 0.95 1

Convergence Rate

Number of Particles

FG OGSD OGSS OGS OG

(a) Convergence rate

1000 3000 5000

0 10 20 30 40 50 60

Convergence Time (sec)

Number of Particles

OG OGS OGSS OGSD FG

(b) Convergence time

Fig. 3. Comparison of convergence. (a) Convergence rate in percentage. (b) Average and standard deviation of convergence time in second.

discriminated in mapping and localization.

A. Global Localization

We first evaluate the performance of the localization approaches in terms of global localization. This experiment is designated to investigate the effectiveness of the representation of the environment and the sensor model in localization.

Without a known initial robot pose, a rapid convergence within a vicinity of the true robot location is crucial for mobile robots to achieve autonomous tasks. As the vehicle navigated through the loop twice in the data set, the 2-fold cross validation is used to evaluate the convergence of the approaches, wherein one part of the data is retained as the validation data for localization with the map created using the other part of the data as the training data. An arbitrary time step is chosen as the initial time step from the validation data at each run for testing the map built using the training data.

In order to inspect whether an estimate has converged, the standard deviations along x and y axes of the particle set is calculated at each time step. It is defined that a particle set converges when its standard deviations shrink to a vicinity of the true location within a 5m×5m circle. Ideally, the particle set should converge towards the solution if these exists some particles initially within the neighborhood of the true location. However, ambiguity for which particles are deprivated can lead to divergence, particularly in dynamic scenes that lack of sufficient static landmarks. Our approach can be thought of as a variation of the conventional Monte Carlo localization algorithm in which the ambiguous situations can be eliminated by additionally taking into account the non-static landmarks.

In this experiment, the localization procedure processed the data at 7.5 Hz and was repeated 100 times for each configuration. Fig. 3 shows the convergence rate, and the mean and the standard deviation of the convergence time.

Note that the statistics of the convergence time is taken over the converged runs. As can be seen, our approach, denoted FG, yields a convergence rate of over 95%, as well as rapid convergences with a low variance. The occupancy grids, denoted OG, on the contrary is unreliable and can be hardly applicable in an urban scene without a known initial pose. Localization with the occupancy grids created using only stationary object information, denoted OGS and OGSS,

results in slight improvements, particular in the convergence time. Localization with the occupancy grids using the dual sensor models, denoted OGSD, leads to further improvements over OGS by considering moving object information, instead of filtering out. It is observed that the more the information properly modeled, the more the effectiveness of the approach. Our approach in particular outperforms the alternatives, in which moving object information can be beneficial for resolving the ambiguous situations in global localization.

B. Position Tracking

In addition to the convergence, we also evaluate the accuracy of our approach quantitatively. Due to the absence of the ground truth locations, we use the offline Graph- SLAM algorithm described in Sec. III to obtain a reliable trajectory for benchmarking by aligning the range scans against the existing map. As the offline method is supposed to yield highly accurate estimates, the residual errors of the comparative approaches are calculated with respect to the trajectory estimate from the GraphSLAM algorithm.

In this experiments, a set of 5000 particles is employed.

The first part of the data is used for evaluation using the map created with the second part of the data. The initial particle set is normally distributed around the true location with a standard deviation of 5m, whereas the orientation is uniformly distributed.

Fig. 4 shows the RMSEs in the longitudinal and lateral directions. To facilitate the interpretation of the assessment, the RMSEs are calculated with respect to each time step interval, whereas the overall RMSEs are also presented, as shown in Fig. 4(a) and 4(b). The dynamics of the data within each time step interval represented by the ratio of measurements from moving objects are given in Fig. 4(c). As can be seen, the occupancy grid, denoted OG, is unreliable and bears large offsets. Localization using entire observations with the occupancy grids created using only stationary object information, denoted OGS, performs fairly but the RMSEs are even worse because of the presence of highly dynamic scenes in the data set which bias the estimates significantly. Furthermore, localization using only stationary observations, denoted OGSS, and with dual sensor models, denoted OGSD, achieves improvements as the non-stationary objects are removed or discriminated. Our approach, denoted

(7)

0 250 500 750 1000 1250 1500 1750 0

0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Time Step

Longitudinal RMSE (m)

OG Overall=4.5186 OGS Overall=5.0059 OGSS Overall=1.9689 OGSD Overall=1.9498 FG Overall=0.5121

(a) Longitudinal RMSE

0 250 500 750 1000 1250 1500 1750

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Time Step

Lateral RMSE (m)

OG Overall=4.7022 OGS Overall=5.4145 OGSS Overall=2.1413 OGSD Overall=2.1389 FG Overall=0.4840

(b) Lateral RMSE

0 250 500 750 1000 1250 1500 1750

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35

Time Step Interval

Dynamics

(c) Moving object ratio Fig. 4. Comparison of accuracy. (a) Longitudinal RMSE. (b) Lateral RMSE. (c) Moving object ratio for each time step interval

FG, outperforms the alternatives, wherein the RMSEs are reduced from meters to within around 50cm. In either longitudinal or lateral error, the feasibility grid demonstrates its robustness for localization in highly dynamic environments.

VI. CONCLUSION ANDFUTUREWORK

In this paper, we proposed a novel variant of the grid-based representation of the map. The feasibility grid is introduced realizing the integration of moving object information in robotic mapping. The feasibility grid permitting a straightforward use of the dual sensor models also facilitates the process of incorporating moving object information in mobile robot localization. As shown in the extensive experiments, our approach which improves upon the alternatives is capable of rapid convergence and robust performance by handling dynamic objects explicitly. Without a known initial robot pose, the convergence of over 95% can be achieved in the urban environment. The RMSEs in the longitudinal and lateral directions have been reduced from meters to within 50cm in the dense and dynamic data set, which was acquired using a planar laser range finder. To the best of our knowledge, we have also presented the first detailed analysis of the merit of discriminating moving objects for mobile robot localization in terms of convergence and accuracy. Localization consequently can be benefited from, rather than degraded by, the moving objects. Our approach is subsequently able to reliably localize a vehicle with sufficient accuracy for navigating crowded urban scenes.

A promising future direction is to incorporate the 3D range data into the feasibility grids by analyzing the crossability directly from terrain smoothness and infrared intensity. It is in particular useful to create a feasibility grid map with increased coverage of the crossable areas of the environment.

In addition, the implementation of the experiments employs a very smooth sensor model to ensure against particle depletion. It is also of interest to reason about the conditional dependence among the range readings, particularly for the moving objects, to further improve the dual sensor models.

REFERENCES

[1] A. Elfes, “Uncertain geometry in robotics,” IEEE Journal of Robotics and Automation, vol. 3, no. 3, pp. 249–265, June 1987.

[2] D. Fox, W. Burgard, and S. Thrun, “Markov localization for mobile robots in dynamic environments,” Journal of Artificial Intelligence Research, vol. 11, no. 3, pp. 391–427, February 1999.

[3] F. Dellaert, D. Fox, W. Burgard, and S. Thrun, “Monte Carlo localization for mobile robots,” in Proceedings of the IEEE International Conference on Robotics and Automation, vol. 2, Detroit, MI, USA, May 1999, pp. 1322–1328.

[4] S. Thrun, “A probabilistic online mapping algorithm for teams of mobile robots,” The International Journal of Robotics Research, vol. 20, no. 5, pp. 335–363, April 2001.

[5] S. Thrun, D. Fox, W. Burgard, and F. Dellaert, “Robust Monte Carlo Localization for Mobile Robots,” Artificial Intelligence, vol. 128, no.

1-2, pp. 99–141, 2000.

[6] D. Fox, W. Burgard, S. Thrun, and A. B. Cremers, “Position Estima- tion for Mobile Robots in Dynamic Environments,” in Proceedings of the National Conference on Artificial Intelligence, Madison, Wiscon- sin, July 1998.

[7] Y. Bar-Shalom, T. Kirubarajan, and X.-R. Li, Estimation with Applica- tions to Tracking and Navigation. New York, NY, USA: John Wiley

& Sons, Inc., 2002.

[8] S.-W. Yang and C.-C. Wang, “Multiple-Model RANSAC for Ego- Motion Estimation in Highly Dynamic Environments,” in Proceedings of the IEEE International Conference on Robotics and Automation, Kobe, Japan, May 2009, pp. 3531–3538.

[9] S.-W. Yang, C.-C. Wang, and C.-H. Chang, “RANSAC Matching:

Simultaneous Registration and Segmentation,” in Proceedings of the IEEE International Conference on Robotics and Automation, Anchor- age, Alaska, May 2010.

[10] A. Doucet, N. de Freitas, and N. Gordon, Sequential Monte Carlo Methods in Practice. Heidelberg: Springer, 2001.

[11] B. Limketkai, D. Fox, and L. Liao, “CRF-Filters: Discriminative Particle Filters for Sequential State Estimation,” in Proceedings of the IEEE International Conference on Robotics and Automation, Roma, Italy, April 2007.

[12] J. Levinson and S. Thrun, “Robust Vehicle Localization in Urban Environments Using Probabilistic Maps,” in Proceedings of the IEEE International Conference on Robotics and Automation, Anchorage, Alaska, May 2010.

[13] D. Arbuckle, A. Howard, and M. Matari´c, “Temporal Occupancy Grids: a Method for Classifying the Spatio-Temporal Properties of the Environment,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, October 2002.

[14] N. C. Mitsou and C. S. Tzafestas, “Temporal Occupancy Grid for mobile robot dynamic environment mapping,” in The 15th Mediterranean Conference on Control and Automation, Athens, Greece, July 2007.

[15] F. Lu and E. Milios, “Globally Consistent Range Scan Alignment for Environment Mapping,” Autonomous Robots, vol. 4, no. 4, pp. 333–

349, 1997.

[16] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge, MA: MIT press, 2005.

[17] F. Lu and E. Milios, “Robot Pose Estimation in Unknown Envi- ronments by Matching 2D Range Scans,” Journal of Intelligent and Robotic Systems, vol. 18, no. 3, pp. 935–938, 1997.

[18] T. Duckett and U. Nehmzow, “Mobile robot self-localisation using occupancy histograms and a mixture of Gaussian location hypotheses,”

Robotics and Autonomous Systems, vol. 34, no. 2–3, pp. 117–129, February 2001.

[19] S.-W. Yang, C.-C. Wang, and C. Thorpe, “The Annotated Laser Data Set for Navigation in Urban Areas,” The International Journal of Robotics Research, in press.