Part II · 第 7 章结构与运动学 · 7.6

静力学与 wrench

机器人的活儿都干在接触里：拧螺丝、打磨、插孔、搬东西。接触靠力，而刚体上的"一个力"远不止三个数：同样 10 N，推在门把手上和推在门轴上，转动效果天差地别。把力矩和力打包成一个六维向量，就是 wrench，它与 twist 互为对偶，功率正是两者的内积。有了它，换坐标系只要用伴随的转置，关节力矩就是 τ = g(q) + JᵀF，手臂在哪里有力、哪里无力，一个力椭球就说清楚了。

约 32 分钟前置：3.4 机器人雅可比 · 5.8 伴随 · 1.8 叉积 · 7.4 速度运动学 6 个交互

1为什么机器人需要它

拧螺丝要沿轴向压、同时绕轴转；打磨要贴着曲面维持一个恒定的法向力；插孔对不准时，要让接触力把零件"引"进去；搬箱子时，手腕要承受箱子的重量，以及重量对手腕的力矩。这些任务里，机器人和世界之间交换的不是位置，而是力。可是作用在刚体上的力不只是三个数。作用点不同，同一个力对物体的转动效果就不同，所以描述接触，必须把力和它对参考点的力矩放在一起，成为一个六维量：wrench（力旋量）。

它与 7.4 节的 twist 是一对。twist 说刚体怎么动（角速度、线速度），wrench 说外界怎么推它（力矩、力），两者配对得到功率。本课要回答四个问题：

怎样用一个 6 维向量描述接触力，并在传感器系、工具尖端系和世界系之间换算？（FIG 1、FIG 2）
手爪处的 wrench 需要多大的关节力矩去支撑？连杆和负载自己的重量又要多少？（FIG 3）
给定电机力矩上限，手臂在工作空间的哪里最有力，哪里最弱？（FIG 4、FIG 5）
腕部力传感器的读数里混着工具的重力，怎样把它扣掉，留下真正的接触力？（FIG 6）

3.4 节已经在平面里用虚功原理推出过 $\boldsymbol\tau=J^\top\mathbf F$，还画了力椭圆。这一课是它的三维续篇，不再重复那些推导，而是补上三件新东西：力矩成了向量（它有方向，也有参考点）、换坐标系要用伴随的转置、以及一旦手臂不水平躺在桌上，重力就成了静力学里最大的"外力"。

2直觉：wrench = 力 + 相对一点的力矩

推一扇门。力的大小方向都一样，推在门把手上，门轻松打开；推在门轴上，门纹丝不动。差别在于杠杆臂：力沿作用线可以滑动而效果不变，但作用线离转轴越远，转动效果越大。要记录"外界对这个刚体做了什么"，就先选一个参考点 $O$，然后记下两样东西：总力 $\mathbf f$，以及所有力对 $O$ 的总力矩 $\mathbf m$（$\mathbf m=\sum\mathbf x_i\times\mathbf f_i$，叉积见 1.8）。这一对就是 $O$ 处的 wrench。

参考点换到 $O'$，总力不变，力矩却变了，因为每个力的杠杆臂变了：若 $\mathbf r$ 是从 $O$ 指向 $O'$ 的向量，则 $\mathbf m'=\sum(\mathbf x_i-\mathbf r)\times\mathbf f_i=\mathbf m-\mathbf r\times\mathbf f$。注意是减号：参考点向力的作用线靠近，力矩变小；落在作用线上，力矩恰好为零。再看两个特殊情形。一对大小相等方向相反、不共线的力叫力偶：总力为零，力矩 $\mathbf m$ 与参考点无关（拧瓶盖时双手的作用），所以力偶是"自由向量"，可以放在刚体上任何地方。一般的 wrench 是力加力偶，总存在一条中心轴：参考点落在这条线上时，力矩与力平行，大小为 $|h|\,\|\mathbf f\|$，$h$ 叫螺距。它正是 twist 的螺旋轴（5.6）在力学里的镜像。

约定：moment 在前，force 在后

本课（和整个课程）的 wrench 写作 $\mathbf F=(\mathbf m,\mathbf f)\in\mathbb R^6$，力矩在前、力在后，与 twist $\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)$（角速度在前）一一对应，这样功率就是 $P=\boldsymbol\xi^\top\mathbf F=\boldsymbol\omega\cdot\mathbf m+\mathbf v\cdot\mathbf f$。单位：力矩 N·m，力 N，功率 W。

下图是一个刚体。选择"力""力偶"或"力+力偶"，拖动滑块改变力和力偶，再移动参考点 $O'$（青色点）：看 $O'$ 处的力矩（紫色箭头）怎样随杠杆臂变化，而力（琥珀色）纹丝不动。绿色虚线是中心轴。

FIG 1wrench = 力 + 对一点的力矩

拖动画面旋转视角

琥珀色：力 $\mathbf f$（0.35 m/N），作用在方块上表面的一点 $P$，虚线是作用线。紫色：参考点处的力矩 $\mathbf m'$（0.5 m 对应 1 N·m，过长时截短）。青色点是参考点 $O'$，虚线是从原点指向它的 $\mathbf r$。力偶用弯曲的紫色箭头画在方块中心。绿色虚线是中心轴；读数里"直接求和"是按定义 $\sum(\mathbf x_i-\mathbf r)\times\mathbf f_i+\mathbf c$ 算出的力矩，与公式 $\mathbf m-\mathbf r\times\mathbf f$ 之差应在 $10^{-15}$ 量级。

