速度运动学：twist 与雅可比

用 SpaceMouse 遥操作机械臂、让手眼相机盯着一个目标、在狭窄的工位里躲开奇异位形、让末端以 5 N 的力压住工件……这些任务都在说同一句话："手爪应该这样动"，而电机只听得懂"每个关节转多快"。把前一句翻译成后一句的，是雅可比。

3.4 节在平面里造出了它：每一列是"关节到手爪的杠杆臂转 90°"，手爪的速度是各列的叠加。到了三维，"手爪的速度"不再是一个向量。刚体上每个点的速度都不同，整个速度场由 6 个数决定：角速度 \(\boldsymbol\omega\) 和线速度 \(\mathbf v\)，合起来就是 5.8 节的 twist \(\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)\)（角速度在前）。于是

• 速度运动学：\(\boldsymbol\xi = J(\boldsymbol\theta)\,\dot{\boldsymbol\theta}\)，\(J\) 是 \(6\times n\) 的矩阵。关节速度 → 手爪的 twist。
• 笛卡尔速度控制与逆运动学：反过来解 \(\dot{\boldsymbol\theta}\)（7.5 节每一步迭代都要解一次）。操作员给的命令常常写在工具系里，规划器给的写在基座系里，这两种说法要用不同的雅可比。
• 奇异位形：\(J\) 在哪些姿态下丢掉一个方向？丢掉的是平移、转动，还是两者的混合？
• 力与静力学：\(\boldsymbol\tau=J^\top\mathbf F\)（3.4 节），三维里的 \(\mathbf F\) 是力矩加力的 wrench，下一课 7.6 讲。

平面里的杠杆臂图景到三维不够用了：关节轴可以指向任意方向，轴与轴之间还可以是异面的。5.10 节的 PoE 给出了更好的语言：每个关节是一根螺旋轴。这一课只有一个新想法：雅可比的第 \(i\) 列，就是第 \(i\) 根螺旋轴在当前位形下的样子。

先回顾 5.8 节。刚体运动时，体上每个点 \(\mathbf p\) 的速度是 \(\mathbf u(\mathbf p)=\mathbf v+\boldsymbol\omega\times\mathbf p\)：只要知道 \(\boldsymbol\omega\) 和"此刻恰在世界原点的那个点"的速度 \(\mathbf v\)，整个场就定了。这 6 个数就是空间 twist \(\boldsymbol\xi_s\)。只要 \(\boldsymbol\omega\ne0\)，这个速度场就是一次绕某条直线的螺旋运动（Chasles 定理，5.6 节）：

在轴上的点 \(\mathbf q_c\)，速度恰好是 \(h\boldsymbol\omega\)，只沿轴方向：这就是"螺旋"。每个转动关节单独转时，是最简单的 twist：\(S_i=(\hat{\boldsymbol\omega}_i,\,-\hat{\boldsymbol\omega}_i\times\mathbf q_i)\)，螺距为 0，轴上所有点速度为零。

现在让几个关节同时转。关键事实：速度场是线性的。几个刚体运动的速度场逐点相加，得到的还是一个刚体运动的速度场，所以twist 可以直接相加：手爪的 twist 就是每个关节"在当前位置的螺旋轴"乘以关节速度之和。下面的实验用两个关节看清这件事：A 轴是竖直的 z 轴，B 轴是水平的、可以平移和转向的一根轴。两根轴都是纯转动（螺距 0），可是当它们异面（B 轴离开 z 轴一段距离 \(r\)）时，两个纯转动之和是一个有螺距的螺旋运动：边绕边沿轴前进，像拧螺丝。

用 5.10 节那类六自由度臂：肩上的 J1 绕竖直 z 轴（腰），J2、J3 是肩和肘（绕 y 轴），J4、J5、J6 是三个相交于手腕中心的腕关节（绕 x、y、x）。零位时每个关节的螺旋轴 \(S_i\) 都已知。关节 1 转过 \(\theta_1\) 之后，关节 2 的轴被带到了新的位置，这根"被搬运过的轴"就是雅可比的第二列。一般地：

