659.56 쿼터니언과 오일러 각 간 변환

1. 개요

쿼터니언과 오일러 각은 3차원 회전을 표현하는 두 가지 주요 방식이며, 로봇 시스템에서 두 표현 간의 변환은 빈번하게 수행된다. ROS2에서는 내부적으로 쿼터니언을 사용하지만, 사용자 인터페이스와 설정 파일에서는 오일러 각(RPY)을 사용하므로, 양방향 변환의 정확한 구현이 필수적이다. 본 절에서는 RPY 규약을 기준으로 쿼터니언과 오일러 각 간의 변환 공식, 수치적 안정성 고려사항, 그리고 ROS2 환경에서의 구현 방법을 체계적으로 기술한다.

2. 오일러 각(RPY)에서 쿼터니언으로의 변환

2.1 변환 공식 유도

RPY(외재적 XYZ) 규약에서 Roll=\phi, Pitch=\theta, Yaw=\psi에 대한 합성 회전은:

R = R_z(\psi) R_y(\theta) R_x(\phi)

각 기본 회전에 대응하는 쿼터니언을 구한 후 곱셈으로 합성한다.

x축 \phi 회전의 쿼터니언:

\mathbf{q}_x = \left(\cos\frac{\phi}{2}, \; \sin\frac{\phi}{2}, \; 0, \; 0\right) \quad \text{(w, x, y, z 순서)}

y축 \theta 회전의 쿼터니언:

\mathbf{q}_y = \left(\cos\frac{\theta}{2}, \; 0, \; \sin\frac{\theta}{2}, \; 0\right)

z축 \psi 회전의 쿼터니언:

\mathbf{q}_z = \left(\cos\frac{\psi}{2}, \; 0, \; 0, \; \sin\frac{\psi}{2}\right)

합성 쿼터니언은 곱셈 순서에 유의하여:

\mathbf{q} = \mathbf{q}_z \cdot \mathbf{q}_y \cdot \mathbf{q}_x

2.2 전개된 직접 변환 공식

약기법 c_\alpha = \cos(\alpha/2), s_\alpha = \sin(\alpha/2)을 사용하면, 합성 쿼터니언의 각 성분은:

w = c_\phi c_\theta c_\psi + s_\phi s_\theta s_\psi

x = s_\phi c_\theta c_\psi - c_\phi s_\theta s_\psi

y = c_\phi s_\theta c_\psi + s_\phi c_\theta s_\psi

z = c_\phi c_\theta s_\psi - s_\phi s_\theta c_\psi

여기서 (w, x, y, z)는 해밀턴 순서이며, ROS2의 (x, y, z, w) 순서로 변환하여 사용한다.

2.3 구현

#include <tf2/LinearMath/Quaternion.h>
#include <cmath>

// tf2 라이브러리의 내장 메서드 사용
tf2::Quaternion rpy_to_quaternion(double roll, double pitch, double yaw) {
    tf2::Quaternion q;
    q.setRPY(roll, pitch, yaw);
    return q;
}

// 직접 구현
geometry_msgs::msg::Quaternion rpy_to_quaternion_manual(
    double phi, double theta, double psi) 
{
    double cp = std::cos(phi / 2.0);
    double sp = std::sin(phi / 2.0);
    double ct = std::cos(theta / 2.0);
    double st = std::sin(theta / 2.0);
    double cy = std::cos(psi / 2.0);
    double sy = std::sin(psi / 2.0);
    
    geometry_msgs::msg::Quaternion q;
    q.w = cp * ct * cy + sp * st * sy;
    q.x = sp * ct * cy - cp * st * sy;
    q.y = cp * st * cy + sp * ct * sy;
    q.z = cp * ct * sy - sp * st * cy;
    
    return q;
}

import numpy as np
from tf_transformations import quaternion_from_euler

# tf_transformations 사용
q = quaternion_from_euler(roll, pitch, yaw)  # 반환: [x, y, z, w]

# 직접 구현
def rpy_to_quaternion(roll, pitch, yaw):
    cp = np.cos(roll / 2)
    sp = np.sin(roll / 2)
    ct = np.cos(pitch / 2)
    st = np.sin(pitch / 2)
    cy = np.cos(yaw / 2)
    sy = np.sin(yaw / 2)
    
    w = cp * ct * cy + sp * st * sy
    x = sp * ct * cy - cp * st * sy
    y = cp * st * cy + sp * ct * sy
    z = cp * ct * sy - sp * st * cy
    
    return np.array([x, y, z, w])  # ROS2 순서

