10.27 쿼터니언 시간 적분과 자세 전파

10.27 쿼터니언 시간 적분과 자세 전파

1. 자세 전파의 개념

자세 전파(attitude propagation)는 각속도 측정값으로부터 시간에 따른 쿼터니언의 변화를 계산하는 과정이다. 이는 자세 추정 시스템(특히 IMU 기반 시스템)의 핵심 연산이며, 다음의 쿼터니언 운동학 방정식을 이산 시간에 적분하는 작업이다.

\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q}\otimes\boldsymbol{\omega}_q

이산 시간 단계 \Delta t로 이 미분 방정식을 수치적으로 풀어 \mathbf{q}_{k+1}을 \mathbf{q}_k와 각속도 측정값 \boldsymbol{\omega}_k로부터 계산한다.

2. 적분 방법의 분류

쿼터니언 적분 방법은 정확도, 효율성, 매니폴드 보존 측면에서 다양한 선택이 가능하다.

2.1 1차 오일러 적분

가장 단순한 방법이다.

\mathbf{q}_{k+1} = \mathbf{q}_k + \dot{\mathbf{q}}_k\Delta t = \mathbf{q}_k + \frac{1}{2}\mathbf{q}_k\otimes\boldsymbol{\omega}_{q,k}\Delta t

단순하고 빠르지만 두 가지 단점이 있다.

  • 단위 노름 위반: 결과가 단위 쿼터니언이 아님. 정규화가 필요.
  • 수치 오차: 1차 근사이므로 정확도가 낮음.

2.2 정확한 지수 적분

각속도가 시간 단계 내에서 일정하다고 가정하면 다음의 정확한 해를 얻는다.

\mathbf{q}_{k+1} = \mathbf{q}_k\otimes\exp\left(\frac{\boldsymbol{\omega}_{q,k}\Delta t}{2}\right)

여기서 \exp는 쿼터니언 지수 함수이다. 이 방법은 다음의 장점이 있다.

  • 단위 노름 보존: 결과가 자동으로 단위 쿼터니언.
  • 정확성: 각속도가 일정한 가정하에 정확한 해.
  • 권장: 실제 시스템에서 표준적으로 사용.

2.3 고차 룬게-쿠타 적분

더 높은 정확도가 필요한 경우 룬게-쿠타 방법을 사용할 수 있다. 그러나 매 단계마다 정규화가 필요하다.

3. 쿼터니언 지수 함수

쿼터니언 지수 함수의 닫힌 형태는 다음과 같다. 순수 벡터 쿼터니언 \mathbf{p} = (0, \mathbf{p}_v)에 대해

\exp(\mathbf{p}) = \left(\cos\lVert\mathbf{p}_v\rVert, \sin\lVert\mathbf{p}_v\rVert\frac{\mathbf{p}_v}{\lVert\mathbf{p}_v\rVert}\right)

이는 축-각도 표현과 같은 형태이다. 자세 전파에서 \mathbf{p} = \boldsymbol{\omega}_q\Delta t/2이며 크기가 \lVert\boldsymbol{\omega}\rVert\Delta t/2이다.

3.1 작은 각도의 처리

\lVert\boldsymbol{\omega}\rVert\Delta t가 작으면 \sin(x)/x의 계산이 수치적으로 불안정해질 수 있다. 테일러 전개로 근사한다.

\frac{\sin(x)}{x} \approx 1 - \frac{x^2}{6} + \cdots

4. 알고리즘

4.1 정확한 지수 적분 알고리즘

function propagate_attitude(q, omega, dt):
    # 각속도 크기와 방향 계산
    angular_rate = norm(omega)
    
    if angular_rate < epsilon:
        # 작은 회전
        delta_q_w = 1 - (angular_rate * dt)^2 / 8
        delta_q_v = omega * dt / 2
    else:
        # 일반적 경우
        half_angle = angular_rate * dt / 2
        delta_q_w = cos(half_angle)
        delta_q_v = sin(half_angle) * omega / angular_rate
    
    delta_q = Quaternion(delta_q_w, delta_q_v)
    return q * delta_q

4.2 오일러 적분 알고리즘 (단순 참조)

function propagate_euler(q, omega, dt):
    omega_q = Quaternion(0, omega.x, omega.y, omega.z)
    q_dot = 0.5 * q * omega_q
    q_new = q + q_dot * dt
    return normalize(q_new)

