Part IV · 第 13 章反馈控制 · 13.6

操作空间控制

想让手爪走直线、在每个方向上都一样"听话"，就要在手爪所在的任务空间里写动力学。操作空间控制用 8.3 节的操作空间惯量 $\Lambda$ 把期望加速度换成力，用 $\boldsymbol\mu$ 抵消速度带来的漂移，再用 $J^\top$ 换成关节力矩。这一节把它和雅可比转置控制放在一起比较，看环境在两种控制下感受到的刚度有何不同，看冗余臂怎样在动力学一致的零空间里做第二任务而不打扰手爪，以及为什么在奇异位形附近必须给等效质量封顶。

约 40 分钟前置：3.4 机器人雅可比 · 7.4 速度运动学 · 8.3 质量矩阵与科氏力 · 13.5 阻抗与导纳控制 5 个交互

1为什么机器人需要它

很多任务用手爪的坐标来描述最自然：把杯子平移 30 cm，沿直线擦桌子，插孔时轴向推进、侧向保持柔顺。一种做法是先用 7.5 节的逆运动学把手爪路径换成关节轨迹，再交给关节控制器（13.1 节的 PID，或 8.3 节的计算力矩）。位置控制的工业臂通常就这么做。可是对力矩控制的手臂（Franka、KUKA iiwa，以及四足和人形机器人的腿），我们更希望控制器直接"站在手爪处"思考：离目标还差多少，该以多大的加速度靠近，碰到东西时该用多大的力。

最简单的想法来自 3.4 节和 7.6 节的力对偶：在手爪处虚拟一根弹簧加一个阻尼器 $\mathbf F=K_p\mathbf e-K_d\dot{\mathbf x}$，再用 $\boldsymbol\tau=J^\top\mathbf F$ 送到关节。这叫雅可比转置控制。它稳定，而且不需要动力学模型。但 8.3 节已经告诉我们：手爪在不同方向上的等效质量 $\Lambda$ 可以差好几倍甚至几十倍。同一根弹簧，往"轻"的方向拉得快，往"重"的方向拉得慢，于是路径会弯，响应还随姿态改变。

操作空间控制（operational-space control，OSC，Khatib 1987）的答案是：用 8.3 节推导的任务空间动力学 $\Lambda\ddot{\mathbf x}+\boldsymbol\mu+\mathbf p=\mathbf F$ 把手爪看成一个"质点"，像计算力矩控制那样把 $\Lambda$、$\boldsymbol\mu$、$\mathbf p$ 抵消掉，让每个任务方向都变成一个独立的单位质量。这一节回答四个问题：(1) 为什么要乘 $\Lambda$、加 $\boldsymbol\mu$；(2) 和雅可比转置控制相比，得到了什么，又付出了什么；(3) 冗余臂多出来的自由度怎样才能不打扰手爪；(4) 奇异位形附近该怎么办。

全篇沿用 8.3 节的平面两连杆臂（$l_1=1$ m，$l_2=0.8$ m，$m_1=2$ kg，$m_2=1.5$ kg，均匀杆），讲冗余时换成三连杆臂。为了把注意力集中在惯性上，所有手臂都在水平面里运动（俯视图，类似 SCARA），重力不作用在关节上，$\mathbf p=0$；竖直平面里的手臂只要再加上 8.3 节的重力补偿 $\hat{\mathbf p}$ 即可。

2直觉：力不等于加速度

8.3 节 FIG 3 里我们推过手爪：沿 $\mathbf F$ 推，手爪的加速度是 $\Lambda^{-1}\mathbf F$，一般不沿 $\mathbf F$。做控制时把问题反过来：我们想要一个加速度 $\mathbf a^*$，应该施加什么力？

雅可比转置的想法：把手爪当成质量为 $\bar m$ 的质点，施加 $\mathbf F=\bar m\,\mathbf a^*$。真实的响应是 $\bar m\Lambda^{-1}\mathbf a^*$：大小和方向都被 $\Lambda^{-1}$ 扭曲了。
操作空间的想法：施加 $\mathbf F=\Lambda\mathbf a^*$，用真实的等效质量去"配"想要的加速度，$\Lambda^{-1}\Lambda\mathbf a^*=\mathbf a^*$。

手臂在动的时候还有一层：即使关节力矩为零，手爪也会自己加速。一部分来自关节空间的离心力和科氏力 $C\dot{\mathbf q}$（8.3 节），一部分来自几何本身：关节匀速转动时手爪走的是圆弧，圆弧运动就有向心加速度，这就是 7.4 节末尾预告的 $\dot J\dot{\mathbf q}$。由 $\ddot{\mathbf x}=J\ddot{\mathbf q}+\dot J\dot{\mathbf q}$ 和 $\ddot{\mathbf q}=M^{-1}(\boldsymbol\tau-C\dot{\mathbf q})$（水平面里没有重力）得到

$$ \boldsymbol\tau=J^{\top}\mathbf F\quad\Longrightarrow\quad \ddot{\mathbf x}=\hl{Lam}{\Lambda^{-1}}\mathbf F+\hl{mu}{\mathbf d},\qquad \mathbf d=\dot J\dot{\mathbf q}-JM^{-1}C\dot{\mathbf q},\qquad \boldsymbol\mu=-\Lambda\,\mathbf d . $$

$\mathbf d$ 是"漂移"：不施力时手爪自己的加速度。8.3 节的 $\boldsymbol\mu=\Lambda(JM^{-1}C-\dot J)\dot{\mathbf q}$ 恰好是抵消它所需的力（取负号）。所以完整的操作空间指令是 $\mathbf F=\Lambda\hl{a}{\mathbf a^*}+\boldsymbol\mu$，代入得 $\ddot{\mathbf x}=\mathbf a^*$，一点不差。下图左右拖动关节圆圈改变姿态，拖动琥珀色箭头的箭尖改变想要的加速度 $\mathbf a^*$，用滑块给关节加上速度。

