第 3 章正运动学¶

正运动学（forward kinematics, FK）解决"给定关节角，末端在哪里"。它是运动链的输入侧，逆运动学、雅可比、动力学都建立在它之上。本章给两条主流建模路线——Denavit–Hartenberg（DH）参数法与 Lynch–Park 的乘积指数公式（product of exponentials, POE）——并说明 POE 如何与 URDF 的 joint origin 直接对应。示例用一个平面二连杆、一个空间二关节链与一台 6R 球腕臂。

两条建模路线与 URDF 的对应关系：

flowchart LR
    dh["DH 参数法<br/>相邻连杆 4 参数 θ d a α"] --> T["T(θ) ∈ SE(3)"]
    poe["POE 指数积<br/>螺旋轴 (ω, v) + 关节角"] --> T
    urdf["URDF joint origin"] -.->|"直接对应螺旋轴与关节角"| poe

1. DH 参数法与连杆变换¶

DH 约定在每个连杆 \(i\) 上放一个坐标系 \(\{i\}\)，用四个数描述 \(\{i-1\}\to\{i\}\)：绕 \(x_{i-1}\) 的扭角 \(\alpha_{i-1}\)、沿 \(x_{i-1}\) 的杆件长度 \(a_{i-1}\)、绕 \(z_{i-1}\) 的关节角 \(\theta_i\)、沿 \(z_{i-1}\) 的偏距 \(d_i\)。标准 DH（Craig 记号）的连杆变换为这四个基本算子的复合：

\[ {}^{i-1}A_i = R_{z}(\theta_i)\,T_z(d_i)\,T_x(a_{i-1})\,R_x(\alpha_{i-1}) =\begin{bmatrix}c\theta&-s\theta c\alpha&s\theta s\alpha&a c\alpha\\ s\theta&c\theta c\alpha&-c\theta s\alpha&a s\alpha\\ 0&s\alpha&c\alpha&d\\ 0&0&0&1\end{bmatrix}. \]

串联 \(n\) 个关节的正运动学就是连乘 \({}^{0}T_{n}=A_1(q_1)\cdots A_n(q_n)\)。DH 的软肋是约定分裂：标准 DH 把坐标系建在连杆近端、修正 DH（modified / kinematic）建在远端，前向 / 后向又各有版本，同一台机器人换本教材会给出不同的参数表，混用直接算错。

2. POE 公式与指数积¶

POE 不在连杆上逐个建坐标系，而是只在零位（home configuration）读出一根旋量轴（screw axis）\(\mathcal{S}_i\)。对绕单位轴 \(\omega_i\)、过点 \(q_i\) 的转动副，\(\mathcal{S}_i=(v_i,\omega_i)=(-\omega_i\times q_i,\ \omega_i)\)。正运动学写成指数积：

\[ T(\theta)=e^{[\mathcal{S}_1]\theta_1}\,e^{[\mathcal{S}_2]\theta_2}\cdots e^{[\mathcal{S}_n]\theta_n}\,M, \]

\(M\in SE(3)\) 是零位时末端坐标系相对基坐标系的变换，每个 \(e^{[\mathcal{S}_i]\theta_i}\) 用第 2 章的指数映射（旋转部分 Rodrigues、平移部分配套 \(G\) 矩阵）算出。

## 空间 2R：关节1绕 z 过原点，关节2绕 y 过 (L1,0,0)
S1 = np.array([0, 0, 0,  0, 0, 1.0])                 # (v, w)
w2 = np.array([0, 1.0, 0]);  q2pt = np.array([1.0, 0, 0])
S2 = np.concatenate([-np.cross(w2, q2pt), w2])        # v = -w×q
M  = np.eye(4);  M[0, 3] = 1.5                        # 零位末端 (L1+L2,0,0)
T  = exp_twist(S1, th1) @ exp_twist(S2, th2) @ M      # exp_twist: 6 旋量 -> 4x4

零位 \(T(0,0)\) 的末端位置为 \((1.5,0,0)\)；令 \(\theta_2=90^\circ\)，末端落到 \((1.0,0,-0.5)\)，与"把 \((1.5,0,0)\) 绕 \((1,0,0)\) 沿 \(y\) 轴转 \(90^\circ\)"的直接几何复合逐元素一致，两者差为 \(0.0\)。

3. URDF joint origin 到 POE 的映射¶

POE 的优势在于与 URDF 的关节定义天然对齐，不必再引入中间坐标系。一个 URDF revolute joint 携带：

origin xyz / rpy：该关节坐标系相对父连杆的姿态，正是零位下用来读 \(\omega_i\) 与轴上一点 \(q_i\) 的参照；
axis：关节轴方向，取作 \(\omega_i\)；
由 origin 链式相乘可得各关节零位变换，进而组装出 \(M\) 与各 \(\mathcal{S}_i\)。

