Localization Nischal K N System Overview Mapping Hector Mapping - - PowerPoint PPT Presentation

β–Ά
localization
SMART_READER_LITE
LIVE PREVIEW

Localization Nischal K N System Overview Mapping Hector Mapping - - PowerPoint PPT Presentation

Dec 05, 2023 308 likes β€’923 views

F1/10 th Autonomous Racing Localization Nischal K N System Overview Mapping Hector Mapping Localization Path Planning Control System Overview Mapping Hector Mapping Adaptive Monte Localization Carlo Localization Path Planning

slide-1
SLIDE 1

Localization

F1/10th Autonomous Racing Nischal K N

slide-2
SLIDE 2

Hector Mapping

Mapping Localization Path Planning Control

System Overview

slide-3
SLIDE 3

Adaptive Monte Carlo Localization Hector Mapping

Mapping Localization Path Planning Control

System Overview

slide-4
SLIDE 4

Where am I ???

? ? ?

slide-5
SLIDE 5

Where am I ???

? ? ?

Position & Orientation

slide-6
SLIDE 6

Localization using Odometry

Walls Free Space Initial Car Position Car Trajectory

slide-7
SLIDE 7

Drawbacks of Localization using Wheel Odometry

Wheel spin due to lack of traction

slide-8
SLIDE 8

Drawbacks of Localization using Hector odometry

Failed scan matching due to lack of features

slide-9
SLIDE 9
  • A mechanism to compensate the mistakes committed by odometry
  • A solution robust to compensate for lack of information on initial

position

Issue

slide-10
SLIDE 10
  • A mechanism to compensate the mistakes committed by odometry
  • A solution robust to compensate for lack of information on initial

position

Issue

Solution: Monte Carlo Localization

Alternate Solutions: Kalman Filter, Topological Markov Localization

slide-11
SLIDE 11

Particle Filter

A Example in 1 Dimension

Robot Door

Belief State

slide-12
SLIDE 12

Particle Filter

A Example in 1 Dimension

Robot Door Direction of motion

Belief State

slide-13
SLIDE 13

Particle Filter

A Example in 1 Dimension

Robot Door

At time t = 1

Direction of motion

slide-14
SLIDE 14

Particle Filter

A Example in 1 Dimension

Robot Door

Measurement Model At time t = 1

Direction of motion

slide-15
SLIDE 15

Particle Filter

A Example in 1 Dimension

Robot Door

Measurement Model Belief State At time t = 1

Direction of motion

slide-16
SLIDE 16

At time t = 2, robot moves forward a certain distance

slide-17
SLIDE 17

Motion Model update At time t = 2, robot moves forward a certain distance

slide-18
SLIDE 18

Motion Model update Measurement model At time t = 2, robot moves forward a certain distance

slide-19
SLIDE 19

Belief State Motion Model update Measurement model At time t = 2, robot moves forward a certain distance

slide-20
SLIDE 20

Continuous State

slide-21
SLIDE 21

Continuous State Discrete State Discrete State

slide-22
SLIDE 22

Continuous State Discrete State

slide-23
SLIDE 23

Particle Filter in 2D

slide-24
SLIDE 24

Particle Filter in 2D

Odometry pose

slide-25
SLIDE 25

Particle Filter in 2D

slide-26
SLIDE 26

Scan Correlation

slide-27
SLIDE 27

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

Scan Correlation

slide-28
SLIDE 28

Particle Weight Particle 1 S1

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-29
SLIDE 29

Particle Weight Particle 1 S1 Particle 2 S2

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-30
SLIDE 30

Particle Weight Particle 1 S1 Particle 2 S2 Particle 3 S3

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-31
SLIDE 31

Particle Weight Particle 1 S1 Particle 2 S2 Particle 3 S3 Particle 4 S4

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-32
SLIDE 32

Particle Weight Particle 1 S1 Particle 2 S2 Particle 3 S3 Particle 4 S4 Particle 5 S5

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-33
SLIDE 33

Particle Weight Particle 1 S1 Particle 2 S2 Particle 3 S3 Particle 4 S4 Particle 5 S5 Particle 6 S6

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-34
SLIDE 34

Particle Weight Particle 1 S1 Particle 2 S2 Particle 3 S3 Particle 4 S4 Particle 5 S5 Particle 6 S6

Scan Correlation

