Cubature 칼만 필터를 사용한 INS/UWB 결합항법시스템

Cubature 칼만 필터를 사용한 INS/UWB 결합항법시스템

조성윤^1,＊

1

INS/UWB Integrated Navigation System using Cubature Kalman Filter

Seong-Yun Cho^1*

요 약

사람 및 로봇의 항법정보는 위치기반서비스, VR (Virtual Reality), AR (Augmented Reality), 길안내, 로봇 제어 등의 목적으로 중요하게 평가된다. 정확성 및 가용성 면에서 실외 뿐 아니라 실 내에서도 정확하고 연속적인 항법정보를 필요로 하지만 실내 환경에서 GPS (Global Positioning System)/GNSS (Global Navigation Satellite System) 신호를 수신하지 못하므로 대체 인프라 및 센서 융합 기법을 요구한다. 본 논문에서는 INS (Inertial Navigation System)와 UWB (Ultra Wideband)를 결합한 항법시스템을 제시한다. 두 시스템을 결합하기 위해 CKF (Cubature Kalman Filter)를 이용하는 방법을 제안하고 시뮬레이션을 통해 성능을 분석한다.

Abstract

Navigation information of pedestrians and robots is evaluated for the purpose of location based service, VR, AR, guidance, and robot control. Accurate and continuous navigation information is required not only in outdoor but also indoor space, but GPS/GNSS signals cannot be received in indoor environment, so alternative infrastructure and sensor fusion techniques are required. In this paper, a navigation system integrating INS and UWB is proposed. To integrate the two systems, CKF is used and the performance is analyzed through simulation.

Keywords : Navigation system, INS/UWB integration, cubature Kalman filter

1경일대학교 로봇공학부

＊Corresponding Author : sycho@kiu.kr 접수일자 : 2018. 05. 21.

1차 심사 : 2018. 05. 29.

2차 심사 : 2018. 06. 11.

심사완료 : 2018. 06. 15.

1. 서 론

항법시스템은 이동체의 위치정보 및 때로는 속도정보와 자세정보까지 추정한다. GPS (Global Positioning System)/GNSS (Global Navigation Satellite System) 신호를 수신할 수 있는 실 외에서는 GPS 수신기가 일반적인 항법시스템 으로 사용된다. 그러나 실내 환경에서는 GPS 신호를 수신할 수 없어 항법을 위한 대체 인프 라를 요구하며 다양한 센서들의 융합을 통한 복합 항법시스템이 사용될 수 있다.

실내 항법용 무선통신 인프라에는 Wi-Fi, ZigBee, BLE (Bluetooth Low Energy), UWB (Ultra Wideband), CSS (Chirp Spread Spectrum) 등이 있다. 여기서 두 송수신기 사 이에서 전송되는 신호의 정확한 전파 시간 (Flight Time)을 측정하지 못하므로 수신된 신호의 전파 세기 정보인 RSSI (Received Signal Strength Indicator)를 활용하여 위 치를 추정하는 Wi-Fi, ZigBee 및 BLE와 전 파 시간을 측정할 수 있는 UWB 및 CSS로 나 눌 수 있다. RSSI를 사용하는 경우 기 구축되 어 있는 인프라를 사용하여 저가의 위치추정 시스템을 구축할 수 있는 장점이 있으나 정확 도가 낮으며 정확도를 높이기 위한 RSSI 기반 전자지문 (Fingerprinting) 기법은 주기적인 전자지문 데이터베이스 갱신이 이루어져야 하 는 번거로움을 갖는다 [1, 2]. 이에 반해 UWB는 기 구축되어 있는 인프라는 없으나 최 근에는 저가의 칩셋 구입이 가능해 저가의 시 스템의 구축할 수 있으며 정확도 또한 높아 많 은 관심을 받고 있다 [3]. 그러나 UWB 기반 항법시스템은 정확한 위치정보는 제공할 수 있 으나 AR (Augmented Reality) 및 VR (Virtual Reality) 응용 시스템에서 필요로 하 는 자세정보는 제공할 수 없다. 따라서 본 논문 에서는 실내 환경에서 위치 뿐 아니라 속도, 자 세정보 등의 항법정보를 필요로 하는 다양한 응용시스템에 활용할 수 있도록 INS (Inertial Navigation System)와 UWB를 결합한 시스 템을 제안한다.

항법시스템에서 두 가지 이상의 센서 및 시

