Simultaneous Localization & Mapping Paril Jain Previous Week - PowerPoint PPT Presentation

F1/10 th Autonomous Racing Simultaneous Localization & Mapping Paril Jain

Previous Week Localization IMU and PID Control LIDAR 2

Limitations : Basic Path Planning • High Level Path Assignments – 2 nd right, 2 nd right, 1 st right, 1 st left, 1 st right 3

Race Lines 4

Limitations : No Future Information 5

System Overview Mapping Localization Path Planning Control 6

System Overview Mapping Hector SLAM Localization Path Planning Control 7

SLAM : A Chicken-Egg problem MA LOCALIZATION P MA LOCALIZATION P 8

Overview of SLAM 9

Video provided separately • Car running in corridor • Map being generated • Video will be used for explaining the overview 10

Occupancy Grid Mapping Measurement Model Occupied Cell Free Cell Un-Explored Cell 11

Occupancy Grid Mapping Measurement Model • Measurement : m x,y = 1 LiDAR hit Occupied Cell Free Cell m x,y = 0 No occlusion Un-Explored Cell 12

Occupancy Grid Mapping Measurement Model • Measurement : m x,y = 1 LiDAR hit Occupied Cell Free Cell m x,y = 0 No occlusion • Map Cell: Un-Explored Cell Z = 1 Occupied Z = 0 UnExplored Z = -1 Free 13

Occupancy Grid Mapping Measurement Model • Measurement : m x,y = 1 LiDAR hit Occupied Cell Free Cell m x,y = 0 No occlusion • Map Cell: Un-Explored Cell Z = 1 Occupied Z = 0 UnExplored Z = -1 Free • Measurement Model : 14

Occupancy Grid Mapping Occupied Cell Free Cell Un-Explored Cell Log Probability for occupied cells Log Probability for free cells 15

Occupancy Grid Mapping Map Update : • Cells with z = 1: o log odd = log odd + log odd_occ • Cells with z = -1: o log odd = log odd - log odd_free

Occupancy Grid Mapping Map Update : • Cells with z = 1: o log odd = log odd + log odd_occ • Cells with z = -1: o log odd = log odd - log odd_free • Threshold the cell values by upper /lowerlimit to avoid being completely certain 17

Occupancy Grid Mapping Map Update : • Cells with z = 1: o log odd = log odd + log odd_occ • Cells with z = -1: o log odd = log odd - log odd_free • Threshold the cell values by upper /lowerlimit to avoid being completely certain 1 0.5 log odds probability 18

Occupancy Grid Mapping Map Update : • Cells with z = 1: o log odd = log odd + log odd_occ • Cells with z = -1: o log odd = log odd - log odd_free • Threshold the cell values by upper /lowerlimit to avoid being completely certain 1 0.5 Saturation Limit log odds probability 19

Registering the first Scan 20

Registering the first Scan 21

Scan Matching Pose of the Car at t = t1 Laser Scans w.r.t car at Time t = t 1 22

Scan Matching Pose of the Car at t = t1 Laser Scans w.r.t car at Time t = t 2 Laser Scans w.r.t car at Time t = t 1 23

Scan Matching Pose of the Car at t = t1 Pose of the Car at t=t2 Laser Scans w.r.t car at Time t = t 2 Laser Scans w.r.t car at Time t = t 1 24

Scan Matching Iterative Closest Point 25 Source : Mathworks – File Exchange : Iterative Closest Point Package

Scan Matching Iterative Closest Point • Minimize Root Mean Squared Distance between Point Clouds 26 Source : Mathworks – File Exchange : Iterative Closest Point Package

Scan Matching Iterative Closest Point • Minimize Root Mean Squared Distance between Point Clouds • Find R & T matrix for transformed Point Cloud w.r.t original cloud. 𝑜 2 𝑒𝑗𝑡𝑢(𝑆 ∗ 𝑆𝑓𝑒 i + 𝑈, 𝐶𝑚𝑣𝑓𝑗) 2 𝑆, 𝑈 = argmin ෍ 𝑆,𝑈 𝑗=1 27 Source : Mathworks – File Exchange : Iterative Closest Point Package

Scan Matching Iterative Closest Point • Minimize Root Mean Squared Distance between Point Clouds • Find R & T matrix for transformed Point Cloud w.r.t original cloud. 𝑜 2 𝑒𝑗𝑡𝑢(𝑆 ∗ 𝑆𝑓𝑒 i + 𝑈, 𝐶𝑚𝑣𝑓𝑗) 2 𝑆, 𝑈 = argmin ෍ 𝑆,𝑈 𝑗=1 28 Source : Mathworks – File Exchange : Iterative Closest Point Package

Scan Matching Iterative Closest Point • Minimize Root Mean Squared Distance between Point Clouds • Find R & T matrix for transformed Point Cloud w.r.t original cloud. 𝑜 2 𝑒𝑗𝑡𝑢(𝑆 ∗ 𝑆𝑓𝑒 i + 𝑈, 𝐶𝑚𝑣𝑓𝑗) 2 𝑆, 𝑈 = argmin ෍ 𝑆,𝑈 𝑗=1 29 Source : Mathworks – File Exchange : Iterative Closest Point Package

Scan matching: Hector Slam S 1 S 2 x y 30

Scan matching: Hector Slam Robot Pose S 1 S 2 x y 31

Scan matching: Hector Slam Robot Pose Impact coordinates of i th scan in world frame S 1 Total of n scans S 2 x y 32

Scan matching: Hector Slam Robot Pose Impact coordinates of i th scan in world frame S 1 Total of n scans S 2 x Map Value at coordinates given by S i y 33

Scan matching: Hector Slam 34

Scan matching: Hector Slam Taylor Expansion of Function M 35

Scan matching: Hector Slam Taylor Expansion of Function M 36

Scan matching: Hector Slam Taylor Expansion of Function M Solving for ∆ξ yields Gauss-Newton Equation Evaluation of Gauss-Newton equation gives a step ∆ξ that minimizes the objective function 37

Raw LiDAR Scans Baseframe Axes 38

Scans after transforming by ∆ξ at each stage Mapframe Axes 39

Map Update 40

Multi-Resolution Map Representation 20 cm Grid Cell 10 cm Grid Cell 5 cm Grid Cell 41

Saving the map 42

Saving the map • ROS Package called MAP Server • Allows saving a map currently being published over /map topic • Save the map: rosrun map_server map_saver [-f mapname] • Load the map: rosrun map_server map_server <name.yaml> 43

Odometry Using Hector Mapping 44

Odometry Using Hector Mapping • Using Hector Slam for measuring ∆ξ , while discarding the map 45

Odometry Using 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 Tf tree Map Frame Odom Frame Tf Provided by Hector dometry Base Frame Tf required by Hector package Laser Frame 47

Parameters 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 cells 48

Next Lecture • Using the map generated today • Localizing using Adaptive Monte Carlo localization (AMCL) • Integrating hector odometry and AMCL 49