第 7 章牛顿-欧拉递推与参数辨识¶

第 6 章给出动力学"长什么样"（标准形式 \(M/C/G\)），剩下的两件事是"怎么高效算"与"参数从哪来"。牛顿-欧拉（Newton–Euler, N-E）递推用两趟线性递推算逆动力学，复杂度对关节数 \(n\) 线性；参数辨识把动力学写成对物理参数线性的回归形式，用最小二乘从测量数据定出参数——但只有"最小线性参数化"下的基参数可辨识，其余参数彼此耦合无法分开。最后把产出的被控对象模型交回 control-note。

1. 牛顿-欧拉双向递推¶

1.1 双向递推与三处约定¶

N-E 把每个连杆当作隔离体，对 \(i=1\to n\) 做外向递推（外推，outward）由父连杆算出本连杆的角速度/角加速度与质心加速度，再对 \(i=n\to1\) 做内向递推（回传，inward）由末端往基座累接力与力矩。外向段的角度速度合成与内向段的牛顿方程为

\[ \omega_i = \omega_{i-1} + \dot\theta_i\,\hat z_{i-1},\qquad f_i = f_{i+1} + m_i\,a_{c_i}, \]

前者逐连杆累加角速度、后者把各连杆惯性力逐节回传（\(a_{c_i}\) 为质心加速度，\(f\) 为关节间作用力）。对力矩同理逐节回传，取第 \(i\) 关节轴方向投影即得 \(\tau_i=\hat z_{i-1}^{\top}n_i\)。重力可作为基座一个等效加速度 \(-g\) 注入，无需单独列 \(G\)。

递推落地有三处约定必须先钉死，否则算出的 τ 会差一个符号或差一个参考点：参考系（惯量张量与质心位置要相对同一参考点表达，换点用平行轴定理）；关节型别（转动关节贡献 \(\dot\theta\hat z\)，移动关节贡献线速度，二者是两种不同的更新）；基座条件（固定基座取 \(\omega_0=0\)、\(\dot\omega_0=-g\) 把重力一次注入，别在末端再补）。任一处写错，静态构型下可能完全看不出来，一旦运动就出现"随速度平方增长"的假力矩——这类错误靠看静态数据发现不了，只能用已知构型的能量/动量关系交叉核对。

1.2 实现层：Pinocchio 与手写递推的对照¶

上面的递推不必手写：工程侧的刚体动力学事实标准是 Pinocchio（C++/Python，URDF 直读），最小用法就四个调用（pip install pin）：

import pinocchio as pin

model = pin.buildModelFromUrdf("panda.urdf")   # 读 URDF 建动力学模型
data  = model.createData()                     # 每线程一份的可写工作区

tau = pin.rnea(model, data, q, v, a)           # 逆动力学：q,v,a → 关节力矩
M   = pin.crba(model, data, q)                 # 质量矩阵（CRBA）
pin.computeJointJacobians(model, data, q)      # 关节雅可比
J   = pin.getJointJacobian(model, data, model.getJointId("panda_joint7"),
                           pin.ReferenceFrame.LOCAL)

内部同样是 N-E 递推，替你处理了参考系、关节型别与稀疏结构；data 对象是为避堆分配而预分配的工作区，不共享。与手写实现的分工：教学与验证用手写最小实现（上面的约定要自己钉死，才看得见错在哪）；工程与参数辨识用 Pinocchio（或 KDL、引擎自带的动力学模块），手写版只当交叉核对基准。三者的角色差异要分清：MuJoCo 是仿真引擎（带接触与约束），Pinocchio 是动力学/运动学计算库（无仿真循环、无接触），别拿后者当仿真器用。

与本章两条主线直接相关的还有三块：正动力学用 pin.aba(model, data, q, v, tau)（ABA 递推，对应第 6 章「正动力学与数值积分」）；参数辨识用 pin.computeJointTorqueRegressor(model, data, q, v, a) 直接产出第 4 节的回归矩阵 \(W\)（不必靠叠加原理手拼列）；导数与优化用 pinocchio.casadi 取符号化的动力学及其偏导，供 MPC 与参数估计；质心量用 pin.centerOfMass(model, data, q, v, a)。

