Part II · 第 8 章动力学 · 8.1Part II · Ch. 8 Dynamics · 8.1

拉格朗日动力学Lagrangian Dynamics

运动学只回答"关节在这里，手爪在哪里"；动力学回答"关节出这么大力矩，机器人怎么加速"。要仿真一台机器人，或者让电机去跟踪轨迹，你都需要它的运动方程。用牛顿定律逐个连杆列受力，关节处的约束力会让方程迅速变得又长又乱；拉格朗日的办法只用两个标量——动能和势能，约束力从头到尾不出现。本节从"自然选择作用量驻定的路径"出发，在单摆、平面 2R 机械臂、倒立摆小车上推出同一个方程 $M(\mathbf q)\ddot{\mathbf q} + C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q} + \mathbf g(\mathbf q)=\boldsymbol\tau$，并看它如何驱动计算力矩控制。 Kinematics only answers "the joints are here, so where is the gripper?" Dynamics answers "the joints supply this torque, so how does the robot accelerate?" To simulate a robot, or to make motors track a trajectory, you need its equations of motion. Writing Newton's laws link by link drowns you in constraint forces at the joints; Lagrange's way uses two scalars, kinetic and potential energy, and the constraint forces never appear. Starting from "nature picks the path of stationary action", this lesson derives the same equation $M(\mathbf q)\ddot{\mathbf q} + C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q} + \mathbf g(\mathbf q)=\boldsymbol\tau$ on a pendulum, a planar 2R arm and a cart-pole, and then shows how it powers computed-torque control.

约 35 分钟~35 min 前置：2.5 常微分方程 · 3.2 梯度 · 3.4 机器人雅可比 · 7.1 自由度 · 7.3 正运动学Prereqs: 2.5 ODEs · 3.2 Gradient · 3.4 Robot Jacobian · 7.1 DoF · 7.3 Forward Kinematics 5 个交互5 interactives

1为什么机器人需要它Why robots need this

前面所有关于机械臂的内容都是"位置层面"的：关节角 $\mathbf q$ 给定，手爪在哪里（7.3），速度怎么传递（3.4），静止时力怎么传递（$\boldsymbol\tau=J^\top F$）。可电机给的是力矩，不是位置。只要你想做下面这些事，就必须知道"力矩 $\boldsymbol\tau$ 会产生什么加速度 $\ddot{\mathbf q}$"：

仿真：MuJoCo、Isaac、Drake、Pinocchio 每一个时间步都在算 $\ddot{\mathbf q} = M^{-1}(\boldsymbol\tau - C\dot{\mathbf q} - \mathbf g)$，再积分（2.5 节）。强化学习的训练环境本质上就是这个方程。
力矩控制：计算力矩、阻抗控制、重力补偿、人机共融的力控机械臂，全都用模型预测"需要多大力矩才能产生这个加速度"（逆动力学）。
系统辨识与故障诊断：从测得的 $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q},\boldsymbol\tau)$ 反推质量、质心、惯量。
轨迹优化与 MPC（Part IV）：把方程当作约束，决定"最省能量的路径"。

怎样得到这个方程？牛顿的办法是对每一块刚体写 $F=ma$，再把关节处的约束力一个一个消去，连杆一多就是一场灾难。拉格朗日的办法换了个视角：选好描述构型的独立变量（广义坐标，7.1 节），写出整个系统的动能 $T$ 和势能 $V$，然后做一次机械的求导。约束力不做功，所以它们根本不会出现。

Everything so far about arms was at the level of positions: given joint angles $\mathbf q$, where is the gripper (7.3), how do velocities map (3.4), how do static forces map ($\boldsymbol\tau=J^\top F$). But motors supply torque, not position. Whenever you want any of the following you must know "what acceleration $\ddot{\mathbf q}$ does torque $\boldsymbol\tau$ produce":

Simulation: MuJoCo, Isaac, Drake and Pinocchio compute $\ddot{\mathbf q} = M^{-1}(\boldsymbol\tau - C\dot{\mathbf q} - \mathbf g)$ at every step and integrate it (Lesson 2.5). A reinforcement-learning training environment is, at heart, this equation.
Torque control: computed torque, impedance control, gravity compensation and every force-controlled collaborative arm use the model to predict "what torque yields this acceleration" (inverse dynamics).
System identification and fault diagnosis: recover masses, centers of mass and inertias from measured $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q},\boldsymbol\tau)$.
Trajectory optimization and MPC (Part IV): the equation becomes a constraint that decides "the cheapest path".

How do we get this equation? Newton writes $F=ma$ for each rigid body and then eliminates the joint constraint forces one by one, a disaster once there are many links. Lagrange changes the viewpoint: pick independent variables that describe the configuration (generalized coordinates, Lesson 7.1), write the kinetic energy $T$ and potential energy $V$ of the whole system, then differentiate mechanically. Constraint forces do no work, so they simply never appear.

2直觉：自然是"懒"的Intuition: nature is "lazy"

把一个球向上抛，它在 $t=0$ 离手、$t=T$ 落回同一高度。球可以走无数条路径，比如先猛冲再慢慢爬，或者来回抖动，但它走的恰好是那条抛物线。能不能用一个全局的准则解释为什么？

定义拉格朗日量 $\mathcal L = T - V$（动能减势能），再定义整条路径的作用量 $S[q]=\int_0^T \mathcal L\,dt$。驻定作用量原理（哈密顿原理）说：真实运动是使 $S$ 驻定的路径，也就是在固定两端的所有路径中，往任何方向做一点微小扰动，$S$ 的一阶变化都为零。下图把真实路径加上一个扰动 $\varepsilon\sin(k\pi t/T)$（两端不动），画出 $S$ 随 $\varepsilon$ 的变化。

Throw a ball upward: it leaves the hand at $t=0$ and lands at the same height at $t=T$. It could take infinitely many routes, a violent dash then a slow climb, or a jittery wobble, yet it follows exactly the parabola. Is there a global criterion that explains why?

Define the Lagrangian $\mathcal L = T - V$ (kinetic minus potential energy) and the action of an entire path, $S[q]=\int_0^T \mathcal L\,dt$. The principle of stationary action (Hamilton's principle) says the true motion is the path that makes $S$ stationary: among all paths with fixed endpoints, a small perturbation in any direction changes $S$ by nothing at first order. The figure below adds a perturbation $\varepsilon\sin(k\pi t/T)$ (endpoints fixed) to the true path and plots $S$ against $\varepsilon$.

FIG 1作用量：真实路径是谷底The action: the true path sits at the bottom of the valley

路径 q(t)：实线真实，虚线被扰动PATH q(t): solid = true, dashed = perturbed

S(ε) − S(0)S(ε) − S(0)

左：横轴是时间，两端的圆点固定。拖动琥珀色手柄（或 ε 滑块）改变扰动幅度；右：每个 ε 对应的作用量增量，谷底恰在 ε=0。球：$m=1$、$T=1$ s、$q=$ 高度；摆：$m=l=1$、$T=0.8$ s、$q=\theta$，真实路径是用打靶法解出的两点边值问题。扰动模态 $k$ 越大，路径抖得越厉害，$S$ 增加越快（球：$\Delta S = m\pi^2k^2\varepsilon^2/4T$）。 Left: time on the horizontal axis, the dots at both ends are fixed. Drag the amber handle (or the ε slider) to change the perturbation; right: the action increase for each ε, with the valley exactly at ε=0. Ball: $m=1$, $T=1$ s, $q=$ height; pendulum: $m=l=1$, $T=0.8$ s, $q=\theta$, the true path solved as a two-point boundary-value problem by shooting. A larger mode $k$ wiggles the path harder and raises $S$ faster (ball: $\Delta S = m\pi^2k^2\varepsilon^2/4T$).

试一试Try this

把 ε 拉到 $|\varepsilon|>0.3$，再回到 $|\varepsilon|<0.03$：作用量最小的位置恰好是哪条路径？Pull ε out to $|\varepsilon|>0.3$, then back to $|\varepsilon|<0.03$: which path is exactly where the action is smallest?
选扰动模态 $k=3$，把 $|\varepsilon|$ 拉到 0.3 以上，再换到"单摆"重做。两种系统的谷底都在真实路径上吗？Choose perturbation mode $k=3$ with $|\varepsilon|\ge0.3$, then redo it with the "Pendulum". Is the valley at the true path in both systems?

核心洞见：欧拉–拉格朗日方程 = "一阶变化为零"Key insight: the Euler–Lagrange equation = "no first-order change"

谷底的切线是水平的：$\delta S = 0$。把 $\delta S$ 展开并做一次分部积分（5 节），它等于 $\int_0^T\big(\tfrac{\partial\mathcal L}{\partial q}-\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot q}\big)\eta(t)\,dt$，其中 $\eta$ 是任意的扰动形状。要对每一种 $\eta$ 都为零，括号里的东西必须在每个时刻都为零，这就是 $\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot q}-\tfrac{\partial\mathcal L}{\partial q}=0$。一个全局的"路径整体最优"，变成了一个局部的、每个时刻的微分方程。

