スクリュー理論㉙空間ヤコビアンの代数学的な導出

Last updated at 2025-11-15Posted at 2025-11-15

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

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

1. はじめに

今週の記事では、空間ヤコビアン$J_s$の代数的な導入について解説します。ツイスト$\boldsymbol{\nu}$のアジョイント変換と順運動学を多用するので、アジョイント変換の記事と順運動学の記事を先に読んでおくことをおすすめします。2章でアジョイント変換の復習、3章でちょっとした計算、4章で空間ヤコビアンの代数的な導入を行います。
　内容が盛りだくさんですね。記事の構成自体は前回と同じですので、こちらを理解していればすぐに読めてしまうと思います。

2. ツイストのアジョイント変換の復習

　ここでは本記事の解説に必要となる、アジョイント変換と基準座標系の順運動学についてまとめます。基準座標系を$\Sigma_s$、ボディ座標系を$\Sigma_b$とし、それぞれの座標系でツイスト$\boldsymbol{\nu_s},\boldsymbol{\nu_b} \in \mathbb{R}^6$を考えます。基準座標系$\Sigma_s$から見たボディ座標系$\Sigma_b$の位置姿勢を同次変換行列$T_{sb}\in
SE(3)$で表したとき、$\boldsymbol{\nu_s}$と$\boldsymbol{\nu_b}$はアジョイント変換によって以下のように変換できます。

\boldsymbol{\nu_s}
=
Ad_{T_{sb}}(\boldsymbol{\nu_b})
\tag{1}

また、各ツイストの行列表現を$[\boldsymbol{\nu_s}],[\boldsymbol{\nu_b}] \in
\mathbb{R}^4 \times \mathbb{R}^4$としたとき、以下の関係がなりたちます。

[\boldsymbol{\nu_s}]
=
\frac{d T_{sb}}{dt}
{T_{sb}}^{-1}
\tag{2}

[\boldsymbol{\nu_b}]
=
{T_{sb}}^{-1}
\frac{d T_{sb}}{dt}
\tag{3}

[\boldsymbol{\nu_s}]
=
T_{sb}
[\boldsymbol{\nu_b}]
{T_{sb}}^{-1}

\therefore
[Ad_{T_{sb}}(\boldsymbol{\nu_b})]
=
T_{sb}
[\boldsymbol{\nu_b}]
{T_{sb}}^{-1}
\tag{4}

ツイストの行列表現については過去記事を参照ください。
　さらに(4)式の逆変換を考えると以下の関係も得られます。

[\boldsymbol{\nu_b}]
=
{T_{sb}}^{-1}
[\boldsymbol{\nu_s}]
T_{sb}

\therefore
[Ad_{T_{sb}}(\boldsymbol{\nu_b})]
=
{T_{sb}}^{-1}
[\boldsymbol{\nu_s}]
T_{sb}
\tag{5}

また、系の基準座標系$\Sigma_s$からみたスクリューを$\boldsymbol{\zeta}$としたとき、順運動学は以下の式で与えられます。

T_{sb}
=
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M
\tag{6}

ただし、各スクリュー$\boldsymbol{\zeta_k}$の回転角を$\theta_k$、基準座標系$\Sigma_s$からみた、各回転角$\theta_k$をまとめたベクトル$\boldsymbol{\theta} = \begin{bmatrix}\theta_1 \cdots \theta_n \ \end{bmatrix}^T \in \mathbb{R}^n$が零ベクトルのときのボディ座標系$\Sigma_b$の位置姿勢を$M \in SE(3)$とします。右下の添え字はロボットアームの関節番号を意味しており、根元に近いものから先端に近づくにつれて数字を大きくしていきます。詳しくは順運動学の記事を参照ください。

3. 順運動学の時間微分と逆変換

　空間ヤコビアンを考えるにあたり、2章で紹介した順運動学(6)式の時間微分を考える必要があります。変数$x$の時間微分を$\dot{x}$のように表記することにすると、チェインルールより

\frac{d T_{sb}}{dt}
=
\frac{\partial T_{sb}}{\partial \theta_1} \frac{d \theta_1}{dt}
+
\frac{\partial T_{sb}}{\partial \theta_2} \frac{d \theta_2}{dt}
+
\cdots
\frac{\partial T_{sb}}{\partial \theta_2} \frac{d \theta_2}{dt}

\therefore
\frac{d T_{sb}}{dt}
=
\sum_{k=1}^n
\frac{\partial T_{sb}}{\partial \theta_k} \dot{\theta_k}
\tag{7}

になります。(7)式より、$\frac{\partial T_{sb}}{\partial \theta_k}$を計算すればよいと考えられますね。
　実際に計算してみましょう。(4)式より

