雅可比矩阵是标量函数的导数概念的一个推广，是在机器人运动分析和控制中最重要的量之一。速度雅可比矩阵（Velocity Jacobian）是机器人学中连接关节空间与操作空间的关键工具，用于描述关节速度与末端执行器速度之间的线性映射关系。
刚体的角速度与线速度
绕固定轴旋转的角速度
当物体围绕一个固定轴线做纯转动时，刚体上每一点都做圆周运动，这些圆的中心位于旋转轴线上。当刚体转动时，刚体上任意一点到旋转轴线的垂线都会扫过一个角度 $\theta$，这个角度对刚体上每一点都是相同的。如果 $k$ 是沿着旋转轴线方向的一个单位向量，那么角速度可以由下式给出：
$$
\omega = \dot \theta k
$$
其中 $\dot \theta$ 是角度 $\theta$ 对时间的导数。
角速度的主要作用是在刚体上诱生点的线速度，给定一个刚体的角速度后可以推导出线速度为：
$$
v = \omega \times r
$$
其中r为从旋转轴上到该点的向量。
为了确定一个刚体对象的姿态角度，我们在该刚体上固连一个参考坐标系，然后确定参考坐标系的姿态角度，由于刚体对象上每一个点都具有相同的角速度，并且刚体上各点相对于参考坐标系的几何关系都是固定的，因此角速度是参考坐标系自身的一个属性，不是单个点的属性，因此上面的式子中的 $v$对应着一个点的线速度，而 $\omega$ 对应着参考坐标系相关联的角速度。
反对称矩阵
反对称矩阵（Skew-Symmetric Matrix）是线性代数中的一种特殊矩阵：一个 $ n \times n $ 的矩阵 $ A $ 如果满足：
$$
A^T + A = 0
$$
则称 $A$ 为反对称矩阵。
用 $so(3)$表示所有 $3 \times 3$ 反对称矩阵的集合。
如果 $A \in so(3)$，具有元素$a_{ij}(i,j = 1,2,3)$，那么有下面9个式子成立：
$$
a_{ij} + a_{ji} = 0 \quad (i,j = 1,2,3)
$$
即：
$$
A = \begin{bmatrix}
0 & -c & b \\
c & 0 & -a \\
-b & a & 0
\end{bmatrix}
$$
反对称矩阵中仅含有3个独立项。
如果 $a=(a_x,a_y,a_z)^T$ 是 $3 \times 1$ 向量，那么我们将对应的反对称矩阵 $S(a)$ 表示为：
$$
S(a) = \begin{bmatrix}
0 & -a_z & a_y \\
a_z & 0 & -a_x \\
-a_y & a_x & 0
\end{bmatrix}
$$
若使用$i,j,k$分别表示基坐标系的三个单位坐标向量：
$$
i = [1,0,0]^T \\
j = [0,1,0]^T \\
k = [0,0,1]^T \\
$$
他们的反对称矩阵为：
$$
S(i) = \begin{bmatrix}
0 & 0 & 0 \\
0 & 0 & -1 \\
0 & 1 & 0
\end{bmatrix} \\
S(j) = \begin{bmatrix}
0 & 0 & 1 \\
0 & 0 & 0 \\
-1 & 0 & 0
\end{bmatrix} \\
S(k) = \begin{bmatrix}
0 &-1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix}
$$
- 反对称矩阵的操作是线性的，对 $\forall a,b \in \mathbb{R}^3, \text{以及} \forall \alpha, \beta \in \mathbb{R}$，有：
$$
S(\alpha a + \beta b) = \alpha S(a) + \beta S(b) \tag{\text{式}0-1}
$$
- 对于$ \forall a,p \in \mathbb{R}^3$，有：
$$
S(a)p = a \times p
$$
上式右侧是叉积。
- 对于 $R \in SO(3) , \forall a \in \mathbb{R}^3$，有：
$$
RS(a)R^T = S(Ra)
$$
对它的证明可以在多种参考资料中查阅得到，该式在对旋转矩阵的求导中需要用到。
- 对于一个$n \times n$ 的反对称矩阵S以及任意的向量$X \in \mathbb{R}^n$，有：
$$
X^T S X = 0
$$
旋转矩阵求导
对于旋转矩阵$R = R(\theta) \in SO(3)$，因为对于所有的$\theta$，$R(\theta)$都是正交矩阵，因此：
$$
R(\theta) R(\theta)^T = I \tag{1}
$$
两端对$\theta$求导得：
$$
[\frac{d}{d \theta} R]R(\theta)^T + R(\theta) [\frac{d}{d \theta} R^T] = 0 \tag{2}
$$
现在定义一个矩阵$S$如下：
$$
S = \begin{bmatrix} \frac{d}{d \theta} R \end{bmatrix} R(\theta)^T \tag{3}
$$
$$
\Downarrow
$$
$$
S^T = \left( \begin{bmatrix} \frac{d}{d \theta} R \end{bmatrix} R(\theta)^T \right)^T = R(\theta) \begin{bmatrix} \frac{d}{d \theta} R^T \end{bmatrix} \tag{4}
$$
由（2）、（3）、（4）可知：
$$
S + S^T = 0 \tag{5}
$$
式（5）意味着：在式（3）中定义的S矩阵是反对称矩阵，那么对式（3）两端同时右乘$R(\theta)$，注意到 $R^TR = I$ ,可得：
$$
SR(\theta) = \begin{bmatrix} \frac{d}{d \theta} R \end{bmatrix} R(\theta)^T R(\theta) \\
\Downarrow \\
\frac{d}{d \theta} R = SR(\theta) \tag{6}
$$
式（6）表明：计算旋转矩阵R的导数，等同于乘以一个反对称矩阵S的乘法操作，这对后续的很多计算都有重要作用。
例如绕$X$轴旋转 $\theta$ 角度的旋转矩阵为：
$$
R_X(\theta) = \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\theta & -\sin\theta \\
0 & \sin\theta & \cos\theta
\end{bmatrix}
$$
对 $\theta$) 求导，即对 $ R_X(\theta)$ 的每个元素关于$\theta$求导，得到：
$$
\frac{d}{d\theta} R_X(\theta) = \begin{bmatrix}
0 & 0 & 0 \\
0 & -\sin\theta & -\cos\theta \\
0 & \cos\theta & -\sin\theta
\end{bmatrix}
$$
求
$$
S =[\frac{d}{d\theta} R] R^T = \begin{bmatrix}
0 & 0 & 0 \\
0 & -\sin\theta & -\cos\theta \\
0 & \cos\theta & -\sin\theta
\end{bmatrix} \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\theta & \sin\theta \\
0 & -\sin\theta & \cos\theta
\end{bmatrix} \\
= \begin{bmatrix}
0 & 0 & 0 \\
0 & 0 & -1\\
0 & 1 & 0
\end{bmatrix} = S(i)
$$
在机器人学和刚体动力学中，$S(i)$ 通常表示与旋转轴相关的 反对称矩阵（Skew-Symmetric Matrix）。它用于描述旋转运动中的角速度向量与旋转矩阵之间的关系。
对于旋转轴 $i$（$ i = x, y, z$），其对应的反对称矩阵 $S(i)$ 定义如下：
$$
S(x) = \begin{bmatrix}
0 & 0 & 0 \\
0 & 0 & -1 \\
0 & 1 & 0
\end{bmatrix}
\\
S(y) = \begin{bmatrix}
0 & 0 & 1 \\
0 & 0 & 0 \\
-1 & 0 & 0
\end{bmatrix}
\\
S(z) = \begin{bmatrix}
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix}
$$
反对称矩阵 $S(i)$ 的主要作用是将角速度向量 $\omega$ 转换为对应的反对称矩阵形式 $[\omega]_\times$ ，从而描述旋转运动的性质。
对于角速度向量 $\omega = [\omega_x, \omega_y, \omega_z]^T$，其反对称矩阵为：
$$
[\omega]_\times = \omega_x S(x) + \omega_y S(y) + \omega_z S(z)
$$
即：
$$
[\omega]_\times = \begin{bmatrix}
0 & -\omega_z & \omega_y \\
\omega_z & 0 & -\omega_x \\
-\omega_y & \omega_x & 0
\end{bmatrix}
$$
旋转矩阵$R $的导数与角速度的反对称矩阵 $[\omega]_\times$ 之间存在以下关系：
$$
\frac{dR}{dt} = [\omega]_\times R
$$
示例
假设绕 $Z$ 轴旋转 $\theta$ 角度，其旋转矩阵为：
$$
R_Z(\theta) = \begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0 & 0 & 1
\end{bmatrix}
$$
对 $\theta$ 求导，得到：
$$
\frac{d}{d\theta} R_Z(\theta) = \begin{bmatrix}
-\sin\theta & -\cos\theta & 0 \\
\cos\theta & -\sin\theta & 0 \\
0 & 0 & 0
\end{bmatrix}
$$
根据旋转矩阵导数与角速度的关系：
$$
\frac{d}{d\theta} R_Z(\theta) = S(z) R_Z(\theta)
$$
验证：
$$
S(z) R_Z(\theta) = \begin{bmatrix}
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix}
\begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0 & 0 & 1
\end{bmatrix}
= \begin{bmatrix}
-\sin\theta & -\cos\theta & 0 \\
\cos\theta & -\sin\theta & 0 \\
0 & 0 & 0
\end{bmatrix}
$$
与 $\frac{d}{d\theta} R_Z(\theta)$ 的结果一致。
一般情况下的角速度
考虑角速度的轴线可能变化的情况，假设旋转矩阵$R$随时间变化，$\forall t \in \mathbb{R}$，有：$R = R(t) \in SO(3)$。假设$R(t)$ 是t的连续可导函数，那么$R(t)$的导数$R'(t)$为：
$$
\dot R(t) = S(\omega(t)) R(t)
$$
其中矩阵$S(\omega(t))$为反对称矩阵，向量$\omega(t)$为$t$时刻旋转坐标系相对于固定坐标系的角速度。
机器人学中的机械臂通常都是具有多个自由度的，常有几个坐标系之间运动进行叠加，下面推导两个移动坐标系 $o_1x_1y_1z_1$ 和 $o_2x_2y_2z_2$ 相对于固定坐标系 $o_0x_0y_0z_0$ 的角速度运动的叠加，假设三个坐标系拥有共同的原点，用旋转矩阵$R_1^0(t)$和$R_2^1(t)$表示相对姿态，用$\omega^k_{i,j}$表示与$R^i_j$对应的角速度相对于参考系$k$的表达式，则：
$$
\begin{align*}
R^0_2(t) &= R_1^0(t) R_2^1(t) \\
&\Downarrow \notag \\
\dot R^0_2 &= \dot R_1^0 R_2^1 + R_1^0 \dot R_2^1 \\
&\Downarrow \notag \\
S(\omega^0_{0,2}) R^0_2 &= S(\omega^0_{0,1}) R^0_1 R_2^1 + R^0_1 S(\omega^1_{1,2}) R_2^1 \notag \\
&= S(\omega^0_{0,1}) R^0_2 + R^0_1 S(\omega^1_{1,2}) {R^0_1}^T R^0_1 R_2^1 \notag \\
&= S(\omega^0_{0,1}) R^0_2 + S(R^0_1 \omega^1_{1,2})R^0_1 R_2^1 \notag \\
&= S(\omega^0_{0,1}) R^0_2 + S(R^0_1 \omega^1_{1,2})R^0_2 \notag \\
&= \{ S(\omega^0_{0,1}) + S(R^0_1 \omega^1_{1,2})\} R^0_2 \\
&\Downarrow \notag \\
S(\omega^0_{0,2}) &= S(\omega^0_{0,1}) + S(R^0_1 \omega^1_{1,2}) \\
&\Downarrow \\
\omega^0_{2} &= \omega^0_{0,1} + R^0_1 \omega^1_{1,2} \\
\end{align*}
$$
将此式推广，有：
$$
\dot R^0_n = S(\omega^0_{0,n}) R^0_n \\
\omega^0_{n} = \omega^0_{0,1} + \sum_{i=1}^{n-1} R^0_{i} \omega^i_{i,i+1} \\
= \omega^0_{0,1} + \sum_{i=1}^{n-1}\omega^{0}_{i,i+1}
$$
推导雅可比矩阵
给定一个$n$自由度的机械臂，这一部分的目的是根据机器人的关节速度求解末端执行器的线速度和角速度。
雅可比矩阵（Jacobian matrix）是连接关节速度与末端执行器速度的一种重要工具。
假设机器人具有 $ n$ 个自由度，关节角度向量为 $ \mathbf{q} = [q_1, q_2, \dots, q_n]^T$，关节速度向量为 $ \dot{\mathbf{q}} = [\dot{q}_1, \dot{q}_2, \dots, \dot{q}_n]^T$。末端执行器的位置和姿态在任务空间中由向量 $ \mathbf{x}$ 描述（例如，笛卡尔坐标中的位置和欧拉角）。任务空间速度向量为 $ \dot{\mathbf{x}}$，表示末端执行器的线速度和角速度。
目标是找到关节速度 $ \dot{\mathbf{q}}$ 和任务空间速度 $ \dot{\mathbf{x}}$ 之间的关系：
$$
\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}}
$$
其中 $ \mathbf{J}(\mathbf{q})$ 是机器人雅可比矩阵。
机器人末端执行器的线速度 $ \mathbf{v}$ 和角速度 $ \boldsymbol{\omega}$ 的组合向量（有些教材中被称为体速度，用$\xi$表示） $ \dot{\mathbf{x}} = [\mathbf{v}; \boldsymbol{\omega}]$ 通过雅可比矩阵与关节速度相关联：
$$
\dot{\mathbf{x}} = \begin{bmatrix}
\mathbf{v} \\
\boldsymbol{\omega}
\end{bmatrix}
= \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}}
$$
其中：
$$
\mathbf{J}(\mathbf{q}) = \begin{bmatrix}
\mathbf{J}_v \\
\mathbf{J}_\omega
\end{bmatrix}
$$
- $ \mathbf{J}_v$ 为线速度部分（位置雅可比矩阵）。
- $ \mathbf{J}_\omega$ 为角速度部分（旋转雅可比矩阵）。
速度雅可比矩阵（Velocity Jacobian）是机器人学中连接关节空间与操作空间的关键工具，用于描述关节速度与末端执行器速度之间的线性映射关系。以下是对其的详细解释：
速度雅可比矩阵 $\mathbf{J}$ 是一个 $6 \times n$ 的矩阵（$n$ 为关节数）。
线速度推导
末端执行器的位置 $\mathbf{p}$ 对时间的导数为：
$$
\mathbf{v} = \dot{\mathbf{p}}
$$
对于第 $i$ 个关节，若关节变量为 $q_i$：
对于旋转关节： 第 $i$ 个关节的贡献为：
$$
\mathbf{v}_i = \boldsymbol{\omega}_i \times (\mathbf{p} - \mathbf{p}_i)
$$
其中：
- $\boldsymbol{\omega}_i = \dot{q}_i \mathbf{z}_i$，表示关节角速度，$\mathbf{z}_i$ 是第 $i$ 个关节的旋转轴方向。
- $\mathbf{p}_i$ 是第 $i$ 个关节的位置。
对于移动关节： 第 $i$ 个关节的贡献为：
$$
\mathbf{v}_i = \dot{q}_i \mathbf{z}_i
$$
其中 $\mathbf{z}_i$ 是第 $i$ 个关节的运动方向。
移动关节的运动方向是其本地坐标系的 Z 轴方向，用单位向量 $\mathbf{z}_i$ 表示。移动关节不会产生角速度，因此对末端执行器的 线速度 的贡献直接沿 $\mathbf{z}_i$ 的方向，大小与关节速度 $\dot{q}_i$ 成正比。
在推导机械臂末端线速度时，所有关节（包括旋转关节和移动关节）的贡献叠加在一起。对末端执行器线速度 $\mathbf{v}$，线速度部分的雅可比矩阵 $\mathbf{J}_v$ 可以分解为：
$$
\mathbf{v} = \mathbf{J}_v \dot{\mathbf{q}}
$$
其中：
$$
\mathbf{J}_v =
\begin{bmatrix}
\mathbf{z}_1 \times (\mathbf{p} - \mathbf{p}_1) & \mathbf{z}_2 \times (\mathbf{p} - \mathbf{p}_2) & \cdots & \mathbf{z}_n \times (\mathbf{p} - \mathbf{p}_n)
\end{bmatrix}
$$
上式是 旋转关节的贡献。
对于移动关节的贡献 $\mathbf{v}_i$，直接加上 移动关节项：
$$
\mathbf{J}_v =
\begin{bmatrix}
\mathbf{z}_1 \times (\mathbf{p} - \mathbf{p}_1) + \mathbf{z}_1 & \cdots & \mathbf{z}_n \times (\mathbf{p} - \mathbf{p}_n) + \mathbf{z}_n
\end{bmatrix}
$$
- 如果关节 $i$ 是 旋转关节：仅有 $\mathbf{z}_i \times (\mathbf{p} - \mathbf{p}_i)$ 。
- 如果关节 $i$ 是 移动关节：仅有 $\mathbf{z}_i$。
雅可比矩阵的构造
将线速度和角速度综合起来，雅可比矩阵可以表示为：
$$
\dot{\mathbf{x}} = \begin{bmatrix}
\mathbf{v} \\
\boldsymbol{\omega}
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{J}_v \\
\mathbf{J}_\omega
\end{bmatrix}
\dot{\mathbf{q}}
$$
其中：
-
线速度部分 $\mathbf{J}_v$：
$$
\mathbf{J}_v = \begin{bmatrix}
\mathbf{z}_1 \times (\mathbf{p} - \mathbf{p}_1) & \mathbf{z}_2 \times (\mathbf{p} - \mathbf{p}_2) & \cdots & \mathbf{z}_n \times (\mathbf{p} - \mathbf{p}_n)
\end{bmatrix} + \text{(移动关节项)}
$$
-
角速度部分 $\mathbf{J}_\omega$：
$$
\mathbf{J}_\omega = \begin{bmatrix}
\mathbf{z}_1 & \mathbf{z}_2 & \cdots & \mathbf{z}_n
\end{bmatrix}
$$
因此速度雅可比矩阵在三维空间下的具体形式为：
$$
\mathbf{J}(\mathbf{q}) =
\begin{bmatrix}
\mathbf{z}_1 \times (\mathbf{p} - \mathbf{p}_1) & \cdots & \mathbf{z}_n \times (\mathbf{p} - \mathbf{p}_n) \\
\mathbf{z}_1 & \cdots & \mathbf{z}_n
\end{bmatrix}
$$
这为任意 $n$-自由度机械臂的线速度和角速度提供了关节速度的线性映射关系。