姿勢表現の基礎を勉強したのでその備忘録として記します。
ローカル座標からグローバル座標への変換を行うのに必要な知識までを記します。
やることは姿勢の合成・逆回転・ベクトルの回転です。
前提知識
グローバル座標は絶対座標ともいわれ、共通の基準点を使って座標を表します。高校までは基本的にこれを使います。原点Oがここでいう基準点です。つまり高校数学で使ういつもの座標軸ですね。
ローカル座標は自身から見てどこにあるかを表したものです。高校数学で一番近い要素は位置ベクトルですかね。ベクトルABはAから見たBの座標とも言えますし。実際のローカル座標は姿勢が関与し、自身が時計回りに90度回転して、もともと(0,1)[上方]だったのが、自身から見て(-1,0)[左方]になるといったことが起こります。簡単にいうと、「あなたから見て右斜め前15m」といったものです。
姿勢表現
姿勢表現で有名なのは回転行列・オイラー角・クォータニオンの三つです
これらのうち、ベクトル回転を容易に行えるのは回転行列とクォータニオンとなります。
回転行列は前方・右方・上方の方向ベクトルを使い姿勢を表しています。
大きさは１にそろえられ、すべて垂直に交わります。
やってることは座標軸の再定義ですね。
大きさは1、垂直であればこのうち2本でも一意の姿勢を表せるのですが、行列計算を行うには結局3本必要となります。理由がなければ削らない方がいいです。なお、左手系か右手形かを定義しておかないと一意の姿勢は表せません。第三のベクトルの方向の候補が二つ存在してしまうためですね。
行列同士の合成
姿勢同士の合成は行列1*行列2となります。
この計算で姿勢が合成できるのはなぜでしょうか？
まずは回転行列を振り返りましょう。
回転行列は前方・右方・上方の三つで姿勢を表しますが、これを新たな座標軸の定義だと捉えることができます。
言い換えると「グローバル座標軸を回転させるとこの座標軸になる」となります。
回転行列1*回転行列2では、回転行列2の元々の座標軸をグローバル座標軸ではなく、回転行列1の座標軸とした時にどうなるかを計算しています。
言うなれば「回転を順番に2回適用している」だけですね。
最適化を行なっていない、何を行なっているのかがわかりやすい合成用の関数を書いておきます。
合成の式はネット上に多く公開されているので実際はそちらを使った方がいいでしょう。
なお、ここから先の行列はすべてベクトルが横一列に記述される行優先を前提とします。
function m_mul(m1, m2)
local function dot_row_col(m1_row_idx, m2_col_idx)
-- m1_row_idx = 0..2（行番号）、m2_col_idx = 0..2（列番号）
-- row i of m1: indices 1+3*i .. 3+3*i
-- col j of m2: m2[1 + j], m2[4 + j], m2[7 + j]
local r = 0
r = r + m1[1 + 3*m1_row_idx] * m2[1 + m2_col_idx]
r = r + m1[2 + 3*m1_row_idx] * m2[4 + m2_col_idx]
r = r + m1[3 + 3*m1_row_idx] * m2[7 + m2_col_idx]
return r
end
return {
-- row 1
dot_row_col(0, 0), dot_row_col(0, 1), dot_row_col(0, 2),
-- row 2
dot_row_col(1, 0), dot_row_col(1, 1), dot_row_col(1, 2),
-- row 3
dot_row_col(2, 0), dot_row_col(2, 1), dot_row_col(2, 2)
}
end
なお、回転の順序が逆になると結果が異なるので、気をつける必要があります。
行列同士の計算を何度か行ったら正規化を行う必要があるのですが、割と複雑なので省略です。数回程度ならやらなくてもなんとかなります。
気になる人はグラム・シュミットの正規直交化で調べてください
この性質は姿勢表現である以上どうしても共通するので、どの規格でも気をつけてください。
行列でのベクトル回転
ベクトル回転の場合、回転行列*ベクトルで計算できます。
これはなぜでしょうか？
これを理解するにはまずベクトルの表し方を行列スタイルに慣らす必要があります。
皆さんが慣れているのはおそらくV=(x,y,z)だと思います。
これはX方向にX分だけ進むという意味を組み合わせたものです。
これをV=(1,0,0)x+(0,1,0)y+(0,0,1)zという書き方とします。
xyzは各軸方向の“大きさ”だと認識してください。
回転行列では(1,0,0)などで軸の方向を表します。
そもそもX軸などの方向が再定義されるのでこうした書き方になります。
とりあえずx成分の部分を抽出すると(1,0,0)xとなります。
回転によってこれが(0.7,0.7,0)xの様に変化したりします。
回転によってx成分がどれほど他の軸にも影響を与えるかをこうして表しているわけです。
これを三軸すべてで行うと回転後のベクトルが求められる様になります。
関数化したものはこれです。
-- ベクトルの回転 (vector*M)
function m_rot(m, vector)
--v' = V * M
return {
vector[1]*m[1] + vector[2]*m[4] + vector[3]*m[7],
vector[1]*m[2] + vector[2]*m[5] + vector[3]*m[8],
vector[1]*m[3] + vector[2]*m[6] + vector[3]*m[9]
}
end
最適化を求めるのであればネットで検索した方がいいかもしれません。
私は普段クォータニオンを使うのでこの関数があっていると保証できないのです。
逆行列の求め方
回転行列を逆回転にするにはどうすればいいでしょうか？
転置という操作を行うことで逆回転に変換できます。
根本的な理解は厳しいのでそういうものだ程度の理解でいいです。
なんで？と言われるとはっきりとは答えられないのですが、(転置行列)(回転行列)=(逆行列)(回転行列)=(単位行列)だから、方程式上は同じになるという回答が一番簡単ですかね？
正規直交基底は逆行列と転置行列が同じになる性質があり、正規直交基底である条件は、各ベクトルが直交しており、大きさが１になっているというもので、回転行列は各ベクトルは直交しており大きさが１だから正規直交基の性質よりという説明になります。
関数はこれです。
function m_reverse(m)
-- 転置を返す
return {
m[1], m[4], m[7],
m[2], m[5], m[8],
m[3], m[6], m[9]
}
end
クォータニオンは回転軸を定義してその軸周りに回転させるという表現を行います。
回転量を実数で、回転軸を三種の虚数で表します。
それぞれを実部・虚部といいます。
全体の大きさが1になるので一部のパラメータをいじると他の値も変化します。
このためとても複雑に見えてしまいますが実部と虚部を分けて見れば非常に単純です。
表し方の意地が悪いだけでただの回転量と回転軸ですからね。
クォータニオンには交換法則が成立しないなど特殊な性質をいくつか持っていますがそこらはこちらの方の動画わかりやすかったです。
本当は勉強しなければいけない内容なんですが、使うだけならば案外知らなくてもなんとかなるので省略です。
クォータニオンの合成
クォータニオン同士の合成はどう行うのでしょうか？
q1*q2で合成します。
ただし虚数部分に癖があるのである程度計算式を事前に展開する必要があります。
検索したことのある方はこの式を見たと思います。
W=w1w2-x1x2-y1y2-z1z2
X=w1x2+x1w2+y1z2-z1y2
Y=w1y2-x1z2+y1w2+z1x2
Z=w1z2+x1y2-y1x2+z1w2
この複雑な式はクォータニオン同士の乗算を要素ごとに分類したものです。
虚数を持たない部分はwに、iを含む項はXに、jはYに、kはZといったように分類します。
これらをまとめた関数は概要欄に記載しておきます。
ここからは計算の順番があたえる結果について話します・
synthesis_q1*q2(q1,q2)という関数があったとします。
q1を現在の姿勢、q2を右に90度回転を意味するクォータニオンとします。
q1,q2をそのまま代入するとq1をローカル座標軸でq2分回転させた姿勢を表します。
q1,q2を逆に代入するとq1をグローバル座標軸でq2分回転させた姿勢を表します。
左から右か右から左かは表記によりますが、ここでは左から右です。
ここで最も大切なのはどの順番で回転させたか、それがすべてです。
合成の関数はこれです。
function q_mul(q1, q2)
local w1, x1, y1, z1 = q1.w, q1.x, q1.y, q1.z
local w2, x2, y2, z2 = q2.w, q2.x, q2.y, q2.z
-- ハミルトン積
local w = w1*w2 - x1*x2 - y1*y2 - z1*z2
local x = w1*x2 + x1*w2 + y1*z2 - z1*y2
local y = w1*y2 - x1*z2 + y1*w2 + z1*x2
local z = w1*z2 + x1*y2 - y1*x2 + z1*w2
-- 正規化
local n = math.sqrt(w*w + x*x + y*y + z*z)
return {
w = w / n,
x = x / n,
y = y / n,
z = z / n
}
end
最後にnで割っているのは正規化を行うためでありこれによって大きさが1に保たれます。
合成が乗算によって行われる理由
このセクションがあまりに短すぎるのでなぜ合成が掛け算なのかという話をします。
クォータニオンの前提知識の一つですが、使うだけなら飛ばしていいです。
必須じゃないです。
複素平面の基となる公式にオイラーの公式があります。
e^iθ=cosθ+isinθという式で、一つの回転を扱うことができます。
二つの式を乗算してみましょう。
(e^ia)*(e^ib)=cos(a+b)+isin(a+b)となります。
乗算によって指数が加算されています。
これがオイラーの公式において乗算が姿勢の合成となる理由となります。
クォータニオンはオイラーの公式の三次元拡張を行ったものなので複素平面と同じ様に掛け算で合成します。
クォータニオンでのベクトル回転
クォータニオンでベクトルを回すにはどうすればいいのでしょうか？
q1Vq1^-1、つまりクォータニオンと逆クォータニオンでベクトルを回転させます。
この計算をサンドイッチ積と言います。
よくわからないと思うので詳しく説明します。必須じゃないので聞き流してもいいです。使えればいいんだよ使えれば
まずはベクトルは(0,x,y,z)とすることでクォータニオンで計算できるようにします。
このとき正規化は行いません。姿勢じゃないからですね。
サンドイッチ積を使う理由はWを0にできて都合がいいからです。
単純にq*vのような操作をするとwに不要なものが混ざります。
wが0でなければベクトルではない何かになってしまい、計算に使えません。
逆クォータニオンが式に入っているあたりで打ち消すのでは？と疑問を持った人もいるでしょうが理論上は大丈夫です。
計算式を展開していくとwは0になり、虚部はロドリゲスの回転公式というベクトルを回転させる公式と同じ形に変形できることがわかります。
ただし等倍のθであるべきところが2θになっているので、そこを何とかする必要があります。
これに対して数学者は最初からθ/2にするという手法で対応したようです。
これによって副作用どころか一回転目と二回転目が見分けられるといった特異な性質を獲得しました。
姿勢どうしの補完でクォータニオンが強いといわれる理由がこれです。
サンドイッチ積を感覚的に理解するのは正直厳しい気がします。
投稿者もまったくできてません。
理論上行けるぐらいでいいと思ってます。
計算する関数はこれです。
function Quaternion_rotate(q, v)
local qw, qx, qy, qz = q.w, q.x, q.y, q.z
local iw = -qx * v.x - qy * v.y - qz * v.z
local ix = qw * v.x + qy * v.z - qz * v.y
local iy = qw * v.y + qz * v.x - qx * v.z
local iz = qw * v.z + qx * v.y - qy * v.x
return {
x = ix * qw - iw * qx - iy * qz + iz * qy,
y = iy * qw - iw * qy - iz * qx + ix * qz,
z = iz * qw - iw * qz - ix * qy + iy * qx
}
end
オイラー角はあまり知らないので今回省略です。
一つ言えることはジンバルロックという重大な欠陥があるので初手変換が安定です。
変換
ここからは変換だけを扱います。
最初に資料を提示します。ここで使われているのはおそらく内部回転を前提としたオイラー角だと思われます。多くのパターンの相互変換を扱っています。
回転行列、クォータニオン(四元数)、オイラー角の相互変換 #数学 - Qiita
https://qiita.com/aa_debdeb/items/3d02e28fb9ebfa357eaf#回転順zyx
オイラー角は回転軸が移動する内部回転と固定されている外部回転の二種類が存在しますが、これによって変換の方法が変わります。
オイラー角はrollの次にyaw、pitch のように順々に回転を適用して行きます。
これは環境によって全く異なるので要確認となります。
各軸の回転ごとならばクォータニオンも回転行列も容易に組み立てることができるのでこれらを合成していきます。
例えばオイラー角がZ→Y→Xのとき、内部回転であればX→Y→Zの順番で、外部回転であればZ→Y→Xの順番で合成していきます。
これは回転qなどを姿勢qで乗算するとローカル基準での回転を表せるということを利用しています。内部回転ですね。計算式は左から右の時qzqyqxです。
逆に姿勢ｑを回転ｑで合成するとグローバル基準で回せます。外部回転ですね。計算式は左から右の時qxqyqzです。
先に回転を適用した時はローカル基準で適用したような姿勢に変化し、後で適用するとグローバル基準で適用したような姿勢になりました。
まあそのまま計算すると割と重たいので最適化されたものが基本使われます。
内部回転のZ→Y→Xオイラー角から回転行列はこの関数です。
function Euler_to_matrix(pitch, yaw, roll)
--内部回転 Z(roll)→Y(yaw)→X(pitch)
--行優先右手系回転行列
local sx, cx = math.sin(pitch), math.cos(pitch)
local sy, cy = math.sin(yaw), math.cos(yaw)
local sz, cz = math.sin(roll), math.cos(roll)
--m11,m12,m13
--m21,m22,m23
--m31,m32,m33
return {
cy*cz,
-cy*sz,
sy,
sx*sy*cz + cx*sz,
-sx*sy*sz + cx*cz,
-sx*cy,
-cx*sy*cz + sx*sz,
cx*sy*sz + sx*cz,
cx*cy
}
end
環境によって使う関数は全く違ってくるので添付した資料のほうを見てください。
このサイトが一番親切だと思います。
内部回転かつZ→Y→Xオイラー角からクォータニオンへの変換はこのコードです。
function Euler_to_quaternion(pitch, yaw, roll)
--内部回転 Z(roll)→Y(yaw)→X(pitch)
local sx, cx = math.sin(pitch/2), math.cos(pitch/2)
local sy, cy = math.sin(yaw/2), math.cos(yaw/2)
local sz, cz = math.sin(roll/2), math.cos(roll/2)
return {
w = cx*cy*cz + sx*sy*sz,
x = sx*cy*cz - cx*sy*sz,
y = cx*sy*cz + sx*cy*sz,
z = cx*cy*sz - sx*sy*cz
}
end
これも環境によって関数が全く変わってくるので資料のほうを見てください。
姿勢制御で変換などを扱うときに引っかかるトラップがあります。
左手系と右手系の違いに気を付けないとAIでも気づきにくい論理エラーを引き起こします。
オイラー角の場合、左手系と右手系の変換はroll,yawの角度の値と、pitch方向の座標の符号を反転させることでできます。
より詳しく言うと、pitchに当たる回転軸以外を反転させ、右側の座標軸の符号を反転させます。
XYZで言わないのは環境によって異なる部分だからです。でもpitchとかは変わりようがないからこの表現をしています。
左手系と右手系の変換はクォータニオンや回転行列などに変換する前にやった方がいいと思います。
ちなみに今提供した変換関数はStormworksに合わせてあるので、入力値を左手系から右手系に変換するだけで使えます。
グローバル ローカル変換
ここからはグローバルとローカル変換を行っていきます。
ここからの話はStormworks環境を前提としています。
やることは結局のところ座標を回したり足したりするだけです。
ローカル座標を姿勢を使って回転させることで、グローバル座標と軸を合わせます。
このままでは自身基準の座標なので、自身のグローバル座標を加えることで原点基準のグローバル座標に変換します。
自身の10m前→(0,10)→(自身の座標)+(0,10)
(ローカル)→(自身基準の座標)→(グローバル)
ローカル座標への変換はさっきと逆のことを行います。
自身の座標分減算し、自身の姿勢を使ってベクトルを逆回転します。
コードはStormworks仕様になってるから環境に合わせて変更する必要があります。
StormworksはX:pitch,Y:yaw,Z:rollの左手系座標軸を採用していて、Stormworksにおいては座標軸も回転軸も割り当てられた文字は同じです。
別環境では異なることもあります。
-- IO
INN = input.getNumber
OUN = output.setNumber
--========================================
-- Local direction (pitch, yaw → vector)
--========================================
function Local_direction(pitch, yaw)
local cp = math.cos(pitch)
return {
x = cp * math.sin(yaw),
y = math.sin(pitch),
z = cp * math.cos(yaw)
}
end
--========================================
-- Quaternion rotate
--========================================
function Quaternion_rotate(q, v)
local qw, qx, qy, qz = q.w, q.x, q.y, q.z
local iw = -qx * v.x - qy * v.y - qz * v.z
local ix = qw * v.x + qy * v.z - qz * v.y
local iy = qw * v.y + qz * v.x - qx * v.z
local iz = qw * v.z + qx * v.y - qy * v.x
return {
x = ix * qw - iw * qx - iy * qz + iz * qy,
y = iy * qw - iw * qy - iz * qx + ix * qz,
z = iz * qw - iw * qz - ix * qy + iy * qx
}
end
--========================================
-- Euler → Quaternion (Stormworks)
--========================================
function StormworksEuler_to_quaternion(pitch, yaw, roll)
--左手系から右手系へ変換したうえでクォータニオンに変換
yaw = -yaw
roll = -roll
local sx, cx = math.sin(pitch/2), math.cos(pitch/2)
local sy, cy = math.sin(yaw/2), math.cos(yaw/2)
local sz, cz = math.sin(roll/2), math.cos(roll/2)
return {
w = cx*cy*cz + sx*sy*sz,
x = sx*cy*cz - cx*sy*sz,
y = cx*sy*cz + sx*cy*sz,
z = cx*cy*sz - sx*sy*cz
}
end
--========================================
-- onTick
--========================================
function onTick()
-- 入力
local pitch = INN(1) -- ローカル pitch
local yaw = INN(2) -- ローカル yaw
local dist = INN(3) -- 距離
local e_pitch = INN(4)
local e_yaw = INN(5)
local e_roll = INN(6)
local base_x = INN(7)
local base_y = INN(8)
local base_z = INN(9)
-- 変換
local q = StormworksEuler_to_quaternion(e_pitch, e_yaw, e_roll)
local d_local = Local_direction(pitch, yaw)
local d_global = Quaternion_rotate(q, d_local)
local gx = d_global.x * -1 * dist + base_x--左手系への変換
local gy = d_global.y * dist + base_y
local gz = d_global.z * dist + base_z
-- グローバル座標出力
OUN(1, gx)
OUN(2, gz)
OUN(3, gy)
end