Content uploaded by Antonio Sgorbissa

Author content

All content in this area was uploaded by Antonio Sgorbissa on Jan 16, 2018

Content may be subject to copyright.

Human Navigation And Mapping with a 6DOF IMU and a Laser

Scanner

Marco Baglietto, Antonio Sgorbissa, Damiano Verda, Renato Zaccaria1

DIST - University of Genova, Via Opera Pia 13, 16145 Genova, Italy.

Abstract

This article focuses on human navigation, by proposing a system for mapping and self-

localization based on wearable sensors, i.e., a laser scanner and a 6 Degree of Freedom Inertial

Measurement Unit (6DOF IMU) ﬁxed on a helmet worn by the user. The sensor data are fed to

a Simultaneous Localization And Mapping (SLAM) algorithm based on particle ﬁltering, an ap-

proach commonly used for mapping and self-localization in mobile robotics. Given the speciﬁc

scenario considered, some operational hypotheses are introduced in order to reduce the eﬀect of

a well–known problem in IMU–based localization, i.e., position drift. Experimental results show

that the proposed solution leads to improvements in the quality of the generated map with respect

to existing approaches.

Keywords: human navigation and mapping, IMU-based dead reckoning, laser-based SLAM

1. INTRODUCTION

This article focuses on human navigation and is especially oriented towards guaranteeing the

safety and eﬃciency of a group of ﬁrst-responders involved in indoor search and rescue oper-

ations, such as during earthquakes, ﬁres or ﬂoods. The objective is to propose a system for

mapping and self-localization based on wearable sensors. The information returned by the sys-

tem could be used, for instance, to explore the area in an optimal way or to ﬁnd a safe evacuation

route (i.e., providing guidance similarly to a GPS navigation system).

Human navigation has started to receive attention from the scientiﬁc community only recently

[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]. In this domain, an Inertial Measurement Unit (IMU)

is a very common source of information to detect human motion. Since IMUs are often noisy

and, even worse, it is necessary to integrate acceleration data twice to estimate the position, most

works in the ﬁeld focus on solutions to reduce the resulting position drift, which is a consequence

of errors in the estimated velocities. For example, the authors of [2, 3, 4, 5, 14] propose to use

a technique called Zero Velocity Update: they ﬁx the accelerometers to the user’s foot and then,

analysing the pattern of accelerations, identify a time interval in which the foot stands on the

ground and therefore velocities can be reliably estimated to be zero.

1The work has been partially funded by University of Genova and Regione Liguria /SIIT.

Email address: marco.baglietto@unige.it, antonio.sgorbissa@unige.it,

damiano.verda@unige.it, renato.zaccaria@unige.it (Marco Baglietto, Antonio Sgorbissa, Damiano Verda,

Renato Zaccaria)

Preprint submitted to Robotics and Autonomous Systems April 6, 2012

Figure 1: An helmet with a 6DOF IMU and a Hokuyo URG–04LX laser scanner mounted on it. An additional Hokuyo

UTM-30LX laser scanner is on the table.

The present project exhibits some major diﬀerences with respect to the works above. The

main idea of this work is to take inspiration from technologies exploited in mobile robotics (e.g.,

in rescue robotics [15, 16]) not only to estimate the user position, but also to generate a map

of the environment: this concept is also referred to as Simultaneous Localization And Mapping

(SLAM) [17].

In order to perform SLAM, it is assumed that the user is equipped not only with inertial sen-

sors, but also with a laser scanner which returns proximity measurements about the environment.

In order for such measurements to be signiﬁcant, the laser scanner itself must be raised to an

height allowing it to detect obstacles which play an important role for updating the map (like

walls and furniture) and to ignore, for example, small objects on the ﬂoor. The solution adopted

here is to couple a laser scanner and a 6DOF IMU on a helmet (Figure 1). Since the Zero Veloc-

ity Update is not applicable in this case, considering that the inertial sensors are not ﬁxed to the

user’s foot, the proposed approach requires to deal with the well–known problems deriving from

the double integration of noisy data.

The major contribution of this work is to propose a novel solution for Human Navigation and

Mapping which relies on an original approach to face position drift: speciﬁcally, we show that

the output of a standard SLAM algorithm for 2D mapping and some additional heuristics can be

used to improve the position estimate provided by the IMU.

The possibility of adapting SLAM technique for human navigation has been considered also

in other works, e.g., [18, 19]. The work described in [18] relies on a Particle Filter (PF) [20], but

the approach is diﬀerent in that the data provided by the IMU are not integrated to compute an

estimate of the user position, but they are simply used to distinguish if the user is moving or not.

This information is then used to update position by assuming that the user is walking forward

with an almost constant velocity. The approach proposed by [19] has a completely diﬀerent

focus. It relies on a diﬀerent technique based on the Extended Kalman Filter (EKF) [20], and puts

strong constraints on the environment, which is modelled as constituted of orthogonal structural

planes.

2

Section 2 describes related work. Section 3 describes the system architecture, the approach

adopted for Human Localization and Mapping, and for drift reduction. Section 4 describes ex-

perimental results. Conclusions follow.

2. RELATED WORK

Existing solutions to the problem of human navigation can be roughly divided into two

classes: approaches which deal only with path-reconstruction (also referred to as Pedestrian

Dead-Reckoning), and approaches which build also a map of the environment as the user moves

(i.e., performing SLAM).

2.1. Dead-reckoning

Solutions belonging to the ﬁrst class often rely on models of the human gait [21] in order to

deal with the drift problem which follows from the double integration of noisy data. The authors

of [1] propose to ﬁx three accelerometers to the user’s foot, under the assumption that, at each

step, it is possible to recognize a characteristic acceleration pattern [22]. The direction of each

step is estimated using magnetometers. Then, the user position can be computed by considering

that, as shown in [23, 24], the length of a human step is not constant, but it varies around a

stable pivotal value. The system achieves good results, but it is subject to a position error which

grows with the number of steps that the user takes. To reduce this eﬀect, the authors of [2, 3]

propose a technique called Zero Velocity Update. By analysing the acceleration pattern during

a step, they observe that it is possible to identify a time interval during which the foot stands

