range-only slam with a mobile robot and a …zanella/paper/cr_2009/icra09_0701_fi...range-only slam...

7
Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella, Stefano Zilli, Francesco Zorzi, Enrico Pagello Abstract— This paper presents the localization of a mobile robot while simultaneously mapping the position of the nodes of a Wireless Sensor Network using only range measurements. The robot can estimate the distance to nearby nodes of the Wireless Sensor Network by measuring the Received Signal Strength Indicator (RSSI) of the received radio messages. The RSSI measure is very noisy, especially in an indoor environment due to interference and reflections of the radio signals. We adopted an Extended Kalman Filter SLAM algorithm to integrate RSSI measurements from the different nodes over time, while the robot moves in the environment. A simple pre-processing filter helps in reducing the RSSI variations due to interference and reflections. Successful experiments are reported in which an average localization error less than 1 m is obtained when the SLAM algorithm has no a priori knowledge on the wireless node positions, while a localization error less than 0.5 m can be achieved when the position of the node is initialized close to the their actual position. These results are obtained using a generic path loss model for the trasmission channel. Moreover, no internode communication is necessary in the WSN. This can save energy and enables to apply the proposed system also to fully disconnected networks I. INTRODUCTION This work has been carried out within the RAMSES2 project (integRation of Autonomous Mobile robots and wire- less SEnsor networks for Surveillance and reScue) . The RASMES2 project aims at integration of Wireless Sensor Networks (WSN) with autonomous mobile robots, leveraging on the complementarities of WSN and robots. The project aims at developing strategies to enhance the performance of both systems and enable novel functionalities, in particular in the context of surveillance and rescue. In this work, results on the Simultaneous Localization of the robot and of the Mapping of the wireless nodes in the environment (SLAM problem) are reported. The SLAM problem is frequent in scenarios in which an a priori map of the environment is not provided or cannot be conveniently created. Thus, if the environment model is unknown or it is subject to unexpected variations, as in case of fire or crashes, it is necessary to apply map–building techniques which require suitable sensors. A. Related Works In the last years, several works have tackled the problem of localization of the wireless nodes in a WSN, proposing This work was partially supported by the University of Padua under the project ”RAMSES2” and partially supported by Fondazione Cassa di Risparmio Padova e Rovigo under the project ”A large scale wireless sensor network for pervasive city-wide ambient intelligence”. All authors are with University of Padova, Dep. of Information Engi- neering (DEI), via Gradenigo 6/B, 35131 Padova, Italy phone: +39 049 827 7840 fax: +39 049 827 7826 mailto: [email protected] the use of one or more mobile nodes mounted on board of robots. Several authors make use of sensors that can measure both the distance and the angle ( e.g., laser [7] or sonar [21]) of the robot with respect to reference objects. Other solutions, based on less expensive sensors, make use of the angle measure only [14]. Active Bat [19] is one of the first localization systems based only on range estimations, which are obtained by measuring the time of flight of acoustic (ultrasound) impulses emitted by the mobile unit to reach some ceiling- mounted receivers. When the receivers are 1.2 m apart, the system is proved to have localization errors of less than 14 cm in more than 95% of the cases. A more advanced system, called Cricket [18], uses a combination of RF and ultrasound technologies to provide location information to attached host devices. Basically, the system consists of a number of pre- installed beacon nodes that periodically broadcast a radio signal and a ultrasonic pulse. Comparing the time interval between the reception of the radio signal (which propagates at speed of light) and the acoustic pulse (which propagates at the speed of sound), the receiver is capable of performing a fairly accurate ranging. Cricket offers localization errors less than a few centimeters with beacons placed from 1 up to 1, 5 meters apart. In several works, ranging is obtained from the Received Signal Strength Indicator (RSSI), through the inversion of the path-loss law governing indoor radio propagation. This approach offers the advantage of being readily employable in any radio device, since the RSSI is typically provided by any radio transceiver. On the other hand, the range estimate is usually very imprecise due to the stochastic nature of the radio propagation. One of the first systems based on pure-RSSI ranging is RADAR [1], in which a pre-built RSSI map of the environment is used to determine the most likely position of the strayed node according to the power received from beacons. Radar achieves an accuracy of 2 meters in 50% of the cases. However, it requires a notable effort for building the RSSI map. Furthermore, the system is incapable of adapting to variations of the propagation environment due, for instance, to moving elements (furniture, desks, chairs, doors, etc...). In [15], Sichiuti e Ramadurai adopt a Bayesian approach to localize PDAs equipped with IEEE 802.11 communication as static target nodes, while the mobile element was a remotely controlled truck carrying a PDAs and a GPS sensor. The localization error obtained through this approach is generally below 3 meters. The localization problem becomes more complex when the position of the landmark is also unknown, in which

