10.68 자이로스코프 바이어스 추정과 쿼터니언 갱신

1. 자이로스코프 바이어스의 개념

자이로스코프는 각속도를 측정하는 관성 센서이지만, 실제 측정값에는 참값 외에 시변 오차 성분이 포함된다. 이 오차의 주된 구성 요소가 바이어스(bias)이다. 바이어스는 자이로스코프가 정지 상태에 있을 때에도 0이 아닌 출력값으로 나타나며, 온도, 시간, 기계적 응력 등에 의해 천천히 변동한다. 바이어스를 추정하지 않고 그대로 적분하면 자세 추정에 누적 오차가 발생하여 시간이 지남에 따라 추정 정확도가 급격히 저하된다.

2. 자이로스코프 측정 모형

저가형 MEMS 자이로스코프의 일반적 측정 모형은 다음과 같이 표현된다.

$\boldsymbol{\omega}_{\text{meas}} = \boldsymbol{\omega}_{\text{true}} + \mathbf{b}_g + \mathbf{n}_g$

여기서

$\boldsymbol{\omega}_{\text{meas}} \in \mathbb{R}^3$ : 자이로스코프의 측정 각속도
$\boldsymbol{\omega}_{\text{true}} \in \mathbb{R}^3$ : 참 각속도
$\mathbf{b}_g \in \mathbb{R}^3$ : 자이로스코프 바이어스
$\mathbf{n}_g \in \mathbb{R}^3$ : 백색 가우시안 측정 잡음

바이어스 $\mathbf{b}_g$ 는 시간에 따라 천천히 변하며, 일반적으로 임의 보행(random walk) 모형으로 기술된다.

$\dot{\mathbf{b}}_g = \mathbf{n}_b$

여기서 $\mathbf{n}_b$ 는 바이어스 구동 백색 잡음이다.

3. 바이어스 보정된 각속도

자세 추정 과정에서는 측정값으로부터 바이어스 추정값을 차감하여 보정된 각속도를 얻는다.

$\hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_{\text{meas}} - \hat{\mathbf{b}}_g$

이 보정된 각속도가 쿼터니언 갱신의 입력으로 사용된다. $\hat{\mathbf{b}}_g$ 는 추정 알고리즘에 의해 실시간으로 갱신되는 바이어스 추정값이다.

4. 쿼터니언 운동학과 갱신

4.1 연속 시간 운동학 방정식

단위 쿼터니언 $\mathbf{q} = [q_w, q_x, q_y, q_z]^T$ 의 시간 미분은 본체 좌표계의 각속도 $\boldsymbol{\omega}$ 와 다음의 관계를 가진다.

$\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \boldsymbol{\omega}_{\text{quat}}$

여기서 $\boldsymbol{\omega}_{\text{quat}} = [0, \omega_x, \omega_y, \omega_z]^T$ 는 각속도 벡터를 순허수 쿼터니언으로 확장한 것이며, $\otimes$ 는 쿼터니언 곱이다. 행렬 형태로는 다음과 같이 표현된다.

$\dot{\mathbf{q}} = \frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\mathbf{q}$

여기서

$\boldsymbol{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}$

4.2 이산 시간 갱신

샘플 주기 $\Delta t$ 의 이산 시간 시스템에서 바이어스 보정된 각속도로 쿼터니언을 갱신한다. 가장 정확한 형태는 정확 적분(exact integration)이며, 회전 벡터의 지수 사상을 사용한다.

$\Delta\mathbf{q} = \begin{bmatrix} \cos(\lVert \hat{\boldsymbol{\omega}} \rVert \Delta t / 2) \\ \dfrac{\hat{\boldsymbol{\omega}}}{\lVert \hat{\boldsymbol{\omega}} \rVert}\sin(\lVert \hat{\boldsymbol{\omega}} \rVert \Delta t / 2) \end{bmatrix}$

$\mathbf{q}_{k+1} = \mathbf{q}_k \otimes \Delta\mathbf{q}$

각속도가 작은 경우에는 1차 근사로 갱신이 가능하다.

$\mathbf{q}_{k+1} \approx \mathbf{q}_k + \frac{\Delta t}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})\mathbf{q}_k$

수치 적분 후에는 단위성을 유지하기 위해 정규화를 적용한다.

$\mathbf{q}_{k+1} \leftarrow \frac{\mathbf{q}_{k+1}}{\lVert \mathbf{q}_{k+1} \rVert}$