试一试

在"力"模式下把参考点 $O'$ 移开原点 1 m 以上，观察 $\mathbf m'$ 的变化，同时留意 $\mathbf f$ 是否变了。
用"移到作用线上"按钮（或滑块）让 $O'$ 落在力的作用线上：$\|\mathbf m'\|<0.06$。力矩去哪了？
切换到"力偶"（$\|\mathbf c\|>0.3$ N·m），把 $O'$ 拖到离原点 1.5 m 以外。$\mathbf m'$ 变了吗？为什么力偶叫"自由向量"？
切换到"力+力偶"（保持 $|h|>0.25$ m，默认值即可），按"移到中心轴上"。此时 $\mathbf m'$ 与 $\mathbf f$ 平行，且 $\mathbf m'=h\mathbf f$。螺距 $h$ 是多少？

核心洞见

wrench 不是"一个力"，而是"力 + 参考点"。换参考点，力不变、力矩按 $\mathbf m'=\mathbf m-\mathbf r\times\mathbf f$ 变。有一个量不随参考点变：$\mathbf f\cdot\mathbf m$（力在力矩方向上的投影），它就是螺距 $h=\mathbf f\cdot\mathbf m/\|\mathbf f\|^2$ 的分子。读数里可以验证。

3动手玩：同一个 wrench，换三个坐标系

真实机器人里 wrench 总要换坐标系：腕部力传感器报告的是传感器系 {s} 里的读数，你关心的是工具尖端接触点 {c} 处的力，控制器又想要世界系 {w} 里的量。物理上是同一个相互作用，只是换了参考点和坐标轴。规则只有一条，来自功率不变（5.8 已经推过，推导见第 5 节）。设 $T_{ab}$ 是 {b} 在 {a} 中的位姿：

$$ \underbrace{\boldsymbol\xi_a=\operatorname{Ad}_{T_{ab}}\,\boldsymbol\xi_b}_{\text{twist: }b\to a},\qquad \underbrace{\mathbf F_b=\operatorname{Ad}_{T_{ab}}^{\!\top}\,\mathbf F_a}_{\text{wrench: }a\to b},\qquad\text{so}\qquad \mathbf F_s=\operatorname{Ad}_{T_{cs}}^{\!\top}\,\mathbf F_c. $$

一句话记法：twist 用 $\operatorname{Ad}$ 往"父系"搬，wrench 用 $\operatorname{Ad}^\top$ 往"子系"搬；换成 $\mathbf F_s$ 时，要用"{s} 在 {c} 里的位姿" $T_{cs}$。下面的实验里，接触点 {c} 在工具尖端，传感器 {s} 在手腕，工具相对传感器歪着。设一个接触 wrench $\mathbf F_c$，看传感器和世界系的读数。最后一行是功率检查：取同一个刚体运动在三个系里的 twist，三个功率 $\boldsymbol\xi^\top\mathbf F$ 必须相等。把"变换"切到"Ad（错误）"，看它怎么崩溃。

FIG 2换坐标系：F_s = Ad_{T_cs}ᵀ F_c

拖动画面旋转视角

三个坐标系：世界 {w}（大）、传感器 {s}（手腕处的圆盘）、接触 {c}（工具尖端）。每个系里都画了琥珀色的力和紫色的力矩（都换算到世界方向画出，力矩 0.12 m 对应 1 N·m，力 0.035 m 对应 1 N）：力是同一个向量，只是出现在三个不同的参考点上，力矩却各不相同：杠杆臂不同。选"Ad（错误）"后箭头变红，指向了不存在的方向。读数的最后两行是功率检查与用 $\operatorname{Ad}$ 算错时的偏差。

令接触 wrench 是一个纯法向力（$f_{cx}=f_{cy}=0$，$f_{cz}>5$ N，三个力矩为 0）。传感器却读到了力矩 $\|\mathbf m_s\|>0.5$ N·m。这个力矩是哪来的？
把"变换"切到"Ad（错误）"，看功率检查的偏差变成多大。换回"Adᵀ"，偏差应回到 $10^{-15}$。

4第二、第三视角：关节力矩、负载地图与力椭球

六轴臂的静力学：τ = g(q) + JᵀF

现在把 wrench 加到手臂上（用 7.3 和 7.4 里同一条 6R 臂）。设手爪对环境施加 wrench $\mathbf F$（环境反推 $-\mathbf F$）。让手臂静止不动，每个关节电机需要提供的力矩是