Upload: others

Post on 24-Jun-2020

2 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

Range-only SLAM with a Mobile Robot anda Wireless Sensor Networks

Emanuele Menegatti, Andrea Zanella, Stefano Zilli, Francesco Zorzi, Enrico Pagello

Abstract— This paper presents the localization of a mobilerobot while simultaneously mapping the position of the nodes ofa Wireless Sensor Network using only range measurements. Therobot can estimate the distance to nearby nodes of the WirelessSensor Network by measuring the Received Signal StrengthIndicator (RSSI) of the received radio messages. The RSSImeasure is very noisy, especially in an indoor environment dueto interference and reflections of the radio signals. We adoptedan Extended Kalman Filter SLAM algorithm to integrate RSSImeasurements from the different nodes over time, while therobot moves in the environment. A simple pre-processing filterhelps in reducing the RSSI variations due to interference andreflections. Successful experiments are reported in which anaverage localization error less than 1 m is obtained when theSLAM algorithm has no a priori knowledge on the wirelessnode positions, while a localization error less than 0.5 m canbe achieved when the position of the node is initialized closeto the their actual position. These results are obtained using ageneric path loss model for the trasmission channel. Moreover,no internode communication is necessary in the WSN. This cansave energy and enables to apply the proposed system also tofully disconnected networks

I. INTRODUCTIONThis work has been carried out within the RAMSES2

project (integRation of Autonomous Mobile robots and wire-less SEnsor networks for Surveillance and reScue) . TheRASMES2 project aims at integration of Wireless SensorNetworks (WSN) with autonomous mobile robots, leveragingon the complementarities of WSN and robots. The projectaims at developing strategies to enhance the performance ofboth systems and enable novel functionalities, in particularin the context of surveillance and rescue.

In this work, results on the Simultaneous Localizationof the robot and of the Mapping of the wireless nodes inthe environment (SLAM problem) are reported. The SLAMproblem is frequent in scenarios in which an a priori map ofthe environment is not provided or cannot be convenientlycreated. Thus, if the environment model is unknown or itis subject to unexpected variations, as in case of fire orcrashes, it is necessary to apply map–building techniqueswhich require suitable sensors.

A. Related Works

In the last years, several works have tackled the problemof localization of the wireless nodes in a WSN, proposing

This work was partially supported by the University of Padua underthe project ”RAMSES2” and partially supported by Fondazione Cassa diRisparmio Padova e Rovigo under the project ”A large scale wireless sensornetwork for pervasive city-wide ambient intelligence”.

All authors are with University of Padova, Dep. of Information Engi-neering (DEI), via Gradenigo 6/B, 35131 Padova, Italy phone: +39 049 8277840 fax: +39 049 827 7826 mailto: [email protected]

the use of one or more mobile nodes mounted on board ofrobots.

Several authors make use of sensors that can measure boththe distance and the angle ( e.g., laser [7] or sonar [21]) of therobot with respect to reference objects. Other solutions, basedon less expensive sensors, make use of the angle measureonly [14]. Active Bat [19] is one of the first localizationsystems based only on range estimations, which are obtainedby measuring the time of flight of acoustic (ultrasound)impulses emitted by the mobile unit to reach some ceiling-mounted receivers. When the receivers are 1.2 m apart, thesystem is proved to have localization errors of less than 14 cmin more than 95% of the cases. A more advanced system,called Cricket [18], uses a combination of RF and ultrasoundtechnologies to provide location information to attached hostdevices. Basically, the system consists of a number of pre-installed beacon nodes that periodically broadcast a radiosignal and a ultrasonic pulse. Comparing the time intervalbetween the reception of the radio signal (which propagatesat speed of light) and the acoustic pulse (which propagatesat the speed of sound), the receiver is capable of performinga fairly accurate ranging. Cricket offers localization errorsless than a few centimeters with beacons placed from 1 upto 1,5 meters apart.