FIG 1想要的加速度 → 力 → 关节力矩：三种做法

俯视 · 拖动关节圆圈与琥珀色箭尖

琥珀色箭头是想要的加速度 $\mathbf a^*$。三种力矩都作用在同一个真实手臂上，箭头是由正动力学算出的手爪实际加速度：紫色是雅可比转置 $\boldsymbol\tau=J^\top(\bar m\mathbf a^*)$（$\bar m=2$ kg），青色是只乘 $\Lambda$、不加 $\boldsymbol\mu$ 的 $\boldsymbol\tau=J^\top\Lambda\mathbf a^*$，绿色圆环标出完整的操作空间控制 $\boldsymbol\tau=J^\top(\Lambda\mathbf a^*+\boldsymbol\mu)$ 的结果。青色虚线箭头是漂移 $\mathbf d$，从 $\mathbf a^*$ 的箭尖指向青色箭尖。蓝色闭合曲线是各方向的等效质量 $m(\mathbf u)$（每 kg 画 0.15 m，超过 6 kg 截断），细的无色箭头是手爪速度 $\dot{\mathbf x}$（1 m/s ↔ 0.15 m）。加速度的比例是 1 m/s² ↔ 0.12 m，长于 1.2 m 的箭头被截短，读数给出完整数值。

试一试

关节速度为零时，找一个姿态和 $\mathbf a^*$ 的方向，让雅可比转置的加速度偏离 $\mathbf a^*$ 超过 45°。提示：先看蓝色曲线在哪个姿态下最扁。
给关节加上速度（$\|\dot{\mathbf q}\|\ge1$ rad/s），保持 $\|\mathbf a^*\|\ge2$ m/s²，让"不加 $\boldsymbol\mu$"的加速度偏离 $\mathbf a^*$ 超过 30°，同时确认完整的操作空间控制仍然偏差不到 1°。

核心洞见

在手爪处，"力 = 质量 × 加速度"里的质量是一个随姿态变化的矩阵 $\Lambda(\mathbf q)$，而且手臂在动时还有一个与力无关的漂移 $\mathbf d$。操作空间控制就是在手爪处做一次计算力矩：先用 $\boldsymbol\mu$ 抵消漂移，再用 $\Lambda$ 把想要的加速度翻译成力。想要的加速度从哪里来，是下一步的事。

3动手玩：雅可比转置 vs 操作空间

想要的加速度来自任务空间的 PD：$\mathbf a=\omega_n^2\mathbf e-2\omega_n\dot{\mathbf x}$，$\mathbf e=\mathbf x_d-\mathbf x$。这是一个单位质量的临界阻尼弹簧，带宽 $\omega_n$。下面两个控制器用同一个 $\omega_n$，只差在把加速度换成力的方式：

$$ \hl{JT}{\boldsymbol\tau_{J^\top}=J^{\top}\,\bar m\big(\omega_n^2\mathbf e-2\omega_n\dot{\mathbf x}\big)},\qquad\qquad \hl{osc}{\boldsymbol\tau_{\mathrm{OSC}}=J^{\top}\Big(\hat\Lambda\big(\omega_n^2\mathbf e-2\omega_n\dot{\mathbf x}\big)+\hat{\boldsymbol\mu}\Big)} . $$

雅可比转置控制的增益按一个"名义质量" $\bar m$ 来定：如果手爪真是一个 $\bar m$ kg 的质点，它就是临界阻尼。操作空间控制用模型 $\hat\Lambda,\hat{\boldsymbol\mu}$（帽子表示控制器心目中的模型）。模型准确时，手爪的误差满足 $\ddot{\mathbf e}+2\omega_n\dot{\mathbf e}+\omega_n^2\mathbf e=0$：两个方向各是一个独立的临界阻尼系统。从静止出发时 $\mathbf e(t)=(1+\omega_nt)e^{-\omega_nt}\,\mathbf e(0)$，误差向量只缩短、不转向，所以手爪走直线。

拖动蓝色目标圈，松手后两条手臂同时从同一个起点出发（起点就是上一个目标）。也可以加一个控制器不知道的末端负载，或者把负载写进模型。

FIG 2点到点：雅可比转置与操作空间控制

俯视 · 拖动蓝色目标，松手出发

误差分量 (m) vs 时间 (s)

琥珀色 = 操作空间控制，紫色 = 雅可比转置；无色虚线是起点到目标的直线。整条路径先淡淡画出，回放到的部分加粗。右图：实线是沿直线还剩的距离 $e_\parallel$，虚线是偏离直线的侧向误差 $e_\perp$。读数里"偏离"是 $|e_\perp|$ 的最大值，"过冲"是手爪沿直线越过目标的最大距离，"到位"是手爪最后一次离目标超过 1 cm 的时刻。仿真用 RK4、1 ms 步长，控制器 1 kHz 零阶保持，没有力矩上限（真实控制器会插值一条参考轨迹，而不是一下子跳到目标）。目标限制在离基座 0.4 到 1.72 m 之间，起点到目标的直线离基座至少 0.3 m，以避开手臂折叠时的奇异位形。

试一试

拖动目标，找一次运动，让雅可比转置偏离直线超过 15 cm，而操作空间控制偏离不到 2 mm（负载为 0 或已写进模型）。
再找一个目标，让雅可比转置的到位时间比操作空间控制长 1.6 倍以上（同样要求负载为 0 或已建模）。操作空间控制的到位时间只取决于距离，雅可比转置的还取决于什么？
把末端负载调到 1 kg 以上，模型保持"不含负载"，让操作空间控制冲过目标超过 10 cm。为什么雅可比转置几乎不受影响？
负载调到 2 kg 以上，再把模型切换为"含负载"：操作空间控制的路径重新变直（偏离和过冲都小于 2 mm）。

