AMCL: 20년 넘게 살아남은 로봇 로컬라이제이션 — Particle Filter + KLD-Sampling 완전 해부

TL;DR

AMCL(Adaptive Monte Carlo Localization)은 사전에 만들어진 2D 맵 위에서 로봇이 자신의 위치를 추정하는 확률론적 로컬라이제이션 알고리즘입니다. 핵심: 입자 필터(Particle Filter)로 위치 확률 분포를 표현하고, KLD-Sampling으로 입자 수를 동적으로 조절합니다. 수렴 전에는 수천 개의 입자로 넓은 불확실성을 표현하고, 수렴 후에는 최소 입자만 유지해 계산량을 아낍니다. Dieter Fox 등이 1999년 MCL을, 2003년 KLD-Sampling을 발표한 이후 20년 이상 로봇 내비게이션의 표준 로컬라이제이션으로 자리잡았으며, ROS2 Nav2의 기본 로컬라이제이션 패키지입니다.

Background: 로봇은 어떻게 자신의 위치를 아는가

로봇 내비게이션의 첫 번째 질문: "나는 지금 어디에 있는가?"

직관적인 답은 odometry입니다. 바퀴 인코더로 이동 거리와 방향을 누적하면 위치를 알 수 있습니다. 하지만 odometry에는 치명적인 문제가 있습니다:

오차가 누적됩니다. 바퀴 슬립, 불균일한 바닥, 인코더 오차 — 10m 이동하면 수십 cm 오차가 생기고 100m면 수 미터가 틀립니다.

그렇다면 센서(LiDAR, 카메라)로 현재 환경을 보고 맵과 비교하면? 이게 맵 매칭(map matching) 입니다. 하지만 문제는:

• 위치 불확실성이 비선형 + 다중 모드(multimodal): 복도에서는 로봇이 복도 어디에 있는지 모름 → 확률이 여러 군데에 분산
• 가우시안 분포를 가정하는 EKF(Extended Kalman Filter)로는 이런 분포를 표현 불가

AMCL의 해결책: 입자(particle)들로 확률 분포 자체를 표현.

EKF vs Particle Filter

Core Algorithm

Particle Filter: 입자로 불확실성을 표현한다

각 입자(particle)는 로봇의 가능한 포즈 하나입니다:

particle_i = (x_i, y_i, θ_i, w_i)

• x_i, y_i: 맵 상의 위치 (m)
• θ_i: 방향각 (rad)
• w_i: 가중치 (이 포즈가 맞을 확률)

전체 파티클 셋이 로봇 위치의 확률 분포(belief)를 표현합니다. 위치가 불확실하면 입자들이 넓게 퍼져있고, 확신하면 한 곳에 몰려있습니다.

알고리즘 한 사이클:

1. [예측] 모든 입자에 Motion Model 적용
   → odometry 변화량만큼 각 입자를 이동 + 노이즈 추가

2. [업데이트] Sensor Model로 각 입자의 가중치 재계산
   → 이 입자의 위치에서 LiDAR가 이 값을 관측할 확률

3. [리샘플링] 가중치 비율로 입자를 다시 뽑음
   → 높은 가중치 입자는 복제, 낮은 가중치 입자는 제거

이 사이클을 계속 반복하면 입자들이 실제 로봇 위치로 수렴합니다.

Motion Model: 로봇 움직임에 노이즈 추가

Odometry는 틀립니다. 얼마나? Motion model이 그 불확실성을 표현합니다.

Differential Drive 모델에서 odometry 변화량 (δrot1, δtrans, δrot2)에 노이즈를 추가:

δrot1_noisy  = δrot1  + noise(α1·|δrot1| + α2·δtrans)
δtrans_noisy = δtrans + noise(α3·δtrans + α4·(|δrot1|+|δrot2|))
δrot2_noisy  = δrot2  + noise(α1·|δrot2| + α2·δtrans)

ROS2 Nav2 파라미터 alpha1~alpha4 (기본값 0.2)가 바로 이 노이즈 크기입니다:

바퀴 슬립이 많은 환경 → alpha 값 크게. 정밀한 인코더 → alpha 값 작게.

Sensor Model: 관측값과 맵을 비교한다

각 입자의 위치에서 "LiDAR가 이 값을 관측할 확률"을 계산합니다. 두 가지 모델:

1. Beam Model (빔 모델)

LiDAR 빔 하나의 측정값 z를 4개 항으로 분해:

p(z|x, map) = z_hit·p_hit + z_short·p_short + z_max·p_max + z_rand·p_rand

이론적으로 완전하지만 계산 비용이 큼.

2. Likelihood Field Model (우도 필드 모델)

맵의 각 점에서 가장 가까운 장애물까지의 거리를 사전에 계산한 likelihood field를 만들어 놓습니다. 측정 시 lookup만 하면 되어 훨씬 빠름.

p(z|x, map) = z_hit·p_hit(dist_to_obstacle) + z_rand·(1/z_max)

Nav2 권장: likelihood_field_prob 모델 — 확률 이론에 더 충실한 구현.

KLD-Sampling: "적응형"의 핵심

