剛体の運動(回転行列と角軸)

こんにちは。だいCです。

初投稿です。投稿を、自主勉のノート代わりに使わせてもらいます。

その一発目は、

 

姿勢の回転をどう表現するか

 

をノートに取りたいと思います。

 

1. 目的

姿勢の回転表現である回転行列と角軸の関係を示す。

 

2. 準備

以下では断りがないかぎり、行列は 3×3、ベクトルは3×1とします。

2.1. 「変数」「値」

以降、「変数」という言葉は、以下をまとめて表現した言葉とします。

  • 単純に数値を文字で置き代えた値:代入値、入力(代入値)と出力(返り値)が同じ。
  • 関数(作用素、オペレータ):入力(代入値)から何かしらの計算をして出力(返り値)とする。

「変数\( f \)は~」と説明したあと「\( f(x) \)」と書いていたら、

「\(f \)は入力\(x \)をとる関数(作用素、オペレータ)」と解釈してください。

 

また「」という言葉も、以下のバリエーションを含んでいるので、注意してください。

  1. 一つの数値(例. 0。 1。 1.5など)
  2. 配列。羅列しただけの数値列(例. [33.3, 1.2, 0.01]など)
  3. 構造体、リスト。配列と同じようにただの数値の羅列。リストは名前&値のペア。リストの中のリストという感じで入れ子にもなる。
  4. ベクトル(ベクトルの公理が成り立つ。座標表現できる)
  5. 行列(行列の定義が成り立つ。「ベクトルにかける」という操作をすると作用素となる)

以降、変数を太字で表現するとき、その値は上記の1.以外であることを示しています。

 

「それ違う」

と言われそうですが、すみません、不勉強なものでよくわかっておりません。

我流でごめんなさいの意味も含めて「自分はこういう意味で使っております。」ということを、一応先に断っておきました。

 

3. 姿勢の角軸表現

姿勢を変数\( \bf a \)とし、姿勢の表現方法は角軸表現とします。

(表現方法には他にもオイラー角表現、ロールピッチヨー表現などありますが)。

角軸表現では、姿勢\( \bf a \)は、回転軸ベクトル\( \bf n \)と回転角\( \theta \)により表現される変数です。

数式チックに書くと\( {\bf a} = {\bf a}({\bf n}, \theta) \)です。

太字で書きましたが、角軸\(\bf a \)はベクトルではありません。配列です。

さらに、この回転軸ベクトル\( \bf n \)と回転角\( \theta \)の値は、入力値に時刻\( t \)を取る関数でもあります(軌道計算)。

 

さて、変数\( \bf R \)を姿勢の回転変換を行うための変数とします。

この変換の入力-出力は以下とします。

  • 入力:変換前の姿勢\( \bf a_{\rm in} \)、回転姿勢(角軸表現で回転軸+回転角)\(\bf a \)
  • 出力:変換後の姿勢\( \bf a_{\rm out} \)

内部での変換計算は行列計算、クォータニオン計算どちらでもOKですが、

ここでは行列計算で説明します。

以降、変数\( \bf R \)は、回転変換に用いられる「回転行列」という言葉で称します。

 

入出力をみると分かるように、回転変換は角軸\( \bf a\)で構成される変数です。

この変換計算を\( \bf atoR \)で表し、回転行列\( \bf R \)を角軸\( \bf a\)から構成する作用の式で表現すると、\( {\bf R} := {\bf atoR}( {\bf a} ) \)です。

逆に変数\( \bf R \)から角軸\( \bf a\)を求める変換計算を\( \bf Rtoa \)と新たに置くと、角軸\( \bf a \)を回転行列\( \bf R \)から構成する作用の式は、\( {\bf a} := {\bf Rtoa}( {\bf R} ) \)です。

決して\( {\bf R} = {\bf R}( {\bf a} ) \)ではないので注意してください。(もし書くとしたら\( {\bf R} = {\bf R}( {\bf n}, \theta ) \))

 

3.1. 角軸\( \bf a \)→回転行列\( \bf R \)

角軸姿勢\({\bf a}({\bf n}, \theta)\)と回転行列\( \bf R \)との間では、等価角軸表現すると、以下の関係が成り立ちます(導出はいつか書きます)。