The tangent at the valley floor is flat: $\delta S = 0$. Expand $\delta S$ and integrate by parts once (Section 5) and it equals $\int_0^T\big(\tfrac{\partial\mathcal L}{\partial q}-\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot q}\big)\eta(t)\,dt$, with $\eta$ an arbitrary perturbation shape. To vanish for every $\eta$, the bracket must vanish at every instant: $\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot q}-\tfrac{\partial\mathcal L}{\partial q}=0$. A global "the whole path is optimal" statement turns into a local, instant-by-instant differential equation.

注意"驻定"不总是"最小"：路径时间足够长时，$S$ 的谷底会变成鞍点（摆的 $T$ 超过半个周期时就是这样）。这里选的 $T$ 都足够短，所以看到的是真正的最小值。机器人学里我们只用到"一阶变化为零"这一点。

"Stationary" is not always "minimum": for long enough durations the valley turns into a saddle (as when the pendulum's $T$ exceeds half a period). The durations here are short, so you see a true minimum. In robotics only the "zero first-order change" part is used.

3动手玩：单摆，一步一步推出方程Play: the pendulum, deriving the equation step by step

最简单的机器人关节：质量 $m$ 的小球挂在长 $l$ 的轻杆端点，杆绕枢轴转动，角度 $\theta$ 从正下方量起（逆时针为正）。枢轴处有电机力矩 $\tau$ 和黏性摩擦 $b\dot\theta$。只有一个自由度 $\theta$，所以只有一条方程。照着右下角实时更新的四步走：

写动能和势能：$T=\tfrac12 ml^2\dot\theta^2$，$V=mgl(1-\cos\theta)$，$\mathcal L=T-V$。
求 $\partial\mathcal L/\partial\dot\theta = ml^2\dot\theta$（"广义动量"）。
对时间求导：$\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot\theta}=ml^2\ddot\theta$。
求 $\partial\mathcal L/\partial\theta=-mgl\sin\theta$，代入 $\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot\theta}-\tfrac{\partial\mathcal L}{\partial\theta}=\tau-b\dot\theta$：

The simplest robot joint: a ball of mass $m$ at the end of a light rod of length $l$, turning about a pivot, with $\theta$ measured from straight down (counter-clockwise positive). The pivot has a motor torque $\tau$ and viscous friction $b\dot\theta$. There is just one degree of freedom $\theta$, so one equation. Follow the four steps, updated live at the bottom right:

Write kinetic and potential energy: $T=\tfrac12 ml^2\dot\theta^2$, $V=mgl(1-\cos\theta)$, $\mathcal L=T-V$.
Take $\partial\mathcal L/\partial\dot\theta = ml^2\dot\theta$ (the "generalized momentum").
Differentiate in time: $\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot\theta}=ml^2\ddot\theta$.
Take $\partial\mathcal L/\partial\theta=-mgl\sin\theta$ and plug into $\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot\theta}-\tfrac{\partial\mathcal L}{\partial\theta}=\tau-b\dot\theta$:

$$ \cx{m l^2\,\ddot\theta} \;+\; \cv{m g l\,\sin\theta} \;=\; \cw{\tau} - b\dot\theta . $$

小角度时 $\sin\theta\approx\theta$（2.4 节），就是熟悉的弹簧振子。下图用 RK4 积分这条方程（2.5 节）。拖动小球设定初始角；右边是相图（$\theta$ 对 $\dot\theta$），虚线是分界线（刚好能摆到最高点的轨道）；底部的能量条显示 $T$、$V$ 和 $T+V$。切换到"牛顿视角"，看看同一个摆用牛顿的办法要多画什么。

For small angles $\sin\theta\approx\theta$ (Lesson 2.4) and this is the familiar spring-mass oscillator. The figure integrates the equation with RK4 (Lesson 2.5). Drag the ball to set the starting angle; on the right is the phase portrait ($\theta$ versus $\dot\theta$), the dashed curve is the separatrix (the orbit that just reaches the top); the energy bars at the bottom show $T$, $V$ and $T+V$. Switch to the "Newton view" to see what the same pendulum costs you in Newton's way.

FIG 2单摆：能量、相图与两种推导Pendulum: energy, phase portrait and two derivations

相图：θ（横）与 θ̇（纵）PHASE PORTRAIT: θ (horizontal), θ̇ (vertical)

$m=l=1$。拖动小球改变角度（拖动时暂停）。拉格朗日视角：右下角逐项代入当前数值，等号两边始终相等。牛顿视角：必须画出重力 $mg$（琥珀）、杆的张力 $T_c$（紫）和杆传给小球的切向力 $(\tau-b\dot\theta)/l$（青），沿径向、切向分别列式；张力是约束力，拉格朗日的推导里从未出现。能量条中 $T$、$V$ 来回交换，$T+V$ 在 $\tau=0$、$b=0$ 时不变。 $m=l=1$. Drag the ball to change the angle (the simulation pauses while dragging). Lagrange view: the bottom-right panel substitutes the current numbers term by term, and the two sides always agree. Newton view: you must draw gravity $mg$ (amber), the rod tension $T_c$ (purple) and the tangential force the rod passes to the ball, $(\tau-b\dot\theta)/l$ (cyan), then write radial and tangential equations; the tension is a constraint force, which never appeared in Lagrange's derivation. In the bars $T$ and $V$ trade back and forth while $T+V$ stays constant when $\tau=0$, $b=0$.

试一试Try this

点"复位"，打开阻尼（$b\ge0.5$），调力矩 $\tau$，让摆最后稳稳停在 45°（$|\theta-\pi/4|<0.03$，$|\dot\theta|<0.05$）。静止时 $\ddot\theta=0$，方程告诉你 $\tau$ 应该是多少？如果摆被推进了持续旋转的状态，把它拖回来（恒定力矩、小阻尼下，转动与静止两种稳定状态可以并存）。Press Reset, turn damping on ($b\ge0.5$) and tune $\tau$ so that the pendulum finally rests at 45° ($|\theta-\pi/4|<0.03$, $|\dot\theta|<0.05$). At rest $\ddot\theta=0$: what does the equation say $\tau$ must be? If the pendulum gets pushed into a continuous spin, drag it back (with a constant torque and small damping the spinning and the resting state can coexist).
把摆拉到 120° 以上松手，加上阻尼（$b\ge0.3$），等能量掉到初始值的 10% 以下。看相图里的轨迹怎样螺旋收缩。Pull the pendulum above 120° and release it with damping on ($b\ge0.3$) until the energy falls below 10% of its start. Watch the phase-plane trajectory spiral in.
在牛顿视角下，让杆的张力超过 25 N。它出现在摆的什么位置？为什么那里最大？In the Newton view make the rod tension exceed 25 N. Where in the swing does it occur, and why is it largest there?

4第二、第三视角：多连杆机械臂与欠驱动小车Second and third views: a multi-link arm and an underactuated cart

平面 2R 机械臂：质量矩阵、科氏力与重力The planar 2R arm: mass matrix, Coriolis terms and gravity

两根均匀杆：$m_1=2$ kg，$l_1=1$ m；$m_2=1.5$ kg，$l_2=0.8$ m；质心在杆中点，$l_{c}=l/2$，绕质心的转动惯量 $I=ml^2/12$。角度 $\theta_1$ 从水平的 $+x$ 轴量起，$\theta_2$ 是相对第一根杆的角度，重力沿 $-y$。两个关节力矩 $\tau_1,\tau_2$。把它们收成向量：$\mathbf q=(\theta_1,\theta_2)$，$\boldsymbol\tau=(\tau_1,\tau_2)$。同样的做法：写 $T$ 与 $V$，对 $\theta_1,\theta_2$ 各做一次欧拉–拉格朗日，结果整理成

Two uniform rods: $m_1=2$ kg, $l_1=1$ m; $m_2=1.5$ kg, $l_2=0.8$ m; the center of mass is at the midpoint, $l_{c}=l/2$, and the moment of inertia about the center of mass is $I=ml^2/12$. Angle $\theta_1$ is measured from the horizontal $+x$ axis, $\theta_2$ relative to link 1, and gravity points along $-y$. Two joint torques $\tau_1,\tau_2$. Collect them as vectors: $\mathbf q=(\theta_1,\theta_2)$, $\boldsymbol\tau=(\tau_1,\tau_2)$. The same recipe: write $T$ and $V$, apply Euler–Lagrange to $\theta_1$ and $\theta_2$, and arrange the result as

$$ \hl{M}{\cx{M(\mathbf q)}\,\ddot{\mathbf q}} \;+\; \hl{C}{\cx{C(\mathbf q,\dot{\mathbf q})\,\dot{\mathbf q}}} \;+\; \hl{g}{\cv{\mathbf g(\mathbf q)}} \;+\; b\dot{\mathbf q} \;=\; \hl{tau}{\cw{\boldsymbol\tau}} . $$

