Professional Documents
Culture Documents
10icra Joydeep PDF
10icra Joydeep PDF
10icra Joydeep PDF
this is done, a path is planned to traverse every edge of Fig. 2 shows the mean signal strength of one access point
the skeleton graph. The robot then follows this path, and (a1 ) for different vertices vi = hli i, li = (xi , yi ) across the
along each edge, it stops at regularly inter-spaced sample map. Fig. 3 shows a sampling of the values of P (S|l) for
locations of a user-defined maximum spacing, and defines a S = [−100, −59] from Access Points 1 and 2, for various
new vertex. Thus, each edge of the skeleton graph is split up locations l across the map.
into multiple smaller edges. At each sample location (which With this estimate of the perceptual model P (S|l) and the
is also a vertex in the newly generated complete graph), motion model of the robot P (lt |lt−1 , ut−1 ), the localization
the robot collects WiFi signal strength readings for a pre- belief of the robot can be recursively updated.
determined (and user-defined) duration. During this process,
the robot uses odometry alone for estimating its localization,
and errors in localization are corrected manually using a
graphical user interface. Thus, the skeleton graph is used
to generate the complete graph, and the matrices M and D
are populated.
Mi1
III. W I F I L OCALIZATION (dBm)
Our WiFi Localization algorithm uses Monte Carlo Lo- −40
calization [10] with Bayesian filtering to maintain a set of −70
100
hypotheses of the robot location in real time. As part of −100 90
80
the Bayesian filter, we need a perceptual model which can 70
40
be used to calculate the probability of making a particular 30
60
50
signal strength measurement S at a location l, P (S|l). 20 40
30
xi (m)
10
A. Estimating the WiFi Signal Strength Map, And The Per- yi (m) 20
0 10
ceptual Model 0
0.7 100
0.47 90
0.23 80
0 70
60
50
30
40 Fig. 4. Properties of a particle pi : Associated edge epi (epi = hva , vb , Γi),
20
x (m)
30
Projected location di , Offset oi , Cartesian location (xi , yi ), Orientation θi
y (m) 10
20
0
10 the positions and orientations of the particles. The Update
0
step is executed every 500ms when new WiFi signal strength
Fig. 3. Probability P (S|l) of making a Signal Strength observation S =
data is available, and updates the weights of the particles.
[−100, −59], for different locations l = (x, y) on the map The Constrain step is executed whenever the Predict step is
B. Monte Carlo Localization executed, and updates the weights and edge associations of
the particles based on map data and constraints. After every
The recursive Bayesian update for a location belief B(lt )
Update iteration, the particles are re-sampled in the Resample
for location l at time step t using sensor readings St and
step.
odometry readings ut−1 [11] is given by:
Z 1) Predict: Given new odometry data of the motion of
B(lt ) = ηP (St |lt ) P (lt |lt−1 , ut−1 )B(lt−1 )dlt−1 (4) the robot, for each particle pi , its properties θi ,xi ,yi ,di ,oi
are updated using the motion model of the robot, where the
Here, η is a normalization constant, St the WiFi signal robot’s linear and angular motions are modeled as having
strength observation at time step t, and ut−1 the odometry Gaussian error.
data between time steps t−1 and t. The term P (lt |lt−1 , ut−1 ) 2) Update: To update the weights of the particles based
is obtained from the motion model of the robot. The term on the latest WiFi signal strength observation, the estimated
P (St |lt ) is given by the perceptual model, eq. 3. observation model as described in Section III-A is used. For
1) Representation of the Location Hypotheses: The mul- each particle pi , the observation probability for that location
tiple hypotheses of the robot location are sampled and repre- P (S|l) is calculated using the location of that location in
sented by particles pi = hepi , di , oi , xi , yi , θi , wi , wci i, pi ∈ equation 3.
P. The number of particles is |P|. Each particle pi has the 3) Constrain: Following the Update step, the edge associ-
following properties: ation of each particle is re-evaluated, and the map constrained
• epi , the edge that the particle is associated with. weights computed as outlined by the pseudocode in Algo-
• di , the projected location of the particle on the edge. rithm 1. Here, the thresh term is a threshold term, which is
• oi , the offset of the location of the particle from the set to 0.1. Prior to renormalizing the particle weights, the sum
edge. of the updated and constrained weights, wcsum is computed,
• xi , yi the Cartesian location of the particle on the map which is required for the next step, Resample.
with respect to a global reference frame. 4) Resample: After every Update step, the particles need
• θi , the orientation of the particle with respect to the to be resampled. The number of particles to be resampled
global reference frame. N is calculated based on Sensor Resetting Localization
• wi , the normalized weight assigned to the particle. [12], subject to a maximum Nmax and minimum Nmin . In
• wci , the map constrained weight assigned to the parti- addition to this, particles with weight less than a threshold
cle. This weight is calculated in the Constrain step of wcmin are also resampled. Algorithm 2 implements this. All
the Run-Time phase, as described in Section III-C.3. resampled particles are re-initialized to location xi , yi with
The properties of the particle are graphically illustrated in probability P (S|(xi , yi )), based on the latest WiFi signal
Fig. 4. strength observation.
Uncertainty σ (m)
Individual Trials
19: wcsum ← wcsum + wci 101
20: end for
i=|P|
X
21: Renormalize weights wci such that wci = 1
i=1
100
Algorithm 2 ‘Resample’ Algorithm
0 2 4 6 8
1: N ←0
Time (s)
2: Sort Particles pi in increasing order of wci
3: for i = 1 to |P| do
Fig. 5. Uncertainty σ vs. t: Convergence of Location Estimates. Black,
4: Let pi = hepi , di , oi , xi , yi , θi , wi , wci i solid: Mean of all trials. Grey, dotted: Individual trials.
5: if wci < wcmin then
6: N ←N +1 2) Accuracy: In this test, the robot was again stopped at
7: end if 20 randomly chosen locations on the map, and the particles
8: end for initialized with equal weights randomly distributed across
9: N ← N + |P| ∗ min( N|P| max
, max( N|P| min
, wcsum /κ)) the map. The location estimates were allowed to converge
10: for i = 1 to N do till the uncertainty of localization σ stopped decreasing. The
11: Draw (xi , yi ) with probability P (S|xi , yi ) reported location estimates were then compared to the ground
12: wci ← P (S|xi , yi ) truth. Table I sums up the results of this test.
13: wi ← P (S|xi , yi ) Value Mean Minimum Maximum
14: oi ← 0 Localization Error 0.7m 0.2m 1.6m
15: Find edge e best associated with pi Localization Uncertainty 0.6m 0.2m 0.9m
Convergence Time 11s 7.4s 16.5s
16: calculate di for particle pi on edge e
17: end for TABLE I
ACCURACY OF W I F I L OCALIZATION
3) “Incidental” Observations: While the robot is per-
forming various tasks, it is reasonable to expect that it would
“uncertainty” (σ) of the location hypothesis as the weighted drop WiFi Signal Strength readings from some access points.
standard deviation of all the particles in the cluster with We wish to investigate the impact of dropped signals on
the maximum weight. The “confidence” (c) of the location the localization accuracy in this experiment. To do so, the
estimate is estimated as the weight of the cluster with the robot is stopped at a fixed location on the map, and is
maximum weight. allowed to collect at least one signal strength measurement
from all WiFi access points in range. Next we selectively
E. Orientation Estimation and deliberately drop signal strength measurements from
The orientation of the robot could be estimated using the all permutations of the access points to simulate dropped
particle filter too, but this would increase the dimension of signals. Fig. 6 shows the results of this test. A total of 15
the hypothesis space, and the number of particles would access points were accessible from the location, and even
correspondingly increase. Instead, we use a mixture of dead with 9 dropped signals (6 visible access points), the mean
reckoning with global wall orientation data, similar to the error in localization is less than 2m.
8 Algorithm 3 “Next Maximal Action” Navigation Algorithm
procedure NAVIGATE(V ,E,vd )
Localization Error (m)
1:
6 2: Dist,π,vcurrent ,x,y,Path,pathProgress ← null
3: , σ, T ← 0
4 4: actionF ail ← false
5: C OMPUTE P OLICY(V ,E,vd ,Dist,π)
2 6: G ET L OCATION(x,y,σ)
7: C OMPUTE PATH(Path,π,x,y)
0 8: PATH P ROJECT(Path,x,y,vcurrent ,)
2 4 6 8 10 12 14 16
Number of visible Access Points 9: pathProgress ← Path(0)
10: while vcurrent 6= vd do
Fig. 6. Localization Error vs. Number of visible Access Points 11: a ← next action primitive from π(vcurrent )
IV. NAVIGATION 12: Compute Va for action primitive a
13: E XECUTE(a,actionF ail)
We desire the navigation algorithm of the robot to be
14: G ET L OCATION(x,y,σ)
capable of producing smooth and uninterrupted robot motion.
15: PATH P ROJECT(pathProgress,x,y,vcurrent ,)
In order to do so, we simplify the robot motion such that it
16: if > max or actionF ail = true then
can be split up into the following action primitives:
17: T ←0
1) M OVE D OWN C ORRIDOR(d, vs , vf ) : Moves in a 18: while σ > σmax and T < Tmax do
straight line between start vertex vs and end vertex 19: Halt Thalt seconds
vf , traveling a maximum distance d. 20: T = T + Thalt
2) I NTEGRATED T URN(direction) : Takes the next avail- 21: G ET L OCATION(x,y,σ)
able turn (as sensed by the LIDAR) in the desired 22: end while
direction. 23: C OMPUTE PATH(Path,π,x,y)
3) I N P LACE T URN(φ) : Turns the robot by φ radians in 24: PATH P ROJECT(Path,x,y,vcurrent ,)
place. 25: pathProgress ← Path(0)
Algorithm 3 outlines the “Next Maximal Action” Naviga- 26: else
tion Algorithm which generates these action primitives from 27: pathProgress ← pathProgress + Va
the robot’s current location, given a destination location. It 28: end if
internally calls the following subroutines: 29: end while
• GetLocation(x,y,σ) : Returns current location (x,y) of 30: end procedure
the robot, and the uncertainty of localization, σ.
• ComputePolicy(V ,E,vd ,Dist,π) : Computes global pol-
icy π for destination vd
In order to test the localization and navigation system, we
• ComputePath(Path,Policy,x,y) : Accepts the current lo-
ran a series of experiments, each of which were conducted
cation (x,y) of the robot, and generates a path (Path)
as follows. At the start of each experiment, a list of 12
to follow in order to reach destination vd based on the
random locations over the map were generated such that no
given policy (Policy)
two successive locations were from the same corridor, and
• PathProject(Path,x,y,vcurrent ,) : Accepts the current
the robot had to autonomously navigate to these locations
location (x,y) of the robot, and returns the vertex
sequentially.
vcurrent (vcurrent ∈Path) that is closest to the robot’s
location. Also returns the error of projection as B. Results
• Execute(a,actionF ail) : Executes action primitive a
and sets failure indicator actionF ail when execution Each experiment lasted for an average of 28 minutes, and
of a fails. covered an average distance of 818 meters. In total, this
experiment was repeated 8 times, adding up to over 3.5
V. E XPERIMENTAL R ESULTS hours of robot navigation time, and a total path length of
Our WiFi localization and navigation algorithms were over 6.5 km. Out of these runs, for 2 of these runs, while
tested using CoBot, deployed on the third floor of Wean the robot navigated between these locations, its true location
Hall at Carnegie Mellon University. was manually recorded periodically to use as ground truth
for comparison with the localization estimates.
A. Parameters Of The Tests and Test Methodology Fig. 7 shows a trace of the path followed by the robot
The graph representation of the map built during the during one of the experiments. This trace was reconstructed
Learning Phase had 223 vertices and 222 edges. There using the localization estimate of the WiFi localization algo-
were a total of 106 unique WiFi access points. The particle rithm. Fig. 8 shows the evolution of the robot’s uncertainty
filter used 500 particles that were initialized with random σ over the first ten minutes of this experiment. During all
orientations and locations, and equal weights. the 8 experiments, the robot encountered a total of 33 action
1
40
Cumulative Fraction
0.8
30
0.6
y (m)
20
0.4
10
0.2
0
0
0 10 20 30 40 50 60 70 80 90 100 0 0.5 1 1.5 2 2.5 3 3.5 4
x (m) Localization error (m)
Fig. 7. Trace of the path traversed by the robot Fig. 9. Cumulative histogram of localization error
4
Uncertainty σ
Uncertainty σ (m)
Fraction
2
0.05
1
0 0
0 100 200 300 400 500 600 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (s) Robot speed (m/s)
15
Fig. 8. σ vs. t: Uncertainty of localization with time Fig. 10. Histogram of the speed of the robot during the experiments
failures, but autonomously recovered from all of them. Fig. 9 R EFERENCES
shows a cumulative histogram of the location accuracy of
[1] A. Elfes. Sonar-based real-world mapping and navigation. IEEE
the robot for the 2 experiments with the manually annotated Journal of robotics and automation, 3(3):249–265, 1987.
ground truth data. The robot’s localization error is a mean [2] J.S. Gutmann and C. Schlegel. AMOS: Comparison of Scan Matching
of 1.2m, and the error is less than 1.8m for greater than Approaches for Self-Localization in Indoor Environments. In Proceed-
ings of EUROBOT, volume 96, page 61, 1996.
80% of the time. It is worth noting that while the robot is in [3] R. Sim and G. Dudek. Learning and evaluating visual features for pose
motion, the localization has more errors compared to when estimation. In Proceedings of the Seventh International Conference on
the robot was stopped (Section III-F). This can be attributed Computer Vision (ICCV99), 1999.
[4] O. Serrano, J.M. Canas, V. Matellan, and L. Rodero. Robot localization
to odometry errors, latency in signal strength readings, and using WiFi signal without intensity map. WAF04, March, 2004.
unobserved signal strengths. Fig. 10 shows a histogram of [5] M. Ocana, LM Bergasa, MA Sotelo, J. Nuevo, and R. Flores. Indoor
the robot’s speed during the experiment. The largest peak Robot Localization System Using WiFi Signal Measure and Mini-
mizing Calibration Effort. In Proceedings of the IEEE International
around 0.57m/s corresponds to the straight line speed (the Symposium on Industrial Electronics, pages 1545–1550, 2005.
variations being due to obstacle avoidance), and the smaller [6] P. Bahl and V. Padmanabhan. RADAR: An in-building RF-based user
peak around 0.3m/s is the turning speed of the robot. location and tracking system. In IEEE infocom, pages 775–784, 2000.
[7] Y.C. Chen, J.R. Chiang, H. Chu, P. Huang, and A.W. Tsui. Sensor-
VI. C ONCLUSION assisted wi-fi indoor location system for adapting to environmental
dynamics. In Proceedings of the 8th ACM international symposium
In this paper, we introduced an algorithm using WiFi on Modeling, analysis and simulation of wireless and mobile systems,
signal strength measurements for the localization of an pages 118–125, 2005.
[8] B. Ferris, D. Haehnel, and D. Fox. Gaussian processes for signal
indoor mobile robot on a map as represented by a graph. The strength-based location estimation. In Proceedings of Robotics:
data collected during the Learning Phase of the algorithm Science and Systems, August 2006.
was used to generate a perceptual model for the robot’s [9] S. Zickler and M. Veloso. RSS-Based Relative Localization and
Tethering for Moving Robots in Unknown Environments. In IEEE
location hypotheses, which along with odometry data and International Conference on Robotics and Automation, 2010.
map constraints constituted our localization algorithm. We [10] F. Dellaert, D. Fox, W. Burgard, and S. Thrun. Monte carlo localization
introduced our “Next Maximal Action” navigation algorithm, for mobile robots. In IEEE International Conference on Robotics and
Automation, pages 1322–1328, 1999.
and demonstrated the simultaneous functioning of the lo- [11] D. Fox, S. Thrun, W. Burgard, and F. Dellaert. Particle filters
calization and the navigation algorithms through extensive for mobile robot localization. Sequential Monte Carlo Methods in
testing: a total traversed distance of over 6.5km and a total Practice, pages 499–516, 2001.
[12] S. Lenser and M. Veloso. Sensor resetting localization for poorly mod-
robot navigation time in excess of 3.5 hours. elled mobile robots. In IEEE International Conference on Robotics
and Automation, pages 1225–1232, 2000.
ACKNOWLEDGMENTS [13] P.S. Bradley and U.M. Fayyad. Refining initial points for k-means
This work was partially supported by the Computational clustering. In Proceedings of the 15th International Conference on
Machine Learning, pages 91–99, 1998.
Thinking Center at Carnegie Mellon, and by the Lockheed [14] S. Thrun, A. Bucken, W. Burgard, D. Fox, T. Frohlinghaus, D. Hennig,
Martin, Inc. under subcontract 8100001629/1041062, and by T. Hofmann, M. Krell, and T. Schimdt. Map learning and high-
Intelligent Automation, Inc, under subcontract 654-1. The speed navigation in RHINO. AI-based Mobile Robots: Case studies
of successful robot systems, 1998.
views and conclusions contained in this document are those
of the authors only.