$$ {\bf R} := {\bf atoR}( {\bf a} ) = {\bf I} + {\rm sin}\theta {\bf[ n\times]} + (1 – {\rm cos}\theta) {\bf[ n\times]}^2 $$

これをロドリゲスの公式というそうです。

ここで、\(\bf I\)は単位行列です。\( {\bf[ n\times]} \)は下記に述べる行列です。

回転軸ベクトル\( \bf n \)を座標値で以下のように表し、

$$ {\bf n} := [ n_{x}, n_{y}, n_{z} ]^{\rm T} $$

行列\( {\bf[ n\times]} \)は、以下のように表します。

$${\bf[ n\times]} := \left[ \begin{array}{ccc}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{array} \right]$$

この行列は外積計算を行列で表現した形になるため、外積行列というそうです。外積行列は交代行列(歪対称行列)にあたります。

外積なので、その性質から以下の式が成り立ちます。

$${\bf[ n\times]} {\bf n} =  {\bf n} \times {\bf n} = {\bf 0}$$

 

\( {\bf[ n\times]}^2 \)は、上の行列を2回かけあわせた行列です。

$$ \begin{array}{rcl}
{\bf[ n\times]}^2 &=& \left[ \begin{array}{ccc}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{array} \right]
\left[ \begin{array}{ccc}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{array} \right] \\
&=&
\left[ \begin{array}{ccc}
-n_{y}^2-n_{z}^2 & n_{x}n_{y}           & n_{z}n_{x} \\
n_{x}n_{y}           & -n_{z}^2-n_{x}^2 & n_{y}n_{z} \\
n_{z}n_{x}           & n_{y}n_{z}           & -n_{x}^2-n_{y}^2
\end{array} \right]
\end{array} $$

 

3.2. 回転行列\( \bf R \)→角軸\( \bf a \)

逆に、\( {\bf R} \)から角軸\(\bf a \)を導出します。

まず、回転変換\( {\bf atoR} \)は、先に述べた通り、「ある入力姿勢を回転変換して出力姿勢にする」計算式です。

ここで、以下の仮定を考えます。

ある回転行列\( {\bf R} \)による回転変換において、

「もし、入力の角軸\(\bf a_{\rm in} \)の回転角\( \theta_{\rm in} \)がいくらであっても、

入力の角軸\(\bf a_{\rm in} \)の回転軸ベクトル\( \bf n_{\rm in} \)に対して、

出力の角軸\(\bf a_{\rm out} \)の回転軸ベクトル\( \bf n_{\rm out} \)が変化しない(入力の回転軸と出力の回転軸が常に同じ)」

となる回転軸ベクトルを\({\bf n}_{\rm id} (= {\bf a}_{\rm in} = {\bf a}_{\rm out}) \)とします。

(\({\bf n}_{\rm id} \)は、\( {\bf a} \)の\( {\bf n} \)と、\( {\bf atoR}( {\bf a}) \)の\( {\bf n} \)が同じとなるベクトル)

回転行列\( \bf R \)と回転軸ベクトル\( {\bf n}_{\rm id} \)の関係は、以下の式のようになります。

$$ {\bf R}{\bf n}_{\rm id} = {\bf n}_{\rm id} $$

\( {\bf R}{\bf n}_{\rm id} = {\bf n}_{\rm id} \)が成り立つ場合を以下の条件(i), (ii)で考えます。

(i) \( {\bf R} \neq {\bf I} \)のとき

\( {\bf R} \neq {\bf I} \)のとき、すなわち回転行列が単位行列でない場合を考えます。

回転行列の性質(直交行列の性質)\( {\bf R}^{-1} = {\bf R}^{\rm T}\)より、

$$\begin{array}{rcl}
{\bf R}{\bf n}_{\rm id} &=& {\bf n}_{\rm id} \\
{\bf n}_{\rm id} &=& {\bf R}^{-1}{\bf n}_{\rm id} \\
&=& {\bf R}^{\rm T}{\bf n}_{\rm id}
\end{array}$$

となります。さらに右辺を\( \bf 0 \)の式に直すと以下のようになります。