颜色表示每一项的来源：青色来自动能（质量矩阵 $M$ 和科氏/离心项 $C\dot{\mathbf q}$），琥珀色来自势能（重力 $\mathbf g$），紫色是外部输入（力矩）。把鼠标悬停在公式的各项上，图里会高亮对应的物理量。下图是完整的仿真器：给两个力矩，开关重力，选积分器和步长，右图是能量守恒的检验。

Colors show where each term comes from: cyan comes from kinetic energy (the mass matrix $M$ and the Coriolis/centrifugal term $C\dot{\mathbf q}$), amber from potential energy (gravity $\mathbf g$), purple is the external input (torque). Hover the terms of the formula and the figure highlights the matching physical quantity. The figure is a full simulator: set two torques, toggle gravity, choose the integrator and step size, and the right panel checks energy conservation.

FIG 32R 机械臂仿真器2R arm simulator

能量账本：E − E₀（琥珀）与外力做功 W（紫）ENERGY LEDGER: E − E₀ (amber) vs. work W (purple)

拖动肘部和末端的圆圈设定初始姿态（拖动时暂停）。公式里青色是 $M\ddot{\mathbf q}$ 与 $C\dot{\mathbf q}$，琥珀色是 $\mathbf g$，紫色是 $\boldsymbol\tau$。右图：功–能原理 $\dot E=\boldsymbol\tau^\top\dot{\mathbf q}-b\|\dot{\mathbf q}\|^2$ 的账本，琥珀色是 $E(t)-E(0)$，紫色是积分得到的外力功 $W(t)$，两条线之差就是数值积分误差，读数里显示。$\boldsymbol\tau=0$、$b=0$ 时 $E$ 应当恒定。 Drag the elbow and tip circles to set the initial posture (the simulation pauses while dragging). In the formula, cyan is $M\ddot{\mathbf q}$ and $C\dot{\mathbf q}$, amber is $\mathbf g$, purple is $\boldsymbol\tau$. Right: the ledger of the work–energy principle $\dot E=\boldsymbol\tau^\top\dot{\mathbf q}-b\|\dot{\mathbf q}\|^2$; amber is $E(t)-E(0)$, purple is the integrated work $W(t)$ of the external forces, and the gap between the two is the numerical integration error shown in the readout. With $\boldsymbol\tau=0$, $b=0$ the energy $E$ should stay constant.

试一试Try this

令 $\boldsymbol\tau=0$、$b=0$、开重力，选显式欧拉、$h=10$ ms，点"跑 10 秒"：能量误差超过 2 J。真实系统不会无缘无故多出能量，这是积分器造出来的。With $\boldsymbol\tau=0$, $b=0$, gravity on, pick explicit Euler, $h=10$ ms and press "Run 10 s": the energy error exceeds 2 J. A real system does not gain energy from nowhere; the integrator manufactured it.
换成 RK4，把 $h$ 降到 2 ms 以下，跑满 10 秒：能量误差小于 $10^{-4}$ J。Switch to RK4, lower $h$ to 2 ms or less and run a full 10 s: the energy error stays below $10^{-4}$ J.
拖一个姿态，开重力，点"τ = g(q) 保持"：手臂静止 3 秒不动。想一想：为什么保持不动所需的力矩只和 $\mathbf g(\mathbf q)$ 有关？（试试拖到水平伸直，$\tau_1$ 有多大？）Drag a posture, gravity on, press "Hold: τ = g(q)": the arm stays still for 3 s. Why does holding still depend only on $\mathbf g(\mathbf q)$? (Try fully stretched horizontal: how large is $\tau_1$?)

一个方程，三种读法One equation, three readings

惯性：$M(\mathbf q)$ 描述"推动这个姿态下的手臂有多费劲"，伸直时 $M_{11}$ 大（质量离轴远），折起时小；$M_{12}$ 说明加速关节 2 会反过来拽关节 1。速度耦合：$C\dot{\mathbf q}$ 在静止时为零，转得越快越大：离心项（$\dot\theta_i^2$）把手臂往外甩，科氏项（$\dot\theta_1\dot\theta_2$）让两个关节互相"拖拽"。它们不改变总能量：$C\dot{\mathbf q}$ 的功率 $\dot{\mathbf q}^\top C\dot{\mathbf q}$ 一般不为零，但恰好被 $M(\mathbf q)$ 变化带来的动能变化抵消（第 5 节性质 2：$\dot M-2C$ 反对称）。位置：$\mathbf g(\mathbf q)$ 只依赖姿态，是保持不动需要的力矩，完全不依赖速度。

Inertia: $M(\mathbf q)$ says "how hard is it to push the arm in this posture": $M_{11}$ is large when stretched (mass far from the axis) and small when folded; $M_{12}$ says accelerating joint 2 drags joint 1 back. Velocity coupling: $C\dot{\mathbf q}$ vanishes at rest and grows with speed: centrifugal terms ($\dot\theta_i^2$) fling the arm outward, Coriolis terms ($\dot\theta_1\dot\theta_2$) make the joints "drag" each other. They never change the total energy: their power $\dot{\mathbf q}^\top C\dot{\mathbf q}$ is generally nonzero, but it is exactly offset by the change of kinetic energy as $M(\mathbf q)$ changes (property 2 in Section 5: $\dot M-2C$ is skew). Position: $\mathbf g(\mathbf q)$ depends only on posture; it is the torque needed to hold still and does not depend on velocity at all.

倒立摆小车：欠驱动The cart-pole: underactuation

小车（质量 $M_c$）沿水平导轨滑动，摆杆（长 $l$，端点质量 $m$，角度 $\theta$ 从竖直向上量起）铰接在车上。两个自由度 $(x,\theta)$，但只有一个驱动：只能推小车，没有电机直接转动摆杆。这叫欠驱动（7.1 节的自由度 vs. 驱动数）。拉格朗日框架处理它毫无困难，只是右端的广义力 $\boldsymbol\tau=(F,0)$，第二项为零。下图把"能不能同时指定两个加速度"画了出来：右边平面的横轴是小车加速度 $\ddot x$，纵轴是摆杆角加速度 $\ddot\theta$，当前状态下，无论推多大的力 $F$，能到达的 $(\ddot x,\ddot\theta)$ 只落在一条直线上。

A cart (mass $M_c$) slides along a horizontal rail with a pole (length $l$, tip mass $m$, angle $\theta$ from straight up) hinged on it. There are two degrees of freedom $(x,\theta)$ but only one actuator: you can push the cart, but no motor turns the pole directly. This is underactuation (DoF versus actuators, Lesson 7.1). The Lagrangian framework handles it effortlessly; only the generalized force is $\boldsymbol\tau=(F,0)$, with a zero second entry. The figure draws "can we command both accelerations?": in the right-hand plane the horizontal axis is the cart acceleration $\ddot x$ and the vertical axis the pole angular acceleration $\ddot\theta$; at the current state, whatever force $F$ you push with, the reachable $(\ddot x,\ddot\theta)$ lies on a single line.

FIG 4倒立摆小车：一个驱动，两个自由度Cart-pole: one actuator, two DoF

F 变化时可达的 (ẍ, θ̈)REACHABLE (ẍ, θ̈) AS F VARIES

$M_c=1$ kg，$m=0.3$ kg，$l=0.8$ m。操作：用滑块给恒定的力，按住"◀ 推 / 推 ▶"按钮，或者点一下画面后用键盘 ← → 推小车，让摆杆保持竖直（绿色虚线内，±20°）。打开"自动平衡"，一个 LQR 反馈律（Part IV）接管。"踢一下"给摆杆一个冲量。左边小车碰到两端的挡板会停住。右图：直线的斜率固定为 $-\cos\theta/l$，因为方程之一是 $l\ddot\theta+\cos\theta\,\ddot x=g\sin\theta$，与 $F$ 无关；紫色菱形是"两个加速度都为零"。$\theta\neq0$ 时直线不经过它。 $M_c=1$ kg, $m=0.3$ kg, $l=0.8$ m. Controls: apply a constant force with the slider, hold the "◀ push / push ▶" buttons, or click the scene and use the keyboard ← → to push the cart, trying to keep the pole upright (inside the green dashed ±20° band). Turn "Auto-balance" on and an LQR feedback law (Part IV) takes over. "Kick" gives the pole an impulse. The cart stops against the end stops. Right: the line's slope is fixed at $-\cos\theta/l$, because one of the equations is $l\ddot\theta+\cos\theta\,\ddot x=g\sin\theta$, independent of $F$; the purple diamond is "both accelerations zero". For $\theta\neq0$ the line misses it.

试一试Try this

用力滑块（"自动平衡"关闭）依次推到 $F\ge15$ N 和 $F\le-15$ N，观察右图中的点：它只能沿直线滑动。能否让点同时回到紫色菱形？With auto-balance off, push the force slider to $F\ge15$ N and then to $F\le-15$ N and watch the dot on the right: it only slides along the line. Can you bring it onto the purple diamond?
让摆杆保持在 ±20° 内连续 5 秒（手动或者用"自动平衡"）。手动不成功？开 0.3× 慢动作再试。Keep the pole within ±20° for 5 s in a row (by hand or with "Auto-balance"). No luck by hand? Try the 0.3× slow motion.

