INS/vision Integrated Navigation System in Environments with Insufficient Number of Landmarks

랜드마크가 충분하지 않은 환경에서의 관성/비전 통합항법시스템

Abstract

An INS/vision integrated navigation algorithm is proposed for environments with insufficient number of landmarks. In the proposed algorithm, the raw measurements on the focal plane are directly used in order to cope with the situation where the number of landmarks are not sufficient. In addition to this, the combination of landmarks, which has smallest value of DOP, is used in the update of measurement in order to improve navigation performance. In order to evaluate the performance of the proposed integrated navigation system, Monte-Carlo simulation and van test was performed. The results of the simulation and experiment show that the proposed navigation system gives better navigation performance than an INS/vision integrated navigation system which does not use the raw measurements on the focal plane and the navigation system provides navigation solutions even in environments with insufficient number of landmarks.

1. 서 론

관성항법시스템(INS, Inertial Navigation System)은 외부의 도움 없이 자체적으로 항법해를 연속적으로 계산할 수 있는 장점이 있으므로 신뢰성이 요구되는 시스템에 적합하다. 그러나 비용상의 문제로 중/저급의 관성측정유닛(IMU, Inertial Measurement Unit)를 사용하는 시스템의 경우에는 위치결정(position fixing) 방식의 센서 정보를 함께 이용하는 것이 일반적이며 대표적인 예가 GPS(Global Positioning System)/INS 통합항법시스템이다. 그러나, GPS는 전파방해, 장애물과 같은 외부 간섭에 취약한 단점이 있으며, 빌딩이 많은 도심 지역이나 숲속과 같이 시선각이 확보되지 않은 곳에서는 신뢰성 있는 항법 정보를 제공하지 못한다. 따라서 최근에는 카메라, 레이저, 레이다, 초음파 센서, 고도계 등을 이용하여 GPS를 대체하는 항법들에 대한 연구가 활발히 이루어지고 있다[1-4]. 이중, 카메라에서 측정한 영상을 이용하는 비전항법시스템은 계산이 많고, 조도의 영향을 받는 단점이 있지만, 가볍고, 소모 전력이 적으며 가격이 저렴한 장점 때문에 소형/무인 항공기, 차량, 이동 로봇 등에 많이 사용되고 있다[5-9]. 특히, 최근 디지털 영상기술과 컴퓨터 처리 기술의 발달에 따라 영상을 이용한 실시간 항법이 가능하게 되었다[8-9].

한편, 항법시스템의 정확도는 항체가 있는 장소 혹은 시간에 따라 달라질 수 있다. DOP(Dilution of Precision)는 항법시스템에서 항법오차에 대한 측정오차의 증폭도를 나타내는데, 항체와 대상의 기하학적 위치에 따라 달라진다[10]. 랜드마크 기반의 비전항법에서는 랜드마크와 항체의 위치에 따라 항법 정확도가 달라진다. 2차원의 시선각 측정 기반 비전항법시스템에서 각 랜드마크와 항체의 기하학적 관계가 위치오차와 자세오차에 미치는 영향은 [11]에서 볼 수 있다. TOA(Time of Arrival) 기반의 항법시스템인 GPS는 수신기와 위성의 배치에 따라 항법오차가 달라지며, 이에 대한 영향을 GDOP(Geometric DOP)로 나타낼 수 있다. GPS에서 4개 위성인 경우에 대하여 수신기를 중심으로 사면체를 구성할 때 GDOP는 체적에 반비례하는 것으로 알려져 있다[7]. Dempster는 2차원의 AOA(Angle of Arrival) 항법시스템에서 기지국들과 항체 사이의 위치가 항법오차에 미치는 영향을 분석한 바 있다[12].

카메라의 응용이 다양한 만큼 현재까지 여러 형태의 INS/비전 통합항법이 연구되어 왔다. 두 센서 통합을 위해 주로 칼만필터와 파티컬 필터를 이용하고, 통합항법의 측정값을 생성하기 위하여 비전항법의 속도, 헤딩, 위치와 자세, 김블각, 베어링 정보 등을 이용하였다[4, 13-17]. 또한 스테레오 카메라를 이용한 비주얼 오도메트리 시스템과 INS의 통합방법도 볼 수 있으며[18], 두 개의 연속 영상과 INS 데이터를 이용해 항체의 움직임에 대한 상대적인 변화를 추정하는 방식의 통합항법을 연구하기도 하였다[9].

