回転行列からオイラー角のパラメータ抽出を行う

...回転行列からオイラー角のパラメータ抽出を行いたいって要望って意外に高いんですね

個人的には、あまり意味が無いと思うのですが、一応まとめときます。

ってか、元のパラメータは一意に求めることが出来ないんです！！

その辺り、ちゃんとわかってます？

まず、回転行列をヨーピッチロール行列だと仮定します。

つまり

$\LARGE R = R_z R_x R_y$
ここで、 $R$ は回転行列、 $R_x$ はx軸周りの回転行列、 $R_y$ はy軸周りの回転行列、 $R_z$ はz軸周りの回転行列とします。

復習のため、それぞれの回転行列を書いておきます

$\LARGE R = \begin{pmatrix}m_{00} & m_{01} & m_{01} \\ m_{10} & m_{11} & m_{12} \\ m_{20} & m_{21} & m_{22} \end{pmatrix}$
$\LARGE R_x(\theta) = \begin{pmatrix}1 & 0 & 0 \\ 0 & cos(&theta) & -sin(&theta) \\ 0 & sin(&theta) & cos(&theta) \end{pmatrix}$
$\LARGE R_y(\theta) = \begin{pmatrix}cos(&theta) & 0 & sin(&theta) \\ 0 & 1 & 0 \\ -sin(&theta) & 0 & cos(&theta) \end{pmatrix}$
$\LARGE R_z(\theta) = \begin{pmatrix}cos(&theta) & -sin(&theta) & 0 \\ sin(&theta) & cos(&theta) & 0 \\ 0 & 0 & 1 \end{pmatrix}$

よって、ヨーピッチロール回転行列は次のような形になります

$\LARGE R = \begin{pmatrix}m_{00} & m_{01} & m_{02} \\ m_{10} & m_{11} & m_{12} \\ m_{20} & m_{21} & m_{22} \end{pmatrix} = \begin{pmatrix}c_z c_y - s_z s_x s_y & -s_z c_x & c_z s_y + s_z s_x c_y \\ s_z c_y + c_z s_x s_y & c_z c_x & s_z s_y - c_z s_x c_y \\ -c_x s_y & s_x & c_x c_y\end{pmatrix}$

ここから、 $m21=sin(x)$ なので、 $\theta_x=asin(m21)$ とわかります。

$tan=\frac{sin}{cos}$ から、 $\frac{m_{01}}{m_{11}} = \frac{-sin(\theta_z)}{cos{\theta_z}} = -tan(\theta_z)$ とわかります。

すなわち、 $\theta_z=atan(-m_{01},m_{11})$ ということですね。

同様に、 $\frac{m_{20}}{m_{22}} = \frac{-sin(\theta_y)}{cos(\theta_y)} = -tan(\theta_y)$ なので、 $\theta_y=atan(-m_{20},m_{22})$ となります。

ここまでをまとめると

$\LARGE \theta_x=asin(m21)$
$\LARGE \theta_z=atan(-m_{01},m_{11})$
$\LARGE \theta_y=atan(-m_{20},m_{22})$

ってことです。

さて、これだけで終われば話は簡単なんですが、例外があります。

ヨーピッチロール行列を良く眺めてみればわかりますが、 $cos(\theta_x) = 0$ の時には、 $m_{01}=m_{11}=0$ となってしまい、値が一意に求まりません。

$cos(\theta_x) = 0$ の時の行列がどのような形になっているか書いてみると、次のようになります。

$\LARGE R = \begin{pmatrix}m_{00} & m_{01} & m_{02} \\ m_{10} & m_{11} & m_{12} \\ m_{20} & m_{21} & m_{22} \end{pmatrix} = \begin{pmatrix}c_z c_y \pm s_z s_y & 0 & c_z s_y \pm s_z c_y \\ s_z c_y \pm c_z s_y & 0 & s_z s_y \pm c_z c_y \\ 0 & \pm1 & 0 \end{pmatrix}$
※ $cos(\theta_x) = 0$ の時、 $sin(\theta_x) = \pm1$ となる点に注意

ここで、三角関数の加法定理を思い出してみることにします

$\LARGE sin(\alpha \pm \beta) = sin(\alpha) cos(\beta) \pm cos(\alpha) sin(\beta)$
$\LARGE cos(\alpha \pm \beta) = cos(\alpha) cos(\beta) \pm sin(\alpha) sin(\beta)$

加法定理を用いて、 $cos(\theta_x) = 0$ の時のヨーピッチロール回転行列を整理してみます

$\LARGE R = \begin{pmatrix}m_{00} & m_{01} & m_{01} \\ m_{10} & m_{11} & m_{12} \\ m_{20} & m_{21} & m_{22} \end{pmatrix} = \begin{pmatrix}cos(\theta_z \pm \theta_y) & 0 & sin(\theta_z \pm \theta_y) \\ sin(\theta_z \pm \theta_y) & 0 & cos(\theta_z \pm \theta_y) \\ 0 & \pm 1 & 0 \end{pmatrix}$

行列を眺めてみればわかるとおり、求めるべき値の $\theta_y$ と $\theta_z$ は、加算(減算)された形で $sin$ と $cos$ のパラメータとして現れているので、値が一意に求まりません。

そこで、適当に $\theta_y = 0$ と置くと、 $\frac{m_{10}}{m_{00}} = \frac{-sin(\theta_z)}{cos{\theta_z}} = tan(\theta_z)$ なので、 $\theta_z = atan(m_{10},m_{00})$ となります

値が一意に求まらないとは、 $cos(\theta_x)=0$ となる場合、 $\theta_y$ と $\theta_z$ が設定したときの値で抽出できないことを意味しています。
(求めたパラメータを用いて、再度同じ形の行列を作ることは出来る)

さて、 $cos(\theta_x)=0$ の場合とは、すなわち $\theta_x = \pm \frac{\pi}{2}$ ですよね
(要は、 $\pm90$ 度回転しているってこと)

ここから、 $m_{21}=1$ の場合には、 $\theta_x=\frac{\pi}{2}$ となり、 $m_{21}=-1$ の場合には、 $\theta_x=-\frac{\pi}{2}$ となります。

やっとのことで、パラメータ抽出を行うことができました。

まとめておくと

$sin(\theta_x) = \pm1$ 以外の場合
$\LARGE \theta_x=asin(m21)$
$\LARGE \theta_z=atan(-m_{01},m_{11})$
$\LARGE \theta_y=atan(-m_{20},m_{22})$

$sin(\theta_x) = 1$ の場合( $m_{21}=1$ の場合)
$\LARGE \theta_x=\frac{\pi}{2}$
$\LARGE \theta_y=0$
$\LARGE \theta_z = atan(m_{10},m_{00})$
※ ただし、値は一意にも止まらないので、 $\theta_y=0$ と仮定
$sin(\theta_x) = -1$ の場合( $m_{21}=-1$ の場合)
$\LARGE \theta_x=-\frac{\pi}{2}$
$\LARGE \theta_y=0$
$\LARGE \theta_z = atan(m_{10},m_{00})$
※ ただし、値は一意にも止まらないので、 $\theta_y=0$ と仮定

今回は、ヨーピッチロール回転(zxy回転)を例に挙げましたが、回転表現をオイラー角で表現する場合(x,y,z軸周りのそれぞれの回転の合成)、合成の方法は、xyz,xzy,yxz,yzx,zxy,zyxの六通りあるので、それぞれ自分が利用している回転表現に合わせてパラメータ抽出の方法を変える必用があります。

ま、同じ要領でパラメータ抽出を行えばよいだけですけどね