introduction to map representation, and state outline (1a ...1 introduction to map representation,...

1

Introduction to Map Representation, and State Estimation, and MOOS programming

J. LeonardSeptember 8, 2008

MIT 2.166

J. Leonard MIT 2.166, Fall 2008

Outline (1A)

Introduction to the course (Part 2)Representations for Probabilistic Robotics

State Estimation in Probabilistic Robotics

Introduction to programming robots using MOOS and C++Overview of MOOS

Demonstration of creating and running a simple MOOSApp


Choices for Map Representation

Features (Landmarks)Occupancy GridsPoses and Laser ScansParticlesTopological

1. Move2. Sense3. Associate measurements with known features4. Update state estimates for robot and previously mapped features5. Find new features from unassociated measurements6. Initialize new features

Feature-based SLAM: Points

2

http://www.informatik.uni-bremen.de/~ufrese/slamvideos1_e.html http://www.informatik.uni-bremen.de/~ufrese/slamvideos1_e.html

Feature extraction: Laser

Obtain line segments from a laser scan:

SegmentationLine estimation

Split and merge:1. Recursive Split:

1. Obtain the line passing by the two extreme points

2. Obtain the point more distant to the line

3. If distance > error_max, split and repeat with the left and right sub-scan

2. Merge:1. If two consecutive segments are

close enough, obtain the common line and the more distant point

2. If distance <= error_max, merge both segments

3. Prune short segments4. Estimate line equation

-14000 -12000 -10000 -8000-4000

-3000

-2000

-1000

0

1000

2000

Split and Merge

Split

Split

Split

No more Splits

Merge

3

Problematic case

Split and merge: uses only extreme pointsOther options:

TLS: Total Least squares (not much better)RANSAC: RANdom SAmpling Consensus


Features (Landmarks)Occupancy GridsPoses and Raw Laser ScansParticlesTopological

Representation: Occupancy Grids

Regular grid.Free and occupied spaceProbability of presence of obstacle surfaceBayesian Update with new measurementsAssumes independancebetween grid elements

Representation: Occupancy Grids

Images courtesy of Olle Wijk, KTH

4


Features (Landmarks)Occupancy GridsPoses and Raw Laser ScansParticlesTopological

Representation: Poses and Scans

Robot scans, moves, scans againShort-term odometry errorcauses misregistration of scansScan matching is the process ofbringing these scans into alignment

Ground truth (unknown)

1 2 1 2

Scan from pose 1 Scan from pose 2

Olsen 2005

SLAM based onLaser scan matching

Vision-based SLAM using Poses and Images (R. Eustice)

Real-time fusion of “zero-drift” camera measurements with navigation

sensor data to close-the-loop on dead-reckoned error.

Seafloor Imagery

I1I2I3

IN

IN-1IN-2

I4I5I6

I7I8

Camera Constraints

Visually Augmented Navigation Estimate

Navigation Sensors(Doppler Velocity Sensor, Compass, …)

+

Eustice, Pizarro, and Singh, ICRA 2004

5

Results for100 Image Sequence

Temporal “Break”

DR / VAN VAN

Evolution of XY Pose Uncertainty

Loop-closing event

Eustice, Pizarro, and Singh, ICRA 2004J. Leonard MIT 2.166, Fall 2008




Sample-based Localization (sonar)

Courtesy of Dieter Fox

6





SLAM using hybrid metric/topological maps with Atlas

Geometry: rigid frames, submapsTopology: map adjacenciesHybrid: uncertain transforms

A

C

B

ED

0 20 40 60 80 100-70-60-50-40-30-20-100

1020

Bosse, Newman, Leonard & Teller, 2004


Example of a globally optimized map

0 20 40 60 80 100 120 140 160-80

-60

-40

-20

0

20

40

60


Atlas Framework: Demonstration of loop closing (using laser data)

~100 m

Before Loop Closing After Loop Closing

7


Outline (1B)






Bayes Filters: Framework

Given:Stream of observations z and action data u:

Sensor model P(z|x).Action model P(x|u,x’).Prior probability of the system state P(x).

Wanted: Estimate of the state X of a dynamical system.The posterior of the state is also called Belief:

),,,|()( 121 tttt zuzuxPxBel −= K

},,,{ 121 ttt zuzud −= K


Choices for State Estimation

Types of uncertaintyKalman filterInformation FilterParticle filter

8

Why is Probabilistic Robotics difficult?

“The key scientific and technological issue in robotics is that of coping with uncertainty ... In fact, the uncertainty is such that one of the most challenging activities for a mobile robot is simply going from point A to point B.'’

Tomas Lozano-Perez, 1990

Forms of uncertainty in robotic mapping

Loop closingNavigation drift

Global

Data association

Sensor noiseLocal

DiscreteContinuousUncertainty:

Scale:

What tools and techniques are available for SLAM?

Examples from the literature …Recursive least-squares/EKF Probabilistic data associationMultiple hypothesis trackingHough transformSequential Monte Carlo methods (particle filters) Random Sample Concensus(RANSAC)Bundle adjustment Variable state dimension filter

Hartley and Zisserman, © 2001

(courtesy of Udo Frese)

9

(courtesy of Udo Frese) (courtesy of Udo Frese)

Basic Linear-Gaussian SLAM (Smith, Self, and Cheeseman, 1987)

1. Move2. Sense3. Associate measurements with known features4. Update state estimates for robot and previously

mapped features5. Find new features from unassociated measurements6. Initialize new features

Feature-based SLAM

Hans Jacob Feder, MIT ME PhD 1999

10

Example: Land robot using laser scanner data

MIT Indoor Track

64 hurdles, 4 tennis courts

Example: Land robot using laser scanner data

Example: Land robot using laser scanner dataGround Truth Map and Dead-Reckoning

Example: Land robot using laser scanner dataVideo of SLAM Measurement Processing

11

Comparison of Mapped Target Locations (blue circles) with Ground Truth (red stars)


Outline (2)






What has caused our recent progress in Robotics Research?

Better robot platforms?

Better computers?

Better sensors?

Better software tools?

Better algorithms?


Public-domain Robot Software Packages

Carmen (CMU/Stanford/MIT)

MOOS (MIT/Oxford)

Player/Stage (USC et al.)

LCM (MIT)

12

Carnegie Mellon Navigation System

Semi-autonomous map-building, autonomous navigation and localizationSupports 9 base types, 3 sensor typesUsed for winning entries in AAAI Challenge 2002 (CMU/Nasa/NRL/Swarthmore/Northwestern) and again in 2003 (WUStL).Used by Stanford’s robot Stanley that won the 2005 DARPA Grand ChallengeAlso used in MIT Subject 6.141http://www.cs.cmu.edu/~carmenDeveloped by Nicholas Roy and Mike Montemerlo J. Leonard MIT 2.166, Fall 2008


Public-domain Robot Software Packages

Carmen (CMU/Stanford/MIT)

MOOS (MIT/Oxford)

Player/Stage (USC et al.)

LCM (MIT)

Mission Oriented Operating Suite

Software for Developing and Deploying Autonomous Vehicles

Paul M. Newman Oxford and MIT

13


Aims

To present a unified architecture for (oceanic) vehicles with slow dynamicsTo facilitate the rapid development of new vehicle functionalityTo provide a one-file, one-mission paradigmTo allow simultaneous execution of multiple mission tasks/rules


Advantages

Very simple code templateVery robustMulti-platform (Linux, Solaris, NT, Neutrino)Intuitive mission specification

Typical MOOS Community

iDepth

iDVL

iLBL

iGPS

iINSiRemote

pNav pHelm iActuation

pLogger

MOOSScope

Remote Group

IO Group

Core Group

On BoardOff B

oard

i implies Interface (serial port,human)p implies Pure (RAM only)u implies utility

Underneath the Hood

MOOSComms

14

The Role of the DataBase

MOOSDB

CommsCommsComms

user code

user code

user code

Everything is storedIn the DB

App1App2 App3

Each App hasa link into MOOScommunity

Apps user code does not ‘know’about DB or other Apps!(But the comms knows about the DB)

Comms Components

MOOSDB

MOOS Client

ApplicationCode

•Central repository of system state•Manages subscriptions and publications

•Manages “check in” with DB•Decouples application from comms

Client Process

Server

•Application-specific code

MOOS Service Level

MOOS Client Level

Application Level

Tcp/ip

The ASCII Code

2 3 4 5 6 7 30 40 50 60 70 80 90 100 110 120

------------- ---------------------------------

0: 0 @ P ` p 0: ( 2 < F P Z d n x

1: ! 1 A Q a q 1: ) 3 = G Q [ e o y

2: " 2 B R b r 2: * 4 > H R \ f p z

3: # 3 C S c s 3: ! + 5 ? I S ] g q {

4: $ 4 D T d t 4: " , 6 @ J T ^ h r |

5: % 5 E U e u 5: # - 7 A K U _ i s }

6: & 6 F V f v 6: $ . 8 B L V ` j t ~

7: ' 7 G W g w 7: % / 9 C M W a k u DEL

8: ( 8 H X h x 8: & 0 : D N X b l v

9: ) 9 I Y i y 9: ' 1 ; E O Y c m w

A: * : J Z j z

B: + ; K [ k {

C: , < L \ l |

D: - = M ] m }

E: . > N ^ n ~

F: / ? O _ o DEL

How Do I use it?

Making a MOOSApp

15

Component Architecture

CMOOSApp

CMOOSInstrument

Has Attributes:•MOOSClient•MOOSClockFunctions:•OnServerConnect()•OnMail()•Iterate()

Has Attributes:•Serial PortFunctions:•ReadTelegram•WriteTelegram

Derives from

MOOS Application Programming -“Simple”

In CMOOSApp class

Forever{

if(HaveNewMail()){

OnNewMail()}

Iterate()

Sleep(1/Freq)

}

In CMOOSApp derived class

OnNewMail(){

for all mail{

if(Msg.Name = “Rudder”){

SetRudder(Msg.nVal);}etc…..

}}

Iterate(){

ApplicationSpecific(){

ReadSerialPort()ParseData()SendNotification()

}

overloaded

overloaded

Using CMOOSApp

CMOOSApp::Run(){

SetUpMOOSComms();

OnStartUp();

while(1){

//other stuff here..... if(m_Comms.Fetch(MailIn)){

//process mailOnNewMail(MailIn);

}

//do application specific processingIterate();

//other stuff here..}

}

Does everything for you!Lots of goodies inlcuded:

•FileReading / parsing•IO•Rate control•Comms Setup

Skel

eton

of :

:Run

Overload Worker Functions

bool Iterate();

bool OnNewMail(MOOSMSG_LIST &NewMail);

bool OnConnectToServer();

bool OnStartUp();

Do your processing here

Handle new mail here

Subscribe for mail here

Configure yourself here

16

Example Codebool CMOOSJanitor::OnConnectToServer(){

//register for a restart signalm_Comms.Register("JANITOR_RESTART",0);

//register for explicit circuit controlm_Comms.Register("JANITOR_SWITCH",0);

if(IsBluefinVehicle()){

//register for wathc dog hit from iACtuation...

m_Comms.Register("ACTUATION_WD_HIT",3.0);}return true;

}

bool CMOOSJanitor::OnNewMail(MOOSMSG_LIST &NewMail){

CMOOSMsg Msg;

if(m_Comms.PeekMail(NewMail,"JANITOR_RESTART",Msg,true)){

SetUpSwitches();}

if(m_Comms.PeekMail(NewMail,"ACTUATION_WD_HIT",Msg,true)){

if(IsBluefinVehicle()){

HitTailConeWD();}

//more stuff here}

bool CMOOSJanitor::Iterate(){

if(m_bAutoWatchDog){

HitTailConeWD();}

GetTemperature();

return true;}

bool CMOOSJanitor::OnStartUp(){

//call base class member firstCMOOSInstrument::OnStartUp();

string sWD;m_MissionReader.GetValue("AUTOWATCHDOG",sWD);

m_bAutoWatchDog = MOOSStrCmp(sWD,"TRUE");

//more stuff here…return true;

}

MOOS Frame

pLogger

Trivial Configuration: eg “Log = Variable @ watch-period”

Two kinds of logs: 1. Synchronous – less accurate but can be read into matlab

• Can only log numeric data• Unchanged data written as NaN• Writes a row of a matrix every time step

2. Asynchronous – logs every change to watched variables• Allows replay of data• Specifies who wrote data• Logs all data types

Typical Mission File

/////////////////////////////////////////////// depth sensor configuration blockProcessConfig = iCompass{

AppTick = 8CommsTick = 4Port = COM1BaudRate = 9600Streaming = true

}

/////////////////////////////////////////////// pNav control blockProcessConfig = pNav{

X = GPS @ 3.0 , DVL @ 4.0 , LBL @ 5.0Y = GPS @ 3.0 , DVL @ 4.0 , LBL @ 5.0Z = DVL @ 3.0 , LBL @ 15.0Depth = PARA @ 5.0, DVL @ 4.0 Altitude = DVL @ 4.0Yaw = CROSSBOW @ 2.0,COMPASS

}

Process Config (.moos file)

17

Example .alog file

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% LOG FILE: /home/moos/MOOSLogs/ChuckNav2/MOOSLog_11_4_2002_____08_47.alog%% FILE OPENED ON Thu Apr 11 08:47:55 2002%% LOGSTART 1018529273.23%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

1.368 GPS_Y iGPS 6.49364 1.368 GPS_X iGPS -2.96852 1.368 GPS_RAW iGPS $GPGGA,134652,4221.5105,N, 1.397 DEPTH_DEPTH iDepth 0.06 1.847 DEPTH_DEPTH iDepth 0.08 2.207 GPS_Y iGPS 6.49364 2.207 GPS_X iGPS -2.96852 2.207 GPS_RAW iGPS GPGGA,134653,4221.5105,N,07105.252.307 DEPTH_DEPTH iDepth 0.09 2.767 DEPTH_DEPTH iDepth 0.08 3.227 DEPTH_DEPTH iDepth 0.08 3.187 GPS_Y iGPS 6.49364 3.187 GPS_X iGPS -2.96852

Example .slog File

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% LOG FILE: /home/moos/MOOSLogs/ChuckNav/MOOSLog_10_4_2002_____10_56.slog

%% FILE OPENED ON Wed Apr 10 10:56:46 2002

%% LOGSTART 1018450604.28

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% (1) TIME

%% (2) DESIRED_THRUST

%% (3) DESIRED_RUDDER

%% (4) DESIRED_ELEVATOR

%% (5) LBL_TOF

%% (6) LBL_CH_4

%% (7) LBL_CH_5

%% (8) LBL_CH_6

%% (9) LBL_CH_7

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% TIME DESIRED_THRUST DESIRED_RUDDER DESIRED_ELEVATOR LBL_TOF LBL_CH_4

%% TIME [ 2.70] [ 2.70] [ 2.70] [ 2.70] [ 2.70]

3.263 NaN NaN NaN NaN NaN





5.844 NaN NaN NaN NaN 0.698087

Approaches to desigining robot control software:

Traditional model-based approach vs.

Subsumption Architecture

18

Model-based Control (Conventional Approach)

Illustration courtesy of Siegwart and Nourbakhsh

Traditional Decomposition

perception

modeling

planning

task execution

motor control

sensors actuators

a.

Figures copyright Rodney A. Brooks, MIT

avoid hitting things

locomote

explore

build maps

manipulate the world

actuatorsensors

b.

An Alternative Approach: Decomposition by Functionality in the World/

Layers of increasing complexity


Genghis Walking Robot (1988)


19

Genghis control architecture

for/backpitch

alphacollide

betapos

legdown

up legtrigger

betaforce

betabalance

s

i

i

IRsensors

walk

steer

s

d

alphaadvance

alphabalance s alpha

pos

prowl feelers






Behavior-based Control of the Roomba (2004)

Video courtesy of David Moore

20

Behavior-based Control of the Roomba

Data from Moore et al., Sensys 2004

0 50 100 150 200

-180

-160

-140

-120

-100

-80

-60

-40

-20

0

20 Localized pathTrue path

Distance (cm)

Robot path over time

Example final project from 6.836 Embodied Intelligence, Spring, 2004

Explore while avoiding obstaclesRecognize and seek signsRead signs and verbalize contentsAssociate sign locations with sign contentsPerform simultaneous localization and mapping, using sign contents to help solve data association problem

Alexey Radul & Jim Psota, 2004

Spring, 2004: MapQuest for Indoors (6.836 Project of Alexey Radul and Jim Psota)

Crazy building!

Where ever am I?

Hi, I’m VentriloBot. Let me show you the way!

Radul & Psota, 2004

iCam

screenOut

Laser

pMerge-Laser

Rect-Finder OCR

pFoundSign

iSpeakpSeekSign

pRandWay

pWaySeq pWayPt

pAvoid pLooseHF pBumper

pHF MotorsS SSSS

S

Implementation using the Subsumption Architecture

Radul & Psota, 2004

21

6.836 Live Class Demo

Radul & Psota, 2004

6.836 Live Class Demo

Radul & Psota, 2004

introduction to map representation, and state outline (1a ...1 introduction to map representation,...

Documents