비정형 환경 내 지도 작성과 자율주행을 위한 GNSS-라이다-관성 상태 추정 시스템

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. 선행 연구 조사

3. 연구 방법

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 변환관계를 추정하며, 추정된 T_i+1 ∈ SE(3)와 이전 keyframe T_i 사이의 상대적인 변환 관계를 LiDAR factor로 정의한다.

$\[ \Delta T_{i,i+1}=T_i^T{T}_{i+1} \]$

(2)

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

3.2.2 IMU Factor

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

$\[ \begin{array}{c}\Delta v_{i,i+1}=R_i^T\left(v_{i+1}^w-v_i^w-g\Delta t_{i,i+1}\right)\hfill \\ \Delta p_{i,i+1}=R_i^T\left(p_{i+1}^w-p_i^w-v_i^w\Delta t_{i,i+1}-\frac{1}{2}g\Delta t_{i,i+1}^2\right)\hfill \\ \Delta R_{i,i+1}=R_i^T{R}_{\left(i+1\right)}\hfill \end{array} \]$

(3)

3.2.3 GNSS Code Pseudorange Factor

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

$\[ {\tilde{P}}_r^k=‖p_k^e-p_r^e‖+b_i+T_r^k+I_r^k+M_r^k+{\text{ϵ}}_{psr}^k \]$

(4)

$$ p_k^e$$, $$ p_r^e$$은 각각 ECEF 좌표계에서의 위성과 수신기의 위치이고, b_i (i = {U, R, E, C})는 수신기의 시계 bias, $$ T_r^k$$, $$ I_r^k$$는 각각 전리층과 대류권 내 시간 지연 효과, $$ M_r^k$$은 다중 경로(multipath) 효과, 그리고 $$ {\text{ϵ}}_{psr}^k$$는 측정 오차이다.

3.2.4 GNSS Doppler Factor

수신기가 관측한 각 위성의 PRN 코드의 주파수 변화 $$ \Delta {\tilde{f}}_r^k$$를 관측했을 때, 위성의 속도를 알고 있다면 이를 이용해서 로봇의 속도를 추정할 수 있다.

$\[ \lambda \Delta {\tilde{f}}_r^k=-1_r^k\cdot \left(v_k^e-v_r^e\right)+\dot{b}+{\text{ϵ}}_{dopp}^k \]$

(5)

λ는 신호의 파장, $$ 1_r^k$$는 수신기로부터 위성까지의 방향 벡터, $$ v_k^e$$, $$ v_r^e$$은 각각 위성과 수신기의 속도, $$ \dot{b}$$는 수신기 시계의 bias 변화율, 그리고 $$ {\text{ϵ}}_{dopp}^k$$은 측정 오차이다. 위성 시계의 drift rate, 전리층 및 대류권에서의 신호 지연 변화율, 그리고 다중 경로 효과의 시간 변화율은 주된 오차 원인이 아니므로 무시할 수 있다^[24].

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

$\[ {\sigma }_{r,psr}^s=\frac{\sqrt{n_s\cdot n_{pr}}}{sin{\theta }_{el}}, {\sigma }_{r,dopp}^s=\frac{\sqrt{n_s\cdot n_{pr}}}{sin{\theta }_{el}} \]$

(6)

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

3.2.5 GNSS clock bias factor

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

$\[ \Delta b_{i,i+1}=b_{i+1,k}-b_{i,k}-{\int }_{t_i}^{t_{i+1}}\dot{b }dt, k=\left\{U,R,E,C\right\} \]$

(7)

3.2.6 GNSS clock bias rate factor

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

