ベクトルの回転と座標回転の違い

ある座標においてベクトルを $\theta$ 回転させることと、ある座標とそれを $\theta$ 回転させた座標において、固定されたベクトルの座標表現がどのように変わるかの2つのケースを考えてみる。

固定座標でのベクトル回転（固定角表現とも呼ぶ）

座標Aにおいてベクトルaが $\vec{a}$ と表されていたとし、 $\theta$ 回転した後のベクトルを $\vec{b}$ とすると、

$\begin{eqnarray} \vec{b} = \left( \begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array} \right) \vec{a} \end{eqnarray}$

となる。

座標回転における固定ベクトルの表現（オイラー角表現とも呼ぶ）

座標系Aで $\vec{c}^{A}$ と表されていたベクトルが、座標系Bで $\vec{c}^{B}$ と表されるときの関係性を知りたい。この場合

$\begin{eqnarray} \vec{c}^{A} = \left( \begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array} \right) \vec{c}^{B} \end{eqnarray}$

となる。

考察

上の2つの回転の様子を比較してみると、ベクトル座標の方は回転後の座標表現が等号の左側にある一方で、座標回転の方は回転前の表現の方等号の左側にある。ベクトルの回転と座標の回転でこの順序が変わっていることがポイントである。

固定座標での連続ベクトル回転

X、Y、Zの順番で軸周りにベクトルを回転させる場合

$\begin{eqnarray} \vec{d} &=& R_{Z}\vec{c} \\ &=& R_{Z}R_{Y}\vec{b} \\ &=& R_{Z}R_{Y}R_{X}\vec{a} \\ \end{eqnarray}$

となり、回転演算の順序は $R=R_{Z}R_{Y}R_{X}$ となる。

連続座標回転における固定ベクトル表現

X、Y、Zの順番で軸周りに座標を回転させ固定ベクトル $\vec{a}$ を観測する場合、

$\begin{eqnarray} \vec{a}^{A} &=& R_{X}\vec{a}^{B} \\ &=& R_{X}R_{Y}\vec{a}^{C} \\ &=& R_{X}R_{Y}R_{Z}\vec{a}^{D} \\ \end{eqnarray}$

となり、回転演算の順序は $R=R_{X}R_{Y}R_{Z}$ となる。

オイラー角の表記方法

オイラー角 $(\theta, \phi, \psi)$ を使って姿勢を表現する方法には方法がたくさんある。 1. 回転角度の順番 2. 固定角表現かオイラー角表現か

については、連続で同じ回転軸を選択しなければよいので、 $3\times 2 \times 2 = 12$ 通りの選択肢がある。2.は2通りなので、合計24通りの表記方法がある。ただ、本章の議論で、固定角でXYZの順で表現された回転とオイラー角でZYXとして表現された回転は一致するので実質表現自由度は12である。

参考

http://www.st.nanzan-u.ac.jp/info/akiran/mces/mech_ctrl_eng_study_20160120.pdf

回転行列が直行行列であることの証明

ある座標 $F$ から $F'$ への回転行列を $R$ とする。 $F$ における基底ベクトルを $\vec{e}_{1}, \vec{e}_{2}, \vec{e}_{3}$ 、 $F'$ における基底ベクトルを $\vec{e}_{1}^{'}, \vec{e}_{2}^{'}, \vec{e}_{3}^{'}$ とする。それぞれの成分を $F$ の座標系で表すことにし（すなわち $\vec{e}_{1}^{'}, \vec{e}_{2}^{'}, \vec{e}_{3}^{'}$ はそれぞれ第1、第2、第3成分のみが1で他が0のベクトル）、回転行列の定義から

