1. 讲两句

前面我们了解了机器人的正运动学和逆运动学，听起来运动学也就这了，但我们其实只研究了机器人在某一瞬时的位置表达，运动学往往要涉及位姿、速度、加速度，位姿我们用正运动学的DH法可以在世界坐标系里表达任意杆的位置和姿态（位姿），那对位姿求导是啥？对谁求导？求关于谁的导？求导导出来了个啥？有啥用？好，下面详细说。

2. 一阶（速度）运动学

机器人末端执行器的位姿用最小数目（设该数目为m）的坐标来表示，该坐标写作一个m维向量x，对时间求导就可以得到这个坐标的速度 $\dot{x}=\frac{dx}{dt}$ ，我们在实际驱动机器人末端到达m维向量x的过程中要用到另一套坐标体系，就是关节坐标，假设有n个关节，关节坐标$\theta \in R^n$
之前我们正运动学本质上是确立从关节坐标 $\theta$ 到 x 的映射（也就是齐次变换矩阵），而现在我们要建立的是从关节速度 $\dot{\theta (t)}$ 到 $\dot{x(t)}$ 的映射，也就是雅可比矩阵，这对我们如何设置关节电机的速度从而使末端达到对应的速度这一实际问题十分重要。

2.1 对平面2R机构的末端点的一阶运动学分析

我们可以直接写出末端点的坐标x：
$$x=\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]=\left[\begin{array}{c}{L}_1\cos {\theta }_1+L_2\cos ({\theta }_1+{\theta }_2)\\ L_1\sin {\theta }_1+L_2\sin ({\theta }_1+{\theta }_2)\end{array}\right]$$
利用链式法则对时间求导得到：
$$\begin{array}{c}l\stackrel{.}{x}=\frac{\mathrm{d}x}{d\theta }=\left[\begin{array}{cc}\frac{\partial x}{\partial {\theta }_1}& \frac{\partial x}{\partial {\theta }_2}\end{array}\right]\left[\begin{array}{c}\stackrel{.}{{\theta }_1}\\ \stackrel{.}{{\theta }_2}\end{array}\right]\\ \\ =\left[\begin{array}{cc}-L_1\sin {\theta }_1-L_{2}sin({\theta }_1+{\theta }_2)& -L_{2}sin({\theta }_1+{\theta }_2)\\ L_{1}cos{\theta }_1+L_{2}cos({\theta }_1+{\theta }_2)& L_2\cos ({\theta }_1+{\theta }_2)\end{array}\right]\left[\begin{array}{c}\stackrel{.}{{\theta }_1}\\ \stackrel{.}{{\theta }_2}\end{array}\right]\end{array}$$
显然雅可比矩阵 $J(\theta )$ 就是 ：
$$\left[\begin{array}{cc}-L_1\sin {\theta }_1-L_{2}sin({\theta }_1+{\theta }_2)& -L_{2}sin({\theta }_1+{\theta }_2)\\ L_{1}cos{\theta }_1+L_{2}cos({\theta }_1+{\theta }_2)& L_2\cos ({\theta }_1+{\theta }_2)\end{array}\right]$$
写成列向量的形式就可以看出：
$$v_{\mathrm{tip}}=J_1(\theta )\stackrel{.}{{\theta }_1}+J_2(\theta ){\theta }_2$$
而雅可比矩阵这两列的实际物理含义就是：
第一列的含义是当其他关节不动，只有第一个关节运动时对末端产生的速度分量
第二列的含义是当其他关节锁死，只有第二个关节运动时对末端产生的速度分量
如图所示：

---

以上就是简单的平面2R机构末端点的雅可比矩阵推导，生动又形象，而且简单，我们就借此来了解雅可比矩阵的一些神奇之处：

2.2 奇异

从上述内容可以看出，机器人末端 速度是当前时刻机器人关节坐标确定的雅可比矩阵 $J(\theta )$ 的各列的线性组合，这个雅可比矩阵是随着关节角变化而改变的，当处于某些位置时，矩阵的秩可能会下降，也就是雅可比矩阵某些列之间会线性相关，这会让末端在某些方向的运动能力丧失，这些位置称为奇异位形。
从我们2R机构例子可以看出来，这个机构的奇异位形是两个杆所在直线重合的时候，那一时刻末端就丧失了延杆所在直线方向运动的能力而只能沿着垂直于杆的方向运动。

2.3 可操作度椭球和可操作度