注意：不是所有加速度都能指定，但不代表系统"不可控"。平衡一根倒立摆需要"让车跑到摆杆下面去"，而想把小车拉回中间，必须先把摆杆往中间倾，也就是先把车推离开中间（试试自动平衡：LQR 对小车位置 $x$ 的反馈增益符号看上去是"反的"）。欠驱动系统的控制要利用 $M$ 的结构，不能直接"选加速度再求力矩"。步行机器人的浮动基座正是这样的欠驱动系统（9.2 节）。

Note: not every acceleration can be commanded, yet the system is not "uncontrollable". Balancing means "get the cart under the pole", and to pull the cart back to the middle you must first tilt the pole toward the middle, i.e. first push the cart away from the middle (try auto-balance: the LQR feedback gain on the cart position $x$ has what looks like the "wrong" sign). Controlling an underactuated system exploits the structure of $M$; you cannot simply "pick accelerations and solve for torques". A legged robot's floating base is exactly such an underactuated system (Lesson 9.2).

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上（$M$、$C\dot{\mathbf q}$、$\mathbf g$、$\boldsymbol\tau$、$T$、$V$），FIG 3 中对应的物理量会被高亮。Tip: hover the highlighted symbols in the formulas ($M$, $C\dot{\mathbf q}$, $\mathbf g$, $\boldsymbol\tau$, $T$, $V$); the matching physical quantity in FIG 3 lights up.

广义坐标与动能Generalized coordinates and kinetic energy

取 $n$ 个独立变量 $\mathbf q=(q_1,\dots,q_n)$ 描述整个机器人的构型（7.1 节的自由度个数，7.2 节的构型空间），这就是广义坐标。关节角、滑动关节的位移都可以做广义坐标。用了它们，刚性连接和铰链带来的约束就已经"吃掉"了：不再有约束力要处理。

先看一块刚体。设质心速度为 $v_c$，角速度为 $\omega$，绕质心的转动惯量为 $I$（平面情形，三维时是惯量张量，$\tfrac12\omega^\top I_c\,\omega$）。把刚体分成质点 $dm$，每个质点的速度是 $v=v_c+\omega\times r$（$r$ 是相对质心的位置）：

Take $n$ independent variables $\mathbf q=(q_1,\dots,q_n)$ describing the whole robot's configuration (the number of degrees of freedom of Lesson 7.1, the configuration space of Lesson 7.2): these are the generalized coordinates. Joint angles and prismatic displacements qualify. With them, the constraints of rigid links and hinges are already "used up": there are no constraint forces left to handle.

First, one rigid body. Let its center-of-mass velocity be $v_c$, its angular velocity $\omega$, and its moment of inertia about the center of mass $I$ (planar case; in 3D it is the inertia tensor and the term is $\tfrac12\omega^\top I_c\,\omega$). Cut the body into mass elements $dm$, each moving with $v=v_c+\omega\times r$ ($r$ measured from the center of mass):

$$ T_{\text{link}}=\int\tfrac12\|v_c+\omega\times r\|^2dm = \tfrac12 m\|v_c\|^2 + v_c\cdot\Big(\omega\times\underbrace{\int r\,dm}_{=\,0}\Big) + \tfrac12\underbrace{\int\|r\|^2dm}_{I}\,\omega^2 = \tfrac12 m\|v_c\|^2+\tfrac12 I\omega^2 . $$

中间项为零，因为 $r$ 是从质心量起的（$\int r\,dm=0$）。这就是"平动动能 + 转动动能"，也是常见的第一个坑：忘记转动惯量项。每个连杆的 $v_c$ 和 $\omega$ 都是 $\dot{\mathbf q}$ 的线性函数，系数正是 3.4 节的雅可比（质心的雅可比，也是"杠杆臂转 90°"）：$v_{ci}=J_{vi}(\mathbf q)\dot{\mathbf q}$，$\omega_i=J_{\omega i}(\mathbf q)\dot{\mathbf q}$。把所有连杆加起来：

The middle term vanishes because $r$ is measured from the center of mass ($\int r\,dm=0$). That is "translational plus rotational kinetic energy", and the first common trap: forgetting the rotational inertia term. For each link both $v_c$ and $\omega$ are linear in $\dot{\mathbf q}$, with the Jacobians of Lesson 3.4 as coefficients (the Jacobian of the center of mass: "lever arm rotated by 90°"): $v_{ci}=J_{vi}(\mathbf q)\dot{\mathbf q}$, $\omega_i=J_{\omega i}(\mathbf q)\dot{\mathbf q}$. Add up all links:

$$ \hl{T}{T} = \tfrac12\,\dot{\mathbf q}^\top M(\mathbf q)\,\dot{\mathbf q},\qquad M(\mathbf q)=\sum_{i=1}^n\Big(m_i\,J_{vi}^\top J_{vi}+J_{\omega i}^\top I_i\,J_{\omega i}\Big),\qquad \hl{V}{V}=\sum_i m_i\,g\,h_i(\mathbf q). $$

$T$ 是 $\dot{\mathbf q}$ 的二次型（3.5 节），$M=\partial^2T/\partial\dot{\mathbf q}^2$ 正是它的 Hessian，称为质量矩阵（惯量矩阵）。因为对任何 $\dot{\mathbf q}\ne0$ 都有 $T>0$，$M$ 是对称正定的，永远可逆，这是"给力矩就能解出加速度"的根本原因。$V$ 只依赖 $q$，$h_i$ 是质心的高度。

$T$ is a quadratic form in $\dot{\mathbf q}$ (Lesson 3.5), and $M=\partial^2T/\partial\dot{\mathbf q}^2$ is exactly its Hessian: the mass matrix (inertia matrix). Since $T>0$ for every $\dot{\mathbf q}\ne0$, $M$ is symmetric positive definite and always invertible, which is why "give a torque, get an acceleration" has a unique answer. $V$ depends on $q$ only; $h_i$ is the height of the center of mass.

欧拉–拉格朗日方程与广义力The Euler–Lagrange equation and generalized forces

设 $\mathcal L=T-V$，真实运动使 $S=\int\mathcal L\,dt$ 驻定。取一个满足 $\delta\mathbf q(0)=\delta\mathbf q(T)=0$ 的变分 $\delta\mathbf q(t)$（FIG 1 中的扰动 $\eta$），一阶变化为

Let $\mathcal L=T-V$ and let the true motion make $S=\int\mathcal L\,dt$ stationary. Take a variation $\delta\mathbf q(t)$ with $\delta\mathbf q(0)=\delta\mathbf q(T)=0$ (the perturbation $\eta$ of FIG 1); the first-order change is

$$ \delta S=\int_0^T\Big(\frac{\partial\mathcal L}{\partial \mathbf q}\,\delta\mathbf q+\frac{\partial\mathcal L}{\partial\dot{\mathbf q}}\,\delta\dot{\mathbf q}\Big)dt=\underbrace{\Big[\frac{\partial\mathcal L}{\partial\dot{\mathbf q}}\delta\mathbf q\Big]_0^T}_{=\,0}+\int_0^T\Big(\frac{\partial\mathcal L}{\partial \mathbf q}-\frac{d}{dt}\frac{\partial\mathcal L}{\partial\dot{\mathbf q}}\Big)\delta\mathbf q\,dt . $$

对任意 $\delta\mathbf q$ 都为零，被积函数必须恒为零。若还有不来自势能的力（电机力矩、摩擦、手爪受的外力），用虚功原理：它们在虚位移 $\delta\mathbf q$ 上做的功是 $Q^\top\delta\mathbf q$，其中 $Q_i=\sum_k F_k\cdot\partial r_k/\partial q_i$ 称为广义力。关节力矩直接就是 $Q_i=\tau_i$；手爪上的力 $F$ 的广义力就是 $J^\top F$，正是 3.4 节的力–力矩对偶。于是

For this to vanish for every $\delta\mathbf q$, the integrand must vanish identically. If there are forces not derived from a potential (motor torques, friction, an external force on the gripper), use virtual work: they do work $Q^\top\delta\mathbf q$ on a virtual displacement $\delta\mathbf q$, where $Q_i=\sum_k F_k\cdot\partial r_k/\partial q_i$ is the generalized force. A joint torque is directly $Q_i=\tau_i$; a force $F$ at the gripper has generalized force $J^\top F$, exactly the force–torque duality of Lesson 3.4. Hence

$$ \frac{d}{dt}\frac{\partial\mathcal L}{\partial\dot q_i}-\frac{\partial\mathcal L}{\partial q_i}=Q_i,\qquad i=1,\dots,n,\qquad Q=\hl{tau}{\boldsymbol\tau}-b\dot{\mathbf q}+J^\top F_{\text{ext}} . $$

从欧拉–拉格朗日到操作臂方程From Euler–Lagrange to the manipulator equation

把 $T=\tfrac12\dot{\mathbf q}^\top M(\mathbf q)\dot{\mathbf q}$ 代入。每一项都是一步机械的求导：

