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

▶
simultaneous localization mapping
SMART_READER_LITE
LIVE PREVIEW

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

Oct 29, 2022 1.26k likes •1.78k views

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,

slide-1
SLIDE 1

Simultaneous Localization & Mapping

F1/10th Autonomous Racing

Paril Jain

slide-2
SLIDE 2

Previous Week

2

IMU and LIDAR Localization PID Control

slide-3
SLIDE 3

Limitations : Basic Path Planning

  • High Level Path Assignments

– 2nd right, 2nd right, 1st right, 1st left, 1st right

3

slide-4
SLIDE 4

Race Lines

4

slide-5
SLIDE 5

Limitations : No Future Information

5

slide-6
SLIDE 6

System Overview

6

Mapping Localization Path Planning Control

slide-7
SLIDE 7

System Overview

7

Hector SLAM

Mapping Localization Path Planning Control

slide-8
SLIDE 8

SLAM : A Chicken-Egg problem

8

MA P

LOCALIZATION LOCALIZATION

MA P

slide-9
SLIDE 9

Overview of SLAM

9

slide-10
SLIDE 10

Video provided separately

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

10

slide-11
SLIDE 11

Occupancy Grid Mapping

11

Occupied Cell Free Cell Un-Explored Cell

Measurement Model

slide-12
SLIDE 12

Occupancy Grid Mapping

  • Measurement :

mx,y = 1 LiDAR hit mx,y = 0 No occlusion

12

Occupied Cell Free Cell Un-Explored Cell

Measurement Model

slide-13
SLIDE 13

Occupancy Grid Mapping

  • Measurement :

mx,y = 1 LiDAR hit mx,y = 0 No occlusion

13

Occupied Cell Free Cell Un-Explored Cell

  • Map Cell:

Z = 1 Occupied Z = 0 UnExplored Z = -1 Free

Measurement Model

slide-14
SLIDE 14

Occupancy Grid Mapping

  • Measurement :

mx,y = 1 LiDAR hit mx,y = 0 No occlusion

14

Occupied Cell Free Cell Un-Explored Cell

  • Map Cell:

Z = 1 Occupied Z = 0 UnExplored Z = -1 Free

  • Measurement Model :

Measurement Model

slide-15
SLIDE 15

Occupancy Grid Mapping

15

Occupied Cell Free Cell Un-Explored Cell

Log Probability for occupied cells Log Probability for free cells

slide-16
SLIDE 16

Occupancy Grid Mapping

Map Update :

  • Cells with z = 1:
  • log odd = log odd + log odd_occ
  • Cells with z = -1:
  • log odd = log odd - log odd_free
slide-17
SLIDE 17

Occupancy Grid Mapping

17

Map Update :

  • Cells with z = 1:
  • log odd = log odd + log odd_occ
  • Cells with z = -1:
  • log odd = log odd - log odd_free
  • Threshold the cell values by upper /lowerlimit to avoid being

completely certain

slide-18
SLIDE 18

Occupancy Grid Mapping

18

Map Update :

  • Cells with z = 1:
  • log odd = log odd + log odd_occ
  • Cells with z = -1:
  • log odd = log odd - log odd_free
  • Threshold the cell values by upper /lowerlimit to avoid being

completely certain

0.5 1

log odds probability

slide-19
SLIDE 19

Occupancy Grid Mapping

19

Map Update :

  • Cells with z = 1:
  • log odd = log odd + log odd_occ
  • Cells with z = -1:
  • log odd = log odd - log odd_free
  • Threshold the cell values by upper /lowerlimit to avoid being

completely certain

0.5 1

log odds probability

Saturation Limit

slide-20
SLIDE 20

Registering the first Scan

20

slide-21
SLIDE 21

Registering the first Scan

21

slide-22
SLIDE 22

Scan Matching

22

Laser Scans w.r.t car at Time t = t1 Pose of the Car at t = t1

slide-23
SLIDE 23

Scan Matching

23

Laser Scans w.r.t car at Time t = t1 Laser Scans w.r.t car at Time t = t2 Pose of the Car at t = t1

slide-24
SLIDE 24

Scan Matching