on the ground. Since velocity can be assumed to be equal to zero in this time interval (unless

the user is dragging his foot), this information can be used to limit the error introduced by the

integration of noisy acceleration data. Experimental results show that this technique reduces the

position error. The authors of [7] propose a model of human motion based on multiple motion

modalities, which correspond to diﬀerent ways to update the user position. In [25] the authors

focus on the development of an algorithm for adaptive step length estimation. Other works

propose innovative pedestrian dead-reckoning systems: for instance by considering the situation

that the user is keeping a mobile phone (equipped with inertial sensors) in her hands [11], or

in the pocket of her trousers [12], i.e., without making a priori assumptions on the position and

orientation of the sensor itself. Finally, the idea of assisting ﬁrst responders with this kind of

technologies is exploited in the WearIT@Work project3presented in [14]. Our approach diﬀers

from the works above in that it does not assume any model of the human gait. This is also a

consequence of the fact that we couple a 6DOF IMU and a laser scanner on a helmet, which

turns out to be a good design solution to reduce occlusions and correctly interpret range data

while performing SLAM, but not for detecting human steps. Then, to reduce positioning drift,

we propose a diﬀerent approach which relies on heuristics and on the output of SLAM itself.

Some approaches in the literature propose to integrate a number of sensors to achieve a higher

positioning accuracy. This is the case, for instance, of [6], which relies on a IMU, a GPS, an

electronic compass, as well as a pedometer based on heel and toe contact switches. The ap-

proach proposes an adaptive knowledge system based on Artiﬁcial Neural Networks (ANN) and

Fuzzy Logic, which is trained during the GPS signal reception and used to maintain navigation

under GPS-denied conditions. In [9] a pedestrian inertial navigation device called NavShoe is

3http://www.wearitatwork.com/

3

presented. This device can work in arbitrary unstructured indoor /outdoor environments and it

consists of a miniature low-power inertial /magnetometer package tucked into the shoelaces of

one foot, wirelessly coupled to a PDA that runs software to merge inertial, geomagnetic, and

optional GPS measurements to derive an optimal position estimate. In [4, 5] the user is provided

with ultrasonic sensors to be deployed in the environment during exploration, and to be used as

landmarks to reduce the estimation error. This approach produces good experimental results, en-

suring also the possibility to manage a high level of electromagnetic noise (which deteriorates the

accuracy of data taken by magnetometers) in a better way. Our approach diﬀers from the works

above in that it does not integrate GPS, compasses, magnetometers, or environmental sensors as

a support to estimate the absolute position or heading of the user.

2.2. Map-building

Solutions belonging to the second class usually refers to SLAM approaches in the literature,

and adapt them to human navigation. SLAM itself is a broad ﬁeld of research, and an extensive

review is outside the scope of this article [20]: therefore, only works related to humanoid robots

and human navigation are discussed in the following.

The authors of [26, 8, 27, 28] deal with the problem of performing SLAM with humanoid

robots: with respect to mobile robotics, this ﬁeld of research deserves a greater attention since it

shares more similarities with human navigation. In [26] the authors present a technique designed

to perform 3D EKF-based SLAM for a humanoid robot equipped with a single camera. Exper-

iments with a human-size robot show accurate positioning, but only when the robot is moving

within an area smaller than 2 ×2 m2. Moreover, the system heavily relies on the fact that the

walking-motion pattern generator is known in advance and can be fed to the EKF: this informa-

tion is not available in our approach (and in human navigation in general). In [28] the authors

propose a system for autonomous navigation and mapping with odometry data and two laser

scanners mounted on the feet of a human-size robot. As in our approach, the authors choose DP-

SLAM [29, 30], and produce maps which are suﬃciently accurate for autonomous navigation in

a typical oﬃce environment. However, the position of laser scanners makes the approach infeasi-

ble for a real-world application in a cluttered environment. A similar approach is proposed in [8],

by mounting the laser scanner on the robot head: once again, the approach relies on accurately

modelling odometry to produce a more accurate particle distribution for the particle ﬁlter which

performs SLAM. In [27] the problem of learning grid maps with a 1-meter tall humanoid robot

is solved using a Rao-Blackwellized particle ﬁlter. The robot is equipped with a laser scanner

and inertial sensors for computing the chest attitude, and exhibits accurate positioning and map

building in a 20×20 m2area without relying on odometry. The approach shares some similarities

with the present work: however, the motion velocity of humanoid robots is necessarily smaller

than that of humans, and therefore comparing performance is diﬃcult.

Few works [18, 19, 31, 13] deal explicitly with human navigation and SLAM. Speciﬁcally, the

HeadSLAM proposed in [18] shares many similarities with the present work, in that it couples

a laser scanner and an IMU on a helmet in order to estimate motion and correct scan data.

Diﬀerently from our approach, which integrates acceleration data to produce an estimate of the

position, the authors use vertical accelerations as a “boolean sensor” to distinguish between two

opposite states, i.e., the user is either walking or not. In the former case, an average linear

speed of 1.35 m/s is assumed, and used to provide an initial odometry guess to be fed to SLAM.

Gyroscopes are used to provide heading information: nevertheless, the approach looks incapable

of distinguish those situations where the user is moving backward or laterally, or even when the

4

head is not aligned with the direction of motion of the body. The authors of [19] present a Laser-

Aided Inertial Navigation System (L-INS) especially thought for visually impaired people: the

IMU measurements are integrated to obtain pose estimates, which are subsequently corrected

through an EKF using line-to-plane correspondences between linear segments in the laser-scan

data and structural planes of the building (e.g., walls). Experiments shows that the system is

able to build reliable maps: however, diﬀerently from our work, it assumes that indoor structural

planes are orthogonal to each other, thus working only in simpler indoor environments. It is worth

noticing that neither of the approaches above rely on an accurate model of the human gait: the

work described in [19] switches to Zero Velocity Update techniques only when range data are not

available for some time. Finally, [31, 13] propose FootSLAM and PlaceSLAM, two systems for

human navigation and mapping which do not rely on the observations provided by range or visual

data. Instead, FootSLAM relies only on the information provided by wearable accelerometers

to detect steps: since human motion is constrained by the topology of the environment, this

information can be integrated to produce a map of the environment itself. PlaceSLAM further

