LoginSignup
0
3
@Philosophistoria(A. Takahashi)

IMU/MARGでの姿勢推定のためのQuaternion導入

Last updated at Posted at 2020-10-11

Abstruct

別記事(編集中) で用いるクォータニオンに関する準備知識の導入．
計算しながら書いてるため途中式多め．また計算ミスもあるかもしれないので見つけたらご指摘いただけると助かります．
また別記事から分量が多くなったため分けてきたので，文脈的に唐突なのはその記事参照すると分かるかもしれない．

クォータニオンの基礎と計算

（クリックで開く）

実数，ベクトル，クォータニオンの表記

過去文献1を参考に以下のようにした．

  • 実数はイタリックの小文字アルファベット: $w,x,y,z,\alpha,\beta,\gamma$
  • ベクトルは太字イタリックの小文字アルファベット: $\boldsymbol{v}$
  • クォータニオンは太字イタリックの小文字アルファベットの上に "$\sim$" がついたもの: $\boldsymbol{\tilde{q}}$

またユークリッド空間上の位置ベクトルなどの3次元ベクトル$\boldsymbol{v}$を純虚クォータニオン（実部が0）
として拡張して考えた場合，特に断りなく$\boldsymbol{\tilde{v}}$ と書くことがある．
またクォータニオンのベクトル部を表わす$\boldsymbol{v}$は純虚クォータニオンのベクトル表現として用られる．

クォータニオンの定義

クォータニオンは日本語で言うと四元数で複素数の拡張版といえる 2．

クォータニオン，複素数のざっくりとした理解

いろいろ書いたけど大体wikipediaに載ってたので飛ばしてよい．

ノルムが1の複素数が2次元平面の回転を表すようにノルムが1の四元数は4次元空間の回転を表す 3．
三元数はないのって思う機会がある人は勉強熱心でいいと思う．（自分はそんな疑問を持つ前に4元数に出会ってしまった）
クォータニオンの発見者？開発者？のハミルトンも三元数うまくいかねーってなってたけどとある橋のそばで四元数はできるじゃんっと思いついたらしい 4．
三元数は複素数に似たように定義しても群としてさえの要請も満たさない（三元数同士の積が三元数に閉じてない）5ので使われてない．

複素数は2実数の組み合わせ（2次元ベクトル）の集合のうち，
積がちゃんと2数の組み合わせの集合に閉じるように定義されたものとみることができる．
さらにただの2次元ベクトルと違うところが，商がちゃんと同様に2数の組み合わせの集合に閉じている．
クォータニオン（四元数）も同様に4実数の組み合わせ（4次元ベクトル）の集合のうち，
和・差・積・商がちゃんと4数の組み合わせの集合に閉じるように定義されたものとみることができる．
（ただし交換法則は成り立たない．がこれが回転順序を変えると結果の姿勢も変わることをちょうど表す）．

なので昨今ベクトル解析で行われているようなものは昔は複素数やクォータニオンで研究されていた．
ただ回転に関して言えばやっぱり複素数やクォータニオンが強いので，今も主に空間上の回転がかかわるものにはこうして使われている．

クォータニオンの導出

素朴に四元数を複素数から拡張してみる．
複素数 $a+ib$ に対して，$a, b$ が実数ではなく$i$ とは垂直な別の虚数単位$j$で表せられる複素数だった場合，これを四元数（$\boldsymbol{\tilde{q}}$）とする，すなわち

\begin{align}
\boldsymbol{\tilde{q}}
&=(a_r+ja_i) + i(b_r+jb_i) \\
&= a_r + ib_r + ja_i + ijb_i\\
&=w + ix + jy + kz \hspace{2em} \mathrm{where} \quad w=a_r,\ x=b_r,\ y=a_i,\ z=b_i \in \mathbb R;\quad k:=ij
\end{align}

ここで $k$ は実数単位元1でもなく$i,j$とも別な虚数単位で $k^2=-1$．このとき以下のルールに従う

\begin{gather}
i^2 = j^2 = k^2 = ijk = -1\\ 
\begin{aligned}
ij &= k,  &jk &= i,  &ki &= j \\
ji &= -k, &kj &= -i, &ki &= -j
\end{aligned}
\end{gather}
kが虚数単位であることの確認（クリックで開く） 
i,jは虚数単位なのでi^2=j^2=-1\\
このもとで， 
\begin{align}
ij =  k  \Leftrightarrow i(ij) =  ik  &\Leftrightarrow ik = -j \quad (\because i^2=-1)\\
ij =  k  \Leftrightarrow (ij)j =  kj  &\Leftrightarrow kj = -i \quad (\because j^2=-1)\\
\\
ik = -j \Leftrightarrow (ik)j =(-j)j &\Leftrightarrow ikj = 1  \quad (\because j^2=-1)\\
ik = -j \Leftrightarrow j(ik) =j(-j) &\Leftrightarrow jik = 1  \quad (\because j^2=-1)\\
kj = -i \Leftrightarrow (kj)i =(-i)i &\Leftrightarrow kji = 1  \quad (\because i^2=-1)\\
\\
\end{align}

