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) fixed on a helmet worn by the user. The sensor data are fed to
a Simultaneous Localization And Mapping (SLAM) algorithm based on particle filtering, an ap-
proach commonly used for mapping and self-localization in mobile robotics. Given the specific
scenario considered, some operational hypotheses are introduced in order to reduce the effect 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 efficiency of a group of first-responders involved in indoor search and rescue oper-
ations, such as during earthquakes, fires or floods. 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 find a safe evacuation
route (i.e., providing guidance similarly to a GPS navigation system).
Human navigation has started to receive attention from the scientific 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 field 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 fix 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 differences 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 significant, 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 floor. 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 fixed 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: specifically, 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 different 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 different
focus. It relies on a different 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 first 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 fix 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 effect, 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 different 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 first responders with this kind of
technologies is exploited in the WearIT@Work project3presented in [14]. Our approach differs
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 different 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 Artificial 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 differs 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 field 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 field 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 sufficiently accurate for autonomous navigation in
a typical office 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 filter 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 filter. 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 difficult.
Few works [18, 19, 31, 13] deal explicitly with human navigation and SLAM. Specifically, 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.
Differently 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, differently 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 different 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 flow.
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 significant 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 floor, 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
floor (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 fixed coordinate frame with
respect to which the location of the IMU-frame must be computed (with the NW plane parallel
to the floor, and the Uaxis pointing upward).
We define 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 defined 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,
4Different 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)
Specifically, 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
final 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: first, 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 effect, 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 affected 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 floor itself. Specifi-
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 flat 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 flat 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 efficient.
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 filtering:
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 differentiating 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 offthe 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 effect 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 reflects on the corresponding components of the state,
finally 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 affects the
motion model and the measurement, are evaluated through experiments. Roughly speaking, the
covariance on zk,1,zk,2,zk,3must be sufficiently “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 sufficiently “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, differently 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 different 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 finally
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 fixed 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 sufficient 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
significant 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 floor,
and by performing a rotation of about 45 deg around the Naxis: the final 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
different environments: the second floor 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 classification 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 floor 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 finally 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 off-line in different 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 difference 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 differences: our approach initially tends to underestimate the
length of the path travelled by the user, but then it improves and finally 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 effect 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 affected 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 floor 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 floor of DIST. a) Our algorithm with 50 particles; a) our algorithm with 30 particles.
Figure 9: Office at the second floor 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 different 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 finally enters
the office 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 office, 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 different environments, i.e., a small apartment and a
laboratory at the first floor 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 confirms that the contribution of the IMU is fundamental for achieving the results above.
Finally, Figure 11.e, compared with Figure 11.a, offers 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 field 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 find evacuation routes.
The approach is different 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 different environments, validating
the solutions proposed throughout the article and offering insights on how to improve the system.
We are currently extending the approach to work in more complex scenarios involving different
floors of the same building, to consider the possibility that the user switches among different
walking modalities (i.e., crawling, running, lying on the ground, or climbing), and finally 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 first 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. Moafipoor, 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 filter 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. Steinhoff, 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 first 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 fire-fighting 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-finder : Robotics assistance to fire-fighting 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: efficient position estimation for mobile robots,
in: Proceedings of the sixteenth national conference on Artificial intelligence, AAAI ’99/IAAI ’99, American
Association for Artificial 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 Artificial 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
