機器人學（MATLAB機器人工具箱10.x）——4.1-雅克比矩陣（1）

我們首先研究關節坐標的微小變化如何影響末端執行器的位姿。使用齊次坐標變換表示位姿，我們可以通過一個一階微分近似得到位姿相對於關節坐標的導數：
$\dfrac{\text{d}T}{\text{d}q}\approx\dfrac{T(q+\delta_q)-T(q)}{\delta_q}$根據 $T$ 的定義，可以得到
$\tag{1} \dfrac{\text{d}T}{\text{d}q}\approx\dfrac{1}{\delta_q} \left( \begin{matrix} R\left( q+\delta _q \right) -R\left( q \right)& \begin{array}{c} \delta _x\\ \delta _y\\ \delta _z\\ \end{array}\\ \begin{matrix} 0& 0& 0\\ \end{matrix}& 0\\ \end{matrix} \right)$其中， $(\delta_x,\delta_y,\delta_z)$ 表示末端執行器的平移位移。下面我們舉個例子

>> mdl_puma560
>> T0 = p560.fkine(qn);
>> dq = 1e-6;
>> Tp = p560.fkine(qn + [dq 0 0 0 0 0]);
>> dTdq1 = (Tp - T0) / dq
dTdq1 =
    0.0000   -1.0000   -0.0000    0.1500
   -0.0000   -0.0000    1.0000    0.5963
         0         0         0         0
         0         0         0         0

得到的 矩陣顯然不是齊次變換矩陣，其左上角的 $3\times3$ 矩陣不是正交矩陣，右下角元素不為1。這說明了什么呢？

我們將dTdq1中的第四列元素與方程 $(1)$ 的矩陣中相應元素相等，得到
$\left( \begin{array}{c} \delta _x\\ \delta _y\\ \delta _z\\ \end{array} \right) =\left( \begin{array}{c} 0.1500\\ 0.5963\\ 0\\ \end{array} \right) \delta _{q_1}$
它代表了 $q_1$ 的變化引起的末端執行器位置的變化。

從圖中我們能更容易地理解——腰關節（ $q_1$）的一個微小的旋轉將使得末端執行器在世界坐標系水平的 $xy$ 平面內移動，但卻不產生垂直方向上的移動。

>> Tp = p560.fkine(qn + [0 dq 0 0 0 0]);
>> dTdq2 = (Tp - T0) / dq

dTdq2 =

    1.0000   -0.0000   -0.0000    0.0144
    0.0000         0    0.0000         0
    0.0000    0.0000    1.0000    0.5963
         0         0         0         0

同樣可以寫出
$\left( \begin{array}{c} \delta _x\\ \delta _y\\ \delta _z\\ \end{array} \right) =\left( \begin{array}{c} 0.0144\\ 0\\ 0.5963\\ \end{array} \right) \delta _{q_2}$
正如我們所預料的，肩關節 $q_2$ 的一個微小轉動會引起末端執行器在垂直的 $xz$ 平面內的運動，而沒有 $y$ 方向運動。等式兩邊同除以一個無限小的時間長度 $\delta_t$，則得到關節角速度和末端執行器速度之間的一個關系：
$\left( \begin{array}{c} \dot x\\ \dot y\\ \dot z\\ \end{array} \right) =\left( \begin{array}{c} 0.0144\\ 0\\ 0.5963\\ \end{array} \right) {\dot q_2}$
現在考慮方程 $(1)$ 的矩陣中左上方的 $3×3$ 子矩陣。我們將其乘以 $\delta_q/\delta_t$ ，從而得到 $R$ 的一階近似導數：
$\dot{R}\approx \left( \frac{R\left( q+\delta _q \right) -R\left( q \right)}{\delta _q} \right) \frac{\delta _q}{\delta _t}$
回顧前面對正交旋轉矩陣導數的定義，可以寫出：
$S\left( \omega \right) R\approx \left( \frac{R\left( q+\delta _q \right) -R\left( q \right)}{\delta _q} \right) \dot{q}_1$
$S\left( \omega \right) \approx \left( \frac{R\left( q+\delta _q \right) -R\left( q \right)}{\delta _q}R^T \right) \dot{q}_1$
從上式可以得出一個末端執行器角速度和關節速度之間的關系：
$\omega \approx \text{vex}\left( \frac{R\left( q+\delta _q \right) -R\left( q \right)}{\delta _q}R^T \right) \dot{q}_1$
繼續利用前面算的關節1的導數，得到：

