11.19 리 군 기반 SLAM의 수학적 기초

1. SLAM의 개요

SLAM(Simultaneous Localization and Mapping)은 알 수 없는 환경에서 로봇이 자신의 위치를 추정하면서 동시에 환경의 지도를 구축하는 문제이다. 자율 주행, 모바일 로봇, 무인 항공기, 증강 현실 등 다양한 분야에서 핵심 기술로 활용된다. SLAM의 수학적 기초는 본질적으로 리 군 위의 추정 문제이며, 로봇의 자세는 SE(3) 또는 SE(2)의 원소이고, 지도의 점은 일반적으로 유클리드 공간의 원소이다. 본 절에서는 리 군 기반 SLAM의 수학적 토대를 체계적으로 다룬다.

2. SLAM의 수학적 구조

2.1 변수의 정의

SLAM 문제에서 추정해야 할 변수는 두 가지이다.

• 로봇의 자세 궤적: \mathbf{T}_1, \mathbf{T}_2, \ldots, \mathbf{T}_n \in SE(3)
• 지도의 점 또는 랜드마크: \mathbf{l}_1, \mathbf{l}_2, \ldots, \mathbf{l}_m \in \mathbb{R}^3

2.2 측정값

측정값에는 다음과 같은 것들이 있다.

• 자기 운동 측정: 휠 인코더, IMU 등
• 외부 측정: 카메라, 라이다, 초음파 등
• 랜드마크 측정: 환경의 특징점 위치

2.3 문제 정의

SLAM의 목적은 측정값으로부터 자세 궤적과 지도를 동시에 추정하는 것이다.

P(\mathbf{T}_{1:n}, \mathbf{l}_{1:m} | \mathbf{z}_{1:k})

여기서 \mathbf{z}_{1:k}는 모든 측정값의 집합이다.

3. SLAM의 두 가지 접근법

3.1 필터 기반 SLAM

필터 기반 SLAM(filter-based SLAM)은 칼만 필터나 입자 필터를 사용하여 상태를 점진적으로 갱신한다. 측정이 도착할 때마다 상태가 갱신된다.

3.1.1 EKF SLAM

확장 칼만 필터를 사용한 SLAM이다. 상태 벡터에 로봇의 자세와 모든 랜드마크의 위치가 포함된다. 상태가 커지면 계산 비용이 급격히 증가한다.

3.1.2 입자 필터 SLAM

입자 필터를 사용한 SLAM이다. FastSLAM이 대표적이며, 자세는 입자 필터로, 랜드마크는 각 입자에 대한 EKF로 추정한다.

3.2 그래프 기반 SLAM

그래프 기반 SLAM(graph-based SLAM)은 모든 측정을 그래프로 표현하고, 전체 그래프를 최적화한다. 노드는 자세이고 엣지는 측정 제약이다.

3.2.1 장점

• 전체 정보를 활용한 최적 해를 산출한다.
• 큰 규모의 문제에서 효율적이다.
• 비선형 최적화 라이브러리를 활용한다.

3.2.2 알고리즘

g2o, GTSAM, Ceres Solver 등의 라이브러리가 그래프 SLAM을 지원한다.

4. 그래프 SLAM의 수학적 표현

4.1 노드와 엣지

그래프 SLAM은 다음의 구조를 가진다.

• 노드: 로봇의 자세 (또는 랜드마크 위치)
• 엣지: 두 노드 사이의 측정 제약

4.2 비용 함수

비용 함수는 모든 엣지의 잔차의 제곱합이다.

J = \sum_{(i, j) \in E}\lVert\mathbf{r}_{ij}(\mathbf{T}_i, \mathbf{T}_j)\rVert^2_{\Sigma_{ij}}

여기서 \mathbf{r}_{ij}는 잔차, \Sigma_{ij}는 측정 공분산이다.

4.3 잔차의 계산

자세 사이의 측정에 대한 잔차는 로그 사상을 사용하여 정의된다.

\mathbf{r}_{ij} = \log(\mathbf{T}_{ij}^{-1}\mathbf{T}_i^{-1}\mathbf{T}_j)^\vee

여기서 \mathbf{T}_{ij}는 측정된 상대 자세이다. 잔차는 6차원 벡터(트위스트)로 표현된다.

4.4 최적화

비용 함수의 최적화는 가우스-뉴턴법이나 레벤버그-마쿼트법으로 수행된다.

5. 자코비안의 계산

5.1 잔차의 자코비안

각 잔차의 자세에 대한 자코비안이 필요하다. 자세가 SE(3)의 원소이므로 리 군 위의 미분이 등장한다.

