10.28 쿼터니언 적분의 수치적 방법과 정규화

1. 수치 적분의 필요성

쿼터니언의 연속 시간 운동학 방정식 $\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q}\otimes\boldsymbol{\omega}_q$ 를 디지털 시스템에서 이산 시간으로 구현하려면 수치 적분이 필요하다. 다양한 적분 방법이 정확도, 효율성, 수치적 안정성 측면에서 서로 다른 특성을 가진다.

2. 적분 방법의 분류

2.1 명시적 방법

현재 시점의 정보만으로 다음 시점을 계산한다.

오일러 적분: 1차 정확도
2차 룬게-쿠타 (RK2): 2차 정확도
4차 룬게-쿠타 (RK4): 4차 정확도

2.2 지수 방법

리 군의 지수 사상을 활용하여 매니폴드 구조를 존중한다.

정확한 지수 적분: 각속도 일정 가정하에 정확
리 군 적분기: 매니폴드 상의 고차 적분

2.3 암묵적 방법

미래 시점의 정보를 포함한다.

후진 오일러: 1차 암묵적
중점 규칙: 2차 암묵적

암묵적 방법은 강체 동역학에서 안정성이 필요할 때 사용되지만, 쿼터니언 적분에서는 덜 일반적이다.

3. 오일러 적분

3.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$

3.2 장단점

장점:

구현이 매우 단순
계산 비용이 낮음

단점:

1차 정확도 (오차 $O(\Delta t^2)$ )
단위 노름 보존하지 않음
정규화 필수

3.3 정규화 후 오일러

function euler_integrate(q, omega, dt):
    q_dot = 0.5 * q * quaternion(0, omega)
    q_new = q + q_dot * dt
    return normalize(q_new)

단순하지만 장기간 적분에서 정확도가 떨어진다.

4. 정확한 지수 적분

4.1 공식

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

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

4.2 지수의 닫힌 형태

순수 벡터 쿼터니언 $\boldsymbol{\omega}_q\Delta t/2$ 의 지수는

$\exp\left(\frac{\boldsymbol{\omega}_q\Delta t}{2}\right) = \left(\cos\left(\frac{\lVert\boldsymbol{\omega}\rVert\Delta t}{2}\right), \frac{\sin(\lVert\boldsymbol{\omega}\rVert\Delta t/2)}{\lVert\boldsymbol{\omega}\rVert}\boldsymbol{\omega}\right)$

4.3 장단점

장점:

각속도 일정 가정하에 정확
단위 노름 자동 보존
안정적

단점:

삼각 함수 계산 필요
작은 각속도에서 수치 안정성 주의

4.4 알고리즘

function exact_exponential_integrate(q, omega, dt):
    angular_rate = norm(omega)
    
    if angular_rate < epsilon:
        # 작은 회전
        delta_q_w = 1
        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) / angular_rate * omega
    
    delta_q = Quaternion(delta_q_w, delta_q_v)
    return q * delta_q

5. 차 룬게-쿠타 (RK2)

5.1 중점 방법

$\mathbf{k}_1 = \frac{1}{2}\mathbf{q}_k\otimes\boldsymbol{\omega}_{q,k}$

$\mathbf{q}_{\text{mid}} = \mathbf{q}_k + \frac{\Delta t}{2}\mathbf{k}_1$

$\mathbf{k}_2 = \frac{1}{2}\mathbf{q}_{\text{mid}}\otimes\boldsymbol{\omega}_{q,\text{mid}}$

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

5.2 특성

2차 정확도 (오차 $O(\Delta t^3)$ )
중간 단계의 각속도가 필요
정규화 필요

6. 차 룬게-쿠타 (RK4)

6.1 공식

표준 RK4의 쿼터니언 버전이다.

$\mathbf{k}_1 = f(\mathbf{q}_k, \boldsymbol{\omega}_k)$

$\mathbf{k}_2 = f\left(\mathbf{q}_k + \frac{\Delta t}{2}\mathbf{k}_1, \boldsymbol{\omega}_{k+1/2}\right)$

$\mathbf{k}_3 = f\left(\mathbf{q}_k + \frac{\Delta t}{2}\mathbf{k}_2, \boldsymbol{\omega}_{k+1/2}\right)$

