3.1 几何雅可比矩阵

位姿运动学 $x_e=k(q)$ 是高度非线性的；但对时间求导之后，非线性问题就在速度层面线性化了——线性化后的映射正是雅可比矩阵。本节从几何角度（矢量传递）把它推出来：先解决「旋转矩阵怎么求导」（3.1.1），再沿关节链递推连杆速度（3.1.2），最后把每个关节的贡献拼成矩阵（3.1.3）。录播 0:27:00–1:12:12。

3.1.1 旋转矩阵求导：反对称算子登场

考虑 $n$ 自由度机械手，DH 法给出的正运动学

$$T_e(q)=\begin{bmatrix}n_e(q)&s_e(q)&a_e(q)&p_e(q)\0&0&0&1\end{bmatrix}, \qquad R_e=\begin{bmatrix}n_e&s_e&a_e\end{bmatrix},\qquad T=\begin{bmatrix}R&P\0&1\end{bmatrix}$$

$R$ 与 $P$ 都是位形 $q$ 的函数（$R_e$ 的三列 $n_e,s_e,a_e$ 是末端坐标系三根轴的单位向量——法向 / 滑动 / 接近向量，见 定义 2.8.2）。微分运动学要找的是：关节速度与末端执行器线速度、角速度之间的关系。

位置部分好办——位移求导就是速度；关键在姿态部分：旋转矩阵怎么对时间求导？

$S$ 里装的是什么？——角速度。 考虑常模向量 $p'$（在刚体系下不动），$p(t)=R(t)p'$：它的模长始终不变，方向却在变。求导并代入 $\dot R=SR$：

$$\dot p(t)=S(t),R(t),p' .$$

另一方面，从力学角度（大学物理）：随坐标系 $R(t)$ 一起转动的点，线速度是角速度叉乘矢径：

$$\dot p(t)=\omega(t)\times p(t)=\omega\times R(t)p' .$$

两式对任意 $p'$ 成立，比较即得 $S(t)R(t)=S(\omega)R(t)$，即

3.1.2 连杆速度：三项合成与两条递推

这一小节的（a）–（e）五步层层递进：先看单个点如何做速度合成（a），再用 DH 记号把它搬到相邻连杆之间（b–c），接着推出角速度递推（d），最后落到单个关节的特例（e）——这些结论稍后组装雅可比时都要用。

（a）点的速度合成。 设点 $P$ 在坐标系 $1$ 中的位置为 $p^1$，坐标系 $1$ 的原点在基系 $0$ 中为 $o_1^0$、姿态为 $R_1^0$：

$$p^0=o_1^0+R_1^0,p^1 .$$

老师强调（0:40:13）：「不能直接加——能直接相加的量都必须在同一空间。要先把 $p^1$ 转到基坐标系，计算才是正确的、有意义的。」两边求时间导数，记 $r_1^0\triangleq R_1^0p^1$（$p^1$ 在基系中的表示），姿态项用 定理 3.1.A：

$$\dot p^0=\dot o_1^0+\dot R_1^0p^1+R_1^0\dot p^1 =\underbrace{\dot o_1^0}_{\text{原点速度}}+\underbrace{\omega_1\times r_1^0}_{\text{牵连转动}}+\underbrace{R_1^0\dot p^1}_{\text{相对速度}}$$

三项合起来，正是大学物理里的「绝对速度 = 牵连速度 + 相对速度」；若 $P$ 与刚体固联（$\dot p^1=0$），它就进一步退化为定轴转动公式 $\dot p=\dot o+\omega\times r$。相对速度项仍带 $R_1^0$——同样是「先转到同一坐标系再相加」。

（b）DH 建系回顾与记号。 要把（a）用到相邻连杆之间，先得把记号交代清楚——沿用第 2 章约定：连杆 $i$ 连接关节 $i$（近端）与关节 $i+1$（远端），坐标系 $i$ 建在远端（原点在关节 $i+1$ 轴上）。老师特意警告（0:43:04）：「也有 modified DH 建在近端的，咱们整本教材都按远端建，去年有同学搞混了这个问题，作业怎么做都不对。」记 $p_{i-1},p_i$ 为两坐标系原点在基系中的位置，$r_{i-1,i}^{i-1}$ 为坐标系 $i$ 的原点相对 $i-1$ 的位置在 $i-1$ 系中的表示，则

$$p_i=p_{i-1}+R_{i-1},r_{i-1,i}^{i-1}.$$

（c）连杆线速度递推。 对上式求时间导数（即把（a）的公式套到两连杆之间），记基系表示 $r_{i-1,i}=R_{i-1}r_{i-1,i}^{i-1}$：

$$\dot p_i=\dot p_{i-1}+R_{i-1}\dot r_{i-1,i}^{i-1}+\omega_{i-1}\times R_{i-1},r_{i-1,i}^{i-1}$$

连杆 $i$ 的线速度 = 前一连杆线速度 $+$ 相对速度 $+$ 前一连杆转动的牵连速度——又是三项合成（0:47:23）。

（d）连杆角速度递推。 从旋转合成 $R_i=R_{i-1}R_i^{i-1}$ 出发，两边求导并用 $\dot R=SR$：

$$S(\omega_i)R_i=S(\omega_{i-1}),R_{i-1}R_i^{i-1}+R_{i-1}S(\omega_{i-1,i}^{i-1})R_i^{i-1}$$

第二项用 定理 3.1.B 把 $R_{i-1}$ 「拿进」算子（$R_{i-1}S(\omega_{i-1,i}^{i-1})=S(R_{i-1}\omega_{i-1,i}^{i-1})R_{i-1}$），三个算子便对同一矩阵 $R_i=R_{i-1}R_i^{i-1}$ 作用，比较得