5. 정확도의 비교

5.1 오일러 적분

  • 1차 정확도: 오차가 O(\Delta t^2)
  • 매 단계마다 정규화 필요
  • 계산 비용이 낮음

5.2 정확한 지수 적분

  • 각속도 일정 가정하에 정확
  • 단위 노름 자동 보존
  • 삼각 함수 계산 필요 (약간 더 비쌈)

대부분의 응용에서 정확한 지수 적분이 권장된다.

6. 변수 각속도의 처리

실제 시스템에서 각속도는 시간에 따라 변한다. 이를 처리하는 방법은 다음과 같다.

6.1 단계별 상수 근사

각 시간 단계 내에서 각속도가 일정하다고 가정한다. 이는 \Delta t가 충분히 작으면 정확한 근사이다.

6.2 평균 각속도

두 시점의 각속도의 평균을 사용한다.

\bar{\boldsymbol{\omega}} = \frac{\boldsymbol{\omega}_k + \boldsymbol{\omega}_{k+1}}{2}

그리고 평균 각속도로 지수 적분을 수행한다.

\mathbf{q}_{k+1} = \mathbf{q}_k\otimes\exp\left(\frac{\bar{\boldsymbol{\omega}}_q\Delta t}{2}\right)

이 방법은 2차 정확도를 제공한다.

6.3 중점 법칙

중점에서의 각속도를 추정하여 사용한다.

\boldsymbol{\omega}_{\text{mid}} = \boldsymbol{\omega}_k + \frac{\boldsymbol{\omega}_{k+1} - \boldsymbol{\omega}_k}{2}

6.4 룬게-쿠타 방법

4차 룬게-쿠타 등의 고차 방법을 사용한다. 매 단계마다 여러 번 각속도를 평가해야 하므로 비용이 증가한다.

7. 자세 전파의 정확도 요구

응용에 따라 필요한 정확도가 다르다.

7.1 실시간 자세 추정 (IMU 기반)

  • 100-1000 Hz의 센서 주기
  • 단계당 회전 각이 작음 (수 mrad)
  • 정확한 지수 적분으로 충분

7.2 긴 시간 드리프트 최소화

  • 누적 오차가 중요
  • 더 정확한 적분과 다른 센서와의 융합 필요
  • 칼만 필터 등의 필터링이 필수

7.3 고속 회전

  • 단계당 회전 각이 큼
  • 룬게-쿠타 등의 고차 방법 권장
  • 또는 더 작은 시간 단계 사용

8. 자세 전파의 수치적 안정성

8.1 단위 노름의 보존

정확한 지수 적분은 단위 노름을 자동으로 보존하지만, 부동 소수점 오차로 서서히 손실될 수 있다. 주기적으로 정규화하는 것이 권장된다.

if abs(norm(q) - 1) > threshold:
    q = normalize(q)

8.2 부호 일관성

전파 과정에서 쿼터니언의 부호가 매끄럽게 변해야 한다. 부호 점프가 발생하지 않도록 일관된 처리가 필요하다.

8.3 오차 누적

긴 시간 적분에서 수치 오차가 누적된다. 이를 해결하기 위해 외부 참조(GPS, 카메라 등)와의 융합이 일반적이다.

9. 자세 전파와 센서 융합

자세 전파만으로는 센서 잡음과 편향으로 인해 장기간의 정확한 자세를 유지할 수 없다. 다른 센서와의 융합이 필수이다.

9.1 자이로스코프 + 가속도계

  • 자이로스코프: 짧은 시간의 정확한 각속도
  • 가속도계: 장기 중력 방향 참조

두 센서를 상보적 필터(complementary filter) 또는 칼만 필터로 융합하여 안정한 자세 추정을 얻는다.

9.2 자이로스코프 + 자력계

  • 자력계: 지자기 방향 참조

자력계는 요(yaw) 방향의 드리프트를 보정한다.

9.3 자이로스코프 + 가속도계 + 자력계

세 센서의 융합이 현대 IMU 기반 자세 추정의 표준 구성이다.

9.4 자세 추정 + 시각 오도메트리 (VIO)

드론과 자율 주행에서 자세 추정에 시각 정보를 결합하여 더 정확한 결과를 얻는다.

