""" ============================================================================= 第 6 讲 · 代码 4：LQR、Kalman 滤波（LQG）、线性化的极限，以及 RL 调试的三个问题 ============================================================================= 对应讲义：lectures/lecture06_rl_control.md 第 4 节（演示 1–4）和第 6 节（演示 5） ----------------------------------------------------------------------------- 【这个文件要回答的问题 What this file answers】 ----------------------------------------------------------------------------- 连续状态的一个特殊情形有精确解：线性动力学 + 二次代价 = LQR（linear quadratic regulator）。 V 恰好是二次函数，最优策略恰好是线性反馈 a = −K s，Riccati 递推一步算出来，不需要离散化也不需要 FVI。 演示 1：Riccati 递推。有限时域的增益 K_t 怎样随时间变化？时域越长，K_0 怎样收敛到稳态增益 K∞？ 演示 2：LQR vs 手调的 PD 控制器；过程噪声不改变最优增益（certainty equivalence），只增加一个常数代价。 演示 3：非线性系统（倒立摆）上的 LQR：在线性化点附近很好用；力矩有上限时，离远了就不行 → DDP / iLQR 的动机。 演示 4：只能观测到带噪声的位置：Kalman 滤波 + LQR（LQG）vs 直接用噪声观测（速度用差分）。 演示 5：Andrew Ng 的 RL 调试流程：策略表现不好，是模拟器错了、RL 算法错了，还是奖励（代价）函数错了？ ----------------------------------------------------------------------------- 【运行方式】 ----------------------------------------------------------------------------- python3 04_lqr_lqg.py # 约 1 秒 需要 numpy。依赖本目录的 rl_tools.py。 """ import numpy as np from rl_tools import (double_integrator, expected_cost_linear, kalman_predict, kalman_update, lqr_finite_horizon, lqr_stationary, print_header, rollout_linear) # 全文件共用的设置：双积分器（小车在直线上，s = [位置, 速度]，a = 力），dt = 0.1 s，T = 100 步（10 秒） DT, T = 0.1, 100 A, B = double_integrator(DT, mass=1.0) U = np.diag([1.0, 0.1]) # 状态代价：主要惩罚位置偏差，也轻微惩罚速度 W = np.array([[0.1]]) # 动作代价：惩罚用力（燃料 / 电机负担） S0 = np.diag([1.0, 0.25]) # 初始状态的分布 N(0, S0)：位置标准差 1，速度标准差 0.5 SW = np.diag([0.0, 0.01]) # 过程噪声：每步速度上的随机扰动（阵风），标准差 0.1 def fmt(v, spec=".3f"): """把向量格式化成 [a, b] 的字符串。""" return "[" + ", ".join(format(float(x), spec) for x in np.ravel(v)) + "]" # ============================================================================= # 演示 1：Riccati 递推与增益的收敛 # ============================================================================= def demo_riccati(): print_header("演示 1：Riccati 递推 —— 有限时域增益 K_t，以及它向稳态增益 K∞ 的收敛") K, P, _ = lqr_finite_horizon(A, B, U, W, T) K_inf, P_inf, n_it = lqr_stationary(A, B, U, W) print(f"双积分器 dt = {DT}，U = diag(1, 0.1)，W = 0.1，T = {T}。稳态增益 K∞ = {fmt(K_inf)}" f"（迭代 Riccati {n_it} 次收敛）。\n") print("(a) 同一个 T = 100 的问题里，不同时刻 t 的最优增益（a_t = −K_t s_t）：\n") print(f"{'t':>5}{'steps to go':>13}{'K_t (position, velocity)':>28}{'‖K_t − K∞‖':>14}") for t in [0, 50, 80, 90, 95, 97, 98, 99]: print(f"{t:>5}{T - t:>13}{fmt(K[t]):>28}{np.max(np.abs(K[t] - K_inf)):>14.1e}") print("\n(b) 时域 T 越长，第一步的增益 K_0 越接近 K∞：\n") print(f"{'T':>5}{'K_0':>20}{'‖K_0 − K∞‖':>14}") gaps = {} for TT in [1, 2, 5, 10, 20, 30, 50]: K_T, _, _ = lqr_finite_horizon(A, B, U, W, TT) gaps[TT] = np.max(np.abs(K_T[0] - K_inf)) print(f"{TT:>5}{fmt(K_T[0]):>20}{gaps[TT]:>14.1e}") s0 = np.array([1.0, 0.0]) cost, states, actions = rollout_linear(A, B, U, W, lambda t, s: -K[t] @ s, s0, T) print(f"\n(c) 从 s0 = [1, 0] 出发：Riccati 预测的最优代价 s0ᵀP_0 s0 = {s0 @ P[0] @ s0:.6f}，" f"实际执行的代价 = {cost:.6f}") print(f""" 观察 Observations： · 离终点很远时（t ≤ 50），K_t 和 K∞ 完全一样；只有最后十来步，增益迅速变小（t = 99 时只剩 {fmt(K[99], '.2f')}）。 直觉：快结束时，"现在用力、以后享受小偏差"的买卖不划算了 —— 以后已经没有多少步了。 · 所以有限时域 LQR 的增益在大部分时间里是常数。时域每多 10 步，‖K_0 − K∞‖ 大约缩小 {gaps[10] / gaps[20]:.0f} 倍： Riccati 递推本身就是一个收缩的不动点迭代（和价值迭代一样；它就是线性-二次情形下的价值迭代）。 这就是"无限时域 LQR = 迭代 Riccati 到不动点"，也是实践中常常直接用 K∞ 的理由。 · (c) 预测代价和实际代价逐位相同：LQR 的 V_t(s) = sᵀP_t s 是**精确的**，不是近似。连续状态、无穷多个状态， 却只需要一个 2×2 的矩阵 —— 这就是线性 + 二次结构的威力（对比 03 里的离散化和 FVI）。""") return K_inf # ============================================================================= # 演示 2：LQR vs 手调 PD；certainty equivalence # ============================================================================= def demo_compare_controllers(K_inf): print_header("演示 2：LQR vs 手调的 PD 控制器；噪声不改变最优增益（certainty equivalence）") K, P, c = lqr_finite_horizon(A, B, U, W, T, noise_cov=SW) print("PD 控制器 a = −(kp·位置 + kd·速度)，正是一个线性反馈 a = −[kp, kd] s。") print("期望代价用协方差传播精确计算（s0 ~ N(0, S0)，过程噪声 N(0, Σ_w)），没有抽样误差。\n") rows = [("LQR (time-varying K_t)", K), ("LQR stationary K_inf", K_inf)] for kp, kd in [(1, 1), (3, 2), (10, 4), (30, 10)]: rows.append((f"hand-tuned PD ({kp}, {kd})", np.array([[float(kp), float(kd)]]))) print(f"{'controller':<26}{'gain (kp, kd)':>18}{'E[cost] with noise':>20}{'E[cost] no noise':>18}") for name, Kc in rows: gain = fmt(Kc[0] if isinstance(Kc, list) else Kc, ".2f") print(f"{name:<26}{gain:>18}{expected_cost_linear(A, B, U, W, Kc, S0, T, SW):>20.3f}" f"{expected_cost_linear(A, B, U, W, Kc, S0, T):>18.3f}") predicted = float(np.trace(P[0] @ S0) + c[0]) print(f"\nRiccati 的预测：E[cost] = tr(P_0 S0) + c_0 = {np.trace(P[0] @ S0):.3f} + {c[0]:.3f} = {predicted:.3f}" "（c_0 = Σ_t tr(Σ_w P_(t+1)) 就是噪声带来的额外代价）") # 暴力搜索最好的 PD 增益：有噪声和没噪声时，最好的增益是不是同一个？ grid = np.arange(0.5, 6.01, 0.25) best = {} for label, noise in [("with noise", SW), ("no noise", None)]: costs = np.array([[expected_cost_linear(A, B, U, W, np.array([[kp, kd]]), S0, T, noise) for kd in grid] for kp in grid]) i, j = np.unravel_index(np.argmin(costs), costs.shape) # 【numpy 小知识】把"展平后的下标"换回二维下标 best[label] = (grid[i], grid[j], costs[i, j]) print(f"\n在 kp, kd ∈ {{0.5, 0.75, …, 6}} 的网格上暴力搜索最好的 PD 增益：") for label, (kp, kd, cst) in best.items(): print(f" {label:<11} best (kp, kd) = ({kp:.2f}, {kd:.2f}), E[cost] = {cst:.3f}") print(f""" 观察 Observations： · LQR 的期望代价最低（{expected_cost_linear(A, B, U, W, K, S0, T, SW):.3f}）。手调的 PD：太软（1, 1）追不上，太硬（30, 10）用力太多， （3, 2）已经不错但仍差一点。对双积分器，LQR 的稳态增益本身**就是**一个 PD 控制器， 网格搜索找到的最好 PD 增益 ({best['with noise'][0]:.2f}, {best['with noise'][1]:.2f}) ≈ K∞ = {fmt(K_inf, '.2f')}。 LQR 替你"调参"：给定 U、W，最优的 kp、kd 一步算出来。但 U、W 本身还得人来选 —— 调参变成了设计代价函数（演示 5）。 · Certainty equivalence：噪声改变了代价（每一行"有噪声"都比"没噪声"大），但**不改变最优增益**： Riccati 递推里 K_t 不依赖 Σ_w，暴力搜索在有噪声和没噪声时也找到同一组增益。 噪声只贡献一个和策略无关的常数 c_0 = {c[0]:.3f}（"不管你怎么控制，阵风都会把你吹偏这么多"）。 · Riccati 的预测 {predicted:.3f} 和精确计算的 LQR 期望代价一致。""") # ============================================================================= # 演示 3：非线性系统上的 LQR —— 线性化的极限 # ============================================================================= def demo_pendulum(): print_header("演示 3：倒立摆 —— 在竖直向上的点线性化做 LQR；力矩有上限时离远了就不行") g, dt, steps = 9.81, 0.02, 300 # m = l = 1；θ = 0 表示竖直向上 A_lin = np.array([[1.0, dt], [g * dt, 1.0]]) # sin θ ≈ θ 的线性化 B_lin = np.array([[0.0], [dt]]) K, _, _ = lqr_stationary(A_lin, B_lin, np.diag([1.0, 0.1]), np.array([[0.01]])) thetas = np.array([0.1, 0.3, 0.5, 0.8, 1.0, 1.5, 2.0, 2.5, 3.0]) print("真实动力学 θ'' = g·sin θ + u（非线性），线性化 θ'' ≈ g·θ + u。LQR 增益 K = " f"{fmt(K, '.2f')}，从静止的初始角度 θ0 出发跑 6 秒，") print("最后 |θ| < 0.05 且 |ω| < 0.1 算成功（1）。力矩被截断到 |u| ≤ u_max。mgl = 9.81：u_max 小于它时，") print("连“水平”位置都撑不住。\n") print(f"{'u_max':>8} " + "".join(f"{t:>5.1f}" for t in thetas) + f"{' max |u| used':>15}") success, peaks = {}, {} for u_max in [np.inf, 20.0, 10.0, 5.0, 3.0]: th, om = thetas.copy(), np.zeros_like(thetas) u_peak = np.zeros_like(thetas) for _ in range(steps): u = np.clip(-(K[0, 0] * th + K[0, 1] * om), -u_max, u_max) u_peak = np.maximum(u_peak, np.abs(u)) om = om + dt * (g * np.sin(th) + u) # semi-implicit Euler：先更新速度，再用新速度更新角度 th = th + dt * om wrapped = np.angle(np.exp(1j * th)) # 把角度折回 (−π, π]（转了整圈也算回到原点） ok = (np.abs(wrapped) < 0.05) & (np.abs(om) < 0.1) success[u_max] = ok peaks[u_max] = u_peak.max() label = "inf" if np.isinf(u_max) else f"{u_max:g}" print(f"{label:>8} " + "".join(f"{int(x):>5}" for x in ok) + f"{u_peak.max():>15.1f}") print("（列：初始角度 θ0，单位 rad；π ≈ 3.14 是竖直向下。最后一列是这一行所有初始角度里用过的最大力矩。）") lim5 = np.arcsin(5.0 / g) lim3 = np.arcsin(3.0 / g) print(f""" 观察 Observations： · 力矩不限时，线性化的 LQR 从 θ0 = 3.0 rad（离竖直向下只差 8°）都能救回来 —— 代价是峰值力矩 {peaks[np.inf]:.0f}（≈ {peaks[np.inf] / g:.0f} 倍 mgl）：只要力气够大，"偏了就往回推"这条线性规则在很大范围内都对。 · 力矩上限低于 mgl 时（u_max = 5、3），只在 θ0 ≤ {thetas[success[5.0]].max():.1f}、{thetas[success[3.0]].max():.1f} 时成功。 这几乎就是"静止地撑住"的极限 arcsin(u_max / mgl) = {lim5:.2f}、{lim3:.2f}：再远一点，任何控制器都没法直接推上去， 必须先往反方向摆、一来一回地"荡"上去（swing-up）—— 这是一个非线性的计划，线性模型根本表示不出来 （在线性模型里，θ 再大也只是"多推一点"）。 · 这正是 DDP / iLQR 的动机（讲义 §4.4）：不在一个固定点线性化，而是沿着当前猜测的**轨迹**线性化、 做一次时变 LQR 得到新轨迹，再沿新轨迹线性化 …… 迭代下去。它能找到 swing-up 这样的非线性轨迹。""") # ============================================================================= # 演示 4：LQG —— Kalman 滤波 + LQR # ============================================================================= def demo_lqg(): print_header("演示 4：只能观测带噪声的位置 —— Kalman 滤波 + LQR（LQG）vs 直接用观测") K, _, _ = lqr_finite_horizon(A, B, U, W, T) C = np.array([[1.0, 0.0]]) # 只观测位置：y = p + v，v ~ N(0, σ²) runs = 2000 print(f"观测 y_t = 位置 + N(0, σ²)，速度看不到。每种设置 {runs} 次模拟（三种控制器用相同的随机数）。") print(" oracle : 直接拿到真实状态（理想情况，做不到）") print(" LQG : Kalman 滤波估计 [位置, 速度]，把估计值代入 LQR：a = −K_t ŝ_t") print(" naive : 位置 = y_t，速度 = (y_t − y_(t−1)) / dt，代入同一个 LQR\n") print(f"{'sigma':>6}{'controller':>11}{'mean cost':>11}{'(SE)':>8}{'RMS err pos':>13}{'RMS err vel':>13}") out = {} for sigma in [0.1, 0.3, 1.0]: Sv = np.array([[sigma ** 2]]) for mode in ["oracle", "LQG", "naive"]: rng = np.random.default_rng(43) # 同一个种子：三种控制器面对完全相同的噪声 S = rng.multivariate_normal(np.zeros(2), S0, size=runs).T # (2, runs)：每一列是一条轨迹的状态 mean, cov = np.zeros((2, runs)), S0.copy() # Kalman 的先验 = 初始分布；cov 和数据无关，所有轨迹共用 prev_y = None cost = np.zeros(runs) sq_err = np.zeros(2) for t in range(T): y = C @ S + sigma * rng.standard_normal((1, runs)) mean, cov = kalman_update(mean, cov, y, C, Sv) if mode == "oracle": est = S elif mode == "LQG": est = mean else: vel = np.zeros(runs) if prev_y is None else (y[0] - prev_y) / DT est = np.vstack([y[0], vel]) prev_y = y[0] sq_err += np.mean((est - S) ** 2, axis=1) a = -K[t] @ est # (1, runs) cost += np.einsum("ir,ij,jr->r", S, U, S) + W[0, 0] * a[0] ** 2 # 每条轨迹的 sᵀUs + aᵀWa S = A @ S + B @ a + np.sqrt(SW[1, 1]) * np.vstack([np.zeros(runs), rng.standard_normal(runs)]) mean, cov = kalman_predict(mean, cov, a, A, B, SW) cost += np.einsum("ir,ij,jr->r", S, U, S) rms = np.sqrt(sq_err / T) out[(sigma, mode)] = (cost.mean(), rms) err_s = ("-", "-") if mode == "oracle" else (f"{rms[0]:.3f}", f"{rms[1]:.3f}") print(f"{sigma:>6}{mode:>11}{cost.mean():>11.2f}{cost.std() / np.sqrt(runs):>8.2f}{err_s[0]:>13}{err_s[1]:>13}") exact = expected_cost_linear(A, B, U, W, K, S0, T, SW) o, l, n = (out[(0.3, m)] for m in ["oracle", "LQG", "naive"]) print(f""" （oracle 的精确期望代价是 {exact:.3f}；表中 oracle 的平均值和它的差别在抽样误差之内。） 观察 Observations： · 差分求速度把观测噪声放大了约 √2/dt ≈ {np.sqrt(2) / DT:.0f} 倍：σ = 0.3 时 naive 的速度误差 {n[1][1]:.2f}，LQR 拿着这么吵的速度去用力， 代价 {n[0]:.0f}，是 oracle（{o[0]:.1f}）的 {n[0] / o[0]:.0f} 倍。σ 越大越糟。 · Kalman 滤波利用了"动力学模型"：位置的变化必须和速度、力一致。它把 100 步的观测按模型融合起来， 速度误差只有 {l[1][1]:.2f}，位置误差 {l[1][0]:.3f} 甚至小于观测本身的噪声 0.3。LQG 的代价 {l[0]:.1f}： 是 oracle 的 {l[0] / o[0]:.1f} 倍（看不到真实状态总要付出代价），但只有 naive 的 1/{n[0] / l[0]:.0f}。 · 把估计值直接代入 LQR 不只是一个"合理的近似"：在线性-高斯-二次的设定下它就是最优的 （separation principle：估计和控制可以分开设计；CS229 main notes 第 16 章的 LQG 一节）。 · Kalman 滤波就是第 5 讲"高斯的条件分布"在时间上的递推；它的非线性推广（EKF、粒子滤波）是机器人定位的标配。""") # ============================================================================= # 演示 5：RL 调试 —— 模拟器？算法？奖励？ # ============================================================================= def diagnose(q_sim_rl, q_real_rl, q_real_human, J_rl, J_human, slack=1.25): """ Andrew Ng 的 RL 调试流程（CS229 Autumn 2018 Lecture 20 的直升机例子）。 【思路】我们有：模拟器、RL 算法（在模拟器上最大化奖励 / 最小化代价）、奖励函数，以及一个"飞得好"的人类驾驶员。 设计假设是：(1) 模拟器准确；(2) RL 算法能在模拟器里找到代价最小的策略；(3) 代价最小 ⇔ 飞得好。 RL 策略在真实世界飞得不好时，依次检查： 1. π_RL 在模拟器里飞得好、在真实世界飞得不好 → 模拟器的问题（假设 1 错了）。 2. 否则比较代价：如果人类策略的代价 J(π_human) < J(π_RL)，说明有一个代价更低的策略 RL 却没找到 → RL 算法的问题（假设 2 错了）。 3. 否则 J(π_RL) ≤ J(π_human)：RL 确实把代价降得比人类还低，却飞得更差 → 代价函数的问题（假设 3 错了）。 【参数】q_*：飞行质量（这里是位置误差的 RMS，越小越好，是我们**真正**关心的东西）； J_*：在真实系统上按 RL 使用的代价函数算出的期望代价；slack：比人类差多少以内算"飞得好" 【返回】诊断结论（英文短语，表格里用） """ good = lambda q: q <= slack * q_real_human if good(q_real_rl): return "nothing to fix" if good(q_sim_rl): return "simulator" if J_human < J_rl: return "RL algorithm" return "cost/reward function" def demo_debugging(): print_header("演示 5：RL debugging —— 策略飞得不好，是模拟器、RL 算法还是代价函数的错？") U_pos, W_zero = np.diag([1.0, 0.0]), np.zeros((1, 1)) def quality(A_sys, B_sys, K): """飞行质量：整个回合位置误差的 RMS（精确的期望值），这是"我们真正关心的东西"。""" return np.sqrt(expected_cost_linear(A_sys, B_sys, U_pos, W_zero, K, S0, T, SW) / (T + 1)) K_human = np.array([[3.0, 2.0]]) # 人类驾驶员：演示 2 里那个不错的 PD A_real, B_real = A, B # 真实系统：质量 1 U_wrong = np.diag([0.0, 1.0]) # 设计者"忘了"惩罚位置，只惩罚速度 scenarios = [] # 场景 0：一切正确 K_ok, _, _ = lqr_finite_horizon(A, B, U, W, T) scenarios.append(("all correct", A, B, K_ok, U)) # 场景 1：模拟器把质量估成 0.1（比如忘了载荷） A_sim, B_sim = double_integrator(DT, mass=0.1) K_sim, _, _ = lqr_finite_horizon(A_sim, B_sim, U, W, T) scenarios.append(("simulator: mass 0.1", A_sim, B_sim, K_sim, U)) # 场景 2：RL 算法只往前看 3 步（0.3 秒）就决定动作（短视的规划器），每一步都用 3 步时域的第一个增益 K_myopic = lqr_finite_horizon(A, B, U, W, 3)[0][0] scenarios.append(("algorithm: 3-step lookahead", A, B, K_myopic, U)) # 场景 3：代价函数错了（只惩罚速度、不惩罚位置），RL 算法和模拟器都没问题 K_wrong, _, _ = lqr_finite_horizon(A, B, U_wrong, W, T) scenarios.append(("cost: forgot position", A, B, K_wrong, U_wrong)) q_h = quality(A_real, B_real, K_human) print("真实系统：双积分器，质量 1。人类驾驶员 = PD (3, 2)。飞行质量 q = 位置误差的 RMS（越小越好）。") print(f"人类的 q = {q_h:.3f}；q ≤ 1.25 × {q_h:.3f} = {1.25 * q_h:.3f} 算\"飞得好\"。J = 按 RL 用的代价函数在真实系统上的期望代价。\n") print(f"{'scenario':<29}{'q_sim(RL)':>10}{'q_real(RL)':>11}{'J_real(RL)':>11}{'J_real(human)':>14} diagnosis") rows = {} for name, A_s, B_s, K_rl, U_used in scenarios: q_sim, q_real = quality(A_s, B_s, K_rl), quality(A_real, B_real, K_rl) J_rl = expected_cost_linear(A_real, B_real, U_used, W, K_rl, S0, T, SW) J_h = expected_cost_linear(A_real, B_real, U_used, W, K_human, S0, T, SW) verdict = diagnose(q_sim, q_real, q_h, J_rl, J_h) rows[name] = (q_sim, q_real, J_rl, J_h, verdict) print(f"{name:<29}{q_sim:>10.3f}{q_real:>11.3f}{J_rl:>11.2f}{J_h:>14.2f} {verdict}") s1, s2, s3 = (rows[k] for k in ["simulator: mass 0.1", "algorithm: 3-step lookahead", "cost: forgot position"]) print(f""" 观察 Observations： · 场景 1：模拟器以为飞行器轻 10 倍（推一下就动得很快），于是 LQR 选了偏软的增益。在模拟器里它飞得比人还好 （q = {s1[0]:.3f} < {q_h:.3f}），到真实世界就软绵绵的（q = {s1[1]:.3f}）。"模拟器里好、现实里差" → 先去修模拟器 （收集真实数据重新拟合动力学，第 1 讲的回归），而不是去调 RL 算法或奖励。 · 场景 2：模拟器是对的（q_sim = q_real = {s2[1]:.3f}，两边一样差）。比较代价：人类的代价 {s2[3]:.2f} 比 RL 的 {s2[2]:.2f} 低 —— 明明存在代价更低的策略，RL 却没找到 → RL 算法有问题（这里是规划时域太短）。 · 场景 3：RL 的代价（{s3[2]:.2f}）比人类（{s3[3]:.2f}）还低 —— RL 完美地完成了"最小化这个代价"的任务， 可是飞得一塌糊涂（q = {s3[1]:.3f}）：它只学会了刹车，停在哪儿算哪儿。"代价更低却飞得更差" → 代价函数没有表达 我们真正想要的东西。这就是代码 01 演示 3 说的"奖励定义了最优"。 · 这个流程的价值在于**把一个模糊的问题（"它飞得不好"）拆成几个能用数字回答的问题**，避免在错误的组件上浪费几个月。 第 3 讲的 ML 诊断（是优化算法的问题还是目标函数的问题）是同一个思路。""") def main(): K_inf = demo_riccati() demo_compare_controllers(K_inf) demo_pendulum() demo_lqg() demo_debugging() if __name__ == "__main__": main()