스템을 결합하는 경우 필터가 사용된다. 이때 항법시스템은 비선형으로 모델링 되므로 비선 형 필터를 사용하게 된다. 일반적으로 많이 사 용되던 EKF (Extended Kalman Filter)는 초기 추정오차가 큰 경우 필터의 수렴 특성이 나쁘거나 때로는 발산하는 단점을 갖는다. 이런 문제를 극복하기 위해 UKF (Unscented Kalman Filter)가 연구되었다. UKF는 상태 변수의 확률분포를 갖는 시그마 포인트를 사용 하여 확률분포의 비선형 시간 전파를 수행함으 로써 EKF의 단점을 보안하고자 하였다 [4].

그러나 최근 UKF의 음의 가중치로 인한 비안 정성 문제를 해결하기 위한 CKF (Cubature Kalman Filter)가 연구되어 활용되고 있다 [5]. 본 논문에서는 이런 필터들의 특성을 고 려하여 CKF 기반으로 INS와 UWB를 결합한 항법시스템을 설계한다. 그리고 시뮬레이션 기 반으로 제안된 시스템의 성능을 분석한다. 이때 UKF를 사용한 INS/UWB 시스템과의 비교를 통해 CKF를 사용한 시스템의 성능이 보다 우 수하며 안정적인 것을 보인다.

본 논문은 다음과 같이 구성되어 있다. 2장 에서 CKF 기반 INS/UWB 결합항법시스템을 설계하고, 3장에서 시뮬레이션 기반으로 성능 을 검증하며, 마지막 장에서 결론을 맺는다.

2. INS/UWB 결합항법시스템 설계 INS는 3축의 가속도계와 자이로로 구성되는 IMU (Inertial Measurement Unit)를 사용 하여 자세 및 속도, 위치정보를 6 자유도 운동 미분방정식을 풀어 연속적으로 계산한다 [6].

먼저 IMU를 장착한 이동체가 정지상태에서 의 초기 자세정보를 계산하는 초기정렬 과정을 수행한다. 정지상태에서 IMU는 자세에 따라 결정되는 중력가속도와 지구자전 각속도만을 출력한다. 중력가속도 정보를 사용하여 롤 (Roll)과 피치 (Pitch)각을 계산한 다음 이 정 보를 지구자전 각속도 정보와 함께 사용하여 요 (Yaw)각을 계산한다. 그러나 사용되는 IMU가 저가의 MEMS형 자이로로 구성되는

경우 지구자전 각속도를 측정할 수 없다. 즉 초 기 자세정보 중 요각을 정확하게 계산할 수 없 게 된다. 초기 자세정보를 계산한 다음 자이로 출력 주기에 맞추어 자세정보를 갱신한다. 이때 자세정보는 롤, 피치 및 요각으로 나타내는 Euler 각의 미분방정식을 사용하여 갱신할 수 있으며, 또한 Euler 각으로 구성되는 방향코사 인행렬 (Direction Cosine Matrix) 또는 쿼 터니언 (Quaternion)의 미분방정식을 사용하 여 갱신할 수 있다. 일반적으로 IMU의 출력주 파수가 작은 경우 샘플링 사이의 비교환 오차 (Non-Commutative Error)에 의한 영향이 작은 쿼터니언 기반의 자세정보 갱신이 사용된 다. 갱신된 자세정보를 사용하여 가속도계 출력 을 항법좌표계로 변환한 후 적분 과정을 통해 속도 및 자세정보를 갱신하게 된다. 이 과정에 서 중요한 것 중 하나는 실제 이동체의 움직임 과 상관없이 가속도계의 출력에 포함되는 중력 가속도 성분을 제거해 주어야 하는 것이다. 이 를 위해서는 정확한 자세정보가 필요하며 위치 에 따른 정확한 중력모델을 사용하는 것이 중 요하다.

INS의 의한 항법정보는 처음에는 정확하지 만 자이로와 가속도계 출력의 적분 과정에서 센서 오차의 누적현상이 발생함으로써 점점 오 차가 증가하는 단점을 갖는다. 따라서 일반적으 로 비 관성센서를 함께 사용하는 결합항법시스 템을 사용한다. 본 논문에서는 UWB를 INS와 결합하는 방법을 제안한다.

INS와 UWB를 결합하는 방법은 UWB 기반 으로 측정되는 정보에 따라 결정된다. 먼저 UWB 기반 위치추정 시스템을 통해 획득된 위 치정보를 사용하는 약결합 (Loosely Coupled) 방법을 고려할 수 있으며, 고정노드 와 이동노드 사이에 측정된 거리정보를 사용하 는 강결합 (Tightly Coupled) 방법을 고려할 수 있다. UWB는 500MHz 이상의 대역폭을 갖는 극초단파을 사용하여 30cm 이하의 정확 도를 갖는 거리측정치를 제공할 수 있다. 그리 고 두 노드 사이에 장애물이 있어도 투과성이 좋아 LoS (Line-of-Sight) 거리를 측정할 가 능성이 높다. 그러나 공장, 물류창고와 같이 신

