【机器人学习】小白学习第12讲：动力学（下）——牛顿-欧拉递推、参数辨识与操作空间动力学

一句话核心：拉格朗日法给你一个好看的闭式公式，牛顿-欧拉法给你一个跑得快的递推算法；而模型里的质量惯量往往不准，要靠"对参数线性"这个性质用最小二乘辨识出来。

• 本讲目标：理解牛顿-欧拉"向外递推 + 向内递推"两步走；分清正动力学与逆动力学；知道参数辨识为什么是最小二乘；能把动力学搬到操作空间（任务空间）。

一、为什么需要这个

小讲 1：为什么还要牛顿-欧拉：全局记账 vs 逐节车厢算

1.1 全局记账 vs 逐节车厢算

上一讲你学会了用拉格朗日法推导动力学，得到了漂亮的闭式表达式。但直接指出了它的软肋：自由度一多，手推又慢又容易错，而且算起来贵。

生活比喻：算一列火车的运动。

• 拉格朗日法像"全局记账"：先算出整列车的总动能、总势能，再统一求导。账目清楚、形式漂亮，但车厢一多，账本爆炸。
• 牛顿-欧拉法像"逐节车厢算"：从车头开始，一节一节往后传"每节车厢此刻的速度和加速度"；再从车尾开始，一节一节往前传"挂钩上要拉多大的力"。每节车厢只跟它前后相邻的两节打交道。

第二种方法没有漂亮的闭式，但每节车厢只算一次，总工作量与车厢数成正比。

1.2 第二个动机：模型参数是猜不准的

第二个动机更现实：模型里的参数是猜不准的。说得很直白——用 CAD 按几何和材料算出来的惯量不准确（几何建模必然简化），而关节摩擦这种复杂动态效应根本没法从图纸算出来；把机器人拆开逐个称量和测量？不现实。所以必须用辨识。

停下来想一想：为什么关节摩擦这种效应没法从 CAD 图纸算出来、只能靠辨识？把机器人拆开逐个称量为什么也不现实？

二、核心概念

小讲 2：单杆力平衡方程与向外、向内两步递推

2.1 牛顿-欧拉法：力的平衡，而不是能量

对第 i 个增广连杆（连杆 i + 装在它上面、驱动关节 i+1 的电机转子），写两条方程。

$$
\begin{aligned}
\text{牛顿方程（质心平动平衡）：} \
f_{i} - f_{i+1} + m_{i} g_{0} = m_{i} \ddot{p}{Ci} \
\text{欧拉方程（绕质心转动平衡）：} \
\mu{i} + f_{i} \times r_{i-1,Ci} - \mu_{i+1} - f_{i+1} \times r_{i,Ci} \
= Ī_{i} \dot{\omega}{i} + \omega{i} \times ( Ī_{i} \omega_{i} ) \

• k_{r,i+1} \ddot{q}{i+1} I{m,i+1} z_{m,i+1} \
• k_{r,i+1} \dot{q}{i+1} I{m,i+1} \omega_{i} \times z_{m,i+1}
  \end{aligned}
  $$

符号	含义	大白话
f_i	连杆 i−1 施加在连杆 i 上的力	前面那节车厢拉我的力
−f_{i+1}	连杆 i+1 反作用在连杆 i 上的力	后面那节车厢拽我的力
μ_i	连杆 i−1 施加在连杆 i 上的力矩（对坐标系 i−1 原点取矩）	前面传来的扭矩
ω_i × (Ī_i ω_i)	陀螺项，明确说明它来自 Ī_i 随连杆姿态变化	高速旋转的物体"不愿意改变转轴方向"
k_{r,i+1} q̈_{i+1} I_{m,i+1} z_{m,i+1}	转子自身加速需要的力矩	电机转子也要被带着转

两个聪明的处理：

1. 重力 m_i g₀不产生力矩，因为它作用在质心上，对质心取矩为零。
2. 进一步，把基座的线加速度取成p̈₀ − g₀（而不是 p̈₀）。这样重力就"伪装成"了一个基座加速度，被自动带进后面每一级的 p̈_Ci 里，牛顿方程 (7.87) 里的 +m_i g₀ 项从此消失，递推式简化成 f_i = f_{i+1} + m_i p̈_Ci。这是牛顿-欧拉实现里最常被问"为什么"的一处。

