Probabilistic Fundamentals in Robotics

Transcription

1 Probablstc Fundamentals n Robotcs Probablstc Models of Moble Robots Robotc mappng Baslo Bona DAUIN Poltecnco d Torno Course Outlne Basc mathematcal framework Probablstc models of moble robots Moble robot localzaton problem Robotc mappng Probablstc plannng and control Reference textbook Thrun, Burgard, Fox, Probablstc Robotcs, MIT Press, robotcs.org/ Baslo Bona 2 1

2 Robotc mappng Occupacy grd mappng SLAM GraphSLAM Sparse extended nformaton flter FastSLAM Baslo Bona 3 Mappng In many cases maps are already avalable and are used to perform robot localzaton: we have already dscussed the localzaton problem wth known maps However n many cases maps are unknown and must be bult by the robot tself as t moves n the envronment Snce odometry s nexact, the true pose of the robot s not known wth precson, therefore mappng s and localzaton must be performed at the same tme, post processng the robot measurements The concurrent process of localzaton and mappng s called SLAM (smultaneous localzaton and mappng) Baslo Bona 4 2

3 Example The robot has to acqure the map of the envronment whle localzng tself relatve to ths map What happens f we smply perform localzaton usng the flters learned so far and we do the mappng wth the (uncertan) pose estmate? post process Occupancy grd map Raw range data, poston ndexed by odometry Baslo Bona 5 Map types landmarks Dgtal elevaton map Landmark based representaton (stochastc map) Occupancy grd map Pont cloud representaton Baslo Bona 6 3

4 Mappng and world representatons Two partcular representatons are common n the applcatons of moble robotcs n ndoor scenaros Landmark based maps: stochastc maps that contan a probablstc descrpton (usually mean+covarance) of the poston of salent features of the scenaro Occupancy grd maps: hgh resoluton models of the envronment; t s a grd n whch each cell contans the probablty of the cell beng occuped Landmarks (doors, corners, objects) Free cell Unknow cell Occuped cell Baslo Bona 7 3D mappng Other types of maps, as 3D mappng are possble, but computatonally much more complex Baslo Bona 8 4

5 Mappng Assumng that we know the robot poses, mappng s the task of buldng a consstent t trepresentaton tt of the envronment Dependng on the scenaro t s convenent to use dfferent world representatons Occupancy grd maps are the best choce for maps, and the related famly of algorthms s called occupancy grd mappng Baslo Bona 9 The mappng process The smplest case s ntroduced frst: the robot pose s know but the measurements are nosy known pose MAPPING nosy measurements 2D maps are the most common; they can be used when robot moton takes place on a flat surface and sensors capture essentally a slce of the surroundng envronment Occupancy grd maps Baslo Bona 10 5

6 Occupacy grd map algorthm In prncple, the algorthm shall compute the posteror probablty of the map pm ( z, x ) 1: t 1: t The (occupancy grd) map s descrbed by a set of N cells m = { m m m },,, N 1 2 At each cell a probablty of occupancy (0 = free whte, 1 = occuped black) s attached p( m ) The mappng problem s decomposed nto N smpler problems pm ( z, x ) = p( m z, x ) 1: t 1: t 1: t 1: t Ths decomposton does not allow to represent dependences among neghborng cells, and we throw away much nformaton Baslo Bona 11 Bnary Bayes flter The prevous factorzaton transforms the problem nto a bnary estmaton problem wth statc state; a bnary Bayes flter s adopted The log odds representaton of occupancy s used; the advantage s that numercal nstabltes near 0 or 1 are avoded l t, = p( m z, x ) 1: t 1: t log 1 - p ( m z 1: t, x1: t ) 1 p( m z, x ) = 1-1: t 1: t 1 + exp( l ) t, Baslo Bona 12 6

7 Occupancy grd mappng Occupancy_grd_mappng( { l }, x, z ) t-1, t t 1: forall cells m 2: f m n sensor feld of z t then 3: p( m z, x ) t t l = l + log -l t t- 1 - p ( m zt, xt ) 4: else 5: l = l 6: 7: endf endfor return, 1, 0 t, t- 1, { } l t, Ths s the nverse sensor model n log form Baslo Bona 13 Inverse sensor model ( m, x, z ) Inverse_sensor_model t t 1: Let x, y be the c.o.m. of m x = (,, x y q) 2: ( ) ( ) 2 2 r = x - x + y -y ( y y x x ) 3: f = atan2 -, - -q 4: k = argmn f-q k ( max t ) j, sens j, sens t robot pose 5: f r > mn z, z + a or f- q > b 2 then 6: return l 0 k k 7: f z < z and r - z < a 2 max 8: return l k 9: f r zt 10 : return l 11 : endf t j occ free outsde range occuped cells free cells Baslo Bona 14 t 7

8 Parameters of the nverse sensor model range closed to the measured range k th beam xy, r f b a xy, q sensor cone Baslo Bona 15 Example usng ultra sound sensors Intal map Fnal map Baslo Bona 16 8