호의 투과성이 높지 않은 장애물이 많은 공간 에서는 채널별로 거리 측정치를 획득하지 못하 는 경우가 발생할 수도 있다. 이런 경우 UWB 기반 위치정보를 생성할 수 없는 경우도 발생 하며 약결합 기반 INS/UWB 결합이 불가할 수도 있다. 따라서 본 논문에서는 강결합 기반 으로 결합 항법시스템을 설계함으로써 결합의 가용성을 향상시키고자 한다.

INS의 항법식은 비선형 함수이며, UWB 기 반 거리 측정식도 비선형 함수이다. 따라서 두 시스템을 결합하기 위한 필터는 EKF, UKF, CKF 등과 같은 비선형 필터를 사용해야 한다.

EKF는 비선형 식의 선형화를 통해 계산된 자 코비안 (Jacobian) 행렬을 사용하여 오차 공 분산 행렬의 시간 전파를 수행한다. 만약 초기 추정오차가 큰 경우 계산된 자코비안 행렬에 큰 오차가 포함되며 결국 오차 공분산 행렬의 잘못된 계산으로 인해 필터의 안정성은 깨지게 된다 [4]. 이 문제를 해결하기 위해 개발된 UKF는 상태변수의 확률분포를 사용하여 설정 된 시그마 포인트들의 시간 전파를 비선형 함 수를 그대로 사용하여 수행한 다음 이를 사용 하여 오차 공분산을 계산함으로써 초기 추정오 차에 의한 필터의 불안정성은 감소하게 된다.

그러나 시그마 포인트들의 가중치 합을 통해 상태변수와 오차 공분산을 계산하게 되며 이때 사용되는 가중치가 음의 값을 갖는 경우로 인 해 필터가 불안정해 지는 문제가 발생한다. 이 에 반해 CKF는 EKF와 UKF에서 갖는 이런 불안정성이 없는 안정된 필터구동이 가능한 장 점을 갖는다. 본 논문에서는 다음과 같이 CKF 를 기반으로 INS와 UWB를 결합하는 필터를 설계한다.

상태변수는 다음과 같이 설정한다.

   ^ ^ ∇^ ^^ (1)

여기서 ^는 위도, 경도, 고도로 나타내는 위 치이며, ^는 항법좌표계 상의 속도, ^는 Euler 각을 의미한다. 그리고 ^∇와 ^는 각각 동체좌표계 상의 가속도계와 자이로 바이어스 를 의미한다.

시스템 차수는 15차이다. 그리고 3차 CKF 를 구동하기 위해 시스템 차수의 2배에 해당하 는 30개의 Cubature 포인트를 다음과 같이 설정 한다 [5].

_  _±_^ _{ × }_ (2)

여기서 ^는 ^ 시점에서의 상태변수 추정치,

_{ × }_는 ^{ × } 크기의 단위행렬의 ^번째 열벡

터를 의미하며, ^는 오차 공분산 행렬의 Cholesky 분해 (^__^ )를 통해 계산된 다. 그리고     ⋯ .

Cubature 포인트는 IMU의 센서 데이터 출 력 주기에 맞추어 다음과 같이 시간 전파 된다.

_{  }  ^     (3)

여기서 ^는 INS의 계산식을 나타낸 것이다.

시간 전파된 Cubature 포인트를 다음과 같 이 가중치 합의 과정을 통해 상태변수와 오차 공분산 행렬을 계산한다.

_{  } 

_{  }



_{  } 

_{  }



^

^

^

 

(5)

여기서 ^는 시스템 공정잡음 (Process Noise) 공분산을 의미한다.

이동체에 장착된 UWB 노드에서 각 고정노 드와의 통신을 통해 획득된 거리측정치의 한 세트가 제공되면 측정치 갱신을 다음과 같이 수행한다.

_ _{  } _





_ _{  } __{ }^ (7)

여기서 ^는 UWB 거리 측정치를 의미하며,

_{  }는 고정노드의 위치정보와 상태변수 추정

치를 사용하여 계산된 거리값으로 다음과 같이

나타낼 수 있다.

_{  } 

_{  }



⋯ _{  } ^

(8)

여기서 ^은 UWB 기반 거리 측정치의 수를

의미하며 ^   는 ^번째 Cubature 포인트

를 사용하여 계산된 ^번째 거리값으로 다음과 같이 계산된다.