\frac{\partial T_{sb}}{\partial \theta_k}
=
\frac{\partial}{\partial \theta_k}
\bigg(
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M
\bigg)

=
\frac{\partial}{\partial \theta_k}
\bigg(
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
e^{[\boldsymbol{\zeta_k}]\theta_k}
e^{[\boldsymbol{\zeta_{k+1}}]\theta_{k+1}}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M
\bigg)
\tag{8}

であり、$\theta_k$以外は全て定数とみなすため

\frac{\partial T_{sb}}{\partial \theta_k}
=
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
\frac{\partial}{\partial \theta_k}
\Big(
e^{[\boldsymbol{\zeta_k}]\theta_k}
\Big)
e^{[\boldsymbol{\zeta_{k+1}}]\theta_{k+1}}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M

=
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
[\boldsymbol{\zeta_k}]
e^{[\boldsymbol{\zeta_k}]\theta_k}
e^{[\boldsymbol{\zeta_{k+1}}]\theta_{k+1}}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M
\tag{9}

が得られます。行列指数の微分について以下がなりたつ点に注意してください。

\frac{\partial}{\partial \theta_k}e^{[\boldsymbol{\zeta_k}]\theta_k}
=
[\boldsymbol{\zeta_k}]
e^{[\boldsymbol{\zeta_k}]\theta_k}
\frac{d \theta}{dt}
=
e^{[\boldsymbol{\zeta_k}]\theta_k}
[\boldsymbol{\zeta_k}]
\frac{d \theta}{dt}
\tag{10}

また、順運動学の逆変換${T_{sb}}^{-1}$についても考えてみましょう。(※あくまでも同次変換行列の逆行列であり、逆運動学とは別の概念です。)
　シンプルに(6)式の両辺の逆行列を取ると以下の式が得られます。

