3次元の回転角度の求め方について教えてください。

質問

3次元の回転角度の求め方について教えてください。

3軸の加速度センサーがあります。
まず加速度センサーのZ軸を重力方向に置いたときの加速度センサーの値を(x1,y1,z1)=(0,0,1)とします。
加速度センサーのx軸、y軸、z軸をそれぞれ回転させたあとの加速度センサーの値を(x2,y2,z2)とします
（このとき加速度センサーは静止しているので、センサーの値は重力の分力になります）。
(x2,y2,z2)が既知のとき(x1,y1,z1)に戻すためのそれぞれの回転角はどのように求めれば良いのか教えてください。

(x2,y2,z2)→(x1,y1,z1)へ移動するときの回転角を
φ（z軸の回転角）、ψ（x軸の回転角）、θ（y軸の回転角）
とします。
回転行列
(x1) = (cosφ -sinφ 0) (cosθ 0 sinθ) (1 0 0 ) (x2)
(y1) = (sinφ cosφ 0) (0 1 0 ) (0 cosψ -sinψ) (y2)
(z1) = (0 0 1) (-sinθ 0 cosθ) (0 sinψ cosψ ) (z2)
より,3行3列の行列を計算すると
0=cosφcosθx2 + (-sinφcosψ+cosφsinθsinψ)y2+(sinφsinψ+cosφsinθcosψ)z2
0=sinφcosθx2 + (cosφcosψ+sinφsinθsinψ)y2+(-cosφsinψ+sinφsinθcosψ)z2
1=-sinθx2 + cosθsinψy2 + cosθcosψz2

となると思うのですが、この式からφ、ψ、θが導きだせません。
どうすれば求めることができるか教えていただけますか。

回答No.5

nag0720 · ベストアンサー

＞cosφ=(y2z1-y1z2)/√(z1^2+z2^2)について、どういう方程式から導かれたのか教えていただけますか？

２点(x1, y1, z1),(x2, y2, z2)と直交する点を(x3, y3, z3)とします。（２点の外積）
(x3, y3, z3)=(y2z1-y1z2, z2x1-z1x2, x2y1-x1y2)

このとき次のことが成り立ちます。
x3^2+y3^2+z3^2=1
x1x3+y1y3+z1z3=0
x2x3+y2y3+z2z3=0

最初のz軸での回転は、点(x3, y3, z3)がxz平面に移動するように回転させます。（y座標=0になる）
次のy軸での回転は、その点が(1, 0, 0)に移動するように回転させます。
最後のx軸での回転は、(x1, y1, z1),(x2, y2, z2)はyz平面に移動しているので、(x1, y1, z1)が(0, 0, 1)に移動するように回転させます。

最初の回転角度は、点(x3, y3)のy座標を0にするので、
cosφ=x3/√(x3^2+y3^2)
sinφ=-y3/√(x3^2+y3^2)

x3^2+y3^2
=(y2z1-y1z2)^2+(z2x1-z1x2)^2
=y2^2*z1^2-2y1y2z1z2+y1^2*z2^2+z2^2*x1^2-2z1z2x1x2+z1^2*x2^2
=(x1^2+y1^2)z2^2+(x2^2+y2^2)z1^2-2(x1x2+y1y2)z1z2
=(1-z1^2)z2^2+(1-z2^2)z1^2-2(-z1z2)z1z2
=z1^2+z2^2
なので、
cosφ=(y2z1-y1z2)/√(z1^2+z2^2)
sinφ=(x2z1-x1z2)/√(z1^2+z2^2)

ψ,θも同じようにして求められます。

お礼

＃2様
回答ありがとうございます。
外積を利用することで求められるのですね。こちらで質問をしなければ到底ここまでたどり着けなかったと思います。
すぐに実用には結び付けられないとは思いますが、大学のころの数学が復習でき刺激になりました（久しぶりに教科書を開きました）。
今まで丁寧に回答していただきありがとうございました。

回答No.4

nag0720

回転角度を計算してみました。

＃３で方向と書きましたが、方向の代わりに、
直交する２点を(0,0,1)と(0,1,0)に移動する回転角度を求めてみます。
（直交＝中心から２点を臨む角度が９０°）
ある点から別の点の方を向くと考えれば方向と同じ意味です。

２点を(x1, y1, z1),(x2, y2, z2)とします。
２点の条件は、
x1^2+y1^2+z1^2=1　（長さが１）
x2^2+y2^2+z2^2=1　（長さが１）
x1x2+y1y2+z1z2=0　（直交条件）

z軸での回転角φを、
cosφ=(y2z1-y1z2)/√(z1^2+z2^2)
sinφ=(x2z1-x1z2)/√(z1^2+z2^2)

y軸での回転角ψを、
cosψ=√(z1^2+z2^2)
sinψ=x1y2-x2y1

x軸での回転角θを、
cosθ=z1/√(z1^2+z2^2)
sinθ=-z2/√(z1^2+z2^2)

ただし、z1=z2=0のときは、
cosφ=1
sinφ=0
cosψ=0
sinψ=x1y2-x2y1
cosθ=x1
sinθ=y1

この角度で、z→y→xの順に回転すれば、
(x1, y1, z1)→(0, 0, 1)
(x2, y2, z2)→(0, 1, 0)
となります。

実際に計算してみましょう。
(x1, y1, z1)=(1/√3, 1/√3, 1/√3)
(x2, y2, z2)=(1/√2, 0, -1/√2)
すると、

