Performance Evaluation of a Compressed-State Constraint Kalman Filter for a Visual/Inertial/GNSS Navigation System

Abstract

Autonomous driving systems are likely to be operated in various complex environments. However, the well-known integrated Global Navigation Satellite System (GNSS)/Inertial Navigation System (INS), which is currently the major source for absolute position information, still has difficulties in accurate positioning in harsh signal environments such as urban canyons. To overcome these difficulties, integrated Visual/Inertial/GNSS (VIG) navigation systems have been extensively studied in various areas. Recently, a Compressed-State Constraint Kalman Filter (CSCKF)-based VIG navigation system (CSCKF-VIG) using a monocular camera, an Inertial Measurement Unit (IMU), and GNSS receivers has been studied with the aim of providing robust and accurate position information in urban areas. For this new filter-based navigation system, on the basis of time-propagation measurement fusion theory, unnecessary camera states are not required in the system state. This paper presents a performance evaluation of the CSCKF-VIG system compared to other conventional navigation systems. First, the CSCKF-VIG is introduced in detail compared to the well-known Multi-State Constraint Kalman Filter (MSCKF). The CSCKF-VIG system is then evaluated by a field experiment in different GNSS availability situations. The results show that accuracy is improved in the GNSS-degraded environment compared to that of the conventional systems.

1. INTRODUCTION

센서 융합은 자율 주행 자동차, Personal Air Vehicles (PAVs), Urban Air Mobilities (UAMs), electronic Vertical Take-Off and Landing (eVTOL) 항공기와 같이 정확한 위치 정보를 필요로 하는 차세대 자율 항법 시스템에 있어 중요한 기술이다. 다중 센서 융합에 있어 Inertial Navigation System (INS)은 핵심적인 항법시스템이다. INS는 Inertial Measurement Unit (IMU)로부터 고속으로 출력되는 가속도와 각속도 측정치를 활용하여 사용자의 위치, 속도, 자세를 제공한다 (Goshen-Meskin & Bar-Itzhack 1992, Britting 2010). INS는 외부의 영향인 신호 간섭, 전천후 영향 등에 강건하며 동적 특성이 좋다는 장점이 존재한다. 그러나 비관성 센서의 도움으로 관성 센서 오차를 적절히 보정하지 않으면 시간이 지남에 따라 위치 오차가 급격히 증가하는 단점을 지닌다.

시간이 지남에 따라 증가하는 INS의 오차를 보정하기 위하여 비관성 항법시스템인 Global Positioning System (GPS)를 결합한 INS/GPS 시스템이 활발히 연구되어 왔다 (Godha & Cannon 2005, Noureldin et al. 2008). INS/GPS 시스템은 시간에 따른 오차의 누적 없는 GPS의 절대 측정치를 활용함으로써 INS 오차를 효과적으로 보정하고 추정된 항법 정보가 지역 좌표계에 제한되지 않는 특징을 가진다. 미국에서 개발된 GPS 뿐만 아니라 중국의 BeiDou, 러시아의 GLONASS 그리고 유럽연합의 Galileo와 같이 다양한 위성항법 시스템이 존재하며 이들은 전역 위성항법 시스템 (Global Navigation Satellite System; GNSS)으로 불린다. 위성의 가용성을 극대화한 multi-GNSS와 INS를 결합한 시스템도 연구되었으며 단독 GPS에 비해 효과적인 성능을 나타냈다 (Gao et al. 2016). 반면 GNSS의 성능은 정상적인 GNSS 신호를 받기 어려운 환경적인 조건에 민감하다 (Kim et al. 2022). 예를 들어, 높은 빌딩이 많은 도심지나 터널 환경에서는 비정상적인 GNSS 신호가 수신되거나 신호가 수신되지 않을 수 있으며 GNSS를 활용하여 INS 오차를 효과적으로 보정할 수 없게 된다.

카메라는 가볍고 저가이며 상대적으로 전력 소모가 적으면서 제공되는 영상에는 많은 양의 정보 담고 있다는 장점을 지닌다. 이러한 장점으로 인해 여러 카메라 시점에서 획득한 영상을 활용하여 카메라의 상대적인 위치/자세를 연속적으로 추정하는 Visual Odometry (VO) 알고리즘이 개발되었다 (Nistér et al. 2006, Scaramuzza & Fraundorfer 2011, Forster et al. 2014). 반면 영상은 실제 3차원 정보에서 depth 정보가 손실된 2차원 정보이기 때문에 카메라의 움직임이나 주변 환경의 움직임을 구별하기가 어렵다. 또한 영상은 날씨와 조명 조건에 매우 민감한 단점을 가진다. 이를 극복하기 위하여 상호 보완적인 특성을 가진 카메라와 INS를 결합한 Visual-Inertial Odometry (VIO) 알고리즘이 활발히 연구되어 왔다 (Mourikis & Roumeliotis 2007, Leutenegger et al. 2015, Qin et al. 2018, Zhang et al. 2020). 연속되는 영상으로부터 획득된 상대 측정치와 빠른 동적 움직임에 탁월한 INS를 결합한 VIO는 기존 단독 INS나 VO에 비교하여 전반적으로 성능이 향상되었다. 반면 VIO도 VO와 마찬가지로 매 시점 계산된 위치 증분치를 적산하는 상대 측위 (relative positioning) 방식으로, 시간이 지남에 따라 위치 오차가 누적되는 문제점은 여전히 존재한다. 상대 측위의 한계를 극복하기 위하여 Visual Simultaneous Localization and Mapping (V-SLAM)이나 Visual-Inertial SLAM (VI-SLAM)은 VO나 VIO에 loop closing 기법을 추가하여 누적된 오차를 줄인다 (Cadena et al. 2016). 그러나 해당 기법을 적용하기 위해서는 알려진 위치를 항상 재방문 해야 한다 (Cadena et al. 2016).

최근에 Lee et al. (2022)는 안정적이며 시간에 따른 누적된 오차 없이 정확한 항법 정보를 제공하기 위하여 GNSS, INS 그리고 단안 카메라를 결합한 새로운 강결합 기반의 복합 항법 시스템을 제안하였다. 새로운 강결합 기반의 Visual/Inertial/GNSS (VIG) 시스템은 기존 시스템과 달리 카메라 상태변수를 필요로 하지 않으며 전체 시스템 상태변수는 항법에 필수적인 최소한의 상태변수로 구성된다. Lee et al. (2022)에서 이러한 특성을 고려하여 제안된 필터를 Compressed-State Constraint Kalman Filter (CSCKF)로 명명하였다. 또한 해당 시스템에 사용된 세 종류의 센서를 포함한 이름으로 CSCKF-VIG로 줄여서 부른다. 본 논문에서는 새로운 필터 기반의 CSCKF-VIG 시스템을 상세히 소개하고 수집된 실제 차량에 탑재된 센서 데이터를 기반으로 기존 항법 시스템과의 성능을 비교 분석한다.

본 논문은 다음과 같이 구성되었다. 2장에서는 기존 센서 융합 관련 연구를 소개하고, 3장에서는 CSKCF-VIG에 대해서 소개한다. 4장에서는 실제 실험을 통해 기존 시스템과 CSCKF-VIG의 성능을 평가한다. 마지막으로 5장은 결론이다.

2. RELATED WORKS

다중 센서 융합과 관련하여 로봇공학, 항법 그리고 컴퓨터 비전에 이르기까지 광범위한 연구 문헌이 존재한다. 특히, 카메라와 IMU 그리고 GNSS 수신기 기반의 센서 융합은 널리 연구되고 있다.

먼저 카메라와 IMU를 결합한 항법 알고리즘인 VIO (Mourikis & Roumeliotis 2007, Leutenegger et al. 2015, Qin et al. 2018, Zhang et al. 2020)는 필터링 방식 (recursive estimator)과 최적화 방식 (batch estimator)으로 크게 구분된다. 필터링 방식 중에 가장 잘 알려진 VIO는 Multi-State Constraint Kalman Filter (MSCKF) (Mourikis & Roumeliotis 2007)이다. MSCKF는 동일한 3차원 특징점 (feature point)에 대한 여러 카메라 시점에서 관찰한 결과를 삼각 측량 (triangulation)하여 기하학적 구속 조건으로 활용할 수 있다는 특징을 지닌 필터이다. 카메라와 IMU가 부착된 항체가 운동함에 따라 다양한 위치에서 동일한 3차원 정보인 특징점을 카메라로 관찰할 수 있게 된다. 각각의 카메라 위치에서 동일하게 관찰된 특징점의 정보를 슬라이딩 윈도우에 기록하며 일정 조건이 만족되는 경우 필터에 측정치를 업데이트한다. 이때 특징점에 대하여 서로 다른 카메라 위치에서 삼각 측량 수행하므로 기하학적 구속 조건을 활용할 수 있게 된다. MSCKF는 수학적으로 Extended Kalman Filter (EKF)-SLAM과 동일하지만 MSCKF는 슬라이딩 윈도우의 시간 폭에 비례하여 SLAM 상태변수 (특징점 상태변수)를 제한하는 특징을 지닌다. 이와는 다르게 최적화 방식은 batch 형식으로 수행되므로 필터링 방식에 비해 높은 연산량 요구하지만 더 나은 성능을 제공한다. VINS-mono (Qin et al. 2018)는 대표적인 최적화 방식의 VIO 알고리즘으로 많이 활용되고 있다. 앞서 소개된 VIO 알고리즘은 비록 영상 측정치와 INS의 통합하여 VO 비해 성능이 향상되었을 지라도 결국 상대 측위이기 때문에 시간이 지남에 따라 누적되는 오차는 여전히 VIO의 한계로 인식된다.