$\[ \Delta \dot{b}{}_{i,i+1}=\dot{b}{}_{i+1}-\dot{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 $$ M_{sub}^L$$이라고 정의한다. 이후 현재까지 추정된 상태를 이용해, 현 위치에서 GNSS 수신기가 관측하고 있는 위성 s_k의 라이다 좌표계 기준 고각 $$ {\tilde{\theta }}_{s_k}^L$$과 방위각 $$ {\widetilde{\text{ϕ}}}_{s_k}^L$$을 계산한다. $$ M_{sub}^L$$로부터 각 위성의 방위각 주변 점군만을 subsampling한 점군 $$ M_{sub,{\tilde{\varphi }}_{s_k}^L}^L$$ 내의 모든 3d point $$ p_i^L$$들에 대해서, 수신기로부터 위성 s_k까지의 방향 벡터 $$ l_k^L$$와의 line-to-point 거리 d_k,i를 구한다.

$\[ d_{k,i}=\frac{\left|l_k^L\times p_i^L\right|}{\left|l_k^L\right|} \]$

(9)

d_k,i < d_thres인 점군 P_nlos = {p_i}(i = 1, ⋯, N)가 N_thres보다 많다면, 위성은 주변 장애물로 인해 LOS가 확보되지 못하는 것으로 판단한다. 그리고 P_nlos의 질량 중심 $$ p_{com}=\frac{{\sum }_{i=1}^N{p}_i}{N}$$이 $$ l_k^L$$로부터 얼마나 벗어나 있는지를 의미하는 $$ d_{com}=\frac{\left|l_k^L\times p_{com}\right|}{\left|l_k^L\right|}$$로 pseudorange factor의 uncertainty를 조절한다.

$\[ {\sigma }_{r,psr-nlos}^{s_k}=e^{\alpha \left(1-\frac{d_{com}}{d_{thres}}\right)}{\sigma }_{r,psr}^{s_k},\left(\alpha >0\right) \]$

(10)

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

한 가지 더 고려할 사항은 라이다의 작은 수직 화각으로 인해 항상 주변 장애물의 정확한 높이를 파악할 수 없다는 점이다. 이는 나무와 같이 약 3 m 정도의 장애물보다는 인공 구조물에 대해 문제가 되는데, 주행 중 큰 pitch가 발생하지 않을 때 Velodyne VLP-16 라이다를 기준으로 약 27 m이상의 구조물은 정확한 높이를 파악할 수 없다. 이러한 경우를 대비해 높은 고각의 위성은 d_thres를 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]를 통해 구한 변환관계 $$ T_{end}^{start}$$로부터 시작점과 도착점의 차이를 계산했다. 재방문이 없는 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)

하지만 [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

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

4.4 자율주행 실험

마지막으로, 본 논문에서 제안한 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]에 작성하였다.

[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

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

• 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. [https://doi.org/10.1109/ICRA40945.2020.9197567]
• 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. [https://doi.org/10.1109/TRO.2022.3141876]
• 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. [https://doi.org/10.1109/IROS45743.2020.9341176]
• 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. [https://doi.org/10.1109/ICRA.2019.8793511]
• 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. [https://doi.org/10.1109/TRO.2016.2597321]
• 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. [https://doi.org/10.1109/IROS.2018.8594299]
• 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. [https://doi.org/10.48550/arXiv.1901.03642]
• 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. [https://doi.org/10.1186/s43020-020-00033-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. [https://doi.org/10.1109/TRO.2021.3133730]
• 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. [https://doi.org/10.1109/LRA.2022.3224367]
• 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. [https://doi.org/10.1109/TIM.2020.3039640]
• 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. [https://doi.org/10.48550/arXiv.2209.14649]
• X. Ye, P. Ma, W. Liu, and F. Wang, “How NLOS Signals affect GNSS relative positioning,” Journal of Physics: Conference Series, vol. 1693, 2020. [https://doi.org/10.1088/1742-6596/1693/1/012184]
• L.-T. Hsu, “Analysis and modeling GPS NLOS effect in highly urbanized area,” GPS solutions, vol. 22, no. 7, 2018. [https://doi.org/10.1007/s10291-017-0667-9]
• 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. [https://doi.org/10.1007/s10291-021-01101-6]
• 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. [https://doi.org/10.1109/SSD.2012.6198059]
• 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. [https://doi.org/10.1109/TVT.2019.2944680]
• 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. [https://doi.org/10.33012/2019.16845]
• 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. [https://doi.org/10.1109/ICRA.2011.5980424]
• 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. [https://doi.org/10.1109/TITS.2019.2961128]
• 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. [https://doi.org/10.3390/rs14061380]
• 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. [https://doi.org/10.1109/PLANS.2016.7479701]
• 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. [https://doi.org/10.1109/TITS.2022.3167710]
• P. K. Enge, “The global positioning system: Signals, measurements, and performance,” International Journal of Wireless Information Networks, vol. 1, pp. 83-105, 1994. [https://doi.org/10.1007/BF02106512]
• 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. [https://doi.org/10.1109/ITSC.2016.7795701]
• 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. [https://doi.org/10.1109/CISP.2012.6469977]
• 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. [https://doi.org/10.33012/2021.17895]
• MichaelGrupp/evo, [Online], https://github.com/MichaelGrupp/evo, , Accessed: Jan. 06, 2023.
• 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, .