Part I · 第 6 章优化 · 6.12

动态规划

想知道从这里走到目标的最优路线，只需要知道"下一步走到哪里，从那里出发的最优代价是多少"。把这句话从终点往回递推，就是动态规划：它把一个指数级的路径搜索变成一张"到达目标的代价表"。Dijkstra、A*、LQR、值迭代、强化学习里的价值函数，都是同一个想法的不同面孔。

约 30 分钟前置：6.1 什么是优化问题 · 1.4 矩阵乘法 4 个交互

1为什么机器人需要它

机器人的许多问题都是"一步接一步的决策"：扫地机器人每一格往哪走，机械臂每个时刻施加多大的力矩，无人机每个控制周期怎样推力。每一步选得好不好，要看它对以后所有步骤造成的后果。如果穷举所有"动作序列"，选择数量随步数指数增长：3 个选择走 20 步就是 $3^{20}\approx3.5\times10^9$ 种。

动态规划 (dynamic programming, DP) 观察到一件事：整条最优路径的后半段，一定也是从那个中间点出发的最优路径。这条看似平凡的"最优性原理"（Bellman, 1950s）让我们不必再枚举路径，而是为每个状态记一个数：从这里出发到目标的最优代价，叫作代价到终点 (cost-to-go) 或价值函数。有了这张表，每个状态下的最优动作就是"走向代价到终点更小的邻居"。

它在机器人里无处不在：栅格地图上的最短路（Dijkstra、A*）是 DP；LQR（线性二次调节器）用 DP 导出解析的反馈增益；强化学习学的就是价值函数，贝尔曼方程是它的核心；轨迹优化里的 DDP/iLQR 在轨迹附近做 DP。本课先用最直观的格子世界建立图像，再看随机转移，最后看 DP 如何在连续状态里变成 LQR。

2直觉：从目标向外"涨水"

在格子地图上，每走进一个格子要付出该格的代价（普通格 1，泥地格 4），墙不能走。我们想知道每个格子到目标的最小总代价 $V(s)$。

值迭代从最笨的猜测开始：目标格 $V=0$，其余格 $V=\infty$（还不知道怎么到）。然后反复"扫一遍"，对每个格子取：

$$ V_{k+1}(s)=\min_{a}\Big[\ \hl{step}{c(s')}+\hl{cost}{V_k(s')}\ \Big],\qquad s'=\text{the cell reached by action }a,\qquad V(\text{goal})=0 $$

读作：一步的代价 + 邻居已知的最优代价，在所有动作里取最小。第 $k$ 次扫描之后，$V_k(s)$ 就是"至多走 $k$ 步能到目标的最小代价"。于是信息像水波一样从目标向外扩散，一圈一圈把每个格子的 $V$ 填上；再没有格子的值变化时就收敛了。把代价到终点画出来，最优路线就是沿着"最陡下坡"的方向，每个格子里的小箭头指向代价更低的邻居。

用下面的图玩一玩：拖动滑块看第 $k$ 次扫描的结果；在格子上点击或拖动涂墙、泥地，或改变目标；拖动 S 改变起点。

FIG 1格子世界里的值迭代

蓝色越浓表示到目标的代价越小，数字是 $V_k$，小箭头是当前贪心动作，琥珀色粗线是起点 S 按最终 $V$ 走出的最优路线。有琥珀色描边的格子是这一扫描里刚刚变化的格子：它们就是向外扩散的"波前"。"8 方向"允许走斜线（代价乘 $\sqrt2$）。

试一试

按"运行"，看波前从目标扩散到起点；记下它扩散到起点用了多少次扫描。
用"墙"工具把最优路线上的一个通道堵死：最优路线立刻绕行，起点的 $V$ 变大。
用"泥地"工具在最优路线上铺一块泥地：路线是穿过去还是绕过去，取决于绕路多出的代价是否小于每块泥地多付的 3（4 减去普通格的 1）。

核心洞见

值迭代没有"搜索路径"。它只是反复让每个格子问邻居"你到终点要多少"，自己取最小的一份加上一步的代价。最优路径是读出来的：从任何起点顺着代价下坡走即可。所以换一个起点完全不用重算，只要目标不变，这张表对所有起点都有效，这是 DP 与"为一个起点搜索一条路"的根本不同。

