2. 机械臂运动学与动力学基础
六轴机械臂的力矩控制与柔顺操作，本质上是对机械臂动力学模型的实时求解与补偿。若将机械臂比作人体上肢，运动学描述的是“手在哪里、关节转了多少度”，而动力学则回答“肌肉需要施加多大的力才能完成这个动作”。本章作为力矩控制的理论基石，系统梳理从D-H参数建模到正逆运动学求解，再到雅可比矩阵映射与拉格朗日/牛顿-欧拉动力学方程推导的完整链路，并重点阐明这些数学工具在力矩控制、重力补偿、惯性前馈与柔顺阻抗控制中的具体作用。理解本章内容，是后续实现零力拖动、碰撞检测与力位混合控制的前提。
2.1 D-H参数建模与正运动学
核心原理
Denavit-Hartenberg（D-H）参数法是描述相邻连杆之间位姿关系的标准化方法。每个连杆通过四个参数唯一确定：连杆长度 \(a_{i-1}\)、连杆扭转角 \(\alpha_{i-1}\)、连杆偏距 \(d_i\) 和关节角 \(\theta_i\)。对于六轴机械臂，依次建立六个连杆坐标系，相邻坐标系间的齐次变换矩阵为：
A_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)正运动学即通过连乘六个变换矩阵得到末端执行器相对于基座的位姿：\(T_6^0 = A_1 A_2 A_3 A_4 A_5 A_6\)。实际工程中，UR5、ABB IRB1200等常见六轴机器人均采用改进D-H（Modified D-H）或标准D-H（Standard D-H）约定，两者在坐标系放置上存在差异，建模时必须明确采用哪种约定。
操作流程
- 确定各关节轴线方向，按D-H规则建立连杆坐标系 {i}；
- 测量或从机械图纸获取四个D-H参数（\(a, \alpha, d, \theta\)）；
- 编写齐次变换矩阵函数，依次计算 \(A_1\) 至 \(A_6\)；
- 连乘得到正运动学矩阵 \(T_6^0\)，提取位置向量和旋转矩阵；
- 用激光跟踪仪或高精度三坐标测量仪对标定板上的实际末端位姿进行验证。
案例拆解：UR5机械臂D-H参数表
| 关节 i | a (mm) | α (deg) | d (mm) | θ 范围 (deg) |
|---|---|---|---|---|
| 1 | 0 | 90 | 162.5 | ±360 |
| 2 | -425 | 0 | 0 | ±360 |
| 3 | -392 | 0 | 0 | ±360 |
| 4 | 0 | 90 | 133.3 | ±360 |
| 5 | 0 | -90 | 99.7 | ±360 |
| 6 | 0 | 0 | 99.6 | ±360 |
在力矩控制中，D-H参数不仅是运动学求解的基础，更是动力学建模中惯性参数（质量、质心、惯性张量）的参考坐标系。若D-H参数存在误差，将直接导致重力矩计算偏差，表现为机械臂在静止状态下关节电流异常增大或出现缓慢漂移。
2.2 逆运动学求解策略
核心原理
逆运动学（IK）是已知末端位姿 \(T_6^0\)，求解关节角 \(\theta_1 \sim \theta_6\) 的过程。六轴机械臂的逆解通常存在多组解（一般最多8组），需要根据关节限位、避障要求和能量最优原则进行选择。求解方法分为解析法（Pieper准则）和数值迭代法（如雅可比转置法、阻尼最小二乘法）。
判断方法与操作流程
- 解析法适用条件：满足Pieper准则（三个相邻关节轴交于一点或平行），大多数工业六轴臂（如UR、ABB、KUKA）均满足，可得到闭式解，计算速度快、精度高；
- 数值法适用场景：冗余机械臂或特殊构型，通过迭代优化目标函数 \( \min \|T_{desired} - T_{current}\| \) 求解；
- 多解选择策略：优先选择与当前关节角最接近的解（最小行程原则），同时检查关节限位和奇异规避。
实战场景：奇异位形处理
当机械臂处于奇异位形（如腕部中心与肩部轴线重合）时，雅可比矩阵秩亏，逆解速度趋于无穷大。某装配任务中，机械臂在接近奇异点时出现末端抖动，关节6速度指令超过额定值。解决方案：
- 在逆解前计算雅可比矩阵条件数，当条件数 > 阈值（如100）时判定接近奇异；
- 切换为阻尼最小二乘法（DLS）：\( \dot{q} = J^T (J J^T + \lambda^2 I)^{-1} \dot{x} \)，其中 \(\lambda\) 为阻尼系数；
- 在奇异点附近适当降低末端速度，保证关节速度不超过限幅。
2.3 雅可比矩阵：速度与力的双向映射
核心原理
雅可比矩阵 \(J(q) \in \mathbb{R}^{6 \times 6}\) 建立了关节速度 \(\dot{q}\) 与末端笛卡尔速度 \(\dot{x}\) 之间的线性映射：\(\dot{x} = J(q)\dot{q}\)。根据虚功原理，关节力矩与末端力之间存在对偶关系：\(\tau = J^T F\)。这一关系是力矩控制中实现笛卡尔空间柔顺操作的关键桥梁。
雅可比矩阵的构造方法
| 行类型 | 表达式 | 物理含义 |
|---|---|---|
| 线速度行 | \(J_{vi} = z_{i-1} \times (p_n - p_{i-1})\) | 关节i转动对末端线速度的贡献 |
| 角速度行 | \(J_{\omega i} = z_{i-1}\) | 关节i转动对末端角速度的贡献 |
其中 \(z_{i-1}\) 为关节i的旋转轴方向向量，\(p_n\) 为末端位置，\(p_{i-1}\) 为关节i坐标系原点位置。实际编程中，可通过正运动学递推逐列计算。
案例拆解：力控打磨中的雅可比应用
在某铝合金工件打磨场景中，要求末端接触力恒定在15N。初始方案采用位置控制，因工件尺寸公差导致实际接触力在8~25N之间波动。改用力矩控制后：
- 实时计算雅可比矩阵 \(J(q)\)，将期望笛卡尔力 \(F_d = [0,0,15,0,0,0]^T\) 映射为关节力矩 \(\tau_d = J^T F_d\)；
- 在关节力矩指令中叠加该前馈项，同时保留阻抗控制的位置修正项；
- 实测接触力波动降低至±1.5N以内，表面粗糙度Ra从3.2μm降至1.6μm。
该案例表明，雅可比矩阵的准确性直接影响力控精度。若D-H参数误差超过0.5mm，\(J^T\) 映射的力矩误差可达5%~8%，导致接触力偏差超过2N。
2.4 动力学方程：拉格朗日法与牛顿-欧拉法
核心原理
机械臂动力学描述关节力矩与运动状态（位置、速度、加速度）之间的关系。两种主流建模方法：
- 拉格朗日法：基于能量标量函数 \(L = K - P\)，推导出 \(\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q)\)。其中 \(M(q)\) 为惯性矩阵，\(C(q,\dot{q})\) 包含科氏力和离心力，\(G(q)\) 为重力项。该方法物理意义清晰，适合控制器设计；
- 牛顿-欧拉法：从连杆1到6向外递推速度与加速度，再从连杆6到1向内递推力与力矩。计算效率高（O(n)复杂度），适合实时控制，但推导过程繁琐。
动力学参数辨识流程
- 建立机械臂CAD模型，初步获取质量、质心、惯性张量参数；
- 设计激励轨迹（如傅里叶级数轨迹），充分激励各关节动力学特性；
- 采集关节角度、速度、力矩数据，采用最小二乘法辨识修正参数；
- 验证模型精度：对比预测力矩与实际力矩的均方根误差，通常要求小于额定力矩的2%。
实战场景：重力补偿在零力拖动中的应用
零力拖动（手把手示教）是柔顺操作的基础功能。某协作机器人实现零力拖动的控制律为：
τ_cmd = G(q) + τ_friction + τ_user
其中：
G(q) — 重力补偿项（由动力学模型实时计算）
τ_friction — 摩擦力补偿（库仑+粘滞模型）
τ_user — 用户施加的等效关节力矩（由关节力矩传感器测得）实际调试中发现，未进行动力学参数辨识前，重力补偿误差约5%，导致机械臂在静止时缓慢下沉。通过激励轨迹辨识后，重力补偿精度提升至0.5%以内，拖动时操作力从15N降至3N，实现了轻便灵活的拖动示教。
常见误区与总结复盘
| 对比维度 | 拉格朗日法 | 牛顿-欧拉法 |
|---|---|---|
| 数学基础 | 能量标量函数 | 牛顿第二定律/欧拉方程 |
| 计算复杂度 | O(n³) | O(n) |
| 物理直观性 | 强（各项物理意义明确） | 弱（递推过程抽象） |
| 实时性 | 适合离线仿真 | 适合实时控制 |
| 参数辨识友好度 | 易于线性化分离参数 | 需重新整理为线性形式 |
在实际工程中，多数商业控制器（如UR的servoj、KUKA的RSI）内部采用牛顿-欧拉法进行实时动力学计算，同时提供拉格朗日形式的接口供用户进行力矩前馈。理解两种方法的本质联系，有助于正确配置控制器的动力学参数，避免因模型误差导致的力矩波动或系统失稳。