[논문정리] LIO-SAM: Tightly-coupled Lidar Inertial Odometry viaSmoothing and Mapping
https://arxiv.org/abs/2007.00258
LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
We propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO-SAM, that achieves highly accurate, real-time mobile robot trajectory estimation and map-building. LIO-SAM formulates lidar-inertial odometry atop a factor gr
arxiv.org
Abstract
LIO-SAM(Lidar-Inertial Odometry via Smoothing and Mapping) 라이다 관성(inertial) 오도메트리(odometry) 프레임워크 제안.
이는 고정밀 실시간 모바일 trajectory 추정 및 지도 구축.
라이다 관성 오도메트리를 factor graph 위에 정의해 다양한 소스로부터 나온 측정값을 통합할 수 있게 함.
IMU(inertial measurement unit)의 사전 통합(pre-integration)에서 추정된 움직임은 포인트 클라우드를 왜곡 제거(de-skew)하고, 라이다 오도메트리 최적화를 위한 초기 추정을 생성한다.
얻어진 라이다 오도메트리 솔루션은 IMU의 바이어스를 추정하는데 사용된다.
실시간에서 높은 성능을 보장하기 위해 오래된 라이다 스캔을 한계화(marginalize)하여 포즈 최적화한다.
로컬 스케일에서의 스캔 매칭(scan-matching)은 글로벌 스케일에서의 매칭보다 실시간 성능을 크게 향상시킨다.
1. Introduction
상태 추정, 로컬라이제이션 그리고 매핑은 지능형 모바일 로봇을 위한 전제 조건으로 피트백 제어, 장애물 회피, 플래닝 등의 기능들에 필수적이다.
비전 및 라이다 기반 센싱 기술로 실시간 simultaneous localization and mapping (SLAM) 기술은 높은 성능을 보여주며 모바일 로봇이 6자유도(6DOF) 상태 추정을 가능하게 한다.
비전 기반 방식들은 place recognition에 적합하지만, 여러 제약 때문에 자율주행 시스템에는 홀로 쓰이기 어렵다.
라이다 기반 방식들은 조도 변화와 무관하며, 광범위 3d 정보를 갖기 때문에 정밀한 디테일을 찾기에는 라이다가 더 적합하다.
따라서 이 논문에서는 라이다 기반 상태 추정 및 매핑 방식을 소개한다.
Lidar odometry and mapping (LOAM) 방식은 라이다, IMU를 사용하여 KITTI 오도메트리 벤치마크에서 1위를 했지만, 글로벌 복셀 맵에 데이터를 저장함으로써 간혹 루프 클로져 탐지가 잘 안되거나, GPT 등의 절대값을 포즈 수정에 통합하는것이 어려웠다. 또한, 복셀 맵이 dense해지면 실시간성도 떨어졌다.
이 논문에서는 라이다와 IMU를 긴밀히 결합한 LIO-SAM 프레임워크를 소개한다. 이는 포인트 클라우드 왜곡 제거를 수행하며, 라이다 스캔중의 센서 움직임은 원시 IMU 측정값을 사용한다. 추정된 움직임은 초기 라이다 오도메트리 추정에 사용된다. 얻어진 라이다 오모데트리 솔루션은 factor graph의 IMU 편향을 측정하는데에 사용된다.
글로벌 factor graph를 로봇 경로 추정에 사용하면서 라이다와 IMU 센서퓨전을 효울적으로 수행하고, 로봇 포즈간의 장소 인식을 통합할 수 있다. 또한, 가능한 경우 GPS, 나침판 방향 등의 절대 측정값을 사용하여 정확도를 더욱 향상시킨다.
이러한 여러 요소들은 그래프의 joint 최적화를 위해 사용된다.
포즈 최적화를 위한 오래된 라이다 스캔은 LOAM처럼 글로벌 맵에 매칭하는 것이 아니라, 한계화한다. 글로벌 스케일이 아니라 로컬 스케일에서 스캔 매칭하여 실시간 성능을 높인다.
• A tightly-coupled lidar inertial odometry framework built atop a factor graph, that is suitable for multi-sensor fusion and global optimization.
• An efficient, local sliding window-based scan-matching approach that enables real-time performance by registering selectively chosen new keyframes to a fixed-size set of prior sub-keyframes.
2. RELATED WORK
라이다 오도메트리는 일반적으로 ICP(Iterative Closest Point), GICP(Generalized Iterative Closest Point) 등의 스캔 매칭 방법을 사용하여 두 연속적인 프레인 사이의 상대 변환(relative transformation)을 찾아 수행된다.
모든 포인트 클라우드를 매칭하는 대신, feature-based 매칭 방식은 계산 효율성 때문에 주로 사용된다.
- plane-based registration approach [5]: 포인트 클라우드로부터 planes를 추출하고 이를 least-square 문제로 풀어 매칭.
- collar line-based method [6]: 오리지널 포인트 클라우드로부터 랜덤하게 line segment가 생성되고 등록됨.
라이다를 홀로 사용하는 것은 rotation mechanism 외곡과 센서 움직임 때문에 문제가 있다.
따라서, IMU나 GPS 등의 센서와 함께 상태를 추정하는 기법이 나왔다.
센서 퓨전 기법은 크게 두 가지로 나눌 수 있다: loosely-coupled fusion and tightly-coupled fusion.
- LOAM: IMU를 라이다 스캔 왜곡을 없애고 스캔 매칭의 motion prior를 주기 위해 위해 사용. loosely-coupled method.
- extended Kalman filters (EKF): 라이다, IMU와 선택적으로는 GPS를 사용하여 로봇 상태 추정
Tightly-coupled는 보통 더 높은 정확도.
- [15]: 사전 통합된 IMU 측정값이 포인트 클라우드의 왜곡을 없애기 위해 사용.
- R-LINS: robocentric lidar-inertial state estimator. error-state Kalman filter를 사용하여 상태를 재귀적으로 추정. 다른 센서는 사용하지 않기 때문에, 장시간 사용하면 오차 누적(drift)문제 발생.
- LIOM: 라이다, IMU 측정값을 함꼐 최적화. 실시간성은 안나옴.
3. LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING
A. System Overview
- $W$: world frame
- $B$: robot body frame
- $x$: robot state, $x = [ R^T, p^T, v^T, b^T ]^T$
- $R$: rotation matrix, $R \in \text{SO}(3)$
- $p$: position vector, $p \in \mathbb{R}^3 $
- $v$: speed
- $b$: IMU bias
- $T$: transformation, $T \in \text{SE}(3)$ from $B$ to $W$, $T = \left[ R \mid p \right]$
위 그림은 lio-sam의 구조이다. 시스템은 라이다, IMU와 선택적으로는 GPS로부터 데이터를 받는다. 로봇의 상태와 주행경로를 찾기 위해 이 센서들을 사용한다.
상태 추정 문제는 maximum a posteriori (MAP) problem 으로 공식화할 수 있다.
저자는 factor 그래프를 사용해 이 문제를 설계했다. 이는 Bayes nets.와 비교했을 때 실시간성이 더 뛰어나다.
가우시안 노이즈 모델을 가정하면 MAP 추론은 nonlinear least-squares 문제를 푸는 것과 동일하다.
factor 그래프를 만들기 위한 4가지 factors와 한 개의 variable을 설정한다. 이 변수(variable)는 특정 시점의 로봇의 상태를 나타내며, 그래프의 노드로 할당된다. 4가지 factors는 다음과 같다.
- IMU preintegration factors
- lidar odometry factors
- GPS factors
- loop closer factors
새로운 로봇 state 노드인 $x$는 로봇 포즈가 임계값을 넘었을 때 그래프에 추가된다.
Factor 그래프는 새로운 노드가 삽입될 때 Bayes tree(iSAM2) 방식으로 점진 스무딩 및 매핑으로 최적화된다.
B. IMU Preintegration Factor
IMU로부터 나온 angular velocity and accleration값은 다음 식으로 정의된다.
이 값을 로봇의 움직임 추정에 사용할 수 있다. Velocity, position and rotation은 다음 식으로 정의된다.
IMU preintegration은 factor 그래프의 제약 요소로 활용되기도 한다. IMU bias는 그래프의 라이다 오도메트리 factors와 함께 최적화된다.
C. Lidar Odometry Factor
새로운 라이다 스캔이 도착하면 feature extraction을 진행한다.
엣지(edge)와 평면(planner) features는 지역적인 영역에서 점들의 거칠기(roughness)를 평가하여 추출된다. 큰 거친값은 엣지로, 작은 거친값은 평면으로 분류된다.
- $F^e_i$: 라이다 스캔으로부터 추출된 엣지 특징
- $F^p_i$: 라이다 스캔으로부터 추출된 평면 특징
- $\mathbb{F}_i$: $i$시점에 추출된 특징들로 구성된 라이다 프레임. $\mathbb{F}_i = \{ F^e_i, F^p_i \}$
- $B$: 라이다 프레임 $\mathbb{F}_i$
모든 라이다 프레임을 그래프에 추가하는 것은 계산 한계가 있기 때문에 $keyframe$ selection을 사용. SLAM 분야에서 자주 사용되는 기법.
쉽고 효과적인 휴리스틱한 방법으로 로봇 포즈가 변화할 때의 $\mathbb{F}_{i+1}$을 keyframe으로 설정. 이는 factor graph에서 로봇 상태 노드 $x_{i+1}$와 연결되어있다. 이런 방식으로 keyframe을 추가하는 것은 map density와 메모리 소비 사이의 균형을 맟추고, 실시간 nonlinear 최적화에 적합하도록 sparse factor graph를 유지할 수 있게 도와준다.
이 연구에서는 position and rotation 변화 임계값을 1m, 10도로 설정했다.
새로운 상태 노드 $x_{i+1}$을 추가할 때, 이에 상응하는 라이다 keyframe은 $\mathbb{F}_{i+1}$이다.
라이다 오도메트리 factor 생성은 다음 단계로 진행된다.
1. Sub-keyframes for voxel map
슬라이딩 윈도우 방식을 최근 라이다 스캔의 고정된 수를 포함하는(?) 포인트 클라우드 맵을 만들 때 사용.
두 연속적인 라이다 스캔 사이의 transformation을 최적화하기 보다는 최근 $sub-keyframe$이라 칭하는 $n$개의 keyframes을 추출해 추정에 사용한다.
$sub-keyframe$ 셋은 그들과 관련된 변환 $T$ 을 사용하여 프레임 $W$로 변환된다.
이후 복셀 맵 $M_i$로 합쳐진다.
이전 특징 추출 단계에서 2 타입의 특징(엣지, 평면)을 추출했기 때문에 $M^e_i$는 엣지 특징 복셀 맵이고, $M^p_i$는 평면 특징 복셀 맵이다.
라이다 프레임과 복셀 맵은 아래처럼 상호 연관되어 있다.
2. Scan-matching
이제 새롭게 얻어진 라이드 프레임을 스캔 매칭을 통해 $M_i$로 매칭시켜야 한다.
$\{ F^e_{i+1}, F^p_{i+1} \}$을 좌표계 $B$에서 $W$러 변환하고 $\{ 'F^e_{i+1}, 'F^p_{i+1} \}$을 얻는다.
초기 transformation은 IMU로부터 예측된 로봇 모션 $\tilde{T}_{i+1}$로부터 얻는다.
$\{ 'F^e_{i+1}, 'F^p_{i+1} \}$과 연관된 엣지 혹은 평면 correspodence는 $M^e_i, M^p_i$ 이다.
상세 설명 논문 [1] 참조(라고 논문에 쓰여있다.)
3. Relative transformation
특징과 각 엣지와 평면의 patch correspondence 사이의 거리는 아래 식으로 계산된다.
GaussNewton 방식은 최적의 transformation을 찾기 위해 사용된다.
마지막으로, $x+i, x_{i+1}$ 사이의 relative transformation인 $\Delta T_{i, i+1}$를 찾는다. 이것은 두 포즈를 연결하는 라이다 오도메트리 factor이다.
여기서 주목할 것은, $\Delta T_{i, i+1}$을 얻기 위한 대안적 방법으로 $sub-keyframe$을 $x_i$로 변환하는 방식을 사용한다. $\mathbb{F}_{i+1}$을 복셀 맵으로 매치하여 $x_i$라 표현한다. 이런 방식으로 실제 상대 변환식인 $\Delta T_{i, i+1}$가 직접적으로 얻어진다. 왜냐하면 변환 특징 $\{ 'F^e_{i+1}, 'F^p_{i+1} \}$는 여러번 사용될 수 있기 때문이다.
D. GPS Factor
상태 추정 및 매핑을 IMU와 라이다 만으로도 충분히 할 수 있지만, dirft 문제가 있으므로 이를 제거하기 위해 alimeter(고도계), 나침반(compass), GPS를 사용한다.
GPS 측정값을 받으면 그것을 local Cartesian coordinate frame으로 변환한다.
Factor 그래프에 새로운 노드를 추가한 후 이를 GPS factor와 연동한다. 만약 GPS 신호가 하드웨어와 동기화가 되어있지 않다면 라이다 프레임의 타임스탬프를 기준으로 GPS 측정값을 선형적으로 보정한다.
GPS는 추정된 위치 covariance(불확실성)가 GPS 위치 covariance(불확실성) 보다 클 때만 사용한다.
E. Loop Closure Factor
Factor graph 덕분에 루프 클로져가 더 쉬워짐. [22] and [23] 등의 기법들과 결합해서 사용도 가능(point cloud descriptor and place recognition).
설명을 위해 Euclidean distance 기반 루프 클로져 사용.
새로운 상태 $x_{i+1}$가 factor 그래프에 추가되면 $x_{i+1}$와 가장 가까운 이전 상태를 찾는다. Fig 1. 의 검은색 선을 참고하면 $x_3$이 제일 가깝다.
그 다음 스캔 매칭을 통해 $\mathbb{F}_{i+1}$을 서브 키프레임 집합 $\{ F_{3-m}, \dots, F_3, \dots, F_{3+m} \}$ 으로 매칭시킨다. $\mathbb{F}_{i+1}$와 이전 서브 키프레임들은 스캔 매칭 전에 $W$로 변환된다. Relative transformation $\Delta T_{3, i+1}$을 얻으면 이를 그래프의 루프 클로져 factor로 추가한다.
실제로 루프 클로져 요인을 추가하는 것은 GPS가 유일한 절대 센서일 때 로봇의 고도 drift를 조정하는데 효과적이다,
