[数学的知識] 3次元空間回転行列のオイラー角表現、四元数表現、両者間の変換、Matlabコード実装

シリアルナンバー	コンテンツ
1	【数学的知識】自由度と自由度の計算方法
2	【数学的知識】剛体 剛体と剛体の運動
3	【数学的知識】剛体の基本運動、平行移動、回転
4	【数学的知識】ベクトル乗算、内積、外積、MATLABコード実装
5	[数学的知識] 最小二乗法、線形回帰から始まり、数値例を示し、最小二乗法を使用して回帰モデルを解く
6	【数学的知識】最小二乗法、一般線形状況、行列表現過程、最適パラメータ解公式過程
7	【数学的知識】共分散、確率変数の共分散、確率変数がそれぞれ単一の数値とベクトルの場合の共分散
8	【数学的知識】データの線形変換の観点から理解する特異値分解
9	[数学的知識] 回転行列の導出プロセスは、ユークリッド変換の非線形制限を解決しながら、ベクトルの回転に基づいています。
10	[数学的知識] 3次元空間回転行列のオイラー角表現、四元数表現、両者間の変換、Matlabコード実装
11	[数学的知識] 前後のモーメントにおける N>=3 点の座標が与えられたとき、剛体の並進行列と回転行列を求めます。これらの N>=3 点間の距離は常に変化せず、剛体を表します。

回転行列については以前に説明しました。繰り返しになりますが、回転の順序は重要であり、最終結果に影響します。最初に$X$軸、 YY回転$Y$軸、最終回転$Z$軸で得られる結果は、$Z$軸、 YY回転$Y$軸、最後の回転$X$軸で得られる結果は異なります。この順序の違いにより、方向の違いや空間方向の変化が生じます。このため、実際のアプリケーションでは、正しく一貫した結果が得られるように、回転順序を明示的に指定する必要があります。

今回は、3次元空間をベースに、オイラー角表現とクォータニオン表現という回転行列の2つの表現方法と、両者の変換関係について説明します。

3 次元空間では、回転行列$R$の次元は3 × 3 3 \times 3です$3\times 3$ 、行列式が1 1の直交行列です。$1$。

1. オイラー角の表現

オイラー角は通常 3 つの角で構成されます

• ロール角（ロール）、一般的な記号は$\varphi$
• ピッチ角 (ピッチ)、一般的な記号は$私$
• ヨー角 (yaw)、一般的な記号は$p$

これら 3 つの角度は、それぞれX、Y、ZX、Y、Z周りの角度を表します。$\times 、やあ、Z$軸の回転角度。

XX頃$X$軸回転$\varphi$角度の回転行列は次のとおりです。

