LiDAR SLAM의 기본 개념 - 소프트웨어 융합

LiDAR SLAM (Simultaneous Localization and Mapping)은 자율 주행 로봇이나 드론과 같은 이동체가 주어진 환경에서 자신의 위치를 파악하고 동시에 주변 환경의 지도를 구축하는 기술이다. 이 과정은 복잡한 수학적 원리와 알고리즘을 바탕으로 동작하며, 특히 3D 공간에서의 정확한 위치 추정과 지도의 품질은 매우 중요한 요소다.

SLAM의 기본 문제

SLAM의 핵심 문제는 다음과 같은 두 가지 주요 요소로 나눌 수 있다:

로봇의 위치 추정 (Localization): 로봇이 환경 내에서 자신의 위치를 어떻게 추정할 것인가?
지도 작성 (Mapping): 로봇이 주변 환경의 지도를 어떻게 작성할 것인가?

이 두 가지 문제는 상호 의존적인 관계에 있다. 로봇의 위치를 정확히 알아야 지도를 정확하게 그릴 수 있으며, 정확한 지도가 있어야 로봇이 자신의 위치를 정확히 추정할 수 있다. 이와 같은 상호 의존성을 해결하는 것이 SLAM 알고리즘의 핵심 과제다.

LiDAR를 이용한 SLAM

LiDAR는 레이저를 이용해 거리 정보를 측정하는 센서로, 특히 3D 공간에서 매우 높은 해상도로 정확한 환경 정보를 제공할 수 있다. 이러한 특성 덕분에 LiDAR는 SLAM에 자주 사용되며, 주로 2D 또는 3D SLAM 알고리즘에서 활용된다.

LiDAR 기반의 SLAM은 환경의 특징점 (feature point)을 감지하고 이를 바탕으로 로봇의 위치를 추정하는 방식으로 작동한다. LiDAR에서 생성된 포인트 클라우드(point cloud)는 로봇의 현재 위치와 지도를 갱신하는 데 사용된다. 이 과정에서 여러 가지 수학적 기법이 동원된다.

수학적 모델

LiDAR SLAM의 기본 수학적 모델은 로봇의 위치와 지도를 상태 벡터로 표현하고, 이를 확률론적 방법으로 추정한다. 로봇의 상태는 다음과 같이 정의된다:

$\mathbf{x}_t = \begin{bmatrix} x_t \\ y_t \\ \theta_t \end{bmatrix}$

여기서 $\mathbf{x}_t$ 는 시간 $t$ 에서의 로봇의 상태를 나타내는 벡터이며, $x_t$ 와 $y_t$ 는 로봇의 2D 공간 상의 좌표, $\theta_t$ 는 로봇의 방향(heading)을 나타낸다.

LiDAR로부터 측정된 환경 정보는 센서 모델로 표현되며, 다음과 같은 측정 벡터로 나타낼 수 있다:

$\mathbf{z}_t = \begin{bmatrix} z_{t,1} \\ z_{t,2} \\ \dots \\ z_{t,n} \end{bmatrix}$

여기서 $\mathbf{z}_t$ 는 시간 $t$ 에서 LiDAR 센서로 측정된 거리 정보이다. 각 측정값 $z_{t,i}$ 는 로봇과 환경 내의 특정 지점 간의 거리를 나타낸다.

확률론적 접근

SLAM 문제는 불확실성을 동반하기 때문에 확률론적 접근이 필요하다. 특히, 상태 추정과 측정 업데이트 과정에서 베이즈 필터가 널리 사용된다. 베이즈 필터는 로봇의 상태 $\mathbf{x}_t$ 에 대한 확률 분포 $p(\mathbf{x}_t | \mathbf{z}_{1:t}, \mathbf{u}_{1:t})$ 를 추정하는 방식으로 동작한다.

여기서, - $\mathbf{z}_{1:t}$ : 시간 1부터 $t$ 까지의 모든 센서 측정값 - $\mathbf{u}_{1:t}$ : 시간 1부터 $t$ 까지의 모든 제어 명령

베이즈 필터의 기본적인 과정은 다음과 같이 두 단계로 나눌 수 있다:

예측 단계 (Prediction Step): 로봇의 이전 상태와 제어 명령을 바탕으로 현재 상태를 예측한다.

예측 모델은 다음과 같다:

$p(\mathbf{x}_t | \mathbf{u}_t, \mathbf{x}_{t-1}) = \int p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t) p(\mathbf{x}_{t-1} | \mathbf{z}_{1:t-1}, \mathbf{u}_{1:t-1}) d\mathbf{x}_{t-1}$

갱신 단계 (Update Step): LiDAR로부터 측정된 데이터를 바탕으로 예측된 상태를 갱신한다.

갱신 모델은 다음과 같다:

$p(\mathbf{x}_t | \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) = \eta p(\mathbf{z}_t | \mathbf{x}_t) p(\mathbf{x}_t | \mathbf{z}_{1:t-1}, \mathbf{u}_{1:t})$

여기서 $\eta$ 는 정규화 상수이다.

지도 작성 (Mapping)

LiDAR SLAM에서 지도를 작성하는 과정은 로봇이 수집한 LiDAR 데이터를 기반으로 환경의 2D 또는 3D 구조를 점진적으로 구축하는 작업이다. 이때, 지도의 각 요소는 주로 그리드 형태의 공간 표현 방식 또는 포인트 클라우드로 표현된다.

그리드 기반 지도 (Grid-based Map)

그리드 기반 지도는 환경을 일정한 크기의 셀로 나누어 각 셀에 특정한 확률 값을 할당하는 방식이다. 각 셀은 로봇이 그 지점을 지나갈 수 있는지 여부(점유 여부)를 나타낸다. 각 셀의 점유 확률은 $p(m_i | \mathbf{x}_t, \mathbf{z}_t)$ 로 표현되며, 여기서 $m_i$ 는 지도의 i번째 셀을 의미한다. 이 확률 값은 시간이 지남에 따라 로봇의 위치 추정 및 LiDAR 측정값을 통해 점진적으로 갱신된다.

포인트 클라우드 기반 지도 (Point Cloud Map)

포인트 클라우드 방식은 LiDAR 센서로부터 직접 수집된 거리 데이터를 3D 좌표로 변환하여, 해당 좌표들을 모아놓은 방식이다. 이 방식은 특히 3D SLAM에 널리 사용되며, 포인트 클라우드는 로봇이 움직일 때마다 LiDAR로 측정된 각 지점의 공간 좌표들이 더해지면서 점진적으로 환경의 구조가 형성된다.

포인트 클라우드의 각 점은 다음과 같은 3D 좌표로 표현될 수 있다:

$\mathbf{p}_i = \begin{bmatrix} x_i \\ y_i \\ z_i \end{bmatrix}$

여기서 $\mathbf{p}_i$ 는 i번째 포인트의 3차원 좌표를 나타낸다.

포인트 클라우드 기반 지도 작성에서는 주로 ICP (Iterative Closest Point) 알고리즘이 사용된다. ICP 알고리즘은 두 포인트 클라우드 간의 변환 행렬 $\mathbf{T}$ 를 계산하는데, 이를 통해 로봇의 이동 경로에 따른 지도 상의 좌표를 일치시킨다.

상태 및 지도 추정의 통합

SLAM 문제는 로봇의 상태 추정과 지도의 추정이 상호 의존적인 문제임을 다루기 때문에, 이를 효과적으로 해결하기 위해 상태-지도 공동 추정(joint state-map estimation)이 필요하다. 이 개념은 로봇의 상태와 환경 지도를 하나의 확률 분포로 모델링하는 것이다.

다음과 같이 로봇의 상태와 지도를 결합한 상태 벡터 $\mathbf{x}_t$ 와 지도를 고려할 수 있다:

$p(\mathbf{x}_t, m | \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) = p(\mathbf{x}_t | \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) p(m | \mathbf{x}_t, \mathbf{z}_{1:t})$

여기서 $m$ 은 환경의 지도를 나타내며, 이 확률 분포는 베이즈 필터의 업데이트 과정에서 동시적으로 추정된다.

루프 클로징 (Loop Closing)

SLAM에서 중요한 개념 중 하나는 루프 클로징(loop closing)이다. 로봇이 이미 방문했던 장소를 다시 방문했을 때, 기존의 지도와 새로운 측정 데이터를 비교하여 로봇의 위치와 지도의 불확실성을 줄이는 과정이다. 루프 클로징을 통해 지도에서의 드리프트(drift)를 최소화할 수 있으며, 이는 SLAM 알고리즘의 정확도를 크게 향상시킨다.