两趟递推算逆动力学，回归形式定参数：

flowchart LR
    fwd["前向递推：基座 → 末端<br/>速度与加速度"] --> bwd["后向递推：末端 → 基座<br/>力与力矩"]
    bwd --> tau["τ，复杂度 O(n)"]
    data["激励轨迹 q, q̇, q̈, τ"] --> reg["回归矩阵 Y"]
    reg --> lip["最小线性参数化<br/>只有基参数可辨识"]
    lip --> ls["最小二乘<br/>参数估计"]

2. 逆动力学与计算量¶

N-E 递推就是逆动力学的高效算法：给定 \(q,\dot q,\ddot q\)（及基座加速度），两趟递推 \(O(n)\) 出全部 \(\tau\)。对比第 6 章拉格朗日路线——显式组装稠密 \(M(q)\in\mathbb{R}^{n\times n}\) 与 Christoffel \(C\) 需 \(O(n^2)\) 甚至更高、且符号表达式随 \(n\) 膨胀——N-E 的正/逆递推适合在线逐时刻计算，是计算力矩法（computed torque）里实时求 \(\tau_{\text{ff}}\) 的标准内核。

计算量的差距在 \(n\) 变大时才拉开：组装 \(M\) 要 \(O(n^3)\) 量级的项，N-E 是 \(O(n)\)。反过来，需要 \(M\) 显式的算法（阻抗控制、部分 MPC 形式）可以逐列求——把 \(\ddot q\) 取成单位向量、其余为零，跑 \(n\) 次逆动力学就得到 \(M\) 的 \(n\) 列，总代价 \(O(n^2)\)，仍比符号组装便宜。

逆动力学与正动力学的分工在第 6 章已定：N-E 直接算的是逆问题（代数、无状态）；正问题（给 \(\tau\) 积分出 \(q(t)\)）不用 N-E，而交给 mujoco-textbook 的广义坐标积分器。二者不可互相"取反"冒充。

3. 最小线性参数化 LIP¶

关键事实：无论多复杂，动力学对惯性参数是线性的。把每连杆的 10 个参数（质量 \(m_i\)、质心一阶矩 \(m_ic_i\) 三分量、惯量张量 6 个独立元素）连同重力项收进向量 \(\pi\)，则可写成回归形式

\[ \tau = W(q,\dot q,\ddot q)\,\pi, \]

\(W\in\mathbb{R}^{n\times p}\) 完全由可测的位形/速度/加速度算出，与未知参数无关。但 \(\pi\) 并非全部可辨识：某些参数在 \(W\) 中列线性相关（例如连杆绕与关节轴平行的惯量分量、或一阶矩与质量项的耦合），物理上无法从 \(\tau\) 分开。把这些必合并的项归并、消去线性相关列后剩下的独立参数集，称为最小线性参数化（linearly independent / base parameter set）\(\pi_b\)，对应 \(b=W_b(q,\dot q,\ddot q)\pi_b\)。

对第 6 章的平面二连杆（质点模型），\(\tau\) 对 \((m_1,m_2)\) 线性，基参数就是这两个质量本身：

col1 = tau(q,qd,qdd, m1=1, m2=0)     # 叠加原理取 m1 通道
col2 = tau(q,qd,qdd, m1=0, m2=1)     # m2 通道 → W 的第二列

并非所有动力学参数都可辨识：真实臂的 10×n 惯性参数里，只有一小部分能组进 \(\pi_b\)，其余只能以其线性组合出现，辨识得到的是合并后的基参数、不是每个原始惯量。

4. 激励轨迹与最小二乘辨识¶

有了线性回归，辨识就是最小二乘：采集一批 \((q,\dot q,\ddot q,\tau)\) 样本，堆出 \(A=\begin{bmatrix}W_1\\ \vdots \\ W_N\end{bmatrix}\)、\(b=\begin{bmatrix}\tau_1\\ \vdots\end{bmatrix}\)，解 \(\hat\pi_b=\arg\min\|A\pi_b-b\|^2\)。能否解出、解得稳不稳，取决于激励轨迹是否让 \(A\) 满列秩、条件数是否可控。

