FAST-LIVO 알고리즘 개요

1. LiDAR 및 Visual Inertial Odometry의 결합

FAST-LIVO(Fast LiDAR Inertial Visual Odometry)는 LiDAR SLAM과 VIO(Visual Inertial Odometry)를 결합하여 더욱 정밀한 자율 주행 로봇의 위치 추정을 수행하는 알고리즘이다. 이 알고리즘은 LiDAR 센서의 정밀한 거리 정보와 카메라와 IMU(Inertial Measurement Unit)의 관성 정보를 결합하여 로봇의 위치와 자세(orientation)를 추정한다. SLAM 문제는 기본적으로 로봇이 미지의 환경에서 이동하면서 자신의 위치와 주변 환경의 지도를 동시에 추정하는 문제를 다루며, FAST-LIVO는 이러한 SLAM 문제를 해결하기 위해 설계된 알고리즘 중 하나이다.

2. 알고리즘의 기본 구조

FAST-LIVO는 LiDAR 데이터를 기반으로 한 Odometry와 VIO 데이터를 결합하여 로봇의 위치를 추정한다. FAST-LIVO는 두 가지 주요 단계를 포함한다:

프론트엔드: LiDAR 데이터를 통해 3D 지형 정보와 로봇의 상대적 이동을 계산하는 단계이다. 여기서 FAST-LIVO는 FAST(Feature from Accelerated Segment Test)를 사용하여 특징점을 추출하고, 이를 LiDAR와 IMU 데이터를 결합하여 로봇의 움직임을 추정한다.
백엔드: LiDAR, VIO, IMU 데이터를 결합하여 로봇의 전역적인 위치 추정을 하는 단계이다. 최적화 알고리즘을 통해 로봇의 위치와 자세를 지속적으로 업데이트한다.

3. 프론트엔드: LiDAR 기반 특징 추출

FAST-LIVO에서 프론트엔드의 핵심은 LiDAR 데이터를 처리하여 특징점을 추출하는 것이다. LiDAR 데이터는 3D 포인트 클라우드를 형성하며, 이 포인트 클라우드에서 FAST 알고리즘을 사용하여 특징점을 추출하게 된다. 이러한 특징점은 이후의 위치 추정에 중요한 역할을 하며, 로봇이 이동할 때마다 갱신된다.

특징점 추출을 통해 얻어진 포인트 클라우드는 다음과 같은 수식으로 표현될 수 있다:

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

여기서 $\mathbf{p}_i$ 는 포인트 클라우드에서 $i$ 번째 점의 3D 좌표를 나타내며, $x_i$ , $y_i$ , $z_i$ 는 해당 점의 각 좌표축 성분이다.

4. LiDAR Odometry

LiDAR Odometry는 주어진 두 시점에서의 LiDAR 포인트 클라우드를 비교하여 로봇의 상대적인 움직임을 계산한다. 이를 위해 두 시점의 포인트 클라우드를 정렬하는 작업이 필요하며, 이를 수학적으로 설명하면 다음과 같다.

주어진 두 시점에서의 포인트 클라우드 $\mathbf{P}_1$ 와 $\mathbf{P}_2$ 가 있을 때, 이 둘을 정렬하여 최적의 변환 행렬 $\mathbf{T}$ 를 찾는다. 이 변환 행렬 $\mathbf{T}$ 는 회전 행렬 $\mathbf{R}$ 과 평행 이동 벡터 $\mathbf{t}$ 로 구성된다:

$\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{bmatrix}$

여기서, $\mathbf{R}$ 은 $3 \times 3$ 회전 행렬이고, $\mathbf{t}$ 는 $3 \times 1$ 이동 벡터이다. 두 시점의 포인트 클라우드가 주어졌을 때, 변환 행렬 $\mathbf{T}$ 를 통해 시점 $1$ 의 포인트 $\mathbf{p}_1$ 이 시점 $2$ 의 포인트 $\mathbf{p}_2$ 와 일치하도록 변환할 수 있다:

$\mathbf{p}_2 = \mathbf{T} \mathbf{p}_1$

이 과정을 통해 로봇의 상대적인 위치 변화를 계산할 수 있다.

5. 백엔드: 최적화 기반 위치 추정

FAST-LIVO의 백엔드에서는 LiDAR Odometry로 얻은 상대적인 위치 정보와 VIO에서 얻은 관성 정보를 결합하여 최종적인 로봇의 위치를 추정한다. 이 과정은 최적화 문제로 표현되며, 목적 함수는 각 센서에서 얻은 정보 간의 잔차를 최소화하는 방향으로 정의된다.

