special interest group on networking signet range-only slam with a mobile robot and a wireless...

Special Interest Group on NETworking

SIGNET

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

UNIVERSITY OF PADUADept. of information Engineering

Emanuele Menegatti*, A. Zanella^, S. Zilli*, F. Zorzi^, E. Pagello*

Intelligent Autonomous Systems Lab University of Padua

2 Luca Lazzaretto, A.A. 2006-07

RAMSES2 - Project RAMSES2: integRation of Autonomous Mobile robots and

wireless SEnsor networks for Surveillance and reScue

Autonomous

Mobile

Robot

Wireless networkchannel 802.15.4

Wireless

Sensors

Network

Laptop

eyesIFX motesfrom Infineon

802.11b wireless channel

autonomous mobile robot

312 September 2007 Andrea Zanella

Experimental Set upExperimental 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

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 2008 Stefano Zilli 2

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

RealWSN08 Workshop - Glasgow April 1st 2008

Problem StatementProblem 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

RealWSN08 Workshop - Glasgow April 1st 2008

Localization ApproachesLocalization 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

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:

• robot’s odometry;• range measurements from the nodes to the robot

28 Aprile 2008 Stefano Zilli 2

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

8 Luca Lazzaretto, A.A. 2006-07

Node on the robot

Allow a bidirectional serial communication(ASCII chars)

Allow robot’s 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/Output Functions

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

SLAM AlgorithmWe want to estimate:

• Robot absolute position (Xr,Yr) and heading (Ɵr)

• Motes absolute position (Xni,Yni)

28 Aprile 2008 Stefano Zilli 4

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

12 September 2007 Andrea Zanella

Radio Channel modelRadio Channel model

• The robot-mote range is estimated from the received power using the radio channel model

• Path loss channel model: received power Pi @ distance di

Received power

Transmitted power

Path loss coefficient

reference distance

environmental constant

real transmitter-receiver distance Shadowing Shadowing

fast fading

12

12 September 2007 Andrea Zanella

How harsh is the indoor radio channel?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!

Noisy measurements

28 Aprile 2008 Stefano Zilli 7

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

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 robot’s mobilityto reduce SHADOWING

Filter on RSSI measurements

28 Aprile 2008 Stefano Zilli 8

•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

Filter on RSSI measurements

28 Aprile 2008 Stefano Zilli 8

Blue diamonds = measured RSSI

Green diamonds = filtered RSSI

Green are now much more close to hypothetical line

SLAM Algorithm layout

28 Aprile 2008 ICRA09 5

Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless

Extended Kalman Filter

Odometry

RSSI Measures

Initialization

Filter

Mote pose and robot position estimation

Mote position initialization•EKF needs initialization for each mote.

•we use trilateration based on first filtered RSSI measurements from each mote.

28 Aprile 2008 Stefano Zilli 9

Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless

19

Experiments

28 Aprile 2008 Stefano Zilli 10

Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless

EyesIFX v2 Mote

Robot “Bender”

Results (1/4) - SLAM

28 Aprile 2008 Stefano Zilli 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

•Slightly better results taking only highest RSSI measurements (Elab 2)

Fig. 1 residual mean error on robot and motes position

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

Where does the error come from?

28 Aprile 2008 Stefano Zilli 13

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

28 Aprile 2008 Stefano Zilli 14

Algoritmo SLAM per Robot Mobile e Rete di Sensori Wireless

Results (4/4) - SLAM

•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 2008 Stefano Zilli 15

Delayed Initialization based on Particle Filter

25

On-going work

15

meters

meters

Many thanks to S. Zanconato e A. Pretto for their work

Special Interest Group on NETworking

SIGNET

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

UNIVERSITY OF PADUADept. of information Engineering

Emanuele Menegatti*, A. Zanella^, S. Zilli*, F. Zorzi^, E. Pagello*

Intelligent Autonomous Systems Lab University of Padua

27

12 September 2007 Andrea Zanella

Why taking highest RSSI?Why taking highest RSSI?

Noise free RSSI

RSSI+=RSSI + |Ψ|

RSSI-=RSSI - |Ψ|

Δd+ Δd-