두 센서의 통합을 위해 카메라의 김블각이나 베어링 정보를 이용하는 경우[16-17]에는 측정값으로부터 데이터 처리과정을 한번 거친 데이터이므로 잡음의 특성에 따라 최적의 해를 제공 못할 수도 있다. 또한 비전항법에서 제공하는 속도, 헤딩 정보를 이용하는 경우[13-15]에는 랜드마크의 수가 충분하지 않은 환경에서는 측정값 갱신이 불가능하여 항법성능의 저하가 발생할 수 있다. 한편, 연속적인 영상을 이용하여 항법을 수행하는 비주얼 오도메트리를 INS와 통합하는 방법은 두 시스템 모두 추측 항법의 방식을 사용하므로 통합의 효과가 제한적이며, 연속적인 영상 사이의 관계를 놓치지 않고 연속적으로 추적하는 것이 매우 중요하다.

본 논문에서는 카메라의 초점면의 랜드마크 측정값을 직접 사용하여 랜드마크가 충분하지 않은 환경에서도 강인한 INS/비전 통합항법시스템을 제안하고자 한다. 제안한 시스템은 두 센서가 서로 상호보완적으로 동작하여 각 센서의 오차를 보상하고 항법성능을 향상시킨다. 또한 통합필터의 측정값 갱신시에 랜드마크 배치에 대한 DOP 값이 최소인 조합의 랜드마크를 이용하여 항법성능을 더욱 개선한다. 논문에서는 카메라의 초점면의 랜드마크 좌표와 INS에서 추정한 값의 차를 이용하고 각 센서의 오차 보정을 위한 통합필터의 모델을 도출하였다. 제안한 시스템의 동작과 항법성능을 검증하기 위해 몬테카를로 시뮬레이션과 차량실험을 수행하였으며 비전항법과 INS의 위치 및 자세 오차를 이용하는 통합항법, 랜드마크 배치에 대한 DOP를 고려하지 않은 경우의 통합항법 결과와 항법성능을 비교, 분석하였다.

논문의 2절에서는 INS/비전 통합항법시스템을 제안하고 3절에서는 시뮬레이션과 실험을 통하여 제안한 알고리즘의 항법성능을 평가한 결과를 기술한다. 4절에서 논문의 결론을 정리하였다.

 

2. INS/비전 통합항법시스템 설계

본 논문에서는 랜드마크의 수가 적은 환경에서도 강인한 특성을 가지며 두 센서가 서로 상호보완적으로 동작하는 INS/비전 통합항법시스템을 제안한다. 제안한 시스템은 INS항법 정보와 카메라의 랜드마크 측정값을 이용하여 항법오차, INS의 센서오차, 카메라의 센서오차를 추정하고 보정하여 항법성능을 개선한다. 또한 카메라 측정값과 INS 출력값을 이용하여 랜드마크 배치에 대한 DOP 계산 후, 낮은 DOP를 갖는 랜드마크 조합을 선택하여 측정값 갱신에 이용함으로써 항법성능을 더욱 개선한다. 그림 1은 제안한 통합 항법시스템의 구조를 보여준다. 통합항법시스템은 관성항법모듈, 비전항법 모듈, 통합 모듈, DOP 계산 모듈, 항법결과 출력모듈로 구성되어 있다.

그림 1INS/비전 통합항법시스템 구조 Fig. 1 Architecture of INS/vision integration navigation system

2.1 관성항법 모듈과 비전항법 모듈

관성항법 모듈은 관성측정유닛(IMU, Inertial Measurement Unit)의 가속도 및 각속도를 이용하여 항체의 자세, 속도 및 위치를 계산하고 통합 모듈이 추정한 항법오차와 센서오차를 수신하여 이를 보정한다. 비전항법 모듈은 영상데이터를 수신하여 영상데이터 처리 과정과 데이터베이스의 랜드마크 정보를 이용하여 랜드마크를 검출한 후 카메라좌표계에서의 랜드마크의 좌표를 통합필터에 제공한다. 또한 통합항법 모듈에서 추정한 오차를 수신하여 카메라의 센서오차를 보정한다.