최적화 문제는 일반적으로 다음과 같은 형태로 정의된다:

$\min_{\mathbf{x}} \sum_{i} \left\| \mathbf{z}_i - h(\mathbf{x}_i) \right\|^2$

여기서 $\mathbf{z}_i$ 는 $i$ 번째 센서로부터 얻은 측정값이고, $h(\mathbf{x}_i)$ 는 상태 변수 $\mathbf{x}_i$ 에 대한 측정 함수이다. 잔차 $\mathbf{z}_i - h(\mathbf{x}_i)$ 를 최소화하는 방향으로 최적화를 수행하게 된다.

최적화 과정에서 중요한 부분은 IMU 데이터를 활용한 자세 변화 추정이다. IMU는 가속도와 각속도를 측정하며, 이를 통해 로봇의 회전과 이동을 계산할 수 있다. IMU의 관성 측정값 $\mathbf{a}$ 와 $\mathbf{\omega}$ 는 다음과 같이 로봇의 움직임을 설명한다:

$\mathbf{v}_{t+1} = \mathbf{v}_t + \Delta t (\mathbf{a}_t - \mathbf{g})$

$\mathbf{\theta}_{t+1} = \mathbf{\theta}_t + \Delta t \mathbf{\omega}_t$

여기서 $\mathbf{v}_t$ 는 $t$ 시점의 속도, $\mathbf{a}_t$ 는 가속도, $\mathbf{\omega}_t$ 는 각속도, $\mathbf{\theta}_t$ 는 자세, $\mathbf{g}$ 는 중력 가속도를 나타낸다.

6. VIO (Visual Inertial Odometry)와 LiDAR Odometry 결합

FAST-LIVO의 주요 혁신 중 하나는 VIO와 LiDAR Odometry를 결합하여 보다 정밀한 위치 추정을 가능하게 한다는 점이다. VIO는 카메라와 IMU 데이터를 사용하여 로봇의 움직임을 추정하는 방식으로, LiDAR Odometry와 상호 보완적인 특성을 갖는다. LiDAR는 거리 기반의 정보로 환경의 3D 지형을 잘 파악하는 반면, VIO는 주변의 시각적 특징을 기반으로 비교적 높은 빈도로 자세 정보를 제공한다.

VIO는 카메라 이미지에서 특징점(Feature Point)을 추출하여 이를 추적한다. IMU는 가속도 및 각속도 데이터를 제공하여, 카메라의 특징점 추적과 IMU 데이터를 결합하여 로봇의 자세 및 위치 변화를 추정한다. VIO의 기본적인 추정 모델은 다음과 같이 수학적으로 표현된다:

카메라에서 특정 시점 $t$ 에 포착한 3D 포인트 $\mathbf{X}_w$ 의 월드 좌표는 다음과 같이 표현된다:

$\mathbf{X}_w = \mathbf{R}_t \mathbf{X}_c + \mathbf{t}_t$

여기서, - $\mathbf{R}_t$ 는 월드 좌표계에서 카메라 좌표계로 변환하는 $3 \times 3$ 회전 행렬, - $\mathbf{t}_t$ 는 카메라의 위치를 나타내는 $3 \times 1$ 이동 벡터, - $\mathbf{X}_c$ 는 카메라 좌표계에서의 3D 포인트이다.

VIO는 이러한 특징점 기반의 위치 변화를 지속적으로 추적하며, IMU에서 제공되는 관성 정보와 융합하여 최적화한다. 이를 통해 VIO는 상대적인 위치 변화를 매우 정밀하게 추정할 수 있다.

7. 비동기적 센서 데이터 통합

FAST-LIVO에서 중요한 또 다른 부분은 비동기적인 센서 데이터를 효과적으로 통합하는 방법이다. LiDAR, 카메라, IMU는 각기 다른 주기로 데이터를 수집하는데, 이를 일관된 시간 축에서 결합하는 것이 매우 중요하다.

FAST-LIVO는 주로 IMU 데이터를 기준으로 하여 다른 센서 데이터를 동기화한다. IMU는 고주파수로 데이터를 수집하는데, 이를 기준으로 LiDAR와 카메라의 데이터를 시간 축 상에서 보간(interpolation)하여 통합한다. 이는 다음과 같은 기본적인 수식으로 설명할 수 있다.

