Part I · 第 3 章多元与向量微积分 · 3.4Part I · Ch. 3 Multivariable & Vector Calculus · 3.4

机器人雅可比The Robot Jacobian

关节在转，手爪怎么动？反过来，想让手爪以某个速度走，关节该转多快？手爪要推一个物体，各个关节要出多大力矩？这三个问题共用同一个矩阵：机械臂雅可比。它的每一列都有一个朴素的几何含义：关节到手爪的"杠杆臂"转 90°。看懂这一点，奇异位形、可操作度椭圆、力与速度的对偶、数值逆运动学就都顺理成章了。 The joints turn: how does the gripper move? Conversely, to move the gripper at a given velocity, how fast must each joint turn? And to push on an object, how much torque must each joint supply? All three questions share one matrix: the manipulator Jacobian. Each of its columns has a plain geometric meaning: the "lever arm" from a joint to the gripper, rotated by 90°. See that, and singularities, the manipulability ellipse, the force–velocity duality and numerical inverse kinematics all follow naturally.

约 30 分钟~30 min 前置：1.12 SVD · 1.13 最小二乘与伪逆 · 3.3 雅可比Prereqs: 1.12 SVD · 1.13 Least Squares & Pseudo-inverse · 3.3 The Jacobian 5 个交互5 interactives

1为什么机器人需要它Why robots need this

用遥操作手柄或者视觉伺服去控制机械臂，你想要的是"手爪朝这个方向、以这个速度走"，电机能听懂的却是"每个关节转多快"。中间的翻译官就是雅可比 $J(\boldsymbol\theta)$：

速度运动学：$\mathbf v = J(\boldsymbol\theta)\,\dot{\boldsymbol\theta}$，关节速度 → 手爪速度。
微分/数值逆运动学：$\dot{\boldsymbol\theta} = J^{+}\mathbf v$，手爪速度 → 关节速度。每一步迭代都要解一次。
静力学：$\boldsymbol\tau = J^\top \mathbf F$，手爪受力 → 关节力矩。力控制、阻抗控制、接触力估计都靠它。
奇异性与可操作性：$J$ 什么时候"失去一个方向"？手臂在哪些姿态下又快又灵活？

3.3 节已经说过雅可比是局部线性变换，并且对两连杆臂做了求导。这一节换一个角度：不求导，直接从几何出发造出雅可比，然后用它搭一个能用的逆运动学求解器。（等到 5.10 节学了指数积公式，会看到同样的结构在三维中的推广。）

Whether you teleoperate an arm with a joystick or servo it from vision, what you want is "move the gripper this way at this speed", while the motors only understand "turn each joint this fast". The translator in between is the Jacobian $J(\boldsymbol\theta)$:

Velocity kinematics: $\mathbf v = J(\boldsymbol\theta)\,\dot{\boldsymbol\theta}$, joint velocities → gripper velocity.
Differential/numerical inverse kinematics: $\dot{\boldsymbol\theta} = J^{+}\mathbf v$, gripper velocity → joint velocities. Every iteration solves this once.
Statics: $\boldsymbol\tau = J^\top \mathbf F$, gripper force → joint torques. Force control, impedance control and contact-force estimation all rely on it.
Singularities and manipulability: when does $J$ "lose a direction"? In which postures is the arm fast and agile?

Lesson 3.3 showed that a Jacobian is a local linear map and differentiated a two-link arm. This lesson takes another route: build the Jacobian straight from geometry, without differentiating, then use it to assemble a working inverse-kinematics solver. (Once you meet the product of exponentials in Lesson 5.10, you will see the same structure carried over to 3D.)

2直觉：一次只转一个关节，手爪画一段圆弧Intuition: turn one joint, and the tip sweeps a circle

考虑平面内的串联臂，每个关节都是转轴垂直于纸面的转动关节。先只转第 $i$ 个关节，其余关节锁住：位于第 $i$ 个关节之后的整条手臂变成一个刚体，绕关节 $i$ 的转轴转动，手爪自然绕这个轴心画一段圆弧。圆弧的半径，就是关节 $i$ 到手爪的距离 $\|\mathbf r_i\|$，其中 $\mathbf r_i = \mathbf p_{\text{tip}} - \mathbf p_i$，称为杠杆臂。

刚体绕定轴转动时，距离轴心 $r$ 的点速度大小是 $\omega r$，方向垂直于杠杆臂。平面里"垂直"有一个现成的算子：把 $\mathbf r$ 逆时针转 90°，即 $\hat{\mathbf z}\times\mathbf r = (-r_y,\;r_x)$。所以关节 $i$ 以单位角速度转动时，手爪的速度就是

Consider a planar serial arm in which every joint is a revolute joint with its axis perpendicular to the page. First turn only joint $i$ and lock all the others: everything beyond joint $i$ becomes one rigid body spinning about joint $i$'s axis, so the tip naturally sweeps an arc about that axis. The arc's radius is the distance $\|\mathbf r_i\|$ from joint $i$ to the tip, where $\mathbf r_i = \mathbf p_{\text{tip}} - \mathbf p_i$ is the lever arm.

For a rigid body spinning about a fixed axis, a point at distance $r$ from the axis has speed $\omega r$, directed perpendicular to the lever arm. In the plane, "perpendicular" has a ready-made operator: rotate $\mathbf r$ counter-clockwise by 90°, i.e. $\hat{\mathbf z}\times\mathbf r = (-r_y,\;r_x)$. So when joint $i$ turns at unit angular rate, the tip velocity is

$$ \mathbf J_i \;=\; \frac{\partial \mathbf p_{\text{tip}}}{\partial \theta_i} \;=\; \hl{z}{\hat{\mathbf z}\times}\;\hl{lever}{\big(\mathbf p_{\text{tip}} - \mathbf p_i\big)} \;=\; \begin{bmatrix} -(y_{\text{tip}} - y_i) \\ \;\;x_{\text{tip}} - x_i \end{bmatrix}. $$