2.2 통합 모듈

통합 칼만필터는 INS 항법오차, IMU 및 카메라의 센서오차를 추정하고 이를 관성항법 모듈과 비전항법 모듈로 피드백하여 항법오차와 센서오차를 보정하도록 한다. 카메라에서 하나의 랜드마크도 측정하지 못한 경우에는 시간 갱신만 수행한다. 통합 모듈은 두 센서의 통합을 위해 피드백 구조의 항법오차모델을 사용한다.

2.2.1 시스템 모델

INS와 센서 오차의 시스템 모델은 아래 식(1), 식(2)와 같다.

여기서 w(t), Q(t)는 프로세스 잡음과 이의 공분산행렬이다. 식(2)에서 상태변수 벡터중 항법오차 벡터 𝛿𝓍nav와 센서오차 벡터 𝛿𝓍sen는 다음 식(3), 식(4)와 같다.

여기서 𝛿R, 𝛿V와 𝜓는 각각 위치오차, 속도오차, 자세오차를 나타낸다. 그리고 ∇과 𝜖는 가속도계 오차와 자이로 오차이고, 𝛿u, 𝛿v는 수평, 수직축의 카메라 초점면 바이어스 오차, 𝛿ƒ는 카메라의 초점거리 오차이다. 그리고 아래첨자 N,E,D는 항법좌표계에서 북쪽, 동쪽, 수직하강방향을 나타내고 아래첨자 x,y,z는 동체좌표계의 롤축, 피치축, 요축 방향을 나타낸다. 식(2)의 부행렬 F11과 F12는 식(5), (6)과 같다[19].

여기서, 은 항법좌표계에서 표현한 항체의 이동각속도(craft rate) 벡터의 왜대칭 행렬, 는 항법좌표계에서 표현한 지구자전 각속도 벡터의 왜대칭 행렬, 이미지은 항법좌표계에서 표현한 관성좌표계에 대한 항법좌표계의 각속도 벡터의 왜대칭 행렬을 나타내고 ƒn ×은 항법좌표계에서 표현한 비력 벡터(specific force vector)의 왜대칭 행렬이다. 그리고 각축의 대한 가속도계 오차와 자이로 오차, 카메라의 오차를 랜덤상수로 둔다.

2.2.2 측정 모델

랜드마크에 대한 INS의 추정값()과 카메라의 측정값()과 차이를 나타내는 측정방정식은 식(7)과 같다.

여기서 N(0,R(t))는 평균이 0이고 공분산이 R(t)인 가우시안 가우시안(Gaussian) 분포를 나타낸다. 식(7)은 다음 식(8)과 같이 나타낼 수 있다.

칼만필터의 측정값은 카메라 초점면의 랜드마크 좌표와 INS에서 추정한 값의 차이다. 그림 2는 카메라 핀홀 모델에서 랜드마크가 카메라의 초점면으로 투영되는 과정을 보여준다. 그림과 같이 카메라좌표계에서 에 위치하는 랜드마크는 카메라 초점면 상의 좌표 으로 투영되며 식(9), 식(10)과 같이 표현할 수 있다.

그림 2카메라 핀홀 모델 및 랜드마크의 초점면 투영 Fig. 2 Pinhole camera model and landmark projection

여기서 ui, vi는 각각 카메라좌표계에서 y축, z축의 랜드마크 측정값이고, ƒ는 카메라의 초점거리, Xi, Yi, Zi는 카메라 좌표계에서의 i번째 랜드마크의 각축에 대한 위치로 다음과 같다.

여기서 [x y z]T는 항법좌표계에서 표현한 항체의 위치, C11, C12, ···, C33은 항법좌표계에서 카메라좌표계로의 좌표변환 행렬 의 각 요소이고, [xi yi zi]T는 항법좌표계에서 표현한 i번째 랜드마크의 위치이다.

카메라 초점면의 랜드마크의 수평축 측정오차는 식(12)와 같이 도출할 수 있다.

여기서 x*는 선형화 기준값을 나타낸다. 는 i번째 랜드마크의 카메라 초점면에서 랜드마크의 수평축 측정값이고 는 이에 대한 INS의 추정값을 나타내며 데이터베이스로부터의 랜드마크 위치 정보, INS 항법해를 이용하여 계산한다. 𝑣u는 카메라의 수평축 측정잡음이다. 카메라 초점면의 수직축 측정오차도 식(13)과 같이 표현할 수 있다.