{T_{sb}}^{-1}
=
M^{-1}
e^{-[\boldsymbol{\zeta_n}]\theta_n}
e^{-[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
\cdots
e^{-[\boldsymbol{\zeta_2}]\theta_2}
e^{-[\boldsymbol{\zeta_1}]\theta_1}
\tag{11}

すごく長い式ですが、すべての項の指数に$-1$がついて、かける順番が逆になっただけです。
　これらの計算により、空間ヤコビアンの導出に必要な準備が完了しました。

4. 空間ヤコビアン

　空間ヤコビアン$J_s$の定義は、前回の記事で紹介したとおり以下の計算式で与えられます。

\boldsymbol{\nu_s}
=
J_s
\dot{\boldsymbol{\theta}}
\tag{12}

本記事では(2)式を直接計算した結果と、(12)式を比較することで、$J_s$を導出していこうと思います。では(7)式と(11)式を(2)式に代入し、直接$[\boldsymbol{\nu_s}]$を計算してみましょう。

[\boldsymbol{\nu_s}]
=
\Bigg(
\sum_{k=1}^n
\frac{\partial T_{sb}}{\partial \theta_k} \dot{\theta_k}
\Bigg)
M^{-1}
e^{-[\boldsymbol{\zeta_n}]\theta_n}
e^{-[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
\cdots
e^{-[\boldsymbol{\zeta_2}]\theta_2}
e^{-[\boldsymbol{\zeta_1}]\theta_1}

=
\sum_{k=1}^n
\Bigg(
\frac{\partial T_{sb}}{\partial \theta_k} 
M^{-1}
e^{-[\boldsymbol{\zeta_n}]\theta_n}
e^{-[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
\cdots
e^{-[\boldsymbol{\zeta_2}]\theta_2}
e^{-[\boldsymbol{\zeta_1}]\theta_1}
\dot{\theta_k}
\Bigg)
\tag{13}

一度に(13)式を計算するのは大変なので、まずは$ \frac{\partial T_{sb}}{\partial \theta_k} {T_{sb}}^{-1}$を計算してみましょう。(9)式を代入すると以下の計算結果が得られます。

\frac{\partial T_{sb}}{\partial \theta_k}{T_{sb}}^{-1}
=
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
[\boldsymbol{\zeta_k}]
e^{[\boldsymbol{\zeta_k}]\theta_k}
e^{[\boldsymbol{\zeta_{k+1}}]\theta_{k+1}}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M
M^{-1}
e^{-[\boldsymbol{\zeta_n}]\theta_n}
e^{-[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
\cdots
e^{-[\boldsymbol{\zeta_2}]\theta_2}
e^{-[\boldsymbol{\zeta_1}]\theta_1}

=
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
[\boldsymbol{\zeta_k}]
e^{[\boldsymbol{\zeta_k}]\theta_k}
e^{[\boldsymbol{\zeta_{k+1}}]\theta_{k+1}}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
e^{[\boldsymbol{\zeta_n}]\theta_n}
M
M^{-1}
e^{-[\boldsymbol{\zeta_n}]\theta_n}
e^{-[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
\cdots
e^{[\boldsymbol{\zeta_{k+1}}]\theta_{k+1}}
e^{[\boldsymbol{\zeta_k}]\theta_k}
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
\cdots
e^{-[\boldsymbol{\zeta_2}]\theta_2}
e^{-[\boldsymbol{\zeta_1}]\theta_1}

=
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
[\boldsymbol{\zeta_k}]
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
\cdots
e^{-[\boldsymbol{\zeta_2}]\theta_2}
e^{-[\boldsymbol{\zeta_1}]\theta_1}

\therefore
\frac{\partial T_{sb}}{\partial \theta_k}{T_{sb}}^{-1}
=
\bigg(
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
\bigg)
[\boldsymbol{\zeta_k}]
{\bigg(
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
\bigg)}^{-1}
\tag{14}

ここで(4)式のアジョイント変換によるツイストの変換により、(14)式はアジョイント変換を用いて以下のように書き換えられます。

\frac{\partial T_{sb}}{\partial \theta_k}{T_{sb}}^{-1}
=
[
Ad_
{
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
}
(\boldsymbol{\zeta_k})]
\tag{15}

では(13)式に(15)式を代入してみましょう。だいぶシンプルになりますね。

[\boldsymbol{\nu_s}]
=
\sum_{k=1}^n
[
Ad_
{
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
}
(\boldsymbol{\zeta_k})] 
\dot{\theta_k}

=
\Bigg [
\sum_{k=1}^n
Ad_
{
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
}
(\boldsymbol{\zeta_k})
\dot{\theta_k}
\Bigg ] 
\tag{16}

(16)式より、行列表現の中身は恒等的に等しいと考えられるため、空間ツイスト$\boldsymbol{\nu_s}$が以下のように定まります。

\boldsymbol{\nu_s}
=
\sum_{k=1}^n
Ad_
{
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{k-1}}]\theta_{k-1}}
}
(\boldsymbol{\zeta_k})
\dot{\theta_k}
\tag{17}

ここで、(17)式を理解しやすくするため、(15)式を書き下してみましょう。

\frac{\partial T_{sb}}{\partial \theta_1}{T_{sb}}^{-1}
=
[\boldsymbol{\zeta_1}]
\tag{18}

\frac{\partial T_{sb}}{\partial \theta_2} {T_{sb}}^{-1} 
=
[Ad_
{e^{[\boldsymbol{\zeta_1}]\theta_1}}(\boldsymbol{\zeta_2})]
\tag{19}

\vdots

\frac{\partial T_{sb}}{\partial \theta_n} {T_{sb}}^{-1} 
=
[Ad_
{
e^{[\boldsymbol{\zeta_1}]\theta_1}
e^{[\boldsymbol{\zeta_2}]\theta_2}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
}


(\boldsymbol{\zeta_n})]
\tag{20}

なんとなくイメージはつかめたでしょうか？(18)～(20)式を(17)式に代入し、行列にまとめると以下のようになります。

\boldsymbol{\nu_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}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
}(\boldsymbol{\zeta_n})
\end{bmatrix}

\begin{bmatrix}
\dot{\theta_1} \\
\dot{\theta_1} \\
\vdots \\
\dot{\theta_n} \\
\\
\end{bmatrix}

\therefore
\boldsymbol{\nu_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}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
}(\boldsymbol{\zeta_n})
\end{bmatrix}

\boldsymbol{\theta}
\tag{21}

なんとなく見たことある形になったかと思います。お察しのとおり(21)式は(12)式の空間ヤコビアン$J_s$の定義に対応しています。したがって、空間ヤコビアン$J_s$は以下のように計算できます。

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}
\cdots
e^{[\boldsymbol{\zeta_{n-1}}]\theta_{n-1}}
}(\boldsymbol{\zeta_n})
\end{bmatrix}
\tag{22}

ボディヤコビアン$J_s$の図形的なイメージについては以前の記事を参照ください。

5. おわりに

本記事では、空間ヤコビアン$J_s$の導出を代数的に行いました。アジョイント変換を図形的にイメージするのは至難の業ですが、代数的には比較的シンプルに導出することができます。これまでに空間ヤコビアンとボディヤコビアンを紹介しましたが、あと2つヤコビアンを紹介しようと考えています。1つは解析ヤコビアンと呼ばれるものであり、6次元のツイストの代わりに自由度分の一般化変数を使用する方式です。もう1つは実用的な目線で空間ヤコビアンやボディヤコビアンを改造したものについて解説します。

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

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

スクリュー理論㉙ 空間ヤコビアンの代数学的な導出

目次