9 Another example unknown occuped free Baslo Bona 17 Mult sensor fuson When robots have dfferent types of onboard sensors, t s natural to try to ntegrate ther measurements Sensors may have dfferent characterstcs,.e., dfferent models of the perceved world; sometmes an obstacle detected by one sensor s not detected by a dfferent sensor To avod the occurrence of conflctng nformaton, a popular soluton bulds separate maps for each sensor type and then ntegrates them usng a sutable combnaton functon Baslo Bona 18 9

10 Mult sensor fuson Let the map bult by the k th sensor type be { m, m,, m 1 2 N} { m } m = = k k k k k If the sensors measurements are ndependent, we can combne them usng the De Morgan s Law NOT (P OR Q) = (NOT P) AND (NOT Q) NOT (P AND Q) = (NOT P) OR (NOT Q) k p ( m ) = 1 - (1 - p ( m )) Alternatvely one can compute the most pessmstc estmate comng from the sensors Baslo Bona 19 k k p( m ) = max p( m ) k Smultaneous localzaton and mappng (SLAM) SLAM s also known as Concurrent Mappng and Localzaton or CML The problem arses when the robot does not have access to the envronment map and does not know ts pose So, both the map and the pose must be estmated concurrently from measurements z 1:t and controls u 1:t SLAM s a very complex problem, sgnfcantly harder that smple localzaton or smple mappng wth known pose Many types and algorthms have been studed; some are ncremental, some are complete Baslo Bona 20 10

11 SLAM We need to estmate the jont dstrbuton of both robot pose and map representaton of the envronment bel p( x, m z, u ) = t t 1: t 1, t Present state only Ths s called onlne SLAM problem snce t dscards past measurements The full SLAM problem consders the entre path bel = p( x, m z, u ) t 1: t 1: t 1, t Entre past state hstory Baslo Bona 21 Onlne SLAM u u u t- 1 t t+ 1 xt-1 x x t t+ 1 zt-1 z zt t+ 1 Estmated varables m Baslo Bona 22 11

12 Full SLAM u u u t- 1 t t+ 1 xt-1 x x t t+ 1 zt-1 z zt t+ 1 Estmated varables m Baslo Bona 23 SLAM The relaton between the two approaches s smple to state px (, m z, u ) px (, m z, u )dx dx dx - = òò ò t 1: t 1, t 1: t 1: t 1, t 1 2 t 1 Moreover, ts nature s both contnuous and dscrete The objects on the map and the robot pose are contnuous The correspondence between a measurement and a feature n the map or between a feature and a prevously detected one s dscrete If the correspondence must be made explct, we wrte px (, mc, z, u ) t t 1: t 1, t or px mc z u 1: t t 1: t 1, t (,,, ) Baslo Bona 24 12

13 Problems arse from SLAM Hgh dmensonalty of the contnuous parameter space Large number of dscrete correspondence varables The algorthms employed depend manly on whch type of map needs to be estmated Landmark based maps EKF SLAM Sparse Extended Informaton Flters GraphSLAM Rao Blackwellzed Partcle Flters (FastSLAM) Occupancy grd maps Rao Blackwellzed Partcle Flters GraphSLAM Baslo Bona 25 Assumptons: EKF SLAM wth landmark based maps Feature (.e., landmark) based maps; the number of landmarks must be relatvely small (< 1,000) Works well f landmarks are unambguous; feature detectors must be optmzed It s based on Gaussan nose assumpton for robot moton and percepton Makes lnearzaton, so the amount of uncertanty of the posteror shall be lmted Can only process postve sghtngs of landmarks (no nformaton from absence of landmarks) Landmark correspondences are exactly known Baslo Bona 26 13

14 EKF SLAM wth landmark based maps EKF SLAM ncludes the poston of the landmarks n the state vector and performs estmaton over the augmented state: y æ ö x t = ç = ( xyqq ) 1, 1, 2, 2,,, çèm ø t x y x y N x N y Localzaton Mappng/landmarks Observng a partcular landmark mproves the poston estmate of all landmarks, not only ths one Ths mprovement s medated by the robot pose: landmark observaton mproves the robot pose, that n turn mproves the localzaton of prevous observed landmarks Baslo Bona 27 Covarance matrx Mathematcally, ths dependence s captured by the off dagonal elements of the covarance matrx x t æ æ ö ö æm ö x 1 1 r s s s s s s s xx xy xq xl xl xl xl r r r r r r r x r y r Nx r Ny m y s s s s s s s xy yy yq yl yl yl yl r r r r r r r r 1x r 1y r Nx r Ny m s s s s s q x q y q q q q l q l r s s q l q l r r r r r r r 1x r 1y rnx rny m l 1x ~ N s s s s s s s x l y l q l l l l l l l l l r 1x r 1x r 1x 1x 1x 1x 1y 1x Nx 1x Ny, m l s s s s s s s 1y x l y l q l l l l l l l l l r 1y r 1y r 1y 1x 1y 1y 1y 1y Nx 1y Ny m l Nx s s s s s s s x l y l q l l l l l l l l l rnx rnx rnx 1xNx 1yNx NxNx NxNy ç m çè l Ny ø s s s s s s s ç è ç è x l y l q l l l l l l l rny rny rny 1xNy 1yNy NxNy l Ny l Ny øø Baslo Bona 28 14