루프 클로징을 수행하기 위해선 로봇이 과거에 있었던 장소를 인식하는 장소 재인식(place recognition) 알고리즘이 필요하다. 대표적인 방법으로 FAB-MAP이나 Bag of Words (BoW)가 있다.

LiDAR SLAM의 주요 과제

LiDAR SLAM은 여러 장점에도 불구하고 다음과 같은 여러 가지 과제가 있다:

노이즈 처리: LiDAR 센서에서 발생하는 노이즈(잡음)와 오차를 처리하는 것은 매우 중요한 문제이다. 특히, 반사율이 낮거나 평면적인 물체는 LiDAR 측정에 혼선을 줄 수 있다.
계산 복잡도: SLAM의 상태 추정 및 지도 작성은 매우 계산 집약적이기 때문에, 실시간 처리를 위해서는 효율적인 알고리즘 설계가 필수적이다.
다양한 환경에 대한 대응: LiDAR SLAM은 환경의 복잡도와 밀접하게 관련되어 있다. 예를 들어, 고정된 환경과 동적인 환경에서의 SLAM 성능은 매우 다를 수 있다.

LiDAR SLAM의 구조적 구성 요소

LiDAR SLAM은 여러 알고리즘적 구성 요소로 이루어져 있으며, 각 요소는 SLAM의 성능과 정확도에 중요한 역할을 한다. 이러한 구성 요소들은 주로 다음과 같은 과정을 포함한다:

전역 위치 추정 (Global Localization)

로봇이 처음 환경에 진입하거나, 초기 위치에 대한 정보가 부족할 때 로봇은 자신의 위치를 전역적으로 추정해야 한다. 이를 전역 위치 추정이라고 하며, 초기 위치를 전혀 모르는 상태에서 시작하는 경우, 이를 해결하기 위해 입자 필터 (Particle Filter)가 널리 사용된다.

입자 필터는 로봇의 위치에 대한 다수의 가설을 생성하고, 이를 점진적으로 업데이트하여 가장 가능성 있는 위치를 추정하는 방법이다. 상태 벡터 $\mathbf{x}_t$ 에 대한 다수의 샘플을 입자로 나타내며, 각 입자는 위치 가설을 나타낸다. 이때 각 입자의 가중치는 측정 데이터를 기반으로 계산된다.

입자 필터의 상태 공간은 다음과 같이 정의될 수 있다:

$\mathbf{S}_t = \{ \mathbf{x}_t^{(i)}, w_t^{(i)} \}_{i=1}^N$

여기서, - $\mathbf{x}_t^{(i)}$ 는 i번째 입자의 상태, - $w_t^{(i)}$ 는 i번째 입자의 가중치, - $N$ 은 입자의 개수이다.

입자 필터의 과정은 크게 세 가지 단계로 나눌 수 있다: 1. 예측(Prediction): 각 입자를 이동 모델을 통해 예측. 2. 갱신(Update): LiDAR 측정 데이터를 바탕으로 각 입자의 가중치를 갱신. 3. 리샘플링(Resampling): 가중치가 높은 입자를 더 많이 선택하여 새로운 샘플을 생성.

지역적 위치 추정 (Local Localization)

로봇이 한 번 초기 위치를 파악한 후, 그 이후에는 상대적으로 작은 범위에서 위치 추정 문제를 해결한다. 이를 지역적 위치 추정이라 하며, 주로 EKF-SLAM (Extended Kalman Filter SLAM) 또는 직접 매칭 기법이 사용된다. 이 방법들은 로봇의 연속적인 위치 추정에서 높은 정확도를 제공한다.

EKF-SLAM에서 로봇의 상태와 환경 특징점(feature points)들은 연속적으로 확장된 칼만 필터에 의해 갱신된다. 칼만 필터의 상태 벡터는 로봇의 위치와 환경의 특징점을 동시에 포함한다:

$\mathbf{x}_t = \begin{bmatrix} \mathbf{x}_{robot, t} \\ \mathbf{f}_{1:t} \end{bmatrix}$

여기서, - $\mathbf{x}_{robot, t}$ : 로봇의 상태 벡터, - $\mathbf{f}_{1:t}$ : 시간 $t$ 까지의 환경 특징점들이다.