$$R_{}\left(\varphi \right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\varphi \right)& -罪\left(\varphi \right)\\ & 罪(\varphi & \cos (\varphi \end{array}\right]$$

绕$Y$軸回転$角度\theta$の回転行列は次のようになります。

$$R_{}\left(私\right)=\left[\begin{array}{ccc}\cos \left(\theta \right)& 0& 罪\left(\theta \right)\\ 0& 1& 0\\ -\sin (\theta & & \cos (\theta \end{array}\right]$$

绕 $Z$軸回転$\psi$角度の回転行列は次のようになります。

$$R_{}\left(p\right)=\left[\begin{array}{ccc}\cos \left(\psi \right)& -罪\left(\psi \right)& 0\\ 罪\left(\psi \right)& \cos \left(\psi \right)& 0\\ & & \end{array}\right]$$

たとえば、固定軸$XYZ$のオイラー角表現、その回転行列は次のとおりです。

$$\begin{array}{rl}R& =R_{}\left(\varphi \right)R_{}\left(i\right)R_{}\left(p\right)\\ & =\left[\begin{array}{ccc}\cos \left(\theta \right)\cos \left(\psi \right)& 罪\left(\varphi \right)罪\left(\theta \right)\cos \left(\psi \right)-\cos \left(\varphi \right)罪\left(\psi \right)& \cos \left(\varphi \right)罪\left(\theta \right)\cos \left(\psi \right)+罪\left(\varphi \right)罪\left(\psi \right)\\ \cos \left(\theta \right)罪\left(\psi \right)& 罪\left(\varphi \right)罪\left(\theta \right)罪\left(\psi \right)+\cos \left(\varphi \right)\cos \left(\psi \right)& \cos \left(\varphi \right)罪\left(\theta \right)罪\left(\psi \right)-罪\left(\varphi \right)\cos \left(\psi \right)\\ -\sin (\theta & 罪\left(\varphi \right)\cos (\theta & \cos \left(\varphi \right)\cos (\theta \end{array}\right]\end{array}$$

この行列は、 XX付近の最初の時刻を表します。$X$軸回転$\varphi$角度、その後YY付近$Y$軸回転$角度\theta$ 、その後ZZ付近$Z$軸回転$\psi$角度の全体的な回転効果

オイラー角の導出と詳細については、次の記事を参照してください:第 3 章 - 数学的知識の基礎 ->ベクトルの回転に基づく座標変換と[数学的知識] 回転行列の導出プロセスユークリッド変換限界の非線形性。

% 给定欧拉角 phi theta psi
phi   = deg2rad(10);  % 示例：10度，记得转换为弧度
theta = deg2rad(22);  % 示例：22度
psi   = deg2rad(35);  % 示例：35度

R_x = [ 1  0         0
        0  cos(phi) -sin(phi)
        0  sin(phi)  cos(phi)];

R_y = [ cos(theta)  0  sin(theta)
        0           1  0
       -sin(theta)  0  cos(theta)];

R_z = [ cos(psi) -sin(psi)  0
        sin(psi)  cos(psi)  0
        0         0         1];

R = R_z * R_y * R_x;

R = [cos(theta)*cos(psi)  sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)  cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)
     cos(theta)*sin(psi)  sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi)  cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)
    -sin(theta)           sin(phi)*cos(theta)                             cos(phi)*cos(theta)];

R =
    0.7595   -0.5116    0.4018
    0.5318    0.8440    0.0694
   -0.3746    0.1610    0.9131

---

% 给定点坐标
point_1 = [ 10
            22
            35];
point_2 = R * point_1;

figure()
scatter3(point_1(1), point_1(2), point_1(3), 150, 'r'); hold on;
scatter3(point_2(1), point_2(2), point_2(3), 150, 'b');

---

2. クォータニオン表現

クォータニオンは$1$実数プラス$3\; つの$複素数で構成され、通常は次のように表現できます。

$$q=q_{}+q_{}\text{私}+q_{}\text{j}+q_{}\text{k}$$

その中$q_{}、q_{}、q_{}、q_{}$これらはすべて実数$\text{i、j、k は}$クォータニオンのプリミティブであり、以下に示す乗算関係を満たします。

• ${\text{私}}^2={\text{j}}^2={\text{k}}^2=\text{私は}\text{jk}\text{です}=-1$
• ${\text{私}}^0={\text{j}}^0={\text{k}}^0=1$

クォータニオンは、スカラーとベクトルで構成されるとみなすこともできます。ここで、$q_{}$クォータニオンのスカラー部分$q_{}、q_{}、q_{}$クォータニオンのベクトル部分を構成します。

---

それぞれ 2 つの四元数があるとします。$q_{}=(q_{w1}、[q_{\times }、q_{y1}、q_{}])$ ,$q_{}=(q_{w2}、[q_{\times }、q_{y2\_}、q_{z2}])$として、$v_{}=[q_{\times }、q_{y1}、q_{}]$，$v_{}=[q_{\times }、q_{y2\_}、q_{z2}]$の場合、次のアルゴリズムがあります

• クォータニオンの合計: $q_{}+q_{}=(q_{w1}+q_{w2}、(v_{}+v_{}))$
• 四元数乘法：$q_{}{q}_{}=q_{w1}{q}_{w2}-v_{}\cdot v_{}+q_{w1}{v}_{}+q_{w2}{v}_{}+v_{}\times v_{}=(q_{w1}{q}_{w2}-v_{}\cdot v_{}、(q_{w1}{v}_{}+q_{w2}{v}_{}+v_{}\times v_{}))$
• クォータニオンのモジュール: $\Vert q{\_}_{}\Vert =\sqrt{q_{w1\_}^{}+q_{\times 1}^{}+q_{y1\_}^{}+q_{1で}^{}}$
• 単位四元数: $\Vert q{\_}_{}\Vert =1$
• クォータニオンの共役: $q_1^{}=(q_{w1}、-v{\_}_{})$
• クォータニオンの逆: $q_1^{-}=\frac{q_1^{}}{\Vert q{\_}_{}\Vert }$

---

クォータニオンは、3 次元空間での回転を表すために一般的に使用される拡張複素数系です。

単位四元数 (長さ$1$ ) 3D 空間での回転を表現できます。点を別の位置に回転するには、クォータニオンの乗算を使用します。

XX頃$X$軸回転ϕ \phi角度$\varphi の四元数は次のとおりです。$

$$q_{}=\left(\cos \right(\frac{}{2}）、罪（\frac{}{2}）、0、0)$$

绕$Y$軸回転θ \theta角度$\theta の四元数は次のようになります。$

$$q_{}=\left(\cos \right(\frac{}{2}）、0、罪（\frac{}{2}）、0、0)$$

绕 $Z$軸回転$\psi$角度の四元数は次

$$q_{}=\left(\cos \right(\frac{}{2}）、0、0、罪（\frac{}{2}))$$

回転された四元数の合計は、これら 3 つの四元数の積です。クォータニオン乗算は通常のスカラー乗算ではなく、特定の乗算ルールがあります。

クォータニオン$q$ (モジュールの長さは 1 で、$\sqrt{q_w^{}+q_{バツ}^{}+q_y^{}+q_z^{}}=1$ ) 採用された回転順序が$XYZ$、それに対応する回転行列$R\; は$次のように表すことができます。

$$\begin{array}{rl}& =\left[\begin{array}{ccc}1-2(q_y^{}+q_z^{})& 2(q_{}{q}_{}-q_{}{q}_{})& 2(q_{}{q}_{}+q_{}{q}_{})\\ 2(q_{}{q}_{}+q_{}{q}_{})& 1-2(q_{バツ}^{}+q_z^{})& 2(q_{}{q}_{}-q_{}{q}_{})\\ 2(q_{}{q}_{}-q_{}{q}_{}& 2(q_{}{q}_{}+q_{}{q}_{}& 1-2(q_{バツ}^{}+q_y^{}\end{array}\right]\end{array}$$

単位四元数には、3D 回転を記述するときにいくつかの利点があり、オイラー角の「ジンバル ロック」問題の影響を受けません。

% 给定四元数
quaternion = [0.9376  0.0244  0.2070  0.2782];  

q_w = quaternion(1);
q_x = quaternion(2);
q_y = quaternion(3);
q_z = quaternion(4);

% 计算旋转矩阵 R
R(1,1) = 1 - 2*(q_y^2 + q_z^2);
R(1,2) = 2*(q_x*q_y - q_w*q_z);
R(1,3) = 2*(q_x*q_z + q_w*q_y);
R(2,1) = 2*(q_x*q_y + q_w*q_z);
R(2,2) = 1 - 2*(q_x^2 + q_z^2);
R(2,3) = 2*(q_y*q_z - q_w*q_x);
R(3,1) = 2*(q_x*q_z - q_w*q_y);
R(3,2) = 2*(q_y*q_z + q_w*q_x);
R(3,3) = 1 - 2*(q_x^2 + q_y^2);

R =
    0.7595   -0.5116    0.4017
    0.5318    0.8440    0.0694
   -0.3746    0.1609    0.9131

---

3. クォータニオンをオイラー角に変換する

特定の回転ではオイラー角の表現が複数あるため、クォータニオンからオイラー角への変換は一意ではありません。ただし、ほとんどの実際的なアプリケーションでは、特定のオイラー角のセットを特定の四元数から計算できます。

q = ( qw , qx , qy , qz ) q = ( q_w , q_x , q_y , q_z ) を構築します。$q=(q_{}、q_{}、q_{}、q_{})$ 、 XYZ XYZに変換したい場合$XYZ$次$(\varphi ,私、\psi )$。クォータニオンからオイラー角に変換する方法は次のとおりです。

$$\begin{array}{rl}\varphi & =\text{atan2}\left(2\right(q_{}{q}_{}+q_{}{q}_{}）、1-2(q_{バツ}^{}+q_y^{}))\\ 私& =\arcsin \left(2\right(q_{}{q}_{}-q_{}{q}_{}))\\ & =\text{atan2}\left(2\right(q_{}{q}_{}+q_{}{q}_{}）、1-2(q_y^{}+q_z^{})\end{array}$$

ここで、$\text{atan2}()\; は$arctanではありません$\mathrm{アークタン}\left(\right)$。

たとえば、$\arctan (y/x)$、その戻り値は$[-\pi /2,\pi /2]$ 、 xxを区別できないため$x$の正の値と負の値。そして$\text{atan2}(y,x)$、その戻り値は$[-\pi ,\pi ]$ 、 xxを区別できます$x$の符号なので、特にオイラー角を計算する場合に、より実用的です。さらに重要なのは、$\text{atan2}(y,x)$はx = 0 x=0を処理できます$バツ=0$。角度またはオイラー角を計算する場合に便利です。

arcsin ⁡ ( ) \arcsin()を使用しているため、注意してください。$\arcsin ()$，当 $\theta$は±90°に近い$\pm 90°$では、数値が不安定になる可能性があります。これは、このような極端なケースでは、ヘディングとロールが区別できなくなるためであり、これはジンバル ロック問題として知られています。

% 给定四元数
quaternion = [0.9376  0.0244  0.2070  0.2782];  

q_w = quaternion(1);
q_x = quaternion(2);
q_y = quaternion(3);
q_z = quaternion(4);

% 转换四元数到欧拉角
phi   = atan2(2*(q_w*q_x + q_y*q_z), 1 - 2*(q_x^2 + q_y^2));
theta = asin(2*(q_w*q_y - q_z*q_x));
psi   = atan2(2*(q_w*q_z + q_x*q_y), 1 - 2*(q_y^2 + q_z^2));

% 如果需要角度形式而不是弧度，可以转换为度
phi_deg   = rad2deg(phi);
theta_deg = rad2deg(theta);
psi_deg   = rad2deg(psi);

phi_deg =
    9.9953

theta_deg =
   21.9990

psi_deg =
   34.9983

---

4. オイラー角を四元数に変換する

3 つのオイラー角$\varphi 、私、\psi$、対応する四元数は次のとおりです。

$$\begin{array}{rl}{q}_{}& =\cos \left(\frac{}{2}\right)\cos \left(\frac{}{2}\right)\cos \left(\frac{}{2}\right)+罪（\frac{}{2})罪（\frac{}{2})罪（\frac{}{2})\\ q_{}& =罪（\frac{}{2}\left)\cos \right(\frac{}{2}\left)\cos \right(\frac{}{2})-\cos (\frac{}{2})罪（\frac{}{2})罪（\frac{}{2})\\ q_{}& =\cos \left(\frac{}{2}\right)罪（\frac{}{2}\left)\cos \right(\frac{}{2})+罪（\frac{}{2})\cos \left(\frac{}{2}\right)罪（\frac{}{2})\\ q_{}& =\cos \left(\frac{}{2}\right)\cos \left(\frac{}{2}\right)罪（\frac{}{2})-罪（\frac{}{2})罪（\frac{}{2}\left)\cos \right(\frac{}{2}\end{array}$$

結果の四元数は$(q_{}、q_{}、q_{}、q_{}）$。

phi   = deg2rad(10);  % 示例：10度，记得转换为弧度
theta = deg2rad(22);  % 示例：22度
psi   = deg2rad(35);  % 示例：35度

% 计算四元数
q_w = cos(phi/2) * cos(theta/2) * cos(psi/2) + sin(phi/2) * sin(theta/2) * sin(psi/2);
q_x = sin(phi/2) * cos(theta/2) * cos(psi/2) - cos(phi/2) * sin(theta/2) * sin(psi/2);
q_y = cos(phi/2) * sin(theta/2) * cos(psi/2) + sin(phi/2) * cos(theta/2) * sin(psi/2);
q_z = cos(phi/2) * cos(theta/2) * sin(psi/2) - sin(phi/2) * sin(theta/2) * cos(psi/2);

quaternion = [q_w, q_x, q_y, q_z];

quaternion =
    0.9376    0.0244    0.2070    0.2782

---

参照

1. 回転行列 - ウィキペディア
2. 役立つ情報のコレクション: オイラー角、回転行列、四元数