こんにちは。だい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.以外であることを示しています。
2.2. 等価角軸変換
前回のおさらいですが、回転行列\( \bf R \)と角軸\( {\bf a}( {\bf n}, \theta) \) ({\bf n}は回転軸ベクトル、\( \theta \)は回転角)の変換は以下のようになります。
2.2.1. 角軸\( \bf a \)→回転行列\( \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[ n\times] \)は\( {\bf n} \)による外積行列 ）
2.2.2. 回転行列\( \bf R \)→角軸\( \bf a \)
回転行列\( \bf R \)の要素を以下のように表します。
$$ {\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 a \)を構成する回転軸ベクトル\( \bf n\)(大きさ１)は、\( {\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] $$
$${\bf n} = \frac{ {\bf n}_{\rm id} }{ \| {\bf n}_{\rm id} \| }$$
角軸\( \bf a \)を構成する回転角\( \theta\)は、同じく上記のベクトル\( {\bf n}_{\rm id} \)を用いて以下のように得られます。
$$ \theta = { \rm atan2 } \left( \frac{1}{2} \| {\bf n}_{\rm id} \|, \frac{r_{11} + r_{22} + r_{33}-1}{2} \right) $$
以上をまとめて、
$$ {\bf a} := {\bf Rtoa}( {\bf R} ) $$
とします。
3. 座標系と回転軸と角速度ベクトル
速度を考えるときにややこしいのは、回転軸が移動することです。
今注目する剛体が、何か別の乗り物(剛体)に乗っかっている状態を想像します。
その乗り物が、慣性座標系(静止している座標系、基準座標系)から見て、並進・回転移動しているとします。
そして、注目している剛体も、その乗り物の上で回転しているとします。
このとき「慣性座標系(基準座標系)から見ると、注目する剛体の回転が、一体どう表現されるか考えよう」という話です。
乗り物が移動しながら剛体が回転していると、その回転軸ベクトルも移動しており、慣性座標系(基準座標系)から見た回転軸ベクトルの座標値が、乗り物の移動中変化し続けてしまうことを理解しておかないといけません。
吉川恒夫先生の教科書「ロボット制御基礎論」、有本卓先生の教科書「ロボットの力学と制御」では、
「回転軸ベクトルの座標系を、移動している剛体と一緒に動き、かつ原点を慣性座標系(基準座標系)と一致するようにとって、回転運動を考える」という風にしています（…してますよね？間違ってたらゴメンナサイ…）。
この座標系を、回転しているので回転座標系と称します。すると、
「回転軸は、慣性座標系(基準座標系)から見た、回転座標系の回転軸ベクトルと一致している」
と解釈できます。
要は、移動している剛体の運動について、常に回転座標系の原点を慣性座標系(基準座標系)の原点と一致するようにとることで、乗り物の移動とかすっとばして剛体自身の回転のみで考えられるようにしているのです。
では、
慣性座標系からみた、回転座標系の回転を表す回転行列\(\bf R\)、角軸\( {\bf a} \)を使って、回転速度を表現してみます(回転軸ベクトルの座標値は慣性座標系基準になります)。
ここからは、両変数とも時間\( t \)の関数になります。
回転行列は\({\bf R}(t)\)と表します。
角軸\( {\bf a}(t) \)を構成する回転軸ベクトル\( {\bf n} \)は、原点が固定された回転座標系の回転軸を表しているので時間が変化しても一定です。
角軸\( {\bf a}(t) \)を構成する回転角は\( \theta(t) \)と表し、時間の関数です。
回転する前の時刻を\( t \)、瞬時移動時間を\(\Delta t \)とすると、移動後の時刻\( t + \Delta t \)の回転行列\( {\bf R} ( t + \Delta t ) \)は、ロドリゲスの公式より以下の式で表せます。
$$ \begin{array}{rcl}
{\bf R}(t + \Delta t) &=& {\bf R}(\Delta t) {\bf R}(t)\\
&=& ( {\bf I} + {\rm sin}\Delta \theta [{\bf n}\times]
+ (1 – {\rm cos}\Delta \theta)[{\bf n}\times]^2 ) {\bf R}(t)
\end{array} $$
この式は、\( \Delta t \)間に\( \bf n \)まわりに\( \Delta \theta \)回転した後の回転行列を表します。
速度の定義より、回転行列の速度\( \dot {\bf R}(t) \)が以下の式で表されます。
$$ \begin{array}{rcl}
\dot {\bf R}(t) &=& { \displaystyle \lim_{\Delta t \to 0} } \frac{ {\bf R}(t + \Delta t) – {\bf R}(t) }{ \Delta t } \\
&=& { \displaystyle \lim_{\Delta t \to 0} } \left(
\frac{ {\rm sin}\Delta \theta }{ \Delta t } [{\bf n}\times]
+ \frac{ (1 – {\rm cos}\Delta \theta) }{ \Delta t } [{\bf n}\times]^2
\right) {\bf R}(t)
\end{array} $$
ここで、
$$
{ \displaystyle \lim_{\Delta t \to 0} } \Delta \theta = 0,
$$
$$ \begin{array}{rcl}
&& { \displaystyle \lim_{\Delta t \to 0} } \frac{ {\rm sin}\Delta \theta }{ \Delta t }
= { \displaystyle \lim_{\Delta t \to 0} } \frac{ {\rm sin}\Delta \theta }{ \Delta \theta} \frac{ \Delta \theta }{ \Delta t }
= \dot \theta(t), \\
&& \left (
{ \displaystyle \lim_{\Delta t \to 0} } \frac{ {\rm sin}\Delta \theta }{ \Delta \theta} =
{ \displaystyle \lim_{\Delta \theta \to 0} } \frac{ {\rm sin}\Delta \theta }{ \Delta \theta} = 1
\right )
\end{array} $$
$$
{ \displaystyle \lim_{\Delta t \to 0} } \frac{ ( 1 – { \rm cos } \Delta \theta ) }{ \Delta t } =
{ \displaystyle \lim_{\Delta t \to 0} } \frac{ {\rm sin}^2 \frac{ \Delta \theta }{2} }{ \left(\frac{\Delta \theta}{2}\right)^2 }
\cdot \frac{ \Delta \theta }{ \Delta t }
\cdot 2\Delta \theta
= 0,
$$
$$ \left( \begin{array}{rcl}
&& { \displaystyle \lim_{\Delta t \to 0} } \frac{ ( {\rm sin}^2 \frac{\Delta \theta}{2} ) }{ \frac{\Delta \theta}{2} }
= 1, \ \\
&& { \displaystyle \lim_{\Delta t \to 0} } \frac{ \Delta \theta }{ \Delta t }
= \dot \theta(t), \ \\
&& { \displaystyle \lim_{\Delta t \to 0} } 2\Delta \theta
= 0 \
\end{array} \right) $$
となるので、結局回転行列の速度\( \dot {\bf R}(t) \)は、以下のようになります。
$$ \begin{array}{rcl}
\dot {\bf R}(t) &=& \dot \theta(t) [{\bf n} \times] {\bf R}(t) \\
&:=& [{\bf \omega} \times](t) {\bf R}(t)
\end{array} $$
ただし、\( {\bf \omega}(t) := {\dot \theta}(t) {\bf n} \)とし、この\( {\bf \omega}(t) \)を角速度ベクトルというそうです。
4. 角速度ベクトル\( {\bf \omega} \)の性質
先程の述べたように、
\( {\bf \omega}(t) := {\dot \theta}(t) {\bf n} \)の回転軸ベクトル\( {\bf n} \)は、
時間によらず一定、つまり\( {\bf \omega}(t) \)は\( {\bf n} \)の方向変化を含みません。
これは何を意味するかを考えます。
実際に剛体を回転させるとき回転軸をいくつか方向の方向に分けて、順番にある回転角ずつ回転させたとします。
たとえば、
時刻0.0[s]～0.5[s]の間はz軸まわりに90°、
時刻0.5[s]～1.0[s]の間はy軸まわりに90°に回転させとします。
この回転変化を回転(A)とします。
同じように、ただし今度は回転軸の順番を変えて
時刻0.0[s]～0.5[s]の間はy軸まわりに90°、
時刻0.5[s]～1.0[s]の間はz軸まわりに90°に回転させたとします。
この回転変化を回転(B)とします。
回転変化は非可換(かける順番を入れ替えると値が変わる)なので、
回転(A)と回転(B)の姿勢の結果は当然変わります。
この回転運動を角速度ベクトル\( {\bf \omega} \)で表すと、
$$
(A)\ \ {\bf \omega}(t) = \left\{
\begin{array}{c}
\frac{\pi}{2} {\bf e}_{z}\ (0.0 \leq t \lt 0.5) \\
\frac{\pi}{2} {\bf e}_{y}\ (0.5 \leq t \leq1.0)
\end{array}
\right.
$$
$$
(B)\ \ {\bf \omega}(t) = \left\{
\begin{array}{c}
\frac{\pi}{2} {\bf e}_{y}\ (0.0 \leq t \lt 0.5) \\
\frac{\pi}{2} {\bf e}_{z}\ (0.5 \leq t \leq1.0)
\end{array}
\right.
$$
になりますが、合計した移動姿勢を求めようとそれぞれ角速度ベクトルを積分すると、
(A)も(B)も
$$
\int^1_0{\bf \omega}(t) dt = \frac{\pi}{2} \left( {\bf e}_{y} + {\bf e}_{z} \right)
$$
となってしまいます。
回転後の姿勢の結果は(A)と(B)違うのに、姿勢の回転速度を表した\( {\bf \omega}(t) \)の積分値は(A)と(B)で同じになってしまっているのです。
これは、角速度ベクトル\( {\bf \omega}(t) \)の積分値はなんら意味を有さないことを表しています。
そもそも回転行列の速度の導出で、回転軸\( {\bf n} \)は、その時点の回転運動において時間変化によらず一定と考えていました。
この積分の意味は、全ての回転が、合わせて一つの回転軸まわりに回転するよう表現されることと同じになります。そこに各回転軸方向の回転をかける順序など情報として存在しないのです。
これは重要な性質になります。
ふう、、、
何というか重たいです。
これ教科書とかだと2，3ページで終わりそうな内容ですが、よく誤解しちゃう定義なのでがっつりノートしてみました。
でも、説明とか間違ってたらゴメンナサイ。。
以上です。