以上までは$k$が虚数単位であることは忘れて単に$i^2=j^2=-1 \land k=ij$ということだけでわかること．
これ以上はもう一つ式がないと展開できない．

ここでいま例えば三元数をあきらめきれずに$k \in \mathbb R$とかすると$ik = -j$より $i$ と $j$ が互いにスカラー倍で一致となって矛盾．
したがって$k\notin \mathbb R$である．

そのため，$i$, $j$にならって$k$も虚数単位とし，$k^2=-1$と定義してみると，
以下$i^2=j^2=k^2=-1\land k=ij$の下で

\begin{align}
ik = -j  \Leftrightarrow (ik)k = -jk  &\Leftrightarrow jk = i \quad (\because k^2=-1)\\
\therefore jk = -kj = i \\
\\
kj = -i  \Leftrightarrow k(kj) = -ki  &\Leftrightarrow ki = j \quad (\because k^2=-1)\\
\therefore ki = -ik = j \\
\\
kji = 1  \Leftrightarrow k(kji) = -k  &\Leftrightarrow ji = k \quad (\because k^2=-1)\\
\therefore ji = -ij = k \\

\end{align}

\begin{align}
\therefore ijk 
&= (-ji)k = -jik\\
&= -j(-ki) = jki\\
&= (-kj)i = -kji\\
&= -k(-ij) = kij\\
&= (-ik)j = -ikj = -1
\end{align}

また，各実数係数（$w, x, y, z$）はオイラーパラメータとよばれる．オイラー角とは別物．
計算機で計算するとき，取り扱うのは結局係数（オイラーパラメータ）の計算になる．

またクォータニオン $\boldsymbol{\tilde{q}}$ は実数項をスカラー部（$w$），虚数単位がつく3項をベクトル部（$ix+jy+kz$）と呼び，
しばしば，略記としてベクトル部を $\boldsymbol v={}^t(x,y,z)$ とし，
$$ \boldsymbol{\tilde{q}} = w + \boldsymbol{v} $$
と書くことがある．

これは複素数で実数項を実部，虚数項を虚部と呼ぶのと似ている．
ただし，複素数では実部と虚部それぞれの係数が平面上の位置ベクトルを表していたが，
クォータニオンにおいてはベクトル部を3次元空間上の位置ベクトルに対応させる．

またいくつかの文献でクォータニオンを4次元ベクトル表記をすることがあるが，

  • スカラー部が第1要素になる表記 ${}^t(w, {}^t \boldsymbol{v})={}^t(w, x, y, z)$
  • スカラー部が第4要素になる表記 ${}^t({}^t \boldsymbol{v}, w)={}^t(x, y, z, w)$

の2つがあるので注意されたし．

複素数を勉強し，複素平面をやっているときベクトルと複素数は結局どっちも2つの数のまとまりなのだけれど，本質的な違いは何だろうかと疑問に思ったことがあった．
あまりちゃんと勉強したわけではないのでこれは現在のところの理解ではあるのだが，結局のところ，複素数は「複素数の積」が定義された2次元ベクトルであると考えられる．

同様に，クォータニオンは4次元ベクトルの内，次に示すようなクオータニオンの積（ハミルトン積）が定義されたものと考えることができる．

複素数の積のやハミルトン積は一般的な内積，外積と違って逆元を定義できる．すなわち「割り算」ができるという点で今までのベクトルの拡張ととらえることができる．

クォータニオンのベクトル表現とハミルトン積の行列表現

上記でも少し触れたが，クォータニオンを4次元ベクトル表現（クォータニオンベクトルと呼ぶことにする）をする場合がある．
以下ではスカラー部が第1要素になる表記 ${}^t(w, {}^t \boldsymbol{v})={}^t(w, x, y, z)$を採用する．

ハミルトン積と内積，外積の関係

こうして分けることで面白い性質があり，それはベクトル部（純虚クォータニオン）のハミルトン積はベクトルの内積と外積の和で表せられるというものである．
実際，$\boldsymbol{v_1}=(x_1, y_1, z_1)$, $\boldsymbol{v_2}=(x_2, y_2, z_2)$に対し，
3つの演算子：ハミルトン積 $\otimes$，内積（スカラー積）$\cdot$，外積（クロス積）$\times$とすると，

\begin{align}
\boldsymbol{v_1} \otimes \boldsymbol{v_2}
&= (0 + ix_1 + jy_1 + kz_1) (0 + ix_2 + jy_2 + kz_2)\\
&= -(x_1x_2 + y_1y_2 + z_1z_2) +
 \left|
    \begin{array}{ccc}
       i  &  j  &  k  \\
      x_1 & y_1 & z_1 \\
      x_2 & y_2 & z_2 
    \end{array}
  \right|\\