elaborates this concept by integrating on-line features observed by the user herself to label the

map, thus improving performances. These latter approaches are very diﬀerent from the present

work both in their general spirit and in the sensors and techniques adopted for map-building.

3. HUMAN NAVIGATION AND MAPPING

An overview of the system architecture is shown in Figure 2. Two main blocks are visible: 2D

SLAM and Velocity &Attitude Estimation.

1. Prediction

2.

correction

SSS

yx

ψ

,,

map

laser

range

data

d/dt

U

ˆ

Z

measurement

model

−−−−−−

ψθϕ

ˆ

,

ˆ

,

ˆ

,

ˆ

,

ˆ

,

ˆ

Zyx

vvv

heuristics

velocity & attitude estimation

observation

3.

Projection

xS

v

)(),(),(

θϕ

&

&

&EEvE

z

θϕ

ˆ

,

ˆ

,

ˆ

,

ˆ

yx

vv

bzby

ω

ω

ˆ

,

ˆ

IMU

data

2D laser range data

yS

v

ψ

uuu

yx

ˆ

,

ˆ

,

ˆ

S

ψ

EKF

2D

SLAM

Figure 2: Block diagram which shows the main components of the system and information ﬂow.

The 2D SLAM block represents a typical laser-based SLAM algorithm returning a 2D map

5

of the environment4. In the current implementation we have chosen Distributed Particle SLAM

(DP-SLAM)[29, 30], which has the interesting property that it does not make any assumptions

about landmarks: the problem of data association is solved by storing not only the hypothetical

position of signiﬁcant environmental features, but whole maps. Even if a map is a much more

complex type of data with respect to a landmark, DP-SLAM can still guarantee acceptable ex-

ecution time using an accurate technique, called DP-Mapping, to manage the usage of memory

and computational resources. Every iteration, 2D SLAM requires in input an estimate of the user

motion in the 2D Workspace corresponding to a plane parallel to the ﬂoor, as well as a vector of

observations projected on the same plane.

The Velocity &Attitude Estimation block, which is the focus of the present work, has three

main subcomponents:

1. Prediction integrates the data provided by the 6DOF IMU to estimate the user’s velocity

and attitude in the 3D Workspace;

2. Correction, together with Prediction, implements an EKF-inspired algorithm to correct the

estimated velocity and attitude, on the basis of the information returned by 2D SLAM and

additional heuristics, to the end of reducing position drift;

3. Projection projects the user’s velocity as well as laser range data on a plane parallel to the

ﬂoor (i.e., to be fed to 2D SLAM).

In the following, all these components are described in greater details.

3.1. Velocity &attitude estimation

Figure 3 on the right shows the 6DOF IMU mounted on the helmet. The body frame b,

also referred to as IMU-frame, is aligned with the axes of the IMU, with the bxaxis pointing

forward, the byaxis pointing leftward, and the bzaxis pointing upward. The navigation frame n,

represented by the orthogonal axes North, West, and UP (N WU), is a ﬁxed coordinate frame with

respect to which the location of the IMU-frame must be computed (with the NW plane parallel

to the ﬂoor, and the Uaxis pointing upward).

We deﬁne a state vector

x=x y z vxvyvzφ θ ψT,(1)

where the position and velocity of the IMU-frame with respect to the navigation frame are rep-

resented by x,y,z,vx,vy,vz, and its orientation is represented by the three Euler angles φ(roll),

θ(pitch), and ψ(yaw), where the order of rotation is about bz, followed by byand then bx.

The control vector is deﬁned as

u=abx aby abz ωbx ωby ωbz T,(2)

where abx ,aby and abz indicate the linear accelerations along the three axis of the IMU-frame,

4Diﬀerent SLAM algorithms and implementations are available at http://www.openslam.org.

6

bz

a

ˆ

bz

ω

ˆ

by

ω

ˆ

N

IMU-frame

navigation frame

W

U

ψ

ϑ

ϕ

,,,,,,,,

zyx

vvvzyx

n b

bx

a

ˆ

by

a

ˆ

bx

ω

ˆ

Figure 3: The navigation frame n(on the left) and the IMU-frame n(on the right).

and ωbx ,ωby and ωbz indicate the angular velocities. Then, the state equations of the system are

˙x=vx,(3)

˙y=vy,(4)

˙z=vz,(5)

˙vx=abx cos θcos ψ+aby (sin φsin θcos ψ−cos φsin ψ)+

abz(sin φsin ψ+cos φsin θcos ψ),(6)

˙vy=abx cos θsin ψ+aby (cos φcos ψ+sin φsin θsin ψ)+

abz(cos φsin θsin ψ−sin φcos ψ),(7)

˙vz=−abx sin θ+aby sin φcos θ+abz cos φcos θ+g,(8)

˙φ=ωbx +ωby sin φtan θ+ωbz cos φtan θ, (9)

˙

θ=ωby cos φ−ωbz sin φ, (10)

˙

ψ=ωby

sin φ

cos θ+ωbz

cos φ

cos θ.(11)

Given that it is impossible to know the actual components of u, these quantities are measured

through the IMU, which returns an approximation of the control vector, i.e.,

ˆ

u=ˆabx ˆaby ˆabz ˆωbx ˆωby ˆωbzT.(12)

Using equations (3)–(11), the data returned by the IMU can be integrated to produce a pre-

diction of the user attitude φ,θ,ψ, velocity vx,vy,vz, and hence position x,y,z. This leads to

discrete time state equations in the form

xk=f(xk−1,uk).(13)

Speciﬁcally, Prediction integrates (6)–(11) to produce an estimate of the user velocity ˆv−

x,

ˆv−

y, ˆv−

zand attitude ˆφ−,ˆ

θ−,ˆ

ψ−in the 3D Workspace; Correction updates this estimate when

observations are available (described in the next Section); Projection refers to the estimated

values ˆvx, ˆvy, ˆφ,ˆ

θ, together with the control inputs ˆωby, ˆωby, to estimate the user motion on the

NW plane of the navigation frame according to (3) (4), (11), i.e.,

7

0 5 10 15

−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

time (sec)

ˆωbz (rad/sec)

0 5 10 15

−1

0

1

2

3

4

5

6

7

time (sec)

ˆ

ψ(rad)

Figure 4: The measurements taken by one gyroscope vs. time: angular velocity (on the left) and position (on the right).

ˆux=ˆvx,(14)

ˆuy=ˆvy,(15)

ˆuψ=ˆωby

sin ˆφ

cos ˆ

θ+ˆωbz

cos ˆφ

cos ˆ

θ.(16)

2D SLAM uses ˆux, ˆuy, ˆuψto update particles, and returns the most likely hypothesis about the

position xS,ySand heading ψSof the user by taking into account the additional laser range data.

Experiments are performed with a low-cost IMU composed of a 3–axes accelerometer5and

three 1–axis gyroscopes6. By naively integrating (9)–(11) with a sampling time ∆Tequal to

10 ms, an acceptable accuracy in angular measurements is observed. In Figure 4 the angular

velocity and position are shown, while performing a rotation of 2πradians around one axis. The

ﬁnal angle is approximatively equal to 2π, with an error in the order of 10−2radians.

On the other hand, the linear accelerations need to be integrated twice to get a position esti-

mate: ﬁrst, accelerations are projected on the navigation frame and integrated to produce linear

velocities (6)–(8), then velocities must be integrated again to produce an estimate of the position

(3)–(5). In absence of any strategy for drift reduction, errors in the estimated velocities intro-

duces an increasing positioning error which deteriorates more and more the position estimate as

time passes. Figure 5 refers to an experiment where the IMU is initially not in motion, next it is

moved for a given distance along one axis, and then stopped. The estimated velocity increases

(and so does position) when the user starts moving, but at the end of the experiment the velocity

does not decrease to 0, thus producing a drift in the position estimate.

The presence of 2D SLAM helps in reducing this eﬀect, since it relies on laser range data to

provide an estimate of the user position xS,ySand heading ψSwhich is more reliable than com-

puted by naively integrating (3)–(11). However, SLAM itself is aﬀected by errors in ˆvx, ˆvyand

5MMA7260Q model.

6LISY300AL model.

8

0 5 10

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

time (sec)

ˆabx (m/sec2)

0 5 10

−0.4

−0.35

−0.3

−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

time (sec)

ˆvx(m/sec)

0 5 10

−1.2

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

time (sec)

ˆx(m)

Figure 5: The measurements taken by one accelerometer vs. time: linear acceleration (on the left), velocity (in the

middle), and position (on the right).

ˆφ,ˆ

θ, since these components of the state are required by Projection to compute the user motion

on the NW plane, and to consequently propagate particles. This problem will be discussed in the

next Section, where the role played by Correction is described in details.

Finally notice that Projection implements also a policy to correctly interpret laser data. In

fact, when the user neck is inclined forward/backward or left/right (i.e., θ,0 or φ,0), not only

laser range data must be correctly projected on the NW plane (which can be done through sim-

ple trigonometric transformations), but it can even happen that the laser scanner detects objects

which should not be part of the map, i.e., small removable objects, or the ﬂoor itself. Speciﬁ-

cally, given that |θ|and |φ|are below a given threshold, laser range data corresponding to walls,

furniture, or even collapsed structures (in case of an earthquake) are straightforwardly integrated

in a ﬂat 2D model of the environment. If, otherwise, |θ|or |φ|exceed the given threshold, the data

returned by the laser are simply discarded. Projecting all environmental features on a ﬂat 2D

model of the environment yields some limitations, e.g., it needs a manual intervention to label

areas of the map requiring the user to crawl or to climb an object: however, it is considered as an

acceptable compromise to keep the approach simple and computationally eﬃcient.

3.2. Drift reduction

The objective of Correction is to correct the estimates of vx,vy,φ,θ,ψprovided by Prediction

through observations, which partly derive from the outputs (O.1 and O.2) of 2D SLAM, and partly

from operational hypotheses that are used as heuristics (H.1 and H.2) for data ﬁltering:

O.1 the angle ψSreturned by 2D SLAM can be used to update ˆ

ψ;

O.2 the planar velocities vxS ,vyS can be estimated by numerically diﬀerentiating xS,yS(returned

by 2D SLAM) with respect to time, and used to update ˆvx, ˆvy;

H.1 the expected acceleration E(˙vz) along the Uaxis of the navigation frame is equal to 0, with

a given variance;

H.2 the expected neck rotational velocities E( ˙φ) and E(˙

θ) are equal to 0, with a given variance.

9

The estimate of the user velocity provided by SLAM (O.2) proves to be particularly useful

when the user is not moving: in this case, laser data are “very similar” in subsequent scans

and data association is easier, therefore allowing to estimate vx≈vy≈0. Asserting that the

acceleration along the Uaxis of the navigation frame is equal to 0 (H.1) corresponds to saying

that there cannot be a persistent movement along the vertical axis. If it were not so, we would

have to deduce from (8) that the user is going oﬀthe ground. To avoid this, it is assumed that

the average value is 0 with a given variance. The situation in which the user suddenly falls or

starts crawling does not pose any problem from this point of view: this kind of action produces

a sudden change in the acceleration along the Uaxis and, after that, the average value tends to 0

again (laser range data are possibly discarded during the transition). Similarly, it is assumed that

the neck angular velocities are equal to 0 on average, with a given variance (H.2). If it were not

so, it could be necessary to accept that, at some point, the user is walking upside down.

Let the reduced state vector x′

kinclude all components of the state to be corrected, i.e.,

x′

k=vx,kvy,kφkθkψkT.(17)

The discrete state equations for x′

k(used in the prediction phase of the EKF) are not dependent

on the remaining components of xk, and hence directly follow from the integration of (6), (7),

(9)–(11), i.e., x′

k=f′(x′

k−1,uk).

The expected measurement at time step k, i.e., the measurement model h(x′

k,uk), is

h1=−abx,ksin θk+aby,ksin φkcos θk+abz,kcos φkcos θk+g,(18)

h2=ωbx,k+ωby,ksin φktan θk+ωbz,kcos φktan θk,(19)

h3=ωby,kcos φk−ωbz,ksin φk,(20)

h4=vx,k,(21)

h5=vy,k,(22)

h6=ψk.(23)

whereas, according to O.1, O.2, H.1, H.2, the actual measurement vector zkat time step kis

zk=0 0 0 vxS,kvyS,kψS,kT,(24)

where zk,1represents the average acceleration along the Uaxis, zk,2and zk,3represent the average

derivatives of the roll and the pitch, zk,4and zk,5represent the linear velocities along the Nand W

axes, zk,6represents the yaw.

Strictly speaking, the components zk,1,zk,2,zk,3in (24) are not “measurements,” since they

do not depend on the state at time step k. Then, concerning those components, the EKF has

simply the eﬀect of a High Pass Filter, which suppresses the low frequencies of ˙vz, ˙φ,˙

θ(forcing

derivatives to have zero mean), and attenuates or leaves high frequencies unaltered depending

on the value of the Kalman gain. This reﬂects on the corresponding components of the state,

ﬁnally producing a dynamic behaviour which is coherent with the motion constraints that have

been hypothesized in H.1, H.2.

The covariance matrices Qand R, associated respectively with the noise which aﬀects the

motion model and the measurement, are evaluated through experiments. Roughly speaking, the

covariance on zk,1,zk,2,zk,3must be suﬃciently “high” to take into account that the corresponding

components in (24) refer to expected values instead of actual measurements. On the opposite,

10

the covariance on zk,4,zk,5,zk,6must be suﬃciently “small” to guarantee that the components of

the state observed by 2D SLAM are weighted more than those predicted on the basis of the IMU

alone. Finally notice that the prediction and the measurement model depend on the control vector

uk: this is not a problem because, while computing the Jacobian matrices to linearise the model,

the controls can be treated as constant values for every sampling instant k.

In principle, other estimators could be used in place of the EKF: the EKF has been chosen

since it provides a framework which can be easily extended to consider other measurements,

provided by other sensors or deduced from other hypotheses. Notice also that, diﬀerently from

the approach adopted here, the EKF could ideally be implemented in a single logical block with

the SLAM algorithm. By adopting a loosely coupled approach, it is possible to integrate in the

system diﬀerent SLAM algorithms with greater ease.

3.3. Observability analysis

It is necessary to verify that all the estimated components of the state, i.e., vx,vy,φ,θ,ψ, are

locally corrected by the EKF. To verify this, we compute the Jacobian matrices F′

kand Hkat time

step k, where

F′

k,i j =∂f′

i

∂x′

j

(xk,uk),Hk,i j =∂hi

∂x′

j

(xk,uk).(25)

Then, we compute the Kalman observability matrix

O=HT

k(HkF′

k)T· · · (HkF′4

k)TT(26)

and analyse how rank(O) varies depending on the linearisation performed on f′(x′

k−1,uk) and

h(x′

k,uk). Since O∈ ℜ30×5we initially perform this analysis with the support of the MatLab

Symbolic Math Toolbox. The analysis returns that, under some conditions, the system is fully

observable with rank(O)=5. In particular, by analysing (21)–(23), it is straightforward to verify

that vx,k,vy,k,ψkare always observable (regardless of the values assumed by the other components

of x′

kand by uk). Then we check under which conditions the remaining components φk,θkare

observable.

The equations in (9)–(10) and (18)–(20) show that a reduced state vector

x′′

k=φkθkT(27)

can be decoupled from the other components of x′

k. In fact (9)–(10) depend only on φk,θk, and

the measurement model can be analogously split into two parts, a set of equations

h′(x′

k,uk)=h1h2h3(28)

which depends only on φk,θk(18)–(20), and a second set of equations depending only on vx,k,

vy,k,ψy,k(21)–(23).

Considering the reduced state x′′

kand the corresponding measurement vector h′(x′

k,uk), let the

corresponding Jacobian matrices be

F′′

k=a c

b0(29)

H′

k=

d e

a c

b0

(30)

11

where

a=ωby,kcos φktan θk−ωbz,ksin φktan θk

b=−ωby,ksin φk−ωbz,kcos φk

c=−b(tan2θk+1)

d=aby,kcos φkcos θk−abz,ksin φkcos θk

e=−abx,kcos θk−aby,ksin φksin θk−abz,kcos φksin θk

(31)

and ﬁnally

O′=H′T(H′F′′)TT=d a b ad +be a2+bc ab

e c 0cd ac bcT

.(32)

The maximum possible rank of O′is 2. By computing the determinant of all 2 ×2 submatrices

of O′, it can be inferred that O′loses rank if and only if ae =0 and b=0, since in this case

c=0 as well (31). For instance, the system is not fully observable if both the angular speeds

ωby,kand ωbz,kare equal to 0, and consequently a=b=0. In this case, which corresponds to

the IMU-frame moving on a straight line with a ﬁxed yaw and pitch, the heuristic H.2 gives no

contribution, and the heuristic H.1, taken alone, is only able to correct a linear combination of

φ,θ. However, it is suﬃcient that the user performs a curve, or temporarily rotates her neck to

the left or to the right, to cause ωbz ,0, and to consequently correct all components of the state.

3.4. Calibration

A calibration procedure is available to detect the initial attitude of the IMU before operations,

or whenever deemed necessary to re-calibrate the system. The IMU is required to stand still

during the calibration procedure: then, an acceleration equal to 0 is expected to be measured

along all axes of the navigation frame, and the measurement vector z′′

kat time step kcan be

assumed to be

z′′

k=000T,(33)

where z′′

k,1,z′′

k,2,z′′

k,3represent the accelerations along N,W, and U.

The state vector to be estimated during calibration is

x′′′

k=φkθkψkT(34)

and the corresponding measurement model h′′(x′

k,uk) is

h1=abx,kcos θkcos ψk+

aby,k(sin φksin θkcos ψk−cos φksin ψk)+

abz,k(sin φksin ψk+cos φksin θksin ψk)

(35)

h2=abx,kcos θksin ψk+

aby,k(cos φkcos ψk+sin φksin θksin ψk)+

abz,k(cos φksin θksin ψk−sin φkcos ψk)

(36)

h3=−abx,ksin θk+ˆaby,ksin φkcos θk+

abz,kcos φkcos θk+g(37)

(38)

12

0 20 40 60 80 100

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

time (10−2sec)

ˆϕ(rad)

0 20 40 60 80 100

−0.05

−0.045

−0.04

−0.035

−0.03

−0.025

−0.02

−0.015

−0.01

−0.005

0

time (10−2sec)

ˆ

θ(rad)

0 20 40 60 80 100

−0.03

−0.025

−0.02

−0.015

−0.01

−0.005

0

time (10−2sec)

ˆ

ψ(rad)

Figure 6: Plot of φ,θ, and ψduring calibration. The EKF is iterated every 10−2s.

The calibration phase corresponds to computing the Euler angles of a vector abxbx+aby by+

abzbzwith norm gby measuring its projection ˙vx, ˙vy, ˙vzon the axes of the navigation frame, and

therefore this procedure necessarily yields a solution which is not unique in the sense of the Euler

angle representation.

4. EXPERIMENTAL RESULTS

In the current set-up, the following sensors have been selected: i) the low–cost Atomic 6DOF

