机器人操作数学重学 02：从指数积到雅可比与奇异性

两根长度都是 1 米的连杆，第一关节不转，第二关节转 90°：末端显然在 (1,1)。下面让三角函数、指数积和雅可比都回答同一个例子，再把机械臂拉直，观察为什么某个方向的瞬时运动消失了。

重学路线 · 前置：坐标、旋转与螺旋运动 · 后续：动力学与控制

1. 从画出零位开始，而不是从背公式开始

本篇采用 MLS 顺序 $V=(v,\omega)$。两个转动关节均绕 z 轴，正方向按右手规则；$q_1$ 是第一杆相对固定 x 轴的角度，$q_2$ 是第二杆相对第一杆的角度。零位时两杆沿 x 轴伸直。

先用三角函数写位置和姿态：

$$p(q)=\begin{bmatrix} l_1\cos q_1+l_2\cos(q_1+q_2)\\ l_1\sin q_1+l_2\sin(q_1+q_2)\\0 \end{bmatrix},\qquad R(q)=R_z(q_1+q_2).$$

这里全部角度以弧度参与计算。取 $l_1=l_2=1$、$q=(0,\pi/2)$，得到 $p=(1,1,0)^T$、$R=R_z(\pi/2)$。

调节角度前先预测末端往哪里走。特别比较“只改变 $q_1$”与“只改变 $q_2$”：前者让整个下游机械臂绕基座转，后者只绕第二关节转。这就是雅可比两列要表达的两个速度场。

2. 指数积把每个关节都写成一个刚体运动

在零位中读取每条关节轴，全部用同一个空间坐标系表达。归一化转轴的线性部分满足 $v=-\omega\times r$，其中 $r$ 是轴上的一点。两关节因此为

$$\xi_1=\begin{bmatrix}0\\0\\0\\0\\0\\1\end{bmatrix},\qquad \xi_2=\begin{bmatrix}0\\-1\\0\\0\\0\\1\end{bmatrix},\qquad M=\begin{bmatrix}I_3&(2,0,0)^T\\0&1\end{bmatrix}.$$

第二轴通过 $(1,0,0)^T$，所以它的线性部分不是零。零位末端变换 $M$ 把“所有关节都为零时末端在哪里”保存下来。

空间形式的指数积（PoE）为

$$g(q)=e^{\widehat\xi_1q_1}e^{\widehat\xi_2q_2}M.$$

轴 $\xi_i$ 是零位轴，不需要随着每次输入角度重新手工填写；前面指数因子的组合已经体现上游关节对下游轴的影响。串联机构的一般公式与这个构造过程一致，但每条轴的坐标和排列必须统一。原书第 3 章、Modern Robotics 官方 PoE 讲解

展开一次，确认它等于三角函数

记 $e_x=(1,0,0)^T$、$R_i=R_z(q_i)$。第二关节绕 $e_x$ 所在的轴转动：

$$e^{\widehat\xi_2q_2} =\begin{bmatrix}R_2&(I-R_2)e_x\\0&1\end{bmatrix}.$$

将它乘到零位 $M$ 上，平移变为 $(I-R_2)e_x+2R_2e_x=e_x+R_2e_x$；再乘第一关节变换：

$$p=R_1e_x+R_1R_2e_x.$$

这正是两根杆的向量和。公式不是另一套答案，而是把相同几何组织成能扩展到空间多关节机构的表示。

3. 雅可比的一列就是“只动这一关节”

空间速度满足 $V_s=J_s(q)\dot q$。第一关节轴在空间中不受其他关节影响；第二关节轴先被第一关节变换过去：

$$J_s(q)=\begin{bmatrix} \xi_1&\operatorname{Ad}_{e^{\widehat\xi_1q_1}}\xi_2 \end{bmatrix} =\begin{bmatrix} 0&\sin q_1\\ 0&-\cos q_1\\ 0&0\\ 0&0\\ 0&0\\ 1&1 \end{bmatrix}.$$

这是一个 $6\times2$ 矩阵，前三行是空间旋量的 $v$，后三行是角速度。它与 Modern Robotics 的雅可比差一个行置换，不能直接对照上三行称为角速度。官方空间雅可比推导

在 $q=(0,\pi/2)$ 时，两列分别是 $\xi_1=(0,0,0;0,0,1)$ 与 $(0,-1,0;0,0,1)$。第一列线性部分为零，是否表示第一关节转动时末端不动？不是：上一讲已经说明空间旋量里的 $v_s$ 并非末端位置的导数。

从旋量雅可比转到点速度雅可比

按行分块 $J_s=[J_v^T,J_\omega^T]^T$。由于 $\dot p=v_s+\omega_s\times p$：

$$\dot p=\left(J_v-\widehat pJ_\omega\right)\dot q, \qquad J_p=J_v-\widehat pJ_\omega.$$

只取平面 x、y 速度，得到常见的 $2\times2$ 点雅可比：

$$J_p^{xy}= \begin{bmatrix} -l_1\sin q_1-l_2\sin(q_1+q_2)&-l_2\sin(q_1+q_2)\\ l_1\cos q_1+l_2\cos(q_1+q_2)&l_2\cos(q_1+q_2) \end{bmatrix}.$$

在当前算例中：

$$J_p^{xy}=\begin{bmatrix}-1&-1\\1&0\end{bmatrix}.$$

