7.134 SLAM 백엔드 최적화에서의 응용

1. 그래프 기반 SLAM의 구조

동시 위치 추정 및 지도 작성(Simultaneous Localization and Mapping, SLAM)에서 백엔드(back-end)는 프론트엔드(front-end)가 제공하는 센서 측정과 데이터 연관(data association) 정보를 기반으로 로봇의 포즈 궤적과 지도를 최적화하는 역할을 수행한다.

그래프 기반 SLAM에서 문제는 포즈 그래프(pose graph)로 표현된다. 노드는 로봇의 포즈(위치와 자세)를 나타내고, 에지는 연속 포즈 간의 오도메트리 측정 또는 루프 폐합(loop closure) 감지에 의한 상대 포즈 측정을 나타낸다.

2. 최적화 문제의 정식화

포즈 그래프의 노드 변수를 \mathbf{x} = [\mathbf{x}_1^T, \ldots, \mathbf{x}_N^T]^T로 하고, 에지 (i, j) \in \mathcal{E}에 대한 상대 측정을 \mathbf{z}_{ij}, 측정의 정보 행렬(information matrix)을 \boldsymbol{\Omega}_{ij}라 하면, 최적화 문제는 다음의 비선형 최소 제곱 문제이다.

\min_{\mathbf{x}} \quad \sum_{(i,j) \in \mathcal{E}} \mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j)^T \boldsymbol{\Omega}_{ij} \, \mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j)

여기서 오차 함수 \mathbf{e}_{ij}는 측정값과 현재 추정치로부터 예측된 상대 포즈의 차이이다.

2차원 SLAM의 경우 각 포즈가 (x, y, \theta) \in \mathbb{R}^2 \times SO(2)이고, 3차원 SLAM의 경우 (x, y, z, \text{자세}) \in \mathbb{R}^3 \times SO(3)이다.

3. 매니폴드 위의 최적화

3차원 자세는 리 군 SE(3)의 원소이며, 유클리드 공간의 벡터가 아니다. 따라서 갱신을 리 대수(Lie algebra) \mathfrak{se}(3)에서 수행하고, 지수 사상(exponential map)으로 리 군으로 투영하는 방법이 사용된다.

\mathbf{T}_{k+1} = \exp(\hat{\boldsymbol{\xi}}_k) \circ \mathbf{T}_k

여기서 \boldsymbol{\xi}_k \in \mathbb{R}^6은 리 대수에서의 갱신 벡터이다. 이 접근법은 회전의 과매개변수화 문제를 피하면서 국소적으로 유클리드 최적화와 동일한 구조를 제공한다.

4. 희소 구조의 활용

포즈 그래프의 야코비안은 매우 희소하다. 각 에지의 오차 함수가 두 개의 노드에만 의존하므로, 야코비안의 각 행에는 최대 두 개의 비영 블록만 존재한다. 정규 방정식 \mathbf{H}\Delta\mathbf{x} = -\mathbf{b}의 행렬 \mathbf{H} = \mathbf{J}^T\boldsymbol{\Omega}\mathbf{J}는 그래프의 인접 행렬과 동일한 희소 패턴을 갖는다.

순차적 포즈 에지만 존재하면 \mathbf{H}는 블록 삼대각이고, 루프 폐합 에지가 추가되면 대응하는 위치에 비영 블록이 발생한다. 희소 촐레스키 분해(CHOLMOD, Eigen 등)를 적용하여 효율적으로 풀 수 있으며, 적절한 변수 순서(ordering)의 선택이 채움(fill-in)을 최소화하여 분해 비용을 줄인다. 근사 최소 차수법(Approximate Minimum Degree, AMD)이 표준 순서 결정 방법이다.

5. 가우스-뉴턴과 레벤버그-마쿼트의 적용

SLAM 백엔드에서 가우스-뉴턴법 또는 레벤버그-마쿼트 알고리즘이 표준 해법이다. 각 반복에서 다음의 절차가 수행된다.

1. 현재 추정 \mathbf{x}_k에서 모든 에지의 오차 \mathbf{e}_{ij}와 야코비안 \frac{\partial \mathbf{e}_{ij}}{\partial \mathbf{x}_i}, \frac{\partial \mathbf{e}_{ij}}{\partial \mathbf{x}_j}를 계산한다.
2. 정보 행렬 \mathbf{H} = \sum_{(i,j)} \mathbf{J}_{ij}^T\boldsymbol{\Omega}_{ij}\mathbf{J}_{ij}와 정보 벡터 \mathbf{b} = \sum_{(i,j)} \mathbf{J}_{ij}^T\boldsymbol{\Omega}_{ij}\mathbf{e}_{ij}를 조립한다.
3. 희소 선형 시스템 \mathbf{H}\Delta\mathbf{x} = -\mathbf{b}를 풀어 갱신 방향을 구한다.
4. 매니폴드 위에서 갱신: \mathbf{x}_{k+1} = \mathbf{x}_k \boxplus \Delta\mathbf{x}

통상 5~20회의 반복으로 수렴한다.

6. 증분적 최적화

SLAM은 로봇이 이동함에 따라 새로운 포즈와 에지가 점진적으로 추가된다. 매번 전체 문제를 처음부터 풀면 비효율적이므로, 이전 해를 활용하여 증분적으로(incrementally) 갱신하는 방법이 연구되어 있다.

iSAM2(Incremental Smoothing and Mapping 2): 베이즈 트리(Bayes tree) 구조를 이용하여, 새로운 변수와 인수가 추가될 때 영향을 받는 부분만 선택적으로 재선형화하고 재분해한다. 전체 재최적화에 비해 현저히 적은 계산으로 해를 갱신할 수 있다.

7. 강건 비용 함수의 적용

루프 폐합의 오탐지(false positive)에 의한 이상치(outlier) 에지가 존재하면, 최소 제곱 비용이 이상치에 의해 심각하게 왜곡된다. 강건 비용 함수(후버, 코시, 동적 공분산 스케일링 등)를 적용하여 이상치의 영향을 억제한다.

8. 대표적 해법기

9. 참고 문헌

• Grisetti, G., Kümmerle, R., Stachniss, C., & Burgard, W. (2010). “A Tutorial on Graph-Based SLAM.” IEEE Intelligent Transportation Systems Magazine, 2(4), 31–43.
• Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., & Burgard, W. (2011). “g2o: A General Framework for Graph Optimization.” Proceedings of ICRA, 3607–3613.
• Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. J., & Dellaert, F. (2012). “iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree.” The International Journal of Robotics Research, 31(2), 216–235.
• Carlone, L., Tron, R., Daniilidis, K., & Dellaert, F. (2015). “Initialization Techniques for 3D SLAM: A Survey on Rotation Estimation and Its Use in Pose Graph Optimization.” Proceedings of ICRA, 4597–4604.

version: 1.0