Archive

Journal of Korea Robotics Society - Vol. 18 , No. 1

[ ARTICLE ]
The Journal of Korea Robotics Society - Vol. 18, No. 1, pp. 72-81
Abbreviation: J. Korea Robot. Soc.
ISSN: 1975-6291 (Print) 2287-3961 (Online)
Print publication date 01 Mar 2023
Received 31 Oct 2022 Revised 05 Dec 2022 Accepted 16 Dec 2022
DOI: https://doi.org/10.7746/jkros.2023.18.1.072

비정형 환경 내 지도 작성과 자율주행을 위한 GNSS-라이다-관성 상태 추정 시스템
길현재1 ; 이동재2 ; 송관형2 ; 안승욱3 ; 김아영

Tightly-Coupled GNSS-LiDAR-Inertial State Estimator for Mapping and Autonomous Driving
Hyeonjae Gil1 ; Dongjae Lee2 ; Gwanhyeong Song2 ; Seunguk Ahn3 ; Ayoung Kim
1Master’s Student, Mechanical Engineering, Seoul National University, Seoul, Korea (h.gil@snu.ac.kr)
2Student, Mechanical Engineering, Seoul National University, Seoul, Korea (pur22@snu.ac.kr)(skh8464@snu.ac.kr)
3Research Engineer, Advanced Technology Team, Hanwha Aerospace, Seoul, Korea (seunguk.ahn@hanwha.com)
Correspondence to : Associate Professor, Mechanical Engineering, Seoul National University, Seoul, Korea (ayongk@snu.ac.kr)


CopyrightⓒKROS
Funding Information ▼

Abstract

We introduce tightly-coupled GNSS-LiDAR-Inertial state estimator, which is capable of SLAM (Simultaneously Localization and Mapping) and autonomous driving. Long term drift is one of the main sources of estimation error, and some LiDAR SLAM framework utilize loop closure to overcome this error. However, when loop closing event happens, one’s current state could change abruptly and pose some safety issues on drivers. Directly utilizing GNSS (Global Navigation Satellite System) positioning information could help alleviating this problem, but accurate information is not always available and inaccurate vertical positioning issues still exist. We thus propose our method which tightly couples raw GNSS measurements into LiDAR-Inertial SLAM framework which can handle satellite positioning information regardless of its uncertainty. Also, with NLOS (Non-light-of-sight) satellite signal handling, we can estimate our states more smoothly and accurately. With several autonomous driving tests on AGV (Autonomous Ground Vehicle), we verified that our method can be applied to real-world problem.


Keywords: LiDAR-Inertial SLAM, GNSS Fusion, Autonomous Driving

1. 서 론

자율주행을 위한 인지 시스템에 있어서 가장 중요한 2가지는 정확한 주변 환경 인식과 안정적인 상태 추정이다. 특히 야외 자율주행의 경우, 자신 주변의 차량, 보행자, 건물 등 모든 주변 환경을 정확하게 인식해야 함과 동시에 현재 자신의 위치나 속도 등의 상태를 항상 관측 가능해야 한다. 이러한 요구에는 거리 정보를 정확하게 측정할 수 있는 라이다 SLAM (Simultaneously Localization and Mapping) 이 적절한 해결책이 될 수 있으며, 관련해서 많은 연구들이 진행되어 왔다. 최근에는 상대적으로 측정 주기가 느린 라이다의 단점을 보완하고자 빠른 측정 주기의 관성 측정 장치(IMU)를 융합한 라이다-관성 SLAM (LiDAR-Inertial SLAM) 분야에서 특히 많은 발전이 이루어졌다.

잘 설계된 센서 측정 모델을 통해 인접한 상태 사이에서는 일관성을 유지할 수 있지만, 천천히 쌓이는 오차로 인해 시간이 지날수록 드리프트가 발생한다. 이를 해결하기 위해서 대부분의 SLAM 알고리즘은 특정 위치를 재방문했을 때 loop closure를 통해 모든 상태를 업데이트하며 드리프트를 해결한다. 하지만 운용 상황에 따라 항상 같은 위치 재방문이 불가능할 수도 있으며, 무엇보다 loop closure를 통해 모든 상태를 업데이트하는 순간 최신 상태가 크게 변한다. 이는 자율주행 차량처럼 상태를 매끄럽게 추적해야 하는 상황에서는 사용할 수 없으며, loop closure 없이도 드리프트가 적은 SLAM이 필요하다.

대표적인 해결책으로 GNSS(Global Navigation Satellite System)을 활용하는 방법이 있다. GNSS를 사용하면 수신기의 절대 위치를 알 수 있기 때문에 드리프트가 생기더라도 이를 보정할 수 있다. 이러한 장점 덕분에 SLAM에서 GNSS를 함께 사용하는 많은 연구들이 있었으며, 이를 통해 확연히 적은 경로 오차를 보여주었다.

하지만 일반적으로 라이다 odometry에 비해 GNSS가 제공하는 데이터는 상대적으로 큰 오차를 가지며, 이를 그대로 시스템에 사용하게 되면 상태 추정기가 국소적으로 진동하는 문제가 발생할 수 있다. 이를 필터링하고 신뢰도 높은 PVT (Point, Velocity and Time)을 제공하는 수신기는 가격이 높고 부피도 큰 편이라, 성능뿐만 아니라 가격 및 안전도 중요한 상업 레벨의 자율주행 차량에 이러한 장비를 사용하는 것은 현실적으로 불가능하다. 따라서 우리는 저가형 RTK GNSS 수신기를 활용해서 문제를 해결하고자 했다. RTK (Real Time Kinematics) 란 기지국으로부터 받은 보정 데이터를 이용해서 로버가 받은 측정값을 보정하는 기술로, 훨씬 적은 오차의 데이터를 수신할 수 있다. 또한, 수신기가 내부적으로 처리한 PVT를 그대로 사용하지 않고, GNSS로부터 수신한 의사 거리, frequency shift 등의 측정값을 활용해 라이다 odometry, 관성 측정 장치의 pre-integration과 함께 최적화를 수행함으로써 상태 추정기가 외란이 섞인 GNSS 측정값에도 강건해지도록 설계했다.