시간에 따른 오차 누적의 문제를 해결하기 위해서는 GNSS 수신기 혹은 다른 외부 센서로부터 제공되는 절대 측정치를 사용해야 한다. Sun et al. (2020)는 단일 수신기에서 획득되는 GNSS 항법해 (위치, 속도)를 영상 및 관성 측정치와 약결합한 방식으로, 두개의 EKF를 활용하여 VO, GNSS 그리고 INS를 결합한 방식을 제안하고, Cioffi & Scaramuzza (2020)과 Gong et al. (2020)은 단일 수신기에서 획득되는 GNSS 항법해를 VIO와 약결합한 방식을 제안하였다. 앞서 언급된 약결합 기반 VIG 알고리즘은 기존 VO 혹은 VIO와 GNSS의 시스템 레벨의 결합이므로 이미 구성되어 있는 각 시스템의 알고리즘을 그대로 적용할 수 있기 때문에 시스템 구성이 용이하다는 장점을 가진다. 반면 GNSS 가시 위성수가 4개 미만인 환경에서는 GNSS 수신기로부터 항법해를 제공받을 수 없다는 문제가 발생한다. 또한 GNSS 가시 위성수가 충분한 환경에서도 non-line-of-sight (NLOS) 혹은 multipath 오차와 같은 환경적 요인에 의해 항법해의 정확도가 크게 저하될 수 있다. 따라서, 해당 VIG 시스템은 기존 VIO 시스템으로 전락될 수 있으며 만약 GNSS 수신 환경 조건이 빠르게 개선되지 않는다면 시간에 따른 오차 누적 문제는 해결되기 어려워질 수 있다.

하나의 대안으로 GNSS 항법해가 아닌 GNSS 원시 측정치를 직접 활용하여 영상 및 관성 측정치와 결합한다면 앞서 언급된 문제를 극복할 수 있다. Vu et al. (2012)는 기존 VIO 시스템에 단일 수신기에서 획득되는 GNSS 의사거리 (pseudorange) 측정치와 랜드마크를 강결합한 방식을 제안하였다. Liu et al. (2021)는 필터링 방식이 아닌 최적화 방식을 활용하여 단일 수신기에서 획득되는 GNSS의 의사거리 및 도플러 (doppler) 측정치를 기존 VIO 시스템과 강결합한 방식을 제안하였다. 앞서 언급된 강결합 방식의 VIG 시스템은 강결합 방식의 이점으로 인해 GNSS 가시 위성수가 4개 미만인 경우에서도 큰 오차 증가 없이 위치 정확도를 유지한다. 그러나 단일 GNSS 수신기 기반의 VIG 시스템은 GNSS 신호 전파에서 발생되는 다양한 오차 요인으로 인해 meter-level 정밀도의 한계를 가진다 (Misra & Enge 2001). 이는 짧은 시간 동안에 centimeter-level 정밀도를 갖는 VIO 시스템과 달리 단일 수신기로부터 획득되는 GNSS 의사거리 측정치의 정밀도가 상대적으로 낮아 발생되는 불일치로 인해 VIG 시스템의 정밀도를 향상시키기 어렵기 때문이다.

GNSS를 기반으로 decimeter 혹은 centimeter 정확도를 얻기 위해서는 기준국으로부터 추가적인 GNSS 측정치 활용하거나 반송파 (carrier) 측정치를 활용한 방법을 고려해야 한다. 기준국으로부터 획득된 GNSS 측정치를 추가적으로 활용한다면, 단일 차분 (Single-Difference; SD) 기법을 통해 공통 오차 요소인 위성과 관련된 오차, 전리층 오차 그리고 대류층 오차를 제거할 수 있다. 이중 차분 (Double-Difference; DD) 기법을 활용한다면 SD 방정식에 포함된 차분된 시계오차도 제거 가능하다. Liao et al. (2021)는 차분된 다중 GNSS 측정치와 스테레오 영상 측정치를 활용하여 INS 오차를 보정하는 방식을 제안하였으며 이는 기존 스테레오 VIO와 차분된 GNSS를 결합한 약결합 방식이다. Li et al. (2019)는 차분된 다중 GNSS 원시 측정치와 단일 카메라의 영상 측정치를 활용하여 INS 오차를 보정하는 강결합 방식을 제안하였으며 DD 코드와 반송파 측정치 기반의 Real-Time Kinematic (RTK) 기법도 적용하였다.

앞서 소개된 연구에서 볼 수 있듯이 차분된 GNSS 측정치를 활용하게 된다면 VIO 시스템과의 결합에 있어 측위 성능을 효과적으로 향상시킬 수 있다. 특히, 반송파 측정치를 활용하면 centimeter-level 정밀도를 얻을 수 있다. 반면 이러한 정밀한 반송파 측정치를 효과적으로 사용하기 위해서는 해당 측정치에 포함된 미지정수를 반드시 알아야 한다. 상용 GNSS 시스템의 경우 미지정수를 추정하기 위해 몇 초에서 몇 분의 시간이 소요되며 추정해야 하는 미지정수의 수는 사용되는 위성 채널 수만큼 증가한다 (Rizos 2001). 도심지 환경이나 터널과 같은 환경에서는 GNSS 신호 수신이 끊기는 현상이 빈번하게 발생되며 이는 cycle slip를 발생시킨다. 따라서, 위성 신호가 끊기게 될 경우 해당 채널에 대해서 미지정수를 다시 추정해야 한다. 이러한 이유로 Li et al. (2019)는 GNSS 수신이 열악한 환경에서는 DD 반송파 측정치 대신에 DD 의사거리 측정치를 활용하였다.

3. CSCKF-BASED VISUAL/INERTIAL/GNSS NAVIGATION SYSTEM

이번 장에서는 Lee et al. (2022)에서 제안된 CSCKF-VIG 시스템을 소개한다. CSCKF-VIG 시스템은 강결합 기반으로 새롭게 정의된 특징점 측정치와 차분된 다중 GNSS 측정치를 활용하여 INS 오차를 효율적으로 보정한 것이 특징이다. 또한 기존 VIG 시스템과 달리 CSCKF-VIG 시스템은 항법에 필요한 최소한의 상태변수만으로 시스템이 설계된 것이 특징이다. CSCKF-VIG 시스템은 데이터 압축 기법 (Bar-Itzhack 1980, Medan & Bar-Itzhack 1985, Lee & Lee 2007)에 영감을 받아 압축 필터 (Compression Filter; CF) 이론 (Lee & Lee 2007)을 바탕으로 새로운 개념의 시전달 특징점 측정치 (time-propagated feature measurement)를 제안하였다. 해당 측정치를 활용하면 기존 VIO 혹은 VIG 시스템에서 수행된 카메라 상태변수 추가 과정이 없이도 효율적으로 영상 측정치를 활용할 수 있다는 이점이 존재한다. 또한 전체 시스템 상태변수가 INS 상태변수와 동일하고 상태변수의 수가 일정하기 때문에 기존 시스템에 비하여 연산량 부담이 크게 줄어든다는 장점을 지닌다.

CSCKF-VIG 시스템에서 GNSS 반송파 측정치를 활용함에 있어 시스템 속도 보정 용도로 사용한다. DD 반송파 측정치에 시간 차분을 추가로 수행하여 삼중 차분된 (Triple-Differenced; TD) 반송파 측정치를 생성한다. 추가적인 차분으로 반송파 측정치에 포함된 미지정수 오차가 소거되기 때문에 시스템에서 별도의 미지정수 추정을 필요로 하지 않는다. GNSS 측정치 활용에 있어 cycle slip과 multipath는 반드시 검출되어야 한다. CSCKF-VIG 시스템에서는 비정상적인 GNSS 측정치를 검출 위하여 Fault Detection Exclusion (FDE) 알고리즘이 적용되었다 (Kim et al. 2019). 이러한 방식으로 CSCKF-VIG 시스템은 열악한 GNSS 수신 환경에서도 안정적이고 정확한 항법 정보를 제공한다.