_{  }  



여기서 ^   는 ^번째 Cubature 포인트

의 ^번째 구성요소를 의미한다. 그리고 ^{} 는 ^번째 고정노드의 위치벡터를 의미한다.

식 (7)에서 나타낸 Kalman 이득은 다음과 같이 계산된다.

__{ }^^{ } (10)

여기서 ^은 측정치 잡음 공분산을 나타낸 것 이며 ^ 와 ^는 각각 다음과 같이 계산된다.

_{ } 

_{  }



^

^

^

(11)

_ 

_{  }



^

^

^

(12)

식 (2) - 식 (12)의 과정은 IMU의 센서 데 이터 출력 주기와 UWB 거리 측정치의 획득 주기에 맞추어 주기적으로 구동된다.

3. 시뮬레이션 기반 성능 분석 제안된 CKF 기반 INS/UWB 결합항법시스 템의 성능을 분석하기 위해 시뮬레이션을 수행 하였다. INS/UWB 결합항법을 위한 필터를 UKF와 CKF를 사용하여 각각 수행된 시뮬레

이션 결과를 비교하였다.

시뮬레이션을 위해 사용된 IMU는 Analog Devices사의 ADIS16364, UWB는 DecaWave 사의 DW1000을 기본으로 하였으며 표 1과 같이 Spec.을 설정하였다 [7, 8].

표 1. 센서 규격.

Table 1. Sensor Spec.

센서 파라미터 값 (1) 단위

Gyro

바이어스 ±3 

각 랜덤 워크 1.9 ^

출력 주파수 100 

Accel

바이어스 ±15 

속도 랜덤 워크 0.35 ^

출력 주파수 100 

UWB

Accuracy 30 

Precision 10 

출력 주파수 1 

 ×의 공간 내에 4개의 고정노드가 설 치되고 이동체의 이동 궤적은 그림 1. 과 같은 것으로 하였다. 이동체의 초기 요각 오차가 5 도인 경우 (Case I)와 초기 요각 오차가 90도 인 경우 (Case II)에 대한 시뮬레이션을 각각 수행하여 그 결과를 분석하였다.

0 5 10 15 20 25 30 35 40 45 50

0 5 10 15 20 25 30 35 40 45 50

Y [m]

X [m]

Start

AN 1 AN 2

AN 3 AN 4

그림 1. 이동 궤적.

Fig. 1. Moving trajectory.

그림 2. 는 Case I에서 수행된 시뮬레이션의 결과를 나타낸 것으로 위치, 요각, 그리고 자이 로 바이어스 추정 오차를 각각 나타내고 있다.

초기 요각 추정오차가 5도로 설정된 경우이며, 이 경우 UKF와 CKF는 유사한 성능을 갖는다.

위치 추정 오차는 3축 오차의 Euclidean 거리 를 나타낸 것으로 점점 오차가 측정치 오차 범 위 내로 줄어드는 것을 볼 수 있으며 요각 추

정 오차도 이동체가 회전을 하면서부터 가관측 하기 시작하여 약 30초부터 0 근처의 값으로 수렴하는 것을 볼 수 있다. 그리고 자이로 바이 어스 추정 오차도 이동체가 기동하고 회전하면 서 점점 0 근처로 수렴하게 된다.

그림 3. 은 Case II에서 수행된 시뮬레이션 결과이며 그림 2. 와 같은 내용의 그림을 나타 내고 있다. 이 경우 초기 요각 추정 오차는 90 도로 큰 경우에 해당한다. 위치 추정 오차가 Case I과는 달리 측정치 오차 범위 내로 수렴 하지 않는 것을 볼 수 있다. 그 이유를 분석하 기 위해 요각 추정 오차를 보면 UKF와 CKF 모두 0이 아닌 값으로 수렴하는 형상을 보이고 있다. 이 경우 EKF를 사용하면 필터는 발산하 는 결과를 보이게 된다. 이에 반해 UKF는 EKF 보다는 초기 요각 추정 오차에 강인한 특 성을 보이지만 90도와 같이 큰 초기 요각 추정 오차에 대해서는 Case I과 같이 좋은 수렴 특 성은 보이지 않는다. 그 이유는 자이로 바이어 스 추정 오차에서 볼 수 있듯이 Z축 자이로 바 이어스를 정확하게 추정하지 못하기 때문이다.