$$ \boldsymbol\tau \;=\; \underbrace{\mathbf g(\mathbf q)}_{\text{weights of links and payload}} \;+\; \underbrace{J_s(\mathbf q)^{\top}\mathbf F_s}_{\text{the tool's wrench}} \;=\; \mathbf g(\mathbf q)+J_b(\mathbf q)^{\top}\mathbf F_b . $$

两种写法给出完全相同的 $\boldsymbol\tau$：$\mathbf F_b$ 是在工具系里、以工具原点为参考点的 wrench；$\mathbf F_s$ 是在基座系里、以基座原点为参考点的 wrench，二者差一个 $\operatorname{Ad}^\top$，而 $J_b$ 与 $J_s$ 差一个 $\operatorname{Ad}$，两个正好抵消（第 5 节）。FIG 3 里切换"wrench 的坐标轴"，滑块的数值会被换算成另一个系里的同一个 wrench，力矩条一动不动。

重力项 $\mathbf g(\mathbf q)=\partial U/\partial\mathbf q$ 是位能的梯度。本课的模型：三根连杆各是一个质点，放在连杆中点（质量 5、3、1.5 kg，最后一个包含腕部和工具），外加负载质点（离工具原点沿工具 y 轴偏出 $d$）。关节力矩上限 150、200、100、50、60、25 N·m 只是示意数字。先玩一下：用"举起 5 kg"预设，把手臂水平伸直（$q_2$、$q_3$ 都接近 0），哪个关节离上限最近？再加大负载，直到某根条变红：是哪个关节？再试"拧螺丝"：除了向下压 30 N，还在工具轴上加 8 N·m 的力矩，为什么关节 1、4、6 都要出力？

FIG 36R 臂的静力学：τ = g(q) + JᵀF

拖动画面旋转视角

琥珀色箭头是手爪施加的力 $\mathbf f$（0.012 m/N），紫色箭头是力矩 $\mathbf m$（0.04 m/(N·m)），都画在工具原点上。右侧每条力矩条：红色竖线是上限，条底部的细线是其中重力 $g(\mathbf q)$ 的部分，整条是总力矩 $\boldsymbol\tau$；超限的条变红，对应关节在手臂上出现红圈。读数里 $J_b^\top\mathbf F_b-J_s^\top\mathbf F_s$ 是两种写法的差，应在 $10^{-13}$ 量级。

让某个关节的力矩超过上限（条变红）。再想办法让它回到限内：改变姿态、减小负载偏心，哪一种最有效？
加一个非零 wrench，然后切换"wrench 的坐标轴"（工具 ↔ 基座）。滑块数值变了，力矩条变了吗？

手臂在哪里有力？负载地图

重力项对每个关节都是姿态的函数，所以"这条臂能举多重"没有一个数字的答案，而是一张地图。对垂直平面（$q_1=0$）里的每个手爪位置，用解析逆运动学（肘部朝上、工具俯仰角 $\psi$ 固定）求出姿态，再解出使所有六个关节力矩都不超限的最大负载。力矩对负载质量是线性的，$\tau_i=a_i+m\,b_i$，所以答案是闭式的。哪个关节先到上限，也一并标出。

FIG 4最大负载地图（垂直剖面）

横轴是离基座轴线的水平距离 $r$，纵轴是高度 $z$；基座在左侧。色块的青色越浓，负载越大；没有色块的地方不可达或关节超出转角限位。拖动圆圈选择手爪位置，手臂以侧视图画出。负载质点偏离工具原点 $d$（沿工具 y 轴，水平时这就是垂直于纸面的方向，所以偏心只让滚转关节 4、6 受力，不改变平面内的力矩）。"电机规格"把所有关节力矩上限一起乘上一个系数。

找到两个位置：一个由关节 2（肩）先到上限，另一个由关节 3（肘）先到上限。读数里会写出限制关节。它们在地图上大致怎么分布？

三维的力椭球与对偶

3.4 节在平面里画过：关节速度的单位球 $\|\dot{\boldsymbol\theta}\|\le1$ 经 $J$ 映成手爪速度椭圆（半轴 $\sigma_i$），单位关节力矩球 $\|\boldsymbol\tau\|\le1$ 经 $J^\top$ 的"逆"映成力椭圆（半轴 $1/\sigma_i$，方向相同）。三维里把 $J$ 换成线速度部分 $J_v\in\mathbb R^{3\times6}$（点 $\mathbf p$ 的速度：$J_v=J_{s,\text{lin}}-[\mathbf p]_\times J_{s,\text{ang}}$），得到两个椭球：速度椭球（琥珀）和力椭球（紫）。两者主轴相同，但一个的长轴正是另一个的短轴：手爪容易动的方向出不了大力，难动的方向顶得住大力。手臂拉直时 $\sigma_{\min}\to0$：速度椭球被压成一个盘，力椭球拉成一根针，沿手臂方向，结构本身承担载荷，电机几乎不出力（3.4 节"速度与力"）。

切换到"转动"：用 $J_\omega$（角速度部分）得到角速度椭球和力矩椭球。注意单位：$J_v$ 是 m/rad，$J_\omega$ 是无量纲，不能把两块拼成 6×6 再做 SVD（3.4 节误区 4），所以平动与转动要分开看，或者先用一个特征长度做归一化。

FIG 5速度椭球与力椭球

拖动画面旋转视角

预设姿态

琥珀色线框：速度椭球 $\{J\dot{\boldsymbol\theta}:\|\dot{\boldsymbol\theta}\|\le1\}$（半轴 $\sigma_i$ 乘 0.35 m 绘制）。紫色线框：力椭球 $\{\mathbf f:\|J^\top\mathbf f\|\le1\}$（半轴 $1/\sigma_i$ 乘 0.3 m 绘制，超过 1.3 m 的部分截断，实际长度见读数）。两者共享同一组主轴，实线段标出主轴方向。

在"平动"模式下，把手臂拉到近乎伸直（$\sigma_{\min}<0.12$，用"拉直"预设，或把 $q_3$ 和 $q_5$ 都调到接近 0，让工具也与手臂对齐）。力椭球变成了什么形状？沿哪个方向？

一个 wrench，三种看法

几何：一条作用线上的力，加上一个力偶，画成箭头（FIG 1）。代数：一个 6 维向量 $(\mathbf m,\mathbf f)$，随坐标系用 $\operatorname{Ad}^\top$ 变换（FIG 2）。能量：一个把 twist 变成功率的线性泛函 $\boldsymbol\xi\mapsto\boldsymbol\xi^\top\mathbf F$。第三种看法最深：twist 是"向量"，wrench 是"余向量"（对偶空间里的元素）。向量和余向量变换时用互逆转置的矩阵（$\operatorname{Ad}$ 与 $\operatorname{Ad}^{-\top}$），就像速度 $J\dot{\boldsymbol\theta}$ 用 $J$、力矩 $J^\top\mathbf F$ 用 $J^\top$。凡是看到"转置"，想想"功率要保持不变"。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的元素会被高亮。

wrench 的定义，以及为什么是（力矩，力）

设刚体上有一组力 $\mathbf f_i$，作用在点 $\mathbf x_i$（都在坐标系 {a} 里写）。{a} 原点处的 wrench 是

$$ \mathbf F_a=\begin{bmatrix}\hl{m}{\cw{\mathbf m}}\\ \hl{f}{\cv{\mathbf f}}\end{bmatrix},\qquad \cv{\mathbf f}=\sum_i\mathbf f_i,\qquad \cw{\mathbf m}=\sum_i\mathbf x_i\times\mathbf f_i . $$

刚体的 twist $\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)$，其中 $\mathbf v$ 是刚体上此刻位于 {a} 原点的那个点的速度（5.8 节），点 $\mathbf x_i$ 的速度是 $\mathbf v+\boldsymbol\omega\times\mathbf x_i$。所有力做功的功率：

$$ P=\sum_i\mathbf f_i\cdot(\mathbf v+\boldsymbol\omega\times\mathbf x_i)=\mathbf v\cdot\sum_i\mathbf f_i+\boldsymbol\omega\cdot\sum_i\mathbf x_i\times\mathbf f_i=\boldsymbol\omega\cdot\mathbf m+\mathbf v\cdot\mathbf f=\boldsymbol\xi^\top\mathbf F, $$

其中用到标量三重积 $\mathbf f\cdot(\boldsymbol\omega\times\mathbf x)=\boldsymbol\omega\cdot(\mathbf x\times\mathbf f)$（1.8 节）。这就说明了顺序：为了让功率是两个 6 维向量的普通内积，wrench 必须按 twist 的 $(\boldsymbol\omega,\mathbf v)$ 排成 $(\mathbf m,\mathbf f)$。

换参考点与换坐标系：Adᵀ

只换参考点（坐标轴不变），$\mathbf r$ 为从旧点指向新点的向量，就是 FIG 1 里的公式。写成矩阵：

$$ \begin{bmatrix}\cw{\mathbf m'}\\ \mathbf f\end{bmatrix}=\begin{bmatrix}I & -\hl{lever}{[\mathbf r]_\times}\\ 0 & I\end{bmatrix}\begin{bmatrix}\mathbf m\\ \mathbf f\end{bmatrix},\qquad \cw{\mathbf m'}=\mathbf m-\hl{lever}{\mathbf r\times\mathbf f}. $$

再让坐标轴也转过去。设 {b} 在 {a} 中的位姿是 $T_{ab}=(R,\mathbf p)$。twist 变换是 $\boldsymbol\xi_a=\operatorname{Ad}_{T_{ab}}\boldsymbol\xi_b$。功率是标量，不随坐标系变：$\boldsymbol\xi_a^\top\mathbf F_a=\boldsymbol\xi_b^\top\mathbf F_b$。代入，对任意 $\boldsymbol\xi_b$ 成立，所以

$$ \mathbf F_b=\operatorname{Ad}_{T_{ab}}^{\!\top}\mathbf F_a=\begin{bmatrix}R^\top & -R^\top[\mathbf p]_\times\\ 0 & R^\top\end{bmatrix}\begin{bmatrix}\mathbf m_a\\ \mathbf f_a\end{bmatrix}=\begin{bmatrix}R^\top(\mathbf m_a-\mathbf p\times\mathbf f_a)\\ R^\top\mathbf f_a\end{bmatrix}. $$

最后一个等号一目了然：先按 $\mathbf m-\mathbf p\times\mathbf f$ 换参考点（$\mathbf p$ 是新点相对旧点的位置），再用 $R^\top$ 把两个向量转到新轴上。所以 wrench 的换系 = FIG 1 的公式 + 一次旋转。反过来 $\mathbf F_a=\operatorname{Ad}_{T_{ba}}^{\!\top}\mathbf F_b$，因为 $\operatorname{Ad}_{T_{ab}}^{-1}=\operatorname{Ad}_{T_{ba}}$。把 wrench 链式地从接触点搬到传感器再搬到世界，只需连乘 $\operatorname{Ad}^\top$（顺序与位姿相反）。

	twist $\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)$	wrench $\mathbf F=(\mathbf m,\mathbf f)$
