四元数で3次元回転

中田 亨, 2003年11月25日 

★こうすれば四元数で3次元の回転が計算できる

四元数(しげんすう, クォータニオン, quaternion)を使った回転の取り扱い手順を説明します。

(1)四元数の実部と虚部と書き方

四元数とは、4つの実数を組み合わせたものです。4つの要素のうち、ひとつは実部、残り3つは虚部です。たとえば、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

とも書きますが、こっちはあまり使いません。

(2)四元数同士の掛け算

虚数単位同士の掛け算は

ii = -1, ij = -ji = k (この他の組み合わせでも、サイクリックに以下同文)

という規則があります。

この規則を使ってちまちま計算するのは面倒なので、以下のような公式で計算してください。

A = (a; U)
B = (b; V)
AB = (ab - U・V; aV + bU + U×V)

ただし、「・」は内積、「×」は外積のことです。

注意:たいていは AB ≠BA なので掛け算の左右には気をつけて!

(3)3次元の座標の四元数での表現

ある座標(x, y, z)を四元数で表すには、

P = (0; x, y, z)

とします。

ちなみに、実部をゼロ以外の値にしても、下記の結果は同じです。ゼロにすると手間がかからないのでお勧めします。

(4)回転の四元数での表現

原点を回転中心として、回転の軸が(α, β, γ)
(但し α^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;      
}      
              

★四元数の長所と欠点

ジンバル
氷川丸のジンバル。
前後左右の傾きを吸収し水平を保つ。

長所

「直感的」
回転の軸と回転の角度が与えられれば、オイラー角とかを考えなくても、すぐに計算できます。
「連続性」
似ている回転を意味するなら、その四元数の値も似ている。ジンバルロックがない。
(ジンバルロックとは、「北極や南極などの特異点は自転では動かすことができない」「中華料理店の回転テーブルで、ど真ん中に醤油を置かれると回しても近づかない」などの、有効な自由度や次元が失われる現象。ソフトウエアでは例外処理を講じるハメになるので厄介である。
もともとジンバルとは羅針盤を吊っておく機構。船が揺れても羅針盤は水平を保って揺れないようにするもの。ジンバルの腕は“てれこてれこ”で羅針盤を吊っているが、これらの向きを揃えてしまうと、フニャフニャしなくなり(=ロック)、振動が羅針盤に伝わってしまう。)
「メモリ効率がいい。計算が速い」
回転を4つの数値だけで記述できる。アフィン変換行列などに比べると、メモリを使わない。無駄な計算が減る。

欠点

「見た目で何を意味するのか分らない」
四元数の成分から、それが何を意味するのか、一見しては分らない。
「多数回転を表現しない」
ごらんのように、cos やsinを使っていますので、θが10度なのか、370度なのか、-350度なのか、区別できません。何回もぐるぐるまわすアニメーションを作りたい場合は、いっぺんに回転させようとせず、細かく区切ってください。
「原点中心の回転である」
原点以外を回転の中心にしたい場合は、座標に下駄を履かせて、回して、下駄を戻す操作をしてください。(下記の一般変位篇もチェックしてね!)

★一般変位篇: 回転以外の変形である拡大縮小(と併進?)の手順

四元数のスカラー倍を考えると、回転篇での結果座標を、拡大縮小することができます。例えば、

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; 回転後のXYZ座標のkの自乗倍)

となって、原点中心の回転と、原点中心の拡大縮小をあわせた操作ができます。

とすれば、原点からの方位とその距離を自由に操れることを意味します。つまり、ある一点(X,Y,Z)を、任意の点(X´,Y´,Z´)に映す変換は、四元数で表現することができます。但し、X=Y=Z=0の場合は例外。

★所望の四元数を作ろう篇: ある回転を実施する四元数の作り方

ある点Aを、別の点Bに移す、原点中心の回転を表す四元数は、次のように作ります。

回転の軸(α,β,γ)は、位置ベクトルの外積 B×A と平行で同じ向き。(AとBの順は、四元数RとQのかけ算の順によって入れ替わります。この文書のかけ算順の定義だと、BAの順です。) 割り算して長さを1に直します。

(α,β,γ)= (B×A) / (|B×A|)

cos(θ)は、位置ベクトルの内積 B・A を、|B|・|A|で割った値として作ります。

cos(θ)= (B・A) / (|B|・|A|)

三角関数の半角の公式によれば、

cos(θ/2)=±√{ 0.5 * (1 + cosθ) }
sin(θ/2)=±√{ 0.5 * (1 - cosθ) }

復号は、0°≦θ≦180°ですので、 cos(θ/2)≧0 かつ sin(θ/2)≧0 つまり、 両方ともプラス

cos(θ/2)=√{ 0.5 * (1 + cosθ) }

sin(θ/2)=√{ 0.5 * (1 - cosθ) }

拡大縮小も行う場合は、k = √{|B|/|A|}

オイラー角から四元数への変換やその逆変換 という問題 (背景説明篇)

この問題はしばしば相談を受けるのですが、数学的理由と、数学の外の事情で難しいのです。

3次元の回転は
(1) 回転軸である直線
(2) 回転角度
の2つの情報があれば、表現できます。これぞ「万能な3次元回転表現」です。
このような単発的な回転を single rotation といいます。
(注意: single rotation には回転中心点はありません。回転軸があるだけです。 ボールジョイントのような関節での回転における、異なる回転軸を有する single rotation が複数連なった rotation sequence であるならば回転中心点は考えることができます。)
(注意: 四元数は single rotation を回転軸と回転角度の情報で素直に表しています。
オイラー角は、一つの single rotation を、3つの single rotation に分解することで表そうとします。わざわざ複雑にしていると言えます。また、ジンバルロックの問題を引き起こす可能性を孕みます。)