3贝尔曼最优性原理：为什么不用穷举

下图是一张小小的"多阶段图"：从起点 S 到终点 G 要经过 4 个阶段，每个阶段 3 个节点，相邻阶段之间每对节点都有一条带代价的边。从 S 到 G 一共有 $3^4=81$ 条路径。穷举要把 81 条路径的代价都加一遍；DP 从 G 往回算，每个节点的 $J$ 只需要看它出发的 3 条边。拖动滑块让 DP 一个阶段一个阶段往回走。

每个节点的数字 $J$ 是"从这里到 G 的最小代价"。看第一个节点是怎么算出来的：它是 3 个"边的代价 + 下一个节点的 $J$"里最小的，下一个节点的 $J$ 已经是最优了，所以不必再看它之后的所有路径。

FIG 2多阶段图上的反向递推

边上的数字是代价，点击数字可把它加 1（超过 9 回到 1）。节点上方是 $J$。绿色粗边是每个节点的最优后继（$\arg\min$）。滑块到 0 时，沿着绿边从 S 走到 G 的琥珀色路径就是全局最优。试着改一条最优路径上的边，看看 DP 重新给出的路径。

把滑块从右拖到左，观察 $J$ 是怎么一个阶段一个阶段往回填满的。
点击最优路径上的一个边代价，把它加大，直到最优路径改道。

两种计算量

$N$ 个阶段、每阶段 $m$ 个节点：穷举要比较 $m^N$ 条路径；DP 只需要 $O(N m^2)$ 次"边的代价 + 后继的 $J$"。在 $m=3,\ N=4$ 时是 81 对 33。$N=20$ 时是 $3^{20}\approx3.5\times10^9$ 对 $\approx 180$。DP 把乘法关系变成了加法关系：这是它最重要的好处。

4加入随机性：离悬崖远一点

真实的机器人不是想走哪就走到哪：轮子打滑，风吹歪了无人机。设想"悬崖行走"：目标在右边，最下面一排是悬崖（掉下去代价 100，游戏结束）。我们执行一个动作时，以概率 $1-p$ 如愿以偿，以概率 $p/2$ 各偏向左右两个垂直方向（"打滑"）。此时下一个状态是随机的，值迭代里的"邻居的 $V$"换成期望：

$$ V(s)=\min_a\ \sum_{s'}\ \hl{prob}{P(s'\mid s,a)}\,\big[\,c(s,a,s')+V(s')\,\big] $$

当 $p=0$ 时最短路径紧贴悬崖边走。当 $p$ 变大，紧贴悬崖的路线每走一格都有 $p/2$ 的概率掉下去，期望代价迅速上升，DP 自己就会把路线移到离悬崖更远的一排。这种"根据噪声自动留出安全余量"的行为，是规划不确定性时 DP 的强项（也是强化学习学到的东西）。拖动 $p$，对比"最优"策略和"按无噪声最短路径照走"的天真策略。

FIG 3打滑的格子世界：最优策略随噪声改变

红色格子是悬崖（$V=100$），绿圈是目标。箭头是当前选定策略在各格的动作；琥珀色线是"每步都成功"时的路线。右侧读数里的"掉崖概率"是按所选策略从起点出发、在打滑噪声下最终掉下悬崖的概率（由策略评估精确算出，不是蒙特卡洛）。

从 $p=0$ 慢慢加大噪声，找到最优路线从紧贴悬崖跳到远离悬崖的那个 $p$（大约 1%）。
设 $p=0.2$，切到"天真策略"：它的期望代价和掉崖概率是最优策略的多少倍？

与强化学习的联系