EKF-SLAM의 기본적인 프로세스는 다음과 같다: 1. 예측(Prediction): 이동 모델을 기반으로 로봇의 상태를 예측한다. 2. 갱신(Update): LiDAR 데이터를 통해 특징점을 감지하고, 이를 바탕으로 상태 벡터를 갱신한다.

센서 융합 (Sensor Fusion)

LiDAR SLAM은 때때로 단일 LiDAR 센서의 한계를 극복하기 위해 센서 융합을 통해 다른 센서의 데이터를 함께 활용한다. 예를 들어, 카메라를 사용하여 시각 정보를 결합하거나, IMU(Inertial Measurement Unit)와의 결합을 통해 로봇의 이동 궤적을 보정할 수 있다.

센서 융합의 주요 방법으로는 확장 칼만 필터 (EKF), 비선형 차분 필터 (Unscented Kalman Filter, UKF), 정보 필터 (Information Filter) 등이 사용되며, 이를 통해 각 센서의 불확실성을 상호 보완하여 더욱 정확한 위치 추정과 지도를 생성한다.

데이터 연속성 및 드리프트 보정

LiDAR SLAM에서 또 다른 중요한 문제는 데이터 연속성과 드리프트 보정이다. 로봇이 이동하는 동안 측정된 데이터는 시간이 지남에 따라 누적되며, 이 과정에서 작은 오차들이 누적되어 전체 지도에 큰 드리프트(drift)를 일으킬 수 있다. 이러한 문제를 해결하기 위해 여러 가지 보정 기법이 적용된다.

대표적인 기법으로 전역 최적화 (Global Optimization) 방법이 있으며, 이 방법은 로봇이 수집한 데이터와 지도의 누적된 오차를 최소화하는 방향으로 전체 지도를 최적화한다. 이때 일반적으로 그래프 기반 SLAM이 사용되며, 그래프 기반 SLAM에서는 로봇의 위치와 특징점을 그래프로 모델링하고, 이를 통해 최적화 문제를 푼다.

그래프 기반 SLAM (Graph-based SLAM)

그래프 기반 SLAM에서는 로봇의 위치와 환경의 특징점들을 노드로, 로봇의 이동 경로를 엣지로 모델링한다. 노드와 엣지로 구성된 그래프는 다음과 같은 형태로 표현된다:

$\mathcal{G} = (\mathcal{V}, \mathcal{E})$

여기서, - $\mathcal{V}$ : 로봇의 위치와 특징점을 나타내는 노드 집합, - $\mathcal{E}$ : 로봇의 이동과 측정을 나타내는 엣지 집합이다.

그래프 기반 SLAM에서 최적화 문제는 주어진 그래프 $\mathcal{G}$ 에서 각 노드 간의 위치 관계를 최적화하여 드리프트를 보정하는 방식으로 해결된다. 이때, 비선형 최소 제곱법 (Nonlinear Least Squares)을 적용하여 그래프를 최적화한다.

그래프 기반 SLAM의 최적화 문제

그래프 기반 SLAM에서 각 노드 간의 위치 관계는 제약식(constraints)으로 표현되며, 이러한 제약식들은 로봇의 이동이나 센서 측정을 기반으로 한다. 예를 들어, 로봇이 특정 위치에서 다른 위치로 이동했을 때의 변화를 나타내는 제약식은 다음과 같은 수식으로 표현할 수 있다:

$\mathbf{z}_{ij} = f(\mathbf{x}_i, \mathbf{x}_j) + \mathbf{n}_{ij}$

여기서, - $\mathbf{z}_{ij}$ 는 노드 $i$ 에서 노드 $j$ 로의 상대적인 이동 측정값, - $f(\mathbf{x}_i, \mathbf{x}_j)$ 는 두 노드 간의 상태 벡터의 관계를 나타내는 함수, - $\mathbf{n}_{ij}$ 는 측정 노이즈이다.

이러한 측정값을 기반으로 SLAM 문제는 각 노드의 상태를 최적화하는 문제로 바뀐다. 이를 위해 SLAM은 최소 제곱 오차를 줄이는 방향으로 노드들의 위치를 업데이트한다. 이때 목표는 아래의 목적함수를 최소화하는 것이다:

$\min_{\mathbf{x}} \sum_{(i,j) \in \mathcal{E}} \| \mathbf{z}_{ij} - f(\mathbf{x}_i, \mathbf{x}_j) \|_{\Sigma_{ij}}^2$

