前言

在前几讲我们所介绍的例子中，我们用到的都是用解析法来准确的求解IK问题，而这一讲中，我们将介绍一种数值法求解IK的方法，叫做Jacobian Matrix。用这种方法，我们不需要计算复杂的方程式，只要知道了手臂的结构，就可以通过不断迭代的方式来逼近我们的目标点。

Jacobian Matrix

首先我们看Jacobian Matrix是什么样的,其中n是手臂joint的数量，x，y，z是手臂末端点的坐标位置：
$$
J=\begin{bmatrix}
\frac{\partial x}{\partial \theta_1}&\frac{\partial x}{\partial \theta_2}&\cdots&\frac{\partial x}{\partial \theta_n} \\
\frac{\partial y}{\partial \theta_1}&\frac{\partial y}{\partial \theta_2}&\cdots&\frac{\partial y}{\partial \theta_n} \\
\frac{\partial z}{\partial \theta_1}&\frac{\partial z}{\partial \theta_2}&\cdots&\frac{\partial z}{\partial \theta_n} \\
\end{bmatrix}
$$

Jacobian Matrix的意义

我们发现Jacobian Matrix的每一列其实就是将手臂末端点坐标位置对每一个joint求偏导数。换句话说，Jacobian Matrix每一列的意义就是这个joint的变化对手臂末端点位置的影响或者说是变化率。举例来说，第一列指的就是joint 1发生的变化会对手臂末端点产生什么样的影响。然后我们将每个joint对手臂末端点的影响列在一个矩阵里，这个矩阵就是Jacobian Matrix。

我们知道对$y=f(x)$的函数求导，就会得到这个函数在各个x点处的斜率。当x变化很小时，我们就可以通过这个斜率来估算y的变化，可以写为$\Delta y=f’(x)\Delta x$。而Jacobian Matrix充当的就是这里$f’(x)$的角色，于是就有了如下关系：$\Delta r=J\Delta \theta$
$$
\begin{aligned}
&\Delta r&=\begin{bmatrix}
\Delta x \\
\Delta y \\
\Delta z
\end{bmatrix} &=\begin{bmatrix}
\frac{\partial x}{\partial \theta_1}&\frac{\partial x}{\partial \theta_2}&\cdots&\frac{\partial x}{\partial \theta_n} \\
\frac{\partial y}{\partial \theta_1}&\frac{\partial y}{\partial \theta_2}&\cdots&\frac{\partial y}{\partial \theta_n} \\
\frac{\partial z}{\partial \theta_1}&\frac{\partial z}{\partial \theta_2}&\cdots&\frac{\partial z}{\partial \theta_n} \\
\end{bmatrix}\begin{bmatrix}
\Delta \theta_1 \\
\Delta \theta_2 \\
\vdots \\
\Delta \theta_n \\
\end{bmatrix}\\
&&&=\begin{bmatrix}
\frac{\partial x}{\partial \theta_1}\Delta \theta_1+\frac{\partial x}{\partial \theta_2}\Delta \theta_2+\cdots+\frac{\partial x}{\partial \theta_n}\Delta \theta_n \\
\frac{\partial y}{\partial \theta_1}\Delta \theta_1+\frac{\partial y}{\partial \theta_2}\Delta \theta_2+\cdots+\frac{\partial y}{\partial \theta_n}\Delta \theta_n \\
\frac{\partial z}{\partial \theta_1}\Delta \theta_1+\frac{\partial z}{\partial \theta_2}\Delta \theta_2+\cdots+\frac{\partial z}{\partial \theta_n}\Delta \theta_n
\end{bmatrix}\\
\end{aligned}
$$
通过这个等式，我们将每个joint对手臂末端点位置的影响都累加起来，就得到了最终x，y，z的变化量。

但是这个过程只解决了顺向运动学的问题，那么IK问题怎么解决呢？很简单，首先定义手臂末端需要移动的方向和大小$\Delta r$，然后在等式两边左乘Jacobian Matrix的逆矩阵，这样就得到了每个joint的变化量$\Delta \theta=J^{-1}\Delta r$。$J^{-1}$就相当于$\Delta r$对$\Delta \theta$的变化率，当$\Delta r$是指向目标点的向量时，就可以计算出到达目标点所需要的$\Delta \theta$，但是这个等式只有在$\Delta r$比较小的时候才能近似成立，所以我们需要一步一步的迭代，慢慢移动到目标点。然而，在绝大多数情况下Jacobian Matrix都是不可逆的，因此会用到pseudoinverse：$J^+=J^T(JJ^T)^{-1}$

Jacobian Matrix的建立

在了解了Jacobian Matrix的定义以及意义后，我们接下来看如何得到Jacobian Matrix。以一个平面3轴的手臂为例：

上面第一幅图中，如果只有手臂的第一个joint动，而其他的joint都不动的话，手臂末端点的运动轨迹就是那段弧形虚线，而那段切线就是当前手臂末端点对joint 1的偏导数，也就对应了我们Jacobian Matrix的第一列，以此类推。所以Jacobian Matrix就是这些切线的集合。

