速度运动学：twist 与雅可比Velocity Kinematics: Twists & Jacobians

用 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\) 根螺旋轴在当前位形下的样子。

Teleoperating an arm with a SpaceMouse, keeping a hand-eye camera on a target, steering clear of singular postures in a cramped workcell, pressing on a workpiece with 5 N: all these tasks say the same thing, "the hand should move like this", while the motors only understand "turn each joint this fast". The translator from the first sentence to the second is the Jacobian.

Lesson 3.4 built it in the plane: each column is "the lever arm from joint to hand, rotated by 90°", and the hand's velocity is the superposition of the columns. In 3D, "the hand's velocity" is no longer a vector. Every point of a rigid body moves differently, and the whole velocity field is fixed by six numbers: the angular velocity \(\boldsymbol\omega\) and the linear velocity \(\mathbf v\), together the twist \(\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)\) of Lesson 5.8 (angular part first). So

• Velocity kinematics: \(\boldsymbol\xi = J(\boldsymbol\theta)\,\dot{\boldsymbol\theta}\), with \(J\) a \(6\times n\) matrix. Joint rates → the hand's twist.
• Cartesian velocity control and inverse kinematics: solve for \(\dot{\boldsymbol\theta}\) the other way round (every iteration of Lesson 7.5 does it once). An operator's command is often written in the tool frame and a planner's in the base frame, and those two statements need different Jacobians.
• Singularities: in which postures does \(J\) lose a direction? Is what's lost a translation, a rotation, or a mixture?
• Force and statics: \(\boldsymbol\tau=J^\top\mathbf F\) (Lesson 3.4); in 3D, \(\mathbf F\) is a wrench (torque plus force), the topic of the next lesson, 7.6.

The lever-arm picture of the plane is not enough in 3D: joint axes can point anywhere, and two axes can be skew. The PoE of Lesson 5.10 supplies a better language: every joint is a screw axis. This lesson has one new idea: column \(i\) of the Jacobian is the \(i\)-th screw axis as it looks in the current configuration.

先回顾 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 节）：

Recall Lesson 5.8. When a rigid body moves, the velocity of the body point \(\mathbf p\) is \(\mathbf u(\mathbf p)=\mathbf v+\boldsymbol\omega\times\mathbf p\): knowing \(\boldsymbol\omega\) and the velocity \(\mathbf v\) of "the body point that is at the world origin right now" fixes the whole field. Those six numbers are the space twist \(\boldsymbol\xi_s\). As long as \(\boldsymbol\omega\ne0\), the field is an instantaneous screw motion about a line (Chasles' theorem, Lesson 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\)）时，两个纯转动之和是一个有螺距的螺旋运动：边绕边沿轴前进，像拧螺丝。

At the point \(\mathbf q_c\) on the axis the velocity is exactly \(h\boldsymbol\omega\), purely along the axis: that is the "screw". A revolute joint turning alone is the simplest twist: \(S_i=(\hat{\boldsymbol\omega}_i,\,-\hat{\boldsymbol\omega}_i\times\mathbf q_i)\), with pitch 0 and every point on the axis at rest.

Now let several joints turn at once. The key fact: velocity fields are linear. Adding the velocity fields of several rigid motions point by point gives again the velocity field of a rigid motion, so twists simply add: the hand's twist is the sum, over joints, of "the joint's screw axis at its present place" times the joint rate. The lab below shows it with two joints: axis A is the vertical z-axis, axis B is a horizontal axis you can shift and turn. Both are pure rotations (pitch 0), yet when they are skew (B is a distance \(r\) away from the z-axis), the sum of two pure rotations is a screw motion with pitch: it turns and slides along its axis at the same time, like driving a screw.

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

Take a six-degree-of-freedom arm of the kind in Lesson 5.10: J1 about the vertical z-axis (waist), J2 and J3 the shoulder and elbow (about the y-axis), and J4, J5, J6 three wrist joints whose axes meet in the wrist centre (about x, y, x). The screw axis \(S_i\) of every joint is known at home. After joint 1 has turned by \(\theta_1\), joint 2's axis has been carried to a new place, and this "carried axis" is the second column of the Jacobian. In general:

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 做有限差分"得到的雅可比逐项比较。

FIG 2 draws every column \(J_{s,i}\) as a line: the joint axis, carried to its present place, in the joint's own colour. "Focus" on one joint and the readout gives its six numbers and checks one thing: a point on the axis has zero velocity, \(\mathbf v_i+\boldsymbol\omega_i\times\mathbf p_i=0\), where \(\mathbf p_i\) is a point on the axis (the joint centre). Then give joint rates: each joint's contribution \(\dot\theta_i(\mathbf v_i+\boldsymbol\omega_i\times\mathbf p_{\text{tool}})\) to the velocity at the hand (the tool point) is chained head to tail, ending at the total linear velocity \(\mathbf u\), and the total angular velocity \(\boldsymbol\omega\) is drawn too. Finally the readout compares the analytic \(J_s\) entry by entry with a Jacobian obtained by finite-differencing 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\)。

