4元数はオイラー角の弱点を超える
このページはマス旅の一部です。
前回は4元数を計算の基本を中心に見てきました。
今回は4元数の３Dの道具としての側面を調べます。
オイラー角（α、β、γ）は直観的にわかりやすい反面、弱点があります。
その弱点を4元数がどう乗り越えるのかを見ていきましょう。
１．オイラー角の弱点は特異点
＜ジンバルロック＞
cosをc、sinをsとかきます。
z軸を中心にPをγ（ヨー角）回転するとき、Rz={{cγ, -sγ, 0}, {sγ, cγ,0 }, { 0,0,1}}だった。
y軸を中心にPをβ（ピッチ角）回転するとき、Ry={{cβ,0, sβ}, {0, 1,0 }, { -sβ,0,cβ}}となり、
x軸(ベクトルp)をα（ロール角）回転して姿勢変化するとき、Rx={{1,0,0}, {0,cα,-sα }, { 0,sα,cα}}となり、
回転の合成によって、
G=Rz◦Ry◦Rx=
[[cγcβ, cγsβsα-sγcα, cγsβcα+sγsα],
[sγcβ, sγsβsα+cγcα, sγsβcα-cγsα],
[-sβ, cβsα , cβcα ]]
となった。この３つの角はオイラー角というものだった。
ジンバルロックという不都合は、2番目の回転であるピッチ角 βが±90度のときに発生します。
β＝90度のとき、cβ=0, sβ=1から、
G=
[[０, cγsα-sγcα, cγcα+sγsα],
[０, sγsα+cγcα, sγcα-cγsα],
[-１, ０, ０ ]]
=[[０, s(α-γ), c(α-γ)],
[０, c(α-γ), -s(α-γ)],
[-１, ０, ０ ]]
α-γがsin,cosの引数になっている。αとγの独立ができない。
同様に、β=-90度のときは、sin,cosの引数がα＋γとなり、やはりαとγの独立性が失われる。
さらに逆算時に「ゼロ割り」が発生して破綻する。
これをオイラー角のロックともいいますね。
オイラー角の最大の問題は、ある姿勢から「次の姿勢」へ変化させるとき、あるいは現在の姿勢から逆に角度 α、β、γを逆算（センサデータから姿勢を推定するなど）しようとするときに起こります。
オイラー角の計算では,βが±90度逆算の過程でcβ＝０ による割り算が発生し、システムがフリーズしたり、数値が無限大に発散したりします。
一方で、4元数は常に4つの成分の平方和が1（q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1）となる4次元球面上の点を移動するだけです。どのような角度が代入されても、4元数の積の計算には割り算が一切登場しません（すべて掛け算と足し算のみ）。 そのため、数学的な破綻（特異点）がどこにも存在しません。
＜ 4元数の自由さのメリットとデメリット＞
オイラー角は常に「世界固定の3軸」に縛られているため、ジンバルロック時に失われた方向への回転命令を出すことができません。
しかし4元数は、βが±90度になった状態（現在の姿勢を q_{current} とする）から、さらに別の任意の軸 (v = [v_x, v_y, v_z]) のまわりに微小角Δθ だけ回転させたいとき、新しい微小回転4元数Δqを作り、単純に後ろから掛けるだで計算が完了します。q_{new}=q_{current}◦Δq
四元数は「3つの角度の組み合わせ」ではなく、「空間上のダイレクトな向き」そのものを保持しているため、直前の状態がどうであろうと、そこからあらゆる方向へ滑らかに、等速で回転を続けることができます。姿勢を「更新し続ける（動かし続ける）」処理においては、4元数が圧倒的に安全な理由がここにあります。
ただし、自由に回転できるということは、いいことですが、基準となる「世界固定の3軸」は、常識的で理解しやすいモノサシでもあります。そのモノサシを回転の基準として採用しないとしても、センサーとしての基準系としては「世界固定の3軸」は有用です。だから、操作系とセンサー系のシステムを別に作り、操作系は4元数で、センサー系はオイラー角で作るという、ハイブリッドなソフトを作ることが必要になりますね。
課題：ジンバルロックをgeogebraで体験するにはどうしたらよいですか。
タイトルは「ジンバルロックを体験しよう」
α = Slider(0, 2π, 2π/10) #回転軸
β = π/2
γ = Slider(0, 2π, 2π/10) #回転角
R = (1, 2, 3) #回転させたい点
Vector((0,0,0), R) #位置ベクトルR
s = sin(α-γ)
c = cos(α-γ)
x_p = 0*x(R) + s*y(R) + c*z(R)
y_p = 0*x(R) + c*y(R) - s*z(R)
z_p = - x(R) + 0*y(R) + 0*z(R)
R' = (x_p, y_p, z_p)
Vector((0,0,0), R') #回転後のベクトルR'
a=α-γ
text1="+a+" # 太字で特大にする。
αもγも10分の2π単位で増減できます。
βを π/2に固定したことにより、ジンバルロックが発生中です。
だから、αとγを同じ単位だけ増やすと、差α-γ=0になりますので、
回転行列は同じになります。
言い方を変えると、αの動きをγの動きが邪魔して相殺してしまいます。
独立性が失われて、αとγが同じ１つの次元に収まってしまったことが実感できますね。
本来なら「左右に回す（ヨー）」「傾ける（ロール）」という独立した2つの自由度があったはずなのに、
合成行列の中で2つの角度が束ねられ、「同じ1本の軸をまわる1つの自由度」へと退化（潰れて）しまったのです。
ジンバルロックを体験しよう
２．オイラー角の弱点は補完のぎこちなさ
オイラー角 (α1、β1、γ1) から、目標姿勢 (α2、β2、γ2)へ数値を徐々に変化させてアニメーションを作るとします。各角度を単純に直線的に変化、線形補間Lerp（ラープ）すると、3つの軸が複雑に絡み合うため、「途中でヘンテコな遠回りをする」「回転速度が途中で加速・減速して不自然になる」という現象が発生します。それをさける計算が4元数にはあります。それが球面線形補間 Slerp（スラープ）
＜Slerp（スラープ）＞
姿勢を表す4元数は4次元の単位球面上の1点になるので、姿勢の連続的な変化は、球面上の点移動ですね。
なめらかで無駄のない動きは、2つの姿勢点を通る大円に沿って、一定の角速度で滑らかに点を移動させることですね。これを実現する補間が球面線形補間 Slerp（スラープ） っていうものだ。
考え方は２ベクトルa,bの内分点の計算(1-t)a+tb（ｔは０以上1以下）と似ています。
単位球面上の3つの単位4元数を q_1, q_2, xとしましょう。
もちろん、ｘはq_1,q_2を通る大円上Cにあります。
すると、３つのq_1,q_2,xは単位円周上のベクトルと見ることができますね。
そこで、ベクトルとみなした内積から、 q_1・q_2 = cosΩとすれば、
Ωがq_1とq_2の2ベクトルの作る角です。ｘはその角の途中にあるので、
q_1・ｘ=cos(tΩ),ｘ・ q_2＝cos((1-t)Ω)（tは0以上1以下）とおけるね。
xはq_1とq_2の張る平面にあるから、x=a q_1 + b q_2とおける。
内積は交換法則が成り立ち、三角関数の加法定理などを使うと、
x・q_1=(a q_1 + b q_2)・q_1＝a q_1・q_1 + b q_2・q_1= a + b cosΩ= cos(tΩ)
x・q_2=(a q_1 + b q_2)・q_2＝a q_1・q_2 + b q_2・q_2= a cosΩ + b = cos((1-t)Ω)
このa,bの連立方程式を解こう。
a + b/cosΩ = cos((1-t)Ω)/cosΩ を第１式からひく。
b(cosΩ-1/cosΩ)= cos(tΩ)-cos((1-t)Ω)/cosΩ
b(cos^{2}Ω-1)= cos(tΩ)cosΩ-cos((1-t)Ω)
b = [cos((1-t)Ω)-cos(tΩ)cosΩ]/sin^{2}Ω
=[ cos(Ω)cos(tΩ)+sin(Ω)sin(tΩ)-cos(tΩ)cosΩ]/sin^{2}Ω
=sin(Ω)sin(tΩ)/sin^{2}Ω=sin(tΩ)/sinΩ
a = cos(tΩ)-cosΩ*sin(tΩ)/sinΩ=[sinΩcos(tΩ)-cosΩ*sin(tΩ)]/sinΩ
= sin(Ω-tΩ)/sinΩ=sin((1-t)Ω)/sinΩ
a=sin((1-t)Ω)/sinΩ, b=sin(tΩ)/sinΩだから、
x= q(t) = sin((1-t)Ω)/sinΩ q_1 + sin(tΩ)/sinΩ q_2
これがスラープの計算式だ。
課題：geogebraでスラープを視覚化するにはどうしますか。
タイトルは「スラープで２つの4元数をスムーズに補間する」
θ = Slider(0, 2π, 0.01) # 総回転角
α = Slider(0, 2π, 0.01) # 回転軸の傾き
β = Slider(-π/2, π/2, 0.01) # 回転軸の仰角
t = slider(0, 1, 0.01) #アニメーション
n = (cos(β)*cos(α), cos(β)*sin(α), sin(β)) #単位回転軸ベクトルn
#q_1={1,0,0,0)
#q_2=(cos(θ), sin (θ) x(n), sin (θ) y(n),sin (θ) z(n)}
# q1=(1,0,0,0) と q2 の内積より Ω = θ / 2
Ω = θ / 2
# 4元数qtの虚部： sin((1-t)Ω)/sin(Ω)*0 + sin(tΩ)/sin(Ω)*sin(θ/2) * n
# = sin(t*θ/2)/sin(θ/2) と簡略化
x_q = (sin(t*Ω) / sin(Ω)) * sin(θ/2) * x(n)
y_q = (sin(t*Ω) / sin(Ω)) * sin(θ/2) * y(n)
z_q = (sin(t*Ω) / sin(Ω)) * sin(θ/2) * z(n)
q_1 = (0, 0, 0)#始点のベクトル部
q_2 = (sin(θ/2)*x(n), sin(θ/2)*y(n), sin(θ/2)*z(n)) # 終点のベクトル部
q_t = (x_q, y_q, z_q)# Slerpで動く位置ベクトル
ベクトルが原点からq_2までスムーズに動くことがわかりますね。