关节广义力由 (7.92) 给出：移动关节把力f_i 投影到关节轴 z_{i−1} 上；旋转关节把力矩μ_i 投影到 z_{i−1} 上；两者都要再加一项转子惯性力矩 k_ri I_mi ω̇_miᵀ z_mi。

2.2 递推的两步走（本讲的核心）

提示

第一步向外递推（forward recursion，基座 → 末端）：已知 q、q̇、q̈ 和基座条件 ω₀ = 0、ω̇₀ = 0、p̈₀ − g₀，用式 (7.107)–(7.111) 依次算出每根杆的 ω_i、ω̇_i、p̈_i、p̈_Ci 以及转子的 ω̇_mi。传的是运动学量。

第二步向内递推（backward recursion，末端 → 基座）：已知末端边界条件 f_{n+1}、μ_{n+1}（无接触时 = 0），用式 (7.112)(7.113) 从 i = n 倒着算到 i = 1，得到每根杆的 f_i、μ_i，再用 (7.114) 投影出关节力矩 τ_i（含摩擦项）。传的是力与力矩。

为什么能递推：说得很清楚——牛顿-欧拉方程不是闭式，单根连杆的运动通过速度/加速度的运动学关系与相邻连杆耦合。但也正因为每根杆只依赖前一级，才能一遍扫过去算完。

停下来想一想

一个重要的实现技巧：这里要强调，虽然推导时假设所有向量都在基坐标系，但真正写代码时全部改到连杆自身坐标系 i 上会更好。原因是：Ī_i、r_{i,Ci}、z_mi 这些量在连杆自身坐标系里是常数，而 z₀ = [0 0 1]ᵀ。代价只是相邻坐标系之间要乘一个旋转矩阵（i+1 → i 乘 Rⁱ_{i+1}，i−1 → i 乘 R^{i−1}_iᵀ），但省掉了每步重算随构型变化的惯量张量，净赚。

小讲 3：O(n) 复杂度与正动力学、逆动力学的分工

2.3 计算复杂度：为什么实时控制爱用牛顿-欧拉

$$\begin{array}{r}\text{给出的结论：对}n\text{关节机械臂，}\\ •\text{逆动力学计算：}O(n)\\ •\text{正动力学计算：}O(n^2)\end{array}$$

直觉上这是在说什么：O(n) 意味着"关节数翻一倍，计算量也只翻一倍"。6 轴和 12 轴机器人之间的差距是线性的，而不是爆炸式的。这就是为什么实时伺服环（第 10 讲里那个 1 ms 的伺服级）普遍用牛顿-欧拉。

补充：常听到"拉格朗日闭式是 O(n⁴) 量级"的说法，指的是按闭式逐项展开计算；本身只给出上面两条 O(n) / O(n²) 的结论，本讲以为准。另外也补了一句：拉格朗日法经过相当的重构努力，也能做成计算高效的递推形式。

	拉格朗日法	牛顿-欧拉法
优点	系统、直观；给出紧致的解析形式（B、C、g 一目了然），便于控制设计；便于加入连杆柔性等复杂效应	天然递推、计算高效，适合在线实时
缺点	闭式展开计算量大；自由度多时手推易错	不直接给出 B、C、g 的解析形式

2.4 正动力学 vs 逆动力学

正动力学 Direct dynamics

已知：关节力矩 τ(t)、末端受力 h_e(t)、初始状态 q(t₀)、q̇(t₀)。

求：关节加速度 q̈(t)，进而积分出速度、位置。

用途：仿真——不给真机，也能预测机器人会怎么动。

逆动力学 Inverse dynamics

已知：期望的关节位置 q(t)、速度 q̇(t)、加速度 q̈(t)、末端受力 h_e(t)。

求：需要施加的关节力矩 τ(t)。

用途：轨迹规划可行性检查+控制器里的非线性项补偿（动力学前馈）。