mujoco-textbook 的 URDF→MJCF 转换、ROS2-note 的 TF2 树都建立在同一 origin 语义上。URDF XML 规范（ros.org/urdf/XML/joint）把 rpy 定义为绕固定轴 X、Y、Z 依次施加 roll、pitch、yaw 的角度（外旋），因此 \(R = R_z(\text{yaw})\,R_y(\text{pitch})\,R_x(\text{roll})\)；它与绕活动轴按 Z→Y→X 的内旋写法等价，轴序写反或改用内旋 XYZ 都会得到不同矩阵。

4. 串联开链 FK 复合¶

无论是 DH 的 \({}^{0}T_{n}=A_1\cdots A_n\) 还是 POE 的 \(e^{[\mathcal{S}_1]\theta_1}\cdots M\)，都是把每个关节的一次运动按第 1 章「帧的复合与相对描述」的规则连乘。区别只在"每一步表达什么"：DH 表达相邻坐标系间固定的几何偏置加一个关节变量；POE 表达绕空间固定轴的一个指数运动。两条路线对同一机构给出相同末端位姿，可互为校验——把 DH 表转成旋量轴、或把旋量轴拟合成 DH 参数，是工业臂建模迁移的常规操作。

求末端位置时可只取各 \(A_i\) / \(e^{[\mathcal{S}_i]\theta_i}\) 作用后的前三行第四列；求完整位姿则保留整块 \(SE(3)\)，姿态部分留给雅可比与轨迹使用。

把同一套 POE 用于六关节串联臂即得完整空间机械臂。以一台自定义几何的 6R 球腕臂（肩偏置 \(d_1=0.5\)、大/小臂 \(a_2=0.6,a_3=0.4\)、腕部三轴交于腕心，故满足 Pieper 可分）演示：六根零位旋量轴 \(\mathcal{S}_i=(-\omega_i\times q_i,\omega_i)\) 与 \(M\) 一旦列出，FK 就是六个指数因子的连乘。

## 6 R 球腕臂，θ=(20,30,-10,0,40,15)° 代入指数积
T = exp_twist(S1,th1) @ exp_twist(S2,th2) @ ... @ exp_twist(S6,th6) @ M
## FK 腕心位置 = (0.434589, 0.158177, -0.186808)

这一位形下末端的 \(SE(3)\) 位姿可直接算出；它的 \(6\times6\) 雅可比与腕奇异在第 4 章用有限差分核验，逆解在第 5 章用阻尼最小二乘迭代——三章共用同一台 6R 臂，把 FK→雅可比→IK 串成闭环。

5. 平面二连杆手推与落地¶

平面二连杆（\(L_1=L_2=1\)，两轴均绕 \(\hat z\)）是最小可读实例。末端位置

\[ p_x = L_1\cos q_1 + L_2\cos(q_1+q_2),\qquad p_y = L_1\sin q_1 + L_2\sin(q_1+q_2). \]

def fk2(q):
    q1, q2 = q
    return np.array([np.cos(q1) + np.cos(q1+q2), np.sin(q1) + np.sin(q1+q2)])

print(fk2([np.radians(30), np.radians(90)]))   # [0.366025 1.366025]

取 \(q=(30^\circ,90^\circ)\) 得末端 \((0.366, 1.366)\)。它同时是第 5 章逆运动学的测试靶：对这一目标点解析求逆，再回代 FK，误差为 \(0.0\)。平面臂各轴同向，POE 退化为绕同一 \(\hat z\) 的指数之和，演示了"轴共线时旋量耦合"的边界，空间臂（上一节的 2R）才会显现各轴异向的复合。

工程里 FK 不手推：Pinocchio 的 pin.forwardKinematics(model, data, q) 算关节帧位姿，再用 pin.updateFramePlacements(model, data) 把它刷到带名字的 frame 上，取 data.oMf[frame_id] 就是齐次变换。一个易错点：joint 与 frame 是两套命名——getJointId("panda_joint8") 与 getFrameId("panda_link8") 往往差一个安装变换（工具中心点常在 frame 上而不是 joint 上），拿错得的是法兰而不是 TCP。

6. 要点¶

DH 用四参数描述相邻坐标系、连乘得 FK，但存在标准 / 修正 / 前向 / 后向多套约定，同一机器换口径即换表，不可混用。
POE 只在零位读旋量轴 \(\mathcal{S}_i=(-\omega_i\times q_i,\omega_i)\) 与 \(M\)，用指数积 \(T=e^{[\mathcal{S}_1]\theta_1}\cdots M\) 求 FK，免建中间坐标系。
POE 与 URDF 关节语义直接对齐；rpy 是固定轴 X→Y→Z 外旋，等价于活动轴 Z→Y→X 内旋。
两路线连乘本质同于帧复合，可对同一机构互校；平面二连杆 \(q=(30^\circ,90^\circ)\to(0.366,1.366)\) 是 FK 与后续 IK 的最小验证靶。

参考文献与延伸阅读¶

Craig, J. J. Introduction to Robotics: Mechanics and Control (4th ed.). Pearson, 2017.
Lynch, K. M., & Park, F. C. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. 书籍主页
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. Robotics: Modelling, Planning and Control. Springer, 2009. DOI

第 3 章 正运动学¶