여기서, 는 i번째 랜드마크의 카메라 초점면에서 수직축 측정값이고 는 이에 대한 INS의 추정값을 나타낸다. 𝑣v는 카메라의 수직축 측정잡음이다. 식(12)와 식(13)을 이용하여 측정행렬 H를 식(14)와 같이 정리할 수 있으며 부행렬 H1과 H2는 식(15), 식(16)과 같다.

식(15)에서 P= [x y z]T와 𝜓=[α 𝛽 𝛾]T는 각각 항법좌표계의 위치벡터와 자세벡터이다. 식(15)에서 ∂ui/∂P를 식(17)과 같이 계산할 수 있다.

여기서 C11, C12, ···, C33 은 좌표변환 행렬 의 각 요소이다. 또한 ∂ui/∂𝜓는 식(18)과 같다.

여기서 α, 𝛽와 𝛾 는 각각 롤, 피치, 요각을 나타낸다. 식(3)의 상태변수 중에서 자세오차 변수의 경우에 INS 오차모델로부터 좌표계 섭동에 의한 회전벡터 오차 𝜓=[𝜓N 𝜓E 𝜓D]T로 정의한 반면, 식(13), 식(14)의 측정모델에서는 오일러각 오차 𝛿e=[𝛿α 𝛿𝛽 𝛿𝛾]T 로 표현하였으므로 둘 사이의 관계를 고려하여 측정행렬을 구성하여야 한다. 식(15)에서 A는 회전벡터 오차와 오일러각 오차의 관계를 나타내며 식(19)와 같다[20].

식(15)에서 ∂𝑣i/∂P는 식(20)과 같다.

∂𝑣i/∂𝜓는 다음과 같이 계산할 수 있다.

2.3 DOP(Dilution of Precision) 계산 모듈

DOP 계산 모듈은 데이터베이스로부터의 랜드마크 위치정보, 관성항법 모듈로부터 INS에서 제공하는 현재 항체의 항법 정보와 카메라에서 측정한 랜드마크 측정값을 이용하여 DOP 값 계산을 수행하한 후 가장 낮은 DOP를 갖는 랜드마크 조합을 선택하여 통합 모듈에 전달한다. 이때 카메라로부터 입력받은 랜드마크의 측정값 수가 3개 미만인 경우에는 별도의 DOP 계산을 수행하지 않는다.

비전항법에서 DOP는 카메라에서의 랜드마크 측정오차와 항법오차의 관계식으로부터 도출할 수 있다. 그림 3은 랜드마크 기반 비전항법에서 카메라 측정값으로부터 항법해를 결정하는 과정을 보여준다. 항법방정식을 방향각(azimuth)에 관한 식과 앙각(elevation)에 관한 식으로 표현하면 식(22), 식(23)과 같다.

그림 3비전항법에서 3차원 항법해의 결정 Fig. 3 Determination of 3D solution in vision navigation

여기서 [x y z]T와 [α 𝛽 𝛾]T는 항법좌표계에서 항체의 위치벡터와 자세벡터, [ui 𝑣i]T는 카메라좌표계에서 표현한 랜드마크의 초점면 측정벡터이고, ƒ는 카메라의 초점거리, [xi yi zi]T는 항법좌표계에서 표현한 i번째 랜드마크의 위치이다.

항법방정식 식(22)와 식(23)을 섭동시키면 식(24), 식(25)와 같은 식을 얻을 수 있다.

식(24)와 식(25)를 테일러급수를 이용하여 전개하고 정리하면 식(26)과 같이 항법오차와 측정오차의 관계식을 얻을 수 있다.

여기서,𝛿mi = [𝛿ui 𝛿vi]T는 측정오차 벡터, 𝛿x= [𝛿x 𝛿y 𝛿z 𝛿α 𝛿𝛽 𝛿 𝛾]T는 항법오차 벡터를 나타낸다. 식(26)에서 Hi 는 식(27)과 같다.