5. 바이어스 추정의 필요성

자이로스코프 바이어스가 보정되지 않은 경우, 자세 추정 오차는 다음과 같이 누적된다. 바이어스가 $\mathbf{b}_g$ 만큼 일정하다면 시간 $t$ 후의 자세 오차는 대략 $\mathbf{b}_g t$ 로 선형 증가한다. 일반적인 MEMS 자이로의 바이어스 크기가 시간당 수 도에서 수십 도에 이르므로, 수 분 내에 자세 추정이 사용 불가능한 수준으로 발산한다. 따라서 바이어스의 실시간 추정과 보상은 관성 항법에서 필수적이다.

6. 칼만 필터 기반 바이어스 추정

6.1 상태 벡터의 확장

쿼터니언과 자이로 바이어스를 함께 추정하는 상태 벡터는 다음과 같이 정의된다.

$\mathbf{x} = \begin{bmatrix} \mathbf{q} \\ \mathbf{b}_g \end{bmatrix} \in \mathbb{R}^7$

또는 오차 상태 형태로

$\delta\mathbf{x} = \begin{bmatrix} \delta\boldsymbol{\theta} \\ \delta\mathbf{b}_g \end{bmatrix} \in \mathbb{R}^6$

여기서 $\delta\boldsymbol{\theta}$ 는 미소 회전 벡터로 표현된 자세 오차이다.

6.2 예측 단계

쿼터니언과 바이어스의 예측은 다음과 같이 수행된다.

$\hat{\mathbf{q}}_{k+1|k} = \hat{\mathbf{q}}_{k|k} \otimes \Delta\mathbf{q}(\hat{\boldsymbol{\omega}}_k\Delta t)$

$\hat{\mathbf{b}}_{g,k+1|k} = \hat{\mathbf{b}}_{g,k|k}$

바이어스는 임의 보행 모형이므로 예측 단계에서 평균값이 변하지 않는다. 다만 공분산은 잡음 $\mathbf{n}_b$ 의 영향으로 증가한다.

6.3 오차 상태의 선형화 동역학

오차 상태의 연속 시간 동역학은 다음과 같이 선형화된다.

$\delta\dot{\boldsymbol{\theta}} = -[\hat{\boldsymbol{\omega}}]_\times\delta\boldsymbol{\theta} - \delta\mathbf{b}_g - \mathbf{n}_g$

$\delta\dot{\mathbf{b}}_g = \mathbf{n}_b$

이는 오차 상태 칼만 필터의 상태 천이 행렬을 구성하는 기초가 된다.

$\mathbf{F} = \begin{bmatrix} -[\hat{\boldsymbol{\omega}}]_\times & -\mathbf{I}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 \end{bmatrix}$

자세 오차와 바이어스 오차의 결합 항 $-\mathbf{I}_3$ 은 바이어스 변화가 자세 오차의 미분에 직접 영향을 준다는 사실을 표현한다. 이 결합이 있어야 측정 갱신을 통해 바이어스가 관측 가능해진다.

6.4 측정 갱신과 바이어스 보정

가속도계와 지자기계 등 외부 측정으로부터 자세 잔차를 계산하고, 칼만 게인 $\mathbf{K}$ 를 통해 오차 상태를 갱신한다.

$\delta\mathbf{x} = \mathbf{K}(\mathbf{z} - h(\hat{\mathbf{x}}))$

오차 상태에서 추출된 $\delta\boldsymbol{\theta}$ 와 $\delta\mathbf{b}_g$ 는 다음과 같이 명목 상태에 적용된다.

$\hat{\mathbf{q}} \leftarrow \hat{\mathbf{q}} \otimes \exp\left(\frac{1}{2}\delta\boldsymbol{\theta}\right)$

$\hat{\mathbf{b}}_g \leftarrow \hat{\mathbf{b}}_g + \delta\mathbf{b}_g$

여기서 $\exp(\cdot)$ 는 회전 벡터에서 단위 쿼터니언으로의 지수 사상이다. 갱신 후 쿼터니언은 정규화하고 오차 상태는 0으로 재설정한다.

7. 바이어스 관측 가능성 분석

