3.1 상태변수 및 시스템 모델

확장 칼만 필터를 구성하기 위해, 상태 변수와 시스템 모델을 정의한다. 본 논문의 상태 변수 벡터 x와 모델은 MSCKF⁽¹⁵⁾논문을 참고하였다.

상태변수 x에서 p, v, q는 각각 위치, 속도, 자세 쿼터니언이며 $b _{a}$, $b _{g}$는 가속도 및 각속도 센서의 운항 중 바이어스 성분이다. 여기서 첨자 n, b는 각각 변수가 로컬 레벨 좌표계(local- level frame), 몸통 좌표계에서 표현되었음을 의미하며 $q _{n} ^{b}$는 로컬 레벨 좌표계에서 몸통 좌표계로의 좌표회전을 나타내는 단위 쿼터니언이다. 본 논문에서는 지자기센서나 고성능 자이로 센서를 사용하지 않기 때문에 북쪽 방향을 알 수 없다. 따라서 ENU (East-North-Up) 또는 NED(North-East-Down) 좌표계는 사용할 수 없으며, 수평 축은 x, y로, 중력방향(Down) 축은 z로 표현하였다.

간접 칼만 필터를 구성하기 위한 상태변수 $x$의 오차 $\tilde { \mathrm { x } }$는 추정치 $\hat { \mathrm { x } }$에 대해 $\tilde { \mathrm { x } } = \mathrm { x } - \hat { \mathrm { x } }$를 만족하며, 아래와 같다.

단, $\delta \theta$의 경우 MSCKF⁽¹⁵⁾와 같이 상태변수 쿼터니언 오차에서 벡터 부분을 표현한 것이다. 쿼터니언 오차는 그 오차가 매우 작을 때, 다음과 같이 근사할 수 있다.

따라서 쿼터니언 오차는 쿼터니언의 벡터부분만 사용하여 표현이 가능하며, $\tilde { q } = q \otimes \hat { q } ^ { * }$이다. 여기서 $\otimes$는 쿼터니언 곱, *는 쿼터니언 conjugate이다. 또한, 필터 propagation을 위한 항법 방정식은 다음과 같다.

여기서 $\mathrm { C } ( \mathrm { q } )$는 쿼터니언 q를 DCM으로 변환한 것이며, $\Omega ( \cdot )$는 식(5)와 같이 skew-symmetric 행렬인 $[ \cdot \times ]$를 사용하여 표현되는 행렬이다.

식(4)에서, $\omega _ { \mathrm { ie } } ^ { \mathrm { n } }$는 지구 자전 각속도이다. 만약 잡음(noise)이 큰 저급(low-grade) MEMS센서를 사용한다면 $\omega _ { \mathrm { ie } } ^ { \mathrm { n } }$성분은 무시될 수 있다. 따라서 각속도와 가속도는 각 센서 출력 $a _{m}$, $\omega _{m}$을 통해 다음과 같이 나타낼 수 있다.

위 식들을 기반으로 오차 상태변수를 위한 선형화된 모델은 아래와 같다.

여기서 $n = [{n _{ a } ^ { T }} \quad {n _ { g } ^ { T }} \quad {n _ { wa } ^ { T }} \quad { n _ { Wg } ^ { T }}] ^ { T }$이며 $n _{a}$, $n _{g}$는 각각 가속도 및 자이로의 영평균 백색 가우시안 잡음, $\mathrm { n } _ { \mathrm { wa } } ^ { \mathrm { T } }$, $\mathrm { n } _ { \mathrm { wg } } ^ { \mathrm { T } }$는 가속도 및 자이로의 random walk rate이다.

식(7), 식(8)을 바탕으로, 이산화된 센서 출력에 따른 공분산 갱신은 다음과 같이 수행된다. Q 행렬은 벡터 n의 공분산 행렬로, IMU의 센서 특성에 따라 달라지게 된다.

3.2 측정 모델

3.2.2 Measurement Update with Velocity Vector