下图是一条三连杆臂。选一个关节，看它的杠杆臂（虚线）、手爪走的圆弧，以及速度箭头 $\mathbf J_i$。拖动关节上的圆圈改变姿态，箭头始终垂直于虚线，长度等于虚线的长度。

Below is a three-link arm. Pick a joint and look at its lever arm (dashed), the arc the tip travels, and the velocity arrow $\mathbf J_i$. Drag the circles on the joints to change the posture: the arrow always stays perpendicular to the dashed line, and has the same length.

FIG 1杠杆臂与单关节速度Lever arm and one-joint velocity

三个关节分别用红、绿、蓝表示（后面雅可比的三列也用同样的颜色）。粗弧线是只转这个关节 ±0.5 rad 时手爪的轨迹；实心箭头是单位角速度下手爪的速度 $\mathbf J_i$，暗淡的箭头是另外两列（箭头均按 0.6 倍长度绘制）。拖动圆圈：关节 $k$ 处的圆圈决定第 $k$ 根连杆的朝向。 The three joints are red, green and blue (the three Jacobian columns will keep these colors). The thick arc is the tip's path when only this joint turns by ±0.5 rad; the solid arrow is the tip velocity $\mathbf J_i$ at unit joint rate, and the faded arrows are the other two columns (all arrows drawn at 0.6× length). Drag the circles: the circle at the end of link $k$ sets that link's direction.

依次选中三个关节，观察红、绿、蓝箭头：离手爪越远的关节，箭头越长。Select the three joints in turn and watch the red, green and blue arrows: the farther a joint is from the tip, the longer its arrow.
把手臂折回，让关节 2 的箭头比关节 3 的更短。关节 3 的箭头长度永远等于什么？Fold the arm back so that joint 2's arrow is shorter than joint 3's. What does the length of joint 3's arrow always equal?

核心洞见Key insight

雅可比的第 $i$ 列 = 把关节 $i$ 到手爪的杠杆臂转 90°。转得越"外面"的关节杠杆臂越长，对手爪速度的贡献越大；手爪每个时刻的速度，就是各关节贡献的叠加：$\mathbf v = \sum_i \dot\theta_i\,\mathbf J_i$。

Column $i$ of the Jacobian is the lever arm from joint $i$ to the tip, rotated by 90°. Joints farther from the tip have longer lever arms and contribute more to the tip velocity, and the tip velocity at each instant is the superposition of the joint contributions: $\mathbf v = \sum_i \dot\theta_i\,\mathbf J_i$.

3动手玩：关节速度的叠加Play: superposing joint velocities

现在三个关节同时转。用滑块给出关节角速度 $\dot\theta_1,\dot\theta_2,\dot\theta_3$：红、绿、蓝箭头是 $\dot\theta_i\mathbf J_i$，首尾相接地从手爪出发，终点连成的琥珀色箭头就是手爪的速度 $\mathbf v$。紫色虚线是"按这个速度真实运动一小段时间"时手爪走过的路径。因为雅可比只是一阶近似，时间越短，琥珀色预测与真实位置越贴合。

Now all three joints turn at once. Set the joint rates $\dot\theta_1,\dot\theta_2,\dot\theta_3$ with the sliders: the red, green and blue arrows are $\dot\theta_i\mathbf J_i$, chained head to tail from the tip, and the amber arrow joining the ends is the tip velocity $\mathbf v$. The purple dashed path is where the tip really goes if the joints keep these rates for a short time. Since the Jacobian is only a first-order approximation, the shorter that time, the better the amber prediction matches the true position.

FIG 2速度叠加实验室Velocity superposition lab

箭头按 1 s 的位移画出（单位 m/s 对应 1 m）。琥珀色空心圆圈是 $\mathbf p + \mathbf v\,T$，紫色路径是关节匀速转动 $T$ 秒、手爪的真实轨迹，终点的实心紫点是真实位置。读数里的"几何 − 数值"是用上面的杠杆臂公式算出的雅可比，与对 FK 做数值差分得到的雅可比的最大差，应当在 $10^{-10}$ 量级（极小）。 Arrows are drawn as the displacement over 1 s (1 m/s ↔ 1 m). The hollow amber ring is $\mathbf p + \mathbf v\,T$; the purple path is the tip's true trajectory when the joints turn at constant rates for $T$ seconds, and the solid purple dot its end. The "geometric − numeric" readout is the largest difference between the Jacobian from the lever-arm formula and one obtained by numerically differencing FK; it should be around $10^{-10}$ (tiny).

试一试Try this

用三个滑块让手爪竖直向上运动：$v_y>0.6$，$|v_x|<0.04$。Use the three sliders to make the tip move straight up: $v_y>0.6$, $|v_x|<0.04$.
切换成"2 连杆"。能否让手爪向任意方向运动？换成"3 连杆"，同样的手爪速度有几种关节速度组合？Switch to "2 links". Can the tip move in any direction? Switch to "3 links": how many joint-rate combinations give the same tip velocity?

注意：2 个关节、2 维速度，方程数等于未知数，解唯一；3 个关节、2 维速度，未知数比方程多一个，满足同一个 $\mathbf v$ 的关节速度有无穷多组，它们之间相差一个"手爪完全不动"的关节运动。这叫冗余，后面的逆运动学与零空间控制里会反复遇到。

Note: with 2 joints and a 2D velocity there are as many equations as unknowns and the solution is unique; with 3 joints and a 2D velocity there is one more unknown than equations, so infinitely many joint-rate sets give the same $\mathbf v$, differing by a joint motion that leaves the tip still. This is redundancy, which returns in inverse kinematics and null-space control.

4第二、第三视角：椭圆与力Second and third views: the ellipse and the force

可操作度椭圆与奇异位形The manipulability ellipse and singular postures

