尧图网站建设 
如果你做机器人规划时只优化状态平均值大概率会在现实世界翻车。原因并不复杂真实系统每一步都在被过程噪声、模型误差和传感器方差推离名义轨迹而规划器算出来的轨迹却默认“状态等于期望值”。尤其在无人机抗风、自动驾驶切入强非线性区域、机械臂接触未知载荷这类场景里忽略不确定性会让约束形同虚设控制性能跟着急剧恶化。不确定性下的解析规划Analytic Planning under Uncertainty要回答的核心问题是能不能把不确定性显式放进规划过程中并且不让计算复杂度爆炸。矩闭包Moment Closure就是这条路上非常关键的技术。它不试图完整描述状态分布而是用有限阶矩均值、方差必要时加偏度压缩分布信息把随机系统的预测变成一个光滑的确定性递推方程。这个方程可以求梯度、可以进优化器、可以放进 MPC 循环在线运行时依然保持可预测的计算量。这篇文章会围绕“Analytic Planning under Uncertainty with Moment Closure”这个主题展开从底层概念、数学推导讲到可运行的 Python 示例。读完你应该能理解三件事为什么随机系统的矩方程会“不闭合”高斯闭包是如何让递推方程闭合的以及矩闭包在实际规划和预测问题中怎么用、有哪些坑、什么时候不该用。1. 为什么要做不确定性下的解析规划1.1 名义轨迹与真实轨迹之间的落差传统轨迹规划通常只处理确定性模型给定初始状态和动作序列用状态转移方程算出一条名义轨迹。但真实系统存在两类不确定性一类来自感知和状态估计另一类来自执行过程本身。感知不确定性表现为初始状态不是点而是一个分布过程噪声则让每一步转移都叠加随机扰动。如果规划器只优化名义轨迹从统计角度看它是在“希望噪声完全消失”的前提下做决策。当系统非线性较弱、噪声较小时这种近似可能够用但一旦进入强非线性区域分布会从简单的高斯形态变成偏斜、厚尾甚至多模态形态单纯用均值轨迹做决策方差信息就完全被丢弃了。1.2 采样法与一阶线性化都不是银弹处理不确定性传播有两种直觉方案。第一种是用粒子滤波或蒙特卡洛仿真直接把上千个粒子前传统计得到下一时刻分布。好处是接近真实坏处是计算量大、结果带抽样噪声尤其放进优化器做梯度求解时非常难受。第二种是像 EKF 那样把非线性系统在均值处线性化再用卡尔曼滤波的公式递推均值和协方差。好处是快坏处是强非线性下会低估真实方差而且 EKF 本质上把分布硬性压成高斯形状丢失了偏度和多模态信息。采样法太贵纯线性化太粗糙两者之间需要一个折中。矩闭包站的地方就是这个中间地带它保留不确定性的低阶统计特征同时保证方程是解析、光滑、可微的规划器拿它可以做数值优化。1.3 矩闭包在规划链路中的位置一个完整的随机规划链路通常是这样的状态估计器先给出当前状态的后验分布常见形式为均值和协方差预测模块把这个分布经过动力学模型推到未来几个时刻代价函数和约束则变成基于预测分布的函数最后由优化器搜索最佳动作。矩闭包解决的是链路中的“预测模块”。它把分布传播等价转换成矩递推现在知道 x_k 的均值、方差就去求 x_{k1} 的均值、方差其中出现的三阶矩、四阶矩、六阶矩等通过闭包假设由低阶矩近似表达。这样规划器看到的不再是随机变量而是光滑的确定性代数方程目标函数 J 可以直接对动作 u 求导。从工程角度说这一点比“更准确”更重要。解析规划的核心优势在于可微性而矩闭包恰好提供了让随机动态过程保持可微的通道。2. 核心概念矩、闭包与解析规划的连接点2.1 矩到底是什么对一个随机变量 X它的分布可以用一系列“矩”来描述。一阶原点矩是均值 E[X]二阶中心矩是方差 E[(X−E[X])²]描述散布程度三阶中心矩描述偏度反映分布左右不对称四阶矩描述峰度反映尾部厚薄。常见的最简做法是只保留均值和方差并假设分布是高斯的。高斯分布有个好性质任意高阶矩都能由均值和方差解析表达。因此一旦接受了“分布始终是高斯”的假设矩的链条就自动终止在二阶不需要额外处理更高阶矩里的未知信息。2.2 为什么随机系统的矩方程天然不闭合这是理解矩闭包的关键。假设你有一个非线性系统x_{k1} f(x_k) w_k想求下一时刻均值 E[x_{k1}]就要求 E[f(x_k)]。如果 f 是线性函数比如 f(x)ax那么只需要知道 E[x]方程是闭合的。可如果 f 是多项式或者三角函数比如 f(x)bx³那么 E[f(x)] 就依赖 E[x³]。而要递推 E[x³]又需要更高阶的矩 E[x⁵]、E[x⁷]…… 如此下去方程数量永远不够未知数越来越多。这就是“不闭合”的含义矩方程形成了一条望不到头的链。矩闭包策略就是主动把这条链在某处斩断用低阶矩去近似逼近被截断的高阶项从而得到一个有限维封闭方程组。2.3 三种常见的闭包方案第一种是高斯闭包Gaussian Closure。它假设每一时刻的分布近似高斯因此三阶及以上矩全部用均值、协方差表达。实现简单代价是模型表达力有限。第二种是累积量截断Cumulant Truncation。它把高斯闭包推广到更高阶保持如果保留前 R 阶累积量就假设高于 R 阶的累积量为零。相比高斯闭包可以捕捉轻度的偏度和峰度但推导复杂度明显上升。第三种是张量多项式近似。用多项式逼近状态分布的高阶项灵活但容易产生数值不稳定或负方差。工程上最常见、最实用的是高斯闭包。后面所有示例代码都围绕它展开。2.4 与经典滤波算法的关系方法分布假设传播方式计算量适用场景KF线性高斯严格解析极低线性系统EKF非线性高斯均值处线性化低弱非线性UKF高斯近似确定性采样点中中等非线性矩闭包低阶矩截断解析矩方程低到中可导数传播粒子滤波无假设大量随机采样高强非线性、非线性估计从定位看矩闭包和 UKF 有点像都在“精度和速度之间取平衡”。区别在于 UKF 用少量确定性采样点去近似积分而矩闭包直接构造矩递推方程。前者更方便后者在需要解析梯度的规划场景里更有优势。3. 从分布传播到矩方程高斯闭包的数学推导3.1 系统模型为了把方法说明白同时保证代码可运行我选取一个一维非线性随机系统x_{k1} a * x_k b * x_k^3 u_k w_k w_k ~ N(0, q)这里 a 是线性增益b 是三次非线性强度u_k 是控制输入w_k 是零均值高斯噪声。这个系统足够简单能手工推导矩递推公式又足够复杂x³ 项会让分布逐渐偏离高斯从而展示闭包的价值。初始状态假设为x_0 ~ N(m0, P0)已知当前 x_k 的近似分布为 N(m, P)要做的是求 x_{k1} 的均值和方差。3.2 高斯假设下高阶矩的解析公式如果 X ~ N(m, P)那么它的前几阶原点矩有固定公式E[X] m E[X²] P m² E[X³] m³ 3*m*P E[X⁴] m⁴ 6*m²*P 3*P² E[X⁶] m⁶ 15*m⁴*P 45*m²*P² 15*P³这些公式本身并不神秘来自高斯分布矩生成函数的性质。有了它们原来的矩链就能在任意高阶位置截断因为任何高阶矩都只是 m 和 P 的代数函数。3.3 高斯闭包的一步递推式对系统方程两边同时求期望可以得到均值递推m_{k1} a*m_k b*(m_k³ 3*m_k*P_k) u_k再求二阶原点矩E[x_{k1}²] a²*(P_k m_k²) 2*a*b*(m_k⁴ 6*m_k²*P_k 3*P_k²) b²*(m_k⁶ 15*m_k⁴*P_k 45*m_k²*P_k² 15*P_k³) q最后得到方差P_{k1} E[x_{k1}²] - m_{k1}²这个递推式就是高斯闭包的核心从 (m_k, P_k) 精确算出 (m_{k1}, P_{k1})中间没有引入任何额外随机变量也没有采样噪声。注意“精确”是针对高斯假设而言的如果真实分布偏离高斯P_{k1} 就会与真实方差存在偏差。3.4 解析性对规划意味着什么最直接的价值在于可求导。上述公式全部由加、乘、幂运算组成对动作 u 的偏导数可以解析计算或用自动微分得到。规划器因此可以采用梯度类优化算法而不必面对粒子滤波中代价函数带噪声、梯度几乎无意义的窘境。这也是“Analytic Planning”里 Analytic 一词的落点不确定性以解析形式被嵌入代价函数和约束而不是等仿真结束后再统计。4. 环境准备与最小代码实现4.1 环境准备本文的示例只需要 Python 3.9 及以上版本加上 NumPy。建议用虚拟环境隔离项目依赖python3 -m venv mc_env source mc_env/bin/activate pip install numpy如果你还需要画分布对比图再装 matplotlibpip install matplotlib不需要 GPU不需要大型深度学习框架这部分内容对轻量级设备也友好。4.2 实现高斯闭包的一步传播先把最核心的递推做出来。下面这段代码是后面所有示例的基础# 文件路径moment_closure.py import numpy as np def gaussian_moments(m: float, P: float): 在 X ~ N(m, P) 的假设下计算 X 的若干阶原点矩。 返回一阶、二阶、三阶、四阶、六阶原点矩。 m1 m m2 P m * m m3 m**3 3 * m * P m4 m**4 6 * m * m * P 3 * P * P m6 m**6 15 * m**4 * P 45 * m * m * P * P 15 * P**3 return m1, m2, m3, m4, m6 def step_gaussian_closure(m, P, u, a0.9, b0.1, q0.1): 一步矩传播使用高斯闭包。 系统模型 x_{k1} a * x_k b * x_k^3 u_k w_k w_k ~ N(0, q) 输入 m : 当前均值 P : 当前方差 u : 控制动作 返回 (m_next, P_next) _, _, e3, e4, e6 gaussian_moments(m, P) # 均值递推 m_next a * m b * e3 u # 二阶原点矩递推 e2_next ( a * a * (P m * m) 2 * a * b * e4 b * b * e6 q ) # 由 二阶原点矩 - 均值² 得到方差 P_next e2_next - m_next * m_next return m_next, P_next代码本身不复杂重点在 gaussian_moments 函数。它把高斯分布的高阶矩全部折算成 m 和 P 的函数这正是闭包发生的地方我们不引入新的状态变量也不做抽样所有递推都是确定性的。5. 完整示例矩闭包驱动的不确定性预测与动作选择5.1 多步预测闭包 vs 蒙特卡洛不确定性传播做久了需要有一个对照基准。蒙特卡洛不是规划时的在线选择但非常适合离线验证闭包精度。下面这段代码同时跑高斯闭包和大量粒子的蒙特卡洛仿真把两者的预测均值、方差打印出来对比。# 文件路径predict.py import numpy as np from moment_closure import step_gaussian_closure def predict_gaussian(m0, P0, actions, a0.9, b0.1, q0.1): 用高斯闭包做多步前向预测返回每一步的 (均值, 方差)。 m, P m0, P0 forecasts [] for u in actions: m, P step_gaussian_closure(m, P, u, a, b, q) forecasts.append((m, P)) return np.array(forecasts) def predict_monte_carlo(m0, P0, actions, a0.9, b0.1, q0.1, n_samples200000, seed0): 用大量粒子近似真实分布作为闭包结果的对照基准。 rng np.random.default_rng(seed) x rng.normal(m0, np.sqrt(P0), n_samples) samples [] for u in actions: w rng.normal(0.0, np.sqrt(q), n_samples) x a * x b * x**3 u w samples.append((x.mean(), x.var())) return np.array(samples) if __name__ __main__: m0, P0 0.5, 0.2 actions [0.0, 0.0, 0.0, 0.0, 0.0] gc predict_gaussian(m0, P0, actions) mc predict_monte_carlo(m0, P0, actions, n_samples200000, seed42) print(步数 高斯闭包(m,P) 蒙特卡洛(m,P) 差异(|dm|, |dP|)) for k, ((gm, gp), (mm, mp)) in enumerate(zip(gc, mc), start1): diff_m abs(gm - mm) diff_p abs(gp - mp) print(f{k:3d} ({gm:7.4f}, {gp:7.4f}) ({mm:7.4f}, {mp:7.4f}) f({diff_m:8.5f}, {diff_p:8.5f}))运行步骤python predict.py如果一切正常你会看到两列预测均值比较接近预测方差在最初几步也接近但随着步数增加闭包方差与蒙特卡洛方差的差异会逐步显现。原因在 3.3 节已经说过真实分布在 x³ 项作用下逐渐偏斜、厚尾高斯闭包却强迫它保持高斯形状方差估计自然会与真实值拉开距离。5.2 用矩闭包做一步动作选择多步预测只是传播真正的规划需要做决策。现在把控制量 u 当作待优化变量假设目标状态为 target代价函数设计成J(u) (m_next - target)² λ * P_next第一项惩罚均值偏离目标第二项惩罚预测方差λ 控制决策者的风险厌恶程度。下面用一组候选动作做简单搜索# 文件路径action_selection.py import numpy as np from moment_closure import step_gaussian_closure def choose_action(m, P, target, u_candidates, lam1.0, a0.9, b0.1, q0.1): 用一步矩闭包预测评估每个候选动作。 代价函数 J(u) (m_next - target)^2 lam * P_next best_u, best_cost, best_pred None, np.inf, None for u in u_candidates: m_next, P_next step_gaussian_closure(m, P, u, a, b, q) cost (m_next - target) ** 2 lam * P_next if cost best_cost: best_cost, best_u, best_pred cost, u, (m_next, P_next) return best_u, best_cost, best_pred if __name__ __main__: m0, P0 0.5, 0.2 target 0.8 u_candidates np.linspace(-0.5, 0.5, 101) u_star, cost_star, (m_pred, P_pred) choose_action( m0, P0, target, u_candidates, lam1.0 ) print(f最优动作 u* {u_star:.4f}) print(f预测均值 {m_pred:.4f}) print(f预测方差 {P_pred:.4f}) print(f最小代价 {cost_star:.4f})这个示例虽然简单但已经具备规划的核心要素有一个代价函数、有候选动作集合、有对下一步分布的前向预测、有选择逻辑。如果把这里的单步决策扩展成滚动时域控制就变成了一个带方差惩罚的模型预测控制器。5.3 扩展成规划闭环的思路实际项目中你不会只做一步决策而是做 N 步预测。扩展方式如下给定当前状态分布 (m, P)。生成 N 步候选动作序列 u_0, ..., u_{N-1}。用 step_gaussian_closure 迭代 N 步得到每一步的 (m_k, P_k)。将所有步的代价累加得到总代价 J。用非线性优化器或随机优化方法更新动作序列。执行第一步动作状态估计器更新分布进入下一轮。这里的每一步都是解析的因此总代价 J 对动作序列的梯度可以直接用自动微分工具计算。不要在这里手动求导直接用 JAX、CasADi 或 PyTorch 实现会省很多时间。5.4 当动作带有约束时怎么办如果系统有状态约束比如位置不能超过某个安全边界你可以在代价函数里加入软约束也可以用约束优化器强行施加。矩闭包在这里有局限性它只给出均值和方差无法直接表达“状态落在安全集合内的概率”除非你再做一次分布假设。一种常见做法是假设预测分布仍然近似高斯把均值和方差转成一个椭球形状的置信区域再判断该区域与约束集合的关系。这种近似可能过于乐观安全关键场景必须额外处理。6. 运行结果与效果验证6.1 预期输出与观察角度运行 predict.py 后从趋势上你应该观察到三个现象对比项高斯闭包蒙特卡洛说明均值光滑变化平滑但有抽样抖动闭包更适合进优化器方差整体变化趋势一致更接近真实分布非线性强时闭包会低估方差高阶信息完全丢失隐含在粒子中多峰场景下闭包失效风险高如果闭包与蒙特卡洛的均值差异很小、方差差异在可接受范围说明当前参数下高斯闭包是合理的。如果方差差异迅速增大说明系统非线性太强高斯假设已经不够用。6.2 怎么判断闭包是否“足够好”不要只看某个单点的均值。建议把闭包预测分布与蒙特卡洛直方图放在同一张图里对比。你可以从蒙特卡洛粒子中采样画出直方图再叠加 N(m_closed, P_closed) 的高斯曲线。如果直方图明显偏斜、双峰或者尾部特别厚那么闭包得到的高斯曲线会显著偏离真实分布。这种视觉验证比数值对比更能说明问题也更容易让团队理解闭包的适用范围。6.3 运行失败时的基本检查顺序先看模型参数是否存在不稳定平衡点 不确定性下的解析规划：矩闭包与高斯闭包实战
如果你做机器人规划时只优化状态平均值大概率会在现实世界翻车。原因并不复杂真实系统每一步都在被过程噪声、模型误差和传感器方差推离名义轨迹而规划器算出来的轨迹却默认“状态等于期望值”。尤其在无人机抗风、自动驾驶切入强非线性区域、机械臂接触未知载荷这类场景里忽略不确定性会让约束形同虚设控制性能跟着急剧恶化。不确定性下的解析规划Analytic Planning under Uncertainty要回答的核心问题是能不能把不确定性显式放进规划过程中并且不让计算复杂度爆炸。矩闭包Moment Closure就是这条路上非常关键的技术。它不试图完整描述状态分布而是用有限阶矩均值、方差必要时加偏度压缩分布信息把随机系统的预测变成一个光滑的确定性递推方程。这个方程可以求梯度、可以进优化器、可以放进 MPC 循环在线运行时依然保持可预测的计算量。这篇文章会围绕“Analytic Planning under Uncertainty with Moment Closure”这个主题展开从底层概念、数学推导讲到可运行的 Python 示例。读完你应该能理解三件事为什么随机系统的矩方程会“不闭合”高斯闭包是如何让递推方程闭合的以及矩闭包在实际规划和预测问题中怎么用、有哪些坑、什么时候不该用。1. 为什么要做不确定性下的解析规划1.1 名义轨迹与真实轨迹之间的落差传统轨迹规划通常只处理确定性模型给定初始状态和动作序列用状态转移方程算出一条名义轨迹。但真实系统存在两类不确定性一类来自感知和状态估计另一类来自执行过程本身。感知不确定性表现为初始状态不是点而是一个分布过程噪声则让每一步转移都叠加随机扰动。如果规划器只优化名义轨迹从统计角度看它是在“希望噪声完全消失”的前提下做决策。当系统非线性较弱、噪声较小时这种近似可能够用但一旦进入强非线性区域分布会从简单的高斯形态变成偏斜、厚尾甚至多模态形态单纯用均值轨迹做决策方差信息就完全被丢弃了。1.2 采样法与一阶线性化都不是银弹处理不确定性传播有两种直觉方案。第一种是用粒子滤波或蒙特卡洛仿真直接把上千个粒子前传统计得到下一时刻分布。好处是接近真实坏处是计算量大、结果带抽样噪声尤其放进优化器做梯度求解时非常难受。第二种是像 EKF 那样把非线性系统在均值处线性化再用卡尔曼滤波的公式递推均值和协方差。好处是快坏处是强非线性下会低估真实方差而且 EKF 本质上把分布硬性压成高斯形状丢失了偏度和多模态信息。采样法太贵纯线性化太粗糙两者之间需要一个折中。矩闭包站的地方就是这个中间地带它保留不确定性的低阶统计特征同时保证方程是解析、光滑、可微的规划器拿它可以做数值优化。1.3 矩闭包在规划链路中的位置一个完整的随机规划链路通常是这样的状态估计器先给出当前状态的后验分布常见形式为均值和协方差预测模块把这个分布经过动力学模型推到未来几个时刻代价函数和约束则变成基于预测分布的函数最后由优化器搜索最佳动作。矩闭包解决的是链路中的“预测模块”。它把分布传播等价转换成矩递推现在知道 x_k 的均值、方差就去求 x_{k1} 的均值、方差其中出现的三阶矩、四阶矩、六阶矩等通过闭包假设由低阶矩近似表达。这样规划器看到的不再是随机变量而是光滑的确定性代数方程目标函数 J 可以直接对动作 u 求导。从工程角度说这一点比“更准确”更重要。解析规划的核心优势在于可微性而矩闭包恰好提供了让随机动态过程保持可微的通道。2. 核心概念矩、闭包与解析规划的连接点2.1 矩到底是什么对一个随机变量 X它的分布可以用一系列“矩”来描述。一阶原点矩是均值 E[X]二阶中心矩是方差 E[(X−E[X])²]描述散布程度三阶中心矩描述偏度反映分布左右不对称四阶矩描述峰度反映尾部厚薄。常见的最简做法是只保留均值和方差并假设分布是高斯的。高斯分布有个好性质任意高阶矩都能由均值和方差解析表达。因此一旦接受了“分布始终是高斯”的假设矩的链条就自动终止在二阶不需要额外处理更高阶矩里的未知信息。2.2 为什么随机系统的矩方程天然不闭合这是理解矩闭包的关键。假设你有一个非线性系统x_{k1} f(x_k) w_k想求下一时刻均值 E[x_{k1}]就要求 E[f(x_k)]。如果 f 是线性函数比如 f(x)ax那么只需要知道 E[x]方程是闭合的。可如果 f 是多项式或者三角函数比如 f(x)bx³那么 E[f(x)] 就依赖 E[x³]。而要递推 E[x³]又需要更高阶的矩 E[x⁵]、E[x⁷]…… 如此下去方程数量永远不够未知数越来越多。这就是“不闭合”的含义矩方程形成了一条望不到头的链。矩闭包策略就是主动把这条链在某处斩断用低阶矩去近似逼近被截断的高阶项从而得到一个有限维封闭方程组。2.3 三种常见的闭包方案第一种是高斯闭包Gaussian Closure。它假设每一时刻的分布近似高斯因此三阶及以上矩全部用均值、协方差表达。实现简单代价是模型表达力有限。第二种是累积量截断Cumulant Truncation。它把高斯闭包推广到更高阶保持如果保留前 R 阶累积量就假设高于 R 阶的累积量为零。相比高斯闭包可以捕捉轻度的偏度和峰度但推导复杂度明显上升。第三种是张量多项式近似。用多项式逼近状态分布的高阶项灵活但容易产生数值不稳定或负方差。工程上最常见、最实用的是高斯闭包。后面所有示例代码都围绕它展开。2.4 与经典滤波算法的关系方法分布假设传播方式计算量适用场景KF线性高斯严格解析极低线性系统EKF非线性高斯均值处线性化低弱非线性UKF高斯近似确定性采样点中中等非线性矩闭包低阶矩截断解析矩方程低到中可导数传播粒子滤波无假设大量随机采样高强非线性、非线性估计从定位看矩闭包和 UKF 有点像都在“精度和速度之间取平衡”。区别在于 UKF 用少量确定性采样点去近似积分而矩闭包直接构造矩递推方程。前者更方便后者在需要解析梯度的规划场景里更有优势。3. 从分布传播到矩方程高斯闭包的数学推导3.1 系统模型为了把方法说明白同时保证代码可运行我选取一个一维非线性随机系统x_{k1} a * x_k b * x_k^3 u_k w_k w_k ~ N(0, q)这里 a 是线性增益b 是三次非线性强度u_k 是控制输入w_k 是零均值高斯噪声。这个系统足够简单能手工推导矩递推公式又足够复杂x³ 项会让分布逐渐偏离高斯从而展示闭包的价值。初始状态假设为x_0 ~ N(m0, P0)已知当前 x_k 的近似分布为 N(m, P)要做的是求 x_{k1} 的均值和方差。3.2 高斯假设下高阶矩的解析公式如果 X ~ N(m, P)那么它的前几阶原点矩有固定公式E[X] m E[X²] P m² E[X³] m³ 3*m*P E[X⁴] m⁴ 6*m²*P 3*P² E[X⁶] m⁶ 15*m⁴*P 45*m²*P² 15*P³这些公式本身并不神秘来自高斯分布矩生成函数的性质。有了它们原来的矩链就能在任意高阶位置截断因为任何高阶矩都只是 m 和 P 的代数函数。3.3 高斯闭包的一步递推式对系统方程两边同时求期望可以得到均值递推m_{k1} a*m_k b*(m_k³ 3*m_k*P_k) u_k再求二阶原点矩E[x_{k1}²] a²*(P_k m_k²) 2*a*b*(m_k⁴ 6*m_k²*P_k 3*P_k²) b²*(m_k⁶ 15*m_k⁴*P_k 45*m_k²*P_k² 15*P_k³) q最后得到方差P_{k1} E[x_{k1}²] - m_{k1}²这个递推式就是高斯闭包的核心从 (m_k, P_k) 精确算出 (m_{k1}, P_{k1})中间没有引入任何额外随机变量也没有采样噪声。注意“精确”是针对高斯假设而言的如果真实分布偏离高斯P_{k1} 就会与真实方差存在偏差。3.4 解析性对规划意味着什么最直接的价值在于可求导。上述公式全部由加、乘、幂运算组成对动作 u 的偏导数可以解析计算或用自动微分得到。规划器因此可以采用梯度类优化算法而不必面对粒子滤波中代价函数带噪声、梯度几乎无意义的窘境。这也是“Analytic Planning”里 Analytic 一词的落点不确定性以解析形式被嵌入代价函数和约束而不是等仿真结束后再统计。4. 环境准备与最小代码实现4.1 环境准备本文的示例只需要 Python 3.9 及以上版本加上 NumPy。建议用虚拟环境隔离项目依赖python3 -m venv mc_env source mc_env/bin/activate pip install numpy如果你还需要画分布对比图再装 matplotlibpip install matplotlib不需要 GPU不需要大型深度学习框架这部分内容对轻量级设备也友好。4.2 实现高斯闭包的一步传播先把最核心的递推做出来。下面这段代码是后面所有示例的基础# 文件路径moment_closure.py import numpy as np def gaussian_moments(m: float, P: float): 在 X ~ N(m, P) 的假设下计算 X 的若干阶原点矩。 返回一阶、二阶、三阶、四阶、六阶原点矩。 m1 m m2 P m * m m3 m**3 3 * m * P m4 m**4 6 * m * m * P 3 * P * P m6 m**6 15 * m**4 * P 45 * m * m * P * P 15 * P**3 return m1, m2, m3, m4, m6 def step_gaussian_closure(m, P, u, a0.9, b0.1, q0.1): 一步矩传播使用高斯闭包。 系统模型 x_{k1} a * x_k b * x_k^3 u_k w_k w_k ~ N(0, q) 输入 m : 当前均值 P : 当前方差 u : 控制动作 返回 (m_next, P_next) _, _, e3, e4, e6 gaussian_moments(m, P) # 均值递推 m_next a * m b * e3 u # 二阶原点矩递推 e2_next ( a * a * (P m * m) 2 * a * b * e4 b * b * e6 q ) # 由 二阶原点矩 - 均值² 得到方差 P_next e2_next - m_next * m_next return m_next, P_next代码本身不复杂重点在 gaussian_moments 函数。它把高斯分布的高阶矩全部折算成 m 和 P 的函数这正是闭包发生的地方我们不引入新的状态变量也不做抽样所有递推都是确定性的。5. 完整示例矩闭包驱动的不确定性预测与动作选择5.1 多步预测闭包 vs 蒙特卡洛不确定性传播做久了需要有一个对照基准。蒙特卡洛不是规划时的在线选择但非常适合离线验证闭包精度。下面这段代码同时跑高斯闭包和大量粒子的蒙特卡洛仿真把两者的预测均值、方差打印出来对比。# 文件路径predict.py import numpy as np from moment_closure import step_gaussian_closure def predict_gaussian(m0, P0, actions, a0.9, b0.1, q0.1): 用高斯闭包做多步前向预测返回每一步的 (均值, 方差)。 m, P m0, P0 forecasts [] for u in actions: m, P step_gaussian_closure(m, P, u, a, b, q) forecasts.append((m, P)) return np.array(forecasts) def predict_monte_carlo(m0, P0, actions, a0.9, b0.1, q0.1, n_samples200000, seed0): 用大量粒子近似真实分布作为闭包结果的对照基准。 rng np.random.default_rng(seed) x rng.normal(m0, np.sqrt(P0), n_samples) samples [] for u in actions: w rng.normal(0.0, np.sqrt(q), n_samples) x a * x b * x**3 u w samples.append((x.mean(), x.var())) return np.array(samples) if __name__ __main__: m0, P0 0.5, 0.2 actions [0.0, 0.0, 0.0, 0.0, 0.0] gc predict_gaussian(m0, P0, actions) mc predict_monte_carlo(m0, P0, actions, n_samples200000, seed42) print(步数 高斯闭包(m,P) 蒙特卡洛(m,P) 差异(|dm|, |dP|)) for k, ((gm, gp), (mm, mp)) in enumerate(zip(gc, mc), start1): diff_m abs(gm - mm) diff_p abs(gp - mp) print(f{k:3d} ({gm:7.4f}, {gp:7.4f}) ({mm:7.4f}, {mp:7.4f}) f({diff_m:8.5f}, {diff_p:8.5f}))运行步骤python predict.py如果一切正常你会看到两列预测均值比较接近预测方差在最初几步也接近但随着步数增加闭包方差与蒙特卡洛方差的差异会逐步显现。原因在 3.3 节已经说过真实分布在 x³ 项作用下逐渐偏斜、厚尾高斯闭包却强迫它保持高斯形状方差估计自然会与真实值拉开距离。5.2 用矩闭包做一步动作选择多步预测只是传播真正的规划需要做决策。现在把控制量 u 当作待优化变量假设目标状态为 target代价函数设计成J(u) (m_next - target)² λ * P_next第一项惩罚均值偏离目标第二项惩罚预测方差λ 控制决策者的风险厌恶程度。下面用一组候选动作做简单搜索# 文件路径action_selection.py import numpy as np from moment_closure import step_gaussian_closure def choose_action(m, P, target, u_candidates, lam1.0, a0.9, b0.1, q0.1): 用一步矩闭包预测评估每个候选动作。 代价函数 J(u) (m_next - target)^2 lam * P_next best_u, best_cost, best_pred None, np.inf, None for u in u_candidates: m_next, P_next step_gaussian_closure(m, P, u, a, b, q) cost (m_next - target) ** 2 lam * P_next if cost best_cost: best_cost, best_u, best_pred cost, u, (m_next, P_next) return best_u, best_cost, best_pred if __name__ __main__: m0, P0 0.5, 0.2 target 0.8 u_candidates np.linspace(-0.5, 0.5, 101) u_star, cost_star, (m_pred, P_pred) choose_action( m0, P0, target, u_candidates, lam1.0 ) print(f最优动作 u* {u_star:.4f}) print(f预测均值 {m_pred:.4f}) print(f预测方差 {P_pred:.4f}) print(f最小代价 {cost_star:.4f})这个示例虽然简单但已经具备规划的核心要素有一个代价函数、有候选动作集合、有对下一步分布的前向预测、有选择逻辑。如果把这里的单步决策扩展成滚动时域控制就变成了一个带方差惩罚的模型预测控制器。5.3 扩展成规划闭环的思路实际项目中你不会只做一步决策而是做 N 步预测。扩展方式如下给定当前状态分布 (m, P)。生成 N 步候选动作序列 u_0, ..., u_{N-1}。用 step_gaussian_closure 迭代 N 步得到每一步的 (m_k, P_k)。将所有步的代价累加得到总代价 J。用非线性优化器或随机优化方法更新动作序列。执行第一步动作状态估计器更新分布进入下一轮。这里的每一步都是解析的因此总代价 J 对动作序列的梯度可以直接用自动微分工具计算。不要在这里手动求导直接用 JAX、CasADi 或 PyTorch 实现会省很多时间。5.4 当动作带有约束时怎么办如果系统有状态约束比如位置不能超过某个安全边界你可以在代价函数里加入软约束也可以用约束优化器强行施加。矩闭包在这里有局限性它只给出均值和方差无法直接表达“状态落在安全集合内的概率”除非你再做一次分布假设。一种常见做法是假设预测分布仍然近似高斯把均值和方差转成一个椭球形状的置信区域再判断该区域与约束集合的关系。这种近似可能过于乐观安全关键场景必须额外处理。6. 运行结果与效果验证6.1 预期输出与观察角度运行 predict.py 后从趋势上你应该观察到三个现象对比项高斯闭包蒙特卡洛说明均值光滑变化平滑但有抽样抖动闭包更适合进优化器方差整体变化趋势一致更接近真实分布非线性强时闭包会低估方差高阶信息完全丢失隐含在粒子中多峰场景下闭包失效风险高如果闭包与蒙特卡洛的均值差异很小、方差差异在可接受范围说明当前参数下高斯闭包是合理的。如果方差差异迅速增大说明系统非线性太强高斯假设已经不够用。6.2 怎么判断闭包是否“足够好”不要只看某个单点的均值。建议把闭包预测分布与蒙特卡洛直方图放在同一张图里对比。你可以从蒙特卡洛粒子中采样画出直方图再叠加 N(m_closed, P_closed) 的高斯曲线。如果直方图明显偏斜、双峰或者尾部特别厚那么闭包得到的高斯曲线会显著偏离真实分布。这种视觉验证比数值对比更能说明问题也更容易让团队理解闭包的适用范围。6.3 运行失败时的基本检查顺序先看模型参数是否存在不稳定平衡点