[논문 리뷰] Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter
[IEEE RA-L,2021] Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter 논문 리뷰
Introduction
Simultaneous localization and mapping, SLAM이라 불리는 기술은 unmanned aerial vehicles(UAVs)와 같은 mobile 로봇의 기본 전제 조건이다.
Stereo VO나 monocular VO같은 Visual (-inertial) odometry(VO)는 lightweight와 low-cost때문에 mobile 로봇에서 보통 사용되고 있었다.
VO는 풍부한 RGB 정보를 제공하지만, visual solution은 직접적인 깊이 측정이 부족하고 trajectory planning을 위해 3D환경을 reconstruction하는 데 많은 계산 리소스가 필요하다.
게다가 주변 조명 환경에 매우 민감하다.
Light detection and ranging(LiDAR) 센서는 이러한 어려움들을 국목하지만 작은 크기의 mobile 로봇에서 사용하기에는 too costly(and bulky)하다.
Solid-state LiDAR는 최근 micro-electro-mechanical-system(MEMS) 스캐닝 및 rotation prisms 기반한 LiDAR와 같은 LiDAR 개발의 주요 동향으로 부상하고 있다.
이러한 LiDAR는 비용적으로 매우 효율적이고, 경량이며, 고성능이다.
(global sutter camera와 비슷한 range에서, 소규모 UAB에 장착가능, long-range와 high-accuracy인 직접적인 3D measurement를 생성한다.)
이러한 특징들은 라이다를 통해 정확한 3D 지도를 획득하거나 심각한 조도 변화가 있는 어수선한 환경에서 동작할 수 있는 UAV등에 사용가능하게한다.
큰 잠재력을 가지고 있지만, Solid-state LiDAR는 동시에 SLAM에 새로운 어려움을 가지고 왔다.
-
LiDAR 측정에서 특징점은 주로 환경 내에서 가장자리와 면같은 기하학적 구조물이다. UAV가 특징점이 부족한 혼잡한 환경에서 가동할 때 LiDAR에 기반한 solution이 쉽게 퇴화할 수 있다.
이 문제는 LiDAR의 시야각이 작을 때 더욱 두드러진다고 한다. -
스캔 방향을 따르며, 고해상도로 인해, LiDAR 스캔은 일반적으로 수천 개의 많은 특징점을 포함한다.
이러한 특징점들은 퇴화(degeneration) 상황에서 신뢰할 수 있는 자세를 결정하기에는 충분하지 않으며, 이를 IMU 측정값과 tightly하게 fusing하려면 UAV 내부의 컴퓨터가 감당할 수 없는 엄청난 계산 자원이 필요하다. -
LiDAR는 몇 개의 laser/receiver 쌍으로 순차적으로 점을 샘플링하므로 scan내의 laser point는 항상 다른 시간에 샘플링된다.
이로 인해 scan registration에 심각한 모션 왜곡이 발생하며, 프로펠러 및 모터의 지속적인 회전은 IMU 측정값에 상당한 노이즈를 도입한다.
“FAST-LIO“는 UAV와 같은 small-scale mobile에 대한 내비게이션을 실용적으로 만들기 위한 계산 효율적이고, Robust한 LiDAR-inertial odometry 패키지이다.
본 논문에서 말하는 구체적인 기여는 다음과 같다.
-
빠른 움직임, 노이즈가 있는 환경 혹은 혼잡한 환경에서 발생하는 degeneration을 해결하기 위해, LiDAR feature point을 IMU 측정치를 퓨전하기 위해 tightly-coupled iterated Kalman filter를 적용한다.
본 논문은 움직임에 대한 왜곡을 보상하기 위해 formal한 back-propagation과정을 제안한다. -
많은 수의 LiDAR 특징점으로 인한 computation load(계산 부하)를 줄이기 위해,
Kalman gain을 계산하기 위한 새로운 공식을 제안하고, 기존 kalman gain 공식과 동등함을 증명한다.
새로운 공식은 measurement dimension이 아닌 state dimension에 따라 계산 복잡도가 결정된다. -
fast and robust한 LiDAR-inertial odometry 소프트웨어 패키지를 구현한다.
이 시스템은 소규모 쿼드로터의 내장 컴퓨터에서 작동할 수 있다. -
다양한 실내외 환경에서 실험을 수행하고, 실제 UAV 비행 실험을 진행하여, 빠른 움직임이나 강한 진동 노이즈에 대해 시스템이 견고하다는 것을 입증한다.
Methodology
Framework Overview
.png)
System overview of FAST-LIO
LiDAR 입력은 feature extraction 모듈에 입력되어 planar와 edge feature를 얻는다.
그 다음, 추출된 특징과 IMU 측정값이 state estimation 모듈에 입력되어 10Hz~50Hz의 속도로 상태 추정이 이루어진다.
추정된 자세는 feature point를 global frame에 등록하고(register), 지금까지 구축된 feature points map과 병합된다.
업데이트된 맵은 다음 단계에서 새로운 점을 register하는데 사용된다.
System Description
Operator
먼저 notation은 다음과 같다.
.png)
Table 1
그리고 $\mathcal{M}$은 n차원의 manifold라고 하자.
(예 $\mathcal{M}=SO(3)$)
.png)
$\mathcal{M}=SO(3)$이면 연산이 Lie군 Lie대수로 이루어지는 것 같고,
$\mathcal{M}=\mathbb{R}^n$이면 평범하게 연산하는 것 같다.
$\boxplus$ 연산자는 Lie group 원소 $R$에 $r$만큼 추가적인 변환을 적용하는 연산자이다. 참고
$\boxminus$ 연산자는 두 Lie group원소 $R_1$과 $R_2$가 존재할 때 $R_1$에서 $R_2$의 상대 변화량을 의미한다. 참고
Continuous model
$I$는 IMU frame, $L$은 LiDAR frame이다.
- IMU와 LiDAR는 rigid하게 부착
변환 관계:
.png)
- IMU frame(as the body frame) kinematic model
첫번째 줄은 위치, 속도, 가속도에 대한 상관관계를 알 수 있다.
두번째 줄은 회전 변화량, 각속도 등에 대해 알 수 있다.
.png)
Global frame($G$)은 첫 번째 IMU frame이다.
$p$는 위치, $R$은 자세, $V$는 속도 $a_m$과 $w_m$은 각각 IMU measurement이다.
$n_a$와 $n_{\omega}$는 IMU measurement의 white noise이다.
$b_a$와 $b_{\omega}$는 IMU의 편향(bias)를 나타낸다.
이들은 가우시안 노이즈($n_{ba}$, $n_{b\omega}$)가 있는 random walk process로 모델링된다.
(랜덤 워크 프로세스는 현재 값이 이전 값에 랜덤한 변화를 더한 것으로 모델링되는 확률적인 과정, IMU 편향의 경우, 랜덤 워크 프로세스로 모델링하는 것은 시간이 지남에 따라 편향이 서서히 누적되는 현상을 나타내는 것을 의미)
Discrete model
정의된 $\boxplus$ 연산자를 바탕으로,
Continuous model을 discretize한다.
zero-order holder를 이용하여 IMU 샘플링 주기 $\Delta t$에서 continuous model을 이산화한다.
.png)
.png)
function $f$에 이전 state, input, noise를 입력으로 넣고 샘플링 주기 $\Delta t$마다의 변화가 다음 state가 된다.
function $f$는 (1)식에서 봤던 수식이고, Global에 대한 IMU의 kinematic model을 discrete하게 반영하는 function정도로만 이해했다.
(현재상태와 입력, 노이즈를 이용해 다음 상태를 계산)
Preprocessing of LiDAR measurements
raw LiDAR 포인트들은 매우 높은 속도로 샘플링된다.
따라서 수신되는 각각의 새로운 점을 개별적으로 처리하는 것은 일반적으로 불가능하다.
실용적인 접근 방식은 일정 시간 동안 이러한 점들을 누적하고, 한번에 처리하는 것이다.
FAST-LIO에서는 최소 누적 간격을 20ms로 설정하고, 50Hz(1초에 50번)로 전체 상태 추정(odometry output) 및 map update가 가능하다.
.png)
Fig. 2. (a)
이러한 누적된 점들의 집합을 scan이라고 부른다.
(해당 집합을 처리하는데 걸리는 시간은 $t_k$로 표시한다.)
.png)
Fig. 2. (b)
raw points로부터 high local smoothness로 planar point를 추출하고 low local smoothness로 edge point를 추출한다.
feature point의 개수를 $m$이라고 가정하고, 각각은 $\rho \in (t_{k-1}, t_k]$ 시간에서 샘플링된다.
이는 다음과 같이 정의된다. $L_j$는 시간 $\rho_j$에서 LiDAR local frame이다.
.png)
LiDAR scan 중에는 IMU 측정값도 있으며, 각각의 측정값은 $\tau \in [t_{k-1}, t_k]$ 시간에서 state $x_i$로 샘플링된다.
마지막 LiDAR feature point는 scan의 끝을 나타낸다. 즉, $\rho_{m} = t_k$
하지만 IMU 측정값은 scan의 시작 혹은 끝과 정렬되지 않을 수 있다.
State Estimation
state formulation인 (2)에서 상태를 추정하기 위해, 본 논문에서는 Iterated Extended Kalman Filter를 사용한다.
$t_{k-1}$에서 last LiDAR scan의 optimal state estimate를 $\bar{x}_{k-1}$
로 가정하고, 이때 공분산 행렬은 $\bar{P}_{k-1}$으로 표현한다.
그러면 $\bar{P}_{k-1}$는 아래 정의된 random error state vector의 공분산을 나타낸다.
.png)
여기서 $\delta \theta$는 자세 오차를 나타내며, 나머지는 standard additive error를 나타낸다.
.png)
즉, 자세 오차 $\delta \theta$가 실제 자세와 추정 자세 간의 편차를 설명하고,
장점은 자세의 불확실성을 3x3 공분산 행렬인 $\mathbb{E} [ \delta \theta \delta \theta^{T} ]$ 로 나타낼 수 있다는 것이다.
Forward Propagation
Forward propagation은 IMU 입력을 받은 후 한 번 수행된다.
(IMU data로 state vector를 update하는 과정)
Fig. 2. (b)
아래 식에 따라 propagation되며 process noise $w_i$를 0으로 설정하여 수행된다.
.png)
where $\Delta t = \tau_{i+1}-\tau_{i}$.
Convariance를 propagate하기 위해, 아래 식(5)에서 얻은 error state dynamic model을 사용한다.
.png)
white noise $w$의 공분산을 $Q$로 표시할 때, propagated covariance $\hat{P}_i$는 아래 식을 따라 반복적으로 계산된다.
.png)
Propagation은 시간 $t_k$에서 새로운 scan의 종료 시간에 도달할 때까지 계속된다.
Backward Propagation and Motion Compensation
점 누적 시간이 시간 $t_k$에 도달했을 때, feature point의 새로운 scan을 propagated scan $\hat{x}_k$와 공분산 $\hat{P}_k$와 융합하여 optimal state update를 생성해야한다.
새로운 scan은 $t_k$에서 이루어지지만, feature point는 각각의 sampling 시간 $t_j <= t_k$에서 측정된다.
이는 body frame의 mismatch를 유발한다고 한다.(motion distortion)
시간 $t_j$와 $t_k$ 사이의 relative motion(motion distortion)을 보강하기 위해 (2)식을 아래식으로 역으로 propagate한다.
.png){kind=link}
.png){kind=link}
.png)
backward propagation은 일반적으로 IMU 주파수보다 훨씬 높은 feature point의 주파수에서 수행된다.
아래 식으로 backward propagation이 수행된다.
.png)
backward propagation 과정을 통해 $\rho_{j}$와 scan-end time인 $t_k$사이의 relative pose를 생성한다.
이 relative pose는 local measurement를 scan-end measurement로 투영할 수 있게 해준다.
.png)
Residual computation
(10) 식을 통해 motion compensation을 수행하고,
.png){kind=link}
이를 통해 샘플링된 모든 feature point의 scan을 동시에 $t_k$에서 볼 수 있다.
(Fig2(b)에서도 확인할 수 있다.)
.png){kind=link}
.png){kind=link}
이를 이용해 residual을 구성할 수 있다.
.png)
Iterated Kalman filter의 현재 iteration : $\kappa$
.png)
식(11)을 통해 global frame으로 변환.
.png){kind=link}
각각의 LiDAR 특징점에 대해, map에서 이 특징점 근처에 정의된 가장 가까운 plane 혹은 edge가 실제로 해당 점이 속하는 곳으로 간주된다.
즉 residual(잔차)은 추정된 전역 프레임 좌표와 가장 가까운 plane 혹은 edge와의 거리 차이이다.
.png)
위 식으로 정의되며, $G_j$는 planar feature나 edge feature의 vector이다.
$G_j$를 결정하는 $u_j$의 계산과 평면 또는 모서리를 정의하는 map 내에서 근처 점의 탐색은 최근 map들의 점들로 KD-tree를 구성하여 수행한다.
또한 norm이 일정 임계값 이하인 residual만을 고려한다.
(임계값을 초과하는 residual은 outlier 혹은 새로 관측된 점들일 수 있다.)
Iterated state update
식 (12)에서 계산된 residual을 IMU 데이터로부터 propagate된 공분산 $\hat{P}_k$와 상태 예측 $\hat{x}_k$와 fuse하기 위해서는
.png){kind=link}
residual과 ground-truth state $x_k$ 및 measurement noise 사이의 measurement model을 선형화해야 한다.
measurement model을 만들기 위해 점 측정 $^{L_j}p_{f_{j}}$를 측정할 때 노이즈 $^{L_j}n_{f_{j}}$를 정의한다.
noise를 빼면 아래와 같은 gt점을 알 수 있다.
.png){kind=link}
.png)
이러한 ground-truth점은 말 그대로 정답이기에 식 (10)에 대입 후 식 (11)에 대입하여 global frame으로 바꾸고, 식 (12)에 대입하면 결과적으로 0이 되어야한다.
(아래 식과 같다.)
.png)){kind=link}
.png)
이후 first order approximation 수행
.png)
이를 통해, $h_j$를 error state $\tilde{x}_k^{\kappa}$에 대한 Jacobian 행렬 $H_j^{\kappa}$를 구할 수 있다.
Convariance matrix $P$는 다음 식을 통해 update를 수행한다.
\[P = (J^{\kappa})^{-1} \hat{P}_k (J^{\kappa})^{-T}\]$J$는
.png)
위 식을 $\tilde{x}_k^{\kappa}$에 대한 편미분을 수행하여 구한다고 한다.
.png)
이 다음 maximum a-posteriori estimate (MAP)에 대해 설명한다.
.png)
보면 최적화 식이긴하지만 명확하게는 이해하지 못했다.
결국 error가 최소화되는 최적의 state vector $\tilde{x}_k^{\kappa}$를 구하는 것이다.
이후 Kalman filter 계산 과정이 계속 나오는데
.png)
식 (18) 처럼 구하고, 업데이트 된 추정 값인 $\tilde{x}_k^{\kappa + 1}$과 이전 추정 값인 $\tilde{x}_k^{\kappa}$의 차이가 임계값보다 작을 때까지 업데이트하겠다 라고 설명한다.
.png)){kind=link}
하지만 식 (18)처럼 구하면 일반적이지만 괄호안의 행렬을 역행렬로 만들어야한다는 문제점이 있다고 한다.
실제로 LiDAR 특징점의 수가 매우 많고 이 크기의 행렬을 역행렬로 만드는 것이 현실적으로 어렵다한다.
그래서 FAST-LIO에서는 식 (20)과 같은 새로운 형태의 Kalman gain을 계산한다고 한다.
.png)){kind=link}
.png)
.png)
전체 과정은 다음과 같다.
.png)
Map Update
기존 맵에 추가하는 방식이고,
update된 optimal state의 feature point를 global frame으로 변환하는 과정만 수행한다.
.png)
결론
scan to map match
Forward & Backward propagation
Iterated Extended Kalman Filter를 통한 추정
참고자료
Lie Theory 개념 정리 (SO(3), SE(3))
[Paper Review] FAST-LIO 요약 및 설명
📣
포스팅에서 오류나 궁금한 점은 Comments를 작성해 주시면, 많은 도움이 됩니다.💡
Leave a comment