3차원으로 SLAM 알고리즘을 확장하는것에 대해서 이전 색션에서 살펴보았습니다. 차이는 2차원 스캔 매치와 루프 클로저 검출을 3차원 포인트 클라우드에서 동작하는 3차원 알고리즘들로 대체하면 됩니다. 구현하기 위해서 유명한 ICP 알고리즘을 사용하였고, Steder가 제안한 알고리즘으로 루프 클로저를 검출하였습니다. 게다가 그래프의 각 노드와 각 제약들은 SE(3)에서 존재합니다. 이 알고리즘의 결과는 그림 2(a)와 (b)가됩니다.
그림 2. 그래프 기반 3차원 지도작성 결과
SE(3)의 요소를 나타내기 위한 파라미터의 최소 갯수는 6개로 3차원 평행이동 백터와 3차원 오일러각으로 구성됩니다. 이 파라미터화를 사용하여 알고리즘 1을 얻을수 있습니다. 하지만 이 최소화 표현은 특이해지도작성
3차원으로 SLAM 알고리즘을 확장하는것에 대해서 이전 색션에서 살펴보았습니다. 차이는 2차원 스캔 매치와 루프 클로저 검출을 3차원 포인트 클라우드에서 동작하는 3차원 알고리즘들로 대체하면 됩니다. 구현하기 위해서 유명한 ICP 알고리즘을 사용하였고, Steder가 제안한 알고리즘으로 루프 클로저를 검출하였습니다. 게다가 그래프의 각 노드와 각 제약들은 SE(3)에서 존재합니다. 이 알고리즘의 결과는 그림 2(a)와 (b)가됩니다.
SE(3)의 요소를 나타내기 위한 파라미터의 최소 갯수는 6개로 3차원 평행이동 백터와 3차원 오일러각으로 구성됩니다. 이 파라미터화를 사용하여 알고리즘 1을 얻을수 있습니다. 하지만 이 최소화 표현법은 특이해를 가지며, 과 파라미터화된 상태공간을 사용하여 피할수 있습니다.
대안으로 최소 표현법에서는 자세에서 벗어날때 최적화 문제의 상대적인 작은 변화를 $\Deta \breve{x}$로 나타낼수 있습니다. 이를 통해 알고리즘 2를 구할수가 있게 됩니다. 이번 장에서는 시뮬레이션된 로봇 얻은 자세 그래프에 두갖 ㅣ최적화 알고리즘들을 비교하여보겠습니다.
해시안의 희소 패턴은 두 케이스다 같으며, 선형 시스템을 계싼하는 시간은 무시하겠습니다. 파라미터화의 선택에 따라 수렴 속도에 영향을 주며, 한번 반복하는데 필요한 시간에는 영향을 주지는 않습니다. 이 효과를 강조하기 위해서 두 알고리즘을 사용하여 한번 최적화가 수행되는 동안 반복당 에러 변화를 보여주겠습니다.
그림 11. 구에서 주행한 시뮬레이션된 로봇으로 얻은 자세-그래프
좌측 : 초기 형태, 우측 : 자세-그래프를 최적화 한후 결과로 알고리즘 2로 정밀하게 복원되었습니다.
이 실험에서는 구 표면을 주행하는 로봇의 시뮬레이션된 3차원 데이터 셋을 사용하겠습니다. 측정치는 에러의 영향을 받고 있으며, 오도메트리 정보로 초기한 경우에 그림 11의 왼쪽 그림과 같은 그래프가 만들어 졌습니다. 초기 추측에서부터 시작하여, 가우스-뉴턴 알고리즘에 매니폴드 선형화를 적용한 경우나 적용하지 않은 상태(오일러 각 사용)로 실행하였습니다.
그림.12 3차원 구 데이터셋에 오일러각을 사용한 경우와 매니폴드 선형화를 사용하여 가우스-뉴턴 최적화로 얻은 오차 F(x)의 변화
그림 12는 두 방법으로 반복하는 동안 오차의 변화를 보여주고 있습니다. 처음에는 두 방법다 에러가 줄어들고 있었으나, 특이해가 알고리즘1이 발산하도록 하며, 알고리즘 2는 올바른 해를 향해 수렴하도록 하고 있습니다.
6. 결론
본 튜토리얼에서는 그래프 기반 SLAM에 대해서 소개하였습니다. 이 자료의 목적은 독자들이 제안된 방법을 쉽게 구현하고 세부적인 내용들을 효과적으로 전달하기 위함이고, 이 자료에서 소개한 알고리즘은 더복잡한 방법으로 블록을 만들어 사용해도 됩니다. 하지만 이 알고리즘을 최적화하여 구현하는것은 상당히 힘든 문제가 됩니다.
이번 장에서는 제안된 방법을 이용한 몇가지 응용들을 보겠습니다. 첫번째 시나리오에서는 2차원 지도작성을 보여줄것이고, 두번째 시나리오에서는 3차원 지도 작성에 대해서 설명하고 매니폴드 표현법의 이점을 강조하겠습니다.
A. 2차원 레이저 기반 지도 작성
그림 8. 2차원 지도 작성 실험에서 사용한 로봇. 이 로봇 플랫폼은 표준 액디브미디어 피오니어2로 SICK-LMS 레이저 거리계가 장착되어 있습니다.
시에들의 인탤 연구소에서 그림 8과같은 레이저 거리계가 장착된 이동 로봇으로 데이터들을 기록하였습니다. 이 데이터는 연속 시간 좌표계들 사이의 이 로봇 플랫폼에 대응하는 2차원 변화를 나타내는 오도메트리 측정치와 2차원 레이저 거리계 데이터로 이루어집니다.
이 그래프는 다음의 과정으로 생성 됩니다.
- 로봇이 0.5미터 이상 움직이거나, 0.5라디안 이상 회전할때 이 알고리즘은 그래프에 새로운 정점을 추가하고 현재 레이저 관측과 함께 그 정점에 라벨을 붙입니다.
- 이 레이저 스캔은 이전에 얻었던 것과 매치시켜 오도메트리 추정치를 개선시키고, 이에 대응하는 에지가 그래프 상에 추가됩니다. 우리는 Olson이 제안한 스캔 매치 변형 알고리즘들을 사용하였습니다.
- 로봇이 오랜 시간 동안 모르는 장소들을 돌아다닌 후에 이제 알고있는 장소에 다시 방문할때, 이 알고리즘은 현재 스캔과 이전의 관측과 매치되는지 찾게됩니다(루프 클로징). 만약 현제 관측과 다른 노드의 관측 사이에 매칭이 성공한다면, 이 알고리즘은 새로운 에지를 그래프 상에 추가합니다. 이 에지는 두 스캔들이 최적으로 겹치게 만들어주는 상대적인 변환이 라벨되어 집니다.
현재 측정과 이전의 모든 스캔을 매칭시키는것은 비효율적이고, 에러를 발생시킬수 있습니다. 대신 이 알고리즘은 과거의 노드 들 중에서 현재 로봇의 자세를 가지고 있는 3$\sigma$ 주변 공분산을 가진 후보 노드를 선택합니다. 이 공분산은 축소된 헤시안 $H_{red}$의 역 대각 블록으로 얻을수 있습니다. $H_{red}$는 H에서 새로 입력된 로봇 자세의 행과 열들을 제거하여 얻을수 있습니다. $H_{red}$는 현재 위치가 고정되었다고 가정할때 모든 궤적의 정보행렬이 되겠습니다.
- 이 알고리즘은 루프 클로저가 감지될때마다 최적화를 수행하게 됩니다.
실행이 종료될때, 그래프가 1,802개의 노드와 3,556개의 에지로 구성되어 있었습니다. 원래 상대적으로 큰 공간에서 최적화를 수행하기 힘든 문제가 있지만 표준 랩탑(인텔 코어2 2.4GHz)에서 100ms로 수행될수 있었습니다. 로봇은 약 1 m/s 속도로 주행하였기 때문에, 그래프 최적화는 루프 클로저를 검출한 뒤가 아니라 모든 노드가 추가가 된 후에 실행 될수 있었습니다.
그림 9. 인텔 연구소. 좌측 : 결과 지도와 최적화돼지 않은 자세 그래프, 우측 : 일관성을 보이는 결과 지도와 최적화된 자세 그래프
그림 10. 현실 세계 데이터셋에 대한 자세 불확실성 추정
그림 9는 주행 궤적에 대한 최적화 과정의 효과를, 그림 10에서는 불확실성 타원들을 보여주고 있습니다. 로봇은 타원이 작은 공간에 위치하고 있으며, SE(2)에서 자세는 파라미터화 될 필요가 없으므로, 매니폴드를 사용하여도 이점은 없습니다.
비 유클리디안 공간을 수치적으로 다루는 흔한 방법은 매니폴드에서 최적화를 수행하는 것입니다. 매니폴드는 전역 스케일에서 반드시 유클리디안이 아니나 지역적으로는 유클리디안이 나타나는 수학적 공간을 말합니다. 여기서 소개하는 매니폴드 기반 방법은 태일러와 크래이그만이 소개한 SO(3)에서 함수의 최소화 방법과 비슷합니다.
SLAM 문제에서 각 파라미터 블록 $x_i$는 전이 벡터 $t_i$와 회전 요소 $\alpha_{i}$로 구성되어 있고, 전이 벡터 $t_i$는 유클리디안 공간에서 나타나나, 회전 요소 $\alpha_i$는 비유클리디안 2차원이나 3차원 회전 그룹 SO(2)나 SO(3)에서 나타납니다. 이러한 특이성을 피하기 위해서 이러한 공간들은 회전 행렬이나 쿠터니언 같은 모수화를 벗어난 방법들을 사용하기도 합니다.
식 (16)에 이런 모수화를 벗어난 표현법들을 적용하면, 이로인해 제약관계가 망가질 수 있습니다. 모수화를 벗어나는건 추가적인 자유도를 만들고, 오차를 만들어버립니다. 이 문제를 극복하기 위해서 회전에 대해 (3차원에서 오일러각과같은) 최소한 표현법을 사용할 수도 있겠습니다. 하지만 이러는것은 특이성에 영향을 주게 됩니다. 2차원에서 특이성은 각을 정규화하여 쉽게 복원할수 있으나 3차원에서 이과정은 직관적이지 않게 됩니다.
그래서 다른 방법은 다루어야 하는 공간을 매니폴드로 보고, 유클리디안 공간상의 지역적인 변화 $\Delta$x를, 매니폴드에서의 변화화 사상시키는 연산자를 정의하면 됩니다. 수학적으로 자세한 내용은 해르츠버그의 연구 자료를 참고하면 되겠습니다. 이 연산자를 사용하여 새로운 오차 함수를 정의할수 있게 됩니다.
여기서 $\breve{x}$는 쿼터니언 인스턴스로 모수화를 벗어난 공간상에 나타납니다. 여기서 $\Delta \tilde{x}$는 원점 위치 $\breve{x}$에서의 작은 증분으로, 최소 표현상에서 사용됩니다.
3차원 SLAM 예제에서는 단위 쿼터니언의 백터를 사용하는것이 회전 모수화에서 좋은 선택이 됩니다. 자세히 설명하자면, 증분 $\Delta \tilde{x}$를 평행이동 $\Delta \tilde{t}$와 3차원 회전을 나타내는 단위 쿼터니언의 백터 파트인 $\tilde{q}^{T}$로 6차원 벡터 $\Delta \tilde{x}^{T}$ = ($\Delta \tilde{t}^{T}$ $\tilde{q}^{T}$)와 같이 정의할수 있겠습니다.
역으로 $\breve{x}^{T}$ = ($\breve{t}^T$ $\breve{q}$^{T})에서 쿼터니언 $\breve{q}$를 회전 부분을 나타내기 위해 사용할수 있겠습니다. 그래서 그 연산자는 $\Delta \breve{q}$를 완전 쿼터니언 $\Delta q$로 변환하고, $\Delta x^{T}$ = ($\Delta t^{T}$$q^{T}$)를 $\breve{x}$로 변환하는데 사용할 수 있겠습니다.
쿼터니언을 이용하여 오차 최소화 시에는, 이러한 연산들이 다음의 연산자로 정리할 수 있고, 자코비안 $\breve{J}_{ij}$는 다음과 같이 나타낼수 있겠습니다.
이전의 방정식에서 e는 $\Delta \breve{x}_i$와 $\Deta \breve{x}_j$에만 의존하므로 다음과 같이 전개할 수 있겠습니다.
편미분에 대한 규칙을 사용하고, $\Delta \breve{x} = 0$에서의 자코비안을 을 사용하면, 영이 아닌 블록들은 다음과 같이 됩니다.
이 표기법을 확장하여 식 (9)에 식 (21)을 대입하고, 이를 통해 다음의 선형 시스템을 구할 수 있습니다.
증분 $\Delta \breve{x*}$는 초기 추측 $\breve x$ 주위의 지역 유클리디안에서 계산되기 때문에 이들은 그 연산자를 이용해 모수를 벗어난 공간상에서 재사상되어야 합니다. 식 (16)의 갱신 규칙에 따라 다음과 같이 됩니다.
매니폴드에서의 최소화 문제 공식화에서는 우선 식 (26)으로 초기 추측 주위의 지역 유클리디안 근사 증분 집합을 계산하고, 다음에는 식 (27)로 전역 비 유클리디안 공간상에서 증분들을 누적해야 합니다.
매니폴드 표현법에서 계싼되는 선형 시스템은 유클리디안 공간에서 계사되는 선형시스템과 같은 구조를 가지며, 비 매니폴드 버전으로부터 그래프 최소화 하는 매니폴드 버전을 쉽게 얻을수 있으며, 대응하는 파라미터 블록에 대해 그 연산자와 자코비안 $M_t$만 정의하면 됩니다. 알고리즘 2는 SLAM에 적용하기 위한 가우스 뉴턴의 매니폴드 버전을 보여주고 있습니다.
매니폴드 문제에서의 헤시안 $\breve{H}$는 더이상 궤적의 정보 행렬로 사용되지 않습니다. 알고리즘 2에서 궤적의 정보 행렬을 얻기 위해서 원래의 자세 x공간에서 H를 계산하면 되겠습니다.
알고리즘 2. 알고리즘 1의 매니폴드 버전.
이 알고리즘은 같은 계산 복잡도를 가지며, 특히 3차원의 경우에 비 매니폴드 버전보다 더 강인합니다.
식 (14)에 따르면 행렬 H와 벡터 b는 하나 하나가 제약조건들인 행렬과 벡터들의 합으로 얻을수 있습니다. 모든 제약조건들은 시스템에 추가가 되어지는데, 추가되어지는 시스템의 구조는 에러 함수의 자코비안에 의존하게 됩니다. 제약 조건의 에러함수가 두 노드의 값에만 의존하므로 식 (7)에서 자코비안은 다음과 같은 형태를 가지게 됩니다.
여기에 $A_{ij}$와 $B_{ij}$는 각각 $x_i$와 $x_j$에 대한 에러함수의 미분으로, 식 (10)으로부터 다음의 블록 행렬 $H_{ij}$를 얻을수가 있게 됩니다.
표기를 편하게 하기위해 0인 블록들은 생략하겠습니다.
알고리즘 1. 그래프의 제약조건으로부터 로봇의 자세 사후확률의 다변수 가우시안 근사의 평균 $x*$와 정보 행렬 H* 계산
알고리즘 1은 로봇의 자세에 대한 사후확률의 평균과 정보행렬을 계산하는 반복 가우스-뉴턴 과정을 요약하였습니다. 이 시스템의 구조 대부분이 희소하기 때문에 시스템의 헤시안 H을 저장하기 위해서 메모리 효율적인 표현법을 사용하기를 추천합니다.
헤시안의 구조는 그래프의 연결성으로부터 즉시 알수 있으므로, 반복의 시작시에 헤시안을 미리 만들고, 새로운 선형화가 수행될때마다 모든 에지에 대해 루프함으로서 갱신시키는것을 추천 합니다. 각 에지들은 블록 $H_{[ii]}$, $H_{[ij]}$, $H_{[ji]}$, $H_{[jj]}$과 계수 벡터의 블록 $b_{[i]}$, $b_{[j]}$에 영향을 줍니다. 추가적인 최적화는 H의 위 3가지 부분에서만 수행하면 되겠습니다.
제약 조건의 오차 $e_{ij}$는 연결된 자세 $x_i$와 $x_j$의 상대적인 사에에만 의존한다는 점을 생각하면 자세 x의 특정 부분의 에러 F(x)는 모든 자세의 강체 변환에 대해 불변하게 됩니다. 이를 통해 식 (15)와 같은 결과가 나오게 됩니다.
이 시스템을 수치적으로 풀기 위한 시용적인 방법으로는 증분치들 중 하나를 0으로 제한하면 됩니다. 그러므로 이 과정은 $k^{th}$ 대각 블록 H[kk]에 아이덴티티 행렬을 추가하여 수행할수 있겠습니다. 알고리즘 1에서 일반화 손실 없이, 첫 노드 $x_!$을 고칠수가 있습니다.
자세 그래프의 특정 노드를 고치기위한 다른 방법으로 식 (15)의 선형 시스템의 $k^{th}$ 블록의 행과 열을 억제시키면 되겠습니다.
그림 6. SLAM 과정의 자세 그래프 표현. 그래프 상의 모든 노드들은 로봇의 자세를 의미합니다. 가까운 자세들끼리는 에지로 연결되어 있으며, 이 에지는 로봇의 자세 사이에서 관측으로 생기는 공간적인 제약들을 나타냅니다. 에지 $e_{t-1 t}$는 연속적인 자세들 사이에 있으며, 오도메트리 측정치를 설계하며, 다른 에지들은 동일하 환경에서 다중 관측으로부터 만들어지는 공간적인 제약조건들을 의미합니다.
x = ($x_1$, . . ., $x_T$$)^T$는 자세 노드 $x_i$의 벡터이며, $z_{ij}$와 $\Omega_{ij}$는 노드 i와 노드 j사이 가상 측정의 평균과 정보행렬을 의미합니다. 이 가상 측정은 i로부터 얻은 관측들이 j에서 얻은 관측들로 포개도록 만드는 변환입니다. 노드 $x_i$와 $x_j$의 형태가 주어질때 가상 측정의 예측치를 $\hat{z_{ij}}$($x_i$, $x_j$)라고 합시다.
이 예측은 두 노드 사이에 상대적인 변환이라고 할수 있으며, 관측 $z_{ij}$의 로그 우도 $l_{ij}$는 그러므로 다음과 같습니다.
e($x_i$, $x_j$, $z_{ij}$)는 기대 관측 $\hat{z_{ij}}$($x_i$, $x_j$)과 실제 관측 $z_{ij}$ 사이의 차이를 계산하는 함수로, 이 표기를 간단하게 정리하자면 관측 인덱스들을 에러 함수의 인덱스들로 바꾸겠습니다.
그림 7. 정점 $x_i$와 $x_j$를 연결하는 에지의 측면들. 이 에지는 관측 $z_{ij}$에서 시작합니다. 두 노드사이의 상대적인 차이로부터, $x_i$ 좌표계에서 보여지는 $x_j$를 나타내는 기대 측정치 $\hat{z_{ij}}$를 계산할수 있습니다. 에러 $e_{ij}$($x_i$, $x_j$)는 기대와 실제 측정치 사이의 차이에 의존하게되며, 에지는 에러 함수 $e_{ij}$($x_i$, $x_j$)와 불확실성을 나타내는 측정의 정보 행렬 $\Omega_{ij}$로 정리할수 있겠습니다.
그림 7은 그래프의 에지를 정의하는 양과 함수들을 보여주고 있습니다. C는 관측 제약 z가 존재하는 쌍 인덱스들의 집합이며, 최대 우도 방법의 목표는 모든 관측들에 대해 음의 로그 우도 F(X)를 최소화하는 노드 $x^{*}$의 형태를 찾는것이 되겠습니다.
이를 정리하면, 다음의 식을 푸는문제라고 할수 있겠습니다.
이번 섹션에서는 식 (5)를 풀고, 로봇의 주행 궤적에 따른 가우시안 근사를 계산하는 방법에 대해 설명하겠습니다. 가우스-뉴턴이나 Levenberg-Marquardt 알고리즘 같은 기존 최적화 방법을 사용하는 방법도 있지만, 이 방법은 구조 문제로 다루기 때문에 특히나 효율적으로 동작하게 됩니다.
우선 기존의 비선형 최소제곱 최적화 구현을 설명하고, 다음에는 로봇 자세 표현의 특이성을 다룰수 있도록 도와주는 다른 방법들을 소개하겠습니다.
A. 지역 선형화 반복을 통한 에러 최소화
로봇의 자세에 대한 좋은 초기 추측 guess $\breve{x}$를 알고있다면, 식 (5)의 수치적인 해는 가우스-뉴턴 법이나 Levenberg-Marquardt 알고리즘을 사용하여 얻을수 있겠습니다. 이 아이디어는 현재 초기 추측 $\breve{x}$ 주위에 1차 태일러 전개로 에러 함수를 근사화하는것으로 다음과 같습니다.
여기서 $J_{ij}$는 $\breve{x}$와 $e_ij$ = $e_{ij}$($\breve{x}$)로 계산되는 $e_{ij}$(x)의 자코비안입니다. 식 (4)의 에러 항 $F_{ij}$에 식 (7)을 대입하면 다음의 식을 얻을수 있겠습니다.
식 (14)와 같은 이차 식은 식 (13)에서 c = $\Sigma$$c_{ij}$, b = $\Sigma$$b_{ij}$, 그리고 H = $\Sigma$$H_{ij}$로 설정하여 얻을수 있으며, 선형 시스템의 해를 구함으로서 $\Delta$를 최소화 시킬수 있겠습니다.
행렬 H는 자코비안을 통해 궤정의 상태 공간에서 관측 에러를 사영함으로서 얻을수 있기 때문에 H는 시스템의 정보 행렬이 됩니다. 제약 조건에 의해 연결되는 자세들 사이에는 0이 아닌 값들을 가지며, 희소한 행렬이 됩니다. 이 행렬은 희소 Cholesky 분해를 이용하여 식 (15)을 풀수 있도록 도와줍니다. 희소 Cholesky 분해를 효과적으로 구현하기 위해 CSparse라이브러리를 사용할수 있겠습니다.
초기 추정을 더함으로서 선형화된 해률 얻을수 있게됩니다.
많이 사용되는 가우스-뉴턴 알고리즘은 식 (14)의 선형화와 식 (15)의 해, 식 (16)에서 갱신 단계를 반복해서 수행합니다. 모든 반복과정에서 이전의 해가 선형화 지점과 초기 추정치로 사용됩니다.
위에서 설명한 과정들은 다변수 함수 최소화에 사용하는 일반적인 방법으로 SLAM 문제의 특별한 경우에서 사용됩니다. 하지만 이 일반적인 방법은 파라미터 x들의 공간이 유클리디안이라고 가정하고 있으며, 이 경우에는 SLAM에 사용할수 없으며, 하부 최적의 해를 구해야 할수 있습니다.
그래프 기반 SLAM 방법은 가공되지 않은 센서 데이터들 이용하여 간소화된 추정 문제로 만듭니다. 이러한 로 관측 데이터들은 그래프 상에 에지로 대체되며, 가상의 측정치들로 볼수 있겠습니다. 더 자세하게 설명하자면, 두 노드 사이에 에지가 존재하며 이들은 두 자세 사이의 상대적인 위치가 주어질때 관측치에 대한 확률 변수로 표기 됩니다.
일반적으로 관측 모델 p($p_t$ | $x_t$, $m_t$)는 멀티 모달이므로 가우시안 가정을 적용할수 없습니다. 이런 이유로 단일 관측 $z_t$는 그래프 상에 존재하는 다른 자세들과 연결되는 다중 포텐셜 에지들이 될수 있으며, 그래프 연결성은 확률 분포로 설명해야 합니다. 추정 과정에서 멀티 모달을 직접적으로 다룬다면 복잡도가 크게 증가될수 있습니다. 그 결과 가장 실용적인 방법들은 가장 가능성있는 토플로지로 추정치들을 제한합니다.
그래서 관측을 이용하여 가장 높은 가능성을 가진 제약을 결정하여야 합니다. 이 결정은 로봇의 자세에 대한 확률 분포를 이용하며, 이 문제를 데이터 연관으로 볼수 있으며, SLAM의 프론트앤드에서 주로 다루게 됩니다. 올바른 데이터 연관을 구하기 위해서 프론트엔드는 로봇의 궤적 p($x_{1:T}$ | $z_{1:T}$, $u_{1:T}$)에 대한 조건부 사전확률의 일관적인 추정치를 필요로 합니다. 이 작업은 로봇이 주행하는 동안 프론트 앤드와 백앤드 사이에 끼워져야 합니다. 그러므로 백앤드의 정확도와 효율성은 좋은 SLAM 시스탬을 설계하는대 매우 중요합니다.
이 튜토리얼에서는 데이터 연관에 대한 복잡한 방법을 다루지는 않을 것이나, 그런 방법들로는 스팩트럴 클러스터링, 조건부 한정분기나 백트래킹 방법같은게 있습니다. 이런걸 고려하기 보다는 프론트엔드에서 일관된 추정치를 주고있다고 가정해서 보겠습니다.
그림 6. SLAM 과정의 자세 그래프 표현. 그래프 상의 모든 노드들은 로봇의 자세를 의미합니다. 가까운 자세들끼리는 에지로 연결되어 있으며, 이 에지는 로봇의 자세 사이에서 관측으로 생기는 공간적인 제약들을 나타냅니다. 에지 $e_{t-1 t}$는 연속적인 자세들 사이에 있으며, 오도메트리 측정치를 설계하며, 다른 에지들은 동일하 환경에서 다중 관측으로부터 만들어지는 공간적인 제약조건들을 의미합니다.
관측이 가우시안 노이즈의 영향을받고, 데이터 연관을 알고 있다면 그래프 기반 지도작성 알고리즘의 목표는 로봇의 궤적에 대한 가우시안 사후확률 근사를 계산하는것이 될겁니다. 이는 관측 우도를 최대화시키도록 하는 노드 형태로서 가우시안의 평균을 계산하는 것을 다루게 됩니다. 이 평균은 가우시안의 정보 행렬이라고도 알려져 있습니다. 앞으로 제약조건 최적화 문제로 이러한 최대치를 찾는 작업들에 대해 정리하겠습니다. 그리고 앞으로 사용할 표기법들을 그림 6에서 소개하겠습니다.
로봇 공학 커뮤니티에 수많은 SLAM 방법들이 연구되고 있습니다. 이 튜토리얼 전반에서 그래프 기반의 방법들을 중심으로 다룰것이고,
이러한 방법과 관련된 연구들에 대해서도 살펴보겠습니다.
Lu와 Milos는 제약을 이용해서 오차를 줄이는 시스템 방정식들을 전역적으로 최적화하는 지도를 처음 정의하였습니다. Gutmann은 그런 네트워크를 만들고, 중분 추정 알고리즘이 동작하는 동안 루프 폐쇄를 검출하는 효율적인 방법을 제안하였습니다. 이후에는 제약 네트워크 상의 오차를 최소화하는 수많은 방법들이 나왔는데요.
예를들자면 Howard는 로봇의 위치 추정과 지도 작성에 완화를 적용하였습니다. Frese는 멀티 레벨 완화 multi level relaxation이라 부르는 가우스-시델 완화의 변형 기술을 제안하였으며, 서로다른 해상도에서 완화기술이 적용됩니다. Dellaert와 kaess는 처음으로 오프라인 슬램에서 선형화된 문제를 풀기 위해 희소 행렬 분해를 사용하였는데요. 이후 Kaess가 희소 분할을 계산하는 편 재정렬을 사용한 온라인 버전인 iSAM을 소개하였습니다.
최근 Konolige는 효율적인 방법으로 선형화된 시스템을 생성하는 포즈 그래프 방법을 오픈소스로 구현하여 소개하였습니다. Olson은 확률적 경사 하강법을 기반하는 최적화 방법을 소개하였고, 이를 통해 큰 크기의 포즈그래프도 효과적으로 고칠수가 있게 되었습니다. Grisetti는 노드들을 트리로 나타내는 Olson의 방법의 개선판을 소개하였으며 이를 통해 수렴 속도를 올렸습니다.
GraphSLAM은 최적화 문제의 차원성을 줄이기 위해 다양한 제거 기술들을 적용한 것이고, ATLAS 프레임워크는 두 개층의 그래프를 만들어 냅니다. 바텀 계층을 만들기 위해서 Kalman filter를 사용하였고, 2계층에서 전역 최적화 방법으로 지역 지도를 정렬하였습니다. ATLAS와 비슷하게, Estrada는 독립적인 지역들을 사용하는 계층 SLAM을 제안하였습니다.
많은 최적화 기술들이 주어진 제약관계를 이용해서 최적의 지도를 계산하는데 목표를 두고 있으며 이를 SLAM 백엔드라고 부릅니다. 이와 반대로 SLAM 프론트엔드는 센서 데이터를 해석하여 최적화 방법에 기반이 되는 제약관계를 얻습니다. Olson은 스펙트럴 클러스팅을 기반으로하는 아웃라이어 제거를 사용한 프론트엔드 기술을 소개하였는데요. SLAM 프론트엔드에서 데이터 연관을 만들기 위해 통계학적인 테스트 $\xi^2$ 시험이나 결합 확률 시험 같은것들이 종종 사용되기도 합니다.
Nuchter의 연구에서는 3차원 지도 작성을 SLAM 시스템에 통합하는 것을 목표로 하였는데, 이 연구에서 매인 포커스는 SLAM 프론트엔드에서 제약조건들을 찾는것이었습니다. 최적화를 위해서 Lu와 Millios의 3차원 세팅을 위한 다양한 방법들이 사용되었고, 이러한 방법들이 프론트엔드 상에서 효과적으로 사용될수 있습니다.
SLAM 문제를 푸는것은 로봇의 궤적과, 로봇이 이동한 황경의 지도를 추정하는것으로 이루어집니다. 센서에는 노이즈를 갖고 있기 때문에 SLAM 문제를 확률적인 도구를 사용해서 나타냅니다. 로봇이 모르는 환경에서 이동한다고 할때, 궤적의 시퀀스를 확률변수 $x_{1:T}$ = {$x_1$, . . ., $x_T$}로 표기하겠습니다. 이동하는 동안 로봇은 오도메트리 측정 시퀀스인 $u_{1:T}$ = {$u_1$, . . .,$u_T$}와 관측 시퀀스인 $z_{1:T}$ = {$z_1$, . . .,$z_T$} 또한 얻게 됩니다.
완전 SLAM을 푸는 는 것은 모든 관측치들과 초기자세 $x_0$가 주어질때, 로봇의 궤적 $x_{1:T}$와 지도 m의 사후확률을 추정하는것으로 이루어 집니다.
초기 자세 $x_0$는 지도의 위치를 정의하며, 임의로 지정할수 있습니다. 편하게 표기하기 위해서 이 문서에서는 $x_0$는 생략하겠습니다. 자세들인 $x_{1:T}$와 오도메트리 $u_{1:T}$는 SE(2)나 SE(3)에서 2차원 혹은 3차원으로 변환되어 표현되기도 합니다.
지도는 점유격자같은 밀집된 표현이나 처리되지 않은 센서측정치로 공간상에 존재하는 랜드마크의 집합을 나타낸것이라 할수 있습니다. 어떤 지도 표현법을 사용할지는 사용하는 센서나, 주위 환경의 특성, 추정 알고리즘에 따라 선택하여야 합니다.
랜드마크 지도는 특징들이 쉽게 구분할수 있는 환경에서 선호되며, 특히 카레라를 사용하는 경우 그렇습니다. 하지만 밀집 지도에서는 레이저 거리계들이 사용되곤 합니다. 이러한 타입에 독립적으로 지도는 측정치와 그 측정치를 획득한 장소를 이용하여 만들어 집니다.
그림 2에서는 2차원과 3차원으로 3가지 밀집 지도 표현을 보여주고 있는데, 다층 표면 지도와 점구름, 점유격자가 있습니다. 그림 3의 경우는 일반적인 2차원 랜드마크 기반 지도를 보여줍니다.
그림 3. 독일 항공 센터에서 취득한 특징 기반 지도. 이 환경에서 랜드마크는 딱 위에 흰 원으로 이루어지며, 이 랜드마크는 왼쪽 그림처럼 로봇이 영상을 이용하여 감지 합니다. 우측 그림은 로봇의 궤적과 랜드마크의 추정 위치들을 보여주고 있습니다.
식 (1)의 사후확률을 추정하는데에는 고차원 상태 공간에서 연산이 수행됩니다. 그래서 SLAM 문제가 구조적으로 잘 정의되지 않는데면, 추적을 수행할수 없게 됩니다. 이 구조는 확실하고 일반적인 가정들을 따르는데, 이러한 가정으로 정적 환경 과정과 마르코브 가정이 있겠습니다.
그림 4. SLAM 과정의 동적 베이지안 네트워크
이러한 구조를 표현하기 위한 편리한 방법은 그림 4와 같은 동적 베이지안 네트워크 DBN을 사용하는것이 되겠습니다. 베이지안 네트워크는 유향 그래프로 확률 과정을 나타내는 그래프적인 모델이라 할수 있습니다. 이 그래프는 과정에 있어서 각 확률 변수로 노드와 두 노드사이에 조건부 의존을 나타내는 에지를 가지고 있습니다.
그림 4는 파란/회색 노드로 구분할수 있겠는데, 이는 측정된 변수들($z_{1:T}$, $u_{1:T}$)이고, 흰색 노드들은 은닉 변수들입니다. 은닉 변수인 $x_{1:T}$과 m은 로봇의 궤적과 주위에 대한 지도를 나타냅니다. DBN의 연결은 상태 전이 모델과 관측 모델에 의한 재귀적인 패턴으로 이루어 집니다.
상태 전이 모델 p($x_{t}$ | $x_{t-1}$, $u_t$)는 $x_t$를 구하는데 사용되는 2개의 에지들로 이루어지고, 시간 t-1에서의 로봇의 상태와 오도메트리 측정 $u_t$로 시간 t의 로봇의 상태에 대한 확률을 의미합니다. 관측 모델 p($z_t$ | $x_t$, $m_t$)는 로봇의 자세 $x_t$에서 수행한 관측 $z_t$의 확률로, $z_t$로 들어가는 화살표로 나타냅니다. $z_t$는 외부 수용적인 관측치로 정적인 지도 m와 현재 로봇의 위치 $x_t$에 의해 결정 됩니다.
SLAM을 DBN으로 표현함으로서 임시적인 구조를 강조할수 있고, 이를 통해 SLAM 문제를 다룰수 있는 필터 처리 과정을 묘사하는데 적합하게 됩니다.
DBN의 대안으로 그래프 기반, 네트워크 기반이라고 부르는 방법이 있는데, 이러한 방법은 공간적인 구조를 강조하게 됩니다. 그래프 기반 SLAM에서는 로봇의 자세들이 노드들로 설계되고, 이러한 자세들에 대해 라벨을 붙이게 됩니다. 이러한 자세들 간에 공간적인 제약(관측 $z_t$나 오도메트리 측정 $u_t$로 얻을수 있는)들은 노드들의 사이에 에지로 나타냅니다.
더 자세히 설명하자면 그래프 기반 슬램 알고리즘은 센서 측정치로 그래프를 만들어 냅니다. 그래프에서 각 노드들은 로봇의 위치와 해당 위치에서 얻은 측청치들을 나타내고, 두 개의 노드 사이의 에지는 로봇의 2개의 자세들 간에 관련된 공간적인 제약을 나타냅니다. 이 제약은 두 자세 사이의 상대적인 변환에 대한 확률 분포로 이루어 집니다. 이러한 변환들은 로봇의 자세 시퀀스 사이의 오도메트리 측정치이거나 두 장수에서 취득한 관측 데이터에 의해 정해질수 있겠습니다.
그래프가 생성이되면, 제약 조건들에 최적으로 만족하는 로봇 자세들의 형태를 찾게 됩니다. 그래서 그래프 기반 SLAM 문제는 두가지 작업으로 분리될수 있겠습니다. 우선 가공되지 않은 측정 데이터로 그래프를 생성하는 그래프 생성 작업과 그래프의 에지들이 주어질때 가장 높은 가능성을 가지는 자세 설정을 결정하는 그래프 최적화 작업이 있습니다.
그래프 생성 작업을 프론트 엔드라고도 부르며, 매우 센서에 의존적인 작업입니다. 두번째의 것을 백엔드라고 부르며, 이는 데이터의 추상적인 표현이나 센서에는 자유롭습니다. 2차원 레이저 SLAM에서 프론트 엔드의 간단한 예시가 5번째 장의 A에서 볼수 있습니다.
그림 5. MIT 킬리언 재판소에서 기록한 데이터 셋으로 만든 자세 그래프로 오른쪽은 최적화 후를 보여주고 있습니다. 이 지도는 그래프 상에 로봇의 자세들에 따른 레이저 스캔으로 만들었습니다.
이 튜토리얼에서는 쉽게 구현할수 있으나 효율적인 백엔드로 구성된 그래프 기반 SLAM에 대해서 소개하겠습니다. 그림 5번은 올바르지 않은 자세 그래프와 올바르게된 경우를 보여주고 있습니다.
그림 2는 이 논문에서 설명할 SLAM알고리즘으로 추정할수 있는 2차원 3차원 지도를 보여주고 있습니다.
그림 2. (a) 센서를 내장한 차로 취득한 스탠포드대 주차장의 3차원 지도(아래)와 위성 뷰(위). 이 지도는 차후에 자율 주차 동작을 하는대 사용됩니다. (b) Freiburg 대에서 취득한 점군 지도와 위성 뷰, (c) Freiburg 병원에서 얻은 점유 격자 지도. 위 : 버드아이뷰, 아래 : 점유 격자 표현. 여기서 회색 공간은 관측되지 않은 영역이고, 흰 부분은 움직일 수 있는 공간, 검은 점들은 점유된 공간을 나타냅니다.
SLAM 문제를 다루기 위한 직관적인 방법을 그래프 기반 방법이라 하겠습니다. 이 그래프 기반의 SLAM을 푸는 것에는 로봇의 자세나 랜드마크를 나타내는 노드와 노드 사이에서 연결된 자세들을 제약하는 센서 관측을 부호화하는 에지를 만드는 것을 포함합니다.
이러한 제약들은 센서가 노이즈에 항상 영향을 받으니 모순될수 있겠지만, 그래프가 생성이되면 이 치명적인 문제는 관측이 일관되게 최대가 되도록 노드의 형태를 찾아 해결할 수 있습니다.
그래프 기반 SLAM 문제는 1997년 Lu와 Milios가 제안했지만, 표준 기술을 사용해서 오차 최소화 문제를 풀기에는 높은 복잡도때문에 이 방법이 대중화되기 까지는 오랜 세월이 걸렸습니다. 최근 SLAM 문제의 구조에 대한 통찰과 희소 선형 대수 분야의 진보로 최적화 문제를 효율적으로 풀게 되었습니다.
결과적으로 그래프 기반 SLAM 방법은 르네상스기를 겪는 중이고, 현재 빠르고 정확한 최신 알고리즘이라고 할수 있습니다. 이 튜토리얼의 목적은 SLAM 문제를 확률적인 형태로 소개하고, 독자들에게 최신 그래프 기반의 SLAM 방법을 안내하고자 합니다.
이 튜토리얼을 이해하기 위해서 선형 대수와 ㄷ변수 최소화, 확률 이론에 대해서 이해하고 있어야 합니다.