IMU의 고주파수 데이터를 기반으로 두 시점 $t_1$ 과 $t_2$ 에서의 상태 변화는 다음과 같이 표현된다:

$\mathbf{x}(t_2) = \mathbf{x}(t_1) + \int_{t_1}^{t_2} f(\mathbf{u}(t)) dt$

여기서, - $\mathbf{x}(t)$ 는 상태 벡터(위치 및 속도 등), - $\mathbf{u}(t)$ 는 IMU에서 측정된 입력(가속도 및 각속도)이다.

LiDAR와 카메라 데이터는 해당 시간 간격에서 선형 보간 또는 상관 관계를 이용한 시간 보정을 통해 IMU 데이터와 결합된다. 이를 통해 각 센서 간의 비동기성을 최소화하고, 일관된 정보로 위치를 추정할 수 있다.

8. 최적화 문제 정의

FAST-LIVO는 최종적으로 LiDAR, VIO, IMU 데이터를 기반으로 전역적인 위치 추정을 수행하는 최적화 문제를 정의한다. 이 최적화 문제는 센서 데이터 간의 일관성을 최대화하는 방향으로 구성되며, 목표는 각 센서로부터 얻은 측정값과 추정값 간의 차이를 최소화하는 것이다.

최적화 문제의 일반적인 형태는 다음과 같이 정의될 수 있다:

$\min_{\mathbf{X}} \sum_{i=1}^{N} \left\| \mathbf{z}_i^{LiDAR} - h_{LiDAR}(\mathbf{X}_i) \right\|^2 + \sum_{j=1}^{M} \left\| \mathbf{z}_j^{VIO} - h_{VIO}(\mathbf{X}_j) \right\|^2$

여기서, - $\mathbf{z}_i^{LiDAR}$ 와 $\mathbf{z}_j^{VIO}$ 는 각각 LiDAR와 VIO로부터 얻은 측정값, - $h_{LiDAR}(\mathbf{X}_i)$ 와 $h_{VIO}(\mathbf{X}_j)$ 는 각각 상태 변수 $\mathbf{X}$ 에 따른 LiDAR 및 VIO 모델이다.

이 최적화 문제는 각 센서로부터 얻은 정보들이 상호 보완적으로 사용되도록 설계되며, 잔차(residual)를 최소화하는 방식으로 풀린다.

9. FAST-LIVO의 시간 복잡도와 성능 최적화

FAST-LIVO는 실시간으로 동작해야 하기 때문에, 시간 복잡도 또한 중요한 요소로 작용한다. 특히 LiDAR 포인트 클라우드의 처리와 특징점 추출, IMU 데이터와의 융합 과정에서 많은 계산 자원이 요구된다.

FAST-LIVO는 이러한 계산 자원을 최소화하기 위해 다음과 같은 최적화 기법을 사용한다:

포인트 클라우드 다운샘플링: LiDAR로부터 얻은 포인트 클라우드를 일정한 간격으로 다운샘플링하여 처리량을 줄이다.
이벤트 기반 IMU 데이터 처리: IMU 데이터는 고주파수로 들어오기 때문에, 불필요한 데이터를 제외하고 중요한 이벤트에 집중하여 처리하는 방식으로 연산량을 줄이다.
프레임 간 간격 최적화: VIO와 LiDAR 간의 프레임 간격을 조정하여 실시간 처리 성능을 최적화한다. 필요 없는 프레임을 생략하여 계산을 경량화할 수 있다.

FAST-LIVO의 이러한 최적화 기법들은 실시간 자율 주행 로봇에 적합한 성능을 제공하며, 특히 자원이 제한된 환경에서 효율적으로 작동하도록 설계되었다.

10. LiDAR, VIO, IMU 데이터의 융합을 위한 정합 기법

FAST-LIVO에서 LiDAR, VIO, IMU 데이터를 효과적으로 융합하기 위해 다양한 정합(registration) 기법을 사용한다. 이러한 기법들은 각 센서가 제공하는 데이터의 상관관계를 최대한 활용하여, 더 정확한 위치 추정을 가능하게 한다.

LiDAR와 IMU 데이터 정합

LiDAR Odometry로 얻어진 상대적인 움직임 정보는 IMU 데이터를 기반으로 보정된다. 이를 통해 IMU가 제공하는 고주파수의 자세 정보를 LiDAR 데이터에 반영하여, LiDAR 측정의 불확실성을 줄이고 보다 안정적인 추정을 수행한다.