换系	$\boldsymbol\xi_a=\operatorname{Ad}_{T_{ab}}\boldsymbol\xi_b$	$\mathbf F_b=\operatorname{Ad}_{T_{ab}}^{\!\top}\mathbf F_a$
换参考点 $\mathbf r$	$\mathbf v'=\mathbf v-\mathbf r\times\boldsymbol\omega$	$\mathbf m'=\mathbf m-\mathbf r\times\mathbf f$
不随点变的部分	$\boldsymbol\omega$	$\mathbf f$
不变量与螺距	$\boldsymbol\omega\cdot\mathbf v$;\ $h=\tfrac{\boldsymbol\omega\cdot\mathbf v}{\\|\boldsymbol\omega\\|^2}$	$\mathbf f\cdot\mathbf m$;\ $h=\tfrac{\mathbf f\cdot\mathbf m}{\\|\mathbf f\\|^2}$
特殊情形	纯平移（$\boldsymbol\omega=0$）：自由向量	力偶（$\mathbf f=0$）：自由向量

中心轴与螺距

$$ \mathbf m_{c_0}=\mathbf m-\mathbf c_0\times\mathbf f=\hl{axis}{h}\,\mathbf f,\qquad h=\frac{\mathbf f\cdot\mathbf m}{\|\mathbf f\|^2},\qquad \text{axis}=\{\mathbf c_0+s\,\mathbf f\}. $$