본 논문에서 제안하는 논점은 다음과 같다.

  • - 센서 노이즈를 효과적으로 처리할 수 있는 Factor graph optimization 기반의 GNSS-라이다-관성 SLAM을 제안했다.
  • - 라이다 점군 지도 기반의 GNSS NLOS 신호 탐지와 sliding window 기반 측정값 추적을 통해 부정확한 GNSS 데이터에 대응할 수 있다.
  • - 본 논문의 방법론을 통해 자율주행이 가능함을 실험적으로 검증하였다.

2. 선행 연구 조사
2.1 라이다-관성 SLAM

주변 환경의 거리 정보를 정밀하게 파악할 수 있는 라이다 센서와, 측정 주파수가 높은 관성 측정 장치의 상호 보완적인 관계를 이용한 라이다-관성 SLAM 분야는 활발히 연구되어 왔다. 크게 filter 기반과 graph 기반으로 분류할 수 있으며, 대표적인 filter 기반 방법론들은[1,2] 현재 측정값만을 optimization에 활용하기 때문에 빠르게 현재 pose를 추정할 수 있다는 점에서 널리 사용되고 있다. 이와는 달리 graph 기반 방법론은[3,4] 현재 측정값을 이전 측정값과 함께 optimize하기 때문에, 이전 측정값에 대한 re-linearization을 통해 더 높은 정확도를 이끌어낼 수 있다고 알려져 있다. 더 나아가, 다양한 factor를 구성하고 추가할 수 있다는 장점 덕분에 GPS 등 새로운 측정값을 optimization 단계에 같이 고려하기에 적합하다. Graph 기반의 강한 결합 SLAM 중 state-of-the-art로 알려진 LIO-SAM[3]은 임계값보다 큰 폭으로 변한 pose를 node로 추가하고, 각 node를 IMU pre-integration[5] factor, odometry factor, GNSS factor, 그리고 loop closure factor로 연결한다. LeGO-LOAM[6]과 유사하게 현재 scan으로부터 edge feature와 planar feature를 추출하여 각각 Levenberg-Marquardt (LM) optimization을 수행한 결과를 odometry factor로 사용하고, GNSS PVT fix 신호를 prior GNSS factor로 사용함으로써 신뢰도 높은 상태 추정 성능을 보였다. 본 논문에서는 기존 LIO-SAM 시스템에 융합될 수 있도록 모델링한 GNSS 원시(raw) 측정값 factor를 통해 상태 추정 정확도를 높이고자 한다.

2.2 GNSS 센서 융합
2.2.1 카메라-관성-GNSS 센서 융합

4 Degree-of-Freedom에 대해 관측이 불가능한 VIO (Visual-Inertial odometry)에 절대 위치 정보를 제공하는 GNSS를 융합하는 많은 시도가 있었다. VINS-Fusion[7]는 카메라, 관성 측정 장치 등 local 센서와 위성 항법 시스템, 지자기계 등 global 센서를 융합하는 프레임워크를 제시하였다. [8]은 스테레오 VINS와 GNSS PPP (Precise Point Positioning)를 결합하여 측위 정확도를 크게 향상시키면서 drift를 감소시켰다. 이러한 약한 결합(loosely-coupled) 방식과 달리, GVINS[9]은 GNSS raw 측정값을 사용해 의사 거리와 Doppler shift를 factor로 모델링하여 이를 카메라-관성 시스템과 함께 강한 결합(tightly-coupled) 방식으로 factor graph 프레임워크에서 융합하였다. 이 덕분에 GNSS 신호가 자주 끊기는 환경이거나 사용 가능한 위성의 개수가 적을 때에도 VIO의 drift 현상을 효과적으로 억제하는 성과를 보였다. IC-GVINS[10]은 관성 측정 장치의 이상치 제거 시 GNSS 측정값을 활용하여 GVINS보다 외란에 강건하고 우수한 성능을 보였다.

2.2.2 라이다-관성-GNSS 센서 융합

[3], [11]와 같이 대부분의 라이다-관성-GNSS 센서 융합은 GNSS 수신기에서 자체적으로 계산된 지구 좌표계 상의 위치를 통해 라이다-관성 시스템의 로봇 위치를 보정하는 과정으로 구성된다. 이러한 약한 결합 기반의 센서 융합은 일반적으로 GNSS 수신기의 상태에 크게 의존적이라는 한계가 있다. [12]은 [11]처럼 GNSS의 원시 측정값을 활용하여 강한 결합 시스템을 구축했지만 localization에 초점을 맞추었다. 또한, 라이다-관성 시스템에 GNSS를 결합한 기존 시도들은 NLOS (Non-line-of-sight) 신호를 탐지하여 필터링하는 과정이 없어 의사 거리 산출에 큰 불확실성을 내포하고 있다. 이에 본 연구는 GNSS 원시 측정값에서 NLOS 신호를 필터링한 라이다-관성-GNSS 강한 결합 기반의 SLAM 프레임워크를 최초로 제안한다.

2.3 GNSS NLOS 탐지

도시 협곡(Urban canyon)이나 숲과 같이 하늘이 트이지 않은 환경에서는 GNSS 신호가 구조물에 막히거나 반사되는 다중 경로 효과(Multipath effects)가 발생한다. 구조물과 다중 반사를 거쳐 GNSS 수신기에 도달하는 NLOS 신호는 의사 거리를 산출할 때 정확도를 현저히 감소시키는 원인으로 작용한다. [13]는 NLOS 신호를 제거하고 LOS 신호만을 사용하였을 때 3D position의 정확도가 30.86% 향상되었다고 밝혔다. [14]에서는 NLOS 신호에 의한 의사 거리 오차가 고각이나 수신기로부터 주변 구조물까지의 거리 등의 요인에 따라 최대 10m에 이를 수 있다고 주장하였다. 따라서 어떤 위성에서 온 신호가 NLOS인지 탐지한 후 적절하게 대처를 하는 것은 GNSS의 정확도를 향상시키는데 있어 매우 중요한 과제이다.