여기서, - $\| \cdot \|_{\Sigma_{ij}}$ 는 노이즈 공분산 행렬 $\Sigma_{ij}$ 를 고려한 거리 함수이다.

이 문제는 일반적으로 비선형 최소 제곱 문제로 해결되며, 가우스-뉴턴 방법(Gauss-Newton Method) 또는 Levenberg-Marquardt 알고리즘과 같은 최적화 기법들이 사용된다. 이러한 최적화 기법들은 각 노드의 위치를 반복적으로 갱신하여 그래프 상의 제약을 만족하도록 만든다.

정보 필터 (Information Filter)

그래프 기반 SLAM에서 자주 사용되는 또 다른 방법은 정보 필터(Information Filter)이다. 정보 필터는 상태 벡터를 확률 분포 대신 정보 행렬로 표현하며, 이를 통해 계산 복잡도를 줄이는 장점이 있다. 정보 행렬 $\mathbf{\Omega}$ 와 정보 벡터 $\mathbf{\xi}$ 는 상태 벡터와 공분산 행렬을 다음과 같은 방식으로 변환하여 표현한다:

$\mathbf{\Omega} = \Sigma^{-1}, \quad \mathbf{\xi} = \mathbf{\Omega} \mathbf{x}$

정보 필터를 사용하는 SLAM에서는 상태 벡터와 측정값을 정보 벡터와 정보 행렬로 변환하여 처리하므로, 계산의 효율성이 높아진다. 특히, 그래프 기반 SLAM에서 노드와 엣지의 수가 많을수록 정보 필터를 사용하는 방식은 매우 유리하다.

LiDAR SLAM에서의 드리프트 보정

SLAM에서 로봇의 위치가 계속해서 이동할 때, 위치 추정에서 발생하는 누적 오차를 드리프트(drift)라고 부른다. 드리프트는 지도 작성과 위치 추정의 정확도에 심각한 영향을 미칠 수 있다. 이를 보정하기 위한 방법으로 루프 클로징(Loop Closing)과 전역 최적화(Global Optimization)가 주로 사용된다.

루프 클로징 (Loop Closing)

루프 클로징은 로봇이 이미 지나간 경로를 다시 방문했을 때, 그 구간의 데이터를 기존의 지도와 비교하여 누적된 오차를 최소화하는 방법이다. 루프 클로징이 성공적으로 이루어지면, 전체 지도의 정확도가 크게 개선된다.

루프 클로징의 과정은 크게 두 가지 단계로 이루어진다: 1. 장소 재인식 (Place Recognition): 로봇이 이전에 방문했던 장소를 인식하는 과정으로, 이를 통해 로봇이 이미 지나간 경로를 식별할 수 있다. 2. 최적화 (Optimization): 장소 재인식이 완료되면, 로봇의 경로와 지도에서 발생한 누적 오차를 보정하기 위한 최적화 과정이 수행된다. 이를 위해 전역 최적화가 이루어지며, 그래프 기반 SLAM에서 노드 간의 관계를 재조정한다.

전역 최적화 (Global Optimization)

전역 최적화는 전체 경로와 지도를 고려하여 누적된 오차를 보정하는 방법이다. 이는 그래프 기반 SLAM에서 주로 사용되며, 로봇의 경로와 특징점 간의 관계를 재정의하여 정확한 지도를 구축한다. 전역 최적화는 특히 장시간 운영되는 로봇 시스템에서 중요한 역할을 하며, 지도와 위치 추정에서 발생하는 드리프트를 효과적으로 줄일 수 있다.

전역 최적화는 보통 포즈 그래프 최적화(Pose Graph Optimization)를 통해 이루어진다. 포즈 그래프는 로봇의 각 위치를 노드로, 이동 경로를 엣지로 나타내며, 이를 통해 전역적인 최적화를 수행한다. 이때 사용하는 대표적인 기법은 g2o(Graph Optimization) 또는 Ceres Solver와 같은 최적화 라이브러리들이다.

LiDAR SLAM은 LiDAR 센서 데이터를 기반으로 실시간으로 로봇의 위치를 추정하고 환경 지도를 작성하는 복잡한 기술이다. 이 기술은 확률론적 모델링, 그래프 기반 최적화, 센서 융합 등의 수학적 기법과 알고리즘을 통합하여 자율 주행 로봇이나 드론에서 실시간으로 환경을 탐색하고 위치를 추정하는 데 활용된다.