MATLABの回転行列の関数で入出力の前後で値が一致しない！？回転行列と固定角の変換【学習#1】

pall@資格を取りたい機械系修士2年

2024年12月8日 00:25

研究の中で、MATLABの回転行列から姿勢の回転角度を算出する関数を使用していた際に

【変換①】姿勢の回転角度 ⇒ 回転行列
【変換②】回転行列 ⇒ 姿勢の回転角度

のように２段階の変換をしたときに「変換①で入力した回転角度」と「変換②で出力された回転角度」で値が異なる場合がありました。

なぜ値が変わってしまうのか気になったので、3次元空間の物体の回転を表す際に使用する回転行列と固定角の変換のための数式についてまとめました。MATLABで実装もしてみました。

※※※間違いもあるかもしれません※※※

1. 固定角とオイラー角

勘違いしやすい用語について確認しておきます。回転行列とセットで使用されやすい用語に、今回対象とする固定角以外に、オイラー角があります。

インターネットで回転行列について調べると固定角とオイラー角の定義が混同されて使われている記事があったり、論文では明確に固定角を使っているのかオイラー角を使っているのか明言されていなかったりする場合があるので、回転行列を扱う際は注意する必要があります。

今回の記事では変換の式を主としているため、それぞれの定義について詳しく述べませんが、簡単な違いは以下のようになります。
　固定角：回転軸が変化しない（回転軸が地面に固定）。
　オイラー角：回転軸が物体の回転ともに変化する。
詳しい違いについては以下のインターネットの記事、書籍で説明されています。

オイラー角に潜む5つの罠

ロボット工学　著　関口叡範

2. MATLABの関数で入力した角度と出力される角度が一致しない

私が回転行列と固定角の式について勉強したいと思った動機です。以下のようなコードで
【変換①】姿勢の回転角度 ⇒ 回転行列
【変換②】回転行列 ⇒ 姿勢の回転角度
を行いました。

%% 指定された角度（度単位）
z_deg = 50;  % Z軸回り
y_deg = 60;  % Y軸回り
x_deg = 70;  % X軸回り
% 角度をラジアンに変換
z_rad = deg2rad(z_deg);
y_rad = deg2rad(y_deg);
x_rad = deg2rad(x_deg);

%% 回転行列を計算 (固定角:'ZYX'順序)
R = eul2rotm([z_rad, y_rad, x_rad], 'ZYX');

%% 回転行列から固定角を逆算
angles_matlab = rotm2eul(R, 'ZYX');


%% 結果の表示（度単位に戻す）
disp('MATLAB rotm2eul() の出力:');
disp(['γ:', num2str(rad2deg(angles_matlab(1))), '°,' ...
      'β:', num2str(rad2deg(angles_matlab(2))), '°,' ...
      'α:', num2str(rad2deg(angles_matlab(3))), '°']);

はじめに姿勢の回転角度を設定し、MATALABのeul2rotm()で回転行列を算出します。その算出した回転行列をrotm2eul()により再度回転角度に変換し、値を出力するコードです。

ここで、回転行列の算出時の固定角の回転の定義は次のようにしています。

$${\mathrm{X}}$$軸まわりの回転角度$${\alpha}$$、$${\mathrm{Y}}$$軸まわりの回転角度$${\beta}$$、$${\mathrm{Z}}$$軸まわりの回転角度$${\gamma}$$とし、$${\alpha \rightarrow \beta \rightarrow \gamma}$$の順で回転させるとき、つまり$${\mathrm{X}}$$軸$${\rightarrow \mathrm{Y}}$$軸$${\rightarrow \mathrm{Z}}$$軸まわりの順の回転行列

そのため、先述のコードでは、

R = eul2rotm([z_rad, y_rad, x_rad], 'ZYX');

のように回転行列を掛ける順番の引数を'ZYX'としています。ここで「$${\mathrm{X}}$$軸$${\rightarrow \mathrm{Y}}$$軸$${\rightarrow \mathrm{Z}}$$軸まわりの順」ですが'ZYX'になる点は注意です。

少し長くなりましたが、先述のコードを実行した結果です。最初に指定する回転角度の設定を次の３通りで行い、実行した結果です。

①$${\gamma = 50°}$$、$${\beta = 60°}$$、$${\alpha = 70°}$$
入力と出力がすべて同じです。