1.12 节说过：矩阵把单位圆变成椭圆，半轴就是奇异值。这里的"单位圆"是所有满足 $\|\dot{\boldsymbol\theta}\|\le 1$ 的关节速度，它在 $J$ 的作用下变成手爪速度的一个椭圆，即可操作度椭圆：椭圆越圆，手爪在各个方向上都同样灵活；椭圆被压扁成线段，说明某个方向上无论关节怎么转，手爪都走不动，这就是奇异位形。右图是 $J$ 的两个奇异值 $\sigma_1\ge\sigma_2$ 随肘部角度 $\theta_2$ 的变化曲线。

Lesson 1.12 said a matrix turns the unit circle into an ellipse whose semi-axes are the singular values. Here the "unit circle" is the set of all joint velocities with $\|\dot{\boldsymbol\theta}\|\le 1$; under $J$ it becomes an ellipse of tip velocities, the manipulability ellipse. A rounder ellipse means the tip is equally agile in every direction; an ellipse squashed to a segment means some direction cannot be moved in, no matter how the joints turn: a singular posture. The right panel plots the two singular values $\sigma_1\ge\sigma_2$ of $J$ against the elbow angle $\theta_2$.

FIG 3可操作度椭圆Manipulability ellipse

工作空间：|θ̇| ≤ 1 的速度椭圆WORKSPACE · velocity ellipse for |θ̇| ≤ 1

σ₁, σ₂ 随 θ₂ 变化σ₁, σ₂ vs. θ₂

琥珀色椭圆的半轴是 $\sigma_1$、$\sigma_2$，方向是 $JJ^\top$ 的特征向量。右图中红线是 $\sigma_1$，绿线是 $\sigma_2$，竖直虚线标出当前的 $\theta_2$。2 连杆时 $\sigma_2$ 在 $\theta_2 = 0, \pm\pi$ 处降到 0。 The semi-axes of the amber ellipse are $\sigma_1$, $\sigma_2$, pointing along the eigenvectors of $JJ^\top$. In the right panel the red curve is $\sigma_1$, the green curve $\sigma_2$, and the vertical dashed line marks the current $\theta_2$. For two links $\sigma_2$ drops to 0 at $\theta_2 = 0, \pm\pi$.

用"2 连杆"，把手臂拉直，让 $\sigma_2<0.03$：椭圆变成一条线段。线段沿什么方向？With "2 links", stretch the arm out until $\sigma_2<0.03$: the ellipse becomes a segment. Along what direction does it lie?
用"3 连杆"，把椭圆调成几乎正圆（$\sigma_1/\sigma_2<1.15$，读数里的 κ）。再试"2 连杆"：这组连杆长度下它最好只能到约 1.19（在 $\theta_2\approx\pm3\pi/4$），因为两连杆要做到严格正圆需要 $l_1=\sqrt2\,l_2$。With "3 links", make the ellipse almost a circle ($\sigma_1/\sigma_2<1.15$, the κ in the readout). Then try "2 links": with these link lengths the best it can do is about 1.19 (at $\theta_2\approx\pm3\pi/4$), because an exactly round ellipse with two links needs $l_1=\sqrt2\,l_2$.

力的对偶：$\boldsymbol\tau = J^\top\mathbf F$The force duality: $\boldsymbol\tau = J^\top\mathbf F$

让手爪顶住一个物体、对它施加力 $\mathbf F$，各关节需要多大力矩来"撑住"？用虚功原理：手爪位移 $\delta\mathbf x = J\,\delta\boldsymbol\theta$ 时，手爪做的功 $\mathbf F^\top\delta\mathbf x$ 必须等于各关节做的功 $\boldsymbol\tau^\top\delta\boldsymbol\theta$。对任意 $\delta\boldsymbol\theta$ 成立，就得到 $\boldsymbol\tau = J^\top\mathbf F$。几何上更直观：$\tau_i = \mathbf J_i\cdot\mathbf F = \hat{\mathbf z}\cdot(\mathbf r_i\times\mathbf F)$，就是力 $\mathbf F$ 对关节 $i$ 的力矩。

于是同一个矩阵管着两件事：$J$ 把关节速度映到手爪速度，$J^\top$ 把手爪的力映到关节力矩。速度椭圆的长轴方向"容易动"，在这个方向上，同样的关节力矩只能撑起很小的力；速度椭圆的短轴方向"难动"，同样的力矩却能撑起很大的力。力椭圆（紫色）的半轴恰好是 $1/\sigma_i$：形状与速度椭圆互为倒数。

Let the gripper push on an object with force $\mathbf F$: how much torque must each joint supply to "hold it up"? By the principle of virtual work: for a tip displacement $\delta\mathbf x = J\,\delta\boldsymbol\theta$, the work $\mathbf F^\top\delta\mathbf x$ at the tip must equal the work $\boldsymbol\tau^\top\delta\boldsymbol\theta$ done at the joints. Demanding this for every $\delta\boldsymbol\theta$ gives $\boldsymbol\tau = J^\top\mathbf F$. Geometrically it is even simpler: $\tau_i = \mathbf J_i\cdot\mathbf F = \hat{\mathbf z}\cdot(\mathbf r_i\times\mathbf F)$, which is just the moment of $\mathbf F$ about joint $i$.

So one matrix runs two things: $J$ maps joint velocities to tip velocity, $J^\top$ maps tip force to joint torques. Along the long axis of the velocity ellipse the tip moves easily, and the same joint torques can then support only a small force; along the short axis the tip is hard to move, and the same torques can support a large force. The force ellipse (purple) has semi-axes $1/\sigma_i$: its shape is the reciprocal of the velocity ellipse's.

FIG 4手爪的力与关节力矩Tip force and joint torques

τ₁

τ₂

τ₃