&= - \boldsymbol{v_1} \cdot \boldsymbol{v_2} + \boldsymbol{v_1} \times \boldsymbol{v_2}
\end{align}

よって一般のクォータニオンの積は

\begin{align}
(w_1 + \boldsymbol{v_1}) \otimes (w_2 + \boldsymbol{v_2})
&= w_1 w_2 + w_1 \boldsymbol{v_2} + w_2 \boldsymbol{v_1} + \boldsymbol{v_1} \otimes \boldsymbol{v_2}\\
&= w_1 w_2 + w_1 \boldsymbol{v_2} + w_2 \boldsymbol{v_1} - \boldsymbol{v_1} \cdot \boldsymbol{v_2} + \boldsymbol{v_1} \times \boldsymbol{v_2}\\
&= (w_1 w_2 - \boldsymbol{v_1} \cdot \boldsymbol{v_2}) + (w_1 \boldsymbol{v_2} + w_2 \boldsymbol{v_1} + \boldsymbol{v_1} \times \boldsymbol{v_2})
\end{align}

ハミルトン積の行列表現

また2つのクォータニオンの積に関して，片方のクォータニオンベクトルにもう一方のクォータニオンのオイラーパラメータからなる行列を左からかける形で表現することがある．
これは計算機でクォータニオン積を計算するプログラムを書く際に，決まって処理される部分（虚数単位の二乗）があったり，
最終的に虚数単位ごとに係数をくくることが決まっているので，行列表現が適している場合がある．

2つのクォータニオン$\boldsymbol{\tilde{q}} = q_w + iq_x + jq_y + kq_z = {}^t(q_w,q_x,q_y,q_k) = {}^t(q_w,\boldsymbol{q})$，$\boldsymbol{\tilde{p}} = p_w + ip_x + jp_y + kp_z = {}^t(p_w,p_x,p_y,p_k) = {}^t(q_w,\boldsymbol{p})$に対して，
クォータニオン積（ハミルトン積）は以下のように表せる．

\begin{align}
\boldsymbol{\tilde{q}\tilde{p}}
&=(\color{pink}{q_w p_w} - \color{red}{\boldsymbol{q} \cdot \boldsymbol{p}}) + (\color{green}{q_w \boldsymbol{p}} + \color{blue}{p_w \boldsymbol{q}} + \color{brown}{\boldsymbol{q} \times \boldsymbol{p}})\\
&=
\begin{pmatrix}
\color{pink}{q_w} & \color{ red }{-q_x} & \color{ red }{-q_y} & \color{ red }{-q_z}\\
\color{blue}{q_x} & \color{green}{ q_w} & \color{brown}{-q_z} & \color{brown}{ q_y}\\
\color{blue}{q_y} & \color{brown}{ q_z} & \color{green}{ q_w} & \color{brown}{-q_x}\\
\color{blue}{q_z} & \color{brown}{-q_y} & \color{brown}{ q_x} & \color{green}{ q_w}\\
\end{pmatrix}
\begin{pmatrix}
p_w\\p_x\\p_y\\p_z
\end{pmatrix}
\\
&=
\begin{pmatrix}
\color{pink }{p_w} & \color{ red }{-p_x} & \color{ red }{-p_y} & \color{ red }{-p_z}\\
\color{green}{p_x} & \color{blue }{ p_w} & \color{brown}{ p_z} & \color{brown}{-p_y}\\
\color{green}{p_y} & \color{brown}{-p_z} & \color{blue }{ p_w} & \color{brown}{ p_x}\\
\color{green}{p_z} & \color{brown}{ p_y} & \color{brown}{-p_x} & \color{blue }{ p_w}\\
\end{pmatrix}
\begin{pmatrix}
q_w\\q_x\\q_y\\q_z
\end{pmatrix}
\end{align}

共役（共軛）クォータニオンとノルムと逆クォータニオン

共役クォータニオン

複素数 $\boldsymbol c = a+ib$ に対して，共役複素数 $\boldsymbol c ^{*} = a-ib$ があったのと同様に，
クォータニオン $\boldsymbol{\tilde{q}}=w+(ix+jy+kz)$ に対し，共役クォータニオン $\boldsymbol{\tilde{q}}^{*} = w-(ix+jy+kz)$ がある．
（共役と書くとどうも毎回「きょうえき」と読んでああちがうちがう「きょうやく」だとなりがちなので，共軛の方がよみやすいまである）

クォータニオンのノルム

複素数ではノルムの自乗（$||\boldsymbol{c}||^2$）が

\|\boldsymbol{c}\|^2 = \boldsymbol{c} \cdot \boldsymbol{c}^{*} = (a+ib)(a-ib) = a^2 + b^2

