8.66 입자 필터의 로봇 위치 추정 응용

1. 몬테카를로 위치 추정(Monte Carlo Localization)

몬테카를로 위치 추정(Monte Carlo Localization, MCL)은 이동 로봇의 위치 추정에 입자 필터를 적용한 방법이다. 델러트(Dellaert) 등(1999)이 실내 로봇 위치 추정에 도입하였으며, 이후 확률론적 로봇 위치 추정의 표준 방법이 되었다.

2. MCL의 기본 구조

로봇의 상태를 2차원 자세 $\mathbf{x} = (x, y, \theta)$ 로 설정한다. 사후 분포 $p(\mathbf{x}_t \vert \mathbf{z}_{1:t}, \mathbf{u}_{1:t})$ 를 $N$ 개의 가중 입자로 근사한다.

$p(\mathbf{x}_t \vert \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \approx \sum_{i=1}^{N}w_t^{(i)}\delta(\mathbf{x}_t - \mathbf{x}_t^{(i)})$

3. MCL의 단계

3.1 예측 단계: 모션 모델에 의한 입자 전파

이전 시각의 각 입자를 로봇의 모션 모델 $p(\mathbf{x}_t \vert \mathbf{x}_{t-1}, \mathbf{u}_{t-1})$ 로 전파한다. 차동 구동 로봇의 경우 제어 입력 $\mathbf{u}_{t-1} = (v, \omega)$ 와 잡음을 반영하여:

$\mathbf{x}_t^{(i)} = g(\mathbf{x}_{t-1}^{(i)}, \mathbf{u}_{t-1}) + \boldsymbol{\epsilon}^{(i)}$

여기서 $\boldsymbol{\epsilon}^{(i)}$ 는 오도메트리 잡음이다.

3.2 갱신 단계: 관측 모델에 의한 가중치 갱신

센서 관측 $\mathbf{z}_t$ 의 가능도로 입자 가중치를 갱신한다.

$w_t^{(i)} \propto p(\mathbf{z}_t \vert \mathbf{x}_t^{(i)}, \mathbf{m})$

여기서 $\mathbf{m}$ 은 환경 지도이다. 거리 센서의 경우 광선 추적(ray casting)으로 예상 거리를 계산하고 실제 거리와 비교한다.

3.3 리샘플링

유효 입자 수가 임계값 이하이면 리샘플링을 수행한다. 가중치가 큰 입자는 복제되고 작은 입자는 제거된다.

4. MCL의 주요 특성

4.1 글로벌 위치 추정

초기 위치가 알려지지 않은 경우, 입자를 전체 지도에 균일하게 분산시켜 글로벌 위치 추정을 수행할 수 있다. 입자 필터는 다봉 분포를 자연스럽게 표현하므로, 다수의 가설을 동시에 추적하다가 관측 증거가 축적되면서 올바른 위치에 수렴한다.

4.2 납치 로봇 문제(Kidnapped Robot Problem)

로봇이 갑자기 다른 위치로 이동된 경우(납치), 기존 입자 집합이 새 위치의 분포를 표현하지 못한다. 이를 해결하기 위해 적응적 MCL(AMCL)에서 소량의 무작위 입자를 주기적으로 추가한다.

4.3 계산 효율성

입자 수 $N$ 이 필요한 정밀도를 결정한다. 통상 수백~수천 개의 입자로 실시간 성능이 달성되며, 격자 기반 방법보다 효율적이다.

5. 적응적 입자 수

KLD-샘플링(Kullback-Leibler Divergence sampling)은 입자 분포의 KL 발산에 기반하여 입자 수를 적응적으로 조정한다. 사후 분포가 넓게 분산되면 많은 입자를, 집중되면 적은 입자를 사용한다.

6. 센서 모델

6.1 거리 센서 모델

거리 센서의 측정은 다음의 혼합 모델로 표현된다.

$p(z_k^{(t)} \vert \mathbf{x}_t, \mathbf{m}) = w_{hit}p_{hit} + w_{short}p_{short} + w_{max}p_{max} + w_{rand}p_{rand}$

각 성분은 정상 측정, 짧은 거리 측정, 최대 거리 측정, 무작위 측정에 해당한다.

6.2 가능도 필드(Likelihood Field) 모델

거리 센서의 정확한 광선 추적 대신 가능도 필드를 사용한다. 각 셀의 가능도는 가장 가까운 장애물까지의 거리로 결정된다. 계산이 효율적이며 연속적 가능도를 제공한다.

7. FastSLAM

FastSLAM은 입자 필터를 SLAM 문제로 확장한 것이다. 각 입자가 로봇 궤적의 가능한 가설을 나타내고, 각 입자에 대해 랜드마크의 독립적 EKF를 유지한다. 이 분해는 로-블랙웰화된 입자 필터(Rao-Blackwellized Particle Filter)의 예로, 랜드마크의 조건부 사후 분포가 가우시안이라는 구조를 활용한다.

8. 실용적 구현 고려

입자 수 결정: 실시간 성능과 정밀도의 균형을 고려. 통상 500~5000개.

리샘플링 빈도: 매 갱신마다 리샘플링하면 샘플 빈곤이 발생할 수 있으므로, 유효 입자 수 기준으로 조건부 리샘플링이 권장된다.

입자 초기화: 초기 분포가 참 위치를 포함하도록 충분히 넓게 설정해야 한다.

9. 참고 문헌

Dellaert, F., Fox, D., Burgard, W., & Thrun, S. (1999). “Monte Carlo Localization for Mobile Robots.” Proceedings of ICRA, 1322–1328.
Fox, D. (2003). “Adaptive Particle Filters for Robot Localization.” International Journal of Robotics Research, 22(12), 985–1003.
Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
Montemerlo, M., Thrun, S., Koller, D., & Wegbreit, B. (2002). “FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem.” Proceedings of AAAI, 593–598.

version: 1.0