IMU - XBee Ready, which includes a 3–axis accelerometer (MMA7260Q model) and three

1–axis gyroscopes (LISY300AL model); ii) the Hokuyo URG–04LX laser scanner, with a max-

imum sensing range of about 4 m, a scanning area of 240 deg with angular resolution 0.36 deg,

and a scanning time of 100 ms for a full scan; iii) the more performing Hokuyo UTM-30LX

Laser (used only for some experiments) with a sensing range of about 30 m, a scanning area of

270 deg with angular resolution 0.25 deg, and a scanning time of 25 ms for a full scan. As it is

shown in Figure 1, the sensors are mounted on a helmet, in order to detect the obstacles that are

signiﬁcant for mapping, e.g., walls or furniture: obstacles which are slightly above or below the

head are simply projected on the map on the basis of the current estimate of the helmet attitude

(as described in Section 3.1). The sensors are chosen by considering also their weight, so that the

whole helmet weighs no more than a helmet with a lamp mounted on it (the Hokuyo UTM-30LX

exhibits higher performance, but it is also bigger and heavier than the Hokuyo URG–04LX, i.e.,

0.23 Kg vs. 0.14 Kg).

Figure 6 shows the accuracy of the calibration procedure by placing the helmet on the ﬂoor,

and by performing a rotation of about 45 deg around the Naxis: the ﬁnal values are very close to