LiDAR와 IMU의 정합은 다음과 같이 수학적으로 표현될 수 있다. IMU가 제공하는 가속도 정보 $\mathbf{a}$ 와 각속도 정보 $\mathbf{\omega}$ 를 기반으로, 현재 시점에서의 로봇의 회전 변환 행렬 $\mathbf{R}_t$ 와 속도 $\mathbf{v}_t$ 를 계산한다:

$\mathbf{v}_{t+1} = \mathbf{v}_t + \Delta t (\mathbf{a}_t - \mathbf{g})$

$\mathbf{R}_{t+1} = \mathbf{R}_t \exp(\Delta t \mathbf{\omega}_t)$

여기서, $\mathbf{g}$ 는 중력 가속도, $\exp(\Delta t \mathbf{\omega}_t)$ 는 각속도를 기반으로 한 로봇의 회전 변환이다. LiDAR에서 추정된 이동 벡터와 회전 행렬을 IMU의 계산 값과 결합하여 보정된 위치 추정을 얻는다.

LiDAR와 VIO 데이터 정합

LiDAR 데이터는 로봇의 위치 변화를 3D 공간에서 정확히 측정하지만, 실시간으로 얻은 VIO 데이터와 비교적 낮은 주파수로 제공되는 경우가 많다. 이를 해결하기 위해 LiDAR Odometry와 VIO 데이터를 상호 보완적으로 결합하여, 두 센서 간의 차이를 보완하는 방식으로 동작한다.

VIO 데이터에서 얻은 위치 및 자세 변환은 다음과 같은 형태로 정의된다:

$\mathbf{T}_t^{VIO} = \begin{bmatrix} \mathbf{R}_t^{VIO} & \mathbf{t}_t^{VIO} \\ 0 & 1 \end{bmatrix}$

이와 같이, VIO 데이터는 회전 $\mathbf{R}_t^{VIO}$ 와 이동 $\mathbf{t}_t^{VIO}$ 을 포함한 4x4 변환 행렬로 표현되며, LiDAR에서 추정된 변환 행렬과 결합된다.

LiDAR와 VIO 간의 정합은 다음과 같은 형태로 잔차를 정의하여 최적화할 수 있다:

$\mathbf{r}_t = \mathbf{T}_t^{LiDAR} - \mathbf{T}_t^{VIO}$

이때 $\mathbf{T}_t^{LiDAR}$ 는 LiDAR로 추정된 변환 행렬을 나타내며, $\mathbf{T}_t^{VIO}$ 는 VIO에서 얻은 변환 행렬이다. 잔차 $\mathbf{r}_t$ 를 최소화하는 방향으로 최적화를 수행하여 두 센서 간의 데이터 정합을 최적화한다.

11. FAST-LIVO의 비선형 최적화 방법

FAST-LIVO는 비선형 최적화 문제를 해결하기 위해 주로 비선형 최소제곱(NLS, Nonlinear Least Squares) 기법을 사용한다. 이는 센서 데이터의 불확실성을 최소화하고, 각 센서가 제공하는 측정값과 추정값 간의 차이를 줄이기 위한 목적으로 사용된다.

NLS 문제는 일반적으로 다음과 같은 형태로 정의된다:

$\min_{\mathbf{x}} \sum_{i} \left\| \mathbf{z}_i - h(\mathbf{x}_i) \right\|^2$

여기서, - $\mathbf{z}_i$ 는 측정된 데이터, - $h(\mathbf{x}_i)$ 는 상태 변수 $\mathbf{x}_i$ 에 대한 모델 함수이다.

FAST-LIVO에서 사용되는 비선형 최적화 기법으로는 Gauss-Newton 또는 Levenberg-Marquardt 알고리즘이 주로 사용된다. 이러한 기법들은 센서 데이터 간의 잔차를 반복적으로 계산하고, 잔차를 최소화하는 방향으로 상태 변수를 갱신한다.

12. FAST-LIVO의 불확실성 모델링

FAST-LIVO는 센서로부터 얻어진 데이터의 불확실성을 고려하여 최적화를 수행한다. LiDAR, VIO, IMU 각각의 센서 데이터는 일정 수준의 노이즈와 불확실성을 포함하고 있으며, 이를 정확하게 모델링하는 것이 중요하다.

각 센서의 불확실성은 일반적으로 다음과 같은 공분산 행렬 $\mathbf{Q}_i$ 로 모델링된다:

$\mathbf{Q}_i = \text{cov}(\mathbf{z}_i)$

여기서 $\mathbf{Q}_i$ 는 센서 측정값 $\mathbf{z}_i$ 의 불확실성을 나타내는 공분산 행렬이다. FAST-LIVO는 이러한 공분산 행렬을 기반으로 센서 데이터의 신뢰도를 계산하고, 최적화 과정에서 가중치로 반영한다. 예를 들어, IMU는 일반적으로 고주파수의 자세 변화를 제공하지만 노이즈가 상대적으로 많기 때문에, LiDAR 데이터에 비해 더 낮은 가중치를 부여하는 방식으로 처리된다.

13. 시간 보정(Time Calibration)과 시간 오차 처리

FAST-LIVO는 다양한 센서 데이터를 융합하여 로봇의 위치를 추정하므로, 각 센서 간의 시간 오차(time delay)가 위치 추정 정확도에 미치는 영향을 줄이기 위한 시간 보정 기법이 필수적이다. 시간 보정은 특히 LiDAR, 카메라, IMU와 같은 서로 다른 주기로 데이터를 수집하는 센서들의 경우 중요하게 다루어져야 한다.

FAST-LIVO에서는 시간 보정을 위해 IMU 데이터를 기준으로 다른 센서들의 데이터를 시간 축에서 보간하거나, 예측 모델을 사용하여 시간 오차를 수정한다. 이러한 시간 보정 기법은 센서 간의 동기화를 최대한 정확하게 맞추는 데 중요한 역할을 한다.

이 과정을 수학적으로 설명하면, 주어진 두 시점에서 센서 A와 센서 B의 측정값이 각각 $\mathbf{z}_A(t)$ 와 $\mathbf{z}_B(t + \Delta t)$ 로 제공될 때, 시간 오차 $\Delta t$ 를 최소화하는 방향으로 보정한다.

$\min_{\Delta t} \left\| \mathbf{z}_A(t) - \mathbf{z}_B(t + \Delta t) \right\|$

이 최적화 문제를 풀어 시간 오차를 최소화한 뒤, 센서 데이터를 융합하여 보다 정밀한 위치 추정을 수행한다.

14. IMU의 드리프트 보정

IMU(Inertial Measurement Unit)는 FAST-LIVO의 핵심적인 센서 중 하나로, 가속도와 각속도 정보를 제공하여 로봇의 움직임을 추적하는데 중요한 역할을 한다. 그러나 IMU는 시간이 지남에 따라 드리프트(Drift) 현상이 발생하여 측정값이 점점 부정확해지는 경향이 있다. 이를 보정하지 않으면 IMU만으로는 신뢰할 수 있는 위치 추정을 할 수 없다.

IMU 드리프트는 가속도와 각속도 측정에서 누적 오차가 발생하면서 시간이 지남에 따라 위치와 자세의 추정이 왜곡되는 현상이다. 이를 해결하기 위해 FAST-LIVO는 다른 센서(LiDAR 및 카메라)의 데이터를 사용하여 IMU의 드리프트를 보정한다.

IMU 드리프트 보정 과정은 크게 두 가지 방법으로 이루어진다:

LiDAR 및 VIO 기반 보정: LiDAR 및 VIO에서 얻은 상대적인 움직임 정보는 IMU에서 측정된 움직임과 비교된다. 이 비교를 통해 IMU에서 발생한 드리프트를 확인하고, 보정이 이루어진다. 수학적으로는 다음과 같이 표현할 수 있다.

$\mathbf{x}_t^{\text{corrected}} = \mathbf{x}_t^{\text{IMU}} + \mathbf{\delta x}_t$

여기서 $\mathbf{x}_t^{\text{IMU}}$ 는 IMU로부터 추정된 위치 및 자세이고, $\mathbf{\delta x}_t$ 는 LiDAR 및 VIO로부터 계산된 보정 값이다. 이 보정 값은 IMU의 드리프트를 상쇄하는 방향으로 추가된다.

상태 변수의 확장: FAST-LIVO는 IMU의 드리프트를 상태 변수의 확장 형태로 모델링한다. 즉, IMU의 드리프트를 하나의 변수로 보고 이를 상태 추정 과정에서 추정한다. IMU 드리프트는 점진적으로 축적되기 때문에, 확장된 상태 공간에서 드리프트를 지속적으로 추적하며 보정하는 방식이다.

확장된 상태 변수 $\mathbf{X}_t$ 는 다음과 같이 표현될 수 있다:

$\mathbf{X}_t = \begin{bmatrix} \mathbf{p}_t \\ \mathbf{v}_t \\ \mathbf{R}_t \\ \mathbf{b}_a \\ \mathbf{b}_\omega \end{bmatrix}$

여기서, - $\mathbf{p}_t$ 는 로봇의 위치, - $\mathbf{v}_t$ 는 속도, - $\mathbf{R}_t$ 는 회전 행렬, - $\mathbf{b}_a$ 와 $\mathbf{b}_\omega$ 는 각각 가속도와 각속도의 바이어스(드리프트)를 나타낸다.

상태 공간에 바이어스 변수 $\mathbf{b}_a$ 와 $\mathbf{b}_\omega$ 를 포함시킴으로써, IMU의 드리프트를 지속적으로 추정하고 보정할 수 있다.

15. FAST-LIVO에서의 데이터 융합 프레임워크

FAST-LIVO에서 LiDAR, VIO, IMU 데이터를 융합하는 과정은 주로 비율가중합(Weighted Fusion) 방식으로 이루어진다. 각 센서가 제공하는 데이터는 불확실성에 따라 가중치가 부여되며, 이 가중치를 기반으로 융합이 이루어진다. 이 과정은 각 센서의 장단점을 보완하여 더욱 정밀한 위치 추정을 가능하게 한다.

가중치 기반 센서 융합

각 센서의 데이터는 불확실성을 반영한 가중치 $\mathbf{W}_i$ 가 부여된다. 이 가중치는 해당 센서 데이터의 신뢰도를 나타내며, LiDAR, VIO, IMU 데이터가 상호 보완적으로 결합된다. 가중치 기반 융합은 다음과 같은 수식으로 표현된다:

$\mathbf{X}_t = \sum_{i} \mathbf{W}_i \mathbf{X}_t^i$

여기서, - $\mathbf{X}_t$ 는 최종 융합된 위치 및 자세 추정 값, - $\mathbf{X}_t^i$ 는 각 센서(i: LiDAR, VIO, IMU)로부터 얻어진 추정 값, - $\mathbf{W}_i$ 는 각 센서의 불확실성에 반비례하는 가중치이다.

이 과정에서 각 센서가 제공하는 정보의 신뢰도가 다르게 반영되며, 불확실성이 큰 센서 데이터는 적은 비중으로, 신뢰도가 높은 센서 데이터는 더 큰 비중으로 융합된다.

확률적 센서 융합

FAST-LIVO는 또한 확률적 모델을 기반으로 각 센서의 데이터를 결합한다. 각 센서로부터 얻은 측정값은 확률 분포로 모델링되며, 이를 기반으로 상태 변수를 추정한다. 확률적 센서 융합은 일반적으로 베이지안 필터링 기법을 사용하여 수행되며, 이는 다음과 같은 형태로 표현된다:

$p(\mathbf{X}_t | \mathbf{Z}_t) = \frac{p(\mathbf{Z}_t | \mathbf{X}_t) p(\mathbf{X}_{t-1})}{p(\mathbf{Z}_t)}$

여기서, - $p(\mathbf{X}_t | \mathbf{Z}_t)$ 는 주어진 센서 측정값 $\mathbf{Z}_t$ 에 대한 상태 변수 $\mathbf{X}_t$ 의 사후 확률 분포, - $p(\mathbf{Z}_t | \mathbf{X}_t)$ 는 측정 모델(센서 데이터가 주어진 상태에서의 확률), - $p(\mathbf{X}_{t-1})$ 는 이전 시점에서의 상태 추정 값이다.

FAST-LIVO는 이러한 베이지안 필터링 과정을 통해 각 센서 데이터를 융합하며, 이를 통해 보다 신뢰할 수 있는 위치 추정을 수행한다.

16. SLAM과 비슷한 확률적 지도 작성 과정

FAST-LIVO는 SLAM 문제를 해결하기 위해 지도 작성(map building)을 수행하며, 이 과정은 주로 LiDAR 데이터를 기반으로 한다. 지도 작성은 환경의 구조를 이해하고 로봇의 위치를 지속적으로 추정하기 위한 중요한 요소이다.

LiDAR로부터 수집된 포인트 클라우드는 환경의 3D 모델을 생성하는 데 사용되며, 이 3D 모델은 로봇이 이동할 때 계속해서 업데이트된다. FAST-LIVO에서는 LiDAR 데이터를 기반으로 한 3D 포인트 클라우드 맵을 작성하며, 이 지도는 다음과 같이 수학적으로 표현될 수 있다:

$\mathbf{M}_t = \mathbf{M}_{t-1} \cup \mathbf{P}_t$

여기서, - $\mathbf{M}_t$ 는 시점 $t$ 에서의 지도(맵), - $\mathbf{P}_t$ 는 시점 $t$ 에서 LiDAR로부터 새롭게 얻은 포인트 클라우드 데이터이다.

새로운 포인트 클라우드는 기존 지도에 통합되며, 이를 통해 환경의 정확한 모델을 작성한다.

지도 작성 과정은 로봇이 이동하면서 얻은 포인트 클라우드를 정렬하고, 중복된 정보를 제거하는 방식으로 진행된다. 이 과정에서 ICP(Iterative Closest Point) 알고리즘과 같은 정합 기법이 사용된다. ICP 알고리즘은 두 포인트 클라우드를 비교하여, 최적의 회전 및 평행 이동 변환을 계산하여 정렬한다.

지도 작성은 또한 로봇의 위치 추정과 상호작용하며, SLAM 문제의 본질인 위치 추정과 환경 모델링을 동시에 해결한다.

17. FAST-LIVO의 루프 클로저(Loop Closure) 기법

FAST-LIVO에서 중요한 또 하나의 개념은 루프 클로저(Loop Closure)이다. 루프 클로저는 로봇이 이미 지나갔던 경로를 다시 통과할 때 발생하는 현상으로, SLAM 시스템에서는 이러한 반복적인 경로를 인식하여 로봇의 위치 추정 오차를 줄이는 데 중요한 역할을 한다. 루프 클로저가 적절히 처리되면, 오랜 시간 동안 축적된 위치 추정의 드리프트를 줄일 수 있다.

루프 클로저 과정은 다음과 같은 단계로 이루어진다:

루프 감지: 로봇이 이미 지나갔던 위치를 다시 방문했음을 감지한다. 이 과정에서 로봇의 현재 위치와 과거의 위치 간의 유사성을 비교하여, 두 위치가 동일한 환경에 있다는 사실을 확인한다. 이를 위해 LiDAR 데이터나 카메라 영상에서 얻은 특징점을 비교한다.
정합(Alignment): 감지된 루프 클로저 지점을 기준으로 로봇의 현재 위치와 과거 위치 간의 변환을 계산한다. 이 단계에서 주로 ICP(Iterative Closest Point) 알고리즘이나 RANSAC 같은 알고리즘을 사용하여, 두 지점 간의 최적의 변환 행렬을 찾는다.

이 변환 행렬은 다음과 같이 표현될 수 있다:

$\mathbf{T}_{LC} = \begin{bmatrix} \mathbf{R}_{LC} & \mathbf{t}_{LC} \\ 0 & 1 \end{bmatrix}$

여기서, $\mathbf{T}_{LC}$ 는 루프 클로저 변환 행렬, $\mathbf{R}_{LC}$ 는 회전 행렬, $\mathbf{t}_{LC}$ 는 평행 이동 벡터이다.

그래프 최적화: 루프 클로저에서 얻어진 변환을 SLAM 시스템의 그래프에 반영하여, 전역적으로 로봇의 위치 추정을 최적화한다. 이를 위해 포즈 그래프(Pose Graph) 기반 최적화 기법을 사용하며, 이 과정은 다음과 같은 형태의 최적화 문제로 표현된다.

포즈 그래프에서 각 노드는 로봇의 위치(포즈)를 나타내며, 엣지는 서로 다른 시점에서의 위치 변환을 나타낸다. 루프 클로저에 의해 새롭게 발견된 변환 엣지 $\mathbf{T}_{LC}$ 는 기존 그래프에 추가되고, 다음 최적화 문제가 정의된다:

$\min_{\mathbf{X}} \sum_{(i,j) \in \mathcal{E}} \left\| \mathbf{T}_{ij} - f(\mathbf{X}_i, \mathbf{X}_j) \right\|^2$

여기서, - $\mathbf{X}_i$ 와 $\mathbf{X}_j$ 는 그래프에서 각각 노드 $i$ 와 $j$ 의 위치(포즈), - $\mathbf{T}_{ij}$ 는 노드 $i$ 와 $j$ 간의 변환 행렬, - $\mathcal{E}$ 는 그래프 상에서 정의된 엣지들의 집합이다.

