第 6 章拉格朗日动力学与标准形式¶

动力学回答"要产生某个运动需要多大的力/力矩"。与运动学只管几何不同，它引入质量、惯量与重力，是力控、拖动示教与仿真的共同底座。拉格朗日方法把 n 自由度串联臂写成标准形式 \(M(q)\ddot q + C(q,\dot q)\dot q + G(q) = \tau\)，并由它导出 \(M\) 的正定性、\(\dot M-2C\) 的斜对称性两条结构性质；正动力学（给 \(\tau\) 求 \(\ddot q\)）与逆动力学的计算差别也由此而来。

示例用第 3 章的平面二连杆。牛顿–欧拉递推与参数辨识见第 7 章；基于此模型的闭环控制律归 control-note。

1. 拉格朗日方程与能量项¶

取广义坐标 \(q\in\mathbb{R}^n\)，系统动能 \(T(q,\dot q)\) 与势能 \(V(q)\) 构成拉格朗日量 \(L=T-V\)。第二类拉格朗日方程（Euler–Lagrange equation）

\[ \frac{d}{dt}\frac{\partial L}{\partial \dot q_i}-\frac{\partial L}{\partial q_i}=\tau_i,\qquad i=1,\dots,n \]

把"能量对广义速度/坐标的偏导"转成力矩方程。它的工程价值在于：只需写出 \(T\) 与 \(V\) 两个标量再求导，不必逐连杆画受力图，机械地推广到任意自由度——这正是它比牛顿-欧拉更适合符号推导与建模的原因。对平面二连杆（\(L_1=L_2=1\)，质点 \(m_1\) 在关节2、\(m_2\) 在末端、重力沿 \(-y\)）：

\[ T=\tfrac12 m_1 L_1^2\dot q_1^2+\tfrac12 m_2\big[L_1^2\dot q_1^2+L_2^2(\dot q_1+\dot q_2)^2+2L_1L_2\cos q_2\,\dot q_1(\dot q_1+\dot q_2)\big],\quad V=g\big[(m_1+m_2)L_1\sin q_1+m_2L_2\sin(q_1+q_2)\big]. \]

拉格朗日方程的边界也要说清：理想约束力（光滑铰链）不做虚功，因此不出现在方程里——这省掉了逐连杆解约束反力的麻烦，代价是接触力、摩擦、关节柔性与电机反电动势这些非保守效应必须作为右端广义力显式加进来。漏掉哪一项，方程在对应工况下就会系统性偏离；符号推导出来的模型总要再补一层摩擦项，原因就在这里。

三个矩阵项由图中的量合成，正动力学按最后一步反解：

flowchart LR
    q["q, q̇"] --> m["M(q) 质量矩阵"]
    q --> c["C(q, q̇) 科氏与离心"]
    q --> g["G(q) 重力项"]
    m --> eq["M q̈ + C q̇ + G = τ"]
    c --> eq
    g --> eq
    eq -->|"正动力学"| qdd["q̈ = M⁻¹(τ − C q̇ − G)"]

2. 标准形式 M/C/G¶

对上面的 \(T,V\) 求导并整理，n 自由度动力学总可写成标准形式

\[ M(q)\,\ddot q + C(q,\dot q)\,\dot q + G(q) = \tau, \]

\(M(q)\in\mathbb{R}^{n\times n}\) 是质量矩阵（惯性项，只依赖位形），\(C(q,\dot q)\dot q\) 收拢科里奥利与离心项，\(G(q)=\partial V/\partial q\) 是重力项。平面二连杆各元素为

\[ M=\begin{bmatrix}(m_1+m_2)L_1^2+m_2L_2^2+2m_2L_1L_2 c_2 & m_2L_2^2+m_2L_1L_2 c_2\\ m_2L_2^2+m_2L_1L_2 c_2 & m_2L_2^2\end{bmatrix},\quad G=\begin{bmatrix}(m_1+m_2)gL_1 c_1+m_2gL_2 c_{12}\\ m_2gL_2 c_{12}\end{bmatrix}. \]

取 \(m_1=2,m_2=1\)、\(q=(30^\circ,60^\circ)\)、\(\dot q=(1,-0.5)\)、\(\ddot q=(2,1.5)\)：

## M = [[5.  , 1.5],
##      [1.5 , 1. ]]
## C(q,qd) = [[ 0.433013, -0.433013],
##            [ 0.866025,  0.      ]]
## G = [25.487128, 0.0]        （该位形下 c12=cos(90°)=0，第二分量归零）
## tau = M qdd + C qd + G = [38.386647, 5.366025]

\(C\) 采用由 \(M\) 的 Christoffel 符号构造的约定，使下一节的能量性质严格成立；不同教材对 \(C\) 的元素分配不唯一，但 \(C\dot q\) 作为一个整体唯一。

两个结构性事实在后面会反复用到：\(M\) 与 \(G\) 的每个元素都是惯性参数（质量、一阶矩、惯量张量）的线性组合，这正是第 7 章把动力学写成 \(\tau=W\pi\) 的前提；\(G\) 依赖重力在基座坐标系里的方向，同一台臂装到倾斜基座或移动平台上，\(G(q)\) 就换一套值，重力补偿不能照搬水平安装时的表。

从公式到库：标准形式的三个量在 Pinocchio 里各有对应调用——pin.crba(model, data, q) 给 \(M\)，pin.nonLinearEffects(model, data, q, v) 给 \(C\dot q+G\)，pin.rnea(model, data, q, 0, 0) 给静态重力项 \(G\)；把三者拼回来与直接的 pin.rnea(model, data, q, v, a) 对照，就是本节的公式验证。工程里不手算 \(M\)：控制器、MPC 与参数辨识都从库取这三块，手算只用在小算例对拍。

