10.22 쿼터니언과 회전 벡터의 상호 변환

1. 회전 벡터의 정의

회전 벡터(rotation vector)는 축-각도 표현의 축과 각도를 단일 3차원 벡터로 결합한 형태이다.

$\boldsymbol{\phi} = \phi\hat{\mathbf{u}}$

여기서

$\phi$ : 회전 각
$\hat{\mathbf{u}}$ : 회전 축 (단위 벡터)

회전 벡터의 크기가 회전 각이고, 방향이 회전 축이다. 이는 지수 좌표(exponential coordinates)의 다른 이름이기도 하다.

$\phi = \lVert\boldsymbol{\phi}\rVert, \quad \hat{\mathbf{u}} = \boldsymbol{\phi}/\phi$

회전 벡터는 $\mathfrak{so}(3)$ 리 대수의 자연 좌표이며, 비선형 최적화에서 자세를 매개화하는 데 자주 사용된다.

2. 쿼터니언에서 회전 벡터로의 변환

2.1 일반 공식

단위 쿼터니언 $\mathbf{q} = (q_w, q_x, q_y, q_z)$ 로부터 회전 벡터는 다음과 같이 계산된다.

$\phi = 2\,\mathrm{atan2}(\lVert\mathbf{q}_v\rVert, q_w)$

$\hat{\mathbf{u}} = \mathbf{q}_v / \lVert\mathbf{q}_v\rVert$

$\boldsymbol{\phi} = \phi\hat{\mathbf{u}}$

이를 결합하면

$\boldsymbol{\phi} = \frac{2\,\mathrm{atan2}(\lVert\mathbf{q}_v\rVert, q_w)}{\lVert\mathbf{q}_v\rVert}\mathbf{q}_v$

2.2 작은 회전의 처리

$\lVert\mathbf{q}_v\rVert$ 가 0에 가까우면( $\phi \approx 0$ ), 공식의 분모가 작아져 수치 오차가 발생한다. 테일러 전개를 사용하여 근사한다.

$\frac{\sin(\phi/2)}{\phi/2} \approx 1 - \frac{(\phi/2)^2}{6} + \cdots$

회전 벡터 공식은 다음과 같이 근사된다.

$\boldsymbol{\phi} \approx 2\mathbf{q}_v \quad (\text{when } \phi \to 0)$

이는 작은 회전에서 $\mathbf{q}_v \approx (\phi/2)\hat{\mathbf{u}}$ 이고, 2를 곱하면 $\phi\hat{\mathbf{u}} = \boldsymbol{\phi}$ 가 되기 때문이다.

2.3 견고한 알고리즘

function quaternion_to_rotvec(q):
    # 부호 정규화
    if q.w < 0:
        q = -q
    
    norm_v = sqrt(q.x*q.x + q.y*q.y + q.z*q.z)
    
    if norm_v < epsilon:
        # 작은 회전
        return (2 * q.x, 2 * q.y, 2 * q.z)
    else:
        angle = 2 * atan2(norm_v, q.w)
        factor = angle / norm_v
        return (factor * q.x, factor * q.y, factor * q.z)

3. 회전 벡터에서 쿼터니언으로의 변환

3.1 일반 공식

회전 벡터 $\boldsymbol{\phi}$ 로부터 단위 쿼터니언은 다음과 같이 계산된다.

$\phi = \lVert\boldsymbol{\phi}\rVert$

$\hat{\mathbf{u}} = \boldsymbol{\phi}/\phi$

$\mathbf{q} = (\cos(\phi/2), \sin(\phi/2)\hat{\mathbf{u}})$

성분별로

$q_w = \cos(\phi/2)$

$\mathbf{q}_v = \frac{\sin(\phi/2)}{\phi}\boldsymbol{\phi}$

3.2 직접 형태

$\mathbf{q} = \left(\cos(\lVert\boldsymbol{\phi}\rVert/2), \frac{\sin(\lVert\boldsymbol{\phi}\rVert/2)}{\lVert\boldsymbol{\phi}\rVert}\boldsymbol{\phi}\right)$

3.3 작은 회전의 처리

$\lVert\boldsymbol{\phi}\rVert$ 가 0에 가까우면 $\sin/\phi$ 의 분모가 작아진다. 테일러 전개를 사용한다.

$\frac{\sin(\phi/2)}{\phi} \approx \frac{1}{2} - \frac{\phi^2}{48} + \cdots$

