파티클 필터 Localization — 측정 모델(Measurement Model)
지난 모션 모델에서는 오도메트리의 불확실성만큼 파티클을 맵 위에 퍼뜨려 보았습니다. 이제는 흩어진 수많은 파티클 중 ‘누가 진짜 내 위치에 있는지’ 를 가려낼 차례입니다. 이때 쓰는 것이 측정 모델(Measurement Model), 또는 센서 모델(Sensor Model) 입니다.
모션 모델이 ‘내가 어디쯤 와 있을까?’ 를 추측하는 과정이라면, 측정 모델은 ‘내 눈앞의 이 풍경이 내 추측과 얼마나 일치하는가?’ 를 채점하는 과정입니다. 측정 모델은 각 파티클이 자기 위치에 있다고 가정했을 때 관측될 가상 스캔을 예상하고, 이를 실제 로봇이 관측한 데이터와 비교합니다. 두 데이터가 비슷할수록 그 파티클이 실제 위치일 확률이 높다고 보고 높은 가중치(weight) 를, 다르면 낮은 가중치를 줍니다. 이 과정을 거쳐야 이어지는 리샘플링에서 진짜 위치에 가까운 파티클만 살아남아 뭉칩니다.
그렇다면 이 측정 모델을 어떻게 구현할까요?
1. 가상 라이다 매칭(Raycasting)의 한계와 연산 병목
가장 직관적이면서 우리 스택이 메인으로 채택한 센서 모델은, 각 파티클의 가상 자세에서 점유격자지도(Occupancy Grid Map)를 향해 가상의 레이를 쏘아 거리를 계산하는 레이캐스팅(Raycasting) 방식입니다.
흔히 쓰는 레이캐스팅은 Bresenham 이나 DDA 로 격자 지도의 셀을 하나하나 훑으며 벽을 찾아내야 하므로 연산량이 매우 무겁습니다.
- 파티클 개수: $M$ (예: 1000)
- 라이다 레이 개수: $N$ (예: 채널당 360)
- 총 연산량: 매 루프마다 $O(M \times N)$ 번의 레이트레이싱
이 작업은 모션 모델 업데이트와 비교가 안 될 정도로 무겁습니다. CPU 단일 스레드로는 실시간성(최소 20Hz)을 확보하기가 거의 불가능합니다.
무거운 레이캐스팅을 실시간으로 돌리는 법
다행히 레이캐스팅은 각 파티클·각 레이의 연산이 서로 완벽히 독립입니다. 즉, 당혹스러울 만큼 병렬화하기 쉬운(Embarrassingly Parallel) 구조죠. 그래서 GPU가 있다면 수천 개 파티클의 레이캐스팅을 한꺼번에 처리해(예: NVIDIA Jetson 의 CUDA 빌드) 연산 속도를 크게 끌어올릴 수 있습니다.
물론 GPU가 필수는 아닙니다. CPU만으로도 충분히 돌릴 수 있고, 메모리에 여유가 있다면 매번 격자를 훑는 대신 결과를 미리 구워둔 룩업 테이블(LUT) 을 조회하는 식으로 CPU에서도 실시간성을 확보할 수 있습니다. 이런 LUT 기반을 포함한 다양한 최적화 기법은 다음 절에서 살펴봅니다.
참고: 레이캐스팅 없이 채점하는 방법 (AMCL의 거리 변환)
레이캐스팅이 무거운 이유는 ‘지도 내부를 통과하며 벽을 찾는 과정’ 때문입니다. ROS의 표준 패키지 AMCL(Adaptive Monte Carlo Localization) 은 이 과정을 아예 없애는 가능도 필드(Likelihood Field) 방식도 제공합니다. 지도를 로드할 때 거리 변환(Distance Transform) 으로 각 셀에서 가장 가까운 벽까지의 거리를 미리 구워두고(pre-computation), 실행 중에는 가상 레이를 쏘는 대신 실제 스캔의 종점(end point) 만 지도에 투영해 그 거리를 $O(1)$ 로 조회합니다. GPU 없이 CPU만으로도 빠르지만, 정밀한 거리 필드 테이블이 메모리를 많이 잡아먹고 맵에 없는 동적 장애물에 상대적으로 취약합니다. 우리는 GPU 병렬성을 살린 레이캐스팅을 메인으로 쓰되, “raycast 없이 점수를 만드는 길도 있다” 는 정도로 알아두면 됩니다.
아래 데모에서 거리 변환으로 미리 구운 가능도 필드 위에, 실제 스캔의 종점만 투영해 raycast 없이 점수를 매기는 과정을 확인해보세요.
2. 다양한 레이캐스팅 기법과 트레이드오프
가상 라이다 매칭도 단순히 격자를 하나씩 훑는 Bresenham만 있는 것은 아닙니다. 하드웨어·메모리 제약에 따라 다양한 최적화 기법을 적용할 수 있습니다.
- Bresenham / DDA — 방문 셀을 하나씩 검사. 사전 계산이 필요 없고 맵 데이터만 있으면 되지만, 연산량이 많고 격자 단위 정밀도.
- 레이 마칭 (Sphere Tracing) — 거리 변환 맵을 1회 사전 계산. 각 위치에서 벽까지의 최소 거리만큼 빈 공간을 한 번에 크게 점프하며 탐색 속도를 높임.
- CDDT / PCDDT — 방향별 룩업 테이블을 생성·압축 저장. 2개를 조회해 보간하며, 가지치기로 연산량을 줄임.
- Giant LUT — 모든 $x, y, \theta$ 결괏값을 전부 미리 구움. 1번의 $O(1)$ 조회로 끝나 가장 빠르지만, 항목이 기하급수적으로 늘어 엄청난 RAM을 소모.
- 선분 해석 (Analytic) — 격자를 쓰지 않고 선분 리스트를 직접 검사. 격자 해상도와 무관하게 정확한 정밀도.
아래 데모에서 여러 레이캐스팅 기법이 격자 지도 위에서 어떻게 벽을 찾는지, 방식별 특성을 직접 확인해보세요.
3. 가중치 계산 (Weight Computation)
각 파티클의 가상 스캔을 만들었다면, 이제 실제 라이다 스캔과 비교해 점수를 매길 차례입니다. 각 파티클의 가상 스캔과 실제 스캔의 차이를 가우시안 분포 식에 넣어 확률값을 구합니다.
\[p(o_t \mid \bar{x}_t^i) = \frac{1}{\sigma\sqrt{2\pi}}\, e^{-\frac{(o_t - r^i)^2}{2\sigma^2}}\]여러 레이($k$)에 대해서는 각 레이의 확률을 곱해 파티클의 최종 가중치를 얻습니다.
\[w^i \propto \prod_k \exp\!\left(-\frac{\left(z_k^{\text{obs}} - z_k^{\text{exp}}\right)^2}{2\sigma^2}\right)\]의미는 단순합니다. 파티클 위치에서 예상되는 스캔이 실제 관측과 가까울수록, 그 파티클은 기하급수적으로 더 큰 가중치를 받습니다.
아래 데모에서 예상 거리와 실제 관측의 차이(오차)에 따라 가우시안 점수가 어떻게 매겨지는지, 그리고 여러 레이의 점수가 곱해져 하나의 파티클 최종 가중치가 되는 과정을 직접 확인해보세요.
4. 리샘플링과 최종 위치 결정 (Resampling & Final Pose)
가중치 계산이 끝나면, 점수가 높은 파티클을 살리고 낮은 파티클을 버리는 리샘플링을 수행합니다. 보통 누적 가중치 분포 위에서 $1/M$ 간격으로 균등하게 샘플링합니다. 이렇게 하면 가중치가 큰 파티클은 자연스럽게 여러 번 선택되어 복제되고, 0에 가까운 파티클은 도태됩니다.
리샘플링을 거치면 살아남은 파티클이 실제 로봇 위치 주변으로 조밀하게 군집합니다. 이때 가장 빈번한 파티클 위치를 고르거나, 전체 분포의 기댓값을 계산해 로봇의 최종 추정 포즈로 확정합니다.
How to use
실차에서 저장된 맵으로 particle filter localization을 시작합니다.
1
ros2 launch stack_master localization.launch.xml map:=<map_name> localization:=pf
센서·mux(low_level)와 map_server가 함께 실행되고, 파티클 필터의 위치 추정 결과는 EKF를 거쳐 /car_state/odom으로 발행됩니다.
초기 위치는 RViz의 2D Pose Estimate(/initialpose)로 지정합니다 — 클릭한 자세 주변에 파티클을 재초기화합니다.
마무리
측정 모델은 결국 라이다 데이터를 바탕으로 수많은 가설을 평가하고(가중치), 좋은 가설만 추려(리샘플링) 최종 정답을 도출하는 핵심 과정입니다.
우리는 가상 라이다(레이캐스팅)로 채점하는 방식을 메인으로 삼았습니다. 레이캐스팅은 무겁지만 완벽히 병렬적이라, GPU가 있으면 그대로 가속하고 없으면 CPU에서 LUT로 밀어붙일 수 있어 하드웨어를 크게 가리지 않습니다. 결국 알고리즘 선택은 유명한 방법을 그대로 가져다 쓰는 것을 넘어, 하드웨어 아키텍처와 컴퓨팅 자원의 한계와 함께 고민해야 합니다.
파티클 필터는 이처럼 코드가 단순하고 직관적이라 이해하고 다루기에 좋은 알고리즘입니다. 다만 실제 고속 주행 환경에서는 포즈 추정이 다소 불안정한 모습을 보였습니다. 그래서 다음 글에서는 이를 보완하기 위해 Cartographer 기반의 포즈 추정을 소개하려 합니다.
참고자료
- mit-racecar/particle_filter — 파티클 필터 베이스 구현
- kctess5/range_libc — 레이캐스팅(Bresenham·Ray Marching·CDDT·GLT) 라이브러리
- https://www.youtube.com/watch?v=SRBdpoPl57Q