3. M 正定与 C 斜对称¶

标准形式有两条贯穿控制分析的结构性质：

\(M(q)\) 对称正定：\(M=M^\top\) 且对所有 \(\dot q\neq0\) 有 \(\dot q^\top M\dot q=2T>0\)。上例 \(M=\begin{bmatrix}5&1.5\\1.5&1\end{bmatrix}\) 的特征值为 \((0.5,\,5.5)\)，均正，故可逆、正定。
\(\dot M - 2C\) 斜对称（等价 \(\dot M = C + C^\top\)）：它保证无外力时动能守恒，是系统无源性（passivity）与基于 Lyapunov 的控制器设计的基石。

两条性质落成两个恒等式，可用于核对实现：

print(np.linalg.norm(Mdot - C - C.T))   # 结构恒等式：值应为 0（浮点下到机器精度）

\(\dot M\) 由 \(\dot{M}=\sum_k(\partial M/\partial q_k)\dot q_k\) 逐项算出，凡由 \(M\) 经 Christoffel 符号生成的 \(C\) 都自动满足 \(\dot M=C+C^\top\)——这也是选这一约定的理由：让结构性质成为构造的一部分，而不是事后碰巧成立。

两条性质直接对应两个工程结论：\(M\) 正定保证正动力学 \(\ddot q=M^{-1}(\cdots)\) 始终可解；\(\dot M-2C\) 斜对称给出无源性，闭环稳定性分析才有能量函数可用。此外 \(M\) 还有界（存在 \(\lambda_{\min},\lambda_{\max}\) 使 \(\lambda_{\min}I\preceq M\preceq\lambda_{\max}I\)），这是自适应与鲁棒控制器能给出收敛结论的前提。控制侧如何利用无源性做闭环设计不属这一层，归 control-note。

4. 正动力学与数值积分¶

标准形式给出两个方向的问题，计算性质完全不同：

逆动力学：给定轨迹 \(q,\dot q,\ddot q\) 求 \(\tau=M\ddot q+C\dot q+G\)，是一次代数运算（逐时刻闭式，无需积分），第 7 章的牛顿-欧拉递推把它算得线性于 \(n\)。
正动力学：给定 \(\tau\) 求运动，需解 \(\ddot q = M(q)^{-1}\big(\tau - C(q,\dot q)\dot q - G(q)\big)\) 这是一个初值问题，须对时间积分（Runge–Kutta 等）才能得到 \(q(t)\)，且每步都要解一次 \(M\ddot q=\cdots\)。

qdd_back = np.linalg.solve(M, tau - C @ qd - G)   # 正动力学反解
## = [2. , 1.5]，与输入 qdd 差 3.0e-15

逆动力学并非正动力学的取反：一个是无状态的代数式、一个是需求解 ODE 的初值问题，计算量与数值稳定性皆不同。正动力学的积分步长、能量漂移等属仿真侧，与 mujoco-textbook 的求解篇直接相关。

正动力学的数值难点在 \(M\) 的条件数：伸直、近奇异构型下 \(M\) 接近奇异，逆解对 \(\tau\) 的扰动放大，显式积分步长随之被压小。工程上取半隐式或隐式积分，并把约束（闭链、接触）与自由度方程联立成 DAE 处理——此时 \(\ddot q\) 里多了约束力，标准形式那一行不再是完整模型。这也是仿真引擎与手写 ODE 在接触工况下结果差异大的根源。

5. 与 MuJoCo 广义坐标求解的对照¶

mujoco-textbook 的仿真引擎处理的正是本章正动力学问题：它把机器人表示成广义坐标 \(q\) 上的刚体系统，每个时刻组装出与本章 \(M(q)\) 对应的质量矩阵，再通过稀疏因子化（利用铰接树的低带宽结构）高效解 \(M\ddot q=\tau-h(q,\dot q)\)（\(h\) 即科氏项与重力项之和），并叠加约束与接触求解。二者的对应关系是：本章标准形式是数学侧的 \(M/C/G\)，MuJoCo 是它在广义坐标 + 稀疏数值线性代数下的工程实现，闭链、接触与约束则是 MuJoCo 超出理想串联开链模型的额外部分。具体的 MuJoCo 引擎版本与实现细节属跨板块范畴，取该书结论作对照。

6. 要点¶

拉格朗日法只需写动能 \(T\) 与势能 \(V\)，经 \(\frac{d}{dt}\partial_{\dot q}L-\partial_q L=\tau\) 机械导出 n 自由度方程。
标准形式 \(M(q)\ddot q+C(q,\dot q)\dot q+G(q)=\tau\)：\(M\) 惯性、\(C\dot q\) 科氏/离心、\(G\) 重力；平面二连杆在测试位形 \(M=[[5,1.5],[1.5,1]]\)、\(\tau=(38.39,5.37)\)。
两条结构性质可数值核验：\(M\) 对称正定（特征值 \(0.5,5.5\)）、\(\dot M-2C\) 斜对称（\(\|\dot M-C-C^\top\|=0\)），后者保证无源性与能量守恒。
逆动力学是代数式、正动力学是需求积分的初值问题，二者不可混为一谈；正动力学的工程实现即 MuJoCo 广义坐标 + 稀疏因子化求解。

参考文献与延伸阅读¶

Craig, J. J. Introduction to Robotics: Mechanics and Control (4th ed.). Pearson, 2017.
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. Robotics: Modelling, Planning and Control. Springer, 2009. DOI
Featherstone, R. Rigid Body Dynamics Algorithms. Springer, 2008.

第 6 章 拉格朗日动力学与标准形式¶