여기서,S(x), C(x), T(x)는 각각 sin(x), cos(x), tan(x)를 의미하고, 는 항체와 i번째 랜드마크간의 거리를 의미하고 는 항체와 i번째 랜드마크간의 수평각, 는 항체와 i번째 랜드마크간의 수직각을 의미한다. 식(26)에서 측정치가 n개이면, 식(26)과 같은 식이 n개 얻어지며, 이를 행렬로 식(28)과 같이 표현할 수 있다.

식(28)로부터 DOP를 구할 수 있으며 이를 PDOP(Position DOP)와 ADOP(Attitude DOP)로 나누어 표현할 수 있다. 이때, PDOP는 위치오차에 대한 측정오차의 증폭도, ADOP는 자세오차에 대한 증폭도를 나타낸다.

여기서 𝜎m 은 초점면상에서의 측정오차의 표준편차, 𝜎x , 𝜎y, 𝜎z는 각 축에 대한 위치오차의 표준편차, 𝜎α, 𝜎𝛽, 𝜎𝛾는 각 축에 대한 자세오차의 표준편차를 의미한다. PDOP와 ADOP는 식(31), 식(32)와 같다.

여기서 D11, D22, ···, D66은 아래식과 같이 (HTH)-1 행렬의 값이다.

 

3. 시뮬레이션 및 실험

제안한 통합항법시스템의 동작과 항법성능을 평가하고 랜드마크 배치에 대한 항법오차 특성을 살피기 위해 50회의 몬테카를로 시뮬레이션과 차량실험을 수행하였다. 시뮬레이션과 실험에서 순수 관성항법, 비전항법의 항법해, 즉 위치오차와 자세오차를 통합필터의 측정값으로 이용하는 통합항법(통합방법1), DOP 계산을 포함하지 않는 통합항법 알고리즘(통합방법2)과 항법성능을 비교하였다.

3.1 시뮬레이션

시뮬레이션에서 사용한 카메라와 IMU의 사양 및 오차 특성은 표 1, 2와 같다.

표 1시뮬레이션의 카메라 사양 및 오차 Table 1 Camera specifications for simulation

표 2시뮬레이션의 관성센서 사양 및 초기정렬오차 Table 2 IMU specifications and INS initial alignment error for simulation

시뮬레이션에서 그림 4와 같이 일정 고도에서 8자의 궤적을 생성하고, 난수발생기(random number generator) 사용하여 랜드마크를 항체 주변에 배치하였으며, 일부구간은 임의로 랜드마크 환경이 취약하게 하여 비전항법에서 위치, 속도 정보를 제공하지 못함으로써 비전항법해를 이용하는 통합항법에서는 측정값 갱신이 불가능하도록 하였다.

그림 4시뮬레이션에서 항체궤적과 랜드마크 Fig. 4 Vehicle trajectory and landmarks in simulation

그림 5는 3차원 맵에서의 항법결과를 보여준다. 결과로부터 순수 관성항법은 출발이후 점점 오차가 증가하지만 INS/비전 통합항법시스템은 모두 참 궤적(true trajectory)을 잘 추종하는 것을 알 수 있다.

그림 5시뮬레이션에서 항법결과 Fig. 5 Navigation results in simulation

그림 6과 그림 7은 항법시간에 따른 위치오차와 자세오차를 보여준다. 결과로부터 비전항법해를 이용하는 통합항법은 랜드마크 환경이 취약하여 비전항법해를 제공하지 못하는 경우에 갑자기 오차가 증가하는 것을 알 수 있다. 또한 논문에서 제안한 것처럼 DOP 계산을 수행하여 낮은 DOP 값을 갖는 랜드마크 조합을 이용하여 통합항법을 수행할 때 더 좋은 성능을 보임을 알 수 있다. 표 3에 각 항법의 위치, 속도, 자세에 대한 항법결과를 RMS 오차로 나타내었다.

그림 6시뮬레이션에서 위치오차 Fig. 6 Position errors in simulation

그림 7시뮬레이션에서 자세오차 Fig. 7 Attitude errors in simulation

표 3시뮬레이션에서 RMS 항법오차 Table 3 RMS navigation errors in simulation

3.2 차량실험