3.1 Overview

CSCKF-VIG 시스템의 전체적인 구조는 Fig. 1에 나타내었다. 시스템 초기화 이후에 INS는 위치, 속도, 자세 추정치를 빠른 주기로 제공한다. GNSS 수신기나 카메라로부터 획득되는 측정치의 사용 가능여부에 따라 INS 오차를 보정하기 위하여 측정치 갱신이 수행된다.

Fig. 1. The overall structure of the CSCKF-VIG system.

새로운 영상이 획득될 때마다 FAST 알고리즘 (Rosten & Drummond 2005)을 활용하여 2차원 특징점을 추출한다. 추출된 2차원 특징점은 KLT 알고리즘 (Lucas & Kanade 1981)을 통해 여러 영상에서 추적되며 해당 시점의 INS 정보와 함께 슬라이딩 윈도우에 저장된다. 만약 슬라이딩 윈도우가 일정 크기에 도달하면, 추적된 2차원 특징점은 INS 정보를 활용하여 CF에 의해서 시전달 특징점 측정치를 생성한다. 카메라 측정치 갱신은 해당 특징점 측정치를 기반으로 수행된다. Fig. 2는 MSCKF와 CSCKF의 차이점에 대한 그림이다. 그림에서 볼 수 있듯이 CSCKF는 MSCKF와 달리 매 카메라 시점마다 카메라 상태변수를 추가할 필요가 없다.

Fig. 2. Difference between the MSCKF and the CSCKF in a graph representation.

GNSS 측정치 갱신은 다음과 같이 수행된다. 먼저 사용자와 기준국으로부터 GNSS 원시 데이터를 획득한다. 다음으로 FDE 알고리즘을 수행하여 이상 GNSS 측정치를 검출한 뒤 제거한다. 정상 GNSS 측정치를 기반으로 차분 기법을 활용하여 DD 의사거리 및 TD 반송파 측정치를 생성한다. 차분된 GNSS 측정치를 기반으로 GNSS 측정치 갱신이 수행한다.

고속으로 출력되는 Strapdown INS 항법해는 융합 필터에 의해 획득된 시스템 오차 최적 추정치를 시스템에 되먹임하여 보정한다. 또한 필터 업데이트에 의해 추정된 IMU 바이어스는 향후 IMU 측정치를 보정한다.

3.2 Coordinate Frames, Notation, and Definitions

\(V\)-좌표계 (Vision frame)는 비전 좌표계를 가리키며 일반적으로 이미지 평면 (image plane)으로 알려져 있다. \(C\)-좌표계 (Camera frame)는 카메라 좌표계를 가리킨다. 카메라의 원점은 카메라의 센서 중심을 나타내고 \(XYZ\) 축 방향은 전방-우측-아래를 나타낸다. \(B\)-좌표계 (Body frame)는 동체 좌표계를 가리킨다. 동체 좌표계의 원점은 IMU의 중심과 같으며 \(XYZ\) 축 방향은 카메라 \(XYZ\) 축 방향과 동일하다. \(N\)-좌표계 (Navigation frame)는 항법 좌표계이며, 동체좌표계와 원점이 동일하며, \(N\) 축이 북쪽, \(E\) 축이 동쪽, \(D\) 축이 수직 아래로 정의되는 local-level 좌표계이다. \(E\)-좌표계 (Earth-Centered Earth-Fixed; ECEF)는 지구중심지구고정 좌표계로, 지구의 자전에 따라 함께 회전하는 좌표계이다. \(I\)-좌표계 (Inertial frame)는 관성 좌표계로서 뉴턴의 운동 법칙이 적용되는 항법에서 기준이 되는 좌표계이다. 지구의 움직임에 관계없이 공간상에 고정되어 있는 가상의 좌표계이다.

CSCKF-VIG 시스템에서는 기존 MSCKF 알고리즘에서 사용한 JPL 방식 (Breckenridge 1999)과는 다르게 가장 일반적인 Hamilton 방식 (Lam 2003)의 쿼터니언 (quaternion)을 활용하여 동체의 회전 정보를 표현하였다. 일반적으로 쿼터니언은 정규화의 수행이 쉽고 계산량이 적으며 회전벡터의 크기를 나타내는 하나의 스칼라 \((q_w)\)와 회전 방향을 나타내는 하나의 벡터\((\textbf{q}_v=[q_x i~~q_y j~~q_z k]^T)\)인 \(\textbf{q}[q_w ~~ q_v ]^T\)로 표현된다.

본 논문에서는 굵은 소문자는 벡터를 나타내며 일반 소문자는 벡터의 성분을 나타낸다. 굵은 소문자의 위 첨자와 아래 첨자는 각각 좌표계와 센서 종류를 나타낸다. 예를 들어, \(\textbf{p}_C^N\)는 항법 좌표계로 표현된 카메라의 위치 벡터를 의미하며 해당 벡터의 성분은 \([x_c~~y_c~~z_c]^\rm{T}\)로 표현된다. 굵은 대문자는 행렬을 나타낸다. 예를 들어, \(\textbf{C}_C^N\)는 카메라 좌표계에서 항법 좌표계로의 좌표변환행렬을 가리킨다.

오차 모델은 일반적인 덧셈 오차 (standard additive error) 정의를 따르며 위치, 속도, IMU 바이어스에 적용된다. 하지만 쿼터니언 오차의 정의는 곱으로 표현된다. 각 오차 정의는 Eqs. (1, 2)와 같다.

\(\boldsymbol{\delta} \textbf{x}=\hat{\textbf{x}}-\textbf{x}\)                                                                     (1)

\(\hat{\textbf{q}}_B^N=\textbf{q}_N^N \otimes \textbf{q}_B^N,~~~~ \textbf{q}_N^{\hat{N}} [1~~-0.5(\boldsymbol{\phi})^T]^T\)                                               (2)

여기서 \(\boldsymbol{\delta} \textbf{x},\hat{\textbf{x}}\) 그리고 \(\textbf{x}\)는 각각 상태변수 오차, 상태변수 추정치, 상태변수 참값을 각각 나타낸다. Eq. (2) 에서 \(\hat{\textbf{q}}_B^N\)는 쿼터니언 추정치 (Yu et al. 1997), \(\boldsymbol{\phi}\)는 지역 수평 좌표계로 표현된 실제 항법 좌표계와 추정된 항법 좌표계 사이의 자세 오차 (= tilt angle)를 나타낸다. \(\otimes\) 기호는 쿼터니언 곱을 가리킨다.

3.3 System Model

CSCKF-VIG 시스템은 시스템 오차의 추정과 보정을 위한 칼만필터를 구성하기 위하여 일반적인 \(N\)-좌표계 기반의 Strapdown INS (SDINS) 오차 모델로 항법 오차 모델을 구성하고 관성 센서 오차 모델로는 랜덤 상수 (random constant)로 모델링 된다. Eqs. (1)과 (2)의 오차 정의에 의하여 INS 오차 상태변수 \(\boldsymbol{\delta}\textbf{x}_{INS} \in \textbf{R}^{15\times 1}\)는 Eq. (3)과 같다.

\(\boldsymbol{\delta}\textbf{x}_{INS}=[\begin{matrix} (\boldsymbol{\delta}\textbf{llh})^T & (\boldsymbol{\delta}\textbf{v}^N )^T & (\boldsymbol{\phi}^N )^T & (\boldsymbol{\delta}\textbf{b}_a )^T & (\boldsymbol{\delta}\textbf{b}_g )^T )\end{matrix}]^T\)                               (3)

여기서 \(\textbf{llh}\)는 위도 (\(L\)), 경도 (\(l\)), 고도 (\(h\))를 포함하는 경위도 좌표값을, \(\textbf{v}^N\)는 항법 좌표계로 표현된 항체의 속도를, \(\boldsymbol{\phi}^N\)는 자세 오차를 각각 나타낸다. \(\textbf{b}_a\)와 \(\textbf{b}_g\)는 각각 가속도계와 자이로스코프의 바이어스를 나타낸다.

잘 알려진 필터 기반 VIO 알고리즘인 MSCKF는 매 카메라 시점마다 INS 항법 정보를 활용하여 카메라 상태변수 추정치 (카메라 위치 및 자세 추정치)를 계산한다. 그리고 해당 상태변수는 전체 시스템 상태변수에 추가된다. 즉, 특징점이 관측된 카메라 시점 수에 따라서 시스템 상태변수의 크기와 오차 공분산 행렬의 크기가 가변적이다. 반면 CSCKF-VIG 시스템은 새로운 시전달 특징점 측정치를 활용함에 따라 전체 시스템 상태변수의 수가 일정하며 추가적인 카메라 상태변수를 필요로 하지 않는다. CSCKF-VIG 시스템의 상태변수는 Eq. (4)와 같으며 INS 오차 상태변수와 동일하다.

