FAST-SLAM : Feature Based SLAM with Particle Filters

FAST SLAM 은 EKF-SLAM 과 비슷하게 Feature 를 이용해서 map 을 작성하는 SLAM 방식입니다. 다만 EKF 와는 달리, pose 의 가정을 Gaussian 으로 하지 않고 파티클 필터를 사용합니다. 

이전 Monte-Carlo Localization 에서 state 로 두었던 것은 1차원, 혹은 2차원 상에서의 로봇의 pose 였습니다. 그런데 우리가 다루는 환경은 3차원이고, 차원의 갯수가 늘어날 수록 파티클 갯수도 지수적으로 증가하게 됩니다. 따라서 파티클 필터는 파티클의 갯수가 많지 않은 low-dimension 에서 잘 작동하는 것입니다. 

 

그래서 EKF SLAM 에서처럼 state 를 정의한다면 너무나 많은 파티클들이 필요해집니다. 이것이 랜드마크까지 포함하고 있는 state 에 파티클 필터를 직접 적용하기 어려운 이유입니다. (dimensionality problem)

 

그러면 High Dimension 에서는 어떻게 접근하면 좋을까요 ? 

모든 요소에 다 파티클 필터를 적용하는 것이 아니라, 로봇의 pose 에 대해서만 파티클 필터를 적용하자는 아이디어가 있을 수 있습니다. 

path (포즈의 집합) 에 대해서만 파티클 필터를 적용하고, 랜드마크들은 pose 에 dependent 하기 때문에 따로 계산을 하자는 것이 핵심 아이디어입니다. 

Rao-Blackwellization

 

Rao-Blackwellization 은 FAST SLAM 관련 논문에서 자주 나오는 기법입니다. 이는 우리가 동시에 추정하기 어려운 어떤 a,b 에 대한 joint probability 를 conditional probability 와 prior 로 나타내는 방식을 말합니다. 

 

a 와 b 를 동시에 추정하는 것은 어렵지만 a 의 확률을 추정하고 a 가 주어졌을 때 b 의 확률을 추정하는 것은 상대적으로 쉽습니다. 물론 같은 것이지만, 이런 단계를 거쳐서 추정하는 것이 좀 더 쉬운 경우가 있다는 것입니다. 

SLAM 에 Rao-Blackwellization 을 적용하면 다음과 같습니다. 

 

그러면 M 개의 랜드마크가 있을 때, 이들에 대한 확률 분포는 어떻게 구할 수 있을까요 ?

이 상황에서 모든 x 값을 알고 있다고 가정하면 모든 m 들은 서로 independent 하게 됩니다. 따라서 각각을 구해서 이들의 확률을 곱하는 것이 전체 랜드마크들에 대한 확률이 됩니다. 

 

이 확률 분포를 추정할 때는 다시 EKF 를 사용하게 되는데, 이는 랜드마크들에 대한 확률 분포를 추정할 때 다시 가우시안으로 표현한다는 것입니다. 로봇의 pose 는 파티클 필터를 쓰지만, 랜드마크 하나 하나에 대해서는 가우시안 확률 분포를 따른다고 다시 가정하게 됩니다. 

 

이런 방식의 장점은, EKF 에서는 랜드마크 사이의 covariance 도 independent 하다고 할 수 없었기 때문에 100개의 랜드마크를 사용하면 100x100의 covariance matrix 가 필요했지만, 여기에서는 (랜드마크들이 conditionally independent 하다는 조건이 있다면) 각각의 랜드마크들에 대한 확률만 구하면 된다는 것입니다. 

 

따라서 이런 경우에는 2 dimensional EKF 가 사용됩니다. 

 

정리를 하자면, FAST SLAM 은 로봇의 pose 에 대해서는 파티클 필터를 사용하고, 지금 추정하고 있는 랜드마크들의 위치는 모든 파티클들마다 별개이기 때문에 이들은 가우시안으로 추정합니다. 

 

 

각각의 파티클들은 로봇의 pose 와 추정하고 있는 랜드마크들의 위치에 대한 [mean, covariance] 를 가지고 있습니다. 

 

Action Update

 

모든 파티클들이 공유하고 있는 값이 하나 있는데, 그건 바로 observation 입니다. 각각의 파티클이 따로 관측을 하는 건 아니니까요. 파티클은 정확히 말하면 확률의 candidate 입니다. 

 

 

파티클이 추정한 랜드마크의 위치와 실제 관측값을 비교해서, 랜드마크와 pose 를 가장 높은 확률로 추정하고 있는 파티클에게 큰 가중치를 부여합니다. 

 

---

위 내용은 경희대학교 소프트웨어융합학과 황효석 교수님의 2021년 <로봇프로그래밍> 수업 내용을 요약한 것입니다.