Substitute $T=\tfrac12\dot{\mathbf q}^\top M(\mathbf q)\dot{\mathbf q}$. Every term is one mechanical differentiation:

$$ \frac{\partial\mathcal L}{\partial\dot{\mathbf q}}=M\dot{\mathbf q},\qquad \frac{d}{dt}\frac{\partial\mathcal L}{\partial\dot{\mathbf q}}=M\ddot{\mathbf q}+\dot M\,\dot{\mathbf q},\qquad \frac{\partial\mathcal L}{\partial q_i}=\tfrac12\dot{\mathbf q}^\top\frac{\partial M}{\partial q_i}\dot{\mathbf q}-\frac{\partial V}{\partial q_i}. $$

注意第二项的 $\dot M\dot{\mathbf q}$：因为 $M$ 依赖 $\mathbf q$，对时间求导时必须多出这一项（常见错误之一）。它和 $\tfrac12\dot{\mathbf q}^\top\partial_{q_i}M\,\dot{\mathbf q}$ 合并，得到 $C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}$；分量形式是 Christoffel 符号：

Note the $\dot M\dot{\mathbf q}$ in the second line: because $M$ depends on $\mathbf q$, differentiating in time produces this extra term (a classic mistake to drop it). It combines with $\tfrac12\dot{\mathbf q}^\top\partial_{q_i}M\,\dot{\mathbf q}$ into $C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}$; in components these are the Christoffel symbols:

$$ \underbrace{M(\mathbf q)\ddot{\mathbf q}}_{\text{inertia}}+\underbrace{C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}}_{\text{Coriolis / centrifugal}}+\underbrace{\mathbf g(\mathbf q)}_{\partial V/\partial \mathbf q}=\boldsymbol\tau-b\dot{\mathbf q},\qquad C_{ij}=\sum_k c_{ijk}\dot q_k,\quad c_{ijk}=\tfrac12\Big(\frac{\partial M_{ij}}{\partial q_k}+\frac{\partial M_{ik}}{\partial q_j}-\frac{\partial M_{jk}}{\partial q_i}\Big). $$

平面 2R 的显式形式The explicit planar 2R

第二根杆的质心速度为 $v_{c2}=l_1\dot\theta_1(-s_1,c_1)+l_{c2}(\dot\theta_1+\dot\theta_2)(-s_{12},c_{12})$，两项的方向夹角正是 $\theta_2$，所以 $\|v_{c2}\|^2=l_1^2\dot\theta_1^2+l_{c2}^2(\dot\theta_1+\dot\theta_2)^2+2l_1l_{c2}\cos\theta_2\,\dot\theta_1(\dot\theta_1+\dot\theta_2)$。加上两根杆的转动动能 $\tfrac12I_1\dot\theta_1^2+\tfrac12I_2(\dot\theta_1+\dot\theta_2)^2$，对 $\dot\theta$ 求 Hessian 得到

Link 2's center-of-mass velocity is $v_{c2}=l_1\dot\theta_1(-s_1,c_1)+l_{c2}(\dot\theta_1+\dot\theta_2)(-s_{12},c_{12})$; the angle between the two directions is exactly $\theta_2$, so $\|v_{c2}\|^2=l_1^2\dot\theta_1^2+l_{c2}^2(\dot\theta_1+\dot\theta_2)^2+2l_1l_{c2}\cos\theta_2\,\dot\theta_1(\dot\theta_1+\dot\theta_2)$. Add the rotational energies $\tfrac12I_1\dot\theta_1^2+\tfrac12I_2(\dot\theta_1+\dot\theta_2)^2$ and take the Hessian in $\dot\theta$:

$$\begin{aligned} \cx{M_{11}}&=m_1l_{c1}^2+I_1+m_2\big(l_1^2+l_{c2}^2+2l_1l_{c2}\cos\theta_2\big)+I_2, & \cx{M_{12}}&=m_2\big(l_{c2}^2+l_1l_{c2}\cos\theta_2\big)+I_2, \\ \cx{M_{22}}&=m_2l_{c2}^2+I_2, & h&=m_2l_1l_{c2}\sin\theta_2, \end{aligned}$$ $$ \cx{C\dot{\mathbf q}}=\begin{bmatrix}-h\dot\theta_2 & -h(\dot\theta_1+\dot\theta_2)\\ h\dot\theta_1 & 0\end{bmatrix}\!\begin{bmatrix}\dot\theta_1\\\dot\theta_2\end{bmatrix}=\begin{bmatrix}-h\,(2\dot\theta_1\dot\theta_2+\dot\theta_2^2)\\ h\,\dot\theta_1^2\end{bmatrix},\qquad \cv{\mathbf g}=\begin{bmatrix}(m_1l_{c1}+m_2l_1)\,g\cos\theta_1+m_2l_{c2}\,g\cos(\theta_1+\theta_2)\\ m_2l_{c2}\,g\cos(\theta_1+\theta_2)\end{bmatrix}. $$

这些公式已经用暴力数值微分的欧拉–拉格朗日做过核对（随机姿态、速度、力矩，加速度吻合到 $10^{-5}$ 量级，即有限差分的精度）。FIG 3 的读数里用的就是它们。几点观察：$M_{12}\ne0$ 且依赖 $\theta_2$，所以两关节的惯性耦合；$g_1$ 里的 $\cos$ 来自"$\theta$ 从水平量起"，水平伸直时重力力臂最大；$C$ 的所有元素都含 $h\propto\sin\theta_2$，臂完全伸直或折回（$\theta_2=0,\pi$）时科氏力消失。

These formulas were checked against a brute-force numerical Euler–Lagrange (random postures, velocities and torques; accelerations agree to about $10^{-5}$, the accuracy of finite differences). FIG 3's readouts use them. Observations: $M_{12}\ne0$ and depends on $\theta_2$, so the two joints are inertially coupled; the $\cos$ in $g_1$ comes from "$\theta$ measured from the horizontal", the gravity lever arm being largest when stretched out horizontally; every entry of $C$ contains $h\propto\sin\theta_2$, so Coriolis forces vanish when the arm is fully stretched or folded ($\theta_2=0,\pi$).

三个重要性质Three important properties

$M$ 对称正定，所以正动力学 $\ddot{\mathbf q}=M^{-1}(\boldsymbol\tau-C\dot{\mathbf q}-\mathbf g-b\dot{\mathbf q})$ 总有唯一解。
$\dot M-2C$ 是反对称的（$C$ 取 Christoffel 形式时）。于是 $\dot{\mathbf q}^\top(\dot M-2C)\dot{\mathbf q}=0$，对 $T$ 求导得 $\dot T=\dot{\mathbf q}^\top(\boldsymbol\tau-\mathbf g-b\dot{\mathbf q})$；再加上 $\dot V=\dot{\mathbf q}^\top\mathbf g$，总能量 $E=T+V$ 满足 $\dot E=\dot{\mathbf q}^\top\boldsymbol\tau-b\|\dot{\mathbf q}\|^2$：能量的变化等于外力做的功。这是 FIG 3 右图的账本，也是基于无源性的控制证明稳定性的出发点。我们数值验证了 $\dot M-2C$ 的反对称（残差 $\sim10^{-10}$）。注意只有 $C\dot{\mathbf q}$ 这个乘积是唯一的，$C$ 本身不唯一，反对称性依赖于 Christoffel 的选择。
对动力学参数线性：存在回归矩阵 $Y(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})$，使 $\boldsymbol\tau=Y\,\pi$，$\pi$ 是质量、一阶矩、惯量等组成的参数向量。这是系统辨识的基础：用足够多的测量点做最小二乘（1.13 节）。

$M$ is symmetric positive definite, so forward dynamics $\ddot{\mathbf q}=M^{-1}(\boldsymbol\tau-C\dot{\mathbf q}-\mathbf g-b\dot{\mathbf q})$ always has a unique solution.
$\dot M-2C$ is skew-symmetric (with $C$ in Christoffel form). So $\dot{\mathbf q}^\top(\dot M-2C)\dot{\mathbf q}=0$, and differentiating $T$ gives $\dot T=\dot{\mathbf q}^\top(\boldsymbol\tau-\mathbf g-b\dot{\mathbf q})$; adding $\dot V=\dot{\mathbf q}^\top\mathbf g$, the total energy $E=T+V$ obeys $\dot E=\dot{\mathbf q}^\top\boldsymbol\tau-b\|\dot{\mathbf q}\|^2$: the change of energy equals the work of the external forces. That is the ledger of FIG 3's right panel and the starting point of passivity-based stability proofs. We verified numerically that $\dot M-2C$ is skew (residual $\sim10^{-10}$). Note that only the product $C\dot{\mathbf q}$ is unique; $C$ itself is not, and skew-symmetry depends on the Christoffel choice.
Linear in the dynamic parameters: there is a regressor $Y(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})$ with $\boldsymbol\tau=Y\,\pi$, where $\pi$ stacks masses, first moments and inertias. This is the basis of system identification: least squares (Lesson 1.13) over many measurements.

正动力学与逆动力学Forward and inverse dynamics