물론 이동체의 기동을 오랫동안 다양하게 수행 하면 점점 성능은 향상하게 된다. 이 경우 UKF는 음의 가중치로 인해 다소 불안한 특성 을 보이게 되며, 이와 달리 가중치의 불안 요소 를 갖지 않는 CKF는 UKF 보다 다소 좋은 성 능을 보이는 것을 알 수 있다. 따라서 본 논문 에서 제안된 CKF 기반 INS/UWB 결합항법 시스템은 EKF 및 UKF 보다 안정적인 항법 솔루션을 제공할 수 있음을 확인할 수 있다.

0 10 20 30 40 50 60 70

0 0.5 1 1.5 2 2.5

Position Estimation Error [m]

Error

time [sec]

UKF CKF Measurement

(a) 위치 추정 오차

0 10 20 30 40 50 60 70 -5

0 5 10 15 20 25 30 35 40

Heading Estimation Error [deg]

Heading Error

time [sec]

UKF CKF

(b) 요각 추정 오차

0 10 20 30 40 50 60 70

0 1 2 3

Gyro Bias Estimation Error [deg/s]

X-axis Error

UKF CKF

0 10 20 30 40 50 60 70

0 2 4 6

Y-axis Error

0 10 20 30 40 50 60 70

0 1 2 3

Z-axis Error

time [sec]

(c) 자이로 바이어스 추정 오차 그림 2. 경우 I에서의 시뮬레이션 결과.

Fig. 2. Simulation results in Case I.

0 10 20 30 40 50 60 70

0 0.5 1 1.5 2 2.5 3

Position Estimation Error [m]

Error

time [sec]

UKF CKF Measurement

(a) 위치 추정 오차

0 10 20 30 40 50 60 70

-60 -40 -20 0 20 40 60 80 100

Heading Estimation Error [deg]

Heading Error

time [sec]

UKF CKF

(b) 요각 추정 오차

0 10 20 30 40 50 60 70

0 2 4 6

Gyro Bias Estimation Error [deg/s]

X-axis Error

UKF CKF

0 10 20 30 40 50 60 70

0 1 2 3

Y-axis Error

0 10 20 30 40 50 60 70

0 2 4 6

Z-axis Error

time [sec]

(c) 자이로 바이어스 추정 오차 그림 3. 경우 II에서의 시뮬레이션 결과.

Fig. 3. Simulation results in Case II.

4. 결 론

본 논문에서는 사람 및 로봇의 실내 환경에 서 항법정보를 제공하기 위해 CKF 기반으로 INS와 UWB를 융합한 결합항법시스템을 제안 하였다. 제안된 항법시스템은 저급 IMU를 사 용하는 것을 가정하였으며 이때 초기 요각 추 정 오차는 크게 발생하게 된다. 이 경우 기존의 EKF는 안정적인 해를 제공할 수 없으며 이를 보완한 UKF는 EKF 보다는 안정적이지만 음 의 가중치로 인해 불안한 해를 보이기도 한다.

본 논문에서 사용한 CKF는 큰 초기 요각 추정 오차가 발생하는 상황에서 다른 필터보다는 안 정적인 항법해를 제공할 수 있음을 시뮬레이션 기반으로 보였다.

감사의 글

이 논문은 2015년도 정부(교육부)의 재원으 로 한국연구재단의 지원을 받아 수행된 기초연 구사업임 (NRF2015R1D1A1A01059606).

참고 문헌

[1] S. Y. Cho, “Measurement error observer- based IMM filtering for mobile node localization using WLAN RSSI measure-

ment”, IEEE Sensors Journal, Vol. 16, No. 8, pp. 2489-2499, 2016.

[2] B. Li, Y. Wang, H. K. Lee, A. Dempster, and C. Rizos, “Method for yielding a database of location fingerprints in WLAN”, IEE Proceedings - Communications, Vol.

152, No. 5, pp. 580-586, 2005.

[3] S. Y. Cho, and Y. W. Choi, “Access point-less wireless location method based on peer-to-peer ranging of impulse radio ultra-wideband”, IET-Radar, Sonar and Navigation, Vol. 4, No. 5, pp. 733-743, 2010.

[4] S. Julier, J. Uhlmann, and H. G.

Durrant-Whyte, “A new method for the nonlinear transformation of means and covariances in filters and estimators”, IEEE Trans. Automatic Control, Vol. 45, No. 3, pp. 477-482, 2000.

[5] I. Arasaratnum, and S. Haykin,

“Cubature Kalman filter”, IEEE Trans.

Automatic Control, Vol. 54, No. 6, pp.

185-208, 2002.

[6] J. A. Farrel, and M. Barth, The Global Positioning System and Inertial Navigation, NY: McGraw-Hill, 1999.

[7] www.analogdevice.com [8] http://www.decawave.com