Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

---

Abstract

• mapping과 smoothing을 이용한 높은 정확도, 실시간성, 맵 생성이 가능한 TC lidar-inertial odometry 프레임워크를 제안함.

• fractor graph를 사용하며, 여러 종류의 relative / absolute measurement를 사용 가능하며, loop closure가 포함되어 있다.

• IMU pre-integration를 통해 예측된 motion을 통해 pointcloud deskew를 수행하고, lidar odometry의 초기값으로 활용된다.

• lidar odometry의 결과를 이용해 factor graph에서 IMU센서의 bias를 보정한다.

• 실시간에 높은 정확도를 위해 라이다 스캔을 global map에 매칭시키기 보다는 오래된 lidar scan을 무시한다. (sliding window). key frame을 사용함.

Instruction

LIO via Smoothing and Mapping

System overview

robot state : $\begin{pmatrix}R^w_b\\p^w_{wb}\\v^w_{wb}\\b_a\\b_w\end{pmatrix}$

transformation $T^w_b=[R^w_b|p^w_{wb}]$

Lidar, IMU 센서와 옵션으로 GPS 센서를 추가할 수 있다.

state estimation problem은 MAP 문제로 공식화 가능하다.

factor graph를 사용한다. Gaussian noise model 가정을 통해 MAP 문제는 nonlinear least squares 문제를 푸는 것과 동일해진다.

제안된 시스템은 altimeter나 compass와 같은 추가적인 센서를 유연하게 추가할 수 있다.

4종류의 factor와 1종류의 variable을 소개한다.

variable은 robot의 특정 시간의 state를 표현하며, graph에서 node로 사용된다.

4종의 factor는 IMU preintegration factor / lidar odometry factor / GPS factor / loop closure factor이다.

robot state node $\bold{x}$는 robot의 pose가 우리가 정한 threshold를 넘어설 때 새롭게 추가된다.

새로운 node가 추가될 때 'incremental smoothing and mapping with the Bayes tree'를 사용해 factor graph가 최적화된다.

B. IMU Preintegration Factor

$\hat{w}(t)=w(t)+b_w(t)+n_w$
$\hat{a}(t)=R^b_w(t)(a^w(t)-g^w)+b_a(t)+n_a$

$\hat{w},\hat{a}$ : imu measurements

IMU 센서 값을 이용해 robot의 motion을 추정할 수 있다.

$v^w_{wb}(t+\Delta t)=v^w_{wb}(t)+g^w\Delta t+R^w_b(t)(\hat{a}(t)-b_a(t)-n_a)\Delta t$

$p^w_{wb}(t+\Delta t)=p^w_{wb}(t)+v^w_{wb}(t)\Delta t+\frac{1}{2}g^w\Delta t^2+\frac{1}{2}R^w_b(t)(\hat{a}(t)-b_a(t)-n_a)\Delta t^2$

$R^w_b(t+\Delta t)=R^w_b(t)exp((\hat{w}(t)-b_w(t)n_w)\Delta t)$

IMU preintegration method를 적용해 두 timestep 사이의 relative motion을 얻는다.

$\Delta v_{ij}=R^{b(i)}_w(v^w_{wb(j)}-v^w_{wb(i)}-g^w\Delta t_{ij})$

$\Delta p_{ij}=R^{b(i)}_w(p^w_{wb(j)}-p^w_{wb(i)}-v^w_{wb(i)}\Delta t_{ij}-\frac{1}{2}g^w\Delta t_{ij}^2)$

$\Delta R_{ij}=R^{wT}_{b(i)}R^w_{b(j)}$

IMU bias는 factor graph 내부에서 lidar odometry factor와 함께 최적화된다.

C. Lidar Odometry Factor

lidar scan이 도착하면, 처음으로 feature extraction을 실시한다.

edge와 planar feature를 roughness of points over a local region을 평가해 추출한다.

큰 roughness를 가진 point는 edge feature로 분류된다.

small roughness를 가지면 planar feature 분류된다.

lidar scan time i에 추출된 edge / plar feature를 각각 $F^e_i,F^p_i$로 표기한다.

time i에 추출된 모든 feature들은 lidar frame $\bold{F}_i$를 구성한다. 이 feature들은 body 좌표계로 표현됨. range image를 사용해 feature를 추출한다.

모든 lidar frame을 사용하기에는 연산량이 너무 크므로 visual slam에서 자주 사용하는 keyframe selection 컨셉을 사용한다.

간단한 휴리스틱한 방식으로 robot pose가 미리 정해둔 threshold를 넘어설만큼 움직이면 새 keyframe을 추가하는 방식으로 동작한다.