$\mathbf{k}_4 = f(\mathbf{q}_k + \Delta t\mathbf{k}_3, \boldsymbol{\omega}_{k+1})$

$\mathbf{q}_{k+1} = \mathbf{q}_k + \frac{\Delta t}{6}(\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4)$

여기서 $f(\mathbf{q}, \boldsymbol{\omega}) = \frac{1}{2}\mathbf{q}\otimes\boldsymbol{\omega}_q$ 이다.

6.2 특성

4차 정확도 (오차 $O(\Delta t^5)$ )
매우 정확
계산 비용이 상당함 (4 번의 함수 평가)
정규화 필요

7. 리 군 적분기

리 군 적분기(Lie group integrator)는 매니폴드 구조를 존중하는 특별한 적분 방법이다. 쿼터니언이 $S^3$ 매니폴드 상에 있으므로, 이 구조를 보존하는 적분이 자연스럽다.

7.1 Munthe-Kaas 방법

리 대수 공간에서 고차 룬게-쿠타를 수행하고, 지수 사상으로 다시 리 군으로 매핑하는 방법이다. 매니폴드를 정확히 보존하면서 고차 정확도를 제공한다.

7.2 Crouch-Grossman 방법

여러 번의 지수 사상을 결합한 방법이다. 정확도와 효율성의 균형이 있다.

이러한 방법은 매우 정밀한 적분이 필요한 경우 사용된다. 일반적인 응용에서는 정확한 지수 적분이 충분하다.

8. 정규화

8.1 정규화의 필요성

적분 방법에 따라 단위 노름이 점차 손실될 수 있다.

오일러 적분: 즉시 단위 노름 손실
정확한 지수 적분: 이론적으로 보존, 부동 소수점 오차로 점진적 손실
RK 방법: 각 단계 후 단위 노름 손실

8.2 정규화 공식

$\hat{\mathbf{q}} = \frac{\mathbf{q}}{\lVert\mathbf{q}\rVert}$

8.3 정규화의 빈도

매 단계 정규화: 가장 안전하지만 계산 비용 증가
주기적 정규화: 일정 단계마다 정규화
임계값 기반: 노름이 1에서 임계값 이상 벗어나면 정규화

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

대부분의 응용에서 매 단계 정규화가 권장된다.

8.4 Brin과 Feldman의 1차 정규화

매 단계마다 완전한 정규화 대신 1차 보정을 사용할 수 있다.

$\hat{\mathbf{q}} \approx \mathbf{q}(1.5 - 0.5\lVert\mathbf{q}\rVert^2)$

제곱근을 피하므로 더 빠르지만 정확도가 약간 낮다.

9. 정확도와 효율성의 균형

9.1 일반적 응용

대부분의 IMU 기반 자세 추정 시스템에서 정확한 지수 적분이 권장된다. 이는 정확도와 효율성의 균형이 좋으며, 단위 노름을 자동으로 보존한다.

9.2 고속 회전

각속도가 매우 크면 지수 적분의 삼각 함수가 여러 번 호출되어 비싸질 수 있다. 이 경우 RK4가 더 효율적일 수 있다.

9.3 고정밀 응용

우주선 자세 제어 등 매우 정밀한 응용에서는 리 군 적분기가 사용된다.

9.4 실시간 시스템

실시간 시스템에서는 결정적 계산 시간이 중요하다. 오일러 적분 또는 정확한 지수 적분이 예측 가능한 실행 시간을 제공한다.

10. 수치 오차의 원인

10.1 모델 오차

각속도가 시간 단계 내에서 일정하다는 가정은 정확하지 않다. 실제 각속도는 연속적으로 변하므로 이 가정에서의 오차가 발생한다.

10.2 적분 공식 오차

선택한 적분 공식의 차수에 따른 오차이다. 오일러(1차), RK2(2차), RK4(4차) 등.

10.3 부동 소수점 오차

컴퓨터의 유한한 정밀도로 인한 오차이다. 일반적으로 다른 오차보다 훨씬 작지만, 장기간 적분에서 누적될 수 있다.

10.4 센서 노이즈

각속도 측정값의 노이즈가 적분으로 통해 자세 오차로 전파된다. 이는 수치 방법 자체의 문제가 아니라 센서의 한계이다.

11. 오차 누적과 드리프트