10. 쿼터니언 전파의 편향 보정

자이로스코프는 측정에 편향(bias)을 포함한다.

\boldsymbol{\omega}_{\text{measured}} = \boldsymbol{\omega}_{\text{true}} + \mathbf{b} + \mathbf{n}

여기서 \mathbf{b}는 편향, \mathbf{n}은 노이즈이다. 정확한 자세 전파를 위해서는 편향을 추정하고 보정해야 한다.

10.1 편향 추정

칼만 필터의 상태 벡터에 편향을 포함하여 시간에 따라 추정한다. 편향은 보통 천천히 변하므로 상관 시간이 긴 랜덤 워크 모델로 근사된다.

10.2 편향 보정

전파 전에 측정값에서 추정된 편향을 뺀다.

\boldsymbol{\omega}_{\text{corrected}} = \boldsymbol{\omega}_{\text{measured}} - \hat{\mathbf{b}}

11. 구체적 구현 예시

11.1 순수 자이로스코프 기반 전파

function gyro_attitude_propagation():
    q = initial_quaternion
    dt = 1.0 / gyro_rate
    
    while True:
        omega = read_gyroscope()
        omega_corrected = omega - bias_estimate
        q = propagate_attitude(q, omega_corrected, dt)
        
        if should_normalize(q):
            q = normalize(q)
        
        publish_attitude(q)

11.2 상보적 필터

function complementary_filter():
    q = initial_quaternion
    alpha = 0.98  # 자이로스코프 가중치
    dt = 1.0 / loop_rate
    
    while True:
        omega = read_gyroscope()
        a = read_accelerometer()
        
        # 자이로스코프 전파
        q_gyro = propagate_attitude(q, omega, dt)
        
        # 가속도계 기반 참조
        q_acc = compute_from_accelerometer(a)
        
        # 가중 결합 (SLERP)
        q = slerp(q_acc, q_gyro, alpha)
        
        publish_attitude(q)

12. 자세 전파의 응용

12.1 항법 시스템

항공기, 드론, 선박의 자세 추정에 쿼터니언 전파가 사용된다. 긴 시간의 자세 추적이 필요하다.

12.2 로봇 제어

매니퓰레이터와 이동 로봇의 자세 제어에서 전파가 상태 업데이트의 핵심이다.

12.3 가상 현실과 증강 현실

VR/AR 헤드셋의 자세 추적이 IMU 기반 쿼터니언 전파와 시각 정보의 융합이다.

12.4 운동 분석

스포츠와 의료 분야에서 사람의 자세 추적에 IMU가 사용된다.

12.5 위성 자세 제어

우주선의 자세 추정이 장시간 쿼터니언 전파에 의존한다.

13. 시간 단계의 선택

시간 단계 \Delta t의 선택은 정확도와 계산 비용의 균형이다.

13.1 하드웨어 제약

대부분의 IMU는 100-1000 Hz의 고정 주기로 데이터를 출력한다. 이 주기가 자연스러운 \Delta t이다.

13.2 각속도의 크기

각속도가 클수록 단계당 회전이 커지므로 더 작은 \Delta t가 필요하다. 대부분의 응용에서는 IMU 주기가 충분히 작다.

13.3 응용의 요구

고속 회전, 곡예 비행 등에서는 더 빠른 전파가 필요할 수 있다.

14. 장기 드리프트

장기간의 쿼터니언 전파는 다음의 드리프트 원인으로 인해 정확도가 떨어진다.

  1. 자이로스코프 편향: 시간에 따른 편향 변화
  2. 자이로스코프 노이즈: 적분으로 인한 오차 누적
  3. 수치 오차: 부동 소수점 정밀도 한계
  4. 단위 노름 손실: 반복 적분으로 인한 정규화 필요

이러한 드리프트를 완화하기 위해 외부 참조(가속도계, 자력계, 카메라 등)와의 융합이 필수이다.

15. 참고 문헌

  • Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.
  • Titterton, D. H., & Weston, J. L. (2004). Strapdown Inertial Navigation Technology (2nd ed.). IET.
  • Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.
  • Farrell, J. A. (2008). Aided Navigation: GPS with High Rate Sensors. McGraw-Hill.
  • Groves, P. D. (2013). Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (2nd ed.). Artech House.

version: 1.0