沿 $\mathbf f$ 方向平移参考点不改变 $\mathbf c\times\mathbf f$，所以整条直线都是中心轴；在这条线上力矩最小（$\|\mathbf m\|$ 取最小值 $|h|\,\|\mathbf f\|$）。与 twist 的螺旋轴 $(\boldsymbol\omega,\mathbf v)$（5.6 节）逐项对应，只是把 $(\boldsymbol\omega,\mathbf v)$ 换成 $(\mathbf f,\mathbf m)$。顺带一个重要的概念：若 $\boldsymbol\xi^\top\mathbf F=0$，称两者互易，该 wrench 沿这个运动不做功。铰链提供的约束 wrench 与它允许的 twist 互易：这是 8.4 节描述接触约束的语言。

虚功：为什么是 JᵀF（三维版）

3.4 节在平面里推过。三维里完全一样，只需把"力×位移"换成"wrench×twist"。关节虚位移 $\delta\mathbf q$ 使手爪产生虚位移 twist $\delta\boldsymbol\xi=J\delta\mathbf q$（$J$ 是 7.4 节的空间或机体雅可比，$\mathbf F$ 取同一个系里的 wrench）。手爪对环境做的虚功必须由电机提供（扣除位能的增加 $\delta U$）：

$$ \boldsymbol\tau^\top\delta\mathbf q=\delta U+\mathbf F^\top\delta\boldsymbol\xi=\Big(\tfrac{\partial U}{\partial\mathbf q}\Big)^{\!\top}\delta\mathbf q+\mathbf F^\top J\,\delta\mathbf q\quad\forall\,\delta\mathbf q\ \Longrightarrow\ \boldsymbol\tau=\mathbf g(\mathbf q)+J^\top\mathbf F . $$

$J_s^\top\mathbf F_s=J_b^\top\mathbf F_b$ 的验证：$J_b=\operatorname{Ad}_{T_{bs}}J_s$，$\mathbf F_b=\operatorname{Ad}_{T_{sb}}^{\!\top}\mathbf F_s$，故 $J_b^\top\mathbf F_b=J_s^\top\operatorname{Ad}_{T_{bs}}^{\!\top}\operatorname{Ad}_{T_{sb}}^{\!\top}\mathbf F_s=J_s^\top(\operatorname{Ad}_{T_{sb}}\operatorname{Ad}_{T_{bs}})^{\top}\mathbf F_s=J_s^\top\mathbf F_s$。位能 $U=\sum_k m_kg\,z_k(\mathbf q)$，所以 $\mathbf g=\sum_k m_kg\,J_{v,k}^\top\hat{\mathbf z}$，其中 $J_{v,k}$ 是第 $k$ 个质点的线速度雅可比，只有靠近基座一侧、会移动这个质点的关节才有非零列。（FIG 3 的重力项、FIG 4 的负载地图都是这样算的。这里的 $\mathbf g(\mathbf q)$ 是静止时的保持力矩；速度不为零时还有惯性与科氏项，那是第 8 章动力学的内容，8.3。）

一节连杆的平衡：递推（8.2 预告）

不用 $J$ 也能算关节力矩：从末端往基座逐节做力和力矩的平衡。设 $\mathbf F_i$ 是连杆 $i-1$ 通过关节 $i$ 施加给连杆 $i$ 的 wrench（在 {i} 系里），$\mathbf F_{n+1}$ 是手爪施加给环境的 wrench。静止时连杆 $i$ 受 $\mathbf F_i$、反作用 $-\mathbf F_{i+1}$（换到 {i} 系）和重力，三者平衡：

$$ \mathbf F_i=\operatorname{Ad}_{T_{i+1,i}}^{\!\top}\mathbf F_{i+1}+\mathbf F^{\text{g}}_i,\qquad \tau_i=\mathbf F_i^\top\mathbf A_i,\qquad i=n,\dots,1 . $$

其中 $\mathbf F^{\text{g}}_i$ 是支撑连杆 $i$ 自重所需的 wrench（重力 wrench 的反号）；$\mathbf A_i$ 是关节 $i$ 在 {i} 系里的螺旋轴，$\tau_i=\mathbf F_i^\top\mathbf A_i$ 取的是关节能传递的那个分量：电机只承担 wrench 在自己轴上的投影，其余五个分量由轴承的约束反力承担（与上面的"互易"同一回事）。这就是牛顿–欧拉逆动力学的静态特例，8.2 加上加速度项就是完整算法。