\(\boldsymbol{\delta}\textbf{x} \doteq \boldsymbol{\delta}\textbf{x}_{INS}\)                                                             (4)

IMU 가속도계 \((\hat{\textbf{f}}^B)\)와 자이로스코프 \((\tilde{\boldsymbol{\omega}}_{IB}^B)\) 측정치는 Eq. (5)로 모델링 된다.

\(\hat{\textbf{f}}^B=\textbf{f}^B+\textbf{b}_a+\textbf{w}_a\)                                                            
\(\tilde{\boldsymbol{\omega}}_{IB}^B = \boldsymbol{\omega}_{IB}^B + \textbf{b}_g+\textbf{w}_g\)                                                      (5)

여기서 \(\textbf{w}_a\sim N(\textbf{0},\textbf{W}_a), \textbf{W}_a=\sigma_a^2 \textbf{I}_{3\times 3}, \textbf{w}_g~N(\textbf{0},\textbf{W}_g), \textbf{W}_g=σ_g^2 \textbf{I}_{3\times3}\). \(\sigma_a\)와 \(\sigma_b\)는 각각 가속도계와 자이로 측정치 잡음의 표준편차를 나타낸다.

항법 좌표계 기반의 SDINS 오차 모델 (Goshen-Meskin & Bar-Itzhack 1992, Yu et al. 1997, Britting 2010)은 비선형인 SDINS 항법방정식을 섭동방법 (perturbation method)을 이용하여 유도하며, 샘플링 주기가 충분히 작다는 가정 하에서 이산화되고 선형화된 SDINS 오차방정식은 Eq. (6)과 같다.

\(\boldsymbol{\delta}\textbf{x}(k+1)=\boldsymbol{\Phi}(k+1,k)\boldsymbol{\delta}\textbf{x}(k)+\textbf{w}(k) \)                                               (6)

Eq. (6)에 대한 상세한 정보는 Lee et al. (2022)의 Appendix에서 찾아볼 수 있다. Eq. (6)의 상태변수 시간 전달에 대한 시스템 오차 공분산 행렬 \((\textbf{P})\)의 시간 전달은 Eq. (7)과 같이 표현 된다.

\(\textbf{P}(k+1)^-=\boldsymbol{\Phi}(k+1,k)\textbf{P}(k)^+ \boldsymbol{\Phi}(k+1,k)^T+\textbf{Q}(k)\)                                       (7)

여기서 \(\boldsymbol{\Phi}(k+1,k)\)는 상태천이행렬을 가리킨다. \(\textbf{Q}(k)\triangleq \overset{\cap}{\textbf{G}}(k)\textbf{W}(k)\overset{\cap}{\textbf{G}}^T (k)\)이며, \(\textbf{W}(k) \triangleq \left[ \begin{matrix} \textbf{W}_a & O_{3\times 3} \\ O_{3\times 3} & \textbf{W}_g \end{matrix} \right ]\)이고 \(\overset{\cap}{\textbf{G}}(k)=\textbf{G}\Delta t\)이다.

3.4 Time-Propagated Feature Measurement Update – Camera

이번 절에서는 CSCKF-VIG 시스템을 상세히 소개한다. 먼저 일반적인 영상 측정치 중 하나인 특징점에 대해서 설명한다. 다음으로 기존 항법 시스템에서의 카메라 측정치 갱신 방법과 측정치 융합 개념에 대해 간단히 소개한다. 마지막으로 시전달 특징점 측정치를 소개하며 해당 측정치 기반의 카메라 측정치 갱신 방법을 설명한다.

일반적으로 카메라가 부착된 항체가 움직임에 따라 다양한 위치에서 동일한 3차원 물체를 카메라로 관찰할 수 있다. 3차원 물체를 표현하는 다양한 방법이 존재하지만 일반적으로 점 (point)을 활용하여 해당 물체를 기술한다. FAST와 같은 특징점 추출 알고리즘을 활용하면 3차원 물체가 찍힌 영상으로부터 2차원 특징점을 추출할 수 있다. 혼동을 피하기 위하여 본 논문에서는 동일한 물체에 대한 특징점을 3차원 (3D)과 2차원 (2D)으로 구별하여 설명한다. 카메라가 움직이면서 동일한 하나의 3D 특징점을 관찰했을 때, 획득된 여러 카메라 시점의 영상으로부터 추적된 다 시점의 2D 특징점이 존재하며 해당 특징점을 2D tracked feature라고 한다.

여러 카메라 시점에서 추적된 2D 특징점은 해당 시점의 카메라 정보와 함께 슬라이딩 윈도우에 저장된다. 슬라이딩 윈도우의 상태가 일정 조건을 만족하면 저장된 정보를 활용하여 여러 카메라 시점에서 동일하게 관찰된 3D 특징점을 삼각 측량하여 기하학적 구속 조건을 만든다 (Mourikis & Roumeliotis 2007). MSCKF는 구속 조건을 측정치 갱신에 활용하기 위하여 INS 상태변수에 카메라-IMU 센서 사이의 상대 벡터를 반영하여 카메라 상태변수를 만들고 이를 시스템 상태변수에 추가한다. 추정된 3D 특징점을 여러 카메라 시점으로 재투영하여 획득한 추정된 2D 특징점과 영상으로부터 추출된 2D 특징점과의 오차를 최소화하는 방향으로 잔차 (residual)를 생성하고 측정치 갱신에 활용한다.

MSCKF에서 매 카메라 시점에 추가하는 카메라 상태변수를 자세히 살펴보면 INS 상태변수와 매우 닮아 있는 것을 알 수 있다. 이는 카메라 상태변수가 단순히 INS의 항법 정보를 기반으로 계산되어 IMU와 카메라 센서 사이의 고정된 위치와 자세 차이 정도만 존재하기 때문이다. 원칙적으로 상태변수 정의는 시스템을 기술할 수 있는 최소한의 변수를 의미하므로 카메라 상태변수는 이에 위배되는 상태변수로 간주될 수 있다. 반면 CSCKF는 카메라 상태변수와 INS 상태변수 사이의 밀접한 닮음 관계를 고려하여 불필요한 카메라 상태변수의 추가 없이 영상 측정치를 효율적으로 활용할 수 있는 새로운 개념의 시전달 특징점 측정치를 제안한다.

측정치를 시간 전파하는 것은 측정치 융합 (measurement fusion) 개념으로 설명될 수 있다 (Bar-Itzhack 1980, Medan & Bar-Itzhack 1985, Lee & Lee 2007). 이는 상호연관성을 가지지 않는 측정치 관계를 이용하여 다 시점에서 획득된 측정치에 의한 융합을 먼저 수행한 다음 측정치 갱신을 수행하는 개념으로 매 시점 측정치 갱신을 수행한 경우와 같은 결과를 보여준다. 즉, 센서로부터 연속적으로 얻어지는 순간 측정치를 측정치 갱신에 사용하지 않고, 시간 전파하여 누적된 시전달 측정치 (time-propagated measurement)를 원하는 시점에서 측정치 갱신하여도 동일한 최적 추정치를 얻을 수 있다는 것이다 (Lee & Lee 2007). CSCKF는 카메라와 IMU가 고정되어 있다는 점을 착안하여 INS 정보를 기반으로 특징점 측정치를 시간 전파하여 시전달 특징점 측정치를 생성한다. 동적 특성이 포함된 INS 정보를 반영하여 특징점 측정치를 시간 전파하기 때문에 카메라 상태변수 없이도 동일한 3D 특징점에 대한 기하학적 구속 조건이 형성된다. CSCKF는 기존 MSCKF와 같이 슬라이딩 윈도우 방식 (Mourikis & Roumeliotis 2007)을 활용하지만 추적된 2D 특징점을 그대로 사용하는 MSCKF와 달리 INS 정보를 활용하여 2D 특징점을 시간 전파한다는 점이 다르다. 지금부터 CSCKF에 대한 내용을 자세히 다룬다.

CSCKF는 잘 알려진 핀홀 카메라 측정치 모델을 활용하며 카메라 캘리브레이션 파라미터는 알려져 있다고 가정한다. 핀홀 카메라 모델을 기반으로 \(j\)번째 3D 특징점 \((\textbf{p}_j^C(\triangleq\textbf{p}_{f,j}^C))\)과 해당 3D 특징점이 영상에 투영된 \(j\)번째 2D 특징점 \((\tilde{\textbf{u}}^j)\) 간의 관계는 Eq. (8)과 같이 표현된다.

\(\tilde{\textbf{u}}^j={f \over x_f^j} \left[\begin{matrix} y_f^j \\ z_f^j \end{matrix}\right]+\textbf{c}_{xy}+\textbf{v}_u^j\)                                                     (8)