であったのと同様に，クォータニオンではノルムの自乗（$||\boldsymbol{\tilde{q}}||^2$）が

\begin{align}
\|\boldsymbol{\tilde{q}}\|^2
&:=\boldsymbol{\tilde{q}} \boldsymbol{\tilde{q}}^{*} = \boldsymbol{\tilde{q}}^{*} \boldsymbol{\tilde{q}}\\
&= (w+(ix+jy+kz))(w-(ix+jy+kz))\\
&= w^2 - (ix+jy+kz)^2\\
&= w^2 - \boldsymbol{v}^2\\
&= w^2 + x^2 + y^2 + z^2
\\
\because \boldsymbol{v}^2 &= -x^2 -y^2 -z^2 + (ijxy+jixy) + (jkyz+kjyz) + (kizx+ikzx)

\end{align}

である．ただし， $\boldsymbol{v}^2$ はベクトルの内積ではなく，スカラー部が0の純虚クォータニオンのハミルトン積である．見ての通り，互いに共役なクォータニオンのハミルトン積$||\boldsymbol{\tilde{q}}||^2$ は4次元ベクトルと見做したときの内積と一致する．

逆クォータニオン

また共役クォータニオンに似た概念として逆クォータニオンというものがあり，

\boldsymbol{\tilde{q}}^{-1} := \frac{\boldsymbol{\tilde{q}}^{*}}{\|\boldsymbol{\tilde{q}}\|}\\
\Leftrightarrow \boldsymbol{\tilde{q}}\boldsymbol{\tilde{q}}^{-1} = \boldsymbol{\tilde{q}}^{-1}\boldsymbol{\tilde{q}} = 1

またノルムが1（$|\boldsymbol{\tilde{q}}|=1$）のとき

\boldsymbol{\tilde{q}}^{-1} = \boldsymbol{\tilde{q}}^{*}

である．回転を扱うときはたいていノルムが1のを考えるのでしばしば混同してしまう．

クォータニオンの幾何的な意味（姿勢と回転）

（クリックで開く）

3次元空間上の位置ベクトルを表すクォータニオン

上記でもふれたが，クォータニオンのベクトル部に三次元空間上のベクトルをあてる．
すなわち，位置ベクトル$\boldsymbol{a}={}^t(a_x,a_y,a_z)$をクォータニオンで表そうとしたとき，スカラー部が0の純虚クォータニオンとして

\boldsymbol{\tilde{a}}={}^t(0,a_x,a_y,a_z) = 0 + ia_x +ja_y +ka_z

と表す．

3次元空間上の点（位置，方向）の回転を表すクォータニオンと姿勢を表すクォータニオン

複素平面上の点 $(a,b)$ を表す複素数は $z=a+ib$ で，これを $\theta$ だけ回転させて別の点$z'$に移動させるとき，回転を表す複素数 $\alpha = \cos\theta + i\sin\theta$ をかけることで実現できた．すなわち，

\begin{align}
z' &= z\alpha = \alpha z \\
&= (a\cos\theta - b\sin\theta) + i(b\cos\theta + a\sin\theta)
\end{align}