$$\begin{array}{r}\text{由式}\text{解出加速度：}\\ \ddot{q}=B^{-1}(q)(\tau -{\tau }^{\prime })(7.115)\\ {\tau }^{\prime }(q,\dot{q})=C(q,\dot{q})\dot{q}+F_v\dot{q}+F_s\mathrm{sgn}(\dot{q})+g(q)+J^{\top }(q)h_e(7.116)\end{array}$$

仿真的操作步骤：已知 t_k 时刻的 q(t_k)、q̇(t_k) → 用 (7.115) 算 q̈(t_k) → 用数值积分步长 Δt 推进到 q̇(t_{k+1})、q(t_{k+1}) → 循环。

提示

牛顿-欧拉也能做正动力学：

• 算 τ′(q, q̇)：令q̈ = 0，跑一遍递推算法，输出的就是 τ′；

• 算 B(q) 的第 i 列 b_i：令g₀ = 0、q̇ = 0、q̈_i = 1、其余 q̈_j = 0，跑一遍递推算法，输出就是 b_i；对 i = 1…n 各跑一次，拼出整个 B。

拿到 B 和 τ′ 后再套 (7.115) 积分即可。这个"用逆动力学算法拼出正动力学"的思路很典型，值得记住。

小讲 4：参数辨识的最小二乘解法与轨迹动态缩放

2.5 参数辨识：为什么它是最小二乘

上一讲留下的性质 τ = Y(q, q̇, q̈) π 在这里派上用场。沿一条轨迹在 N 个时刻采样，把方程摞起来：

$$\begin{array}{r}\bar{\tau }=\left[\begin{array}{c}\tau (t_1)\\ \dots \\ \tau (t_N)\end{array}\right]=\left[\begin{array}{c}Y(t_1)\\ \dots \\ Y(t_N)\end{array}\right]\pi =\bar{Y}\pi \\ \pi =({\bar{Y}}^{\top }\bar{Y}{)}^{-1}{\bar{Y}}^{\top }\bar{\tau }\\ \uparrow \text{这就是}\bar{Y}\text{的左伪逆}\end{array}$$

问题	的回答
需要测什么？	关节位置 q、速度 q̇（可实测）与加速度 q̈（靠数值重建，可以用非因果滤波器，因为不必实时）；力矩 τ（有关节力矩传感器最好，否则用腕力传感器或电机电流估算）
采样点要多少？	N·n ≫ p（测量点数远大于待估参数个数），否则 Ȳ 病态
有些参数估不出来？	不可辨识参数：某些参数对任何轨迹都不出现在方程里，Y 中对应列全零，必须删掉。举例：两连杆平面臂 22 个可能参数里只有 8 个真正出现（见上一讲 2×8 矩阵）
只以线性组合出现？	每个线性组合要删掉"组合中参数个数 − 1"列，只保留一个代表
Ȳ 不满秩怎么办？	用阻尼最小二乘（damped least-squares），精度取决于阻尼因子权重；或用奇异值分解（SVD）做数值分析
轨迹怎么选？	选多项式型且足够"丰富"的轨迹，让 ȲᵀȲ 的条件数低；但不要剧烈到激发关节弹性、连杆柔性等未建模效应，否则估计不可靠
还能干什么？	可以辨识末端未知负载——把负载看作末杆的结构修改；有腕力传感器时可直接从受力测量反推负载参数

直觉上这是在说什么：因为 τ 对 π 是线性的，"猜参数"就退化成了你在任何一门数值课上学过的超定线性方程组的最小二乘解。如果模型对参数是非线性的，就得做非线性优化，麻烦十倍。

还提到一种逐关节顺序辨识的思路（利用 Y 的块三角结构，从最外侧关节往基座逐个解），但警告说误差会逐级累积，所以通常还是让所有关节同时运动、做全局辨识。

2.6 轨迹动态缩放：来不及就放慢