雅可比矩阵降秩不是一蹴而就的，在某些接近奇异位形的关节坐标处我们就可以提前察觉到异常，为了评鉴当前关节坐标对应的雅可比矩阵接近奇异位形的程度，我们用可操作度来衡量。
要求可操作度，我们先引入可操作度椭球。
对2R机构，给你一组关节角，怎么评价这组关节角对应的位形在各个方向的运动能力，最简单粗暴的方法就是：

在关节速度空间里选取关节速度向量的一个单位圆（其实就是所有可能的单位速度都映射一遍），然后把圆上所有点都映射到对应末端速度向量，观察这些向量分布特征就可以了。

可以预见，如果我们求得的雅可比矩阵可逆，关节速度空间中的单位圆应该映射为一个椭圆（可操作度椭圆），椭圆的长轴短轴对应雅可比矩阵的特征向量方向，长轴短轴的长短对应两个特征值：
下图分别是关节坐标 $(0,\frac{\pi }{2})$ 和 $(0,\frac{3\pi }{2})$的可操作度椭圆：

可以看出后者（$(0,\frac{3\pi }{2})$）显然在各个方向上的运动性能好于前者，前者在空间中朝着第一三象限的运动能力很弱，但是在第二四象限运动能力很显著，而其实在奇异位形的时候，可操作度椭圆退化为一条线，所以我们可以用椭圆的面积是正相关于可操作度的（也就是远离奇异位形的程度），椭圆的面积为 $\pi ab$ ,a 、b为长轴短轴长可以用雅可比矩阵的两个特征值表示，既然面积是特征值的乘积，而特征值的乘积就是矩阵的行列式，所以有很多对可操作度的度量：
$|det(J)|$
当然对于维数更高的雅可比矩阵当然就对应椭球了，这里简单一提，可以类比，些有不同。

2.4 静力和关节力矩的转换

利用雅可比矩阵和关节驱动功率与末端作业功率相同，可以推出关节力矩和末端静力之间的变换关系：
$$\begin{array}{l}{f}_{tip}^T{v}_{tip}={\tau }^T\dot{\theta }\\ \\ \iff f_{tip}^{T}J(\theta )\dot{\theta }={\tau }^T\dot{\theta }\\ \\ \Rightarrow \tau =J^T(\theta )f_{tip}\iff f_{tip}=J^{-T}(\theta )\tau \end{array}$$
非常有用

3.真正的完全体机器人雅可比矩阵

为了推导各各参考系的角速度之间的关系，我们需要先了解角速度和其对应的反对称矩阵的性质。

3.1 角速度

根据物理知识，角速度和线速度之间的关系是：

$$\mathrm{v}=\omega \times r$$
令旋转矩阵R为坐标系1在坐标系0中的表示（坐标系0旋转R到坐标系1）R的三列其实就是坐标系1的三个坐标轴对应的单位基向量在坐标系0中的坐标，加入坐标系1绕表示在坐标系0中的一个角速度旋转，那坐标系1三个基坐标断点的线速度就是以下矩阵的三个列向量：
$$V=\omega \times R$$

计算叉乘有好多方式，这里引入反对称矩阵（其实就是特殊正交群SO(3)的李代数）用$[\omega ]\text{或}S(\omega )$ 来表示(下面都用 $S(\omega )$ 表示) 。
有如下关系：
$$S(\omega )=S(\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right])=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$

3.1.1 反对称矩阵的性质

1. 线性：$S(\alpha a+\beta b)=\alpha S(a)+\beta S(b)$
2. 叉积：$S(s)p=a\times p$
3. $.RS(a)R^T=S(Ra)$
   证明：（其实肉眼看可以看出相似变换的感觉，但还是严格证明一下）
   对旋转矩阵R有：
   $$R(a\times b)=Ra\times Rb$$
   对性质3中式子两边同时乘以一个向量b：
   $$\text{左边}=RS(a)R^{T}b=R(a\times R^{T}b)=Ra\times b=S(Ra)b=\text{右边}$$
   用相似变换来解释的话，就是R是坐标系1在坐标系0中的表示，坐标系1中的 $a\times =S(a)$ 这个线性变换相似变换到坐标系0就是 $RS(a)R^T$,这是推导公式的结果，但如果从物理分析的角度，坐标系1中的向量a在坐标系0中就是 $Ra$,所以公式推导的结果$RS(a)R^T$就应该等于S(Ra).
4. 对任意的同维度向量X ：$X^{T}S(a)X=0$

