확률적 로봇공학 - 11. 확장 정보 형태 알고리즘

11. 확장 정보 형태 알고리즘 extended information form algorithm

11.1 소개

 이전에 살펴본 EKF SLAM은 수많은 한계점들을 가지고 있습니다. 이번에는 다른 방법인 확장 정보 형태 SLAM 알고리즘 extended information form SLAM algorithm EIF SLAM에 대해서 살펴보겠습니다. EIF SLAM은 가우시안으로 사후확률을 나타내는 EKF처럼 흔하게 사용되며, 온라인 슬램 문제를 푸는 EKF SLAM과 다르게 EIF SLAM은 완전 슬램 문제를 풀게 됩니다. 이전에 보았지만 온라인 슬램 사후확률은 지도가 주어질때 가장 최근 로봇의 자세로 정의하였고, 완전 슬램의 사후확률은 지도가 주어진대에 대해 전체 로봇의 경로를 정의하였었습니다. 다른 중요한 차이는 EIF SLAM은 정보 행렬과 정보 상태로 이루어진 정보 형태로 가우시안 사후확률을 표현한다는 점입니다. 명확하게, 정보와 모멘트 표현법은 같은 정보를 전달하지만, 정보 형태의 갱신 동작이 EKF와 다르게 동작합니다.

 

 EIF SLAM은 증가하는 알고리즘이 아닌데, 이는 로봇 경로에 대한 사후확률을 구하기 때문입니다. 대신 EIF SLAM은 완전 슬램 사후확률을 구하기 위해 전체 데이터 집합을 처리하게 됩니다. 이 접근 방법은 증가하면서 로봇이 지도를 갱신했던던 EKF SLAM과는 근본적으로 다릅니다. EIF SLAM은 고정된 크기의 데이터셋으로부터 지도를 작성하고, 데이터를 메모리에 유지시킬 여유가 있는경우 가장 적합한 방법이 됩니다.

 

 EIF SLAM은 지도 작성하는 동안 모든 데이터를 가지므로, 개선된 선형화와 데이터 연관 기술들을 적용할수 있습니다. 반면에 EKF SLAM에서 관측에 대한 선형화와 대응관계가 시간 t의 데이터를 기반으로 계산하였던것과는 다르게 EIF SLAM은 모든 데이터를 선형화와 대응관계 계산에 사용합니다.

 

 더 나아가 EIF SLAM은 지도 작성과 대응관계 변수 계산, 관측 모델과 동작 모델의 선형화 등 최적의 근사치를 얻기위한 작업들을 반복하면서 EKF 보다 더 정확한 지도를 만들게 됩니다. EIF SLAM은 잘못된 데이터 연관시에 덜 불안정해지며, EKF SLAM보다 더 큰 회전 불확실성에 대처할 수 있습니다. 마지막으로 EIF SLAM은 EKF에서 사용했던것보다 더 큰 크기의 지도에 사용할수 있게 됩니다.

 

 이번 장에서는 EIF SLAM에 대해 간략한 설명과 기본 갱신 단계를 우선 다루어보갰습니다. 그러고 나서 수학적으로 다양한 갱신 단계들을 구하고 선형 가정에서 올바르게 구한것인지 증명하겠습니다. 지도가 주어질때 사후확률을 계산할수 있도록 실제 구현하는 과정에 대해 살펴봅시다.

 