$$\begin{array}{r}\text{把}C(q,\dot{q})\dot{q}\text{改写成}\Gamma (q)[\dot{q}\dot{q}]\text{，其中}[\dot{q}\dot{q}]\text{是所有}{\dot{q}}_i{\dot{q}}_j\text{组成的}\\ n(n+1)/2\text{维向量}(7.117)\\ \text{令}q(t)=\bar{q}(r(t))\text{，则}\dot{q}=\dot{r}{\bar{q}}^{\prime },\ddot{q}={\dot{r}}^2{\bar{q}}^{\prime \prime }+\ddot{r}{\bar{q}}^{\prime }\\ \Rightarrow {\tau }^s(t)={\dot{r}}^2{\bar{\tau }}^s(r)+\ddot{r}B(\bar{q}){\bar{q}}^{\prime }(r)(7.126)\\ \text{线性缩放}r=ct\Rightarrow {\tau }^s(t)=c^2{\bar{\tau }}^s(ct)(7.126\text{特例})\end{array}$$

直觉上这是在说什么：与速度、加速度有关的力矩，按时间缩放因子的平方变化。取 c > 1 时，新轨迹用更长的参数区间覆盖同一条路径，其 τ̄ˢ 变成原 τˢ 的1/c²——也就是放慢一点，力矩按平方降下来。而重力项完全不受影响，因为它只依赖位置、不依赖时间怎么走。

提示

工程做法：用递推逆动力学边跑边算力矩 → 发现某个关节超了电机力矩上限（且超限不是纯重力造成的）→ 找出超得最厉害的那个关节 → 算出需要的缩放因子 → 直接在时间轴上拉伸，不必重算整条轨迹的逆动力学。

也指出缺点：线性缩放会"惩罚"整条轨迹，哪怕超限只发生在很短的一段时间里。

小讲 5：操作空间动力学与动态可操作度椭球

2.7 操作空间（任务空间）动力学

上一讲的模型是"关节力矩 ↔ 关节加速度"。但很多任务是在末端上描述的（“沿这条直线以某加速度移动”）。从关节空间模型出发做变量替换：

$$\begin{array}{r}{B}_A=(J_A{B}^{-1}{J}_A^{\top }{)}^{-1}(7.130)\\ C_A{\dot{x}}_e=B_A{J}_A{B}^{-1}C\dot{q}-B_A{\dot{J}}_A\dot{q}(7.131)\\ g_A=B_A{J}_A{B}^{-1}g(7.132)\\ \Rightarrow B_A(x_e){\ddot{x}}_e+C_A(x_e,{\dot{x}}_e){\dot{x}}_e+g_A(x_e)={\gamma }_A-h_A(7.133)\end{array}$$

符号	含义
J_A	解析雅可比（与位置/姿态的解析表示相对应，区别于几何雅可比 J）
B_A, C_A, g_A	操作空间里的"等效惯量矩阵 / 科氏矩阵 / 重力向量"有的文献（Khatib 系列）记作 Λ、μ、p，含义相同
γ_A	关节驱动折算成的等效末端力
h_A	末端与环境的接触力

形式与关节空间完全类似——这是刻意强调的。对非冗余机械臂且 J_A 非奇异时，还能进一步化简成：

$$\begin{array}{r}{B}_A=J_A^{-\top }B{J}_A^{-1}{C}_A{\dot{x}}_e=J_A^{-\top }C\dot{q}-B_A{\dot{J}}_A\dot{q}{g}_A=J_A^{-\top }g\\ (7.134)(7.135)(7.136)\end{array}$$

常见误区

成立条件：J_A B⁻¹ J_Aᵀ 可逆，当且仅当 J_A 满秩，也就是既没有运动学奇异、也没有表示奇异（比如欧拉角表示的奇异性）。到了奇异位形，操作空间动力学模型就不存在了。

还说明：这个模型的价值在于它对冗余机械臂也形式有效（此时 B_A 表示"动能伪能量"）；冗余度可以在动力学层面通过 τ₀（投影到零空间的任意力矩）来消解。

2.8 动态可操作度椭球

第 5 讲你见过（运动学）可操作度椭球，它衡量"各方向上能跑多快"。这里加上惯量，衡量"各方向上能加速多猛"。

