Part I · 第 5 章 刚体运动与李群 · 5.10

指数积公式与正运动学

机械臂的每个关节，都是绕着一根空间中固定的轴做螺旋运动。把各个关节的指数映射 Exp(Sᵢθᵢ) 依次乘起来，再乘上零位时的末端位姿 M，就得到末端的位姿。这就是指数积公式：不用逐连杆建系，只需要看着机械臂的零位图写下每个关节轴。

约 28 分钟 前置：5.3 SE(3) · 5.6 指数映射 · 5.8 伴随 4 个交互

    1为什么机器人需要它

    正运动学 (forward kinematics, FK) 回答机械臂最基本的问题：给定每个关节的角度，末端执行器在哪里、朝向哪里？输入是关节向量 \(\boldsymbol\theta = (\theta_1,\dots,\theta_n)\)，输出是末端位姿 \(T(\boldsymbol\theta)\in\SE\)。控制器每个周期都要算它；逆运动学（给定末端位姿求关节角）要反复调用它；碰撞检测要用它算出每一节连杆在世界里的位置；运动规划、标定、可视化都离不开它。

    传统上，教科书用 Denavit-Hartenberg (DH) 参数：给每个连杆按一套约定建立坐标系，每个关节用 4 个参数描述，再把相邻连杆的 4×4 变换一个个乘起来。它能用，可是建系的约定很多（标准 DH、改进 DH），遇到相邻关节轴平行或相交时约定会有歧义，而且你要在图上一个一个地找公共法线。

    另一种写法是指数积公式 (product of exponentials, PoE)，它直接使用你在 5.6 节认识的螺旋运动：每个关节就是绕空间中一根固定轴的螺旋运动。只需要一个世界坐标系、一张"零位"（所有关节角为 0）时的图，写出每根关节轴的螺旋轴 \(S_i\) 和零位时末端的位姿 \(M\)，就可以直接写出 FK。它的"每个关节一个指数"的结构，还让速度运动学（雅可比，3.4 节）自然地长出来。

    2直觉：关节 = 一根固定的螺旋轴

    想象把机械臂摆成零位，然后只转动第 \(i\) 个关节，其他关节锁死。第 \(i\) 个关节之后的所有连杆会作为一个整体，绕着关节 \(i\) 的轴转。这是一次纯转动（转动关节），或者一次纯平移（移动关节）。把它写成 5.6 节的螺旋轴 \(S_i = (\boldsymbol\omega_i, \mathbf v_i)\)：转动 \(\theta_i\) 弧度的运动就是 \(\Exp(S_i\theta_i)\)。

    怎么从零位图上读出 \(S_i\)？只需要两个信息：轴的方向 \(\hat{\boldsymbol\omega}_i\)（单位向量），和轴上任意一点 \(\mathbf q_i\)。5.6 节的螺旋参数公式反过来用：

    $$ S_{\text{rev}} = \begin{bmatrix}\hl{w}{\cw{\hat{\boldsymbol\omega}}}\\ \hl{v}{\cv{-\hat{\boldsymbol\omega}\times\mathbf q}}\end{bmatrix}, \qquad S_{\text{prismatic}} = \begin{bmatrix}\mathbf 0\\ \hat{\mathbf s}\end{bmatrix}, \qquad S_{\text{screw}} = \begin{bmatrix}\hat{\boldsymbol\omega}\\ -\hat{\boldsymbol\omega}\times\mathbf q + h\,\hat{\boldsymbol\omega}\end{bmatrix}. $$

    顺序仍是 \((\boldsymbol\omega,\mathbf v)\)。转动关节要求 \(\|\hat{\boldsymbol\omega}\| = 1\)，这样 \(\theta_i\) 的单位就是弧度；移动关节要求 \(\|\hat{\mathbf s}\|=1\)，\(\theta_i\) 的单位就是米。最容易搞错的是 \(\mathbf v = -\hat{\boldsymbol\omega}\times\mathbf q\) 这一项：\(\mathbf v\) 不是轴上的点的速度，而是"此刻恰好在世界原点的那个点"的速度（5.8 节）；轴离原点越远，\(\mathbf v\) 越大。下面的实验让你自己摆一根轴，看 \(S\) 怎么变。黄色箭头是世界原点那个点的速度 \(\mathbf v\)，紫色小球是点 \(\mathbf q\)，青色曲线是一个固定在运动物体上的测试点 \(\mathbf p\)（初始在 \((1.5,\,-0.6,\,0.8)\)）在 \(\Exp(sS)\) 下走过的路径。

    FIG 1由关节几何写出螺旋轴 S
    拖动画面旋转视角
    紫色虚线是关节轴，穿过点 \(\mathbf q\)，方向 \(\hat{\boldsymbol\omega}\)。三种关节：转动（螺距 0）、螺旋（螺距 \(h\)，沿轴每转一弧度前进 \(h\)）、移动（\(\boldsymbol\omega = 0\)，方向由方位角和仰角给出）。读数里的 \(\mathbf u(\mathbf q)\) 是点 \(\mathbf q\) 的速度 \(\mathbf v + \boldsymbol\omega\times\mathbf q\)：它应该等于 \(h\hat{\boldsymbol\omega}\)，只沿轴方向。
    试一试
    • 做一个绕 z 轴、过点 \((1,0,0)\) 的转动关节。读数里的 \(S\) 应该是 \((0,0,1;\ 0,-1,0)\)。把轴平移到过原点，\(\mathbf v\) 变成了什么？
    • 切换到移动关节，让它沿 x 轴运动：\(S = (0,0,0;\ 1,0,0)\)。这时测试点 \(\mathbf p\) 的路径是什么形状？轴的位置 \(\mathbf q\) 还重要吗？

    3动手玩：把机械臂"展开"

    从最简单的平面二连杆 (2R) 开始：连杆长 \(L_1, L_2\)，两个转动关节，轴都垂直纸面（z 方向）。零位时手臂沿 x 轴完全伸直，末端在 \(M = \text{Trans}(L_1+L_2,\,0,\,0)\)。关节 1 在原点，关节 2 在 \(\mathbf q_2 = (L_1, 0, 0)\)，所以

    $$ S_1 = \begin{bmatrix}0\\0\\1\\ 0\\0\\0\end{bmatrix},\qquad S_2 = \begin{bmatrix}0\\0\\1\\ -\hat{\mathbf z}\times\mathbf q_2\end{bmatrix} = \begin{bmatrix}0\\0\\1\\ 0\\-L_1\\0\end{bmatrix},\qquad T(\boldsymbol\theta) = \Exp(S_1\theta_1)\,\Exp(S_2\theta_2)\,M. $$

    读公式的办法是从右往左。先看 \(M\)：手臂摆在零位，这是起点。然后施加 \(\Exp(S_2\theta_2)\)：绕关节 2 的轴转动第二根连杆。这根轴是零位时的位置 \((L_1,0)\)，此刻连杆 1 还没动，轴就在那里。最后施加 \(\Exp(S_1\theta_1)\)：绕原点把整条手臂（已经弯好的）转一个 \(\theta_1\)。\(S_2\) 的位置始终取自零位，不会随着关节 1 转动而改变，原因是后面的 \(\Exp(S_1\theta_1)\) 会把它也一起带走。FIG 2 里拖动"阶段"滑块，演示这个展开的过程。

    FIG 22R 机械臂的指数积：从右往左展开
    淡虚线：零位 \(M\)。阶段 0 → 1：绕 \(S_2\)（紫点，零位时的肘部）转动第二根连杆 \(\theta_2\)；阶段 1 → 2：绕 \(S_1\)（原点）转动整条手臂 \(\theta_1\)。黄色曲线是末端走过的路径。注意：当阶段为 2 时，紫点 \(S_2\) 还留在零位的肘部，而真正的肘部（标着"真实肘部"的空心圆点）已经转到别处。
    试一试
    • 把两个关节角都设成 \(|\theta| > 20^\circ\)，再把阶段拖到 2。比较紫点（\(S_2\) 的位置）和真正的肘部：它们为什么不在同一处？
    • 设 \(\theta_1 = 90^\circ\)，\(\theta_2 = 90^\circ\)，阶段 = 2。由三角公式，末端在 \((L_1\cos\theta_1 + L_2\cos(\theta_1+\theta_2),\ L_1\sin\theta_1 + L_2\sin(\theta_1+\theta_2)) = (-L_2,\ L_1)\)。读数里的 PoE 结果一致吗？

    三维：一条 6 自由度机械臂

    同样的做法对任意机械臂都成立。FIG 3 是一条带球形手腕的 6R 臂：关节 1 是腰部转动（绕 z），关节 2 和 3 是肩、肘（绕 \(-y\)，使正角度把手臂抬起来），关节 4、5、6 是手腕（绕 x、\(-y\)、x，三根轴交于同一点）。零位时手臂沿 x 轴水平伸直，肩高 \(H\)。每个关节的轴方向 \(\hat{\boldsymbol\omega}_i\) 与轴上一点 \(\mathbf q_i\) 如下，读数面板会实时列出对应的 \(S_i\)：

    i关节\(\hat{\boldsymbol\omega}_i\)\(\mathbf q_i\)
    1腰\((0,0,1)\)\((0,0,0)\)
    2肩\((0,-1,0)\)\((0,0,H)\)
    3肘\((0,-1,0)\)\((L_1,0,H)\)
    4腕 1\((1,0,0)\)\((L_1+L_2,0,H)\)
    5腕 2\((0,-1,0)\)\((L_1+L_2,0,H)\)
    6腕 3\((1,0,0)\)\((L_1+L_2,0,H)\)

    FIG 3 里取 \(H=0.6\)、\(L_1=1.0\)、\(L_2=0.9\)；手腕中心之外还有一段长 \(L_3=0.35\) 的工具，所以零位末端位姿是 \(M=\text{Trans}(L_1+L_2+L_3,\,0,\,H)\)，朝向与世界系相同。

    FIG 36R 机械臂实验室：T(θ) = Exp(S₁θ₁) ⋯ Exp(S₆θ₆) M
    拖动画面旋转视角
    预设
    淡色的臂是零位。紫色虚线是零位时的 6 根关节轴（它们不跟着连杆走，永远留在原地）。实心连杆由部分乘积 \(\prod_{j<i}\Exp(S_j\theta_j)\) 把各关节点从零位搬到当前位置得到；末端坐标系是 \(T(\boldsymbol\theta)\)。"全部连杆坐标系"模式显示每个关节处的部分乘积 \(T_i = \prod_{j\le i}\Exp(S_j\theta_j)\,M_i\)，其中 \(M_i\) 是零位时放在关节点 \(\mathbf q_i\)、与世界系同向的坐标系。读数里同时用空间形式和机体形式计算 \(T\)，二者的差应为 \(10^{-16}\) 量级。
    试一试
    • 先点"零位"。然后只转动手腕三个关节（任一个超过 \(30^\circ\)）。手腕中心点（肘部右侧那个三轴交点）动了吗？工具尖怎么动？
    • 设 \(\theta_2 = 45^\circ\)，\(\theta_3 = -90^\circ\)，其余为 0。前臂此时朝斜下方。由 \(z = H + L_1\sin 45^\circ + (L_2+L_3)\sin(-45^\circ)\) 预测末端高度，再和读数核对。

    4再看数学

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

    指数积公式

    $$ T(\boldsymbol\theta) = \Exp(\hl{S}{S_1}\hl{theta}{\theta_1})\,\Exp(S_2\theta_2)\cdots\Exp(S_n\theta_n)\;\hl{M}{M}, \qquad \Exp(S\theta) = \begin{bmatrix}\Exp(\hat{\boldsymbol\omega}\theta) & G(\theta)\mathbf v\\ \mathbf 0\T & 1\end{bmatrix},\quad G(\theta) = I\theta + (1-\cos\theta)\skew{\hat{\boldsymbol\omega}} + (\theta-\sin\theta)\skew{\hat{\boldsymbol\omega}}^2. $$

    其中 \(S_i\) 是第 \(i\) 个关节的螺旋轴，在零位时用世界系写出；\(M\) 是零位时的末端位姿；\(G(\theta)\) 是 5.6 节 \(V\) 矩阵在单位 \(\hat{\boldsymbol\omega}\) 下的写法。对移动关节，\(\Exp(S\theta) = \begin{bmatrix}I & \hat{\mathbf s}\theta\\ 0 & 1\end{bmatrix}\)。

    为什么公式是对的

    用归纳法。只动关节 \(n\)：末端相当于被绕轴 \(S_n\) 转，\(T = \Exp(S_n\theta_n)M\)（轴的位置是零位的）。再让关节 \(n-1\) 动起来：它让"第 \(n-1\) 个关节之后的整体"（包括关节 \(n\) 的轴、末端）绕 \(S_{n-1}\) 做一次刚体运动。对世界系里的一个刚体运动 \(\Exp(S_{n-1}\theta_{n-1})\)，作用到已有位姿上就是左乘（5.6 节误区 4），所以 \(T = \Exp(S_{n-1}\theta_{n-1})\Exp(S_n\theta_n)M\)。依此类推。结果的形状是：越靠近基座的关节，越靠左。

    机体形式

    也可以把螺旋轴写在末端坐标系里（零位时）。用 5.8 节的 \(M\,\Exp(B\theta)M^{-1} = \Exp(\operatorname{Ad}_M B\,\theta)\)，令 \(S_i = \operatorname{Ad}_M B_i\)，把所有 \(M\) 依次挪到最右边，得到：

    $$ T(\boldsymbol\theta) = M\,\Exp(B_1\theta_1)\cdots\Exp(B_n\theta_n), \qquad B_i = \operatorname{Ad}_{M^{-1}}S_i. $$

    两种形式算出同一个 \(T\)，只是读轴的坐标系不同（FIG 3 的读数同时算了两个）。对 2R 例子，\(B_1 = (0,0,1;\ 0,\,L_1+L_2,\,0)\)，\(B_2 = (0,0,1;\ 0,\,L_2,\,0)\)：轴在末端系里分别离末端 \(L_1+L_2\) 和 \(L_2\)。

    与 DH 参数对照

    DH 参数指数积 (PoE)
    坐标系每个连杆一个，按约定建立只要一个世界系和一个末端零位 \(M\)
    每个关节的数据\((a,\alpha,d,\theta)\)，其中一个是变量零位时的轴 \(S_i\)，变量是 \(\theta_i\)
    轴平行或相交公共法线不唯一，需要额外约定没有特殊情形
    变体标准 DH、改进 DH，符号和顺序都不同空间形式、机体形式（用 \(\operatorname{Ad}\) 互换）
    雅可比另外推导第 \(i\) 列 \(=\operatorname{Ad}_{\Exp(S_1\theta_1)\cdots\Exp(S_{i-1}\theta_{i-1})}S_i\)
    优点厂商手册、旧代码里很常见；参数少，表格紧凑几何含义直接，容易推广到并联、软体等结构

    两种写法描述的是同一台机器，可以互相换算。机器人描述文件 URDF 对每个关节写的是"父连杆到关节的固定变换 + 轴向量"，精神上更接近 PoE 的局部形式，而不是 DH。

    通向雅可比

    对 \(T(\boldsymbol\theta)\) 求导，第 \(i\) 个关节转动一点，等于对第 \(i\) 个因子的右边所有东西做一次左乘，用 5.8 节的结论，空间雅可比的第 \(i\) 列就是"关节 \(i\) 的轴，在当前位形下的样子"：

    $$ J_s(\boldsymbol\theta) = \begin{bmatrix}S_1 & \operatorname{Ad}_{\Exp(S_1\theta_1)}S_2 & \operatorname{Ad}_{\Exp(S_1\theta_1)\Exp(S_2\theta_2)}S_3 & \cdots\end{bmatrix}, \qquad \hat{\boldsymbol\xi}_s = \dot TT^{-1} = J_s(\boldsymbol\theta)\,\dot{\boldsymbol\theta}. $$

    3.4 节已经从几何上造出了同样的列（那里把线速度放在上面、角速度放在下面，与这里的 \((\boldsymbol\omega,\mathbf v)\) 顺序相反）；Part II 的 7.4 节会在此基础上讨论雅可比的性质、奇异性和它的用法。

    5机器人中的它：可达工作空间

    买一台机械臂、布置一个工作站时，第一个问题是：它够得着吗？够得着的区域叫可达工作空间。最直接的办法就是让 FK 来回答：在关节范围内随机抽很多个 \(\boldsymbol\theta\)，每个都算一次 \(T(\boldsymbol\theta)\)，把工具尖的位置画成一团点云。这个云就是工作空间的近似，云的边缘就是机械臂的"够得着的极限"。FIG 4 用 FIG 3 同一台 6R 臂做这件事。滑块调的是各个关节的活动范围（关节 2、3 的范围分别以 \(30^\circ\) 和 \(-60^\circ\) 为中心的半宽）。

    FIG 4用 FK 采样可达工作空间
    拖动画面旋转视角
    每个点是一组随机关节角下的工具尖位置（共 1200 个）：红色在地面以下（\(z<0\)），青色是低处，绿色是高处。黄色的臂是关节范围中点处的位形，网格是地面 \(z=0\)。把腰部和手腕范围都拉到 0，云会收缩成一个竖直的平面切片（手腕转动会把工具尖带出这个平面）。
    试一试
    • 把腰部范围和手腕范围都设成 0。云变成了什么形状？这和 2R 平面臂的工作空间有什么关系？
    • 调整关节范围和工具长度，使最大水平伸展超过 2.3（读数里的 "最大水平伸展"）。伸展上限由哪几个量决定？
    真实系统里

    逆运动学（Part II）迭代求解 \(T(\boldsymbol\theta) = T_{\text{goal}}\)，每一步都要调用 FK 和上面的雅可比。碰撞检测把每个连杆的几何体放到 \(T_i(\boldsymbol\theta)\) 上。标定要把测量到的末端位姿与 FK 的预测比较，反过来调整 \(S_i\) 和 \(M\)：PoE 的参数随几何误差平滑变化，相邻轴平行时也不会像 DH 那样出现奇异（一个微小的偏差就让 DH 参数跳变），因此很适合做这种参数辨识。运动规划（MoveIt、OMPL 等）在关节空间里采样，用 FK 把采样转到笛卡尔空间检查约束。URDF/SDF 里的关节原点和轴，加上 Pinocchio、KDL 这类库里的 FK 实现，本质上都是在算同一个乘积。

    6常见误区

    误区 1：用当前位形的关节轴

    PoE 空间形式里的 \(S_i\) 必须是零位时的轴（FIG 3 的紫色虚线），一个都不能随着手臂移动而更新。关节 1 转动后，关节 2 的轴在世界里确实挪了位置，可是这一点已经由 \(\Exp(S_1\theta_1)\) 左乘处理了。重复更新一次，等于把这个转动算了两遍。

    误区 2：乘积的顺序写反

    空间形式：\(\Exp(S_1\theta_1)\cdots\Exp(S_n\theta_n)M\)，基座侧的关节在最左边。机体形式：\(M\Exp(B_1\theta_1)\cdots\Exp(B_n\theta_n)\)，\(M\) 在最左边，而关节顺序依旧是 1 到 \(n\) 从左到右。把 \(M\) 放错侧，或者把关节顺序倒过来，FK 在 \(\boldsymbol\theta = 0\) 时仍然正确（所有指数都是单位阵），所以单靠零位测试发现不了，必须用非零位形核对。

    误区 3：\(\mathbf v = -\hat{\boldsymbol\omega}\times\mathbf q\) 的符号和 \(\mathbf q\) 的取法

    写成 \(+\hat{\boldsymbol\omega}\times\mathbf q\) 会得到一根镜像到原点另一侧的轴，转动方向不变，位置却错了。\(\mathbf q\) 必须是世界系里、零位时轴上的一个点，任意哪个点都可以，因为沿轴平移 \(\mathbf q\) 时叉积不变。验证办法：用 FIG 1 那样的小实验，检查 \(S\) 在轴上点处的速度是否为零（转动关节）。

    误区 4：零位与真机的编码器零点不一致

    \(M\) 和 \(S_i\) 描述的"零位"必须与控制器里 \(\theta=0\) 对应的机械位置一致。很多商用机械臂的零点不是"完全伸直"，甚至有一个固定的偏置。要么把偏置加到 \(\theta_i\) 里，要么按真实的零位重新写 \(M\) 和 \(S_i\)。轴的正方向也要和编码器的计数方向一致，不然关节转一个正角度，仿真里的手臂却往反方向走。

    误区 5：twist 顺序、单位与 DH 混用

    本课的 \(S_i = (\boldsymbol\omega_i,\mathbf v_i)\)，和 Modern Robotics 一致；有些库把平移放前面，引用现成的 \(S_i\) 表之前要先对齐。移动关节的 \(\theta_i\) 是长度，要和转动关节的弧度分开处理（尤其是写关节限位、做数值雅可比时）。DH 表与 PoE 的 \(S_i\) 不能直接混着用，转换时要把每个连杆的 DH 变换乘出来，再对照 \(M\) 与各个 \(S_i\)。

    7小练习

    1. 平面 2R 臂零位沿 x 轴伸直，关节 2 位于 \((L_1,0,0)\)，转轴为 \(+z\)。它的螺旋轴 \(S_2 = (\boldsymbol\omega;\mathbf v)\) 是？
    \(\mathbf v = -\hat{\boldsymbol\omega}\times\mathbf q = -(0,0,1)\times(L_1,0,0)\)。叉积 \((0,0,1)\times(L_1,0,0) = (0\cdot0-1\cdot0,\ 1\cdot L_1-0\cdot0,\ 0) = (0,L_1,0)\)，取负号得 \((0,-L_1,0)\)。选项 A 是过原点的轴（\(S_1\)），C 是漏了负号。
    2. \(L_1 = L_2 = 1\)。\(\theta_1 = \theta_2 = 90^\circ\) 时，平面 2R 臂的末端位置是？
    第一根连杆指向 \(+y\)（\(\theta_1=90^\circ\)），肘部在 \((0,1)\)。第二根连杆相对第一根再转 \(90^\circ\)，总朝向 \(180^\circ\)，指向 \(-x\)，末端在 \((0,1)+(-1,0)=(-1,1)\)。与 \((L_1\cos\theta_1+L_2\cos(\theta_1+\theta_2),\ L_1\sin\theta_1+L_2\sin(\theta_1+\theta_2))\) 一致。
    3. 沿单位方向 \(\hat{\mathbf s}\) 的移动关节，螺旋轴是？
    移动关节没有转动，\(\boldsymbol\omega = \mathbf 0\)，\(\mathbf v = \hat{\mathbf s}\)，\(\Exp(S\theta)\) 就是平移 \(\hat{\mathbf s}\theta\)，\(\theta\) 的单位是米。选项 A 是绕 \(\hat{\mathbf s}\) 过原点的转动关节。
    4. 一条 \(n\) 个关节的臂，只有 \(\theta_1\ne0\)，其余都为 0。\(T(\boldsymbol\theta) =\) ？
    其余指数都是单位阵，剩下 \(\Exp(S_1\theta_1)M\)。选项 C 把 \(S_1\)（世界系里写的轴）当作了机体系里的轴：只有换成 \(B_1\) 才可以右乘。
    5. 空间形式和机体形式的螺旋轴之间的关系是？
    \(M\Exp(B\theta)M^{-1} = \Exp(\operatorname{Ad}_MB\,\theta)\)，所以 \(S_i = \operatorname{Ad}_MB_i\)，\(B_i = \operatorname{Ad}_{M}^{-1}S_i = \operatorname{Ad}_{M^{-1}}S_i\)。选项 C 把 4×4 矩阵直接乘在 6 维向量上，维数都对不上。

    8延伸阅读

    • K. Lynch & F. Park, Modern Robotics, Ch. 4 (Forward Kinematics) — 空间形式、机体形式、URDF；本课的记号和 (ω, v) 顺序与它一致
    • R. Brockett, "Robotic manipulators and the product of exponentials formula", in Mathematical Theory of Networks and Systems, Springer LNCIS 58, 1984 — 指数积公式的出处
    • R. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 3 — 用螺旋理论系统地推导正运动学和雅可比
    • B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 2 — DH 参数和标准的建系步骤
    • J. Craig, Introduction to Robotics: Mechanics and Control, Ch. 3 — 改进 DH 的约定