核心洞见

雅可比转置控制在手爪处装的是一根"不知道手臂多重"的弹簧，它的响应由 $\Lambda(\mathbf q)^{-1}$ 塑形：方向不同、姿态不同，快慢就不同，路径因此弯曲。操作空间控制先把手爪变成单位质量，再装弹簧，于是每个方向的响应都一样，误差解耦、路径笔直。代价是它依赖模型：负载没写进 $\hat\Lambda$，等效增益就被悄悄缩小，阻尼不足而过冲。

4第二、第三视角：环境感受到的刚度，与冗余臂的零空间

环境感受到的刚度

FIG 2 看的是自由运动。现在让手臂停在目标处，用一个外力 $\mathbf F_{\text{ext}}$ 去推手爪。静止时 $\dot{\mathbf x}=0$、$\boldsymbol\mu=0$，两种控制器的手爪力分别是 $\bar m\omega_n^2\mathbf e$ 和 $\Lambda\omega_n^2\mathbf e$，平衡时它们抵消外力，偏移 $\boldsymbol\delta=-\mathbf e$ 满足

$$ \underbrace{\bar m\,\omega_n^2\,\boldsymbol\delta=\mathbf F_{\text{ext}}}_{J^{\top}}\qquad\qquad \underbrace{\hl{K}{\omega_n^2\,\Lambda(\mathbf q)}\,\boldsymbol\delta=\mathbf F_{\text{ext}}}_{\mathrm{OSC}} . $$

雅可比转置给出一根各向同性的弹簧，刚度 $\bar m\omega_n^2$。操作空间控制给环境的刚度却是 $\omega_n^2\Lambda(\mathbf q)$：轻方向软、重方向硬，并且随姿态变化。对这条手臂，最轻方向的等效质量在各种姿态下都在 0.45 到 0.5 kg 之间，而最重方向在手肘弯成直角附近约 2.2 kg，接近伸直或折叠时超过 20 kg。所以"把手爪变成单位质量"让运动整齐了，环境感受到的刚度却恰恰继承了 $\Lambda$ 的不均匀。下图中拖动琥珀色的推力手柄，两条手臂在同一个推力下各自偏移。

FIG 3推一下手爪：两种控制器的刚度

俯视 · 拖动蓝色目标与琥珀色推力手柄

两条手臂（琥珀色 = 操作空间，紫色 = 雅可比转置）实时仿真，都停在蓝色目标处，$\omega_n=8$ rad/s，$\bar m=2$ kg，所以雅可比转置的刚度是 128 N/m。推力从目标指向琥珀色手柄（1 N ↔ 0.04 m，最大 8 N），同样的推力作用在两条手臂的手爪上，画在各自的手爪处。以目标为中心，琥珀色椭圆和紫色圆是按线性刚度预测的偏移范围：推力大小不变、方向转一圈时手爪会偏到的地方（推力小于 2 N 时按 2 N 画）。两种控制器在目标处都没有稳态误差，偏移完全来自外力。

试一试

用至少 2 N 的推力，找一个方向，让操作空间控制的偏移达到雅可比转置的 3.5 倍以上（等两条手臂停稳）。这是哪个方向？
反过来：移动目标改变姿态，找到操作空间控制的偏移不到雅可比转置一半的情况（推力至少 2 N，停稳后判定）。

这就是 13.5 节的阻抗控制要处理的问题：接触任务关心的是环境感受到的刚度和阻尼，应该直接以 N/m 为单位来设计，例如 $\mathbf F=K\mathbf e-D\dot{\mathbf x}+\boldsymbol\mu$（带科氏补偿的雅可比转置），而把 $\Lambda$ 留给自由运动。要同时得到"想要的刚度"和"想要的惯量"，就得测量外力，见 13.5 节。

冗余臂：动力学一致的零空间

三连杆平面臂控制手爪的 2 维位置，还多出 1 个自由度：手爪不动时，手肘仍然可以摆（1.6 节的零空间，7.4 节叫它自运动）。我们想用这份自由做第二个任务，比如让第一根连杆朝向角度 $\psi$：关节力矩 $\boldsymbol\tau_0=k_\psi(\psi-\theta_1)\,\mathbf e_1-k_d\dot{\mathbf q}$（$\mathbf e_1$ 是第一个单位向量，只拉关节 1；阻尼作用在所有关节上）。但 $\boldsymbol\tau_0$ 不能直接加上去，它会推动手爪。要先用一个矩阵 $N^\top$ 过滤：$\boldsymbol\tau=J^\top\mathbf F+N^\top\boldsymbol\tau_0$。过滤后的力矩不得改变手爪加速度，即 $JM^{-1}N^\top=0$。

3.4 节和 7.4 节在速度层面用过伪逆投影 $I-J^+J$。把它原样搬到力矩上是一个常见的错误：$JM^{-1}(I-J^+J)$ 一般不为零。正确的选择是用 8.3 节误区 5 提到的动力学一致广义逆 $\bar J=M^{-1}J^\top\Lambda$：

$$ \hl{N}{N^{\top}=I-J^{\top}\bar J^{\top}},\qquad \bar J=M^{-1}J^{\top}\Lambda, $$ $$ JM^{-1}N^{\top}=JM^{-1}-\underbrace{JM^{-1}J^{\top}}_{\Lambda^{-1}}\Lambda JM^{-1}=0 . $$

