Simultaneous Localization & Mapping

F1/10th Autonomous Racing

Paril Jain

Previous Week

2

IMU and LIDAR

Localization PID Control

Limitations : Basic Path Planning

• High Level Path Assignments

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

3

Race Lines

4

Limitations : No Future Information

5

System Overview

6

Mapping

Localization

Path Planning

Control

System Overview

7

Hector SLAMMapping

Localization

Path Planning

Control

SLAM : A Chicken-Egg problem

8

MA

P

LOCALIZATION

LOCALIZATION

MA

P

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

11

Occupied Cell

Free Cell

Un-Explored Cell

Measurement Model

Occupancy Grid Mapping

• Measurement :

mx,y = 1 LiDAR hit

mx,y = 0 No occlusion

12

Occupied Cell

Free Cell

Un-Explored Cell

Measurement Model

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

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

Occupancy Grid Mapping

15

Occupied Cell

Free Cell

Un-Explored Cell

Log Probability for occupied cells Log Probability for free cells

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

17

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

Occupancy Grid Mapping

18

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

0.5

1

log odds probability

Occupancy Grid Mapping

19

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

0.5

1

log odds probability

Saturation Limit

Registering the first Scan

20

Registering the first Scan

21

Scan Matching

22

Laser Scans w.r.t car at Time t = t1

Pose of the Car at t = t1

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

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

Scan Matching

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

Iterative Closest Point

Scan Matching

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

• Minimize Root Mean Squared

Distance between Point Clouds

Iterative Closest Point

Scan Matching

27Source : 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

Scan Matching

28Source : 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

Scan Matching

29Source : 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

Scan matching: Hector Slam

S2

S1

y

x

30

Scan matching: Hector Slam

S2

S1

y

x

31

Robot Pose

Scan matching: Hector Slam

S2

S1

Impact coordinates of ith scan in world frame

Total of n scans

y

x

32

Robot Pose

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

Scan matching: Hector Slam

34

Scan matching: Hector Slam

35

Taylor Expansion of

Function M

Scan matching: Hector Slam

36

Taylor Expansion of

Function M

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

Raw LiDAR Scans

38Baseframe Axes

Scans after transforming by ∆ξ at each

stage

39Mapframe Axes

Map Update

40

Multi-Resolution Map Representation

41

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

Saving the map

42

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>

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

47

Map Frame

Odom Frame

Base Frame

Laser Frame

Tf Provided by

Hector dometry

Tf required by

Hector package

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