In several works, ranging is obtained from the ReceivedSignal Strength Indicator (RSSI), through the inversion ofthe path-loss law governing indoor radio propagation. Thisapproach offers the advantage of being readily employablein any radio device, since the RSSI is typically provided byany radio transceiver. On the other hand, the range estimateis usually very imprecise due to the stochastic nature ofthe radio propagation. One of the first systems based onpure-RSSI ranging is RADAR [1], in which a pre-builtRSSI map of the environment is used to determine the mostlikely position of the strayed node according to the powerreceived from beacons. Radar achieves an accuracy of 2meters in 50% of the cases. However, it requires a notableeffort for building the RSSI map. Furthermore, the systemis incapable of adapting to variations of the propagationenvironment due, for instance, to moving elements (furniture,desks, chairs, doors, etc...). In [15], Sichiuti e Ramaduraiadopt a Bayesian approach to localize PDAs equipped withIEEE 802.11 communication as static target nodes, whilethe mobile element was a remotely controlled truck carryinga PDAs and a GPS sensor. The localization error obtainedthrough this approach is generally below 3 meters.

The localization problem becomes more complex whenthe position of the landmark is also unknown, in which

Page 2: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

Fig. 1. The robot and its equipment.

case we better speak of SLAM (Simultaneous Localizationand Mapping). Kurth, et al [10] have developed a SLAMsystem based on the time of flight of only radio signals. Thelocalization error, however, is nearly one meter. Pathiranaet al. estimated the position of the nodes of a Delay-tolerantsensor network measuring the received signal strength (RSSI)of the radio messages received by a mobile node mountedon a Lego Mindstorm robot [11]. Here, a Robuts ExtendedKalman Filter (REKF)-based state estimator solves the local-ization given the RSSI measurements. Thier system requiresa careful tuning of the parameters of the communicationchannel and of the weights associated to the measurements.Shenoy and Tan present a system in which a mobile robotcan be navigated to the event location by the un-localizedsensor nodes, which in turn are finely localized by the mobilerobot [13]. The localization is achieved by building boundingboxes representing the communication range of the mobilenode. Accumulating this information, while the robot moves,shrinks the bounding box and then increase the accuracy ofthe localization of the nodes. Unfortunately, this system wasimplemented in simulation only.

In this paper, we report experiments in the field a SLAMalgorithm for localizing the sensors in the WSN and si-multaneously tracking the motion of the mobile robot. Thealgorithm makes use of two types of inputs: the RSSI thatthe robot collects from the nodes in the WSN and the robotodometry. Our experiments have been performed indoor,which is recognized as a much more hostile environmentfor self–localization schemes [15], [12].

Our approach is very similar to the one presented bySichitiu et Ramadurai in [15]. They used PDAs equippedwith IEEE 802.11 communication as static target nodes,while the mobile element was a remotely controlled truckcarrying a PDAs and a GPS sensor. The position estimationis based on the different measurements of the RSSI receivedby the static nodes while the transmitting mobile node ismoving. Each node applies Bayesian inference to refine itsposition estimation at each received message.

II. THE INFRASTRUCTURE

The testbed used for the experiments consists of an au-tonomous mobile robot and of a WSN composed of a dozenof wireless sensor nodes.

A. The Wireless Sensor Network

The Wireless Sensor Network is composed of EyesIFXsensor nodes, produced by Infineon Technologies The Eye-sIFX can be programmed and powered via USB for easyinterconnection with other digital devices [3], [4]. Eachwireless node is equipped with a radio interface that provides19.2 kbps transmission rate by using an FSK modulation inthe 868.3 MHz band. The platform is fitted with light andtemperature sensors. Furthermore, an integrated ReceivedSignal Strength circuit can be used to estimate distance.

B. The robot

The robot is a wheeled custom designed robot based onthe Pioneer 2 by MobileRobots Inc, see Fig. 1. The robotis equipped with a standard ATX motherboard with an Intel1,6 GHz Intel Pentium 4, a 256 MB RAM and a 160 GBhard disk, running Linux OS. The only on–board sensors are:an omnidirectional camera (composed of a standard CCDcamera and a convex omnidirectional mirror) and the odome-ters connected to the two driven wheels. Communication areprovided by a PCMCIA wireless ethernet card toward thelaboratory Intranet and by an EyesIFX node connected toone of the robot’s USB ports toward the WSN. The EyesIFXsensor is mounted on top of the omnidirectional camera.

Mounting an EyesIFX node on the robot enables tointegrate the robot in WSN. Thus, among the rest, the robotitself can act as a gateway for the WSN services. To seamlessuse the special command set of the EyesIFX mounted onthe robot from the robot control software, we exploited therobotics architecture MIRO [9], [17].

