articles:jacobian_matrix [RoboLab.Suehiro]

この文書は読取専用です。文書のソースを閲覧することは可能ですが、変更はできません。もし変更したい場合は管理者に連絡してください。
====== ロボットアームのヤコビ行列  ======

一般に多変数関数の微小量の関係は偏微分係数行列（ヤコビ行列
((ロボットの分野ではヤコビ行列（Jacobian matrix）をヤコビアンと呼ぶことが多い．
しかし，数学の分野ではヤコビアンはヤコビ行列式（Jacobian determinant）の意味で用いることも多いので
ここではヤコビ行列で統一する．
))
）で線形関係として近似的に表現できる．
ロボットアームのヤコビ行列はロボットの関節の微小な動き（もしくは速度）とロボットの手先の位置（および姿勢）の
微小な動き（もしくは速度）との関係を表現したものである．
これを使うと[[articles:newton_raphson|ニュートンラフソン法]]や[[articles:gauss_newton|ガウスニュートン法]]を
応用して逆運動学を数値的に解くことが出来る．
またアームの制御に直接用いて分解運動制御と呼ばれる多数の関節の協調動作を利用した動作が実現できる．
さらにロボットアーム単体だけでなく双腕，指，ビジョンなどのセンサ情報との協調にも用いることが出来る．

ここでは座標変換の微分表現を使ったヤコビ行列の導出を行う．この手法はロボットの運動学だけでなく
座標系のキャリブレーションや物体の形状フィッティングにも利用できる応用範囲の広いものとなっている．

また画像中の特徴量とアームの動きの微小量の関係を表すヤコビ行列（イメージ・ヤコビアンと呼ばれることも）を
用いてビジュアルフィードバックなどに応用することもできる．

===== 原理（2リンクアーム） =====


[{{ articles:jacobian_matrix_01.png?200|図1 2リンクアーム}}]
[{{ articles:jacobian_matrix_02.png?200|図2 $\theta_1$を少しだけ動かす}}]
[{{ articles:jacobian_matrix_03.png?200|図3 $\theta_2$を少しだけ動かす}}]
まずは簡単な図1の2リンクアームを考える．

関節角が$\boldsymbol{\theta}=\left( \begin{array}{c} \theta_1 \\ \theta_2 \end{array} \right)$のとき
手先位置が$\boldsymbol{p}=\left( \begin{array}{c} x \\ y \end{array} \right)$だとする．

ここで図2のように$\theta_1$を少しだけ動かす．つまり，$\theta_1$を$\theta_1+\Delta\theta_1$にする．
そのとき$\boldsymbol{p}$が$\boldsymbol{p}+\Delta\boldsymbol{p}_1 = \left( \begin{array}{c} x+\Delta x_1 \\ y+\Delta y_1 \end{array} \right)$になったとする．

これは，
$\Delta x_1 \simeq \dfrac{\partial x}{\partial \theta_1} \Delta \theta_1 , \;
\Delta y_1 \simeq \dfrac{\partial y}{\partial \theta_1} \Delta \theta_1
$
と書くことが出来る．
つまり，
$$
\Delta\boldsymbol{p}_1 \simeq \left( \begin{array}{c} \dfrac{\partial x}{\partial \theta_1} \\ \dfrac{\partial y}{\partial \theta_1} \end{array} \right) \Delta \theta_1
$$

つぎに図3のように$\theta_2$を少しだけ動かす．つまり，$\theta_2$を$\theta_2+\Delta\theta_2$にする．
そのとき$\boldsymbol{p}$が$\boldsymbol{p}+\Delta\boldsymbol{p}_2 = \left( \begin{array}{c} x+\Delta x_2 \\ y+\Delta y_2 \end{array} \right)$になったとする．

これは，
$\Delta x_2 \simeq \dfrac{\partial x}{\partial \theta_2} \Delta \theta_2 , \;
\Delta y_2 \simeq \dfrac{\partial y}{\partial \theta_2} \Delta \theta_2
$
と書くことが出来る．
つまり，
$$
\Delta\boldsymbol{p}_2 \simeq \left( \begin{array}{c} \dfrac{\partial x}{\partial \theta_2} \\ \dfrac{\partial y}{\partial \theta_2} \end{array} \right) \Delta \theta_2
$$

[{{ articles:jacobian_matrix_04.png?200|図4 $\theta_1$,$\theta_2$の両方をを少しだけ動かす}}]


ここで図4のように$\theta_1$,$\theta_2$の両方をを少しだけ動かすとどうなるだろうか．

それぞれの動きが十分小さいとすると手先の変化は
$\Delta\boldsymbol{p} \simeq  \Delta\boldsymbol{p}_1 +\Delta\boldsymbol{p}_2$
となる．すなわち
$$
\Delta\boldsymbol{p} \simeq 
\left( \begin{array}{cc} 
\dfrac{\partial x}{\partial \theta_1} & \dfrac{\partial x}{\partial \theta_2} \\ 
\dfrac{\partial y}{\partial \theta_1} & \dfrac{\partial y}{\partial \theta_2} 
\end{array} \right) 
\left( \begin{array}{c} \Delta\theta_1 \\ 
\Delta\theta_2 \end{array} \right)
$$
この偏微分係数行列をヤコビ行列（$J$と書くことが多い）と呼び，
$$
\Delta\boldsymbol{p} \simeq J\Delta\boldsymbol{\theta}
$$
と書かれる．

ヤコビ行列$J$は，関節角度$\boldsymbol{\theta}$の関数であり，関節角度によって変化する．

==== 分解運動制御 ====
[{{ articles:jacobian_matrix_05.png?200|図5 手先を狙った方向に動かす}}]

逆に$\Delta\boldsymbol{p}$が与えられたら関節角はどうなるのだろうか？
それぞれの動きが十分小さいとすると，
$$
\Delta\boldsymbol{p} = J\Delta\boldsymbol{\theta}
$$
とすることが出来る．これを解くことで，
$$
\Delta\boldsymbol{\theta} = J^{-1}\Delta\boldsymbol{p}
$$
が得られる．

つまり動きが小さければ任意の場所（方向）を狙って動かすことができるということである．

これを微小時間で考えると，
$$
\boldsymbol{\omega} = \dfrac{d\boldsymbol{\theta}}{dt} = J^{-1}\dfrac{d\boldsymbol{p}}{dt} = J^{-1}\boldsymbol{v}
$$
このようにヤコビ行列の逆行列で手先での任意の速度を関節角の角速度に分解して制御する方法を分解運動速度制御と呼ぶ．

==== 逆運動学の数値解法 ====
[{{ articles:jacobian_matrix_06.png?200|図6 逆運動学を解く}}]
図6のように目標位置$\boldsymbol{p}_T$が与えられたら関節角はどうなるか？

これは逆運動学の問題である．これはヤコビ行列の逆行列を利用して以下のように解くことが出来る．

  - まず適当な関節角$\boldsymbol{\theta}_0 = \left( \begin{array}{c} \theta_{0,1} \\ \theta_{0,2} \end{array}\right)$を開始状態とする．
  - $\boldsymbol{\theta}_i$の順運動学解$\boldsymbol{p}_i$を求める．
  - $\boldsymbol{p}_i$が$\boldsymbol{p}_T$に十分一致していれば終了．\\ ある程度十分に近ければその差を$\Delta\boldsymbol{p}_i$とし，離れている場合はその方向の微小ステップを$\Delta\boldsymbol{p}_i$とする．
  - $\Delta\boldsymbol{\theta}_i=J^{-1}\Delta\boldsymbol{p}_i$を計算し，$\boldsymbol{\theta}_{i+1} = \boldsymbol{\theta}_i + \Delta\boldsymbol{\theta}_i$とする．
  - 終了するまで2から4を繰り返す．

終了時点での$\boldsymbol{\theta}_i$が$\boldsymbol{p}_T$の逆運動学解の数値解$\boldsymbol{\theta}_T$となっている．

このように順運動学とヤコビ行列があれば逆運動学を数値的に解くことができる
((このヤコビ行列を用いた逆運動学の数値解法は先に述べた分解運動速度制御を連続させたものと同等である．
つまり，分解運動速度制御を適応すると逆運動学を明示的に解くことなく手先の位置を任意の場所に制御できるということになる．))．

===== ３自由度アームのヤコビ行列（書き下しの式からの導出） =====

以下の[[articles:forward_kinematics#３自由度アームの順運動学]]の式を素直に偏微分する．
$$
{^0x_{wrist}} = (l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3))\mathrm{cos}\,\theta_1
$$
$$
{^0y_{wrist}} = (l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3))\mathrm{sin}\,\theta_1
$$
$$
^0z_{wrist} = l_b+l_1+l_2 \mathrm{cos}\, \theta_2 + l_3 \mathrm{cos}(\theta_2 + \theta_3)
$$

$x$について計算すると，
$$
\dfrac{\partial x}{\partial\theta_1} = -(l_2\mathrm{sin}\,\theta_2 + l_3\mathrm{sin}(\theta_2+\theta_3))\mathrm{sin}\,\theta_1
$$
$$
\dfrac{\partial x}{\partial\theta_2} = (l_2\mathrm{cos}\,\theta_2 + l_3\mathrm{cos}(\theta_2+\theta_3))\mathrm{cos}\,\theta_1
$$
$$
\dfrac{\partial x}{\partial\theta_3} =l_3\mathrm{cos}(\theta_2+\theta_3)\,\mathrm{cos}\,\theta_1
$$

$y$について計算すると，
$$
\dfrac{\partial y}{\partial\theta_1} = (l_2\mathrm{sin}\,\theta_2 + l_3\mathrm{sin}(\theta_2+\theta_3))\mathrm{cos}\,\theta_1
$$
$$
\dfrac{\partial y}{\partial\theta_2} = (l_2\mathrm{cos}\,\theta_2 + l_3\mathrm{cos}(\theta_2+\theta_3))\mathrm{sin}\,\theta_1
$$
$$
\dfrac{\partial y}{\partial\theta_3} =l_3\mathrm{cos}(\theta_2+\theta_3)\,\mathrm{sin}\,\theta_1
$$
$z$について計算すると，
$$
\dfrac{\partial z}{\partial\theta_1} = 0
$$
$$
\dfrac{\partial z}{\partial\theta_2} = -l_2\mathrm{sin}\,\theta_2 - l_3\mathrm{sin}(\theta_2+\theta_3)
$$
$$
\dfrac{\partial z}{\partial\theta_3} = - l_3\mathrm{sin}(\theta_2+\theta_3)
$$
これをまとめると，

$$
J = \left( \begin{array}{cc}
-(l_2\mathrm{sin}\,\theta_2 + l_3\mathrm{sin}(\theta_2+\theta_3))\mathrm{sin}\,\theta_1 &
(l_2\mathrm{cos}\,\theta_2 + l_3\mathrm{cos}(\theta_2+\theta_3))\mathrm{cos}\,\theta_1 &
l_3\mathrm{cos}(\theta_2+\theta_3)\,\mathrm{cos}\,\theta_1 \\
(l_2\mathrm{sin}\,\theta_2 + l_3\mathrm{sin}(\theta_2+\theta_3))\mathrm{cos}\,\theta_1 &
(l_2\mathrm{cos}\,\theta_2 + l_3\mathrm{cos}(\theta_2+\theta_3))\mathrm{sin}\,\theta_1 &
l_3\mathrm{cos}(\theta_2+\theta_3)\,\mathrm{sin}\,\theta_1 \\
0 &
-l_2\mathrm{sin}\,\theta_2 - l_3\mathrm{sin}(\theta_2+\theta_3) &
- l_3\mathrm{sin}(\theta_2+\theta_3)
\end{array}\right)
$$
このヤコビ行列が何を表しているのか？
この式のままでは[[#原理（2リンクアーム）]]の図で説明したような関節の微小変位と手先の微小変位の関係という
直感的なイメージはわきにくい．

次の節では座標変換を用いた順運動学を微分することで，直感的なイメージと式との対応付けを試みる．
===== ３自由度アームのヤコビ行列（座標変換からの導出） =====

[[articles:forward_kinematics#座標変換を使う|座標変換を使った3自由度アームの順運動学]]の式は，
$$
^0T_{wrist}(\theta_1,\, \theta_2, \, \theta_3)
= {^0T_1}(\theta_1){^1T_2}(\theta_2){^2T_3}(\theta_3){^3T_{wrist}}
$$
となる．これを手首位置だけを問題にしていたので手首位置だけの式に書き換える．
$$
^0\boldsymbol{p}_{wrist}(\theta_1,\, \theta_2, \, \theta_3)
= {^0T_1}(\theta_1){^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}} \tag{1}
$$
ここで
$$
^3\boldsymbol{p}_{wrist} = \left( \begin{array}{c} 0 \\ 0 \\ l_3 \\ 1 \end{array} \right)
$$
$$
^{0}T_{1} 
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_1 & - \mathrm{sin}\, \theta_1 & 0  & 0\\
\mathrm{sin}\, \theta_1 & \mathrm{cos}\, \theta_1 & 0 & 0 \\
0 & 0 & 1 & l_b \\
0 & 0 & 0 & 1
\end{array} \right) \tag{2}
$$
$$
^{1}T_{2}
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_2 & 0 & \mathrm{sin}\, \theta_2 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_2 & 0 & \mathrm{cos}\, \theta_2 & l_1 \\

0 & 0 & 0 & 1
\end{array} \right)
$$
$$
^{2}T_{3}
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_3 & 0 & \mathrm{sin}\, \theta_3 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_3 & 0 & \mathrm{cos}\, \theta_3 & l_2 \\

0 & 0 & 0 & 1
\end{array} \right)
$$
となる．最後にリンク3から見た手首座標系への変換は，
$$
^{3}T_{wrist}
=
\left( \begin{array}{cc} 
1 & 0 & 0  & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & l_3 \\
0 & 0 & 0 & 1
\end{array} \right)
$$
であるが今回はこの行列ではなく位置ベクトルだけを使うことにする
((
3自由度アームの場合，位置と姿勢を独立に決めることが出来ないので位置だけを問題にする．
))．

==== $\theta_1$による偏微分 ====

式(1)を$\theta_1$で偏微分すると$\theta_1$にかかわる部分は第1項なので，
$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\theta_1}}
= \dfrac{\partial{^0T_1}}{\partial{\theta_1}} {^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}}
$$
式(2)を$\theta_1$について偏微分すると，
$$
 \dfrac{\partial{^0T_1}}{\partial{\theta_1}}
=
\left( \begin{array}{cc} 
-\mathrm{sin}\, \theta_1 & - \mathrm{cos}\, \theta_1 & 0  & 0\\
\mathrm{cos}\, \theta_1 & -\mathrm{sin}\, \theta_1 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right) 
$$
[[articles:forward_kinematics#座標変換を使わない方法との比較|座標変換を使った3自由度アームの順運動学]]で計算したように，
$$
^1\boldsymbol{p}_{wrist} =
{^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}} 
=
\left( \begin{array}{c} 
l_3\mathrm{sin}(\theta_2 + \theta_3) + l_2\mathrm{sin}\,\theta_2 \\ 
0 \\ 
l_3\mathrm{cos}(\theta_2 + \theta_3) + l_2\mathrm{cos}\,\theta_2 + l_1 \\ 
1 \end{array} \right)
$$
よって，
$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\theta_1}}

=
\dfrac{\partial{^0T_1}}{\partial{\theta_1}}{^1\boldsymbol{p}_{wrist}}
=
\left( \begin{array}{c} 
-(l_2\mathrm{sin}\,\theta_2 + l_3\mathrm{sin}(\theta_2+\theta_3))\mathrm{sin}\,\theta_1 \\ 
(l_2\mathrm{sin}\,\theta_2 + l_3\mathrm{sin}(\theta_2+\theta_3))\mathrm{cos}\,\theta_1 \\ 
0 \\
0 \end{array} \right)
$$
当然ではあるが，これは順運動学の書き下しの式から求めたものと同じ結果となっていることがわかる．

==== ヤコビ行列の直感的意味 ====

式(2)を$\theta_1$について偏微分した式を改めてよく見てみると以下のようになることがわかる．
$$
 \dfrac{\partial{^0T_1}}{\partial{\theta_1}}
=
\left( \begin{array}{cc} 
-\mathrm{sin}\, \theta_1 & - \mathrm{cos}\, \theta_1 & 0  & 0\\
\mathrm{cos}\, \theta_1 & -\mathrm{sin}\, \theta_1 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right) 
$$
$$
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_1 & - \mathrm{sin}\, \theta_1 & 0  & 0\\
\mathrm{sin}\, \theta_1 & \mathrm{cos}\, \theta_1 & 0 & 0 \\
0 & 0 & 1 & l_b \\
0 & 0 & 0 & 1
\end{array} \right)

\left( \begin{array}{cc} 
0 & -1 & 0  & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right)
=
^{0}T_{1} 
\left( \begin{array}{cc} 
0 & -1 & 0  & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right)
$$
この
$$
\left( \begin{array}{cc} 
0 & -1 & 0  & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right)
$$
はz軸周りの回転の微分を表す行列となっている．これをベクトルに掛けると
$$
\left( \begin{array}{cc} 
0 & -1 & 0  & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right)
\left( \begin{array}{c} x \\ y \\ z \\ 0 \end{array} \right)
=
\left( \begin{array}{c} -y \\ x \\ 0 \\ 0 \end{array} \right)
$$
であり，これは3次元ベクトルとして見た$z$軸ベクトルとの外積と同じである．
$$
\left( \begin{array}{c} 0 \\ 0 \\ 1 \\0 \end{array} \right) \times
\left( \begin{array}{c} x \\ y \\ z \\0 \end{array} \right)
=
\left( \begin{array}{c} -y \\ x \\ 0 \\0 \end{array} \right)
$$
これを踏まえて式の展開を見直すと
$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\theta_1}}
= \dfrac{\partial{^0T_1}}{\partial{\theta_1}} {^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}}
$$

$$
= ^{0}T_{1} 
\left( \begin{array}{cc} 
0 & -1 & 0  & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right)

{^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}}
$$
$$
= ^{0}T_{1} 
\left( \begin{array}{cc} 
0 & -1 & 0  & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array} \right)
{^1\boldsymbol{p}_{wrist}}
$$
$$
= ^{0}T_{1} 
\left(
\left( \begin{array}{c} 0 \\ 0 \\ 1 \\0 \end{array} \right) \times
{^1\boldsymbol{p}_{wrist}}
\right)
$$
これはリンク1からみたリンク1の回転軸$\boldsymbol{z}$と
リンク1から手先までの腕${^1\boldsymbol{p}_{wrist}}$との外積を$^0T_1$でベースから見たものに変換していることが分かる．
この結果はそれぞれ先に${^{0}T_{1}}$による座標変換を行いベースから見たリンク1の回転軸
$$
^0\boldsymbol{r}_1 = {^{0}T_{1}}\left( \begin{array}{c} 0 \\ 0 \\ 1 \\0 \end{array} \right)
$$
とリンク1から手先までの腕
$$
^0\boldsymbol{l}_{wrist-1} = {^{0}T_{1}}{^1\boldsymbol{p}_{wrist}}
$$
との外積と同じものになる．すなわち，
$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\theta_1}} 
= \left(
{^{0}T_{1}}\left( \begin{array}{c} 0 \\ 0 \\ 1 \\0 \end{array} \right) \times
{^{0}T_{1}}{^1\boldsymbol{p}_{wrist}}
\right)
= {^0\boldsymbol{r}_1} \times {^0\boldsymbol{l}_{wrist-1}}　\tag{3}
$$

[{{ articles:jacobian_matrix_08.png?200| 図7 回転による腕の先端の移動}}]
これは腕を回転軸周りに回転させたときその先端がどのように動くかということを表しており，直感的にも理解しやすいものになっている（図7）．

==== $\theta_2$，$\theta_3$による偏微分 ====
[{{ articles:jacobian_matrix_07.png?200| 図8 ヤコビ行列の直感的意味}}]

$\theta_2$，$\theta_3$の回転軸は$y$軸なので，偏微分を求めると以下のようになる．

$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\theta_2}}
= {^0T_1}(\theta_1)\dfrac{\partial{^1T_2}(\theta_2)}{\partial{\theta_2}} {^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}}
$$
$$
= {^{0}T_{1}(\theta_1) } {^{1}T_{2}(\theta_2) }
\left(
\left( \begin{array}{c} 0 \\ 1 \\ 0 \\0 \end{array} \right) \times
{^2\boldsymbol{p}_{wrist}}
\right)
= {^0\boldsymbol{r}_2} \times {^0\boldsymbol{l}_{wrist-2}} \tag{4}
$$

$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\theta_3}}
= {^0T_1}(\theta_1){^1T_2}(\theta_2)\dfrac{\partial{^2T_3}(\theta_3)}{\partial{\theta_3}} {^3\boldsymbol{p}_{wrist}}
$$
$$
= {^{0}T_{1}(\theta_1) } {^{1}T_{2}(\theta_2) }{^2T_3}(\theta_3)
\left(
\left( \begin{array}{c} 0 \\ 1 \\ 0 \\0 \end{array} \right) \times
{^3\boldsymbol{p}_{wrist}}
\right)
= {^0\boldsymbol{r}_3} \times {^0\boldsymbol{l}_{wrist-3}} \tag{5}
$$
これらの結果は図8に示すようにヤコビ行列の直感的意味との整合性が明確なものになっている．
==== 座標変換からの導出のまとめ ====
ヤコビ行列を座標変換を用いた順運動学から導出するのは展開された順運動学からの導出と比べて
その直感的意味も把握しやすくなっている．

3自由度アームの逆運動学の座標変換からの導出をまとめると以下のようになる．

$$
\dfrac{\partial{^0\boldsymbol{p}_{wrist}}}{\partial{\boldsymbol\theta}}
= \left( \begin{array}{c}
{^0\boldsymbol{r}_1} \times {^0\boldsymbol{l}_{wrist-1}} &
{^0\boldsymbol{r}_2} \times {^0\boldsymbol{l}_{wrist-2}} &
{^0\boldsymbol{r}_3} \times {^0\boldsymbol{l}_{wrist-3}}
\end{array}\right)
$$

このヤコビ行列を求める場合は数式としての順運動学の展開は不要であり，
式(3),(4),(5)のベクトルや行列の演算を行うことで具体的な
数値を求めることが出来る．

座標変換を用いた順運動学からヤコビ行列を求めるには，個々の関節パラメタを含んだ座標変換行列の微分を考えればよい．
座標変換行列の微分は，座標変換行列自身と微分を表す行列との積で表現できる．
その微分を表す行列は，ベクトルとの演算を考えることでよりシンプルで直感的な表現を与えてくれる．

===== 種々の関節構造のヤコビ行列 =====

座標変換を用いた順運動学からヤコビ行列を求めたことにより，ヤコビ行列の各列は対応する関節の
微小変位による手先の座標系の微小変位を表しているという当然の事実を式の上からも確認することができた．


具体的には，手先座標系原点の回転関節パラメタによる偏微分は，
手先に位置を$\boldsymbol{p}$，関節回転軸を$\boldsymbol{r}$，
関節軸から手先までの腕の長さを$\boldsymbol{l}$とすると
$$
\dfrac{\partial{\boldsymbol{p}}}{\partial{\theta}} 
= {\boldsymbol{r}} \times {\boldsymbol{l}}　
$$
となるという具合である．

ここではこの直感を広げて手先の座標系の位置および姿勢の関節パラメタ（回転，並進）による偏微分係数を求め，
ヤコビ行列を導出する．式の展開は行わないが，それは座標変換を用いた順運動学を関節パラメタで微分することで得られる．

実際的には，行列での計算を行うよりは，回転軸や腕の長さなどの必要なパラメタを座標計算で求める方が
シンプルで分かりやすい．

==== 回転関節の偏微分係数 ====

[{{ articles:jacobian_matrix_09.png?400| 図9 回転関節と手先の座標回転との関係}}]
手先位置に関する偏微分係数はすでに求めてあるのでここでは
図9の回転関節による手先の座標回転に対する偏微分係数を求める．

手先座標系の回転を回転角度に比例した大きさの回転軸ベクトル$\boldsymbol{\phi}$で表すと
座標系の位置の変化は回転軸からの距離と関係があるが，姿勢の変化は回転軸からの距離とは無関係であり
その回転が直接手先の座標の回転となる．
すなわち，
$$
\boldsymbol{\phi} = \boldsymbol{r}\theta
$$
よって
$$
\dfrac{\partial{\boldsymbol{\phi}}}{\partial{\theta}} = \boldsymbol{r}
$$
一般に，回転はベクトルのようには足せない（加法の交換法則が成り立たない）．
しかし，微小変化を考えたときはベクトルとして足すことが出来る．
よってその極限である微分係数に個々の関節角の微小量や関節角速度を乗じたものは
それらを足し合わせることで手先での微小回転や回転角速度とすることが出来る．

==== 並進関節の偏微分係数 ====
[{{ articles:jacobian_matrix_10.png?400| 図10 並進関節と手先の回転，並進の関係}}]

次に図9の並進関節に関する偏微分係数を考える．

関節の並進軸を$\boldsymbol{r}$として並進移動量を$q$とすると，
並進移動がそのまま手先位置の移動量となる．よって，
$$
\boldsymbol{p} = \boldsymbol{r}q
$$
これを$q$で微分すると，
$$
\dfrac{\partial{\boldsymbol{p}}}{\partial{q}}=\boldsymbol{r}
$$
また関節の並進移動では手先の姿勢は変化しないので，
$$
\dfrac{\partial{\boldsymbol{\phi}}}{\partial{q}} = \boldsymbol{0}
$$

==== ｎ自由度回転関節のヤコビ行列 ====
まずは単純なｎ個の回転関節で構成されたヤコビアンを考える．
表示をシンプルにするためにベース座標系から見た各リンクの回転軸を$\boldsymbol{r}_i$,
各リンクから手先座標系原点までの腕を$\boldsymbol{l}_i$とするとヤコビ行列は
$$
J = \left( \begin{array}{c} 
           \dfrac{\partial{\boldsymbol{p}}}{\partial{\boldsymbol{\theta}}} \\
           \dfrac{\partial{\boldsymbol{\phi}}}{\partial{\boldsymbol{\theta}}}
    \end{array} \right)
=
\left( \begin{array}{cc}
     \boldsymbol{r}_1 \times \boldsymbol{l}_1 & \boldsymbol{r}_2 \times \boldsymbol{l}_2 &
     ... & \boldsymbol{r}_n \times \boldsymbol{l}_n \\
     \boldsymbol{r}_1 & \boldsymbol{r}_2 & ... & \boldsymbol{r}_n
\end{array} \right)
$$
これを使って並進と回転の微小量を表すと，
$$
\left( \begin{array}{c} \Delta\boldsymbol{p} \\ \Delta\boldsymbol{\phi} \end{array} \right)
= J\Delta\boldsymbol{\theta}
$$
また，速度，角速度を表すと，
$$
\left( \begin{array}{c} \boldsymbol{v} \\ \boldsymbol{\omega} \end{array} \right)
=
\left( \begin{array}{c}
\dfrac{d\boldsymbol{p}}{d t} \\
\dfrac{d\boldsymbol{\phi}}{dt}
\end{array} \right)
= J\dfrac{d\boldsymbol{\theta}}{dt}
= J\dot{\boldsymbol{\theta}}
$$
となる．

==== 並進，回転関節のヤコビ行列 ====
一般的な並進，回転関節が混在しているロボットアームを考える．

並進，回転の関節パラメタをまとめて
$\boldsymbol{q} = \left( \begin{array}{c} q_1 & q_2 & ... & q_n \end{array} \right)^T$,
手先座標の変位もまとめて
$\boldsymbol{p} = \left( \begin{array}{c} p_x & p_y & p_z & \phi_x & \phi_y & \phi_z \end{array} \right)^T$
とするとヤコビ行列は，
$$
J = \left( \dfrac{\partial{\boldsymbol{p}}}{\partial{\boldsymbol{q}}} \right) 
= \left( \begin{array}{cc} 
     ... & \dfrac{\partial{\boldsymbol{p}}}{\partial{q_i}} & ... \\
     ... & \dfrac{\partial{\boldsymbol{\phi}}}{\partial{q_i}} & ... 
  \end{array} \right)　\tag{6}
$$
ここで，
$$
\dfrac{\partial{\boldsymbol{p}}}{\partial{q_i}} 
= \left\{ \begin{array}{cc} 
     \boldsymbol{r}_i  & (\mathrm{translation})\\
     \boldsymbol{r}_i \times \boldsymbol{l}_i & (\mathrm{rotational})
   \end{array} \right.

$$
$$
\dfrac{\partial{\boldsymbol{\phi}}}{\partial{q_i}} 
= \left\{ \begin{array}{cc} 
     \boldsymbol{0}  & (\mathrm{translation})\\
     \boldsymbol{r}_i & (\mathrm{rotational})
   \end{array} \right.

$$
となる．

==== ヤコビ行列の計算方法 ====
[{{ articles:jacobian_matrix_11.png?450| 図10 ヤコビ行列の計算手順}}]

図10にヤコビ行列の計算手順を示す．

図の赤矢印のように手先側からベースに向かって順運動学の行列計算をしていく．
その過程で各リンク$i$において，回転軸（または並進軸）ベクトル$^i\boldsymbol{r}_i$と
そこから手先までの腕ベクトル$^i\boldsymbol{l}_i$を抽出する．
抽出されたベクトルはベースに向かってベクトルとしての座標変換（姿勢のみの変換）を行っていく．

ベースまで計算が完了すると，順運動学$^0T_{hand}$と
ヤコビ行列の計算に必要な$^0\boldsymbol{l}_i$，$^0\boldsymbol{r}_i$とが得られる．

これを使って回転／並進を考慮しながら式(6)を計算するとヤコビ行列が得られる．