正运动学机器人学中最“老实”的部分 给定输入输出唯一没有歧义没有搜索没有迭代。大家好本文出自《具身智能基础》专栏是本栏目下的第八篇文章。在之前的连载中我们系统梳理了机器人学的数学基础与位姿空间。接下来的内容我们将进入机器人运动学的核心正向运动学FK与逆向运动学IK的完整理论与实操本文将聚焦于正运动学——这个看似“简单直接”却支撑起整个具身智能底层物理交互的基石。全文8000余字建议收藏阅读。目录01 位姿的数学空间机器人末端活在哪里位置和姿态两个不能混为一谈的东西旋转矩阵SO3的严格定义齐次变换矩阵把旋转和平移焊在一起02 坐标系的建立DH 参数DH 四参数几何直觉坐标系分配规则标准 DH vs 改进型 DH03 连杆变换矩阵的推导四步走四个基本变换的分解完整变换矩阵矩阵各元素的物理意义推导验证矩阵乘法的展开过程04 正运动学连乘的艺术从基座到末端的链式相乘完整推导示例平面 3R 机械臂不同关节角下的末端位姿05 从公式到代码Python 实现DH 参数表驱动的通用 FK 函数完整示例平面 3R 机械臂完整示例6DOF 工业机械臂雅可比矩阵的数值计算预告06 场景痛点 × 解决方案 × 关键代码场景 A工业机械臂标定精度第一场景 B人形机器人全身姿态计算实时性第一场景 CSim2Real 的运动学一致性验证可靠性第一07 写在最后一个“正着算”的问题为什么值得讲想象一个场景一台六轴机械臂正在执行焊接任务工程师在示教器上输入了六个关节角度基座旋转 30°、肩部抬起 45°、肘部弯曲 -20°、腕部翻转 60°、俯仰 -15°、末端旋转 90°。按下启动键的瞬间机械臂末端精准地落在了焊缝起点。这一过程机械臂控制器做的事情可以用三句话概括先把六个关节角度作为输入通过一系列确定的矩阵运算算出末端执行器在三维空间中的精确位置和朝向。这就是正运动学Forward KinematicsFK——机器人学中最基础、也最确定的计算模块。说它确定是因为给定关节角度输出唯一、数值稳定、计算量可控。它不像逆运动学那样需要在高维空间中搜索、可能多解、可能无解、可能在奇异点附近发散。正运动学是一条单行道输入关节角 → 连乘变换矩阵 → 输出末端位姿整个过程没有“歧义”。但确定不等于简单。要把一根根物理连杆、一个个旋转关节变成可以在计算机里精确计算的数学模型需要一套严谨的约定体系。这套体系的核心就是 DH 参数Denavit-Hartenberg Convention。本文将从最底层的数学概念出发走完「位姿表示 → DH参数定义 → 连杆变换推导 → 正运动学连乘 → 代码实现 → 工程落地」这条完整链路。它是理解后续逆运动学、动力学、轨迹规划的前提具身智能的每一层上层能力都建立在这个看似枯燥的正运动学基础之上。01 位姿的数学空间机器人末端活在哪里位置和姿态两个不能混为一谈的东西机械臂末端在空间中的状态由两部分组成位置Position末端在基坐标系中的坐标是一个三维向量。姿态Orientation末端坐标系相对于基坐标系的旋转关系用旋转矩阵描述。合在一起位姿Pose就是位置 姿态。位置很好理解三个数确定一个点。但姿态就不那么直观了——为什么不能也用三个数来描述呢因为你不能简单地对旋转做加法。两个旋转矩阵的平均不是旋转矩阵两个旋转的线性插值会产生畸变。旋转构成了一个非交换群先绕轴转 90° 再绕轴转 90°和反过来做结果是不同的。举个例子把一本书平放在桌上先沿长边翻 90°书竖起来了再沿短边翻 90°书朝侧面倒了。然后复原先沿短边翻 90°再沿长边翻 90°——你看到的封面方向完全不同因为旋转不满足交换律。旋转矩阵SO3的严格定义旋转矩阵是的正交矩阵满足所有满足这两个条件的矩阵构成特殊正交群。第一个条件保证旋转不改变向量长度保距第二个条件排除反射保定向。旋转矩阵的每一列有明确的物理含义第列就是旋转后坐标系第个坐标轴在原坐标系中的方向向量。其中是旋转后坐标系的三个轴是原坐标系的轴。矩阵的每个元素就是两组基向量的点积。齐次变换矩阵把旋转和平移焊在一起机器人学中一个连杆相对于另一个连杆的关系既包含旋转又包含平移。为了把它们统一在一个矩阵里我们引入齐次坐标和齐次变换矩阵本专栏前面有过具体介绍字母含义齐次变换矩阵同时编码旋转和平移旋转矩阵描述姿态平移向量描述位置零向量最后一行维持齐次坐标结构▲齐次变换矩阵结构齐次变换矩阵意义在于两个变换的复合变成了矩阵乘法。如果坐标系相对于的变换是坐标系相对于的变换是那么相对于的变换就是一个矩阵乘法就把两次旋转 两次平移的复合算清楚了——这就是正运动学能用连乘来实现的原因代数基础。02 坐标系的建立DH 参数先提个问题如何给每个连杆贴一个坐标系一个自由度的机械臂有个关节每个连杆都在运动。要描述整个机械臂的运动学我们需要在每个连杆上固联一个坐标系用确定的参数描述相邻坐标系之间的关系通过矩阵连乘得到末端坐标系的位姿但是难点在于相邻两个关节轴在空间中的关系可能是任意的。它们可能平行、相交、也可能既不平行也不相交异面直线。那么如何用最少的参数完整描述这种关系1955 年Jacques Denavit 和 Richard Hartenberg 给出了答案任意两条空间直线之间的关系只需要 4 个参数就能完全确定。这就是 DH 参数。DH 四参数几何直觉DH 参数的核心思想利用两条关节轴之间的公垂线Common Perpendicular建立坐标系。▲DH参数几何含义对于第个关节定义四个参数对于转动关节Revolute Joint是变量其余三个参数为常量对于移动关节Prismatic Joint是变量其余三个参数为常量每个连杆只有一个自由度所以 DH 参数中只有一个变量。为什么是 4 个参数而不是 6 个一般的刚体变换有 6 个自由度3 旋转 3 平移。DH 参数只用了 4 个是因为它利用了一个关键约束关节轴的方向决定了轴坐标系约束减少了自由度。这种约束是合理的因为机械臂的关节轴是物理存在的不是任意方向。坐标系分配规则给机械臂建立 DH 坐标系遵循以下步骤▲多连杆机械臂坐标系分配1.编号基座为第一个连杆为以此类推末端执行器为。2.轴沿第个关节的运动轴线方向转动关节为旋转轴移动关节为平移方向。3.轴沿和的公垂线方向从指向。如果两轴平行取两轴间任意垂线方向。4.轴由右手定则确定。5.原点与的交点即公垂线在上的垂足。特殊情况处理当和相交时无公垂线取两轴的叉积方向当和平行时公垂线不唯一取通过的那条基坐标系通常与在时重合标准 DH vs 改进型 DH在实际工程中存在两种 DH 约定标准 DHDenavit-Hartenberg 原始版本改进型 DHCraig 在《Introduction to Robotics》中提出。▲标准DH与改进型DH对比核心区别在于坐标系固联的位置不同。标准 DH 变换矩阵改进型 DH 变换矩阵两种约定没有对错之分但千万不能混用。在一个项目中选定一种后贯彻到底。Craig 的改进型 DH 在教学和现代教材中更常见但标准 DH 在工业控制器和早期文献中仍是主流。本文后续推导采用标准 DH。03 连杆变换矩阵的推导四步走四个基本变换的分解DH 变换的本质是把一个复杂的空间变换分解为 4 个基本操作的有序组合。每个操作只涉及一个参数。▲四步变换分解Step 1: 绕轴旋转让轴绕转过角度转到与对齐的方向。对于转动关节这是唯一随关节运动的变量。Step 2: 沿轴平移沿轴移动距离使坐标原点到达与公垂线的交点。Step 3: 沿轴平移沿公垂线方向移动连杆长度到达轴上。Step 4: 绕轴旋转让绕转过连杆扭角使其与对齐。完整变换矩阵将四个变换按顺序相乘展开计算后得到这就是 DH 参数法中最核心的公式单个连杆的变换矩阵。矩阵各元素的物理意义当变化时转动关节矩阵中的旋转部分和平移部分同时变化——这正是关节运动导致末端位姿变化的数学表达。是自变量矩阵是因变量。推导验证矩阵乘法的展开过程为了确保大家能理解这个矩阵是怎么来的这里展示关键步骤其中机器人学中常用简写记法。继续乘最后乘推导完成。每一步都是确定的矩阵乘法不涉及任何迭代或搜索——这也是正运动学确定性的来源。04 正运动学连乘的艺术从基座到末端的链式相乘有了单个连杆的变换矩阵正运动学的计算就变成了简单的矩阵连乘▲正运动学链式相乘其中末端执行器坐标系在基坐标系中的位姿第个连杆坐标系相对于第个连杆坐标系的变换自由度数量关节数为什么连乘是对的这是齐次变换矩阵的核心性质——变换的复合等于矩阵的乘积。考虑三个坐标系相对于的位姿为相对于的位姿为那么中的点 $\boldsymbol{p}^{C}$ 在 $\{A\}$ 中的坐标为所以推广到个连杆就是连乘。完整推导示例平面 3R 机械臂为了把抽象的公式落地我们用一个最经典的例子平面三连杆机械臂3R Planar Arm。▲平面3R机械臂DH坐标系三个连杆在同一平面内三个关节都是转动关节所有轴垂直于平面指向纸面外。DH 参数表为什么因为所有关节轴平行都垂直于平面没有沿方向的偏移也没有连杆扭角。这使得变换矩阵大大简化。逐个计算变换矩阵由于DH 变换矩阵简化为第一级变换其中。第二级变换前两级复合利用和角公式化简令最终结果令末端位姿为末端位置平移部分末端姿态旋转部分这个结果完全符合物理直觉——平面机械臂的末端位置就是各连杆在 方向的投影之和末端朝向就是所有关节角的累加。DH 参数法的作用在此体现即使是复杂的 3D 机械臂也只需要同样的连乘过程只是参数表不同。不同关节角下的末端位姿▲不同关节角下的机械臂姿态同一台机械臂给定不同的关节角度输入通过正运动学计算得到不同的末端位姿输出。这就是正运动学的本质从关节空间到位姿空间的确定映射。05 从公式到代码Python 实现DH 参数表驱动的通用 FK 函数正运动学的代码实现非常直接——就是矩阵连乘。以下是完整的 Python 实现import numpy as np def dh_transform(theta, d, a, alpha): 计算单个连杆的 DH 变换矩阵 (Standard DH) 参数: theta: 关节角 (弧度) d: 连杆偏距 a: 连杆长度 alpha: 连杆扭角 (弧度) 返回: 4x4 齐次变换矩阵 T_i^{i-1} ct, st np.cos(theta), np.sin(theta) ca, sa np.cos(alpha), np.sin(alpha) T np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d ], [0, 0, 0, 1 ] ]) return T def forward_kinematics(dh_params, joint_angles): 正运动学从关节角计算末端位姿 参数: dh_params: DH 参数表list of (theta_offset, d, a, alpha) theta_offset 是零位时的关节角偏移 joint_angles: 关节角向量 (弧度) 返回: T_n^0: 4x4 齐次变换矩阵末端在基坐标系中的位姿 T np.eye(4) # 从单位矩阵开始 for i, (theta_off, d, a, alpha) in enumerate(dh_params): theta joint_angles[i] theta_off # 实际关节角 输入角 偏移 T_i dh_transform(theta, d, a, alpha) T T T_i # 连乘 return T def extract_pose(T): 从齐次变换矩阵中提取位置和姿态 返回: position: (x, y, z) 位置 euler_angles: (roll, pitch, yaw) 欧拉角 (ZYX 顺序) position T[:3, 3] R T[:3, 3] # 从旋转矩阵提取 ZYX 欧拉角 pitch np.arctan2(-R[2, 0], np.sqrt(R[0, 0]**2 R[1, 0]**2)) yaw np.arctan2(R[1, 0], R[0, 0]) roll np.arctan2(R[2, 1], R[2, 2]) return position, np.array([roll, pitch, yaw])完整示例平面 3R 机械臂# # 平面 3R 机械臂正运动学示例 # # DH 参数表: (theta_offset, d, a, alpha) # 平面机械臂: d0, alpha0 dh_params_3r [ (0, 0, 0.5, 0), # 关节1: l10.5m (0, 0, 0.4, 0), # 关节2: l20.4m (0, 0, 0.3, 0), # 关节3: l30.3m ] # 测试三组关节角 test_configs [ [0, 0, 0], # 完全伸直 [np.radians(45), np.radians(-30), np.radians(20)], # 弯曲状态1 [np.radians(90), np.radians(60), np.radians(-45)], # 弯曲状态2 ] print( * 60) print(平面 3R 机械臂正运动学计算结果) print( * 60) for i, angles in enumerate(test_configs): T forward_kinematics(dh_params_3r, angles) pos, euler extract_pose(T) print(f\n配置 {i1}: θ [{, .join(f{np.degrees(a):.1f}° for a in angles)}]) print(f 末端位置: x{pos[0]:.4f} m, y{pos[1]:.4f} m, z{pos[2]:.4f} m) print(f 末端姿态: φ{np.degrees(euler[2]):.1f}° (平面内朝向)) print(f 变换矩阵:\n{T.round(4)})输出示例配置 1: θ [0.0°, 0.0°, 0.0°] 末端位置: x1.2000 m, y0.0000 m, z0.0000 m 末端姿态: φ0.0° (平面内朝向) 配置 2: θ [45.0°, -30.0°, 20.0°] 末端位置: x0.8136 m, y0.6036 m, z0.0000 m 末端姿态: φ35.0° (平面内朝向)完整示例6DOF 工业机械臂# # 6DOF 机械臂正运动学 (类似 UR5 结构) # dh_params_6dof [ # (theta_offset, d, a, alpha) (0, 0.089, 0, np.pi/2), # 关节1: 基座旋转 (0, 0, -0.425, 0), # 关节2: 肩部 (0, 0, -0.392, 0), # 关节3: 肘部 (0, 0.109, 0, np.pi/2), # 关节4: 腕部翻转 (0, 0.094, 0, -np.pi/2), # 关节5: 腕部俯仰 (0, 0.082, 0, 0), # 关节6: 末端旋转 ] # 计算零位时的末端位姿 T_home forward_kinematics(dh_params_6dof, [0]*6) print(零位末端位姿矩阵:) print(T_home.round(4)) # 计算任意关节角时的末端位姿 joint_angles [np.radians(30), np.radians(45), np.radians(-20), np.radians(60), np.radians(-15), np.radians(90)] T_target forward_kinematics(dh_params_6dof, joint_angles) pos, euler extract_pose(T_target) print(f\n关节角: [{, .join(f{np.degrees(a):.1f}° for a in joint_angles)}]) print(f末端位置: ({pos[0]:.4f}, {pos[1]:.4f}, {pos[2]:.4f}) m) print(f末端姿态: RPY ({np.degrees(euler[0]):.1f}°, {np.degrees(euler[1]):.1f}°, {np.degrees(euler[2]):.1f}°))雅可比矩阵的数值计算预告正运动学计算的是在某一个关节构型下末端在哪里。如果要计算关节微小运动如何影响末端运动就需要雅可比矩阵——它是正运动学对关节角的偏导数虽然雅可比主要用于逆运动学但它的计算基础正是正运动学def compute_jacobian(dh_params, q, delta1e-6): 数值雅可比矩阵 (数值微分法) 参数: dh_params: DH 参数表 q: 当前关节角 delta: 微分步长 返回: J: 6×n 雅可比矩阵 (前3行线速度, 后3行角速度) n len(q) J np.zeros((6, n)) # 当前末端位姿 T0 forward_kinematics(dh_params, q) p0 T0[:3, 3] R0 T0[:3, 3] for i in range(n): # 对第 i 个关节做数值微分 q_plus q.copy() q_plus[i] delta T_plus forward_kinematics(dh_params, q_plus) # 位置差分 - 线速度列 J[:3, i] (T_plus[:3, 3] - p0) / delta # 姿态差分 - 角速度列 (用旋转矩阵差分的反对称部分) dR T_plus[:3, 3] R0.T # 从 dR 提取旋转轴角 (近似) skew 0.5 * (dR - dR.T) J[3:, i] np.array([skew[2,1], skew[0,2], skew[1,0]]) / delta return J06 场景痛点 × 解决方案 × 关键代码场景 A工业机械臂标定精度第一痛点机械臂出厂时的名义 DH 参数与实际值存在偏差加工误差、装配误差、温度变形导致理论 FK 计算的末端位置与实际位置有 0.5~2mm 的误差。在精密装配场景如手机螺丝拧紧、芯片贴装中这个误差不可接受。解决方案用激光跟踪仪测量一组标定点通过非线性最小二乘拟合实际 DH 参数运动学标定。from scipy.optimize import least_squares def calibrate_dh(dh_params_nominal, measured_poses, joint_configs): DH 参数标定: 通过测量的末端位姿修正 DH 参数 参数: dh_params_nominal: 名义 DH 参数 measured_poses: 标定点测量值 [(x, y, z), ...] joint_configs: 对应的关节角 [q1, q2, ...] 返回: dh_calibrated: 标定后的 DH 参数 # 将 DH 参数展平为优化变量 # 优化: d, a, alpha 的偏差 (theta_offset 通常已知) n_joints len(dh_params_nominal) def unpack_params(flat_params): 将展平的参数重组为 DH 参数表 dh [] for i in range(n_joints): d flat_params[i*3] a flat_params[i*3 1] alpha flat_params[i*3 2] theta_off dh_params_nominal[i][0] # 保持名义 theta_offset dh.append((theta_off, d, a, alpha)) return dh def residual(flat_params): 计算所有标定点的残差 dh unpack_params(flat_params) residuals [] for q, measured in zip(joint_configs, measured_poses): T forward_kinematics(dh, q) predicted T[:3, 3] residuals.extend(predicted - np.array(measured)) return residuals # 初始猜测 名义参数 x0 [] for (_, d, a, alpha) in dh_params_nominal: x0.extend([d, a, alpha]) # Levenberg-Marquardt 优化 result least_squares(residual, x0, methodlm) return unpack_params(result.x) # 标定效果: 典型工业机器人标定后精度从 ~1mm 提升到 ~0.05mm场景 B人形机器人全身姿态计算实时性第一痛点人形机器人有 30 自由度双腿各 6、双臂各 7、躯干 3、颈部 2需要在 1kHz 控制频率下实时计算全身各关键点的位姿不只是末端还包括质心、肩部、髋部等。解决方案预计算运动学树Kinematic Tree用拓扑排序并行计算各分支。class KinematicTree: 运动学树: 高效计算多分支机器人的所有连杆位姿 def __init__(self, dh_params_list, parent_indices): 参数: dh_params_list: 各关节的 DH 参数 parent_indices: 各关节的父关节索引 (-1 表示连接基座) self.n len(dh_params_list) self.dh dh_params_list self.parent parent_indices # [−1, 0, 0, 1, 2, ...] def compute_all_frames(self, joint_angles): 一次性计算所有连杆坐标系的位姿 按拓扑顺序计算, 避免重复运算 transforms [np.eye(4)] * (self.n 1) # T_0^0, T_1^0, ..., T_n^0 for i in range(self.n): theta_off, d, a, alpha self.dh[i] theta joint_angles[i] theta_off T_local dh_transform(theta, d, a, alpha) parent_idx self.parent[i] if parent_idx -1: transforms[i 1] T_local # 连接基座 else: transforms[i 1] transforms[parent_idx 1] T_local return transforms # 所有连杆在世界坐标系中的位姿 # 人形机器人: 左臂和右臂可以并行计算 # 30 DOF 的一次 FK 计算: ~0.1ms (NumPy 向量化)场景 CSim2Real 的运动学一致性验证可靠性第一痛点强化学习策略在仿真器Isaac Gym, MuJoCo中训练但部署到真机时如果仿真器的 FK 与真机 FK 不一致策略输出会产生系统性偏差。常见问题包括关节零点偏移、连杆长度误差、坐标系定义不一致。解决方案建立运动学一致性校验管线——在仿真和真机上执行相同的关节角序列对比末端位姿差异。def sim2real_kinematics_check(sim_fk_func, real_fk_func, test_configs): Sim2Real 运动学一致性校验 参数: sim_fk_func: 仿真器的 FK 函数 real_fk_func: 真机的 FK 函数 (通过编码器读数FK计算) test_configs: 测试关节角序列 返回: max_error: 最大位姿误差 mean_error: 平均位姿误差 errors [] for q in test_configs: T_sim sim_fk_func(q) T_real real_fk_func(q) # 位置误差 pos_error np.linalg.norm(T_sim[:3, 3] - T_real[:3, 3]) # 姿态误差 (用旋转矩阵的测地线距离) R_diff T_sim[:3, 3].T T_real[:3, 3] trace np.clip(np.trace(R_diff), -1, 3) rot_error np.arccos((trace - 1) / 2) # 弧度 errors.append({ joint_config: q, pos_error_mm: pos_error * 1000, rot_error_deg: np.degrees(rot_error) }) pos_errors [e[pos_error_mm] for e in errors] rot_errors [e[rot_error_deg] for e in errors] print(f位置误差: max{max(pos_errors):.2f}mm, mean{np.mean(pos_errors):.2f}mm) print(f姿态误差: max{max(rot_errors):.3f}°, mean{np.mean(rot_errors):.3f}°) return max(pos_errors), np.mean(pos_errors) # 合格标准: 位置误差 1mm, 姿态误差 0.5°07 写在最后正运动学是机器人学中最“老实”的部分——给定输入输出唯一没有歧义没有搜索没有迭代。也正是因为这份确定性使它成为整个机器人控制栈的基石。从 DH 参数的四个变量到单个连杆的变换矩阵再到个矩阵的连乘——这条数学链路虽然简单却精确地描述了从关节角到末端位姿的完整映射。理解了它才真正理解了机械臂“为什么能动”。在具身智能时代正运动学的地位不降反升。它是仿真环境的物理基础URDF 本质就是 DH 参数的 XML 化是 Sim2Real 迁移的一致性基准是强化学习 reward 计算中位姿误差的定义来源也是逆运动学、动力学、轨迹规划所有上层模块的底层依赖。正运动学是一道函数题。但这个函数的每一个矩阵元素都包含了对物理世界的数学抽象——坐标系的约定、连杆的几何关系、刚体变换的复合法则。把这些搞清楚后面的路才走得稳。