Autonomous Navigation of an Outdoor Mobile Robot

in a Cluttered Environment

Amir Monjazeb

, Jurek Z. Sasiadek

and Dan Necsulescu

Department of Mechanical and Aerospace Engineering, Carleton University, 1125 Colonel By Drive, Ottawa, Canada

Department of Mechanical Engineering, Ottawa University, 161 Louis Pasteur, CBY A205, Ottawa, Canada

Keywords: Simulataneous Localization and Mapping (SLAM) Problem, EKF, Unscented Kalman Filter, Fastslam,

Hybridslam, Unscented Hybridslam, Cluttered Environment, Loop Closing, Long Trajectory.

Abstract: This paper proposes a modification of HybridSLAM strategy that is used to navigate an outdoor

autonomous mobile robot in a cluttered environment. HybridSLAM is combining extended Kalman filter

SLAM EKF-SLAM and FastSLAM to take advantage of strengths and to cover shortcomings of both filters.

By the use of an unscented version of Kalman filter instead of EKF-SLAM, the formulation of the

HybridSLAM is revised. Same as HybridSLAM, the new revised algorithm uses the state distribution

capabilities of the unscented Kalman filter to keep the uncertainty of the system to be remembered for a

long trajectory, and at each time step, FastSLAM is used to produce local maps. Presented simulations and

results evaluate the performance of the proposed approach using Unscented Kalman filter in a cluttered

environment.

1 INTRODUCTION

Simultaneous localization and mapping (SLAM)

problem refers to a situation in which a mobile robot

in an unknown environment, incrementally

constructs a navigation map of its surroundings. At

the same time, the robot simultaneously, and with

the observations of features (landmarks), uses this

navigation map for localization (Durrant-Whyte and

Bailey, 2006). Due to the imperfections of the

movement and observation, there are error inputs to

the system. Since location of landmarks and pose of

the robot are highly correlated, there will be

uncertainty accumulated at next time steps to the

extent that it becomes unbounded. To prevent the

accumulation of uncertainty in the system, an

algorithm needs register such errors. Two dominant

approaches to solve SLAM problem are Extended

Kalman filter SLAM (EKF-SLAM) and FastSLAM.

EKF-SLAM has been known as an optimal

solution to SLAM problem for more than two

decades. However, EKF algorithm has the linearity

and Gaussian underlying assumptions and it suffers

form two major problems; computational complexity

and its sensitivity to single hypothesis data

association. FastSLAM, on the other hand, can

better handle non-linearity in the motion and carries

less complexity. FastSLAM is also carrying the

multi hypothesis data association property which

makes it relatively more reliable compare to EKF-

SLAM. Nonetheless, the latter is sensitive in the

presence of accurate sensors and, moreover, over

long vehicle trajectories such as a loop closing

scenario, the algorithm becomes overconfident by

not considering the existing uncertainty. A combined

(Brooks and Bailey, 2009) called “HybridSLAM”

uses a Hybrid mapping to add up strengths of both

filters where the FastSLAM is employed to construct

a local map and reduces the ambiguity of the

motion. However, experimental results prove that

for a very large loop, the use of EKF as a back-end

in HybridSLAM causes a complexity in the

computation of the global map. This paper takes

advantage of the same strategy that was used in

HybridSLAM to combine two filters used as front-

end and back-end but instead of EKF-SLAM, an

unscented version of Kalman filter (UKF) is

combined with a Rao-Blackwellised particle filter. A

loop closing scenario in a cluttered environment is

presented in this study. Several experimental results

are presented and three SLAM strategies are

compared.

485

Monjazeb A., Z. Sasiadek J. and Necsulescu D..

Autonomous Navigation of an Outdoor Mobile Robot in a Cluttered Environment .

DOI: 10.5220/0004047304850488

In Proceedings of the 9th International Conference on Informatics in Control, Automation and Robotics (ICINCO-2012), pages 485-488

ISBN: 978-989-8565-22-8

 2012 SCITEPRESS (Science and Technology Publications, Lda.)

2 HYBRIDSLAM

By combining EKF-SLAM and FastSLAM (Thrun,

and Montemerlo, 2004), HybridSLAM (HS) was

introduced (Brooks and Baily, 2009). HS is taking

advantage of strengths in both filters and helps to

overcome their limitations and weaknesses. The

resultant algorithm acts as a modified filter. The use

of EKF-SLAM on the side, helps FastSLAM not to

become over-confident. As a result, by combining

EKF-SLAM and FastSLAM, the modified filter

performs much better in comparison with either its

component. FastSLAM needs to be presented as a

continuous (Gaussian) form so that it can be

combined with EKF-SLAM. The modification may

be done in a way that FastSLAM builds local maps

and is allowed to run for long enough to

disambiguate associations. Before the path becomes

so long that particle diversity becomes problematic,

a single dimensional Gaussian is computed from the

FastSLAM posterior. At the end, this Gaussian local

map can be fused into the global map to be