クォータニオンでも3次元空間上の点 $\boldsymbol{a}={}^t(a_x,a_y,a_z)$ をある軸 $\boldsymbol{n}={}^t(n_x,n_y,n_z), |\boldsymbol{n}| = 1$ まわりに $\theta$ だけ回転させて別の点 $\boldsymbol{a'}$ に移動させるとき，
回転を表わすクォータニオン（$\boldsymbol{\tilde{q}}$）を

\begin{align}
\boldsymbol{\tilde{q}} &= \cos\frac{\theta}{2} + \boldsymbol{n}\sin\frac{\theta}{2}\\
&= \cos\frac{\theta}{2} + \boldsymbol{\tilde{n}}\sin\frac{\theta}{2}\\
&= \cos\frac{\theta}{2} + in_x\sin\frac{\theta}{2} + jn_y\sin\frac{\theta}{2} + kn_z\sin\frac{\theta}{2}
\end{align}

と表わし，点 $\boldsymbol{a}$ を表わすクォータニオン $\boldsymbol{\tilde{a}}$ を以下の操作で $\boldsymbol{\tilde{a}'}$に移動させる．

\begin{align}
\boldsymbol{\tilde{a}'} = \boldsymbol{\tilde{q}} \boldsymbol{\tilde{a}} \boldsymbol{\tilde{q}^{*}}
\end{align}

これをさらに別の回転軸（$\boldsymbol{m}$）をもつクォータニオン$\boldsymbol{\tilde{p}}$で回転して別の点（$\boldsymbol{\tilde{a}''}$）にもっていくとすると，

\begin{align}
\boldsymbol{\tilde{a}''} 
&= \boldsymbol{\tilde{p} \tilde{a}' \tilde{p}^{*}}\\
&= \boldsymbol{\tilde{p}(\tilde{q} \tilde{a} \tilde{q}^{*})\tilde{p}^{*}}\\
&= \boldsymbol{(\tilde{p}\tilde{q}) \tilde{a} (\tilde{p}\tilde{q})^{*}}
\end{align}

したがって2つの回転を表わすクォータニオンの積（$\boldsymbol{\tilde{p}\tilde{q}}$）は2つの回転の合成を表している．
これはまた，初期位置$\boldsymbol{\tilde{a}}$に対して，$\boldsymbol{\tilde{a}''}$ が $\boldsymbol{\tilde{p}\tilde{q}}$ という状態にあることを表わしている．

このとき一本のベクトルだけの回転を考えている場合，クォータニオンは方向（の変化）の状態の情報を持っていることが分かる．
さらに，クォータニオンは任意のベクトルを同様に一つの軸周りに回転させる．すなわち，基底ベクトルの集合もすべて 同様に一つの軸周りに回転させられる．したがって，クォータニオンは姿勢（の変化）を表わす．

上の例で言うと，$\boldsymbol{\tilde{a}}$ を $\boldsymbol{\tilde{a}''}$ まで回転させるクォータニオン$\boldsymbol{\tilde{p}\tilde{q}}$ は初期の姿勢 $\boldsymbol{\tilde{q_0}}={}^t(1,\boldsymbol{0})=1$に対して $\boldsymbol{\tilde{p}\tilde{q}}$ という状態の姿勢を表わしている．
(スカラー部が1ベクトル部が\boldsymbol{0}のクォータニオンはハミルトン積における単位元で，姿勢を変化させない．)

ある物体，例えば各辺が1の立方体が初期の姿勢の時，各頂点が$(0,0,0),\ (1,0,0),\ (1,1,0,),\ (0,1,0),\ (0,1,1),\ (0,0,1),\ (1,0,1),\ (1,1,1)$ にあったとする．この時，$\boldsymbol{\tilde{q_0}}={}^t(1,\boldsymbol{0})$を各ベクトルにかけても各点の位置は変わらない（スカラー1をかけてるのと同じなので当たり前）．したがって初期姿勢を表す．これに対し任意のノルムが1のクォータニオン$\boldsymbol{\tilde{q}}$を姿勢の情報として持っていた時，このクォータニオンとその共役を各点の位置ベクトルの両側からかけると，各点は互いの相対的な位置関係を維持しながらある軸周りに回転する．すなわち，姿勢の情報として持っておくべきなのは片方のクォータニオンで十分であり，姿勢の変化の連続を記述するときはクォータニオンの積を連ねていけばよいが，実際にある点の位置ベクトルを計算するときは両側から掛けるという操作が必要．

回転を表すクォータニオンのしくみ

なぜ$\frac{\theta}{2}$なのか，なぜ両側から共役なもの同士でかけるのかという点についてはHMMNRSTさんの記事3が詳しい．
かじった程度で説明すると，クォータニオンは結局演算が特殊な4次元ベクトルであり，複素数と同様に一回の積は4次元空間の回転を表わしており，
上の回転を表わすクォータニオンの定義ではいらない軸方向への回転も入ってしまうため，
逆から共役クォータニオンをかけることでそのいらない方向への回転を打ち消す作業が必要で，
そのため2回かけちゃうからほしい方向にはちょうど2倍回転している
ということらしい．

あるクォータニオンに対する別のクォータニオンがあらわす姿勢

二つのクォータニオン $\boldsymbol{\tilde{q}_0, \tilde{q}_1}$ があり，姿勢 $\boldsymbol{\tilde{q}_0}$ に対する $\boldsymbol{\tilde{q}_1}$ の相対的な姿勢を $\boldsymbol{\mathit\Delta\tilde{q}}$ と表すとするとこれは

\boldsymbol{\mathit\Delta\tilde{q}}\boldsymbol{\tilde{q}_0} = \boldsymbol{\tilde{q}_1}\\

したがって相対的な姿勢は次のように表せる：

\boldsymbol{\mathit\Delta\tilde{q}} =  \boldsymbol{\tilde{q}_1(\tilde{q}_0^{-1})}

アフィン変換との関係

アフィン変換との関係

また，行列で表現すると以下のようになる：

\begin{align}
\boldsymbol{\tilde{q}} \boldsymbol{\tilde{a}} \boldsymbol{\tilde{q}^{*}}
&=
\begin{pmatrix}
q_w & -q_x & -q_y & -q_z\\
q_x &  q_w & -q_z &  q_y\\
q_y &  q_z &  q_w & -q_x\\
q_z & -q_y &  q_x &  q_w\\
\end{pmatrix}
\begin{pmatrix}
 q_w &  q_x &  q_y &  q_z\\
-q_x &  q_w & -q_z &  q_y\\
-q_y &  q_z &  q_w & -q_x\\
-q_z & -q_y &  q_x &  q_w\\
\end{pmatrix}
\begin{pmatrix}
0\\a_x\\a_y\\a_z
\end{pmatrix}\\
&=
\begin{pmatrix}
  1  &  0  & 0 &  0\\
  0  &  q_w^2 + q_x^2 - q_y^2 - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y)\\
  0  &  2(q_x q_y + q_w q_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_y q_z - q_w q_x)\\
  0  &  2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & q_w^2 - q_x - q_y + q_z\\
\end{pmatrix}
\begin{pmatrix}
0\\a_x\\a_y\\a_z
\end{pmatrix}\\
\end{align}
\\
\mathrm{where} \quad (q_w, q_x, q_y, q_z) = (\cos\frac{\theta}{2}, n_x\sin\frac{\theta}{2}, n_y\sin\frac{\theta}{2}, n_z\sin\frac{\theta}{2})

この表現行列は3次元ベクトルに関するアフィン変換の回転の部分のみの場合と一致する．
実際，表現行列をスカラー倍すれば自明に拡大縮小となるし，
回転を単位行列にして第1列の第2行から第4行までの要素に数字を入れ，クォータニオンベクトルの実部を1とすると平行移動になる：

\begin{align}
\begin{pmatrix}
  1   &  0  &  0  &  0\\
  b_x &  1  &  0  &  0\\
  b_y &  0  &  1  &  0\\
  b_z &  0  &  0  &  1\\
\end{pmatrix}
\begin{pmatrix}
\color{red}{1}\\a_x\\a_y\\a_z
\end{pmatrix}
=
\begin{pmatrix}
\color{red}{1} \\ a_x + b_x \\ a_y + b_y \\ a_z + b_z
\end{pmatrix}\\
\end{align}

3次元ベクトルをクォータニオンに拡張するときに，スカラー部が0の純虚クォータニオンにするのは，
変換行列でアフィン変換による平行移動を許さない状態にしていることになっているという解釈もできそう．

ただし，ベクトルの平行移動はベクトルの和を取ることであるが，3次元ベクトルをクォータニオンに拡張する際に
アフィン変換で平行移動の計算を許すようにベクトルのスカラー部も1になるようにすると，ノルムが3次元ベクトルの時と異なるし，二つのベクトル（を拡張したクォータニオン）の和で平行移動を計算しようとするたびにスカラー部が加算され，ノルムがどんどん大きくなる．
平行移動を表すクォータニオンの演算を，ベクトルを拡張したクォータニオン同士の和ではなく，スカラー部が1になるような演算を定義したり，あるいはもうベクトルクオータニオンのスカラー部を任意の整数$a$とし，表現行列の$b_i$を$b_i/a$)などとすれば表現行列との整合性は取れる．
が，前者は線形ではないのと，後者は計算スカラー部が大きいときに計算精度が悪くなる．
クォータニオンの和の演算と表現行列での計算を混ぜなければ問題はない

クオータニオン積による回転の表現と回転ベクトルに対するロドリゲスの回転公式との関係

回転ベクトルに対するロドリゲスの回転公式との関係

純虚クォータニオン同士のハミルトン積が内積と外積の和で表せられることに注目して，$\boldsymbol{\tilde{q}} = (q_w, \boldsymbol{q_v}) = (\cos\frac{\theta}{2}, \boldsymbol{n}\sin\frac{\theta}{2}) = (\cos\frac{\theta}{2}, n_x\sin\frac{\theta}{2}, n_y\sin\frac{\theta}{2}, n_z\sin\frac{\theta}{2}のとき$：

\begin{align}
\boldsymbol{\tilde{q}} \boldsymbol{\tilde{a}} \boldsymbol{\tilde{q}^{*}}
&= (q_w + \boldsymbol{q_v}) (0 + \boldsymbol{a}) (q_w - \boldsymbol{q_v})\\
&= q_w^2 \boldsymbol{a} 
- \boldsymbol{q_v \otimes a} \otimes \boldsymbol{q_v}
- q_w \boldsymbol{a \otimes q_v}
+ q_w \boldsymbol{q_v \otimes a}\\
&= q_w^2 \boldsymbol{a} 
- \boldsymbol{q_v} \otimes (-\boldsymbol{a \cdot q_v} + \boldsymbol{a} \times \boldsymbol{q})
- q_w (-\boldsymbol{a \cdot q_v} + \boldsymbol{a \times q_v})
+ q_w (-\boldsymbol{q_v \cdot a} + \boldsymbol{q_v \times a})\\
&= q_w^2 \boldsymbol{a} 
+ (\boldsymbol{a\cdot q_v}) \boldsymbol{q_v} 
- \boldsymbol{q_v} \otimes (\boldsymbol{a} \times \boldsymbol{q_v})
+ 2 q_w \boldsymbol{q_v} \times \boldsymbol{a}\\
&= q_w^2 \boldsymbol{a} 
+ (\boldsymbol{a} \cdot \boldsymbol{q_v}) \boldsymbol{q_v} 
+ \boldsymbol{q_v} \cdot (\boldsymbol{a} \times \boldsymbol{q_v}) 
- \boldsymbol{q_v} \times (\boldsymbol{a} \times \boldsymbol{q_v})
+ 2 q_w \boldsymbol{q_v} \times \boldsymbol{a}\\
&= \cos^2\frac{\theta}{2}\boldsymbol{a} 
+ (\boldsymbol{n \cdot a}\sin\frac{\theta}{2}) \boldsymbol{n}\sin\frac{\theta}{2}
+ 0 
- \boldsymbol{n}\sin\frac{\theta}{2} \times (\boldsymbol{a} \times \boldsymbol{n}\sin\frac{\theta}{2})
+ 2 \cos\frac{\theta}{2} \boldsymbol{n}\sin\frac{\theta}{2} \times \boldsymbol{a}\\
&= \cos^2\frac{\theta}{2}\boldsymbol{a} 
+ (\boldsymbol{n \cdot a}) \boldsymbol{n}\sin^2\frac{\theta}{2}
- \boldsymbol{n} \times (\boldsymbol{a \times n}) \sin^2\frac{\theta}{2}
+ \boldsymbol{n} \times \boldsymbol{a} \sin{\theta}\\

\end{align}

ここで，$\boldsymbol{n\times a}\perp \boldsymbol{n} \Leftrightarrow \boldsymbol{(n\times a)\cdot n} = 0$であることに注意．
また，$\boldsymbol{(n\times a)\times n}$は$\boldsymbol{n}$と$\boldsymbol{a}$とで張る平面上のベクトルだから$\boldsymbol{n}$と$\boldsymbol{a}$の線形和で表せることに注目して

\begin{align}
\boldsymbol{n} \times (\boldsymbol{a} \times \boldsymbol{n})
&= (\|\boldsymbol{n}\|^2\boldsymbol{a} - (\boldsymbol{n\cdot a})\boldsymbol{n})\\
&= (\boldsymbol{a} - (\boldsymbol{n\cdot a})\boldsymbol{n})  \quad (\because \|\boldsymbol{n}\| = 1) 
\end{align}
導出

$\boldsymbol{a}$の$\boldsymbol{n}$方向成分（$\boldsymbol{a_n}$）は

\boldsymbol{a_n}:=\frac{\boldsymbol{n\cdot a}}{\|\boldsymbol{n}\|}\boldsymbol{n}

であるから，$\boldsymbol{(n\times a)\times n}$成分（$\boldsymbol{a_m}$）は

\begin{align}
\boldsymbol{a_m} &:= \boldsymbol{a} - \frac{\boldsymbol{n}\cdot \boldsymbol{a}}{\|\boldsymbol{n}\|}\boldsymbol{n}\\
\|\boldsymbol{a_m}\| &= \sqrt{\|\boldsymbol{a}\|^2 - \frac{\|\boldsymbol{n\cdot a}\|^2}{\|\boldsymbol{n}\|^2}}
\end{align}

今，$\boldsymbol{n}$と$\boldsymbol{a}$とのなす角を$\phi$とすると，

\begin{align}
\|\boldsymbol{a_m}\| 
&= \sqrt{\|\boldsymbol{a}\|^2 - \frac{\boldsymbol{\|n\|^2\|a\|^2\cos^2\phi}}{\|\boldsymbol{n}\|^2} }\\
&=\|\boldsymbol{a}\|\sin\phi\\
\\
\| \boldsymbol{(n\times a)\times n} \| 
&= \boldsymbol{\| n \|^2 \| a \| \sin \phi}\\
&= \boldsymbol{\| a \|} \sin \phi \quad (\because \|\boldsymbol{n}\| = 1) 
\end{align}

$\boldsymbol{a_m}$は$\boldsymbol{(n\times a)\times n}$と同じ方向を向いているから，

\begin{align}
\boldsymbol{(n\times a)\times n} 
&= \frac{\|\boldsymbol{(n\times a)\times n}}{\|\boldsymbol{a_m}\|} \boldsymbol{a_m}\\
&= \frac{\|\boldsymbol{a}\|\sin\phi}{\|\boldsymbol{a}\|\sin\phi} \boldsymbol{a_m}\\
&= \boldsymbol{a} - \boldsymbol{(n\cdot a)n}\\
\end{align}
よって 
\begin{align}
\boldsymbol{\tilde{q}} \boldsymbol{\tilde{a}} \boldsymbol{\tilde{q}^{*}}
&=\boldsymbol{a} \cos^2\frac{\theta}{2}
+ (\boldsymbol{n \cdot a}) \boldsymbol{n}\sin^2\frac{\theta}{2}
- (\boldsymbol{a} - \boldsymbol{(n\cdot a)n}) \sin^2\frac{\theta}{2}
+ \boldsymbol{n} \times \boldsymbol{a} \sin{\theta}\\
&= \boldsymbol{a} \cos\theta
+ 2\boldsymbol{(n\cdot a)n} \sin^2\frac{\theta}{2}
+ \boldsymbol{n} \times \boldsymbol{a} \sin{\theta}\\
&= \boldsymbol{a} \cos\theta
+ \boldsymbol{(n\cdot a)n} (1-\cos\theta)
+ \boldsymbol{n} \times \boldsymbol{a} \sin{\theta}\\
\end{align}

これはロドリゲスの回転公式と呼ばれるものである．

あるベクトルをある軸となる単位ベクトル（$\boldsymbol{n}$）周りに $\theta$ 回転させる際の公式．幾何学的な考察に基づいたもの．幾何学的な見方はほかにたくさん記事があるのそちらを参照． 単位ベクトルと回転角の大きさの積は回転ベクトルと呼ばれるもので，回転ベクトルの時間微分が角速度ベクトルになっている． 角速度ベクトルはクォータニオンの微分に密接なかかわりがある．

クオータニオンの時間微分

前節の流れから，回転軸となる単位ベクトル $\boldsymbol{n} = {}^t(n_x, n_y, n_z)$ と回転角の大きさ $\theta$ の積として，回転ベクトル $\boldsymbol{\theta} = \theta\boldsymbol{n} = {}^t(\theta n_x, \theta n_y, \theta n_z)$があるとき，
角速度ベクトル $\boldsymbol{\omega}$ は

\boldsymbol{\omega} = \frac{d}{dt}\theta\boldsymbol{n} = \frac{d\theta}{dt}\boldsymbol{n} = \omega\boldsymbol{n}

である．
今姿勢が経時的に徐々に変わっているとしたとき，現在の姿勢がクォータニオン$\boldsymbol{\tilde{q}}(t)$，$\mathit\Delta t$秒後の姿勢が$\boldsymbol{\tilde{q}}(t+\mathit\Delta t)$として表せられるとき，クォータニオンの性質から

\begin{align}
\boldsymbol{\tilde{q}}(t+\mathit\Delta t) &= \boldsymbol{\tilde{q}}(t) \boldsymbol{\tilde{q}}(\mathit\Delta t)\\
\end{align}

$\mathit\Delta t$ が十分小さいとき，$\boldsymbol{\tilde{q}}(\mathit\Delta t)$ について1次の項までマクローリン展開すると

\begin{align}
\boldsymbol{\tilde{q}}(\mathit\Delta t) 
&= \cos\frac{\mathit\Delta\theta}{2} + \boldsymbol{n}\sin\frac{\mathit\Delta\theta}{2}\\
&= 1+\boldsymbol{n}\frac{1}{2}\theta
\end{align}

\begin{align}
\therefore \boldsymbol{\tilde{q}}(t+\mathit\Delta t) 
&= \boldsymbol{\tilde{q}}(t) (1+\boldsymbol{n}\frac{1}{2}\mathit\Delta\theta)\\
&=\boldsymbol{\tilde{q}}(t) + \boldsymbol{\tilde{q}}(t)\boldsymbol{n}\frac{1}{2}\mathit\Delta\theta\\
\end{align}

\Leftrightarrow \frac{\boldsymbol{\tilde{q}}(t+\mathit\Delta t) - \boldsymbol{\tilde{q}}(t)}{\mathit{\Delta}t}
= \boldsymbol{\tilde{q}}(t)\boldsymbol{n}\frac{1}{2}\frac{\mathit\Delta\theta}{\mathit\Delta t}

したがって$\mathit\Delta t\rightarrow 0$ のとき

\frac{d\boldsymbol{\tilde{q}}(t)}{dt} 
= \frac{1}{2}\boldsymbol{\tilde{q}}(t)\omega\boldsymbol{n} 
= \frac{1}{2}\boldsymbol{\tilde{q}}(t)\boldsymbol{\omega}

Reference

  1. Srivatsan, R. A., Rosen, G. T., Mohamed, D. F. N., & Choset, H. (2016). Estimating SE (3) elements using a dual quaternion based linear Kalman filter. In Robotics: Science and systems. ↩

  2. wikipediaの記事:四元数 ↩

  3. @ HMMNRST 四元数を用いた三次元回転計算の幾何学的意味 ↩ ↩2

  4. 物理のかぎしっぽの記事：四元数 ↩

  5. 木村真琴 (Makoto KIMURA)先生の2019 年 7 月 27 日茨城大学オープンキャンパス模擬授業：複素数と四元数 ↩

0
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
3