$$\begin{array}{r}\text{条件：机器人静止（}\dot{q}=0\text{）、不接触环境（}{h}_e=0\text{）}\Rightarrow \text{简化模型}B(q)\ddot{q}+g(q)=\tau \\ \text{设关节力矩满足}{\tau }^{\top }\tau =1\text{（单位球面上的点）}\\ \text{由二阶微分运动学（}\dot{q}=0\text{时）：}{\dot{v}}_e=J(q)\ddot{q}\\ \text{最小范数解反代：}\tau =BJ†{\dot{v}}_e+g\\ \Rightarrow ({\dot{v}}_e+J{B}^{-1}g{)}^{\top }J{†}^{\top }{B}^{\top }BJ†({\dot{v}}_e+J{B}^{-1}g)=1(7.148)\\ \text{非冗余时}J†=J^{-1}\text{：}\\ ({\dot{v}}_e+J{B}^{-1}g{)}^{\top }{J}^{-\top }{B}^{\top }B{J}^{-1}({\dot{v}}_e+J{B}^{-1}g)=1(7.149)\end{array}$$

看点	说明
二次型核心 J†ᵀ BᵀB J†	由机械臂的几何 + 惯量特性决定，决定椭球的体积和主轴方向
平移量 −J B⁻¹ g	重力让椭球整体平移（对每个构型是一个常数平移）
几何含义	椭球面上一点到末端的距离 = 在该方向上能施加的末端加速度大小
的观察	因为重力把椭球中心往下移，向下加速的能力明显大于向上——符合直觉（给了一个三连杆平面臂的例子）

用途：机械结构设计的评价工具，也可以用来寻找"最优构型"（比如在哪个姿势下末端加速能力最强）。

停下来想一想：向内递推的公式 f_i = f_{i+1} + m_i p̈_Ci 里为什么不再出现 m_i g₀？重力被藏到哪一步里去了？

三、关键公式

小讲 6：两步递推完整公式、符号读法与初始边界条件

3.1 两步递推的完整公式（7.107–7.114）

$$
\begin{aligned}
【\text{向外递推}】i = 1 \dots n\text{（所有向量写在连杆自身坐标系} i \text{上）} \
\omega_{i} = R^{i-1}_{i}{\top} ( \omega_{i-1} + \dot{\vartheta}{i} z{0} ) (7.107) \
\dot{\omega}{i} = R^{i-1}_{i}{\top} ( \dot{\omega}{i-1} + \ddot{\vartheta}{i} z{0} + \dot{\vartheta}{i} \omega{i-1} \times z_{0} ) (7.108) \
\ddot{p}{i} = R^{i-1}_{i}{\top} \ddot{p}{i-1} + \dot{\omega}{i} \times r{i-1,i} + \omega_{i} \times (\omega_{i} \times r_{i-1,i}) (7.109) \
\ddot{p}{Ci} = \ddot{p}{i} + \dot{\omega}{i} \times r{i,Ci} + \omega_{i} \times (\omega_{i} \times r_{i,Ci}) (7.110) \
\dot{\omega}{mi} = \dot{\omega}{i-1} + k_{ri} \ddot{q}{i} z{mi} + k_{ri} \dot{q}{i} \omega{i-1} \times z_{mi} (7.111) \
【\text{向内递推}】i = n \dots 1 \
f_{i} = R^{i}{i+1} f{i+1} + m_{i} \ddot{p}{Ci} (7.112) \
\mu{i} = - f_{i} \times ( r_{i-1,i} + r_{i,Ci} ) + R^{i}{i+1} \mu{i+1} \

• R^{i}{i+1} f{i+1} \times r_{i,Ci} \
• Ī^{i}{i} \dot{\omega}{i} + \omega_{i} \times ( Ī^{i}{i} \omega{i} ) \
• k_{r,i+1} \ddot{q}{i+1} I{m,i+1} z_{m,i+1} \
• k_{r,i+1} \dot{q}{i+1} I{m,i+1} \omega_{i} \times z_{m,i+1} (7.113) \
  \tau_{i} = \mu_{i}^{\top} R^{i-1}_{i}{\top} z_{0} + k_{ri} I_{mi} \dot{\omega}{mi}^{\top} z{mi} \
• F_{vi} \dot{\vartheta}{i} + F{si} \operatorname{sgn}(\dot{\vartheta}_{i}) \text{（旋转关节）} (7.114)
  \end{aligned}
  $$

