キャチロボ Advent Calendar 2025

スクリュー理論㉞キャチロボ機体を用いたボディヤコビアンの具体例

Last updated at 2025-12-20Posted at 2025-12-20

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

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

1. はじめに

　今回の記事ではボディヤコビアン$J_b$の具体例の解説として、キャチロボに使用したロボットのボディヤコビアン$J_b$を導出してみたいと考えています。使用するロボットの詳細パラメータはこちらの記事にまとめてあるので参考にしてください。また、本記事では大量のアジョイント変換を行うので、ボディヤコビアン$J_b$の計算方法についてもこちらの記事で確認しておくとよいと思います。

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

　今回計算に使用するロボットの同次変換行列について復習しておきます。使用するロボットの外観や寸法は図1～3に示すとおりです。パラメータについてはこちらの記事を参照ください。

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

図2　関節の配置

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

使用するロボットは4自由度の開鎖機構であり、そのボディスクリューの行列指数$e^{[\boldsymbol{\beta}_1]\theta_1}$～$e^{[\boldsymbol{\beta}_4]\theta_4}$は以下に示すとおりです。なお、これらの計算過程は前回の記事に示してあります。

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{1}

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{2}

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{3}

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{4}

本記事では(1)～(4)式の行列指数をもとにアジョイント変換を計算し、ボディヤコビアン$J_b$を導出します。

3. キャチロボ機体のボディヤコビアン

　本記事で扱うロボットは4自由度なので、ボディヤコビアン$J_b$は$6 \times 4$行列になります。また、その成分は以下に示すとおりです。詳しくはこちらの記事を参照ください。

J_b
=
\begin{bmatrix}
Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
Ad^{-1}_{e^{[\boldsymbol{\beta}_3] \theta_3}}
Ad^{-1}_{e^{[\boldsymbol{\beta}_2] \theta_2}}
\boldsymbol{\beta}_1
&
Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
Ad^{-1}_{e^{[\boldsymbol{\beta}_3] \theta_3}}
\boldsymbol{\beta}_2
&
Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
\boldsymbol{\beta}_3
&
\boldsymbol{\beta}_4
\end{bmatrix}
\tag{5}

まずは各アジョイント変換の成分を計算していきます。同次変換行列$T_{sb}$を

T_{sb}
=
\begin{bmatrix}
R_{sb} & \boldsymbol{p_{sb}} \\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{6}

としたとき、アジョイント逆表現${Ad_{T_{sb}}}^{-1}$は以下のように表記できます。

{Ad_{T_{sb}}}^{-1}
=
\begin{bmatrix}
{R_{sb}}^{T} & O \\
-{R_{sb}}^{T}[\boldsymbol{p_{sb}}] & {R_{sb}}^{T} \\
\end{bmatrix}
\tag{7}

導出過程はこちらの記事に詳しくまとめています。

　(7)式に対して(1)～(4)式の各成分を代入することで以下のようにアジョイント逆表現が得られます。大きな行列ですが、計算自体はそこまで複雑ではありません。

Ad^{-1}_{e^{[\boldsymbol{\beta}_2] \theta_2}}
=
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 1 & 0 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 & 0 \\
0 & 0 & \theta_2 & 0 & 1 & 0  \\
0 & -\theta_2 & 0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{8}

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

Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
=
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 1 & 0 & 0 & 0 \\
0 & \theta_4 & 0 & 1 & 0 & 0 \\
-\theta_4 & 0 & 0 & 0 & 1 & 0  \\
0 & 0 & 0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{10}

では実際に準備が整ったので、ボディヤコビアン$J_b$の各成分を頑張って計算してみましょう。行列の掛け算を頑張れば同じ結果が得られるはずです。各スクリューの値$\boldsymbol{\beta}_1$～$\boldsymbol{\beta}_4$はこちらを確認してください。

Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
Ad^{-1}_{e^{[\boldsymbol{\beta}_3] \theta_3}}
Ad^{-1}_{e^{[\boldsymbol{\beta}_2] \theta_2}}
\boldsymbol{\beta}_1
=
\begin{bmatrix}
0 \\
0 \\
1 \\
(L + \theta_2)\sin{\theta_3} \\
(L + \theta_2)\cos{\theta_3} \\
0 \\
\end{bmatrix}
\tag{11}

Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
Ad^{-1}_{e^{[\boldsymbol{\beta}_3] \theta_3}}
\boldsymbol{\beta}_2
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\cos{\theta_3} \\
-\sin{\theta_3} \\
0 \\
\end{bmatrix}
\tag{12}

Ad^{-1}_{e^{[\boldsymbol{\beta}_4] \theta_4}}
\boldsymbol{\beta}_3
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{13}

以上の計算結果を(5)式にまとめることで、キャチロボのロボットのボディヤコビアン$J_b$が以下のようにもとまります。

J_b
=
\begin{bmatrix}
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
1 & 0 & 1 & 0 \\
(L + \theta_2)\sin{\theta_3} & \cos{\theta_3} & 0 & 0 \\
(L + \theta_2)\cos{\theta_3} & -\sin{\theta_3} & 0 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{18}

以上でボディヤコビアン$J_b$の導出ができました。

4. おわりに

　今週の記事では、キャチロボのロボットのパラメータを実際に使用して、ボディヤコビアン$J_b$の具体的な計算例を示しました。来週の記事ではいよいよ大会本番でも使用した実用的なヤコビアン$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. おわりに

スクリュー理論㉞キャチロボ機体を用いたボディヤコビアンの具体例