작은 회전에서는

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

이는 $\cos(\phi/2) \approx 1 - \phi^2/8$ 과 $\sin(\phi/2)/\phi \approx 1/2$ 를 사용한 것이다.

3.4 견고한 알고리즘

function rotvec_to_quaternion(phi):
    norm_phi = sqrt(phi.x*phi.x + phi.y*phi.y + phi.z*phi.z)
    
    if norm_phi < epsilon:
        # 작은 회전
        q.w = 1 - norm_phi*norm_phi / 8
        q.x = phi.x / 2
        q.y = phi.y / 2
        q.z = phi.z / 2
    else:
        half_angle = norm_phi / 2
        sin_half = sin(half_angle)
        factor = sin_half / norm_phi
        
        q.w = cos(half_angle)
        q.x = factor * phi.x
        q.y = factor * phi.y
        q.z = factor * phi.z
    
    return q

4. 회전 벡터의 이점

4.1 자유 매개변수

회전 벡터는 3개의 자유 매개변수를 가진다. 쿼터니언의 4 매개변수와 달리 제약(단위 노름)이 없다. 이는 비선형 최적화에서 장점이다.

4.2 작은 회전에서의 근사

작은 회전에서 회전 벡터는 각속도와 단순한 관계를 가진다.

$\boldsymbol{\phi}(t) \approx \boldsymbol{\omega}t \quad (\text{for small rotations})$

이는 무한소 회전의 가환 근사를 반영한다.

4.3 리 대수 좌표

회전 벡터는 $SO(3)$ 리 군의 리 대수 $\mathfrak{so}(3)$ 의 자연 좌표이다. 지수 사상 $\exp: \mathfrak{so}(3) \to SO(3)$ 이 회전 벡터를 회전 행렬로 변환한다.

5. 회전 벡터의 한계

5.1 합성의 복잡성

회전 벡터의 합성은 단순한 벡터 합이 아니다(두 회전이 가환하지 않기 때문). 정확한 합성은 BCH 공식 또는 쿼터니언 곱을 통해야 한다.

5.2 특이점

회전 각이 0에 가까우면 축이 결정되지 않는다. 또한 $\phi = \pm\pi$ 근처에서 매개변수 공간의 위상적 복잡성이 있다.

5.3 큰 회전의 처리

회전 각의 크기가 $\pi$ 를 초과하면 표현이 모호해진다. 관례적으로 $\lVert\boldsymbol{\phi}\rVert \leq \pi$ 로 제한한다.

6. 변환의 응용

6.1 비선형 최적화

SLAM과 번들 조정 등 비선형 최적화에서 자세를 회전 벡터로 매개화한다. 자유 매개변수이므로 표준 최적화 알고리즘과 호환된다.

$\text{Optimize } \boldsymbol{\phi} \in \mathbb{R}^3$

매 반복에서 회전 벡터가 갱신되고, 쿼터니언이나 회전 행렬로 변환하여 실제 회전 연산이 수행된다.

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

ESKF에서 명목 자세는 쿼터니언으로, 오차 상태는 작은 회전 벡터로 표현된다. 이는 작은 회전의 가환성을 활용하여 칼만 필터의 선형 모델을 적용할 수 있게 한다.

6.3 자세 적분

각속도 $\boldsymbol{\omega}$ 를 적분하여 자세를 갱신할 때, 회전 벡터가 자연스럽다.

$\boldsymbol{\phi}_{\Delta t} = \boldsymbol{\omega}\Delta t$

그 후 회전 벡터를 쿼터니언으로 변환하여 기존 자세와 합성한다.

$\mathbf{q}_{k+1} = \mathbf{q}_k \otimes \mathrm{rotvec\_to\_quat}(\boldsymbol{\omega}\Delta t)$

6.4 자세 차이의 표현

두 자세의 차이를 회전 벡터로 표현하는 것은 매끄러운 제어 피드백에 적합하다.

$\Delta\mathbf{q} = \mathbf{q}_1^*\mathbf{q}_2$

$\Delta\boldsymbol{\phi} = \mathrm{quat\_to\_rotvec}(\Delta\mathbf{q})$

회전 벡터의 크기가 회전 오차, 방향이 회전 축이다.

7. 수치적 정확도

7.1 작은 회전