[15]는 통계적인 방법을 이용하여 NLOS 신호를 검출하는 방법을 제시하였으나, PVT를 제공하는 VTL(Vector Tracking Loop) 기반 수신기를 필요로 한다. [16]은 NLOS를 따로 분류하지 않고 의사 거리를 산출한 다음 outlier에 강건한 비선형 최소 자승법을 적용하는 방법을 제시하였으나, 연결된 위성 수나 수신 상태에 따라 정확도가 크게 좌우된다. 통계적 방법을 기반으로 한 솔루션은 명확한 한계가 존재하므로, 본 연구는 다중 센서를 기반으로 한 직접적인 경로 추정 모델을 사용하였다.

[17-19]은 다양한 센서(어안 카메라, 적외선 카메라)로 하늘을 촬영한 후, 구조물에 의해 차폐된 영역에 위성이 있는 경우 그 위성으로부터 받은 신호를 NLOS로 분류한다. 직접적인 경로 추정으로 간단하게 NLOS 신호를 분류한다는 장점이 있지만, 오로지 GNSS 필터링을 위해 일반적인 자율 주행 센서 시스템에 포함되지 않는 sky-view camera가 추가로 요구된다.

라이다는 자율 주행 시스템에 대표적으로 활용되는 센서이자 주변 구조물의 형상을 점군 지도로 쉽게 파악할 수 있으므로, 직접적인 경로 추정 방식으로 NLOS 신호를 효과적으로 분류할 수 있다. 따라서 본 연구는 라이다 점군 지도를 기반으로 한 NLOS 탐지 방식을 채택하였다. [20]는 라이다에 의해 생성된 점군 지도를 GNSS Skyplot에 투영하여 위성 신호가 구조물에 차폐되는지 여부를 통해 NLOS를 제외한 후, LOS 신호를 기반으로 가중 최소 자승법을 수행하였다. [21], [22]는 수신기와 위성을 이은 직선이 점군 지도와 겹치는지 여부를 판단하여 NLOS를 분류하고, 의사 거리 계산 시 NLOS에 작은 가중치를 곱하여 함께 반영하였다. [23]은 구조물 상의 반사점을 탐색하여 NLOS 신호에 의한 의사 거리를 보정하는 방법을 제시하였지만, 숲과 같이 불규칙한 형태의 구조물이 많은 환경에서는 위성 신호가 난반사할 가능성이 있다. 이에 본 연구는 [21], [22]와 같이 NLOS와 LOS 신호 간에 서로 다른 가중치를 부여하여 의사 거리를 추정하는 방법을 사용하였다.


3. 연구 방법
3.1 시스템 정의

추정하고자 하는 상태를 정의하기 앞서, 사용하는 좌표계와 그 변환 관계를 먼저 정리할 필요가 있다. [9]과 같은 좌표계를 사용했으며, 이를 간략하게 정리하면 다음과 같다.

  • - 지구 중심을 기준으로 하며, 북극 방향을 z축으로하고 적도 평면과 본초 자오선이 만나는 지점까지의 방향을 x축으로 하는 ECEF 좌표계 (⋅)e
  • - 지구 위 임의의 한 점을 기준으로, 동쪽을 x축, 북쪽을 y축, 그리고 중력 반대방향을 z축으로 하는 ENU 좌표계 (⋅)n
  • - 라이다와 관성 측정 장치를 통해 초기화를 진행한 local world 좌표계 (⋅)w

상태 추정기는 local world 좌표계를 기준으로 자신의 상태를 추정함과 동시에 맵핑을 진행하며, 유효한 GNSS 정보를 수신하면 ECEF, ENU, local world 사이의 변환 관계를 추정하기 시작한다. ENU 좌표계는 ECEF 좌표계에서의 임의의 점을 원점으로 한다. 이 임의의 점을 anchor point pance라 부르며, anchor point는 local world 좌표계의 원점과 일치하기 때문에, ENU 좌표계와 local world 좌표계 사이의 변환관계는 z축을 기준으로 yaw 각도 ψ만큼 회전으로 표현된다. 이를 바탕으로 추정하고자 하는 로봇의 상태 X를 다음과 같이 정의할 수 있다.

X=ptw,vtw,Rtw,bt,bU,bR,bE,bC,b˙,pance,ψ(1) 

ptwR3, vtw R3, RtwSO3는 각각 시간 t일 때의 로봇의 위치, 속도, 그리고 방향을 의미하며, bt∈R6는 관성 측정 장치의 가속도와 각속도의 bias를 의미한다. bU, bR, bE, bC[m]는 GPS, GLONASS, GALILEO, BEIDOU 위성 항법 시스템과 관련된 GNSS 수신기의 시계 bias δtU, δtR, δtE, δtC로 인한 bias 항이다. 수신기의 시계 bias, anchor point, 그리고 yaw 각도는 유효한 GNSS 관측값이 들어오기 시작하면 추정하기 시작하는 파라미터이다.

본 논문에서 제안하는 시스템은 라이다, 관성 장치, 그리고 GNSS로부터 받아온 센서 측정값을 통해 로봇이 움직이는 동안의 상태 x를 추정하는 것을 목표로 한다. 상태는 측정값으로부터 추정된 사후 확률로 표현하며, 이 때 노이즈 모델이 가우시안 분포를 따른다고 가정하면 사후 확률 추정 문제는 비선형 최소자승법을 해결하는 문제로 표현할 수 있다.

3.2 Factor Graph 구성

Factor graph는 추정하고자 하는 상태와 관련된 측정 모델을 measurement factor로 정의하기에 용이하고, 서로 다른 센서로부터의 measurement factor을 결합하기에 적절하다. 본 논문에는 [Fig. 1]과 같이 IMU pre-integration factor, LiDAR odometry factor, 그리고 GNSS factor를 사용했다. GNSS factor는 총 4가지(code pseudorange, doppler shift, clock bias, and clock bias rate factor) 로 이루어져 있으며, [9]의 모델링을 참고했다.


[Fig. 1] 
Overall factor graph diagram for our system. System utilizes LiDAR odometry, IMU pre-integration, GNSS clock bias, clock bias rate, code pseudorange, and doppler frequency shift measurement. For GNSS-LIO initialization, sliding window is used to track local drift-free state variables[9]