拖动琥珀色手柄设定手爪力 $\mathbf F$（1 m 对应 1 N），拖动关节圆圈改变姿态。红色竖线是关节力矩上限 $\tau_{\max}$，超限的力矩条变红。浅琥珀色椭圆是速度椭圆，紫色椭圆是力椭圆 $\{\mathbf F : \|J^\top\mathbf F\|\le 1\}$，半轴超过 4 N 的部分被截断。 Drag the amber handle to set the tip force $\mathbf F$ (1 m ↔ 1 N) and the joint circles to change the posture. The red ticks are the joint torque limit $\tau_{\max}$; a torque bar turns red when over its limit. The faint amber ellipse is the velocity ellipse, the purple one is the force ellipse $\{\mathbf F : \|J^\top\mathbf F\|\le 1\}$, with semi-axes clipped at 4 N.

让手爪力 $\|\mathbf F\|\ge 1$ N，而所有关节力矩都几乎为 0（$\|\boldsymbol\tau\|<0.12$）。提示：什么姿态下，沿着手臂推一把，关节完全不需要出力？Make $\|\mathbf F\|\ge 1$ N while every joint torque is almost 0 ($\|\boldsymbol\tau\|<0.12$). Hint: in what posture can you push along the arm and ask nothing of the joints?

速度与力：一个矩阵，两个方向Velocity and force: one matrix, two directions

$\mathbf v = J\dot{\boldsymbol\theta}$（关节 → 手爪）和 $\boldsymbol\tau = J^\top\mathbf F$（手爪的力 → 关节）。奇异位形下手爪沿某个方向"走不动"，同一个方向上手爪却能"顶得住"，结构本身承担了载荷，关节力矩为零：这是为什么伸直的手臂可以举很重的东西，也是为什么这样的姿态在力控制里不可靠，因为这个方向上对力几乎没有控制能力。

$\mathbf v = J\dot{\boldsymbol\theta}$ (joints → tip) and $\boldsymbol\tau = J^\top\mathbf F$ (tip force → joints). In a singular posture the tip cannot move along some direction, yet along that very direction it can "hold its ground": the structure itself carries the load and the joint torques are zero. That is why a straight arm can hold a heavy weight, and also why such a posture is useless for force control, since you have almost no authority over force in that direction.

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的元素会被高亮。Tip: hover the highlighted symbols in the formulas; the matching element in FIG 1 lights up.

从正运动学求导，验证几何公式Differentiate forward kinematics to confirm the geometric formula

$n$ 连杆平面臂，连杆长 $l_k$，第 $k$ 根连杆的绝对朝向 $\phi_k = \theta_1+\cdots+\theta_k$。手爪位置与关节 $i$ 的位置是：

For a planar arm with $n$ links of length $l_k$, let link $k$'s absolute heading be $\phi_k = \theta_1+\cdots+\theta_k$. The tip position and the position of joint $i$ are:

$$ \mathbf p_{\text{tip}} = \sum_{k=1}^{n} l_k \begin{bmatrix}\cos\phi_k\\ \sin\phi_k\end{bmatrix},\qquad \mathbf p_i = \sum_{k=1}^{i-1} l_k \begin{bmatrix}\cos\phi_k\\ \sin\phi_k\end{bmatrix}. $$

$\theta_i$ 只出现在 $\phi_k$（$k\ge i$）里，且 $\partial\phi_k/\partial\theta_i = 1$，因此

$\theta_i$ appears only in $\phi_k$ for $k\ge i$, with $\partial\phi_k/\partial\theta_i = 1$, hence

$$ \frac{\partial\mathbf p_{\text{tip}}}{\partial\theta_i} = \sum_{k=i}^{n} l_k\begin{bmatrix}-\sin\phi_k\\ \cos\phi_k\end{bmatrix} = \hat{\mathbf z}\times\Big(\sum_{k=i}^{n} l_k\begin{bmatrix}\cos\phi_k\\ \sin\phi_k\end{bmatrix}\Big) = \hat{\mathbf z}\times\big(\mathbf p_{\text{tip}} - \mathbf p_i\big). $$

求导和杠杆臂两条路给出同一个结果。对 $n=2$ 就是 3.3 节的 $J = \begin{bmatrix}-l_1s_1-l_2s_{12} & -l_2s_{12}\\ l_1c_1+l_2c_{12} & l_2c_{12}\end{bmatrix}$；第二列只含 $l_2$，因为关节 2 之后只有一根连杆。

Differentiating and using lever arms give the same answer. For $n=2$ it is Lesson 3.3's $J = \begin{bmatrix}-l_1s_1-l_2s_{12} & -l_2s_{12}\\ l_1c_1+l_2c_{12} & l_2c_{12}\end{bmatrix}$; column 2 contains only $l_2$ because only one link lies beyond joint 2.

完整的平面雅可比与三维的推广The full planar Jacobian and its 3D generalization

手爪的朝向 $\phi_n = \sum_i\theta_i$，对每个 $\theta_i$ 的偏导都是 1，即角速度 $\omega = \sum_i\dot\theta_i$。把位置和朝向合在一起，平面臂的完整雅可比是 $3\times n$ 的：

The tip heading is $\phi_n = \sum_i\theta_i$, with partial derivative 1 in every $\theta_i$: the angular velocity is $\omega = \sum_i\dot\theta_i$. Stacking position and heading gives the full planar Jacobian, $3\times n$:

$$ \begin{bmatrix} v_x\\ v_y\\ \omega\end{bmatrix} = \begin{bmatrix} \ci{\mathbf J_1} & \cj{\mathbf J_2} & \cdots & \mathbf J_n\\ \ci{1} & \cj{1} & \cdots & 1\end{bmatrix}\begin{bmatrix}\dot\theta_1\\ \dot\theta_2\\ \vdots\\ \dot\theta_n\end{bmatrix}, \qquad \mathbf J_i = \hat{\mathbf z}\times(\mathbf p_{\text{tip}}-\mathbf p_i). $$

