キャチロボ Advent Calendar 2025

スクリュー理論㉝キャチロボ機体を用いた順運動学計算（ボディ座標系）

Last updated at 2025-12-13Posted at 2025-12-13

　はじめまして、りょーつといいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする33個目の記事です。前回までの記事は以下のリンクを参照してください。

　スクリュー理論は剛体の運動を記述する方法で、ロボット工学などでよく使われています。日本の高専や大学ではDH法を使った記述を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。スクリュー理論を扱う日本語の文献は少ないので、この記事が誰かの助けになればいいなと思います。

1. はじめに

　今回の記事ではボデイヤコビアンの具体例の解説として、キャチロボに使用したロボットのボデイヤコビアンを導出してみたいと考えています。使用するロボットの詳細パラメータはこちらの記事にまとめてあるので参考にしてください。また、本記事ではボディ座標系を用いた順運動学やボディヤコビアンの計算を多用するので、これらを復習しておくと幸せになれます。

2. キャチロボ機体のボディスクリュー

　今回計算に使用するロボットの順運動学を導出していきます。使用するロボットの外観や寸法は図1～3に示すとおりです。パラメータについてはこちらの記事を参照ください。

図1　キャチロボバトルコンテスト出場【ぷりん】

図2　関節の配置

図3　主要な寸法と座標系の定義

まずはボディスクリューを求めていきましょう。ボディ座標系$\Sigma_b$の姿勢は基準座標系$\Sigma_s$と同じになるよう取れば、角速度ベクトル(回転軸の方向)は見た目通りに記述することができます。ボディ座標系$\Sigma_b$の初期位置姿勢$M$は以下のとおりです。

M
=
\begin{bmatrix}
1 & 0 & 0 & L \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & H-B \\
0 & 0 & 0 & 1  \\
\end{bmatrix}
\tag{1}

　また、各回転軸の動きによってエンドエフェクタに生じる速度を考えると各軸のボデイスクリューは以下のようにまとめることができます。過去にも似たような操作を何度か行っているため、各成分の詳しい導出はこちらの記事を参考にしてください。

\boldsymbol{\beta}_1
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
L \\
0 \\
\end{bmatrix}
\tag{2}

\boldsymbol{\beta}_2
=
\begin{bmatrix}
0 \\
0 \\
0 \\
1 \\
0 \\
0 \\
\end{bmatrix}
\tag{3}

\boldsymbol{\beta}_3
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{4}

\boldsymbol{\beta}_4
=
\begin{bmatrix}
0 \\
0 \\
0 \\
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{5}

いかがでしょうか？ボディ座標系で考えると、直接エンドエフェクタを動かすような計算になるので、基準座標を用いた計算よりも直感的かと思います。

3. キャチロボ機体の順運動学

　本章では2章で導出したボディスクリューの行列指数をとり、それらの積をもとに順運動学を導出します。スクリューの行列指数の定義はこちらの記事に示すとおりです。本記事ではあえて計算過程を省略しますので、演習問題として計算してみてください。

　まずは各ボディスクリューの行列指数を計算しましょう。計算結果は以下のとおりです。

e^{[\boldsymbol{\beta}_1]\theta_1}
=
\begin{bmatrix}
\cos{\theta_1} & -\sin{\theta_1} & 0 & -L(1 - \cos{\theta_1}) \\
\sin{\theta_1} & \cos{\theta_1} & 0 & L\sin{\theta_1} \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1  \\
\end{bmatrix}
\tag{6}

e^{[\boldsymbol{\beta}_2]\theta_2}
=
\begin{bmatrix}
1 & 0 & 0 & \theta_2 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1  \\
\end{bmatrix}
\tag{7}

e^{[\boldsymbol{\beta}_3]\theta_3}
=
\begin{bmatrix}
\cos{\theta_3} & -\sin{\theta_3} & 0 & 0 \\
\sin{\theta_3} & \cos{\theta_3} & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1  \\
\end{bmatrix}
\tag{8}

e^{[\boldsymbol{\beta}_4]\theta_4}
=
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & \theta_4 \\
0 & 0 & 0 & 1  \\
\end{bmatrix}
\tag{9}

では順運動学$T_{sb}$を計算してみましょう。ボディ座標系を用いた順運動学の計算より、以下を計算すればよいことが分かります。

T_{sb}
=
M
e^{[\boldsymbol{\beta}_1]\theta_1}
e^{[\boldsymbol{\beta}_2]\theta_2}
e^{[\boldsymbol{\beta}_3]\theta_3}
e^{[\boldsymbol{\beta}_4]\theta_4}
\tag{10}

では実際に計算してみるとどうなるでしょうか？(10)式の右辺を愚直に展開すると以下の結果が得られます。

e^{[\boldsymbol{\beta}_3]\theta_3}
=
\begin{bmatrix}
\cos{(\theta_1 + \theta_3)} & -\sin{(\theta_1 + \theta_3)} & 0 & (L + \theta_2)\cos{\theta_1} \\
\sin{(\theta_1 + \theta_3)} & \cos{(\theta_1 + \theta_3)} & 0 & (L + \theta_2)\sin{\theta_1} \\
0 & 0 & 1 & H - B + \theta_4 \\
0 & 0 & 0 & 1  \\
\end{bmatrix}
\tag{11}

これで順運動学の計算は完了です！

　興味深いのは(11)式の計算結果が基準座標系を用いた結果としっかり一致している点です。学問の美しさを感じますね(^▽^)

4. おわりに

　本稿では、ボディ座標系を用いた順運動学の計算について、キャチロボに出場した機体を用いた具体例を示しました。似た計算が多く登場するので少し省略した形になりましたが、ボディ座標系の使い方を学ぶ良い例題になったかと思います。
　多くの場合は、ボディ座標系$\Sigma_b$ではなく基準座標系$\Sigma_s$が用いられますが、手計算で実用的なヤコビアン$J_e$の導出を行う際などはボディ座標系を用いたほうがラクな場合も多く、使えるようにしておくと汎用性が高まります。また、ボディ座標系を用いた考え方を抑えておくと、複数の座標系を扱う場合に適切な座標変換ができるようになるのでぜひ挑戦してみてください。

　今週も最後まで読んでいただきありがとうございました！

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up

スクリュー理論㉝ キャチロボ機体を用いた順運動学計算（ボディ座標系）

目次

1. はじめに

2. キャチロボ機体のボディスクリュー

3. キャチロボ機体の順運動学

4. おわりに

スクリュー理論㉝キャチロボ機体を用いた順運動学計算（ボディ座標系）