「オイラー角→四元数」
という直接的な変換を考えるのは非常に難しい問題です。
正しくは
「オイラー角→万能表現→四元数」
といった
「ある表現形式→万能表現→別の表現形式」
のルートを取るべきです。これが一番安全で、考えやすいと思います。

変換の難しさの数学的理由

変換の難しさの数学以外の理由

今さら、なぜオイラー角使うのか? 出力が、ロール、ピッチ、ヨーであるジャイロが多いからかしら。砲の銃座のように機構がロール、ピッチ、ヨーを使いようになっている場合もあります。

オイラー角とは何か

Euler さんは、「ひとつの三次元のsingle rotationは、座標系の軸を回転軸とする三次元回転を3個組み合わせることで表現することができる」ということを考えつきました。これが広義のオイラー角。

つまり、座標系から見えると傾いている軸を回転軸とする回転は、、うまいことやれば、まずZ軸中心でα度回転させ、次にX軸中心にβ度回転し、最後にZ軸中心にγ度回転させれることで、そっくり同じ回転を作り出すことができるというものです。(ジンバルロック発生の場合は、解の一意性が失われて、面倒くさいことになりますが。)

オイラーさん自身は、回転軸の選び方をZ-X-Zを使っています。この軸選びの順のケースが、狭義のオイラー角です。(独楽の運動を考えるとき、まず独楽の高速な自転をZ軸回転で表し、独楽の芯棒が地面に向かって倒れ込む回転をX軸回転で表し、独楽の芯棒が水平回転することをZ軸回転で表しています。)

世の中にはZ-Y-Zとか、X-Z-X, Y-X-Yのようなパターンのことを、オイラー角と称する人もいるようです。

軸の選び方は、X-Y-Zとか、Y-Z-Xのような3軸じゅんぐり使用のパターンもあります。(なお、3つの軸の立場は平等ではありません。回転操作の先後に起因する違いがあります。)これらを広義のオイラー角とする人もいて、狭義のオイラー角しかオイラー角と認めない人とは誤解の元になります。

X-Y-Z式は頻用されるパターンです。これをロール・ピッチ・ヨー角式と呼ばれたり、カルダン角 (Cardan angles)と呼ぶ人もいます。

オイラー角から四元数表現へ

これは極めて簡単で、オイラー角の3回の回転を、それぞれ四元数で表し、これらを順番にかけ算すれば出来上がり。

四元数からオイラー角へ

「オイラー角→四元数」の逆変換を考えることになります。公式化してもいいのですが、オイラー角がZ-X-Z式なのかX-Y-Z式なのかの違いで、大きく変わってしまいます。シンバルロック発生時には、公式の中でゼロ割りが起きますので、その処理も面倒みてください。

シングルローテーションの不思議

ひとつのシングルローテーションは、ひとつの四元数で書き表せるのでした。

すると、シングルローテーションをNつ続けて行って作る回転は、N個の四元数の積である四元数1個で書き表せることが言えます。つまり、複数のシングルローテーションの数珠繋ぎ実行は、経過を無視して、その結果だけを見るなら、たった1つの四元数で表すことができます。

結局、いくらゴロゴロ回しても、たった1つのシングルローテーションを行ったに過ぎないというわけです。サッカーボールをあっちに回し、こっちに回しと繰り返しても、初期姿勢と最終姿勢とに於いて、全く移動していない点が少なくとも2つはあるということです! あるいは、1回の回転で最終姿勢まで一っ飛びでいけることも意味します。

コマはなぜ簡単に倒れないのか?

コマの運動を瞬間的に観察してみましょう。コマの高速な自転と、芯棒の倒れ込み回転は、それぞれシングルローテーションです。従って、2つの回転が存在するようにに見えても、合成してみると、コマはひとつのシングルローテーションを行っているに過ぎないと言えます。自転軸とは微妙に斜めの軸が真の回転軸であり、コマはその軸を中心として高速回転しているわけです。

この合成されたシングルローテーションの回転軸は、機械的な拘束の都合上、コマの接地点を通らなければなりません。しかしそれでは、回転はコマの重心を外しています。

重心を外した回転のせいで、コマの芯棒は方向を変えます。この方向転換により、芯棒が単純に地面に激突することは防がれているのです。

シングルローテーションなのか、まぎらわしい例

まず、地球儀を用意し、自転させます。その状態で、地軸を一定の速度で決まった方向に回転させてみましょう。

このとき、自転はシングルローテーション。また、地軸の回転もシングルローテーションです。しかし、地球儀の各点は、シングルローテーションではない、複雑な動きをします。

一瞬だけを見れば、地球儀はシングルローテーションを行っています。しかし、次の瞬間には、地軸の方向が変わってしまい、地球儀は先ほどとは違う自転を強制されます。従って、もはや自転は単調なシングルローテーションの繰り返しにはなりません。

このように、機械的拘束をする回転軸がシングルローテーションしつづける場合には、注意が必要です。

四元数ムダ知識

※ 四元数の絶対値

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 つまり (1; 0, 0 ,0) になるような四元数を、逆四元数といいます。

逆四元数の実態は、共役四元数の各成分を、四元数の絶対値の2乗で割ったものです。

変形操作のUndoをしたい場合は、逆四元数を使ってください。

なお、四元数の絶対値がゼロなら、逆四元数は無限大になり、要するに存在しません。