3.2 递推公式里的符号怎么读

符号	含义
R^{i−1}_i	把向量从坐标系 i−1 转到坐标系 i 的旋转矩阵（所以向前传要乘它的转置）
Rⁱ_{i+1}	把向量从坐标系 i+1 转到坐标系 i 的旋转矩阵（向内递推要用）
r_{i−1,i}	坐标系 i−1 原点指向坐标系 i 原点的向量
r_{i,Ci}	坐标系 i 原点指向增广连杆质心 C_i 的向量
z₀	恒等于 [0 0 1]ᵀ（写在自身坐标系里就是这个好处）
Īⁱ_i	增广连杆在自身坐标系下的惯性张量，常数

3.3 初始条件、边界条件与整体直觉

初始 / 边界条件：向外递推从 ω₀ = 0、ω̇₀ = 0、p̈₀ − g₀出发；向内递推从末端 f_{n+1}、μ_{n+1} 出发（无接触时取 0）。

直觉上这是在说什么：向外那一步是在回答"每根杆此刻到底动成什么样"，向内那一步是在回答"要维持这个运动，各关节得互相使多大劲"。两步合起来，就是在 1 ms 内算出上一讲那个 B q̈ + C q̇ + F q̇ + g 的总和。

停下来想一想

停下来想一想：向外递推的初始加速度为什么要取 p̈₀ − g₀ 而不是 p̈₀？写代码时为什么要把所有向量改写到连杆自身坐标系？

**

小讲 7：牛顿-欧拉双向递推图怎么读

停下来想一想

停下来想一想：图上蓝色链和橙色链各传播什么量？末端在无接触时给出的边界条件 f_{n+1}、μ_{n+1} 取多少？

四、例题 / 动手

例 1：两连杆平面臂，手算一遍递推

初始条件：p̈₀⁰ − g₀⁰ = [0 g 0]ᵀ（注意这里重力是 −g 沿 y，变换后基座加速度写成 +g），ω₀⁰ = ω̇₀⁰ = 0；末端边界条件 f₃³ = 0，μ₃³ = 0。常向量：r¹_{1,C1} = [ℓ_C1 0 0]ᵀ，r¹_{0,1} = [a₁ 0 0]ᵀ，r²_{2,C2} = [ℓ_C2 0 0]ᵀ，r²_{1,2} = [a₂ 0 0]ᵀ。

$$
\begin{aligned}
【\text{向外}】\text{连杆} 1\text{：} \omega_{1}^{1} = [0 0 \dot{\vartheta}{1}]^{\top} , \dot{\omega}{1}^{1} = [0 0 \ddot{\vartheta}{1}]^{\top} \
\ddot{p}^{1}{1} = \begin{bmatrix} -a_{1}\dot{\vartheta}{1}^{2} + g s{1} \ a_{1}\ddot{\vartheta}{1} + g c{1} \ 0 \end{bmatrix}^{\top} \
\ddot{p}^{1}{C1} = \begin{bmatrix} -( \ell {C1}+a{1})\dot{\vartheta}{1}^{2} + g s_{1} \ ( \ell {C1}+a{1})\ddot{\vartheta}{1} + g c{1} \ 0 \end{bmatrix}^{\top} \
【\text{向外}】\text{连杆} 2\text{：} \omega_{2}^{2} = [0 0 \dot{\vartheta}{1}+\dot{\vartheta}{2}]^{\top} , \dot{\omega}{2}^{2} = [0 0 \ddot{\vartheta}{1}+\ddot{\vartheta}{2}]^{\top} \
\ddot{p}^{2}{2} \text{含} a_{1}s_{2}\ddot{\vartheta}{1} - a{1}c_{2}\dot{\vartheta}{1}^{2} - a{2}(\dot{\vartheta}{1}+\dot{\vartheta}{2})^{2} + g s_{12} \text{等项} \
【\text{向内}】\text{连杆} 2\text{：} f_{2}^{2} = m_{2} \ddot{p}^{2}{C2} \
\tau{2} = [ Î_{2}zz + m_{2}(( \ell {C2}+a{2})^{2} + a_{1}( \ell {C2}+a{2})c_{2}) + k_{r}{2}I{m}{2} ] \ddot{\vartheta}{1} \