𝑇 = σ𝑛 Οƒπ‘œ(π΅π‘›π‘œ βˆ’ 𝐡)(πΆπ‘›π‘œ βˆ’ 𝐢) σ𝑛 Οƒπ‘œ π΅π‘›π‘œ βˆ’ 𝐡

2

σ𝑛 Οƒπ‘œ πΆπ‘›π‘œ βˆ’ 𝐢

2

slide-35
SLIDE 35

Odometry Particle Filter Localization using

slide-36
SLIDE 36
  • Update the particle cloud with

the update in position from the odometry

  • Repeat Scan matching

process for each particle and determine the weights.

Update step

slide-37
SLIDE 37

Odometry update

  • Update the particle cloud with

the update in position from the odometry

  • Repeat Scan matching

process for each particle and determine the weights.

Update step

slide-38
SLIDE 38

Particle Weights

𝑋𝑒 ← 𝑋𝑒 βˆ’ 1 Γ— 𝑇

Odometry update

  • Update the particle cloud with

the update in position from the odometry

  • Repeat Scan matching

process for each particle and determine the weights.

Update step

slide-39
SLIDE 39

Particle Filter without Resampling

Particles Weights

slide-40
SLIDE 40

Resampling

Original Particles After N iterations Resampling

slide-41
SLIDE 41

Resampling

Original Particles After N iterations Resampling

slide-42
SLIDE 42

Resampling

Original Particles After N iterations Resampling

slide-43
SLIDE 43

Particles

slide-44
SLIDE 44

Particles Weights

Particle filter with Resampling

slide-45
SLIDE 45
  • Variable Particle size
  • Sample size is proportional to error between odometry position

and sample based approximation

  • i.e smaller sample size when particles have converged

Kullback–Leibler divergence (KLD Sampling)

slide-46
SLIDE 46

Particle Filters in ROS

  • Adaptive Monte Carlo Localization Package
  • Localization for a robot moving in a 2D space
  • Localizes against a pre-existing map
slide-47
SLIDE 47

Tf tree – Where does AMCL fit in

world_frame map

  • dom

base_frame

slide-48
SLIDE 48

Tf tree – Where does AMCL fit in

world_frame map

  • dom

base_frame

Odometry (Hector)

slide-49
SLIDE 49

Tf tree – Where does AMCL fit in

world_frame map

  • dom

base_frame

Odometry (Hector) Odometry Drift (AMCL)

slide-50
SLIDE 50

Input and Output Parameters

slide-51
SLIDE 51

Input Parameters:

  • 1. Laser Scan

Input and Output Parameters

slide-52
SLIDE 52

Input Parameters:

  • 1. Laser Scan
  • 2. Dead Reckoning/Odometry

Input and Output Parameters

slide-53
SLIDE 53

Input Parameters:

  • 1. Laser Scan
  • 2. Dead Reckoning/Odometry
  • 3. Map

Input and Output Parameters

slide-54
SLIDE 54

Input Parameters:

  • 1. Laser Scan
  • 2. Dead Reckoning/Odometry
  • 3. Map

Output Parameters:

  • 1. AMCL pose

Input and Output Parameters

slide-55
SLIDE 55

Input Parameters:

  • 1. Laser Scan
  • 2. Dead Reckoning/Odometry
  • 3. Map

Output Parameters:

  • 1. AMCL pose
  • 2. Particle Cloud

Input and Output Parameters

slide-56
SLIDE 56

Video of AMCL particles

slide-57
SLIDE 57

min_particles

Default: 100 The minimum number of particles to be used for calculating correlation

AMCL Parameters max_particles

Default: 500 The maximum number of particles to be used for calculating correlation

slide-58
SLIDE 58

update_min_d

Default: 0.2m The minimum translation movement required by the vehicle before an pose update is published

AMCL Parameters update_min_a

Default: Ξ€ Ο€ 6 radians The minimum angular movement required by the vehicle before an pose update is published

slide-59
SLIDE 59

initial_pose_x

Default: 0

initial_pose_y

Default: 0

initial_pose_a

Default: 0 The initial mean position of the particles to initialize the particle filter

AMCL Parameters

slide-60
SLIDE 60

initial_cov_xx

Default: 0

initial_cov_yy

Default: 0

initial_cov_aa

Default: 0 The covariance of particles distributed around the mean

AMCL Parameters

slide-61
SLIDE 61

What Next?

  • Path Planning and Trajectory Generation
  • Cost Maps
  • Control Algorithms For Navigation