10.69 가속도계-자이로스코프 상보 필터

1. 상보 필터의 개념

상보 필터(complementary filter)는 서로 다른 주파수 특성을 가진 두 센서의 측정값을 결합하여 단일 추정값을 산출하는 신호 처리 기법이다. 가속도계와 자이로스코프의 자세 추정에서, 자이로스코프는 고주파 영역에서 신뢰할 수 있는 각속도 정보를 제공하지만 저주파 영역에서 바이어스 표류가 누적된다. 가속도계는 정적 상태에서 중력 방향을 이용한 절대 자세 정보를 제공하지만 고주파 진동과 동적 가속도에 취약하다. 상보 필터는 두 센서의 상호 보완적 특성을 결합하여 전 주파수 대역에서 신뢰성 있는 자세 추정을 가능하게 한다.

2. 가속도계와 자이로스코프의 주파수 특성

2.1 자이로스코프

자이로스코프는 본체 좌표계의 각속도 \boldsymbol{\omega}를 직접 측정한다. 각속도를 적분하여 자세를 얻으므로, 짧은 시간 간격에서는 정확하지만 긴 시간 동안의 적분은 바이어스로 인한 표류 오차가 누적된다. 결과적으로 자이로 기반 자세 추정은 고역 통과(high-pass) 특성을 가진다.

2.2 가속도계

가속도계는 본체 좌표계에서의 비중력 가속도와 중력 가속도의 합을 측정한다. 정적 또는 준정적 상태에서는 중력 벡터의 방향이 측정되어 절대 자세(피치와 롤)를 제공한다. 그러나 동적 운동 중에는 가속도가 중력에 더해져 측정에 오차가 발생하며, 진동에도 민감하다. 결과적으로 가속도계 기반 자세 추정은 저역 통과(low-pass) 특성에 적합하다.

3. 상보 필터의 기본 구조

상보 필터의 핵심 아이디어는 가속도계 자세 추정에 저역 통과 필터를, 자이로스코프 적분 자세에 고역 통과 필터를 적용하여 두 결과를 합산하는 것이다. 두 필터의 전달 함수는 다음의 보완 조건을 만족해야 한다.

H_{\text{LP}}(s) + H_{\text{HP}}(s) = 1

가장 단순한 1차 형태에서는 다음과 같은 전달 함수를 사용한다.

H_{\text{LP}}(s) = \frac{1}{\tau s + 1}, \quad H_{\text{HP}}(s) = \frac{\tau s}{\tau s + 1}

여기서 \tau는 시상수이다. 두 함수의 합이 1이므로 보완 관계가 성립한다.

4. 차 상보 필터의 시간 영역 표현

이산 시간 형태로 1차 상보 필터의 자세 갱신 식은 다음과 같이 표현된다.

\hat{\theta}_{k+1} = \alpha(\hat{\theta}_k + \omega_k \Delta t) + (1 - \alpha)\theta_{\text{acc},k}

여기서

• \hat{\theta}_k: 시점 k에서의 자세 추정값
• \omega_k: 자이로스코프로 측정된 각속도
• \theta_{\text{acc},k}: 가속도계로부터 계산된 자세
• \alpha \in (0, 1): 가중치 계수

가중치 \alpha는 시상수 \tau와 샘플 주기 \Delta t로부터 다음과 같이 결정된다.

\alpha = \frac{\tau}{\tau + \Delta t}

\alpha가 1에 가까울수록 자이로스코프의 가중치가 커지고, 0에 가까울수록 가속도계의 가중치가 커진다. 일반적으로 \alpha는 0.95에서 0.99 사이의 값으로 설정되어 단기간에는 자이로 적분에 의존하고 장기간에는 가속도계 측정에 의해 보정되는 구조를 만든다.

5. 가속도계 기반 자세 계산

가속도계 측정 벡터를 \mathbf{a} = [a_x, a_y, a_z]^T라 할 때, 정적 상태에서의 피치 \theta와 롤 \phi는 다음과 같이 계산된다.

\phi_{\text{acc}} = \arctan\!\left(\frac{a_y}{a_z}\right)

\theta_{\text{acc}} = \arctan\!\left(\frac{-a_x}{\sqrt{a_y^2 + a_z^2}}\right)

이 두 각도가 가속도계로부터 직접 추정되는 절대 자세이다. 다만 요(yaw) 각은 가속도계만으로 결정되지 않으며, 지자기계가 추가로 필요하다.