雅可比降秩时：Jᵀ 的零空间

$\boldsymbol\tau=J^\top\mathbf F$：若 $\mathbf F\in\ker J^\top=(\operatorname{im}J)^\perp$，则 $\boldsymbol\tau=0$。六轴臂在非奇异位形下 $J$ 是可逆的 6×6，$\ker J^\top=\{0\}$，任何非零 wrench 都需要关节出力。奇异位形下 $\ker J^\top\neq\{0\}$：存在非零 wrench（FIG 5 里的"针"）完全由结构承担，电机出力为零。反过来估计接触力：$\hat{\mathbf F}=J^{-\top}(\boldsymbol\tau_{\text{meas}}-\mathbf g)$，测量误差被放大 $1/\sigma_{\min}$ 倍，在奇异方向上估计会崩溃。

预告：摩擦锥与抓取方程

摩擦（8.4）：点接触能传递的 wrench 不是任意的。在接触系里（$z$ 沿法向）是 $(\mathbf m,\mathbf f)=(0,\ f_x,f_y,f_z)$，且 $f_z\ge0$、$\sqrt{f_x^2+f_y^2}\le\mu f_z$：六维 wrench 空间里的一个圆锥，称摩擦锥。抓取（9.3）：$k$ 个接触把各自的 wrench 用 $\operatorname{Ad}^\top$ 搬到物体系再相加，$\mathbf w=\sum_iG_i\mathbf f_i=G\mathbf f_c$，静止要求 $G\mathbf f_c=-\mathbf w_{\text{ext}}$（例如重力）且每个 $\mathbf f_i$ 都在摩擦锥内。接触力能不能"顶住"任意方向的外力，就是力封闭的问题。

6机器人中的它：腕部力传感器与重力补偿

腕部六维力/力矩传感器（F/T sensor）把应变片的信号标定成传感器系里的 wrench。把它装在手腕与工具之间，它测的却是工具对传感器的总 wrench：接触力，再加上工具自重。工具歪着时重力在传感器各轴上的分量都不为零，即使没碰到任何东西，读数也不是零。重力方向固定在世界系里，随着手腕转动，它在传感器系里的分量随之变化，必须在线扣除：用工具质量 $m_t$、质心 $\mathbf c_t$（在传感器系里）和当前姿态算出重力 wrench，再相减：

$$ \mathbf F_{\text{contact}}^{s}=\mathbf F_{\text{raw}}^{s}-\underbrace{\begin{bmatrix}\mathbf c_t\times\mathbf f_g^{s}\\ \mathbf f_g^{s}\end{bmatrix}}_{\text{gravity wrench}},\qquad \mathbf f_g^{s}=R_{ws}^\top\begin{bmatrix}0\\0\\-m_tg\end{bmatrix} . $$

这里的 $\mathbf c_t\times\mathbf f_g^s$ 就是 FIG 1 的 $\mathbf r\times\mathbf f$：重力作用在质心而不是传感器原点。下图是侧视图。把传感器倾斜，看原始读数怎么随角度变；再给一个接触力；最后故意把质量参数标错 10%，看补偿后的残差。

FIG 6F/T 传感器：重力偏置与补偿

侧视图（x–z 平面）

原始读数 = 接触 + 重力（传感器系，参考点为传感器原点）。四列依次是原始读数、重力模型、补偿后的读数、真实的接触 wrench；力的条满幅为 ±20 N，力矩的条满幅为 ±3 N·m。侧视图里中性色（与工具同色）的箭头是重力（0.04 m/N），琥珀色是环境推在工具尖端上的力，红蓝线是传感器的 x、z 轴。图只画了倾角，"滚转"绕工具轴转动，只改变 $f_x,f_y$ 的分配。

把接触力调到 0，再把工具倾斜到接近水平，让原始读数里的力矩超过 1.5 N·m。什么也没碰到，传感器却读到了力矩：重力作用在质心上，质心离传感器原点有一段杠杆臂（FIG 1）。

真实系统里

六维力/力矩传感器：ATI（Mini/Gamma/Nano 系列）、Robotiq FT 300、OnRobot HEX 等装在腕部，报告传感器系里的 wrench，使用前要先"去皮"（tare）并做重力补偿。关节力矩传感：Franka Emika Panda 的每个关节都有力矩传感器，KUKA LBR iiwa 同样；控制器用 $\hat{\mathbf F}_{\text{ext}}=J^{-\top}(\boldsymbol\tau_{\text{meas}}-\mathbf g-\dots)$ 估计末端受到的外力（libfranka 把估计值放在机器人状态里，顺序为先力后力矩）。电机电流估计：没有力矩传感器的臂（例如 UR 的 CB3 一代）靠电流估计关节力矩，再用 $J^\top$ 反推，精度受齿轮摩擦限制（9.4 节）。重力补偿 + 手把手示教：控制器输出 $\boldsymbol\tau=\mathbf g(\mathbf q)$，人推一下手臂就能轻松移动，这是示教与数据采集的基础。阻抗/导纳控制（Hogan 1985；Part IV）：把 $\mathbf F=K\,\delta\mathbf x+\dots$ 做成控制律，插孔、打磨、人机协作都靠它。遥操作与力反馈：力觉手柄（如 Phantom、Omega 系列）把远端测到的 wrench 经 $J^\top$ 变成手柄电机力矩。足式机器人：脚底接触 wrench 经 $J^\top$ 变成腿部关节力矩，全身控制把它们当约束来优化（9.2 节）。