3.2.1 LiDAR Factor

3D 라이다 스캔 사이의 변환관계를 추정하는 LiDAR factor는 [3]의 모델을 사용했다. 연산과 메모리의 효율성을 위해 모든 라이다 스캔이 아닌 keyframe을 기준으로 작업을 수행한다. 국소적인 곡률을 기반으로 라이다 스캔 내부 점군을 edge 특징점과 planar 특징점으로 구분하며, edge 특징점 점군 지도와 planar 특징점 점군 지도를 나누어 관리한다. 점군 지도와 현재 스캔 사이의 거리가 최소화되는 방향으로 현재 스캔의 좌표계와 local world 좌표계 사이의 6 DoF 변환관계를 추정하며, 추정된 Ti+1SE(3)와 이전 keyframe Ti 사이의 상대적인 변환 관계를 LiDAR factor로 정의한다.

ΔTi,i+1=TiTTi+1(2) 

즉, LiDAR factor는 두 keyframe 사이의 상대적인 변환 관계에 대한 constraint가 된다.

3.2.2 IMU Factor

관성 측정 장치는 순간적인 움직임으로 인해 발생한 가속도와 각속도를 100 Hz 이상의 주기로 측정한다. 이 정보를 적분함으로써 짧은 순간동안 발생한 속도 변화, 위치 변화, 그리고 orientation 변화를 추정할 수 있다. 현재 keyframe과 이전 keyframe 사이의 짧은 순간에 측정된 가속도와 각속도 정보를 pre-integration[5] 함으로서 도출된 속도, 위치, 방향의 변화를 IMU factor로서 사용한다.

Δvi,i+1=RiTvi+1w-viw-gΔti,i+1Δpi,i+1=RiTpi+1w-piw-viwΔti,i+1-12gΔti,i+12ΔRi,i+1=RiTRi+1(3) 
3.2.3 GNSS Code Pseudorange Factor

각 위성은 고유한 PRN 코드를 가지고 있으며, 해당 코드를 이용해 변조된 라디오파를 송신한다. GNSS 수신기는 수신된 정보를 바탕으로 해당 코드를 재구성하고, 원본 코드와 재구성된 코드 사이의 시간 변화를 통해 위성과 수신기 사이의 의사 거리(pseudorange)를 측정한다. pseudorange는 아래와 같이 모델링할 수 있으며[24], 이를 code pseudorange factor로 표현할 수 있다[9].

P~rk=pke-pre+bi+Trk+Irk+Mrk+ϵpsrk(4) 

pke, pre은 각각 ECEF 좌표계에서의 위성과 수신기의 위치이고, bi (i = {U, R, E, C})는 수신기의 시계 bias, Trk, Irk는 각각 전리층과 대류권 내 시간 지연 효과, Mrk은 다중 경로(multipath) 효과, 그리고 ϵpsrk는 측정 오차이다.

3.2.4 GNSS Doppler Factor

수신기가 관측한 각 위성의 PRN 코드의 주파수 변화 Δf~rk를 관측했을 때, 위성의 속도를 알고 있다면 이를 이용해서 로봇의 속도를 추정할 수 있다.

λΔf~rk=-1rkvke-vre+b˙+ϵdoppk(5) 

λ는 신호의 파장, 1rk는 수신기로부터 위성까지의 방향 벡터, vke, vre은 각각 위성과 수신기의 속도, b˙는 수신기 시계의 bias 변화율, 그리고 ϵdoppk은 측정 오차이다. 위성 시계의 drift rate, 전리층 및 대류권에서의 신호 지연 변화율, 그리고 다중 경로 효과의 시간 변화율은 주된 오차 원인이 아니므로 무시할 수 있다[24].

Code pseudorange와 doppler shift에 대한 측정 오차는 서로 독립적이고 기댓값이 0인 가우시안 분포를 따른다고 가정하며, 이 때의 uncertainty는 [9]과 동일하게 적용한다.

σr,psrs=nsnprsinθel,  σr,dopps=nsnprsinθel(6) 

임계값 이하의 고각에 위치한 위성은 factor 연산에서 제외하고, 위성의 고각이 낮을수록 factor uncertainty를 높임으로써, 낮은 고도의 위성에서 발생할 수 있는 전리층 시간 지연을 보상할 수 있다. 하지만 높은 고각을 가지는 위성이라도 주변 환경에 높은 건물이나 장애물이 있다면 신호가 분리되고 지연되어 수신되는 다중 경로 효과가 발생할 수 있다. 이에 대응하기 위해 3.3에서는 가변적인 분산을 소개한다.

3.2.5 GNSS clock bias factor

GNSS 수신기의 시계 bias는 시간이 지남에 따라 조금씩 변한다. 본 논문에서 사용하는 4개의 GNSS 시스템(GPS, GLONASS, GALILEO, BEIDOU) 마다 서로 다른 시계 bias가 존재하며, 각 시계의 bias 변화율은 b˙로 같다고 가정한다. 이를 통해 다음과 같이 clock bias factor를 모델링할 수 있다.

Δbi,i+1=bi+1,k-bi,k-titi+1b˙dt, k=U,R,E,C(7) 
3.2.6 GNSS clock bias rate factor

GNSS 수신기의 시계 bias는 시간에 따라 천천히 변한다고 가정했을 때, 시계 bias 변화율에 대한 clock bias rate factor를 다음과 같이 작성할 수 있다.

Δb˙i,i+1=b˙i+1-b˙i(8) 
3.3 Submap 기반의 NLOS (Non-line-of-sight) 감지