11.2 개요 설명

 EIF SLAM의 개본 개념은 매우 간단합니다. 우선 연관 대응 변수 $c_{1:t}$와 관측치 집합 $z_{1:t}$, 제어 집합 $u_{1:t}$가 주어졌다고 가정해봅시다. EIF SLAM의 첫 단계는 이러한 데이터를 사용하여 지도 m = {$m_j$}와 로봇 자세 $x_{1:t}$의 결합 공간에 대한 정보 행렬과 정보 벡터를 만들게 됩니다. 여기서 정보 행렬을 $\Omega$, 정보 벡터를 $\xi$라고 표기하겠습니다. 아래에서 보면 각각의 측정과 제어는 $\Omega$와 $\xi$의 지역적인 갱신을 수행시키는데, 제어나 관측치를 $\Omega$, $\xi$에 반영하는것은 지역적인 추가가 되어 정보는 추가되는 양으로서 중요한 역활을 하게 됩니다.

 

 관측치 $z_t^i$로 살펴보겠습니다. 이 관측치는 시간 t에 대해 로봇의 자세 $x_t$와 랜드마크 j = $c_t^i$의 위치 사이의 정보를 제공하고 있습니다. 이 정보는 일종의 제약조건으로 생각할수 있는데, EIF에서 각 갱신들은 자세 $x_t$와 지도 특징 $m_j$의 연결 요소들 사이에 값을 더하는것이라 할수 있습니다. 이러한 값의 크기는 관측 노이즈 때문에 불확실성을 반영하게 되며, 센서의 노이즈가 작을수록 $\Omega$와 $\xi$에 더해지는 값이 커지게 됩니다.

 

 모든 관측 $z_{1:t}$와 제어 $u_{1:t}$를 합친 후에 정보 행렬 $\Omega$를 얻게 되는데, 이 행렬의 비대각 요소는 2가지를 제외하고 모두 0이 됩니다. 두 연속적인 자세 $x_{t-1}$과 $x_t$사이에 0이 아닌 값이 되는데 이는 제어 $u_t$에 의한 정보 연결을 의미하고, 로봇이 $x_t$에 서 특징 $m_j$가 관측되었을때 지도 특징 $m_j$와 자세 $x_t$사이를 나타내는 요소도 0이 아닌 값이 됩니다. 이는 상대적인 위치에 대한 정보를 얻는것이 아니라 SLAM을 통해 얻는 것들은  로봇의 자세에 상대적인 랜드마크의 위치 제약을 나타내는 관측치만을 얻을 수 있습니다. 그래서 정보 행렬은 희소하게 되며, 많은 선형 요소들이 0이 됩니다.

 

 물론 정보 표현법은 지도를 주지 않으며 로봇의 추정 경로를 알려준다고 할수 없습니다. 이 둘다는 $\mu$ = $\Omega^{-1}$ $\xi$(식 3.72)로 구할수 있는데, 이 연산은 희소 행렬의 역을 포함하고 있으며 이는 얼마나 효율적으로 지도와 사후확률 $\Sigma$를 복원할수 있을까 에 대한 질문을 주게 됩니다. 이는 EIF SLAM의 핵심저인 질문으로 EIF는 수백만개의 특징으로 이루어진 지도에 적용되고, 이 크기의 역행렬을 계산하는건 어려울 것입니다.

 

 이런 복잡도 질문에 대한 답은 위상에 의존하게 되는데, 특징이 한번만 보여진다면 $\Omega$에서의 제약을 나타내는 그래프는 선형이 될겁니다. 그래서 $\Omega$는 밴드 대각 행렬이 되도록 재정렬할수 있고, 모든 0이 아닌 값들은 대각 근처에 존재하게 됩니다. 이러한 역행렬의 표준 변수 제거 기술은 $\Omega$를 한번 돌면서 선형 시간에서 $\Omega$를 뒤집어 버리고,이는 한번 가로질러지는 사이클이 없는 공간에 대해 연속적인 시간동안 짧게 각 특징들이 보여집니다.

 

 하지만 더 일반적인 경우는 긴 시간 딜레이를 가지는 여러 시간동안 관측된 특징들을 다루는데, 로봇이 복도를 갔다 오거나 사이클이 존재하는 경우, 로봇이 완전 사이클로 돈 경우가 이런 경우라 할수 있겠습니다. 이런 경우에 매우 다른 시간 간격 $x_{t1}$, $x_{t_2}$ , $t_2$ >> $t_1$을 두는 특징 $m_j$가 존재할수도 있습니다. 우리가 사용할 제약 그래프 constraint graph에서 이 사이클 의존성을 소개할건데 $x_{t1}$과 $x_{t2}$는 제어 $u_{t1 + 1}$, $u_{t1 + 1}$, ..., $u_{t2}$의 시퀀스와 $x_{t1}$, $m_{j}$, $x_{t2}$의 결합 관측 링크 전반에 연결 됩니다. 이러한 연결은 선형 행렬 역행의 트릭을 사용할수 없게 만들며, 역행렬은 더 복잡해집니다. 많은 세상이 사이클을 가지고 있으므로 흥미로운 경우가 됩니다.

 

 중략

 