the expected ones (φ=0.785, θ=0, ψ=0 rad) with an error in the order of 10−2rad. It is also

possible to observe that, in this case, only 20 measurements are needed to have a good estimate;

in all the executed tests, the number of measurements needed for calibration never exceeds 100.

Considering that the IMU sampling time is 10 ms, the maximum time needed for calibration

is about 1 second. This is particularly important whenever the user wishes to perform a full

re-calibration of the system during operations.

13

In order to validate the approach during motion, experiments have been performed in two

diﬀerent environments: the second ﬂoor of the DIST research department and the interiors of

S.I.I.T. (Integrated Intelligent Systems and Technologies), a large research facility in Genoa

where the University and industrial companies run joint laboratories to cooperate on research

projects related to security and automation. The approach described in the present article have

been compared with HeadSLAM [18]. As discussed in Section 2, HeadSLAM uses accelerom-

eters only to classify the motion state of the user. In order to avoid classiﬁcation errors in this

phase (i.e., to compare our algorithm with an “ideal implementation” of HeadSLAM), we have

re-implemented HeadSLAM by manually specifying when the user is walking or not: in the for-

mer case, as prescribed by the algorithm, an average linear velocity of 1.35 m/s is assumed in the

forward direction.

Figure 7 corresponds to an experiment performed at the second ﬂoor of DIST. The user starts in