三维空间里，每个转动关节有转轴方向 $\hat{\boldsymbol\omega}_i$ 和轴上一点 $\mathbf q_i$，对应的列是 $\begin{bmatrix}\hat{\boldsymbol\omega}_i\times(\mathbf p_{\text{tip}}-\mathbf q_i)\\ \hat{\boldsymbol\omega}_i\end{bmatrix}$：上半部分仍然是"转轴叉乘杠杆臂"，下半部分是转轴本身（1.8 节的叉积）；平移关节的列则是 $\begin{bmatrix}\hat{\mathbf a}_i\\ \mathbf 0\end{bmatrix}$，$\hat{\mathbf a}_i$ 是滑动方向。这就是 $6\times n$ 的几何雅可比，5.10 节会用 twist 把它写得更紧凑。

In 3D each revolute joint has an axis direction $\hat{\boldsymbol\omega}_i$ and a point $\mathbf q_i$ on that axis; its column is $\begin{bmatrix}\hat{\boldsymbol\omega}_i\times(\mathbf p_{\text{tip}}-\mathbf q_i)\\ \hat{\boldsymbol\omega}_i\end{bmatrix}$: the top half is still "axis crossed with lever arm", and the bottom half is the axis itself (the cross product of Lesson 1.8). A prismatic joint's column is $\begin{bmatrix}\hat{\mathbf a}_i\\ \mathbf 0\end{bmatrix}$, with $\hat{\mathbf a}_i$ the sliding direction. This is the $6\times n$ geometric Jacobian; Lesson 5.10 writes it more compactly with twists.

用 SVD 读椭圆Reading the ellipse from the SVD

设 $J = U\Sigma V^\top$（1.12 节）。单位关节速度球 $\|\dot{\boldsymbol\theta}\|\le1$ 的像是速度椭圆，半轴为 $\sigma_i$，沿 $U$ 的第 $i$ 列。力椭圆 $\{\mathbf F:\|J^\top\mathbf F\|\le1\}$：因为 $\|J^\top\mathbf F\|^2 = \sum_i\sigma_i^2(\mathbf u_i\cdot\mathbf F)^2$，半轴是 $1/\sigma_i$，方向相同。常用的标量指标有：

Let $J = U\Sigma V^\top$ (Lesson 1.12). The image of the unit joint-velocity ball $\|\dot{\boldsymbol\theta}\|\le1$ is the velocity ellipse, with semi-axes $\sigma_i$ along the columns $\mathbf u_i$ of $U$. For the force ellipse $\{\mathbf F:\|J^\top\mathbf F\|\le1\}$: since $\|J^\top\mathbf F\|^2 = \sum_i\sigma_i^2(\mathbf u_i\cdot\mathbf F)^2$, its semi-axes are $1/\sigma_i$ along the same directions. Common scalar indices:

$$ w(\boldsymbol\theta) = \sqrt{\det\!\big(JJ^\top\big)} = \sigma_1\sigma_2, \qquad \kappa(J) = \frac{\sigma_1}{\sigma_2}, \qquad \sigma_{\min}(J). $$

$w$ 是 Yoshikawa 可操作度，正比于椭圆面积；$\kappa$ 是条件数，等于 1 时椭圆是正圆（各向同性）；$\sigma_{\min}$ 衡量离奇异有多远。对 2 连杆，$J$ 是方阵，$w = |\det J| = l_1l_2|\sin\theta_2|$（3.3 节）。

$w$ is Yoshikawa's manipulability, proportional to the ellipse's area; $\kappa$ is the condition number, equal to 1 when the ellipse is a circle (isotropic); $\sigma_{\min}$ measures the distance to a singularity. For two links $J$ is square and $w = |\det J| = l_1l_2|\sin\theta_2|$ (Lesson 3.3).

逆运动学：从 $J^{-1}$ 到伪逆与阻尼最小二乘Inverse kinematics: from $J^{-1}$ to the pseudo-inverse and damped least squares

目标位置 $\mathbf t$，当前手爪位置 $\mathbf p$，误差 $\mathbf e = \mathbf t - \mathbf p$。把 $\mathbf e$ 当作"想要的位移"，在当前姿态线性化（3.3 节），每一步解 $J\,\Delta\boldsymbol\theta\approx\mathbf e$，更新 $\boldsymbol\theta\leftarrow\boldsymbol\theta+\Delta\boldsymbol\theta$，重新计算 $J$，重复。解法有三种常见选择：

Target $\mathbf t$, current tip $\mathbf p$, error $\mathbf e = \mathbf t - \mathbf p$. Treat $\mathbf e$ as the displacement you want, linearize at the current posture (Lesson 3.3), solve $J\,\Delta\boldsymbol\theta\approx\mathbf e$, update $\boldsymbol\theta\leftarrow\boldsymbol\theta+\Delta\boldsymbol\theta$, recompute $J$ and repeat. Three common ways to solve it:

$$ \Delta\boldsymbol\theta = \underbrace{\alpha\,J^\top\mathbf e}_{\text{transpose}},\qquad \underbrace{J^{+}\mathbf e = J^\top(JJ^\top)^{-1}\mathbf e}_{\text{pseudo-inverse}},\qquad \underbrace{J^\top\big(JJ^\top+\lambda^2 I\big)^{-1}\mathbf e}_{\text{damped least squares}}. $$

伪逆（1.13 节）在 $J$ 行满秩时给出所有解里 $\|\Delta\boldsymbol\theta\|$ 最小的一个。它在奇异点附近出问题：$\|J^+\| = 1/\sigma_{\min}\to\infty$，关节被要求以极大速度转动。阻尼最小二乘（Nakamura–Hanafusa、Wampler）在 $JJ^\top$ 上加 $\lambda^2I$，等价于最小化 $\|J\Delta\boldsymbol\theta-\mathbf e\|^2+\lambda^2\|\Delta\boldsymbol\theta\|^2$：奇异值 $\sigma$ 对应的放大倍数从 $1/\sigma$ 变成 $\sigma/(\sigma^2+\lambda^2)$，最大不超过 $1/(2\lambda)$，用一点精度换取稳定。转置法不需要求逆，只是沿代价 $\tfrac12\|\mathbf e\|^2$ 的负梯度（3.2 节）$J^\top\mathbf e$ 走，收敛慢但永远安全，6.3 节会说明这正是梯度下降。