\frac{\partial\mathbf{r}_{ij}}{\partial\mathbf{T}_i}, \quad \frac{\partial\mathbf{r}_{ij}}{\partial\mathbf{T}_j}

이 자코비안의 닫힌 형태는 BCH 공식과 좌측/우측 야코비안을 사용하여 유도된다.

5.2 매개변수화

자세 변수의 매개변수화는 명목 자세 주위의 작은 변화로 이루어진다.

\mathbf{T}_i \leftarrow \mathbf{T}_i\exp(\hat{\boldsymbol{\delta}}_i)

여기서 \boldsymbol{\delta}_i가 새로운 매개변수이며, 자코비안은 \boldsymbol{\delta}_i에 대해 계산된다.

6. 정보 행렬과 헤시안

6.1 정보 행렬

가우스-뉴턴법의 정보 행렬(또는 헤시안 근사)은 다음과 같이 표현된다.

\mathbf{H} = \mathbf{J}^T\mathbf{W}\mathbf{J}

여기서 \mathbf{J}는 모든 잔차의 자코비안, \mathbf{W}는 가중치 행렬(공분산의 역)이다.

6.2 희소성

SLAM의 정보 행렬은 매우 희소(sparse)하다. 한 자세는 인접한 자세나 보이는 랜드마크와만 연결되기 때문이다.

6.3 효율적 풀이

희소 행렬을 활용하는 솔버(예: 콜레스키 분해, QR 분해)가 사용되어 효율적으로 풀린다.

7. 키프레임 기반 접근

7.1 키프레임의 선택

모든 프레임을 그래프에 포함시키면 그래프가 너무 커진다. 이를 해결하기 위해 키프레임(keyframe) 기반 접근이 사용된다.

7.2 키프레임의 정의

키프레임은 그래프에 포함되는 특별한 프레임이다. 일정 시간 간격이나 일정 거리 이동마다 새로운 키프레임이 생성된다.

7.3 비키프레임의 처리

비키프레임은 키프레임 사이의 트래킹에만 사용되며, 그래프 최적화에는 포함되지 않는다.

7.4 효율성

키프레임 기반 접근은 그래프 크기를 관리 가능한 수준으로 유지하며, 실시간 동작이 가능하다.

8. 시각적 SLAM

8.1 카메라 모형

시각적 SLAM에서 카메라는 핀홀 모형으로 표현된다. 3차원 점이 카메라 좌표계로 변환된 후 이미지 평면에 투영된다.

\mathbf{u} = \pi(\mathbf{K}, \mathbf{T}, \mathbf{p})

여기서 \mathbf{K}는 내부 매개변수, \mathbf{T}는 카메라 자세, \mathbf{p}는 3차원 점, \mathbf{u}는 픽셀 좌표이다.

8.2 재투영 오차

재투영 오차(reprojection error)는 측정된 픽셀 좌표와 예측된 픽셀 좌표 사이의 차이이다.

\mathbf{r}_{ij} = \mathbf{u}_{ij} - \pi(\mathbf{K}, \mathbf{T}_i, \mathbf{p}_j)

이 오차의 제곱합이 시각적 SLAM의 비용 함수이다.

8.3 번들 조정

번들 조정(bundle adjustment)은 모든 카메라 자세와 3차원 점을 동시에 최적화하는 과정이다. 이는 시각적 SLAM의 핵심 알고리즘이다.

8.4 자코비안

번들 조정의 자코비안은 카메라 자세와 3차원 점에 대해 계산된다. 카메라 자세의 자코비안은 리 군 위의 미분으로 표현된다.

9. 라이다 SLAM

9.1 점 군의 정합

라이다 SLAM은 점 군의 정합을 통해 자세를 추정한다. ICP(Iterative Closest Point) 알고리즘이 대표적이다.

9.2 ICP 알고리즘

ICP는 다음의 단계로 동작한다.

1. 두 점 군 사이의 대응점을 찾는다.
2. 대응점들을 가장 잘 정합시키는 변환을 계산한다.
3. 변환을 적용하고 반복한다.

이 과정에서 변환은 SE(3)의 원소이며, 리 군 위의 최적화로 풀린다.

9.3 라이다 그래프 SLAM

라이다 그래프 SLAM은 ICP의 결과를 그래프의 엣지로 사용한다. 전체 그래프 최적화를 통해 일관된 지도가 얻어진다.

10. 비주얼-관성 SLAM

10.1 IMU와의 결합

비주얼-관성 SLAM은 카메라와 IMU의 측정을 결합한다. IMU는 자기 운동을 측정하고, 카메라는 환경 정보를 제공한다.

10.2 미리 적분