여기서 \(\textbf{p}_j^C = [x_f^j~~y_f^j~~z_f^j]^{\rm T}\)는 카메라 좌표계로 표현된 \(j\)번째 3D 특징점의 위치를 나타낸다. \(\textbf{v}_u\)는 \(j\)번째 2D 특징점의 픽셀 잡음을 가리키며 해당 공분산 행렬은 \(\textbf{r}_u=\sigma_{img}^2 \textbf{I}_{2\times 2}\)이고 \(\sigma_{img}\)는 픽셀 잡음의 표준편차를 나타낸다. \(f\)와 \(\textbf{c}_{xy}\)는 카메라 내부 파라미터 (intrinsic parameter)로서 카메라 캘리브레이션을 통해 얻을 수 있는 초점거리 (focal length)와 주점 (principal point)을 각각 가리킨다. 카메라 내부 파라미터는 사전에 미리 알려져 있다고 가정한다.

\(k\) 시점의 카메라 좌표계로 표현된 \(j\)번째 3D 특징점 추정치 \(\hat{\textbf{p}}_j^C(k)\)를 시스템 상태변수의 각 성분으로 재구성하면 Eq. (9)와 같다.

\(\hat{\textbf{p}}_j^C(k)=\textbf{C}_E^C (k) (\textbf{p}_j^E-\textbf{p}_B^E (k)-\textbf{C}_B^E (k)\textbf{p}_{BC}^B )+\textbf{C}_E^C(k) \boldsymbol{\delta} \textbf{p}_j^E\)                                              

\(-\textbf{C}_E^C (k)\Pi_{LLH}^{ECEF} \boldsymbol{\delta}\textbf{llh}(k)-\textbf{C}_N^C (k)⟨\textbf{C}_E^N (k)(\textbf{p}_j^E-\textbf{p}_C^E (k))⟩\boldsymbol{\phi}(k)\)                         (9)

여기서 \(\textbf{p}_{BC}^B\)는 \(B\)-좌표계로 표현된 IMU에서 카메라로의 이동벡터이며 사전에 미리 알려져 있다고 가정한다.

특징점 측정치의 잔차 \(\textbf{z}\)를 생성하기 위하여 Eq. (9)에 있는 \(E\)-좌표계로 표현된 \(j\)번째 3D 특징점 \((\textbf{p}_{j}^E)\)을 먼저 추정해야 한다. Lee et al. (2022)에서는 inverse-depth parametrization 기법 (Civera et al. 2008)을 활용하여 여러 카메라 시점에서 동일하게 관측된 3D 특징점을 추정한다. 여기서 \(\textbf{p}_{j}^E\)는 지구 중심을 기준으로 표현된 위치 벡터로 \(E\)-좌표계로 표현되었기 때문에 시불변이다.

핀홀 카메라 모델을 기반으로 선형화된 특징점 측정치 모델은 Eq. (10)과 같다.

\(\textbf{z}_u^j (k)\triangleq \boldsymbol{\pi}(\tilde{\textbf{p}}_j^C (k)-\tilde{\textbf{p}}_j^C (k))\)                                                                         
\(\cong\textbf{h}_f^j (k)\boldsymbol{\delta}\textbf{p}_j^E+\textbf{h}_\textbf{x}^j (k)\boldsymbol{\delta}\textbf{x}(k)+\textbf{v}_u^j (k)\)                                               (10)

여기서,

\(\textbf{h}_f^j (k)\cong -\textbf{J}_f^j (k)\textbf{C}_E^C (k)\), \(\textbf{h}_{\textbf{x}}^j (k)\cong \textbf{J}_f^j (k)\Pi_{LLH}^{ECEF}+\textbf{J}_f^j(k)\textbf{C}_N^C (k) ⟨\textbf{C}_E^N (k)(\textbf{p}_j^E-\textbf{p}_C^E (k))⟩\),

\(\textbf{J}_f^j (k)={f \over (x_f^j (k))^2} \left[\begin{matrix}-y_f^j (k)&x_f^j (k)&0 \\ -z_f^j (k)&0&x_f^j (k)\end{matrix}\right]\), \(k, j\): 시점과 특징점의 색인, \(\boldsymbol{\pi}(\cdot)\): perspective-projection mapping 함수, \(\textbf{J}_f\): perspective-projection의 Jacobian 행렬, \(\textbf{h}_f\): 3차원 특징점에 대한 \(\hat{\textbf{u}}_u\)의 관측행렬, \(\textbf{h}_\textbf{x}\): 시스템 상태변수에 대한 \(\hat{\textbf{u}}_u\)의 관측행렬, \(\Pi_{LLH}^{ECEF}\): \(E\)-좌표계에서의 위치에 대한 경위도 좌표계 변화율에 대한 Jacobian 행렬이다. 행렬 \(\Pi_{LLH}^{ECEF}\)는 Lee et al. (2022)의 Appendix에서 찾아볼 수 있다.

MSCKF의 \(k\) 시점 카메라 모델 잔차 식은 카메라 상태변수를 기준으로 선형화 되었기 때문에 Eq. (10)과는 상이하다. 또한 MSCKF는 매 시점 카메라의 위치와 자세에 대한 상태변수를 전체 시스템 상태변수에 추가하기 때문에 특징점 추적이 끊기지 않는 경우 전체 상태변수와 오차 공분산 행렬의 크기가 증가하게 된다. 반대로 CSCKF는 카메라 상태변수 추가 없이 시스템 상태변수가 INS 상태변수와 동일하다. 따라서, 전체 상태변수와 오차 공분산 행렬의 크기가 일정하다.

시전달 특징점 측정치는 Fig. 1에 나타난 CF에서 생성된다. 특징점 측정치가 시간 전파된다는 것은 Eq. (10)에 표기된 잔차와 관측행렬이 시간 전파된다는 의미로 해석할 수 있다. 두 카메라 시점 (\(k-1\) 시점과 \(k\) 시점)에서 특징점 측정치가 시간 전파된 경우, 해당 잔차 벡터와 그에 상응하는 관측행렬과 측정치 잡음 행렬은 Eq. (11)과 같이 기술된다.

\(\hat{\textbf{H}}_f^j (k)=\textbf{H}_f^j (k-1)\)                                                                                                          
\(\hat{\textbf{Z}}_u^j (k)=\textbf{Z}_u^j (k-1)\)                                                                                                          
\(\hat{\textbf{H}}_x^j (k)=\textbf{H}_x^j (k-1)\boldsymbol{\Phi}(k,k-1)^{-1}\)                                                                                     
\(\hat{\textbf{R}}_u^j (k)=\textbf{R}_u^j (k-1)+(\hat{\textbf{H}}_x^j (k-1)\textbf{G}(k-1))\textbf{W}(k-1)(\hat{\textbf{H}}_x^j (k-1)\textbf{G}(k-1))^T\)               (11)

여기서 \(\textbf{H}_f^j, \textbf{Z}_u^j, \textbf{H}_\textbf{x}^j, \textbf{R}_u^j\)는 추후 언급될 동일한 3D 특징점이 관측된 카메라 시점 수만큼 Eq. (13)과 같이 누적된다.

Eq. (11)의 \(\textbf{Z}_u^j (0), \textbf{H}_\textbf{x}^j (0), \textbf{H}_f^j (0)\) 그리고 \(\textbf{R}_u^j (0)\)의 초기 값은 각각 \(\textbf{z}_u^j (0), \textbf{h}_f^j (0), \textbf{h}_\textbf{x}^j (0)\) 그리고 \(\textbf{r}_u^j (0)\)로 설정된다. Eq. (11)의 상호 연관 행렬 (cross-covariance matrix), \(\bar{\textbf{S}}\)와 \(\textbf{S}\)의 사용을 피하기 위하여 비상관화를 진행한다 (Lee & Lee 2007). 비상관화 된 Eq. (11)의 식은 Eq. (12)와 같다.

\(\bar{\textbf{H}}_f^j (k)=\hat{\textbf{H}}_f^j (k)\)                                                                                                 
\(\bar{\textbf{Z}}_u^j (k)=\hat{\textbf{Z}}_u^j (k)\)                                                                                                  
\(\bar{\textbf{H}}_\textbf{x}^j (k)=\hat{\textbf{H}}_\textbf{x}^j (k)(\textbf{I}-\textbf{GWG}^T (\textbf{P}(k)^- )^{-1} )\)                                                            
\(\bar{\textbf{R}}_u^j (k)=\hat{\textbf{R}}_u^j (k)-(\hat{\textbf{H}}_x^j (k)\textbf{GWG}^T ) (\textbf{P}(k)^- )^{-1} (\hat{\textbf{H}}_\textbf{x}^j (k)\textbf{GWG}^T )^T\)                     (12)