下图用三种过滤器跑同一个第二任务：动力学一致的 $N^\top$，伪逆投影 $I-J^+J$，以及不过滤。按"摆动"让 $\psi$ 以 0.4 Hz、±0.6 rad 来回摆，看手爪误差。

FIG 4三连杆冗余臂：零空间里的第二任务

俯视 · 拖动蓝色目标

手爪误差 (mm)，对数，最近 6 s

连杆长 0.7、0.6、0.5 m，质量 1.6、1.2、0.8 kg。主任务是手爪位置的操作空间控制（$\omega_n=10$ rad/s），第二任务 $k_\psi=30$ N·m/rad、$k_d=4$ N·m·s/rad。紫色虚线从基座指向想要的第一连杆方向 $\psi$，紫色细线是第一个关节点（连杆 1 的末端）最近走过的轨迹，琥珀色细线是手爪轨迹。右图曲线的颜色表示当时用的过滤器：绿色 = 动力学一致，紫色 = 伪逆，红色 = 不过滤。读数里的"最大误差"与"摆幅"统计最近 3 s（这段时间里没有拖动目标、没有切换过滤器）。

试一试

用动力学一致的过滤器，让第一连杆在 3 s 内摆动超过 0.6 rad，同时手爪的最大误差保持在 1 mm 以下。
换成伪逆投影 $I-J^+J$，做同样的摆动，让手爪误差超过 2 cm。第二任务的力矩"漏"进了主任务。

一个概念，三个视角

代数视角：$\bar J$ 是用 $M$ 加权的伪逆，它满足 $J\bar J=I$，在所有满足 $J\dot{\mathbf q}=\dot{\mathbf x}$ 的关节速度里，$\bar J\dot{\mathbf x}$ 的动能最小（8.3 节）。力学视角：速度和力矩是对偶的两类量（3.4 节的 $\dot{\mathbf x}=J\dot{\mathbf q}$ 与 $\boldsymbol\tau=J^\top\mathbf F$）。"不影响手爪"对速度说的是 $J\dot{\mathbf q}=0$，对力矩说的却是"不产生手爪加速度" $JM^{-1}\boldsymbol\tau=0$，中间隔着 $M^{-1}$，所以投影要按 $M$ 加权。只有当 $M$ 与单位阵成比例时，两种投影才一样。层级视角：$N^\top$ 给出一个严格的优先级，主任务永远不受第二任务干扰，第二任务只能在剩下的自由里尽力而为。多层任务的层级就是 13.7 节全身控制的内容。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，对应的图元素会被高亮：$\Lambda$ 是 FIG 1 的蓝色等效质量曲线，$\mathbf d$ 是 FIG 1 的青色虚线漂移箭头，$\mathbf a^*$ 是 FIG 1 的琥珀色箭头，两个控制律是 FIG 2 的紫色与琥珀色路径，刚度 $\omega_n^2\Lambda$ 是 FIG 3 的琥珀色椭圆，$N^\top$ 是 FIG 4 的第一关节点轨迹与 $\psi$ 虚线，封顶的 $\Lambda_\varepsilon$ 是 FIG 5 的琥珀色力矩曲线。

任务空间动力学（8.3 节的结论）

设任务坐标 $\mathbf x\in\R^m$（这里是手爪位置，$m=2$），$\dot{\mathbf x}=J(\mathbf q)\dot{\mathbf q}$，关节动力学为 8.3 节的 $M(\mathbf q)\ddot{\mathbf q}+\mathbf b(\mathbf q,\dot{\mathbf q})=\boldsymbol\tau$，$\mathbf b=C\dot{\mathbf q}+\mathbf g$。8.3 节把 $\boldsymbol\tau=J^\top\mathbf F$ 代入、消去 $\ddot{\mathbf q}$，得到

$$ \hl{Lam}{\Lambda}\,\ddot{\mathbf x}+\boldsymbol\mu+\mathbf p=\mathbf F,\qquad \Lambda=(JM^{-1}J^{\top})^{-1}, $$ $$ \boldsymbol\mu=\Lambda\big(JM^{-1}C-\dot J\big)\dot{\mathbf q},\qquad \mathbf p=\Lambda JM^{-1}\mathbf g . $$

（$\boldsymbol\mu$ 和 $\mathbf p$ 正是偏置项 $\mathbf b$ 的速度部分和重力部分在任务空间里的样子：$\boldsymbol\mu+\mathbf p=\Lambda(JM^{-1}\mathbf b-\dot J\dot{\mathbf q})$。13.7 节沿用浮动基座文献的习惯，把同一个 $\mathbf b$ 记作 $\mathbf h$。）这里假设 $J$ 行满秩，于是 $JM^{-1}J^\top$ 对称正定、可逆。对非冗余臂（$J$ 方阵）这个式子就是把关节动力学换了坐标；对冗余臂它描述的是"手爪在力 $\mathbf F$ 作用下、关节取动能最小的那种运动时"的动力学，下面的零空间部分会用到。

操作空间控制律与解耦

取参考轨迹 $\mathbf x_d(t)$，误差 $\mathbf e=\mathbf x_d-\mathbf x$，令

$$ \mathbf F=\hat\Lambda\,\hl{a}{\mathbf a}+\hat{\boldsymbol\mu}+\hat{\mathbf p},\qquad \mathbf a=\ddot{\mathbf x}_d+K_d\dot{\mathbf e}+K_p\mathbf e,\qquad \boldsymbol\tau=J^{\top}\mathbf F . $$

模型准确时代入任务空间动力学，$\Lambda\ddot{\mathbf x}=\Lambda\mathbf a$，$\Lambda$ 可逆，于是 $\ddot{\mathbf x}=\mathbf a$，也就是

$$ \ddot{\mathbf e}+K_d\dot{\mathbf e}+K_p\mathbf e=0 . $$