3.1.2 求解角速度

因为
$$V=\omega \times R$$
这里的V也就是三个坐标轴端点的速度还可以通过对向量对时间求导得到，也就是
$$V=\dot{R}=\omega \times R=S(\omega )R$$
我们可以轻松推导出反对称矩阵:
$$S(\omega )=\dot{R}{R}^T$$
注意这里的角速度是在世界坐标系中的表示，而在R对应的旋转坐标系中角速度对应的坐标看起来很相近：
$$S({\omega }^R)=R^T\dot{R}$$
这可以从下式推出：
$$\dot{R}=R(S({\omega }^R)\times I)$$
这个式子怎么来的仔细观察，好好思考看看吧。

3.2 角速度叠加

因为机器人相邻两个杆件坐标系之间都有关节连接，如果是转动关节的话，那么两个坐标系之间的相对角速度很容易求出就是一个大小为关节变量，方向为驱动轴的向量，但是为了求解雅可比矩阵，我们想要了解的是每个杆系相对于基坐标系的角速度，这就需要角速度的叠加。
其实很自然如果知道n+1个坐标系之间两两的相对角速度，并且在基坐标系中表示记为：${\omega }_{n-1,n}^0$ ，这个就表示坐标系n相对于坐标系n-1的角速度，而且在坐标系0中表示，其实根据DH约定建系的话，这个相对角速度在坐标系n-1中表示为：${\omega }_{n-1,n}^{n-1}=\dot{{\theta }_n}{z}_{n-1}^{n-1}$ ,那么借助旋转矩阵，在坐标系0中表示就是：${\omega }_{n-1,n}^0=R_{n-1}^0{\omega }_{n-1,n}^{n-1}=R_{n-1}^0\dot{{\theta }_n}{z}_{n-1}^{n-1}$
而 $R_{n-1}^0{z}_{n-1}^{n-1}$ 实际上就是 $R_{n-1}^0$ 的第三列，而 $R_{n-1}^0$ 可以从DH法中求出。
所以我们很自然的得到：
$${\omega }_{0,n}^0={\omega }_{0,1}^0+{\omega }_{1,2}^0+{\omega }_{2,3}^0+...+{\omega }_{n-1,n}^0$$
当然需要我们用DH法求出每个杆系的变换矩阵，好在这个过程可以演绎递推，其实没有啥重复工作要做。
对于简化情形下　${\omega }_2^0={\omega }_{0,1}^0+R_1^0{\omega }_{1,2}^1$　的严格数学证明可以对下式求导，然后利用反对称矩阵的性质推导:
$$R_2^0(t)=R_1^0(t)R_2^1(t)$$
这里就留作习题吧 : P

3.3 雅可比矩阵的构造

空间中物体的速度可以分为坐标系原点速度和物体旋转产生的线速度的叠加，所以我们用旋量的形式表达空间物体的速度（v和 $\omega$ 的顺序好像没大关系）：
$$\xi =\left[\begin{array}{c}v\\ \omega \end{array}\right]$$
我们可以分别求关节速度映射到角速度分量的雅可比矩阵 $J_{\omega }$ 和另一个分量 $J_v$

3.3.1 角速度雅可比矩阵

易知 $J_{\omega }$ 是一个 $3\times n$ 的矩阵，他的第k列为
$$J_{\omega k}={\rho }_k{z}_{k-1}^0$$
其中当第k个关节为旋转关节的时候， ${\rho }_k=1$,为线性关节的时候为0。
$z_{k-1}^0$ 就是 $R_{k-1}^0$ 的第三列

3.3.2 原点线速度雅可比矩阵

1. 当第k个关节为平动关节的时候：
   $$J_{vk}=z_{k-1}^0$$
2. 当第k个关节为平转关节的时候：
   根据一开始2R机构的例子，雅可比矩阵第k列为其他关节锁死，只有第k关节运动时对末端提供的速度分量，第k个关节对应的坐标系原点为 $o_{k-1}^0$,对末端坐标系原点提供的速度分量为：
   $$\dot{\theta }{z}_{k-1}^0\times (o_n^0-o_{k-1]}^0)$$
   所以
   $$J_{vk}=z_{k-1}^0\times (o_n^0-o_{k-1]}^0)$$

---

这样，雅可比矩阵就构造完毕了。

---

当然这不是唯一的构造方式，现代机器人学提供了另一种基于PoE的方式，十分优美。

4.分析雅可比矩阵

这里的知识以后再来填补吧！