# SciPy 사용
from scipy.spatial.transform import Rotation

r = Rotation.from_euler('xyz', [roll, pitch, yaw])
q = r.as_quat()  # [x, y, z, w]

3. 쿼터니언에서 오일러 각(RPY)으로의 변환

3.1 변환 공식 유도

쿼터니언 \mathbf{q} = (w, x, y, z)에 대응하는 회전 행렬의 원소를 쿼터니언 성분으로 표현하면:

R = \begin{pmatrix} 1 - 2(y^2 + z^2) & 2(xy - wz) & 2(xz + wy) \\ 2(xy + wz) & 1 - 2(x^2 + z^2) & 2(yz - wx) \\ 2(xz - wy) & 2(yz + wx) & 1 - 2(x^2 + y^2) \end{pmatrix}

RPY 합성 행렬의 주요 원소와 대조하면:

r_{31} = -\sin\theta = 2(xz - wy)

r_{32} = \cos\theta \sin\phi = 2(yz + wx)

r_{33} = \cos\theta \cos\phi = 1 - 2(x^2 + y^2)

r_{21} = \sin\psi \cos\theta = 2(xy + wz)

r_{11} = \cos\psi \cos\theta = 1 - 2(y^2 + z^2)

이로부터 RPY 각도를 추출한다:

3.2 표준 경우 (\cos\theta \neq 0)

\theta = -\arcsin(2(xz - wy))

\phi = \text{atan2}(2(yz + wx), \; 1 - 2(x^2 + y^2))

\psi = \text{atan2}(2(xy + wz), \; 1 - 2(y^2 + z^2))

3.3 짐벌 락 경우 (\theta = \pm\pi/2)

2(xz - wy) \approx \pm 1인 경우 별도 처리가 필요하다:

\theta = +\pi/2인 경우 (\sin\theta = 1):

\phi + \psi = 2 \cdot \text{atan2}(x, w)

관례적으로 \phi = 0으로 고정하고 \psi = 2 \cdot \text{atan2}(x, w)로 설정한다.

\theta = -\pi/2인 경우 (\sin\theta = -1):

\phi - \psi = 2 \cdot \text{atan2}(x, w)

관례적으로 \phi = 0으로 고정하고 \psi = -2 \cdot \text{atan2}(x, w)로 설정한다.

3.4 구현

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

// tf2 라이브러리의 내장 메서드 사용
void quaternion_to_rpy(const tf2::Quaternion& q, 
                       double& roll, double& pitch, double& yaw) {
    tf2::Matrix3x3 m(q);
    m.getRPY(roll, pitch, yaw);
}

// 직접 구현 (짐벌 락 처리 포함)
void quaternion_to_rpy_manual(double qx, double qy, double qz, double qw,
                               double& roll, double& pitch, double& yaw) 
{
    // sin(pitch) 계산
    double sin_pitch = 2.0 * (qx * qz - qw * qy);
    
    // 짐벌 락 검사
    if (std::abs(sin_pitch) >= 1.0 - 1e-6) {
        // 짐벌 락 근방
        pitch = std::copysign(M_PI / 2.0, -sin_pitch);
        roll = 0.0;
        yaw = 2.0 * std::atan2(qx, qw);
        if (sin_pitch > 0) yaw = -yaw;
    } else {
        pitch = std::asin(-sin_pitch);
        roll = std::atan2(2.0 * (qy * qz + qw * qx), 
                          1.0 - 2.0 * (qx * qx + qy * qy));
        yaw = std::atan2(2.0 * (qx * qy + qw * qz), 
                         1.0 - 2.0 * (qy * qy + qz * qz));
    }
}

from tf_transformations import euler_from_quaternion
import numpy as np

# tf_transformations 사용
(roll, pitch, yaw) = euler_from_quaternion([qx, qy, qz, qw])

# 직접 구현
def quaternion_to_rpy(qx, qy, qz, qw):
    sin_pitch = 2.0 * (qx * qz - qw * qy)
    
    if abs(sin_pitch) >= 1.0 - 1e-6:
        pitch = np.copysign(np.pi / 2, -sin_pitch)
        roll = 0.0
        yaw = 2.0 * np.arctan2(qx, qw)
        if sin_pitch > 0:
            yaw = -yaw
    else:
        pitch = np.arcsin(-sin_pitch)
        roll = np.arctan2(2*(qy*qz + qw*qx), 1 - 2*(qx*qx + qy*qy))
        yaw = np.arctan2(2*(qx*qy + qw*qz), 1 - 2*(qy*qy + qz*qz))
    
    return roll, pitch, yaw