여기서, \(\textbf{GWG}^T=\textbf{G}(k-1)\textbf{W}(k-1)\textbf{G}(k-1)^T\)이다.

\(j\)번째 3D 특징점은 앞서 설명된 두 카메라 시점이 아닌 여러 카메라 시점 \((k = 1, 2, …, N)\)에서 관측될 수 있다. 따라서, \(k\) 시점에서의 \(j\)번째 3D 특징점에 대한 새로운 2D 특징점이 관측된 경우, 시전달 특징점 측정치의 잔차 벡터 \(\bar{\textbf{Z}}_u^j (k)\)와 새롭게 관측된 2D 특징점 측정치의 잔차 벡터 \(\textbf{z}_u^j (k)\)는 \(\textbf{Z}_u^j (k)\)에 순차적으로 쌓이게 되며, 이에 상응하는 관측행렬과 측정치 잡음 행렬은 Eq. (13)과 같다.

\(\textbf{Z}_u^j (k)=\left[ \begin{matrix} \bar{\textbf{Z}}_u^j (k) \\\textbf{z}_u^j (k)\end{matrix}\right],~~ \textbf{H}_\textbf{x}^j (k)=\left[ \begin{matrix} \bar{\textbf{H}}_\textbf{x}^j (k) \\\textbf{h}_\textbf{x}^j (k)\end{matrix}\right], ~~\) \(\textbf{R}_u^j (k)=\left[ \begin{matrix} \bar{\textbf{R}}_u^j (k)&O \\O&r_u^j (k)\end{matrix}\right],~~ H_f^j (k)=\left[ \begin{matrix} \bar{\textbf{H}}_f^j (k) \\ \textbf{h}_f^j (k)\end{matrix}\right]\)       (13)

여기서 \(\textbf{H}_f^j (k)\)는 \(k\) 시점에서의 j번째 3D 특징점에 대한 관측행렬을 의미한다. 해당 행렬은 추정된 3D 특징점이 고정되어 있고 시불변이기 때문에 시간 전파되지 않고 쌓이기만 한다.

연속된 카메라 시점 \((k = 1, 2, …, N)\)에서 동일하게 관측된 \(j\)번째 3D 특징점에 대한 누적된 잔차 벡터는 Eq. (14)와 같다.

\(\textbf{Z}_u^j (k+N)=\textbf{H}_f^j (k+N)\boldsymbol{\delta}\textbf{p}_j^E+\textbf{H}_x^j (k+N)\boldsymbol{\delta}\textbf{x}(k)+\textbf{w}_u^j (k+N)\)                                 
(14)

Eq. (4)의 시스템 상태변수와 Eq. (10)의 \(\textbf{h}_\textbf{x}^j\)를 살펴보면 MSCKF와 달리 CSCKF에서는 카메라 상태변수 추가가 없는 것을 확인할 수 있다. 이는 INS 정보를 기반으로 2D 특징점 측정치를 시간 전파하여 카메라 상태변수 추가 없이도 각 카메라 시점에 대해서 획득한 측정 정보를 효율적으로 활용할 수 있기 때문이다. 따라서, CSCKF-VIG 시스템의 상태변수는 카메라 상태변수 추가 없이도 항법에 필수적인 상태변수로 가능한 최소로 압축할 수 있다.

Eq. (10)과 Eq. (14)을 살펴보면 \(j\)번째 3D 특징점 위치 오차 \(\boldsymbol{\delta}\textbf{p}_j^E\)가 시스템 상태변수인 \(\boldsymbol{\delta}\textbf{x}\)와 상관된 것을 확인할 수 있다. 그러므로 Eq. (14)에 나타난 누적된 측정치 벡터는 일반적인 측정치 모델인 \((Z = HX + n)\) 형태가 아니기에 EKF 측정치 갱신을 수행할 수 없다.

3D 특징점 위치 오차의 상관성을 제거하기 위하여 Eq. (14)의 잔차 벡터를 \(\textbf{H}_f^j (k+N)\)의 left null space로 투영시킨다 (Mourikis & Roumeliotis 2007). 투영된 잔차 벡터와 그에 상응하는 관측행렬과 측정치 잡음행렬은 Eq. (15)와 같다.

\(\textbf{Z}_o^j (k+N)\triangleq[\textbf{N}_f^j (k+N)]^T \textbf{Z}_u^j (k+N)\)                                                                              
\(=[\textbf{N}_f^j (k+N)]^T [ \textbf{H}_f^j (k+N)\boldsymbol{\delta}\textbf{p}_j^E + \textbf{H}_\textbf{x}^j (k+N)\boldsymbol{\delta}\textbf{x}(k)+\textbf{w}_u^j (k+N)]\)                  
\(=\textbf{H}_o^j (k)\boldsymbol{\delta}\textbf{x}(k)+\textbf{w}_o^j (k)\)                                                                                   
\(\textbf{R}_o^j (k+N)\triangleq [\textbf{N}_f^j (k+N)]^T \textbf{R}_u^j (k+N)[\textbf{N}_f^j (k+N)]\)                                                    (15)

여기서, \(\textbf{H}_o^j (k)=[\textbf{N}_f^j (k+N)]^T \textbf{H}_\textbf{x}^j (k+N)\)이다. Eq. (15)의 \(\textbf{Z}_o^j (k+N)\)를 살펴보면 \(j\)번째 3D 특징점 위치 오차인 \(\boldsymbol{\delta}\textbf{p}_j^E\)에 독립인 것을 확인할 수 있다.

앞서 설명한 시전달 특징점 측정치 생성 과정은 연속된 카메라 시점 \((k = 1, 2, …, N)\)에서 관측된 하나의 3D 특징점에 대한 시전달 특징점 측정치를 나타낸다. 이를 여러 개의 3D 특징점 \((j=1, 2, …, M)\)에 대하여 잔차 벡터를 생성하면 Eq. (16)과 같이 표현된다.

\(\textbf{Z}_{VIS} (k)=\textbf{H}_{VIS} (k)\boldsymbol{\delta}\textbf{x}(k)+\textbf{w}_{VIS} (k)\)                                                                                                           (16)

여기서,

\(\textbf{Z}_VIS (k)≜\left[ \begin{matrix}\textbf{Z}_o^1 (k+N) \\ ⋮ \\ \textbf{Z}_o^j (k+N) \\ ⋮ \\ \textbf{Z}_o^M (k+N)\end{matrix}\right],\textbf{H}_{VIS} (k)\triangleq \left[ \begin{matrix}\textbf{H}_o^1 (k+N) \\ ⋮ \\ \textbf{H}_o^j (k+N) \\ ⋮ \\ \textbf{H}_o^M (k+N)\end{matrix}\right], \textbf{w}_{VIS} (k)\triangleq \left[ \begin{matrix}\textbf{w}_o^1 (k+N) \\ ⋮ \\ \textbf{w}_o^j (k+N) \\ ⋮ \\ \textbf{w}_o^M (k+N)\end{matrix}\right]\) ,

\(\textbf{R}_{VIS} (k)\triangleq \left[ \begin{matrix}\textbf{R}_o^1 (k+N)&&O \\ &⋱& \\ O&&\textbf{R}_o^M (k+N)\end{matrix}\right]\)

이다.

Eq. (16)은 관찰된 모든 3D 특징점에 대한 시전달 특징점 측정치를 누적한 잔차 벡터와 관측행렬 그리고 측정치 잡음행렬을 나타낸다. 이렇게 생성된 시전달 특징점 측정치는 Kalman 이득 계산에 활용되고 EKF 측정치 갱신이 수행된다. 측정치 갱신 식은 Eq. (17)과 같다.

\(\textbf{K}_{VIS} (k)=\textbf{P}(k)^- \textbf{H}_{VIS}^T (k)[\textbf{H}_{VIS} (k)\textbf{P}(k)^- \textbf{H}_{VIS}^T (k)+\textbf{R}_{VIS} (k)]^{-1}\)                                                
\(\boldsymbol{\delta}\hat{\textbf{x}}(k)^+=\boldsymbol{\delta}\hat{\textbf{x}}(k)^-+\textbf{K}_{VIS} (k)Z_{VIS} (k)\)                                                                                        
\(\textbf{P}(k)^+=[\textbf{I}-\textbf{K}_{VIS} (k)\textbf{H}_{VIS} (k)]\textbf{P}(k)^- [\textbf{I}-\textbf{K}_{VIS} (k)\textbf{H}_{VIS} (k)]^T -\textbf{K}_{VIS} (k)\textbf{H}_{VIS} (k)\textbf{P}(k)^-\)     (17)