Miro is a distributed object oriented framework for mobilerobot control, based on CORBA1 technology. The Miro corecomponents have been developed in C++ for Linux, butdue to the programming language independency of CORBAfurther components can be written in any language andon any platform that provides CORBA implementations.The Miro core components have been developed under theumbrella of ACE2, an object oriented multi-platform frame-work for OS-independent interprocess, network and real timecommunication. They use TAO3 as their ORB4. TAO is aCORBA implementation designed for high performance andreal time applications.

III. MIRO EYESIFX SERVICE

Miro supports several robot sensors such as: lasers, kick-ers, sonars and several camera types. Miro also supportsthe most used wheeled robot platforms like: Pioneer bases,Sparrow bases and B21 bases. Unfortunately, there are no

1Common Object Request Broker Architecture2Adaptive Communications Environment3The ACE ORB4Object Request Broker

Page 3: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

MIRO modules to support communication with a WSN.Thus, we wrote a new Miro service to allow bidirectionalcommunications between the WSN and the robot (or even-tually, the robots). This was done by extending the ServerMiro class. The new Miro service is called Eyes Service.We created an Eyes Message object as an unique structureto send or receive data between the WSN and the robot. Themessage is an ordered buffer filled with: the command to besend (or received), the message options and the data.

Conversely, the communication process is managed bytwo different classes: the EyesConnection class andthe EyesEvent class. The first class takes care of thecommunication from the robot to sensor (and so, toward theWSN). The second class takes care of the communicationfrom the sensor to the robot (and so, from the WSN). Thisclass is an object wrapper for the event handler registeredwith the ACE ReactorTask.

To use the services of this structure, we exploit TAOservices, in particular the Name Service. When the NameServer is running, one can share services easily adding themto a server or one can ask for a service with a request tothe Name Server. We use this service with the multicastsupport which hides also the Name Server address. TAOimplements several ways to share data, we decided to usethe producer/consumer paradigm. TAO translate this in asupplier/consumer structure wich needed a Notify Channelto share data. Therefore, every time new data are availablefrom the WSN, these are pushed on the robot and are readyto be read by the robot software.

IV. ESTIMATION OF THE ROBOT-NODE DISTANCE

As we said above, the distance between the robot and eachnode of the WSN is estimated measuring the RSSI of the RFsignal. Since the RSSI can be calculated directly by mostradio transceiver, it allows for a (rough) range estimationwithout the need of specialized hardware, with an advantagein terms of device’s cost and size. Another advantage ofRSSI-based ranging is that it does not require the node tobe in line of sight with the robot, since the RF signal passesthrough obstacles as persons, furniture or even walls. Onthe other hand, the range estimate based on RSSI measuresis rather unreliable and subject to random fluctuations dueto a number of environmental factors, such as temperature,humidity, daytime, presence of metal obstacles, and so on[16]. The radio propagation in indoor environments is evenworse due to multiple reflections that generate multi-pathself-interference [8]. In fact, Djugash et al. in their famous2006 paper [6] wrote: ”While ranging with radio remainsthe eventual goal, practical implementations are not avail-able yet.”, probably meaning that the range measurementsfrom RF are too noisy to be useful in indoor environment.However, our approach proved to be successful in such achallenging environment.

A. Channel Model and Characterization

To estimate the distance between two devices by mea-suring the RSSI, one needs to know the model of the

Fig. 2. A plot of the RSSI measures received by the robot from a WSN nodewhen is moving in the environment plotted against the actual distance robot-node. The blue solid line is the used path loss model for the communicationchannel. Note that the RSSI measurements are very noisy.

transmission channel. According to [8], an accurate modelof the indoor channel is difficult to obtain. However, we donot want to model the specific test environment, but we aimat developing solutions as general as possible. Therefore, weconsider a simple and generic path loss channel model, inwhich the generic i–th node, placed at distance di from thetransmitter, receives a signal with mean power Pi (in dBm)given by:

Pi [dBm] = PT x +K−10η log10

[di

d0

]+Ψi . (1)

In (1), PT x is the nominal transmission power (in dBm), Kis a unitless constant that depends on the environment, d0is a reference distance for the antenna far field, and η isthe path loss coefficient. The term Ψi denotes the randomattenuation due to shadowing, which is generally assumedto be Gaussian, with zero mean and variance σ2

Ψi[8]. The

power measure (in dBm) can be obtained through a linearconversion of the RSSI measurements returned by the nodetransceiver.

The path loss model parameters K, η and d0 have beenestimated from the collected RSSI measures according toa mean square error criterion. The deterministic path lossmodel parameters are PT x +K =−30.5 dB, η = 1.5, and d0 =10cm., whereas the shadowing term Ψi, as expected, canbe fairly well approximated by a Normal random variablewith mean µΨ'−0.0348±0.0860dB and standard deviationσΨ ' 6.339±0.0614 dB, where the range corresponds to the95% confidence interval. The resulting model is plotted as ablue solid line in Fig. 2.

