Simple 2D LiDAR Odometry using ICP

본 글에서는 ICP(Iterative Closest Point)를 기반으로 한 간단한 2D LiDAR Odoemtry에 대하여 설명한다.

ICP는 아래의 글을 참고하기 바란다.

https://define.tistory.com/entry/Iterative-Closest-Point

(ICP)Iterative Closest Point

 

LiDAR 센서 데이터의 정합(registration)을 이용하여 움직이는 로봇의 궤적을 결정하는 알고리즘을 통상적으로 LiDAR Odometry라고 한다. 가장 기본적인 정합 방법은 ICP(Iterative Closest Point)가 있으며, 이를 이용한 LiDAR Odometry는 아래와 같이 네 단계로 구분된다.

 

[1] LiDAR 센서로 부터 거리와 각도 데이터(scan data)를 측정하고 polar coordinate로 구성된 데이터를 Eucliean coordinate로 변환한다.

[2] [1]에서 얻어진 Euclidean coordinate 상의 데이터는 robot의 위치가 중심인 robot local coodinate이므로 이를 현재 로봇의 추정 pose를 이용하여 global coordinate로 변환한다.

[3] 바로 이전에 얻어진 global coordinate에서의 scan data와 현재 얻어진 scan data를 정합(ICP를 통해서)하여 두 pose간 relative transform을 계산한다.

[4] 현재 로봇의 추정 pose를 [3]에서 추정된 relative transform을 이용하여 update한다.

 

위 네 단계 중 [1] 단계는 아래의 그림 1과 같이 표현할 수 있다.

그림 1. Step [1] ~ [2]의 예. 시각 t-1과 t에서 측정된 각각의 LiDAR 측정값(A, C 에서의 orange line)은 각각 해당 시점에서의 로봇 pose 추정값을 통해 local coordinate로 옮겨진다(B, D). 이 때, 시각 t에서의 로봇 pose는 아직 update 되지 않았음으로, 추정값은 시각 t-1에서의 로봇 pose이다. 

 

그림 1의 A와 C는 각각 시각 t-1과 t에서의 LiDAR 측정값(orange line)을 나타낸 것 이다. 이 때, C에서 하늘색 원과 붉은색 방향 화살표로 표현된 pose가 실제 시각 t에서의 pose이지만, 시각 t에서의 로봇 pose는 아직 update이 되지 않았으므로 추정값은 시각 t-1에서의 로봇 pose 추정값(회색 pose) 이라고 가정한다(동일 속도를 가정하고 초기 추정값을 계산할 수도 있다.). LiDAR 측정값은 각 각도별 거리로 주어진 polar coodinate 데이터 이기 때문에 이를 먼저 그림 1의 B, D와 같이 robot local coordiante(Euclidea coordinate)에서의 point cloud로 변환한다( Step [1] ).  

그림 2. Step [3] ~ [4]

그런 다음, 그림 2의 E와 같이 해당 시점의 추정 pose를 통해 local coordinate에서의 point cloud를 global coordinate로 변환한다( Step [2] ). 그림 2의 E에서 주황색과 하늘색 LiDAR 측정값은 각각 t-1과 t에서의 측정값을 해당 시점의 추정 pose를 이용하여 변환한 것 이다.

마지막으로, 두 global coordinate에서의 point cloud를 ICP를 통해 정합함으로써 시각 t-1과 t 사이의 relative transform을 구할 수 있고( Step [3] ) 이를 통해 시각 t에서의 pose를 update 할 수 있다( Step [4] ).

 

이 과정을 반복 함으로써 LiDAR 센서 데이터를 통해 로봇의 위치를 연속적으로 추정할 수 있다.

이를 Deutches Museum Dataset에 적용한 결과는 아래의 그림과 같다.

 

그림 3. Deutches Museum Dataset에 적용한 결과

 

관련 코드 및 데이터는 아래의 github repository를 참고 바란다.

https://github.com/93won/2D_LiDAR_Odom_ICP