こんにちは。だいCです。
初投稿です。投稿を、自主勉のノート代わりに使わせてもらいます。
その一発目は、
姿勢の回転をどう表現するか
をノートに取りたいと思います。
1. 目的
姿勢の回転表現である回転行列と角軸の関係を示す。
2. 準備
以下では断りがないかぎり、行列は 3×3、ベクトルは3×1とします。
2.1. 「変数」「値」
以降、「変数」という言葉は、以下をまとめて表現した言葉とします。
- 単純に数値を文字で置き代えた値：代入値、入力(代入値)と出力(返り値)が同じ。
- 関数(作用素、オペレータ)：入力(代入値)から何かしらの計算をして出力(返り値)とする。
「変数\( f \)は～」と説明したあと「\( f(x) \)」と書いていたら、
「\(f \)は入力\(x \)をとる関数(作用素、オペレータ)」と解釈してください。
また「値」という言葉も、以下のバリエーションを含んでいるので、注意してください。
- 一つの数値(例. 0。 1。 1.5など)
- 配列。羅列しただけの数値列(例. [33.3, 1.2, 0.01]など)
- 構造体、リスト。配列と同じようにただの数値の羅列。リストは名前＆値のペア。リストの中のリストという感じで入れ子にもなる。
- ベクトル（ベクトルの公理が成り立つ。座標表現できる）
- 行列（行列の定義が成り立つ。「ベクトルにかける」という操作をすると作用素となる）
以降、変数を太字で表現するとき、その値は上記の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 \)は、上の行列を２回かけあわせた行列です。
$$ \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としています。
大きさが１でない\( \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\)の大きさを１としたことも考慮して(\( 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} ) \)としたのでした。
ふう、、
表現をするだけでめちゃくちゃ長くなりました。
以上。