When \(J\) has rank below 6 the posture is singular: some twist direction cannot be produced however the joints move. Lesson 3.4 met the stretched and folded arm in the plane. A 6R arm has three typical families:

• Shoulder singularity: the wrist centre lies on the J1 axis. Turning J1 then does not move the wrist centre, which loses the direction perpendicular to the arm's plane.
• Elbow singularity: the arm is fully stretched (\(\theta_3=0\)) or fully folded. The wrist centre loses the velocity along the arm; the two-link arm of Lesson 3.4 is exactly this.
• Wrist singularity: \(\theta_5=0\), the axes of J4 and J6 are collinear, the two joints can only produce the same rotation and one direction of rotation is lost.

FIG 3 lets you walk through one family: the slider \(s\) runs from \(-1\) to \(1\), and \(s=0\) is exactly the singular posture. Watch the six singular values \(\sigma_i\) (the last one drops to 0), the sign of the determinant (it flips when you cross, the "jump" between two branches of inverse kinematics), the lost direction (arrows), and the linear-velocity ellipsoid: \(J_v\) is the linear-velocity block of \(J\), and the unit ball of joint rates maps to the ellipsoid of \(J_vJ_v^\top\), whose semi-axes are the singular values of \(J_v\).

Two details deserve attention. First, being singular does not depend on how the six numbers are mixed, but the singular values do: the top half of \(J\) is in m/rad and the bottom half in rad/rad, and an SVD of such a matrix depends on the "characteristic length" \(L\) by which you scale the three rotation rows (Pitfall 4 of Lesson 3.4). The slider \(L\) multiplies the rotation rows by \(L\): the rank stays, but \(\sigma_{\min}\) and the lost direction change. Second, the reference point: the same twist gives different linear velocities at different points. By default the reference is the wrist centre, which makes the lost directions of the shoulder and elbow families clean linear velocities; switch to the tool tip and a \(\boldsymbol\omega\times\mathbf r\) term mixes in.

同一组关节速度 \(\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\) 完全正常。画面里的三根轴是欧拉角的三根转轴：偏航（蓝）、俯仰（绿）、滚转（红）；万向节锁时红轴与蓝轴合成一根。

The same joint rates \(\dot{\boldsymbol\theta}\) can be written down with different "readings" of the hand's motion. The three common ones:

• Space Jacobian \(J_s\): \(\boldsymbol\xi_s=(\boldsymbol\omega_s,\mathbf v_s)\), reference point the world origin, axes of the world frame.
• Body Jacobian \(J_b\): \(\boldsymbol\xi_b=\operatorname{Ad}_{T^{-1}}\boldsymbol\xi_s\), reference point the tool origin, axes of the tool frame. "Go another 2 cm along the gripper's own axis" is said in this language.
• Analytic Jacobian \(J_a\): the derivative of some parametrization of the pose, e.g. the position \(\mathbf p\) plus ZYX Euler angles \((\psi,\theta,\varphi)\): \(\dot{\mathbf x}=(\dot{\mathbf p},\dot\psi,\dot\theta,\dot\varphi)\). The angular velocity is not the derivative of the Euler angles; they differ by a matrix \(E(\psi,\theta,\varphi)\): \(\boldsymbol\omega_s=E\,(\dot\psi,\dot\theta,\dot\varphi)^\top\). For ZYX, \(\det E=-\cos\theta\), and \(E\) is singular at pitch \(\theta=\pm90^\circ\) (gimbal lock).