the corridor at the bottom of the map, performs some movements in place (to purposely produce

an error in the estimated velocities and, consequently, a position drift), and ﬁnally walks straight

toward the hall at the top of the map with an approximately constant velocity. The map built

during operations is shown in gray and the plan of the building is superimposed in red, therefore

allowing for a comparison between ground truth and actual data. IMU and laser data are acquired

during operations, and then processed oﬀ-line in diﬀerent ways in order to validate the solutions

proposed throughout the article. The Figures correspond to: 7.a) our algorithm; 7.b) HeadSLAM;

7.c) our algorithm, without using accelerometers to estimate linear velocities, i.e., by assuming

˙vx=˙vy=˙vx=0 in (6)–(8); 7.d) our algorithm, without correcting vx,vywith the output of

SLAM through the EKF. In all the Figures 100 particles have been used for DP-SLAM, and

data are processed with an Intel Core 2 1.66 GHz processor and 1 GB of RAM: processing time

is from 1.2 to 2.2 times greater than actual operation time, and this would be a problem for a

real-time approach (the work described in [18] do not exhibit real-time results as well).

The corridor in Figure 7 is a critical case: in fact, the diﬀerence between consecutive measure-

ments of the laser, whether the user moves or not, are very similar, and therefore scan matching

turns out to be not very helpful. Under these conditions the experiments in Figures 7.a and 7.b

yield acceptable results, with some diﬀerences: our approach initially tends to underestimate the

length of the path travelled by the user, but then it improves and ﬁnally provides a map which

matches very closely the walls and furniture of the building; HeadSLAM provides a better esti-

mate of the corridor length (since in this case the user is walking looking forward as prescribed

by the model), but it behaves worse when estimating the orientation of the hall at the top of the

map. Figure 7.c shows that, without the contribution of accelerometers, the SLAM algorithm

tends to severely underestimate the length of the corridor: this is due to the low sensing range

of the laser and the small number of features to disambiguate the user position. Finally, Figure

7.d shows the dramatic eﬀect of position drift, when integrating accelerometers in absence of

a method to correct the resulting linear velocities, thus validating the drift reduction approach

based on vxS ,vyS .

Figure 8 shows results when processing the same data as above, by reducing the number of

particles used for SLAM to increase real-time responsiveness. The Figures correspond to: 8.a)

our algorithm with 50 particles; 8.b) our algorithm with 30 particles. Under these conditions, the

map tends to be less accurate: in 8.a the length of the corridor is underestimated, whereas in 8.b

the orientation of walls is aﬀected by errors. In the experiment corresponding to Figure 8.a the