앞서 얻은 VO의 자세, 위치 추정치는 기존 약결합 방식과 같이 별도의 수정 없이 측정치로 활용될 수 있다. 그러나 본 논문에서는 자세 추정 성능을 향상시키기 위해, VO로부터 속도를 유도하여 자세를 보정한다. 속도를 활용하여 자세를 보정하는 방법은 이전 논문⁽¹¹⁾과 같으며 다음과 같이 두 속도 벡터로부터 유도된다.

(13)

$$\begin{gathered} \Delta {\mathbf{v}}_{INS,k}={\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}||\sum _{i=t}^{t+\Delta t}{\mathbf{C}}_b^n{\mathbf{a}}^{b}dt|| \\ \Delta {\mathbf{v}}_{Ref,k}={\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}||\sum _{i=t}^{t+\Delta t}{\mathbf{v}}_{Ref,i}^n+g^n\Delta t|| \end{gathered}$$

여기서 dt는 unit time step, 벡터 $\Delta \mathrm { V } _ { \in \mathrm { S } , \mathrm { k } }$는 IMU를 통해 얻는 비력(specific force)의 적분이며, $\Delta \mathrm { V } _ { \mathrm { R } \mathrm { ef } , \mathrm { k } }$는 GNSS와 같이 외부 센서를 통해 얻은 속도 증분과 중력의 조합이다. 오차가 없는 이상적인 경우, 두 벡터는 일치하게 되지만 IMU적분 과정에서 생기는 자세 오차가 두 벡터간 차이를 만들게 된다. 여기서 일정한 구간 $\Delta t$동안 적분되는 자세 오차의 주 원인은 자이로 바이어스(gyro bias)라고 간주할 수 있다. 일반적으로 자이로 바이어스의 경우 크기가 작고 긴 시간에 걸쳐 변하는 저 대역폭 신호이기 때문에, $\Delta t$동안 일정하며 그 크기는 작다고 가정하였다. 이러한 가정 하에서, 가속도 변화가 작을 경우 두 벡터는 그림. 1과 같이 한 평면상에 위치할 수 있다.

그림. 1. $\Delta \vec { V } _ { R e f , k } ^ { n }$와 $\Delta \vec { V } _ { \in S , k } ^ { n }$의 관계⁽¹¹⁾

Fig. 1. The relation between $\Delta \vec { V } _ { R e f , k } ^ { n }$ and $\Delta \vec { V } _ { \in S , k } ^ { n }$ ⁽¹¹⁾

위 그림에서 dt는 unit time step이며 $\triangle \mathrm { t } = \mathrm { N } \cdot \mathrm { dt }$이다. $\Delta \mathrm { V } _ { \mathrm { Ref } } ^ { \mathrm { n } }$방향을 x축으로 하는 2D 평면에서 표현된 $\Delta \mathrm { V } _ { \in \mathrm { S } } ^ { \mathrm { n } }$를 $\Delta \mathrm { V } _ { \in \mathrm { S, 2D } } ^ { \mathrm { n } }$라고 하면, 해당 벡터는 미소각 근사(small angle assumption)을 사용하여 다음과 같이 정리될 수 있다⁽¹¹⁾.

(14)

$$\begin{gathered} \Delta {\mathbf{v}}_{MS,2D}=\sum _{i=t}^{t+A}\left(||{\mathbf{a}}_{2D,(t+i,dt)}||\left[\begin{array}{c}\cos \left({\epsilon }_g(i·dt)\right)\\ \sin \left({\epsilon }_g(i·dt)\right)\end{array}\right]\right) \\ \cong ||{\mathbf{a}}_{i,2D}^b||dt·\left[\begin{array}{c}N\\ \frac{N(N+1)}{2}{\epsilon }_{g}dt\end{array}\right] \end{gathered}$$

식(14)에서 $\Delta \mathrm { V } _ { \in \mathrm { S, 2D } } ^ { \mathrm { n } }$와 $\Delta \mathrm { V } _ { \mathrm { Rsf, 2D } } ^ { \mathrm { n } }$의 사잇각을 구하면

(15)