取对角的 $K_p=\omega_n^2I$、$K_d=2\omega_nI$，每个任务方向就是一个独立的临界阻尼二阶系统，与姿态无关：这就是 Khatib 所说的解耦。$\mathbf x_d$ 固定、从静止出发时，解是 $\mathbf e(t)=(1+\omega_nt)e^{-\omega_nt}\mathbf e(0)$，方向不变，手爪走直线；误差降到 $\epsilon$ 所需的时间只由 $\|\mathbf e(0)\|/\epsilon$ 和 $\omega_n$ 决定（FIG 2 的"到位"时间）。

与计算力矩的关系。把 $\mathbf F$ 展开，$\boldsymbol\tau=J^\top\Lambda(\mathbf a-\dot J\dot{\mathbf q})+J^\top\Lambda JM^{-1}\mathbf b$。当 $J$ 是可逆方阵时，$J^\top\Lambda J=J^\top J^{-\top}MJ^{-1}J=M$，于是

$$ \boldsymbol\tau=M\,J^{-1}\big(\mathbf a-\dot J\dot{\mathbf q}\big)+\mathbf b . $$

这正是 8.3 节的计算力矩控制，只是关节加速度指令取 $\ddot{\mathbf q}^*=J^{-1}(\mathbf a-\dot J\dot{\mathbf q})$，这种做法叫分解加速度控制（resolved-acceleration control，Luh、Walker、Paul 1980）。对非冗余臂，两者在数学上相同；区别在于冗余臂（$J$ 不是方阵，没有 $J^{-1}$）、多任务的组织方式，以及奇异位形附近的处理。

模型误差。若 $\hat\Lambda\ne\Lambda$，在静止附近 $\ddot{\mathbf x}\approx\Lambda^{-1}\hat\Lambda\mathbf a$：等效增益被矩阵 $\Lambda^{-1}\hat\Lambda$ 缩放。未建模的负载使 $\Lambda$ 变大，$\Lambda^{-1}\hat\Lambda$ 的特征值小于 1；如果把它记作 $s$，等效的 $K_p,K_d$ 都乘以 $s$，阻尼比变成 $\sqrt s$ 倍，系统欠阻尼而过冲（FIG 2 的负载任务）。最轻方向（约 0.5 kg）加上一个 1 kg 的负载，$s$ 约为 1/3。

雅可比转置控制：为什么不要模型也稳定

取固定目标 $\mathbf x_d$、对称正定的 $K_p,K_d$，控制律 $\boldsymbol\tau=J^\top(K_p\mathbf e-K_d\dot{\mathbf x})+\hat{\mathbf g}$（Takegaki、Arimoto 1981），并假设重力补偿准确。取能量函数 $V=\tfrac12\dot{\mathbf q}^\top M\dot{\mathbf q}+\tfrac12\mathbf e^\top K_p\mathbf e$：动能加上虚拟弹簧的势能。利用 8.3 节的功率平衡（$\dot M-2C$ 斜对称）和 $\dot{\mathbf e}=-J\dot{\mathbf q}$：

$$ \dot V=\dot{\mathbf q}^{\top}(\boldsymbol\tau-\mathbf g)-\mathbf e^{\top}K_pJ\dot{\mathbf q}=\dot{\mathbf q}^{\top}J^{\top}(K_p\mathbf e-K_d\dot{\mathbf x})-\mathbf e^{\top}K_pJ\dot{\mathbf q}=-\dot{\mathbf x}^{\top}K_d\,\dot{\mathbf x}\le0 . $$

能量只减不增，而且没有用到 $M$ 的任何数值，所以对质量误差、未知负载都是鲁棒的。若手臂停在某处不再运动，则 $\boldsymbol\tau-\mathbf g=J^\top K_p\mathbf e=0$；$J$ 行满秩时只能 $\mathbf e=0$，由 LaSalle 不变原理，手爪收敛到目标（在奇异位形处 $J^\top$ 有零空间，手臂可能停在 $\mathbf e\ne0$ 处）。它控制的是终点，而不是过程：在目标附近线性化，$\Lambda\ddot{\mathbf x}\approx K_p\mathbf e-K_d\dot{\mathbf x}$，沿 $\Lambda$ 的特征方向 $\mathbf u$，自然频率和阻尼比分别是

$$ \omega(\mathbf u)=\sqrt{\frac{k_p}{m(\mathbf u)}},\qquad \zeta(\mathbf u)=\frac{k_d}{2\sqrt{k_p\,m(\mathbf u)}},\qquad k_p=\bar m\,\omega_n^2,\quad k_d=2\bar m\,\omega_n . $$

只有 $m(\mathbf u)=\bar m$ 的方向才得到设计的 $\omega_n$ 和 $\zeta=1$：比 $\bar m$ 轻的方向更快、阻尼更小，比 $\bar m$ 重的方向更慢、阻尼更大。加速度 $\Lambda^{-1}\mathbf F$ 又一般不沿 $\mathbf F$，最大偏角是 $\arctan\frac{\kappa-1}{2\sqrt\kappa}$（$\kappa$ 是 $\Lambda$ 的条件数），所以路径弯曲。这就是 FIG 1 的紫色箭头和 FIG 2 的紫色路径。

冗余：动力学一致性的推导

设 $n>m$，力矩 $\boldsymbol\tau=J^\top\mathbf F+N^\top\boldsymbol\tau_0$。手爪加速度是 $\ddot{\mathbf x}=JM^{-1}(\boldsymbol\tau-\mathbf b)+\dot J\dot{\mathbf q}$，所以 $\boldsymbol\tau_0$ 对它的贡献是 $JM^{-1}N^\top\boldsymbol\tau_0$。要求它对任何 $\boldsymbol\tau_0$ 都为零：

