3. 动力学基础（下）：拉格朗日法推导动力学方程

好，我们接着往下聊。上一章我们把牛顿-欧拉法讲透了，那个方法很直观，但有个问题——你得把每个关节的约束力都算一遍，计算量不小。我个人习惯在搞复杂构型（比如人形机器人的全身动力学）时，更倾向于用拉格朗日法。

为什么？因为它从能量角度出发，不用管那些内部约束力。说白了，你只需要写出系统的动能和势能，然后套公式就行了。省心不少。

3.1 拉格朗日方程的基本形式

拉格朗日方程长这样：

τ = d/dt (∂L/∂q̇) - ∂L/∂q

其中 L = T - V，T 是动能，V 是势能。q 是广义坐标，τ 是广义力。

你想想看，这个公式其实就干了一件事：把动力学问题转化成了求导问题。只要你能写出 T 和 V，剩下的就是机械的数学运算。

3.2 动能的计算

对于人形机器人的每个连杆，动能由两部分组成：平动动能和转动动能。

T_i = ½ m_i v_i^T v_i + ½ ω_i^T I_i ω_i

这里 v_i 是连杆质心的线速度，ω_i 是角速度，I_i 是惯性张量。

嗯，这里要注意：惯性张量通常是在连杆的局部坐标系下定义的。如果你用全局坐标系算，记得做旋转变换。我曾经在这个坑里摔过一次——仿真出来的力矩曲线看着没问题，但实际机器人一跑就抖，查了两天才发现是惯性张量没转对。

整机动能就是所有连杆动能之和：

T = Σ T_i

3.3 势能的计算

人形机器人的势能主要就是重力势能。简单：

V = Σ m_i g h_i

h_i 是第 i 个连杆质心的高度。注意，这里的高度是相对于你定义的零势能面。我一般把零势能面设在机器人脚底平面，这样算起来直观。

3.4 拉格朗日法推导步骤

我总结了一套标准流程，你照着做基本不会出错：

1. 选广义坐标——通常就是关节角度 q
2. 写每个连杆的位姿——用齐次变换矩阵表达
3. 求速度——对位姿求导，得到 v_i 和 ω_i
4. 算动能 T——累加所有连杆
5. 算势能 V——累加所有连杆的重力势能
6. 构造拉格朗日函数 L = T - V
7. 代入拉格朗日方程——得到动力学方程

说起来简单，但第 3 步和第 7 步的求导运算量很大。我建议你用符号计算工具（比如 MATLAB 的 Symbolic Toolbox 或 SymPy）来辅助，手算太容易出错了。

3.5 一个简单的例子：二连杆臂

我们拿一个平面二连杆臂来演示。假设两个连杆质量集中在末端，长度分别为 l₁ 和 l₂。

广义坐标选 q = [θ₁, θ₂]ᵀ。动能：

T = ½ m₁ (l₁θ̇₁)² + ½ m₂ [ (l₁θ̇₁ cosθ₁ + l₂(θ̇₁+θ̇₂) cos(θ₁+θ₂))² + 
                     (l₁θ̇₁ sinθ₁ + l₂(θ̇₁+θ̇₂) sin(θ₁+θ₂))² ]

势能：

V = m₁ g l₁ sinθ₁ + m₂ g [l₁ sinθ₁ + l₂ sin(θ₁+θ₂)]

然后 L = T - V，代入拉格朗日方程，整理后得到：

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

这就是标准形式。M 是惯性矩阵，C 是科里奥利力和离心力项，G 是重力项。

3.6 动力学参数的辨识简介

好，方程推出来了，但有个现实问题：M、C、G 里面的参数（质量、质心位置、惯性张量）你从 CAD 模型里拿到的值，和实际机器人往往有偏差。

怎么办？靠辨识。

动力学参数辨识的基本思路是这样的：

1. 让机器人跑一组激励轨迹（比如正弦扫频或多项式轨迹）
2. 采集关节角度、角速度、角加速度和力矩数据
3. 把动力学方程写成线性形式：Y(q, q̇, q̈) · π = τ
4. 用最小二乘法或递归算法估计参数向量 π

这里的 π 包含了所有待辨识的参数：质量、质心位置、惯性张量元素等。

另外，不是所有参数都能独立辨识出来。有些参数在线性化后会合并在一起，这叫“参数聚合”。比如两个连杆的质量可能只能辨识出它们的和，而不是各自的值。这时候你需要做参数重组，把可辨识的基参数提取出来。

嗯，这里要提醒一句：辨识出来的参数一定要做验证。让机器人跑一条没在训练集中出现过的轨迹，对比实测力矩和模型预测力矩。如果误差在 5% 以内，就算合格了。

3.7 本章知识体系

下面这张图把拉格朗日法推导动力学方程的核心逻辑串起来了：

从广义坐标出发，分别计算动能和势能，构造拉格朗日函数，代入方程，最终得到标准动力学模型。最后一步，用参数辨识把理论模型校准到实际机器人上。

我个人觉得，拉格朗日法的最大优势就是“干净”——你不需要操心约束力，只需要关注能量。对于人形机器人这种多自由度系统，这个优势尤其明显。