processing time is from 1.1 to 1.6 times greater than actual operation time. In the case in Figure

8.b the processing time is approximately equal to operation time, thus allowing for a real-time

implementation of the algorithm at the price of a lower accuracy in mapping and localization.

14

Figure 7: Corridor of the second ﬂoor of DIST. a) our algorithm with 100 particles; b) HeadSLAM; c) our algorithm

without accelerometers; d) our algorithm without EKF correction on vx,vy.

15

Figure 8: Corridor of the second ﬂoor of DIST. a) Our algorithm with 50 particles; a) our algorithm with 30 particles.

Figure 9: Oﬃce at the second ﬂoor of DIST. a) Our algorithm; b) HeadSLAM; c) our algorithm without EKF correction

on φ,θ.

16

Figure 10: a) Loop closure; b) small apartment; c) laboratory.

Figure 9 shows results of a diﬀerent experiment performed in the same environment. The user

starts in the corridor at the bottom of the map, moves forward for a short walk, and ﬁnally enters

the oﬃce on the left. The Figures correspond to: 9.a) our algorithm; 9.b) HeadSLAM; 9.c) our

algorithm, but without correcting φ,θthrough the EKF according to H.1, H.2. Under these

conditions only the algorithm in Figure 9.a produces acceptable results, whereas HeadSLAM

in Figure 9.b fails completely: this is likely due to the fact that the user does not look in the

direction of motion when entering the oﬃce, but instead it rotates the head before the body,

which produces a wrong estimate of her position and attitude. The result in Figure 9.c is not

acceptable as well, thus validating the contribution of the heuristics introduced.

Figure 10.a shows the system behaviour while dealing with a short loop. The user starts in the

laboratory, then she goes out through one door and gets back in through the other one, moving

along a cyclic path. After that, she gets out the room and moves along the corridor. It can be

noticed that, as in previous experiments, in Figure 10.a the length of the corridor tends to be

underestimated in absence of features (the behaviour in presence of longer loops have not been

tested because the SLAM algorithm per se is not the focus of this article). Figures 10.b and

10.c shows the behaviour of the system in diﬀerent environments, i.e., a small apartment and a

laboratory at the ﬁrst ﬂoor of DIST.

Figure 11 shows experiments in a larger environment performed at SIIT. The correspond to:

11.a) our algorithm, using the URG–04LX laser scanner with a maximum range of 4 m; 11.b)

our algorithm, using the UTM–30LX laser scanner with a maximum range of 30 m; 11.c) same

dataset as 11.b, but processed with HeadSLAM; 11.d) same dataset as 11.d, but without using

IMU data; 11.e) same dataset as 11.b, but ignoring all laser range measurements returning a

distance higher than 4 m.

Figure 11.a shows that, in larger environments, the UTM–30LX laser scanner does not allow

17

Figure 11: S.I.I.T. a) our algorithm, URG–04LX laser scanner; b) our algorithm, UTM–30LX laser scanner; c) Head-

SLAM, UTM–30LX laser scanner; d) our algorithm, without IMU; e) our algorithm, discarding laser range measure-

ments >4 m.

18

the system to produce accurate maps: the topology of the environment is preserved to a certain

extent, but its geometry is deformed. Accurate results are achieved in Figure 11.b by using the

UTM–30LX laser scanner, in which case our algorithm produces a map which matches walls

and furniture very closely, whereas HeadSLAM in Figure 11.c fails almost completely. Figure

11.d conﬁrms that the contribution of the IMU is fundamental for achieving the results above.

Finally, Figure 11.e, compared with Figure 11.a, oﬀers unexpected insights: in fact, it shows that

the superior performance of the UTM–30LX laser scanner is not only due to the higher sensing

range, but more likely to the higher scanning frequency, resolution, and ﬁeld of view.

5. Conclusions

In this paper, a method for human navigation and mapping has been proposed which is based

on wearable sensor (a 6DOF IMU and a laser scanner) to be mounted on a helmet. The system

is especially targeted to search and rescue operations, where it is important to have a decision

support in real-time to optimize exploration and to ﬁnd evacuation routes.

The approach is diﬀerent from the other approaches in the literature in that

•it does not require a model of the human gait, since all data returned by the IMU are inte-

grated to produce an estimate of the sensor position and attitude, and merged with laser data

through a standard 2D SLAM algorithm;

•it proposes an original solution, which is partly based on the output of the SLAM algo-

rithm and partly on heuristics, to reduce the position drift which follows from the double

integration of noisy data.

The approach has been tested in real-world experiments in diﬀerent environments, validating

the solutions proposed throughout the article and oﬀering insights on how to improve the system.

We are currently extending the approach to work in more complex scenarios involving diﬀerent

ﬂoors of the same building, to consider the possibility that the user switches among diﬀerent

walking modalities (i.e., crawling, running, lying on the ground, or climbing), and ﬁnally to rely

on more sensors, or sensors with superior performance.

References

[1] R. Stirling, K. Fyfe, G. Lachapelle, Evaluation of a New Method of Heading Estimation for Pedestrian Dead

Reckoning Using Shoe Mounted Sensors, Journal of Navigation 58 (2005) 31–45.

[2] L. Ojeda, J. Borenstein, Non-GPS Navigation for Security Personnel and First Responders, Journal of Navigation

60 (2007) 391–+.

[3] L. Ojeda, J. Borenstein, Personal dead-reckoning system for gps-denied environments, in: IEEE International

Workshop on Safety, Security and Rescue Robotics, 2007. SSRR 2007., 2007, pp. 1 –6.

[4] C. Fischer, K. Muthukrishnan, M. Hazas, H. Gellersen, Ultrasound-aided pedestrian dead reckoning for indoor

navigation, in: Proceedings of the ﬁrst ACM international workshop on Mobile entity localization and tracking in

GPS-less environments, MELT ’08, 2008, pp. 31–36.

[5] C. Fischer, H. Gellersen, Location and navigation support for emergency responders: A survey, Pervasive Comput-

ing, IEEE 9 (1) (2010) 38 –47.

[6] C. Toth, D. Grejner-Brzezinska, S. Moaﬁpoor, Pedestrian tracking and navigation using neural networks and fuzzy

