파티클 필터 Localization — 모션 모델과 리샘플링
자율주행 자동차나 로봇이 스스로 주행하기 위해 가장 먼저 풀어야 할 숙제는 “내가 지금 맵 어디에 있는가” 를 아는 것입니다. 하지만 현실 세계의 센서는 해상도 한계와 노이즈가 있고, 모터나 구동축 역시 마모·제어 노이즈 때문에 완벽하게 움직이지 못합니다.
이 글에서는 이러한 로봇의 본질적인 불확실성을 다루기 위해 선택한 파티클 필터(Particle Filter) 가 어떤 알고리즘인지 알아보고, 위치 추정 스택의 첫 단추를 채우는 모션 모델(Motion Model) 과 리샘플링(Resampling) 의 핵심 디테일을 정리합니다.
1. 파티클 필터란 무엇인가
파티클 필터는 한 문장으로, “수많은 가상의 로봇(파티클)을 맵 위에 뿌려놓고, 실제 센서 데이터와 가장 비슷한 파티클만 살려내며 위치를 좁혀가는” 알고리즘입니다. 확률 분포를 복잡한 해석적 수식이 아니라 유한한 개수의 샘플(파티클) 집합으로 비모수적(non-parametric) 으로 표현하는 것이 핵심입니다.
기본적으로 파티클 필터는 로봇의 움직임을 예측하는 모션 모델(Motion Model) 과, 센서 관측을 평가하는 측정 모델(Measurement Model, Sensor Model) 로 구성됩니다. 주행하는 동안 다음 3단계를 끊임없이 반복합니다.
- 모션 업데이트 (Motion Model) — 로봇의 이동 명령(control input / odometry)을 바탕으로 모든 파티클을 이동시킵니다.
- 가중치 평가 (Sensor Model) — 각 파티클 위치에서 가상의 센서 스캔을 만들어 실제 라이다 관측과 비교하고, 사후 확률에 따라 가중치를 매깁니다.
- 리샘플링 (Resampling) — 가중치가 높은 파티클은 여러 개로 복제하고, 낮은 파티클은 소멸시켜 새로운 파티클 집합을 만듭니다.
중요한 성질 하나. 파티클 필터는 초기 위치를 전혀 모르는 상태(Global Localization) 에서도 정답을 찾아갈 수 있습니다. 처음엔 파티클이 맵 전체에 균일하게 퍼져 있어도, 로봇이 움직이며 센서 데이터를 얻을수록 점차 실제 위치 주변으로 뭉치게 됩니다.
이 글에서는 이 3단계 중 모션 모델과 리샘플링을 다룹니다. (센서 모델은 다음 글에서 이어집니다.)
2. 모션 모델: 파티클을 움직이려면 ‘동적 정보’가 필요하다
첫 단계인 모션 모델을 파고들어 봅시다. 모션 모델이 하는 일은 단순합니다 — 직전 프레임 대비 로봇이 얼마나 움직였는지를 알아내, 그만큼 모든 파티클을 이동시키는 것입니다. 파티클 필터가 전역 탐색에 강하다고 해서 매 프레임 수천 개를 아무 힌트 없이 뿌리는 것은 낭비이므로, “로봇이 대략 이만큼 움직였다” 는 동적 정보(dynamic information) 로 파티클을 로봇이 있을 법한 영역에 집중시켜야 합니다.
이 동적 정보를 얻는 방법은 여러 가지가 있습니다.
- 이전 pose를 미분 — 직전에 추정된 pose들의 차이를 시간으로 나눠 속도를 구합니다. 추가 센서가 필요 없지만, 추정 자체의 노이즈와 지연이 그대로 속도에 실립니다.
- 추가 정보(센서) 사용 — IMU, 휠 엔코더 등 별도 센서에서 속도·각속도를 직접 받습니다.
간단하게는 VESC(모터 컨트롤러)의 휠 오도메트리(wheel odometry) 를 사용하면 됩니다. VESC가 주는 모터 회전 속도에서 차량의 전진 속도를, 조향각과 결합해 각속도(회전율)를 얻어 “직전 프레임에서 차가 대략 이만큼 움직였다” 는 body-frame 이동량(전진·회전)을 실시간으로 구합니다. 이 값이 모션 모델의 제어 입력(control input) 이 되어 모든 파티클을 그만큼 이동시키고, 덕분에 파티클 필터는 엉뚱한 공간을 뒤지는 대신 로봇이 존재할 법한 영역에 가설을 효율적으로 집중시킵니다.
3. 오도메트리의 불확실성과 프로세스 노이즈(Process Noise)
현실의 레이싱카는 오도메트리 수치만큼 완벽하게 궤적을 그리지 않습니다. 특히 레이싱에서는 여러 이유로 오도메트리 정확도가 크게 저하됩니다.
- 미끄러짐(slippage) — 고속 주행·급선회 시 타이어와 노면 사이에서 필연적으로 발생합니다.
- 급출발 시 헛돌기(wheel spin) — 바퀴가 실제 이동보다 빠르게 헛돌아, 엔코더가 이동 거리를 과대평가합니다.
- 급제동 시 바퀴 잠김(wheel lock) — 바퀴가 멈춰도 차체는 관성으로 미끄러져, 엔코더가 이동 거리를 과소평가합니다.
그래서 모션 모델을 구현할 때, 직전 파티클 위치에 오도메트리 변위만 단순히 더하는 결정론적 방식은 쓰면 안 됩니다. 파티클을 이동시킬 때 반드시 독립적인 가우시안 노이즈(Gaussian noise) 를 샘플링해 명시적으로 더해 줘야 합니다.
이 노이즈는 “오도메트리를 신뢰할 수 없는 만큼, 차량의 실제 위치가 있을 수 있는 범위로 파티클을 퍼뜨린다” 는 불확실성을 파티클 분포로 반영하는 장치입니다.
아래 데모에서 노이즈 파라미터(MOTION_DISPERSION_X/Y/THETA)를 직접 조절하며, 하나의 pose에서 파티클이 어떻게 퍼지는지 확인해보세요.
4. 핵심 디테일: 불확실성 파라미터 튜닝과 파티클의 거동
이 모션 모델의 가우시안 노이즈 파라미터는 휠 오도메트리의 불확실성을 반영한 값입니다. 이 값을 어떻게 설정하느냐에 따라 모션 업데이트 직후 파티클의 분산 반경이 결정됩니다.
1) 너무 작게 튜닝한 경우 (Underestimation)
- 현상 — 필터가 오도메트리를 과신합니다. 모션 업데이트를 거쳐도 파티클이 퍼지지 않고 한 점처럼 뭉친 채 이동합니다.
- 문제 — 코너에서 조금이라도 슬립이 나는 순간, 실제 위치(ground truth)가 파티클 분포를 아예 벗어나 버립니다. 이렇게 되면 센서 매칭·리샘플링을 거쳐도 정답 근처에 살아남은 파티클이 하나도 없어, 필터가 엉뚱한 곳을 가리키며 발산(divergence) 합니다.
2) 너무 크게 튜닝한 경우 (Overestimation)
- 현상 — 필터가 오도메트리를 불신합니다. 모션 업데이트마다 파티클이 가보지도 않은 사방으로 과도하게 퍼집니다.
- 문제 — 파티클이 너무 흩어져 리샘플링 때 넓은 범위가 가중치를 조금씩 나눠 갖습니다. 센서 매칭이 가중치를 확실히 몰아주지 못하는 구간에서는 파티클 중심이 이리저리 흔들리는 노이즈성 출력이 나와, 제어 스택에 불안정한 자세 정보를 공급합니다.
결국 핵심은, 내 오도메트리의 불확실성을 스스로 수치화해서 이 파라미터로 들고 있어야 한다는 것입니다. 문제는 이 적정값이 주행 환경(노면·코너 특성)마다, 그리고 나의 주행 스타일·튜닝마다 달라진다는 점입니다. 그래서 하나의 고정값으로 모든 상황을 감당하기 어렵고, 이 값을 잡는 일 자체가 까다로워집니다. 근본적으로는 베이스 오도메트리가 정확할수록 이 불확실성을 작게 유지할 수 있어 유리합니다.
5. 리샘플링: 뭉치고, 다시 퍼지고
모션 모델로 퍼진 파티클은 센서 모델(다음 글)에서 실제 라이다 관측과 비교돼 가중치를 받습니다. 리샘플링은 이 가중치를 실제 파티클 개수에 반영하는 단계입니다.
핵심은 가중치에 비례한 복원추출(weighted resampling) 입니다. 가중치가 높은 파티클일수록 다음 세대에 여러 번 복제되고, 낮은 파티클은 소멸합니다.
1
2
3
# 가중치(p=weights)에 비례해 N개를 복원추출
proposal_indices = np.random.choice(self.particle_indices, self.MAX_PARTICLES, p=self.weights)
proposal_distribution = self.particles[proposal_indices, :]
여기서 모션 모델과 리샘플링이 한 쌍으로 맞물립니다. 리샘플링은 복원추출이라, 가중치 높은 파티클이 똑같은 좌표의 복제본 여러 개로 남습니다. 이대로면 파티클 다양성이 사라지는데(sample impoverishment), 바로 다음 스텝의 모션 모델 노이즈가 이 복제본들을 다시 흩뿌려 다양성을 회복시킵니다. 즉 “리샘플로 정답 주변에 뭉치고 → 모션 노이즈로 다시 퍼지고” 를 매 프레임 반복하며 위치를 좁혀 갑니다. 앞 절에서 노이즈 파라미터를 “정답을 간신히 감쌀 만큼”으로 튜닝해야 한다고 한 이유가 여기에도 있습니다 — 너무 작으면 복제본이 뭉친 채 굳어 버리고, 너무 크면 애써 모은 파티클이 다시 흩어집니다.
우리 코드의 디테일 두 가지:
- 매 프레임 무조건 리샘플링합니다 (effective sample size 임계값을 두는 적응형 리샘플링은 없음).
np.random.choice기반 multinomial 리샘플링이며, 실제 실행 순서는 리샘플 → 모션 → 센서입니다 (개념적 3단계 순서와는 배치가 조금 다릅니다).
마무리
파티클 필터의 모션 모델은 결국 현실의 불확실성을 인정하고, 오도메트리 오차만큼 파티클을 영리하게 퍼뜨리는 과정입니다. 그리고 리샘플링이 정답 주변으로 파티클을 다시 모으면, 모션 노이즈가 이들을 재확산시키며 이 순환이 매 프레임 반복됩니다.
이렇게 퍼지고 모인 파티클이 실제 라이다 스캔과 만나 어떻게 가중치를 얻고 최종 수렴하는지, 그리고 그 과정의 연산 병목을 해결하는 최적화 기법은 파티클 필터 — 측정 모델(Measurement Model) 에서 이어갑니다.