4. 正运动学建模

聊到机械臂动力学，很多刚入行的朋友第一反应就是「公式好复杂」。说实话，我当年也有这种感觉。但做了十几年机器人，我慢慢发现——动力学其实没那么可怕，关键是你得知道它到底在解决什么问题。

简单来说，动力学就是研究「力与运动的关系」。你给电机发一个力矩指令，机械臂到底怎么动？反过来，你希望机械臂按某个轨迹运动，需要多大的力矩？这就是动力学的核心。

4.1 牛顿-欧拉法：工程派的首选

我个人习惯用牛顿-欧拉法做实时控制。为什么？因为它计算效率高，而且物理意义非常直观。

核心思想其实很简单：从基座开始，向外递推每个连杆的速度和加速度（正向递推）；然后从末端执行器开始，向内递推每个关节的力和力矩（反向递推）。

正向递推（速度与加速度）

从基座（连杆0）开始，依次计算每个连杆的角速度、角加速度、线加速度。公式长这样：

// 角速度递推
ω_{i+1} = R_{i+1}^i · ω_i + θ̇_{i+1} · z_{i+1}

// 角加速度递推
α_{i+1} = R_{i+1}^i · α_i + θ̈_{i+1} · z_{i+1} + ω_{i+1} × (θ̇_{i+1} · z_{i+1})

// 线加速度递推
a_{i+1} = R_{i+1}^i · (a_i + α_i × p_i + ω_i × (ω_i × p_i))

嗯，这里要注意：旋转矩阵 R 的方向别搞反了。我在项目中遇到过好几次，因为坐标系变换搞错，导致仿真结果完全不对。后来我养成了一个习惯——每次写完递推代码，先用一个简单的二连杆模型做验证。

反向递推（力与力矩）

从末端开始，向内计算每个连杆受到的力和力矩：

// 力平衡
f_i = R_{i+1}^i · f_{i+1} + m_i · a_{c_i}

// 力矩平衡
τ_i = R_{i+1}^i · τ_{i+1} + p_i × (R_{i+1}^i · f_{i+1}) + I_i · α_i + ω_i × (I_i · ω_i)

// 关节驱动力矩
τ_joint_i = τ_i · z_i

4.2 拉格朗日法：理论派的利器

拉格朗日法走的是另一条路——从能量角度出发。你不需要关心内部约束力，只需要写出系统的动能和势能。

公式很简洁：

L = T - V   (拉格朗日函数)

d/dt(∂L/∂q̇) - ∂L/∂q = τ   (拉格朗日方程)

其中 T 是系统总动能，V 是系统总势能，q 是广义坐标（关节角度）。

展开后，你会得到标准形式的动力学方程：

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

这里：

• M(q)：惯性矩阵，与关节位置有关
• C(q, q̇)：科里奥利力和离心力矩阵
• G(q)：重力项
• τ：关节驱动力矩

4.3 动力学参数辨识：从理论到真机的关键一步

说实话，理论公式再漂亮，到了真机上也得面对现实——你根本不知道真实的惯性参数是多少。

CAD 模型给的参数？误差通常在 10%~30%。因为电机转子惯量、减速器摩擦、线缆阻力，这些在 CAD 里根本算不准。怎么办？辨识。

辨识的基本思路

说白了就是：让机械臂按特定轨迹运动，采集关节力矩和运动数据，然后用最小二乘法反推参数。

标准流程：

1. 激励轨迹设计：通常是傅里叶级数形式的轨迹，保证充分激励所有参数
2. 数据采集：关节位置、速度、加速度、力矩
3. 滤波处理：去除噪声，尤其是加速度信号需要低通滤波
4. 参数估计：最小二乘法或递推最小二乘法
5. 验证：用另一条轨迹验证辨识结果的准确性

最小二乘辨识公式

将动力学方程写成线性形式：

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

其中 Y 是回归矩阵（已知），θ 是待辨识的参数向量（未知）。

采集 N 组数据后：

θ = (Y^T · Y)^{-1} · Y^T · τ

嗯，这里有个坑——Y^T·Y 可能奇异。因为有些参数是线性相关的，无法独立辨识。解决办法是：先做参数重组，合并那些不可辨识的参数组合。

常用辨识方法对比

方法	优点	缺点	适用场景
批量最小二乘	简单、稳定	需要存储全部数据	离线辨识
递推最小二乘	在线、内存小	对初值敏感	在线自适应
扩展卡尔曼滤波	可处理噪声	调参复杂	高精度辨识
神经网络	无需模型	泛化性差	研究阶段

4.4 工程实践中的几个关键点

最后，分享几个我在项目中积累的经验：

• 先仿真再真机：在仿真环境里把动力学模型调通了，再上真机。我见过太多人直接上真机，结果把机器人搞出奇异位形。
• 摩擦模型别忽视：很多动力学控制效果不好，不是因为惯性参数不准，而是摩擦模型太粗糙。
• 验证要交叉：用一条轨迹做辨识，用另一条完全不同的轨迹做验证。如果验证误差大，说明辨识有问题。
• 注意数值稳定性：尤其是加速度信号，微分放大噪声很严重。我习惯用中心差分+低通滤波的组合。