$\angle \left(\Delta {\mathbf{v}}_{INS,2D},\Delta {\mathbf{v}}_{Ref,2D}\right)\cong {\epsilon }_g\frac{N+1}{2}dt$

여기서 $N d t = \Delta t$이고, 일반적으로 IMU 출력 주기가 고주기임을 고려할 때, (15)는 다음과 같이 근사될 수 있다.

(16)

$\begin{array}{l}{\epsilon }_g\frac{N+1}{2}dt\cong \frac{{\epsilon }_g\Delta t}{2}\\ \angle \left(\Delta {\mathbf{v}}_{MS,2D},\Delta {\mathbf{v}}_{Rf,2D}\right)\cong \frac{{\epsilon }_g\Delta t}{2}\end{array}$

$\epsilon _ { g }$가 충분히 작다고 가정하면 $\Delta t$동안 발생한 자세 오차는 $\epsilon _ { g } \Delta t$로 근사할 수 있으므로, 식(16)은 속도 증분 벡터와 자세 오차 간 관계식이 된다.

위 전개 과정에서, 자이로 오차 $\epsilon _ { g }$가 $a _ { i }$와 평행한 성분을 포함할 수 있다. 이 경우 해당 성분은 두 속도 증분 벡터 간 사잇각에 영향을 주지 못하기 때문에, 적절히 보정되지 못한다. 그러나 가속도 $a _ { i } $의 방향은 일반적으로 $\Delta t$보다 큰 주기에서 끊임없이 변하므로, $\Delta t$보다 긴 시간에 걸쳐 보정 가능함을 알 수 있다.

적분 주기 $\Delta t$의 경우, 적절한 크기를 선정하는 것이 중요하다. 적분 주기가 길면 백색 잡음(white noise)과 같은 고주파 잡음 성분에 대한 영향을 적에 받을 수 있으나, 자이로 바이어스에 의한 오차 역시 증가하여 위 가정을 해칠 수 있다. 반면 구간이 짧은 경우 적분되는 오차는 작을 수 있지만, 백색 잡음이 충분히 억제되지 않아 정확도를 저해할 수 있다. 또한 적절한 적분 구간 $\Delta t$에 의해 항법 출력 주기가 낮아지는 것을 보완하기 위해, [11]⁽¹¹⁾과 같이 슬라이딩 윈도우(sliding window)기법을 사용하여 연속적으로 보정될 수 있게 하였다.

이 방법을 적용하기 위해 (13)의 속도 증분 벡터를 VO에서 얻어진 위치를 차분하여 구성하였다. 속도 증분 벡터는 모두 n-frame(local-level frame)에서 적분하므로 다음과 같이 위치 차분 벡터를 n-frame에서 표현한다.

(17)

${\mathbf{v}}_{VIS}^n=C_w^n{C}_c^w{\dot{\mathbf{p}}}_{wc}^c$

$p _ { w c } ^ { c }$, $C _ { c } ^ { w }$는 각각 VO의 pose 추정 결과값이며, $C _ { w } ^ { n }$은 월드 좌표계에서 local-level frame으로의 변환 DCM이다. 위 식에서 얻어진 $\mathbf { V } _ { \mathrm { VIS } } ^ { \mathrm { n } }$벡터와 IMU로부터 적분구간 T동안의 증분 벡터를 다음과 같이 만들 수 있다.

(18)

$$\begin{gathered} \Delta {\mathbf{v}}_{MS,k}=C_{\mathrm{n}}^b||\sum _{i=k-\Delta t}^k{\mathbf{v}}_{INS,i}^n|| \\ \Delta {\mathbf{v}}_{VIS,k}=C_{\mathrm{n}}^b||\sum _{i=k-\Delta t}^k{\mathbf{v}}_{VIS,i}^n+{\mathbf{g}}^n\Delta t|| \end{gathered}$$

식(16)로부터 자세 오차 쿼터니언과 속도 증분 벡터와 관계는 다음과 같다.

(19)