$$\omega_i=\omega_{i-1}+R_{i-1},\omega_{i-1,i}^{i-1}$$

其中 $\omega_{i-1,i}^{i-1}$ 是坐标系 $i$ 相对 $i-1$ 的角速度在 $i-1$ 系中的表示。「坐标系 $i$ 的角速度 = 前一坐标系的角速度 $+$ 相对角速度」。至于为什么角速度可以直接叠加（0:55:46），老师给的理由是：「两刚体传动相连、牵连到一起就可以叠加——大学物理讲转动合成时思考过的问题，从这里自己看出来。」

（e）单个关节的特例（组装雅可比的零件）。 相邻坐标系之间只有一个关节变量：

• 关节 $i$ 为转动关节（变量 $\vartheta_i$）：$\omega_{i-1,i}=\dot\vartheta_i,z_{i-1}$，且 $r_{i-1,i}^{i-1}$ 不变（$\dot r=0$）；
• 关节 $i$ 为移动关节（变量 $d_i$）：$\omega_{i-1,i}=0$，$r_{i-1,i}^{i-1}$ 沿轴伸缩，$\dot r_{i-1,i}^{i-1}=\dot d_i,z_{i-1}^{i-1}$。

其中 $z_{i-1}$ 是关节 $i$ 轴向的单位向量（基系表示）。

3.1.3 雅可比矩阵的计算：逐列构造（绿框）

3.1.2 的两条递推都是沿链累加的形式；下面把它按「单关节贡献」拆开——每一项正好对应 $J$ 的一列。

线速度。 $p_e(q)$ 是 $q$ 的函数、$q$ 又是时间的函数，复合求导：

$$\dot p_e=\sum_{i=1}^{n}\frac{\partial p_e(q)}{\partial q_i},\dot q_i=J_P,\dot q .$$

（投影仪常年显示不出这个 $\sum$ 符号，老师每年都要口头补，0:59:58。）关键物理解读（1:00:52）：每一项表示其余关节全部锁死时，单个关节的速度对末端线速度的贡献——研究第 $i$ 项时，其他系数全为零。于是逐项用 3.1.2(e)：

• 关节 $i$ 移动：纯平移，$\dfrac{\partial p_e}{\partial q_i}=z_{i-1}$；
• 关节 $i$ 转动：$i\sim n$ 段连成刚体绕轴 $z_{i-1}$ 定轴转动，$\omega=\dot\vartheta_i z_{i-1}$，矢径 $p_e-p_{i-1}$，故 $\dfrac{\partial p_e}{\partial q_i}=z_{i-1}\times(p_e-p_{i-1})$。

角速度。 沿链累加（末端执行器与连杆 $n$ 之间只有固定齐次变换、无关节变量，故 $\omega_e=\omega_n$，1:04:05）：

$$\omega_e=\omega_n=\sum_{i=1}^{n}\omega_{i-1,i}=\sum_{i=1}^{n}J_{O_i},\dot q_i .$$

逐列取法与绿框公式。 把线速度与角速度两部分的结论合起来，就得到逐列的取法：

所需量全部来自正运动学（1:07:26–1:09:52，本节最重要的工程结论）：

• $z_{i-1}$ = 旋转矩阵 $R_{i-1}$（即 $T_{i-1}^0$ 的左上 $3\times3$ 块）的第三列；
• $p_{i-1}$ = $T_{i-1}^0$ 第四列前三行；$p_e$ = $T_n^0$ 第四列前三行。

此外，在求 $T_n^0=A_1^0\cdots A_n^{n-1}$ 的过程中，中间的 $T_1^0,\dots,T_{n-1}^0$ 本来就要逐个算出来——「只要正运动学得到（对每个关节的），几何雅可比矩阵就都得到了」，中间任意关节的雅可比也能顺带求出（1:10:22）。

雅可比的坐标变换。 上述 $J$ 是相对基坐标系建立的。若要在其他坐标系 $u$（约束坐标系、传感器系——「传感器得到的结果都是相对它本身坐标系的」，1:11:30）中表示，由 $\dot p_e^u=R^u\dot p_e$、$\omega_e^u=R^u\omega_e$：

$$\begin{bmatrix}\dot p_e^u\ \omega_e^u\end{bmatrix}=\begin{bmatrix}R^u&0\0&R^u\end{bmatrix}\begin{bmatrix}\dot p_e\ \omega_e\end{bmatrix} \qquad\Longrightarrow\qquad J^u=\begin{bmatrix}R^u&0\0&R^u\end{bmatrix}J .$$

「写代码开发的时候如果不考虑这个因素，结果肯定是错的」（1:12:01）。

3.1 小结与常见错误

• 一条主线：正交性求导 → $\dot R=SR$（$S$ 反对称）→ 常模向量识别出 $S=S(\omega)$ → 点速度三项合成 → 连杆两条递推（$\dot p_i$、$\omega_i$）→ 复合求导拆成单关节贡献 → 绿框逐列公式。
• 两个免费校验：$J$ 的每一列要么是 $\begin{bmatrix}z_{i-1}\times(\cdot)\ z_{i-1}\end{bmatrix}$（转动），要么是 $\begin{bmatrix}z_{i-1}\ 0\end{bmatrix}$（移动），形状不对就说明组装错了；数值上则可以用 $\dot p_e\approx\big(p_e(q+\delta q)-p_e(q-\delta q)\big)/2\delta$ 抽查任一列。