FIG 2 把每一列 \(J_{s,i}\) 画成一条线：线是被搬运到当前位置的关节轴，颜色与关节一一对应。选一个关节"聚焦"，读数会给出它的 6 个数，并核对一件事：轴上的点速度为零，即 \(\mathbf v_i+\boldsymbol\omega_i\times\mathbf p_i=0\)，\(\mathbf p_i\) 是轴上的点（关节中心）。再给出关节速度，各关节在手爪（工具点）处产生的速度 \(\dot\theta_i(\mathbf v_i+\boldsymbol\omega_i\times\mathbf p_{\text{tool}})\) 首尾相连，终点就是总的线速度 \(\mathbf u\)，同时画出总的角速度 \(\boldsymbol\omega\)。最后，读数把解析的 \(J_s\) 与"对 FK 做有限差分"得到的雅可比逐项比较。

\(J\) 的秩小于 6，就是奇异位形：存在一个 twist 方向，无论关节怎么转都产生不了。3.4 节在平面里看到了伸直和折回。6R 臂有三族典型的奇异位形：

• 肩奇异：手腕中心落在 J1 的轴上。此时 J1 转动不会使手腕中心移动，手腕中心失去了垂直于手臂平面的那个方向。
• 肘奇异：手臂完全伸直（\(\theta_3=0\)）或完全折回。手腕中心失去沿手臂方向的那个速度，3.4 节的二连杆就是它。
• 腕奇异：\(\theta_5=0\)，J4 与 J6 的轴共线，两个关节只能产生同一种转动，一个方向的转动丢失了。

FIG 3 让你沿着三族中的一族"穿过"奇异点：滑块 \(s\) 从 \(-1\) 走到 \(1\)，\(s=0\) 恰在奇异位形上。观察六个奇异值 \(\sigma_i\)（最后一个降到 0）、行列式的符号（穿过时翻号：对应逆运动学两个分支之间的"跨越"）、丢失的方向（箭头），以及线速度椭球：\(J_v\) 是 \(J\) 的线速度分块，单位关节速度球映到 \(J_vJ_v^\top\) 的椭球，半轴是 \(J_v\) 的奇异值。

有两个细节值得注意。第一，奇异位形与 6 个数怎么混合无关，奇异值却有关：\(J\) 的上半是 m/rad，下半是 rad/rad，对这样的矩阵做 SVD，结果取决于你给转动那三行乘了多大的"特征长度" \(L\)（3.4 节误区 4）。滑块 \(L\) 把转动行乘上 \(L\)：秩不变，\(\sigma_{\min}\) 与丢失方向却会变。第二，参考点：同一个 twist，在不同的点上读线速度是不同的数。默认把参考点放在手腕中心，那样肩、肘两族的丢失方向是干净的线速度；换成工具点，线速度里会混进 \(\boldsymbol\omega\times\mathbf r\)。

同一组关节速度 \(\dot{\boldsymbol\theta}\)，手爪的运动可以用不同的"读法"写下来。三种常见的读法：

• 空间雅可比 \(J_s\)：\(\boldsymbol\xi_s=(\boldsymbol\omega_s,\mathbf v_s)\)，参考点是世界原点，轴取世界系。
• 机体雅可比 \(J_b\)：\(\boldsymbol\xi_b=\operatorname{Ad}_{T^{-1}}\boldsymbol\xi_s\)，参考点是工具原点，轴取工具系。"沿夹爪自己的轴再走 2 cm"就是在这里说的。
• 解析雅可比 \(J_a\)：对位姿的某种参数化求导，如位置 \(\mathbf p\) 加 ZYX 欧拉角 \((\psi,\theta,\varphi)\)：\(\dot{\mathbf x}=(\dot{\mathbf p},\dot\psi,\dot\theta,\dot\varphi)\)。角速度不是欧拉角的导数，两者差一个矩阵 \(E(\psi,\theta,\varphi)\)：\(\boldsymbol\omega_s=E\,(\dot\psi,\dot\theta,\dot\varphi)^\top\)。ZYX 时 \(\det E=-\cos\theta\)，俯仰角 \(\theta=\pm90^\circ\)（万向节锁）时 \(E\) 奇异。