11.3 EIF SLAM 알고리즘

 이제 EIF SLAM을 계산하는 단계들을 다뤄보겠습니다. 완전 EIF 슬램 알고리즘은 수많은 단계로 이루어지는데, 추가 정보 알고리즘 구현에 있어서 주된 어려움은 조건부 확률 p($z_t^i$ | $x_t$, m)과 p($x_t$ | $u_t$, $x_{t-1}$)를 정보 행렬에서의 링크로 유지시키는 것인데, 이 정보 행렬 요소들은 모두 선형이며, 그래서 이 단계는  p($z_t^i$ | $x_t$, m)과 p($x_t$ | $u_t$, $x_{t-1}$)를 선형화하는 과정을 포함시킵니다. EKF SLAM 에서 이 선형화 과정을 근사 평균 자세 $\mu_{0:t}$에서 자코비안을 계산하면서 볼수  있었는데, 우리의 경우 초기 정보 행렬 $\Omega$와 $\xi$를 만들기 위해서 모든 자세 $x_{0:t}$에 대한 초기 근사 $\mu_{0:t}$가 필요합니다.

 

 이 선형화에 적합한 초기 평균 $\mu$를 찾는 다양한 해결방법이 있는데, 예를들어 EKF SLAM을 실행하고 선형화한 근사치를 사용할수 있겠습니다. 이 장에서는 더 쉬운 방법을 사용할 것인데, 우리가 사용할 초기 근사치는 동작 모델 p($x_t$ | $u_t$, $x_{t-1}$)를 바꾸어서 구할수 있습니다. 표 11.1의 EIF initialize EIF 초기화가 이러한 알고리즘을 보여주고 있습니다.

 

표 11.1 EIF SLAM 알고리즘에서 평균 자세 백터 $\mu_{1:t}$ 초기화

 

 이 알고리즘은 제어 $u_{1:t}$를 입력으로 가지며 결과는 추정 자세 시퀀스 $\mu_{0:t}$가 됩니다. 이 알고리즘은 우선 자세를 0으로 초기화하고, 속도 동작 모델을 사용하여 이후 동작들을 재귀적으로 계산합니다. 우리가 관심있는것은 평균 자세 백터 $\mu_{0:t}$이므로 EIF initialize는 동작 모델의 결정론적인 부분만을 사용하고, 근사에서 다른 관측치를 고려하지 않습니다.

 

 초기 $\mu_{0:t}$이 주어지면, EIF SLAM 알고리즘은 완전 슬램 정보 행렬 $\Omega$와 대응 정보 벡터 $\xi$를 만듭니다. 이는 EIF construct 알고리즘으로 수행되며 표 11.2와 같습니다. 

 

표 11.2 EIF 알고리즘에서 $\Omega$와 $\xi$ 계산

 

 이 알고리즘은 많으며 좋은 수학적 표기들을 사용하는데, 아래의 알고리즘의 수학적 정리에서 명확하게 볼수 있습니다. EIF construct는 입력으로 제어 집합 $u_{1:t}$와 관측 $z_{1:t}$, 그리고 ㄱ연관 대응 변수 $c_{1:t}$와 평균 자세 근사치 $\mu_{0:t}$를 사용합니다. 그러면 각각의 관측치와 제어로 얻은 정보에 따라 지역적으로 내부 행렬들을 추가하여 정보 행렬 $\Omega$와 정보 벡터 $\xi$가 점차 만들어집니다.

 

 부분적으로보면 EIF construct의 2번째 줄은 정보 요소를 초기화 시킵니다. 여기서 3번째 줄의 무한대 정보 요소들은 초기 자세 $x_0$에서 (0 0 0$)^T$로 고정시킵니다. 이는 상대 정보만으로 절대 근사치를 복원할수 없는 사실을 반영하기 때문에 결과 행렬이 특이해가 되므로 필요합니다.

 

 EIF construct의 4 ~ 9번째 줄은 제어치들이 합쳐지게 됩니다. 자세 $\hat{x}$와 자코비안 $G_t$가 5, 6번째 줄에서 계산되고 이는 비선형 관측함수 g의 선형 근사를 의미합니다. 이러한 방정식들로부터 명확한점은 이 선형화 단계는 $\mu_0$ = (0 0 0$)^T$과 자세 근사치 $\mu_{0:t-1}$를 사용하며 7,8번째줄에서 $\Omega$, $\xi$를 갱신하게 됩니다. 두 항은 $\Omega$와 $\xi$의 대응하는 행과 열에 추가 됩니다. 이 추가된 값은 새로운 제약조건을 SLAM 사후확률에 포함시키는 것으로 이전 색션에서 자세하게 살펴보았습니다.

 

 EIF construct의 10 ~ 21번째 줄은 관측치가 합쳐지고 있습니다.  11번째 줄에서 계산된 행렬 $Q_t$는 측정 노이즈 공분산이며, 13 ~ 17번째 줄에서 관측 함수의 태일러 전계를 계산하고 있습니다. 이 계산은 18, 19번째 줄에서 관측 갱신 계산에 반영하면서 끝나게 됩니다. 18번째 줄의 $\Omega$에서 다루는 행렬은 5 x 5 차원으로 이루어지는데, 이 행렬을 추가시키기 위해 3 x 3차원의 자세 $x_t$에 대한 행렬과 2 x 2차원 특징 $m_j$의 행렬, 그리고 $x_t$와 $m_j$ 사이의 링크에 대한 3 x 2, 2 x 3차원의 두 행렬로 분해됩니다. 이 행렬들은 $\Omega$의 올바른 행과 열에 더해지게 되며, 벡터 또한 정보 벡터 $\xi$에 더하게 됩니다. 이 또한 크기가 3과 2인 두 벡터로 분해하여 $x_t$와 $m_j$에 대한 요소로서 더할수 있습니다.

 

 EIF construct의 결과는 정보 벡터 $\xi$와 정보 행렬 $\Omega$가 됩니다. $\Omega$는 경로들 사이, 그리고 자세와 특징 사이에서 주대각성분을따라 0이아닌 하부 행렬들을 가지는 희소행렬이 됩니다. 만약 지도 특징의 수가 자세보다 10배 정도 크다면, $\Omega$의 99% 요소들이 0이 됩니다. 이 알고리즘의 동작 시간은 t에 대해 선형이 됩니다.

 