若 $\dot q=(1,0)^T$，末端速度为 $(-1,1)^T$；若 $\dot q=(0,1)^T$，则为 $(-1,0)^T$。两者分别是绕基座和绕第二关节作圆周运动的切线方向。

4. 奇异性必须连同任务一起描述

对“只控制末端平面位置”的任务：

$$\det J_p^{xy}=l_1l_2\sin q_2.$$

当 $q_2=0$ 或 $\pi$，两杆共线，点雅可比从通常的秩 2 降为秩 1。以 $q=(0,0)$ 为例：

$$J_p^{xy}=\begin{bmatrix}0&0\\2&1\end{bmatrix}.$$

任意关节速度组合都只能产生 y 方向的一阶点速度，不能产生 x 方向的一阶点速度。弯曲关节仍可让末端最终向内移动，但那是有限运动、包含高阶变化，不能据此否定当前点雅可比的降秩。

不要混淆两个矩阵。 上面的 $6\times2$ 空间雅可比 $J_s$，两列在任意 $q$ 下都线性独立：角分量相同，线性分量相差一个长度为 1 的非零向量，因此秩始终为 2。两关节机构本来就不能任意指定六维位姿速度；这与某个构型让二维位置任务从秩 2 降到秩 1 是两种不同限制。判断奇异时，必须说明输出任务和相对于什么最大秩发生下降。官方奇异性说明

椭圆告诉我们哪些方向费力

若把允许的关节速度写成 $\|\dot q\|_2\le1$，经 $\dot p=J_p^{xy}\dot q$ 映射后，在末端速度平面得到椭圆及其内部。奇异值是主半轴长度，较小奇异值趋近零时椭圆变扁，对应方向需要更大的关节速度才能达到相同末端速度。

这个图依赖约定的关节速度尺度。转动关节与移动关节混合时，不能直接把弧度/秒与米/秒放进无权重的欧氏球；做六维速度分析时，也需定义平移和转动的度量。椭圆既描述机构，也描述你给任务选的单位和权重。

5. 逆运动学：同一点可能有两种肘部姿态

给定目标 $(x,y)$，先计算

$$c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}.$$

若 $|c_2|>1$，目标在几何可达范围之外；等价地，距离 $r$ 必须满足 $|l_1-l_2|\le r\le l_1+l_2$。只允许对浮点舍入造成的微小越界做夹紧，不能把真正不可达的目标强行夹到边界而宣称求解成功。

常规非退化情形的两支解为

$$s_2=\pm\sqrt{1-c_2^2},\qquad q_2=\operatorname{atan2}(s_2,c_2),$$

$$q_1=\operatorname{atan2}(y,x) -\operatorname{atan2}(l_2s_2,l_1+l_2c_2).$$

单位长度两杆到达 $(1,1)$ 的两组解是 $(0,\pi/2)$ 和 $(\pi/2,-\pi/2)$。位置一样，末端朝向不同；若再指定姿态，两支未必都满足。关节限位、碰撞和与上一时刻解的连续性还会进一步筛选结果。

等长两杆折叠到原点时，$q_2=\pi$ 而 $q_1$ 可变化，不能盲目把 atan2(0,0) 当成普通的唯一角度。完全伸直或折叠时，两支也可能合并。

6. 数值迭代与阻尼：承认误差，控制步长

复杂机构可用局部线性模型 $\Delta p\approx J_p\Delta q$。当前位置误差为 $e=p_{\mathrm{target}}-p(q)$，阻尼最小二乘给出

$$\Delta q=J_p^T\left(J_pJ_p^T+\lambda^2I\right)^{-1}e, \qquad q_{\mathrm{next}}=q+\alpha\Delta q.$$

其中 $\lambda>0$ 抑制小奇异值造成的大关节增量，$\alpha$ 控制步长。它求的是在残差与关节增量之间折中的正则化解，不是精确逆矩阵，也不会凭空恢复丢失的运动方向。权重和 $\lambda$ 还应与变量的单位/归一化相配。

实际迭代需要：重新计算正运动学与雅可比，检查误差是否下降，限制关节增量，处理关节限位并设置迭代上限。在位姿任务中，还要用相容坐标下的姿态误差与雅可比，不能把欧拉角逐项相减直接塞进空间旋量。

7. 三个可以自检的练习

练习 A： 在 $q=(0,\pi/2)$ 处，希望末端速度为 $(0,1)^T$，关节速度是多少？答案：由 $-\dot q_1-\dot q_2=0$、$\dot q_1=1$，得到 $(1,-1)^T$。末端角速度 $\dot q_1+\dot q_2=0$。

练习 B： 在 $q=(0,0)$ 处，希望末端速度为 $(1,0)^T$，能否精确实现？答案：不能，目标不在当前 $J_p^{xy}$ 的列空间里。阻尼求解也不能让第一行的零变成非零。

练习 C： 如何检查推导或代码里某一列雅可比的符号？答案：对第 i 个关节做中心差分，计算

$$\frac{p(q+\varepsilon e_i)-p(q-\varepsilon e_i)}{2\varepsilon}$$

并与 $J_p$ 第 i 列比较。逐渐缩小 $\varepsilon$，误差应先下降，过小后浮点抵消会占主导。差分的是点位置，就应比较点雅可比；不能拿它直接对照空间旋量雅可比的前三行。

这一讲只决定“关节怎么动会让末端怎么动”。要回答“需要多大力矩、接触力怎样映射到关节”，继续阅读动力学与控制；涉及多指接触时再接到抓取与非完整约束。