氷川丸のジンバル。
前後左右の傾きを吸収し水平を保つ。
(C) 中田 亨 （独立行政法人 産業技術総合研究所 デジタルヒューマン研究センター 研究員 博士(工学)）
2003年11月25日
四元数とは、４つの実数を組み合わせたものです。
４つの要素のうち、ひとつは実部、残り３つは虚部です。
たとえば、Qという四元数が、実部 t で虚部が x, y, z から成り立っているとき、下のように書きます。
Q = (t; x, y, z)
また、V = (x, y, z)というベクトルを使って、
Q = (t; V)
とも書くことがあります。
正統的に虚数単位i, j, kを利用した書き方だと、
Q = t + xi + yj + zk
とも書きますが、こっちはあまり使いません。
虚数単位同士の掛け算は
ii = -1, ij = -ji = k (この他の組み合わせでも、サイクリックに以下同文)
という規則があります。（なんとなく、ベクトル積（外積）みたいでせう）
この規則を使ってちまちま計算するのは面倒なので、以下のような公式で計算してください。
A = (a; U)
B = (b; V)
AB = (ab - U・V; aV + bU + U×V)
ただし、「・」は内積、「×」は外積のことです。
注意：たいていは AB ≠BA なので掛け算の左右には気をつけて！
ある座標(x, y, z)を四元数で表すには、
P = (0; x, y, z)
とします。
ちなみに、実部をゼロ以外の値にしても、下記の結果は同じです。ゼロにすると手間がかからないのでお勧めします。
原点を回転中心として、回転の軸が(α, β, γ)で
（但し α^2 + β^2 + γ^2 = 1）、
（右手系の座標定義なら、ベクトル(α, β, γ)の進む方向に向かって眺めて反時計回りに）
θ回す回転を表す四元数は
Q = (cos(θ/2); α sin(θ/2), β sin(θ/2), γ sin(θ/2))
R = (cos(θ/2); -α sin(θ/2), -β sin(θ/2), -γ sin(θ/2))
と作ります。(ちなみにR は Q の共役四元数といいます。）
さて、回転を実行するには、
R P Q = (0; 答え)
と、サンドイッチ風の計算してください。この値の虚部が回転したあとの点の座標値です。 （ちなみに、実部はゼロになるはずです。検算してみてください）
氷川丸のジンバル。 前後左右の傾きを吸収し水平を保つ。 |
/// Quaternion.cpp
/// (C) Toru Nakata, toru-nakata@aist.go.jp
/// 2004 Dec 29
#include <math.h>
#include <iostream.h>
/// Define Data type
typedef struct
{
double t; // real-component
double x; // x-component
double y; // y-component
double z; // z-component
} quaternion;
//// Kakezan
quaternion Kakezan(quaternion left, quaternion right)
{
quaternion ans;
double d1, d2, d3, d4;
d1 = left.t * right.t;
d2 = -left.x * right.x;
d3 = -left.y * right.y;
d4 = -left.z * right.z;
ans.t = d1+ d2+ d3+ d4;
d1 = left.t * right.x;
d2 = right.t * left.x;
d3 = left.y * right.z;
d4 = -left.z * right.y;
ans.x = d1+ d2+ d3+ d4;
d1 = left.t * right.y;
d2 = right.t * left.y;
d3 = left.z * right.x;
d4 = -left.x * right.z;
ans.y = d1+ d2+ d3+ d4;
d1 = left.t * right.z;
d2 = right.t * left.z;
d3 = left.x * right.y;
d4 = -left.y * right.x;
ans.z = d1+ d2+ d3+ d4;
return ans;
}
//// Make Rotational quaternion
quaternion MakeRotationalQuaternion(double radian, double AxisX, double AxisY, double AxisZ)
{
quaternion ans;
double norm;
double ccc, sss;
ans.t = ans.x = ans.y = ans.z = 0.0;
norm = AxisX * AxisX + AxisY * AxisY + AxisZ * AxisZ;
if(norm <= 0.0) return ans;
norm = 1.0 / sqrt(norm);
AxisX *= norm;
AxisY *= norm;
AxisZ *= norm;
ccc = cos(0.5 * radian);
sss = sin(0.5 * radian);
ans.t = ccc;
ans.x = sss * AxisX;
ans.y = sss * AxisY;
ans.z = sss * AxisZ;
return ans;
}
//// Put XYZ into quaternion
quaternion PutXYZToQuaternion(double PosX, double PosY, double PosZ)
{
quaternion ans;
ans.t = 0.0;
ans.x = PosX;
ans.y = PosY;
ans.z = PosZ;
return ans;
}
///// main
int main()
{
double px, py, pz;
double ax, ay, az, th;
quaternion ppp, qqq, rrr;
cout << "Point Position (x, y, z) " << endl;
cout << " x = ";
cin >> px;
cout << " y = ";
cin >> py;
cout << " z = ";
cin >> pz;
ppp = PutXYZToQuaternion(px, py, pz);
while(1) {
cout << "\nRotation Degree ? (Enter 0 to Quit) " << endl;
cout << " angle = ";
cin >> th;
if(th == 0.0) break;
cout << "Rotation Axis Direction ? (x, y, z) " << endl;
cout << " x = ";
cin >> ax;
cout << " y = ";
cin >> ay;
cout << " z = ";
cin >> az;
th *= 3.1415926535897932384626433832795 / 180.0; /// Degree -> radian;
qqq = MakeRotationalQuaternion(th, ax, ay, az);
rrr = MakeRotationalQuaternion(-th, ax, ay, az);
ppp = Kakezan(rrr, ppp);
ppp = Kakezan(ppp, qqq);
cout << "\nAnser X = " << ppp.x
<< "\n Y = " << ppp.y
<< "\n Z = " << ppp.z << endl;
}
return 0;
}
四元数のスカラー倍を考えると、回転篇での結果座標を、拡大縮小することができます。例えば、
P = (0; x, y, z)
(ちなみに実部は任意の値でよい。0が計算が楽）
kQ = (kcos(θ/2); kα sin(θ/2), kβ sin(θ/2), kγ sin(θ/2))
kR = (kcos(θ/2); -kα sin(θ/2), -kβ sin(θ/2), -kγ sin(θ/2))
(kRはkQの共役四元数です。）
とすると、
kR P kQ = ( 0; 回転後のＸＹＺ座標のkの自乗倍）
となって、原点中心の回転と、原点中心の拡大縮小をあわせた操作ができます。
とすれば、原点からの方位とその距離を自由に操れることを意味します。つまり、ある一点（Ｘ，Ｙ，Ｚ）を、任意の点（Ｘ´，Ｙ´，Ｚ´）に映す変換は、四元数で表現することができます。但し、Ｘ＝Ｙ＝Ｚ＝０の場合は例外。
ある点Ａを、別の点Ｂに移す、原点中心の回転を表す四元数は、次のように作ります。
回転の軸（α，β，γ）は、位置ベクトルの外積 Ｂ×Ａ と平行で同じ向き。（ＡとＢの順は、四元数ＲとＱのかけ算の順によって入れ替わります。この文書のかけ算順の定義だと、ＢＡの順です。） 割り算して長さを１に直します。
（α，β，γ）＝ (Ｂ×Ａ) / (｜Ｂ×Ａ｜)
cos(θ)は、位置ベクトルの内積 Ｂ・Ａ を、｜Ｂ｜・｜Ａ｜で割った値として作ります。
cos(θ)＝ （Ｂ・Ａ） / (｜Ｂ｜・｜Ａ｜)
三角関数の半角の公式によれば、
cos(θ/2)＝±√{ 0.5 * (1 + cosθ) }
sin(θ/2)＝±√{ 0.5 * (1 - cosθ) }
復号は、0°≦θ≦180°ですので、 cos(θ/2)≧０ かつ sin(θ/2)≧０ つまり、 両方ともプラス
cos(θ/2)＝√{ 0.5 * (1 + cosθ) }
sin(θ/2)＝√{ 0.5 * (1 - cosθ) }
拡大縮小も行う場合は、k = √{｜Ｂ｜/｜Ａ｜}
この問題はしばしば相談を受けるのですが、数学的理由と、数学の外の事情で難しいのです。
3次元の回転は
(1) 回転軸である直線
(2) 回転角度
の2つの情報があれば、表現できます。これぞ「万能な3次元回転表現」です。
このような単発的な回転を single rotation といいます。
(注意： single rotation には回転中心点はありません。回転軸があるだけです。
ボールジョイントのような関節での回転における、異なる回転軸を有する single rotation が複数連なった rotation sequence であるならば回転中心点は考えることができます。）
（注意： 四元数は single rotation を回転軸と回転角度の情報で素直に表しています。
オイラー角は、一つの single rotation を、3つの single rotation に分解することで表そうとします。わざわざ複雑にしていると言えます。また、ジンバルロックの問題を引き起こす可能性を孕みます。）
「オイラー角→四元数」
という直接的な変換を考えるのは非常に難しい問題です。
正しくは
「オイラー角→万能表現→四元数」
といった
「ある表現形式→万能表現→別の表現形式」
のルートを取るべきです。これが一番安全で、考えやすいと思います。
変換の難しさの数学的理由
変換の難しさの数学以外の理由
今さら、なぜオイラー角使うのか？ 出力が、ロール、ピッチ、ヨーであるジャイロが多いからかしら。砲の銃座のように機構がロール、ピッチ、ヨーを使いようになっている場合もあります。
Euler さんは、「ひとつの三次元のsingle rotationは、座標系の軸を回転軸とする三次元回転を３個組み合わせることで表現することができる」ということを考えつきました。これが広義のオイラー角。
つまり、座標系から見えると傾いている軸を回転軸とする回転は、、うまいことやれば、まずZ軸中心でα度回転させ、次にX軸中心にβ度回転し、最後にZ軸中心にγ度回転させれることで、そっくり同じ回転を作り出すことができるというものです。（ジンバルロック発生の場合は、解の一意性が失われて、面倒くさいことになりますが。）
オイラーさん自身は、回転軸の選び方をZ-X-Zを使っています。この軸選びの順のケースが、狭義のオイラー角です。(独楽の運動を考えるとき、まず独楽の高速な自転をZ軸回転で表し、独楽の芯棒が地面に向かって倒れ込む回転をX軸回転で表し、独楽の芯棒が水平回転することをZ軸回転で表しています。)
世の中にはZ-Y-Zとか、X-Z-X, Y-X-Yのようなパターンのことを、オイラー角と称する人もいるようです。
軸の選び方は、X-Y-Zとか、Y-Z-Xのような３軸じゅんぐり使用のパターンもあります。(なお、３つの軸の立場は平等ではありません。回転操作の先後に起因する違いがあります。)これらを広義のオイラー角とする人もいて、狭義のオイラー角しかオイラー角と認めない人とは誤解の元になります。
X-Y-Z式は頻用されるパターンです。これをロール・ピッチ・ヨー角式と呼ばれたり、カルダン角 (Cardan angles)と呼ぶ人もいます。
これは極めて簡単で、オイラー角の３回の回転を、それぞれ四元数で表し、これらを順番にかけ算すれば出来上がり。
「オイラー角→四元数」の逆変換を考えることになります。公式化してもいいのですが、オイラー角がZ-X-Z式なのかX-Y-Z式なのかの違いで、大きく変わってしまいます。シンバルロック発生時には、公式の中でゼロ割りが起きますので、その処理も面倒みてください。
ひとつのシングルローテーションは、ひとつの四元数で書き表せるのでした。
すると、シングルローテーションをNつ続けて行って作る回転は、N個の四元数の積である四元数１個で書き表せることが言えます。つまり、複数のシングルローテーションの数珠繋ぎ実行は、経過を無視して、その結果だけを見るなら、たった1つの四元数で表すことができます。
結局、いくらゴロゴロ回しても、たった１つのシングルローテーションを行ったに過ぎないというわけです。サッカーボールをあっちに回し、こっちに回しと繰り返しても、初期姿勢と最終姿勢とに於いて、全く移動していない点が少なくとも２つはあるということです! あるいは、１回の回転で最終姿勢まで一っ飛びでいけることも意味します。
コマの運動を瞬間的に観察してみましょう。コマの高速な自転と、芯棒の倒れ込み回転は、それぞれシングルローテーションです。従って、２つの回転が存在するようにに見えても、合成してみると、コマはひとつのシングルローテーションを行っているに過ぎないと言えます。自転軸とは微妙に斜めの軸が真の回転軸であり、コマはその軸を中心として高速回転しているわけです。
この合成されたシングルローテーションの回転軸は、機械的な拘束の都合上、コマの接地点を通らなければなりません。しかしそれでは、回転はコマの重心を外しています。
重心を外した回転のせいで、コマの芯棒は方向を変えます。この方向転換により、芯棒が単純に地面に激突することは防がれているのです。
まず、地球儀を用意し、自転させます。その状態で、地軸を一定の速度で決まった方向に回転させてみましょう。
このとき、自転はシングルローテーション。また、地軸の回転もシングルローテーションです。しかし、地球儀の各点は、シングルローテーションではない、複雑な動きをします。
一瞬だけを見れば、地球儀はシングルローテーションを行っています。しかし、次の瞬間には、地軸の方向が変わってしまい、地球儀は先ほどとは違う自転を強制されます。従って、もはや自転は単調なシングルローテーションの繰り返しにはなりません。
このように、機械的拘束をする回転軸がシングルローテーションしつづける場合には、注意が必要です。
q = t + xi + yj + zk の時、|q| = √(t^2 + x^2 + y^2 + z^2)
言い換えれば、q = (t; v) の時、|q| = √(t^2 + v^2)
虚部の符号が反対な四元数を、共役四元数といいます。
qの共役四元数をpとすると、
q = t + xi + yj + zk の時、p = t - (xi + yj + zk)
言い換えれば、q = (t; v) の時、p = (t; -v)
ある四元数にかけると、その積が１ つまり (1; 0, 0 ,0) になるような四元数を、逆四元数といいます。
で、逆四元数の実態は、共役四元数の各成分を、四元数の絶対値の２乗で割ったものです。（つーことは、絶対値の計算で平方根を取るのは、無駄だな。）
変形操作のＵｎｄｏをしたい場合は、逆四元数を使ってください。
なお、四元数の絶対値がゼロなら、逆四元数は無限大になり、要するに存在しません。