②$${\gamma = 250°}$$、$${\beta = 60°}$$、$${\alpha = 70°}$$
$${\gamma}$$のみ異なる値です。

③$${\gamma = 50°}$$、$${\beta = 90°}$$、$${\alpha = 70°}$$
$${\beta}$$以外が異なる値です。

上記の結果からわかるように、入力値と出力値が一致しない場合があることがわかります。なぜこのようになるのかを理解するために、変換の式を知りたいと考えました。

③の$${\beta = 90°}$$は、ジンバルロックという状態になっています。3つの回転軸のうち2つが特定の角度で重なり、1つの回転自由度が失われる現象です。文章では理解が難しいですが、実際にイラストや動画の説明を見るとわかりやすいと思います。

3. 回転行列 ⇔ 固定角の変換式

今回、変換の式を調べたところ、以下の論文が見つかりました。

Gregory G Slabaugh, “Computing euler angles from a rotation matrix,” Retrieved on August, vol. 6, no. 2000, pp. 39–63, 1999.

$${\alpha \rightarrow \beta \rightarrow \gamma}$$の順で回転させるときの固定角、回転行列の式です。回転の順によって式が変わる点は注意です。

固定角 ⇒ 回転行列

回転行列を順に掛けることで算出できる。
$${\mathrm{X}}$$軸まわりの回転は

$${R_x(\alpha) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix}}$$

$${\mathrm{Y}}$$軸まわりの回転は

$${R_y(\beta) = \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix}}$$

$${\mathrm{Z}}$$軸まわりの回転は

$${R_z(\gamma) = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix}}$$

$${\alpha \rightarrow \beta \rightarrow \gamma}$$の順で回転させるときの回転行列は

$${\small{\begin{aligned} R &= R_z(\gamma) R_y(\beta) R_x(\alpha) \\ &= \begin{bmatrix} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha - \sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha + \sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha + \cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha - \cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{bmatrix} \end{aligned}}}$$

回転行列 ⇒ 固定角

前述した回転行列$${R}$$を以下のように表す。

$${R = \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{bmatrix}}$$

回転行列から固定角の変換は、$${\beta}$$の値で場合分けされる。$${\beta = \pm\frac{\pi}{2}}$$の場合はジンバルロックにより、自由度が1つ失われ、値が一意ではなくなる。そのため、(b.1)(b.2)に示すように1つの角度を固定して計算される。

(a) $${\beta \neq \pm \frac {\pi}{2}}$$（$${R_{31} \neq \pm 1}$$）の場合
三角関数の周期性により$${\beta}$$は2通りで表現される。

$$
\beta_1 = -\arcsin(R_{31})
$$

$$
\alpha_1 = \text{atan2}\left(\frac{R_{32}}{\cos \beta_1}, \frac{R_{33}}{\cos \beta_1}\right)
$$

$$
\gamma_1 = \text{atan2}\left(\frac{R_{21}}{\cos \beta_1}, \frac{R_{11}}{\cos \beta_1}\right)
$$

$$
\beta_2 = \pi - \beta_1
$$

$$
\alpha_2 = \text{atan2}\left(\frac{R_{32}}{\cos \beta_2}, \frac{R_{33}}{\cos \beta_2}\right)
$$

$$
\gamma_2 = \text{atan2}\left(\frac{R_{21}}{\cos \beta_2}, \frac{R_{11}}{\cos \beta_2}\right)
$$

(b.1) $${\beta = \frac{\pi}{2}}$$（$${R_{31} = 1}$$）の場合

$$
\beta = \frac{\pi}{2}
$$

$$
\alpha = \gamma + \text{atan2}(R_{12}, R_{13})
$$

(b.2) $${\beta = -\frac{\pi}{2}}$$（$${R_{31} = -1}$$）の場合

$$
\beta = -\frac{\pi}{2}
$$

$$
\alpha = -\gamma + \text{atan2}(-R_{12}, -R_{13})
$$

2節の計算結果の確認

ジンバルロックの状態ではない(a)の場合、算出される角度の候補は2通りあります。MATLABの関数rotm2eul()では、$${\beta_1 = -\arcsin(R_{31})}$$とした場合の計算がされています（次の4節でMATLABの関数と自作したコードによる計算結果の比較より）。

