机器人学（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”表明手腕关节的运动对末端执行器的平移运动没有任何影响——这是机械臂拥有球腕和一个零长度工具的必然结果。