확률적 로봇 공학 - 7.5 확장 칼만 필터 위치추정

7.5 확장 칼만필터 위치 추정 EKF Localization

 확장 칼만 필터 위치 추정 or EKF 위치추정은 마르코브 위치 추정의 한가지 경우로, 이는 신뢰도 bel($x_t$)를 1, 2차 모멘트인 평균 $\mu_t$와 공분산 $\Sigma_t$로 나타냅니다. 기본 EKF 알고리즘은 3.3장의 표 3.3에서 설명하였고, EKF 위치 추정은 실제 로봇 문제에 EKF를 구현한게 됩니다.

 

 우리가 사용할 EKF 위치추정 알고리즘은 지도가 특징들로 이루어져있다고 가정하며, 시간 t에대한 어느 지점에서 로봇은 주위의 특징들 $z_t$ =  {$z_t^1$, $z_t^2$,, ...}까지의 방위와 거리에 대한 벡터를 측정하여 얻습니다. 우선은 모든 특징들을 유일하게 구분하수 있는 경우 부터 시작하겠습니다. 특징들의 유일성이 존재하는것은 나쁜 가정이 아닌데, 만약 파리에서 투어중일때 에펠탑은 다른 랜드마크와 햇갈리지 않은 랜드마크이며, 파리 전체에서 넓게 볼수있습니다.

 

 이 구분되는 특징을 대응관계 변수 correspondence variables 집합으로 표현되며 각각의 특징 벡터 $z_t^i$에 대해 $c_t^i$로 표기합니다. 대응관계 변수는 이미 6.6장에서 살펴봤지만, 우리의 첫 알고리즘에서는 이 대응관계를 알고있는것으로 가정해보겠습니다. 그러고나서 구분하기 애매한 특징들을 다룰수 있는 더 일반적인 버전으로 진행하겠습니다. 이 버전에서는 로봇은 특징의 시그니처 $s_t^i$를 구합니다. 두번째로 더 일반적인 버전은 대응 관계 변수 값을 추정하기 위해 최대 우도 추정기를 사용하는 버전으로 이 추정의 결과를 참값으로 사용하겠습니다.

 

 

그림 7.5 이동 로봇 위치추정에서 칼만 필터의 활용. 여기의 밀도 함수들은 유니모달 가우시안을 사용하고 있습니다.

 

7.5.1 설명

 그림 7.5는 확장 칼만 필터 알고리즘을 (그림 7.3의) 1차원 복도 환경에서 이동 로봇의 위치 추정에 사용하는것을 보여주고 있습니다. EKF에서 유니모달 형태의 신뢰도를 다루기 위해서 두가지 편리한 가정을 할건데, 모든 대응관계들을 안다고 가정하여 각각의 문에 (1, 2, 3)고유의 라벨을 줄 것이고, 측정 모델은 p($z_t$ | $x_t$, $c_t$), $c_t$ $\in$ {1, 2, 3}으로 시간 t에 대해 관측된 문을 구분할 수 있습니다.

 

 두번째로 초기 자세를 상대적으로 안다고 가정하겠습니다. 그림 7.5a에서 가우시안 분포로 초기 신뢰도를 보여주고 있으며 문 1에 가우시안의 불확실성의 중심이 위치하고 있습니다. 이 로봇이 우측으로 이동하면 가우시안 동작 모델과 이 신뢰도는 컨볼루션 하게 됩니다. 이동하여 얻은 신뢰도는 폭이 널ㄼ어진 가우시안으로 그림 7.5b와 같습니다.

 

 이제 문 $c_t$ = 2의 앞에서 문을 감지했다고 합시다. 그러면 그림 7.5c의 위 밀도함수는 관측치 p($z_t$ | $x_t$, m, $c_t$)를 보여주고 있으며, 이 관측 확률을 로봇의 신뢰도에 대입하여 그림 7.5c와 같은 사후확률을 구하게 됩니다. 결과로 구한 신뢰도의 분산이 이전의 로봇 신뢰도와 관측 밀도의 분산보다 작은 점을 볼수있는데, 이는 두 독립적인 추징치를 합친 경우 이전의 독립된 추정치보다 더 확실해지기 때문에 그렇습니다. 복도를 이동하면서 로봇의 불확실성은 더 증하게 되는데 EKF가 동작의 불확실성을 로봇의 신뢰도에 계속 반영시키기 때문입니다. 그림 7.5d는 이러한 신뢰도의 하나의 예시를 보여주고 있습니다.

 