The channel model parameters have been characterized forthe same environment where we the SLAM algorithm wastested. Nonetheless, we observe that this choice does notlimit the generality of the approach nor the validity of theresults. In fact, it is reasonable to assume that the channelparameters of a given environment are determine in advanceby the static wireless sensor network and, hence, transferredto the robot when it enters the environment. In any case, we

Page 4: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

have run the SLAM algorithm over the same set of collectedmeasures but with a channel model characterized for atotally different environment. We observed that the resultsachieved by the algorithm in the two cases were qualitativelycomparable, thus confirming the robustness of the algorithmto the radio channel characterization. The comparison hasnot been reported here for space limits.

B. Filtering noisy measurementsIn indoor environment, the RF signal used by the wireless

nodes is strongly affected by interference and multi-pathproblems. This results in measured RSSI values which areway too high or way too low with respect to the expectedmeasure for that distance. To limit these effects, we pre-filtered the RSSI measurements taking into account theodometry information. If the robot moved a short distancebetween two consecutive measures, the RSSI measured inthe second robot position cannot change too much from thefirst measure. However, we do not know (yet) if the robotis approaching the node or moving away from it. The bestwe can do is to to consider the two extreme cases of totalapproach and of total moving away. Let us consider theexample of Fig. 3. The robot is at position A and moves toposition B, the path length is AB and the node is sitting in theknown position D. If one projects the distance AD over theline BD, one finds the new point C. The lenght of the segmentCD is the distance from the node D before the motion to B.After the motion, the two extreme cases are to end in C′

(i.e. CD−AC) or to end in C′′ (i.e. CD + AC). The actualdisplacement will be within these two extremes. Therefore,we even out the actual RSSI measures received in C to themaximum expected RSSI (i.e. the one corresponding to C’)or to the minimum expected RSSI (i.e. the one correspondingto C′′).

Fig. 3. A sketch of the two extreme situation of the robot total approachand of total moving away with respect to a transitting node to understandthe pre-filter to even out the measured RSSI.

Fig. 4 plots the improvements on the RSSI estimationobtained thanks to the pre-filtering of the actual RSSImeasurements in a typical experiment. The blu line is themeasured RSSI, the red line is the RSSI obtained afterthe pre-filter, and the magenta line is the expected RSSIconsidering the channel model discussed in Sec. IV-A and theactual distance between the robot and the transmitting node.One can see that the pre-filtering cannot guess the correctexpected values, but that the red line is closer to the magentaline than the blue one. In particular, several peaks and valleysare evened out. Extensive experiments (not reported here forpaper space constrains) proved the effectiveness of this verysimple filter.

Fig. 4. The effect of pre-filtering the actual RSSI measures in a typicalexperiment. The blu line is the measured RSSI, the red line is the RSSIobtained after the pre-filter, and the magenta line is the expected RSSI.Note that the red line is much closer to the magenta line with respect tothe blue line.

V. SLAM ALGORITHMS

The SLAM algorithm used in this work uses an ExtendedKalman Filter (EKF) and is very similar to the one presentedin [6], so we will use the same formalism. The robot state(pose and heading) at time k is qk = [xk,yk,θk]. The motionmodel of the system is:

qk+1 =

xk +∆Dk cos(θk)yk +∆Dk sin(θk)

θk +∆θk

+νk = f (qk , uk)+νk (2)

where νk is a noise vector, ∆Dk is the odometric distancetraveled, and ∆θk is the heading change both measured bythe robot’s odometers. The system state matrix A(k) is givenby the Jacobian:

A(k +1) =∂ f∂qk

∣∣∣∣q=qk

=

1 0 −∆Dk cos(θk)0 1 ∆Dk sin(θk)0 0 1

(3)

The input matrix B(k) is given by the Jacobian:

B(k +1) =∂ f∂u

∣∣∣∣q=qk

=

cos(θk) 0sin(θk) 0

0 1

(4)

When a new input from the odometry uk is available,the robot state is updated with motion equation Eq. 2.The standard equations of the EKF updates the covariancematrices. When a new range measure between the robot inthe state qk and the node in the position (xn,yn) is available,it an be expressed as:

h(

q1 , [xn , yn]T)

=√

(xk− xn)2 +(yk− yn)

2 (5)

By linearizing with the Jacobian we can write:

Page 5: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

