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
Recommend
More recommend