$$ \underbrace{\ddot{\mathbf q}=M(\mathbf q)^{-1}\big(\boldsymbol\tau-C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}-\mathbf g(\mathbf q)\big)}_{\text{forward dynamics: simulation}},\qquad \underbrace{\boldsymbol\tau=M(\mathbf q)\,\ddot{\mathbf q}_{d}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)}_{\text{inverse dynamics: control, identification}} . $$

正动力学"给力矩求加速度"，用于仿真（再积分，2.5 节）；逆动力学"给想要的加速度求力矩"，用于控制。FIG 3 的 $\boldsymbol\tau=\mathbf g(\mathbf q)$ 保持按钮，就是 $\ddot{\mathbf q}_d=0,\dot{\mathbf q}=0$ 的逆动力学。对 $n\ge3$ 的机械臂，手推符号式的拉格朗日表达式爆炸式地变长（每多一个自由度，项数成倍增加），实用的算法是 8.2 节的牛顿–欧拉递推（逆动力学，$O(n)$）和 8.3 节的质量矩阵算法；数值上 $M$ 常用复合刚体算法（CRBA）直接构造。

Forward dynamics "torque in, acceleration out" serves simulation (then integrate, Lesson 2.5); inverse dynamics "desired acceleration in, torque out" serves control. FIG 3's "Hold: τ = g(q)" button is inverse dynamics with $\ddot{\mathbf q}_d=0$, $\dot{\mathbf q}=0$. For arms with $n\ge3$, hand-deriving symbolic Lagrangian expressions explodes in length (the number of terms grows multiplicatively with each added DoF); the practical algorithms are the recursive Newton–Euler method of Lesson 8.2 (inverse dynamics, $O(n)$) and the mass-matrix algorithms of Lesson 8.3; numerically $M$ is typically built with the composite-rigid-body algorithm (CRBA).

倒立摆小车的方程The cart-pole equations

广义坐标 $\mathbf q=(x,\theta)$，$\theta$ 从竖直向上量起。摆杆端点的位置是 $(x+l\sin\theta,\ l\cos\theta)$。

Generalized coordinates $\mathbf q=(x,\theta)$, with $\theta$ from straight up. The pole tip is at $(x+l\sin\theta,\ l\cos\theta)$.

$$ T=\tfrac12(M_c+m)\dot x^2+ml\cos\theta\,\dot x\dot\theta+\tfrac12ml^2\dot\theta^2,\quad V=mgl\cos\theta;\qquad \begin{aligned}(M_c+m)\ddot x+ml\cos\theta\,\ddot\theta-ml\sin\theta\,\dot\theta^2&=F,\\ ml\cos\theta\,\ddot x+ml^2\ddot\theta-mgl\sin\theta&=0.\end{aligned} $$

写成矩阵形式 $M(\mathbf q)\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=B\,u$，输入矩阵 $B=(1,0)^\top$。若 $B$ 是 $n\times n$ 可逆矩阵，系统是全驱动，任何 $\ddot{\mathbf q}$ 都能通过逆动力学实现（FIG 3 的 2R 机械臂）；这里 $B$ 的秩是 1，小于 $n=2$，所以欠驱动。第二个方程里没有 $F$，它就是 FIG 4 右图直线的来源。

In matrix form $M(\mathbf q)\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=B\,u$, with input matrix $B=(1,0)^\top$. If $B$ is an invertible $n\times n$ matrix the system is fully actuated and any $\ddot{\mathbf q}$ can be realized through inverse dynamics (the 2R arm of FIG 3); here $B$ has rank 1, less than $n=2$: underactuated. The second equation contains no $F$; it is the origin of the line in FIG 4's right panel.

指向别处：哈密顿量与约束Pointers: the Hamiltonian and constraints

哈密顿形式：令广义动量 $p=\partial\mathcal L/\partial\dot{\mathbf q}=M\dot{\mathbf q}$，哈密顿量 $H=\tfrac12p^\top M^{-1}p+V(\mathbf q)$（能量），运动方程变成一阶的 $\dot{\mathbf q}=\partial H/\partial p$，$\dot p=-\partial H/\partial \mathbf q+Q$。它是辛积分器、端口哈密顿控制的出发点，但对机器人入门，二阶形式更直接。

约束：闭链机构（并联机器人、四连杆）和接触（脚踩地面）带有约束 $\phi(\mathbf q)=0$。若保留冗余坐标，约束力又回来了：$M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau+A(\mathbf q)^\top\lambda$，$A=\partial\phi/\partial\mathbf q$，拉格朗日乘子 $\lambda$ 就是约束力，由 $A\ddot{\mathbf q}+\dot A\dot{\mathbf q}=0$ 一并解出。8.4 节的接触就是这个套路，并加上"不等式"（只能推，不能拉）。所以"约束力从不出现"精确地说是：选了满足约束的独立坐标后。

Hamiltonian form: let the generalized momentum be $p=\partial\mathcal L/\partial\dot{\mathbf q}=M\dot{\mathbf q}$ and the Hamiltonian $H=\tfrac12p^\top M^{-1}p+V(\mathbf q)$ (the energy); the equations of motion become first-order, $\dot{\mathbf q}=\partial H/\partial p$, $\dot p=-\partial H/\partial \mathbf q+Q$. It is the starting point for symplectic integrators and port-Hamiltonian control, but for a first course the second-order form is more direct.

Constraints: closed kinematic chains (parallel robots, four-bar linkages) and contacts (a foot on the ground) carry constraints $\phi(\mathbf q)=0$. If you keep redundant coordinates the constraint forces return: $M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau+A(\mathbf q)^\top\lambda$ with $A=\partial\phi/\partial\mathbf q$; the Lagrange multiplier $\lambda$ is the constraint force, solved together with $A\ddot{\mathbf q}+\dot A\dot{\mathbf q}=0$. Contact in Lesson 8.4 follows this pattern plus "inequalities" (push only, never pull). So "constraint forces never appear" is precise only after choosing independent coordinates that satisfy the constraints.

6机器人中的它：计算力矩控制In robotics: computed-torque control

让 2R 机械臂跟踪一条关节轨迹 $\mathbf q_d(t)$。最朴素的是对每个关节做 PD：$\boldsymbol\tau=K_p e+K_d\dot e$（$e=\mathbf q_d-\mathbf q$），可手臂自重要吃掉一部分力矩，快速运动时惯性耦合和科氏力又会引起误差。用动力学模型可以逐级改进：

PD + 重力补偿：$\boldsymbol\tau=K_pe+K_d\dot e+\hat{\mathbf g}(\mathbf q)$，抵消静态的重力。
计算力矩（又叫反馈线性化）：$\boldsymbol\tau=\hat M(\mathbf q)\big(\ddot{\mathbf q}_d+K_d\dot e+K_pe\big)+\hat C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\hat{\mathbf g}(\mathbf q)$。模型完美时，把它代入 $M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau$，得到 $\ddot e+K_d\dot e+K_pe=0$：误差满足一条与姿态无关的线性方程，非线性被逆动力学抵消了。

下面三条控制器同时跟踪同一条轨迹（紫色虚线是目标）。控制律以 500 Hz 运行（零阶保持）。"模型误差"让控制器使用的质量与真实值差 $\pm$ 若干个百分点，"未建模负载"是手爪抓了一个控制器不知道的物体（质量加在末端）。

Make the 2R arm track a joint trajectory $\mathbf q_d(t)$. The simplest approach is a per-joint PD, $\boldsymbol\tau=K_pe+K_d\dot e$ ($e=\mathbf q_d-\mathbf q$), but part of the torque is eaten by the arm's own weight, and during fast motion inertial coupling and Coriolis forces add error. The dynamic model improves this step by step:

PD + gravity compensation: $\boldsymbol\tau=K_pe+K_d\dot e+\hat{\mathbf g}(\mathbf q)$, canceling the static gravity.
Computed torque (a.k.a. feedback linearization): $\boldsymbol\tau=\hat M(\mathbf q)\big(\ddot{\mathbf q}_d+K_d\dot e+K_pe\big)+\hat C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\hat{\mathbf g}(\mathbf q)$. With a perfect model, substitute into $M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau$ and obtain $\ddot e+K_d\dot e+K_pe=0$: the error obeys a posture-independent linear equation; the nonlinearity has been cancelled by inverse dynamics.

Three controllers track the same trajectory at once (the purple dashed arm is the target). The control law runs at 500 Hz (zero-order hold). "Model error" makes the masses the controller uses differ from the true ones by some percent; "unmodeled payload" is an object in the gripper that the controller does not know about (mass added at the tip).

FIG 5逆动力学做控制：PD、PD+重力、计算力矩Inverse dynamics as control: PD, PD + gravity, computed torque

跟踪误差 ‖q_d − q‖（对数坐标）TRACKING ERROR ‖q_d − q‖ (log scale)

