1. Special Interest Group on NETworking SIGNET Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks UNIVERSITY OF PADUA Dept. of information Engineering Emanuele Menegatti*, A. Zanella^, S. Zilli*, F. Zorzi^, E. Pagello* Intelligent Autonomous Systems Lab University of Padua

2. 2 Luca Lazzaretto, A.A. 2006-07 RAMSES2 - Project RAMSES 2 : integRation of Autonomous Mobile robots and wireless SEnsor networks for Surveillance and reScue A utonomous M obile R obot Wireless network channel 802.15.4 W ireless S ensors N etwork Laptop eyesIFX motes from Infineon 802.11b wireless channel autonomous mobile robot

3. 3 12 September 2007 Andrea Zanella Experimental Set up EyesIFX sensor nodes –Infineon Technologies. –19.2 kbps bit rate @ 868 MHz –Light, temperature, RSSI sensors SIGNET IAS AMR Bender –self-made, based on Pioneer 2 ActivMedia platform –Linux OS with Miro middleware –ATX motherboard –1,6 GHz Intel Pentium 4, 256 MB RAM, 160 GB HD EyesIFX connected to ATX via USB + EyesService class added to Miro –Omnidirectional camera, odometers

4. Introduction WSN deploying is an annoying and time consuming task. Motes can be attached to objects that are moved around First goal of the project localize WSN nodes spread in unknown positions inside a building using a mobile robot. 28 Aprile 2008Stefano Zilli2 Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks

5. RealWSN08 Workshop - GlasgowApril 1 st 2008 Problem Statement Position knowledge required by many WSN applications Two main approaches Nodes position hard written: High deployment cost/time Not always feasible Very accurate Motes capable of self-localizing: Easy deployment Need dedicated hardware to achieve high precision

6. RealWSN08 Workshop - GlasgowApril 1 st 2008 Localization Approaches Three main ranging approaches: Angle of Arrival Time of Arrival Received Signal Strength Indicator (RSSI) Focus on RSSI: No specific Hardware required Poor outdoor ranging performance Very poor indoor ranging performance

7. Our Solution SLAM (Simultaneous Localization And Mapping), for a mobile robot moving in an unknown environment in which there is a WSN (Wireless Sensor Network). We use only: robots odometry; range measurements from the nodes to the robot 28 Aprile 2008Stefano Zilli2 Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks

8. 8 Luca Lazzaretto, A.A. 2006-07 Node on the robot Allow a bidirectional serial communication (ASCII chars) Allow robots applications to interact with the WSN Physical connection between robot and mote Serial port emulation over USB (VCP) Standard commands for eyesIFX sensor Predefined actions to access to the WSN Input/OutputFunctions

9. Middleware Miro The robot is programmed exploiting the framework Miro Miro is a framework for mobile robot programming developed by Gerd Mayer and Gerhard Kraetzschmar at Ulm University Miro is a middleware based on CORBA architecture for creating and managing distributed services. Miro is based on TAO libraries of the ACE framework. We interact with the eyesIFX mote on board of the robot through a Miro service we created, called EyesService. Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks

10. SLAM Algorithm We want to estimate: Robot absolute position (X r,Y r ) and heading ( Ɵ r ) Motes absolute position (X ni,Y ni ) 28 Aprile 2008Stefano Zilli4 We can measure: Robot odometry (relatively small errors) Range between mote and robot using RSSI (large errors) RSSI = received signal strength indication is a measurement of the power present in a received radio signal. Most motes have circuits on board to inexpensively calculate RSSI. Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks

11. 11 12 September 2007 Andrea Zanella Radio Channel model The robot-mote range is estimated from the received power using the radio channel model Path loss channel model: received power P i @ distance d i Received power Transmitted power Path loss coefficient reference distance environmental constant real transmitter- receiver distance Shadowing fast fading

12. 12 12 September 2007 Andrea Zanella How harsh is the indoor radio channel? Random variations due to shadowing and fading obscure the log-decreasing law for the received power vs distance RSSI based ranging is VERY noisy!

13. Noisy measurements 28 Aprile 2008Stefano Zilli7 For the same range, we can measure very different RSSI We measure the RSSI to estimate the range... then... Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks

14. 14 Luca Lazzaretto, A.A. 2006-07 Sample Measurements Average RSSI for every cell SOURCE POSITION CELLS of 20x20cm RSSI affected by PATH LOSS and SHADOWING effects. Use of robots mobility to reduce SHADOWING

15. Filter on RSSI measurements 28 Aprile 2008Stefano Zilli8 We know robot motion reliably on a short base Given a certain movement, we can foreseen the maximum change in RSSI We can saturate RSSI measurements to this maximum value

16. Filter on RSSI measurements 28 Aprile 2008Stefano Zilli8 Blue diamonds = measured RSSI Green diamonds = filtered RSSI Green are now much more close to hypothetical line

17. SLAM Algorithm layout 28 Aprile 2008ICRA095 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless Extended Kalman Filter OdometryRSSI Measures Initialization Filter Mote pose and robot position estimation

18. Mote position initialization EKF needs initialization for each mote. we use trilateration based on first filtered RSSI measurements from each mote. 28 Aprile 2008Stefano Zilli9 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless

19. 19 Experiments 28 Aprile 2008Stefano Zilli10 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless EyesIFX v2 Mote Robot Bender

20. Results (1/4) - SLAM 28 Aprile 2008Stefano Zilli11 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless Much better that classical static WSN localization algorithm Large variance on residual error for motes locations Slightly better results taking only highest RSSI measurements (Elab 2) Fig. 1 residual mean error on robot and motes position

21. 11 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless Much better that classical static WSN localization algorithm Large variance on residual error for motes locations Results (2/4) - SLAM

22. Where does the error come from? 28 Aprile 2008Stefano Zilli13 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless If we correctly initialize the mote position in the EKF...( Elab 5 & 6 ) Results: Slight improvements on robot residual error Large improvements on mote residual error Fig. 2 Residual mean error on robot and motes position

23. 28 Aprile 2008Stefano Zilli14 Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless Results (4/4) - SLAM

24. The SLAM solution performed better than the solutions adopted by the WSN community with static nodes The SLAM solution performed comparabily to more complex WSN algorithms with mobile nodes The saturation filter helped to reduce errors The residual error is dominated by the initialization error The trilateration algorithm is not rubust to such a severe noise Robust initialization algorithm needed We are implementing Delayed Initialization based on Particle Filter Conclusions 28 Aprile 2008Stefano Zilli15

25. Delayed Initialization based on Particle Filter 25 On-going work 15 meters Many thanks to S. Zanconato e A. Pretto for their work

26. Special Interest Group on NETworking SIGNET Range-only SLAM with a Mobile Robot and a Wireless Sensor Networks UNIVERSITY OF PADUA Dept. of information Engineering Emanuele Menegatti*, A. Zanella^, S. Zilli*, F. Zorzi^, E. Pagello* Intelligent Autonomous Systems Lab University of Padua

27. 27 12 September 2007 Andrea Zanella Why taking highest RSSI? Noise free RSSI RSSI + =RSSI + | | RSSI - =RSSI - | | Δd+Δd+ Δd-Δd-