Multipath 효과는 다른 GNSS 수신 오차 원인들보다 해결하기 까다롭다[25]. 앞서 언급한대로 낮은 고각의 위성은 factor graph 연산에서 제외되지만, 나무 바로 아래나 높은 건물 옆에 위치해서 특정 위성에 대한 LOS (line-of-sight)가 확보되지 못하는 등 높은 고각의 위성에 대해서도 조치를 취해야 하는 상황이 발생할 수 있다. 즉, 단순히 고정된 고각을 기준으로 factor 연산에 추가할지를 결정하는 것 이외에도, 관측된 위성 정보의 필터링 여부는 건물의 높이 등 주변 환경에 맞추어 유연하게 결정해야 한다. 라이다 SLAM은 주변 환경의 정확한 거리 정보를 얻는 데에 특화된 센서인 라이다를 이용해서 지도를 작성하기 때문에, monocular Visual SLAM보다 정확한 스케일의 점군 지도를 생성할 수 있다. 이러한 이점을 활용해 [Fig. 2]와 같이 현재 로봇의 위치 주변 점군을 불러와 NLOS를 파악할 수 있다.


[Fig. 2] 
NLOS signal detection utilizing LiDAR submap (in bird’s eye view). Yellow satellites are in LOS, and reds are NLOS. The darker the red is, the higher obstacles’ density

먼저 현재까지 저장된 keyframe 점군들을 kdtree에 넣고 현재 라이다 스캔을 중심으로 radius search를 통해 주변 점군을 불러온다. 이렇게 불러온local world 좌표계 기준의 점군을 라이다 좌표계로 변환하고 downsample한 점군의 집합을 submap MsubL이라고 정의한다. 이후 현재까지 추정된 상태를 이용해, 현 위치에서 GNSS 수신기가 관측하고 있는 위성 sk의 라이다 좌표계 기준 고각 θ~skL과 방위각 ϕ~skL을 계산한다. MsubL로부터 각 위성의 방위각 주변 점군만을 subsampling한 점군 Msub,ϕ~skLL 내의 모든 3d point piL들에 대해서, 수신기로부터 위성 sk까지의 방향 벡터 lkL와의 line-to-point 거리 dk,i를 구한다.

dk,i=lkL×piLlkL(9) 

dk,i < dthres인 점군 Pnlos = {pi}(i = 1, ⋯, N)가 Nthres보다 많다면, 위성은 주변 장애물로 인해 LOS가 확보되지 못하는 것으로 판단한다. 그리고 Pnlos의 질량 중심 pcom=i=1NpiNlkL로부터 얼마나 벗어나 있는지를 의미하는 dcom=lkL×pcomlkL로 pseudorange factor의 uncertainty를 조절한다.

σr,psr-nlossk=eα1-dcomdthresσr,psrsk,α>0(10) 

예를 들어, 장애물이 위성 신호를 완벽하게 가리고 있다면 PnloslkL 주변에 균일하게 분포할 것이고, dcom은 거의 0이 되어 해당 위성에 대한 pseudorange factor의 uncertainty가 eα배 증가한다. 반대로 점군 Pnlos가 충분히 많더라도, lkL로부터 너무 멀리 떨어져 있어서 dcomdthres에 가깝게 된다면, 해당 측정값의 uncertainty가 증가하지 않을 것이다.

한 가지 더 고려할 사항은 라이다의 작은 수직 화각으로 인해 항상 주변 장애물의 정확한 높이를 파악할 수 없다는 점이다. 이는 나무와 같이 약 3 m 정도의 장애물보다는 인공 구조물에 대해 문제가 되는데, 주행 중 큰 pitch가 발생하지 않을 때 Velodyne VLP-16 라이다를 기준으로 약 27 m이상의 구조물은 정확한 높이를 파악할 수 없다. 이러한 경우를 대비해 높은 고각의 위성은 dthres를 2배로 설정했다.


4. 실험 및 결과
4.1 사용한 시스템과 데이터셋
4.1.1 하드웨어 구성

데이터 취득과 4.4에서의 자율주행 테스트를 위한 하드웨어 구성은 [Fig. 3]과 같다. 모바일 베이스로서 Clearpath 사의 Husky를사용했으며, handheld 데이터를 취득할 때에는Husky에 설치된 센서 시스템을 그대로 사용했다. 라이다는 Velodyne 사의 VLP-16을, 관성 측정 장치는 Microstrain 사의 3DM-GX-25를 사용했다. GNSS 수신기로 사용한 Ublox 사의 ZED-F9P는 약 $300의 가격으로 RTK 신호를 수신할 수 있는 모듈이다. 마지막으로 안테나로는 4개의 GNSS 시스템을 모두 수신할 수 있는 저가형 모델 Tallysman TW8889를 사용했다.


[Fig. 3] 
Hardware system for data acquisition and autonomous driving test. Our system consists of LiDAR, IMU and low cost GNSS receiver on 4-wheeled UGV (Unmanned Ground Vehicle)

4.1.2 데이터셋 구성

평가에 활용한 데이터는 4개의 경로로 이루어져 있다([Fig. 4] 참고). 1번 경로와 2번 경로는 AGV를 이용해 취득했으며, 각각 넓은 비정형의 들판과 좁고 경사가 높은 통로이다. 2번 경로에는 캠퍼스 내 인공 구조물, 높은 가로수, 그리고 차량과 보행자 등의 동적 장애물이 많이 분포해 있다. 3번과 4번 경로는 handheld로 취득한 데이터이며, 넓은 들판과 좁은 폭의 인도를 주행했기 때문에 관측되는 위성의 개수가 20개 이상에서부터 3개 이하로 다양하다. 따라서 본 논문에서 제안하는 상태 추정 시스템이 위성의 개수에 얼마나 강건한지를 간접적으로 볼 수 있다. NLOS pseudorange의 uncertainty parameter α는 0.5를 사용했다.


[Fig. 4] 
Example of data acquisition environment. Left place is unstructured place with greeneries (sequence 1,3, and 4), and right is partially structured placed with some man-made structures (sequence 2)

4.1.3 평가 지표

3번과 4번 sequence처럼 시작점과 도착점이 일치하는 경우에는 첫 번째 라이다 스캔과 마지막 라이다 스캔 사이의 ICP[26]를 통해 구한 변환관계 Tendstart로부터 시작점과 도착점의 차이를 계산했다. 재방문이 없는 1번과 2번 경로는 시작점과 도착점의 위치를 Sokkia GRX3 RTK GNSS 장비를 이용해서 미리 측량한 후, 두 점의 차이를 계산했다.