7.5.2 EKF 위치 추정 알고리즘

 앞에서는 추상적으로 살펴봤는데, 적절한 동작 모델과 측정 모델, 그리고 EKF 갱신에 사용되는 많은 변수들을 알아봤습니다. 이번에는 특징 기반 지도에서 EKF 구현에 대해 다루어봅시다. 우리의 특징 기반 지도는 점 랜드마크로 구성되어있으며 6.2장에서 이미 소개하였습니다. 이런 점 랜드마크들에 대해서 6.6장에서 살펴본 일반 측정 모델을 사용할 것이고, 5.3장에서 본 속도 동작 모델을 적용하겠습니다. 이번 장을 읽기전에 측정과 동작 방정식에 대해 간단히 살펴보고 오면 더 좋겠습니다.

 

표 7.2 확장 칼만필터 위치 추정 알고리즘. 특징 기반의 지도와 거리 방위 측정 센서가 내장됨. 이 버전은 정확한 대응관계를 알고 있는 경우를 가정함.

 

 표 7.2은 알려진 대응관계를 가진 EKF 위치추정 EKF localization known correspondences 알고리즘을 보여주고 있습니다.  이 알고리즘은 표 3.3의 EKF에서 구하였으며, 입력은 시간 t-1에서 평균 $\mu_{t-1}$, 공분산 $\Sigma_{t-1}$에 대한 로봇 자세의 가우시안 추정치와 제어 $u_t$, 지도 m, 시간 t에 관측된 특징 집합 $z_t$ = {$z_t^1$, $z_t^2$, . . .}과 이에 대한 대응관계 변수 $c_t$ = {$c_t^1$, $c_t^2$, . . .}가 사용됩니다. 출력은 새로 구한 추정치 $\mu_t$, $\Sigma_t$가 됩니다.

 

 이 알고리즘에서 개별적인 연산은 아래에서 설명하겠는데 2~4번째 줄은 선형 동작 모델을 사용해서 동작 갱신을 구현한 것으로 동작 이후의 예측 자세 predicted pose가 2~4번째 줄에서 $\bar{\mu_t}$로 계산되며, 불확실성 타원도 uncertainty ellipse 같이 계산 됩니다. 5 ~ 15번째 줄은 관측 갱신을 구현하였는데, 이 갱신의 핵심은 시간 t에서 관측된 모든 특징 i들을 루프를 돌리게 됩니다. 7번째 줄에서 이 알고리즘은 관측 벡터의 i번째 특징 대응관계 j를 준비하고, 예측된 관측치 $\hat{z_t^i}$와 관측 모델의 자코비안 $H_t^i$를 계산합니다. 칼만 게인 $K_t^i$은 12번째 줄에서 각각의 관측된 특징들에 대해서 계산이 되는데, 14~15번째 줄에서 이 갱신치들의 합이 새로운 자세 추정에 적용이 됩니다. 주의할 점은 $H_t^i$의 마지막 행의 모든 값이 0이되는데 이는 시그니처 $s_t^i$는 EKF 갱신 결과에 아무런 영향을 주지 않기 때문입니다. 이는 올바른 대응관계들을 알고있기 때문에 놀라울게 아닙니다.

 

 

그림 7.6 확장칼만필터를 이용한 위치추정 예시. 로봇은 직선을 따라 이동하면서 불확실성이 점진적으로 증가하는데, 이를 에러 타원이 보여주고 있습니다. 위치를 알고있는 랜드마크가 관측되었을때 이 불확실성은 줄어듭니다.

 

 그림 7.6은 EKF 알고리즘을 단일 랜드마크가 존재하는 환경에서 사용하는것을 보여주고 있습니다. 로봇이 왼쪽에서 출발하여 움죽일수록 불확실성이 커지게 되지만. 랜드마크를 봄으로서 그 불확실성이 점진적으로 줄어드는것을 알 수 있습니다.