Page 1 PS2: due Friday 23:59pm. Final project: 45% of the grade, 10% presentation, 35% write-up Presentations: in lecture Dec 1 and 3 --- schedule:AnnouncementsCS 287: Advanced RoboticsFall 2009Lecture 24: SLAMPieter AbbeelUC Berkeley EECSPage 23Types of SLAM-Problems Grid maps or scans[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…] Landmark-based[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;… State variables:  Robot pose Coordinates of each of the landmarks Robot dynamics model: P(xt+1| xt, ut) Sensor model: P(zt+1| xt, m) Probability of landmark observations given the state Can run EKF, SEIF, various other approaches Result: path of robot, location of landmarksKF-type approaches are a good fit b/c they can keep track of correlations between landmarksNote: Could then use path of robot + sensor log and build a map assuming known robot posesRecap Landmark based SLAMPage 36 Can we solve the SLAM problem if no pre-defined landmarks are available? As with landmarks, the map depends on the poses of the robot during data acquisition If the poses are known, grid-based mapping is easy (“mapping with known poses”)Grid-based SLAM7Occupancy Grid Maps Introduced by Moravec and Elfes in 1985 Represent environment by a grid. Estimate the probability that a location is occupied by an obstacle. Key assumptions Occupancy of individual cells (m[xy]) is independent Robot positions are known!∏==−yxxytttttmBelzuzumPmBel,][121)(),,,|()( KPage 48z+d1z+d2z+d3zz-d1Occupancy Value Depending on the Measured DistancelogP (m[xy]= 1)P (m[xy]= 0)← logP (m[xy]= 1)P (m[xy]= 0)+ logP (m[xy]= 1|zt)P (m[xy]= 0|zt)9Incremental Updating of Occupancy Grids (Example)Page 510Alternative: Simple Counting For every cell count hits(x,y): number of cases where a beam ended at <x,y> misses(x,y): number of cases where a beam passed through <x,y> Value of interest: P(reflects(x,y))),misses(),hits(),hits()(][yxyxyxmBelxy+=12Difference between Occupancy Grid Maps and Counting The counting model determines how often a cell reflects a beam. The occupancy model represents whether or not a cell is occupied by an object. Although a cell might be occupied by an object, the reflection probability of this object might be very small.Page 613Example Occupancy Map14Example Reflection Mapglass panesPage 715Example Out of 1000 beams only 60% are reflected from a cell and 40% intercept it without ending in it. Accordingly, the reflection probability will be 0.6. Suppose p(occ | z) = 0.55 when a beam ends in a cell and p(occ | z) = 0.45 when a cell is intercepted by a beam that does not end in it. Accordingly, after n measurements we will have  Whereas the reflection map yields a value of 0.6, the occupancy grid value converges to 1.2.0*4.0*6.0*4.0*6.0*911911*91155.045.0*45.055.0nnnnn==−16Mapping using Raw OdometryPage 8 Standard particle filter represents the distribution by a set of samplesDistribution over robot poses and maps18Rao-BlackwellizationFactorization first introduced by Murphy in 1999poses mapobservations & movementsPage 919Rao-BlackwellizationSLAM posteriorRobot path posteriorMapping with known posesFactorization first introduced by Murphy in 1999poses mapobservations & movements20Rao-BlackwellizationThis is localization, use MCLUse the pose estimate from the MCL part and apply mapping with known posesPage 1022Rao-Blackwellized Mapping Each particle represents a possible trajectory of the robot Each particle maintains its own map and updates it upon “mapping with known poses” Each particle survives with a probability proportional to the likelihood of the observations relative to its own map23Particle Filter Examplemap of particle 1map of particle 3map of particle 23 particlesPage 1124Problem Each map is quite big in case of grid maps Since each particle maintains its own map Therefore, one needs to keep the number of particles small Solution:Compute better proposal distributions! Idea:Improve the pose estimate before applying the particle filter25Pose Correction Using Scan MatchingMaximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map{})ˆ,|( )ˆ ,|( maxargˆ111 −−−⋅=ttttttxtxuxpmxzpxtrobot motioncurrent measurementmap constructed so farPage 1226Motion Model for Scan MatchingRaw OdometryScan Matching30FastSLAM with Scan-MatchingMap: Intel Research Lab SeattlePage 13Map of the Intel Lab 15 particles four times faster than real-timeP4, 2.8GHz 5cm resolution during scan matching 1cm resolution in final map32FastSLAM with Scan-MatchingLoop ClosurePage 1433Scan matching: likelihood fieldMapmLikelihood field=map convolved with a Gaussian34Scan Matching Extract likelihood field from scan and use it to match different scan.Page 15 Rao-Blackwellized representation: Particle instantiates entire path of robot Map associated with each path Scan matching: improves proposal distribution Original FastSLAM: Map associated with each particle was a Gaussian distribution over landmark positions DP-SLAM: extension which has very efficient map management, enabling having a relatively large number of particles [Eliazar and Parr, 2002/2005]FastSLAM recap Landmark based vs. occupancy grid Probability distribution representation: EKF vs. particle filter vs. Rao-Blackwellized particle filter EKF, SEIF, FastSLAM are all “online” Currently popular 4thalternative: GraphSLAMSLAM thus farPage 16Graph-based Formulation Use a graph to represent the problem Every node in the graph corresponds to a pose of the robot during mapping Every edge between two nodes corresponds to the spatial constraints between them Goal: Find a configuration of the nodes that minimize the errorintroduced by the constraintsJGraphSLAM= x⊤0Ω0x0+t(xt− g(ut, xt−1))⊤R−1t(xt− g(ut, xt−1))+ti(zit− h(xt, m, cit))⊤Q−1t(zit− h(xt, m, cit))The KUKA Production SitePage 17The KUKA Production SiteThe KUKA Production Sitescans 59668total acquisition time 4,699.71 secondstraveled distance 2,587.71 meterstotal rotations 262.07 radianssize 180 x 110 metersprocessing time < 30 minutesPage 18GraphSLAMVisual SLAM for Flying