第 4 章 速度运动学与雅可比¶
速度运动学把关节速度 \(\dot q\) 线性映射到末端速度，其核心矩阵是雅可比（Jacobian）\(J(q)\)。它一头连着位置误差的迭代求解（逆运动学），一头连着力的静力平衡与可操作度分析，是运动学与动力学、控制的接口。内容包括微分运动与速度旋量、几何与解析两种雅可比、奇异位形与可操作度判据，以及静力对偶 \(\tau=J^\top F\)。示例仍用第 3 章的平面二连杆。
1. 微分运动与速度旋量¶
末端位姿 \(T(q)\in SE(3)\) 对时间的导数仍落在机器人本体上，可写成一个小旋量（twist）\(\mathcal{V}\in\mathbb{R}^6\) 作用于当前位姿：
其中 \(\mathcal{V}_s=(v_s,\omega_s)\) 是相对空间（基）坐标系的速度旋量：线速度 \(v\) 与角速度 \(\omega\) 各三分量。对 \(T\) 求导并用链式法则，可得空间雅可比的第 \(i\) 列就是关节 \(i\) 在当前位形下的旋量轴（对转动副 \((-\omega_i\times q_i,\omega_i)\)，\(q_i\) 取关节当前位置）。这正是第 3 章「POE 公式与指数积」里旋量轴的运动学对应物。
空间旋量与物体旋量（body twist）只差一个伴随变换：\(\mathcal{V}_s=\mathrm{Ad}_T\,\mathcal{V}_b\)，雅可比同理 \(J_s=\mathrm{Ad}_T J_b\)。两种表达都合法，混用却会得到错误结果，而且报错时很难判断错在参考系还是错在公式。约定很简单：位姿用哪个系表达，速度就用同系，整条链路（FK、雅可比、IK 更新）保持一致。
同一个雅可比连接两条通路：关节速度到末端速度，末端力到关节力矩：
flowchart LR
j["几何雅可比 J(q)<br/>每列是一个关节的贡献"]
qd["关节速度 q̇"] -->|"J"| v["末端速度旋量 V"]
f["末端力 F"] -->|"Jᵀ"| tau["关节力矩 τ"]
j --- qd
j --- f2. 几何雅可比与解析雅可比¶
同一速度映射有两种常见写法，混用会导致姿态速率算错：
- 几何雅可比（geometric Jacobian）：列由旋量轴直接拼出，末端速度取空间 twist \((v,\omega)\)——角速度部分是真实角速度矢量。它几何意义清晰，与 \(SE(3)\)、伴随（adjoint）衔接。
- 解析雅可比（analytic Jacobian）：末端"速度"取某个姿态参数化的时间导数（如欧拉角速率 \(\dot\phi\)），上半行为线速度、下半行为该参数的速率。因欧拉角速率与真实角速度之间还有一个仅依赖姿态的变换矩阵 \(E(\phi)\)，二者不相等：\(\mathcal{V}_{\text{geom}} = [I,0;0,E]\,\mathcal{V}_{\text{anal}}\)。
对平面二连杆（位置输出 \((p_x,p_y)\)），略去姿态行的 \(2\times2\) 位置雅可比为
q = [np.radians(30), np.radians(90)]
## J(q) = [[-1.366025, -0.866025],
## [ 0.366025, -0.5 ]]
\(q=(30^\circ,90^\circ)\) 处 \(J_v=\begin{bmatrix}-1.366&-0.866\\0.366&-0.5\end{bmatrix}\)。
\(E(\phi)\) 的存在解释了一个常见现象：同样的物理运动，换个姿态参数化就换一套雅可比数值。控制回路里更常用几何雅可比——角速度是真实矢量，可直接与陀螺读数、角速度指令对照；只有需要与欧拉角轨迹逐项对应时（如示教回放）才用解析雅可比，并显式乘上 \(E\)。
库里的三种参考系：手推时得先说清“在哪个系表达”，工程里由 API 明确指定——Pinocchio 的 getJointJacobian / getFrameJacobian 必须给参考系：pin.LOCAL（随体运动系）、pin.WORLD（世界轴、随体原点）、pin.LOCAL_WORLD_ALIGNED（随体原点、世界轴向）。三者数值不同、用途不同：算末端速度常用最后一个（轴是世界向、锁在末端原点），与教材里几何雅可比最常见的那一种对应。
import pinocchio as pin
model = pin.buildModelFromUrdf("panda.urdf"); data = model.createData()
pin.computeJointJacobians(model, data, q)
J = pin.getFrameJacobian(model, data, model.getFrameId("panda_link8"),
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
与手推结果对不上时，先查参考系与表达点（body 原点还是 frame 原点），而不是怀疑公式。
3. 速度映射与奇异位形¶
映射 \(\dot x = J(q)\dot q\) 把 \(n\) 个关节速度映到 \(m\) 维任务速度（平面位置 \(m=2\)，空间位姿 \(m=6\)）。当 \(J\) 失去满秩，某些笛卡尔方向的速度无论怎样组合关节都产生不出来——该位形为奇异位形（singularity），对 \(6\times6\) 空间雅可比即 \(\det J=0\)。
平面二连杆在 \(q_2=0\)（手臂完全伸直）时奇异：此时两连杆共线，末端只能沿垂直于链的方向运动，沿链长方向的速度不可达。
Jsg = jac2([np.radians(45), 0.0])
print(np.linalg.det(Jsg @ Jsg.T)) # 4.16e-16 ≈ 0，秩亏损，奇异
\(\det(J J^\top)\approx 4\times10^{-16}\)（浮点零），可操作度归零。奇异附近 \(J^{-1}\) 元素剧增，逆速度解发散，工程上须回避或降增益处理（见「逆运动学」第 5 章的阻尼最小二乘）。
把同一套速度映射对上面的 6R 球腕臂升成 \(6\times6\) 空间雅可比（列即各关节当前旋量轴 \(\mathrm{Ad}_{T_{i-1}}\mathcal{S}_i\)）：在 \(\theta=(20,30,-10,0,40,15)^\circ\) 处 \(\det J=-0.0286\)、非奇异；令腕俯仰 \(\theta_5=90^\circ\)，yaw 轴转到与 roll 轴共线，\(\det J\approx-1.1\times10^{-17}\)、可操作度 \(w\approx4.4\times10^{-10}\to0\)——球形腕的典型奇异。
球形腕奇异的结构值得记住：三根腕轴共面时末端少一个转动自由度，而这个位形只由腕角决定、与末端位置无关——所以避奇异时可以只调腕，不必整臂让路。
解析列与数值差分给出的是同一张矩阵，逐列相等的前提是列定义与参考系一致，这也是差分常被当作雅可比实现对照的原因：
## 6 R 球腕臂 task_jac(θ) vs 有限差分：‖J - J_FD‖ = 1.9e-10
## θ5=90° 腕奇异：det(J) = -1.1e-17, w = 4.4e-10
4. 可操作度与 Yoshikawa 判据¶
Yoshikawa 可操作度（manipulability）量化"离奇异多远"：
它对应单位关节速度球经 \(J\) 映射后的速度椭球体积——\(w\) 大表示各方向都容易出速度，\(w=0\) 即奇异。对平面二连杆的位置雅可比，\(w=L_1L_2|\sin q_2|\)：
w = np.sqrt(np.linalg.det(J @ J.T)) # q=(30,90) 处 = 1.0 = L1*L2*|sin q2|
\(q=(30^\circ,90^\circ)\) 处 \(w=1.0\)（肘部直角、可操作性最好），且与解析式 \(L_1L_2|\sin q_2|\) 吻合。可操作度越大越远离奇异，是机械臂工作空间着色与避奇异的常用标量。
可操作度有两个使用限制：它有量纲（长度\(^m\)），平面臂与空间臂的 \(w\) 不可直接比，不同尺度的机器人之间也不可比，比较前要归一化；它只看行列式，对“某个方向接近奇异、其余方向很好”的构型不够敏感——那种情形看条件数 \(\sigma_{\max}/\sigma_{\min}\) 或最小奇异值更准。椭球的三主轴就是 \(J\) 的奇异向量，长短轴比即各向异性程度。
5. 静力对偶 τ = JᵀF¶
雅可比的转置把末端力/力矩映射回关节力矩，来自虚功原理：静力平衡下
\(\tau\in\mathbb{R}^n\) 为关节力矩，\(F\in\mathbb{R}^m\) 为末端广义力。这说明同一个 \(J\) 既描述速度正变换 \(\dot x=J\dot q\)，又描述力的逆变换 \(\tau=J^\top F\)——速度映射与力映射互为对偶。
F = np.array([1.0, 0.5])
tau = J.T @ F # [-1.183013, -1.116025]
末端受力 \((1,0.5)\) 在 \(q=(30^\circ,90^\circ)\) 需关节力矩 \((-1.183,-1.116)\) 平衡。奇异位形处 \(J^\top\) 降秩，意味着某些末端力无法由任何关节力矩平衡（力沿不可达方向泄漏），这也是奇异区不宜做力控的原因。力控与导纳律本身不属这一层，归 control-note。
对偶还有另一半：给定关节力矩上限 \(\|\tau\|\le1\)，末端能产生的力集合是 \(J^{-\top}\) 映射后的椭球；力椭球与速度椭球主轴方向相同、轴长互为倒数。所以“某方向速度小”的地方往往“该方向出力大”，这是用杠杆换来的——力控规划里可以利用，但也意味着奇异方向上力会被几何放大、对扰动更敏感。
6. 要点¶
- 末端速度写成 twist \(\mathcal{V}_s=J_s(q)\dot q\)，空间雅可比各列即关节在当前位形的旋量轴。
- 几何雅可比用真实角速度、解析雅可比用姿态参数速率，二者差一个 \(E(\phi)\)，跨写法须换算。
- \(\dot x=J\dot q\) 降秩即奇异（平面臂 \(q_2=0\) 伸直、\(\det JJ^\top\approx0\)），逆速度在奇异附近发散。
- Yoshikawa 可操作度 \(w=\sqrt{\det(JJ^\top)}\) 度量离奇异远近，平面二连杆 \(w=L_1L_2|\sin q_2|\)。
- 静力对偶 \(\tau=J^\top F\)：同一雅可比同时管速度正变换与力逆变换，奇异方向力不可平衡。