6. 차원 상보 필터

6.1 SO(3) 위에서의 상보 필터

3차원 회전을 다루는 상보 필터에서는 회전 행렬 \mathbf{R} \in SO(3) 또는 단위 쿼터니언 \mathbf{q}가 상태 변수가 된다. 자이로스코프와 가속도계의 측정값을 결합하기 위해서는 회전 다양체 위에서의 적절한 갱신 규칙이 필요하다.

6.2 회전 행렬 형태

가속도계로부터의 추정 회전을 \mathbf{R}_{\text{acc}}, 자이로 적분 회전을 \mathbf{R}_{\text{gyro}}라 할 때, 상보 필터의 갱신은 다음의 형태로 일반화된다.

\hat{\mathbf{R}}_{k+1} = \mathbf{R}_{\text{gyro}}^\alpha \cdot \mathbf{R}_{\text{acc}}^{1-\alpha}

이러한 거듭제곱은 회전 다양체 상에서의 보간으로 이해되며, 직접 계산하기보다는 회전 벡터 공간에서 가중 합산 후 지수 사상으로 복원하는 방식이 일반적이다.

6.3 쿼터니언 형태

쿼터니언으로 표현된 상보 필터의 갱신 식은 SLERP(Spherical Linear Interpolation)을 사용하여 다음과 같이 표현된다.

\hat{\mathbf{q}}_{k+1} = \text{SLERP}(\mathbf{q}_{\text{gyro}}, \mathbf{q}_{\text{acc}}, 1 - \alpha)

여기서 \mathbf{q}_{\text{gyro}}는 자이로 적분으로 갱신된 쿼터니언, \mathbf{q}_{\text{acc}}는 가속도계로부터 계산된 쿼터니언이다.

7. 상보 필터의 주파수 영역 해석

상보 필터의 주파수 응답은 두 센서의 신호가 어떻게 결합되는지를 명확히 보여준다. 자이로스코프로부터의 자세 신호는 고역 통과되어 단기 변동만 통과시키고, 가속도계로부터의 자세 신호는 저역 통과되어 장기 평균만 통과시킨다. 두 결과의 합이 전 주파수 대역에서 일관된 추정값을 형성한다.

차단 주파수 \omega_c = 1/\tau는 두 센서의 영향이 균등해지는 주파수이다. 이 주파수보다 높은 영역에서는 자이로의 정보가 우세하며, 낮은 영역에서는 가속도계의 정보가 우세하다. 차단 주파수의 선택은 응용 분야의 동특성에 의존한다.

8. 마호니 필터: 비선형 상보 필터

8.1 명시적 상보 필터의 한계

선형 상보 필터를 3차원 회전에 직접 적용하면 회전 다양체의 비선형성으로 인해 정확한 결과를 얻기 어렵다. 마호니(Mahony) 등은 SO(3) 위에서 동작하는 비선형 상보 필터를 제안하였으며, 이는 가속도계와 자이로스코프 융합의 표준 방법 중 하나이다.

8.2 마호니 필터의 구조

본체 좌표계에서 측정된 단위 가속도 벡터를 \hat{\mathbf{v}}_{\text{meas}}, 추정 자세 \hat{\mathbf{R}}로부터 예측된 중력 방향을 \hat{\mathbf{v}}_{\text{pred}} = \hat{\mathbf{R}}^T\mathbf{e}_3라 할 때, 자세 오차는 두 단위 벡터의 외적으로 정의된다.

\boldsymbol{e} = \hat{\mathbf{v}}_{\text{meas}} \times \hat{\mathbf{v}}_{\text{pred}}

이 오차 벡터를 사용하여 자이로 측정에 비례 및 적분 보정을 가한다.

\hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_{\text{meas}} - \hat{\mathbf{b}}_g + k_P\boldsymbol{e} + k_I\!\int\!\boldsymbol{e}\,dt

비례 항은 즉각적인 자세 보정을, 적분 항은 자이로 바이어스 보정을 담당한다. 갱신된 각속도가 자세 적분에 사용된다.

\dot{\hat{\mathbf{R}}} = \hat{\mathbf{R}}[\hat{\boldsymbol{\omega}}]_\times

이 구조는 본질적으로 상보 필터이며, 비례 이득 k_P가 가속도계의 가중치를, 자이로 적분이 고주파 자세 정보를 제공한다.