FIG 4 fixes \(\theta_4=0\), where the hand's orientation is exactly \(R=R_z(\theta_1)R_y(\vartheta)R_x(\theta_6)\) with \(\vartheta=\theta_2+\theta_3+\theta_5\), so the ZYX angles are \((\theta_1,\vartheta,\theta_6)\). Change the hand's pitch \(\vartheta\) with the slider, choose a set of joint rates, and the side panel lists \(\boldsymbol\xi_s\), \(\boldsymbol\xi_b\) and \(\dot{\mathbf x}\) together. Watch what happens to the Euler rates computed from the same joint rates as the pitch approaches \(\pm90^\circ\), while the arm's own \(J_s\) and \(J_b\) stay perfectly healthy. The three axes in the picture are the Euler rotation axes: yaw (blue), pitch (green), roll (red); at gimbal lock the red and blue axes merge into one line.

\(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 不依赖任何选择。

The rank \(r\) of \(J\in\R^{6\times n}\) is the number of independent velocity directions the hand can produce in this posture. \(r<6\) (for \(n\ge6\)) is a singular posture. The null space has dimension \(n-r\): these joint motions leave the hand's pose unchanged and are called self-motions. A 7-joint arm (such as the Franka Panda) has a one-dimensional self-motion in a non-singular posture, usable for dodging obstacles or staying away from joint limits (Lesson 3.4's null-space formula \(\dot{\boldsymbol\theta}=J^+\boldsymbol\xi+(I-J^+J)\mathbf z\) applies unchanged).

Singularity and the wrench duality: the null space of \(J^\top\) is non-empty, i.e. there is \(\boldsymbol\lambda\ne0\) with \(J^\top\boldsymbol\lambda=0\). Read \(\boldsymbol\lambda\) as a wrench on the hand (torque plus force, Lesson 7.6), then \(\boldsymbol\tau=J^\top\boldsymbol\lambda=0\): this wrench is carried entirely by the link structure and needs no joint torque. This is the general 3D form of Lesson 3.4's "a stretched arm can hold a heavy weight", and the direction of the same \(\boldsymbol\lambda\) is the direction in which the hand cannot be moved however the joints turn.

Three common scalars: Yoshikawa's manipulability \(w=\sqrt{\det(JJ^\top)}=\prod_i\sigma_i\) (\(=|\det J|\) for square \(J\)); the smallest singular value \(\sigma_{\min}\), a "distance" to a singularity; and the condition number \(\kappa=\sigma_1/\sigma_{\min}\), a measure of isotropy. Units are a trap: mix the top half of \(J\) (rotation, rad/rad) with the bottom half (translation, m/rad) and the singular values depend on the length unit. Three remedies: analyze \(J_\omega\) and \(J_v\) separately; multiply the rotation rows by a characteristic length \(L\) (what FIG 3 does); or use a weighted metric whose weights come from the task's tolerances. Rank is unit-free: whether a determinant is zero depends on no choice at all.

遥操作、示教拖动、贴着表面擦拭、给相机一个"往目标靠近"的命令……最终都落在同一条流水线上：每个控制周期读出当前关节角，算雅可比，解 \(\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\) 可调）保持有界，代价是慢下来、偏离。

Teleoperation, kinesthetic teaching, wiping along a surface, giving a camera a "move toward the target" command... all end up in the same pipeline: each control cycle read the joint angles, compute the Jacobian, solve \(\dot{\boldsymbol\theta}=J^{+}\boldsymbol\xi_{\text{cmd}}\) (or damped least squares), integrate, and send the result to the motors. This is resolved-rate control. The lab below runs it on the 6R arm with three goals:

• Which frame is the command in? A "base-frame" command goes through the hybrid Jacobian, a "tool-frame" command through \(J_b\). Choose "mismatched" and you feed the numbers of one frame to the Jacobian of the other, and the hand heads off in a strange direction.
• Open-loop integration drifts. \(J\) is only valid instantaneously, and discrete integration (FIG 5 uses \(\Delta t=0.02\) s) carries a first-order error: with the pseudo-inverse, the end error in the readout comes from here; damped least squares adds a bias of its own from the damping (at the default \(\lambda=0.05\) that bias is actually the larger part). Real controllers add pose feedback, \(\boldsymbol\xi=\boldsymbol\xi_{\text{ff}}+K\mathbf e\).
• What about near a singularity? Turn the speed up and drive the hand toward the limit of the arm's reach: the pseudo-inverse demands joint rates that shoot to the sky, damped least squares (adjustable \(\lambda\)) stays bounded, at the price of slowing down and deviating.