$\begin{eqnarray} \vec{e}_{1}^{'} = R\vec{e}_{1} \\ &=& \left( \begin{array}{ccc} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array} \right) \left( \begin{array}{c} 1 \\ 0 \\ 0 \end{array} \right) \\ &=& \left( \begin{array}{c} r_{11} \\ r_{21} \\ r_{31} \end{array} \right) \end{eqnarray}$

同様な考察から、 $\vec{e}_{i}^{'}$ の $n$ 番目の成分 $e_{in}^{'}$ は

$\begin{eqnarray} \vec{e}_{in}^{'} = r_{ni} \end{eqnarray}$

と表されることがわかる。これを利用すると

$\begin{eqnarray} \vec{e}_{i}^{'} \cdot \vec{e}_{j}^{'} &=& e_{in}^{'} e_{jn}^{'} \\ &=& r_{ni} r_{nj} \\ &=& (R^\mathrm{T})_{in} (R)_{nj} \\ &=& (R^\mathrm{T}R)_{ij} \end{eqnarray}$

これが $\delta_{i, j}$ となるべきことから、 $R^\mathrm{T}R$ が単位行列であること、すなわち回転行列が直行行列であることが示された。

参考

Why is the Rotation Matrix Orthogonal? | Classical Mechanics - YouTube

x, y, z軸周りの3次元回転の可換性

x,y,z軸周りの一般の3次元

$z$ 軸周りの角度 $\theta$ 回転を表す $Z_{\theta}$ 、 $y$ 軸周りの角度 $\phi$ 回転を表す $Y_{\phi}$ 、 $x$ 軸周りの角度 $\psi$ 回転を表す $X_{\psi}$ を考える。

$\begin{eqnarray} Z_{\theta} Y_{\phi} X_{\psi} &=& \left( \begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array} \right) \left( \begin{array}{ccc} \cos \phi & 0 & -\sin \phi \\ 0 & 1 & 0 \\ \sin \phi & 0 & \cos \phi \end{array} \right) \left( \begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \psi & -\sin \psi \\ 0 & \sin \psi & \cos \psi \end{array} \right) \\\ &=& \left( \begin{array}{ccc} \cos \theta \cos \phi & -\sin \theta \cos \psi - \cos \theta \sin \phi \sin \psi & \sin \theta \sin \psi - \cos \theta \sin \phi \cos \psi \\ \sin \theta \cos \phi & \cos \theta \cos \psi - \sin \theta \sin \phi \sin \psi & - \cos \theta \sin \phi - \sin \theta \sin \phi \cos \psi \\ \sin \phi & \cos \phi \sin \psi & cos \phi \cos \psi \end{array} \right) \end{eqnarray}$

一方

$\begin{equation} X_{\psi} Z_{\theta} Y_{\phi} = \left( \begin{array}{ccc} \cos \theta \cos \phi & -\sin \theta & -\cos \theta \sin \phi \\ \sin \theta \cos \phi \cos \psi - \sin \phi \sin \psi & \cos \theta \cos \phi & - \sin \theta \sin \phi \cos \psi - \cos \phi \sin \psi \\ \sin\theta \cos \phi \sin \psi + \sin \phi \cos \psi & \cos \theta \sin \psi & - \sin \theta \sin \phi \sin \psi + \cos \phi \cos \phi \end{array} \right) \end{equation}$

となり両者は一致しない。したがって3次元空間におけるx,y,z軸周りの回転は可換ではない。

回転角が小さい場合

$|\theta|, |\phi|, |\psi| \ll 1$ の場合、それぞれの1次の項まで展開すると

$\begin{eqnarray} Z_{\theta} Y_{\phi} X_{\psi} &\sim& \left( \begin{array}{ccc} 1 & -\theta & -\phi \\ \theta & 1 & \psi \\ \phi & \psi & 1 \end{array} \right) \\\ &\sim& X_{\psi} Z_{\theta} Y_{\phi} \end{eqnarray}$

となり両者は一致する。これは三角関数をテーラー展開すれば納得できることで、任意の行列が単位行列と可換であることから1次の項までは回転する順番に依存しないことがわかる。

$\begin{eqnarray} Z_{\theta} Y_{\phi} X_{\psi} &=& \left( I + \theta \left( \begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array} \right) + \mathcal{O}(\theta^{2}) \right) \left( I + \phi \left( \begin{array}{ccc} 0 & 0 & -1 \\ 0 & 0 & 0 \\ 1 & 0 & 0 \end{array} \right) + \mathcal{O}(\phi^{2}) \right) \left( I + \psi \left( \begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{array} \right) + \mathcal{O}(\psi^{2}) \right) \\\ &=& I + \theta \left( \begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array} \right) + \phi \left( \begin{array}{ccc} 0 & 0 & -1 \\ 0 & 0 & 0 \\ 1 & 0 & 0 \end{array} \right) + \psi \left( \begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{array} \right) + \cdots \end{eqnarray}$

角度が小さい時にオイラーとangle axis表現による姿勢表現が同じベクトルとして表される

角度が小さい時、固定角(1, 2, 3)表現によるオイラー角表現されたベクトル $(\theta, \psi, \phi)$ とangle axisによる回転表現 $(\theta n_{x}, \theta n_{y}, \theta n_{z})$ は一致する。証明の方針として、まず回転をクォータニオンで表現し、それを通じてオイラー角とangle axisに表現し直してどのように変換されるか調べてみる。また、「x, y, z軸周りの3次元回転の可換性」で述べたように回転角が小さい場合は回転の順番は可換になるため、以降の議論で回転の順番は気にしないこととする。

クォータニオンによる表現

