[HDL Localization 사용설명서] 패키지 설명 및 동작 원리 (1)

 

 hdl localization은 PCD 형태의 지도에서 로봇의 위치를 추정하는 기능을 제공해준다. GPS, IMU, Odom 등 다양한 입력데이터가 있지만, 모두 옵션이며 1) PCD 지도 2) 3차원 라이다 데이터 2개의 항목만 있으면 정상적으로 작동한다. 3차원 라이다를 활용한 위치추적 모듈 중 가장 사용이 간편하고, 오픈소스화 되어있기 때문에 다양한 프로젝트에서 활용된다. 필자의 경우도 개인적으로 진행했던 다양한 토이프로젝트며, 학부연구생 시절 의뢰받은 다양한 연구과제도 위 패키지를 사용해 해결한 경험이 있다. 그리고 지금도 다양한 용도로 사용중이다.

 

 최근 hdl localization 패키지의 많은 사용빈도에 비해, 내부구조나 원리에 대한 지식이 너무 부족한 것을 느끼고 있다. hdl localization논문을 찾아봐도, 대부분의 내용이 자매 패키지인 SLAM에 대한 설명이라 코드 단위로 파악하기 힘들었다. 그래서 언제 한번 정리해야겠다고 생각중이었는데, 기회가 되어 hdl localization 패키지의 a부터 z까지 정리하여 투고하고자 한다.

 

1. 개요

1.1 패키지 특징

• 3차원 라이다와 IMU를 사용하여 실시간 localization 수행
• 1)IMU와 자체 Odom을 사용한 예측값과 2)멀티쓰레드 NDT로 획득한 측정값을 무향칼만필터를 통해 합성하는 과정을 반복하는 알고리즘이 특징
• Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019

 

2. 동작 원리

1. IMU의 가속도 및 각속도 데이터로 이동량을 추정함
2. LiDAR의 포인트클라우드와 저장된 지도 데이터로 NDT 수행하여 odom을 측정함
3. 1의 추정값과 2의 측정값을 기반으로 UKF 수행
   • 이 때 발생한 추정값과 측정값의 TF 차이를 통해 칼만필터의 평균 및 오차 공분산을 구함
4. 1~3 과정의 반복을 통해 칼만게인을 수렴시켜 정확한 pose를 예측

 

동작의 원리는 생각보다 단순하다. 무향칼만필터를 사용하여 위치를 추정하는데, 칼만필터의 입력에는 예측값과 측정값이 각각 입력되어 게인을 수렴시킨다.

 

우선 예측값(Prediction)에는 두 가지 데이터가 사용된다.

• IMU를 통해서 입력된 가속도, 각속도 데이터
• 라이다의 Odometry 데이터

위 두 가지 데이터를 통해 이전 프레임과 비교하여 라이다가 어느정도 이동했는지 예측한다.

주의 -> 만약 IMU가 없거나 Odometry 데이터 사용 옵션을 False로 설정한경우, 등속도 운동을 가정하여 라이다 위치를 예측한다.

 

그리고 측정값에는 라이다의 데이터가 사용된다.

• NDT scan matching (default)
• 자신의 컴퓨터 연산능력에 따라 다른 스캔매칭 알고리즘을 사용하는 것도 가능하다.

예측값과 측정값이 입력되면 보정(Correction) 단계를 거친다. 무향칼만필터에 입력하고 최적화시키는 과정이다. 이 과정은 복잡하며, 전체 구조를 살펴보고자 하는 목적의 본 섹션에서 전부 설명하기 힘드므로, 칼만필터에 대한 다른 글을 참조하는 것이 유용하다.

 

위 과정을 반복하면 정확한 위치를 예측할 수 있다.