represented as a whole. State of the system in

FastSLAM representation at time step k can be

expressed as

= {

1,k

, K ,

Mk ,

}

(1)

and the weighted sample for particle n is:

= P (Z

, Z

1−k

, x

) (2)

Using the two above equations:

1,k

,…,

Mk ,

1,k

, K ,

Mk ,

}

= {

,m),

}

(3)

Equation (3) represents RBPF as a Gaussian mixed

model in which each particle is a Gaussian

component with weight

, mean (

,m), and

covariance matrix

. With the use of moment

matching process, the final result of pose of the

robot with covariance

C extracted from all

particles can be expressed as

x =

∑

(4)

C =

∑

[

+ (

−

x )(

−

x )

]

(5)

Since the observation sensor includes noise,

represents the covariance of the map produced by

every individual particle. Due to noisy motion

(

−

x )(

−

x )

represents variation

between the map produced by particles. At each time

step, a new local map is developed by RBPF and

fused into global map (the map produced by EKF-

SLAM).

Constrained Local Sub-Map Filter (CLSF)

technique is used to integrate a local map to a global

map (Williams, Dissanayake, Durrant-Whyte, 2002).

3 UKF VERSUS EKF

The use EKF-SLAM in HybridSLAM has its own

estimation limitations and it has its approximation

issues when facing nonlinearity of the system

(Monjazeb, Sasiadek, Necsulescu, 2011). An

improved version of EKF was introduced and called

unscented Kalman filter (UKF) (Julier, Uhlmann,

2004). Applying UKF in Hybrid SLAM addresses

the approximation issues in constructing a global

map and will result in a more reliable map in the

fusion step. By replacing UKF with EKF in HS the

approximation of the state distribution using a

Gaussian random variable can be done except for the

case where it propagates the state distribution

analytically through the third order linearization of a

non-linear system. EKF optimization may lead to

suboptimal performance resulting in failure of the

filter since it generates substantial error in the

posterior mean and covariance of the system. UKF

represents the state distribution of the system based

on a Gaussian random variable and a deterministic

sample approach. Unscented transform eliminates

calculation of Jacobians in UKF by which the

complexity of computation is reduced substantially.

The system can be expressed as an augmented form

of estimated state and system covariance as follows:

1−k

(augmented) = [(

−1

x )

E[ w

]]

(6)

−

−1k

(augmented) =

⎥

⎦

⎤

⎢

⎣

⎡

−

(7)

For more detail see (Julier, Uhlmann, 2004)

4 SIMULATIONS AND RESULTS

In this experiment, the range sensor was able to

detect approximately 6 point landmarks per meter,

meaning that the shortest distance between two

nearby landmarks is approximately 0.20 meter same

as (Monjazeb, Sasiadek, and Necsulescu, 2008). In

ICINCO2012-9thInternationalConferenceonInformaticsinControl,AutomationandRobotics

486

all simulation experiments, the sampling time is

considered as T=0.2s. Velocity of the robot is set at

3.0 m/s and in agreement with a real

instrumentation. The maximum range for the

range/bearing is set as 50m. In a simulated scenario

the robot is travelling on a road that is curved and

depicts a loop closing situation. Position of

detectable landmarks simulates a cluttered

environment. The systematic error of the motion

This figure illustrates a loop closing scenario with

given waypoints and detectable landmarks that

represent a two dimensional (250,000m

) cluttered

environment. 200 detectable landmarks are

randomly distributed and a set of waypoints is

defined through a passageway. With the assumption

of unknown data association for a HybridSLAM

algorithm, 30 particles are incorporated in the

calculation of local map and fused to the global map

using both EKF-SLAM and UKF-SLAM. Figures 2

and 3 depict a situation that the loop is closed and

the Root Mean Square (RMS) position error is

calculated. Figure 2 shows how the vehicle observes

landmarks with uncertainty involved in the

algorithm. In Figure 3-a, the accuracy of

HybridSLAM is shown using EKF-SLAM as a

back-end. The RMS position error average is around

0.6m. Figure 3-b depicts the RMS position error in

the same situation using UKF-SLAM except for, this

time, the accuracy of HybridSLAM is substantially

improved. The RMS position error using UKF is

0.3m which is 30cm less than the situation in which

the algorithm uses EKF-SLAM as a global map

fusing strategy. Figures 4 shows the orientation

error for both versions of KF an for the same

scenario; the use of EKF-SLAM and UKF-SLAM as

the back-end to fuse the local map to the global map.

Results indicate that in terms of the orientation error,

UKF is outperforming EKF. The average of

orientation error as a result of using UKF-SLAM is

around 0.03rad and the difference with that of EKF-

SLAM is 0.01rad. The simulation of the same

scenario was run for 20 iterations. Figure 5

demonstrates a comparison between HybridSLAM

with a use of EKF-SLAM and HybridSLAM with a

use of UKF-SLAM. The accuracy of both

algorithms is computed in terms of the RMS

position error average.