$$ JM^{-1}N^{\top}=0 . $$

设 $N^\top=I-J^\top G^\top$，$G$ 是某个满足 $JG=I$ 的广义逆。条件变成 $JM^{-1}=JM^{-1}J^\top G^\top=\Lambda^{-1}G^\top$，即 $G^\top=\Lambda JM^{-1}$，$G=M^{-1}J^\top\Lambda=\bar J$。所以动力学一致的广义逆是唯一的（Khatib 1987）。伪逆对应 $G=J^+=J^\top(JJ^\top)^{-1}$，代入得 $JM^{-1}(I-J^+J)$，一般不为零：在 FIG 4 里它让手爪误差升到厘米量级。另外注意 $N^\top$ 是投影：$(N^\top)^2=N^\top$，它把 $\boldsymbol\tau_0$ 中"会推手爪"的部分剔除，剩下的部分只改变自运动。

第二任务本身也可以写成"操作空间"的形式：把手肘位置之类的第二任务坐标 $\mathbf x_2=f_2(\mathbf q)$ 投影到零空间里，再用投影后的等效质量 $\Lambda_{2|1}=(J_2M^{-1}N^\top J_2^\top)^{-1}$ 做解耦（Sentis、Khatib 2005 的层级框架）。更一般的多任务、带不等式约束的版本是 13.7 节的二次规划。

奇异位形与封顶的 $\Lambda$

设 $J$ 的最小奇异值是 $\sigma$，相应的手爪方向是 $\mathbf u$。$\Lambda^{-1}=JM^{-1}J^\top$ 在 $\mathbf u$ 方向上大致正比于 $\sigma^2$，于是 $m(\mathbf u)\propto1/\sigma^2$，而 $J^\top$ 在这个方向上只有 $\sigma$ 的放大倍数，所以让手爪沿 $\mathbf u$ 获得加速度 $a$ 所需的力矩

$$ \|\boldsymbol\tau\|\;\sim\;\sigma\cdot\frac{1}{\sigma^{2}}\cdot a=\frac{a}{\sigma}\;\xrightarrow{\ \sigma\to0\ }\;\infty,\qquad \hl{cap}{\Lambda_\varepsilon=\big(\Lambda^{-1}+\varepsilon I\big)^{-1}},\qquad m_\varepsilon(\mathbf u)\le\frac1\varepsilon . $$

FIG 5 里目标越过可达边界时，$\mathbf e$ 不会归零，控制器一直要求沿伸直方向加速，于是力矩发散。补救办法有三类：(1) 封顶：用 $\Lambda_\varepsilon$ 代替 $\Lambda$，把等效质量限制在 $m_{\max}=1/\varepsilon$ 以下，这和 3.4 节的阻尼最小二乘是同一个思路，代价是在被封顶的方向上不再精确得到 $\mathbf a$；(2) 降维：对 $\Lambda^{-1}$ 做特征分解，奇异值低于阈值的方向从任务里拿掉，改用雅可比转置或关节空间控制；(3) 躲开：用规划、关节限位和零空间任务让手臂远离奇异位形。实际系统通常三者并用。

六维任务与姿态

真实手臂的任务通常是手爪的完整位姿，$J$ 是 7.4 节的 $6\times n$ 雅可比。姿态误差不能用欧拉角相减，常用 5.6 节的对数映射，例如 $\mathbf e_\omega=\Log(R_dR^\top)$（以基座系表示的转角向量），并且要与 $J$ 的角速度行用同一个坐标系。6 维的 $\Lambda$ 把 kg 和 kg·m² 混在一起（8.3 节误区 5），所以转动和平移的增益要分开设计；把姿态误差放大一个特征长度再与位置误差比较，也是常见做法。

6机器人中的它：奇异位形与真实的力矩控制臂

真实任务里最常见的"事故"之一是让手爪去够一个稍微够不着的点：视觉估计的位置有误差，或者人推着手臂往外拉。下面的目标沿 $y=0.3$ m 的水平线从 $x=0.8$ m 出发，滑到 $x_{\max}$ 再回来，周期 4 s；在这条线上，手爪最远只能到 $x=\sqrt{1.8^2-0.3^2}\approx1.775$ m。控制器是带前馈的操作空间控制（$\omega_n=8$ rad/s），可以选择给等效质量封顶。

FIG 5够不着的目标：奇异位形附近的力矩

俯视 · 目标沿虚线往返

max |τᵢ| (N·m)，对数 vs 时间 (s)

左：无色虚线是目标的往返路径，虚线圆是可达边界（半径 1.8 m），蓝点是此刻的目标，琥珀色细线是手爪走过的路。右：琥珀色是当前设置（封顶或不封顶）的最大关节力矩，紫色虚线是同一条路径上不封顶的结果，红色虚线标出 30 N·m。力矩超过 $10^4$ N·m 时仿真停止，标为"发散"：真实的驱动器早已饱和或报警停机。"可达段 RMS"是目标离基座不超过 1.7 m 的那些时刻的手爪误差均方根。仿真为 RK4、1 ms，没有力矩上限。

试一试

不封顶，把路径终点 $x_{\max}$ 推到 1.8 m 以上（刚刚越过可达边界）：力矩在手臂伸直的那一刻发散。
$x_{\max}\ge1.85$ m 时，选一个封顶值，让最大力矩低于 30 N·m，同时可达段的 RMS 误差小于 5 mm。封顶太低、太高各会怎样？

真实系统里