4.2 경로 오차

[Fig. 5]를 통해 4.1.2에서 설명한 경로와 점군 지도를 확인할 수 있으며, 각 경로에 대한 end-to-end translation error는 [Table 1]에 작성하였다. 짧은 경로에 해당하는 sequence 1와 sequence 2를 살펴보면, 주행 시간이 짧다 보니 drift가 적어서 odometry로도 오차가 거의 없는 것을 확인할 수 있다. 이에 따라 loop closing이 없는 LIO-SAM과의 성능 차이를 살펴보기 힘들다.


[Fig. 5] 
Reconstructed pointcloud map and resulting trajectories of each state estimators. Four different sequences were used for evaluation, and pointcloud maps are overlapped on the satellite images. Overall mapping results show that our system could reconstruct the real-world environment

[Table 1] 
End-to-end translation error of GLIO and LIO-SAM (without loop closure) for each sequence (in meters)
GLIO LIO-SAM [3]
Sequence 1 0.04498 0.07502
Sequence 2 0.48934 0.3945
Sequence 3 1.08757 1.20461
Sequence 4 0.47547 5.41202

하지만 [Fig. 6]의 가장 경로가 긴 sequence 4을 살펴보면, LIO-SAM에 비해서 본 논문에서 제안하는 시스템(GLIO)이 월등히 적은 오차를 보여준다. LIO-SAM에서 추정한 상태의 오차의 대부분은 z 성분으로, 전체 translation error에 평균적으로 80% 정도를 차지한다. 이는 기존 라이다-관성 odometry가 spatial 방향으로는 오차가 상대적으로 적지만, 수직 방향으로는 크다는 것을 의미한다. 라이다는 최대 측정 거리에서 이점을 가지지만, 측정 거리가 멀어질수록 관측 모델의 오차가 커진다. 넓은 들판이 많은 비정형 환경 특성 상 항상 가까운 거리에만 feature가 존재하지 않기 때문에 이러한 관측 오차가 커진다. 반대로 GNSS 원시 측정값을 사용하는 GLIO의 경우 이러한 수직 방향의 오차를 의사 거리 측정값을 통해 보상할 수 있다.


[Fig. 6] 
Vertical (z) axis translation error plot of sequence 4. Regarding that the start and end positions are the same, only GLIO could estimate z-position accurately

[Table 2]를 보면, 약한 결합 방식으로 GNSS를 활용하는 LIO-GPS는 위성 관측값을 사용함에도 불구하고 수직축 오차가 줄어들지 않는 것을 확인할 수 있다. 이는 GNSS 수신기가 제공하는 PVT 정보 내 고도의 신뢰도가 위도 경도보다 낮으므로 LIO-GPS는 고도 정보를 사용하지 않기 때문이다. [Table 1]에는 작성되지 않았지만 고도 정보를 활용할 경우 더 높은 translation error를 보이는 것을 실험적으로 확인하였다. 이러한 측면에서, 각 위성으로부터 관측한 정보를 바탕으로 강한 결합 방식으로 상태를 추정하게 되면 로봇의 현재 z축 위치를 더 정확하게 추정할 수 있다. 이러한 이점 덕분에 loop closing을 사용할 수 없는 자율주행 차량의 상태 추정 문제에 본 논문이 제안하는 시스템을 사용할 수 있고, 이는 4.4에서 실험적으로 검증하였다.

[Table 2] 
End-to-end translation error (in meters) of each algorithm for sequence 4. Total error consists of spatial and vertical error. LIO-GPS denotes loosely-coupled LIO-SAM with (pre-built) GPS factor
GLIO LIO-SAM LIO-GPS
error [m] 0.47547 5.41202 7.42899

4.3 오픈 데이터셋 실험

4.1에서 직접 취득한 데이터셋 이외에도 오픈 데이터셋을 사용해 정형의 도시 환경 내에서의 성능을 평가하였다. UrbanNav dataset[27]은 차량 등의 동적 장애물이 많은 도시 협곡 환경 내에서 라이다, 관성 측정 장치, SPAN-CPT RTK GNSS ground truth와 함께 저가형 GNSS 수신기의 원시 측정값을 제공한다. 실험에 사용한 TST sequence는 총 3.64 km의 경로 내에 높은 빌딩이 많이 존재하기 때문에, 3.3에서 언급한 NLOS 대응 여부에 따른 상태 추정 성능 차이를 측정하기에 용이하다. 같은 경로를 두 바퀴 돌기 때문에 본 논문에서는 절반에 해당하는 한 바퀴 경로에 대해서 정량 평가를 진행했다. 평가 지표로는 별도로 제공된 ground truth 정보와 evo[28] 소프트웨어를 활용해 APE (absolute pose error)를 측정하였다. 데이터셋에 포함된 센서 중 중앙에 배치된Velodyne HDL 32E 라이다와 Xsens MTI 10 관성 측정 장치를 LIO 시스템으로 사용했고, 저가형 GNSS 수신기로는 Ublox 사의 ZED-F9P의 원시 측정값을 사용했다. 해당 센서는 주변 기지국으로부터 RTK 보정 신호를 제공받을 수 있지만, 데이터셋에는 해당 사항 없이 상대적으로 부정확한 원시 측정값과 PVT fix가 제공된다. NLOS parameter로는 α = 2를 사용했다.

비교를 위한 기존 방법으로는 LIO-SAM을 사용했고, 본 논문에서 제안하는 시스템(GLIO)과 NLOS 대응이 없는 GLIO (GLIO w/o NLOS)에 대해서 APE를 평가했다.

[Table 3]에서 볼 수 있듯이 본 논문에서 제안하는 방법이 가장 작은 경로 오차를 보였다. 앞선 실험과 마찬가지로, LIO 시스템과 GNSS 원시 측정값을 강한 결합으로 함께 최적화하면 수평 방향과 수직 방향의 오차를 모두 효과적으로 줄여 가장 낮은 경로 오차를 보이는 것을 알 수 있다. 하지만 GLIO w/o NLOS의 결과를 보면, 위성의 NLOS를 고려하지 않았을 때 경로 오차가 오히려 늘어나는 것을 볼 수 있다. 이는 높은 주변 구조물이 다중 경로 효과를 야기해 GNSS 수신기가 큰 오차의 의사 거리를 받는 경우가 빈번하게 발생했고, 그래프 최적화 단계에서 잘못된 상태를 추정한 것으로 판단된다. 이를 통해 submap 기반으로 NLOS 신호를 감지하고 대응하면, 더 정확한 상태를 추정할 수 있음을 실험적으로 확인하였다.