바이어스가 추정 가능하려면 측정 시스템이 관측 가능성(observability) 조건을 만족해야 한다. 자세 측정만으로는 정지 상태에서 자이로 바이어스가 부분적으로만 관측 가능하며, 회전 운동이 있어야 모든 축의 바이어스가 완전히 관측된다. 정지 상태에서는 중력 방향을 기준으로 한 가속도계 측정이 두 축(피치, 롤) 자세를 관측하므로, 해당 두 축의 바이어스가 관측 가능하다. 요(yaw) 축 바이어스는 지자기계 또는 동적 운동을 통해 관측된다.

8. 상보 필터 기반 바이어스 처리

8.1 마호니 필터의 적분 보정

마호니 상보 필터에서는 자세 오차를 적분하여 바이어스 추정을 형성한다.

$\dot{\hat{\mathbf{b}}}_g = -k_I\boldsymbol{e}$

여기서 $\boldsymbol{e}$ 는 가속도계 및 지자기계로부터 계산된 자세 오차 벡터이며, $k_I$ 는 적분 이득이다. 바이어스 추정은 자이로 측정에서 차감되어 보정된 각속도를 산출한다.

$\hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_{\text{meas}} - \hat{\mathbf{b}}_g + k_P\boldsymbol{e}$

이러한 PI 형태의 보정이 바이어스 추정과 자세 추정을 동시에 수행한다.

8.2 경사 하강 기반 보정

경사 하강 기반 자세 추정 알고리즘에서는 자세 오차에 대한 비용 함수의 경사를 사용하여 자이로 적분을 보정하며, 별도의 바이어스 추정 항을 추가하여 정확도를 향상시킬 수 있다. 이 경우 바이어스는 자이로 측정에서 차감되는 추가 상태로 갱신된다.

9. 바이어스 추정의 수치적 고려사항

9.1 초기화

알고리즘 시작 시 바이어스 추정값의 초기화는 정지 상태에서의 자이로 평균값을 사용하는 것이 일반적이다. 일정 시간 동안 측정값을 평균하여 초기 바이어스를 결정한다.

$\hat{\mathbf{b}}_{g,0} = \frac{1}{N}\sum_{k=1}^{N}\boldsymbol{\omega}_{\text{meas},k}$

이 초기화 단계에서 센서가 정지 상태에 있어야 하며, 외부 자세 변화가 없어야 한다.

9.2 바이어스 변동 한계

추정된 바이어스가 물리적으로 가능한 범위를 초과하지 않도록 제한할 수 있다.

$\lVert \hat{\mathbf{b}}_g \rVert \leq b_{\max}$

이 제한은 추정 알고리즘의 발산을 방지하는 안전 장치 역할을 한다.

9.3 온도 보상

자이로 바이어스는 온도에 강하게 의존한다. 정밀 응용에서는 온도 센서를 함께 사용하여 온도-바이어스 모형을 설정하고, 알고리즘에 추가 보정 항으로 반영한다.

10. 응용 예시

10.1 무인 항공기

드론과 무인 항공기의 자세 추정에서 자이로 바이어스 추정은 비행 안정성에 직결된다. 오차 상태 칼만 필터 또는 곱셈 확장 칼만 필터가 일반적으로 사용된다.

10.2 보행 로봇

이족 및 사족 보행 로봇에서 IMU의 자이로 바이어스 추정이 본체 자세 안정화에 사용된다. 보행 중의 충격으로 인해 측정 잡음이 크므로, 견고한 바이어스 추정 알고리즘이 필요하다.

10.3 가상 현실과 증강 현실

머리 장착형 디스플레이의 자세 추정에서 자이로 바이어스가 보정되지 않으면 시야가 천천히 표류한다. 시각 SLAM과 결합한 보정 방식이 사용된다.

10.4 우주선 자세 결정

별 추적기와 자이로의 결합 자세 결정 시스템에서 자이로 바이어스 추정이 핵심적인 역할을 한다. 이는 우주선 자세 제어 시스템(ACS)의 핵심 구성 요소이다.

11. 참고 문헌

Farrell, J. A. (2008). Aided Navigation: GPS with High Rate Sensors. McGraw-Hill.
Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.
Trawny, N., & Roumeliotis, S. I. (2005). “Indirect Kalman Filter for 3D Attitude Estimation.” Technical Report, University of Minnesota.
Sola, J. (2017). “Quaternion kinematics for the error-state Kalman filter.” arXiv:1711.02508.
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.
Madgwick, S. O. H. (2010). “An efficient orientation filter for inertial and inertial/magnetic sensor arrays.” Technical Report, University of Bristol.

version: 1.0