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

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

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

copincopin 2014/07/06 19:11  記憶できないので何回も参照させて頂いています。古い記事=参照されてる方の多い実績ある記事、と思うので、ちょっとビクビクしてますが、最初のヨーピッチロールの回転行列Rの要素m02(表記ではm01となっており、これもtypo?)とm12が間違っていると思います。
m02 = CzSy+SzSxCy
m12 = SzSy-CzSxCy
でしょうか。その後の導出式はうまい具合にそれらを避けてあるけど、その後のRにも出てくるような。

It_lives_vainlyIt_lives_vainly 2014/09/06 01:35 copin さま

ご指摘ありがとうございます.
確かに書き違えていたようです.

慎重に修正した方が良さそうなので、本文の修正は先送りにさせていただきますが、ざっと見直したところ以下に誤りがありそうです.

- 行列の表記で、m02 の部分が m01 となっている部分があります
- ヨーピッチロールの計算結果の部分が、ご指摘の通り(m02=CzSy+SzSxCy, m12 = SzSy-CzSxCy)となります
- cos(theta) = 0 と仮定した場合の式の、m02, m12 が間違えています(m02 = SyCz+CySz, n12 = SzSy-CxCy)

後日見直して、エントリーの方を修正させていただきます.
ご指摘ありがとうございました.

It_lives_vainlyIt_lives_vainly 2014/09/07 17:56 copin さまからご指摘いただいた内容につきまして、エントリーを修正させていただきました.
もし、また誤りなどありましたら是非ご指摘ください.

> copinさま
ご返信が遅れてしまったことお詫び申し上げます.
そして、ご連絡ありがとうございました.

takemitsutakemitsu 2015/08/03 11:13 記事中の式について質問があるのですが、
(当方の勘違い・読解力不足でしたら大変申し訳ありません)

「cos(¥theta_x) = 0の時の行列がどのような形になっているか書いてみると、次のようになります。」以下の式は

m00=(cz*cy)+-(sz*sy)

ではなく
m00=(cz*cy)-+(sz*sy)

ではないでしょうか?(m12も同様)
これに伴って、最後のまとめも変わってくると思います。

It_lives_vainlyIt_lives_vainly 2015/08/27 00:19 > takemitsu さま
コメントありがとうございます.
(blog更新をサボっていたのでコメントに気づいていませんで失礼しました)

コメントを拝見させていただいたのですが、ご指摘の意図を理解できておりません.

m21 = sin(¥theta_x) = 1のケースと、-1のケースがあり

m21 = Sx = 1 の場合
m00 = CzCy - SzSxSy = CzCy - SzSy
m12 = SzSy - CzSxCy = SzSy - CzCy

m21 = Sx = -1 の場合
m00 = CzCy - SzSxSy = CzCy + SzSy
m12 = SzSy - CzSxCy = SzSy + CzCy

なので、まとめて

m00 = CzCy ± SzSy
m12 = SzSy ± CzCy

と書いていると思いますが、なにか根本的な誤りなどありましたでしょうか?
後日、時間があるときに改めて見なおさせていただこうと思いますが、もしお気づきの点などあれば再度コメント欄に書き込みいただければと思います.

スパム対策のためのダミーです。もし見えても何も入力しないでください
ゲスト


画像認証

Connection: close