표 11.3 사후확률의 정보 표현 크기를 줄이기 위한 알고리즘

 

 EIF SLAM 알고리즘의 다음 단계는 정보 행렬/백터의 차원을 줄이는것으로 이는 표 11.3의 EIF reduce알고리즘으로 수행됩니다. 이 알고리즘은 입력으로 모든 자세와 특징에 대해 정의된 $\Omega$와 $\xi$을 사용하며, 출력은 축소된 $\widetilde{\Omega}$와 $\widetilde{\xi}$으로 모든 자세에 대해 정의됩니다. 이 변환은 EIF reduce의 4 ~ 9번째 줄에서 시간마다 특징 $m_j$를 제거하여 수행됩니다.

 

생략 

 

 

 EIF SLAM 알고리즘에서 마지막 단계는 로봇의 주행 경로상에 모든 자세에 대한 평균과 공분산, 그리고 지도 상에 존대하는 모든 특징들의 평균 위치 근사치를 계산하게 됩니다. 이는 표 11.4의 EIF solve에서 수행됩니다. 2, 3번째 줄은 $\widetilde{\Omega}$를 역변환하고, 정보 벡터와 공분산을 곱함으로서 경로 추정 $\mu_{0:t}$를 계산합니다. $\widetilde{\Omega}$에 사이클이 존재할때 이 역변환은 EIF SLAM 알고리즘에서 계산상에서 더 비용이 커지게 됩니다. 다음으로 EIF solve는 4 ~ 7번째 줄 사이에서 각 특징들의 위치를 계산하며, 반환 값은 로봇의 경로와 지도상에 존재하는 모든 특징을 포함하나 공분산은 로봇의 경로에 대한것만 존재합니다.

 

 EIF SLAM 알고리즘의 결과 퀄리티는 EIF initialize로 계산된 초기 평균 추정치가 좋은지에 따라 결정되는데, 이 추정치의 x y 요소들이 선형으로 각 모델에 영향을 주어, 선형화는 이들의 값에 의존하지는 않습니다. 초기 추정치에서 에러는 테일러 근사의 정확도에 영향을 주어 회전시 결과에 영향을 줍니다.

 

 초기 $\mu_{0:t}$의 에러를 보완하기 위해서 EIF construct, EIF reduce, EIF solve를 같은 데이터셋을 이용하여 여러번 돌리면 되는데, 각 반복시 입력은 모든 자세에 대한 평균 추정 벡터 $\mu_{0:t}$와 출력은 개선된 추정치가 됩니다. EIF SLAM 최적화 반복은 초기 자세 추정치가 높은 경우(방위 에러가 20도 이상인 경우)에만 필수적이며, 반복 횟수가 작더라도 충분합니다.(3번 정도)

 

 표 11.5는 이 알고리즘의 결과를 요약하였습니다. 평균으로 초기화 시키고, 생성 단계와 축소, 해결 단계들을 반복하며 2~3번 정도 반복을 수행하면 충분합니다. 결과로 구한 $\mu$는 로봇의 경로와 지도에 대한 최적의 추정치가 됩니다.