It_lives_vainlyの日記 このページをアンテナに追加 RSSフィード

2007-08-30

[][] 回転行列からオイラー角のパラメータ抽出を行う(補足)

先日のエントリーの補足です。

cos(¥theta_x)=0の場合、値が一意に定まらないので、¥theta_y=0と仮定して値を求めていました。

これってどういうことなんでしょう?

cos(¥theta_x)=0になる場合とは、x軸周りの回転が¥pm90度になる時です。

...これちょっと考えればわかるんですけど、いわゆるジンバルロックが起きている状態ですよね。

x軸周りに90度回転してしまうことによって、y軸とz軸が重なってしまって、y軸回転もx軸回転も同じ回転を示してしまい、自由度が一軸失われている状態です。

再度、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}

式からもわかるように、cos(¥theta_z ¥pm ¥theta_y)sin(¥theta_z ¥pm ¥theta_y)となっていて、¥theta_zの値を変えても、¥theta_yの値を変えても、最終的には合計されて三角関数パラメータとして用いられてしまうんです

だから、いくらy軸回転を加えようが、z軸回転を加えようが、同じ方向に回転をしてしまうわけなんです

...ま、ジンバルロックの解釈一つとして、軽く覚えておきましょう

2007-08-29

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

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

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

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

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

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

つまり

 ¥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は、加算(減算)された形でsincosパラメータとして現れているので、値が一意に求まりません。

そこで、適当に¥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の六通りあるので、それぞれ自分が利用している回転表現に合わせてパラメータ抽出の方法を変える必用があります。

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