이 최적화 과정을 통해, 로봇의 과거 위치와 현재 위치를 일관성 있게 연결하며, 루프 클로저를 통해 위치 추정 오차를 감소시킨다.

18. FAST-LIVO의 분산 처리 구조

FAST-LIVO는 실시간으로 동작해야 하기 때문에, 센서 데이터의 처리를 효율적으로 분산하여 계산하는 구조가 필수적이다. 이를 위해 FAST-LIVO는 다중 스레드(Multi-threading)나 분산 처리 기법을 활용하여 각 센서의 데이터를 병렬적으로 처리하고, 최적화 과정도 효율적으로 분할하여 수행한다.

멀티 스레드 기반 처리

FAST-LIVO는 LiDAR, VIO, IMU 데이터를 각각 별도의 스레드에서 처리한다. 이 스레드 간의 데이터 흐름은 다음과 같은 형태로 이루어진다:

LiDAR 스레드: LiDAR 데이터의 포인트 클라우드를 수집하고, 이를 처리하여 특징점 추출과 Odometry 계산을 수행한다.
VIO 스레드: 카메라 영상과 IMU 데이터를 기반으로 VIO 처리를 수행하며, IMU의 자세 추정과 카메라 특징점 추적을 통해 위치 변화를 계산한다.
IMU 스레드: IMU 데이터를 고주파수로 처리하여 가속도 및 각속도 정보를 수집하고, 이를 기반으로 로봇의 자세 변화를 계산한다.

이렇게 각 스레드에서 독립적으로 데이터를 처리한 후, 최종적으로 위치 추정 단계에서 모든 데이터를 통합한다. 이 구조는 실시간 시스템에서 필수적인 낮은 지연 시간을 보장하면서도 각 센서의 데이터를 효율적으로 처리할 수 있도록 설계되었다.

데이터 파이프라인

FAST-LIVO는 각 센서로부터 수집된 데이터를 처리하기 위한 데이터 파이프라인을 구축한다. 이 파이프라인은 센서 데이터의 입력에서부터 최종적인 위치 추정까지의 흐름을 정의하며, 주어진 자율 주행 로봇의 상황에 맞게 최적화된다. 파이프라인의 구조는 다음과 같다:

graph TD A[LiDAR Data] --> B[LiDAR Odometry] C[IMU Data] --> D[IMU Integration] E[Camera Data] --> F[VIO Processing] B --> G[Sensor Fusion] D --> G F --> G G --> H[Global Pose Estimation]

이 파이프라인을 통해 LiDAR, IMU, 카메라 데이터가 서로 다른 스레드에서 처리되면서 최종적으로 융합되어 로봇의 위치와 자세를 추정한다.

19. FAST-LIVO의 확장성

FAST-LIVO는 다양한 로봇 플랫폼에서 적용 가능하도록 설계되었으며, 여러 가지 센서 구성에서도 유연하게 동작할 수 있는 확장성을 가지고 있다. FAST-LIVO는 센서 수가 늘어나거나, 더 고도화된 센서가 추가되더라도 쉽게 통합할 수 있는 구조로 설계되었다.

센서 추가와 확장

FAST-LIVO는 새로운 센서를 추가하거나 기존 센서의 데이터를 확장할 때, 간단한 모듈 확장을 통해 이를 지원한다. 예를 들어, 더 높은 해상도를 제공하는 LiDAR 센서를 추가하거나, 여러 대의 카메라를 결합한 스테레오 카메라 시스템을 추가하는 경우에도 쉽게 적용할 수 있다.

새로운 센서가 추가되면, 해당 센서로부터 얻어진 데이터는 기존의 센서 융합 파이프라인에 추가되며, 기존의 최적화 과정에 새로운 센서의 측정값을 반영하는 방식으로 동작한다.

이동 로봇 및 드론에의 적용

FAST-LIVO는 지상 이동 로봇뿐만 아니라 드론과 같은 공중 로봇에도 적용될 수 있도록 설계되었다. 드론의 경우 IMU의 정보가 더욱 중요한 역할을 하며, FAST-LIVO는 이러한 다양한 플랫폼에 맞게 센서 융합 알고리즘을 조정할 수 있다.

예를 들어, 드론에서는 고도 변화가 중요한 정보가 되기 때문에 LiDAR와 IMU 간의 데이터 융합이 더 긴밀하게 이루어지며, 이를 통해 3D 공간에서의 정밀한 위치 추정이 가능해진다.