红：PD（$K_p=150$，$K_d=20$）；青：PD + 重力补偿；绿：计算力矩（$K_p=150$，$K_d=20$，作用在加速度上）。目标 $\mathbf q_d(t)$ 是两个关节的正弦（频率 2.2、3.0 rad/s）。读数是最近约 2 秒的均方根误差（弧度）。改变滑块会让仿真重新开始。 Red: PD ($K_p=150$, $K_d=20$); cyan: PD + gravity compensation; green: computed torque ($K_p=150$, $K_d=20$, acting on accelerations). The target $\mathbf q_d(t)$ is a sinusoid per joint (2.2 and 3.0 rad/s). The readouts are the RMS error over roughly the last 2 s (radians). Moving a slider restarts the simulation.

试一试Try this

给手爪加上 $\ge1$ kg 的未建模负载，等 5 秒以上：计算力矩的均方根误差退化到 0.2 rad 以上。完美模型下它曾比 PD 好两个数量级。Add an unmodeled payload of $\ge1$ kg and wait over 5 s: the computed-torque RMS error degrades above 0.2 rad. With a perfect model it was two orders of magnitude better than PD.
调"模型误差"和"负载"，找到一个计算力矩比 PD+重力补偿更差的设置（均方根误差大 20% 以上，等 5 秒）。提示：试试负载 2 kg 配模型误差 $-30\%$。Tune "model error" and "payload" until you find a setting where computed torque is worse than PD + gravity (RMS error larger by 20%+, wait 5 s). Hint: try 2 kg payload with $-30\%$ model error.

真实系统里In real systems

仿真器：MuJoCo 和 Drake 用广义坐标（及约束）建模，每步算质量矩阵（MuJoCo 中是 CRB 类算法）并求解正动力学；Isaac Sim / PhysX 的 articulation 同样是 reduced-coordinate 的 Featherstone 类算法；Pinocchio 提供 RNEA（逆动力学）、CRBA（质量矩阵）、ABA（正动力学）及其解析导数，是轨迹优化和 MPC 的常用后端。控制：Franka Emika、KUKA LBR 等力矩控制机械臂内置重力与惯量补偿，所谓"手拖示教"就是把 $\boldsymbol\tau=\mathbf g(\mathbf q)$（加摩擦补偿）施加到关节上；计算力矩、阻抗控制（Part IV）、全身控制里的 $M$ 与偏置项 $\mathbf b=C\dot{\mathbf q}+\mathbf g$（与阻尼系数 $b$ 无关）都来自这个方程。辨识：沿激励轨迹采集 $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q},\boldsymbol\tau)$，用回归形式 $\boldsymbol\tau=Y\pi$ 最小二乘求惯性参数。为什么模型误差重要：实机上负载、电缆、减速器摩擦、关节柔性都不在刚体模型里，FIG 5 里的未建模负载就是缩影；工程上靠高增益反馈、自适应或鲁棒控制、学习残差模型来补。

Simulators: MuJoCo and Drake model in generalized coordinates (plus constraints), build the mass matrix every step (MuJoCo with a composite-rigid-body type algorithm) and solve forward dynamics; Isaac Sim / PhysX articulations also use reduced-coordinate, Featherstone-type algorithms; Pinocchio provides RNEA (inverse dynamics), CRBA (mass matrix), ABA (forward dynamics) and their analytic derivatives, a common backend for trajectory optimization and MPC. Control: torque-controlled arms such as the Franka Emika and KUKA LBR ship with built-in gravity and inertia compensation, and "hand guiding" is essentially applying $\boldsymbol\tau=\mathbf g(\mathbf q)$ (plus friction compensation) at the joints; computed torque, impedance control (Part IV) and whole-body control all take $M$ and the bias term $\mathbf b=C\dot{\mathbf q}+\mathbf g$ (not the damping coefficient $b$) from this equation. Identification: collect $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q},\boldsymbol\tau)$ along an exciting trajectory and solve the inertial parameters by least squares in the regressor form $\boldsymbol\tau=Y\pi$. Why model errors matter: on real robots payloads, cables, gearbox friction and joint flexibility are not in the rigid-body model, and the unmodeled payload in FIG 5 is a miniature of that; practice compensates with high-gain feedback, adaptive or robust control, or learned residual models.

7常见误区Common pitfalls

误区 1：对 $M\dot{\mathbf q}$ 求导时忘了 $\dot M$Pitfall 1: forgetting $\dot M$ when differentiating $M\dot{\mathbf q}$

$\tfrac{d}{dt}(M(\mathbf q)\dot{\mathbf q})=M\ddot{\mathbf q}+\dot M\dot{\mathbf q}$，$\dot M=\sum_k\tfrac{\partial M}{\partial q_k}\dot q_k$ 一般不为零。丢掉它（以及 $\partial T/\partial\mathbf q$）等于丢掉整个 $C\dot{\mathbf q}$：慢速时误差小，仿真看着没问题；转快了，离心力和科氏力消失，能量也不再守恒。在 FIG 3 里用手臂高速甩动，对比"有 $C$"和"无 $C$"的直觉：没有它手臂不会向外甩。

$\tfrac{d}{dt}(M(\mathbf q)\dot{\mathbf q})=M\ddot{\mathbf q}+\dot M\dot{\mathbf q}$, and $\dot M=\sum_k\tfrac{\partial M}{\partial q_k}\dot q_k$ is generally nonzero. Dropping it (and $\partial T/\partial\mathbf q$) drops all of $C\dot{\mathbf q}$: at low speed the error is small and the simulation looks fine; faster, centrifugal and Coriolis forces disappear and energy is no longer conserved. Fling the arm in FIG 3 and ask what it would do without $C$: it would not swing outward.

误区 2：把杆当质点，或者漏掉质心偏移和 $I$Pitfall 2: treating a link as a point mass, or dropping the COM offset and $I$

质量集中在端点的"点质量杆"和均匀杆是两种不同的机器人：均匀杆绕关节的总转动惯量是 $ml^2/3=m\,l_c^2+I$（$l_c=l/2$，$I=ml^2/12$），点质量杆是 $ml^2$，差了 3 倍。把 $M_{22}=m_2l_{c2}^2$ 写成不含 $I_2$，就等于假设杆没有粗细；真实机械臂的电机、减速器、线缆让质心和惯量都不在"几何中点"，要从 CAD 或辨识得到。

A "point-mass rod" with the mass at its tip and a uniform rod are two different robots: the uniform rod's total inertia about the joint is $ml^2/3=m\,l_c^2+I$ ($l_c=l/2$, $I=ml^2/12$) while the point-mass rod's is $ml^2$, a factor of 3 apart. Writing $M_{22}=m_2l_{c2}^2$ without $I_2$ assumes the link has no thickness. On a real arm motors, gearboxes and cables put the center of mass and inertia away from the geometric midpoint; get them from CAD or identification.

误区 3：角度约定和坐标选择Pitfall 3: angle conventions and the choice of coordinates

本节的 2R 从水平量角，$g_1\propto\cos\theta_1$；如果从竖直量（摆那样），$\cos$ 变成 $\sin$，零位也不同。$\theta_2$ 取相对角还是绝对角，$M_{12}$、$C$ 的形式都不同。URDF、DH、厂商手册各有各的零位和正方向，把别人的 $M(\mathbf q)$ 套到自己的坐标上之前要先对齐。另外广义坐标本身可能不是好坐标：用欧拉角描述三维姿态，在万向节锁处 $M$ 变成奇异的，尽管刚体并没有任何问题（5.2 节）。

This lesson measures the 2R angles from the horizontal, so $g_1\propto\cos\theta_1$; measured from the vertical (as for the pendulum) the $\cos$ becomes $\sin$ and the zero position differs. Whether $\theta_2$ is relative or absolute changes the form of $M_{12}$ and $C$. URDF, DH and vendor manuals each have their own zero positions and positive directions; align them before plugging someone else's $M(\mathbf q)$ into your coordinates. Coordinates themselves can also be poor: describing a 3D orientation with Euler angles makes $M$ singular at gimbal lock even though the rigid body is perfectly fine (Lesson 5.2).

误区 4：把非保守力放进 $V$，或忘了传动比Pitfall 4: putting non-conservative forces into $V$, or forgetting the gear ratio

摩擦、电机力矩、接触力不是某个 $V(\mathbf q)$ 的梯度，只能作为广义力出现在方程右边（FIG 2、3 的 $\boldsymbol\tau-b\dot{\mathbf q}$）。把它们塞进 $V$ 会让"能量守恒"的检验失去意义。另一个现实问题是减速器：传动比 $N$ 的关节，电机转子的惯量折算到关节侧是 $N^2I_m$，常常和连杆自身的惯量同量级甚至更大，必须加到 $M$ 的对角线上；电机力矩也要乘以 $N$ 才是关节力矩。

Friction, motor torque and contact forces are not the gradient of any $V(\mathbf q)$; they enter as generalized forces on the right-hand side (the $\boldsymbol\tau-b\dot{\mathbf q}$ of FIG 2 and 3). Putting them into $V$ ruins the energy-conservation check. A practical issue is the gearbox: with ratio $N$, the rotor inertia reflected to the joint side is $N^2I_m$, often comparable to or larger than the link's own inertia, and must be added to the diagonal of $M$; the motor torque is likewise multiplied by $N$ to get the joint torque.