The pseudo-inverse (Lesson 1.13), when $J$ has full row rank, gives the solution with the smallest $\|\Delta\boldsymbol\theta\|$ among all that fit. It misbehaves near singularities: $\|J^+\| = 1/\sigma_{\min}\to\infty$, demanding enormous joint speeds. Damped least squares (Nakamura–Hanafusa, Wampler) adds $\lambda^2I$ to $JJ^\top$, which minimizes $\|J\Delta\boldsymbol\theta-\mathbf e\|^2+\lambda^2\|\Delta\boldsymbol\theta\|^2$: a singular value $\sigma$ is amplified by $\sigma/(\sigma^2+\lambda^2)$ instead of $1/\sigma$, never more than $1/(2\lambda)$, trading a little accuracy for stability. The transpose method needs no inverse: it just steps along the negative gradient (Lesson 3.2) of the cost $\tfrac12\|\mathbf e\|^2$, which is $J^\top\mathbf e$, slow but always safe; Lesson 6.3 shows this is exactly gradient descent.

冗余与零空间Redundancy and the null space

当 $n>2$（位置任务），$J$ 有非平凡零空间（1.6 节）：$J\mathbf n = \mathbf 0$ 的关节运动不改变手爪位置，称为自运动。$3$ 连杆臂的位置雅可比是 $2\times3$，零空间是一维的，方向可以用两行的叉积得到：$\mathbf n\propto\mathbf j_1\times\mathbf j_2$（$\mathbf j_k$ 是 $J$ 的第 $k$ 行，1.8 节）。一般地，所有解为

When $n>2$ (position task) $J$ has a nontrivial null space (Lesson 1.6): joint motions with $J\mathbf n = \mathbf 0$ leave the tip fixed and are called self-motions. A three-link arm has a $2\times3$ position Jacobian whose null space is one-dimensional, and its direction is the cross product of the two rows (Lesson 1.8): $\mathbf n\propto\mathbf j_1\times\mathbf j_2$, where $\mathbf j_k$ is row $k$ of $J$. In general all solutions are

$$ \dot{\boldsymbol\theta} \;=\; J^{+}\mathbf v \;+\; \big(I - J^{+}J\big)\,\mathbf z,\qquad \mathbf z\ \text{arbitrary}. $$

第一项把任务完成，第二项把任意的"次要目标" $\mathbf z$（远离关节限位、躲避障碍、保持肘部朝上）投影到零空间里，不干扰主任务。

The first term accomplishes the task; the second projects an arbitrary "secondary objective" $\mathbf z$ (stay away from joint limits, dodge obstacles, keep the elbow up) into the null space so it never disturbs the primary task.

6机器人中的它：用雅可比做逆运动学In robotics: inverse kinematics with the Jacobian

下图是真实的迭代流程：拖动目标（黄色菱形），三连杆臂每一步按所选方法计算 $\Delta\boldsymbol\theta$，直到误差足够小。右侧画出每次迭代的误差（对数坐标），可以比较三种方法。把目标拖到伸直的边界附近，看伪逆法发生什么；把目标拖出可达范围，看阻尼最小二乘如何"尽量靠近"。

底部的"自运动"按钮让手臂沿零空间运动：手爪纹丝不动，肘部却在扫过一圈。这是冗余机械臂能"绕开障碍物而不打断任务"的原因。

The figure below runs the real iterative recipe: drag the target (amber diamond), and at every step the three-link arm computes $\Delta\boldsymbol\theta$ with the chosen method until the error is small enough. On the right, the error of each iteration (log scale) lets you compare the three methods. Drag the target near the fully-stretched boundary and see what the pseudo-inverse does; drag it outside the reachable region and see how damped least squares "gets as close as it can".

The "self-motion" button at the bottom moves the arm along its null space: the tip stands still while the elbow sweeps around. That is how a redundant arm can "dodge an obstacle without interrupting the task".

FIG 5雅可比逆运动学Jacobian inverse kinematics

误差 ‖e‖（对数）vs 迭代次数ERROR ‖e‖ (log) vs. ITERATION

每一步的关节增量长度被限制在 0.6 rad 以内（真实控制器的速度限幅）。伪逆法在 $\sigma_{\min}$ 很小时会"想"走很大的一步，读数里的"未限幅的步长"就是它；阻尼最小二乘的 $\lambda$ 可以用滑块调节。 Each step's joint increment is clamped to 0.6 rad (a velocity limit, as in real controllers). When $\sigma_{\min}$ is small the pseudo-inverse "wants" a huge step; the "unclamped step" readout shows it. The damping $\lambda$ of damped least squares is adjustable.

用任意方法让误差降到 $10^{-3}$ 以下。Bring the error below $10^{-3}$ with any method.
先把目标拖到可达范围边缘（半径略小于 2.3）并用"连续迭代"让手臂几乎伸直，再把目标拖到可达范围之外（半径大于 2.3）：选"伪逆"，看未限幅的步长有多大；然后换"阻尼最小二乘"重做同样的操作。Drag the target to the edge of the reachable region (radius just under 2.3) and press Run so the arm is almost straight, then drag the target outside the reach (radius above 2.3): with "Pseudo-inverse", see how big the unclamped step gets; then repeat with "Damped least squares".
到达目标后点"自运动"，让手臂在手爪不动的情况下变形至少 1.5 rad（关节角的总变化）。After reaching the target, press "Self-motion" and let the arm reshape itself while the tip stays still, by at least 1.5 rad of total joint change.

真实系统里In real systems

