第二节 机械臂运动学与动力学基础：D-H参数、正逆运动学、牛顿-欧拉与拉格朗日动力学方程、惯性矩阵、科氏力/离心力、重力项

六轴机械臂的动力学参数整定，本质上是对机械臂数学模型中的未知参数进行辨识与修正。若不了解机械臂的运动学与动力学基础，参数整定便如同“盲人摸象”——你看到的可能是电机电流的波动，却无法判断它究竟来自连杆惯性、关节耦合还是重力负载。本节将系统梳理从D-H参数到动力学方程的核心知识链，为后续的摩擦辨识、负载估计与前馈补偿打下坚实的数学与物理基础。

2.1 D-H参数与正逆运动学：机械臂的“骨架”与“关节语言”

核心原理：D-H参数如何描述连杆与关节

Denavit-Hartenberg（D-H）参数是描述相邻连杆坐标系之间相对位姿的四个最小参数：连杆长度 a_i（沿 x_i 轴从 z_i 移动到 z_i+1 的距离）、连杆扭转角 α_i（绕 x_i 轴从 z_i 旋转到 z_i+1 的角度）、关节距离 d_i（沿 z_i 轴从 x_i-1 移动到 x_i 的距离）、关节角 θ_i（绕 z_i 轴从 x_i-1 旋转到 x_i 的角度）。对于六轴机械臂，我们通常使用改进D-H（Modified D-H）或标准D-H（Standard D-H）约定，两者在坐标系附着位置上有差异，但最终都能得到齐次变换矩阵 T_i^i-1 = Rot(z,θ_i) · Trans(z,d_i) · Trans(x,a_i) · Rot(x,α_i)。

正运动学（Forward Kinematics）即通过六个关节角 θ₁…θ₆ 依次连乘变换矩阵，得到末端执行器相对于基座标系的位姿矩阵 T₆⁰。逆运动学（Inverse Kinematics）则相反，给定末端目标位姿，求解满足条件的关节角组合。六轴机械臂通常存在多组逆解（如肘上/肘下、腕部翻转），且在某些位形下会出现奇异（Jacobian矩阵秩亏），导致关节速度趋于无穷。

判断方法：正逆运动学的验证流程

1. 正解验证：将机械臂置于零位（各关节角度为0），读取末端实际位置，与正运动学计算结果对比，误差应小于0.5mm。
2. 逆解验证：随机生成一组关节角，计算正解得到末端位姿，再对该位姿求逆解，检查逆解结果是否包含原始关节角（允许存在多解）。
3. 奇异位形检查：计算Jacobian矩阵的行列式，若接近零则说明接近奇异位形，此时逆解不稳定，应避免在该区域进行参数辨识激励。

案例拆解：六轴机器人D-H参数表与正解验证

以典型的六轴串联关节机器人（如类似KUKA KR6）为例，其改进D-H参数表如下（单位：mm / deg）：

在零位（所有θ=0）时，正运动学计算得到的末端位置应为 (180+600+120, 0, 400+620+100) = (900, 0, 1120) mm。若实测末端位置偏差超过1mm，应检查D-H参数中的d和a值是否与实际机械结构一致，特别是腕部偏置（d₄、d₆）是否被正确建模。

2.2 牛顿-欧拉与拉格朗日动力学方程：两种视角，同一目标

核心原理：从力/能量到动力学方程

机械臂动力学描述的是关节力矩 τ 与关节运动（q, q̇, q̈）之间的关系。两种主流建模方法各有优势：牛顿-欧拉法（Newton-Euler）基于力/力矩的递推计算，从基座到末端进行速度/加速度外推（outward recursion），再从末端到基座进行力/力矩内推（inward recursion），计算复杂度为 O(n)，非常适合实时控制；拉格朗日法（Lagrange）基于系统动能与势能之差构造拉格朗日函数 L = K - P，然后对每个广义坐标求偏导得到方程，其优点是能直接给出闭式动力学方程，便于分析结构特性，但计算量较大（O(n³)）。

无论采用哪种方法，最终都能写成统一的标准形式：

τ = M(q)q̈ + C(q, q̇)q̇ + G(q) + τ_f(q̇)

其中 M(q) 为 n×n 惯性矩阵（对称正定），C(q,q̇)q̇ 为科氏力与离心力项，G(q) 为重力项，τ_f 为摩擦力矩（通常包含库仑摩擦与粘性摩擦）。