새롭게 저장된 키프레임 $\bold{F}_{i+1}$는 factor graph에서 새 robot state 노드 $\bold{x}_{i+1}$와 연관이 있다.

두 키프레임 사이의 라이다 프레임들은 모두 버려진다. 이러한 방식은 map density와 memory 소모 사이의 균형, sparse한 factor graph 유지 등의 이점이 존재한다.

여기서 threshold는 1m 이동 또는 10도 회전을 기준으로 사용하였다.

lidar odometry factor의 생성은 아래 step을 따른다

1. Sub-keyframes for voxel map : 고정된 수의 최근 라이다 스캔을 포함하는 point cloud map을 만들기 위해 sliding window 방식을 채용. 두 라이다 스캔 사이의 transformation을 최적화하기보다 n개의 최근 keyframe들(sub-keyframes이라 부름)을 추출한다. sub-keyframes ${\bold{F}_{i-n},...,\bold{F}_i}$을 ${T_{i-n},...,T_i}$를 이용해 $W$ frame으로 변환하고, 하나의 voxel map $M_i$로 합쳐진다. edge/planar 2 타입의 피처를 추출하므로, $M_i$는 2개의 sub voxel map $M^e_i,M^p_i$으로 구성되어 있다. 두 sub voxel map은 다운샘플링을 거친다. 여기서는 각 cell의 포인트 개수 n=25, downsample resolution은 edge의 경우 0.2m, planar의 경우 0.4m로 사용하였음.

2. Scan matching : $M_i$에 새롭게 들어온 라이다 프레임 $\bold{F}_{i+1}$을 매칭. Low-drift and Real-time Lidar Odometry and Mapping에서 제안된 매칭 방식을 사용. IMU센서를 이용해 robot motion을 예측한 $\tilde{T}_{i+1}$을 이용해 ${F^e_{i+1},F^p_{i+1}}$을 $W$프레임으로 변환. 매칭을 수행해 결과를 얻음.

3. Relative transformation : feature 사이의 distance는 아래 식을 통해 계산한다.

이를 통해 두 pose를 연결하는 lidar odometry factor를 얻는다.

$\Delta T_{i,i+1}=T_i^T T_{i+1}$

D. GPS Factor

IMU / Lidar로 충분한 state estimation과 mapping이 가능하지만, large scale에서 drift 문제가 여전히 존재한다.

이를 해결하기 위해 absolute measurement를 제공하는 GPS 센서를 추가해 drift를 최소화하고자 한다.

GPS 신호를 받아 local cartesian coordinate로 변환한다. factor graph에 새 노드가 추가될 때, 그 노드와 새 GPS factor를 association한다.

만약 GPS 신호가 라이다 프레임과 hardware-syncronized 되어있지 않다면, GPS 측정값을 interpolate시켜 lidar frame과 맞춘다.

GPS 신호를 받을 때마다 GPS factor를 추가하는 것은 가능하지만 굳이 필수적이지는 않다. 왜냐하면 LIO의 drift는 매우 낮게 증가하기 때문이다. 실제로 우리는 추정한 position covariance가 GPS position covariance보다 클 경우에만 GPS factor를 추가하고 있다.

E. Loop Closure Factor

factor graph를 사용하기 때문에 쉽게 loop closure를 도입할 수 있다. 나이브하지만 효과적인 유클리디안 거리 기반의 loop closure detection 방식을 사용함. 우리가 제안한 프레임워크는 point cloud descriptor를 사용하는 다른 프레임웍과 비견됨을 강조함.

• 새로운 state $\bold{x}_{i+1}$이 factor graph에 추가되면 새 state와 가까운 이전의 state가 있는 지 그래프 내에서 탐색한다.

• 탐색에 성공한 경우 예를들어 $\bold{x}_3$가 후보로 선택된 경우, $\bold{F}_{i+1}$와 sub-keyframes $\{\bold{F}_{3-m},...,\bold{F}_{3+m}\}$을 scan matching에 사용한다. 양쪽 모두 $W$ frame으로 변환하는 것을 잊지 말자.

• Scan matching에 성공하면 $\Delta T_{3,i+1}$을 얻고 loop closure factor로 graph에 추가한다.

여기서 m=12, loop closure 후보에 들기 위한 거리는 $\bold{x}_{i+1}$로부터 15m 이내이다.

실제로, absolute sensor로 사용 가능한 것이 GPS 뿐인 경우, loop closure factor가 state의 altitude(고도)에 대한 drift에 효과적이라는 것을 확인하였음. 이는 GPS센서 특성상 altitude의 정확도가 낮기 때문이다.