実際に使える制御工学：ロボット編（３）「ヤコビアン。関節角速度をアーム先端の速度に変える方法。」

＜この記事で伝えたい事＞ 

• ロボット分野におけるヤコビアンは、関節角速度からアーム先端の速度を計算する方法です
  ヤコビアンは、ヤコビ行列とも言います。
  ロボット分野で「ヤコビアン」と言えば、基本的に「関節角速度をアーム先端速度に変換する行列」を意味します。
  （本来の、数学における「ヤコビアン」の意味は、また少し違います）
  
  

• ヤコビアンは静力学解析にも使われます
  静力学とは、動かない物体にかかる力やトルクを扱う、力学の１種です。
  （厳密な定義は、ちょっと違います）
  
  ロボット分野における静力学解析の代表例は、ロボットとワーク（加工や搬送の対象となるモノ）との間にはたらく接触力の解析です。

1. はじめに

自己紹介はこちらです。

この記事は、主にロボット制御に関わる学生や若手～中堅エンジニアに向けた技術紹介・解説記事シリーズの第３弾です。
 
今回は、ロボットのヤコビアン（ヤコビ行列）について紹介・解説します。

2. ２リンクマニピュレータのヤコビアン

下図は、具体例の２リンクマニピュレータです。

この２リンクマニピュレータのアーム先端位置は、以下のようになります。

$$
\left\{
\begin{array}{ll}
X&=L_1\cdot C\theta_1+L_2\cdot C\theta_{12}\\
Y&=L_1\cdot S\theta_1+L_2\cdot S\theta_{12}
\end{array}
\right.
$$

$$
\left(
\begin{array}{lll}
S\theta_{1}=sin\theta_1&,&S\theta_{12}=sin(\theta_1+\theta_2)\\
C\theta_{1}=cos\theta_1&,&C\theta_{12}=cos(\theta_1+\theta_2)
\end{array}
\right)
$$

（当たり前といえばそうですが）
位置を時間微分すれば、速度を求められます。
アーム先端位置の両辺を時間微分すると、以下のようになります。

$$
\begin{array}{ll}
\dot{X}&=-\dot{\theta}_1\cdot L_1\cdot S\theta_1-(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot S\theta_{12}\\
&=-\dot{\theta}_1\cdot (L_1\cdot S\theta_1+L_2\cdot S\theta_{12})-\dot{\theta}_2\cdot L_2\cdot S\theta_{12}
\end{array}
$$

$$
\begin{array}{ll}
\dot{Y}&=\dot{\theta}_1\cdot L_1\cdot C\theta_1+(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot C\theta_{12}\\
&=-\dot{\theta}_1\cdot (L_1\cdot C\theta_1+L_2\cdot C\theta_{12})+\dot{\theta}_2\cdot L_2\cdot C\theta_{12}
\end{array}
$$

これらを行列形式でまとめます。

$$
\begin{array}{ll}
\left[
\begin{array}{c}
\dot{X}\\
\dot{Y}
\end{array}
\right]&=
\left[
\begin{array}{cc}
-(L_1\cdot S\theta_1+L_2\cdot S\theta_{12})&-L_2\cdot S\theta_{12}\\
L_1\cdot C\theta_1+L_2\cdot C\theta_{12}&L_2\cdot C\theta_{12}
\end{array}
\right]
\left[
\begin{array}{c}
\dot{\theta}_1\\
\dot{\theta}_2
\end{array}
\right]\\
&=J_{(\theta)}
\left[
\begin{array}{c}
\dot{\theta}_1\\
\dot{\theta}_2
\end{array}
\right]
\end{array}
$$

上式中の行列$${J_{(θ)}}$$がヤコビアンです。

このように、２リンクマニピュレータくらいシンプルな機構なら、ヤコビアンを求めるのは難しくありません。

また、以下のように、$${J_{(θ)}}$$の逆行列を用いて、アーム先端速度に応じた関節角速度を計算することもできます。

$$
\begin{array}{ll}
\left[
\begin{array}{c}
\dot{\theta}_1\\
\dot{\theta}_2
\end{array}
\right]&=
{J_{(\theta)}}^{-1}
\left[
\begin{array}{c}
\dot{X}\\
\dot{Y}
\end{array}
\right]\\
&=\cfrac{
\left[
\begin{array}{cc}
L_2\cdot C\theta_{12}&L_2\cdot S\theta_{12}\\
-(L_1\cdot C\theta_1 +L_2\cdot C\theta_{12})&-(L_1\cdot S\theta_1+L_2\cdot S\theta_{12})
\end{array}
\right]
\left[
\begin{array}{c}
\dot{X}\\
\dot{Y}
\end{array}
\right]
}{-(L_1\cdot S\theta_1+L_2\cdot S\theta_{12})\cdot L_2\cdot C\theta_{12}+(L_1\cdot C\theta_1+L_2\cdot C\theta_{12})\cdot L_2\cdot S\theta_{12}}\\
&=\cfrac{
\left[
\begin{array}{cc}
L_2\cdot C\theta_{12}&L_2\cdot S\theta_{12}\\
-(L_1\cdot C\theta_1 +L_2\cdot C\theta_{12})&-(L_1\cdot S\theta_1+L_2\cdot S\theta_{12})
\end{array}
\right]
\left[
\begin{array}{c}
\dot{X}\\
\dot{Y}
\end{array}
\right]
}{-L_1\cdot L_2\cdot C\theta_{12}\cdot S\theta_1-{L_2}^2\cdot S\theta_{12}\cdot C\theta_{12}+ L_1\cdot L_{12}\cdot S\theta_{12}\cdot C\theta_1+{L_2}^2\cdot S\theta_{12}\cdot C\theta_{12}}\\
&=\cfrac{
\left[
\begin{array}{cc}
L_2\cdot C\theta_{12}&L_2\cdot S\theta_{12}\\
-(L_1\cdot C\theta_1 +L_2\cdot C\theta_{12})&-(L_1\cdot S\theta_1+L_2\cdot S\theta_{12})
\end{array}
\right]
\left[
\begin{array}{c}
\dot{X}\\
\dot{Y}
\end{array}
\right]
}{L_1\cdot L_2\cdot (S\theta_{12}\cdot C\theta_1-C\theta_{12}\cdot S\theta_1)}\\
&=\cfrac{
\left[
\begin{array}{cc}
L_2\cdot C\theta_{12}&L_2\cdot S\theta_{12}\\
-(L_1\cdot C\theta_1 +L_2\cdot C\theta_{12})&-(L_1\cdot S\theta_1+L_2\cdot S\theta_{12})
\end{array}
\right]
\left[
\begin{array}{c}
\dot{X}\\
\dot{Y}
\end{array}
\right]
}{L_1\cdot L_2\cdot sin\theta_{2}}
\end{array}
$$

勘の良い方は気付いたでしょう。

$${\theta_2=0_{[rad]}}$$または$${\theta_2=\pm \pi_{[rad]}}$$のとき、$${sin\theta_2=0}$$になって、解が得られません。

２リンクマニピュレータにとって、$${\theta_2=0_{[rad]}}$$または$${\theta_2=\pm \pi_{[rad]}}$$は特異点と呼ばれます。

物理的には、アームを伸ばしきった状態であり、それ以上アームを伸ばす方向に動けない（＝速度を出せない）状態と解釈できます。

特異点は、「アームを伸ばしきった状態」の他にもありますが、詳細は5章で説明します。

3. ヤコビアン一般解の求め方

前章で述べたとおり、２リンクマニピュレータくらいなら、手計算でヤコビアンを求められます。

しかし、６軸や７軸の垂直多関節ロボットでそれをやるのは、計算量が多く大変です。
 
そうした複雑な機構のヤコビアンを求めるのに、便利な方法があります。
それは、DH法・同次変換行列と組み合わせて、ヤコビアンの一般解を得る方法です。

3.1 基本イメージの解説

まずは、最もシンプルな例から説明しましょう。

DH法では、関節回転方向は必ず$${Z}$$軸まわりです。
（直動関節の場合は、$${Z}$$軸が動作方向）

また、リンクは必ず$${X}$$軸方向か$${Z}$$軸方向に長さを持ちます。
今は、$${X}$$軸方向にのみ長さを持つとしましょう。

下図は、以上の条件を図で表したものです。

この図を見れば、アーム先端速度が$${v=\omega\cdot L}$$になると、直感的に分かると思います。
その直感は正しいです。

これを言語で表現するなら、
「$${X}$$軸方向長$${L}$$のアーム」があり、
「$${Z}$$軸まわりに角速度$${\omega}$$で回転」したとき、
「アーム先端には$${Y}$$軸方向に$${\omega\cdot L}$$の速度が発生」
となります。

そう、アーム長方向・回転軸方向・アーム先端速度方向は、全てが互いに直交します。
そして、アーム先端速度の大きさは、角速度とリンク長の掛け算です。
（直交：垂直に交わること）

こんなとき使えるのが、ベクトルの外積計算です。
$${X,Y,Z}$$方向の３次元ベクトル同士の外積は、以下のように計算できます。
（$${\times}$$は外積計算符号、外積はただの掛け算と違って順序が大事です）

$$
\left[
\begin{array}{c}
x_a\\
y_a\\
z_a
\end{array}
\right]
\times
\left[
\begin{array}{c}
x_b\\
y_b\\
z_b
\end{array}
\right]
=
\left[
\begin{array}{c}
y_a\cdot z_b-z_a\cdot y_b\\
z_a\cdot x_b-x_a\cdot z_b\\
x_a\cdot y_b-y_a\cdot x_b
\end{array}
\right]
$$

これに角速度ベクトル$${\bm{\omega}}$$、距離ベクトル$${\bm{l}}$$、アーム先端速度ベクトル$${\bm{v}}$$を当てはめると、下式のようになります。

$$
\begin{array}{ll}
\bm{v}&=\bm{\omega}\times\bm{l}\\
&=
\left[
\begin{array}{c}
0\\0\\ \omega
\end{array}
\right]
\times
\left[
\begin{array}{c}
L\\0\\0
\end{array}
\right]\\
&=\left[
\begin{array}{c}
0\\ \omega\cdot L\\0
\end{array}
\right]\\
&=\left[
\begin{array}{c}
0\\L\\0
\end{array}
\right]\cdot \omega
\end{array}
$$

計算上も、$${Y}$$軸方向にだけ$${\omega\cdot L}$$の速度が発生するのが分かります。
 
また、角速度$${\omega}$$を行列の外に持っていったとき、行列$${[0　L　0]^T}$$がヤコビアンとみなせます。
（添え字$${^T}$$は転置行列）

次に、「リンク角度がゼロでない場合はどうなるか？」を説明します。

下図は、リンクが角度$${\theta}$$を持った状態での回転のイメージです。
（上から見下ろすとイメージしやすいので、そうしています）

上図において、$${Z}$$軸まわりに回転することは、角度がゼロのときと変わりません。
このため、角速度ベクトル$${\bm{\omega}}$$は下式のままです。

$$
\bm{\omega}=
\left[
\begin{array}{c}
0\\0\\ \omega
\end{array}
\right]
$$

角度$${\theta}$$を持つときと角度ゼロのときとの違いは、距離ベクトル$${\bm{l}}$$です。

角度$${\theta}$$を持つことによって、距離ベクトル$${\bm{l}}$$は下式に変わります。
（$${\theta=0}$$のとき$${cos\theta=1,sin\theta=0}$$なので、角度ゼロのときの距離ベクトルと一致します）

$$
\bm{l}=
\left[
\begin{array}{c}
L\cdot cos\theta\\
L\cdot sin\theta\\
0
\end{array}
\right]
$$

これら２つのベクトルの外積計算によって、速度ベクトル$${\bm{v}}$$は以下のように求められます。

$$
\begin{array}{ll}
\bm{v}&=\bm{\omega}\times\bm{l}\\
&=
\left[
\begin{array}{c}
0\\0\\ \omega
\end{array}
\right]
\times
\left[
\begin{array}{c}
L\cdot cos\theta\\
L\cdot sin\theta\\
0
\end{array}
\right]\\
&=
\left[
\begin{array}{c}
-\omega\cdot L\cdot sin\theta\\
\omega\cdot L\cdot cos\theta\\
0
\end{array}
\right]\\
&=
\left[
\begin{array}{c}
-L\cdot sin\theta\\
L\cdot cos\theta\\
0
\end{array}
\right]\cdot\omega
\end{array}
$$

外積計算結果が図中の$${X}$$方向速度・$${Y}$$方向速度と一致することから、
「角速度ベクトルと距離ベクトルの外積計算によって、速度ベクトルが求められる」
のは、角度$${\theta}$$を持つ場合でも成り立つと分かってもらえると思います。

また、角速度$${\omega}$$を行列の外に持っていったときの行列$${[-L\cdot cos\theta　L\cdot sin\theta　0]^T}$$がヤコビアンとみなせます。
（添え字$${^T}$$は転置行列）

3.2 具体的な求め方

ここまでの説明で、３次元ベクトルとベクトルの外積計算をうまく使えば、ヤコビアンの一般解が得られそうと思ってもらえたのではないでしょうか？
（なんとなくでも）

ここからは、ヤコビアン一般解の具体的な求め方を説明します。

「角速度ベクトルと距離ベクトルの外積計算によって、速度ベクトルが求められる」
こと自体は変わりません。

ただ、多関節ロボットの場合、角速度ベクトルと距離ベクトルの求め方に一工夫が必要です。
 
DH法でリンク機構を記述する限り、その関節を基準とした座標系では、$${Z}$$軸まわりが関節（モータ）の回転方向です。

しかし、ベースを基準としてみると、必ずしも関節回転がアーム先端に$${Z}$$軸まわりの回転を与えるとは限りません。

ベース座標基準で、多関節ロボットの第$${n}$$関節がアーム先端にどの方向の回転をさせるかは、回転行列$${^0R_n}$$から分かります。
（回転行列の詳細については、ロボット編（２）を参照）

ベースからみた第$${n}$$関節の回転行列$${^0R_n}$$は下式で表せます。

$$
^0R_n=
\left[
\begin{array}{ccc}
N_{xn}&O_{xn}&A_{xn}\\
N_{yn}&O_{yn}&A_{yn}\\
N_{zn}&O_{zn}&A_{zn}
\end{array}
\right]
$$

ここで着目すべきは、回転行列の３列目$${A_{xn},A_{yn},A_{zn}}$$です。

この３要素から成るベクトル$${[A_{xn}　A_{yn}　A_{zn}]^T}$$が、ベースからみて第$${n}$$関節の$${Z}$$軸がどこを向いているかを表します。

その関節基準では、関節の回転方向は必ず$${Z}$$軸まわりです。

このため、回転行列の３列目ベクトルに第$${n}$$関節角速度$${\omega_n}$$をかけたものが、ベース座標基準での第$${n}$$関節の角速度ベクトル$${\bm{\omega_n}}$$になります。

$$
\bm{\omega_n}
=
\left[
\begin{array}{c}
A_{xn}\\A_{yn}\\A_{zn}
\end{array}
\right]\cdot\omega_n
$$

また、ロボットの第$${n}$$関節からアーム先端までの距離ベクトルは、位置ベクトルから求められます。

同次変換行列を使ったキネマティクス計算をよく見ると分かりますが、ベースからアーム先端までの位置ベクトル$${^0p_f}$$は、各関節間の距離ベクトルを足し合わせていったものです。
（下式は６軸ロボット想定）

$$
\begin{array}{ll}
^0p_f&={^0R_6}{^6p_f}+{^0R_5}{^5p_6}+{^0R_4}{^4p_5}+{^0R_3}{^3p_4}+{^0R_2}{^2p_3}+{^0R_1}{^1p_2}+{^0p_1}\\
\\
&={^0R_6}{^6p_f}+{^0R_5}{^5p_6}+{^0R_4}{^4p_5}+{^0R_3}{^3p_4}+{^0R_2}{^2p_3}+{^0p_2}\\
\\
&={^0R_6}{^6p_f}+{^0R_5}{^5p_6}+{^0R_4}{^4p_5}+{^0R_3}{^3p_4}+{^0p_3}\\
\\
&={^0R_6}{^6p_f}+{^0R_5}{^5p_6}+{^0R_4}{^4p_5}+{^0p_4}\\
\\
&={^0R_6}{^6p_f}+{^0R_5}{^5p_6}+{^0p_5}\\
\\
&={^0R_6}{^6p_f}+{^0p_6}\\
\end{array}
$$

よって、$${^0p_f}$$に対して、ベースから第$${n}$$関節までの位置ベクトル$${^0p_n}$$を差し引けば、第$${n}$$関節からアーム先端までの距離ベクトル$${\bm{l_n}}$$が求められます。

$$
\bm{l_n}={^0p_f}-{^0p_n}
$$

角速度ベクトルと距離ベクトルが分かれば、これらの外積計算によって第$${n}$$関節がアーム先端に発生させる速度を求められます。

そして、アーム先端速度は、関節がアーム先端に発生させる速度を、シンプルに合計したものです。

例えば、６軸ロボットのアーム先端速度$${^0v_f}$$は、以下のように求められます。
（細字の$${\omega}$$は普通の掛け算なので、計算の順序関係なくどこにでも持っていけます）

$$
\begin{array}{ll}
^0v_f&=
\left[
\begin{array}{c}
v_{xf}\\v_{yf}\\v_{zf}
\end{array}
\right]\\
&=\bm{\omega_1}\times\bm{l_1}+\bm{\omega_2}\times\bm{l_2}+\bm{\omega_3}\times\bm{l_3}+\bm{\omega_4}\times\bm{l_4}+\bm{\omega_5}\times\bm{l_5}+\bm{\omega_6}\times\bm{l_6}\\
&=
\left[
\begin{array}{c}
A_{x1}\\A_{y1}\\A_{z1}
\end{array}
\right]\cdot\omega_1\times\bm{l_1}+
\left[
\begin{array}{c}
A_{x2}\\A_{y2}\\A_{z2}
\end{array}
\right]\cdot\omega_2\times\bm{l_2}+
\left[
\begin{array}{c}
A_{x3}\\A_{y3}\\A_{z3}
\end{array}
\right]\cdot\omega_3\times\bm{l_3}+
\left[
\begin{array}{c}
A_{x4}\\A_{y4}\\A_{z4}
\end{array}
\right]\cdot\omega_4\times\bm{l_4}+
\left[
\begin{array}{c}
A_{x5}\\A_{y5}\\A_{z5}
\end{array}
\right]\cdot\omega_5\times\bm{l_5}+
\left[
\begin{array}{c}
A_{x6}\\A_{y6}\\A_{z6}
\end{array}
\right]\cdot\omega_6\times\bm{l_6}\\
&=
\left[
\begin{array}{c}
A_{x1}\\A_{y1}\\A_{z1}
\end{array}
\right]\times\bm{l_1}\cdot\omega_1+
\left[
\begin{array}{c}
A_{x2}\\A_{y2}\\A_{z2}
\end{array}
\right]\times\bm{l_2}\cdot\omega_2+
\left[
\begin{array}{c}
A_{x3}\\A_{y3}\\A_{z3}
\end{array}
\right]\times\bm{l_3}\cdot\omega_3+
\left[
\begin{array}{c}
A_{x4}\\A_{y4}\\A_{z4}
\end{array}
\right]\times\bm{l_4}\cdot\omega_4+
\left[
\begin{array}{c}
A_{x5}\\A_{y5}\\A_{z5}
\end{array}
\right]\times\bm{l_5}\cdot\omega_5+
\left[
\begin{array}{c}
A_{x6}\\A_{y6}\\A_{z6}
\end{array}
\right]\times\bm{l_6}\cdot\omega_6
\end{array}
$$

ここで、数式を見やすくするため、第$${n}$$関節のベクトルの外積計算部分を以下のように置き換えます。

$$
\bm{J_n}=
\left[
\begin{array}{c}
A_{xn}\\A_{yn}\\A_{zn}
\end{array}
\right]\times\bm{l_n}
$$

すると、６軸ロボットのアーム先端速度$${^0v_f}$$は、以下のようになります。

$$
\begin{array}{ll}
^0v_f&=
\left[
\begin{array}{c}
v_{xf}\\v_{yf}\\v_{zf}
\end{array}
\right]\\
&=\bm{J_{v1}}\cdot\omega_1+\bm{J_{v2}}\cdot\omega_2+\bm{J_{v3}}\cdot\omega_3+\bm{J_{v4}}\cdot\omega_4+\bm{J_{v5}}\cdot\omega_5+\bm{J_{v6}}\cdot\omega_6
\end{array}
$$

また、アーム先端角速度は、これもシンプルに全軸分の角速度ベクトルを合わせたものです。
例えば、６軸ロボットのアーム先端角速度$${^0\omega_f}$$は、下式で求められます。

$$
\begin{array}{ll}
^0\omega_f&=
\left[
\begin{array}{c}
\omega_{xf}\\ \omega_{yf}\\ \omega_{zf}
\end{array}
\right]\\
&=\bm{\omega_1}+\bm{\omega_2}+\bm{\omega_3}+\bm{\omega_4}+\bm{\omega_5}+\bm{\omega_6}\\
&=
\left[
\begin{array}{c}
A_{x1}\\A_{y1}\\A_{z1}
\end{array}
\right]\cdot\omega_1+
\left[
\begin{array}{c}
A_{x2}\\A_{y2}\\A_{z2}
\end{array}
\right]\cdot\omega_2+
\left[
\begin{array}{c}
A_{x3}\\A_{y3}\\A_{z3}
\end{array}
\right]\cdot\omega_3+
\left[
\begin{array}{c}
A_{x4}\\A_{y4}\\A_{z4}
\end{array}
\right]\cdot\omega_4+
\left[
\begin{array}{c}
A_{x5}\\A_{y5}\\A_{z5}
\end{array}
\right]\cdot\omega_5+
\left[
\begin{array}{c}
A_{x6}\\A_{y6}\\A_{z6}
\end{array}
\right]\cdot\omega_6
\end{array}
$$

ロボット分野におけるヤコビアン$${J_{(\theta)}}$$は、関節角速度をアーム先端速度（および角速度）に変換する行列です。

アーム先端速度$${^0v_f}$$とアーム先端角速度$${^0\omega_f}$$を行列形式でまとめると、ヤコビアンが得られます。

下式は、行列形式でまとめた数式です。

$$
\begin{array}{ll}
\left[
\begin{array}{c}
{^0v_f}\\{^0\omega_f}
\end{array}
\right]
&=
\left[
\begin{array}{c}
{^0v_f}\\{\omega_{xf}}\\{\omega_{yf}}\\{\omega_{zf}}
\end{array}
\right]\\
&=
\left[
\begin{array}{c}
\bm{J_{v1}}\cdot\omega_1+\bm{J_{v2}}\cdot\omega_2+\bm{J_{v3}}\cdot\omega_3+\bm{J_{v4}}\cdot\omega_4+\bm{J_{v5}}\cdot\omega_5+\bm{J_{v6}}\cdot\omega_6\\
A_{x1}\cdot\omega_1+A_{x2}\cdot\omega_2+A_{x3}\cdot\omega_3+A_{x4}\cdot\omega_4+A_{x5}\cdot\omega_5+A_{x6}\cdot\omega_6\\
A_{y1}\cdot\omega_1+A_{y2}\cdot\omega_2+A_{y3}\cdot\omega_3+A_{y4}\cdot\omega_4+A_{y5}\cdot\omega_5+A_{y6}\cdot\omega_6\\
A_{z1}\cdot\omega_1+A_{z2}\cdot\omega_2+A_{z3}\cdot\omega_3+A_{z4}\cdot\omega_4+A_{z5}\cdot\omega_5+A_{z6}\cdot\omega_6
\end{array}
\right]
\end{array}
$$

上式から、６軸ロボットにおけるヤコビアン$${J_{(\theta)}}$$は以下と分かります。
（$${\bm{J_{vn}}}$$は$${3\times1}$$ベクトルなので、このヤコビアン$${J_{(\theta)}}$$のサイズは$${6\times6}$$）

$$
J_{(\theta)}=
\left[
\begin{array}{cccccc}
\bm{J_{v1}}&\bm{J_{v2}}&\bm{J_{v3}}&\bm{J_{v4}}&\bm{J_{v5}}&\bm{J_{v6}}\\
A_{x1}&A_{x2}&A_{x3}&A_{x4}&A_{x5}&A_{x6}\\
A_{y1}&A_{y2}&A_{y3}&A_{y4}&A_{y5}&A_{y6}\\
A_{z1}&A_{z2}&A_{z3}&A_{z4}&A_{z5}&A_{z6}
\end{array}
\right]
$$

軸数が変わっても、これと同じ要領でヤコビアンを求められます。

4. 仮想仕事の原理（静力学解析への応用）

本題に入る前に、各関節に微小な角速度$${\bm{\delta\omega}}$$が発生したとき、アーム先端がどう動くかを考えます。
 
前提の確認ですが、ヤコビアン$${J_{(\theta)}}$$を使えば、関節角速度$${\bm{\omega}}$$とアーム先端速度・角速度$${\bm{v}}$$との関係は、下式で表せます。

$$
\bm{v}=J_{(\theta)}\bm{\omega}
$$

関節角速度$${\bm{\omega}}$$を限りなくゼロに近い微小角速度$${\bm{\delta\omega}}$$とすれば、それに応じた微小アーム先端速度・角速度$${\bm{\delta v}}$$は以下のように求められます。

$$
\bm{\delta v}=J_{(\theta)}\bm{\delta\omega}
$$

$${\bm{\delta\omega}}$$は限りなくゼロに近いので、関節角度をほとんど変えません。
このとき、ヤコビアン$${J_{(\theta)}}$$は定数とみなせます。

よって、各関節の微小角度$${\bm{\delta\theta}}$$と、それによるアーム先端の微小変位$${\bm{\delta x}}$$の関係は以下になります。

$$
\bm{\delta x}=J_{(\theta)}\bm{\delta\theta}
$$

$$
\left(
\begin{array}{c}
\bm{\delta\theta}=\int\bm{\delta\omega}dt   ,   \bm{\delta x}=\int\bm{\delta v}dt
\end{array}
\right)
$$

「仮想仕事の原理」を理解するには、これを把握しておく必要があります。

さて、本題に入ります。

仮想仕事の原理とは、仮想的な仕事のつり合い関係に基づき、関節トルクとアーム先端力・モーメントの関係性を明らかにするものです。

まず、アーム先端が何かと接触している状態を考えます。

ただし、力をかければ、接触対象がごくわずかに変形する（＝限りなくゼロに近いが動く）とします。
 
ごくわずかな変形後、ロボットと接触対象が接触する点において、力がつり合います。

そのとき、ロボットの仕事（＝微小角度$${\times}$$トルク）と、接触対象の仕事（＝微小変位$${\times}$$力）はつり合います。
（ロボットの仕事は全軸の総和、接触対象の仕事は全方向の総和）

例えば、６軸多関節ロボットなら、下式が成り立ちます。

$$
\bm{\delta\theta}^T\bm{\tau}=\bm{\delta x}^T\bm{F}
$$

$$
\left(
\begin{array}{c}
\bm{\delta\theta}=
\left[
\begin{array}{c}
\delta\theta_1\\\delta\theta_2\\\delta\theta_3\\\delta\theta_4\\\delta\theta_5\\\delta\theta_6
\end{array}
\right]   ,   
\bm{\tau}=
\left[
\begin{array}{c}
\tau_1\\\tau_2\\\tau_3\\\tau_4\\\tau_5\\\tau_6
\end{array}
\right]   ,   
\bm{\delta x}=
\left[
\begin{array}{c}
\delta X\\ \delta Y\\ \delta Z\\ \delta\theta_x \\ \delta\theta_y \\ \delta\theta_z
\end{array}
\right]   ,   
\bm{F}=
\left[
\begin{array}{c}
F_x\\F_y\\F_z\\M_x\\M_y\\M_z
\end{array}
\right]\\
\\
M_x,M_y,M_z\text{は}X,Y,Z\text{軸まわりモーメント}
\end{array}
\right)
$$

ここで、$${\bm{\delta x}=J_{\theta}\bm{\delta\theta}}$$です。
転置行列の性質より、$${\bm{\delta x}^T=(J_{\theta}\bm{\delta\theta})^T=\bm{\delta\theta}^T{J_{\theta}}^T}$$となります。

これを仕事のつり合い式に代入すると、下式が得られます。

$$
\bm{\delta\theta}^T\bm{\tau}=\bm{\delta\theta}^T{J_{(\theta)}}^T\bm{F}
$$

$$
\bm{\tau}={J_{(\theta)}}^T\bm{F}
$$

上式が、「仮想仕事の原理」として広く知られている数式です。

また、両辺に$${{J_{(\theta)}}^T}$$の逆行列を左から掛けることで、下式も成り立ちます。

$$
\bm{F}=({J_{(\theta)}}^T)^{-1}\bm{\tau}
$$

もし、$${J_{(\theta)}}$$が正方行列でない場合、以下の擬似逆行列計算が有効です。
（行列の性質上、$${J_{(\theta)}{J_{(\theta)}}^T}$$は必ず行と列のサイズが同じになります）

$$
J_{(\theta)}\bm{\tau}=J_{(\theta)}{J_{(\theta)}}^T\bm{F}
$$

$$
\bm{F}=(J_{(\theta)}{J_{(\theta)}}^T)^{-1}J_{(\theta)}\bm{\tau}
$$

これらにより、
アーム先端力・モーメントに応じた関節トルク、
関節トルクに応じたアーム先端力・モーメント、
の計算が可能になります。

5. 特異点の見つけ方

ロボット分野における「特異点」とは、
動きの自由度が減ってしまう、
または、
関節角度をどう動かすべきかが決まらない
ロボット姿勢を意味します。

この特異点は、以下２つに大別できます。

① ロボットアームが伸びきった姿勢
　（それ以上、アームを伸ばす方向に動けない）
② 複数の関節の回転軸方向が完全に重なる姿勢
　（どの関節を動かすかが一意に決まらない）

６軸垂直多関節ロボットにおいて、②の代表的なものは以下２つです。

• 手首特異点
  ：手首側の２軸（第４関節と第６関節）の回転軸が重なる

• 肩特異点
  ：最根本と最手先側の２軸（第１関節と第６関節）の回転軸が重なる

特に、動けない方向に無理に動かすと、大トルクが必要になってトルクオーバーが発生します。

すると、コントローラの保護機能がはたらいて、ロボットが停止（＝生産ラインがストップ）します。

それは避けるべきことです。

そこで利用できるのが、特異点ではヤコビアンの行列式$${det|J_{(\theta)}|=0}$$になるという性質です。

$${det|J_{(\theta)}|}$$を監視し、それが0に近づく（＝特異点に近づく）と特異点を回避するようロボット動作指令を修正すれば、生産ラインがストップするのを避けられます。

ヤコビアンはこうした使い方もできる便利なものなので、理解して活用していきましょう。

6. おわりに

この記事では、ヤコビアン（ヤコビ行列）について紹介・解説しました。
 
学校の講義等では、ヤコビアンはキネマティクスや逆キネマティクスに比べて、サラっと流されがちな印象があります。

そこで、今回の記事は、そこそこ力を入れて書きました。
（間違ってないかの検算もしてますよ）
 

最初は、「キネマティクスと来たら、その次は逆キネマティクスがいいかなぁ」と思いました。

しかし、よくよく考えると、
キネマティクス→ヤコビアン→逆キネマティクス
の順で説明するとつながりが良いと思えたので、この順序で記事を書くことにしました。
 

次回こそは、逆キネマティクスについて紹介・解説しようと思います。

前回：ロボット編（２）「キネマティクス」はこちら
次回：ロボット編（４）「逆キネマティクス」はこちら