테일러 근사를 사용하면 작은 회전에서 정확한 결과를 얻을 수 있다. 임계값(예: $10^{-6}$ )을 설정하여 작은 회전과 일반 회전을 구분한다.

7.2 큰 회전

$\phi$ 가 $\pi$ 에 가까우면 매개변수 공간의 표면 근처이며, 추가 처리가 필요할 수 있다. 관례적으로 $\phi \leq \pi$ 로 제한하여 처리한다.

8. 변환의 일관성 검증

8.1 왕복 변환

쿼터니언 → 회전 벡터 → 쿼터니언의 결과가 원래 쿼터니언과 같아야 한다(부호 정규화를 고려).

$\mathrm{rotvec\_to\_quat}(\mathrm{quat\_to\_rotvec}(\mathbf{q})) = \pm\mathbf{q}$

8.2 회전 동등성

두 표현으로 같은 벡터를 회전한 결과가 같아야 한다.

9. 변환의 라이브러리 지원

9.1 Sophus

Sophus::SO3d R(q);
Eigen::Vector3d phi = R.log();
Sophus::SO3d R2 = Sophus::SO3d::exp(phi);

9.2 Eigen

Eigen에서는 AngleAxis 클래스를 거쳐서 변환한다.

Eigen::Quaterniond q;
Eigen::AngleAxisd aa(q);
Eigen::Vector3d phi = aa.angle() * aa.axis();

9.3 NumPy/SciPy

from scipy.spatial.transform import Rotation
rotvec = Rotation.from_quat(q).as_rotvec()
q2 = Rotation.from_rotvec(rotvec).as_quat()

10. 회전 벡터와 $\mathfrak{se}(3)$

$SE(3)$ 리 군의 리 대수 $\mathfrak{se}(3)$ 는 6차원 트위스트로 매개화된다. 이 중 3차원은 회전 벡터와 같고, 나머지 3차원은 병진 성분이다.

$\boldsymbol{\xi} = \begin{bmatrix}\boldsymbol{\phi} \\ \boldsymbol{\rho}\end{bmatrix} \in \mathbb{R}^6$

$SE(3)$ 의 지수 사상과 로그 사상도 회전 벡터와 비슷한 공식으로 정의되지만, 병진 부분이 회전과 결합되는 점이 다르다.

11. 변환 공식의 유도

쿼터니언 → 회전 벡터 변환의 유도는 축-각도 표현을 거쳐서 수행된다.

11.1 축-각도 추출

$\mathbf{q} = (\cos(\phi/2), \sin(\phi/2)\hat{\mathbf{u}})$ 로부터

$\phi = 2\,\mathrm{atan2}(\sin(\phi/2), \cos(\phi/2))$

벡터 부분의 노름이 $\sin(\phi/2)$ 이고, 스칼라 부분이 $\cos(\phi/2)$ 이므로 위의 공식이 성립한다.

11.2 회전 벡터 결합

$\boldsymbol{\phi} = \phi\hat{\mathbf{u}} = \phi\frac{\mathbf{q}_v}{\sin(\phi/2)}$

여기서 $\sin(\phi/2) = \lVert\mathbf{q}_v\rVert$ 이므로

$\boldsymbol{\phi} = \frac{\phi}{\lVert\mathbf{q}_v\rVert}\mathbf{q}_v = \frac{2\,\mathrm{atan2}(\lVert\mathbf{q}_v\rVert, q_w)}{\lVert\mathbf{q}_v\rVert}\mathbf{q}_v$

이것이 쿼터니언에서 회전 벡터로의 공식이다.

12. 결론

쿼터니언과 회전 벡터의 상호 변환은 단순한 닫힌 형태로 수행되며, 작은 회전에서 테일러 근사를 사용한다. 회전 벡터는 자유 매개변수의 장점으로 비선형 최적화에 적합하며, 쿼터니언은 효율적 합성과 보간에 적합하다. 두 표현을 혼합하여 사용하는 것이 일반적 패턴이다.

13. 참고 문헌

Hamilton, W. R. (1844). “On Quaternions; or on a New System of Imaginaries in Algebra.” Philosophical Magazine, Vol. 25, 489–495.
Diebel, J. (2006). “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors.” Stanford University Technical Report.
Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.
Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.
Sola, J., Deray, J., & Atchuthan, D. (2018). “A Micro Lie Theory for State Estimation in Robotics.” arXiv:1812.01537.

version: 1.0