2. 机器人运动学基础
在机器人实时运动控制系统中，运动学是连接“任务空间指令”与“关节空间执行”的数学桥梁。无论是让机械臂末端沿直线运动，还是让移动底盘按指定航向行驶，控制器都必须先回答两个问题：末端当前在哪里、姿态如何？要到达目标位置，各关节应转到什么角度？前者是位姿表示问题，后者是正逆运动学求解问题。本节围绕位姿描述、旋转矩阵、欧拉角、四元数、齐次变换以及正逆运动学求解方法展开，为后续轨迹规划、伺服控制和实时插补打下基础。
一、位姿表示：位置与姿态的统一描述
机器人的“位姿”由位置和姿态两部分组成。位置通常用三维向量 p = [x, y, z]ᵀ 表示，描述某坐标系原点在参考坐标系中的坐标；姿态则描述该坐标系三个坐标轴的方向。只描述位置不足以控制机器人，例如机械臂末端到达同一空间点，但工具朝向不同，焊接、抓取或装配的结果会完全不同。
因此，位姿常用一个 4×4 齐次变换矩阵统一表示。它把旋转矩阵 R（3×3）和平移向量 p（3×1）组合在一起，最后一行补 [0 0 0 1]。这种表示既方便描述坐标系之间的相对关系，也便于通过矩阵乘法实现连续变换。
二、旋转矩阵、欧拉角与四元数
旋转矩阵是姿态最基础的表示方式。一个 3×3 正交矩阵 R 满足 RᵀR = I 且 det(R)=1，其列向量分别表示动坐标系三个轴在参考坐标系中的方向。旋转矩阵的优点是可直接参与坐标变换，缺点是 9 个元素中存在冗余，且不直观。
欧拉角用三个角度描述姿态，例如绕 Z-Y-X 依次旋转的偏航、俯仰、滚转角。它直观、便于人机交互，但存在万向节死锁问题：当俯仰角接近 ±90° 时，偏航与滚转失去独立性，逆解不唯一。四元数用四个数 q = [w, x, y, z] 表示旋转，单位四元数满足 w²+x²+y²+z²=1。它没有奇异性，插值平滑，计算效率高，因此成为实时控制、姿态估计和 SLAM 中的主流姿态表示。
三、齐次变换与正运动学
齐次变换矩阵 T 是机器人运动学中最常用的工具。相邻连杆之间的变换可写成 Tᵢ = Rot(z, θᵢ)·Trans(z, dᵢ)·Trans(x, aᵢ)·Rot(x, αᵢ)，这正是标准 DH 参数的构造方式。把从基座到末端的各连杆变换依次相乘，就得到正运动学模型：
T_0n = T_01 · T_12 · T_23 · … · T_(n-1)n
末端位置 p = T_0n[0:3, 3]
末端姿态 R = T_0n[0:3, 0:3]正运动学求解简单、结果唯一，适合实时计算。控制器在每个控制周期读取关节编码器值，代入上述矩阵连乘，即可得到末端当前位姿，用于反馈、碰撞检测和轨迹跟踪。
四、逆运动学求解方法与工程取舍
逆运动学是实时控制中的难点。给定目标位姿 T，求满足 T_0n(θ)=T 的关节角 θ。常见方法包括解析法、几何法、数值迭代法和优化法。解析法速度快、精度高，但依赖具体构型；数值法通用性强，但需要迭代，存在收敛速度和局部极小问题。
| 方法 | 适用场景 | 优点 | 局限 |
|---|---|---|---|
| 解析法 | 6 轴串联臂、结构满足 Pieper 条件 | 微秒级求解，可枚举多解 | 依赖构型，推导复杂 |
| 几何法 | 平面 2R、SCARA、简单连杆 | 直观，便于教学与调试 | 难以推广到高自由度 |
| 数值迭代法 | 冗余臂、并联机构、通用构型 | 通用，可处理约束 | 迭代耗时，可能不收敛 |
| 优化法 | 带关节限位、避障、能耗约束 | 可综合多目标 | 计算量大，实时性挑战高 |
五、案例：从目标位姿到关节指令
假设六轴机械臂需要把末端移动到目标点并保持工具垂直向下。控制流程如下：
- 视觉或上位机给出目标位姿
T_target，包含位置和姿态四元数。 - 将四元数转换为旋转矩阵，与位置组合成 4×4 齐次矩阵。
- 调用逆运动学求解器，得到若干组关节角候选解。
- 根据关节限位、避障、与当前位姿最近原则选择最优解。
- 将关节角送入伺服环，经插补和力矩控制驱动电机。
- 编码器反馈实际关节角，正运动学计算实际位姿，形成闭环。
六、常见误区与复盘
- 把欧拉角当唯一姿态：欧拉角存在多解和万向节死锁，跨象限插值容易突变，实时系统中应优先使用四元数或旋转矩阵。
- 忽略逆解多解性：同一末端位姿可能对应多组关节角，若不进行选优，机械臂可能发生大范围翻转。
- 混淆正逆运动学实时性：正运动学可高频计算，逆运动学若用数值迭代，必须评估单周期耗时，避免拖慢控制周期。
- 忽略奇异位形：雅可比矩阵降秩时，逆解速度趋于无穷，需采用阻尼最小二乘或奇异回避策略。