上面这个带"期望"的方程叫贝尔曼最优方程，$P(s'|s,a)$ 是环境模型。值迭代是在知道模型时解它的方法。强化学习处理"不知道 $P$，只能通过试错采样"的情形：Q-learning、DQN 都是在用采样去逼近同一个方程的不动点。因此本课的值迭代，是理解后续强化学习的第一块基石（Part V）。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 1、FIG 3 中对应的元素会被高亮。

有限时域 DP

系统 $\mathbf x_{k+1}=f(\mathbf x_k,\mathbf u_k)$，总代价 $\sum_{k=0}^{N-1}\ell(\mathbf x_k,\mathbf u_k)+\ell_f(\mathbf x_N)$。定义价值函数 $V_k(\mathbf x)$：从第 $k$ 步状态 $\mathbf x$ 出发能得到的最小剩余代价。贝尔曼方程从终点往回递推：

$$ V_N(\mathbf x)=\ell_f(\mathbf x),\qquad V_k(\mathbf x)=\min_{\mathbf u}\Big[\ell(\mathbf x,\mathbf u)+V_{k+1}\big(f(\mathbf x,\mathbf u)\big)\Big],\qquad \pi_k(\mathbf x)=\arg\min_{\mathbf u}\Big[\cdots\Big] $$

$V_0(\mathbf x_0)$ 就是整个问题的最优代价，$\pi_k$ 是每步的最优反馈策略：它是状态的函数，不只是一条开环轨迹，所以遇到扰动、偏离轨迹时仍然知道该怎么办。这一点和 6.13 节的轨迹优化（只给一条轨迹）形成对比。

无限时域与值迭代的收敛

带折扣因子 $\gamma\in[0,1)$ 的无限时域问题，价值函数满足不动点方程 $V=\mathcal T V$，其中 $\mathcal T$ 是"贝尔曼算子"。它是压缩映射：

$$ (\mathcal T V)(s)=\min_a\sum_{s'}P(s'|s,a)\big[c+\gamma V(s')\big],\qquad \|\mathcal T V-\mathcal T W\|_\infty\le\gamma\,\|V-W\|_\infty\ \Longrightarrow\ \|V_k-V^\star\|_\infty\le\gamma^k\|V_0-V^\star\|_\infty $$

由巴拿赫不动点定理：不动点 $V^\star$ 唯一，且值迭代从任何初值出发都几何级数地收敛到它。FIG 1 的最短路问题相当于 $\gamma=1$，并没有压缩性，但从 $V=\infty$ 出发时，第 $k$ 次扫描恰好得到"至多 $k$ 步"的最优值，所以最多 $|S|$ 次扫描收敛（Bellman–Ford 的思路）。更聪明的"按代价从小到大扩展"的次序就是 Dijkstra 算法：每个格子只处理一次。

LQR：连续状态里的 DP

当动力学是线性的 $\mathbf x_{k+1}=A\mathbf x_k+B\mathbf u_k$、代价是二次的 $\mathbf x^\top Q\mathbf x+\mathbf u^\top R\mathbf u$（终端 $\mathbf x^\top Q_f\mathbf x$）时，贝尔曼方程可以精确求解：价值函数仍然是二次的，$V_k(\mathbf x)=\mathbf x^\top P_k\mathbf x$，只需递推一个矩阵（Riccati 递推）：

$$ P_N=Q_f,\qquad K_k=\big(R+B^\top P_{k+1}B\big)^{-1}B^\top P_{k+1}A,\qquad P_k=Q+A^\top P_{k+1}\big(A-BK_k\big),\qquad \mathbf u_k=-K_k\mathbf x_k $$

推导只有两步：把 $V_{k+1}=\mathbf x^\top P_{k+1}\mathbf x$ 代入贝尔曼方程，里面的 $\min_{\mathbf u}$ 是 $\mathbf u$ 的二次函数，令导数为零得到 $\mathbf u=-K_k\mathbf x$；再把它代回去得到新的二次型 $P_k$。当时域很长时 $P_k$ 收敛到常数矩阵 $P_\infty$，增益 $K$ 也不再变化（稳态 LQR，FIG 4 里能看到）。这是 DP 在连续空间里仅有的几个能不"离散化"的例子，也是控制课里最重要的结论之一（Part IV）。

维数灾难

离散化的 DP 要存一张覆盖所有状态的表。每一维取 $n$ 个格点、$d$ 维状态，就有 $n^d$ 个状态：$n=100$，$d=2$ 是 $10^4$（FIG 1 只有 $12\times8$ 格），$d=6$（比如一个 3 关节机械臂的位置和速度各 3 维）就是 $10^{12}$。这就是 Bellman 所说的"维数灾难"，也是为什么机器人里的高维问题要用近似：线性二次（LQR）、在轨迹附近局部近似（DDP/iLQR）、或者用神经网络表示价值函数（深度强化学习）。

6机器人中的它：Riccati 递推与 LQR

一个一维小车（双积分器）：状态 $\mathbf x=(p,v)$，控制 $u$ 是加速度，$\Delta t=0.1$ s，$A=\left[\begin{smallmatrix}1&\Delta t\\0&1\end{smallmatrix}\right],\ B=\left[\begin{smallmatrix}\Delta t^2/2\\ \Delta t\end{smallmatrix}\right]$。代价 $Q=\operatorname{diag}(1,0.1)$，$Q_f=Q$，$R=r$：要把小车平稳地停在原点。下图是 $k=0$ 时的价值函数 $V_0(\mathbf x)=\mathbf x^\top P_0\mathbf x$ 的等值线（椭圆）：越靠内的椭圆，剩余代价越小。拖动橙色的初始状态 $\mathbf x_0$，看小车沿着 DP 求出的反馈律回到原点的轨迹。

读数里有一个重要的"验算"：用 DP 递推出的 $P_0$ 算的 $V_0(\mathbf x_0)=\mathbf x_0^\top P_0\mathbf x_0$，应该和真的仿真一遍这条闭环轨迹并把每一步的代价累加起来的结果完全相同。这就是"价值函数 = 最优剩余代价"的含义。

FIG 4LQR：价值函数是椭圆

反馈增益 $K_k$ 随时刻 $k$

左：相平面（横轴位置 $p$，纵轴速度 $v$），紫色椭圆是 $V_0$ 的等值线，通过 $\mathbf x_0$ 的那条加粗；琥珀色是有限时域最优反馈（时变 $K_k$）的闭环轨迹，青色虚线是稳态增益 $K_\infty$ 的轨迹。右：增益的两个分量 $K_k=(k_p,k_v)$：离终点很远时它们是常数（等于稳态 LQR），只在接近终点的最后几步才变化。$r$ 越大，控制越"吝啬"，同一个 $\mathbf x_0$ 的剩余代价越大（椭圆收紧）并且椭圆向速度轴倾斜，轨迹越慢。

拖动 $\mathbf x_0$：轨迹形状变了吗？价值 $V_0(\mathbf x_0)$ 随 $\mathbf x_0$ 怎么变？（二次增长：把 $\mathbf x_0$ 放大一倍，$V_0$ 变成 4 倍。）
把 $r$ 从 0.01 拖到 10：增益 $K$ 变小，轨迹变慢，椭圆收紧并转向速度轴。
把时域 $N$ 缩到 5：增益不再是常数，整条轨迹的 $K_k$ 都随时间变化；加长 $N$，开头部分的 $K_k$ 趋于稳态。

真实系统里

LQR 是两轮自平衡车、倒立摆、四旋翼悬停的标准控制器：把非线性系统在平衡点附近线性化，解 Riccati 方程得到常数增益 $K$，实时代价只是一次矩阵乘法 $\mathbf u=-K\mathbf x$。把同样的递推应用到"沿一条轨迹线性化"的系统上，就是 iLQR/DDP，是轨迹优化（6.13 节）和接触丰富的机器人运动规划的常用求解器。栅格上的 DP 则是 Dijkstra、A*、Dubins/Reeds–Shepp 状态格规划的内核。

7常见误区

误区 1：以为 DP 只能做离散问题

贝尔曼方程对连续状态同样成立（FIG 4 的 LQR 就是）。只是在一般情形下，$V$ 是个没有解析形式的函数，只能离散化、或用函数逼近（插值、神经网络）。线性二次和少数结构化问题是例外，价值函数有闭式。

误区 2：把"贪心"与"最优"混为一谈

"每一步走离目标最近的邻居"是贪心，会撞进死胡同（FIG 1 里把墙堵死试试）。DP 里的"贪心"是对 $V$ 贪心，而 $V$ 已经包含了后续所有步骤的最优后果，所以局部的一步选择才是全局最优的。区别在于你贪的是哪张表。

误区 3：忽略折扣、环路和"不可达"

无折扣 ($\gamma=1$) 的问题只有在"所有策略最终都会到达吸收状态"或"环路代价为正"时才有有限的最优值；否则值迭代会发散，或给出"原地打转"的退化策略。真实代价里要保证"每走一步都有正的代价"，或者设置 $\gamma<1$。另外 $V=\infty$ 的格子（目标不可达）在数值上要用一个大数或显式处理，别让 $\infty-\infty$ 出现 NaN。

误区 4：Riccati 递推的方向搞反

$P_k$ 是从终点 $N$ 往回算的，而闭环仿真是从 $k=0$ 往前走的：先把所有 $K_k$ 反向算出来存好，再前向使用。把 $K_k$ 的下标当成"前向递推"会得到错误的增益。若 $R+B^\top PB$ 接近奇异（$R$ 太小而 $B$ 方向有问题），求逆会数值不稳；LQR 要求 $Q\succeq0,\ R\succ0$。

8小练习

1. 节点 A 有两条出边：到 B 的边代价 2，到 C 的边代价 5。已知 $J(B)=4,\ J(C)=2$。$J(A)$ 和最优后继是？

$J(A)=\min(2+4,\ 5+2)=\min(6,7)=6$，经 B。注意不能只看边的代价（2 < 5）：到 C 的边贵，但 C 之后更便宜；也不能只看 $J$（$J(C)<J(B)$）。要看"边 + $J$"的和。

2. 一张多阶段图有 10 个阶段，每个阶段 5 个节点，相邻阶段的每对节点之间都有边。DP 要计算多少条边的代价（"边 + 后继 $J$"）？（穷举有 $5^{10}\approx9.8\times10^6$ 条路径。）

相邻阶段之间有 $5\times5=25$ 条边，10 个阶段有 9 个这样的间隔，所以 $9\times25=225$ 次。从百万量级降到几百，这就是 DP 的威力。

3. 在 FIG 3 的悬崖行走里，打滑概率 $p$ 增大时，最优路线为什么离悬崖越来越远？

DP 的"期望"把掉崖的小概率乘上很大的惩罚后加了进去；沿边走 $\approx7$ 格，累计掉崖概率可观，期望惩罚远大于多绕两步的 2。只要 $p$ 超过一个很小的阈值，DP 就自己选择了更安全的路线。

4. 价值函数 $V(\mathbf x)=\mathbf x^\top P\mathbf x$，其中 $P=\left[\begin{smallmatrix}2&0\\0&0.5\end{smallmatrix}\right]$。状态 $\mathbf x=(1,2)$ 的剩余代价是？

$\mathbf x^\top P\mathbf x=2\cdot1^2+0.5\cdot2^2=2+2=4$。

5. 把一个有 6 个状态变量的系统，每个变量离散成 50 个格点，做表格式 DP。状态总数约为？

$50^6=1.5625\times10^{10}$。状态数随维度指数增长，这就是维数灾难。所以高维问题要用 LQR 这类解析结构、局部近似（DDP/iLQR）或函数逼近（神经网络）。

9延伸阅读

R. S. Sutton & A. G. Barto, Reinforcement Learning: An Introduction, 2nd ed., MIT Press, 2018, Ch. 3–4 — MDP、贝尔曼方程与动态规划（含悬崖行走例子）
D. P. Bertsekas, Dynamic Programming and Optimal Control, Vol. I, 4th ed., Athena Scientific, 2017 — DP 的经典教材：最优性原理、最短路与 LQR
R. Bellman, Dynamic Programming, Princeton University Press, 1957 — 原始来源，"维数灾难"一词出自此书
B. D. O. Anderson & J. B. Moore, Optimal Control: Linear Quadratic Methods, Prentice-Hall, 1990 — LQR 与 Riccati 方程
E. W. Dijkstra, A note on two problems in connexion with graphs, Numerische Mathematik, 1959 — 最短路算法，是按序扩展的值迭代