$$\begin{array}{rcl} {\bf R}{\bf n} = {\bf n}_{\rm id} = {\bf R}^{\rm T}{\bf n}_{\rm id} &\ & \\
\Rightarrow ({\bf R} – {\bf R}^{\rm T}){\bf n}_{\rm id} &=& {\bf 0} \end{array}$$

ここで、回転行列\( \bf R \)を以下のような要素(\( r_{11} \)~\( r_{33} \))で表すことにします。

$$ {\bf R} = \left[ \begin{array}{ccc}
r_{11} & r_{12} & r_{13} \\
r_{21} & r_{22} & r_{23} \\
r_{31} & r_{32} & r_{33}
\end{array} \right]$$

その転置は、以下になります。

$$ {\bf R}^{\rm T} = \left[ \begin{array}{ccc}
r_{11} & r_{21} & r_{31} \\
r_{12} & r_{22} & r_{32} \\
r_{13} & r_{23} & r_{33}
\end{array} \right]$$

これらより、先程の式は以下のように表せます。

$$ \begin{array}{rcl}
( {\bf R} – {\bf R}^{\rm T} ){\bf n}_{\rm id} &=& {\bf 0} \\
\left[ \begin{array}{ccc}
0                     & -(r_{21}  – r_{21}) & r_{13}  – r_{31} \\
r_{21}  – r_{12} &  0                         & -(r_{32}  – r_{23}) \\
-(r_{13}  – r_{31}) &  r_{32}  – r_{23}   & 0
\end{array} \right] {\bf n}_{\rm id} &=& {\bf 0}
\end{array} $$

外積行列の関係からじーっと考えて、例えば以下の様な\( {\bf n}_{\rm id} \)を求めることができます。

(一通りではないです)

$${\bf n}_{\rm id} = \left[ \begin{array}{c}
r_{32}  – r_{23}\\
r_{13}  – r_{31}\\
r_{21}  – r_{12}
\end{array} \right]  $$

(ii) \( {\bf R} = {\bf I} \)のとき

\( {\bf R} = {\bf I} \)のとき、すなわち回転行列が単位行列である場合を考えます。

\( {\bf R} = {\bf I} \)より、\( {\bf R}{\bf n}_{\rm id} = {\bf n}_{\rm id} \)が成り立つとき、

回転軸ベクトル\( \bf n_{\rm id} \)は任意の値になります。

このとき、ロドリゲスの公式より、回転角\( \theta = 0\)です。

 

以上の(i),(ii)より、回転軸ベクトル\( \bf n\)は方向さえ情報として持っていればいいので、大きさを1としています。

大きさが1でない\( \bf n_{\rm id} \)により、\( \bf n \)を以下のように計算して得られます(回転軸ベクトル\({\bf n}_{\rm id}\)の大きさが1であっても以下のように計算しときます。)

$${\bf n}  = \frac{ {\bf n}_{\rm id} }{ \| {\bf n}_{\rm id} \| }$$

 

最後に、回転行列\( \bf R \)から角軸\( \bf a \)の回転角\( \theta \)を求めます。

まずロドリゲスの公式から、\( {\rm cos}\theta \)、\( {\rm sin}\theta \)を求めます。