H(k) =∂h∂q

∣∣∣∣q=qk+1

=[ xk−xn√(xk−xn)2+(yk−yn)2

yk−yn√(xk−xn)2+(yk−yn)2 0

] (6)

The above formulas are for the robot localization only,but in the case of SLAM also the wireless node must belocalized so the new vector of state of the system is:

qk =[

xk,yk,θk,xn1,yn1, . . . ,xni,yni]

(7)

Also the matrices A(k) and B(K) must be modified an theybecome:

A(k +1) =∂ f∂qk

∣∣∣∣q=qk

=

=

1 0 −∆Dk cos(θk) 0 00 1 ∆Dk sin(θk) 0 00 0 1 0 00 0 0 1 0

. . . 00 0 0 0 1

(8)

B(k +1) =∂ f∂uk

∣∣∣∣q=qk

=

cos(θk) 0sin(θk) 0

0 10 0...

...0 0

(9)

Now the time update doen not change with respect towhat is written in Eq. 2, but for the measurement updatethe Jacobian H(k) must be calculated considering that alsothe wireless node position has to be updated. Note that theonly terms non zero in H(k) are the ones relative to the robotposition and those relative to the node corresponding to thecurrent measure.

A. Initial estimation of node position

At the start-up, the EKF needs to be initialized with a firstguess position of the node to be located. Trilateration wasused to guess the position of the node. Trilateration can findthe position of an object D by knowing its distance from nobjects (provided they do not lye on a line) see Fig. 5. Thisis done by looking for the intersection of the circumferencescentered in the reference objects and with radius equal tothe distances from object D. We used least-square methodto find the intersection point and therefore the position of D.Once the first guess on the coordinates of the new node isobtained, this can be inserted in the EKF by expanding it andthe other matrices. In particular, we expand the covariancematrix by inserting the current robot covariance summed tothe mean covariance of the measures used in the trilateration.

The sketched in Fig. 6 can help to summarize up the dataflow in the SLAM algorithm adopted in this work. First theRSSI is measured, then the RSSI values are even-out by thepre-filter. These values are fed together with the odometry

Fig. 5. An example of trilateration. D is the node with unknown position.A, B, and C are robot positions along the path of the robot where the distancerobot-node was estimated using the RSSI.

Fig. 6. The data flow in the implemented SLAM algorithm

into the EKF. The EKF is initialized for every node withthe estimation provided by the trilateration algorithm. Theoutput of the SLAM algorithm is a combined estimation ofthe position of the robot and of the positions of the wirelessnodes.

VI. EXPERIMENTS

