15
12

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

クォータニオンの時間微分と角速度の関係の証明

Last updated at Posted at 2020-03-23

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

は次の式で計算されます

\dot{ \vec{q} }  =\frac{1}{2} \vec{q} \times \vec{\omega_{v}}

と計算されます。
$\vec{q} $はクオータニオンで
$\times$はクオータニオンの積です。
$\vec{\omega} $は角速度です。この後、こんがらがるのですが、次のようにクォータニオン形式にしたものを$\vec{\omega _{q}}$とします
角速度の方向単位ベクトルを$\vec{\lambda}$(表現座標系は、$q$での回転先座標系です)とし、角速度の大きさを$\omega$とします

\begin{eqnarray}
\vec{\omega_{q}}= \left(
    \begin{array}{ccc}
      cos(\omega/2)\\
\lambda_{x}sin(\omega/2)\\
\lambda_{y}sin(\omega/2)\\
\lambda_{z}sin(\omega/2)\\
    \end{array}
  \right)

\end{eqnarray}

角速度の意味自体がクオータニオンと同じで、回転軸と回転量(回転量速度)という2つで回転を表現しているため、簡単に変換できます

$\vec{\omega} $をただのベクトルとみて、第一成分0で、次のようにクォータニオン形式にしたものを$\vec{\omega _{v}}$とします


\vec{\omega_{v}}= \left(
    \begin{array}{ccc}
      0\\
\omega_{x}\\
\omega_{y}\\
\omega_{z}\\
   \end{array}
  \right) =\left(
    \begin{array}{ccc}
      0\\
\lambda_{x} \omega\\
\lambda_{y} \omega\\
\lambda_{z} \omega\\
   \end{array}
  \right)

時間微分を計算するときはこっちの形式で計算します。

証明

現在時刻$t$でのフレームAからフレームBへの姿勢(クオータニオン)を$\vec{q} (t) $とし、角速度$\vec{\omega} $で回転しているとします。
時刻$t+\Delta t$でのフレームAからフレームBへの姿勢(クオータニオン)を$\vec{q} (t+ \Delta t )$ とします。

フレームAのでの点$P_{A}$を考えます(下付は表現に使う座標系)。フレームAからフレームBへの姿勢変化したとき$P_{B}$はどうなるかを考えると(点Pは移動しないとします、観測者が転移します)

P_{B} = \vec{q}^{T} \times P_{A} \times \vec{q}

となります。これは、クォータニオンの基礎なので説明しないです。今後に期待してね

時刻$t+\Delta t$での点$P_{B}$は、さらに(角速度*Δt)クオータニオン$\vec{\omega_{q} \Delta t}$で回転される事になるので

\begin{eqnarray}
P_{B} &=& \vec{\omega_{q} \Delta t}^{T} \vec{q}^{T} \times P_{A} \times \vec{q} \times \vec{\omega_{q} \Delta t}
\\
\vec{\omega_{q} \Delta t}&=& \left(
    \begin{array}{ccc}
      cos(\omega \Delta t  /2)\\
\lambda_{x}sin(\omega \Delta t  /2)\\
\lambda_{y}sin(\omega \Delta t  /2)\\
\lambda_{z}sin(\omega \Delta t  /2)\\
    \end{array}
  \right)
\end{eqnarray}

このようになります。
よって、姿勢クォータニオンは

 $ \vec{q} $ から $ \vec{q} \times \vec{\omega_{q} \Delta t}$ へ
時刻 $\Delta t$の間に変化したといえます。
ここでの、角速度はクォータニオンタイプ(ベクトルタイプではない)という事に注意

ここまでわかれば、ほぼ終了です。

\begin{align}
\dot{ \vec{q} }  &= \frac{ \vec{q}(t+\Delta t) -  \vec{q}(t) }{\Delta t}\\
&=\frac{ \vec{q} \times \vec{\omega_{q}\Delta t}-  \vec{q}(t) }{\Delta t}\\
&= \frac{1}{\Delta t} \vec{q} \times (\vec{\omega_{q} \Delta t} - 
\left(
    \begin{array}{ccc}
     1\\
0\\
0\\
0\\
    \end{array}
  \right)
)
\end{align}

$\left(
\begin{array}{ccc}
1\
0\
0\
0\
\end{array}
\right)$はクォータニオンの単位元です(掛けても変わらない)

\begin{align}
\frac{1}{\Delta t}  ( \vec{\omega_{q} \Delta t} - 
\left(
    \begin{array}{ccc}
     1\\
0\\
0\\
0\\
    \end{array}
  \right)
)
&= \frac{1}{\Delta t}  \left(
    \begin{array}{ccc}
     cos(\omega \Delta t  /2) -1\\
\lambda_{x}sin(\omega \Delta t  /2)\\
\lambda_{y}sin(\omega \Delta t  /2)\\
\lambda_{z}sin(\omega \Delta t  /2)\\
    \end{array}
  \right)\\
&= \lim_{\Delta t->0} \frac{1}{\Delta t}  \left(
    \begin{array}{ccc}
     cos(\omega \Delta t  /2) -1\\
\lambda_{x}sin(\omega \Delta t  /2)\\
\lambda_{y}sin(\omega \Delta t  /2)\\
\lambda_{z}sin(\omega \Delta t  /2)\\
    \end{array}
  \right)\\
&=\left(
    \begin{array}{ccc}
     1 -1\\
\lambda_{x}\omega  /2  \\
\lambda_{y}\omega  /2\\
\lambda_{z}\omega   /2\\
    \end{array}
  \right)\\
&=\frac{1}{2} \left(
    \begin{array}{ccc}
     0\\
\lambda_{x}\omega   \\
\lambda_{y}\omega \\
\lambda_{z}\omega  \\
    \end{array}
  \right)\\
&=\frac{1}{2} \vec{\omega_{v}}

\end{align}

変動部分は結局、角速度をベクトル表示にした$\vec{\omega_{v}}$になるのでした。
証明終了

追記:λの座標系

角速度ベクトルの表現座標系をクォータニオン$q$で回転した先の座標系にするか、$q$で回転する前の座標系にするかで悩むかもしれません。

$\vec{\omega _{q}}$は座標系によりません。

$\vec{ \omega_{v}}$は$q$の$\lambda_{x,y,z}$を使って表現しています。
よって、$q$と同じ座標系で角速度ベクトルを表現すればいいです。
回転先、回転前で座標系は動きますが$\Delta t$と微小なので、どちらの座標系でも関係ありません。

15
12
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
15
12

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?