はじめまして、りょーつといいます。高専出身の大学院1年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。
本稿はスクリュー理論の解説(布教)をする6つ目の記事です。前回までの記事はプロフィールから確認してください。今回は、スクリュー理論の機構学への応用として、機構学の講義によく出てくる剛体の速度・加速度の座標変換公式の導出過程を示そうと思います。導出する式は以下の2つです。
\dot{\boldsymbol{x_s}}
=
\dot{\boldsymbol{p_{sb}}}
+
\boldsymbol{\omega_s} \times R_{sb}\ \boldsymbol{x_b}
+
R_{sb}\ \dot{\boldsymbol{x_b}}
\tag{1}
\ddot{\boldsymbol{x_s}}
=
\ddot{\boldsymbol{p_{sb}}}
+
\dot{\boldsymbol{\omega_s}} \times R_{sb}\ \boldsymbol{x_b}
+
\boldsymbol{\omega_s} \times \boldsymbol{\omega_s} \times \boldsymbol{x_b}
+
2\boldsymbol{\omega_s} \times R_{sb}\ \dot{\boldsymbol{x_b}}
+
R_{sb}\ \ddot{\boldsymbol{x_b}}
\tag{2}
式の詳細については後述します。
目次
1.はじめに
2.スクリュー理論を用いた回転行列の表記
3.変換公式の導出
4.おわりに
1. はじめに
本稿ではスクリュー理論を使って変換公式の導出を行います。スクリュー理論は剛体の運動を記述する手法で、ロボット工学などでよく使われています。日本の高専や大学では他の記法を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。
今回の記事ではスクリュー理論に深入りせず、結果だけを用いることにします。気になる方は以下の記事をご参照ください。
スクリュー理論① 外積の歪対称行列表現
スクリュー理論② 行列の指数関数
スクリュー理論③ 行列指数と回転行列の関係
座標系は図1のように設定し、基準座標系を$\Sigma_s$、剛体とともに移動するボディ座標系を$\Sigma_b$とします。基準座標系$\Sigma_s$から見たボディ座標系$\Sigma_b$の原点の位置を$\boldsymbol{p_{sb}}$とします。また、剛体上の任意の点を$\boldsymbol{x}$と置き、$\boldsymbol{x}$を基準座標系$\Sigma_s$から見たものを$\boldsymbol{x_s}$、ボディ座標系$\Sigma_b$から見たものを$\boldsymbol{x_b}$とします。また、座標系が回転軸$\boldsymbol{\omega}$によって瞬間的に姿勢変化しているとします(※剛体が並進運動をする場合は$\boldsymbol{\omega} = \boldsymbol{0}$です)。回転軸$\boldsymbol{\omega}$も基準座標系$\Sigma_s$から見たものを$\boldsymbol{\omega_s}$、ボディ座標系$\Sigma_b$から見たものを$\boldsymbol{\omega_b}$とします。時刻を$t$とし、$\dot{\boldsymbol{x_s}}$は$\boldsymbol{x_s}$の時間微分を表すこととします。また、基準座標系$\Sigma_s$から見たボディ座標系$\Sigma_b$を表す回転行列を$R_{sb}$と置きます。
図1 座標系とベクトルの定義
2. スクリュー理論を用いた回転行列の表記
まずはスクリュー理論により回転行列とその時間微分をシンプルに記述する方法を紹介します。スクリュー理論において、回転行列$R_{sb}$は行列指数を用いて以下のように表記可能です。
R_{sb}(t)
=
e^{[\boldsymbol{\omega_s}]t} R_{sb}(0)
\tag{3}
ただし、$[\boldsymbol{\omega_s}]$は$\omega_s$の成分を用いて構成される歪対称行列です。詳細は後述します。(3)式をシンプルに微分することで回転行列の微分は以下のように表せます。
\dot{R_{sb}}
=
[\boldsymbol{\omega_s}] e^{[\boldsymbol{\omega_s}]t} R_{sb}(0)
=
[\boldsymbol{\omega_s}] R_{sb}
\tag{4}
ここで$[\boldsymbol{\omega_s}]$はベクトルの外積を意味しており、(4)式を外積の形式で記述すると以下のようになります。
\dot{R_{sb}}
=
\boldsymbol{\omega_s}\times R_{sb}
\tag{5}
不思議な式変形を行いましたが、「はじめに」の章で紹介した記事を追っていただければ(3)～(5)式の変形にも納得いただけると思います。3章以降にも(5)式が登場します。
3. 変換公式の導出
いよいよ変換公式の導出にすすみます。まずは位置ベクトルの座標変換より、以下の関係がなりたちます。
\boldsymbol{x_s}
=
\boldsymbol{p_{sb}}
+
R_{sb} \ \boldsymbol{x_b}
\tag{6}
あとは(5)式に注意しながらシンプルに微分していくことになります。まずは速度$\dot{\boldsymbol{x_s}}$を計算してみましょう。積の微分に注意してください。
\dot{\boldsymbol{x_s}}
=
\dot{\boldsymbol{p_{sb}}}
+
\dot{R_{sb}} \ \boldsymbol{x_b}
+
R_{sb} \ \dot{\boldsymbol{x_b}}
\therefore
\dot{\boldsymbol{x_s}}
=
\dot{\boldsymbol{p_{sb}}}
+
[\boldsymbol{\omega_s}]R_{sb} \ \boldsymbol{x_b}
+
R_{sb} \ \dot{\boldsymbol{x_b}}
\tag{7}
外積に注意する必要がないため、(6)式からすんなりと導出できたのではないでしょうか?ちなみに$[\boldsymbol{\omega_s}]$は最後にまとめて外積に変換します。
同様に加速度$\ddot{\boldsymbol{x_s}}$の計算もしてみましょう。積の微分に注意です。
\ddot{\boldsymbol{x_s}}
=
\ddot{\boldsymbol{p_{sb}}}
+
\dot{\big([\boldsymbol{\omega_s}]R_{sb}\big)} \ \boldsymbol{x_b}
+
\big([\boldsymbol{\omega_s}]R_{sb}\big) \ \dot{\boldsymbol{x_b}}
+
\dot{R_{sb}} \ \dot{\boldsymbol{x_b}}
+
R_{sb}\ \ddot{\boldsymbol{x_b}}
=
\ddot{\boldsymbol{p_{sb}}}
+
\dot{[\boldsymbol{\omega_s}]}R_{sb} \ \boldsymbol{x_b}
+
[\boldsymbol{\omega_s}]\dot{R_{sb}} \ \boldsymbol{x_b}
+
[\boldsymbol{\omega_s}]R_{sb} \ \dot{\boldsymbol{x_b}}
+
\dot{R_{sb}} \ \dot{\boldsymbol{x_b}}
+
R_{sb}\ \ddot{\boldsymbol{x_b}}
=
\ddot{\boldsymbol{p_{sb}}}
+
\dot{[\boldsymbol{\omega_s}]}R_{sb} \ \boldsymbol{x_b}
+
[\boldsymbol{\omega_s}][\boldsymbol{\omega_s}]R_{sb} \ \boldsymbol{x_b}
+
[\boldsymbol{\omega_s}]R_{sb} \ \dot{\boldsymbol{x_b}}
+
[\boldsymbol{\omega_s}]R_{sb} \ \dot{\boldsymbol{x_b}}
+
R_{sb}\ \ddot{\boldsymbol{x_b}}
\therefore
\ddot{\boldsymbol{x_s}}
=
\ddot{\boldsymbol{p_{sb}}}
+
\dot{[\boldsymbol{\omega_s}]}R_{sb} \ \boldsymbol{x_b}
+
[\boldsymbol{\omega_s}][\boldsymbol{\omega_s}]R_{sb} \ \boldsymbol{x_b}
+
2[\boldsymbol{\omega_s}]R_{sb} \ \dot{\boldsymbol{x_b}}
+
R_{sb}\ \ddot{\boldsymbol{x_b}}
\tag{8}
少し複雑ですが丁寧に展開すれば大丈夫です。最後に(7)式と(8)式の$[\boldsymbol{\omega_s}]$を外積に置き換えると..?
\dot{\boldsymbol{x_s}}
=
\dot{\boldsymbol{p_{sb}}}
+
\boldsymbol{\omega_s}\times R_{sb} \ \boldsymbol{x_b}
+
R_{sb} \ \dot{\boldsymbol{x_b}}
\tag{9}
\ddot{\boldsymbol{x_s}}
=
\ddot{\boldsymbol{p_{sb}}}
+
\dot{\boldsymbol{\omega_s}} \times R_{sb} \ \boldsymbol{x_b}
+
\boldsymbol{\omega_s}\times \boldsymbol{\omega_s} \times R_{sb} \ \boldsymbol{x_b}
+
2\boldsymbol{\omega_s} \times R_{sb} \ \dot{\boldsymbol{x_b}}
+
R_{sb}\ \ddot{\boldsymbol{x_b}}
\tag{10}
これで式の導出が完了しました。これらの式はIMUを使って移動する剛体の位置・姿勢の推定に使えたりします(多分)。現在、(9)(10)式を用いて自動車の走行軌道を取得するデバイスを作っているので、うまくいけばそれらも記事にまとめていこうと思います。なお、本記事では回転軸として基準座標系$\Sigma_s$から見た$\boldsymbol{\omega_s}$を用いましたが、実用的にはボディ座標系$\Sigma_b$から見た$\boldsymbol{\omega_b}$を用いた変換のほうが便利です。(9)(10)式を$\boldsymbol{\omega_b}$を使って記述することもできますが、少し複雑になるのでまた別の機会に紹介します。
4. おわりに
本稿では機構学の講義によく出てくる剛体の速度・加速度の座標変換公式の導出過程を示しました。ロボット工学にも、オイラーの方法を使って運動方程式を立てる際に似た式が登場するかと思います。本記事の内容が誰かの助けになれば幸いです。
最後まで読んでいただきありがとうございました！