那我们该如何得到那条切线的向量呢？首先我们定义$r_e$是基坐标指向手臂末端点的向量，$r_j$是基坐标指向joint j的向量，$z_j$是joint j的旋转向量，那么$r_e-r_j$就是图中的黑色箭头。因为每一个joint所对应的那条切线永远都是垂直于$r_e-r_j$和$z_j$的，所以就可以用叉乘的方式得到那条切线的向量：$z_j\times(r_e-r_j)$，因此就可以写出我们的Jacobian Matrix：
$$
J=\begin{bmatrix}
(z_1\times(r_e-r_1))^T&(z_2\times(r_e-r_2))^T&\dots&(z_n\times(r_e-r_n))^T
\end{bmatrix}
$$

在就得到了Jacobian Matrix后，就可以用它来解决IK问题了。其实，Jacobian Matrix提供了一种线性趋近的方式，当手臂的joint发生改变时Jacobian Matrix也发生改变。当$\Delta r$比较小时，这个方法就能比较准确的驱动手臂末端以直线移动到目标点，但是需要更多的迭代运算。当$\Delta r$比较大时，手臂末端就能更快的移动到目标点，但是移动过程中就不一定完全按照所期望的那条直线移动。因此，$\Delta r$大小也可以用来控制移动速度，但是这个速度与时间无关。

其实，手臂的控制不单单只有位置还有方向，为了将手臂末端的方向控制考虑进来，我们需要增加新的限制条件，首先要在$\Delta r$中增加手臂末端绕x，y，z轴旋转的转速，然后再在Jacobian Matrix中添加每个joint的转轴，最后可写为如下形式：
$$
\begin{aligned}
\Delta r&=\left[\begin{array}{ccc|ccc}
\Delta x&\Delta y &\Delta z &w_x &w_y& w_z
\end{array}\right]^T \\
J&=\begin{bmatrix}
(z_1\times(r_e-r_1))^T&(z_2\times(r_e-r_2))^T&\dots&(z_n\times(r_e-r_n))^T \\
\hline
z_1&z_2&\dots&z_n
\end{bmatrix}
\end{aligned}
$$

Jacobian Matrix的补充

最后，关于Jacobian Matrix的建立还有最后一点注意事项，上述我们介绍的都是手臂的joint是旋转的情况，然而手臂的joint还有可能是线性运动的情况，不同类型的joint在建立Jacobian Matrix时也不一样，区别如下：

	Prismatic	Revolute
$\begin{bmatrix}\Delta x&\Delta y&\Delta z\end{bmatrix}^T$	$z_j$	$z_j\times(r_e-r_j)$
$\begin{bmatrix}w_x&w_y&w_z\end{bmatrix}^T$	$\begin{bmatrix}0&0&0\end{bmatrix}^T$	$z_j$

如何用Jacobian Matrix计算每个电机的Torque

当我们将Jacobian Matrix转置就得到了如下Matrix：
$$
J^T=\begin{bmatrix}
\frac{\partial x}{\partial \theta_1}&\frac{\partial y}{\partial \theta_1}&\frac{\partial z}{\partial \theta_1} \\
\frac{\partial x}{\partial \theta_2}&\frac{\partial y}{\partial \theta_2}&\frac{\partial z}{\partial \theta_2} \\
\vdots&\vdots&\vdots \\
\frac{\partial x}{\partial \theta_n}&\frac{\partial y}{\partial \theta_n}&\frac{\partial z}{\partial \theta_n} \\
\end{bmatrix}
$$转置后的Jacobian Matrix每一行就变成了手臂末端点的那条黑色切线，当我们在手臂末端点施加一个力$F=[F_x,F_y,F_z]^T$时，这个$F$力对于每一个joint的torque就是沿着黑色切线方向的力乘以距离，由于这条黑色切线在叉积时已经包含了距离信息，所以直接用这条黑色切线点乘$F$即可：
$$
\tau=\begin{bmatrix}
\tau_1\\
\tau_2\\
\vdots\\
\tau_n\\
\end{bmatrix}
=\begin{bmatrix}
\frac{\partial x}{\partial \theta_1}&\frac{\partial y}{\partial \theta_1}&\frac{\partial z}{\partial \theta_1} \\
\frac{\partial x}{\partial \theta_2}&\frac{\partial y}{\partial \theta_2}&\frac{\partial z}{\partial \theta_2} \\
\vdots&\vdots&\vdots \\
\frac{\partial x}{\partial \theta_n}&\frac{\partial y}{\partial \theta_n}&\frac{\partial z}{\partial \theta_n} \\
\end{bmatrix}\begin{bmatrix}
F_x\\
F_y\\
F_z\\
\end{bmatrix}
$$当然用这个方法计算每个joint的torque时我们忽略了手臂自身的重量，由于手臂自身重量对于每个joint的影响随着手臂的形态的变化而不断变化，所以先暂时不考虑。