Monte Carlo Localization

Monte Carlo Localization

• Monte Carlo Localization 혹은 particle filter localization은 모바일 로봇의 위치 추정에서 널리 사용된다.
  • 특히, 비선형 및 비가우시안 환경에서도 강력하다
• 기본 아이디어
  • MCL은 로봇의 실제 위치에 대한 확률 분포( belief )를 직접적으로 수학적 모델로 표현하는 대신에, 여러개의 "입자들"의 집합으로 근사한다.
  • 각 입자들은 로봇이 있을 법한 하나의 상태와, 그 상태가 실제일 확률적 중요도( 가중치 , weight )를 가진다.
    • ==> 많은 입자들이 모이면, 로봇의 실제 위치에 대한 분포를 샘플 기반으로 표현하는 셈이다.
• 장점
  • 복잡한 환경에서도 사용이 가능하며, 멀티모달 분포 표현이 가능하다.
• 단점
  • 입자가 많아질수록 계산량 증가
  • resampling과정에서 입자 다양성이 줄어드는 문제 발생
  • 좋은 센서 , 모션 모델 설계가 중요

 

---

알고리즘 단계별 구조

• 로봇의 실제 위치 xₜ 에 ​대한 belief는 bel( xₜ )로 표현한다.
• MCL에서는 bel( xₜ ) 를 N개의 입자 집합으로 근사한다. 
  • 하나의 입자집합은 { xₜ  , wₜ }와 같은 구조이며, x는 i번째 입자의 상태를 의미하며, w는 해당 입자의 가중치(확률)이다.
• 1. 초기화
  • 초기에는 로봇에 대한 정보가 거의 없으니, 모든 입자를 맵 전체에 무작위로 배치한다.
    이때 가중치 wₜ = 1/N으로 동일하다.
• 2. Motion update 
  • 입자별로 로봇의 이동 모델( 모션 모델 )을 적용한다.
    즉, 실제 로봇의 odometry , control input등에서 나온 이동량( uₜ , control )을 반영. ( 아래 식 참고 ) 
  • 노이즈도 적용을 해야한다. 실제 로봇의 움직임은 완전히 정확하지 않으니.... 그래서 샘플마다 작은 랜덤 perturbation(노이즈)를 추가해서 불확실성을 모델링
  • 목적
    • 로봇이 제어입력 u를 따라서 움직였을때, 실제로는 여러 가지 위치에 있을 가능성을 입자 분산으로 표현.

모션 모델의 확률분포. v는 선속도이며, w는 각속도이다.

• 3. sensor update
  • 파티클(입자) 상태와 센서 측정값을 비교해서 "입자가 진짜일 가능성"을 평가
  • 현재 입자 위치에서 센서를 "시뮬레이션"하여, 맵과 실제 센서값을 비교
  • 센서 모델은 "beam-based model"  or "Likelihood Field Model"중에서 하나를 선택

센서 모델이란, 해당 위치에서 측정될 가능도이다

• 4. 가중치 정규화 ( normalize )
• 5. resampling
  • 가중치가 높은 입자는 많이, 낮은 입자는 적게 뽑아서 새로운 집합을 생성한다.
    이로 인해서, 시간이 지남에 따라서 높은 확률의 위치에 입자들이 집중하게 함
• 6. 추정
  • 로봇이 최종적으로 위치를 추정하는 방법이며, 최빈값 혹은 가중 평균등의 방법이 존재

 

particles = initialize_particles(N)
for t in range( T ):
	# 예측
	for i in range( N ):
    	particles[i] = sample_motion_model( u_t , particles[i] )
    # 업데이트
    for i in range( N ):
    	weights = compute_sensor_model( z_t , particles[i] , map )

    normalize( weights )
	# 리샘플링
	particles = resample_particles( particles , weights )
    # 추정
    estimate = compute_mean( particles )

---