误区 5：积分器和步长Pitfall 5: the integrator and the step size

方程是对的，仿真仍可能是错的。FIG 3 里显式欧拉在 $\boldsymbol\tau=0,b=0$ 时凭空"生成"几十焦耳的能量；RK4 在小步长下好得多，但步长一大同样漂移。并不是所有"看起来更高级"的积分器都更好：对 $M$ 依赖 $q$ 的机械系统，半隐式欧拉并不是辛的，在我们的 2R 试验里同样会漂移（所以本图没有提供它）。接触、摩擦让方程变刚性，机器人仿真器因此常用隐式或半隐式、小步长的专用方法。控制器自己以固定频率（几百到一千赫兹）采样，力矩在一个周期内是常数，也是一种离散化，FIG 5 就是这样做的。

A correct equation can still give a wrong simulation. In FIG 3 explicit Euler "creates" tens of joules from nothing at $\boldsymbol\tau=0,b=0$; RK4 at small step sizes is much better, but drifts too once the step grows. A fancier-looking integrator is not automatically better: for mechanical systems where $M$ depends on $q$, semi-implicit Euler is not symplectic and also drifts in our 2R test (which is why the figure does not offer it). Contacts and friction make the equations stiff, so robot simulators typically use implicit or semi-implicit methods with small steps. The controller itself samples at a fixed rate (hundreds of hertz to 1 kHz) and holds the torque constant over a period, another discretization; FIG 5 does exactly that.

8小练习Check yourself

1. 单摆 $m=l=1$，$\theta$ 从正下方量起，水平位置（$\theta=90^\circ$）静止松手，$\tau=0$、$b=0$。此刻的 $\ddot\theta$ 是？（$g=9.81$）1. A pendulum with $m=l=1$, $\theta$ from straight down, is released from rest at the horizontal position ($\theta=90^\circ$), with $\tau=0$, $b=0$. What is $\ddot\theta$ at that instant? ($g=9.81$)

$ml^2\ddot\theta=-mgl\sin\theta$，所以 $\ddot\theta=-(g/l)\sin 90^\circ=-9.81$（向下摆，$\theta$ 减小）。此刻速度为零，所以没有离心项；重力对枢轴的力矩最大，加速度最大。$ml^2\ddot\theta=-mgl\sin\theta$, so $\ddot\theta=-(g/l)\sin 90^\circ=-9.81$ (swinging down, $\theta$ decreases). The speed is zero so there is no centrifugal term; gravity's torque about the pivot is at its maximum, hence so is the acceleration.

2. 为什么单摆的拉格朗日推导里从来没有出现杆的张力？2. Why does the rod tension never appear in the Lagrangian derivation of the pendulum?

张力一般不为零（FIG 2 牛顿视角里最大可达几十牛）。它没有出现，是因为 $\theta$ 只描述满足"小球到枢轴距离为 $l$"的运动，在这些允许的虚位移上张力做功为零，被虚功原理消去。需要张力时（比如校核杆会不会断），再由牛顿的径向方程反算。The tension is generally nonzero (it reaches tens of newtons in FIG 2's Newton view). It does not appear because $\theta$ only describes motions that keep the ball at distance $l$ from the pivot, and on those admissible virtual displacements the tension does zero work, so virtual work eliminates it. When you need it (say, to check whether the rod breaks), recover it afterwards from Newton's radial equation.

3. FIG 3 的 2R 臂（$m_1=2,m_2=1.5$ kg，$l_1=1,l_2=0.8$ m，均匀杆）水平伸直并静止（$\theta_1=\theta_2=0$）。保持不动所需的关节力矩 $(\tau_1,\tau_2)$ 约为？3. The 2R arm of FIG 3 ($m_1=2,m_2=1.5$ kg, $l_1=1,l_2=0.8$ m, uniform rods) is stretched out horizontally at rest ($\theta_1=\theta_2=0$). What joint torques $(\tau_1,\tau_2)$ hold it still?

静止时 $\dot{\mathbf q}=\ddot{\mathbf q}=0$，只剩 $\boldsymbol\tau=\mathbf g(\mathbf q)$。$\theta=0$ 时 $\cos=1$：$\tau_1=(m_1l_{c1}+m_2l_1+m_2l_{c2})g=(1+1.5+0.6)\cdot9.81=30.4$；$\tau_2=m_2l_{c2}g=1.5\cdot0.4\cdot9.81=5.9$。直观检查：关节 2 只托住第二根杆的重量，力臂 0.4 m。At rest $\dot{\mathbf q}=\ddot{\mathbf q}=0$, leaving $\boldsymbol\tau=\mathbf g(\mathbf q)$. At $\theta=0$, $\cos=1$: $\tau_1=(m_1l_{c1}+m_2l_1+m_2l_{c2})g=(1+1.5+0.6)\cdot9.81=30.4$; $\tau_2=m_2l_{c2}g=1.5\cdot0.4\cdot9.81=5.9$. Sanity check: joint 2 only carries link 2's weight, with a 0.4 m lever arm.

4. 倒立摆小车有 2 个自由度但只有 1 个驱动。下面哪个说法对？4. The cart-pole has 2 DoF but 1 actuator. Which statement is correct?

第二个方程 $l\ddot\theta+\cos\theta\,\ddot x=g\sin\theta$ 与 $F$ 无关，任何力都改变不了 $(\ddot x,\ddot\theta)$ 必须落在一条直线上。欠驱动不等于不可控：线性化后系统在竖直点可控，LQR 就能平衡它。The second equation $l\ddot\theta+\cos\theta\,\ddot x=g\sin\theta$ does not involve $F$, so no force can move $(\ddot x,\ddot\theta)$ off a line. Underactuated does not mean uncontrollable: the linearization at the upright point is controllable, and LQR balances it.

5. 模型完美时，计算力矩 $\boldsymbol\tau=M(\mathbf q)(\ddot{\mathbf q}_d+K_d\dot e+K_pe)+C\dot{\mathbf q}+\mathbf g$ 使闭环误差满足什么方程？5. With a perfect model, what equation does the closed-loop error obey under computed torque $\boldsymbol\tau=M(\mathbf q)(\ddot{\mathbf q}_d+K_d\dot e+K_pe)+C\dot{\mathbf q}+\mathbf g$?

代入 $M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau$ 得 $M\ddot{\mathbf q}=M(\ddot{\mathbf q}_d+K_d\dot e+K_pe)$，两边乘 $M^{-1}$（$M$ 正定，可逆）：$\ddot{\mathbf q}_d-\ddot{\mathbf q}=-K_d\dot e-K_pe$，即 $\ddot e+K_d\dot e+K_pe=0$。选 $K_p,K_d$ 就是选一个线性二阶系统的极点（2.5 节）。模型不完美时这个结论不再成立（FIG 5）。Substituting into $M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau$ gives $M\ddot{\mathbf q}=M(\ddot{\mathbf q}_d+K_d\dot e+K_pe)$; multiply by $M^{-1}$ ($M$ is positive definite, hence invertible): $\ddot{\mathbf q}_d-\ddot{\mathbf q}=-K_d\dot e-K_pe$, i.e. $\ddot e+K_d\dot e+K_pe=0$. Choosing $K_p,K_d$ means choosing the poles of a linear second-order system (Lesson 2.5). With an imperfect model the conclusion no longer holds (FIG 5).

9延伸阅读Further reading

M. W. Spong, S. Hutchinson & M. Vidyasagar, Robot Modeling and Control, Ch. 7 (动力学Dynamics) — 欧拉–拉格朗日方程、操作臂方程的性质（反对称性、对参数线性）、平面 2R 的完整推导Euler–Lagrange equations, properties of the manipulator equation (skew-symmetry, linearity in parameters), and the full planar 2R derivation
R. M. Murray, Z. Li & S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 4 — 多刚体系统的拉格朗日动力学与 Christoffel 符号，三维刚体的处理Lagrangian dynamics of multibody systems, Christoffel symbols, and the treatment of 3D rigid bodies
K. Lynch & F. Park, Modern Robotics, Ch. 8 (开链动力学Dynamics of open chains) — 拉格朗日形式、牛顿–欧拉递推，以及用旋量写出的质量矩阵The Lagrangian formulation, recursive Newton–Euler, and the mass matrix written with screws
H. Goldstein, C. Poole & J. Safko, Classical Mechanics (3rd ed.), Ch. 2 and 8 — 变分原理、拉格朗日与哈密顿力学的经典教材The standard text on variational principles and Lagrangian and Hamiltonian mechanics
L. D. Landau & E. M. Lifshitz, Mechanics (Course of Theoretical Physics, Vol. 1), §1–§5 — 从最小作用量原理出发的简洁推导（经典参考）A terse derivation from the principle of least action (the classical reference)
R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008 — RNEA、ABA（$O(n)$）、CRBA 等高效的刚体动力学算法，后续两节会用到RNEA, ABA ($O(n)$), CRBA and other efficient rigid-body algorithms, used in the next two lessons