剛体の姿勢表現として、以下の3つの方法が代表的です。
- 回転行列（Direction Cosine Matrix, DCM）
- オイラー角（Euler Angles）
- クォータニオン（Quaternion, 四元数）
それぞれに長所と短所があり、用途に応じて使い分けられます。特に宇宙機の姿勢制御では、クォータニオンが広く使用されています。
本記事の内容
- 回転行列の定義と性質
- オイラー角とジンバルロック問題
- クォータニオンの数学的構造
- 各表現の相互変換
- Pythonでの実装
回転行列（DCM）
定義
回転行列 $\bm{R} \in \text{SO}(3)$ は $3 \times 3$ の直交行列で、以下の性質を満たします。
$$ \bm{R}^T \bm{R} = \bm{R} \bm{R}^T = \bm{I}_3, \quad \det(\bm{R}) = 1 $$
回転行列は、基準座標系から機体座標系への変換を表します。
$$ \bm{r}_{\text{body}} = \bm{R} \bm{r}_{\text{ref}} $$
基本回転行列
各軸周りの回転行列は以下の通りです。
$x$ 軸周り（角度 $\phi$）:
$$ \bm{R}_x(\phi) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & \sin\phi \\ 0 & -\sin\phi & \cos\phi \end{pmatrix} $$
$y$ 軸周り（角度 $\theta$）:
$$ \bm{R}_y(\theta) = \begin{pmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{pmatrix} $$
$z$ 軸周り（角度 $\psi$）:
$$ \bm{R}_z(\psi) = \begin{pmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
長所と短所
- 長所: 数学的に厳密、組み合わせが容易（行列積）
- 短所: 9パラメータ + 6拘束条件、数値計算で直交性が崩れやすい
オイラー角
定義
3つの角度で姿勢を表現します。回転順序（3-2-1, ZYXなど）によって定義が異なります。
ZYX（航空宇宙で標準的）の場合、
$$ \bm{R} = \bm{R}_x(\phi) \bm{R}_y(\theta) \bm{R}_z(\psi) $$
- $\psi$: ヨー角（$z$ 軸周り）
- $\theta$: ピッチ角（$y$ 軸周り）
- $\phi$: ロール角（$x$ 軸周り）
ジンバルロック
オイラー角の重大な欠点がジンバルロック（Gimbal Lock）です。ピッチ角 $\theta = \pm 90°$ のとき、ロール軸とヨー軸が平行になり、1自由度を失います。
$$ \theta = 90° \implies \bm{R} = \begin{pmatrix} 0 & \sin(\phi-\psi) & \cos(\phi-\psi) \\ 0 & \cos(\phi-\psi) & -\sin(\phi-\psi) \\ -1 & 0 & 0 \end{pmatrix} $$
$\phi$ と $\psi$ が独立に決まらず、$\phi – \psi$ のみが決定されます。
クォータニオン
定義
クォータニオン $\bm{q}$ は4つのパラメータで姿勢を表現します。
$$ \bm{q} = q_0 + q_1 \bm{i} + q_2 \bm{j} + q_3 \bm{k} = \begin{pmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{pmatrix} $$
ここで $q_0$ はスカラー部、$(q_1, q_2, q_3)$ はベクトル部です。単位クォータニオンの条件は、
$$ |\bm{q}|^2 = q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 $$
軸-角度表現との関係
回転軸 $\hat{\bm{n}} = (n_x, n_y, n_z)$ 周りに角度 $\theta$ だけ回転するクォータニオンは、
$$ \bm{q} = \begin{pmatrix} \cos(\theta/2) \\ n_x \sin(\theta/2) \\ n_y \sin(\theta/2) \\ n_z \sin(\theta/2) \end{pmatrix} $$
クォータニオンによる回転
ベクトル $\bm{v}$ のクォータニオン $\bm{q}$ による回転は、
$$ \bm{v}’ = \bm{q} \otimes \bm{v} \otimes \bm{q}^* $$
ここで $\bm{q}^* = (q_0, -q_1, -q_2, -q_3)$ は共役クォータニオン、$\otimes$ はクォータニオン積です。
クォータニオンから回転行列への変換
$$ \bm{R} = \begin{pmatrix} 1-2(q_2^2+q_3^2) & 2(q_1q_2+q_0q_3) & 2(q_1q_3-q_0q_2) \\ 2(q_1q_2-q_0q_3) & 1-2(q_1^2+q_3^2) & 2(q_2q_3+q_0q_1) \\ 2(q_1q_3+q_0q_2) & 2(q_2q_3-q_0q_1) & 1-2(q_1^2+q_2^2) \end{pmatrix} $$
長所と短所
- 長所: ジンバルロックなし、4パラメータのみ、補間が容易
- 短所: 直感的に理解しにくい
Pythonでの実装
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def euler_to_dcm(phi, theta, psi):
"""オイラー角(ZYX)から回転行列へ変換"""
Rx = np.array([[1, 0, 0],
[0, np.cos(phi), np.sin(phi)],
[0, -np.sin(phi), np.cos(phi)]])
Ry = np.array([[np.cos(theta), 0, -np.sin(theta)],
[0, 1, 0],
[np.sin(theta), 0, np.cos(theta)]])
Rz = np.array([[np.cos(psi), np.sin(psi), 0],
[-np.sin(psi), np.cos(psi), 0],
[0, 0, 1]])
return Rx @ Ry @ Rz
def euler_to_quaternion(phi, theta, psi):
"""オイラー角からクォータニオンへ変換"""
cp = np.cos(phi / 2)
sp = np.sin(phi / 2)
ct = np.cos(theta / 2)
st = np.sin(theta / 2)
cy = np.cos(psi / 2)
sy = np.sin(psi / 2)
q0 = cp * ct * cy + sp * st * sy
q1 = sp * ct * cy - cp * st * sy
q2 = cp * st * cy + sp * ct * sy
q3 = cp * ct * sy - sp * st * cy
return np.array([q0, q1, q2, q3])
def quaternion_to_dcm(q):
"""クォータニオンから回転行列へ変換"""
q0, q1, q2, q3 = q
R = np.array([
[1-2*(q2**2+q3**2), 2*(q1*q2+q0*q3), 2*(q1*q3-q0*q2)],
[2*(q1*q2-q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3+q0*q1)],
[2*(q1*q3+q0*q2), 2*(q2*q3-q0*q1), 1-2*(q1**2+q2**2)]
])
return R
def quaternion_multiply(q1, q2):
"""クォータニオン積"""
a0, a1, a2, a3 = q1
b0, b1, b2, b3 = q2
return np.array([
a0*b0 - a1*b1 - a2*b2 - a3*b3,
a0*b1 + a1*b0 + a2*b3 - a3*b2,
a0*b2 - a1*b3 + a2*b0 + a3*b1,
a0*b3 + a1*b2 - a2*b1 + a3*b0
])
# 変換の検証
phi = np.radians(30) # ロール
theta = np.radians(45) # ピッチ
psi = np.radians(60) # ヨー
R_euler = euler_to_dcm(phi, theta, psi)
q = euler_to_quaternion(phi, theta, psi)
R_quat = quaternion_to_dcm(q)
print("=== 変換の検証 ===")
print(f"オイラー角: phi={np.degrees(phi):.1f}, theta={np.degrees(theta):.1f}, psi={np.degrees(psi):.1f}")
print(f"クォータニオン: {q}")
print(f"回転行列の差（ノルム）: {np.linalg.norm(R_euler - R_quat):.2e}")
# ジンバルロックの可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# ピッチ角による回転行列の条件数
pitch_angles = np.linspace(-89, 89, 1000)
cond_numbers = []
for th in pitch_angles:
R = euler_to_dcm(0, np.radians(th), 0)
cond_numbers.append(np.linalg.cond(R))
axes[0].plot(pitch_angles, cond_numbers, 'b-', linewidth=1)
axes[0].set_xlabel('Pitch Angle [deg]')
axes[0].set_ylabel('Condition Number')
axes[0].set_title('DCM Condition Number vs Pitch')
axes[0].grid(True)
# クォータニオンの球面線形補間（SLERP）
def slerp(q1, q2, t):
"""球面線形補間"""
dot = np.dot(q1, q2)
if dot < 0:
q2 = -q2
dot = -dot
if dot > 0.9995:
return q1 + t * (q2 - q1)
theta_0 = np.arccos(dot)
sin_theta = np.sin(theta_0)
s1 = np.sin((1 - t) * theta_0) / sin_theta
s2 = np.sin(t * theta_0) / sin_theta
return s1 * q1 + s2 * q2
q_start = euler_to_quaternion(0, 0, 0)
q_end = euler_to_quaternion(np.radians(90), np.radians(45), np.radians(30))
t_values = np.linspace(0, 1, 50)
euler_angles = []
for t in t_values:
q_interp = slerp(q_start, q_end, t)
R = quaternion_to_dcm(q_interp)
theta_interp = np.arcsin(-R[2, 0])
phi_interp = np.arctan2(R[2, 1], R[2, 2])
psi_interp = np.arctan2(R[1, 0], R[0, 0])
euler_angles.append([np.degrees(phi_interp),
np.degrees(theta_interp),
np.degrees(psi_interp)])
euler_angles = np.array(euler_angles)
axes[1].plot(t_values, euler_angles[:, 0], 'r-', label='Roll')
axes[1].plot(t_values, euler_angles[:, 1], 'g-', label='Pitch')
axes[1].plot(t_values, euler_angles[:, 2], 'b-', label='Yaw')
axes[1].set_xlabel('Interpolation Parameter t')
axes[1].set_ylabel('Euler Angle [deg]')
axes[1].set_title('SLERP Interpolation')
axes[1].legend()
axes[1].grid(True)
plt.tight_layout()
plt.show()
まとめ
本記事では、剛体の姿勢表現方法について解説しました。
- 回転行列は $3\times3$ の直交行列で厳密だが、9パラメータが必要
- オイラー角は3パラメータで直感的だが、ジンバルロック問題がある
- クォータニオンは4パラメータでジンバルロックがなく、宇宙機制御で広く使われる
- SLERP（球面線形補間）により、クォータニオンで滑らかな姿勢遷移を実現できる
- 各表現は相互に変換可能であり、用途に応じて使い分ける
次のステップとして、以下の記事も参考にしてください。