力矩控制的手臂：Franka 的 libfranka 以 1 kHz 接受关节力矩指令，franka::Model 给出质量矩阵、科氏力、重力和雅可比，KUKA LBR iiwa 每个关节都有力矩传感器并提供笛卡尔阻抗模式，DLR 的轻型臂（iiwa 的前身）是这类控制的经典平台（Albu-Schäffer、Ott 等）。值得注意的是，franka_ros 自带的笛卡尔阻抗示例控制器用的是 $\boldsymbol\tau=J^\top(-K\tilde{\mathbf x}-D\dot{\mathbf x})+$科氏补偿，零空间用 $J^\top$ 的伪逆投影，也就是本节的"雅可比转置加科氏补偿"，而不是完整的操作空间控制。这和 FIG 3 的结论一致：接触任务里，可预测的刚度往往比笔直的路径更重要。学习里的动作空间：robosuite 给机械臂的默认控制器（例如 Panda 的默认配置）是 OSC_POSE，它让策略输出手爪的位姿增量，由操作空间控制换成力矩；Martín-Martín 等（2019）比较了多种动作空间，发现在接触丰富的任务上，末端空间的可变阻抗控制让强化学习学得更好。人形与全身控制：Khatib 组把操作空间控制推广到带优先级的全身任务（Sentis、Khatib 2005），今天的全身控制器多半把同样的思想写成二次规划（13.7 节）。实践上的经验：Nakanishi 等（2008）在统一的记号下整理了速度、加速度和力三个层面的多种操作空间控制律，并在一条 7 自由度的拟人手臂上做了比较：在模型永远不完美的真机上，一种简化的、基于加速度的做法在任务精度、调参难度和鲁棒性上反而更有优势，而准确、物理上自洽的动力学参数估计是这些实验成功的关键；负载和末端工具的参数必须辨识（8.3 节 FIG 5），Franka 和 iiwa 都要求在软件里设置负载质量与质心。

7常见误区

误区 1：用速度层面的伪逆投影过滤力矩

$I-J^+J$ 在速度层面是对的（$J(I-J^+J)\dot{\mathbf z}=0$），但力矩要经过 $M^{-1}$ 才变成加速度，$JM^{-1}(I-J^+J)\ne0$。FIG 4 里同样的姿态任务，用伪逆投影时手爪误差达到厘米量级，用 $N^\top=I-J^\top\bar J^\top$ 时只有约 0.01 mm（仿真的离散误差）。如果主任务刚度很高，泄漏表现为看起来"莫名其妙"的小误差和抖动；如果刚度低，就是明显的偏移。直觉在哪里失灵？"零空间"这个词让人以为只有一个，其实"对谁为零"取决于度量：速度用欧氏度量，力矩要用 $M$ 加权。

误区 2：直接对 $JM^{-1}J^\top$ 求逆

在奇异位形附近 $\Lambda$ 沿丢失的方向趋于无穷，力矩按 $1/\sigma$ 增长。FIG 5 里目标只比可达边界远 2.5 cm（$x_{\max}=1.8$ m），不封顶的控制器在 1.8 s 时就发散。目标并不需要"明显"够不着：视觉误差、人拉手臂、轨迹规划的一点超调都可能把手臂推到伸直。代码里要么封顶 $\Lambda$，要么检查 $\sigma_{\min}(J)$ 并降维，并且始终保留力矩限幅。手臂完全折叠（$\theta_2=\pi$）是另一个奇异位形，FIG 2 因此禁止经过基座附近的直线。

误区 3：以为操作空间控制让手爪"各向同性地柔顺"

$\ddot{\mathbf e}+K_d\dot{\mathbf e}+K_p\mathbf e=0$ 描述的是没有外力时的运动。有外力时，$\ddot{\mathbf e}+K_d\dot{\mathbf e}+K_p\mathbf e=-\Lambda^{-1}\mathbf F_{\text{ext}}$，环境感受到的刚度是 $\Lambda K_p$，随方向和姿态变化（FIG 3）。要在接触时得到指定的刚度，应该按 13.5 节的阻抗控制，直接以 N/m 设计 $K$；要同时改变表观惯量，则需要力传感器。

误区 4：漏掉 $\boldsymbol\mu$，或者漏掉 $\dot J\dot{\mathbf q}$

慢速运动时 $\boldsymbol\mu$ 很小，省掉它看起来没问题；但 $\boldsymbol\mu$ 随速度平方增长，快速运动时漂移 $\mathbf d$ 可以和想要的加速度一样大（FIG 1 第二个任务）。另一个常见的错误是只补偿了关节空间的科氏力 $C\dot{\mathbf q}$，却忘了 $\dot J\dot{\mathbf q}$：即使关节匀速转动（$\ddot{\mathbf q}=0$），手爪也在走圆弧，有向心加速度 $\dot J\dot{\mathbf q}$，而这一项并不包含在 $C\dot{\mathbf q}$ 里。一个诱人的捷径是 $\boldsymbol\tau=J^\top\Lambda\mathbf a+\mathbf b$：代入可得 $\ddot{\mathbf x}=\mathbf a+\dot J\dot{\mathbf q}$，它漏掉的正是 $-J^\top\Lambda\dot J\dot{\mathbf q}$ 这一项。

误区 5：以为操作空间控制总比雅可比转置好

操作空间控制把模型当成真理：FIG 2 里一个 1 kg 的未建模负载就能让它冲过目标十几厘米，而雅可比转置几乎不受影响，因为它的稳定性证明根本不用 $M$。真实手臂上还有关节摩擦、电机转子惯量、关节弹性、通信延迟，这些都会让 $\hat\Lambda$ 偏离 $\Lambda$。选择取决于任务：自由空间里要走得准、走得直，用操作空间控制并认真辨识模型；接触为主、模型不可靠时，雅可比转置加阻抗设计往往更稳妥。

8小练习