$\tilde{\mathbf{q}}\approx \left[\begin{array}{c}\Delta {\mathbf{v}}_{INS}·\Delta {\mathbf{v}}_{GPS}\\ \Delta {\mathbf{v}}_{MS}\times \Delta {\mathbf{v}}_{GPS}\end{array}\right]\approx \left[\begin{array}{c}1\\ \frac{1}{2}\left(2\tilde{\mathit{\theta }}\right)\end{array}\right]$

따라서 위 식(19)로부터 칼만 필터의 측정치 $\widetilde { z } _ { k }$를 다음과 같이 구할 수 있다.

(20)

$\tilde{\mathbf{z}}={\left[\begin{array}{cc}{\stackrel{\mathbf{~}}{\mathbf{z}}}_p^{\mathrm{T}}& {\tilde{\mathbf{z}}}_{\theta }^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$

(21)

$$\begin{gathered} {\tilde{\mathbf{z}}}_p={\mathbf{p}}_{VIS}-{\mathbf{p}}_{INS} \\ {\tilde{\mathbf{z}}}_{\theta }=\Delta {\mathbf{v}}_{INS}\times \Delta {\mathbf{v}}_{GPS} \end{gathered}$$

관측행렬 H는

(22)

$\mathrm{H}=\left[\begin{array}{c}\overline{)\begin{array}{ccc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}}{\mathbf{0}}_{6\times 6}\end{array}\right]$

이후 상태변수 및 공분산 보정은 다음과 같이 수행된다.

(23)

$\begin{array}{l}{K}_k={\mathrm{P}}_k^{-}{\mathrm{H}}^{\mathrm{T}}{\left({\mathrm{HP}}_k^{-}{\mathrm{H}}^{\mathrm{T}}+\mathrm{R}\right)}^{-1}\\ {\tilde{\mathbf{x}}}_k={\tilde{\mathbf{x}}}_k^{-}+K\left({\tilde{\mathbf{z}}}_k-\mathrm{H}{\tilde{\mathbf{x}}}_k^{-}\right)\\ {\mathrm{P}}_k=\left(\mathrm{I}-{\mathrm{K}}_k\mathrm{H}\right){\mathrm{P}}_k^{-}\end{array}$

정지 상태이거나 속도가 0에 가까울 경우엔 통상적인 약결합과 같이 자세와 위치를 직접 결합한다. 따라서 식(21)에서의 $\tilde { z } _ { \theta }$는 INS 적분으로부터 얻은 $q _ { \in S}$와 VO에서 얻은 $q _ {VIS}$로부터 $\tilde { z } _ { \theta } = \theta _ { L C }$이다.

(24)

$\delta {\mathit{q}}_{LC}={\mathit{q}}_{INS}\otimes {\mathit{q}}_{VIS}\simeq {\left[\begin{array}{cc}1& \frac{1}{2}\delta {\mathit{\theta }}_{LC}^T\end{array}\right]}^T$

4.1 EuRoC Dataset

알고리즘의 성능을 검증하기 위해, 오픈 데이터 셋인 EuRoC MAV Dataset⁽¹³⁾을 사용하였다. EuRoC 데이터 셋은 영상 또는 영상/관성 복합항법을 위한 데이터 셋으로, 스테레오 카메라와 관성센서를 탑재한 MAV(Micro Aerial Vehicle)의 비행 데이터를 포함하고 있다. 관성센서는 ADIS16448로 200Hz의 출력 주기를 갖고, 영상센서 출력 주기는 20Hz이다. 또한 MAV의 움직임과 조도, 텍스처 등에 따라 easy, medium, difficult로 세분화되어 있다.

먼저 제안한 알고리즘과 약결합 방식 (24)을 비교하기 위해, Vicon room sequence에 대해 위치 및 자세의 RMSE(Root Mean Square Error)를 계산하여 표 1에 나타내었다. 여기서 V201, V202셋의 경우, MAV의 착륙시 움직임이 급격하고 착륙 지점 주변의 특징점이 풍부하지 않으므로, 착륙 직전까지의 데이터를 사용하였다. 자세 RMSE의 경우 롤, 피치, 요 오차의 크기(2-norm)를 이용하여 계산하였다. 또한 RMSE는 알고리즘을 5회 수행한 뒤 평균값을 사용하였다.

표 1을 통해 비교해 본 결과 전반적으로 위치 및 자세 추정성능이 향상되었으며, 자세의 경우 대체적으로 2도 이하의 오차를 보였다. 단, V203의 경우 심한 모션 블러로 인해 제안한 알고리즘과 LC 모두 트래킹(tracking)에 실패하였다. 반면 V103의 경우, 그림. 2와 같이 상대적으로 급격한 포즈 변화가 있음에도 다른 셋과 유사한 오차 수준을 보임을 알 수 있다. 이를 통해, 제안된 측정치 기반 보정 알고리즘에서 적용한 가정이 급격한 움직임에도 어느 정도 강인함을 보임을 알 수 있다. 그림. 3에는 V103의 자세 추정 오차를 상세히 나타내어 LC알고리즘과 비교하였고, 그림. 4에서는 위치 오차를 비교하였다. 그림. 3에서 평균 roll, pitch, yaw각 오차는 각각 LC가 1.07, 1.02, 0.92도이며 제안된 알고리즘이 0.70, 0.52, 0.30도를 보였다.

그림. 2. V103 오일러 자세각 추정 결과

Fig. 2. Ground Truth and estimated result of Euler angles of V103

그림. 3. 제안된 알고리즘과 약결합 기반 알고리즘의 V103 자세 오차 비교

Fig. 3. Attitude error of proposed and LC algorithm of V103

그림. 4. 제안된 알고리즘과 약결합 기반 알고리즘의 V103 위치 오차 비교

Fig. 4. Position error of proposed and LC algorithm of V103

제안된 알고리즘의 성능을 타 알고리즘과 비교하기 위해 ATE(Absolute Translational Error)를 계산하여 표 2에 나타내었다. ATE는 참 궤적과 추정된 궤적 사이의 오차가 최소가 되도록 궤적을 회전시킨 후 오차를 구하는 방법이다⁽²⁸⁾. 계산된 ATE RMSE 결과는 LIBVISO2 (Library for visual odometry 2)⁽²⁹⁾, PIVO (Probabilistic inertial-visual odometry)⁽³⁰⁾, S-PTAM (Stereo parallel tracking and mapping), VI-ORB SLAM (Visual-inertial ORB SLAM)⁽³¹⁾과 비교하였다. 알고리즘 별 ATE결과는 각 논문을 참고하였으며 LIBVISO2와 S-PTAM은 [33]⁽³³⁾에서 발췌하였다.

LIBVISO2는 스테레오 기반 VO로, 관성정보는 활용하지 않는 알고리즘이다. V102와 V103에서는 LIBVISO2가 더 나은 성능을 보이지만, 그 외에서는 본 알고리즘의 결과가 더 정확함을 알 수 있다. 특히 V201, V202에서 큰 차이를 보인다. V203의 경우 MAV의 급격한 움직임으로 인해 두 알고리즘 모두 추정에 실패하였다. PIVO는 MSCKF와 유사한 구조를 갖는 EKF기반 영상/관성 결합 오도메트리로 본 알고리즘과 비교하기에 가장 적합하지만, PIVO는 단안 카메라만을 사용한다는 차이점이 있다. 비교 결과 V201~203에서는 PIVO가 보다 정확한 결과를 보이지만, V101~103에서는 본 알고리즘 결과가 더 정확함을 알 수 있다. 두 알고리즘 모두 항법에 실패하지 않은 V101, V103~V202의 RMSE 평균을 비교하면, 본 알고리즘이 0.36, PIVO가 0.47로 본 알고리즘의 오차가 더 적음을 알 수 있다. S-PTAM은 Stereo 카메라 기반 SLAM으로 관성센서는 사용되지 않았다. VI-ORB SLAM은 최적화 기반 영상/관성 ORB-SLAM으로, EuRoC 키프레임을 관리하고 지도를 활용하는 것에 뛰어난 SLAM 알고리즘이다. 표에 나타낸 결과는 단안 카메라를 이용한 결과이다. 위 데이터 셋은 특징점이 풍부하고 MAV가 좁은 공간에서 움직이므로, 지도를 활용함으로써 얻는 이득이 매우 크다. 때문에 VI-ORB SLAM이 거의 모든 셋에서 더 나은 성능을 보인다.

4.2 실외 실험

더욱 다양한 환경에서 성능 평가를 수행하기 위해 영상/관성 센서 모듈을 구성하여 실외 실험을 수행하였다. 사용된 관성센서는 Analog Device의 ADIS16448로 표 3과 같은 성능을 갖추었으며 출력 주기는 100Hz이다. 또한 특징점의 3차원 좌표를 얻기 위한 깊이(depth) 정보는 RGB-D 카메라를 통해 획득하였다. 사용한 RGB-D 카메라는 Intel사의 RealSense R200이며, 영상정보의 해상도는 640x480, 주기는 30Hz로 설정하였다. 관성센서와 RGB-D 카메라는 그림. 5의 오른쪽과 같이 거치대를 제작하여 견고하게 장착하였고, 카메라 센서의 Z축과 관성센서의 X축이 일치되도록 하였다.

그림. 5. 항법 모듈 및 실험 장비

Fig. 5. Navigation module and experimental setup

기준항법 시스템은 KVH사의 KVH 1750 IMU를 사용하여 IMU/GNSS 결합항법을 구성하였다. KVH 1750은 고정밀 FOG (Fiber optic gyro)를 포함하여 매우 정밀한 6자유도 항법 결과를 제공한다. 본 논문에서 제안한 결합항법 알고리즘은 Intel NUC 5i7RYH에서 구동되었으며 필터 출력은 영상 입력 주기와 같은 30Hz이다. CPU는 i7-5557U(쿼드 코어 @3.1GHz), RAM 용량은 8 GB 이다. 구성한 항법모듈을 그림. 5의 좌측과 같이 카트에 장착하여 실험을 진행하였다.

실외 실험은 그림. 5과 같이 반복적인 패턴을 갖는 격자 타일 위에서 수행되었으며, 총 거리는 약 125m이다. 실험 시간은 정지구간을 포함하여 약 225초이며, 실외 실험의 궤적과 위치 추정 결과는 그림7과 같다. 또한 본 실험에서는 격자 타일 바닥으로부터 항법 모듈로 심한 진동이 가해진다. 강한 진동으로부터 발생되는 잡음은 관성센서와 영상센서의 추정 정확도를 저해하는 요인이 되며, 속도로부터 자세 측정치를 얻는 본 알고리즘에 보다 도전적인 요인이 될 것으로 예상된다.

그림. 6. 야외 실험 환경

Fig. 6. Outdoor experimental environments

그림. 7은 수평 및 수직 위치 추정 결과로 기준항법 시스템을 통해 얻은 참 궤적은 검은색, 기존 약결합 결과는 파란색, 제안하는 알고리즘은 마젠타색으로 표현하였다. 그림. 8은 위치 추정 오차를 나타낸 것이다. 위치 추정 결과, 제안한 알고리즘이 수직 위치에서 보다 정확한 추정 결과를 보이는 것을 알 수 있다. 초반부의 정지 구간을 제외한 위치 RMSE(Root Mean Square Error)의 5회 평균값은 LC가 2.89 m, 제안한 알고리즘이 2.07 m이다.

그림. 7. 수평 및 수직 위치 추정 결과

Fig. 7. Estimation results of horizontal and vertical position

그림. 8. 위치 추정 오차 비교

Fig. 8. Estimation error of position

그림. 9의 자세 추정 오차 결과 또한 본 알고리즘이 약결합 방식보다 정확한 추정 성능을 보임을 알 수 있다. 초반부의 정지상태 구간을 제외한 자세 오차의 5회 평균값은 LC가 2.67°(롤), 3.02°(피치), 2.19°(요)이며 결합항법의 경우 1.52°, 1.60°, 2.18° 이다.

그림. 9. 자세 추정 오차 비교

Fig. 9. Estimation error of attitude