10.60 자세 오차의 선형화와 미소 회전 근사

1. 선형화의 필요성

자세 오차는 본질적으로 비선형이다. 이는 $SO(3)$ 매니폴드의 비유클리드 구조 때문이다. 그러나 선형 제어 이론, 칼만 필터, 비선형 최적화 등 많은 고급 기법이 선형 시스템을 전제로 한다. 따라서 자세 오차를 선형화(linearization)하는 것이 실용적으로 중요하다.

미소 회전 근사(small rotation approximation)는 작은 자세 오차를 선형적으로 다루는 방법이며, 대부분의 실용적 자세 시스템의 기반이다.

2. 선형화의 기본 원리

2.1 테일러 전개

비선형 함수 $f(\mathbf{x})$ 를 점 $\mathbf{x}_0$ 주위에서 테일러 전개하면

$f(\mathbf{x}_0 + \delta\mathbf{x}) \approx f(\mathbf{x}_0) + \mathbf{J}_f(\mathbf{x}_0)\delta\mathbf{x}$

여기서 $\mathbf{J}_f$ 는 자코비안이다. 이는 1차 근사이다.

2.2 자세의 경우

자세 오차의 경우 “점“은 자세이고 “증분“은 작은 회전이다. 작은 회전의 1차 근사를 사용한다.

3. 미소 회전 근사

작은 회전 각 $\phi$ 에 대한 근사는 다음과 같다.

3.1 사인과 코사인의 근사

$\sin\phi \approx \phi$
$\cos\phi \approx 1 - \phi^2/2 \approx 1$ (1차 근사)

3.2 회전 행렬의 근사

$\mathbf{R} \approx \mathbf{I} + [\boldsymbol{\phi}]_\times$

이는 로드리게스 공식의 1차 근사이다. 2차 항 $[\boldsymbol{\phi}]_\times^2$ 는 무시된다.

3.3 쿼터니언의 근사

$\mathbf{q} \approx (1, \boldsymbol{\phi}/2)$

스칼라 부분이 1에 가깝고 벡터 부분이 회전 벡터의 절반이다.

4. 자세 오차의 선형화

작은 자세 오차의 선형화는 다음의 형태이다.

4.1 회전 행렬 오차

$\mathbf{R}_e = \mathbf{R}_d\mathbf{R}^T$

작은 오차에서

$\mathbf{R}_e \approx \mathbf{I} + [\delta\boldsymbol{\phi}_e]_\times$

여기서 $\delta\boldsymbol{\phi}_e$ 는 작은 오차 회전 벡터이다.

4.2 쿼터니언 오차

$\mathbf{q}_e \approx (1, \delta\boldsymbol{\phi}_e/2)$

4.3 오차의 추출

회전 행렬 오차로부터 오차 회전 벡터를 추출한다.

$\delta\boldsymbol{\phi}_e = \left(\frac{\mathbf{R}_e - \mathbf{R}_e^T}{2}\right)^\vee$

여기서 $(\cdot)^\vee$ 는 반대칭 행렬에서 벡터로의 사상이다.

4.4 쿼터니언에서의 추출

$\delta\boldsymbol{\phi}_e \approx 2\mathbf{q}_{e,v}$

벡터 부분의 두 배가 근사적으로 오차 회전 벡터이다.

5. 자세 동역학의 선형화

자세 동역학도 작은 오차에서 선형화된다.

5.1 비선형 동역학

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

5.2 선형화된 오차 동역학

작은 오차 $\delta\boldsymbol{\phi}_e$ 의 시간 미분은

$\delta\dot{\boldsymbol{\phi}}_e \approx -[\boldsymbol{\omega}]_\times\delta\boldsymbol{\phi}_e + \delta\boldsymbol{\omega}$

여기서 $\delta\boldsymbol{\omega}$ 는 각속도 오차이다. 이는 선형 미분 방정식이다.

6. 선형화의 응용

6.1 확장 칼만 필터 (EKF)

EKF는 비선형 시스템을 선형화하여 칼만 필터를 적용한다. 자세 추정의 경우 자세 오차를 선형화한다.

6.1.1 예측 단계

선형화된 오차 동역학으로 오차 공분산을 전파한다.

$\mathbf{P}_{k+1} = \boldsymbol{\Phi}_k\mathbf{P}_k\boldsymbol{\Phi}_k^T + \mathbf{Q}_k$

여기서 $\boldsymbol{\Phi}_k$ 는 선형화된 전이 행렬이다.

6.1.2 업데이트 단계

측정 잔차를 선형화하여 칼만 이득을 계산한다.