EKF 측정치 갱신이 수행된 이후에는 사용된 특징점 측정 정보는 더 이상 사용하지 않게되며 슬라이딩 윈도우는 초기화된다. 이에 비하여, MSCKF에서는 사용된 특징점이 계속 관측된다면 슬라이딩 윈도우가 초기화 되지 않고 추후 측정치 갱신에서도 계속 활용하게 된다. 이는 측정치 갱신 이후에도 과거 특징점 측정치에 해당하는 카메라 상태변수로 재 사용한다는 의미가 된다. CSCKF는 독립적인 측정 정보만을 활용해야 하는 EKF의 중요한 가정을 준수하기 위하여 과거 특징점 정보를 측정치 갱신 이후에 추가적으로 활용하지 않지만, 시스템 상태변수에 카메라 상태변수를 포함시키지 않았음에도 불구하고 여러 카메라 시점 사이에서 관찰한 결과를 삼각 측량하여 기하학적 구속 조건을 효율적으로 활용한다.

추가적으로 영상 측정치가 시간 전파하는 도중에 GNSS 측정치에 의하여 EKF가 업데이트 될 수 있다. 이런 경우에는 CF에서 해당 GNSS 측정치를 직접적으로 활용하여 영상 측정치에 반영할 수 없게 된다. 따라서, GNSS 측정치 갱신에 의해 보정된 값을 동일하게 모든 영상 특징점 측정치에 반영해줘야 한다. Fig. 1에서 해당 내용에 대한 부분을 maintenance로 표기하였으며, Eq. (18)로 영상 특징점 측정치 잔차 벡터가 보정된다.

\(\bar{\textbf{Z}}_u^j (k)\leftarrow\bar{\textbf{Z}}_u^j (k)-\bar{\textbf{H}}_u^j (k)\textbf{K}_{GNSS} (k)\textbf{Z}_{GNSS} (k)\)                                   (18)

여기서 \(\textbf{K}_{GNSS} (k)\)와 \(\textbf{Z}_{GNSS} (k)\)는 다음 절에서 다룬다.

3.5 Direct Measurement Update – GNSS

이번 절에서는 GNSS 원시 측정치를 활용한 EKF 측정치 갱신에 대해서 설명한다. 먼저 사용자 수신기로부터 획득 가능한 \(j\)번째 위성에 대한 L1 의사거리 및 반송파 측정치는 각각 \(\tilde{\rho}_u^j\)와 \(\tilde{\phi}_u^j\)로 표기되며, Eqs. (19, 20)과 같이 모델링 된다.

\(\tilde{\rho}_u^j=(\textbf{e}_u^j )^T [\textbf{p}_{SV}^j-\textbf{p}_u^E ]+b_u+\varepsilon_\rho^j+v_u^j\)                                             (19)

\(\tilde{\phi}_u^j=(\textbf{e}_u^j )^T [\textbf{p}_{SV}^j-\textbf{p}_u^E ]+b_u+\varepsilon_\phi^j+\lambda N_u^j+n_u^j\)                                         (20)

여기서 \(\textbf{e}_u^j\)는 사용자 수신기에서 \(j\)번째 위성으로 향하는 시선각 (line-of-sight) 벡터를 나타낸다. \(\textbf{p}_{SV}^j\)와 \(\textbf{p}_u^E\)는 각각 \(E\)-좌표계로 표현된\(j\)번째 위성과 사용자 수신기의 위치 벡터를 나타낸다. \(\varepsilon_\rho^j\)와 \(\varepsilon_\phi^j\)는 각각 \(j\)번째 위성에 대한 코드와 반송파 측정치의 공통 오차 (위성 관련 오차, 전리층 오차, 대류층 오차)를 나타낸다. \(b_u\)는 사용자 수신기의 시계 오차를 나타낸다. \(N^j\)은 \(j\)번째 위성에 대한 반송파 측정치에 포함된 미지정수 오차를 나타낸다. \(\lambda\)는 GNSS 신호의 파장을 나타낸다. \(v_u^j\)와 \(n_u^j\)는 각각 \(j\)번째 위성에 대한 코드와 반송파 측정 잡음을 나타낸다. 아래 첨자 \(u\)와 \(SV\)는 각각 사용자 수신기와 위성을 의미한다.

Lee et al. (2022)는 사용자 수신기 뿐만 아니라 기준국으로부터 획득한 GNSS 원시 측정치를 모두 활용한다. 획득된 측정치를 기반으로 차분 기법 (difference method) (Misra & Enge 2001)을 적용하여 SD 코드 측정치와 TD 반송파 측정치를 각각 생성하여 시스템의 위치와 속도를 보정하는데 활용한다. SD 코드 측정치와 TD 반송파 측정치의 잔차 벡터는 Eqs. (21, 22)와 같다.

\(z_\rho^j (k)\triangleq\tilde{\rho}_{ru}^{jo} (k)-\tilde{\rho}_{ru}^{jo} (k)\)                                                                                
\(\cong\tilde{\rho}_{ru}^{jo} (k)-(\textbf{e}_u^{jo} (k))^T [\textbf{p}_u^E (k)-\textbf{p}_r^E ]\)                                                        
\(\cong-(\textbf{e}_u^{jo} (k))^T \Phi_{LLH}^{ECEF} (k)\boldsymbol{\delta}\textbf{p}_u^N (k)+v_{ru}^{jo} (k)\)                                           (21)

\(z_\phi^j (k)\cong\nabla\tilde{\phi}_{ru}^{jo} (k)+(\textbf{e}_u^{jo} (k))^T [\textbf{p}_r^E-\hat{\textbf{p}}_u^E (k)]\) \(-(\textbf{e}_u^{jo} (k-1))^T [\textbf{p}_r^E-\textbf{p}_u^E (k-1)]\)                    
\(\cong-(\textbf{e}_u^{jo} (k))^T \textbf{C}_N^E (k)\boldsymbol{\delta}\dot{\textbf{p}}_u^N (k)+\nabla n_{ru}^{jo} (k)\)                                                             (22)

여기서 \(\tilde{\rho}_{ru}^{jo} (k)\)와 \(\cong\nabla\tilde{\phi}_{ru}^{jo} (k)\)는 각각 \(k\) 시점의 DD 코드 측정치와 TD 반송파 측정치를 나타낸다. \(\nabla\)는 연속된 두 시점 사이의 차분을 의미한다. 위 첨자 \(jo\)는 \(j\)번째 위성에서 \(o\)번째 위성을 차분한 것을 의미하며, 아래 첨자 \(ru\)는 기준국에서 획득한 GNSS 측정치에서 사용자 수신기에서 획득한 GNSS 측정치를 차분한 것을 의미한다.

관측된 모든 위성에 대한 DD 코드와 TD 반송파 측정치의 잔차 벡터와 그에 상응하는 관측행렬과 측정치 잡음행렬은 Eq. (23)과 같다.

\(\textbf{Z}_{GNSS} (k)=\textbf{H}_{GNSS} (k)\boldsymbol{\delta}\textbf{x}(k)+\textbf{w}_{GNSS} (k)\)                                             (23)

여기서 \(\textbf{Z}_{GNSS}, \textbf{H}_{GNSS}\) 그리고 \(\textbf{w}_{GNSS}\) 행렬은 Lee et al. (2022)의 Appendix에서 찾아볼 수 있다.

카메라 측정치 갱신과 비슷하게 Eq. (23)에 나타난 모든 GNSS 측정치는 Kalman 이득 계산에 활용된다. EKF 측정치 갱신은 Eq. (17)과 같이 진행된다.

4. EXPERIMENT

CSCKF-VIG 시스템의 성능평가를 위하여 실외 실험을 진행하였다. 차량에 GNSS 수신기, IMU 그리고 카메라 센서를 부착하고 주행하면서 데이터를 취득하였다. 실제 도심지에서 참 궤적을 획득하는 것은 매우 어려우며 고가의 항법 장비 일지라고 도심지에서는 오차의 변동이 크기 때문에 참 궤적으로 비교하기에는 난점이 존재한다. 따라서, 본 실험에서는 개활지 환경에서 RTK 기법으로 산출된 정수해를 참값으로 설정하고 획득한 GNSS 측정치의 수를 조절하여 고의적으로 열악한 수신환경을 모사하였다.