고전 Particle Filter의 약점: 입자 수가 고정. 수렴했는데도 계속 2000개를 유지하면 낭비. 너무 적으면 수렴 안 됨.

Fox(2003)의 KLD-Sampling이 해결:

아이디어: 현재 입자 분포와 실제 분포의 KL divergence가 kld_err 이하가 되는 최소 입자 수를 수학적으로 계산.

N_kld = (k-1)/(2·ε) · (1 - 2/(9(k-1)) + √(2/(9(k-1))) · z_{1-δ})³

• k: 현재 입자들이 차지한 격자(bin) 수
• ε = kld_err: 허용 오차
• z_{1-δ} = kld_z: 신뢰 구간

결과: 수렴 후에는 500개 (min_particles)만 사용. 글로벌 로컬라이제이션 중에는 2000개 (max_particles)까지 자동 증가.

논문 결과: 고정 크기 방식 대비 단 6%의 입자만으로 동등한 정확도.

Recovery: 납치된 로봇 문제

"납치된 로봇(Kidnapped Robot) 문제": 로봇이 한 위치에 수렴했는데 갑자기 다른 곳으로 이동되면?

기존 입자들은 모두 틀린 위치를 가리키고 있고, 센서 가중치가 계속 낮아집니다.

Augmented MCL 해결책:

두 개의 이동평균 가중치를 유지:

• w_slow: 느린 이동평균 (장기적 로컬라이제이션 품질)
• w_fast: 빠른 이동평균 (단기적 센서 품질)

if w_fast << w_slow:
    # 로컬라이제이션 실패 감지 → 랜덤 입자 주입
    inject_random_particles(rate = max(0, 1 - w_fast/w_slow))

Nav2 파라미터:

• recovery_alpha_slow (기본: 0.0, 권장: 0.001)
• recovery_alpha_fast (기본: 0.0, 권장: 0.1)

주의: 기본값 0.0은 recovery 비활성화. 납치 복구가 필요하면 반드시 설정.

ROS2 Nav2 AMCL 설정

핵심 파라미터 치트시트

amcl:
  ros__parameters:
    # 입자 수 범위 (KLD-Sampling)
    min_particles: 500
    max_particles: 2000
    kld_err: 0.05          # 낮을수록 더 많은 입자 사용, 더 정확
    kld_z: 0.99

    # 센서 모델
    laser_model_type: "likelihood_field_prob"  # 권장
    max_beams: 60           # 높일수록 정확, 낮출수록 빠름
    laser_likelihood_max_dist: 2.0

    # 센서 혼합 가중치 (합 = 1.0)
    z_hit: 0.5
    z_rand: 0.5
    sigma_hit: 0.2

    # 모션 모델 노이즈
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    alpha1: 0.2   # 회전→회전 오차
    alpha2: 0.2   # 직진→회전 오차
    alpha3: 0.2   # 직진→직진 오차
    alpha4: 0.2   # 회전→직진 오차

    # Recovery
    recovery_alpha_slow: 0.001
    recovery_alpha_fast: 0.1

    # 좌표 프레임
    global_frame_id: "map"
    odom_frame_id: "odom"
    base_frame_id: "base_link"

좌표 프레임 구조

map (고정)
 └─ odom (AMCL이 발행하는 map→odom TF로 drift 보정)
      └─ base_link (바퀴 odometry가 발행하는 odom→base_link)
           └─ base_laser (static TF, URDF에서 설정)

AMCL은 map→odom TF를 지속적으로 발행해 odometry drift를 보정합니다.

Evaluation

수렴 속도

맵 특성이 수렴 속도를 좌우합니다. 직각으로 꺾인 복도, 다양한 방향의 벽 → 빠른 수렴. 열린 공간, 대칭 복도 → 느린 수렴 또는 실패.

정확도

잘 설정된 AMCL은 2D flat 환경에서 ±3~5cm 수준의 위치 정확도 달성 가능. LiDAR 센서 품질과 맵 품질에 크게 의존.

Key Experiments & Tuning 실전 가이드

Scenario 1: 일반 실내 환경

기본값으로 대부분 동작. 수렴이 느리면:

• max_beams 120으로 증가
• min_particles 1000으로 증가
• kld_err 0.02로 낮춤

Scenario 2: 대형 창고/공장

Open space가 많아 로컬라이제이션 어려움:

• max_particles 5000~10000으로 증가
• LiDAR 빔 수 충분히 활용 (max_beams 180~360)
• 초기 포즈를 최대한 정확히 설정

Scenario 3: 비슷한 구조가 반복되는 환경

복사실이 여러 개 있는 오피스, 동일한 창고 열:

• 글로벌 로컬라이제이션으로 시작하면 오랜 시간 걸림
• 가능하면 초기 포즈를 좁은 범위로 설정
• Recovery 파라미터 설정 (recovery_alpha_slow: 0.001, recovery_alpha_fast: 0.1)

Scenario 4: 동적 환경 (사람이 많은 공간)

사람이 LiDAR 빔을 막으면 센서 모델 오염:

• beam_skip_threshold: 0.3 — 30% 이상 빔이 불일치하면 해당 빔 무시
• beam_skip_error_threshold: 0.9
• likelihood_field_prob 모델의 beam skipping 기능 활용