15 EKF SLAM We can smply extend the expresson of the dynamc system used for EKF Localzaton ìï p = f u w ( p ï,, t t- 1 t t ) í ï z = h p u t t t ïî (, map, ) PROCESS MODEL r,, ù ( 1: t t t ) é r p ù é f p u w t t t t 1 1 = N N ê ë ú û ê ú ë û MEASUREMENT MODEL r 1,... N ( p,,, ) z = h p u t t t t t So the estmaton procedure can proceed accordng to EKF, teratng predcton and update phase Baslo Bona 29 EKF SLAM: a typcal stuaton (1) Robot starts; frst measurement of feature A (2) Robot drves forward (uncertanty grows) State vector x t can grow as new landmarks are dscovered (3) Robot makes frst measurements of B and C (4) Robot drves back towards the start (uncertanty grows) (5) Robot re observes A (loop closure); uncertanty shrnks (6) Robot re observes B; also the uncertanty of C shrnks Baslo Bona 30 15

16 Another example: EKF appled to the onlne SLAM problem The robot s path s a dotted lne, and ts estmates of ts own poston are shaded ellpses. Eght dstngushable landmarks of unknown locaton are shown as small dots, and ther locaton estmates are shown as whte ellpses. In (a) (c) the robot s postonal uncertanty s ncreasng, as s ts uncertanty about the landmarks t encounters. In (d) the robot senses the frst landmark agan, and the uncertanty of all landmarks decreases, as does the uncertanty of ts current pose Baslo Bona 31 SLAM wth unknown landmark correspondence When landmark correspondence s unknown, the EKF SLAM algorthm s extended usng an ncremental maxmum lkelhood estmator to determne landmark correspondence A new landmark s created f the Mahalanobs dstance to all exstng landmarks n the map exceeds a value a Outlers detecton: how to detect spurous landmarks that are outsde the uncertanty range of landmarks already present EKF s fragle wth respect to landmark confuson, so one of the followng methods s used to avod t Spatal arrangement: choose landmarks that are far apart so that t s unlkely to confuse them > optmal tradeoff between to few and too many landmarks Sgnatures: chose landmarks wth dstngushable sgnatures (colors, dmensons, etc.) Baslo Bona 32 16

17 EKF: summary EKF landmark based SLAM s an effectve and easy to mplement soluton to many problems It has been successfully appled also n large scale envronments Onlne verson tes to determne the momentary robot pose Global problem tes to determne all poses Wth known correspondence the algorthm s ncremental Maps management wrt outlers Include a provsonal of landmarks that are not observed suffcently often Include a landmark evdence counter that computes the psoteror evdence of the exstence of a landmark Baslo Bona 33 EKF: summary Crtcal Issues Complexty s quadratc n the number of landmarks: O(n 2 ) Can dverge f nonlneartes are large Data assocaton: how do we decde f we are observng an already seen landmark? Baslo Bona 34 17

18 Rao Blackwellzed Partcle Flters (RBPF SLAM) Rao Blackwellzed Partcle Flters (RBPF), also known as FastSLAM, are a sample based technques for solvng SLAM They are partcularly sutable for estmatng occupancy grd maps of an ndoor envronment Rao Blackwellzed Partcle Flters are based on the followng factorzaton of the SLAM belef: Robot trajectory Map of the envronment Measurements Commands bel = prob( x, m z, u ) = t 1: t 1: t 1, t prob( m x, z ) prob( x z, u ) SLAM belef mappng 1: t 1: t 1: t 1: t 1, t localzaton Baslo Bona 35 Rao Blackwellzed Partcle Flters (RBPF SLAM) The prevous formula s known as Rao Blackwell factorzaton, whch gves the name to the correspondng soluton to SLAM The underlyng math translates n the followng smple concept: RBPF estmates potental trajectores of the robot and for each hypothess the SLAM reduces to mappng wth known poses The flter estmates potental trajectores and a map s assocated to each trajectory hypothess Baslo Bona 36 18

19 Summary RBPF SLAM Rao Blackwellzed Partcle Flters (RBPF) have been demonstrated to be an effectve soluton for the estmaton of occupancy grd maps No data assocaton (grd maps does not dstngush elements n the envronment) Issues Each partcle carres on a complete map hypothess: a normal computer s not able to manage more than few hundred partcles But to cope wth large uncertanty you need a large number of partcles Baslo Bona 37 Thank you. Any queston? Baslo Bona 38 19