쿼터니언 적분의 장기 드리프트는 다음의 원인으로 인해 발생한다.

11.1 자이로스코프 편향

시간에 따라 변하는 편향이 적분되어 자세 오차로 누적된다. 편향 보정이 필수이다.

11.2 자이로스코프 노이즈

백색 노이즈가 적분되면 시간에 비례한 표준 편차의 랜덤 워크가 된다. 시간이 길수록 오차가 증가한다.

11.3 수치 오차

부동 소수점 오차의 누적이다. 일반적으로 다른 원인보다 작지만 장시간 적분에서 중요할 수 있다.

11.4 모델 오차

각속도 일정 가정이나 적분 공식의 차수 오차가 누적된다.

12. 드리프트 완화 전략

12.1 편향 추정과 보정

자이로스코프 편향을 칼만 필터 등으로 추정하고 측정값에서 뺀다.

12.2 외부 센서 융합

가속도계, 자력계, GPS, 카메라 등 다른 센서와의 융합으로 드리프트를 완화한다.

12.3 맵 기반 보정

시각 SLAM이나 모션 캡처의 관측으로 절대 자세를 보정한다.

13. 수치 적분의 실시간 구현

13.1 결정적 실행 시간

실시간 시스템에서는 적분 단계의 실행 시간이 일정해야 한다. 분기나 반복이 있는 방법은 부적절할 수 있다.

13.2 메모리 효율

작은 메모리 공간에서 구현되어야 한다. 상태 변수만 유지하고 임시 변수는 최소화한다.

13.3 예외 처리

작은 각속도, 큰 각속도 등의 특수한 경우를 안전하게 처리해야 한다.

14. 코드 최적화

14.1 사전 계산

자주 사용되는 값(예: $\Delta t/2$ )을 사전 계산한다.

14.2 SIMD 활용

쿼터니언 연산이 SIMD로 가속될 수 있다. Eigen 등의 라이브러리가 자동으로 활용한다.

14.3 삼각 함수 최적화

작은 각도에서 테일러 전개를 사용하면 삼각 함수 계산을 피할 수 있다.

15. 검증과 테스트

15.1 합성 데이터

알려진 각속도 프로파일에 대해 이론적 결과와 수치 결과를 비교한다.

15.2 왕복 적분

시간을 앞으로 적분한 후 역시간으로 적분하여 원래 자세로 돌아가는지 확인한다.

15.3 보존량

단위 노름과 같은 보존량이 수치 오차 임계값 내에서 유지되는지 확인한다.

16. 자세 전파의 오차 전파

자세 전파의 오차는 각속도 측정의 오차와 수치 적분의 오차의 결합이다. 오차의 시간 의존성은 다음과 같다.

16.1 자이로스코프 편향

편향으로 인한 자세 오차는 시간에 비례한다.

$\Delta\mathbf{q}_{\text{bias}} \sim \mathbf{b}t$

16.2 자이로스코프 노이즈

노이즈로 인한 자세 오차의 표준 편차는 시간의 제곱근에 비례한다.

$\sigma_{\Delta\mathbf{q}} \sim \sigma_{\boldsymbol{\omega}}\sqrt{t}$

16.3 수치 오차

수치 오차는 적분 공식의 차수와 시간 단계에 따라 결정된다.

이러한 오차 특성을 이해하는 것이 자세 추정 시스템 설계에 필수이다.

17. 라이브러리 지원

17.1 Eigen

Eigen의 Quaternion 클래스는 기본 연산을 제공하지만, 적분 방법은 사용자가 구현해야 한다.

17.2 Sophus

Sophus 라이브러리는 $SO(3)$ 과 $SE(3)$ 의 지수 사상과 로그 사상을 제공한다.

17.3 GTSAM

GTSAM은 인수 그래프 기반 추정에서 쿼터니언과 자세 전파를 지원한다.

17.4 ROS의 IMU 필터

ROS의 imu_filter 패키지는 다양한 자세 추정 알고리즘을 제공한다.

18. 참고 문헌

Hairer, E., Lubich, C., & Wanner, G. (2006). Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations (2nd ed.). Springer.
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.
Iserles, A., Munthe-Kaas, H. Z., Nørsett, S. P., & Zanna, A. (2000). “Lie-Group Methods.” Acta Numerica, 9, 215–365.
Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.

version: 1.0