[Table 3] 
Absolute Pose Error (in meters) of each algorithm for UrbanNav TST dataset. GLIO w/o NLOS refers to our suggested method (GLIO) without NLOS detection and handling part. Results (the lower the better) indicate that tightly-coupled GNSS-LIO with NLOS handling outperforms others
LIO-SAM GLIO
(w/o NLOS)
GLIO
Mean 4.286 5.739 3.227
Median 3.650 3.628 2.787
RMSE 4.916 7.226 3.669

4.4 자율주행 실험

마지막으로, 본 논문에서 제안한 Factor Graph Optimization 기반 라이다-관성 측정장치-GNSS SLAM이 자율주행에 적용이 가능하다는 것을 입증하기 위해 관련 실험을 진행하였다. 앞서 언급한대로 로봇의 자율주행을 위해서는 로봇이 local world 뿐만 아니라 UTM좌표계 상으로도 현재 위치를 정확하고 큰 변화폭 없이 추정하는 것이 필수적이다. 이에 따라 앞서 평가에 사용한 비정형 환경과 유사한 곳에서 자율주행 실험을 수행했다. 먼저 수동으로 주행을 하고자 하는 경로의 UTM 좌표를 취득한 후, 로봇이 추정하는 자신의 상태 pe를 경로 추종 알고리즘의 입력값으로 사용해 로봇이 경로를 따라 주행하도록 하였다. 실험에서 사용한 경로 추종 알고리즘은 로봇의 현재 위치와 전방의 목표 지점 사이의 각을 조향각으로 사용하는 Pure Pursuit[29]를 사용했다.

실험을 위해 총 3가지의 경로(S자형, U자형, 시작점과 도착점이 일치하는 O자형)를 계획하였다. 본 시스템의 상태 추정기에서 추정한 로봇의 상태를 사용하여 [Fig. 7]의 3가지 경로에 대해 각각 자율주행 실험을 진행하였으며, 자세한 경로는 https://youtu.be/uhDbejsSrII 에서 확인할 수 있다.


[Fig. 7] 
Three trajectories used for autonomous driving test. We refer to them as S-shaped, U-shaped, and O-shaped, respectively

각 경로에서 시작점과 도착점 사이 경로 상의 임의의 위치 4곳을 선정하였으며, 로봇이 자율주행으로 각 경로를 주행할 때 임의의 위치 4곳과 얼마나 떨어진 곳을 주행하였는지를 translation error로 측정해 [Table 4]에 작성하였다.

[Table 4] 
Translation error (in meters) of each autonomous driving test path. Among the smooth waypoints, we sampled four and calculated the translation error between the waypoint and robot
Waypoint #1 #2 #3 #4
S-shape Path 0.08900 0.713925 0.620904 0.108850
U-shape Path 1.051110 1.082438 0.913918 1.038486
O-shape Path 0.058741 0.087027 0.425477 0.452374

Translation error의 측정 결과 S자형 경로와 O자형 경로에서는 Translation error가 모두 1 m 이내, U자형 경로의 경우 1 m 안팎 인 것을 확인할 수 있다. 자율주행에 사용한 Pure Pursuit 알고리즘이 현재 위치와 가장 가까운 경로 상의 점으로 수렴하는 것이 아닌 전방의 목표 지점으로 수렴하려고 하는 것을 고려하면, 오차의 크기가 크지 않고 본 시스템에서 추정하는 상태를 자율주행에 사용 가능함을 확인할 수 있다.


5. 결 론

본 논문에서 제안한 저가형 RTK GNSS 수신기를 활용한 factor graph optimization 기반의 GNSS-라이다-관성 SLAM은 외란이 섞인 GNSS 측정값에도 강건하다는 강점을 가진다. 비정형 환경 및 동적 장애물이 많은 환경에서도 월등히 적은 오차를 가지는 것을 실험을 통해 입증하였으며, 특히 기존 LIO-SAM의 취약점인 수직 방향 오차를 의사 거리 측정값을 통해 보상할 수 있어 수직축 오차에서 월등히 높은 성능을 보였다. 또한 약한 결합 방식으로 GNSS를 활용하는 LIO-GPS와 비교하였을 때도 로봇의 현재 z축 위치를 더 정확하게 추정하는 것을 확인하였다. 이러한 라이다-관성-GNSS SLAM의 안정적인 상태 추정은 자율주행을 위한 효과적인 인지 시스템임을 주장하였고, 실제 비정형 환경에서 총 3가지 경로에 대한 자율주행 실험을 진행하여 자율주행에의 원활한 적용이 가능함을 보였다. 향후 연구로는 GNSS 내의 carrier phase 정보를 같이 활용하여 더 정확한 상태 추정 시스템을 계획 중이다.


Acknowledgments

This project was funded and supported by the grant from Hanwha Aerospace as part of the development of 3D SLAM technology for unstructured environment.