24

Laser Scans w.r.t car at Time t = t1 Laser Scans w.r.t car at Time t = t2 Pose of the Car at t = t1 Pose of the Car at t=t2

slide-25
SLIDE 25

Scan Matching

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

Iterative Closest Point

slide-26
SLIDE 26

Scan Matching

26 Source : Mathworks – File Exchange : Iterative Closest Point Package

  • Minimize Root Mean Squared

Distance between Point Clouds Iterative Closest Point

slide-27
SLIDE 27

Scan Matching

27 Source : Mathworks – File Exchange : Iterative Closest Point Package

  • Minimize Root Mean Squared

Distance between Point Clouds

  • Find R & T matrix for transformed

Point Cloud w.r.t original cloud. Iterative Closest Point

𝑆, 𝑈 = argmin

𝑆,𝑈

2

෍

𝑗=1 𝑜

𝑒𝑗𝑡𝑢(𝑆 ∗ 𝑆𝑓𝑒i + 𝑈, 𝐶𝑚𝑣𝑓𝑗)2

slide-28
SLIDE 28

Scan Matching

28 Source : Mathworks – File Exchange : Iterative Closest Point Package

  • Minimize Root Mean Squared

Distance between Point Clouds

  • Find R & T matrix for transformed

Point Cloud w.r.t original cloud. Iterative Closest Point

𝑆, 𝑈 = argmin

𝑆,𝑈

2

෍

𝑗=1 𝑜

𝑒𝑗𝑡𝑢(𝑆 ∗ 𝑆𝑓𝑒i + 𝑈, 𝐶𝑚𝑣𝑓𝑗)2

slide-29
SLIDE 29

Scan Matching

29 Source : Mathworks – File Exchange : Iterative Closest Point Package

  • Minimize Root Mean Squared

Distance between Point Clouds

  • Find R & T matrix for transformed

Point Cloud w.r.t original cloud. Iterative Closest Point

𝑆, 𝑈 = argmin

𝑆,𝑈

2

෍

𝑗=1 𝑜

𝑒𝑗𝑡𝑢(𝑆 ∗ 𝑆𝑓𝑒i + 𝑈, 𝐶𝑚𝑣𝑓𝑗)2

slide-30
SLIDE 30

Scan matching: Hector Slam

S2 S1

y x

30

slide-31
SLIDE 31

Scan matching: Hector Slam

S2 S1

y x

31

Robot Pose

slide-32
SLIDE 32

Scan matching: Hector Slam

S2 S1 Impact coordinates of ith scan in world frame Total of n scans

y x

32

Robot Pose

slide-33
SLIDE 33

Scan matching: Hector Slam

S2 S1 Map Value at coordinates given by Si Impact coordinates of ith scan in world frame Total of n scans

y x

33

Robot Pose

slide-34
SLIDE 34

Scan matching: Hector Slam

34

slide-35
SLIDE 35

Scan matching: Hector Slam

35

Taylor Expansion of Function M

slide-36
SLIDE 36

Scan matching: Hector Slam

36

Taylor Expansion of Function M

slide-37
SLIDE 37

Scan matching: Hector Slam

37

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

slide-38
SLIDE 38

Raw LiDAR Scans

38

Baseframe Axes

slide-39
SLIDE 39

Scans after transforming by ∆ξ at each stage

39

Mapframe Axes

slide-40
SLIDE 40

Map Update

40

slide-41
SLIDE 41

Multi-Resolution Map Representation

41

20 cm Grid Cell 10 cm Grid Cell 5 cm Grid Cell

slide-42
SLIDE 42

Saving the map

42

slide-43
SLIDE 43

Saving the map

43

  • 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>

slide-44
SLIDE 44

Odometry Using Hector Mapping

44

slide-45
SLIDE 45

Odometry Using Hector Mapping

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

45

slide-46
SLIDE 46

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

slide-47
SLIDE 47

System Tf tree

47

Map Frame Odom Frame Base Frame Laser Frame Tf Provided by Hector dometry Tf required by Hector package

slide-48
SLIDE 48

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

slide-49
SLIDE 49

Next Lecture

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

49