주변 환경의 거리 정보를 정밀하게 파악할 수 있는 라이다 센서와, 측정 주파수가 높은 관성 측정 장치의 상호 보완적인 관계를 이용한 라이다-관성 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를 통해 상태 추정 정확도를 높이고자 한다.

도시 협곡(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 신호 간에 서로 다른 가중치를 부여하여 의사 거리를 추정하는 방법을 사용하였다.

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

• - 지구 중심을 기준으로 하며, 북극 방향을 z축으로하고 적도 평면과 본초 자오선이 만나는 지점까지의 방향을 x축으로 하는 ECEF 좌표계 (⋅)^e
• - 지구 위 임의의 한 점을 기준으로, 동쪽을 x축, 북쪽을 y축, 그리고 중력 반대방향을 z축으로 하는 ENU 좌표계 (⋅)ⁿ
• - 라이다와 관성 측정 장치를 통해 초기화를 진행한 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를 다음과 같이 정의할 수 있다.

ptw∈R³, vtw∈ R³, Rtw∈SO3는 각각 시간 t일 때의 로봇의 위치, 속도, 그리고 방향을 의미하며, b_t∈R⁶는 관성 측정 장치의 가속도와 각속도의 bias를 의미한다. b_U, b_R, b_E, b_C[m]는 GPS, GLONASS, GALILEO, BEIDOU 위성 항법 시스템과 관련된 GNSS 수신기의 시계 bias δt_U, δt_R, δt_E, δt_C로 인한 bias 항이다. 수신기의 시계 bias, anchor point, 그리고 yaw 각도는 유효한 GNSS 관측값이 들어오기 시작하면 추정하기 시작하는 파라미터이다.

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

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]

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 수신기가 관측하고 있는 위성 s_k의 라이다 좌표계 기준 고각 θ~skL과 방위각 ϕ~skL을 계산한다. MsubL로부터 각 위성의 방위각 주변 점군만을 subsampling한 점군 Msub,ϕ~skLL 내의 모든 3d point piL들에 대해서, 수신기로부터 위성 s_k까지의 방향 벡터 lkL와의 line-to-point 거리 d_k,i를 구한다.

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

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

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

하지만 [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에서 실험적으로 검증하였다.

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 신호를 감지하고 대응하면, 더 정확한 상태를 추정할 수 있음을 실험적으로 확인하였다.

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

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