# SciPy 사용
from scipy.spatial.transform import Rotation

r = Rotation.from_quat([qx, qy, qz, qw])  # [x, y, z, w]
roll, pitch, yaw = r.as_euler('xyz')

4. 수치적 안정성 고려사항

4.1 atan2 함수의 활용

atan2(y, x) 함수는 atan(y/x) 대비 다음의 장점을 가진다:

1. 사분면 구분: 반환값의 범위가 (-\pi, \pi]로, 4개 사분면을 모두 구분한다.
2. 0으로 나누기 방지: x = 0인 경우에도 정의된다.
3. 정밀도: 인자가 작은 경우에도 수치적으로 안정적이다.

4.2 arcsin의 정의역 클리핑

arcsin 함수의 정의역은 [-1, 1]이며, 부동소수점 오차로 인해 인자가 이 범위를 미세하게 벗어날 수 있다. 반드시 클리핑을 적용해야 한다:

double sin_pitch = std::clamp(2.0 * (qx * qz - qw * qy), -1.0, 1.0);
double pitch = std::asin(-sin_pitch);

클리핑 없이 asin에 |x| > 1인 값이 전달되면 NaN이 반환되어, 후속 연산 전체가 오염된다.

4.3 쿼터니언 정규화

변환 전에 입력 쿼터니언이 단위 쿼터니언인지 확인하고, 필요시 정규화를 수행해야 한다. 비단위 쿼터니언에 대한 변환 공식은 정확한 결과를 보장하지 않는다:

void safe_quaternion_to_rpy(
    const geometry_msgs::msg::Quaternion& q_msg,
    double& roll, double& pitch, double& yaw) 
{
    // 정규화
    double norm = std::sqrt(q_msg.x*q_msg.x + q_msg.y*q_msg.y + 
                            q_msg.z*q_msg.z + q_msg.w*q_msg.w);
    
    if (norm < 1e-10) {
        roll = pitch = yaw = 0.0;
        return;
    }
    
    double qx = q_msg.x / norm;
    double qy = q_msg.y / norm;
    double qz = q_msg.z / norm;
    double qw = q_msg.w / norm;
    
    quaternion_to_rpy_manual(qx, qy, qz, qw, roll, pitch, yaw);
}

5. 변환의 검증

5.1 왕복 변환 검증

RPY → 쿼터니언 → RPY 왕복 변환의 결과가 원래 RPY와 일치하는지 검증한다:

import numpy as np
from tf_transformations import quaternion_from_euler, euler_from_quaternion

def test_roundtrip():
    test_cases = [
        (0, 0, 0),
        (0.1, 0.2, 0.3),
        (np.pi/4, np.pi/6, np.pi/3),
        (0, np.pi/2 - 0.01, 0),  # 짐벌 락 근방
        (-np.pi, 0, np.pi),       # 경계값
    ]
    
    for roll, pitch, yaw in test_cases:
        q = quaternion_from_euler(roll, pitch, yaw)
        r2, p2, y2 = euler_from_quaternion(q)
        
        # 각도 차이 (주기적 동치 고려)
        dr = np.abs(np.arctan2(np.sin(roll - r2), np.cos(roll - r2)))
        dp = np.abs(np.arctan2(np.sin(pitch - p2), np.cos(pitch - p2)))
        dy = np.abs(np.arctan2(np.sin(yaw - y2), np.cos(yaw - y2)))
        
        assert dr < 1e-10 and dp < 1e-10 and dy < 1e-10, \
            f"왕복 변환 실패: ({roll}, {pitch}, {yaw})"

주의할 점은, 짐벌 락 근방에서는 왕복 변환 후에도 roll과 yaw의 개별 값이 달라질 수 있지만, 합성 회전 자체는 동일하다는 것이다. 이 경우 쿼터니언 수준에서의 동치성을 검증하는 것이 적절하다.

5.2 기본 회전의 검증

알려진 단축 회전에 대해 변환 결과를 검증한다:

6. 다른 오일러 각 규약에 대한 변환

6.1 일반화된 변환

tf_transformations 라이브러리는 12가지 회전 순서를 모두 지원한다:

from tf_transformations import quaternion_from_euler, euler_from_quaternion

# 외재적 XYZ (RPY) — ROS2 기본
q = quaternion_from_euler(roll, pitch, yaw, axes='sxyz')
(roll, pitch, yaw) = euler_from_quaternion(q, axes='sxyz')

