6 Simulaneous Localizaion and Mapping (SLAM Auonomous Roboics 6905 Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM Lecure 6: Simulaneous Localizaion and Mapping Dalhousie Universiy i Ocober 14, 2011 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 1
6 Simulaneous Localizaion and Mapping (SLAM Lecure Ouline Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM Inroducion Exended Kalman Filer Paricle Filer Underwaer SLAM based on diagrams and lecure noes adaped from: Probabilisic bili i Roboics (Thrun, e. al. Auonomous Mobile Robos (Siegwar, Nourbakhsh Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 2
6 Simulaneous Localizaion and Mapping (SLAM Conrol Scheme for Auonomous Mobile Robo Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 3
6 Simulaneous Localizaion and Mapping (SLAM Plan for Class Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM Thomas covered generalized Bayesian filers for localizaion las week Kalmanfiler mos useful oucome for localizaion Mae covers pah-planning and navigaion Mae hen follows on wih Bayesian filers o do a specific example, SLAM Thomas o follow wih reinforcemen learning afer ha Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 4
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Robo Mapping Paricle Filer When i is Applied Underwaer SLAM when is simulaneous localizaion and mapping (SLAM needed? when a robo has o be ruly auonomous wih no human inervenion (e.g. underwaer vehicles beyond a few km, millions of miles away in space he operaor has no siuaional awareness of he robo s environmen environmen is unknown and here is no prior knowledge beacons and neworks canno be deployed or used (e.g. in GPS denied areas like underwaer or under-ice Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 5
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Robo Mapping Paricle Filer Where i is Applied Underwaer SLAM in all environmens robos are in indoors undersea space underground Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 6
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Robo Mapping Problems Paricle Filer Difficuly Underwaer SLAM mos difficul percepual inference problem in mobile robos acquiring a spaial model of he robo s environmen for navigaion purposes robo mus have sensors ha enable i o perceive is environmen e.g. cameras, range finders, sonar, laser, acile sensors, compass and GPS sensors are subjec o error (measuremen noise sensors have finie range (e.g. sound can penerae walls his means he robo has o navigae hrough is environmen when map building moions commands (conrols issued during mapping carry informaion for building maps since hey convey info abou locaions where differen sensor measuremens are aken Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 7
6 Simulaneous Localizaion and Mapping (SLAM Markov Localizaion (Bayes Filer Quick Review observaion model: or Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM Pz ( x Pz ( x, m probabiliy of a measuremen z given ha he robo is a posiion x and map m moion model: P ( x x 1, u poserior probabiliy ha acion u akes he robo from saes x -1 o x belief poserior probabiliy condiioned on available daa Bel( x p( x z, u predicion esimae before measuremen: Bel x p( x z u Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 ( 1 Faculies of Engineering & Compuer Science 8
6 Simulaneous Localizaion and Mapping (SLAM Markov Localizaion(Bayes Filers Quick Review predicion (prior: bel ( x p( x u, x 1 bel( x 1 dx 1 (convolves moion model wih belief from previous ime sep Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM bel(x o is uniform over all poses observaion model, p(z x,m bel(x =bel(x 0 p(z x,m 0 updae (poserior: bel x p( z x bel( x ( incorporaes he measuremen robo moves o he righ bel(x p(x u,x -1 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 9
6 Simulaneous Localizaion and Mapping (SLAM Markov Localizaion (Bayes Filer Quick Review Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM for developing a range/bearing sensor model i is useful o inroduce a correspondence variable beween he feaure f i and he landmark m j of he map his variable is he correspondence and i is denoed c i c i is he rue ideniy of he observed feaure f i localizaion assumes he map is represened by a collecion of feaures and ha he correspondences are known Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 10
6 Simulaneous Localizaion and Mapping (SLAM Robo Mapping Challenges Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 1. Modelling Measuremen Noise robo moion iself is subjec o errors and conrols alone are insufficien o deermine a robo s pose wihin is environmen modelling measuremen noise is a key challenge roboic mapping would be relaively l easy if he noise of differen measuremens are saisically independen robo would jus make more measuremens o negae noise effecs unforunaely, wih roboic mapping measuremens errors are saisically dependen errors in conrols accumulae over ime and affec he way sensor measuremens are made Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 11
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Robo Mapping Challenges Paricle Filer Localizaion and Mapping Underwaer SLAM mapping someimes referred o in conjuncion wih localizaion (deermine robo pose esimaing where hings are and deermining where he robo is (boh have uncerainy is solved in conjuncion allows he measuremen and conrol noise o be independen in he robo sae esimaion hus he problem of mapping creaes an inheren robo localizaion problem so robo mapping is also referred o as concurren mapping and localizaion (CML sae-of-he-ar algorihms in mapping are probabilisic due o he uncerainy and sensor noise Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 12
6 Simulaneous Localizaion and Mapping (SLAM Roboic Mapping Challenges Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 1. Modelling Measuremen Noise cumulaive effec of conrol errors on fuure sensor inerpreaions small roaion error a one end of a corridor cumulaes o many meers of error a he oher end relaive o map for robo pah obained by odomery Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 13
6 Simulaneous Localizaion and Mapping (SLAM Roboic Mapping Challenges Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 2. High Dimensionaliy of Eniies consider he info o describe your home environmen wih jus corridors, inersecions, rooms, and doors deailed 2D floor plan requires housands of coordinaes o define 3D visual map would require millions of coordinaes from a saisical perspecive, each coordinae is a dimension of he esimaion problem Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 14
6 Simulaneous Localizaion and Mapping (SLAM Roboic Mapping Challenges Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 3. Correspondence Problem also referred o as he daa associaion problem mos difficul problem deermine if sensor measuremens aken a differen imes correspond o he same physical objec robo rying o map a cyclic environmen; when closing cycle robo has o localize iself relaive o he previous map by hen, cumulaed pose error may be unbounded Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 15
6 Simulaneous Localizaion and Mapping (SLAM Roboic Mapping Challenges Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 4. Environmen Changes wih Time on scales ha vary depending on he environmen: from a ree ha changes very slowly sea boom ha changes due o currens over days locaion of a chair ha could change on he order of minues, or people movemen ha changes consanly environmen changes manifes as inconsisen sensor measuremens (when hey are no few algorihms ha learn meaningful maps of dynamic environmens (los of room for research conribuions here! Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 16
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Robo Mapping Challenges Paricle Filer 5. Pah-Planning On-he-Fly Underwaer SLAM robo mus plan is pah during mapping ask of generaing robo moion plans o build a map is referred o as roboic exploraion opimal pah planning in a fully modelled environmen is relaively l well undersood d robos in unknown environmens has incomplee model have o accommodae coningencies and surprises ha arise during map building generae plans in near real-ime where o move balanced agains map informaion gain and ime and energy o obain info as well as possible loss of pose info along he way Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 17
6 Simulaneous Localizaion and Mapping (SLAM The SLAM Problem Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM A mobile robo can build a map of an environmen and a he same ime use his map o deduce is locaion. The rajecory of he robo and he locaion of all landmarks are esimaed on-line wihou he need for any a priori knowledge of locaion simulaneous esimae of boh robo and landmark locaions required rue locaions are never known or measured direcly observaions are made beween he rue robo and landmark locaions. k = ime index Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 18
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Probabilisic SLAM Paricle Filer Recursive Soluion Underwaer SLAM compue he probabiliy disribuion for all imes p ( X 0:, m Z 0:, U 0:, x 0 (* ( his is he join poserior densiy of he landmark locaion and vehicle sae x given recorded observaions Z & conrol inpus U (up o and including wih iniial ii vehicle pose x o desire a recursive soluion (i.e. calc from he same probabiliy disribuion from previous ime sep sar wih esimae for disribuion p( x1, m Z0: 1, U0: 1 a -1, use Bayes heorem o deermine he join poserior, following conrol u and observaion z Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 19
6 Simulaneous Localizaion and Mapping (SLAM Probabilisic SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Observaion and Moion Models need moion (sae ransiion and observaion models o describe he effec of he conrol inpu, u observaion model when robo and landmark locaion known: p( z x, m moion model for sae ransiions: p( x x 1, u sae ransiion is assumed o be a Markov process where nex sae x depends only on he immediae sae, -1, before i and applied conrol u independen d of observaion and map Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 20
6 Simulaneous Localizaion and Mapping (SLAM SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Problem Formulaion no map available and no pose informaion p ( X :, m Z 0:, U 0 : 0 landmark 1 m 1 observaions z 1 z 3 robo poses x x... 1 x 2 x 3 x 0 conrols u u 1 u -1 u 0 1 landmark 2 m 2 z 2 z Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 21
6 Simulaneous Localizaion and Mapping (SLAM Two Forms of SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM here are really wo forms of he SLAM problem: p full SLAM: esimaes poserior for enire pah (0: and map which is wha is discussed so far (paricle filer soluion: p( X :, m Z 0:, U 0: 0 online SLAM: esimaes poserior for curren pose using mos recen pose and map only (i.e. las ime sep ( soluion ( x, m Z0 :, U0: p( X 0:, m Z0:, U0: dx0dx1dx2... dx1 inegraions i ypically done one a a ime discards pas conrols and measuremens once processed since hey are no used again Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 22
6 Simulaneous Localizaion and Mapping (SLAM SLAM Feaure a coninuous and discree componen coninuous Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM locaion of objecs in he map and he robo pose objecs may be landmarks in he feaure-based represenaion objec paches deeced by range finders discree (more on his laer correspondence or daa associaion beween landmarks and measuremens, i.e. how a newly deeced objec relaes o previously deeced ones eiher he objec was previously deeced or i was no Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 23
6 Simulaneous Localizaion and Mapping (SLAM On-line SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM graphical model of on-line SLAM (one pose a a ime p( x, m Z0 :, U0: p ( X 0:, m Z0:, U0: dx0 dx1 dx2... dx ( 1 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 24
6 Simulaneous Localizaion and Mapping (SLAM Full Blown SLAM graphical model of full blown SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM p( X, m Z, U ( 0: 0: 0: Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 25
6 Simulaneous Localizaion and Mapping (SLAM Probabilisic SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM SLAM implemened in sandard 2-sep recursive predicion (ime updae correcion (measuremen updae form: ime updae (prior disribuion p ( x, m Z0: 1, U0:, x0 p( x1, u p ( x1, m Z0: 1, U0: 1, x0 dx1 measuremen updae (poserior disribuion p( x, m Z 0:, U 0:, x 0 p ( z x, m p ( x, m Z, U p( z now, have a recursive procedure for calculaing Z 0: 1 p( X0:, m Z0:, U0:, x0 0: 1 for robo sae x and map m a ime based on all conrol inpus U and observaions Z as funcions of he moion and observaion models Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6, U 0: 0:, x 0 Faculies of Engineering & Compuer Science 26
6 Simulaneous Localizaion and Mapping (SLAM Srengh of SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM he error beween esimaed & rue landmark locaions are common beween landmarks and come from a single source: errors in knowledge of where he robo is when he landmark observaions were made landmark locaion error esimaes are highly correlaed relaive locaion beween landmarks m i m j known wih good accuracy even when absolue locaions uncerain correlaions beween landmark esimaes increase monoonically as more and more observaions are made knowledge of relaive locaion of landmarks always improves and never diverges regardless of robo moion his is due o observaions being nearly independen for relaive locaions beween landmarks Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 27
6 Simulaneous Localizaion and Mapping (SLAM SLAM Soluions now require represenaions for: moion model observaion model Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM ha allow efficien and consisen compuaion of he prior (ime and poserior (measuremen disribuions mos common represenaion is wih sae space model and addiive Gaussian noise which leads o use of exended Kalman filer ( soluion alernaive represenaion is o describe robo moion model as a se of samples of a more general non-gaussian probabiliy disribuion which leads o he use of paricle filer or FasSLAM as anoher soluion here are many ohers bu will only cover hese wo oday Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 28
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 1 / 9 use inernal represenaions for posiions of landmarks (map sensor parameers assume: robo uncerainy a sar posiion is zero Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM sar: robo has zero uncerainy Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 29
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 2 / 9 Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM predic how he robo has moved measure updae he inernal represenaion firs measuremen of feaure A Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 30
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 3 / 9 robo observes a feaure which is mapped wih an uncerainy relaed o he sensor error model (i.e. measuremen model Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM predic how he robo has moved measure updae he inernal represenaion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 31
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 4 / 9 as robo moves (in response o he moion mode, is pose uncerainy increases Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM predic how he robo has moved measure updae he inernal represenaion robo moves forwards: uncerainy ygrows Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 32
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 5 / 9 robo observes wo new feaures Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM predic how he robo has moved measure updae he inernal represenaion robo makes firs measuremens of B & C Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 33
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 6 / 9 heir posiion uncerainy resuls from he combinaion of he measuremen error wih he robo pose uncerainy map becomes correlaed wih he robo posiion esimae Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM predic how he robo has moved measure updae he inernal represenaion robo makes firs measuremen of B & C Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 34
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 7 / 9 robo moves again and is uncerainy increases (moion model Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM robo moves again: uncerainy grows sill predic how he robo has moved more measure updae he inernal represenaion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 35
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 8 / 9 robo re-observes an old feaure loop closure deecion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM predic how he robo has moved measure updae he inernal represenaion robo re-measures A: loop closure Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 36
6 Simulaneous Localizaion and Mapping (SLAM SLAM in Acion 9 / 9 robo updaes is posiion: he resuling gposiion esimae becomes correlaed wih he feaure locaion esimaes robo s uncerainydecreases and so does he uncerainy in he res of he map Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM robo re-measures A: loop closure predic how he robo has moved uncerainy decreases measure updae he inernal represenaion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 37
6 Simulaneous Localizaion and Mapping (SLAM Abou Covariance Marix Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM correlaion measures he degree of linear dependence beween wo variables covariance of wo variables measure how srongly correlaed wo variables are covariance marix conains he covariance on: robo posiion, landmarks, beween robo posiion and landmarks and beween he landmarks cell A conains he covariance on robo posiion, a 3 by 3 marix (x, y and B is he covariance on he firs landmark, a 2 by 2 marix, since he landmark does no have orienaion, ; C is covariance for he las landmark. D conains he covariance beween he robo sae and he firs landmark; E conains he covariance beween he firs landmark and he robo sae; E can be deduced from D by ransposing sub-marix D F conains he covariance beween he las landmark and he firs landmark, while G conains he covariance beween he firs landmark and he las landmark, which again can be deduced by ransposing F cov(x, Y = E{[X - E(X][Y - E(Y]} cor(x, Y = cov(x, Y / [sqr(var(x *sqr(var(y] covariance marix Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 38
6 Simulaneous Localizaion and Mapping (SLAM SLAM Implemenaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM Kalman filers are Bayesian filers ha represen poserior, p(x,m z,u wih Gaussians Example of Kalman filer esimaion of he map and vehicle pose [1]. Shown is he pah of an AUV wih range measuremens from a sonar. 14 feaures are idenified from he sonar daa. Ellipse around feaures convey uncerainy ha remains afer mapping as specified by he covariance marix Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 39
6 Simulaneous Localizaion and Mapping (SLAM SLAM Implemenaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Resuls Mapped (a map of landmarks obained in simulaion (b correlaion marix afer 278 ieraions of Kalman filer mapping. Checkerboard appearance verifies heoreical find ha in he limi, all landmark locaion esimaes are fully correlaed (c normalized inverse covariance marix of he same esimae shows he dependencies are local. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 40
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Kalman Filering Paricle Filer Assumpions Underwaer SLAM hree main ones: (i nex sae funcion (moion model linear wih added Gaussian noise (ii same is rue of he percepual model (iii he iniial uncerainy mus be Gaussian Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 41
6 Simulaneous Localizaion and Mapping (SLAM Exended Kalman Filer ( Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Sae Model in a linear sae funcion, robo pose x, and map m, a ime ~ linearly wih previous pose x -1, map m -1, and conrol u for map, obviously rue since he map does no change however, x usually governed by a rig funcion ha varies nonlinearly l wih previous pose x -1 and conrol u o accommodae such nonlineariies Kalman filers approximae he robo moion model wih a linear funcion obained via Taylor series expansions o yield he exended Kalman Filer ( moion commands approximaed by a series of smaller moion segmen usually works well for mos roboic vehicles Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 42
6 Simulaneous Localizaion and Mapping (SLAM SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Sae Moion Model p(x x -1, u = Ax -1 + Bu + w A and B are marices ha implemen linear mapping from sae x -1 and moion command u o sae x noise (assumed Gaussian in moion is modeled via w which h is assumed o be normally disribued ib d wih zero mean and covariance Q more specifically, p x x, u x ( 1 f ( x 1, u where f(.. models he robo dynamics / kinemaics / odomery w Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 43
6 Simulaneous Localizaion and Mapping (SLAM SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Observaion Model sensor measuremens usually nonlinear wih non-gaussian noise approximae hrough a firs degree Taylor series expansion, i.e. p(z x, m = Cx + v C is a marix (a linear mapping and v is he normally disribued measuremen noise wih zero mean and covariance R more specifically, p( z x where h(.. describes he geomery of he observaion, m z h( x, m v hese approximaions work well for robos ha can measure heir ranges and bearings o landmarks Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 44
6 Simulaneous Localizaion and Mapping (SLAM SLAM Inroducion SLAM Formulaion Overview Paricle Filer Underwaer SLAM similar o implemenaion for robo localizaion SLAM summarizes all pas experience in an exended sae vecor, y compromising of robo pose x and he posiion of all map feaures m and an associaed covariance marix y :.. mn xx xm1 xm -for a MindSorm robo, x m x m m.. m m 1 1 1 1 size of y m = 3 + 2n since he y,... y nmapfeaurehaveonly2...... coordinaes each m n 1 x m m.. m m 1 n1 1 n1 -size of y = (3+2n 2 y y as robo moves and makes measuremens, y and y are updaed wih he sandard equaions correlaions are imporan for convergence, he more observaions ha are made he more correlaions beween he feaures will grow beer he SLAM soluion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6.. n1 n1 n1 Faculies of Engineering & Compuer Science 45
Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 6 Simulaneous Localizaion and Mapping (SLAM Compue Mean and Covariance Time Updae p apply sandard mehod o calculae he mean Z x x : 0 and covariance: Z m E m : 0 0: xm xx Z T mm T xm m m x x m m x x E : 0 of he join poserior disribuion p(x,m Z 0:,U 0:,x 0 from ime updae m m m m ime updae 1 1, 1, 1 1 1 i d h l d f J bi h i h h, ( T xx xx f f Q f f u x f x Faculies of Engineering & Compuer Science 46 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 1 1 evaluaed a he esimaed he Jacobian of is such ha x - f f
6 Simulaneous Localizaion and Mapping (SLAM Compue Mean and Covariance Observaion Updae observaion updae: Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM x x m W z h( x, m ] m 1 1 W S W T 1 1 and such ha S h - 1 1 W h is he Jacobian of h evaluaed a h T T S 1 R x 1 and m h 1 hp://www.youube.com/wach?v=r-ognddhl34 / h? Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 47
6 Simulaneous Localizaion and Mapping (SLAM SLAM Drawbacks convergence Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM convergence of he map is based on he monoonic convergence of he deerminan of he map covariance marix ( mm, and all landmark pair submarices o zero compuaional effor observaion updae sep requires all landmarks and he covariance marix be updaed every ime an observaion is made compuaion grows quadraically wih # of landmarks, i is a lile beer han ha wih opimizaions Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 48
6 Simulaneous Localizaion and Mapping (SLAM SLAM Drawbacks daa associaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM errors in associaing observaions wih landmarks breaks i loop-closure where a robo reurns o re-observe landmarks afer having been away a long ime is difficul especially difficul if landmarks are no simple poins and look differen from differen direcions (e.g. mines wih side scan sonar images nonlineariy linearized versions of nonlinear model and observaion models used can resul in huge inconsisencies i i in he soluions Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 49
6 Simulaneous Localizaion and Mapping (SLAM FasSLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM beer soluion: FasSLAM [2] using a paricle filer fundamenal shif in recursive probabilisic SLAM paricle filer capures he nonlinear process model and non-gaussian pose disribuion for robo pose esimaion Rao-Blackwellized mehod reduces compuaion effor (FasSLAM sill linearizes observaion model, like Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 50
6 Simulaneous Localizaion and Mapping (SLAM Paricle Filer SLAM Inroducion SLAM Formulaion Definiions Paricle Filer Underwaer SLAM paricle filer: models ha represen probabiliy disribuions as a se of discree paricles which occupy he sae space paricle: a poin esimae of he sae wih an associaed weigh, w, p i = (y i, w i each paricle defines a differen vehicle rajecory hypohesis probabiliy disribuion (ellipse as paricle se (red dos Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 51
6 Simulaneous Localizaion and Mapping (SLAM Paricle Filers Inroducion SLAM Formulaion Overview Paricle Filer Underwaer SLAM high dimensionaliy sae-space of SLAM makes direc applicaion of paricle filers compuaionally infeasible i is possible o reduce he sample space by applying a paricle filer where a join space is pariioned according o produc rule: p ( x1, x2 p ( x2 x1 p ( x1 if p(x 2 x 1 can be represened analyically hen only p(x 1 need be sampled ( 1 ~ p( x1 ( i join disribuion is hen represened by he se: x, p( x x i N and saisics such as he marginal probabiliy p( x 1 1 N 2 p x2 N i 2 x x ( i 1 ( i 1 i Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 52
6 Simulaneous Localizaion and Mapping (SLAM Paricle Filers Inroducion SLAM Formulaion Overview Paricle Filer Underwaer SLAM recursive esimae performed by paricle filering for pose saes and for map saes represens beliefs by random samples esimaion of nonlinear, non-gaussian processes Sampling Imporance Re-Sampling (SIR principle draw he new generaion of paricles assign an imporance weigh o each paricle re-sample as needed weighed samples Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 afer resampling Faculies of Engineering & Compuer Science 53
6 Simulaneous Localizaion and Mapping (SLAM Implemenaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM as wih, he join SLAM sae may be facored ino a robo componen and a condiional map componen: p( x 0:, m Z 0:, U 0:, p ( m X 0:, Z 0: p ( X 0: Z 0:, U 0:, x 0 he probabiliy disribuion is on he rajecory X 0: raher han he single pose x when condiioned on he rajecory he landmarks become independen ha is why paricle filers are so fas map is represened as a se of independen Gaussians x 0 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 54
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Implemenaion Paricle Filer Overview Underwaer SLAM essenial srucure of FasSLAM is a Rao-Blackwellized (RB sae where he rajecory is modelled by weighed samples and he map is deermined analyically join disribuion a ime, is represened by he se: w ( i ( i ( i, X0:, p( m X0:, Z0: N i where he map associaed wih each paricle is composed of independen Gaussian disribuions: ( i M ( i ( m X 0: Y, Z0: p ( m j X 0:, Z0: j p X recursive esimaion performed by paricle filering for he pose saes and he sill for he map saes Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 55
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Implemenaion Paricle Filer Map Underwaer SLAM updaing map for given rajecory paricle X is rivial each observed landmark is processed individually as an measuremen updae from a known pose unobserved landmarks are unchanged ( i 0: A single realizaion of robo rajecory in he FasSLAM process. Ellipsoids show he proposal disribuion for each updae sage from which a robo pose is sampled, and, assuming his pose is perfec, he observed landmarks are updaed. Thus, he map for a single paricle is governed by he accuracy of he rajecory. Many of hese rajecories provide a probabilisic model of robo locaion. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 56
6 Simulaneous Localizaion and Mapping (SLAM Implemenaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Pose Saes propagaing he pose paricles is much more complex paricle filer is derived from a recursive form of sample, sequenial imporan sampling (SIS which samples from a join sae hisory and elescopes he join ino a recursion via he produc rule: p( x, x1,..., xt Z0: T p( x0 Z0: T p( x1 x0, Z0: T,..., p( xt X 0: T 1, Z0: 0 T a each ime sep, paricles are drawn from a proposal disribuion: ( x X 0: 1, Z0: which approximaes he rue disribuion p( x X 0: 1, Z0: T and he samples are given imporance weighs approximaion error grows wih ime increasing he variaion in sample weighs and hus degrade he saisical accuracy Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 57
6 Simulaneous Localizaion and Mapping (SLAM Implemenaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Pose Saes resampling sep reinsaes uniform weighing bu causes loss of hisorical paricle informaion SIS wih resampling produces reasonable saisics only for sysems ha exponenially forge heir pas general lform for RB paricle filer for SLAM: assume a ime -1 he join sae is represened by: ( i ( i ( i w X, p( m X, Z N 1, 0: 1 0: 1 0: 1 N i Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 58
6 Simulaneous Localizaion and Mapping (SLAM Implemenaion Seps predic Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM apply moion predicion o each paricle make measuremens updae, for each paricle: probabiliy disribuion (ellipse as paricle se (red dos compare paricle s predicions of measuremens wih he acual measuremens assign weighs such ha paricles wih good predicions have higher weighs normalize weigh of paricles o sum o 1 resample:generae new se of M paricles which all have equal weighs 1/M reflecing probabiliy densiy of flas paricle se Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 59
6 Simulaneous Localizaion and Mapping (SLAM Paricle Filer SLAM Forma Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM 1. for each paricle, compue a proposal disribuion, condiioned on he specific paricle hisory, draw a sample from i: ( i ( i x ~ ( x X, Z0: 0: 1 his new sample is joined o he paricle hisory 2. weigh samples according o he imporance funcion w ( i w( i 1, u p( z ( i X ( i 0: 1 ( i 0: 1, Z ( x X, Z0: 0: 1, u X ( i ( i 0 : X0: 1, he numeraor erms are he observaion model and he moion model; he observaion model differs because RB requires dependency on he map be marginalized away. p( z X 0:, Z0: 1 p( z x, m p( m X0: 1, Z0: 1 dm x ( i Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 60
6 Simulaneous Localizaion and Mapping (SLAM Paricle Filer SLAM Forma Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM 3. If necessary, resample. When bes o resample is an open problem. Resampling is accomplished by selecing paricles, wih replacemen, from he se ( i ( i X including 0 : N associaed maps, wih probably of selecion proporional o w (i. Seleced paricles are given uniform weigh, ( i w 1/ N 4. For each paricle, perform an updae on he observed landmarks as a simple mapping operaion wih known vehicle pose. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 61
Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k 6 Simulaneous Localizaion and Mapping (SLAM Paricle Filer SLAM Forma several implemenaions of FasSLAM (paricle filer, mos complee is FasSLAM 2.0 p For FasSLAM 2.0, he proposal disribuion includes he b i ( ( i i curren observaion: such ha:, ( ~ ( 0: ( i i u X x p x ( ( 1,, ( ( ( 0: ( 1 0: i i i Z X u Z X x p where C is a normalizing consan, (,, ( ( 1 1 0: ( 1 : 0 i i u x x p Z X x z p C where C is a normalizing consan imporance weigh is C w w i i ( 1 ( Faculies of Engineering & Compuer Science 62 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6
6 Simulaneous Localizaion and Mapping (SLAM Paricle Filer SLAM Forma Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM proposal disribuion is locally opimal each paricle gives he smalles possible variance in imporance weigh ( i condiion upon available informaion, Z, andu X 0 : 1 0: 0: large scale oudoor SLAM [3] Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 63
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Paricle Filer SLAM Paricle Filer FasSLAM Approach Underwaer SLAM solve sae poserior using Rao-Blackwellized Paricle Filer each landmark esimae is represened by a 2x2 each paricle is independen (due o facorizaion from he ohers and mainains he esimae of M landmark posiions hp://www.youube.com/wach?v=m3l8ofbtxh0 Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 64
6 Simulaneous Localizaion and Mapping (SLAM Underwaer SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM use naural feaures of environmen for navigaion imporan in applicaions where odomery and direcion sensors are unavailable for e.g. ship hull inspecion by an AUV where sonar imaging and range sensing presen cos-effecive alernaives o high precision inerial navigaion, and u/w ops near a large seel srucure means no compass, GPS, or long baseline acousic racking a planar marine vehicle using range and bearing measuremens of a se of poin feaures o raverse a pah wih ime-varying conroller and esimaor gains Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 65
6 Simulaneous Localizaion and Mapping (SLAM Navigaion of AUV Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM success of fuure AUVs lies in he abiliy o accuraely localize iself wihin he underwaer domain underwaer world limis he ypes of sensor available compared o above waer GPS is no available underwaer however, if ruly auonomous underwaer vehicles are o be developed, good navigaion sensory informaion is needed o achieve mission goals and provide safe operaion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 66
6 Simulaneous Localizaion and Mapping (SLAM Navigaion of AUV Curren AUV Navigaion Schemes inerial navigaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM uses gyroscopic sensors o deec he acceleraion of he AUV significan improvemen over dead reckoning and is ofen combined wih a Doppler velociy log which h can measure he AUV s relaive velociy acousic navigaion uses ransponder beacons o allow AUV o deermine is posiion mos common mehod are long baseline which uses a leas wo, widely separaed ransponders and ulra-shor baseline which uses GPS calibraed ransponders on a single surface ship Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 67
6 Simulaneous Localizaion and Mapping (SLAM Navigaion of AUV Curren AUV Navigaion Schemes geophysical navigaion Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM uses physical feaures of he AUV s environmenoo produce an esimae of he AUV locaion here can be pre-exising or purposefully deployed feaures mos curren AUV s are equipped wih sensors which can make use of a combinaion of all hree mehods differen sensor daa from each mehod needs o be processed ogeher hroughou a mission o obain an opimal esimae of he AUV posiion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 68
6 Simulaneous Localizaion and Mapping (SLAM Navigaion of AUV Curren AUV Navigaion Schemes Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM echniques currenly used for deriving an esimae of he AUV s posiion from such sensor daa are Kalman filers paricle filers SLAM Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 69
6 Simulaneous Localizaion and Mapping (SLAM Ship Hull Monioring SLAM applied o ship hull inspecions Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM MIT Bluefin Hovering Auonomous Underwaer Vehicle (HAUV designed o perform auonomous ship hull inspecions using SLAM. Idenified mine-like objecs using DIDSON imaging sonar in real-ime ime. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 70
6 Simulaneous Localizaion and Mapping (SLAM Ship Hull Monioring Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Feaure Exracion Performance of real-ime feaure exracor demonsraed using a DIDSON frame. Raw daa (lef and he feaure exracor deecion index for each recangular quadran of image (righ. Areas where feaures were idenified (indicaed by blue aserisk correspond o high peaks in he feaure deecion index. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 71
6 Simulaneous Localizaion and Mapping (SLAM Ship Hull Monioring Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM C l di R k Feaure Exracion (a Real-ime map and vehicle localizaion daa obsained from a survey of he USS Saaoga using an. (b Asonarmosaicimageofhearges image of arges placed on he ship hull Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 72
6 Simulaneous Localizaion and Mapping (SLAM Muli-Vehicle SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM group of unmanned surface vehicles (USV for shallow waer hydrographic missions more efficienly and reliably han a single one over a large environmen issues of iner-vehicle map fusion and daa associaion some level l of collaboraion required Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 73
6 Simulaneous Localizaion and Mapping (SLAM Muli-Vehicle SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM muli-beam sonar scanners used o exrac feaures and objecs on he seabed combine feaures wih accurae posiional informaion o build maps each USV performs SLAM independenly over is local region and a specified imes fuses hese independen measuremens o build an overall global map while improving each vehicle s posiion esimaes combining informaion from muliple USVs challenged by compounding posiional i errors of individual id USVs and varying uncerainies and sensor noise characerisics scalabiliy for numbers of vehicles can be an issue Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 74
6 Simulaneous Localizaion and Mapping (SLAM Muli-Vehicle SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM local sub maps no only faciliae improved daa associaion bu significanly improved performance gains due o periodic map fusions (mosaicking achieved hrough idenifying common feaures from overlapping areas Two robos mapping independenly wih respec o local frames of reference. F G refers o he global reference frame while F L1 and F L2 refers o he local reference frame of he wo robos. Black sars in local frames of reference correspond o he feaures mapped by each vehicle and red ones correspond o he overlapping feaure. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 75
6 Simulaneous Localizaion and Mapping (SLAM Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM SLAM is one of he mos difficul problems in roboics and paricle filer are he wo mos popular soluions for he SLAM problem paricle filer is a more robus soluion bu here are researchers in underwaer SLAM ha ge good resuls wih underwaer SLAM is an area ha is receiving more aenion Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 76
6 Simulaneous Localizaion and Mapping (SLAM References Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM [1] S.Williams, G. Dissanayake, and H.F. Durran-Whye, Towards errain-aided navigaion for underwaer roboics, Advanced Roboics, 15(5, 2001. [2] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei, Fas-SLAM: A facored soluion o he simulaneous localizaion and mapping problem, in Proc. AAAI Na. Conf. Arif. Inell., 2002, pp. 593 598. [3] J.E. Guivan and E.M. Nebo, Opimizaion of he simulaneous localizaion and map-building algorihm for real-ime implemenaion, IEEE Trans. Robo. Auoma., vol. 17, no. 3, pp. 242 257, 2001. Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 77
6 Simulaneous Localizaion and Mapping (SLAM hw #3, ques 2, par (ii Inroducion SLAM Formulaion Paricle Filer Underwaer SLAM for range of 10 unis, range res = 0.25, ang res = 2.5 deg Auonomous Roboics CSCI 6905 / Mech 6905 Secion 6 Faculies of Engineering & Compuer Science 78