3. 机器人动力学基础

运动学解决“机器人在哪里、以什么速度运动”，而动力学回答“要产生这样的运动，各关节需要输出多大力矩”。在实时运动控制系统中，动力学模型直接决定了前馈补偿、重力补偿、碰撞检测、力控柔顺以及参数自整定的效果。本章围绕两条经典建模路线——拉格朗日方程与牛顿-欧拉法，梳理关节力矩计算流程，并进一步讨论动力学模型的线性化形式与参数辨识方法。

一、核心原理：两种建模视角

机器人动力学描述关节力矩 τ 与关节位置 q、速度 q̇、加速度 q̈ 之间的映射关系。通用形式可写为：

τ = M(q)·q̈ + C(q, q̇)·q̇ + G(q) + F(q̇) + τ_ext

其中 M(q) 为惯性矩阵，C(q,q̇) 为科氏力与离心力项，G(q) 为重力项，F(q̇) 为摩擦项，τ_ext 为外部接触力映射到关节的等效力矩。

拉格朗日方程从能量角度出发，定义拉格朗日函数 L = K − P（动能减势能），对每个广义坐标 q_i 建立：

d/dt(∂L/∂q̇_i) − ∂L/∂q_i = τ_i

它的优点是推导过程系统化、物理意义清晰，适合串联机械臂的符号建模；缺点是随着自由度增加，符号展开量急剧膨胀，实时计算效率较低。

牛顿-欧拉法则从力与力矩平衡出发，分为两步：向外递推计算各连杆的速度、加速度和惯性力，再向内递推计算关节力矩。其计算复杂度为 O(n)，非常适合实时控制中的逆动力学求解。

二、关节力矩计算流程

在实时控制周期内，逆动力学计算通常按以下顺序执行：

1. 读取当前关节位置 q 与速度 q̇，并获取期望加速度 q̈_ref；
2. 由正运动学计算各连杆位姿、雅可比矩阵；
3. 向外递推：计算各连杆角速度、角加速度、线加速度及惯性力/力矩；
4. 向内递推：从末端向基座计算关节驱动力矩；
5. 叠加摩擦补偿、重力补偿与外部力估计；
6. 输出前馈力矩 τ_ff，与反馈控制量叠加后下发到关节驱动器。

下面给出一个简化的牛顿-欧拉逆动力学伪代码，便于理解实时计算结构：

for i = 1 to n:
    ω[i]   = R[i]·ω[i-1] + z·q̇[i]
    α[i]   = R[i]·α[i-1] + z·q̈[i] + ω[i] × (z·q̇[i])
    a[i]   = R[i]·a[i-1] + α[i] × r[i] + ω[i] × (ω[i] × r[i])
    F[i]   = m[i]·a_c[i]
    N[i]   = I[i]·α[i] + ω[i] × (I[i]·ω[i])

for i = n down to 1:
    f[i]   = R[i+1]·f[i+1] + F[i]
    n[i]   = R[i+1]·n[i+1] + r[i] × f[i] + N[i]
    τ[i]   = n[i]·z + b[i]·q̇[i] + τ_friction[i]

该流程在每个控制周期内执行一次，典型周期为 1 ms 或 0.5 ms。若模型参数准确，前馈力矩可显著降低反馈增益需求，提高轨迹跟踪精度。

三、动力学模型线性化与参数辨识

机器人动力学方程对惯性参数是线性的，可写成回归形式：

τ = Y(q, q̇, q̈) · θ

其中 Y 为回归矩阵，θ 为待辨识参数向量，通常包含连杆质量、质心位置、惯性张量、摩擦系数等。线性化后，参数辨识转化为最小二乘问题：

θ_hat = argmin || Y·θ − τ_meas ||²

实际辨识流程如下：

• 激励轨迹设计：采用有限项傅里叶级数或五次多项式，使各关节充分运动，保证回归矩阵条件数良好；
• 数据采集：记录 q、q̇、q̈ 与关节力矩 τ_meas，注意滤波与同步；
• 参数估计：使用最小二乘或加权最小二乘求解 θ_hat；
• 模型验证：用另一组轨迹比较预测力矩与实测力矩的残差；
• 在线修正：在负载变化或温升导致摩擦变化时，采用递推最小二乘进行在线更新。

四、案例与场景：前馈补偿在轨迹跟踪中的作用

以六轴协作机器人为例，在 0.5 ms 控制周期内，若仅使用 PID 反馈，高速圆弧轨迹跟踪误差可能达到 2～5 mm。引入逆动力学前馈后，控制器输出变为：

τ_cmd = τ_ff(q, q̇, q̈_ref) + Kp·e + Kd·ė + Ki·∫e dt

其中 τ_ff 由牛顿-欧拉逆动力学计算。由于前馈已补偿了惯性、科氏力和重力，反馈项只需处理模型误差与扰动，比例增益可降低 30%～50%，跟踪误差可降至 0.2 mm 以内，同时避免高增益引起的抖动。

五、常见误区与复盘

• 忽略摩擦项：低速时摩擦占主导，若只辨识惯性参数，前馈效果在低速段会明显变差；
• 激励轨迹不充分：轨迹过于平缓会导致回归矩阵条件数过大，参数估计不稳定；
• 数据不同步：q、q̇、q̈ 与力矩采样时刻不一致，会引入相位误差，破坏辨识精度；
• 把线性化当成近似：动力学对惯性参数的线性是严格成立的，但摩擦模型和柔性关节会破坏该性质，需要单独处理；
• 忽视负载变化：末端负载改变后，惯性矩阵与质心位置变化，需重新辨识或采用自适应律在线更新。

综上，动力学建模是机器人实时控制的核心基础。理解拉格朗日方程与牛顿-欧拉法的差异，掌握逆动力学计算流程，并利用线性化回归完成参数辨识，才能让前馈补偿、力控与碰撞检测在实际系统中稳定落地。