It should be noted that the performance of

HybridSLAM in the presence of either Kalman filter

will depend on motion parameters as well as the

measurement model. Results indicate that UKF-

SLAM performance is better than EKF-SLAM

infusing the local map into the global map.

-200 -100 0 100 200

-250

-200

-150

-100

-50

100

150

200

250

X Direction (m)

(a) Before Closing the Loop

Y Direction (m)

Figure 1: An autonomous mobile robot travelling in a

cluttered environment. (a) before the loop is closed (b)

after the loop is closed.

Figure 2: Uncertainty that arises in the heart of SLAM

problem.

0 500 1000 1500 2000 2500

0.5

1.5

2.5

(a) EKF RMS Position Error

Time (s)

RMS Positon Error (m)

0 500 1000 1500 2000 2500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

(b) UKF RMS Position Error

Time (s)

RMS Positon Error (m)

Figure 3: (a) RMS position error for EKF-SLAM as the

back-end in HybridSLAM (b) RMS position error for

UKF-SLAM as the back-end in HybridSLAM.

X Direction (m)

Y Direction (m)



AutonomousNavigationofanOutdoorMobileRobotinaClutteredEnvironment

487

0 500 1000 1500 2000 2500

0.02

0.04

0.06

0.08

0.1

0.12

0.14

Time (s)

Error (rads)

EKF Orientation Error

0 500 1000 1500 2000 2500

0.02

0.04

0.06

0.08

0.1

0.12

0.14

Time (s)

Error (rads)

(b) UKF Orientation Error

Figure 4: (a) Orientation error for EKF-SLAM as the

back-end in HybridSLAM (b) Orientation error for UKF-

SLAM as the back-end in HybridSLAM.

0.2

0.4

0.6

0.8

1.00

UKF vs. EKF

HybridSLAM

Using UKF-SLAM

Average RMS Positon Error (m)

HybridSLAM

Using EKF-SLAM

Figure 5: A comparison between EKF-SLAM and UKF-

SLAM in a HybridSLAM algorithm.

5 CONCLUSIONS

In this paper, the performance of HybridSLAM

strategy is examined using two different versions of

Kalman filter as the back-end algorithm to fuse local

map into a global map. Results show that with a use

of UKF-SLAM, Root Mean Square (RMS) position

and orientation errors decrease substantially in

comparison with EKF-SLAM. Applying Unscented

Kalman Filter (UKF) allows the state distribution to

be propagated analytically through the third order

linearization of a non-linear system. The

performance of proposed method is compared with a

standard HybridSLAM and accuracy of the process

is examined through 20 iterations. In addition,

simulations and results show that for a non-linear

motion, the use of UKF-SLAM would drastically

increase accuracy of the estimation for a long

trajectory specified in a loop closing case.

REFERENCES

Monjazeb, A., Sasiadek, J. Z., Necsulescu, D.,

Autonomous navigation among large number of

nearby 2011, landmarks using FastSLAM and EKF-

SLAM; a comparative study, Proc. of 16

International Conference on Methods and Models in

Automation and Robotics, pp. 369-374, Miedzyzdroje,

Poland.

Brooks, A., Bailey, T., 2009, HybridSLAM: Combining

FastSLAM and EKF-SLAM for reliable mapping,

Springer Tracts in Advanced Robotics, Volume 57, pp.

647-661.

Sasiadek, J. Z., Monjazeb, A., Necsulescu, D., 2008,

Navigation of an autonomous mobile robot using

EKF-SLAM and FastSLAM, Proc. of 16

Mediterranean Conference on Control and

Automation, pp. 517-522, Ajaccio, France.

Durrant-Whyte H., Bailey T., 2006, Simultaneous

localization and mapping (SLAM): Part I the essential

algorithms, IEEE Robatics and Automation Magazine,

Vol. 13, No. 2, pp. 99-108, June.

Thrun, S., Montemerlo, M., Koller, D., Wegbreit, B.,

Nieto, J., Nebot, E., 2004, FastSLAM: an efficient

solution to the simultaneous localization and mapping

problem with unknown data association, Journal of

Machine Learning Research.

Julier, S. J., Uhlmann, J. K., 2004, Unscented filtering and

nonlinear estimation, Proceedings of IEEE Journal,

Vol. 92, Issue 3, pp. 401-422, March.

Vander Merwe, R., Wan, E., Julier, S., 2004, Sigma-point

Kalman Filters for nonlinear estimation and sensor-

fusion: Applications to integrated navigation”,

Proceedings of the AIAA Guidance, Navigation and

Control Conference, Providence, Rhode Island,

August.

Williams, S. B., Dissanayake, G., Durrant-Whyte, H.,

2002, An Efficient Approach to the Simultaneous

Localisation and Mapping Problem”, Proceedings of

the 2002 IEEE International Conference on Robotics

& Automation, Washington DC.

ICINCO2012-9thInternationalConferenceonInformaticsinControl,AutomationandRobotics

488