キャチロボ Advent Calendar 2025

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

Last updated at 2025-12-06Posted at 2025-12-06

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

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

1. はじめに

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

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

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

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

図2　関節の配置

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

使用するロボットは4自由度の開鎖機構であり、その空間スクリュー$\boldsymbol{\zeta}_1$～$\boldsymbol{\zeta}_4$は以下に示すとおりです。

\boldsymbol{\zeta}_1
=
\begin{bmatrix}
\boldsymbol{\omega}_1 \\
\boldsymbol{v}_1 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{1}

\boldsymbol{\zeta}_2
=
\begin{bmatrix}
\boldsymbol{\omega}_2 \\
\boldsymbol{v}_2 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
0 \\
1 \\
0 \\
0 \\
\end{bmatrix}
\tag{2}

\boldsymbol{\zeta}_3
=
\begin{bmatrix}
\boldsymbol{\omega}_3 \\
\boldsymbol{v}_3 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
-L \\
0 \\
\end{bmatrix}
\tag{3}

\boldsymbol{\zeta}_4
=
\begin{bmatrix}
\boldsymbol{\omega}_4 \\
\boldsymbol{v}_4 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
0 \\
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{4}

そして(1)～(4)式の行列指数をとったものが以下のようになります。

e^{[\boldsymbol{\zeta}_1] \theta_1}
=
\begin{bmatrix}
\cos{\theta_1} & -\sin{\theta_1} & 0 & 0 \\
\sin{\theta_1} & \cos{\theta_1} & 0  & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{5}

e^{[\boldsymbol{\zeta}_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{6}

e^{[\boldsymbol{\zeta}_3] \theta_3}
=
\begin{bmatrix}
\cos{\theta_3} & -\sin{\theta_3} & 0 & L(1 - \cos{\theta_3}) \\
\sin{\theta_3} & \cos{\theta_3} & 0  & -L \sin{\theta_3} \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{7}

e^{[\boldsymbol{\zeta}_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{8}

(5)～(8)式は同次変換行列であり、空間ヤコビアン$J_s$の計算時にはアジョイント表現の引数になります。

3. キャチロボ機体の空間ヤコビアン

　本記事で扱うロボットは4自由度なので、空間ヤコビアン$J_s$は$6 \times 4$行列になります。また、その成分は以下に示すとおりです。

J_s
=
\begin{bmatrix}
\boldsymbol{\zeta}_1
&
Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1}}
(\boldsymbol{\zeta}_2)
&
Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1} e^{[\boldsymbol{\zeta}_2] \theta_2}}
(\boldsymbol{\zeta}_3)
&
Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1} e^{[\boldsymbol{\zeta}_2] \theta_2} e^{[\boldsymbol{\zeta}_3] \theta_3}}
(\boldsymbol{\zeta}_4)
\end{bmatrix}
\tag{9}

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

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

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

Ad_{T_{sb}}
=
\begin{bmatrix}
R_{sb} & O \\
[\boldsymbol{p_{sb}}]R_{sb} & R_{sb} \\
\end{bmatrix}
\tag{11}

(11)式を(5)～(8)式にも用いることで、各同次変換行列のアジョイント表現が以下のように得られます。大きな行列ですが、計算自体はそこまで複雑ではありません。

[Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1}}]
=
\begin{bmatrix}
\cos{\theta_1} & -\sin{\theta_1} & 0 & 0 & 0 & 0 \\
\sin{\theta_1} & \cos{\theta_1} & 0 & 0 & 0 & 0 \\
0 & 0 & 1 & 0 & 0 & 0 \\
0 & 0 & 0 & \cos{\theta_1} & -\sin{\theta_1} & 0 \\
0 & 0 & 0 & \sin{\theta_1} & \cos{\theta_1} & 0  \\
0 & 0 & 0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{12}

[Ad_{e^{[\boldsymbol{\zeta}_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{13}

[Ad_{e^{[\boldsymbol{\zeta}_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 & -L \sin{\theta_3} & \cos{\theta_3} & -\sin{\theta_3} & 0 \\
0 & 0 & -L(1 - \cos{\theta_3}) & \sin{\theta_3} & \cos{\theta_3} & 0  \\
L & 0 & 0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{14}

では実際に準備が整ったので、空間ヤコビアン$J_s$の各成分を頑張って計算してみましょう。

Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1}}(\boldsymbol{\zeta}_2)
=
[Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1}}]\boldsymbol{\zeta}_2
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\cos{\theta_1} \\
\sin{\theta_1} \\
0 \\
\end{bmatrix}
\tag{15}

Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1} e^{[\boldsymbol{\zeta}_2] \theta_2}}(\boldsymbol{\zeta}_3)
=
[Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1}}]
[Ad_{e^{[\boldsymbol{\zeta}_2] \theta_2}}]
\boldsymbol{\zeta}_3
=
\begin{bmatrix}
0 \\
0 \\
1 \\
(\theta_2 + L)\sin{\theta_1} \\
-(\theta_2 + L)\cos{\theta_1} \\
0 \\
\end{bmatrix}
\tag{16}

Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1} e^{[\boldsymbol{\zeta}_2] \theta_2} e^{[\boldsymbol{\zeta}_3] \theta_3}}(\boldsymbol{\zeta}_4)
=
[Ad_{e^{[\boldsymbol{\zeta}_1] \theta_1}}]
[Ad_{e^{[\boldsymbol{\zeta}_2] \theta_2}}]
[Ad_{e^{[\boldsymbol{\zeta}_3] \theta_3}}]
\boldsymbol{\zeta}_4
=
\begin{bmatrix}
0 \\
0 \\
0 \\
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{17}

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

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

以上で空間ヤコビアン$J_s$の導出ができました。

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. おわりに

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