9. 상보 필터의 장점과 한계

9.1 장점

• 계산량이 매우 적어 임베디드 시스템에 적합하다.
• 칼만 필터와 달리 공분산 행렬을 유지할 필요가 없다.
• 매개변수가 적어 튜닝이 용이하다.
• 동적 가속도가 작은 환경에서 안정적인 성능을 보인다.
• 직관적인 주파수 영역 해석이 가능하다.

9.2 한계

• 동적 가속도가 큰 상황에서는 가속도계 측정이 중력 방향을 정확히 반영하지 못해 자세 오차가 증가한다.
• 통계적 최적성이 보장되지 않는다(칼만 필터와 달리).
• 차단 주파수의 선택이 휴리스틱에 의존한다.
• 비등방성 잡음을 적절히 처리하지 못한다.
• 요(yaw) 각 추정에는 별도의 지자기계 또는 외부 정보가 필요하다.

10. 동적 가속도에 대한 처리

동적 운동 중에는 가속도계가 측정한 가속도 벡터의 크기가 중력 가속도 g \approx 9.81 \mathrm{m/s^2}에서 벗어난다. 이 경우 가속도계로부터 추정한 자세는 신뢰할 수 없다. 이를 처리하기 위한 방법은 다음과 같다.

10.1 가속도 크기 기반 가중치 조정

측정된 가속도 벡터의 크기와 중력 가속도의 차이가 임계값을 초과하면 가속도계의 가중치를 줄인다.

\alpha_{\text{eff}} = \begin{cases} \alpha & \text{if } |\lVert\mathbf{a}\rVert - g| < \epsilon \\ 1 & \text{otherwise} \end{cases}

이 적응형 가중치 조정은 동적 운동 중에는 사실상 자이로 적분만을 사용하도록 만든다.

10.2 대역 통과 필터링

가속도계 측정에 대역 통과 필터를 적용하여 동적 가속도 성분을 제거하는 방법도 있다. 그러나 이 방법은 응답 지연을 초래한다.

11. 응용 분야

11.1 무인 항공기

소형 드론의 자세 추정에서 상보 필터는 가장 널리 사용되는 알고리즘 중 하나이다. 마호니 필터와 마드윅 필터가 PX4, ArduPilot 등 오픈소스 비행 제어 시스템의 표준 구성요소이다.

11.2 모바일 기기

스마트폰과 태블릿의 화면 회전 감지 및 게임 제어에 상보 필터가 사용된다. 안드로이드와 iOS의 자세 센서 API가 내부적으로 유사한 알고리즘을 사용한다.

11.3 웨어러블 기기

스마트 워치와 활동 추적기에서 사용자의 동작과 자세를 추정하는 데 사용된다. 저전력 동작이 요구되므로 상보 필터의 낮은 계산 부하가 적합하다.

11.4 보행 로봇

이족 및 사족 보행 로봇의 본체 자세 추정에서 상보 필터가 활용된다. 보행 중의 충격으로 인한 가속도계 잡음이 크므로 적응형 가중치 조정이 중요하다.

11.5 가상 현실 머리 추적

저가형 VR 기기의 머리 추적 시스템에서 상보 필터가 사용된다. 다만 고급 VR 시스템에서는 시각 추적과 결합된 더 정교한 알고리즘이 사용된다.

12. 상보 필터와 칼만 필터의 비교

13. 참고 문헌

• Mahony, R., Hamel, T., & Pflimlin, J. M. (2008). “Nonlinear Complementary Filters on the Special Orthogonal Group.” IEEE Transactions on Automatic Control, 53(5), 1203–1218.
• Higgins, W. T. (1975). “A Comparison of Complementary and Kalman Filtering.” IEEE Transactions on Aerospace and Electronic Systems, AES-11(3), 321–325.
• Brown, R. G., & Hwang, P. Y. C. (2012). Introduction to Random Signals and Applied Kalman Filtering (4th ed.). Wiley.
• Madgwick, S. O. H. (2010). “An efficient orientation filter for inertial and inertial/magnetic sensor arrays.” Technical Report, University of Bristol.
• Euston, M., Coote, P., Mahony, R., Kim, J., & Hamel, T. (2008). “A complementary filter for attitude estimation of a fixed-wing UAV.” IEEE/RSJ International Conference on Intelligent Robots and Systems, 340–345.

version: 1.0