判断方法：如何选择建模方法

• 实时控制场景：优先选择牛顿-欧拉递推算法，计算量小，适合1kHz以上控制周期。
• 参数辨识与离线分析：使用拉格朗日法得到的闭式方程，便于提取回归矩阵 Y(q,q̇,q̈)，将动力学方程线性化为 τ = Y·Φ 的形式，其中 Φ 为待辨识的惯性参数向量。
• 仿真验证：两种方法均可，但需注意数值稳定性，建议使用双精度浮点并做单位一致性检查（SI单位制）。

案例拆解：拉格朗日法推导二连杆动力学

考虑一个平面二连杆机械臂（连杆质量 m₁、m₂，质心距离 a₁、a₂，连杆长度 L₁、L₂），其拉格朗日动力学方程可写为：

M(q) = [[m₁a₁² + m₂(L₁² + a₂² + 2L₁a₂cos q₂), m₂(a₂² + L₁a₂cos q₂)],
         [m₂(a₂² + L₁a₂cos q₂), m₂a₂²]]

C(q,q̇) = [[-m₂L₁a₂sin(q₂)q̇₂, -m₂L₁a₂sin(q₂)(q̇₁+q̇₂)],
           [m₂L₁a₂sin(q₂)q̇₁, 0]]

G(q) = [[(m₁a₁ + m₂L₁)g cos q₁ + m₂a₂g cos(q₁+q₂)],
        [m₂a₂g cos(q₁+q₂)]]

从上述方程可以清晰看到：惯性矩阵 M(q) 随位形变化（与 q₂ 相关），科氏项 C 与速度乘积相关，重力项 G 与角度相关。在参数整定中，我们正是要辨识这些矩阵中的质量、质心位置和转动惯量参数。

2.3 惯性矩阵、科氏力/离心力与重力项：动力学参数整定的三大目标

核心原理：三项的物理意义与辨识价值

惯性矩阵 M(q) 描述了关节加速度与力矩之间的关系，其对角线元素为各关节的有效惯量，非对角线元素为关节间的耦合惯量。M(q) 是位形相关的，当机械臂伸展时，末端关节的有效惯量显著增大。科氏力/离心力项 C(q,q̇)q̇ 来源于运动中的惯性耦合，当多个关节同时高速运动时，科氏力矩可能相当显著（例如SCARA机器人在高速搬运时）。重力项 G(q) 是位形相关的静态力矩，在低速或静止时占主导地位，是前馈补偿中最容易辨识且收益最明显的部分。

判断方法：辨识激励轨迹设计

要准确辨识这三项，需要设计合适的激励轨迹（excitation trajectory），使回归矩阵 Y 的条件数尽量小。常用方法包括：

1. 傅里叶级数轨迹：每个关节的轨迹由多个频率的正弦/余弦叠加而成，频率互质以避免耦合。
2. 有限项多项式轨迹：保证位置、速度、加速度连续，避免激励冲击。
3. 多点静态辨识：将机械臂移动到多个不同位形，记录重力矩，单独辨识 G(q) 中的质量与质心参数。

案例拆解：重力项辨识与补偿

某六轴机械臂在低速运行时（<0.1 rad/s），关节2的电流波动明显，且随位形变化。初步判断为重力项未完全补偿。操作流程如下：

• 数据采集：将关节2分别移动到 -90°、-45°、0°、45°、90° 五个位置，保持静止，记录每个位置的力矩指令（或电流换算值）。
• 模型拟合：假设重力项为 G₂(q) = m₂g·L₂·cos(q₂) + m₃g·(L₂cos q₂ + L₃cos(q₂+q₃))，使用最小二乘法拟合 m₂、m₃ 的质心参数。
• 补偿验证：将辨识得到的重力项加入前馈，重新运行相同轨迹，观察关节2的跟踪误差是否从 ±0.5° 减小到 ±0.1° 以内。

结果：重力前馈补偿后，低速时的力矩波动降低了约70%，电机电流峰值下降15%。

总结复盘：动力学参数整定的核心要点

在实际工程中，动力学参数整定并非一蹴而就。建议按照“先重力、再摩擦、后惯性/科氏”的顺序逐步推进，每一步都通过前馈补偿效果进行验证。同时，要注意动力学模型的简化程度——对于六轴机械臂，通常忽略连杆的柔性、关节间隙和温度影响，这些未建模动态会限制参数整定的精度上限。