7常见误区

误区 1：把 wrench 当 twist 来换系（用 Ad 而不是 Adᵀ）

Ad 和 Adᵀ 的块结构看起来很像，只差一个转置，代码里最容易写错，而且不会报错：数字照样算出来，只是错的。FIG 2 的功率检查就是用来抓这个错误的。一个反例：传感器在世界系里的位置是 $\mathbf p=(1,0,0)$，朝向与世界相同，施加纯力 $\mathbf f=(0,0,1)$ N（力矩为零）。正确答案 $\mathbf F_s=\operatorname{Ad}^\top\mathbf F_w$ 给出力矩 $-\mathbf p\times\mathbf f=(0,1,0)$ N·m，而用 Ad 得到的是 $(0,0,0,\ 0,0,1)$：杠杆臂造成的力矩整个丢了。写完换系的函数，永远用 $\boldsymbol\xi_a^\top\mathbf F_a=\boldsymbol\xi_b^\top\mathbf F_b$ 做单元测试。

误区 2：(m, f) 与 (f, m) 的顺序，以及 Ad 的块顺序

不同库的约定不同：本课、Modern Robotics、Featherstone 的空间向量、Drake 都是（角/力矩在前，线/力在后）；ROS 的 geometry_msgs/Twist 是先 linear 后 angular，Wrench 是分开的 force、torque 两个字段；Pinocchio 的 6 维向量先线性后角；libfranka 的外力估计也是先力后力矩。顺序一变，Ad 的块也要对调（$\begin{bmatrix}R&0\\ [\mathbf p]_\times R&R\end{bmatrix}$ 变成 $\begin{bmatrix}R&[\mathbf p]_\times R\\ 0&R\end{bmatrix}$）。接别人的数据或库时，先用 $\boldsymbol\xi^\top\mathbf F$ 的一个已知例子验证顺序。

误区 3：换了参考点却忘了杠杆臂

"我只在工具尖端推了 10 N 的力，传感器怎么读到了力矩？"因为传感器的参考点在手腕，尖端在 0.4 m 外：$\mathbf m_s=\mathbf p\times\mathbf f$，4 N·m 的力矩纯粹是杠杆臂造成的，没有人"扭"过工具。反过来，若控制器要的是尖端的接触力，直接用传感器原点的 $\mathbf f$ 虽然数值对（力不随点变），但算接触力矩时必须换点。把"力沿作用线滑动不变"与"力垂直于作用线挪开就变"分清：前者只改变力矩为零（$\mathbf r\parallel\mathbf f$）的参考点。

误区 4：忘了传感器的重力偏置（以及它随姿态变化）

在一个姿态下"去皮"（读数清零）只补偿了那个姿态的重力。手腕一转，重力在传感器系里的分量就变了（FIG 6）。必须用姿态实时计算 $\mathbf f_g^s=R_{ws}^\top(0,0,-m_tg)$ 和 $\mathbf c_t\times\mathbf f_g^s$，并标定 $m_t$、$\mathbf c_t$。参数有误会留下与姿态有关的残差，比如质量差 10% 会留下 10% 的重力残差，在力控制里表现为"手臂自己在慢慢爬"。传感器还有温漂，需要定期去皮。负载也包括抓着的零件：抓起一个零件后，$m_t$、$\mathbf c_t$ 都变了。

误区 5：把 τ = JᵀF 当成任何位形、任何电机都成立的真理

两个破绽。其一，奇异位形：FIG 5 的"针"说明某些方向的力矩/力不需要关节出力，反过来也意味着你无法通过关节主动控制这个方向的力，从关节力矩估计力时误差被放大 $1/\sigma_{\min}$ 倍。其二，电机力矩 ≠ 关节力矩：$\boldsymbol\tau$ 是作用在连杆上的力矩；电机出的力矩要先过减速器，经过齿轮摩擦、反向间隙、弹性才到关节，用电流乘力矩常数估计 $\boldsymbol\tau$ 在低速、换向时误差很大（9.4 节）。这也是 Franka、iiwa 要在关节输出端装力矩传感器的原因。另外，$\mathbf g(\mathbf q)$ 只是静态的保持力矩，运动时还要加惯性与科氏项（第 8 章）。

8小练习

1. 原点 $O$ 处的 wrench 是 $\mathbf m=(0,-3,0)$ N·m，$\mathbf f=(0,0,10)$ N。参考点移到 $O'=(0.5,0,0)$ m（坐标轴不变），新的力矩 $\mathbf m'$ 是？

$\mathbf r=(0.5,0,0)$，$\mathbf r\times\mathbf f=(0\cdot10-0\cdot0,\ 0\cdot0-0.5\cdot10,\ 0)=(0,-5,0)$，$\mathbf m'=\mathbf m-\mathbf r\times\mathbf f=(0,-3,0)-(0,-5,0)=(0,2,0)$。检验：原来力的作用点在 $x=0.3$（因为 $\mathbf m=\mathbf x\times\mathbf f=(0,-3,0)$），相对 $O'$ 的杠杆臂是 $(-0.2,0,0)$，$(-0.2,0,0)\times(0,0,10)=(0,2,0)$，吻合。选项 A 是把减号写成了加号，C 是忘了换点。

