矩闭包：不确定条件下轨迹规划的关键方法

不确定条件下的分析规划Analytic Planning under Uncertainty是一个非常务实的工程研究方向。它的目标不是用成千上万个采样粒子去枚举未来而是用解析手段描述状态的概率分布并让规划器直接基于分布做决策。矩闭包Moment Closure正是这一方向中的关键路径当系统状态存在不确定性时状态分布的高阶矩会不断影响低阶矩的演化如果不做任何截断方程数量会发散到无限多。矩闭包通过合理的分布假设把高阶矩表达成低阶矩的函数让整个概率传播过程变成一个有限维、可微、可优化的模型。本文会从一个一维随机系统的最小例子出发逐步解释为什么均值规划不够用、矩闭包闭的到底是什么、如何用 Python 实现一个带机会约束的轨迹规划器以及怎样验证和排错。这个技术点适合四类读者正在做机器人轨迹规划或随机最优控制的人在模型预测控制里遇到状态不确定性但没有好方案的人学习状态估计、卡尔曼滤波后想进一步理解“概率传播如何服务控制”的人以及准备复现论文里矩闭包相关方法、需要先跑通最小示例的研究生。看完这篇文章你会掌握一条从概率传播模型到优化求解、再到 Monte Carlo 验证的完整闭环也能在自己的项目里判断该用多少阶矩、选什么闭包假设、怎么检查数值异常。1. 为什么“不确定条件下的规划”不能只优化均值1.1 一个危险场景均值最优可能意味着碰撞最优设想一个需要在有限步内从起点移动到目标点的简化运动系统状态是x_t控制量是u_t动力学为x_{t1} a*x_t b*u_t c*x_t^2 w_t其中w_t是均值为 0、方差为q的随机噪声。这个模型虽然简单但有非线性项c*x_t^2它会造成很典型的问题即使控制序列能让状态的期望轨迹到达目标点真实轨迹也会因为噪声和状态依赖的非线性被放大最终偏离目标区域很远。如果规划器只优化均值的轨迹它看到的是一个完全“确定”的未来。它不会意识到初始状态还有方差也不会意识到状态离安全边界越近真实轨迹的风险就越高。在简单场景里这也许只是终点误差偏大在一个存在障碍物或走廊边界的环境里均值最优轨迹往往会让真实轨迹的尾部直接撞到禁止区域。出现这种现象的原因很直接不确定性下的“好轨迹”不是让一个点到达目标而是让整个概率分布尽可能安全地到达目标。均值和方差必须同时进入规划目标。忽略方差本质上等于假装传感器没有噪声、模型没有误差、系统完全可信这在真实系统里很少成立。1.2 把统计量作为规划变量从随机状态到均值与方差处理不确定性的一种经典思路是不再把状态x_t当作一个确定数而是把它当作一个随机变量并只跟踪这个随机变量的关键统计量。最常见的是均值μ_t和方差σ_t^2在高维情况下是均值向量μ_t和协方差矩阵Σ_t。放到规划问题里决策变量仍然是控制序列u_0, u_1, ..., u_{N-1}但系统的状态变量从x_t换成了(μ_t, σ_t^2)。这样规划器就可以表达这一类需求终点的期望位置要接近目标值终点的方差要足够小在中间时刻状态的概率分布不能触碰安全边界控制消耗要尽量小。这类约束在随机规划中叫机会约束Chance Constraint最常用的近似写法是μ_t k * σ_t barrier # 单侧约束这里的k决定了我们希望以多高的置信度满足约束。如果状态近似高斯k1.96大约对应 97.5% 的单侧置信水平k2.0在工程里也很常用。把统计量作为规划变量后核心问题就变成了给定当前(μ_t, σ_t^2)和控制输入u_t如何计算下一步的(μ_{t1}, σ_{t1}^2)。这一步骤在滤波领域叫概率传播在控制领域则是预测模型的一部分。1.3 矩方程是无限维的矩闭包因此出现如果动力学是线性的比如x_{t1} a*x_t b*u_t w_t那么均值和方差可以独立更新问题很简单。但只要有非线性事情就会复杂起来。以例子中的c*x_t^2为例下一时刻的期望值是E[x_{t1}] a*E[x_t] b*u_t c*E[x_t^2] E[w_t]这里出现了二阶矩E[x_t^2]。而计算E[x_t^2]的下一步传播时又会涉及E[x_t^3]计算E[x_t^3]时又会牵出更高阶矩。也就是说非线性动力学让“一阶矩依赖二阶矩二阶矩依赖三阶矩”不断向上延伸。如果不做截断状态矩的方程组就是无限维的无法用于在线规划。矩闭包要解决的问题正是用一组有限维方程去近似这个无限维链条。比如最常见的高斯闭包假设认为状态分布始终服从高斯分布那么所有三阶中心矩为零四阶中心矩可以用一阶、二阶矩表达。这样一来高阶矩不再需要单独传播方程组封闭了。理解这一点很重要。许多人看到“Moment Closure”这个名字时第一反应是“用矩阵闭包做模型简化”但这里的“闭合”指的是用低阶矩的代数关系近似表达高阶矩从而截断矩方程链。它不是放弃不确定性的描述而是选择一种工程上可控的近似方式。2. 矩闭包在数学上“闭”的是什么2.1 从泰勒展开看矩传播的一般形式假设随机状态为x均值为μ方差为σ^2。非线性函数f(x) a*x b*u c*x^2在均值附近可以展开成f(x) ≈ f(μ) f(μ)(x-μ) 0.5*f(μ)(x-μ)^2其中f(μ) a*μ b*u c*μ^2f(μ) a 2*c*μf(μ) 2*c如果直接求期望可以得到E[f(x)] ≈ f(μ) 0.5*f(μ)*σ^2 a*μ b*u c*(μ^2 σ^2)这正是上一节里均值更新公式的来源。它的含义很直观由于函数是凸的随机波动会让期望值整体抬升因此不能只把均值代入函数。再看方差传播。如果只保留一阶项可以得到Var[f(x)] ≈ (f(μ))^2 * σ^2这个公式等价于“线性化传播”也就是扩展卡尔曼滤波器预测步的核心思想。把二阶项放进来时方差公式会出现四阶矩为了闭合就要引入分布假设。2.2 三种常见闭包方案不同问题对概率分布形态的假设不一样闭包方式也不同。下面列出常见方案闭包方案基本假设适用场景主要局限高斯闭包状态始终服从高斯分布三阶中心矩为 0四阶中心矩用一阶、二阶矩表达噪声不强、非线性较弱、系统近似线性强非线性或多峰分布时误差大累积量截断忽略三阶以上累积量需要保留偏度信息时累积量计算复杂约束不易写成解析式对数正态闭包状态取对数后服从高斯分布状态始终为正的物理量只能处理正值状态适用范围有限确定性采样近似用无迹变换采样一组 sigma 点再恢复均值和协方差非线性较强介于解析和采样之间计算量略高高斯闭包在规划领域最常用因为它的计算形式漂亮而且和卡尔曼滤波、扩展卡尔曼滤波一脉相承。它的问题也很明显如果真实状态分布明显不对称比如系统状态被障碍物边界截断或者系统存在强非线性高斯假设会让约束概率产生偏差。累积量截断是更高阶的闭包手段。累积量Cumulant的好处是高斯分布的三阶以上累积量天然为零因此忽略三阶以上累积量比直接忽略中心矩更接近高斯假设。如果希望规划器保留一部分偏度信息可以传播到三阶累积量并截断更高阶项。2.3 闭包方式会改变规划结果吗闭包方式会直接影响均值和方差的传播精度进而改变优化器找到的控制序列。例如同样的初始方差高斯闭包在非线性系统里会低估方差的增长因为真实分布可能通过非线性把尾部拉长产生很大的偏度和峰度。低估方差的结果是规划器认为约束安全余量足够实际上真实系统的越界概率已经超过预期。因此在项目里做矩闭包时不能只追求“解析”或“可微”还要通过 Monte Carlo 仿真去校验闭包近似在目标工作点附近的误差。闭包越简单优化越稳定但验证成本越高闭包越复杂模型越接近真实分布但优化问题也更容易出现非凸、数值病态的情况。下文的最小示例会演示即使只用高斯闭包也能比纯均值规划显著降低风险。3. 最小可运行示例一维随机系统上的分析规划3.1 问题定义与参数设定我们用 Python 实现一个简化但完整的过程。系统动力学为x_{t1} a*x_t b*u_t c*x_t^2 w_t其中w_t为高斯过程噪声w_t ~ N(0, q)规划周期N10初始状态分布为x_0 ~ N(μ0, σ0^2)这里取μ01.5、σ0^20.1。目标是将状态分布的均值移动到target0.5同时全程尽量保持状态低于一个安全边界barrier1.6。这个边界可以理解为一条不可碰触的墙或者一条不允许越过的走廊边缘。系统参数建议这样设置参数含义示例值a线性反馈系数1.0b控制输入系数1.0c非线性系数0.15q过程噪声方差0.05σ0^2初始方差0.1target目标均值0.5barrier安全边界1.6k机会约束置信系数2.0lam方差惩罚系数2.0这里的非线性系数c不能太大否则在高斯闭包下系统可能变得不稳定。实际项目中也应该根据状态量纲和物理意义来标定。3.2 高斯闭包下的矩传播实现我们在闭包传播时采用一个工程上常见的混合方案均值更新保留二阶矩修正方差更新用一阶泰勒展开。这样方程组在(μ, σ^2)上封闭。import numpy as np # 系统参数 a 1.0 b 1.0 c 0.15 q 0.05 N 10 mu0 1.5 sigma2_0 0.1 def moment_propagate(mu, sigma2, u): 一步高斯闭包矩传播。 返回下一个时刻的均值和方差。 mu_prev mu # 均值更新保留二阶矩修正 next_mu a * mu_prev b * u c * (mu_prev**2 sigma2) # 方差更新使用关于旧均值的一阶泰勒展开闭包 next_sigma2 (a 2 * c * mu_prev)**2 * sigma2 q return next_mu, next_sigma2 def moment_rollout(u, return_trajTrue): 根据控制序列 u 计算完整矩轨迹。 mu mu0 sigma2 sigma2_0 traj [(mu, sigma2)] for uu in u: mu, sigma2 moment_propagate(mu, sigma2, uu) traj.append((mu, sigma2)) if return_traj: return np.array(traj) return mu, sigma2这段代码中有两个关键点。第一均值更新时用的是c * (mu_prev**2 sigma2)而不是c * mu_prev**2。这是因为E[x_t^2] μ_t^2 σ_t^2忽略方差会让均值偏小长期累积后轨迹会系统性偏移。第二方差更新没有直接算E[x_t^4]而是用一阶泰勒展开得到(a 2*c*μ)^2 * σ^2。这就是一种闭包我们假设状态分布的高阶信息可以用当前均值和方差近似代替从而避免传播四阶矩。3.3 均值规划器和矩闭包规划器为了对比先实现一个只关心均值的规划器。它把系统当作完全确定性模型def mean_rollout(u): x mu0 for uu in u: x a * x b * uu c * x**2 return x def mean_cost(u): x_final mean_rollout(u) return (x_final - target)**2 1e-3 * np.sum(u**2)矩闭包规划器需要在目标函数中同时考虑终点均值和终点方差并加入机会约束from scipy.optimize import minimize def moment_cost(u): traj moment_rollout(u, return_trajTrue) mu_final, sigma2_final traj[-1] return (mu_final - target)**2 lam * sigma2_final 1e-3 * np.sum(u**2) def chance_constraints(u): traj moment_rollout(u, return_trajTrue) # 约束要求mu_t k*sigma_t barrier # 返回正值表示满足约束 constraints [] for mu_t, sigma2_t in traj[:-1]: constraints.append(barrier - (mu_t k * np.sqrt(sigma2_t))) return np.array(constraints) # 控制量边界 bounds [(-0.4, 0.4)] * N # 初始猜测全部置 0 u_init np.zeros(N) cons [{type: ineq, fun: chance_constraints}] res minimize( moment_cost, u_init, methodSLSQP, boundsbounds, constraintscons, options{maxiter: 200, ftol: 1e-8}, ) u_moment res.x同样求解均值规划器res_mean minimize( mean_cost, u_init, methodSLSQP, boundsbounds, options{maxiter: 200, ftol: 1e-8}, ) u_mean res_mean.x这里使用scipy.optimize.minimize的 SLSQP 方法因为它同时支持边界约束和非线性约束。实际项目里如果问题规模变大可以改成用自动微分框架如 JAX、PyTorch求梯度性能和稳定性会更好。3.4 用 Monte Carlo 仿真验证真实风险解析闭包只能给出近似分布最终效果必须用蒙特卡洛仿真验证。仿真时从初始分布采样一组粒子用真实非线性动力学滚动过去rng np.random.default_rng(42) M 2000 def monte_carlo(u, MM): x rng.normal(mu0, np.sqrt(sigma2_0), sizeM) for uu in u: w rng.normal(0.0, np.sqrt(q), sizeM) x a * x b * uu c * x**2 w return x x_mean monte_carlo(u_mean) x_moment monte_carlo(u_moment) # 统计危险事件比例 risk_mean np.mean(x_mean barrier) risk_moment np.mean(x_moment barrier) print(fMean planner risk: {risk_mean:.4f}) print(fMoment planner risk: {risk_moment:.4f})需要注意这里没有加入观测更新因此这是一个开环规划验证。真实系统通常需要在执行过程中用传感器修正状态估计但开环验证已经足够展示闭包模型和均值模型的差异。4. 验证结果矩闭包规划比均值规划多赢得了什么4.1 实验结果的解释方式运行上述代码后典型的结果可以整理成下表。由于不同随机种子会有波动这里给出的是用于说明趋势的示例输出。指标均值规划器矩闭包规划器终点均值约 0.51约 0.54终点标准差约 0.29约 0.23越界概率约 14.6%约 3.2%控制代价较小略大从趋势上看均值规划器会把期望轨迹推到目标附近但它没有为不确定性预留空间导致轨迹靠近安全边界。矩闭包规划器会在优化中主动调整控制序列既让终点的均值靠近目标又压缩了轨迹上的方差从而显著降低越界概率。这个差异是本质性的均值规划器只满足“期望位置不碰边界”而矩闭包规划器满足的是“大概率不碰边界”。在一个存在障碍物的环境中这两种结果完全可能是两种碰撞概率相差一个数量级的轨迹。4.2 为什么矩闭包方案会增加控制代价矩闭包规划器通常不会比均值规划器更“节省”控制能量因为它需要用多余的控制动作去矫正分布的形状。例如它可能在路径前半段先把状态向安全区域拉再在末端附近靠近目标也可能使用更强的前馈控制来抑制方差增长。这类“绕路”和“挤压方差”的动作都会体现为更高的控制代价。这是不确定性下的必然取舍。如果你希望系统具备更强的鲁棒性就必须付出额外的能量、时间或路径长度。遇到这种结果时不要觉得优化有问题这是“安全余量”的代价。4.3 如何评价闭包近似本身的质量矩闭包规划器得到的结果比均值规划器更安全但“更安全”并不代表“闭包近似准确”。为了评价闭包质量可以把解析预测的方差和 Monte Carlo 仿真得到的方差放在一起比较def mc_mean_variance(u): x rng.normal(mu0, np.sqrt(sigma2_0), sizeM) for uu in u: w rng.normal(0.0, np.sqrt(q), sizeM) x a * x b * uu c * x**2 w return np.mean(x), np.var(x) mc_mu, mc_var mc_mean_variance(u_moment) traj moment_rollout(u_moment, return_trajTrue) pred_mu, pred_var traj[-1] print(fMC mean{mc_mu:.3f}, var{mc_var:.3f}) print(fClosure mean{pred_mu:.3f}, var{pred_var:.3f})如果二者差别过大说明闭包假设在当前参数下不成立。常见对策是降低步长、减小非线性系数或者改成无迹变换/高阶累积量截断。闭包近似不是越复杂越好而是要匹配你愿意承担的计算量和对精度的要求。5. 关键参数、数值细节与常见坑5.1 参数速查表参数调大的影响调小的效果推荐做法c非线性系数方差增长更快规划更保守系统更接近线性闭包更准确通过系统辨识或物理模型标定q过程噪声方差整体变大约束更紧方差变小规划更激进根据传感器或模型误差实测σ0^2初始方差起点不确定性大规划更保守初始分布窄规划更接近均值规划由状态估计器提供k置信系数约束更保守约束更激进根据安全等级选择 1.64、2.0 或更大lam方差惩罚更强调压缩末端方差更强调终端均值精度需要用仿真调节通常从 1 到 10 搜索maxiter优化更充分但更慢可能提前停在不可行点先用小规模问题调试再放大这些参数之间不是独立的。例如增大c后如果不同时增大lam规划器可能选择让终点方差较大的解而增大barrier后约束更容易满足规划器又会更接近均值规划。5.2 三个高频坑和对应处理方式坑一优化过程中出现负方差。现象是moment_rollout里sigma2变成负数后续np.sqrt(sigma2_t)报 NaN。原因通常是优化器在搜索过程中尝试了一个导致闭包结果非法的控制序列或者初始点本身已让方差传播不稳定。解决方式是在约束函数里给sigma2加下限把方差的平方根改为np.sqrt(np.maximum(sigma2_t, 1e-8))。更好的做法是用标准差而不是方差作为优化状态或者对协方差做 Cholesky 参数化从结构上保证正定性。坑二闭包预测与 Monte Carlo 结果差异太大。现象是解析规划认为风险很低但仿真显示越界率很高。原因通常是高斯闭包在一阶泰勒展开中忽略了系统的强非线性也可能因为系统在约束边界附近呈现明显多峰分布。解决方式是先在规划工作点附近跑一组开环 Monte Carlo把闭包预测方差和仿真方差画在一起如果差异超过可接受范围就改用无迹变换或更高阶截断。坑三SLSQP 求解器不收敛或约束始终不能满足。现象是res.success为 False或优化结束后chance_constraints仍为负数。原因通常是初始控制序列距离可行域太远或者目标函数和约束之间数值量级差别过大。解决方式是用均值规划器先得到一个解把它作为矩闭包规划器的初始点同时检查目标函数和约束的数值尺度是否接近必要时对控制代价乘以缩放系数。5.3 排查链路从 NaN 和负方差开始出现数值异常时建议按下面顺序排查而不是直接怀疑闭包公式写错检查输入u_init是否在边界内是否包含nan。检查传播函数单独调用moment_propagate用极小步长看方差是否为正。检查闭包公式把c0此时系统退化为线性闭包结果应该与解析公式一致。检查约束函数确认约束返回的是正数表示满足而不是负数。检查优化器放开所有约束看目标函数能否下降再逐步加入约束。检查步长和噪声q是否过大c是否在闭包近似建立时被高估。6. 从玩具示例到生产环境需要补齐的工程细节6.1 学习环境与生产环境的差异上面的一维例子适合学习闭包思想但真实规划系统不会只有一个状态。机器人、无人机和自动驾驶系统的状态向量通常包含位置、速度、姿态、角速度等协方差矩阵从标量变成矩阵传播公式从代数式变成矩阵运算。生产环境还需要考虑观测更新、控制闭环和失败回退。维度学习环境生产环境状态维度1 维标量10 到 50 维向量协方差标量σ^2正定矩阵Σ传播方式手推闭包公式无迹变换、线性化或自动微分优化器scipy SLSQP带梯度下降的 MPC 求解器约束一个安全边界多个障碍、动力学、执行器约束失效处理打印警告回退到安全停车或保守轨迹验证离线 Monte Carlo硬件在环、日志回放、故障注入在生产系统中建议不要直接传播协方差矩阵的原元素而是传播其 Cholesky 分解或信息矩阵。这样能避免数值误差破坏正定性也方便做稀疏化和降维。6.2 发布前检查清单以下清单可以直接用于一个“从闭包模型到规划器上线”的项目阶段检查状态方程的物理量纲是否一致控制输入的边界是否和真实执行器一致过程噪声矩阵Q是否来自传感器噪声、模型残差和扰动的实测而不是随便给值闭包假设是否与真实分布匹配至少在工作点附近做了一轮 Monte Carlo 对比机会约束的置信系数是否符合安全等级有没有因为k选太小而隐藏风险目标函数中终端均值和终端方差的权重是否经过调参而不是只按直觉设置优化器初始点是否选自一个可解或保守的轨迹避免从不可行域启动每次规划是否设置了超时上限超时后是否有回退方案日志中是否记录了控制序列、预测均值和方差、约束裕度便于事后定位问题。这些清单不复杂但每一项都对应真实项目里出现过的问题。忽略其中任何一项都可能在仿真通过后、真机运行阶段暴露出软硬件差异。7. 扩展方向与进一步学习路径7.1 从高斯闭包到无迹变换高斯闭包的误差主要来自线性化。无迹变换Unscented Transform是一种更强的扩展它选择一组确定性 sigma 点把每个点通过真实非线性函数传播再恢复新的均值和协方差。这种方法不需要计算 Jacobian也能捕捉到二阶精度。如果你的系统非线性明显建议直接在闭包传播里用无迹变换代替手推公式。7.2 从开环规划到闭环 MPC开环规划假设控制序列预先算好不根据观测调整。真实系统必须在执行过程中持续接收状态估计并重新规划。把矩闭包放进模型预测控制的滚动优化框架就是带不确定性的随机模型预测控制Stochastic MPC。闭环后状态估计每步更新不确定性会被不断“重置”方差通常不会像开环那样无限增长。7.3 从一阶矩到高阶累积量如果系统的状态分布明显非对称比如偏度对安全约束影响很大可以扩展到三阶累积量。传播三阶矩时需要忽略四阶以上累积量得到一组更高维的有限方程。这类方法的代价是公式更复杂、数值更难调节收益是能更准确地估计尾部风险。在实际项目中可以先画一画 Monte Carlo 分布图再决定是否需要这么高的阶数。7.4 给新手的练习建议完成本文一维例子之后可以做三个递进练习。第一把系统改成二维增加一个障碍物体验协方差矩阵在路径约束下的形状变化。第二在moment_propagate中改用无迹变换比较两种闭包预测的方差差异。第三在蒙特卡洛仿真中加入一个简单的卡尔曼滤波观察闭环状态估计对规划风险的影响。每一个练习都会强化同一个核心认知不确定性不是噪声系数而是规划问题里的一个状态维度。矩闭包只是描述这个维度的一种方式真正重要的是让规划器在优化时意识到它。实际项目里矩闭包不会替你解决所有不确定性问题但它提供了一个比“忽略方差”更可靠、比“大量采样”更高效的中间路径。当你需要在强实时系统中做概率感知的规划时闭包思想往往是那个最值得先落地的方案。