cosφ=1/√5
sinφ=2/√5
cosψ=√5/√6
sinψ=-1/√6
cosθ=√2/√5
sinθ=√3/√5

回転行列にすると、（行列を横に並べて書きます）
Ez=((1/√5, -2/√5, 0), (2/√5, 1/√5, 0), (0, 0, 1))
Ey=((√5/√6, 0, 1/√6), (0, 1, 0), (-1/√6, 0, √5/√6))
Ex=((1, 0, 0), (0, √3/√5, -√2/√5), (0, √2/√5, √3/√5))

ExEyEz(1/√3, 1/√3, 1/√3)=(0, 0, 1)
ExEyEz(1/√2, 0, -1/√2)=(0, 1, 0)
となるはずです。

お礼

＃2様
お礼が遅くなりすいませんでした。
丁寧な回答ありがとうございます。

直行する2点があればできるみたいですね。
この方法ですと、加速度センサー2つを直行するように置くことで実現できそうです。ひとつで実現できればよかったのですが、難しいようですね。
最後に
cosφ=(y2z1-y1z2)/√(z1^2+z2^2)について、どういう方程式から導かれたのか教えていただけますか？
私が空間をイメージできていなければ解説できないようでしたら、あとは私の努力次第なのでがんばりたいと思います。

回答No.3

nag0720

＃２です。

書き忘れていたことが１点。

もし、点の位置だけでなく、その方向も考慮するとしたら、φ、ψ、θは一意に定まるかもしれません。
方向というのは、位置をベクトルP=(x2,y2,z2)としたら、Pと直交する単位ベクトルです。
例えば、球面を地球としたら、地上から見てどの方向（東西南北）を向いているかということです。

お礼

おはようございます。
＃2様　追加の回答ありがとうございます。
＃2様の回答について、勉強不足で私にはまだ理解できないので、参考となるURLや例題をご存知でしたら教えていただけないでしょうか。
回答していただいた内容から、やりたいことができそうなので希望がもてました。私の知識不足のためスムーズに事が運ばないとは思いますが、お付き合いお願いします。

回答No.2

nag0720

点(x2,y2,z2)を点(x1,y1,z1)に、z→y→xの順に回転させるときは、
(x1,y1,z1)=MxMyMz(x2,y2,z2)
のように表記します。（Mx,My,Mzは回転行列）

#1さんが書いているように、回転を３回にした場合は、回転の仕方は無数にあります。
たとえば、点(x2,y2,z2)を点(0,0,1)に移動させる場合、
(1) z軸で回転してy座標を0にし、次にy軸で回転してx座標を0にする。
(2) z軸で回転してx座標を0にし、次にx軸で回転してy座標を0にする。
(3) y軸で回転してx座標を0にし、次にx軸で回転してy座標を0にする。
これらは２回の回転だけですが、みな同じ位置に移動します。
これ以外にも、最初のz軸で適当に回転しても、y軸、x軸の回転で(0,0,1)に移動させることができます。

別の言い方をすれば、z軸の回転角φを適当に決めれば、y軸の回転角ψ,x軸の回転角θは一意に決まると言えます。

お礼

No.2様ご回答ありがとうございます。
行列式の書き方についてありがとうございました。私の間違いをここで正すことができました。

あと、本題についてなんとなくわかってきました。
z軸の回転角も求められるのかと思っていましたが、この回転角は自由なものになってしまうのですね。
地面に水平に置かれ、さらに前後左右の向きが決まっているものに対し、その物体を適当に回転させた後の各軸の加速度の値から回転角をもとめることで、元の状態に戻せるかと思ったのですが、地面と水平までは戻せても、Z軸の回転は自由なので、前後左右の向きまでは保障できないということのようですね。
最初の質問が的確でなかったため、回答していただいた方々にお手数をおかけしてすいませんでした。しかし加速度値だけではこの課題を解くことができなさそうなことがわかりました。
今度はジャイロの仕組みを調べてみて、利用できるか検討してみたいと思います。

回答No.1

Tacosan

ちょっと考えただけなのですが, 「3つを一意に決めることはできない」ような気がします.
例えば, P = (0, 0, 1) から Q = (1/√3, 1/√3, 1/√3）にするときには
・x軸まわり→y軸まわり
・y軸まわり→z軸まわり
のどちらでも可能なはずです.
なお, 今の定式化では「(x2, y2, z2) から (x1, y1, z1) = (0, 0, 1) を得る」ために「x軸まわり→y軸まわり→z軸まわり」の順で回転させていることになりますが, z軸まわりで回転させても z座標は変化しないので最後の回転は無意味だと思います. ということで, 「x軸まわりに回転させて y座標を 0 にしてから y軸まわりに回転させて z座標を 1 にする」と勝手に x座標は 0 になるはずです.

お礼

No.1様
回答ありがとうございます。
確かに本件において、x→y→zの順に回転させた場合、最後にz軸を回転させても意味ないですね。
実は私としては、z→y→xの順番に回転させているつもりでした。
xの回転行列をMx、yの回転行列をMy、zの回転行列をMzとしたとき、
z→y→xに回転させるときの式は
(x1y1z1)=Mz・My・Mx・(x2y2z2)
だと思っていました。
正解は
(x1y1z1)=Mx・My・Mz・(x2y2z2)
なのでしょうか？
そもそも考え方が間違っていたら申し訳ありません。

ｚ軸の回転が無い場合No.1様のとおりでできそうですが、z軸の回転を想定しz→y→xの順に回転させる場合はやはりどうしたらよいか道筋がたちません。引き続きご教授お願いします。