In this work we present two kind of experiments: (i) thesimultaneous estimation of the location of the robot andof the locations of the nodes of the WSN, without anyprior knowledge of the node locations; (ii) the simultaneousestimation of the location of the robot and of the nodesstarting from a rough guess close to the real position. Thissecond experiment relates to a scenario in which a humanoperator (or a loosely localized robot (like the helicopterreported in [2]) drops wireless nodes along his/its path andrecords a first localization that needs to be further improved.

The experiments were performed in the 30 m long and 6m wide corridor depicted in Fig. 7. The mobile robot and aWSN composed by 8 nodes where deployed in a 6x3 meterarea as depicted in Fig. 7. The final SLAM experiments arereported in Fig. 8 and Fig. 9.

VII. DISCUSSION

The final result of our experiments is that the proposedapproach can achieve a mean error on the position of thewireless sensor nodes between 0.5m and 1.0m. This isobtained with an on-line algorithm that updates the SLAMstate variables every time a new measure is received. Asecond optimization step could be added in which all data

Page 6: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

Fig. 7. The robot and the WSN deployed in the test environment.

Fig. 8. The reconstruction of the robot path ad of the wireless nodes’positions starting from no knowledge.

collected are re-processed with a bunch process. This couldalso enables to keep only the best RSSI measures on thecomplete path.

An evidence of our work is that our approach is stronglyrelying on the initial guess of the node position. In Fig. 10 arereported the average final errors obtained by the SLAM algo-rithm for the robot localization and for the nodes’ positions.With respect to Fig. 10, XTE and ATE are respectively theCross-Track mean Error and the Along-Track mean Error;Cartesian is the mean error on the Cartesian distancebetween the estimated robot position and the ground-truth;Nodes is the mean error on the Cartesian distance betweenthe estimated and the actual nodes’ positions. The series1,2,3 in Fig. 10 are those referring to the experimentswith the triangulation algorithm to initialize in the SLAMthe position of the newly encountered nodes along the path.The series 7,8,9 in Fig. 10 are those referring to theexperiments in which a good first guess close to the realposition of the node is given to the SLAM system. Thegood first guess is generated for every node randomly in a 2meter range from the actual node position with an associated

Fig. 9. The reconstruction of the robot path and of the wireless nodes’positions starting from a rough initialization of the nodes’ position.

Fig. 10. The comparison between the average residual error on the robotand nodes’ positions obtained by the SLAM algorithm in the two situations:(i) starting from no prior knowledge on the node position (1, 2, and 3 inthe legend), (ii) starting from an initial gross localization close to the realposition (7, 8, and 9 in the legend).

variance initialized to the average error for that distance asestimated from the RSSI curve. In the latter case the residualerror is almost half of the one achieved in the first case.Therefore, a more robust system to estimate the first guesson the node position is needed.

One should also note that in Fig. 10 the standard deviationassociated to the residual error on the nodes’ positions isvery large (the lines over the bars of the histogram). Thisis because there is a big difference in the initial guess ofthe node position among the different nodes. Some wereinitialized with a position wrong by over 4 m, other nodeswhere initialized with less than 1 m error. This is because thefirst guess is strongly depending on the path of the robot andon the structure of the environment. Thus, a lot of measuresare required to bring to convergences these nodes with a largeinitial error (but most of the time we do not have enoughmeasurements, see the Future Work section).

The differences within the two series of Fig. 10 are givenfrom the way in which multiple measures received at thesame position are managed by the SLAM system (and thisoccurs when the robot is steady or is turning on a spot). Inseries 1,7 when the robot is steady , only the first threemeasures in chronological order of reception are inserted in

Page 7: Range-Only SLAM with a Mobile Robot and a …zanella/PAPER/CR_2009/ICRA09_0701_FI...Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks Emanuele Menegatti, Andrea Zanella,

the SLAM algorithm. In series 2,8 when the robot is steady,only the three measures with highest RSSI are inserted inthe SLAM algorithm (i.e., the SLAM state is updated onlywhen the robot moves again). In series 3,9 a fully off-lineapproach is tested and only the 40 measures with the highestRSSI for each node are inserted in the SLAM algorithm.There are no substantial differences among the three, so thesimplest approach (i.e., the first) is adopted.

VIII. CONCLUSIONS AND FUTURE WORKS

This paper reported experiments on the SLAM of a mobilerobot and a WSN in a indoor environment. Moreover, wedecided to use only range-measurements coming from the RFmessages of the WSN estimating the RSSI of the messagesof the WSN.

The SLAM algorithm is based on an extended Kalmanfilter and it was proved to be able to correctly localize therobot and to correctly find the position of the nodes ofthe WSN both starting from no a priori information andby starting from rough initial estimations of the wirelessnodes’ positions. These can easily provided by a human (oran automatic system) when deploying the network.

Results are good and they are similar to previous worksin which a robot is used as a mobile wireless node andmuch better (almost a factor 4) than those obtained by ofnode localization algorithms exploiting only static nodes.However, these results are very good if one consider theywere obtained: (i) in an indoor environment, (ii) withoutany calibration of the receiving/transmitting systems on thenodes, (iii) without a specific model of the actual communi-cation channel of the environment in which the experimentswhere performed, (iv) exploiting only the RSSI measuresto calculate the robot-node distance, (v) no node-to-nodecommunication has been used.

Much better results on the experiments with the no aprioriknowledge would be obtained, if we could have a betterinitial guess of the wireless nodes’ positions. We are workingto improve triangulation by generating an appropriate robotpath in the exploration steps. An interesting future work willbe the automatic planning of the robot path driven by theaim of minimizing the residual error affecting the positionestimation of each node. One could study techniques to movetoward (and possibly around) those nodes with the highestresidual error to lower it.

On the other hand, we also will test the new algorithmrecently developed by Djugash et al. [5] for the RF andultrasonic range measurement Pinpoint device [20] on ourRF-only setup. This new algorithm is reported to be ”robustto poor initialization, and even no initialization, infrequentmeasurements, and incorrect data association”.

IX. ACKNOWLEDGMENTS

We wish to thank the students Enrico Frigo, Luca Laz-zaretto, and Stefano Zanconato for their work in this project.

REFERENCES

[1] P. Bahl and V. Padmanabhan. Radar: an in-building rf-based userlocation and tracking system. INFOCOM 2000. Nineteenth AnnualJoint Conference of the IEEE Computer and Communications Soci-eties. Proceedings. IEEE, 2:775–784 vol.2, 2000.

[2] P. Corke, S. Hrabar, R. Peterson, D. Rus, S. Saripalli, andG. Sukhatme. Autonomous deployment and repair of a sensor networkusing an unmanned aerial vehicle. Robotics and Automation, 2004.Proceedings. ICRA ’04. 2004 IEEE International Conference on,4:3602–3608 Vol.4, 26-May 1, 2004.

[3] R. Crepaldi, A. Harris, A. Scarpa, A. Zanella, and M. Zorzi. Signetlab:deployable sensor network testbed and management tool. In SenSys’06: Proceedings of the 4th international conference on Embeddednetworked sensor systems, pages 375–376, New York, NY, USA, 2006.ACM Press.

[4] R. Crepaldi, A. Harris, A. Scarpa, A. Zanella, and M. Zorzi. Testbedimplementation and refinement of a range-based localization algo-rithm for wireless sensor networks. In 3rd IEE Mobility Conference2006., Oct., 25-27. 2006.

[5] J. Djugash and S. Singh. A robust method of localization and mappingusing only range. In International Symposium on ExperimentalRobotics, July 2008.

[6] J. Djugash, S. Singh, G. Kantor, and W. Zhang. Range-only slam forrobots operating cooperatively with sensor networks. Robotics andAutomation, 2006. ICRA 2006. Proceedings 2006 IEEE InternationalConference on, pages 2078–2084, 15-19, 2006.

[7] J. Fenwick, P. Newman, and J. Leonard. Cooperative concurrent map-ping and localization. Robotics and Automation, 2002. Proceedings.ICRA ’02. IEEE International Conference on, 2:1810–1817 vol.2,2002.

[8] A. Goldsmith. Wireless Communications. Cambridge UniversityPress, 2005.

[9] G. K. Kraetzschmar, H. Utz, S. Sablatnog, S. Enderle, and G. Palm.Miro - middleware for cooperative robotics. In RoboCup 2001: RobotSoccer World Cup V, pages 411–416, London, UK, 2002. Springer-Verlag.

[10] D. Kurth, G. Kantor, and S. Singh. Experimental results in range-only localization with radio. Intelligent Robots and Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International Conferenceon, 1:974–979 vol.1, Oct. 2003.

[11] P. Pathirana, N. Bulusu, A. Savkin, and S. Jha. Node localization usingmobile robots in delay-tolerant sensor networks. IEEE Transactionson Mobile Computing, 4(3):285–296, 2005.

[12] N. B. Priyantha, H. Balakrishnan, E. Demaine, and S. Teller. Mobile-assisted localization in wireless sensor networks. In IEEE INFOCOM,Miami, FL, March 2005.

[13] S. Shenoy and J. Tan. Simultaneous localization and mobile robotnavigation in a hybrid sensor network. Intelligent Robots and Sys-tems, 2005.(IROS 2005). Proceedings. 2005 IEEE/RSJ InternationalConference on, 1, 2005.

[14] I. Shimshoni. On mobile robot localization from landmark bearings.Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEEInternational Conference on, 4:3605–3611 vol.4, 2001.

[15] M. Sichitiu and V. Ramadurai. Localization of wireless sensornetworks with a mobile beacon. Mobile Ad-hoc and Sensor Systems,2004 IEEE International Conference on, pages 174–183, 2004.

[16] J. Thelen, D. Goense, and K. Langendoen. Radio wave propagation inpotato fields. In First workshop on Wireless Network Measurements(co-located with WiOpt 2005), Riva del Garda, Italy, apr 2005.

[17] H. Utz, S. Sablatnog, S. Enderle, and G. Kraetzschmar. Miro -middleware for mobile robot applications. Robotics and Automation,IEEE Transactions on, 18(4):493–497, Aug 2002.

[18] Y. Wang, S. Goddard, and L. Perez. A study on the cricket location-support system communication protocols. Electro/Information Tech-nology, 2007 IEEE International Conference on, pages 257–262, May2007.

[19] A. Ward, A. Jones, and A. Hopper. A new location technique for theactive office. Personal Communications, IEEE [see also IEEE WirelessCommunications], 4(5):42–47, Oct 1997.

[20] J. Werb and C. Lanzl. Designing a positioning system for findingthings and people indoors. Spectrum, IEEE, 35(9):71–78, Sep 1998.

[21] S. Williams, P. Newman, G. Dissanayake, and H. Durrant-Whyte.Autonomous underwater simultaneous localisation and map building.Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE Inter-national Conference on, 2:1793–1798 vol.2, 2000.