ArticlePDF Available

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

Authors:

Abstract and Figures

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 approach 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.
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 eect 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 eciency 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 dierences 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 dierent 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 dierent
focus. It relies on a dierent 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 eect, 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 dierent 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 diers
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 dierent 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 diers 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 suciently accurate for autonomous navigation in
a typical oce 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 dicult.
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.
Dierently 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, dierently 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 dierent 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.
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,
4Dierent 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(xk1,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 102radians.
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 eect, 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 aected 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 ecient.
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 dierentiating 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 vxvy0. 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 othe 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
k1,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 eect 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 aects the
motion model and the measurement, are evaluated through experiments. Roughly speaking, the
covariance on zk,1,zk,2,zk,3must be suciently “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 suciently “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, dierently 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 dierent 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· · · (HkF4
k)TT(26)
and analyse how rank(O) varies depending on the linearisation performed on f(x
k1,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 θkabz,ksin φkcos θk
e=abx,kcos θkaby,ksin φksin θkabz,kcos φksin θk
(31)
and finally
O=HT(HF′′)TT=d a b ad +be a2+bc ab
e c 0cd ac bcT
.(32)
The maximum possible rank of Ois 2. By computing the determinant of all 2 ×2 submatrices
of O, it can be inferred that Oloses 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 sucient 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 ψkcos φ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 ψksin φ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 (102sec)
ˆϕ(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 (102sec)
ˆ
θ(rad)
0 20 40 60 80 100
−0.03
−0.025
−0.02
−0.015
−0.01
−0.005
0
time (102sec)
ˆ
ψ(rad)
Figure 6: Plot of φ,θ, and ψduring calibration. The EKF is iterated every 102s.
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 102rad. 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
dierent 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 o-line in dierent 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 dierence 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 dierences: 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 eect 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 aected 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: Oce 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 dierent 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 oce 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 oce, 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 dierent 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, oers 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 dierent 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 dierent environments, validating
the solutions proposed throughout the article and oering insights on how to improve the system.
We are currently extending the approach to work in more complex scenarios involving dierent
floors of the same building, to consider the possibility that the user switches among dierent
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. 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 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: ecient 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
... Weingarten and Siegwart [43] rotated the 2D laser range finder (Sick) to generate a 3D point cloud of an indoor environment that feeds 3D SLAM. Some systems that employ 3D SLAM have been designed to explore the 3D space in some cases like human navigation and rescue operations [44]. Recently, the 3D SLAM becomes indispensable to operate mobile service robots in unknown environments [45]. ...
Article
Full-text available
Positioning is a need for many applications related to mapping and navigation either in civilian or military domains. The significant developments in satellite-based techniques, sensors, telecommunications, computer hardware and software, image processing, etc. positively influenced to solve the positioning problem efficiently and instantaneously. Accordingly, the mentioned development empowered the applications and advancement of autonomous navigation. One of the most interesting developed positioning techniques is what is called in robotics as the Simultaneous Localization and Mapping SLAM. The SLAM problem solution has witnessed a quick improvement in the last decades either using active sensors like the RAdio Detection And Ranging (Radar) and Light Detection and Ranging (LiDAR) or passive sensors like cameras. Definitely, positioning and mapping is one of the main tasks for Geomatics engineers, and therefore it's of high importance for them to understand the SLAM topic which is not easy because of the huge documentation and algorithms available and the various SLAM solutions in terms of the mathematical models, complexity, the sensors used, and the type of applications. In this paper, a clear and simplified explanation is introduced about SLAM from a Geomatical viewpoint avoiding going into the complicated algorithmic details behind the presented techniques. In this way, a general overview of SLAM is presented showing the relationship between its different components and stages like the core part of the front-end and back-end and their relation to the SLAM paradigm. Furthermore, we explain the major mathematical techniques of filtering and pose graph optimization either using visual or LiDAR SLAM and introduce a summary of the deep learning efficient contribution to the SLAM problem. Finally, we address examples of some existing practical applications of SLAM in our reality.
... The aforementioned Functionality 1 has been investigated, among the others, in [1], [2], [3], [4], [5], [6], [7]: 2D/3D Simultaneous Localization And Mapping using wearable sensors raise well-known issues that have been intensively addressed in the SLAM Literature [8], [9], such as graph optimisation and loop-closure [10], [11], and therefore do not deserve to be further explored here. Functionality 2, on the opposite, has been only partially addressed by researchers in intelligent systems [12], especially when focusing on emergency scenarios. ...
Conference Paper
Full-text available
This article proposes a framework to model a scenario in which First Responders, citizens, and smart devices and/or robots explore the environment in an emergency situation, i.e., after an earthquake, assessing damages, and searching for people needing assistance. While moving, the agents observe events and exchange the information collected with other agents encountered, thanks to common network connections. When some conditions hold, the agents can upload the collected information to a Control Room/database in the Cloud. The model includes a detailed description of how data are exchanged between agents and stored in the database. A simulated experiment has been carried out in a real-world street network, with the aim of evaluating the feasibility and performances of the approach.
... Weingarten and Siegwart [43] rotated the 2D laser range finder (Sick) to generate a 3D point cloud of an indoor environment that feeds 3D SLAM. Some systems that employ 3D SLAM have been designed to explore the 3D space in some cases like human navigation and rescue operations [44]. Recently, the 3D SLAM becomes indispensable to operate mobile service robots in unknown environments [45]. ...
Article
Full-text available
Positioning is a need for many applications related to mapping and navigation either in civilian or military domains. The significant developments in satellite-based techniques, sensors, telecommunications, computer hardware and software, image processing, etc. positively influenced to solve the positioning problem efficiently and instantaneously. Accordingly, the mentioned development empowered the applications and advancement of autonomous navigation. One of the most interesting developed positioning techniques is what is called in robotics as the Simultaneous Localization and Mapping SLAM. The SLAM problem solution has witnessed a quick improvement in the last decades either using active sensors like the RAdio Detection And Ranging (Radar) and Light Detection and Ranging (LiDAR) or passive sensors like cameras. Definitely, positioning and mapping is one of the main tasks for Geomatics engineers, and therefore it's of high importance for them to understand the SLAM topic which is not easy because of the huge documentation and algorithms available and the various SLAM solutions in terms of the mathematical models, complexity, the sensors used, and the type of applications. In this paper, a clear and simplified explanation is introduced about SLAM from a Geomatical viewpoint avoiding going into the complicated algorithmic details behind the presented techniques. In this way, a general overview of SLAM is presented showing the relationship between its different components and stages like the core part of the front-end and back-end and their relation to the SLAM paradigm. Furthermore, we explain the major mathematical techniques of filtering and pose graph optimization either using visual or LiDAR SLAM and introduce a summary of the deep learning efficient contribution to the SLAM problem. Finally, we address examples of some existing practical applications of SLAM in our reality.
... The camera relies heavily on lighting conditions and cannot work in dark scenes or scenes with insufficient texture information; however, the LiDAR has the characteristics of fast real-time ranging, not limited by the light environment, high ranging accuracy, and relatively constant error, and is more reliable working performance, which has been widely used in the construction of maps, navigation and positioning of robots, environmental modeling, and other aspects. 2,3 Although some problems have been solved, it is also a big challenge in the field of LiDAR SLAM, which has been attracted more attention in mobile robots. [4][5][6] It should be emphasized that the positioning method relying on a single sensor has disadvantages, which is greatly affected by the signal strength, and even fails in the case of the signal occlusion. ...
Article
Full-text available
To overcome the problem of the low accuracy and large accumulated errors of indoor mobile navigation and positioning, a method to integrate the light detection and ranging- and inertial measurement unit-based measurement is proposed. Firstly, the voxel-scale-invariant feature transform feature extraction algorithm for light detection and ranging is studied. Then, the errors of light detection and ranging measurement due to the change of the scan plane compensated based on aiding information from inertial measurement unit. The relative position parameters and the differences of measurements of light detection and ranging at adjacent times are used to estimate the error of inertial measurement unit sensors by a Kalman filter. Several experiments are carried out in the indoor corridor and the results demonstrate that precision of the light detection and ranging/inertial measurement unit-integrated indoor mobile robot localization is higher than that of single light detection and ranging sensor.
... In known environments [4,5], the localization part of SLAM can provide a six-degree-of-freedom sensor pose estimation, and thus plays an essential role in positioning and navigation applications such as navigation and planetary exploration missions, especially in environments where the global navigation satellite system (GNSS) is unavailable. Moreover, recent developments in SLAM have been driven by advances in sensor technology (e.g., RGB and depth (RGB-D) cameras and mobile Light Detection and Ranging (LiDAR) scanners) [6][7][8], as well as by the potential large market opportunities in emerging applications such as mobile augmented reality, self-driving vehicles, unmanned aerial vehicles, and home robots. SLAM has been extensively researched over the past 30 years by the robotics and computer vision community, and thus a comprehensive review on this topic is out of the scope of this paper; instead, we focus on the most relevant literature applicable to this research. ...
Article
Full-text available
In recent years, low-cost and lightweight RGB and depth (RGB-D) sensors, such as Microsoft Kinect, have made available rich image and depth data, making them very popular in the field of simultaneous localization and mapping (SLAM), which has been increasingly used in robotics, self-driving vehicles, and augmented reality. The RGB-D SLAM constructs 3D environmental models of natural landscapes while simultaneously estimating camera poses. However, in highly variable illumination and motion blur environments, long-distance tracking can result in large cumulative errors and scale shifts. To address this problem in actual applications, in this study, we propose a novel multithreaded RGB-D SLAM framework that incorporates a highly accurate prior terrestrial Light Detection and Ranging (LiDAR) point cloud, which can mitigate cumulative errors and improve the system’s robustness in large-scale and challenging scenarios. First, we employed deep learning to achieve system automatic initialization and motion recovery when tracking is lost. Next, we used terrestrial LiDAR point cloud to obtain prior data of the landscape, and then we applied the point-to-surface inductively coupled plasma (ICP) iterative algorithm to realize accurate camera pose control from the previously obtained LiDAR point cloud data, and finally expanded its control range in the local map construction. Furthermore, an innovative double window segment-based map optimization method is proposed to ensure consistency, better real-time performance, and high accuracy of map construction. The proposed method was tested for long-distance tracking and closed-loop in two different large indoor scenarios. The experimental results indicated that the standard deviation of the 3D map construction is 10 cm in a mapping distance of 100 m, compared with the LiDAR ground truth. Further, the relative cumulative error of the camera in closed-loop experiments is 0.09%, which is twice less than that of the typical SLAM algorithm (3.4%). Therefore, the proposed method was demonstrated to be more robust than the ORB-SLAM2 algorithm in complex indoor environments.
Article
Highly accurate 2D maps can supply basic geospatial information for efficient and accurate indoor building modeling. However, problematic scenarios, which are characterized by few features, similar components and large scales, seriously influence data association and cumulative error elimination, and thus degrade simultaneous localization and mapping (SLAM)-based mapping quality. In this paper, a cross-correction LiDAR SLAM method is proposed for constructing high-accuracy 2D maps of problematic scenarios. The method comprises two models. The first model, namely, pose correction for rough mapping (PCRM), increases the data association capacity and generates a rough map with cumulative errors. In the PCRM model, a rough mapping module is developed against the scenario with few features for accurate data association. This module improves the robustness of the data association by using the initial poses from the local pose correction module, especially in similar-component scenarios. The other is a map correction for pose optimization (MCPO) model, which enhances cumulative error elimination capacity. Here, a block-based local map correction module is proposed that takes both map and pose into consideration to construct accurate constraints. The constraints are then added to the global pose optimization module to significantly reduce the cumulative error of the rough map and thus construct a high-accuracy 2D map. The results demonstrate the superiority of our method over 5 other state-of-the-art methods in problematic scenarios. The overall performance of our method in these two scenarios is approximately 1 cm and 0.2% in terms of the absolute and relative map errors, respectively. Moreover, the modeling results demonstrate that our method can be applied to the efficient and accurate indoor modeling.
Article
To cross uncontrolled roadways, where no traffic-halting signal devices are present, pedestrians with visual impairments must rely on their other senses to detect oncoming vehicles and estimate the correct crossing interval in order to avoid potentially fatal collisions. To overcome the limitations of human auditory performance, which can be particularly impacted by weather or background noise, we develop an assisting tool called Acoussist, which relies on acoustic ranging to provide an additional layer of protection for pedestrian safety. The vision impaired can use the tool to double-confirm surrounding traffic conditions before they proceed through a non-signaled crosswalk. The Acoussist tool is composed of vehicle-mounted external speakers that emit acoustic chirps at a frequency range imperceptible by human ears, but detectable by smartphones operating the Acoussist app. This app would then communicate to the user when it is safe to cross the roadway. Several challenges exist when applying the acoustic ranging to traffic detection, including measuring multiple vehicles' instant velocities and directions with the presence many of them who emit homogeneous signals simultaneously. We address these challenges by leveraging insights from formal analysis on received signals' time-frequency (t-f) profiles. We implement a proof-of-concept of Acoussist using commercial off-the-shelf (COTS) portable speakers and smartphones. Extensive in-field experiments have been conducted to validate the effectiveness of Acoussist in improving mobility for people with visual impairments.
Conference Paper
Simultaneous localization and mapping for human body-mounted platforms have been recently the focus of navigation research to support wide range of applications such as rescue, first-responders, mining, and defense. For vehicular platforms, wheel odometry has been used to enhance the accuracy of SLAM. However, wheel odometry is not available in body-mounted platforms. Using raw inertial measurement unit (IMU) as odometry is not accurate enough to support SLAM due to the large and rapid drifts caused by IMU data integration. To address this challenge, we propose a sensor fusion scheme for body-mounted SLAM that integrates IMU-based Pedestrian Dead Reckoning (PDR) model with low-cost lightweight 2D LiDAR sensor. In the proposed fusion, PDR model is used as a replacement for wheel odometry in vehicular platforms. A system prototype consisting of a helmet-mount IMU from Xsens and RPLIDAR A1 2D LiDAR sensor has been developed and used for field data collection. The developed PDR model was integrated into Cartographer SLAM engine and compared with Hector SLAM. Our experiments demonstrated that the integration of PDR has enhanced the SLAM accuracy and contributed in bridging featureless portions of the environment leading to an overall average improvement of 48%.
Article
Full-text available
In this paper, a novel method of sensor based pedestrian dead reckoning is presented using sensors mounted on a shoe. Sensor based systems are a practical alternative to global navigation satellite systems when positioning accuracy is degraded such as in thick forest, urban areas with tall buildings and indoors. Using miniature, inexpensive sensors it is possible to create self-contained systems using sensor-only navigation techniques optimised for pedestrian motion. The systems developed extend existing foot based stride measurement technology by adding the capability to sense direction, making it possible to determine the path and displacement of the user. The proposed dead-reckoning navigation system applies an array of accelerometers and magneto-resistive sensors worn on the subject's shoe. Measurement of the foot's acceleration allows the precise identification of separate stride segments, thus providing improved stride length estimation. The system relies on identifying the stance phase to resolve the sensor attitude and determine the step heading. Field trials were carried out in forested conditions. Performance metrics include position, stride length estimation and heading with respect to a high accuracy reference trajectory.
Article
Full-text available
This paper introduces a "Personal Dead-reckoning" (PDR) navigation system for walking persons. The system is useful for monitoring the position of emergency responders inside buildings, where GPS is unavailable. The PDR system uses a six-axes Inertial Measurement Unit attached to the user's boot. The system's strength lies in the use of a technique known as "Zero Velocity Update" (ZUPT) that virtually eliminates the ill-effects of drift in the accelerometers. It works very well with different gaits, as well as on stairs, slopes, and generally on 3-dimensional terrain. This paper explains the PDR and presents extensive experimental results, which illustrate the utility and practicality of the system.
Conference Paper
Full-text available
In this paper we describe a new Bayesian estimation approach for simultaneous mapping and localization for pedestrians based on odometry with foot mounted inertial sensors. When somebody walks within a constrained area such as a building, then even noisy and drift-prone odometry measurements can give us information about features like turns, doors, and walls, which we can use to build a form of a map of the explored area, especially when these features are revisited over time. Our initial results for our novel scheme which we call “FootSLAM” are very surprising in that true SLAM with stable relative positioning accuracy of 1-2 meters for pedestrians is indeed possible based on inertial sensors alone without any prior known building indoor layout. Furthermore, the 2D maps obtained even for just 10 minutes of walking converge to a good approximation of the true layout forming the basis for future automated collaborative mapping of buildings.
Conference Paper
Full-text available
It might be assumed that dead reckoning approaches to pedestrian navigation could address the needs of first responders, including fire fighters. However, conventional PDR approaches with body-mounted motion sensors can fail when used with the irregular walking patterns typical of urban search and rescue missions. In this paper, a technique using shoe-mounted sensors and inertial mechanization equations to directly calculate the displacement of the feet between footfalls is described. Zero-velocity updates (ZUPTs) at foot standstills limit the drift that would otherwise occur with inexpensive IMUs. We show that the technique is fairly accurate in terms of distance travelled and can handle many arbitrary manoevers, such as tight turns, side/back stepping and stair climbing.
Conference Paper
Full-text available
Human localization is a very valuable information for smart environments. state-of-the-art local positioning systems (LPS) require a complex sensor-network infrastructure to locate with enough accuracy and coverage. Alternatively, inertial measuring units (IMU) can be used to estimate the movement of a person, by detecting steps, estimating stride lengths and the directions of motion; a methodology that is called pedestrian dead-reckoning (PDR). In this paper, we use low-performance microelectromechanical (MEMS) inertial sensors attached to the foot of a person. This sensor has triaxial orthogonal accelerometers, gyroscopes and magnetometers. We describe, implement and compare several of the most relevant algorithms for step detection, stride length, heading and position estimation. The challenge using MEMS is to provide location estimations with enough accuracy and a limited drift. Several tests were conducted outdoors and indoors, and we found that the stride length estimation errors were about 1%. The positioning errors were almost always below 5% of the total travelled distance. The main source of positioning errors are the absolute orientation estimation.
Conference Paper
In this paper we present a novel approach to legged humanoid navigation on indoor environments using classical probabilistic SLAM methods based on odometry information and laser measurements. We use two small lasers installed in the robot feet to capture distance data. Odometry is obtained by calculating the position of each laser-foot at every time step. The SLAM problem is solved by using a multi-laser SLAM solution together with a holonomic motion model. Navigation skills also include a path planning module with obstacle avoidance for autonomous navigation in indoor environments. The whole process is performed within the robot itself. Optionally, localization robustness is increased by adding the detection of landmarks using a camera. Results obtained are presented for the 1.5 m tall Reem-B humanoid robot.
Conference Paper
In the event of an emergency, due to a fire or other crisis, a necessary but time consuming pre-requisite, that could delay the real rescue operation, is to establish whether the ground can be entered safely by human emergency workers. The objective of the VIEW-FINDER project is to develop robots which have the primary task of gathering data. The robots are equipped with sensors that detect the presence of chemicals and, in parallel, image data is collected and forwarded to an advanced base station. One of the problems tackled in this project is the robot navigation. The used robot for the outdoor scenario is equipped with a set of sensors: camera, GPS, inertial navigation system (INS), wheel encoders, and ultrasounds sensors. The robot uses a Simultaneous Localization and Mapping (SLAM) approach to combine data from different sensors for an accurate positioning. The paper gives an overview on the proposed algorithm.
Conference Paper
In this paper we present an extension to odometry based SLAM for pedestrians that incorporates human-reported measurements of recognizable features, or “places” in an environment. The method which we have called “PlaceSLAM” builds on the Simultaneous Localization and Mapping (SLAM) principle in that a spatial representation of such places can be built up during the localization process. We see an important application to be in mapping of new areas by volunteering pedestrians themselves, in particular to improve the accuracy of “FootSLAM” which is based on human step estimation (odometry). We present a description of various flavors of PlaceSLAM and derive a Bayesian formulation and particle filtering implementation for the most general variant. In particular we distinguish between two important cases which depend on whether the pedestrian is required to report a place's identifier or not. Our results based on experimental data show that our approach can significantly improve the accuracy and stability of FootSLAM and this with very little additional complexity. After mapping has been performed, users of such improved FootSLAM maps need not report places themselves.
Conference Paper
In this paper we present a PDR (Pedestrian Dead Reckoning) system with a phone location awareness algorithm. PDR is a device which provides position information of the pedestrian. In general, the step length is estimated using a linear combination of the walking frequency and the acceleration variance for the mobile phone. It means that the step length estimation accuracy is affected by coefficients of the walking frequency and the acceleration variance which are called step length estimation parameters. Developed PDR is assumed that it is embedded in the mobile phone. Thus, parameters can be different from each phone location such as hand with swing motion, hand without any motion and pants pocket. It means that different parameters can degrade the accuracy of the step length estimation. Step length estimation result can be improved when appropriate parameters which are determined by phone location awareness algorithm are used. In this paper, the phone location awareness algorithm for PDR is proposed.