ジンバルロックの状態のときは(b.1)(b.2)であり、$${\gamma}$$次第で無限の角度の候補があります。数値計算では$${\gamma}$$を任意の定数で固定することで、角度の候補を2通りにしています。２節で例示した③の条件（ジンバルロックの状態）のコードの実行結果を確認してみます。

【入力】$${\gamma = 50°}$$、$${\beta = 90°}$$、$${\alpha = 70°}$$
【出力】$${\gamma = 0°}$$、$${\beta = 90°}$$、$${\alpha = 20°}$$

となっていることから、MATLABの関数rotm2eul()では$${\gamma = 0°}$$として固定していると考えると、上記のような結果が得られます。

次に②の条件の実行結果です。$${\gamma = 250°}$$が$${\gamma = -110°}$$となっています。

【入力】$${\gamma = 250°}$$、$${\beta = 60°}$$、$${\alpha = 70°}$$
【出力】$${\gamma = -110°}$$、$${\beta = 60°}$$、$${\alpha = 70°}$$

これは三角関数の周期性の特性が原因です。【変換①】姿勢の回転角度 ⇒ 回転行列の際に、

$${R = R_z(\gamma) R_y(\beta) R_x(\alpha)}$$

を計算します。このときに$${\mathrm{Z}}$$軸まわりの回転は

$$
R_z(\gamma) = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix}
$$

となりますが、三角関数の周期性より、この式に$${\gamma = 250°}$$を代入した場合と$${\gamma = -110°}$$は同じ計算結果となります。【変換①】の時点で$${\gamma = -110°+n×360°}$$の候補がある状態となりますが、【変換②】の式である

$$
\gamma_1 = \text{atan2}\left(\frac{R_{21}}{\cos \beta_1}, \frac{R_{11}}{\cos \beta_1}\right)
$$

の$${\text{atan2}()}$$の解の範囲は$${[-\pi, \pi]}$$であるため、$${\gamma = -110°}$$となります。

4. MATLABで実装

前述の数式をもとにMATLABで変換用のコードを自作し、MATLABに実装されている関数と出力を比較してみました。

MATLABの関数で定義されている式の確認

3節でも述べましたが、(a)は2通りの角度の候補があるので、どちらが採用されているのかを確認します。確認に使用したコードです。

clear

%% 指定された角度
z_deg = 50;  % Z軸回り
y_deg = 60;  % Y軸回り
x_deg = 70;  % X軸回り
% 角度をラジアンに変換
z_rad = deg2rad(z_deg);
y_rad = deg2rad(y_deg);
x_rad = deg2rad(x_deg);

%% 回転行列を計算 (固定角:'ZYX'順序)
R = eul2rotm([z_rad, y_rad, x_rad], 'ZYX');

%% MATLABのeul2rotm関数を使用して固定角を逆算
angles_matlab = rotm2eul(R, 'ZYX');

%% 実装したコード
if R(3,1) ~= 1 && R(3,1) ~= -1
    theta1 = -asin(R(3,1));
    psi1 = atan2(R(3,2)/cos(theta1), R(3,3)/cos(theta1));
    phi1 = atan2(R(2,1)/cos(theta1), R(1,1)/cos(theta1));
    theta2 = pi - theta1;
    psi2 = atan2(R(3,2)/cos(theta2), R(3,3)/cos(theta2));
    phi2 = atan2(R(2,1)/cos(theta2), R(1,1)/cos(theta2));
end


%% 結果の表示
disp('MATLAB eul2rotm() の出力:');
disp(rad2deg(angles_matlab));
if exist('theta1', 'var')
    disp('実装したコードの出力:');
    disp(['γ:', num2str(rad2deg(phi1)), ', β:', num2str(rad2deg(theta1)), ', α:', num2str(rad2deg(psi1))]);
    disp(['γ:', num2str(rad2deg(phi2)), ', β:', num2str(rad2deg(theta2)), ', α:', num2str(rad2deg(psi2))]);
else
    disp(['Theta: ', num2str(rad2deg(theta)), ', Psi: ', num2str(rad2deg(psi))]);
end

if文の箇所が回転行列から固定角を変換するコードです。次に示すコードでは追加してありますが、実際はジンバルロックの状態のときの処理をelseで行う必要があります。

実行結果が以下の通りです。結果から$${\beta_1 = -\arcsin(R_{31})}$$により計算されていることがわかります。

MATLABの関数と自作したコードの比較

次に3つの条件で固定角の入力値を設定した場合を比較しました。使用したコードです。

clear