>> dRdq1 = dTdq1(1:3,1:3);
>> R = T0.R
R =
   -0.0000    0.0000    1.0000
   -0.0000    1.0000   -0.0000
   -1.0000   -0.0000   -0.0000
>> S = dRdq1 * R'
S =
   -0.0000   -1.0000    0.0000
    1.0000   -0.0000    0.0000
         0         0         0

這是一個反對稱矩陣，從中可以提取出角速度向量：

>> vex(S)
ans =
   -0.0000
    0.0000
    1.0000

最后得出
$\left( \begin{array}{c} \omega _x\\ \omega _y\\ \omega _z\\ \end{array} \right) =\left( \begin{array}{c} 0\\ 0\\ 1\\ \end{array} \right) \dot{q}_1$
從之前那張圖可以很容易地理解上式，末端執行器的角速度就是關節 1 繞着世界坐標系 $z$ 軸旋轉的角速度。

對第二關節的微小運動重復上述過程，得到

>> dRdq2 = dTdq2(1:3,1:3);
>> S = dRdq2 * R'
S =
   -0.0000   -0.0000   -1.0000
    0.0000   -0.0000   -0.0000
    1.0000   -0.0000   -0.0000
>> vex(S)
ans =
    0.0000
   -1.0000
    0.0000

最后得出
$\left( \begin{array}{c} \omega _x\\ \omega _y\\ \omega _z\\ \end{array} \right) =\left( \begin{array}{c} 0\\ -1\\ 0\\ \end{array} \right) \dot{q}_2$
再次參見之前的圖片來理解上式，關節 2 的轉動軸線在 $y$ 軸負方向上，其旋轉將使機械臂得到一個負y軸方向上的角速度。

一個雅可比矩陣就是一個向量關於另一個向量的向量值函數導數。如果 $y=F(x)$，且 $x\in\mathbb{R}_n,y\in\mathbb{R}_m$，則雅可比矩陣是一個 $m×n$ 階的矩陣：
$J=\frac{\partial F}{\partial x}\left( \begin{matrix} \frac{\partial y_1}{\partial x_1}& \cdots& \frac{\partial y_1}{\partial x_n}\\ \vdots& \ddots& \vdots\\ \frac{\partial y_m}{\partial x_1}& \cdots& \frac{\partial y_m}{\partial x_n}\\ \end{matrix} \right)$
我們已經建立了各個關節的速度和機器人末端執行器的移動速度和角速度之間的數字關系。我們將機械臂的正運動學寫為以下函數形式：
$^0\xi=\mathcal{K}(q)$
對其求導數得到：
$^0\nu ={}^0J\left( q \right) \dot{q}$
這是一個瞬時正運動學，其中 $\nu=(v_x,v_y,v_z,\omega_x,\omega_y,\omega_z)\in\mathbb{R}^6$ 是一個空間速度，包含平移速度和旋轉速度分量。矩陣 $J(q)\in\mathbb{R}^{6\times N}$ 是機械臂的雅可比矩陣或稱為幾何雅可比矩陣。可以通過jacob0方法計算

>> J = p560.jacob0(qn)
J =
    0.1501    0.0144    0.3197         0         0         0
    0.5963    0.0000    0.0000         0         0         0
         0    0.5963    0.2910         0         0         0
   -0.0000   -0.0000   -0.0000    0.7071   -0.0000    1.0000
    0.0000   -1.0000   -1.0000   -0.0000   -1.0000   -0.0000
    1.0000    0.0000    0.0000   -0.7071    0.0000   -0.0000

矩陣的行對應於笛卡兒自由度，而列對應於各個關節——它們是對應於各相應關節單位速度的末端執行器空間速度。前兩列即為前面使用近似導數所計算出的結果。其右上方 $3×3$ 階的“0”表明手腕關節的運動對末端執行器的平移運動沒有任何影響——這是機械臂擁有球腕和一個零長度工具的必然結果。