F1/10 th Autonomous Racing. Localization. Nischal K N

Transcription

1 F1/10 th Autonomous Racing Localization Nischal K N

2 System Overview Mapping Hector Mapping Localization Path Planning Control

3 System Overview Mapping Hector Mapping Localization Adaptive Monte Carlo Localization Path Planning Control

4 ? Where am I?????

5 ? Where am I????? Position & Orientation

6 Localization using Odometry Car Trajectory Initial Car Position Free Space Walls

7 Drawbacks of Localization using Wheel Odometry Wheel spin due to lack of traction

8 Drawbacks of Localization using Hector odometry Failed scan matching due to lack of features

9 Issue A mechanism to compensate the mistakes committed by odometry A solution robust to compensate for lack of information on initial position

10 Issue A mechanism to compensate the mistakes committed by odometry A solution robust to compensate for lack of information on initial position Solution: Monte Carlo Localization Alternate Solutions: Kalman Filter, Topological Markov Localization

11 Robot Particle Filter A Example in 1 Dimension Door Belief State

12 Robot Particle Filter A Example in 1 Dimension Door Direction of motion Belief State

13 Particle Filter At time t = 1 Robot A Example in 1 Dimension Door Direction of motion

14 Particle Filter At time t = 1 Robot A Example in 1 Dimension Door Direction of motion Measurement Model

15 Particle Filter At time t = 1 Robot A Example in 1 Dimension Door Direction of motion Measurement Model Belief State

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

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

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

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

20 Continuous State

21 Continuous State Discrete State

22 Continuous State Discrete State

23 Particle Filter in 2D

24 Particle Filter in 2D Odometry pose

25 Particle Filter in 2D

26 Scan Correlation

27 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2

28 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1

29 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1 Particle 2 S 2

30 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1 Particle 2 S 2 Particle 3 S 3

31 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1 Particle 2 S 2 Particle 3 S 3 Particle 4 S 4

32 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1 Particle 2 S 2 Particle 3 S 3 Particle 4 S 4 Particle 5 S 5

33 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1 Particle 2 S 2 Particle 3 S 3 Particle 4 S 4 Particle 5 S 5 Particle 6 S 6

34 Scan Correlation S = σ m σ n (Amn A)(Bmn B) σ m σ n A mn A 2 σ m σ n B mn B 2 Particle Weight Particle 1 S 1 Particle 2 S 2 Particle 3 S 3 Particle 4 S 4 Particle 5 S 5 Particle 6 S 6

35 Localization using Odometry Particle Filter

36 Update step Update the particle cloud with the update in position from the odometry Repeat Scan matching process for each particle and determine the weights.

37 Update step 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.

38 Update step 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. Particle Weights W t W t 1 S

39 Weights Particle Filter without Resampling Particles

40 Resampling Original Particles After N iterations Resampling

41 Resampling Original Particles After N iterations Resampling

42 Resampling Original Particles After N iterations Resampling

43 Particles

44 Weights Particle filter with Resampling Particles

45 Kullback Leibler divergence (KLD Sampling) 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

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

47 Tf tree Where does AMCL fit in world_frame map odom base_frame

48 Tf tree Where does AMCL fit in world_frame map odom Odometry (Hector) base_frame

49 Tf tree Where does AMCL fit in world_frame map odom base_frame Odometry Drift Odometry (AMCL) (Hector)

50 Input and Output Parameters

51 Input and Output Parameters Input Parameters: 1. Laser Scan

52 Input and Output Parameters Input Parameters: 1. Laser Scan 2. Dead Reckoning/Odometry

53 Input and Output Parameters Input Parameters: 1. Laser Scan 2. Dead Reckoning/Odometry 3. Map

54 Input and Output Parameters Input Parameters: 1. Laser Scan 2. Dead Reckoning/Odometry 3. Map Output Parameters: 1. AMCL pose

55 Input and Output Parameters Input Parameters: 1. Laser Scan 2. Dead Reckoning/Odometry 3. Map Output Parameters: 1. AMCL pose 2. Particle Cloud

56 Video of AMCL particles

57 min_particles Default: 100 AMCL Parameters The minimum number of particles to be used for calculating correlation max_particles Default: 500 The maximum number of particles to be used for calculating correlation

58 update_min_d Default: 0.2m AMCL Parameters The minimum translation movement required by the vehicle before an pose update is published update_min_a Default: π Τ6 radians The minimum angular movement required by the vehicle before an pose update is published

59 AMCL Parameters 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

60 AMCL Parameters initial_cov_xx Default: 0 initial_cov_yy Default: 0 initial_cov_aa Default: 0 The covariance of particles distributed around the mean

61 What Next? Path Planning and Trajectory Generation Cost Maps Control Algorithms For Navigation