1. 模型准确时，操作空间控制 $\mathbf F=\Lambda(K_p\mathbf e-K_d\dot{\mathbf x})+\boldsymbol\mu$（$K_p=\omega_n^2I$，$K_d=2\omega_nI$，固定目标）让手爪从静止出发走直线。根本原因是什么？

$\Lambda$ 沿路径在变（A 错），控制器每一步都用当前的 $\Lambda(\mathbf q)$ 和 $\boldsymbol\mu$ 把它抵消掉，剩下 $\ddot{\mathbf x}=\mathbf a$。各向同性、解耦的线性系统从静止出发，解是 $\mathbf e(t)=(1+\omega_nt)e^{-\omega_nt}\mathbf e(0)$，方向不变。$J^\top$ 本身并不保持方向（B 错，这正是雅可比转置控制路径弯曲的原因），重力只是另一个被 $\hat{\mathbf p}$ 抵消的项（D 错）。

2. 关于雅可比转置控制 $\boldsymbol\tau=J^\top(K_p\mathbf e-K_d\dot{\mathbf x})+\hat{\mathbf g}$，下列哪个说法正确？

$\dot V=-\dot{\mathbf x}^\top K_d\dot{\mathbf x}\le0$ 的推导只用到斜对称性和 $\dot{\mathbf e}=-J\dot{\mathbf q}$，不用 $M$ 的数值，所以 A 错。在 $\Lambda$ 的特征方向上，自然频率是 $\sqrt{k_p/m(\mathbf u)}$，随方向变化，所以 C 错。在奇异位形处 $J^\top$ 有零空间，$J^\top K_p\mathbf e=0$ 不再推出 $\mathbf e=0$，所以 D 错。

3. 一条 7 关节手臂用操作空间控制管理手爪位姿，同时想用多余的自由度让手肘远离障碍，手肘的力矩是 $\boldsymbol\tau_0$。怎样加入 $\boldsymbol\tau_0$ 才不会干扰手爪？

"不干扰手爪"的意思是 $\boldsymbol\tau_0$ 不产生手爪加速度：$JM^{-1}N^\top=0$。只有 $N^\top=I-J^\top\bar J^\top$ 对任何 $\boldsymbol\tau_0$ 都满足它（选项 D）。伪逆投影（选项 B）在速度层面正确，但 $JM^{-1}(I-J^+J)\ne0$，FIG 4 里它把手爪误差推到厘米量级。直接相加（选项 A）误差更大，缩小（选项 C）只是减小泄漏，并没有消除。

4. 手臂接近伸直时，$J$ 的最小奇异值 $\sigma$ 减小到原来的一半。要让手爪沿这个"难动"的方向得到同样的加速度，操作空间控制需要的关节力矩大约怎样变化？

需要的手爪力是 $m(\mathbf u)a$，$m(\mathbf u)\propto1/\sigma^2$ 变成 4 倍（B 只算了这一半）；这个力经 $J^\top$ 换成力矩时又乘以 $\sigma$（缩小一半），所以 $\|\boldsymbol\tau\|\propto a/\sigma$ 大约变成 2 倍。$M$ 确实没变（C 错），但问题出在 $J$。$\sigma\to0$ 时力矩发散，这就是 FIG 5 里不封顶时发生的事。

5. 对一条非冗余、非奇异的手臂（$J$ 是可逆方阵），操作空间控制 $\boldsymbol\tau=J^\top(\Lambda\mathbf a+\boldsymbol\mu+\mathbf p)$ 与下面哪个控制律完全相同？

$J$ 可逆时 $J^\top\Lambda J=M$，展开得 $\boldsymbol\tau=MJ^{-1}(\mathbf a-\dot J\dot{\mathbf q})+\mathbf b$，即分解加速度控制（Luh、Walker、Paul 1980）。两者的区别只在冗余、多任务和奇异位形处理上显现。

9延伸阅读

O. Khatib, A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation, IEEE J. Robotics and Automation 3(1), 1987 — 操作空间控制、$\Lambda$、$\boldsymbol\mu$、$\mathbf p$ 与动力学一致广义逆的原始论文
M. Takegaki & S. Arimoto, A New Feedback Method for Dynamic Control of Manipulators, ASME J. Dynamic Systems, Measurement, and Control 103(2), 1981 — 雅可比转置加重力补偿的能量稳定性证明
J. Y. S. Luh, M. W. Walker & R. P. C. Paul, Resolved-Acceleration Control of Mechanical Manipulators, IEEE Trans. Automatic Control 25(3), 1980 — 在关节空间做的同一件事
J. Nakanishi, R. Cory, M. Mistry, J. Peters & S. Schaal, Operational Space Control: A Theoretical and Empirical Comparison, Int. J. Robotics Research 27(6), 2008 — 多种操作空间控制律的推导与实验比较
L. Sentis & O. Khatib, Synthesis of Whole-Body Behaviors through Hierarchical Control of Behavioral Primitives, Int. J. Humanoid Robotics 2(4), 2005 — 带优先级的多任务操作空间控制
C. Ott, Cartesian Impedance Control of Redundant and Flexible-Joint Robots, Springer, 2008 — 力矩控制轻型臂上的笛卡尔阻抗与零空间控制
R. Martín-Martín, M. A. Lee, R. Gardner, S. Savarese, J. Bohg & A. Garg, Variable Impedance Control in End-Effector Space: An Action Space for Reinforcement Learning in Contact-Rich Tasks, IROS 2019 — 把末端空间控制当作学习的动作空间
K. Lynch & F. Park, Modern Robotics, Ch. 11 — 任务空间运动控制、力控制与混合控制
B. Siciliano & O. Khatib (eds.), Springer Handbook of Robotics, 2nd ed., Springer, 2016 — 运动控制与力控制的章节