두 키프레임 사이의 IMU 측정값은 미리 적분으로 단일 측정값으로 변환된다. 이는 IMU 측정의 효율적 처리를 가능하게 한다.

10.3 최적화

비주얼-관성 SLAM의 비용 함수는 시각 잔차(재투영 오차)와 IMU 잔차(미리 적분 잔차)의 결합이다.

J = \sum_{i, j}\lVert\mathbf{r}_{\text{visual}, ij}\rVert^2 + \sum_i\lVert\mathbf{r}_{\text{IMU}, i}\rVert^2

11. 회로 폐쇄

11.1 회로 폐쇄의 의미

회로 폐쇄(loop closure)는 로봇이 이전에 방문한 위치로 돌아왔을 때 이를 인식하는 것이다. 이는 누적된 오차를 보정하는 데 핵심적이다.

11.2 검출

회로 폐쇄 검출은 시각적 외관이나 다른 정보를 사용한다. Bag of Words(BoW), VLAD, 신경망 등이 사용된다.

11.3 그래프에 추가

검출된 회로 폐쇄는 그래프에 새로운 엣지로 추가된다. 이 엣지가 누적 오차를 분산시킨다.

11.4 강건성

잘못된 회로 폐쇄는 SLAM의 결과를 크게 손상시킬 수 있다. 강건한 검출과 검증이 필수이다.

12. 자세 그래프 최적화

12.1 자세 그래프

자세 그래프는 노드가 자세이고 엣지가 자세 사이의 상대 측정인 그래프이다. 단순한 형태의 그래프 SLAM이다.

12.2 비용 함수

비용 함수는 다음과 같다.

J = \sum_{(i, j) \in E}\lVert\log(\mathbf{T}_{ij}^{-1}\mathbf{T}_i^{-1}\mathbf{T}_j)^\vee\rVert^2_{\Sigma_{ij}}

12.3 최적화 알고리즘

가우스-뉴턴법, 레벤버그-마쿼트법, 토폴로지 알고리즘 등이 사용된다.

12.4 응용

자세 그래프 최적화는 라이다 SLAM, 비주얼 SLAM의 후처리 등에서 활용된다.

13. 강건성 처리

13.1 이상치 제거

측정값에 이상치(outlier)가 있을 수 있다. 잘못된 회로 폐쇄, 잘못된 특징점 매칭 등이 이상치이다.

13.2 M-추정

M-추정(M-estimator)은 이상치에 강건한 추정 방법이다. Huber, Cauchy 등의 손실 함수가 사용된다.

13.3 RANSAC

RANSAC(Random Sample Consensus)은 이상치가 많은 데이터에서 일관된 모델을 찾는 알고리즘이다. SLAM의 다양한 단계에서 사용된다.

13.4 강건 최적화

강건 최적화는 이상치에 대한 강건성을 명시적으로 고려한 최적화이다. M-추정과 결합하여 사용된다.

14. 분산 SLAM

14.1 다중 로봇 SLAM

여러 로봇이 협력하여 SLAM을 수행하는 문제이다. 각 로봇이 자신의 자세 그래프를 가지고, 협력 측정을 통해 그래프들이 연결된다.

14.2 분산 최적화

분산 최적화 알고리즘이 여러 로봇의 그래프를 효율적으로 결합한다. 통신 비용과 정확도의 균형이 중요하다.

14.3 응용

군집 로봇, 다중 드론 등에서 분산 SLAM이 활용된다.

15. 동적 환경의 SLAM

15.1 도전

동적 환경(사람, 차량 등)에서 SLAM은 더 어렵다. 동적 객체는 잘못된 특징점 매칭을 유발할 수 있다.

15.2 동적 객체 검출

동적 객체를 검출하고 SLAM에서 제외하는 방법이 사용된다. 객체 검출, 의미 분할 등이 활용된다.

15.3 동적 SLAM

동적 객체를 추적하는 동시에 환경을 매핑하는 동적 SLAM이 발전하고 있다.

16. 의미 SLAM

16.1 의미 정보의 활용

객체나 장면의 의미 정보를 SLAM에 통합하는 의미 SLAM(semantic SLAM)이 활발히 연구되고 있다.

16.2 객체 기반 SLAM

객체를 SLAM의 단위로 사용하는 객체 기반 SLAM은 의미 정보와 기하 정보를 결합한다.

16.3 응용

자율 주행, 증강 현실, 로봇 매니퓰레이션 등에서 의미 SLAM이 활용된다.

17. 학습 기반 SLAM

17.1 신경망의 활용

