Simultaneous Localization & Mapping

1 Simultaneous Localization & MappingF1/10thAutonomous Racing Paril JainPrevious Week2 IMU and LIDARL ocalizationPID ControlLimitations : Basic Path Planning High Level Path Assignments 2ndright, 2ndright, 1stright, 1stleft, 1stright 3 Race Lines4 Limitations : No Future Information5 System Overview6 MappingLocalizationPath PlanningControlSystem Overview7 Hector SLAMM appingLocalizationPath PlanningControlSLAM : A Chicken-Egg problem8 MAPLOCALIZATIONLOCALIZATIONMAPO verview of SLAM9 Video provided separately Car running in corridor Map being generated Video will be used for explaining the overview10 Occupancy Grid Mapping11 Occupied CellFree CellUn-Explored CellMeasurement ModelOccupancy Grid Mapping Measurement : mx,y= 1 LiDAR hitmx,y= 0No occlusion12 Occupied CellFree CellUn-Explored CellMeasurement ModelOccupancy Grid Mapping Measurement : mx,y= 1 LiDAR hitmx,y= 0No occlusion13 Occupied CellFree CellUn-Explored Cell Map Cell:Z = 1 OccupiedZ = 0 UnExploredZ = -1 FreeMeasurement ModelOccupancy Grid Mapping Measurement.

2 Mx,y= 1 LiDAR hitmx,y= 0No occlusion14 Occupied CellFree CellUn-Explored Cell Map Cell:Z = 1 OccupiedZ = 0 UnExploredZ = -1 Free Measurement Model :Measurement ModelOccupancy Grid Mapping15 Occupied CellFree CellUn-Explored CellLog Probability for occupied cellsLog Probability for free cellsOccupancy Grid MappingMap Update : Cells with z = 1:olog odd= log odd+ log odd_occ Cells with z = -1:olog odd= log odd-log odd_freeOccupancy Grid Mapping17 Map Update : Cells with z = 1:olog odd= log odd+ log odd_occ Cells with z = -1:olog odd= log odd-log odd_free Threshold the cell values by upper /lowerlimitto avoid being completely certainOccupancy Grid Mapping18 Map Update : Cells with z = 1:olog odd= log odd+ log odd_occ Cells with z = -1:olog odd= log odd-log odd_free Threshold the cell values by upper /lowerlimitto avoid being completely odds probabilityOccupancy Grid Mapping19 Map Update : Cells with z = 1:olog odd= log odd+ log odd_occ Cells with z = -1.

3 Olog odd= log odd-log odd_free Threshold the cell values by upper /lowerlimitto avoid being completely odds probabilitySaturation LimitRegistering the first Scan20 Registering the first Scan21 Scan Matching22 Laser Scans car at Time t = t1 Pose of the Car at t = t1 Scan Matching23 Laser Scans car at Time t = t1 Laser Scans car at Time t = t2 Pose of the Car at t = t1 Scan Matching24 Laser Scans car at Time t = t1 Laser Scans car at Time t = t2 Pose of the Car at t = t1 Pose of the Car at t=t2 Scan Matching25 Source : Mathworks File Exchange : Iterative Closest Point PackageIterative Closest PointScan Matching26 Source : Mathworks File Exchange : Iterative Closest Point Package Minimize Root Mean Squared Distance between Point CloudsIterative Closest PointScan Matching27 Source : Mathworks File Exchange : Iterative Closest Point Package Minimize Root Mean Squared Distance between Point Clouds Find R & T matrix for transformed Point Cloud original Closest Point , =argmin , 2 =1 ( i+ , )2 Scan Matching28 Source : Mathworks File Exchange : Iterative Closest Point Package Minimize Root Mean Squared Distance between Point Clouds Find R & T matrix for transformed Point Cloud original Closest Point , =argmin , 2 =1 ( i+ , )2 Scan Matching29 Source : Mathworks File Exchange.

4 Iterative Closest Point Package Minimize Root Mean Squared Distance between Point Clouds Find R & T matrix for transformed Point Cloud original Closest Point , =argmin , 2 =1 ( i+ , )2 Scan matching: Hector SlamS2S1yx30 Scan matching: Hector SlamS2S1yx31 Robot PoseScan matching: Hector SlamS2S1 Impact coordinates of ithscan in world frameTotal of n scansyx32 Robot PoseScan matching: Hector SlamS2S1 Map Value at coordinates given by SiImpact coordinates of ithscan in world frameTotal of n scansyx33 Robot PoseScan matching: Hector Slam34 Scan matching: Hector Slam35 Taylor Expansion of Function MScan matching: Hector Slam36 Taylor Expansion of Function MScan matching: Hector Slam37 Taylor Expansion of Function MSolving for yields Gauss-Newton EquationEvaluation of Gauss-Newton equation gives a step that minimizes the objective function Raw LiDAR Scans38 BaseframeAxesScans after transforming by at each stage39 MapframeAxesMap Update40 Multi-Resolution Map Representation4120 cm Grid Cell10 cm Grid Cell5 cm Grid CellSaving the map42 Saving the map43 ROS Package called MAP Server Allows saving a map currently being published over /map topic Save the map:rosrunmap_servermap_saver[-f mapname] Load the map.

5 Rosrunmap_servermap_server< >OdometryUsing Hector Mapping44 OdometryUsing Hector Mapping Using Hector Slam for measuring , while discarding the map45 OdometryUsing Hector Mapping Using Hector Slam for measuring , while discarding the map Optional Approach : CSM (Canonical Scan Matcher) by Andrea Censi Scan matching between 2 scans 46 System Tftree47 Map FrameOdom FrameBase FrameLaser FrameTfProvided by Hector dometryTfrequired by Hector packageParameters for Hector SLAM : ROS map resolution-Grid resolution map_update_distance_thresh-minimum distance to be travelled before having a map update map_update_angle_thresh-minimum angle to be travelled before a map update laser_max_dist-Laser sensor specification update_factor_free-Log odds probability for occupied cells update_factor_occupied-Log odds probability for free cells48 Next Lecture Using the map generated today Localizing using Adaptive Monte Carlo Localization (AMCL) Integrating hector odometryand AMCL49