% 指定された角度（度単位）
z_deg = 250;  % Z軸回り
y_deg = 60;  % Y軸回り
x_deg = 70;  % X軸回り

z_deg = 50;  % Z軸回り
y_deg = 90;  % Y軸回り
x_deg = 70;  % X軸回り

z_deg = 50;  % Z軸回り
y_deg = 190;  % Y軸回り
x_deg = 230;  % X軸回り

% 角度をラジアンに変換
z_rad = deg2rad(z_deg);
y_rad = deg2rad(y_deg);
x_rad = deg2rad(x_deg);

%% 回転行列を計算 (固定角:'ZYX'順序)
R = eul2rotm([z_rad, y_rad, x_rad], 'ZYX');

% MATLABのeul2rotm関数を使用して固定角を逆算
angles_matlab = rotm2eul(R, 'ZYX');

%% 実装したコード
if R(3,1) ~= 1 && R(3,1) ~= -1
    theta1 = -asin(R(3,1));
    psi = atan2(R(3,2)/cos(theta1), R(3,3)/cos(theta1));
    phi = atan2(R(2,1)/cos(theta1), R(1,1)/cos(theta1));
else
    phi = 0; % 任意の値
    if R(3,1) == -1
        theta1 = pi/2;
        psi = phi + atan2(R(1,2), R(1,3));
    else
        theta1 = -pi/2;
        psi = -phi + atan2(-R(1,2), -R(1,3));
    end
end


% 結果の表示
disp('MATLAB eul2rotm() の出力:');
disp(rad2deg(angles_matlab));
if exist('theta1', 'var')
    disp('実装したコードの出力:');
    disp(['γ:', num2str(rad2deg(phi)), ', β:', num2str(rad2deg(theta1)), ', α:', num2str(rad2deg(psi))]);
else
    disp(['Theta: ', num2str(rad2deg(theta1)), ', Psi: ', num2str(rad2deg(psi))]);
end

①$${\gamma = 250°}$$、$${\beta = 60°}$$、$${\alpha = 70°}$$

②$${\gamma = 50°}$$、$${\beta = 90°}$$、$${\alpha = 70°}$$

③$${\gamma = 50°}$$、$${\beta = 190°}$$、$${\alpha = 230°}$$

よって全ての条件でMATLABの関数と自作したコードの計算結果が一致しました。

6. さいごに

回転行列と固定角の変換において値が一致しないのはなぜかという点に興味を持ち、文献を調べてMATLABの関数の中身を知ることができました。

rotm2eul()ですが、MATLABのTool Boxを追加で購入しないと使えない関数なので、今回のコードを基に関数を自作すると何かと役に立つかもしれないですね。ただし、rotm2eul()のメリットとしては、N個の回転行列を一度に引数とすると、まとめて計算してくれる優れものです。

実際に変換したい回転行列が多い場合に、自作関数をループ計算で使用するよりも、rotm2eul()を使用した方が計算速度は速くなりました。以下のようにすると、一度に計算することができます。

% 100 個の 3x3 回転行列を作成
N = 100; % 回転行列の数
rotmArray = zeros(3, 3, N); % 初期化

% 回転行列をランダム生成（例: 固定軸の回転行列）
for i = 1:N
    theta = rand() * 2 * pi; % ランダムな回転角 (0 ~ 2π)
    rotmArray(:,:,i) = [cos(theta) -sin(theta) 0;
                        sin(theta)  cos(theta) 0;
                             0          0      1]; % Z 軸回りの回転
end

% オイラー角への変換
eulAngles = rotm2eul(rotmArray, 'ZYX'); % ZYX 順で変換

% 結果の表示
disp(eulAngles);

3×3×Nの行列を引数とすると、N×3の固定角の行列を得ることができます。そのため、お金に余裕があればTool Boxを購入してMATLABの関数を使用することをお勧めします！

【2024/12/08 追記】rotm2eul()の計算時間を比較した記事です

7. 参考にした文献

今回参考にした文献です。

Gregory G Slabaugh, “Computing euler angles from a rotation matrix,” Retrieved on August, vol. 6, no. 2000, pp. 39–63, 1999.

https://scholar.google.co.jp/citations?view_op=view_citation&hl=ja&user=oUK2gu8AAAAJ&citation_for_view=oUK2gu8AAAAJ:2P1L_qKh6hAC

ロボット工学　著　関口叡範