실험 전반에 걸쳐 카메라 캘리브레이션 그리고 각 센서 간의 lever-arm 및 misalignment는 사전에 미리 알고 있다고 가정하였다. 모든 표에서 굵게 표시된 수치는 각 실험 결과에서 가장 좋은 수치를 의미하며 정확도 척도로 Root Mean Square Error (RMSE) 및 maximum error를 활용하였다. CSCKF-VIG 시스템과 성능 비교를 위하여 총 5개의 알고리즘이 비교되었다. 첫번째는 GNSS-SPP로 단일 수신기에서 측정되는 의사거리만을 활용하여 위치를 추정하는 알고리즘이다. 두번째와 세번째는 두 대의 GNSS 수신기를 활용한 INS/GNSS 강결합 EKF 알고리즘이며 사용된 GNSS 원시 측정치에 따라 구별된다. 두번째의 경우 사용된 GNSS 원시 측정치는 의사거리 측정치와 도플러 측정치이며 세번째의 경우 의사거리 측정치와 반송파 측정치이다. 네번째는 기존 MSCKF 기반의 Visual/Inertial/GNSS 알고리즘으로 GNSS 측정치는 DD 의사거리와 TD 반송파 측정치를 활용하였다. 마지막은 CSKCF 기반의 Visual/Inertial/GNSS 알고리즘으로 시전달 특징점 측정치와 IMU 측정치 그리고 GNSS 측정치로 DD 의사거리와 TD 반송파 측정치를 활용하였다. 모든 알고리즘에서 FDE 기법 (Kim et al. 2019)을 적용하여 cycle slip 및 multipath가 포함된 GNSS 측정치는 제거하였다. 모든 표에서 각 알고리즘의 약어로 GNSS-SPP, INS/GNSS (PR+Dpplr), INS/GNSS (PR+CP), VIS/INS/GNSS (MSCKF) 그리고 VIS/INS/GNSS (CSCKF)로 표기하였다.

실험에서 사용된 GNSS 수신기는 다중 위성군 및 다중 주파수를 제공하는 NovAtel ProPak6를 활용하였으며 GNSS 안테나는 NovAtel GPS-703-GGG-HV를 사용하였다. 사용된 IMU는 Microelectromechanical System 사의 (MEMS)-IMU인 Xsens-MTi 모델을 사용하였다. 카메라는 스테레오 카메라인 FLIR (구 PointGrey)사의 BumblebeeXB3 모델을 사용하였으며 스테레오 카메라에서 왼쪽 카메라로부터 획득된 이미지만을 활용하였다. Fig. 3은 실험에 사용된 센서가 부착된 차량을 보여준다. 모든 센서 측정치는 GPS 시간에 시각 동기화되어 측정치를 수집하였으며 각 센서의 샘플링 주기는 IMU는 100 Hz, 카메라는 15 Hz 그리고 GNSS 수신기는 1 Hz이다. 실험에서 사용된 GNSS 신호는 GPS L1C, GLONASS G1, BeiDou B1 그리고 Galileo E1이다. GNSS 기준국은 한국항공대학교 연구동 옥상에 설치된 Trimble Ti-V2 Choke Ring 안테나와 이동체에 사용된 수신기와 동일한 NovAtel ProPak6를 활용하였다. Fig. 4는 Google Earth로 표현된 실험의 참궤적과 해당 실험의 주변 환경을 보여준다.

Fig. 3. Test platform and equipment.

Fig. 4. The real-world dataset for the experiment: trajectory expressed by Google Earth (up) and its surrounding environments (down).

4.1 GNSS 가시성이 좋은 조건에서의 실험 결과

CSCKF-VIG 시스템의 성능평가를 위하여 개활지에서 데이터를 취득하고 각 알고리즘의 결과를 비교하였다. Fig. 5는 GNSS 가시성이 좋은 환경에서의 실험 결과를 총 6가지 그래프로 보여준다. 그림에서 위에 3개 그래프는 왼쪽에서부터 각 알고리즘으로부터 추정된 차량 이동 궤적과 가시 위성 수, Position Dilution of Precision (PDOP)를 보여주며 아래 3개 그래프는 각 알고리즘으로부터 산출된 위치 오차를 East-North-Up (ENU) 좌표계로 표현한 것이다. Table 1은 개활지 환경에서의 각 알고리즘의 결과를 ENU 좌표계로 정리한 표이다. 표에서 볼 수 있듯이 GNSS 가시 위성수가 충분한 환경에서는 GNSS-SPP 알고리즘을 제외하고 추정된 위치해가 정확하고 안정적인 것을 확인할 수 있었다. 전반적으로 영상 측정치를 활용한 두 Visual/Inertial/GNSS 알고리즘이 기존 INS/GNSS 알고리즘에 비해 비교적 위치 정확도가 높은 것을 볼 수 있었다. 두 Visual/Inertial/GNSS 알고리즘의 결과는 비슷한 것을 알 수 있으며, GNSS 가시 위성 수가 충분한 환경에서는 영상 측정치 활용으로 인한 성능 향상이 뚜렷이 보이지 않음을 확인할 수 있었다.

Fig. 5. Estimated trajectories (left up), the number of visible satellites (middle up), PDOP values (right up), position errors in the ENU directions (bottom) under the open-sky condition.

Table 1. Comparison on RMSEs, maximum errors, and solution availability of position by different algorithms under the open-sky condition.

Algorithms	RMSE (m)				Maximum error (m)			Availability (%)
	E	N	U		E	N	U
GNSS-SPP INS/GNSS (PR + Dpplr) INS/GNSS (PR + CP) VIS/INS/GNSS (MSCKF) VIS/INS/GNSS (CSCKF)	1.07 0.50 0.33 0.18 0.17	1.38 0.53 0.35 0.23 0.23	14.26 0.61 0.57 0.48 0.32		6.26 1.40 1.01 0.51 0.56	4.28 1.77 0.98 0.90 0.95	17.50 2.23 1.78 1.61 1.12	100 100 100 100 100

 

4.2 GNSS 수신이 열악한 환경 조건에서의 실험 결과

GNSS 수신이 열악한 환경에서의 CSCKF-VIG 시스템의 성능을 평가하기 위하여 개활지에서 획득한 데이터를 기반으로 GNSS 위성 수를 조절하였다. 고의적으로 앙각 45도 이상의 GNSS 위성을 랜덤하게 선택하여 열악한 환경을 모사하였다. Fig. 6은 GNSS 수신이 열악한 환경에서의 실험 결과를 총 6가지 그래프로 보여준다. Fig. 5에서와 마찬가지로 Fig. 6의 위에 3개 그래프는 왼쪽에서부터 각 알고리즘으로부터 추정된 차량 이동 궤적과 가시 위성 수, PDOP를 보여주며 아래 3개 그래프는 각 알고리즘으로부터 산출된 위치 오차를 \(ENU\) 좌표계로 표현한 것이다. Table 2는 열악한 환경에서의 각 알고리즘의 결과를 \(ENU\) 좌표계로 정리한 표이다. 표에서 볼 수 있듯이 GNSS 위성수가 4개 미만인 경우에는 GNSS-SPP는 항법해를 제공할 수 없기 때문에 가용성이 76.92%인 것을 확인할 수 있었으며 다른 알고리즘은 강결합 방식으로 INS와 결합되어 위치해 가용성은 100%인 것을 확인할 수 있었다. 하지만 두 INS/GNSS 알고리즘은 강결합 방식일 지라도 GNSS 위성수가 4개 미만인 기간이 길어지거나 측정치 갱신이 불가한 경우 RMSE가 급격히 증가하였다. 반면에 영상 측정치를 활용한 두 Visual/Inertial/GNSS 알고리즘은 개선된 위치 정확도를 보여주며 특히 CSCKF-VIG 시스템이 기존 알고리즘에 비해 위치 정확도가 향상된 것을 확인할 수 있었다.

Fig. 6. Estimated trajectories (left up), the number of visible satellites (middle up), PDOP values (right up), position errors in the ENU directions (bottom) under the GNSS-degraded condition.

Table 2. Comparison on RMSEs, maximum errors, and solution availability of position by different algorithms under the GNSS-degraded condition.

Algorithms	RMSE (m)				Maximum error (m)			Availability (%)
	E	N	U		E	N	U
GNSS-SPP INS/GNSS (PR + Dpplr) INS/GNSS (PR + CP) VIS/INS/GNSS (MSCKF) VIS/INS/GNSS (CSCKF)	2.24 1.49 1.19 0.59 0.49	0.77 1.73 1.25 0.56 0.46	4.13 2.82 2.12 1.71 1.29		4.15 8.93 9.08 3.11 2.39	5.77 5.64 5.60 2.25 1.78	15.72 6.46 4.95 5.84 3.94	76.92 100 100 100 100

 

5. CONCLUSION

본 논문에서는 GNSS 수신이 열악한 환경에서도 정확하고 안정적인 항법해를 제공하는 새로운 강결합 방식의 CSCKF-VIG 시스템을 소개하고 실제 실험 데이터를 기반으로 기존 시스템과 성능 분석을 진행하였다. CSCKF-VIG 시스템은 가속도와 자이로 측정치, 이미지 기반의 특징점 측정치 그리고 차분된 GNSS 측정치를 활용하여 성능 손실 없이 불필요한 카메라 상태변수를 제거하고 항법에 필요한 INS 상태변수로만 시스템을 구성한 점이 특징이다. CSCKF-VIG는 GNSS가 열악한 환경에서도 기존 알고리즘과 비교하여 정확한 항법해를 제공한 것을 확인할 수 있었다.