$$  \begin{array}{rcl}
{\bf R} &:=& {\bf atoR}( {\bf a } ) \\
&=& {\bf I} + {\rm sin}\theta {\bf[ n\times]} + (1 – {\rm cos}\theta) {\bf[ n\times]}^2 \\
&=& \left[ \begin{array}{ccc}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & 1
\end{array} \right]
+
{\rm sin}\theta
\left[ \begin{array}{ccc}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{array} \right]
+
(1 – {\rm cos}\theta )
\left[ \begin{array}{ccc}
-n_{y}^2-n_{z}^2 & n_{x}n_{y}           & n_{z}n_{x} \\
n_{x}n_{y}           & -n_{z}^2-n_{x}^2 & n_{y}n_{z} \\
n_{z}n_{x}           & n_{y}n_{z}           & -n_{x}^2-n_{y}^2
\end{array} \right] \\
&=&
\left[ \begin{array}{ccc}
1 – (1-{\rm cos}\theta)(n_{y}^2+n_{z}^2)                     & -{\rm sin}\theta n_{z} + (1-{\rm cos}\theta)n_{x}n_{y} & {\rm sin}\theta n_{y} + (1-{\rm cos}\theta)n_{z}n_{x} \\
{\rm sin}\theta n_{z} + (1-{\rm cos}\theta)n_{x}n_{y} & 1 – (1-{\rm cos}\theta)(n_{z}^2+n_{x}^2)                    & {\rm sin}\theta n_{x} + (1-{\rm cos}\theta)n_{y}n_{z} \\
-{\rm sin}\theta n_{y} + (1-{\rm cos}\theta)n_{z}n_{x} & {\rm sin}\theta n_{x} + (1-{\rm cos}\theta)n_{y}n_{z}   & 1 – (1-{\rm cos}\theta)(n_{x}^2+n_{y}^2)
\end{array} \right] \\
&=&
\left[ \begin{array}{ccc}
r_{11} & r_{12} & r_{13} \\
r_{21} & r_{22} & r_{23} \\
r_{31} & r_{32} & r_{33}
\end{array} \right]
\end{array}  $$

以上の式より、回転軸ベクトルを\(\bf n\)の大きさを1としたことも考慮して(\( n_{x}^2 + n_{y}^2 + n_{z}^2 = 1\))、\( {\rm cos}\theta \)が得られます。

$$ \begin{array}{rcl}
r_{11} +  r_{22} + r_{33} &=& \{ 1 – (1-{\rm cos}\theta)(n_{y}^2+n_{z}^2) \}
+ \{ 1 – (1-{\rm cos}\theta)(n_{z}^2+n_{x}^2) \}
+ \{ 1 – (1-{\rm cos}\theta)(n_{x}^2+n_{y}^2) \} \\
&=& 3 – 2(1-{\rm cos}\theta)(n_{x}^2 + n_{y}^2 + n_{z}^2) \\
&=& 1 + 2{\rm cos}\theta \\
&\ & \Rightarrow {\rm cos}\theta = \frac{r_{11} + r_{22} + r_{33}-1}{2}
\end{array}
$$

さらに\( {\bf n}_{\rm id} \)の要素を見てみて、

$$\begin{array}{rcl}
{\bf n}_{\rm id} &=& \left[ \begin{array}{c}
r_{32}  – r_{23}\\
r_{13}  – r_{31}\\
r_{21}  – r_{12}
\end{array} \right]
= \left[ \begin{array}{c}
2{\rm sin}\theta {n}_{x} \\
2{\rm sin}\theta {n}_{y} \\
2{\rm sin}\theta {n}_{z}
\end{array} \right] \\
\end{array} $$

\({\bf n}\)と\({\bf n}_{\rm id}\)の関係も考えると、\( {\rm sin}\theta \)が得られます。

$$\begin{array}{rcl}
{\bf n}_{\rm id} &=& 2{\rm sin}\theta{\bf n} \\
&=&  2{\rm sin}\theta \frac{ {\bf n}_{\rm id} }{\| {\bf n}_{\rm id} \|} \\
&\ & \Rightarrow  {\rm sin}\theta = \frac{1}{2} \| {\bf n}_{\rm id} \|
\end{array} $$

以上により求まった\( {\rm cos}\theta \)、\( {\rm sin}\theta \)より、アークタンジェント(c標準のmathライブラリだと\(\rm atan2\))によって回転角\(\theta \)が求まります。

$$ \theta = { \rm atan2 } \left( \frac{1}{2} \| {\bf n}_{\rm id} \|, \frac{r_{11} + r_{22} + r_{33}-1}{2} \right) $$

 

はい。

以上で、角軸\( \bf a \)を構成する回転軸ベクトル\( \bf n\)と回転角\(\theta \)が求まり、回転行列\( \bf R \)より角軸\( \bf a \)に変換することができました。

これらの計算をにまとめて、\( {\bf a} := {\bf Rtoa}( {\bf R} ) \)としたのでした。

 

ふう、、

表現をするだけでめちゃくちゃ長くなりました。

 

以上。

コメントを残す

メールアドレスが公開されることはありません。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください