SLAM(Simultaneous localization and mapping) - Kalman Filter second
동시적 위치추정 및 지도작성과 칼만 필터 두 번째
이제 우리는 SLAM(Simultaneous localization and mapping)과 칼만 필터를 통해서 2차원 에서 운동하는 등속 모델의 물체에 대한 추적을 할 수 있게 되었다. 그런데 궁금한 것이 있다. 관측 행렬 \(\mathbf{H}=\left[\begin{array}{cccc}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\end{array}\right]\)에서 알 수 있는 것처럼 측정할 수 있는 값은 위치 뿐이고, 속도에 대한 정보를 입력해 준 적은 없는데, 상태 벡터를 관찰해보면 속도에 대한 값이 생성되고 있다. 왜 그럴까? 공분산 행렬 \(\mathbf{P}\)을 살펴보면 예측 과정에서는 행렬의 대각 성분, 다시 말하면 각 위치와 각 속도 스스로의 항에 더해지지만, 갱신하는 과정에서 대각 성분이 아닌 곳에 연관성(correlation)이 발생하기 때문이다. 그러므로 칼만 필터는 위치에 대한 정보만 입력 받아도 상태 천이 행렬로부터 적절한 속도를 갱신하도록 한다고 생각해 볼 수 있다.
그러면 마찬가지로 입력 받지 않은 다른 측정 값에 대해서도 갱신하는 것이 가능하지 않을까? 그렇다. SLAM의 기본 개념은 측정된 값을 이용해서 측정 되지 않은 다른 값들을 갱신하는 것이다. 현재 위치에서 측정할 수 있는 랜드 마크는 센서의 시야각과 거리의 한계로 제한이 있을 수 밖에 없다. 한정된 측정값을 이용해서 다른 상태 벡터의 값들을 좀 더 신뢰할 수 있는 값으로 갱신하는 것이다.
아래 그림을 보면, 움직이는 로봇이 측정할 수 있는 랜드 마크의 숫자가 한정적인 경우에도 하나의 상태 벡터를 포함하고 있는 상태에서는 모든 랜드 마크와 로봇의 위치에 대한 신뢰도가 향상되는 것을 볼 수 있다. 로봇과 랜드 마크의 타원은 공분산 행렬(Covariance matrix)에 비례하여 그려진 것이므로 해당 항목의 불확실성(Uncertainty)을 의미하는 것으로 생각할 수 있다.
이미지 원본: Andrew Davison의 박사 학위 논문
이렇게 로봇의 상태와 랜드 마크의 좌표가 하나로 합쳐진(Augmented) 상태 벡터를 사용하는 방법은 관측 범위의 제약으로 한정된 관측이 수행되는 경우에 유용하지만, 태생적으로 차원의 저주(Curse of dimensionality) 문제를 갖고 있다.
추정하고자 하는 로봇 좌표계의 차원과 특징점의 차원이 모두 하나의 상태 벡터에 포함되므로 특징점의 개수가 증가함에 따라서, 공분산 행렬 \( \mathbf{P} \)의 크기가 \( 2^d \)에 비례하여 증가하기 때문이다.
SLAM(Simultaneous localization and mapping) - Kalman Filter
동시적 위치추정 및 지도작성과 칼만 필터
국문으로 번역된 이름이 마음에 들지 않지만, 마땅히 다르게 번역할 방법이 없어서 기존에 사용되던 것들 중에서 차용하였습니다.
제목에서 느낄 수 있는 것처럼 이동 로봇이 미지의 세계를 방문할 때 자신의 위치추정과 지도작성을 동시에 수행하는 것을 말한다. 동시적이라고 하면 시간의 흐름상 완전한 동시성을 의미하는 것처럼 느껴지기 때문에 사실은 일관된, 연관된 정도로 번역하는 것이 자연스럽다고 생각된다.
칼만 필터를 이용한 이동 로봇의 SLAM에 대해서 먼저 기술하고 다음으로 다른 종류의 필터(EKF, UKF, Particle Filter)에 대해 적어볼 예정입니다.
위치 추정과 지도 작성은 동시에 진행하는 것이 타당하다. 위치를 인식하기 위해서는 지도가 정확해야 하는데, 지도의 정확성은 위치의 정확성에 의존하기 때문이다. SLAM이 어려운 이유는 그림에서 보는 것처럼 예측할 수 없는 요소들이 많기 때문이다.
왜 SLAM은 어려운 문제인가?
세상엔 정확한 센서는 존재하지 않기 때문에 항상 노이즈를 포함하고 있다. 정확한 센서 하나만 있었더라도 이렇게 힘들게 고민할 이유가 없었을 것이다.
위의 그림을 조건부 확률식으로 적어보면 아래와 같다.
조건부 확률식으로 전개한 SLAM 문제
상태 벡터 \( \mathbf{x}_{k-1} \)에서 \( \mathbf{x}_{k} \)로 이동하는 로봇이 \( \mathbf{m}_{i}, \mathbf{m}_{j}\) 랜드 마크를 관측한 값이 측정치 \( \mathbf{z}_{k,j}\) 로 입력된다. \( \mathbf{u}_k \)는 현재 로봇의 조종 명령이다.
조건부 확률 식을 말로 풀이하면 " \( \mathbf{z}_{1:t}, \mathbf{u}_{1:t} \)가 만족 되는 경우에 즉, 최초 시점 1에서 현재 시점 \( t \)까지의 관측 값과 조종 명령이 주어질 때, 현재 위치를 의미하는 상태 벡터 \( \mathbf{x}_t \)와 지도를 의미하는 \( \mathbf{m} \)이 어떤 확률 분포를 가지는가?" 로 서술할 수 있다.
확률은 그 합이 '1'이 되어야 하기 때문에 조건이 주어지지 않은 경우의 현재 상태와 지도에 대한 확률과 합하면 조건부 확률 값은 '1'이 된다. 위와 같은 조건부 확률 식으로 전개해 두고 나면 앞서 기술한 베이지안 추정 방법이나 마르코프 위치 인식 방법을 사용할 수 있게 되므로 매우 유리한 점이 생긴다.
상태 천이 행렬이 선형(Linearity: Homogeneity와 superposition을 만족)인 경우에 적용할 수 있다. 선형 상태 천이를 한다는 것은 상태 천이 함수가 선형이라는 말이며 등속, 등가속 운동처럼 이후의 운동의 예측할 수 있는 경우를 말한다.
그림에서 상태 천이 함수는 \( A \) 행렬이다. 등속 모델인 경우엔 \( \mathbf{A}=\left[\begin{array}{cccc}1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right] \), 상태 벡터는 \(\mathbf{x}=\left[\begin{array}{c}x \\ y \\ \dot{x} \\ \dot{y} \end{array}\right] \) 로 설정하면 2차원 좌표에서 등속으로 운동하는 물체에 대한 칼만 필터의 추정식이 완성된다. 실제 시스템 모델이 맞도록 공정 잡음과 측정 잡음을 설정하면 된다.
간단하게 몇 줄의 코드 만으로도 칼만 필터는 훌륭하게 동작합니다. 아래에 2차원에서 움직이는 물체에 대한 칼만 필터 예제를 게시합니다.
베이즈의 정리를 구체적으로 기술한 문서들은 많이 있으므로 기본적인 개념만 서술하면, 모르는 사후확률을 계산할 때 알고 있는 확률과 현재 상태에 대한 확률을 이용하는 개념이다. 가령 스팸메일로 추정되는 메일에는 "대리운전"이라는 문자가 포함되어 있을 확률을 알고 있다면, 해당 메일이 스팸메일일 확률을 계산할 수 있다는 식이다.
이동 로봇처럼 연속된 상태를 갖는 경우에는 시간의 흐름에 대해서 재귀적으로 베이지안 추정을 적용할 수 있게 된다. 위치 추정뿐만 아니라 물체 인식/분류와 같은 분야에도 광범위하게 적용될 수 있는데, 최근엔 naive bayes라는 이름으로 적용되고 있다. 1700년대에 최초로 제안되었던 것을 생각하면 베이즈의 위대함을 느낄 수 있다.
마르코프 체인과 함께 정리하면 다음 그림과 같다.
베이지안 추정 필터의 원리를 재미있는 예로 소개한 것이 S. Thrun의 저서 Probabilistic Robotics에 있는데, 이것을 소개한다.
문제 정의 : 로봇의 팔이 문을 여는 동작을 할 경우 문이 열릴 확률과 센서로 문이 열렸는지 감지했을 때 정말로 열려있을 확률을 알고 있을 때 문의 현재 상태에 대해서 베이지안 추론을 적용할 수 있는가?
문제 적용 : 문의 현재 상태(열림, 닫힘)는 상태 벡터를 \(x_t\), 문에 대한 사전 확률은 \(x_{t-1}\)로 정의할 수 있다. 센서를 통해서 감지하는 문의 상태 우도(likelihood)는 \(p(z_t|x_t)\)가 된다.
확률론에서 마르코프 체인은 바로 다음의 상태는 과거의 상태가 현재까지 축적된 현재 상태에만 기인한다는 이론이다. 상식적으로도 그렇지 않은가? 로봇이 움직이는 다음 위치는 바로 이전 상태에만 관계가 있지, 그 보다 과거의 상태에 영향을 받을 이유는 없다.
첨자들과 표기가 바뀌었지만 베이지안 추정 필터에서 소개한 것과 별반 다를 바 없는 식이다.
실제 구현에서는 몬테카를로 기법을 여기에 함께 사용하는 이유는 연속(Continuous)신호를 표현한 수식을 구현하다 보면 필연적으로 데이터 분해능과 성능의 저하가 발생하기 때문이다. 몬테카를로 방법도 마찬가지로 반복의 횟수와 샘플의 갯수에 따라 성능의 차이가 있지만만, 충분히 많은 수의 샘플을 사용하면 만족할만한 정확도를 얻을 수 있다.
다음 그림과 동영상은 실제로 Minerva에서 사용된 천정 영상과 Occupancy grid에 대한 위치 인식을 수행한 결과이다.
위치 인식을 위해서는 알고 있는 위치에 존재한다는 가정이 필요하다. 지도의 형태는 영상 일 수도 있고, 거리정보 일 수도 있지만 모르는 환경이 아니라 알고 있는 환경이라면 자기 위치를 인식하는 것이 가능하다.
가장 독보적인 진전과 활용을 보인 것은 아마도 독일 bonn 대학의 박물관 가이드 로봇 리노와 미네르바(Rhino & Minerva)일 것이다. 실험실에서 벗어나서 실제로 박물관에서 관객들이 있는 동적 환경에서 성공적으로 주행을 한 첫 번째 기록일 것이다. 당시로서는 로봇을 실험실이 아닌 일반 대중 앞에서 시연하는 것은 무모한 것처럼 보일 정도로 시스템의 안정성에 대한 확신이 없었더라면 힘들었을 일을 해냈다.
1998년 8월 24일부터 9월 5일 까지 31시간 동안 44,018m를 주행한 것으로 기록되었다.
The museum tour guide robot minerva
(Photo courtesy Sebastian Thrun)
Rhino를 만져보고 있는 어린이 관람객
(Photo courtesy Sebastian Thrun)
위치 인식 방법은 몬테카를로 확률 기반 위치 인식 방법을 사용하였다. 레이저 거리센서와 천정 카메라를 이용한 위치 인식 방법을 사용한 것으로 기록되어 있다.
