확률적 로봇공학 - 10.2 확장 칼만 필터를 이용한 SLAM

 

10.2 확장 칼만필터를 이용한 SLAM. SLAM with Extended Kalman Filter

10.2.1 설정과 가정들

 역사적으로 일직이 가장 영향력있는 SLAM알고리즘들은 확장 칼만필터 EKF를 기반하고 있습니다. 분명히 예기하자면 EKF SLAM 알고리즘은 EKF를 최대 우도 데이터 연관을 사용한 online SLAM에 적용한 것으로, 이렇게 하여 EKF SLAM은 많은 근사의 영향을 받으며 다음의 가정들로 제약되고 있습니다.

 

- 특징 기반 지도 feature based map

 EKF에서 지도는 랜드마크 점들로 이루어져있습니다. 계산상의 이유로 점 랜드마크의 수는 일반적으로 적습니다(1,000개 이하). 더 나아가 EKF 방법들은 애매한 랜드마크의 수가 적을수록 잘 동작하는 경향이 있습니다. 이러한 이유로 EKF SLAM은 특징 검출기가 중요하며, 인공적인 비콘이나 랜드마크를 특징으로 사용하기도 합니다.

 

- 가우시안 노이즈

 어느 EKF 알고리즘이던, EKF SLAM은 로봇의 동작과 인지시에 가우시안 노이즈 가정을 따르고 있습니다. 사후확률에서 불확실성의 크기는 상대적으로 작아야만 합니다. 사후확률에 있어서 불확실성의 크기는 EKF의 선형화시에 수용할수 없는 에러가 발생할수 있기 때문에 반드시 작아야만 합니다.

 

- 긍정 측정 positive measurement

 EKF 알고리즘은 7.5장에서 살펴본 EKF 위치추정기 처럼 랜드마크의 양의 정보만 처리할수 있습니다. 여기서는 센서 측정시 랜드마크의 부재로 발생하는 부정 정보를 처리할수 없는데, 이것은 가우시안 신뢰도 표현법의 결과로 7.5장에서 살펴보았습니다.

 

10.2.2 대응관계가 알려진 SLAM - slam with known correspondences

 대응 관계가 알려진 경우의 SLAM 에서는 SLAM에서 계속 다뤄지는 문제이기도 합니다. 이 알고리즘을 개발하려면 7.5장에서 본 EKF 위치 추정알고리즘을 이용하면 되는데, 약간의 차이는 자세 $x_t$를 근사시키는것에 더해서 EKF SLAM 알고리즘은 경로중에 만나는 모든 랜드마크의 좌표도 추정하게 됩니다. 이는 상태 벡터에 랜드마크의 좌표들을 포함해야만 하도록 만들게 합니다.

 

 

표 10.1 동시적 위치 추정 및 지도작성 문제에서의 확장 칼만 필터(EKF)