신경망이 SLAM의 일부 또는 전체에 통합되고 있다. 특징점 검출, 매칭, 자세 추정 등이 학습된다.

17.2 자세 학습

자세를 직접 학습하는 신경망(예: PoseNet)이 있다. 다양체 구조를 고려한 학습이 필요하다.

17.3 종단 SLAM

전체 SLAM 파이프라인을 학습하는 종단 SLAM(end-to-end SLAM)이 연구되고 있다. 다만 안정성과 일반화가 도전이다.

18. 라이브러리와 시스템

18.1 ORB-SLAM

ORB-SLAM은 가장 널리 사용되는 시각적 SLAM 시스템 중 하나이다. ORB 특징점을 사용하며, 단안, 스테레오, RGB-D 카메라를 모두 지원한다.

18.2 LSD-SLAM

LSD-SLAM은 직접 방식 시각적 SLAM이다. 특징점을 사용하지 않고 픽셀 강도를 직접 사용한다.

18.3 VINS-Mono / VINS-Fusion

VINS는 비주얼-관성 SLAM 시스템이다. IMU와 단안 카메라(또는 스테레오)를 결합한다.

18.4 Cartographer

Google의 Cartographer는 라이다 SLAM 시스템이다. 2D와 3D 지도를 모두 지원한다.

18.5 LIO-SAM

LIO-SAM은 라이다-관성 SLAM 시스템이며, IMU와 라이다를 결합한다.

18.6 Kimera

Kimera는 의미 SLAM 시스템이며, 객체 정보와 메쉬 재구성을 결합한다.

19. 학습의 가치

리 군 기반 SLAM의 수학적 기초를 깊이 이해하는 것은 다음과 같은 이점을 제공한다.

• SLAM 알고리즘의 본질을 이해할 수 있다.
• 새로운 알고리즘을 설계하고 분석할 수 있다.
• 다양한 SLAM 시스템의 차이와 장단점을 평가할 수 있다.
• 학술 문헌을 깊이 이해할 수 있다.
• 실제 시스템을 디버깅하고 개선할 수 있다.

20. 응용 예시: 자율 주행

자율 주행에서 차량의 위치와 환경 지도가 동시에 추정된다. HD 지도와의 정합, 라이다 SLAM, 비주얼 SLAM 등 다양한 기술이 활용된다.

21. 응용 예시: 무인 항공기

드론과 무인 항공기에서 비주얼-관성 SLAM이 표준이다. 카메라와 IMU의 융합으로 GPS 없이도 정확한 위치 추정이 가능하다.

22. 응용 예시: 증강 현실

증강 현실 기기에서 SLAM은 사용자의 시점 추적과 환경 인식의 핵심이다. Apple ARKit, Google ARCore 등이 SLAM 기술을 사용한다.

23. 응용 예시: 모바일 로봇

청소 로봇, 배달 로봇 등 모바일 로봇에서 SLAM은 자율 항법의 기초이다. 라이다 SLAM이나 시각적 SLAM이 사용된다.

24. 응용 예시: 수중 로봇

수중 환경에서는 GPS가 불가능하므로 SLAM이 위치 추정의 유일한 방법이다. 음향 센서, 카메라, IMU 등이 결합된다.

25. SLAM의 도전과 발전 방향

25.1 도전

• 큰 규모의 환경
• 동적 환경
• 조명 변화와 시각적 모호성
• 회로 폐쇄의 강건성
• 실시간 성능

25.2 발전 방향

• 학습 기반 방법의 통합
• 의미 정보의 활용
• 다중 센서 융합의 최적화
• 분산 SLAM
• 인간-환경 상호 작용

26. 참고 문헌

• Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., Reid, I., & Leonard, J. J. (2016). “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age.” IEEE Transactions on Robotics, 32(6), 1309–1332.
• Mur-Artal, R., Montiel, J. M. M., & Tardós, J. D. (2015). “ORB-SLAM: A versatile and accurate monocular SLAM system.” IEEE Transactions on Robotics, 31(5), 1147–1163.
• Qin, T., Li, P., & Shen, S. (2018). “VINS-Mono: A robust and versatile monocular visual-inertial state estimator.” IEEE Transactions on Robotics, 34(4), 1004–1020.
• Forster, C., Carlone, L., Dellaert, F., & Scaramuzza, D. (2017). “On-manifold preintegration for real-time visual-inertial odometry.” IEEE Transactions on Robotics, 33(1), 1–21.
• Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.
• Triggs, B., McLauchlan, P. F., Hartley, R. I., & Fitzgibbon, A. W. (2000). “Bundle adjustment—a modern synthesis.” International Workshop on Vision Algorithms, 298–372.

version: 1.0