그림 8은 차량실험 장치를 보여준다. 그림과 같이 광학정렬용 평면판에 조립한 카메라를 넓은 시야각을 갖도록 차량지붕의 프레임에 IMU와 함께 설치하였으며, 제안한 통합항법시스템의 항법성능을 평가하기 위해 기준항법시스템으로 고성능의 INS/CDGPS(Carrier-phase Differential GPS) 통합시스템을 함께 탑재하였다. 실험에서 사용한 카메라는 표 4와 같은 사양을 가지며 최대 30Hz의 프레임레이트를 제공 하는데 실험에서는 1.4Hz로 설정하였으며, IMU는 표 5와 같은 사양을 갖는 MEMS(Micro Electro Mechanical System) IMU를 사용하였다. 실험시 데이터 수집 소프트웨어를 이용하여 기준항법시스템의 데이터와 통합항법의 데이터를 동기화하여 저장하였으며 실험 종료 후, 후처리 방법으로 여러가지 성능을 관찰하였다. 그리고 실험에서는 데이터베이스 구축의 편의를 위해 촬영한 영상데이터를 살펴 먼저 랜드마크를 확인한 후 대축척 지도와 항공 사진 등을 이용하여 데이터베이스를 작성하였다.

그림 8차량실험 셋업 Fig. 8 Experimental setup

표 4차량실험에서 카메라 사양 Table 4 Camera specifications for experiment

표 5차량실헝에서 관성센서 사양 Table 5 IMU specifications for experiment

그림 9는 실험시 차량의 이동궤적을 보여준다.

그림 9차량실험 이동궤적 Fig. 9 Vehicle trajectory in experiment

제안한 통합항법과 비전항법해를 이용하는 통합항법(통합방법1), DOP 계산을 포함하지 않을 때(통합방법2)의 항법결과를 비교하였다. 세 가지의 INS/비전 통합항법시스템 모두 기준 궤적을 추종하는 것을 알 수 있다. 그러나 비전항법해를 이용하는 방식의 경우 랜드마크의 수가 적은 지역에서 들어서자 오차가 커지기 시작하는 것을 알 수 있다. 그림 10과 그림 11은 각 항법 알고리즘에 대한 시간에 따른 위치 오차, 자세오차를 보여주며, 표 6에서 각 알고리즘에 대한 항법결과를 RMS 오차로 나타내었다. 결과에서처럼 제안한 통합항법은 취약한 랜드마크 환경에서도 강인한 특성을 보이며, 또한 DOP 계산을 수행하여 통합필터에서 랜드마크 배치를 고려할 때 더 좋은 성능을 보임을 알 수 있다.

그림 10차량실험에서 위치오차 Fig. 10 Position errors in experiment

그림 11차량실험에서 자세오차 Fig. 11 Attitude errors in experiment

표 6차량실험에서 RMS 항법오차 Table 6 RMS navigation errors in experiment

 

3. 결 론

본 논문에서는 랜드마크의 배치에 대한 3차원 항법오차 특성을 고려하면서 랜드마크가 충분하지 않은 비전항법 환경에서도 강인한 INS/비전 통합항법시스템을 제안하였으며 두 센서가 상호보완하며 카메라 초점면의 랜드마크 측정값을 사용하기 위한 칼만필터의 모델을 도출하였다. 제안한 시스템은 INS 출력값과 카메라의 랜드마크 측정값을 이용하여 항법오차, INS의 센서오차, 카메라의 센서오차를 추정하고 보정하여 항법성능을 개선한다. 또한 시스템의 항법성능을 개선하기 위해 INS 항법 정보와 카메라의 랜드마크 측정값을 이용하여 DOP 계산을 수행하며 낮은 DOP를 갖는 랜드마크 조합을 선택하여 통합필터에서 이를 이용한다. 제안한 통합항법시스템의 동작과 항법성능을 검증하기 위해 시뮬레이션과 실험을 수행하였으며, 비전항법과 INS의 위치 및 자세오차를 이용하는 통합항법, 랜드마크 배치에 대한 DOP를 고려하지 않은 경우의 통합항법 결과와 항법성능을 비교하였다. 시뮬레이션과 실험결과로부터 제안한 통합항법은 두 센서의 단점을 보완하고 랜드마크가 충분하지 않은 비전항법 환경에서도 안정적인 항법해를 제공함을 알 수 있었으며 랜드마크 배치에 대한 항법오차를 고려하여 시스템을 설계할 때 더 높은 항법 정확도를 보임을 알 수 있었다.