- 여기서는 특징 기반의 지도와 거리와 방위를 측정하는 센서가 로봇에 장착되어 있습니다. 이 경우는 대응관계에 대해 알고있는 경우를 가정하고 있습니다.

 

 쉽게 볼수 있도록 상태 백터은 로봇의 자세와 지도로 이루어졌으므로 혼합 상태 백터 combined state vector라 하고, 이 벡터를 $y_t$로 표기하겠습니다. 그러면 혼합 벡터는 다음과 같습니다.

 

 

 여기서 x, y, $\theta$는 시간 t에서 로봇의 좌표이고 $m_{i, x}$, $m_{i, y}$는 i번째 랜드마크의 좌표가 되며, $s_i$는 그 랜드마크의 시그니처입니다. 이 상태 백터의 차원은 3N+3이 되는데 여기서 N은 지도상에 존재하는 랜드마크의 수를 나타냅니다. 이 벡터는 7.5장에서 다루었던 자세 벡터보다 상당히 크며 EKF SLAM은 온라인 사후확률을 다음과 같이 계산합니다.

 

 EKF SLAM 알고리즘에 대해서 표 10.1에서 볼수 잇는데 표 7.2에서 본 EKF위치 추정알고리즘과 비슷합니다. 2~5번째 줄에서 동작 갱신이 이루어지고, 6~20번째 줄에서는 관측 백터가 합쳐지고 있습니다. 특히 3~5번째줄에서 동작 모델을 따르는 신뢰도의 평균과 공분산이 구해지는데, 여기서의 계산에서는 신뢰도 분포에서 로봇의 자세와 관련된 요소들만 변하게 됩니다. 지도와 관련된 나머지 모든 평균과 공분산 변수들은 변하지 않고, 자세-지도 공분산도 그렇습니다. 7~18번째 줄은 전체 측정치에 대해서 반복을 수행하게 되는데, 9번째 줄에서는 아직 초기 위치 근사가 되지않은 랜드마크에 대해서만 옳은 값을 반환합니다. 10번째 줄에서는 측정된 거리와 방위를 사영하여 랜드마크의 위치로 초기화 시킵니다. 아래에서 다뤄보겟지만 이 과정은 EKF의 선형화에 있어서 매우 중요합니다. 각각의 측정치에 대해 기대 측정치는 14번째 줄에서 계산되고, 대응하는 칼만 게인은 17번째 줄에서 계산됩니다. 칼만 게인은 크기가 3인 행렬인데, 이 행렬은 비 희소 non-spare이며, 전체 상태 추정치에 대해 이 값이 반영됩니다. 필터의 마지막 갱신은 19, 20번째 줄에서 수행되는데 로봇의 신뢰도에서 변화 innvation이 발생하게 됩니다.

 

 칼만 게인은 관측된 랜드마크와 로봇의 자세 뿐만이 아니라 모든 상태 변수에 완전히 영향을 주므로 매우 중요합니다. SLAM에서 랜드마크를 관찰하는 것은 이 랜드마크의 위치 근사를 개선할 뿐만이 아니라 다른 랜드마크도 개선시키게 됩니다. 이 효과는 로봇의 자세에 의해 이뤄지는데, 랜드마크를 관찰하여 로봇의 자세 근사치가 개선되고, 그 결과 로봇이 이전에 보았었던 랜드마크들의 불확실성이 줄어들게 됩니다.  이전에 없었던 이 놀라운 효과는 과거의 자세도 명확하게 나타내느데 이는 완전 슬램 full slam이 이뤄지게 하고 ekf를 비실시간 알고리즘으로 만들었습니다. 이 대응 관계는 가우시안 사후확률이 아니라 행렬 $\Sigma_t$의 비대각 공분산요소들로 찾을수 있습니다.

 

그림 10.1 EKF를 온라인 슬램에 적용한 결과.

- 로봇의 경로는 점선, 자세 근사치는 회색 타원

- 위치는 모르는 8개의 구분 가능한 랜드마크들은 작은 점으로 되어있고, 이 랜드마크들의 자세 추정은 흰색 타원으로 되어있습니다.

- (a)-(c)에서 랜드마크들을 만날때마다 로봇의 자세 불확실성이 증가하고 있습니다.

- (d)에서 로봇이 첫번째 랜드마크를 다시 만나면서 모든 랜드마크와 자세의 불확실성이 줄어듭니다.

 

 

 그림 10.1은 EKF SLAM알고리즘을 시뮬레이션 예제에 사용한 경우를 보여주고 있습니다. 로봇은 시작점을 좌표계의 원점으로 삼아서 주행을 시작하게 되는데, 로봇이 움직이는 동안 자세 불확실성은 증가하게 되며 이 불확실성 타원의 지름이 증가하는것으로 보여주고 있습니다. 로봇은 근처에 존재하는 랜드마크를 감지하고 이들을 불확실성을 가진것으로 지도 작성을 수행하는데 고정된 관측 불확실성에 증가중인 자세 불확실성이 합쳐지게 됩니다. 그 결과 랜드마크 위치 불확실성은 시간이 흠름에 따라 증가하게 됩니다. 흥미로운 변화는 그림 10.1d에서 일어나는데 지도 작성 시작때 보았던 랜드마크 즉, 상대적으로 잘 알고있는 장소를 관측할때를 보여주고 있습니다. 이 관측을 통해 로봇의 자세 오차가 줄어들어 최종 로봇의 자세를 나타내는 타원이 매우 작아진것을 알수 있습니다. 더 나아가 이 관측은 지도상에 존재하는 다른 랜드마크들의 불확실성도 줄여줍니다. 이 현상은 가우시안 사후확률의 공분산 행렬상의 상관 관계에서 일어나게 되는데, 랜드마크 대부분의 불확실성은 로봇의 자세에 의해 발생하게 되는데 이 불확실성은 시간에 따라 가면서 랜드마크의 위치 근사치는 상관관계를 가지게 됩니다. 로봇의 자세에 대한 정보를 얻을때 이 정보는 이전의 관측 된 랜드마크에 퍼지게 되고, 이 효과는 SLAM 사후 확률의 가장 중요한 특성이라 할수 있습니다. 여기서 정보는 로봇의 위치 추정을 돕고, 지도 전반에 퍼지게 되며, 결과적으로 지도 상에 존재하는 다른 랜드마크의 위치추정도 개선하게 됩니다.