[논문스터디] LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

Estelle Yoon·2025년 3월 18일

LiDAR Odometry inertial lidat odometry study 논문스터디 라이다슬램 라이다오도메트리 슬램

Study

목록 보기

5/8

Bibtex 인용

@INPROCEEDINGS{9341176,
  author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus, Daniela},
  booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
  year={2020},
  volume={},
  number={},
  pages={5135-5142},
  doi={10.1109/IROS45743.2020.9341176}}

요약

smoothing 및 mapping을 활용한 tightly coupled lidat inertial odometry 프레임 워크 제안
factor graph기반 LIO 구성
IMU preintegration 기반 lidat point cloud deskewing 및 초기 추정
odometry 기반 IMU dias 추정
marginalization 기반 포즈 최적화를 통한 실시간 성능 향상
keyframe selection and sliding window를 활용한 sub keyframe을 통한 성능 향상

인트로

비전 기반 SLAM은 장소인식에 유리하지만 initialization, range등이 별로
라이다 기반은 조도 변화에도 불변성 유지가능, fine detail of environment 취득 가능
LOAM이 대표적인데 low-drift, 실시간 pose estimation, mapping제공하지만 voxel map기반이라 loop closing, gps 융합 등에 별로
그리고 LOAM 은 실시간 성능도 구리고 스캔매칭 기반이라 large scale에서 구리다
smoothing and mapping기반 tightly coupled LIO제안
nonlinear 운동 모델 기반 포인트클라우드 deskew
imu를 통해 라이다 스캐닝 동안의 센서 운동 추정 및 최적화의 초기값으로 활용
라이다 오도메트리 IMU bias 추정에 활용
global factor graph를 활용해 traj 추정
- 라이다 imu융하
- pose간 place recognition 통합
- gps, heading등 absolute value활용 가능
- 여러 factor joint optimzation
prior sub keyframe을 통한 pose 최적화
local scan matching → 실시간 good

기여

factor graph기반 tightly coupled LIO 구축
local sliding window based scan matching을 통한 실시간 성능 확보

III. LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING

A. System Overview

로봇 상태
- $x = \begin{bmatrix} R^T, p^T, v^T, b^T \end{bmatrix}^T$
- $R \in SO$ 회전 행렬
- $p \in \R ^3$ 위치 벡터
- $I_r$ IMU bias
3d 라이다, imu, gps 를 입력으로 사용
센서 관측값을 바탕으로 로봇 pose and traj 추정
상태 추정 문제를 MAP 문제로 정식화
- factor graph사용
- 가우시안 노이즈 가정, MAP 추론은 non linear least square 문제랑 같음
factor graph
- state variable
- IMU pre integration
- Lidar odometry
- GPS
- loop closure
새로운 node 추가 조건은 pose 변화량기반
graph 최적화는 bayes tree기반 incrtemental smoothing and mapping 사용 (iSAM2)

B. IMU Preintegration Factor

IMU measurement
- $\hat{\omega}_t = \omega_t + b^\omega_t + n^\omega_t$
- $\hat{a}_t = R^B_W (a_t - g) + b^a_t + n^a_t$
- $\hat{\omega} _t , ~\hat{a} _t$ IMU raw data
- $b^\omega_t, ~ b^a_t$ IMU bias
- $n^\omega_t ,~ n^a_t$ white noise
- $R^B_W$ 월드 좌표계에서 바디기준으로 변환하는 행렬
motion update
- $v_{t+\Delta t} = v_t + g\Delta t + R_t(\hat{a}_t - b^a_t - n^a_t) \Delta t$
- $p_{t+\Delta t} = p_t + v_t \Delta t + \frac{1}{2} g \Delta t^2 + \frac{1}{2} R_t (\hat{a}_t - b^a_t - n^a_t) \Delta t^2$
- $R_{t+\Delta t} = R_t \exp((\hat{\omega}_t - b^\omega_t - n^\omega_t) \Delta t)$
IMU preintegration
- $\Delta v_{ij} = R^T_i (v_j - v_i - g\Delta t_{ij})$
- $\Delta p_{ij} = R^T_i (p_j - p_i - v_i \Delta t_{ij} - \frac{1}{2} g \Delta t^2_{ij})$
- $\Delta R_{ij} = R^T_i R_j$
- IMU bias는 factor graph에서 lidar odometry factor랑 같이 최적화

C. Lidar Odometry Factor

feature extraction
- edge plane 추출
- $F_i = \{F^e_i, F^p_i\}$
key frame selection
- pose 변화가 $1m ~10\degree$ 초과 시 → 메모리 절약 연산 최적화
sub key frame selection based on sliding window
- sub keyframe 기반 voxel 맵 구성
  - 엣지는 0.2미터 해상도 평면은 0.4미터 해상도로
SCAN MATCHING
- IMU 예측 모션 기반 초기값 적용
- feature랑 voxel맵 대응 매칭 수행
relative transformation
- 엣지랑 평면 feature간 거리 계싼 기반
  - $d^e_k = \frac{\left\| (p^e_{i+1,k} - p^e_{i,u}) \times (p^e_{i+1,k} - p^e_{i,v}) \right\|}{\left\| p^e_{i,u} - p^e_{i,v} \right\|}$
    - 그냥 엣지에 대해서 직선 거리 계산하는거임
  - $d^p_k = \frac{\left| (p^p_{i+1,k} - p^p_{i,u}) \cdot \left( (p^p_{i,u} - p^p_{i,v}) \times (p^p_{i,u} - p^p_{i,w}) \right) \right|}{\left\| (p^p_{i,u} - p^p_{i,v}) \times (p^p_{i,u} - p^p_{i,w}) \right\|}$
    - 이건 그냥 평면 사이 거리 계산하는거임
- 가우스 뉴턴 방식으로 최적 변환 도출
  - $\min_{T_{i+1}} \sum_{p^e_{i+1,k} \in 0F^e_{i+1}} d^e_k + \sum_{p^p_{i+1,k} \in 0F^p_{i+1}} d^p_k$
    - 초기 추정값을 가지고 시작해서 오차함수(앞에서 구한 엣지 차이 평면 차이들로 정의됨)을 활용해서 그 차이를 최소화 하는 변환 행렬 구하는거
- 최종적으로 LO factor 계산
  - $\Delta T_{i,i+1} = T_i^{-1} T_{i+1}$

D. GPS Factor

GPS측정값은 local cartesian coordinate로 변환, 새로운 node 추가시에 해당 팩터 같이 넣음
보정 조건
- 라이다 프레임이랑 gps 동기화 안되면 gps를 라이타 프레임 타임스탬프에 맞춰서 선형 보간
- LO 공분산이 GPS 공분산보다 클 경우에만 GPS factor추가 → 항상 추가하는거 아님