2. 机器人运动学基础
双臂机器人协同控制的第一步，是让控制器“知道”两条手臂各自处在什么位姿、末端执行器在哪里、关节速度与末端速度之间是什么关系。运动学基础正是回答这些问题的语言。本节围绕刚体变换、DH参数、正逆运动学、雅可比矩阵与奇异位形五个核心概念展开，它们构成从关节空间到任务空间的完整映射链条。
一、刚体变换：描述位姿的数学工具
刚体在三维空间中的状态由位置和姿态共同决定，称为位姿。位置用三维平移向量 p = [x, y, z]ᵀ 表示，姿态用 3×3 旋转矩阵 R 表示。将两者组合成 4×4 齐次变换矩阵，可以统一描述平移与旋转：
T = [ R p ]
[ 0 1 ]
其中 R 满足 RᵀR = I，det(R) = 1齐次变换的优点是复合变换可以通过矩阵连乘完成。若从基座到末端依次经过 n 个坐标系，则总变换为 T = T₁·T₂·…·Tₙ。旋转矩阵的常见参数化方式包括欧拉角、轴角、四元数等，其中四元数因无万向节死锁、插值平滑，在机器人姿态解算中被广泛使用。理解刚体变换是后续所有运动学推导的起点。
二、DH参数：建立连杆坐标系的规范方法
Denavit-Hartenberg（DH）参数法用四个参数描述相邻连杆之间的几何关系，使坐标系的建立有章可循。标准DH参数定义如下：
| 参数 | 含义 | 作用 |
|---|---|---|
| aᵢ | 连杆长度 | 沿 xᵢ 轴，从 zᵢ 到 zᵢ₊₁ 的距离 |
| αᵢ | 连杆扭角 | 绕 xᵢ 轴，从 zᵢ 到 zᵢ₊₁ 的转角 |
| dᵢ | 连杆偏距 | 沿 zᵢ 轴，从 xᵢ₋₁ 到 xᵢ 的距离 |
| θᵢ | 关节角 | 绕 zᵢ 轴，从 xᵢ₋₁ 到 xᵢ 的转角 |
对于旋转关节，θᵢ 是变量，其余三个为常量；对于移动关节，dᵢ 是变量。相邻坐标系的变换矩阵可写为：
Tᵢ = Rot(z, θᵢ)·Trans(z, dᵢ)·Trans(x, aᵢ)·Rot(x, αᵢ)DH参数的价值在于：只要按规则建立坐标系，就能机械地写出每个连杆的变换矩阵，避免人为错误。双臂机器人中，左右臂各自有一套DH参数，基座之间还存在一个固定的基座变换 T_base，用于统一到同一世界坐标系。
三、正运动学与逆运动学
正运动学是“已知关节角，求末端位姿”。给定各关节变量 q = [q₁, q₂, …, qₙ]ᵀ，通过连乘DH变换矩阵得到末端位姿：
T_end = T_base · T₁(q₁) · T₂(q₂) · … · Tₙ(qₙ)正运动学是唯一确定的，计算直接、稳定，常用于仿真、碰撞检测和可视化。
逆运动学是“已知末端位姿，求关节角”。它更困难，原因有三：解可能不存在（目标超出工作空间）、可能多解（肘上/肘下、肩左/肩右）、也可能无穷多解（冗余机械臂）。常用方法包括解析法、几何法和数值迭代法（如雅可比伪逆迭代）。对于六自由度串联臂，若满足Pieper条件（三轴相交或平行），可获得封闭解析解。
四、雅可比矩阵：关节速度到末端速度的桥梁
雅可比矩阵 J(q) 描述关节速度与末端速度之间的线性映射：
ẋ = J(q)·q̇
其中 ẋ = [v; ω] 为末端线速度与角速度
q̇ 为关节速度向量
J 为 6×n 矩阵雅可比矩阵的每一列表示对应关节单位速度引起的末端运动旋量。它在静力学中同样重要：末端力 F 与关节力矩 τ 满足 τ = Jᵀ(q)·F。因此雅可比矩阵是速度控制、力控制、阻抗控制的核心。
当 J 为方阵且满秩时，可求逆得到 q̇ = J⁻¹·ẋ；当 n > 6（冗余）时，使用伪逆 J⁺ = Jᵀ(JJᵀ)⁻¹，并可在零空间中优化次要目标，如避障、关节限位、可操作度最大化。
五、奇异位形分析
奇异位形是指雅可比矩阵降秩的位形，此时 det(J) = 0（方阵情形），末端在某些方向上失去运动能力。奇异位形分为两类：
- 边界奇异：机械臂完全伸展或折叠，工作空间边界处。
- 内部奇异：两轴或多轴共线，常见于腕部奇异、肩部奇异、肘部奇异。
在奇异点附近，逆雅可比求解会出现关节速度趋于无穷大的现象，导致控制失稳。工程上常用阻尼最小二乘（DLS）方法缓解：
q̇ = Jᵀ(JJᵀ + λ²I)⁻¹·ẋ
λ 为阻尼因子，随可操作度减小而增大可操作度 w = √(det(JJᵀ)) 常用来量化远离奇异的程度。双臂协同中，若两臂同时接近奇异，系统自由度急剧下降，必须提前规划轨迹或调整协同构型。
六、案例与常见误区
以双臂协作搬运一个刚性箱体为例。设左臂末端位姿为 T_L，右臂末端位姿为 T_R，箱体要求两末端保持固定相对变换 T_rel，即 T_L⁻¹·T_R = T_rel。控制流程通常为：
- 根据箱体目标位姿，规划左臂末端轨迹
T_L(t)。 - 由
T_rel计算右臂末端轨迹T_R(t) = T_L(t)·T_rel。 - 分别对两臂做逆运动学，得到关节轨迹
q_L(t)、q_R(t)。 - 用雅可比矩阵将末端速度指令映射为关节速度，并检查可操作度是否远离奇异。
- 若接近奇异，启用DLS或调整协同构型。
运动学基础不是孤立的公式集合，而是连接规划、控制与执行的桥梁。掌握刚体变换与DH建模，才能正确写出正逆运动学；理解雅可比矩阵与奇异位形，才能在双臂协同中既保证精度，又保证稳定。