$\begin{eqnarray} q &=& q_{z} \cdot q_{y} \cdot q_{x} \\ &=& \left( \begin{array}{c} cos(\frac{\theta}{2}) \\ \sin(\frac{\theta}{2}) \\ 0 \\ 0 \end{array} \right) \cdot \left( \begin{array}{c} cos(\frac{\psi}{2}) \\ 0 \\ \sin(\frac{\psi}{2}) \\ 0 \end{array} \right) \cdot \left( \begin{array}{c} cos(\frac{\phi}{2}) \\ 0 \\ 0 \\ \sin(\frac{\psi}{2}) \end{array} \right) \\ &\sim& \left( \begin{array}{c} \sqrt{1-(\frac{\theta}{2})^{2}} \\ \frac{\theta}{2} \\ 0 \\ 0 \end{array} \right) \cdot \left( \begin{array}{c} \sqrt{1-(\frac{\psi}{2})^{2}} \\ 0 \\ \frac{\psi}{2} \\ 0 \end{array} \right) \cdot \left( \begin{array}{c} \sqrt{1-(\frac{\phi}{2})^{2}} \\ 0 \\ 0 \\ \frac{\phi}{2} \end{array} \right) \\ &\sim& \left( \begin{array}{c} \sqrt{1-\frac{\theta^{2} + \psi^{2} + \phi^{2}}{4}} \\ \frac{\theta}{2} \\ \frac{\psi}{2} \\ \frac{\phi}{2} \end{array} \right) \\ &\sim& \left( \begin{array}{c} 1-\frac{1}{2} \left[ \frac{\sqrt{\theta^{2} + \psi^{2} + \phi^{2}}}{2} \right] ^{2} \\ \frac{\theta}{2} \\ \frac{\psi}{2} \\ \frac{\phi}{2} \end{array} \right) \end{eqnarray}$

オイラー角による表現

この資料の式(290)によれば、クォータニオンから固定角(1, 2, 3)表現のオイラー角への変換は

$\begin{eqnarray} \Theta &=& \mathrm{atan}2(2q_{2}q_{3}+2q_{0}q_{1}, q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2}) \\ \Phi &=& \sin^{-1} (2q_{1}q_{3}-2q_{0}q_{2}) \\ \Psi &=& \mathrm{atan}2(2q_{1}q_{2}+2q_{0}q_{3}, q_{1}^{2}-q_{0}^{2}-q_{3}^{2}-q_{2}^{2}) \end{eqnarray}$

元の資料と $(\theta, \psi, \phi)$ の定義が異なっていることに注意。ここではオイラー角を $(\Theta, \Phi, \Psi)$ と定義している。

したがって

$\begin{eqnarray} \Theta &\sim& \mathrm{atan}2(2\frac{\phi}{2}\frac{\psi}{2}+2\frac{\theta}{2}, 1) \\ &\sim& \mathrm{atan}2(\theta,1) \\ &\sim& \theta \\ \Phi &\sim& \sin^{-1} (2\frac{\theta}{2}\frac{\psi}{2}-2\frac{\phi}{2}) \\ &\sim& \sin^{-1}(\phi) \\ &\sim& \phi \\ \Psi &=& \mathrm{atan}2(2\frac{\theta}{2}\frac{\phi}{2}+2\frac{\psi}{2}, 1) \\ &\sim& \mathrm{atan}2(\psi, 1) \\ &\sim& \psi \end{eqnarray}$

となり、オイラー角は $(\theta, \phi, \psi)$ と表されることがわかった。

angle axisによる表現

この資料の式(198)と(199)によれば、クォータニオンからangle axisへの変換は

$\begin{eqnarray} \alpha = 2\cos^{-1}(q_{0}) \\ \vec{n} \propto \vec{q}_{1:3} \end{eqnarray}$

よってこの式から既にangle axisの方向ベクトル $\vec{n}$ は $(\theta, \phi, \psi)$ と一致していることがわかる。また、

$\begin{eqnarray} \alpha &\sim& 2\cos^{-1}(1-\frac{1}{2} \left[\frac{ \sqrt{ \theta^{2} + \phi^{2} + \psi^{2}}}{2} \right]^{2}) \\ &\sim& 2\cos^{-1}(\cos \left[ \frac{\sqrt{ \theta^{2} + \phi^{2} + \psi^{2}}}{2} \right]^{2} ) \\ &\sim& 2 \frac{\sqrt{\theta^{2} + \phi^{2} + \psi^{2}} }{2} \\ &\sim& \sqrt{\theta^{2} + \phi^{2} + \psi^{2}} \end{eqnarray}$

となるので、angle axis表現でも $(\theta, \phi, \psi)$ と表されることがわかった。

ジンバルロック

ジンバルロックとは、オイラー角で3次元の姿勢を表現した時にy軸周りの回転角が+-90度付近の姿勢に特異点が生じることによる不具合のことである。

