本文基于我的《从零学分析力学》《约束和拉格朗日方程》《机器人动力学—拉格朗日法》系列笔记。不满足于”会用
LagrangesMethod出个方程”，而是把为什么是 L=T−V → 广义坐标怎么消掉约束力 → M/C/G 三个矩阵从哪来 → 电机减速器惯量折算 → 线性化与辨识这条链路用 SymPy 完整走一遍，所有代码均可在 SymPy 1.14 下直接运行验证。
为什么机器人动力学要符号推导
手工推导 3 自由度以上的动力学方程几乎必错——我笔记里二连杆推到最后一行写的是”太难了不会”。这不是能力问题：拉格朗日法要求对动能 T 和势能 V 求一堆偏导再对时间求导，n 自由度的中间项以 $O(n^4)$ 增长，6 自由度机械臂的 M 矩阵展开后有几百项，人肉推导必然漏项。
机器人动力学的标准形式（也是控制里天天用的形式）：
$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau$$
- $M(q)$：惯性矩阵，对称正定，只与位形有关；
- $C(q,\dot{q})\dot{q}$：科氏力 + 离心力，速度的二次项；
- $G(q)$：重力项；
- $\tau$：关节广义力（电机力矩经减速器后的等效值）。
计算力矩控制要 M 和 G，前馈补偿要 C，参数辨识要全部三个，仿真要解 $M\ddot{q} = \tau - C\dot{q} - G$。所以”把 M/C/G 符号推出来”不是学术任务，是控制工程的起点。
牛顿-欧拉 vs 拉格朗日：牛顿-欧拉是递归的（$O(n)$），天生适合编程（pinocchio、RBDL 都是它）；拉格朗日基于能量，不需要处理关节约束力，适合手算和符号推导——这就是为什么用 SymPy 做拉格朗日。
从牛顿力学到拉格朗日：三个关键思想
1. 广义坐标：用最少的独立变量描述系统
单摆用笛卡尔坐标 $(x,y)$ 描述时，约束方程 $x^2+y^2=l^2$ 让两个分量方程耦合，还得把绳子张力 T 求出来才能解。改用角度 $\theta$ 一个变量就完全确定位形，张力自动消失。能完整描述系统位形的最少独立变量就是广义坐标 $q_1,\dots,q_s$——不一定是长度，角度、位移、甚至傅里叶系数都行。
2. 理想约束与虚位移：约束力为什么能消掉
虚位移 $\delta \mathbf{r}$：满足约束条件的任意无穷小位移（与时间无关）。理想约束（光滑、刚性）的约束力在虚位移上做的总虚功为零——光滑铰链的力垂直于虚位移方向、刚性杆内力成对抵消。于是虚功原理（静力学）：平衡时主动力虚功和为零：
$$\sum_i \mathbf{F}_i \cdot \delta\mathbf{r}_i = 0$$
达朗贝尔原理（动力学）：把惯性力 $-m_i\ddot{\mathbf{r}}_i$ 当主动力加入：
$$\sum_i (\mathbf{F}_i - m_i\ddot{\mathbf{r}}_i) \cdot \delta\mathbf{r}_i = 0$$
关键点：方程里没有约束力。这就是拉格朗日法最大的工程价值——不用解关节/支撑反力就能得到运动方程（想要反力？之后用拉格朗日乘子法补）。
3. 为什么是 L = T − V 而不是 T + V
拉格朗日量 $L = T - V$，方程：
$$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = Q_i$$
两个观察：
- 广义动量 $p_i = \partial L/\partial \dot{q}_i = \partial T/\partial \dot{q}_i$——势能不含 $\dot{q}$（保守力只依赖位置），所以 $\partial V/\partial \dot{q}=0$；
- 广义力 $\partial L/\partial q_i = \partial T/\partial q_i - \partial V/\partial q_i$——**负号来自 $F = -\nabla V$**：势能沿 $q_i$ 增大方向增加时，系统受的力把它往 $q_i$ 减小方向推。
如果写 $L = T + V$，第一项动量没问题，但第二项力的符号全反，方程就是错的。$T-V$ 不是魔法，是”动能管惯性、势能管力”的拼装约定。
约束与自由度计数
| 约束类型 | 定义 | 效果 | 例子 |
|---|---|---|---|
| 完整约束 | $f(q_1,\dots,q_n,t)=0$，可积 | 减少独立坐标数 | 连杆长度恒定 |
| 非完整约束 | 不可积（速度级约束 $\sum a_i dq_i = 0$ 无法积分成坐标约束） | 不减少坐标数，只减少独立速度数 | 轮子纯滚动、车不能横移 |
| 稳定/非稳定 | 是否显含时间 t | — | 伸缩的摆长 |
| 可解/不可解 | 等式/不等式 | 不等式约束分段处理 | 摆线 $x^2+y^2\le l^2$ |
N 个质点、K 个完整约束 → 广义坐标数 $s = 3N - K$；再叠加 L 个非完整约束 → 独立速度（自由度）数 $r = 3N - K - L$。非完整约束限制运动方向但不限制运动位置：车不能横移，但倒几把就能停进任何车位——这就是”停车问题”为什么是非完整约束的经典例子。
二连杆：手推结论 vs SymPy
假设（和我笔记一致）：质量集中在连杆末端（$I=0$ 的点质量），$y$ 向下为正，$q=0$ 是下垂位形。用 $s_i=\sin q_i,\ c_i=\cos q_i,\ s_{12}=\sin(q_1+q_2)$ 简写，教科书闭式结果：
$$M = \begin{bmatrix} (m_1{+}m_2)l_1^2 + m_2 l_2^2 + 2m_2 l_1 l_2 c_2 & m_2 l_2^2 + m_2 l_1 l_2 c_2 \ m_2 l_2^2 + m_2 l_1 l_2 c_2 & m_2 l_2^2 \end{bmatrix}$$
$$C = \begin{bmatrix} -m_2 l_1 l_2 s_2 \dot{q}_2 & -m_2 l_1 l_2 s_2(\dot{q}_1+\dot{q}2) \ m_2 l_1 l_2 s_2 \dot{q}1 & 0 \end{bmatrix}, \qquad G = \begin{bmatrix} (m_1{+}m_2) g l_1 s_1 + m_2 g l_2 s{12} \ m_2 g l_2 s{12} \end{bmatrix}$$
读这张表能读出所有”为什么”：
- M 对称（$M_{12}=M_{21}$）：动能 $\frac12\dot{q}^T M \dot{q}$ 是二次型，交叉项系数天然对称；
- M 含 $\cos q_2$：第二杆展开/收拢时，它相对第一杆轴的等效惯量在 $l_2^2$ 和 $(l_1+l_2)^2$ 之间变化——耦合项随位形变，这就是为什么机器人是时变非线性系统，不能当常数惯量调 PID；
- C 是速度二次项：$-\sin q_2$ 说明科氏力只在两杆相对运动时出现，静止时为零；
- **G 只含 $\sin$**：重力矩随角度正弦变化，$q_1$ 项还带着第二杆的贡献 $m_2 g l_2 s_{12}$——第二杆的重力会”串扰”到第一关节，这就是重力前馈必须用完整模型的原因。
SymPy 完整推导
定义符号与位形
1 | import sympy as sp |
提取 M，再用 Christoffel 符号构造 C
1 | # eom 对 q̈ 线性！直接求偏导剥离出 M(q) |
运行结果与上表逐项一致（C11 = -l1*l2*m2*sin(q2(t))*Derivative(q2(t), t) 等）。注意不要对 f 直接 $\partial f/\partial \dot{q}$ 拆 C——那样拆出来的”C”里还残留 $\dot{q}$，速度二次项会漏进 G 里，我一开始就这么拆然后 G 里冒出了 $\dot{q}_1\dot{q}_2$ 项，验证直接报错。
电机与减速器：惯量折算（RoboMaster 电机直接相关）
云台/底盘关节的真实模型是”电机转子 + 减速器 + 负载”。设减速比 $n$（电机转 $n$ 圈负载转 1 圈，$\theta_m = n\theta_l$），电机转子惯量 $J_m$ 以 $n$ 倍角速度旋转：
1 | th_l, dth_l = dynamicsymbols('theta_l'), dynamicsymbols('theta_l', 1) |
结论：负载侧等效惯量 $J_{eq} = J_l + n^2 J_m$，等效阻尼 $B_{eq} = B_l + n^2 B_m$，力矩放大 $n$ 倍。M3508 减速比 1:359，$n^2 = 128881$——电机转子惯量折算到负载侧被放大近 13 万倍。反过来看：负载惯量折算到电机侧要除以 $n^2$，大减速比下几乎可以忽略。这直接决定了调参策略：
- 大减速比关节（M3508 底盘/发射机构）：电机侧看到的几乎是纯电机惯量，电流环/速度环按电机自身参数整定即可，负载变化不敏感；
- 小减速比/直驱（云台 yaw/pitch 电机）：负载惯量占比大，惯量比（负载/转子）决定可用带宽上限，惯量比 > 10 时 PID 增益上不去，必须做负载侧补偿或加前馈。
线性化：从动力学到 LQR
机器人控制几乎都在平衡点附近工作（云台锁目标、平衡车直立），把 $M\ddot{q} = \tau - C\dot{q} - G$ 写成状态方程 $\dot{x} = f(x,\tau)$，$x = [q, \dot{q}]^T$，在平衡点求雅可比就是线性化：
1 | qdd = M.inv()*(sp.Matrix([0, 0]) - C*sp.Matrix(dq) - G) # 无力矩，被动系统 |
$A$ 的特征值是 ±3.39 和 ±8.18（实根成对出现）——竖立位形是鞍点，开环必然倒。LQR 就是给这个线性化系统设计状态反馈 $u = -Kx$，让闭环 $A - BK$ 的特征值全进左半平面。这里有个新手常问的问题（我笔记里也记过）：LQR 不需要你”告诉”它平衡点在哪——平衡点是你建模时选的（$q=\pi$ 处重力力矩为零，状态不变），LQR 只是让系统收敛到你指定的期望状态。
参数辨识：动力学对参数是线性的
机器人动力学最漂亮的性质：M/C/G 对惯性参数（质量、质心位置、惯量）是线性的，可以写成回归形式 $\tau = Y(q,\dot{q},\ddot{q}),\theta$，$Y$ 叫回归矩阵：
1 | tau = M*sp.Matrix(ddq) + C*sp.Matrix(dq) + G |
实际标定流程：激励轨迹跑一遍 → 记录 $q, \dot{q}, \ddot{q}, \tau$ → 堆成超定方程组 $Y_{stack}\theta = \tau_{stack}$ → 最小二乘解出质量/惯量参数（数值上要用 QR/SVD 而不是法方程，理由见《线代笔记》篇）。有了 $\theta$ 就能做计算力矩控制 $\tau = M(q)\ddot{q}_d + C\dot{q} + G$。
数值仿真管线
符号推导的终点是仿真/半实物。用 lambdify 转成 numpy 函数，每步用 np.linalg.solve 解 $M\ddot{q} = \tau - C\dot{q} - G$（不要用 M.inv() 乘，数值上慢且病态）：
1 | Mf = sp.lambdify((q1, q2, dq1, dq2, m1, m2, l1, l2, g), M, 'numpy') |
踩坑清单
dynamicsymbols返回的是 $q(t)$ 函数不是普通符号。把它当 Symbol 用（比如sp.symbols('q')混搭）会得到一堆Derivative(q(t), t)和普通符号混在一起的表达式，lambdify 直接报错。凡是随时间变的量一律用dynamicsymbols。- **不要手写 $\frac{d}{dt}\frac{\partial L}{\partial \dot{q}}$**：对 $\partial L/\partial \dot{q}$ 再对 t 求导时，链式法则会展开成 $\ddot{q}, \dot{q}^2$ 混合项，极容易漏。
LagrangesMethod已经处理好了，直接用form_lagranges_equations()。 - C 矩阵要从 Christoffel 符号构造，对 f 乱拆会把科氏项拆进 G（见上文），验证恒等式是唯一可靠的检查手段。
- lambdify 的坑：参数里要传
m1, m2, l1, l2, g这些 Symbol 本身（函数签名按名字匹配），数值代入用字典**p；如果表达式里残留Derivative对象，lambdify 前先.subs()换成普通符号。 - 符号爆炸：拉格朗日法符号项数 $O(n^4)$，6 自由度完整展开 SymPy 会卡到分钟级、表达式几十 MB。工程上 >4 自由度改用牛顿-欧拉递归（pinocchio/RBDL），符号推导只用来验证小模型或推导特定结构。
- 奇异位形仿真 NaN：$q_2 = 0$（完全伸直）时 M 接近奇异，
solve出来的 $\ddot{q}$ 爆炸。仿真要避开奇异、加关节限位，或给 M 加正则化 $M + \epsilon I$。 - 单位：全程 SI（kg·m·s），惯量 kg·m²，力矩 N·m。从 CAD 导出的惯量常是 g·mm²，要乘 $10^{-9}$ 才是 kg·m²（URDF 篇踩过这个坑）。
- 推导结果必须数值验证：随机取参数代入符号表达式和已知闭式/数值仿真对比，残差 < 1e-10 才算过。符号推导也会错，只是错得比人慢。
拉格朗日 + SymPy 的组合拳适用于：验证手推结果、小自由度机构的完整动力学、生成回归矩阵做辨识、推导特殊结构（如云台两轴耦合）的控制律。超过 4 个关节就交给牛顿-欧拉递归库，别硬刚符号。