659.57 쿼터니언과 회전 행렬 간 변환
1. 개요
쿼터니언과 회전 행렬은 3차원 회전을 표현하는 두 가지 핵심적인 수학적 도구이며, 로봇공학의 다양한 계층에서 상호 변환이 요구된다. TF2 라이브러리는 내부적으로 쿼터니언을 사용하지만, 선형대수 연산이나 기하학적 계산에서는 회전 행렬이 필요한 경우가 빈번하다. 본 절에서는 쿼터니언에서 회전 행렬로의 변환 및 역변환에 대한 수학적 공식, 수치적 안정성, 그리고 ROS2 환경에서의 구현 방법을 체계적으로 다룬다.
2. 쿼터니언에서 회전 행렬로의 변환
2.1 변환 공식 유도
단위 쿼터니언 \mathbf{q} = (w, x, y, z)에 의한 벡터 회전 \mathbf{v}' = \mathbf{q}\hat{\mathbf{v}}\mathbf{q}^{*}를 행렬 형식으로 전개하면 3 \times 3 회전 행렬을 얻는다. 순수 쿼터니언 \hat{\mathbf{v}} = (0, v_x, v_y, v_z)에 대해 쿼터니언 곱셈을 수행하고 벡터부를 추출하면:
R(\mathbf{q}) = \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}
이 공식은 단위 쿼터니언(w^2 + x^2 + y^2 + z^2 = 1)을 전제로 한다.
2.2 대안적 표기
\|\mathbf{q}\| = 1 조건을 이용하면, 대각 원소를 다음과 같이 표기할 수도 있다:
r_{11} = w^2 + x^2 - y^2 - z^2 = 2(w^2 + x^2) - 1
r_{22} = w^2 - x^2 + y^2 - z^2 = 2(w^2 + y^2) - 1
r_{33} = w^2 - x^2 - y^2 + z^2 = 2(w^2 + z^2) - 1
첫 번째 형식(1 - 2(\cdot))과 두 번째 형식(2(\cdot) - 1)은 수학적으로 동일하나, 부동소수점 연산에서의 정밀도 특성이 다를 수 있다.
2.3 계산 복잡도
쿼터니언에서 회전 행렬로의 변환에 필요한 연산:
- 제곱 계산: x^2, y^2, z^2 (3회 곱셈)
- 교차곱 계산: xy, xz, xw, yz, yw, zw (6회 곱셈)
- 스케일링: 2를 곱하는 연산 (시프트 연산으로 대체 가능)
- 덧셈/뺄셈: 12회
총 약 **21~27 부동소수점 연산(FLOPs)**이 필요하다.
2.4 구현
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <Eigen/Geometry>
// tf2 라이브러리
tf2::Quaternion q;
q.setRPY(0.1, 0.2, 0.3);
tf2::Matrix3x3 R(q); // 쿼터니언에서 직접 생성
// Eigen 라이브러리
Eigen::Quaterniond q_eigen(w, x, y, z); // (w, x, y, z) 생성자 순서
Eigen::Matrix3d R_eigen = q_eigen.toRotationMatrix();
// 수동 구현
void quaternion_to_matrix(double w, double x, double y, double z,
double R[3][3]) {
double x2 = x * x, y2 = y * y, z2 = z * z;
double xy = x * y, xz = x * z, yz = y * z;
double wx = w * x, wy = w * y, wz = w * z;
R[0][0] = 1.0 - 2.0 * (y2 + z2);
R[0][1] = 2.0 * (xy - wz);
R[0][2] = 2.0 * (xz + wy);
R[1][0] = 2.0 * (xy + wz);
R[1][1] = 1.0 - 2.0 * (x2 + z2);
R[1][2] = 2.0 * (yz - wx);
R[2][0] = 2.0 * (xz - wy);
R[2][1] = 2.0 * (yz + wx);
R[2][2] = 1.0 - 2.0 * (x2 + y2);
}
import numpy as np
from scipy.spatial.transform import Rotation
# SciPy
r = Rotation.from_quat([x, y, z, w]) # [x, y, z, w] 순서
R = r.as_matrix()
# 수동 구현
def quaternion_to_matrix(w, x, y, z):
x2, y2, z2 = x*x, y*y, z*z
xy, xz, yz = x*y, x*z, y*z
wx, wy, wz = w*x, w*y, w*z
return np.array([
[1 - 2*(y2+z2), 2*(xy-wz), 2*(xz+wy)],
[2*(xy+wz), 1 - 2*(x2+z2), 2*(yz-wx)],
[2*(xz-wy), 2*(yz+wx), 1 - 2*(x2+y2)]
])
3. 회전 행렬에서 쿼터니언으로의 변환
3.1 셰퍼드 방법 (Shepperd’s Method)
회전 행렬에서 쿼터니언으로의 가장 널리 사용되는 변환 방법은 셰퍼드(Shepperd, 1978)의 방법이다. 이 방법은 대각 원소의 합과 차를 이용하여 네 가지 경우 중 수치적으로 가장 안정적인 경로를 선택한다.
회전 행렬의 대각합(trace)과 대각 원소로부터:
4w^2 = 1 + r_{11} + r_{22} + r_{33} = 1 + \text{tr}(R)
4x^2 = 1 + r_{11} - r_{22} - r_{33}
4y^2 = 1 - r_{11} + r_{22} - r_{33}
4z^2 = 1 - r_{11} - r_{22} + r_{33}
네 값 중 가장 큰 것을 선택하여 해당 성분을 먼저 계산하고, 나머지 성분은 비대각 원소를 이용하여 계산한다. 가장 큰 값을 선택하는 이유는 제곱근 연산의 수치적 안정성을 보장하기 위함이다.
3.2 알고리즘
Eigen::Quaterniond matrix_to_quaternion(const Eigen::Matrix3d& R) {
double trace = R.trace();
double w, x, y, z;
if (trace > 0) {
double s = 2.0 * std::sqrt(1.0 + trace);
w = 0.25 * s;
x = (R(2,1) - R(1,2)) / s;
y = (R(0,2) - R(2,0)) / s;
z = (R(1,0) - R(0,1)) / s;
} else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
double s = 2.0 * std::sqrt(1.0 + R(0,0) - R(1,1) - R(2,2));
w = (R(2,1) - R(1,2)) / s;
x = 0.25 * s;
y = (R(0,1) + R(1,0)) / s;
z = (R(0,2) + R(2,0)) / s;
} else if (R(1,1) > R(2,2)) {
double s = 2.0 * std::sqrt(1.0 - R(0,0) + R(1,1) - R(2,2));
w = (R(0,2) - R(2,0)) / s;
x = (R(0,1) + R(1,0)) / s;
y = 0.25 * s;
z = (R(1,2) + R(2,1)) / s;
} else {
double s = 2.0 * std::sqrt(1.0 - R(0,0) - R(1,1) + R(2,2));
w = (R(1,0) - R(0,1)) / s;
x = (R(0,2) + R(2,0)) / s;
y = (R(1,2) + R(2,1)) / s;
z = 0.25 * s;
}
return Eigen::Quaterniond(w, x, y, z).normalized();
}
import numpy as np
def matrix_to_quaternion(R):
trace = np.trace(R)
if trace > 0:
s = 2.0 * np.sqrt(1.0 + trace)
w = 0.25 * s
x = (R[2,1] - R[1,2]) / s
y = (R[0,2] - R[2,0]) / s
z = (R[1,0] - R[0,1]) / s
elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
s = 2.0 * np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2])
w = (R[2,1] - R[1,2]) / s
x = 0.25 * s
y = (R[0,1] + R[1,0]) / s
z = (R[0,2] + R[2,0]) / s
elif R[1,1] > R[2,2]:
s = 2.0 * np.sqrt(1.0 - R[0,0] + R[1,1] - R[2,2])
w = (R[0,2] - R[2,0]) / s
x = (R[0,1] + R[1,0]) / s
y = 0.25 * s
z = (R[1,2] + R[2,1]) / s
else:
s = 2.0 * np.sqrt(1.0 - R[0,0] - R[1,1] + R[2,2])
w = (R[1,0] - R[0,1]) / s
x = (R[0,2] + R[2,0]) / s
y = (R[1,2] + R[2,1]) / s
z = 0.25 * s
q = np.array([x, y, z, w])
return q / np.linalg.norm(q)
3.3 대각합(Trace) 기반 간이 방법
대각합이 양수(\text{tr}(R) > 0, 즉 \theta < 120°)인 경우에 사용할 수 있는 간단한 방법:
w = \frac{1}{2}\sqrt{1 + \text{tr}(R)}
x = \frac{r_{32} - r_{23}}{4w}, \quad y = \frac{r_{13} - r_{31}}{4w}, \quad z = \frac{r_{21} - r_{12}}{4w}
이 방법은 구현이 간단하지만, \text{tr}(R) \approx -1인 경우(180° 근방 회전)에서 w \approx 0이 되어 나눗셈이 불안정해지므로, 셰퍼드 방법의 분기 처리가 필요하다.
4. 수치적 안정성
4.1 비직교 입력 행렬의 처리
실제 시스템에서 수신되는 행렬은 직교 조건을 정확히 만족하지 않을 수 있다. 비직교 행렬을 쿼터니언으로 변환하면 비단위 쿼터니언이 산출되므로, 변환 후 정규화가 필수적이다.
보다 엄밀한 처리를 원하면, 변환 전에 SVD 기반 직교화를 수행하여 최근접 회전 행렬을 구한 후 변환한다:
Eigen::Quaterniond safe_matrix_to_quaternion(const Eigen::Matrix3d& M) {
// SVD 기반 직교화
Eigen::JacobiSVD<Eigen::Matrix3d> svd(M,
Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d R = U * V.transpose();
if (R.determinant() < 0) {
U.col(2) *= -1;
R = U * V.transpose();
}
// 직교화된 행렬에서 쿼터니언 변환
return Eigen::Quaterniond(R).normalized();
}
4.2 부호 모호성
회전 행렬에서 쿼터니언으로의 변환은 유일하지 않다. \mathbf{q}와 -\mathbf{q}가 동일한 회전 행렬에 대응하므로:
R(\mathbf{q}) = R(-\mathbf{q})
셰퍼드 방법은 w > 0인 쿼터니언을 산출하도록 설계되어 있으나, 이중 피복 성질에 의한 모호성은 근본적으로 해소되지 않는다. 연속적인 회전 추적(motion tracking)에서는 이전 프레임의 쿼터니언과의 내적이 양수가 되도록 부호를 조정해야 한다:
Eigen::Quaterniond ensure_consistent_sign(
const Eigen::Quaterniond& q_new,
const Eigen::Quaterniond& q_prev)
{
if (q_new.dot(q_prev) < 0) {
return Eigen::Quaterniond(-q_new.w(), -q_new.x(),
-q_new.y(), -q_new.z());
}
return q_new;
}
5. ROS2/TF2에서의 라이브러리 활용
5.1 tf2 라이브러리
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
// 쿼터니언 → 회전 행렬
tf2::Quaternion q(0, 0, 0.7071, 0.7071);
tf2::Matrix3x3 R(q);
// 회전 행렬 → 쿼터니언
tf2::Quaternion q_from_R;
R.getRotation(q_from_R);
5.2 tf2_eigen
#include <tf2_eigen/tf2_eigen.hpp>
#include <Eigen/Geometry>
// geometry_msgs → Eigen
geometry_msgs::msg::Quaternion q_msg;
Eigen::Quaterniond q_eigen;
tf2::fromMsg(q_msg, q_eigen);
Eigen::Matrix3d R = q_eigen.toRotationMatrix();
// Eigen → geometry_msgs
Eigen::Quaterniond q_result(R);
geometry_msgs::msg::Quaternion q_msg_result = tf2::toMsg(q_result);
5.3 SciPy (Python)
from scipy.spatial.transform import Rotation
import numpy as np
# 쿼터니언 → 회전 행렬
r = Rotation.from_quat([x, y, z, w])
R = r.as_matrix()
# 회전 행렬 → 쿼터니언
r = Rotation.from_matrix(R)
q = r.as_quat() # [x, y, z, w]
6. 변환의 검증
6.1 왕복 변환 검증
import numpy as np
from scipy.spatial.transform import Rotation
def test_roundtrip_quat_matrix():
"""쿼터니언 → 행렬 → 쿼터니언 왕복 검증"""
np.random.seed(42)
for _ in range(1000):
# 랜덤 쿼터니언 생성
q = np.random.randn(4)
q = q / np.linalg.norm(q)
if q[3] < 0: # w > 0 보장
q = -q
# 쿼터니언 → 행렬 → 쿼터니언
r = Rotation.from_quat(q)
R = r.as_matrix()
r2 = Rotation.from_matrix(R)
q2 = r2.as_quat()
if q2[3] < 0:
q2 = -q2
np.testing.assert_allclose(q, q2, atol=1e-12)
def test_roundtrip_matrix_quat():
"""행렬 → 쿼터니언 → 행렬 왕복 검증"""
np.random.seed(42)
for _ in range(1000):
r = Rotation.random()
R = r.as_matrix()
q = r.as_quat()
r2 = Rotation.from_quat(q)
R2 = r2.as_matrix()
np.testing.assert_allclose(R, R2, atol=1e-12)
6.2 성질 검증
변환 결과가 다음 성질을 만족하는지 검증한다:
- 직교성: R^T R = I (허용 오차 이내)
- 행렬식: \det(R) = +1 (허용 오차 이내)
- 단위 노름: \|\mathbf{q}\| = 1 (허용 오차 이내)
- 벡터 변환 일관성: R\mathbf{v} = \mathbf{q}\hat{\mathbf{v}}\mathbf{q}^{*}
7. 계산 복잡도 비교
| 변환 방향 | 연산 수 (FLOPs) | 주요 연산 |
|---|---|---|
| 쿼터니언 → 행렬 | ~21-27 | 곱셈, 덧셈 |
| 행렬 → 쿼터니언 | ~20-30 | 곱셈, 덧셈, 제곱근 1회, 분기 |
| 비교: 행렬 곱셈 | ~45 | 곱셈, 덧셈 |
| 비교: 쿼터니언 곱셈 | ~28 | 곱셈, 덧셈 |
변환 비용은 다른 회전 연산에 비해 상대적으로 낮으므로, 필요에 따라 자유롭게 표현을 전환할 수 있다. 그러나 루프 내에서 반복적으로 변환을 수행하는 것은 피하고, 적절한 표현을 선택하여 변환 횟수를 최소화하는 것이 바람직하다.
8. 요약
쿼터니언과 회전 행렬 간의 변환은 ROS2 로봇 시스템에서 빈번하게 수행되는 핵심 연산이다. 쿼터니언에서 회전 행렬로의 변환은 이차 형식의 직접적인 전개로 구현되며, 역변환은 셰퍼드 방법을 통해 수치적으로 안정적인 분기 선택을 수행한다. 비직교 입력 행렬, 부호 모호성, 정규화 등의 수치적 문제에 대한 체계적인 처리가 필수적이다. tf2, Eigen, SciPy 등의 라이브러리에서 검증된 구현이 제공되므로, 가능한 한 라이브러리 함수를 활용하는 것이 권장된다.
9. 참고 문헌
- Shepperd, S. W. (1978). “Quaternion from Rotation Matrix.” Journal of Guidance and Control, 1(3), 223-224.
- Bar-Itzhack, I. Y. (2000). “New Method for Extracting the Quaternion from a Rotation Matrix.” Journal of Guidance, Control, and Dynamics, 23(6), 1085-1087.
- Diebel, J. (2006). “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors.” Stanford University Technical Report.
- 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