2. 传感器系 {s} 在世界系 {w} 中的位姿是 $T_{ws}$，世界系里的 wrench 是 $\mathbf F_w$。传感器读到的是？

规则：$\mathbf F_b=\operatorname{Ad}_{T_{ab}}^{\!\top}\mathbf F_a$，$T_{ab}$ 是新系 {b} 在旧系 {a} 中的位姿。这里旧系是 {w}、新系是 {s}，所以用 $T_{ws}$。B 是 twist 的规则；C 把位姿的方向写反了（那是 {w} 在 {s} 中的位姿）；D 忽略了参考点和坐标轴都变了。

3. 刚体的 twist 是 $\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)=(0,0,2,\ 0.5,0,0)$（rad/s，m/s），受到的 wrench 是 $\mathbf F=(\mathbf m,\mathbf f)=(0,0,1.5,\ 4,0,0)$（N·m，N）。wrench 对刚体做功的功率是？

$P=\boldsymbol\omega\cdot\mathbf m+\mathbf v\cdot\mathbf f=2\times1.5+0.5\times4=3+2=5$ W。选 0 W 的人把两个向量的顺序配错了（拿 $\boldsymbol\omega$ 去乘 $\mathbf f$，$\mathbf v$ 去乘 $\mathbf m$），这正是误区 2。3 W 和 2 W 各漏掉了一项。

4. 一根水平放置的均匀杆，长 1 m、质量 3 kg，一端接在肩关节上（轴水平且垂直于杆），另一端固定着一个 2 kg 的重物（可看成质点）。肩关节要提供多大的保持力矩？（$g=9.81$ m/s²）

$\tau=\sum m_kg\,x_k=3\times9.81\times0.5+2\times9.81\times1=14.7+19.6=34.3$ N·m。这就是 $\mathbf g=\sum m_kg\,J_{v,k}^\top\hat{\mathbf z}$：杆的质量集中在中点（杠杆臂 0.5 m），重物在端点（杠杆臂 1 m）。A 漏了杆，D 漏了重物，C 把杆当成全部质量都在端点。

5. 手臂完全伸直，手爪沿手臂方向向外推 100 N（对着一堵墙）。关于关节力矩与力椭球，哪个说法正确？

力沿手臂的作用线通过肩、肘关节，对它们的杠杆臂为零，$\boldsymbol\tau=J^\top\mathbf F\approx0$（$\mathbf F$ 落在 $\ker J^\top$ 里）。力椭球的半轴 $1/\sigma_{\min}\to\infty$，这个方向"一个单位力矩就能顶出很大的力"。代价是这个方向上手爪动不了，也无法靠关节主动调节力（误区 5）。

9延伸阅读

K. Lynch & F. Park, Modern Robotics, Sec. 3.4 (Wrenches), Sec. 5.2 (Statics of Open Chains) & Sec. 5.4 (Manipulability) — wrench、换系 $\operatorname{Ad}^\top$、$\boldsymbol\tau=J^\top\mathbf F$ 与力椭球；本课的 $(\mathbf m,\mathbf f)$ 顺序与它一致
R. Murray, Z. Li & S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994, Ch. 2 — 刚体运动、twist 与 wrench、互易螺旋系统
B. Siciliano, L. Sciavicco, L. Villani & G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 3 — 运动学–静力学对偶、力椭球与可操作度椭球
N. Hogan, Impedance Control: An Approach to Manipulation (Parts I–III), J. Dynamic Systems, Measurement, and Control 107(1), 1985 — 阻抗控制的出处，把接触力当作被控的变量
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 — 在工作空间里把 $\mathbf F$ 与关节力矩联系起来的力控制框架
R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008, Ch. 2 — 空间向量代数：运动向量与力向量如何对偶地变换
R. S. Ball, A Treatise on the Theory of Screws, Cambridge University Press, 1900 — 螺旋理论的历史源头：twist 与 wrench 是同一种几何对象的两面

	twist \(\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)\)	wrench \(\mathbf F=(\mathbf m,\mathbf f)\)
换系	\(\boldsymbol\xi_a=\operatorname{Ad}_{T_{ab}}\boldsymbol\xi_b\)	\(\mathbf F_b=\operatorname{Ad}_{T_{ab}}^{\!\top}\mathbf F_a\)
换参考点 \(\mathbf r\)	\(\mathbf v'=\mathbf v-\mathbf r\times\boldsymbol\omega\)	\(\mathbf m'=\mathbf m-\mathbf r\times\mathbf f\)
不随点变的部分	\(\boldsymbol\omega\)	\(\mathbf f\)
不变量与螺距	\(\boldsymbol\omega\cdot\mathbf v\);\ \(h=\tfrac{\boldsymbol\omega\cdot\mathbf v}{\\|\boldsymbol\omega\\|^2}\)	\(\mathbf f\cdot\mathbf m\);\ \(h=\tfrac{\mathbf f\cdot\mathbf m}{\\|\mathbf f\\|^2}\)
特殊情形	纯平移（\(\boldsymbol\omega=0\)）：自由向量	力偶（\(\mathbf f=0\)）：自由向量