Part IV · 第 13 章 反馈控制 · 13.3

LQR：最优状态反馈

6.12 节用动态规划解出了有限时域的 LQR。这一节把时域拉到无穷远：Riccati 递推收敛到代数 Riccati 方程，给出一个常数增益 \(K\)，一行 \(\mathbf u=-K\mathbf x\) 就能把倒立摆小车立住。我们会在增益平面上看"最优"长什么样，用 Bryson 法则按物理单位调 \(Q\)、\(R\)，看线性控制器在非线性、有饱和的真实系统上能管多大的范围，用时变 LQR 和 iLQR 把单摆从下方摆起来，最后看 LQR 自带的鲁棒裕度，以及加上卡尔曼滤波器后的 LQG 与分离原理。

约 45 分钟 前置：6.12 动态规划 · 13.2 状态空间与稳定性 · 4.8 卡尔曼滤波 · 8.1 拉格朗日动力学 6 个交互

    1为什么机器人需要它

    13.1 节的 PID 一次只管一个回路：一个误差，一个力矩。可是很多机器人只有一个执行器，却要同时照顾好几个量。8.1 节的倒立摆小车只能推小车，却要同时让摆杆竖直、让小车回到中间；平衡车、球形机器人、四旋翼悬停、人形机器人的质心平衡都是这样。状态有四个分量 \(\mathbf x=(x,\dot x,\theta,\dot\theta)\)，反馈律 \(F=-K\mathbf x\) 有四个耦合在一起的增益，一个一个手调几乎不可能，而且它们的符号还常常"反直觉"。

    13.2 节的极点配置能把闭环极点放到任何指定的位置，却不告诉你该放在哪里：极点越靠左，响应越快，力也越大，会撞上电机的极限。LQR（线性二次调节器，linear-quadratic regulator）换一个问法：你只需写下你在乎什么，

    $$ J=\sum_{k=0}^{\infty}\Big(\mathbf x_k^{\top}Q\,\mathbf x_k+\mathbf u_k^{\top}R\,\mathbf u_k\Big),\qquad \mathbf x_{k+1}=A\mathbf x_k+B\mathbf u_k, $$

    "偏离多少要付多少代价"（\(Q\)）和"用多大力要付多少代价"（\(R\)），LQR 就给出让 \(J\) 最小的反馈律。答案出奇地简单：它是一个常数矩阵 \(K\)，\(\mathbf u=-K\mathbf x\)，离线解一个矩阵方程就能得到，每个控制周期只需做一次矩阵乘向量。6.12 节已经推出了有限时域 \(N\) 步的版本（Riccati 递推和"价值函数是椭圆"），并看到时域足够长时增益趋于常数。这一节接着往下走：

    • 无限时域的增益从哪里来（代数 Riccati 方程），为什么它一定让线性系统稳定；
    • \(Q\)、\(R\) 怎么定（Bryson 法则），"最优"在增益空间里长什么样；
    • 在真实的非线性、有力的上限的系统上，它能管多大的范围（吸引域）；
    • 不止平衡一个点，还要跟踪一条轨迹：时变 LQR，以及用它当求解器的 iLQR；
    • 传感器只测到部分状态、还带噪声时怎么办（LQG），以及 LQR 有多"皮实"（鲁棒裕度）。

    2直觉：每个增益都有一个价钱

    先看最简单的不稳定系统：一个电机直接驱动的倒立单摆（质量 \(m=1\) kg 集中在 \(l=0.5\) m 处，\(\theta\) 从竖直向上量起）。线性化后 \(ml^2\ddot\theta=mgl\,\theta+u\)，控制律 \(u=-k_\theta\theta-k_\omega\dot\theta\) 只有两个增益，于是所有可能的控制器构成一个平面。平面上每一点是一个控制器，它从初始倾角 \(\theta_0=0.2\) rad 出发会产生一条响应，这条响应有一个总代价

    $$ \hl{J}{J(K)}=\sum_{k}\big(\theta_k^2+0.1\,\dot\theta_k^2+r\,u_k^2\big)\,\Delta t ,\qquad \hl{Kopt}{K^\star}=\arg\min_K J(K). $$

    （\(\Delta t=10\) ms，控制器以 100 Hz 运行，求和乘 \(\Delta t\) 只是为了让数值像积分。）下图左边把每个 \(K\) 的代价画成颜色，越青代价越低；红色区域是闭环不稳定的增益，代价为无穷。拖动琥珀色的点亲手调增益，绿色菱形是 LQR 算出的 \(K^\star\)。右边是两者的响应。

    FIG 1增益平面上的代价地形
    左：横轴 \(k_\theta\)（N·m/rad），纵轴 \(k_\omega\)（N·m·s/rad）。颜色是 \(\log_{10}(J/J^\star)\)，青色最浓处是最低点，从最优往外每深一档代价大一个台阶（细青线是 \(J/J^\star=1.01,\,1.1,\,1.5,\,3\) 的等值线）。红色虚线 \(k_\theta=mgl\approx4.9\) N·m/rad 左边，电机的"弹簧"抵不过重力，摆一定倒下。紫色虚线是 \(r\) 从 \(10^{-4}\) 变到 \(10^{4}\) 时 \(K^\star\) 走过的路。右：上面是 \(\theta(t)\)，下面是力矩 \(u(t)\)；琥珀色是你的增益，绿色虚线是 \(K^\star\)。响应用的是线性模型，与 \(J\) 的计算一致。
    试一试
    • 保持 \(r=0.01\)，拖动琥珀色的点，让代价比最优只高不到 1%（\(J/J^\star<1.01\)）。你停下的位置离绿色菱形有多远？代价地形在最优点附近有多平？
    • 把 \(r\) 调到 100 以上（力矩极贵）。\(K^\star\) 会不会一路退到 0，或者退到红线上？它停在哪里？
    核心洞见

    LQR 不是"另一种调参技巧"，而是把调参换成了说清楚目标：一旦写下 \(Q\)、\(R\)，最优增益就唯一确定了，而且它对所有初始状态同时最优（换一个 \(\theta_0\)，地形变了，最低点不变）。最低点附近的地形很平：把 \(K^\star\) 整体放大 1.3 倍，代价只多 1.9%；缩小到 0.7 倍，多 5%。所以真正要花心思的不是把增益算到小数点后几位，而是选对 \(Q\)、\(R\)。另一面是 \(r\to\infty\) 时 \(K^\star\) 不会退到零：对不稳定的系统，"不花力气"不是一个选项，LQR 会停在用最小代价把不稳定极点镜像到左半平面的增益上（这里 \(k_\theta\to9.6\)，大约是 \(2mgl\)）。

    3动手玩：用 LQR 立住倒立摆小车

    现在换成 8.1 节的倒立摆小车：小车 \(M_c=1\) kg，摆端质量 \(m=0.3\) kg，杆长 \(l=0.8\) m，只能推小车。在竖直位置线性化（13.2 节的做法），状态 \(\mathbf x=(x,\dot x,\theta,\dot\theta)\)：

    $$ \dot{\mathbf x}=\underbrace{\begin{bmatrix}0&1&0&0\\0&0&-\frac{mg}{M_c}&0\\0&0&0&1\\0&0&\frac{(M_c+m)g}{M_cl}&0\end{bmatrix}}_{A_c}\mathbf x+\underbrace{\begin{bmatrix}0\\\frac{1}{M_c}\\0\\-\frac{1}{M_cl}\end{bmatrix}}_{B_c}F . $$

    \(A_c\) 有一个特征值 \(+\sqrt{(M_c+m)g/(M_cl)}\approx+3.99\) s⁻¹：不加控制，倾角的误差每 0.25 s 放大 \(e\) 倍。控制器以 100 Hz 运行，所以先按 13.2 节的零阶保持把 \((A_c,B_c)\) 离散化成 \((A,B)\)，再解离散 Riccati 方程得到 \(K\)。

    怎么选 \(Q\) 和 \(R\)？它们的数值本身没有意义（同时乘 10，\(K\) 不变），有意义的是比例。Bryson 法则给出一个按物理单位思考的起点：

    $$ Q_{ii}=\frac{1}{(\text{largest acceptable }x_i)^2},\qquad R_{jj}=\frac{1}{(\text{largest acceptable }u_j)^2}. $$

    每一项代价都变成"相对于可接受值的比例的平方"：小车偏离 \(x_{\max}\)、摆杆倾斜 \(\theta_{\max}\)、用力 \(F_{\max}\)，各算 1 份代价，单位就消掉了。下图的三个滑块就是 \(x_{\max}\)、\(\theta_{\max}\)、\(F_{\max}\)（速度的权重设为 0），右上角的公式实时显示由它们算出的 \(Q\)、\(R\)、\(K\)。把绿色的目标标记拖到导轨上的新位置，松手后小车开到那里；按"踢一下"给摆杆一个 1.5 rad/s 的角速度冲量；也可以直接拖动摆端的小球把摆杆扳歪，松手看它恢复。

    FIG 2LQR 倒立摆小车（非线性仿真，100 Hz 控制）
    小车蓝色，摆杆琥珀色，紫色箭头是控制力 \(F\)（1 cm ↔ 1 N），绿色三角是目标位置 \(x_{\mathrm{ref}}\)（控制器用的是 \(x-x_{\mathrm{ref}}\)）。电机最多出 30 N，导轨两端在 ±2 m 处有挡板。右侧滚动显示最近 8 s 的 \(\theta\)（琥珀，度）、\(x\)（蓝，m）和 \(F\)（紫，N）。仿真用完整的非线性方程（8.1 节），RK4 步长 2 ms，控制力每 10 ms 更新一次并保持不变。摆杆倾角超过 75° 判为倒下，按"复位"重来。
    试一试
    • 用默认权重，把目标挪开至少 0.8 m（拖动绿色三角，或者点"目标 +1 m / −1 m"）。小车出发时先往反方向退了几厘米（至少 3 cm）。为什么要先退？
    • 调成"温柔"的控制器：完成一次至少 0.8 m 的移动并停稳（误差 5 cm 内），整个过程中力的峰值小于 2 N，中途不要踢或拖摆杆。提示：用 Bryson 法则的语言想一想，你愿意让小车偏多远、愿意出多大的力？
    • 反过来，让控制器对小车位置"贪心"：\(x_{\max}\le0.1\) m，\(F_{\max}\ge30\) N（\(\theta_{\max}\) 取 2° 以上），然后踢一下。线性闭环的极点全在左半平面，摆杆却倒了。是什么出了问题？（第 4 节的 FIG 3 会回答。）
    核心洞见

    LQR 自己"发现"了倒立摆的诀窍。要把小车向右开，必须先让摆杆向右倾，否则一加速摆杆就向左倒；而要让摆杆向右倾，小车得先向左退一下。默认权重下，移动 1 m 时小车先倒退约 12 cm，力的峰值 18.4 N，2.4 s 内停到目标 5 cm 以内。这就是增益 \(K\) 里小车位置那一项"符号看上去是反的"的原因（8.1 节提到过）：没有人告诉 LQR 这个策略，它只是 Riccati 方程的解。手调四个耦合的增益很难碰对这种结构。

    4另外两个视角：吸引域与轨迹跟踪

    线性的增益，非线性的系统

    LQR 的"最优"和"一定稳定"都是对线性化模型说的。真实系统里有 \(\sin\theta\)、有离心项 \(ml\dot\theta^2\sin\theta\)，电机还有上限。2.4 节说过：线性化只在工作点附近可信。那么"附近"到底多大？下图回答这个问题：把初始倾角 \(\theta_0\) 和初始角速度 \(\dot\theta_0\) 铺成一张网格（小车静止在原点），每一格都用非线性模型、带力限幅地仿真 6 s，能立回来的格子涂绿色，倒下的涂红色。绿色区域就是这个控制器的吸引域（region of attraction）在这张切片上的样子。

    FIG 3吸引域：力的上限与增益的"贪心"程度
    左：横轴 \(\theta_0\)（−90° 到 90°），纵轴 \(\dot\theta_0\)（−8 到 8 rad/s），61 × 41 格。绿色 = 6 s 后摆杆回到竖直附近（\(|\theta|<0.05\) rad，\(|\dot\theta|<0.3\) rad/s），红色 = 倒下（\(|\theta|\) 超过 100°）或没回来。琥珀色椭圆是二次型 \(\mathbf x^\top P\mathbf x\le c\)（\(P\) 是 Riccati 方程的解）在 \(x=\dot x=0\) 上的切片，\(c\) 取在 4000 个随机方向上都检查到"\(V\) 每个控制周期都在下降"的最大值：一个抽样估计的、偏保守的"保证区"。三套权重用 Bryson 法则给出：温柔 \((1\text{ m},20^\circ,2\text{ N})\)，默认 \((0.5\text{ m},10^\circ,10\text{ N})\)，贪心 \((0.1\text{ m},2^\circ,50\text{ N})\)。点击任意一格，右边回放从该状态出发的运动（导轨不设挡板，地面刻度每 0.5 m 一格），左边用蓝线画出它在 \((\theta,\dot\theta)\) 上的轨迹。
    试一试
    • 用默认权重，先把力的上限调到 100 N（绿色约占网格的 35%），再降到 10 N 或更低：绿色缩到 15% 以下。线性模型对这件事一无所知：它的闭环极点一点没变。
    • 在同一个力上限下，分别看"温柔"和"贪心"两套权重（两者都要算完）。哪一个能接住更多的初始状态？这和 FIG 2 里贪心控制器被踢倒是同一回事吗？
    核心洞见

    在 30 N 的上限下，温柔的权重接住了网格的约 54%，默认的约 28%，贪心的只有约 6%。贪心的增益在线性模型里"更好"（极点更靠左），可它为一点点偏差就要求巨大的力，力一饱和，反馈律就不再是 \(-K\mathbf x\)，最优性和稳定性的证明都不再成立。实践中的结论是：先用 Bryson 法则把 \(F_{\max}\) 设得比电机的真实上限小，让控制器在正常扰动下根本碰不到饱和；必须贴着极限工作时，就要换成能显式处理约束的方法（13.4 节的 MPC）。琥珀色椭圆比绿色区域小得多：Lyapunov 函数给出的保证是保守的，但它是一个证书，而网格仿真只是一些例子。把这种椭圆沿轨迹连成一棵树，就是 LQR-Trees（Tedrake 等，2010）。

    跟踪一条轨迹：时变 LQR 与 iLQR

    平衡只是"守住一个点"。很多任务要沿一条轨迹运动：把单摆从下垂位置甩到竖直，四旋翼翻个跟头，机械臂快速搬运。思路是一样的：在一条名义轨迹 \((\mathbf x^\star_k,\mathbf u^\star_k)\) 附近线性化，偏差 \(\delta\mathbf x=\mathbf x-\mathbf x^\star_k\) 近似满足 \(\delta\mathbf x_{k+1}\approx A_k\delta\mathbf x_k+B_k\delta\mathbf u_k\)，只是 \(A_k,B_k\) 随时间变。这时 6.12 节的有限时域 Riccati 递推正好派上用场，得到时变增益 \(K_k\)，控制律是 \(\mathbf u_k=\mathbf u^\star_k-K_k(\mathbf x_k-\mathbf x^\star_k)\)。这就是时变 LQR（TVLQR）。

    名义轨迹本身从哪里来？iLQR 用同一个工具来求它：从一串猜测的控制量出发，(1) 前向仿真得到轨迹；(2) 沿轨迹线性化动力学、把代价展开到二次；(3) 用 Riccati 递推反向求出"该怎么改"（一个前馈修正 \(\mathbf k_k\) 和一个反馈增益 \(K_k\)）；(4) 带线搜索地前向执行修正，重复直到收敛。6.13 节把它称为打靶法的一个高效求解器。下图让一个单摆（\(m=1\) kg，\(l=0.5\) m）在 4 s 内从下垂摆到竖直。仿真里没有硬性的力矩上限，但力矩的代价 \(R\) 让最优解舍不得用大力矩，峰值远小于 \(mgl\)，只好来回荡几下积攒能量：先运行 iLQR，再把得到的力矩序列用到一个质量有偏差的"真实"单摆上，比较直接回放（开环）和 TVLQR 跟踪。

    FIG 4iLQR 摆起 + 时变 LQR 跟踪
    左：相平面，横轴 \(\theta\)（rad，0 = 竖直向上，π = 下垂），纵轴 \(\dot\theta\)（rad/s）。青色是 iLQR 当前迭代的轨迹（运行时逐次更新，淡青色是之前的迭代），绿点是目标。按"播放"后：紫色是在质量有偏差的单摆上直接回放 \(\mathbf u^\star\)（开环，4 s 后力矩为 0），琥珀色是 TVLQR（4 s 后切换到竖直点的常数 LQR 增益）。右上：两个单摆的动画（同色）。右下：TVLQR 增益 \(K_k=(k_\theta,k_\omega)\) 随时间的变化（红 = \(k_\theta\)，蓝 = \(k_\omega\)），虚线是竖直点的常数增益。代价权重：\(Q=\mathrm{diag}(0.05,0.005)\)，\(R=0.2\)，终端 \(\mathrm{diag}(300,30)\)，步长 20 ms；TVLQR 的跟踪权重 \(Q=\mathrm{diag}(10,1)\)，\(R=0.1\)。
    试一试
    • 按"运行 iLQR"直到收敛（相对代价下降小于 \(10^{-6}\)）。最优轨迹在相平面上先往哪边走？为什么它要先往回荡一下，而不是直接往上推？（看读数里的力矩峰值和 \(mgl\)。）
    • iLQR 收敛后，把质量误差调到 ±10% 或更大，按"播放"：开环回放最后离竖直超过 30°，TVLQR 却停在 1° 以内。
    一个递推，三种用法

    稳态 LQR：把 6.12 节的 Riccati 递推跑到收敛，得到常数 \(K\)，守住一个平衡点（FIG 2）。时变 LQR：沿一条已知轨迹的 \(A_k,B_k\) 跑一遍有限时域递推，得到 \(K_k\)，守住一条轨迹（FIG 4 的琥珀色）。iLQR / DDP：轨迹本身未知，就"递推一次、改一次轨迹"反复迭代，每次迭代就是一个局部的时变 LQR 问题；它最后一次反向传递给出的 \(K_k\) 本身就是一个现成的跟踪控制器。三者的计算核心都是同一个矩阵递推。

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，对应的图元素会被高亮：\(J(K)\) 点亮 FIG 1 的等值线，\(K^\star\) 点亮 FIG 1 的绿色菱形，\(\mathbf u=-K\mathbf x\) 点亮 FIG 2 的力箭头，\(P\) 点亮 FIG 3 的椭圆，\(K_k\) 点亮 FIG 4 的增益曲线，回差不等式点亮 FIG 5 的禁区圆盘，卡尔曼增益 \(L\) 点亮 FIG 6 的估计器极点。

    从有限时域到代数 Riccati 方程

    6.12 节的递推是 \(P_k=Q+A^\top P_{k+1}A-A^\top P_{k+1}B\,(R+B^\top P_{k+1}B)^{-1}B^\top P_{k+1}A\)（与 6.12 节写成 \(Q+A^\top P_{k+1}(A-BK_k)\) 的形式等价）。时域 \(N\to\infty\) 时，若 \(P_k\) 收敛到某个 \(P\)，它必须是递推的不动点，也就是离散代数 Riccati 方程（DARE）的解：

    $$ \hl{P}{P}=Q+A^{\top}PA-A^{\top}PB\,\big(R+B^{\top}PB\big)^{-1}B^{\top}PA,\qquad K=\big(R+B^{\top}PB\big)^{-1}B^{\top}PA,\qquad \hl{u}{\mathbf u=-K\mathbf x}. $$

    什么时候解存在且唯一？若 \((A,B)\) 可镇定（不稳定的模态都能被 \(\mathbf u\) 影响到，13.2 节的可控性条件的弱化版）且 \((A,Q^{1/2})\) 可检测（每个不稳定的模态都会在代价里"被看见"），则 DARE 有唯一的半正定解使 \(A-BK\) 的所有特征值都在单位圆内。第二个条件常被忽略：若 \(Q\) 对某个不稳定模态是零，"让它发散"不花任何代价，LQR 就不会去管它。FIG 2 里速度权重为零却没有问题，因为位置和角度被罚了，而速度会体现在它们的变化里。

    怎么解？最直接的是把 6.12 节的递推一直跑下去。对 8.1 节那组权重（\(Q=\mathrm{diag}(1,1,10,1)\)，\(R=0.1\)）在 100 Hz 的模型上，要跑 536 步，增益相邻两步的相对变化才小于 \(10^{-6}\)。实用的求解器更快：Schur 方法（Laub，1979）对哈密顿矩阵做一次特征分解直接得到 \(P\)，MATLAB、SciPy 的 solve_discrete_are 就是这一类；本页的代码用"倍增"算法，每一步把递推的时域翻倍，同样的问题 12 步就收敛。

    为什么闭环一定稳定：\(P\) 本身就是 Lyapunov 函数

    贝尔曼方程说，最优剩余代价 \(V(\mathbf x)=\mathbf x^\top P\mathbf x\) 等于"这一步的代价 + 下一步的剩余代价"。沿最优闭环 \(\mathbf x_{k+1}=(A-BK)\mathbf x_k\)：

    $$ V(\mathbf x_{k+1})-V(\mathbf x_k)=-\big(\mathbf x_k^{\top}Q\mathbf x_k+\mathbf u_k^{\top}R\mathbf u_k\big)\le0 . $$

    \(V\) 正定、每一步都不增，正是 13.2 节的 Lyapunov 函数，所以 LQR 的稳定性是"白送"的：不需要另外去找证书，Riccati 方程的解就是证书。FIG 3 的椭圆就是 \(V\) 的一个等值面：在非线性系统上，只要在某个水平集 \(\{V\le c\}\) 内 \(V\) 仍然下降，这个集合里的初始状态就都能被接住。

    连续时间版本

    对 \(\dot{\mathbf x}=A_c\mathbf x+B_c\mathbf u\)、\(J=\int_0^\infty(\mathbf x^\top Q\mathbf x+\mathbf u^\top R\mathbf u)\,dt\)，同样的推导（6.12 节的贝尔曼方程换成哈密顿–雅可比–贝尔曼方程）给出连续代数 Riccati 方程（CARE）：

    $$ A_c^{\top}P+PA_c-PB_cR^{-1}B_c^{\top}P+Q=0,\qquad K=R^{-1}B_c^{\top}P . $$

    8.1 节"自动平衡"用的就是它：\(Q=\mathrm{diag}(1,1,10,1)\)，\(R=0.1\) 给出 \(K=(-3.16,-6.10,-55.0,-14.9)\)。用 100 Hz 的离散模型解 DARE（权重同比例），小车位置的增益是 \(-2.97\)，小了约 6%；步长缩到 1 ms，变成 \(-3.14\)，趋向连续解。控制器以多快的频率运行，就应该用那个频率的离散模型来解。

    回差不等式与鲁棒裕度

    在执行器处把回路断开（单输入、全状态反馈、连续时间），环路传递函数是 \(L(s)=K(sI-A_c)^{-1}B_c\)。从 CARE 可以推出（Kalman，1964）：

    $$ \hl{ret}{\big|1+L(j\omega)\big|\ge1}\quad\text{for all }\omega . $$

    几何上：奈奎斯特曲线 \(L(j\omega)\) 永远不进入以 \(-1\) 为圆心、半径为 1 的圆盘。于是它与负实轴的交点要么在 \(-2\) 左边、要么在 0 右边，把执行器增益乘上任何 \(\beta>1/2\) 都不会让曲线穿过 \(-1\)：增益裕度至少是 \([1/2,\infty)\)。单位圆与这个圆盘交在与 \(-1\) 相隔 60° 的两点，所以曲线穿过单位圆时离 \(-1\) 至少 60°：相位裕度至少 60°。这是 LQR 白送的第二件礼物，FIG 5 会把它画出来，并说明它的边界。

    时变 LQR 与 iLQR 的递推

    TVLQR 就是 6.12 节的递推，把 \(A,B\) 换成沿名义轨迹的雅可比 \(A_k=\partial\mathbf f/\partial\mathbf x\)、\(B_k=\partial\mathbf f/\partial\mathbf u\)（在 \((\mathbf x^\star_k,\mathbf u^\star_k)\) 处取值），终端 \(P_N\) 取目标点的稳态 LQR 解：

    $$ \begin{gathered} \hl{Kt}{K_k}=\big(R+B_k^{\top}P_{k+1}B_k\big)^{-1}B_k^{\top}P_{k+1}A_k,\qquad P_k=Q+A_k^{\top}P_{k+1}\big(A_k-B_kK_k\big),\\ \mathbf u_k=\mathbf u^\star_k-K_k\big(\mathbf x_k-\mathbf x^\star_k\big). \end{gathered} $$

    iLQR 的反向传递多带一个一次项：把代价在当前轨迹附近展开成 \(\ell_k\approx\ell+\ell_{\mathbf x}^\top\delta\mathbf x+\ell_{\mathbf u}^\top\delta\mathbf u+\tfrac12(\cdots)\)，价值函数也就多了线性项 \(\mathbf v_k^\top\delta\mathbf x\)。记 \(Q_{\mathbf u}\)、\(Q_{\mathbf u\mathbf u}\)、\(Q_{\mathbf u\mathbf x}\) 为局部"Q 函数"（这一步的代价加下一步的价值函数）对 \(\delta\mathbf u\)、\(\delta\mathbf x\) 的导数（它们不是权重矩阵 \(Q\)），反向递推给出前馈修正 \(\mathbf k_k=-Q_{\mathbf u\mathbf u}^{-1}Q_{\mathbf u}\) 和与 TVLQR 同形的增益 \(K_k=Q_{\mathbf u\mathbf u}^{-1}Q_{\mathbf u\mathbf x}\)，前向传递用 \(\mathbf u_k=\bar{\mathbf u}_k+\alpha\mathbf k_k-K_k(\mathbf x_k-\bar{\mathbf x}_k)\)，步长 \(\alpha\) 从 1 开始减半，直到代价下降。iLQR 线性化动力学时只保留一阶项，相当于对整条轨迹做 6.6 节的高斯–牛顿；保留动力学的二阶导数就是 DDP（Jacobson 与 Mayne，1970），收敛更快，每步更贵。FIG 4 里 \(Q_{\mathbf u\mathbf u}\) 上加了一个很小的正则项 \(\mu\)，线搜索失败时把 \(\mu\) 放大，这是 6.6 节 LM 阻尼的同一个想法。

    LQG 与分离原理

    LQR 假设整个状态 \(\mathbf x\) 都量得到。实际上 FIG 2 的小车只有两个编码器：小车位置 \(x\) 和摆角 \(\theta\)，而且带噪声，即 \(\mathbf y=C\mathbf x+\mathbf v\)，\(C\) 取出 \(x\) 和 \(\theta\) 两行。4.8 节的卡尔曼滤波器给出最优估计 \(\hat{\mathbf x}\)（那里卡尔曼增益记作 \(K\)，这里为了不和 LQR 增益混淆改记为 \(L\)）。它的稳态增益来自对偶的一个 Riccati 方程：把 DARE 里的 \((A,B,Q,R)\) 换成 \((A^\top,C^\top,W,V)\)，\(W\)、\(V\) 是过程噪声和测量噪声的协方差。线性二次高斯（LQG）控制器就是把两者串起来：

    $$ \hat{\mathbf x}_k=\hat{\mathbf x}_k^-+\hl{Lk}{L}\big(\mathbf y_k-C\hat{\mathbf x}_k^-\big),\qquad \mathbf u_k=-K\hat{\mathbf x}_k,\qquad \hat{\mathbf x}_{k+1}^-=A\hat{\mathbf x}_k+B\mathbf u_k . $$

    分离原理：写成真实状态 \(\mathbf x\) 与估计误差 \(\mathbf e=\mathbf x-\hat{\mathbf x}\) 的组合，闭环矩阵是块三角的，它的特征值恰好是 \(A-BK\)（控制器）和 \((I-LC)A\)（估计器）的特征值的并集。所以可以分开设计：LQR 只管 \(Q\)、\(R\)，卡尔曼滤波器只管 \(W\)、\(V\)，合起来既稳定，在线性高斯假设下也是最优的。FIG 6 把 8 × 8 闭环矩阵的特征值直接算出来，与两组 4 个极点逐一重合。

    6机器人中的它：鲁棒性与只量得到一部分状态

    真实的控制器不在完美的模型上运行：电机的力矩常数有误差（相当于执行器增益 \(\beta\neq1\)），通信、滤波和计算会带来延迟 \(\tau\)。下图在连续时间里为倒立摆小车解 CARE，画出 \(L(j\omega)\) 在 \(-1\) 附近的奈奎斯特图，再让你乘上增益 \(\beta\)、加上纯延迟 \(e^{-j\omega\tau}\)。右边是线性化闭环从 3° 初始倾角出发的响应，直接告诉你稳不稳。

    FIG 5LQR 的鲁棒裕度：增益与延迟
    左：复平面上 \(-1\) 附近的区域（横轴实部 −3.2 到 1.2）。红色圆盘是回差不等式划出的禁区（圆心 \(-1\)，半径 1），青色虚线是单位圆，琥珀色是名义环路 \(L(j\omega)\)，\(\omega>0\)（\(\omega<0\) 的镜像画成细虚线），紫色是乘上 \(\beta e^{-j\omega\tau}\) 后的曲线。小车的两个积分器使 \(|L|\) 在 \(\omega\to0\) 时趋于无穷，所以曲线从画面外进来。右：线性化闭环（带增益 \(\beta\) 与延迟 \(\tau\)）的 \(\theta(t)\)（紫色），从 3° 出发，12 s；比较 4–6 s 与 10–12 s 的包络判断稳定与否。三套权重同 FIG 3。
    试一试
    • 选"贪心"权重，\(\beta=1\)，慢慢加大延迟，找到闭环失稳的那个 \(\tau\)（应在 25 ms 以内）。再换成"默认"：同样的延迟还稳吗？60° 的相位裕度为什么挡不住延迟？

    三套权重的相位裕度都在 60° 到 65° 之间，名不虚传；但能承受的延迟相差很大：温柔约 116 ms，默认约 66 ms，贪心只有约 16 ms。延迟裕度等于相位裕度除以穿越频率 \(\omega_c\)，而贪心的控制器 \(\omega_c\approx70\) rad/s，是默认的 4 倍多。增益方向上，保证的是 \(\beta>1/2\) 都稳定，实际的下限是：温柔 0.47，默认 0.27，贪心 0.05。100 Hz 的数字控制器单是零阶保持就相当于约 5 ms 的延迟，再加上传感器滤波和通信，几十毫秒的延迟在真实机器人上并不少见，所以"带宽越高越好"并不成立。

    LQR 的保证还有一个前提：全部状态都被精确地测到。只要中间插进一个观测器，这些保证就没有了。Doyle（1978）的论文标题就是结论："Guaranteed margins for LQG regulators"，摘要只有一句："There are none."。下图在 FIG 2 的小车上只测量带噪声的 \(x\) 和 \(\theta\)，比较三种做法：理想的全状态 LQR、用相邻两次测量的差分来估计速度、卡尔曼滤波器（LQG）。

    FIG 6LQG：卡尔曼滤波 + LQR 与分离原理
    左：10 s 的仿真（非线性小车，默认权重，30 N 限幅，从 \(\theta_0\approx4.6^\circ\) 出发），上面是 \(\theta(t)\)（度），下面是控制力 \(F(t)\)（N）。青色 = 理想全状态 LQR，紫色 = 差分估速度，琥珀色 = LQG。三者用同一串噪声：小车位置噪声 \(\sigma_x=2\) mm，角度噪声 \(\sigma_\theta\) 可调，另有每 10 ms 一个、标准差 1 N 的随机扰动力。"滤波器假设的过程噪声"是卡尔曼滤波器以为的扰动大小与真实值之比：小了就过分相信模型，大了就过分相信测量。右：连续时间等效极点 \(s=\ln z/\Delta t\) 的位置（横轴实部，纵轴虚部，单位 s⁻¹）。琥珀色叉 = \(A-BK\) 的 4 个控制器极点，青色圈 = \((I-LC)A\) 的 4 个估计器极点，紫色大圆环 = 直接对 8 × 8 闭环矩阵求出的 8 个特征值。表中 RMS 取 1–10 s；摆杆倒下则显示"倒下"。
    试一试
    • 把角度噪声调到 1° 左右（1.0° 到 1.3°）：差分估速度的控制器把摆杆弄倒了，LQG 的 \(\theta\) 的 RMS 却仍在 1° 以内。噪声为什么会被差分放大？（每 10 ms 差一次，1° 的噪声变成多大的速度噪声？）
    • 保持 \(\sigma_\theta=0.5^\circ\)，把"滤波器假设的过程噪声"拉到 0.01 倍（过分相信模型），LQG 的 \(\theta\) RMS 涨到原来的 3 倍以上；再拉到 100 倍（过分相信测量），这时变差的是哪一项？

    默认设置（\(\sigma_\theta=0.5^\circ\)）下：理想 LQR 的力 RMS 是 0.37 N，LQG 是 0.68 N，差分估速度是 23.8 N。差分的力几乎一直在 30 N 的上限之间来回打，真实的电机会发热、齿轮会磨损，摆杆的 RMS 是 2.8°，LQG 是 0.49°（理想 LQR 是 0.18°）。卡尔曼滤波器用模型把各次测量"平均"起来：它知道摆杆不会在 10 ms 里突然改变 1 rad/s 的角速度。

    真实系统里

    平衡类机器人：两轮自平衡车、球形机器人（ballbot）、反作用轮倒立摆，常见的做法正是本节的流程：在竖直点线性化，离线解 Riccati 方程，在 100 Hz 到 1 kHz 的回路里做一次 \(\mathbf u=-K\hat{\mathbf x}\)，状态由 IMU 和编码器经滤波器估计。人形机器人：MIT 的 Atlas 控制器（Kuindersma 等，2016）把质心/ZMP 简化模型上时变 LQR 的最优剩余代价（cost-to-go）当作目标函数，放进每个控制周期求解、包含整机动力学与接触约束的二次规划里（13.7 节）。轨迹优化：iLQR/DDP 是很多运动生成工具的核心，例如面向足式机器人多接触最优控制的 Crocoddyl（FDDP 求解器），以及 MuJoCo MPC 里的 iLQG 规划器；它们反向传递得到的 \(K_k\) 常被直接用作跟踪反馈。软件：MATLAB 的 lqr/dlqr/lqi、python-control 的 lqr/dlqr、SciPy 的 solve_continuous_are/solve_discrete_are、Drake 的 LinearQuadraticRegulator 与 FiniteHorizonLinearQuadraticRegulator。边界：一旦约束（力矩、关节限位、摩擦锥）经常起作用，LQR 就让位给 13.4 节的 MPC；而 MPC 的终端代价通常就是这里的 \(\mathbf x^\top P\mathbf x\)。

    7常见误区

    误区 1：把权重当成有绝对意义的数

    \(Q\) 和 \(R\) 同时乘以任何正数，\(K\) 不变（只有 \(P\) 跟着缩放）；把角度从弧度换成度，\(Q\) 里对应的项就差了 \((180/\pi)^2\approx3283\) 倍。"\(Q=I,\ R=1\)"不是一个中性的默认值，它隐含着"1 m 的偏差、1 rad 的倾斜和 1 N 的力一样重要"。先用 Bryson 法则按物理单位写下可接受的范围，再根据仿真结果调整。

    误区 2：以为"最优"就是"好"

    LQR 只对你写下的代价和线性模型最优。它不知道力有上限（FIG 3：同样 30 N 的上限，贪心权重的吸引域只有温柔权重的约八分之一），不知道 \(\sin\theta\ne\theta\)，也不知道模型参数不准。闭环极点"更靠左"只说明线性模型里收敛更快，不说明真实系统更稳。上线前要在非线性、带饱和、带噪声和延迟的仿真里检验。

    误区 3：没有积分作用，却期待零稳态误差

    LQR 是纯比例的状态反馈。对 FIG 2 的默认控制器，小车上持续施加 1 N 的推力，小车最后会停在离目标约 5.4 cm 的地方，而且偏向推力的反方向（因为要靠小车位置那一项产生 −1 N 的力来抵消推力，而那一项的符号是"反的"）。和 13.1 节的 I 项一样，补救办法是把误差的积分 \(\int(x-x_{\mathrm{ref}})\,dt\) 加进状态里一起做 LQR（MATLAB 的 lqi），或者估计出扰动再前馈抵消。

    误区 4：在离散控制器里用连续时间的增益，或忽略延迟

    CARE 的增益假设控制力连续变化。控制器以 100 Hz 运行时，零阶保持本身就带来约半个周期的延迟，第 5 节里同一组权重的离散增益已经比连续增益小了约 6%；频率更低、增益更"贪心"时差别更大，FIG 5 里贪心控制器 16 ms 的延迟就足以失稳。用实际的采样周期离散化后解 DARE，把已知的计算和通信延迟也放进模型（例如给状态加上一拍延迟），再看 FIG 5 那样的裕度。

    误区 5：以为 LQG 继承了 LQR 的裕度

    分离原理保证 LQG 的标称闭环稳定，但 \([1/2,\infty)\) 的增益裕度和 60° 的相位裕度只对全状态 LQR 成立，加上观测器后可能任意小（Doyle，1978）。估计器越"相信测量"（FIG 6 里过程噪声假设得越大），就越接近差分，噪声越多地进到力里。要找回鲁棒性，可以用回路传递恢复（LTR）有意调整滤波器，或者直接用 \(H_\infty\)、\(\mu\) 综合这类鲁棒控制方法，至少要在带参数误差的仿真里检验 LQG。

    8小练习

    1. 把一个 LQR 问题的 \(Q\) 和 \(R\) 都乘以 10，会发生什么？
    代价 \(J\) 整体乘以 10，最小化它的 \(\mathbf u\) 不变，所以 \(K\) 不变；最优代价 \(\mathbf x^\top P\mathbf x\) 也乘以 10，即 \(P\to10P\)。代入 DARE 可以直接验证：\(K=(R+B^\top PB)^{-1}B^\top PA\) 中分子分母各多一个 10 倍。闭环极点由 \(K\) 决定，也不变。只有 \(Q\) 与 \(R\) 的相对大小才影响增益。
    2. FIG 1 的倒立单摆（\(mgl\approx4.9\) N·m/rad），控制权重 \(r\to\infty\) 时，最优增益 \(K^\star\) 会怎样？
    不稳定系统不控制，代价是无穷大，所以 \(K^\star\) 不可能趋于零；停在稳定边界上，响应永远不衰减，代价也是无穷大。"最省力"的稳定化把开环极点 \(+\sqrt{g/l}\) 翻到 \(-\sqrt{g/l}\)：闭环特征多项式 \((s+\sqrt{g/l})^2\) 要求 \(k_\theta=2mgl\approx9.8\) N·m/rad（连续时间），100 Hz 的离散版本是 9.6。FIG 1 的紫色虚线就停在那里。
    3. 对单输入、全状态反馈的连续时间 LQR，下列哪一条是有保证的？
    回差不等式 \(|1+L(j\omega)|\ge1\) 给出 \([1/2,\infty)\) 的增益裕度和 60° 的相位裕度。延迟裕度是相位裕度除以穿越频率，并没有统一的下界：FIG 5 的贪心控制器 16 ms 就失稳了。增益降到 1/2 以下不一定失稳，但不再有保证（FIG 5 的温柔权重在 0.47 处就失稳了）。加上观测器后保证全部消失（Doyle，1978）。
    4. 用 LQR 增益 \(K\) 和稳态卡尔曼增益 \(L\) 组成 LQG 控制器，闭环的极点是什么？
    闭环状态是 \((\mathbf x,\hat{\mathbf x})\)，共 8 维。换成 \((\mathbf x,\mathbf e)\) 坐标后矩阵是块三角的，误差 \(\mathbf e\) 的演化与 \(\mathbf x\) 无关，所以特征值是两个对角块的并集。这就是分离原理，FIG 6 右图的 8 个紫色圆环与 8 个小符号逐一重合。极点由矩阵决定，与噪声的具体取值无关。
    5. 按 Bryson 法则，倒立摆小车可接受的范围是：小车偏离 0.5 m，摆杆倾斜 10°，力 10 N（速度不加权）。\(Q\) 的位置项、角度项（角度用弧度）和 \(R\) 分别约是多少？
    \(1/0.5^2=4\)；10° = 0.1745 rad，\(1/0.1745^2\approx32.8\)；\(1/10^2=0.01\)。这正是 FIG 2 的默认设置，右上角公式里能看到同样的数。选项 A 忘了平方，选项 C 把倒数倒了过来，选项 D 把角度项和 \(R\) 弄反了。

    9延伸阅读

    • R. E. Kalman, Contributions to the Theory of Optimal Control, Boletín de la Sociedad Matemática Mexicana 5, 1960 — LQR 与 Riccati 方程的奠基论文
    • R. E. Kalman, When Is a Linear Control System Optimal?, J. Basic Engineering 86(1), 1964 — 回差不等式与"逆最优"问题
    • B. D. O. Anderson & J. B. Moore, Optimal Control: Linear Quadratic Methods, Prentice Hall, 1990 — LQR、LQG、鲁棒性与回路传递恢复的标准教材
    • A. E. Bryson & Y.-C. Ho, Applied Optimal Control, Hemisphere, 1975 — 最优控制的经典教材，按可接受范围选权重的思路常归于 Bryson
    • J. C. Doyle, Guaranteed Margins for LQG Regulators, IEEE Trans. Automatic Control 23(4), 1978 — "There are none."
    • M. G. Safonov & M. Athans, Gain and Phase Margin for Multiloop LQG Regulators, IEEE Trans. Automatic Control 22(2), 1977 — 多输入 LQR 的增益与相位裕度
    • A. J. Laub, A Schur Method for Solving Algebraic Riccati Equations, IEEE Trans. Automatic Control 24(6), 1979 — 今天的 Riccati 求解器的基础
    • D. H. Jacobson & D. Q. Mayne, Differential Dynamic Programming, Elsevier, 1970 — DDP 的原始专著
    • W. Li & E. Todorov, Iterative Linear Quadratic Regulator Design for Nonlinear Biological Movement Systems, ICINCO 2004 — iLQR
    • Y. Tassa, T. Erez & E. Todorov, Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization, IROS 2012 — 实时 iLQG 模型预测控制
    • R. Tedrake, I. R. Manchester, M. Tobenkin & J. W. Roberts, LQR-Trees: Feedback Motion Planning via Sums-of-Squares Verification, Int. J. Robotics Research 29(8), 2010 — 用 Lyapunov 椭圆拼出的反馈运动规划
    • R. Tedrake, Underactuated Robotics, MIT course notes — LQR、时变 LQR、吸引域与倒立摆小车，附 Drake 代码
    • S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter, T. Koolen, P. Marion & R. Tedrake, Optimization-based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot, Autonomous Robots 40(3), 2016 — 人形机器人上的时变 LQR 与 QP 控制