FIG 4 固定 \(\theta_4=0\)，这时手爪的朝向恰好是 \(R=R_z(\theta_1)R_y(\vartheta)R_x(\theta_6)\)，\(\vartheta=\theta_2+\theta_3+\theta_5\)，ZYX 角就是 \((\theta_1,\vartheta,\theta_6)\)。用滑块改变手爪俯仰角 \(\vartheta\)，选一组关节速度，侧栏同时列出 \(\boldsymbol\xi_s\)、\(\boldsymbol\xi_b\) 和 \(\dot{\mathbf x}\)。看俯仰角靠近 \(\pm90^\circ\) 时，同一组关节速度算出的欧拉角速度如何爆炸，而手臂自己的雅可比 \(J_s\) 与 \(J_b\) 完全正常。画面里的三根轴是欧拉角的三根转轴：偏航（蓝）、俯仰（绿）、滚转（红）；万向节锁时红轴与蓝轴合成一根。

\(J\in\R^{6\times n}\) 的秩 \(r\) 是手爪在这个姿态下能独立产生的速度方向数。\(r<6\)（\(n\ge6\) 时）就是奇异位形。零空间有 \(n-r\) 维：这些关节运动不改变手爪的位姿，叫自运动。\(n=7\)（如 Franka Panda）的臂在非奇异位形有一维自运动，可以用它去躲障碍或避开关节限位（3.4 节的零空间公式 \(\dot{\boldsymbol\theta}=J^+\boldsymbol\xi+(I-J^+J)\mathbf z\) 原样适用）。

奇异性与 wrench 对偶：\(J^\top\) 的零空间不为空，即存在 \(\boldsymbol\lambda\ne0\) 使 \(J^\top\boldsymbol\lambda=0\)。把 \(\boldsymbol\lambda\) 看成手爪上的 wrench（力矩加力，7.6 节），\(\boldsymbol\tau=J^\top\boldsymbol\lambda=0\)：这个 wrench 完全由连杆结构承受，不需要任何关节力矩。这就是 3.4 节"伸直的手臂能举重物"在三维的一般形式；同一个 \(\boldsymbol\lambda\) 的方向也是"无论怎么转关节，手爪都走不动"的方向。

三个常用的标量：Yoshikawa 可操作度 \(w=\sqrt{\det(JJ^\top)}=\prod_i\sigma_i\)（方阵时 \(=|\det J|\)）；最小奇异值 \(\sigma_{\min}\)，到奇异位形的"距离"；条件数 \(\kappa=\sigma_1/\sigma_{\min}\)，各向同性程度。量纲是个陷阱：\(J\) 的上半（转动，rad/rad）和下半（平移，m/rad）混在一起，奇异值就依赖于长度单位。办法有三：分别分析 \(J_\omega\) 和 \(J_v\)；给转动行乘一个特征长度 \(L\)（FIG 3 的做法）；或者用"带权"的度量，权重来自任务的容许误差。秩与单位无关，行列式是否为 0 不依赖任何选择。

遥操作、示教拖动、贴着表面擦拭、给相机一个"往目标靠近"的命令……最终都落在同一条流水线上：每个控制周期读出当前关节角，算雅可比，解 \(\dot{\boldsymbol\theta}=J^{+}\boldsymbol\xi_{\text{cmd}}\)（或阻尼最小二乘），积分后发给电机。这叫 resolved-rate 控制。下面的实验把它搬到 6R 臂上，目的有三：

• 命令在哪个坐标系里？"基座系"命令走混合雅可比，"工具系"命令走 \(J_b\)。选"不匹配"，就是把一个坐标系里的数字喂给另一个坐标系的雅可比，手爪会朝奇怪的方向走。
• 开环积分会漂。\(J\) 只是瞬时有效，离散积分（FIG 5 用 \(\Delta t=0.02\) s）有一阶误差：用伪逆时，读数里的末端误差就来自这里；阻尼最小二乘还会再加上一份由阻尼带来的偏差（默认 \(\lambda=0.05\) 时它反而是主要部分）；真实控制器会加位姿反馈 \(\boldsymbol\xi=\boldsymbol\xi_{\text{ff}}+K\mathbf e\)。
• 奇异位形附近怎么办？把速度调大，让手爪冲向手臂伸展的极限：伪逆要求的关节速度冲上天，阻尼最小二乘（\(\lambda\) 可调）保持有界，代价是慢下来、偏离。