[ICRA2024] MegaParticles: Range-Based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter

MegaParticles: Range-Based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter
Kenji Koide, Shuji Oishi, Masashi Yokozuka, and Atsuhiko Banno
National Institute of Advanced Industrial Science Technology (AIST)
IEEE International Conference on Robotics and Automation (ICRA2024)

Transcript

1. 1 Range-Based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter Kenji Koide，Shuji Oishi，Masashi Yokozuka，and Atsuhiko Banno National Institute of Advanced Industrial Science Technology (AIST)
2. 2 動画
3. 3 Proposal : MegaParticles Process a massive amount of particles (10242=1M) with GPU parallel processing The powerful state distribution representation realizes: Technical key points: - Global localization without any prior information - Extreme robustness to state ambiguity • Efficient state sampling with Stein Variational Gradient Descent (SVGD) • Speed up SVGD using neighbor particle search with Locality Sensitive Hashing (LSH) 𝑂 𝑁2 → 𝑂 𝑁 • Posterior propagation on nearest particle graph for representative state extraction
4. 4 Outline Motion prediction & state update Fitting samples to likelihood with SVGD Prediction step Correction step Posterior estimation on neighbor particle graph Representative state extraction • Each particle represents a hypothesis of the sensor pose Pose hypothesis Particle Particle set • Iteratively update the set of particles through prediction and correction steps • A single representative state is extracted from the set of particles • The state distribution is represented as a set of particles
5. 5 Outline Motion prediction & state update Fitting samples to likelihood with SVGD Prediction step Correction step Posterior estimation on neighbor particle graph Representative state extraction • Each particle represents a hypothesis of the sensor pose Pose hypothesis Particle Particle set • Iteratively update the set of particles through prediction and correction steps • A single representative state is extracted from the set of particles • The state distribution is represented as a set of particles
6. 6 Correction step : Fitting particles to likelihood function Resampling (Conventional PF) [Kitagawa, 1993][Gordon, 1993] SVGD (64 particles) Resampling (1024 particles) Samples a new particle set based on the likelihoods of particles Stein Variational Gradient Descent (SVGD) [Liu+, 2016] Minimizes KLD between particles and true likelihood distribution using likelihood gradient and particle neighborhood relationship Inefficient state sampling (Needs many particles) Sample impoverishment problem (Degraded global estimation) Efficient state sampling with fewer particles Free from sample impoverishment problem (Keeps all particles alive) [Arulampalam+, 2002]
7. 7 Stein Variational Gradient Descent (SVGD) Next Current Update
8. 8 Stein Variational Gradient Descent (SVGD) Next Current Update Update Kernel＝Weight based on distance Gradient of log likelihood ＝Attractive force toward the mode Gradient of kernel ＝Repulsive force between particles Repulsive force + Optimization result All particles Attractive force Good sampling efficiency & diversity Slow convergence & 𝑂 𝑁2 computation
9. 9 Approximated Gauss-Newton SVGD (Proposed) Approximated Gauss-Newton SVGD Use only M=20 neighbor particles (k≒0 for far particles) → Converges in a few iterations Gauss-Newton-based update All particles Grad of likelihood (Attractive force) Grad of kernel (Repulsive force) Original SVGD [Liu+, 2016] Kernel → 𝑂 1 for each sample
10. 10 Approximated Gauss-Newton SVGD (Proposed) Approximated Gauss-Newton SVGD → Converges in a few iterations Gauss-Newton-based update All particles Grad of likelihood (Attractive force) Grad of kernel (Repulsive force) Original SVGD [Liu+, 2016] Kernel Iterative and probabilistic kNN with Locality Sensitive Hashing on SE3 Particle pose Quantization grid Random for each table Grid scale Gaussian noise Random for each sample Integer coordinate on the grid → Hash value Use only M=20 neighbor particles (k≒0 for far particles) → 𝑂 1 for each sample
11. 11 Outline Motion prediction & state update Fitting samples to likelihood with SVGD Prediction step Correction step Posterior estimation on neighbor particle graph Representative state extraction • Each particle represents a hypothesis of the sensor pose Pose hypothesis Particle Particle set • Iteratively update the set of particles through prediction and correction steps • A single representative state is extracted from the set of particles • The state distribution is represented as a set of particles
12. 12 Particle posterior estimation Explicitly estimate the posterior probability of each particle Particle-wise Bayes estimation + Random walk smoothing Re-use the neighbor particle list used in the approximated SVGD for efficiency Initial posterior Likelihood Prior Posterior smoothing on neighbor particle graph Kernel Smoothed posterior Neighbors Posterior Kernel Select the largest posterior particle as the representative state
13. 13 Experiments
14. Indoor experiment：Easy01 & Easy02 sequences Superposition of three position hypotheses Two attitude hypothesis (upright vs upside down) Attitude ambiguity resolved Posterior distribution (visualized by kernel density estimation) Particle set (colored by posterior) Extremely powerful ambiguity representation and global localization ability Z+ Z-
15. Kidnapping experiments Recovered from 8 kidnapping situations State space：280 m × 200 m × 30 m × SO3 Recovered from 6 kidnapping situations State space：50 m × 35 m × 5 m × SO3 Data recorded one month after the mapping (Dynamic objects and vegetation changes)
16. 16 Conclusion Dataset：https://zenodo.org/records/10122133 6-DoF Monte Carlo Localization with GPU acceleration • Process 1M (1024^2) particles in real-time with GPU • Efficient state sampling with Stein Variational Gradient Descent (SVGD) • Locality Sensitive Hashing (LSH)-based scalable neighbor particle search • Posterior probability propagation on neighbor particle graph