Motion Model 튜닝 핵심

실제 로봇에서 odometry 품질 측정:

1. 1m 직진 후 실제 이동거리 측정 → alpha3 조정
2. 90° 회전 후 실제 회전각 측정 → alpha1 조정
3. 슬립이 많은 바닥 → alpha 값 모두 0.4~0.5로 증가

Limitations — 현장 엔지니어 관점

알고리즘적 한계:

1. 맵 정확도에 완전 의존: AMCL은 로컬라이제이션만 합니다. 맵이 틀리면(SLAM 오류, 환경 변화) 아무리 파라미터를 조정해도 정확한 위치 추정 불가. 맵 품질이 전부
2. 대칭 환경에서 다중 모드 수렴: 똑같이 생긴 복도가 두 개면 로봇이 어느 쪽에 있는지 영원히 헷갈릴 수 있음. Particle set이 두 위치에 반반씩 나뉜 채로 유지되는 경우 발생
3. 루프 클로저 없음: AMCL 자체는 loop closure를 하지 않습니다. 맵을 만들 때(SLAM 단계)에 loop closure가 잘 돼야 함
4. 2D만 지원 (기본): 경사로, 계단, 다층 건물에서 2D AMCL은 쓸 수 없음. 3D AMCL 확장이 있지만 계산 비용 급증

현장 관점 추가:

• 초기 포즈 설정이 배포의 현실적 병목: 매 전원 켤 때마다 RViz에서 2D Pose Estimate를 클릭해야 하면 현장 운용이 불편. 자동화를 위해 고정 출발 지점 + set_initial_pose: true 파라미터 조합을 미리 설계해야 함

• 맵 노후화(Map Staleness) 문제: 실내 환경은 변합니다 — 가구 이동, 임시 파티션, 박스 쌓기. 6개월~1년 주기로 맵을 재생성하지 않으면 AMCL 성능이 서서히 저하됨. 특히 좁은 통로가 막히는 경우 로봇이 맵에 없는 장애물과 충돌

• 고속 이동 시 예측 오차: AMCL은 LiDAR 스캔 주기(보통 10~40Hz) 기준으로 업데이트. 로봇이 빠르게 이동하면 두 스캔 사이에 odometry 오차가 커져 입자 분산 심화. 속도가 빠른 플랫폼에서는 update_min_d, update_min_a 파라미터를 작게 설정

• 멀티플로어 환경의 맵 관리: 층별로 맵을 따로 관리하고 층 전환 시 맵과 AMCL을 리셋하는 로직을 애플리케이션 레벨에서 구현해야 함. Nav2에는 이 기능이 기본 포함되지 않음

• AMCL 실패를 감지하는 방법: amcl_pose 토픽의 공분산(covariance) 값을 모니터링하세요. 로컬라이제이션이 실패하면 covariance 대각 성분이 급격히 커집니다. 이를 감지해 자동 글로벌 로컬라이제이션을 재시도하는 Recovery 동작을 Nav2 Behavior Tree에 넣는 것이 프로덕션 배포의 필수 요소

The Lineage — AMCL의 계보

AMCL이 20년 넘게 살아남은 이유: 단순하고, 계산 효율이 좋고, 맵이 있는 환경에서는 여전히 충분히 정확하기 때문. 더 정교한 방법들이 나왔지만 "알려진 맵에서 2D 로컬라이제이션"이라는 구체적 문제에서 AMCL을 압도하는 실용적 대안은 아직 없습니다.

Summary — Key Takeaways

1. 입자가 곧 불확실성이다 — 각 입자는 "로봇이 여기 있을 수도 있다"는 가설. 입자들의 분포가 바로 위치 불확실성. 가우시안 가정이 없어 복잡한 분포도 자연스럽게 표현
2. KLD-Sampling이 실용성을 만든다 — 수렴 전에는 최대 2000개, 수렴 후에는 500개로 자동 조절. 고정 크기 대비 6%의 입자만으로 동등한 정확도. 이게 없으면 실시간 동작 어려움
3. 맵 품질이 AMCL 성능의 천장을 결정한다 — 좋은 맵 없이 파라미터 튜닝은 한계. SLAM으로 맵을 잘 만드는 것이 AMCL 배포의 첫 번째 과제
4. Recovery 파라미터는 반드시 설정 — 기본값(0.0)은 recovery 비활성화. recovery_alpha_slow: 0.001, recovery_alpha_fast: 0.1은 프로덕션 배포의 필수값. 납치 복구뿐 아니라 로컬라이제이션 실패 자동 복구에도 중요
5. 동적 환경에는 beam skipping — 사람이 많은 공간에서는 빔 스킵 설정이 필수. likelihood_field_prob + beam skipping 조합이 현장 배포의 실전 레시피

📚 원논문:

• Fox et al. (1999) Monte Carlo Localization (AAAI)
• Fox (2003) KLD-Sampling (IJRR)

🤖 ROS2 Nav2: AMCL 공식 문서

다음 포스트: π0 — Physical Intelligence의 Flow Matching 기반 범용 로봇 정책