## 40 组随机激励堆叠回归矩阵 A (80x2)
## rank(A) = 2, cond(A) = 2.687
## 奇异值 sv = [92.857, 34.557]   —— 第二奇异值 >0 → m1,m2 均可辨识
pi_hat = np.linalg.lstsq(A, b, rcond=None)[0]     # = [2. 1.]，与真值 [2.0, 1.0] 一致
## 残差 2-范数 = 2.5e-09（模型精确、无噪声时到机器精度）

取真值 \(m_1=2,m_2=1\)，堆叠回归矩阵秩为 2、条件数 \(2.69\)、两个奇异值 \((92.86,34.56)\) 皆非零，最小二乘精确回到 \((2,1)\)，残差 \(2.5\times10^{-9}\)。实际激励轨迹设计要最小化 \(A\) 的条件数（常做成对 \(W\) 奇异值的凸优化），并覆盖足够多的位形-速度组合，否则某个基参数方向激励不足、\(A\) 接近秩亏，辨识方差爆炸。有摩擦、负载等非线性项时改在 \(\pi_b\) 里加入对应通道或用迭代最小二乘。

样本质量决定结论能信到什么程度，三处最常出问题：力矩若由电流乘 \(K_T\) 估出，摩擦与温漂直接进 \(b\)，需单独建模或改直测；加速度由位置二次差分得到，量化噪声被二次放大，必须先低通再对齐相位，否则回归矩阵与观测之间有固定时延，辨识结果系统性偏低；轨迹要覆盖每个基参数方向的独立变化，单方向往复运动激励不出耦合项。

解出来还要过一致性检查：质量非负、惯量张量正定、质心落在连杆包络内。最小二乘不管物理约束，噪声大时能给出"负质量"这种数学上最优、物理上无意义的解。常见做法是加参数上下界做约束最小二乘，或对病态方向加正则项把解拉回可行域。

5. 与 control 被控对象接口的交接¶

第 6 章与第 7 章合起来交付的是被控对象模型 \(M(q)\ddot q+C(q,\dot q)\dot q+G(q)=\tau\) 及其参数 \(\pi_b\)：第 6 章给出结构形式与性质，第 7 章给出高效逆算法（N-E）与参数来源（LIP 辨识）。以此为输入，计算力矩前馈、阻抗/导纳、MPC 等回路设计与状态观测器/滤波融合都在 control-note 展开——这里止于"对象怎么建、参数怎么定"，不写控制律与估计律。

6. 要点¶

牛顿-欧拉用外向（速度/加速度）+ 内向（力/力矩）两趟递推算逆动力学，\(O(n)\)，重力以基座等效加速度注入。
N-E 只算逆问题（代数、无状态）；正问题交给仿真侧积分，二者不可互相取反。
动力学对惯性参数线性 \(\tau=W\pi\)，但原始 \(10\times n\) 参数存在列相关，只有最小线性参数化基参数 \(\pi_b\) 可辨识。
辨识=堆回归做最小二乘，成败在激励轨迹：平面二连杆例中 \(\operatorname{rank}(A)=2\)、\(\operatorname{cond}=2.69\)、奇异值全非零，精确回 \((m_1,m_2)=(2,1)\)、残差 \(2.5\times10^{-9}\)。
被控对象模型与参数交 control-note 做闭环；控制律与状态估计不属这一层。

参考文献与延伸阅读¶

Featherstone, R. Rigid Body Dynamics Algorithms. Springer, 2008.
Atkeson, C. G., An, C. H., & Hollerbach, J. M. Estimation of Inertial Parameters of Manipulator Loads and Links. The International Journal of Robotics Research, 1986. DOI
Khalil, W., & Dombre, E. Modeling, Identification and Control of Robots. CRC Press, 2002.

第 7 章 牛顿-欧拉递推与参数辨识¶