• [ Î_{2}zz + m_{2}( \ell {C2}+a{2})^{2} + k_{r}{2}^{2}I{m}{2} ] \ddot{\vartheta}{2} \
• m_{2}a_{1}( \ell {C2}+a{2})s_{2} \dot{\vartheta}{1}^{2} + m{2}( \ell {C2}+a{2}) g c_{12} \
  【\text{向内}】\text{连杆} 1\text{：} f_{1}^{1} = R^{1}{2} f{2}^{2} + m_{1} \ddot{p}^{1}{C1} \
  \tau{1} = [ Î_{1}zz + m_{1}( \ell {C1}+a{1})^{2} + k_{r}{1}^{2}I{m}{1} + Î{2}zz \
• m_{2}( a_{1}^{2} + ( \ell {C2}+a{2})^{2} + 2a_{1}( \ell {C2}+a{2})c_{2} ) ] \ddot{\vartheta}_{1} \
• [ Î_{2}zz + m_{2}(( \ell {C2}+a{2})^{2} + a_{1}( \ell {C2}+a{2})c_{2}) + k_{r}{2}I{m}{2} ] \ddot{\vartheta}{2} \

• 2m_{2}a_{1}( \ell {C2}+a{2})s_{2} \dot{\vartheta}{1}\dot{\vartheta}{2} - m_{2}a_{1}( \ell {C2}+a{2})s_{2} \dot{\vartheta}_{2}^{2} \

• [ m_{1}( \ell {C1}+a{1}) + m_{2}a_{1} ] g c_{1} + m_{2}( \ell {C2}+a{2}) g c_{12}
  \end{aligned}
  $$

提示

请对照上一讲的结果：把参数换成上一讲的记号（m₁ = m_ℓ₁+m_m₂，m₁ℓ_C1 = m_ℓ₁(ℓ₁−a₁)，Î₁ = I_ℓ₁+m_ℓ₁(ℓ₁−a₁)²+I_m₂，m₂ = m_ℓ₂，m₂ℓ_C2 = m_ℓ₂(ℓ₂−a₂)，Î₂ = I_ℓ₂+m_ℓ₂(ℓ₂−a₂)²），得到的就是式 (7.82)。

原文：可以验证"由此得到的动力学模型与用拉格朗日法推出来的完全一致"。两条完全不同的路，通向同一个模型——这就是本讲最值得记住的一件事。

还提示：μ 分量里标了 ∗ 的那些没有算，因为它们不影响 τ₁、τ₂。这是递推实现里常见的剪枝优化。

例 2：判断该用正动力学还是逆动力学

你要做的事	用哪个	理由
“我设计了一个 PD 控制器，先在计算机上看看机器人会不会抖”	正动力学	已知力矩 → 积分出运动，这就是仿真
“这条轨迹 0.5 s 跑完，电机力矩够不够？”	逆动力学	已知 q, q̇, q̈ → 算力矩，检查可行性
“要在控制器里把重力项提前抵消掉”	逆动力学	每个控制周期算出 g(q) 做前馈
“超力矩了，想把轨迹放慢一点”	逆动力学 + 动态缩放	先算出超限多少，再按 c² 关系拉伸时间

动手建议

1. 拿两连杆平面臂，用 6 行伪代码实现"向外递推 + 向内递推"，代入上一讲 Example 7.2 的数值（a₁=a₂=1 m，ℓ₁=ℓ₂=0.5 m，m_ℓ=50 kg，I_ℓ=10 kg·m²，k_r=100，m_m=5 kg，I_m=0.01 kg·m²），验证 θ̈ 系数与 b₁₁ 一致。
2. 把关节数从 2 改成 6，观察代码结构完全不变——这就是 O(n) 递推的可扩展性，也是它优于手推闭式的地方。

停下来想一想：想用牛顿-欧拉算法拼出惯性矩阵 B(q) 的第 i 列，应该把 q̈、q̇、g₀ 分别设成什么？

五、常见误区

5.1 牛顿-欧拉给出的是同一个模型

常见误区