References
1. C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation,” IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 2020.
2. W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast Direct LiDAR-Inertial Odometry,” IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2053-2073, Aug., 2022.
3. T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping,” IEEE International Workshop on Intelligent Robots and Systems (IROS), Las Vegas, USA, 2020.
4. H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3D lidar inertial odometry and mapping,” IEEE International Conference on Robotics and Automation (ICRA), Montreal, Canada, 2019.
5. C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1-21, Feb., 2017.
6. T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” IEEE International Workshop on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018.
7. T. Qin, S. Cao, J. Pan, and S. Shen, “A general optimization-based framework for global pose estimation with multiple sensors,” Computer Vision and Pattern Recognition, 2019.
8. X. Li, X. Wang, J. Liao, X. Li, S. Li, and H. Lyu, “Semi-tightly coupled integration of multi-GNSS PPP and S-VINS for precise positioning in GNSS-challenged environments,” Satellite Navigation, vol. 2, no. 1, Jan., 2021.
9. S. Cao, X. Lu, and S. Shen, “Gvins: Tightly coupled gnss–visual–inertial fusion for smooth and consistent state estimation,” IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2004-2021, Aug., 2022.
10. X. Niu, H. Tang, T. Zhang, J. Fan, and J. Liu, “IC-GVINS: A Robust, Real-time, INS-Centric GNSS-Visual-Inertial Navigation System,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 216-223, Jan., 2023.
11. Z. Gong, P. Liu, F. Wen, R. Ying, X. Ji, and R. Miao, W. Xue, “Graph-Based Adaptive Fusion of GNSS and VIO Under Intermittent GNSS-Degraded Environment,” IEEE Transactions on Instrumentation and Measurement, vol. 70, 2021.
12. J. Beuchert, M. Camurri, and M. Fallon, “Factor Graph Fusion of Raw GNSS Sensing with IMU and Lidar for Precise Robot Localization without a Base Station,” Robotics, 2022.
13. X. Ye, P. Ma, W. Liu, and F. Wang, “How NLOS Signals affect GNSS relative positioning,” Journal of Physics: Conference Series, vol. 1693, 2020.
14. L.-T. Hsu, “Analysis and modeling GPS NLOS effect in highly urbanized area,” GPS solutions, vol. 22, no. 7, 2018.
15. C. Jiang, B. Xu, and L.-T. Hsu, “Probabilistic approach to detect and correct GNSS NLOS signals using an augmented state vector in the extended Kalman filter,” GPS Solutions, vol. 25, no. 72, 2021.
16. N. Sünderhauf and P. Protzel, “Towards robust graphical models for GNSS-based localization in urban environments,” International Multi-Conference on Systems, Signals and Devices, Chemnitz, Germany 2012.
17. W. Wen, X. Bai, Y. C. Kan, and L.-T. Hsu, “Tightly coupled GNSS/INS integration via factor graph and aided by fish-eye camera,” IEEE Transactions on Vehicular Technology, vol. 68, no. 11, pp. 10651-10662, Nov., 2019.
18. X. Bai, W. Wen, G. Zhang, and L.-T. Hsu, “Real-time GNSS NLOS detection and correction aided by sky-pointing camera and 3D LiDAR,” ION 2019 Pacific PNT Meeting, pp. 862-874, Honolulu, Hawaii, 2019.
19. T. Suzuki, M. Kitamura, Y. Amano, and T. Hashizume, “High-accuracy GPS and GLONASS positioning by multipath mitigation using omnidirectional infrared camera,” IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 2011.
20. W. W. Wen, G. Zhang, and L.-T. Hsu, “GNSS NLOS exclusion based on dynamic object detection using LiDAR point cloud,” IEEE transactions on intelligent transportation systems, vol. 22, no. 2, pp. 853-862, Feb., 2019.
21. Z. Chen, A. Xu, X. Sui, C. Wang, S. Wang, J. Gao, and Z. Shi, “Improved-UWB/LiDAR-SLAM Tightly Coupled Positioning System with NLOS Identification Using a LiDAR Point Cloud in GNSS-Denied Environments,” Remote Sensing, vol. 14, no. 6, Mar., 2022.
22. Y. Gu, Y. Wada, L.-T, Hsu, and S. Kamijo, “SLAM with 3dimensional-GNSS,” IEEE Symposium on Position Location and Navigation (PLANS), Savannah, USA, 2016.
23. W. W. Wen and L.-T. Hsu, “3D LiDAR aided GNSS NLOS mitigation in urban canyons,” IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 10, pp. 18224-18236, Oct., 2022.
24. P. K. Enge, “The global positioning system: Signals, measurements, and performance,” International Journal of Wireless Information Networks, vol. 1, pp. 83-105, 1994.
25. J. Breßler, P. Reisdorf, M. Obst, and Gerd Wanielik, “GNSS positioning in non-line-of-sight context—A survey,” International Conference on Intelligent Transportation, Rio de Janeiro, Brazil, 2016.
26. C. Li, X. Luo, S. Du, and L. Xiao, “A Method for Registration of 2-D Shapes,” Congress on Image and Signal Processing, CIS, Chongqing, China, 2012.
27. L.-T. Hsu, N. Kubo, W. Wen, W. Chen, Z. Liu, T. Suzuki, and J. Meguro, “UrbanNav: An Open-Sourced Multisensory Dataset for Benchmarking Positioning Algorithms Designed for Urban Areas,” The 34th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2021), pp. 226-256, 2021.
28. MichaelGrupp/evo, [Online], https://github.com/MichaelGrupp/evo, Accessed: Jan. 06, 2023.
29. R. C. Conlter, “Implementation of the pure pursuit path tracking algorithm,” The Robotics Institute, Carnegie Mellon Univ., Pittsburgh, Pennsylvania, USA, 1992, [Online], https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf.

길 현 재

2021 한국과학기술원 기계공학과(공학사)

2022~ 현재 서울대학교 공과대학 기계공학부 석사과정

관심분야: SLAM, Place Recognition

이 동 재

2017~현재 서울대학교 공과대학 기계공학부 학사과정

관심분야: Radar SLAM, Sensor Fusion

송 관 형

2020~현재 서울대학교 공과대학 기계공학부 학사과정

관심분야: Robotics

안 승 욱

2010 경북대학교 전자전기컴퓨터학부(공학사)

2012 한국과학기술원 로봇공학학제전공(공학석사)

2012~2018 LG전자 연구원

2018~현재 한화에어로스페이스 연구원

관심분야: Perception, SLAM

김 아 영

2005 서울대학교 기계항공공학과(공학사)

2007 서울대학교 기계항공공학전공(공학석사)

2012 미시간대학교 기계공학전공(공학박사)

2014~2021 한국과학기술원 건설및환경공학과 부교수

2021~현재 서울대학교 공과대학 기계공학부 부교수

관심분야: 영상기반 SLAM