# 내재적 ZYX (YPR)
q = quaternion_from_euler(yaw, pitch, roll, axes='rzyx')
(yaw, pitch, roll) = euler_from_quaternion(q, axes='rzyx')

# 내재적 ZXZ (고전역학)
q = quaternion_from_euler(alpha, beta, gamma, axes='rzxz')
(alpha, beta, gamma) = euler_from_quaternion(q, axes='rzxz')

# SciPy의 일반화된 변환
from scipy.spatial.transform import Rotation

# 다양한 규약 지원
r = Rotation.from_euler('xyz', [roll, pitch, yaw])   # 외재적 xyz
r = Rotation.from_euler('ZYX', [yaw, pitch, roll])    # 내재적 ZYX
r = Rotation.from_euler('ZXZ', [alpha, beta, gamma])  # 내재적 ZXZ

# 쿼터니언으로 변환
q = r.as_quat()  # [x, y, z, w]

# 다른 규약으로 변환
euler_zyz = r.as_euler('ZYZ')
euler_ypr = r.as_euler('ZYX')

6.2 규약 간 변환 유틸리티

다른 규약으로 표현된 오일러 각을 RPY로 변환하려면 쿼터니언을 중간 표현으로 사용한다:

def convert_euler_convention(angles, from_axes, to_axes):
    """오일러 각 규약 간 변환 (쿼터니언 경유)"""
    r = Rotation.from_euler(from_axes, angles)
    return r.as_euler(to_axes)

# ZYZ → XYZ(RPY) 변환
angles_zyz = [1.0, 0.5, 0.3]
angles_rpy = convert_euler_convention(angles_zyz, 'ZYZ', 'xyz')

7. 실용적 활용 패턴

7.1 TF2 변환 발행에서의 사용

#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

void broadcast_transform(
    std::shared_ptr<tf2_ros::TransformBroadcaster> br,
    const rclcpp::Time& stamp,
    const std::string& parent, const std::string& child,
    double x, double y, double z,
    double roll, double pitch, double yaw)
{
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = stamp;
    t.header.frame_id = parent;
    t.child_frame_id = child;
    
    t.transform.translation.x = x;
    t.transform.translation.y = y;
    t.transform.translation.z = z;
    
    tf2::Quaternion q;
    q.setRPY(roll, pitch, yaw);
    q.normalize();
    
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();
    
    br->sendTransform(t);
}

7.2 자세 로깅에서의 사용

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from tf_transformations import euler_from_quaternion

class PoseLogger(Node):
    def __init__(self):
        super().__init__('pose_logger')
        self.sub = self.create_subscription(
            PoseStamped, '/robot/pose', self.callback, 10)
    
    def callback(self, msg):
        q = msg.pose.orientation
        roll, pitch, yaw = euler_from_quaternion([q.x, q.y, q.z, q.w])
        
        self.get_logger().info(
            f'위치: ({msg.pose.position.x:.3f}, '
            f'{msg.pose.position.y:.3f}, '
            f'{msg.pose.position.z:.3f}), '
            f'자세: R={np.degrees(roll):.1f}° '
            f'P={np.degrees(pitch):.1f}° '
            f'Y={np.degrees(yaw):.1f}°')

8. 요약

쿼터니언과 오일러 각(RPY) 간의 변환은 ROS2 시스템에서 내부 표현(쿼터니언)과 사용자 인터페이스(RPY) 사이의 교량 역할을 한다. RPY에서 쿼터니언으로의 변환은 세 기본 회전의 반각 삼각함수를 조합하는 폐쇄형 공식으로 구현되며, 역변환은 회전 행렬의 원소를 쿼터니언 성분으로 표현한 후 atan2와 asin을 사용하여 각도를 추출한다. 짐벌 락 근방에서의 별도 처리, arcsin의 정의역 클리핑, 쿼터니언 정규화 등 수치적 안정성을 위한 세심한 처리가 필수적이다.

9. 참고 문헌

• Diebel, J. (2006). “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors.” Stanford University Technical Report.
• Slabaugh, G. G. (1999). “Computing Euler Angles from a Rotation Matrix.” City University London.
• Kuipers, J. B. (1999). Quaternions and Rotation Sequences. Princeton University Press.
• Open Robotics. “tf2 — ROS 2 Documentation.” https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Tf2.html
• SciPy Documentation. “scipy.spatial.transform.Rotation.” https://docs.scipy.org/doc/scipy/