$\mathbf{K}_k = \mathbf{P}_k\mathbf{H}_k^T(\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T + \mathbf{R}_k)^{-1}$

6.2 오류 상태 칼만 필터 (ESKF)

ESKF는 명목 상태와 오차 상태를 분리한다. 명목 상태는 비선형으로 처리하고, 오차 상태만 선형화한다. 이는 EKF보다 더 안정적이다.

6.2.1 명목 자세

쿼터니언으로 곱셈적으로 업데이트한다.

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

6.2.2 오차 상태

작은 회전 벡터 $\delta\boldsymbol{\phi}$ 로 선형화하여 표준 칼만 필터 업데이트를 수행한다.

6.3 비선형 제어

비선형 자세 제어에서 리아프노프 직접 방법이 자주 사용되지만, 작은 오차에서 선형 제어 이론도 유용하다.

6.3.1 PD 제어

$\mathbf{u} = k_p\delta\boldsymbol{\phi}_e + k_d\delta\boldsymbol{\omega}$

선형 제어 법칙으로 작은 오차를 정규화한다.

6.4 안정성 분석

작은 오차 주위에서 시스템의 안정성을 선형 시스템 이론(폴로 분석, 주파수 응답 등)으로 분석할 수 있다.

7. 선형화의 정확도

7.1 작은 오차 영역

$\lVert\delta\boldsymbol{\phi}\rVert < 10°$ 정도의 오차에서 선형화가 매우 정확하다. 오차는 $O(\delta\phi^2)$ 이다.

7.2 중간 오차 영역

$\lVert\delta\boldsymbol{\phi}\rVert \in [10°, 30°]$ 에서 선형화가 여전히 유용하지만, 2차 효과가 나타난다.

7.3 큰 오차 영역

$\lVert\delta\boldsymbol{\phi}\rVert > 30°$ 에서 선형화가 부정확해진다. 비선형 방법이 필요하다.

8. 차 근사

더 정확한 근사가 필요하면 2차 항을 포함할 수 있다.

8.1 회전 행렬

$\mathbf{R} \approx \mathbf{I} + [\boldsymbol{\phi}]_\times + \frac{1}{2}[\boldsymbol{\phi}]_\times^2$

8.2 쿼터니언

$\mathbf{q} \approx (1 - \lVert\boldsymbol{\phi}\rVert^2/8, \boldsymbol{\phi}/2)$

2차 항은 정확성을 향상시키지만 계산이 복잡해진다.

9. 선형화와 자코비안

자세 오차의 자코비안은 선형화에서 핵심적이다.

9.1 자세 오차의 자코비안

각속도에 대한 자세 오차의 자코비안은

$\frac{\partial\delta\boldsymbol{\phi}}{\partial\boldsymbol{\omega}} = -\Delta t\cdot\mathbf{I}$

이는 1차 근사이다.

9.2 자세에 대한 자코비안

자세 자체에 대한 오차의 자코비안은

$\frac{\partial\delta\boldsymbol{\phi}}{\partial\boldsymbol{\phi}} = \mathbf{I} - \frac{\Delta t}{2}[\boldsymbol{\omega}]_\times$

이는 작은 시간 단계의 근사이다.

10. 선형화의 한계

10.1 작은 오차 제한

선형화는 작은 오차에서만 유효하다. 큰 오차에서는 부정확하다.

10.2 매니폴드 위반

선형 공간 가정은 $SO(3)$ 매니폴드 구조를 위반한다. 장시간 적분에서 매니폴드에서 벗어날 수 있다.

10.3 누적 오차

작은 선형화 오차가 시간에 따라 누적된다. 긴 시뮬레이션에서 문제가 된다.

10.4 2차 효과 무시

1차 선형화는 2차 효과를 무시한다. 이는 일부 응용에서 부정확성을 유발한다.

11. 선형화의 개선

11.1 지역 매개화

현재 자세 주위에서 지역적으로 선형화한다. 매 시각마다 매개화를 갱신하여 선형화의 정확성을 유지한다.

11.2 반복적 선형화

초기 추정을 사용하여 선형화한 후, 결과를 사용하여 재선형화한다. 이는 수렴할 때까지 반복된다.

11.3 리 군 선형화

리 군의 구조를 이용한 선형화이다. 매니폴드 구조를 보존한다.

11.4 고차 항 포함

2차 이상의 항을 포함하여 정확성을 향상시킨다.

12. 선형화와 매니폴드 최적화

매니폴드 최적화는 선형화를 매니폴드 구조와 결합한다.

12.1 접공간 매개화

현재 자세의 접공간에서 작은 변화를 선형 공간으로 다룬다. 이는 리 대수 좌표와 같다.

12.2 지수 사상으로 복원

최적화 후 지수 사상으로 다시 리 군으로 변환한다. 매니폴드 구조가 자연스럽게 보존된다.

$\mathbf{R}_{\text{new}} = \mathbf{R}_{\text{current}}\exp([\delta\boldsymbol{\phi}]_\times)$

13. 선형화와 리 군 자코비안

리 군에서의 선형화는 특별한 자코비안 공식을 사용한다.

13.1 좌측 자코비안

$\mathbf{J}_l(\boldsymbol{\phi}) = \mathbf{I} + \frac{1 - \cos\phi}{\phi^2}[\boldsymbol{\phi}]_\times + \frac{\phi - \sin\phi}{\phi^3}[\boldsymbol{\phi}]_\times^2$

이는 지수 사상의 자코비안이다.

13.2 작은 회전의 근사

작은 회전에서 $\mathbf{J}_l \approx \mathbf{I}$ 이다. 이는 1차 근사이다.

14. 미소 회전의 가환성

일반적으로 회전은 비가환이다. 그러나 매우 작은 회전(미소 회전)은 1차 근사에서 가환적이다.

14.1 두 미소 회전의 합성

$\exp([\delta\boldsymbol{\phi}_1]_\times)\exp([\delta\boldsymbol{\phi}_2]_\times) \approx (\mathbf{I} + [\delta\boldsymbol{\phi}_1]_\times)(\mathbf{I} + [\delta\boldsymbol{\phi}_2]_\times)$

$\approx \mathbf{I} + [\delta\boldsymbol{\phi}_1]_\times + [\delta\boldsymbol{\phi}_2]_\times$

이는 덧셈의 형태이며 가환적이다.

14.2 차 항

정확한 합성은 2차 항을 포함한다.

$\exp([\delta\boldsymbol{\phi}_1]_\times)\exp([\delta\boldsymbol{\phi}_2]_\times) = \exp([\delta\boldsymbol{\phi}_1 + \delta\boldsymbol{\phi}_2 + \frac{1}{2}\delta\boldsymbol{\phi}_1\times\delta\boldsymbol{\phi}_2]_\times + \ldots)$

외적 항이 비가환성을 반영하지만, 매우 작다.

15. 실용적 고려

15.1 시간 단계의 선택

시간 단계가 작을수록 선형화가 정확하다. 일반적으로 $100$ Hz 이상의 적분 주기를 사용한다.

15.2 정규화

선형화된 결과를 주기적으로 정규화하여 매니폴드로 다시 투영한다.

15.3 큰 오차의 감지

선형화 오차가 임계값을 초과하면 비선형 방법으로 전환한다.

15.4 테스트

선형화의 정확성을 단위 테스트로 검증한다.

16. 선형화의 이론적 기반

16.1 리 군 이론

선형화는 리 군의 지역 구조를 활용한다. 단위 원소 주위의 접공간이 선형 공간이며, 여기서 선형 근사가 정확하다.

16.2 미분 기하학

매니폴드 상의 미분 기하학이 선형화의 수학적 기반이다. 접공간, 지수 사상, 야코비안 등이 핵심 개념이다.

17. 선형화와 수치 적분

자세 적분에서 선형화가 사용되지만 한계가 있다.

17.1 차 오일러 적분

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

이는 선형 근사이며 단위 노름을 보존하지 않는다.

17.2 정확한 지수 적분

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

이는 비선형이지만 매니폴드 구조를 보존한다.

정확한 지수 적분이 일반적으로 선호된다.

18. 결론

자세 오차의 선형화는 작은 회전 근사를 통해 비선형 자세 문제를 선형 공간에서 처리할 수 있게 한다. 이는 선형 제어 이론, 확장 칼만 필터, 오류 상태 칼만 필터 등의 적용을 가능하게 한다. 선형화는 작은 오차에서 정확하지만 큰 오차에서는 부정확해지므로, 응용에 따라 적절한 범위에서 사용해야 한다. 리 군 이론과 매니폴드 최적화는 선형화의 매니폴드 구조 위반 문제를 부분적으로 해결한다.

19. 참고 문헌

Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.
Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.
Lefferts, E. J., Markley, F. L., & Shuster, M. D. (1982). “Kalman Filtering for Spacecraft Attitude Estimation.” Journal of Guidance, Control, and Dynamics, 5(5), 417–429.
Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.
Bar-Itzhack, I. Y., & Oshman, Y. (1985). “Attitude Determination from Vector Observations: Quaternion Estimation.” IEEE Transactions on Aerospace and Electronic Systems, 21(1), 128–136.

version: 1.0