误区 1：以为牛顿-欧拉给出的是"另一个不同的模型"。不是。它和拉格朗日法给出同一个模型，只是求解路径不同：一个从能量出发给闭式，一个从力平衡出发给递推算法。用两连杆平面臂明确验证了这一点。

5.2 重力去哪了：p̈₀ − g₀ 的技巧

常见误区

误区 2：忘了"p̈₀ − g₀"这个技巧，于是到处找 m_i g₀ 去哪了。重力没有消失，它被当作基座的加速度在向外递推时就传进了每个 p̈_Ci，所以向内递推的 f_i = f_{i+1} + m_i p̈_Ci 里不再出现 g₀。这是牛顿-欧拉实现里最常见的困惑点。

5.3 参数辨识估不出全部参数

常见误区

误区 3：以为参数辨识能估出所有 13n 个参数。不能。很多参数不可辨识（对任何轨迹都不出现在方程里，Y 的对应列全零，必须删掉），还有些只能以线性组合被估出来。的例子很极端：两连杆平面臂 22 个可能参数只剩 8 个，最简可到 5 个。

5.4 操作空间公式只在非冗余且满秩时成立

常见误区

误区 4：以为操作空间动力学 B_A = J⁻ᵀ B J⁻¹ 随处可用。它只在非冗余且 J_A 满秩时成立。冗余机械臂必须用 (7.130) 的 (J_A B⁻¹ J_Aᵀ)⁻¹ 形式，而且到奇异位形时矩阵不可逆，模型直接失效。

5.5 放慢轨迹降不了重力力矩

常见误区

误区 5：以为放慢轨迹能把重力力矩也降下来。不能。重力项g(q) 只依赖位置，与时间怎么走无关，完全不受时间缩放影响。动态缩放只对速度/加速度相关的那部分力矩有效（按因子平方变化）。因此提醒：超限不能是由纯重力造成的，否则缩放解决不了。

停下来想一想：把轨迹放慢 c 倍，与速度/加速度相关的力矩变成原来的多少？重力项 g(q) 会不会跟着变小？

六、本讲小结

停下来想一想：动态可操作度椭球的中心被重力平移到了哪个方向？这意味着末端向下加速和向上加速，哪个能力更强？

要点回顾

1. 牛顿-欧拉：牛顿方程（平动平衡）+ 欧拉方程（转动平衡）；向外递推传运动学量（ω, ω̇, p̈, p̈_C），向内递推传力与力矩（f, μ）并投影出 τ。每根杆只算一次 → 逆动力学O(n)、正动力学O(n²)，适合实时。
2. 实现要点：所有量写在连杆自身坐标系（Ī、r、z 都是常数）；基座加速度取p̈₀ − g₀，把重力吸收掉。
3. 正动力学= 已知力矩求运动（仿真，要积分）；逆动力学= 已知运动求力矩（规划可行性 + 控制前馈）。
4. 参数辨识：靠"对参数线性" τ = Y π，堆成 τ̄ = Ȳ π，用最小二乘 π = (ȲᵀȲ)⁻¹Ȳᵀτ̄；要处理不可辨识参数、线性组合、Ȳ 病态（阻尼最小二乘 / SVD），轨迹要"丰富"但不激发未建模柔性。
5. 动态缩放：τˢ 按时间缩放因子的平方变化，重力项不受影响。
6. 操作空间动力学：B_A ẍ_e + C_A ẋ_e + g_A = γ_A − h_A，其中 B_A = (J_A B⁻¹ J_Aᵀ)⁻¹；非冗余时 B_A = J_A⁻ᵀ B J_A⁻¹。
7. 动态可操作度椭球：(v̇_e + J B⁻¹g)ᵀ J†ᵀ BᵀB J† (v̇_e + J B⁻¹g) = 1；重力把椭球中心往下平移，所以向下加速能力更大。

一句话总结：拉格朗日管好看，牛顿-欧拉管快

一句话总结：拉格朗日法负责"好看"，牛顿-欧拉法负责"快"；模型有了但参数不准，就靠线性性质做最小二乘辨识；想让机器人听"末端的话"，就把模型搬到操作空间去。

逆动力学就是你已经学过的前馈补偿