遥操作 / 末端笛卡尔控制：Franka、UR 等机械臂的笛卡尔速度模式，本质上每个控制周期（几百 Hz 到 1 kHz）都在算 $\dot{\boldsymbol\theta}=J^{+}\mathbf v$，并在奇异点附近加阻尼或减速。数值 IK 求解器（KDL、MoveIt 的 IK 插件等）都是上面三种方法的变体，再加关节限位与零空间次要任务。力控制与阻抗控制：用 $\boldsymbol\tau = J^\top\mathbf F$ 把期望的笛卡尔力变成关节力矩。接触力估计：反过来，从关节力矩估计手爪受力，需要求 $J^\top$ 的（伪）逆，在奇异方向上估计会失真。规划：采样式规划器常常用 $\sigma_{\min}$ 或可操作度 $w$ 避开奇异位形。

Teleoperation / Cartesian control: the Cartesian-velocity modes of arms such as the Franka and UR series compute $\dot{\boldsymbol\theta}=J^{+}\mathbf v$ every control cycle (a few hundred Hz up to 1 kHz), adding damping or slowing down near singularities. Numerical IK solvers (KDL, MoveIt's IK plugins and the like) are variants of the three methods above, plus joint limits and secondary tasks in the null space. Force and impedance control: use $\boldsymbol\tau = J^\top\mathbf F$ to turn a desired Cartesian force into joint torques. Contact-force estimation: going the other way, estimating the tip force from joint torques needs a (pseudo-)inverse of $J^\top$, and the estimate degrades along singular directions. Planning: sampling-based planners often use $\sigma_{\min}$ or the manipulability $w$ to avoid singular postures.

7常见误区Common pitfalls

误区 1：忽略雅可比所在的坐标系Pitfall 1: ignoring the frame the Jacobian is expressed in

$\mathbf J_i$ 里的 $\mathbf p$ 是在某个坐标系里写的（基座系？工具系？）。基座系里的 $J$ 与工具系里的 $J$ 相差一个旋转（以及三维里的伴随变换，5.8 节）。把基座系的速度指令塞进工具系的雅可比，手爪会朝着奇怪的方向走。

The $\mathbf p$ in $\mathbf J_i$ is written in some frame (base? tool?). A Jacobian in the base frame and one in the tool frame differ by a rotation (and, in 3D, an adjoint transformation, Lesson 5.8). Feed a base-frame velocity command to a tool-frame Jacobian and the gripper heads off in an odd direction.

误区 2：把一次计算的 $J$ 用于大步长Pitfall 2: using one evaluation of $J$ for a big step

$J$ 随姿态变化，$\mathbf v = J\dot{\boldsymbol\theta}$ 只是瞬时关系。FIG 2 里把预览时间 $T$ 拉大，真实路径就会弯离直线预测。位移指令要拆成许多小步，每一步重新计算 $J$（FIG 5 正是这样做的）。

$J$ changes with the posture and $\mathbf v = J\dot{\boldsymbol\theta}$ is only an instantaneous relation. Raise the preview time $T$ in FIG 2 and the true path bends away from the straight-line prediction. A displacement command must be split into many small steps with $J$ recomputed each time (which is what FIG 5 does).

误区 3：直接求 $J^{-1}$Pitfall 3: blindly inverting $J$

冗余臂的 $J$ 不是方阵，没有逆；方阵时在奇异点附近 $J^{-1}$ 的元素爆炸。工程里几乎总是用伪逆加阻尼（或 SVD 截断小的奇异值），并对关节速度限幅。$\|J\|$ 本身并不大，不代表 $J^{-1}$ 安全。

A redundant arm's $J$ is not square and has no inverse; when it is square, $J^{-1}$ explodes near singularities. In practice one almost always uses a damped pseudo-inverse (or an SVD that truncates small singular values) and clamps joint velocities. A modest $\|J\|$ does not make $J^{-1}$ safe.

误区 4：混合不同量纲的行Pitfall 4: mixing rows with different units

完整雅可比的上半部分是 m/rad，下半部分是 rad/rad（无量纲）。对这样的 $6\times n$ 矩阵直接做 SVD 或求"可操作度"，数值取决于你用米还是毫米，没有物理意义。要么分别分析平移和旋转两块，要么先按任务给各行加权。

The top half of the full Jacobian is in m/rad and the bottom half in rad/rad (dimensionless). Taking an SVD or "manipulability" of such a $6\times n$ matrix gives numbers that depend on whether you chose metres or millimetres and have no physical meaning. Either analyse the translational and rotational blocks separately, or weight the rows according to the task.

误区 5：奇异位形只在"工作空间边界"Pitfall 5: singularities only occur at the workspace boundary

伸直、折回确实发生在边界上，但许多机械臂（例如手腕三个轴中有两个共线的"万向节锁"）在工作空间内部也有奇异位形，手爪恰好经过时同样会出问题。路径规划必须检查沿途的 $\sigma_{\min}$，而不仅仅是起点和终点。

Fully stretched or folded postures do lie on the boundary, but many arms (for instance wrists where two of three axes line up, "gimbal lock") also have singularities inside the workspace, and the tip passing through one is just as troublesome. Path planning must check $\sigma_{\min}$ all along the way, not only at the start and the goal.

8小练习Check yourself

1. 两连杆臂 $l_1=l_2=1$，姿态 $\boldsymbol\theta = (0,\ \pi/2)$。它的雅可比是？1. A two-link arm with $l_1=l_2=1$ is at $\boldsymbol\theta = (0,\ \pi/2)$. What is its Jacobian?

手爪在 $(1,1)$，关节 1 在原点，关节 2 在 $(1,0)$。杠杆臂 $\mathbf r_1=(1,1)$，$\mathbf r_2=(0,1)$，各转 90°：$\mathbf J_1=(-1,1)$，$\mathbf J_2=(-1,0)$。所以 $J=\begin{bmatrix}-1&-1\\1&0\end{bmatrix}$，$\det J=1=l_1l_2\sin\theta_2$，与公式吻合。The tip is at $(1,1)$, joint 1 at the origin, joint 2 at $(1,0)$. Lever arms $\mathbf r_1=(1,1)$, $\mathbf r_2=(0,1)$, each rotated 90°: $\mathbf J_1=(-1,1)$, $\mathbf J_2=(-1,0)$. So $J=\begin{bmatrix}-1&-1\\1&0\end{bmatrix}$ and $\det J=1=l_1l_2\sin\theta_2$, as the formula says.

2. 同样的臂与姿态，手爪向下压桌面，对桌面施加 $\mathbf F=(0,-10)$ N。关节需要输出的力矩 $\boldsymbol\tau$（单位 N·m）是？2. Same arm and posture; the gripper presses down on a table, applying $\mathbf F=(0,-10)$ N to it. What joint torques $\boldsymbol\tau$ (N·m) must the joints produce?

$\boldsymbol\tau=J^\top\mathbf F=\begin{bmatrix}-1&1\\-1&0\end{bmatrix}\begin{bmatrix}0\\-10\end{bmatrix}=\begin{bmatrix}-10\\0\end{bmatrix}$。直观检查：手爪施加的力 $\mathbf F$ 对原点的力矩是 $\mathbf r_1\times\mathbf F = 1\cdot(-10)-1\cdot0=-10$；对肘部（$(1,0)$）的杠杆臂 $(0,1)$ 与力平行，力矩为 0。注意符号约定：$\mathbf F$ 是手爪施加给外界的力；若改问"撑住一个挂在手爪上的向下重物"，外力是 $\mathbf F_{\text{ext}}$，关节要输出 $-J^\top\mathbf F_{\text{ext}}=(10,0)$。$\boldsymbol\tau=J^\top\mathbf F=\begin{bmatrix}-1&1\\-1&0\end{bmatrix}\begin{bmatrix}0\\-10\end{bmatrix}=\begin{bmatrix}-10\\0\end{bmatrix}$. Sanity check: the moment of the applied force $\mathbf F$ about the origin is $\mathbf r_1\times\mathbf F = 1\cdot(-10)-1\cdot0=-10$; about the elbow at $(1,0)$ the lever arm $(0,1)$ is parallel to the force, so the moment is 0. Mind the sign convention: $\mathbf F$ is the force the gripper applies to the world; to hold up a downward weight hanging on the gripper (an external force $\mathbf F_{\text{ext}}$), the joints must produce $-J^\top\mathbf F_{\text{ext}}=(10,0)$.

3. 平面 3 连杆臂只关心手爪位置（2 维），雅可比是 $2\times3$，非奇异时它的零空间有几维？3. A planar 3-link arm cares only about the tip position (2D), so $J$ is $2\times3$. At a non-singular posture, what is the dimension of its null space?

秩–零化度定理（1.6 节）：零空间维数 = 列数 − 秩 = $3-2=1$。这一维就是 FIG 5 里的"自运动"：关节在变，手爪不动。Rank–nullity (Lesson 1.6): null-space dimension = columns − rank = $3-2=1$. That single dimension is the "self-motion" in FIG 5: the joints change while the tip stays put.

4. 对冗余臂，$\dot{\boldsymbol\theta}=J^{+}\mathbf v$ 是满足 $J\dot{\boldsymbol\theta}=\mathbf v$ 的所有关节速度里的哪一个？4. For a redundant arm, which of all joint velocities satisfying $J\dot{\boldsymbol\theta}=\mathbf v$ is $\dot{\boldsymbol\theta}=J^{+}\mathbf v$?

所有解是 $J^{+}\mathbf v$ 加上零空间里的任意向量；零空间向量与 $J^{+}\mathbf v$（它在 $J$ 的行空间里）正交，所以加上它们只会增大范数。伪逆给出的是最小范数解。All solutions are $J^{+}\mathbf v$ plus an arbitrary null-space vector; null-space vectors are orthogonal to $J^{+}\mathbf v$ (which lies in the row space of $J$), so adding them can only increase the norm. The pseudo-inverse gives the minimum-norm solution.

5. 手臂靠近奇异位形时，为什么常用阻尼最小二乘 $J^\top(JJ^\top+\lambda^2I)^{-1}$ 代替伪逆？5. Near a singular posture, why use damped least squares $J^\top(JJ^\top+\lambda^2I)^{-1}$ instead of the pseudo-inverse?

对奇异值 $\sigma$，放大倍数是 $\sigma/(\sigma^2+\lambda^2)$，当 $\sigma=\lambda$ 时取最大值 $1/(2\lambda)$。它并不改变真实的 $J$（真实的奇异性还在），也不比伪逆更精确（远离奇异点时两者几乎一样，近处它故意有一点偏差）。For a singular value $\sigma$, the gain is $\sigma/(\sigma^2+\lambda^2)$, peaking at $1/(2\lambda)$ when $\sigma=\lambda$. It does not change the true $J$ (the singularity is still there) and is not more accurate than the pseudo-inverse (far from singularities they nearly coincide; nearby, it is deliberately a little biased).

9延伸阅读Further reading

K. Lynch & F. Park, Modern Robotics, Ch. 5 — 速度运动学与静力学：雅可比、奇异位形、可操作度椭球、力椭球Velocity kinematics and statics: the Jacobian, singularities, manipulability and force ellipsoids
B. Siciliano, L. Sciavicco, L. Villani & G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 3 — 微分运动学与静力学，几何雅可比、冗余度与零空间Differential kinematics and statics, the geometric Jacobian, redundancy and the null space
S. R. Buss, Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares Methods (2004) — 对三种迭代 IK 的简明比较，本节 FIG 5 的参照A concise comparison of the three iterative IK methods; the reference for FIG 5
T. Yoshikawa, Manipulability of Robotic Mechanisms, Int. J. Robotics Research 4(2), 1985 — 可操作度指标的出处Where the manipulability measure comes from
Y. Nakamura & H. Hanafusa, Inverse Kinematic Solutions with Singularity Robustness for Robot Manipulator Control, J. Dynamic Systems, Measurement, and Control 108, 1986 — 阻尼最小二乘用于奇异点附近的逆运动学Damped least squares for inverse kinematics near singularities