そもそもオイラー角とは？

オイラー角とはz, y, xの3つの軸周りの回転 $\theta, \phi, \psi$ として任意の姿勢を表現する手法である。「x, y, z軸周りの3次元回転の可換性」の章で説明しているように、これらの回転は可換ではないので順番を定義する必要があり、ここではz軸、y軸、x軸の順番で $\theta, \phi, \psi$ だけ回転させることにする。このとき、 $0 \le \theta \lt 2\pi$ 、 $-\frac{\pi}{2} \le \phi \lt \frac{\pi}{2}$ 、 $0 \le \theta \lt 2\pi$ 、 $0 \le \psi \lt 2\pi$ の範囲を使って全ての姿勢を一意に表現できる。オイラー角可視化ツールを使ってその表現方法を説明していく。

初期状態では、 $\theta = 0$ 、 $\phi=0$ 、 $\psi=0$ である。このときの初期姿勢を以下とする。

ここで $\theta$ を増やしていくと、青（ $Z$ ）軸まわりに飛行機が回転していく。この状態では青軸はglobalとlocalのframeで一致していることに注意。

次に $\phi$ を増やしていくと、local frameの緑（ $Y$ ）軸まわりに飛行機が回転していく。この状態では緑軸はglobalとlocalのframeで一致していない。

最後に $\psi$ を増やしていくと、local frameの赤（ $X$ ）軸まわりに飛行機が回転していく。この状態では赤軸はglobalとlocalのframeで一致していない。

つまり $Z$ 、 $Y$ 、 $X$ 軸まわりの回転は全てlocal frameの軸周りの回転であると言うことができる。

注意点は $Z$ 、 $Y$ 、 $X$ の順で角度を0から動かしていく必要があるということである。例えば $\theta$ 、 $\psi$ が0でない状態で $\phi$ を回転させても、もはやそれはどの軸まわりの回転でもなくなってしまう。

ジンバルロックとは

ここまででオイラー角を使って任意の3次元姿勢を表現できることがわかったが、 $\phi=\pi/2$ 付近では1つの姿勢に対して複数の表現方法が存在する。例えば $\theta=50 ^\circ, \phi=\pi/2, \psi=50^\circ$ と $\theta=140 ^\circ, \phi=\pi/2, \psi=140^\circ$ は全く同じ姿勢を表していることになる。そして $\phi=\pi/2$ のときには $\theta$ を回転させた時の回転方向と $\psi$ を回転させた時の回転方向が全く同じになる。この現象がジンバルロックと呼ばれている

ジンバルロックの何が問題なのか？

ジンバルロックの問題点は2つ挙げられる。

$\phi=\pi/2$ 付近の姿勢をとっているときに、最適化などをするためにオイラー角で微分したときに、 $\theta$ と $\psi$ が同じ回転方向を表してしまうために微分ベクトルが一次独立でなくなってしまうこと
$\phi=\pi/2$ 付近では2つの姿勢間の補間が不自然になってしまうこと。例えば $\theta=0^\circ, \phi=80^\circ, \psi=180^\circ$ という姿勢と $\theta=180^\circ, \phi=80^\circ, \psi=0^\circ$ という姿勢の間を補間しようとしたとき、図のように飛行機が一回転してしまうのである！
姿勢角の補間が不自然になってしまう例

EigenのeulerAngles関数の動作の確認

EigenのMatrixクラスにはeulerAnglesという関数があり、例えば

const auto a = R.eulerAngles(2, 1, 0);

とすれば、a[0]がz軸周りの回転角、a[1]がy軸周りの回転角、a[2]がx軸周りの回転角を表す。すなわち、

using namespace Eigen;
Matrix3d R = AngleAxisd(psi, Vector3d::UnitZ()) * AngleAxisd(theta, Vector3d::UnitY()) * AngleAxisd(phi, Vector3d::UnitX());
const auto a = R.eulerAngles(2, 1, 0);

とすれば

a[0] = psi;
a[1] = theta;
a[2] = phi;

となる。ただし注意しないといけない点があり、Eigenのドキュメントに

The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].

と記載がある。すなわち、一番左の回転演算の回転角の定義域は $[0, \pi$ ]でないといけない。もしこの定義域から外れる角度を入力した場合、

a[0] = psi;
a[1] = theta;
a[2] = phi;

は成立しなくなる（ $\psi$ が一致しなくなるだけでなく、他の回転角も一致しなくなる）。特に一般的なロールピッチヨーの回転を考える場合、ピッチ（2番目の回転）の定義域が $[-\frac{\pi}{2}, \frac{\pi}{2}$ ]となっていることが多いので、eulerAnglesの関数で取得しても所望の角度が取得できないことに注意。

salpikのブログ

回転に関するまとめ