logic, IEEE International Symposium on Intelligent Signal Processing, 2007. WISP 2007 (2007) 1 –6.

[7] C. Lyons, B. Ommert, S. Thayer, Bipedal motion estimation with the human odometer, IEEE Conference on

Robotics, Automation and Mechatronics, 2004 2 (2004) 673 – 678 vol.2.

[8] S. Thompson, S. Kagami, K. Nishiwaki, Localisation for autonomous humanoid navigation, 6th IEEE-RAS Inter-

national Conference on Humanoid Robots, 2006 (2006) 13 –19.

19

[9] E. Foxlin, Pedestrian tracking with shoe-mounted inertial sensors, IEEE Computer Graphics and Applications

25 (6) (2005) 38 –46.

[10] S. Zhang, An adaptive unscented kalman ﬁlter for dead reckoning systems, International Conference on Information

Engineering and Computer Science, 2009. ICIECS 2009. (2009) 1 –4.

[11] S. H. Shin, M. S. Lee, C. G. Park, H. S. Hong, Pedestrian dead reckoning system with phone location awareness

algorithm, IEEE/ION Position Location and Navigation Symposium (PLANS), 2010 (2010) 97 –101.

[12] U. Steinhoﬀ, B. Schiele, Dead reckoning from the pocket - an experimental study, IEEE International Conference

on Pervasive Computing and Communications (PerCom), 2010 (2010) 162 –170.

[13] P. Robertson, M. Angermann, M. Khider, Improving simultaneous localization and mapping for pedestrian nav-

igation and automatic mapping of buildings by using online human-based feature labeling, IEEE/ION Position

Location and Navigation Symposium (PLANS), 2010 (2010) 365 –374.

[14] S. Beauregard, Omnidirectional pedestrian navigation for ﬁrst responders, 4th Workshop on Positioning, Naviga-

tion and Communication, 2007. WPNC ’07. (2007) 33 –36.

[15] S. Berrabah, Y. Baudoin, H. Sahli, SLAM for robotic assistance to ﬁre-ﬁghting services, 8th World Congress on

Intelligent Control and Automation (WCICA), 2010 (2010) 362 –367.

[16] Y. Baudoin, D. Doroftei, G. De Cubber, S. Berrabah, C. Pinzon, F. Warlet, J. Gancet, E. Motard, M. Ilzkovitz,

L. Nalpantidis, A. Gasteratos, View-ﬁnder : Robotics assistance to ﬁre-ﬁghting services and crisis management,

IEEE International Workshop on Safety, Security Rescue Robotics (SSRR), 2009 (2009) 1 –6.

[17] D. Fox, W. Burgard, F. Dellaert, S. Thrun, Monte carlo localization: eﬃcient position estimation for mobile robots,

in: Proceedings of the sixteenth national conference on Artiﬁcial intelligence, AAAI ’99/IAAI ’99, American

Association for Artiﬁcial Intelligence, Menlo Park, CA, USA, 1999, pp. 343–349.

[18] B. Cinaz, H. Kenn, HeadSLAM - simultaneous localization and mapping with head-mounted inertial and laser

range sensors, in: 12th IEEE International Symposium on Wearable Computers, 2008. ISWC 2008., 2008.

[19] J. Hesch, F. Mirzaei, G. Mariottini, S. Roumeliotis, A laser-aided inertial navigation system (L-INS) for human

localization in unknown indoor environments, IEEE International Conference on Robotics and Automation (ICRA),

2010 (2010) 5376 –5382.

[20] H. Durrant-Whyte, T. Bailey, Simultaneous localization and mapping: part i, Robotics Automation Magazine, IEEE

13 (2) (2006) 99 –110. doi:10.1109/MRA.2006.1638022.

[21] A. Jimenez, F. Seco, C. Prieto, J. Guevara, A comparison of pedestrian dead-reckoning algorithms using a low-cost

mems imu, in: Intelligent Signal Processing, 2009. WISP 2009. IEEE International Symposium on, 2009, pp. 37

–42.

[22] J. Rose, J. G. Gamble, Human Walking, Williams & Wilkins, 1993.

[23] Q. Ladetto, Capteurs at algorithmes pour la localisation autonome en mode p´

edestre, Ph.D. thesis, EPFL (2003).

[24] J. S. J Kappi, J. Saarinen, Mems-imu based pedestrian navigator for handheld devices, Tech. rep., ION GPS (2001).

[25] S. Shin, C. Park, J. Kim, H. Hong, J. Lee, Adaptive step length estimation algorithm using low-cost mems inertial

sensors, IEEE Sensors Applications Symposium, 2007. SAS ’07. (2007) 1 –5.

[26] O. Stasse, A. J. Davison, R. Sellaouti, K. Yokoi, Real-time 3d SLAM for humanoid robot considering pattern

generator information, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006 (2006) 348

–355.

[27] C. Stachniss, M. Bennewitz, G. Grisetti, S. Behnke, W. Burgard, How to learn accurate grid maps with a humanoid,

IEEE International Conference on Robotics and Automation, 2008. ICRA 2008. (2008) 3194 –3199.

[28] R. Tellez, F. Ferro, D. Mora, D. Pinyol, D. Faconti, Autonomous humanoid navigation using laser and odometry

data, 8th IEEE-RAS International Conference on Humanoid Robots, 2008. Humanoids 2008. (2008) 500 –506.

[29] A. Eliazar, R. Parr, DP-SLAM: fast, robust simultaneous localization and mapping without predetermined land-

marks, in: Proceedings of the 18th international joint conference on Artiﬁcial intelligence, Morgan Kaufmann

Publishers Inc., San Francisco, CA, USA, 2003, pp. 1135–1142.

[30] A. Eliazar, R. Parr, Dp-SLAM 2.0, in: IEEE International Conference on Robotics and Automation, 2004. Pro-

ceedings. ICRA ’04. 2004, 2004.

[31] P. Robertson, M. Angermann, B. Krach, Simultaneous localization and mapping for pedestrians using only foot-

mounted inertial sensors, in: Proceedings of the 11th international conference on Ubiquitous computing, Ubicomp

’09, 2009, pp. 93–96.

20