[Carla Project] Localization-1: Kalman Filter based IMU GPS Fusion Localization

Carla 시뮬레이션 상에서 자율주행 인지-측위-판단-제어를 모두 구현하는 프로젝트를 하고 있습니다. 첫 번째로 측위에 관한 내용입니다.

 

자율주행을 구현하기 위해 현재 위치를 정확하게 아는 것이 중요합니다. GPS, IMU와 같은 센서들이 측위를 위해 사용되고 있으며, 최근에는 카메라, 라이다를 이용한 측위에 관한 연구도 많이 진행되고 있습니다. 

 

이번 포스팅에서는 IMU와 GPS를 이용한 간단한 글로벌 좌표 측위 방법에 대해 소개하고, Carla 시뮬레이터를 이용하여 검증해 보겠습니다.

 

1-1 GPS Measurement

GPS는 GNSS(Global Navigation Satellite System)의 한 부분으로, 위성 기반 위치 추적 시스템입니다. 경도와 위도를 측정할 수 있어 현재 차량의 글로벌 위치를 알 수 있습니다. 그러나 오차가 크기 때문에 측정값을 그대로 사용하기에 무리가 있습니다.

 

1-2 IMU Odometry

Odometry란 지나온 경로를 추정하는 것입니다. IMU를 이용해 가속도, 각속도와 글로벌 헤딩각을 측정할 수 있고, Speedometer를 이용해 속력(scalar)을 측정할 수 있습니다. 

IMU로 측정한 헤딩의 방향으로, Speedometer로 측정한 속력만큼 움직인다고 가정하고 시스템을 모델링 했습니다.

 

1-3 IMU+GPS Localization

GPS 측정값을 보정하기 위해 IMU와 GPS를 퓨전하는 알고리즘을 설계했습니다. 칼만 필터를 이용하여 두 센서를 퓨전했고, IMU Odometry에서 사용한 시스템 모델을 이용했습니다.

estimation model

선형 시스템이므로 선형 칼만 필터를 이용했습니다. 파라미터 튜닝은 센서 노이즈 크기를 고려하여 진행했습니다. GPS의 분산이 1m 정도이므로 R을 Identity matrix로 설정하고, Q를 조정하며 튜닝했습니다.

 

Result

Fig.1 Carla simulator

데이터는 carla 리더보드 환경에서 직접 주행하며 수집했습니다. IMU, GPS 센서 정보와 Ego vehicle 위치의 Ground Truth를 함께 수집했습니다.

Fig.2 Odometry result(left), error(right)

그림 2의 왼쪽은 경로를 나타낸 그림입니다. 오른쪽 위에서 출발하여 왼쪽 아래까지 우회전 한번 했습니다. 그림 2의 오른쪽 그래프는 1-1 gps measurement, 1-2 imu odometry, 1-3 imu+gps localization의 결과와 ground truth 사이의 오차를 나타낸 그래프입니다.(오차는 유클리디안 거리를 이용하여 계산했습니다.) 1-1 GPS measurement는 꾸준히 일정한 오차를 보입니다. 그러나 그 값이 크고 불규칙해서 사용하기에는 무리가 있어 보입니다. 1-2 IMU odometry는 초반에는 낮은 오차를 보이고 있지만, 시간이 지날수록 오차가 증가하는 특징을 보입니다. 오차가 누적되는 drift가 발생하는 것을 확인할 수 있습니다. 1-3 IMU+GPS localization은 오차가 확연하게 줄어들고, 오차의 최댓값도 0.5m 정도로 성능이 많이 개선되었습니다.

Fig.3 Odometry result start(left), end(right)

그림 3의 파란색 데이터를 보면 GPS 신호는 노이즈가 심해 진동하는 것을 확인할 수 있습니다. 노란색 데이터는 IMU odometry의 결과로왼쪽 처음 출발할 때는 정확해 보이지만, 오른쪽과 같이 시간이 지났을 때는 오차가 누적되어 Ground Truth에서 멀리 떨어져 있는 것을 확인할 수 있습니다. 빨간색 데이터는 IMU+GPS localization 결과로 처음과 끝 모두 Ground Truntu 근처에 머물고 있는 것을 확인할 수 있습니다.

측위 방법	RMSE(m)
GPS Mesaurement	0.6825
IMU Odometry	0.8024
IMU+GPS Localization	0.1212

 

세 방법 중 1-3 IMU+GPS Localization 방법이 가장 우수한 성능을 보였습니다. IMU Odometry는 경향성을 보았을 때 시간이 지날수록 오차가 커질 것으로 예상됩니다.

 

 

Conclusion

RMSE가 0.1212m로 평균 12cm의 오차를 갖고 있다고 생각할 수 있는데, 자율주행 환경의 스케일을 생각한다면 큰 오차는 아닌 것 같습니다. 하지만 측위 정확성을 요구하는 알고리즘을 제작하기에는 무리가 있는 것 같습니다.

 

사실 현재 사용한 모델에는 치명적인 약점이 있습니다. 모델링 과정에서 '차량이 헤딩 방향으로 진행한다'는 가정을 했는데, 차량은 바퀴가 4개이므로 실제 거동에서는 헤딩과 진행 방향이 일치하지 않습니다.

 

다음번에는 Bicycle model을 이용하여, 이러한 문제를 개선한 측위 알고리즘을 설계해 보겠습니다.