去年做移动机器人定点停靠的仿真项目时我遇到一个非常典型的现象车辆离目标还有三米时一切正常靠近目标之后反而开始原地画圈。起初我把锅全甩给MPC换了好几组权重也压不住。后来把量测数据拉出来看才发现问题根本不在控制器而在状态估计——里程计漂移加上测量噪声让MPC在一个假位置上拼命校正越校越偏。把滚动时域估计MHE和模型预测控制MPC结合成一个闭环之后这个问题才算彻底根治。这篇文章就把我调通这套系统的过程完整写出来从MHE的到达代价和先验误差讲起到MPC镇定控制器如何设计终端代价再到两套QP在Matlab里如何联调以及我踩过的几个明坑暗坑。适合正在做目标点镇定、移动机器人停靠、欠驱动系统定点控制或者打算把估计器与控制器做一体化设计的人参考。1. 目标点镇定为什么必须把估计和控制焊在一起1.1 镇定问题与跟踪问题在本质上的差异很多人是从路径跟踪开始接触MPC的跟踪任务天然带着一条参考轨迹误差来源相对明确控制器只要沿着轨迹追就行。但目标点镇定是另一回事系统的期望状态从头到尾只有一个点比如原点所有状态最终都要被压到这个小邻域里。这个任务看起来简单实际上比跟踪更容易翻车原因在于跟踪时目标在动模型误差会被持续激励并暴露出来而镇定任务在接近目标后控制量逐渐变小系统进入一个微调状态此时任何一点估计偏差都会直接变成稳态误差或被放大器放大成极限环。另一个本质差异是约束的参与程度。跟踪过程中控制量和状态约束通常会全程激活MPC的处理能力能发挥出来而镇定过程中约束只在接近目标时才真正起作用比如速度限制、控制饱和这些约束会让基于无约束LQR设计的心态崩溃。也就是说镇定任务必须从一开始就把约束写进优化问题而不是指望靠线性反馈硬压。1.2 单一MPC解决不了的三件事只靠MPC做镇定会同时撞上三个问题。第一MPC是一个状态反馈控制器它默认当前状态是已知且准确的但实际系统里往往只能量测部分状态或者量测值被噪声污染。第二个问题更隐蔽MPC的预测模型与真实系统之间必然存在失配失配会被反馈回路当成外部扰动处理反映在控制量上就是抖振和偏置。第三MPC自身的预测时域有限如果模型失配导致预测误差累积控制器看到的未来和真实未来偏差很大约束处理就会失效。这三个问题的共同根源是一个控制器对系统现在处于什么状态这件事没有可靠的认知。所以我当时得到的一个明确结论是——镇定问题不能只在控制端发力必须先在估计端把状态可信度做出来。这就轮到MHE出场了。MHE不是简单的低通滤波它把过去一个时间窗内的量测、模型、噪声统计全部纳入一个滚动优化问题估计出的状态不仅平滑还能给出协方差信息这个协方差恰好可以喂给MPC做鲁棒性处理。1.3 先验误差MHE的前情提要机制标题里的先验误差是理解MHE的关键也是很多教程一句话带过、实际实现却卡住的地方。MHE在时刻k处理的是一个滑动窗口比如窗口内有M个采样间隔覆盖从k-M到k的状态序列。优化变量不只有当前状态而是窗口起点状态和窗口内每一步的过程噪声。为了让滑动窗能接上历史信息必须在代价函数里加一项把窗口起点状态和上一轮估计给出的预测值之间的偏差惩罚进去这个偏差就是先验误差。打个比方MHE像是一个记者在写连续报道每篇只覆盖最近M天。新一期动笔前编辑会告诉他上期结尾时我们预测今天应该发生什么这个预测和实际开头的偏差就是先验误差。如果没有这项惩罚每个窗口的起点状态可以任意漂移估计结果就会一段一段地接不上状态曲线出现折点。把先验误差放进代价函数后MHE输出的就是一条既尊重历史又贴合最新量测的平滑状态轨迹。这个先验值的均值和协方差在文献里叫到达代价工程上最常用EKF一步预测去近似更新。2. 滚动时域估计在Matlab中的落地滑窗QP与到达代价更新2.1 把滑窗估计问题写成标准QPMHE的实现核心不是算法而是把一个受限的最优估计问题翻译成二次规划QP。以线性时不变系统为例模型为x(k1)A x(k)B u(k)w(k)量测y(k)C x(k)v(k)其中w和v分别是过程噪声和量测噪声协方差记为Qw和Rv。设在时刻k滑动窗口覆盖k-M到k共M个间隔决策变量取Z[x(k-M); w(k-M); w(k-M1); ...; w(k-1)]也就是窗口起点状态加窗口内的M个过程噪声向量。代价函数由三部分叠加先验误差项(x(k-M)-x̄)^T P0^{-1}(x(k-M)-x̄)过程噪声项Σw_i^T Qw^{-1} w_i量测残差项Σv_i^T Rv^{-1} v_i。其中v_i可以直接写成量测y_i减去由x(k-M)和w序列线性展开得到的预测量测所以整个代价函数是关于Z的二次型。把这三项合并后就得到标准的min 0.5 Z^T H Z f^T Z形式。如果你用Matlab内置的quadprog求解需要注意H矩阵必须对称正定。实际构造时我会分块来写H的左上角n×n块放到达代价协方差逆矩阵P0^{-1}中间放kron(eye(M), inv(Qw))量测残差项则通过一个残差矩阵G来叠加最终H H0 G * kron(eye(M1), inv(Rv)) * G线性项f也由G和量测值算出。把窗口起点状态和噪声序列作为决策变量而不是把窗口内所有状态都当变量矩阵规模能小很多quadprog求解速度也快。2.2 到达代价近似不能只给均值还得给方差到达代价的理论形式是窗口起点状态的精确后验分布这在递推框架里会变得非常复杂实际工程几乎不会维护完整分布而是用一个高斯近似来替代也就是维护均值x̄和协方差P0。均值表示此刻窗口起点最可能在哪协方差表示这个预测有多可信。我刚实现时犯过一个错误只更新均值协方差一直固定不动。结果窗口一滑动先验误差的惩罚权重就不对估计值对老量测过度信任状态曲线明显滞后。正确做法是每次MHE求解完成后把窗口起点状态的估计值x(k-M)和估计出的w(k-M)代进模型得到下一个窗口起点的均值预测协方差则用一步预测更新P_next A P0 A Qw。这个近似在文献里叫平滑约束的EKF到达代价虽然理论上有更严格的Fisher信息矩阵更新方法但对绝大多数工程场景这个近似已经足够稳定。提示P0的初始值不要拍脑袋设。若系统状态量纲差异大先对状态做归一化或给对角线协方差赋合理物理量级否则QP的数值条件会变差quadprog偶尔会报H must be positive definite。2.3 一个可复用的MHE核心代码下面这段是我在Matlab里实际使用的MHE核心逻辑简化版。它接受一个窗口的量测序列和控制序列返回当前状态估计和更新后的到达代价均值与协方差逆。function [xhat, xbar_next, Pinv_next] mhe_step(Y, U, xbar, Pinv, A, B, C, Qw, Rv, M) n size(A,1); m size(B,2); nvars n M*n; H zeros(nvars); f zeros(nvars,1); % 先验误差项到达代价 H(1:n, 1:n) Pinv; f(1:n) -Pinv * xbar; % 过程噪声惩罚 H(n1:end, n1:end) kron(eye(M), inv(Qw)); % 量测残差项y_i C * x_{k-Mi} v_i % x_{k-Mi} A^i*x_start sum_{t0}^{i-1} A^{i-1-t}(B*U_t w_t) G []; ybar []; for i 0:M row_x0 C * A^i; % 对窗口起点状态的贡献 row_w zeros(n, M*n); for t 0:i-1 row_w(:, t*n1 : t*nn) C * A^(i-1-t); end G [G; row_x0, row_w]; % 扣除已知输入贡献后的等效量测 xu zeros(n,1); for t 0:i-1 xu xu A^(i-1-t) * B * U(t1); end ybar [ybar; Y(i1) - C*xu]; end Rinv kron(eye(M1), inv(Rv)); H H G * Rinv * G; f f - G * Rinv * ybar; opts optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); Z quadprog(H, f, [], [], [], [], [], [], [], opts); x0_hat Z(1:n); w_hat Z(n1:end); % 当前时刻状态估计从窗口起点推到最后 xk A^M * x0_hat; for t 0:M-1 xk xk A^(M-1-t) * (B*U(t1) w_hat(t*n1:t*nn)); end xhat xk; % 到达代价均值与协方差传递到下一个窗口 xbar_next A * x0_hat B * U(1) w_hat(1:n); P_next A * (Pinv \ eye(n)) * A Qw; Pinv_next inv(P_next); end这个版本牺牲了一点理论上的最优性换来了代码可读性和调试方便。实际使用时我会把Pinv直接存成Cholesky因子避免每步求逆不过小规模系统这样写问题不大。MHE窗口M一般取5到20窗口太大时QP规模增长很快而且对模型误差反而更敏感。3. 面向原点镇定的MPC设计误差坐标与终端代价3.1 先把目标点平移到原点目标点镇定里一个容易忽略的预处理步骤是坐标平移。无论目标点是(1,2)还是(3,-1)都应该先定义一个偏移量把优化问题的平衡点搬到原点否则MPC的代价函数里会多出常数项约束表达也会变得啰嗦。具体做法是定义误差状态e x - x_ref并假设目标点为系统的平衡点那么开环模型就能改写成关于e的形式e(k1) A e(k) B u(k) d(k)其中d(k)是线性化残差或外部扰动。后文所有代价和约束都基于e描述。对于非线性系统如果目标点不是平衡点还需要先解平衡方程并引入常值前馈控制u_ref再把增量控制作用Δu u - u_ref作为MPC的决策变量。这一步看着简单实际工程里很多人忘了算u_ref导致MPC永远在朝一个不存在的平衡点努力稳态误差死活消不掉。3.2 终端代价比终端约束好用但别随便拿Q放大要保证MPC镇定闭环稳定理论上最干净的方法是加终端约束也就是让预测末端状态必须落在某个不变集内同时切换成局部反馈控制律u-K e。但终端约束在数值上很麻烦它会把可行域缩小quadprog的收敛性也会下降工程上实现成本高。比较务实的做法是只加终端代价也就是在代价函数末尾加一项e(kN)^T P_term e(kN)其中P_term用离散代数黎卡提方程求解来自无约束LQR问题P_term dlqr(A, B, Q, R)。为什么不能把P_term粗暴设为某个大数乘以Q因为终端代价的本质是预测时域之外那段未来的价值近似。如果P_term设置不当MPC会把大量控制能量提前透支系统靠近目标时反而出现超调。dlqr解出的P_term和预测时域N是配套的N越小P_term越要承担稳定性的兜底作用N越大终端代价的影响相对变弱但仍不可省略。实测经验是N取10到20、Q取对角权重、P_term由dlqr算出来后闭环基本能达到镇得住、不飞车的效果。如果想进一步收紧稳态精度可以在此基础上加一个小比例的高增益前馈但不要直接把Q放大否则约束会频繁激活MPC退化成bang-bang控制。3.3 MPC的QP矩阵构建我用预测矩阵方式构建MPC的QP原因是在Matlab里这种写法最直白也方便调试。将未来N步控制序列记为U[u(k); u(k1); ...; u(kN-1)]预测状态可以展开成X Phi x(k) Gamma U。代价函数为X^T Qbar X U^T Rbar U其中Qbar是分块对角阵最后一块用P_term替换。展开后就得到决策变量U的标准二次型function u mpc_solve(x0, A, B, N, Q, R, Pterm, umax) n size(A,1); m size(B,2); Phi zeros(n*N, n); Gamma zeros(n*N, m*N); for i 1:N Phi((i-1)*n1:i*n, :) A^i; for j 1:i Gamma((i-1)*n1:i*n, (j-1)*m1:j*m) A^(i-j) * B; end end Qbar kron(eye(N), Q); Qbar(end-n1:end, end-n1:end) Pterm; Rbar kron(eye(N), R); H 2 * (Gamma * Qbar * Gamma Rbar); f 2 * x0 * Phi * Qbar * Gamma; lb -umax * ones(N*m, 1); ub umax * ones(N*m, 1); options optimoptions(quadprog, Display, off); U quadprog(H, f, [], [], [], [], lb, ub, [], options); u U(1:m); end这个函数每次调用返回第一个控制量然后进入下一个采样周期滚动求解。速度约束如果需要可以写成G_ineq U h_ineq的形式加进去。我不建议在这个阶段把速度约束也做到U的上下界里因为速度是关于状态的量和U是线性关系直接写不等式矩阵更清晰。4. MHE与MPC的集成细节数据流、鲁棒性与延迟4.1 单速率联立先估计后控制的执行序列MHE和MPC集成最朴素也最稳定的方式是单速率联立每个采样周期先求解MHE用当前时刻的最优估计x̂(k)作为MPC的初始状态再求解MPC得到u(k)。由于两个优化器共享同一个离散模型A,B模型不一致带来的隐性偏差可以消除。这个执行顺序不能反过来否则控制器用的还是上一拍的状态系统会出现一拍滞后。在主循环代码里我会把MHE和MPC写成两个独立函数中间不穿插任何滤波。曾经试过在MHE和MPC之间再加一个卡尔曼滤波平滑结果三重滤波引入额外相位滞后估计曲线更平了但镇定响应急剧变慢。MHE本身已经是一个平滑器不需要在它下游再接滤波。数据流方面要注意MHE需要的Y序列是原始量测不能先过均值滤波。MHE的设计前提就是量测含白噪声它会自己处理噪声统计如果提前滤波等效于改变了Rv反而让MHE对噪声统计的设定失真。4.2 估计误差传导约束收紧与协方差利用MHE输出x̂(k)的同时我们手里其实还握着一个协方差矩阵P(k|k)它表示当前估计的不确定度。很多集成方案只把x̂喂给MPC把P丢掉了这很可惜。一个轻量且有效的做法是约束收紧从MHE协方差中提取每个状态分量的标准差σ_i在MPC约束里留下安全裕度。比如控制量约束从|u|≤u_max改成|u|≤u_max - κ·σ_u其中κ取2到3。这么做的好处是当量测噪声变大时MHE的协方差会相应变大MPC的控制边界自动收缩系统会用更保守的控制策略来换取安全当估计很可信时约束自动放松控制性能不受影响。这个自适应特性对目标点镇定特别有价值因为靠近目标点后量测信噪比往往变差如果没有约束收紧MPC很容易为了消除微小的估计误差而猛打方向。4.3 求解延迟补偿一步预测的空窗填补Matlab里quadprog求解QP的速度很快但毕竟不是毫秒级特别当MHE窗口较大或MPC预测时域较长时求解时间可能达到毫秒甚至几十毫秒。如果把这部分时间直接算进控制周期等效于控制量晚送达镇定过程会有可感知的抖动。我用的补偿方法是MHE求解完成后先用模型做一步预测把x̂(k)前推到真正的控制施加时刻比如x_comp Ax̂(k) Bu(k-1)再用这个补偿状态去初始化MPC。这一步预测虽然简单但能把求解延迟的影响从控制周期级别降到模型误差级别。注意这里必须用上一步控制量的实际输出值而不是MPC上一步计算出的优化值因为执行机构可能存在饱和或滞后。5. 双积分器仿真实战从参数表到闭环曲线5.1 为什么会拿双积分器当试金石双积分器模型x_ddotu是检验状态估计与控制集成的经典试金石它同时具备两个关键困难位置量测容易速度状态却往往不可直接量测系统本身是开环临界稳定的不加控制就漂移。把MHE和MPC放到这个模型上跑通再迁移到移动机器人或倒立摆上就有底气了。我仿真的模型取采样周期Ts0.1s连续模型离散化为A[1 Ts;0 1]B[0.5*Ts^2; Ts]量测矩阵C[1 0]也就是只量测位置速度完全交给MHE去估计。目标点是原点初始状态为p(0)5mv(0)1m/s。量测加入标准差0.05m的高斯白噪声过程噪声取标准差0.01模拟模型失配和未建模扰动。5.2 主循环代码框架主循环的逻辑很直接关键是维护一个环形缓冲去存量测序列。初始化时设置到达代价均值为首个量测值协方差设为单位阵乘以一个小系数。下面给出主循环的核心片段。Ts 0.1; A [1 Ts; 0 1]; B [0.5*Ts^2; Ts]; C [1 0]; Qw diag([0.01, 0.01]); Rv 0.05^2; M 10; N 15; Q diag([10, 1]); R 0.5; Pterm dlqr(A, B, Q, R); umax 2; x_true [5; 1]; u_prev 0; xbar [5; 1]; Pinv inv(eye(2) * 0.5); Ybuf []; Ubuf []; k 0; while k 200 % 量测生成仅位置 y C * x_true sqrt(Rv) * randn(1); % 滚动窗口填充 Ybuf [Ybuf, y]; Ubuf [Ubuf, u_prev]; if length(Ybuf) M1 Ybuf(:,1) []; Ubuf(:,1) []; end % MHE [xhat, xbar, Pinv] mhe_step(Ybuf, Ubuf, xbar, Pinv, A, B, C, Qw, Rv, M); % MPC u mpc_solve(xhat, A, B, N, Q, R, Pterm, umax); % 仿真推进 w_true mvnrnd(zeros(2,1), Qw); x_true A * x_true B * u w_true; u_prev u; k k 1; end注意我把到达代价的均值和协方差作为状态变量在循环里传递这是MHE能否持续收敛的关键。一开始窗口不满M步时MHE函数里的量测矩阵会缺行可以采用变窗口策略量测数量少时只构造对应行等缓冲满后再切到完整窗口。5.3 参数表与调参心得我调参时把参数分成了两组估计组控制状态曲线平不平控制组控制收敛快不快。下面是经过多次试验后比较稳定的一组参数以及对应表现。参数取值作用常见错误MHE窗口M10越大越平滑但模型失配影响越大过大会让估计响应迟钝Qwdiag([0.01, 0.01])模型不确定度决定信任模型的程度过小则量测噪声直接穿透Rv0.05^2量测噪声方差过大会让估计过于依赖模型MPC预测时域N15越大约束越好处理但QP变慢过小会牺牲稳定性Qdiag([10, 1])位置误差权重为主速度权重太低会超调R0.5控制量惩罚过大会收敛过慢Ptermdlqr(A,B,Q,R)终端代价不能随意取大数调参顺序上我建议先固定M和Qw、Rv纯调MPC的Q、R、N让系统在状态完全已知的假设下能稳定镇定然后再引入量测噪声去调MHE参数。如果反过来两边同时乱调出了问题根本分不清是估计滞后还是控制振荡。6. 调试中反复踩的坑与规避方法6.1 到达代价初始协方差定得太大第一次跑通MHE时我把P0初始化为100倍单位阵理由是先验不确定就放宽惩罚结果状态估计曲线在开头几百拍剧烈摆动。原因很直接先验误差项权重太小窗口起点状态几乎只由第一个量测决定到达代价形同虚设。后来把P0初始协方差缩到和量测噪声同数量级估计曲线立刻稳定。正确的直觉是到达代价协方差表示历史信息对新窗口起点的约束强度太小会僵化、太大会健忘需要量级匹配。这里我还会做一件事每50步打印一次Pinv的对角线观察它是否漂移。正常情况应该是缓慢变化如果出现指数级增长或衰减说明模型或噪声统计设置有误而不是Numerical的问题。6.2 终端代价与预测时域不匹配我试过把N从15改成5但没同步调整Pterm结果闭环出现持续振荡。5步预测时域太短末端状态还离原点很远终端代价想硬拉也拉不住。后来我把N和Pterm捆绑起来调短时域就必须用更靠近原点的终端集否则要加大Pterm里的位置权重让末端状态更早收敛。严格来说应该用终端代价加终端约束才能理论上保证稳定工程上我倾向于选一个足够大的N比如10到20让终端代价只承担锦上添花的作用这样即使模型失配鲁棒性也更好。6.3 Qw过小/Rv过大的连锁反应MHE和MPC参数之间不是独立的。有一轮我为了追求估计曲线平滑把Qw压到1e-4Rv设成0.5MHE确实输出了极平滑的位置估计但代价是速度估计完全跟不上真实运动。MPC拿到滞后的速度估计后产生了一个持续的相位差镇定任务直接失败。这个现象说明MHE的Qw和Rv本质上在回答你更信模型还是更信量测选哪边都不能极端必须和真实物理系统的噪声水平匹配。我会用一段离线数据跑一次卡尔曼滤波做噪声统计标定再把它换算成Qw和Rv的初值而不是纯靠手感。如果让我重新做一次这个项目我会把MHE窗口起点处保存的到达代价协方差逆矩阵直接传给MPC做约束收紧这样等于把估计不确定性显式带进了控制设计。这套方法虽然比MHEMPC各自为政多花一点调试时间但换来的是系统在噪声变大、模型失配时不会突然失控镇定过程始终有底线。 
长河君 · 2026/10/20 18:04:00 · 阅读 1.2k 滚动时域估计与模型预测控制结合：移动机器人目标点镇定实战
去年做移动机器人定点停靠的仿真项目时我遇到一个非常典型的现象车辆离目标还有三米时一切正常靠近目标之后反而开始原地画圈。起初我把锅全甩给MPC换了好几组权重也压不住。后来把量测数据拉出来看才发现问题根本不在控制器而在状态估计——里程计漂移加上测量噪声让MPC在一个假位置上拼命校正越校越偏。把滚动时域估计MHE和模型预测控制MPC结合成一个闭环之后这个问题才算彻底根治。这篇文章就把我调通这套系统的过程完整写出来从MHE的到达代价和先验误差讲起到MPC镇定控制器如何设计终端代价再到两套QP在Matlab里如何联调以及我踩过的几个明坑暗坑。适合正在做目标点镇定、移动机器人停靠、欠驱动系统定点控制或者打算把估计器与控制器做一体化设计的人参考。1. 目标点镇定为什么必须把估计和控制焊在一起1.1 镇定问题与跟踪问题在本质上的差异很多人是从路径跟踪开始接触MPC的跟踪任务天然带着一条参考轨迹误差来源相对明确控制器只要沿着轨迹追就行。但目标点镇定是另一回事系统的期望状态从头到尾只有一个点比如原点所有状态最终都要被压到这个小邻域里。这个任务看起来简单实际上比跟踪更容易翻车原因在于跟踪时目标在动模型误差会被持续激励并暴露出来而镇定任务在接近目标后控制量逐渐变小系统进入一个微调状态此时任何一点估计偏差都会直接变成稳态误差或被放大器放大成极限环。另一个本质差异是约束的参与程度。跟踪过程中控制量和状态约束通常会全程激活MPC的处理能力能发挥出来而镇定过程中约束只在接近目标时才真正起作用比如速度限制、控制饱和这些约束会让基于无约束LQR设计的心态崩溃。也就是说镇定任务必须从一开始就把约束写进优化问题而不是指望靠线性反馈硬压。1.2 单一MPC解决不了的三件事只靠MPC做镇定会同时撞上三个问题。第一MPC是一个状态反馈控制器它默认当前状态是已知且准确的但实际系统里往往只能量测部分状态或者量测值被噪声污染。第二个问题更隐蔽MPC的预测模型与真实系统之间必然存在失配失配会被反馈回路当成外部扰动处理反映在控制量上就是抖振和偏置。第三MPC自身的预测时域有限如果模型失配导致预测误差累积控制器看到的未来和真实未来偏差很大约束处理就会失效。这三个问题的共同根源是一个控制器对系统现在处于什么状态这件事没有可靠的认知。所以我当时得到的一个明确结论是——镇定问题不能只在控制端发力必须先在估计端把状态可信度做出来。这就轮到MHE出场了。MHE不是简单的低通滤波它把过去一个时间窗内的量测、模型、噪声统计全部纳入一个滚动优化问题估计出的状态不仅平滑还能给出协方差信息这个协方差恰好可以喂给MPC做鲁棒性处理。1.3 先验误差MHE的前情提要机制标题里的先验误差是理解MHE的关键也是很多教程一句话带过、实际实现却卡住的地方。MHE在时刻k处理的是一个滑动窗口比如窗口内有M个采样间隔覆盖从k-M到k的状态序列。优化变量不只有当前状态而是窗口起点状态和窗口内每一步的过程噪声。为了让滑动窗能接上历史信息必须在代价函数里加一项把窗口起点状态和上一轮估计给出的预测值之间的偏差惩罚进去这个偏差就是先验误差。打个比方MHE像是一个记者在写连续报道每篇只覆盖最近M天。新一期动笔前编辑会告诉他上期结尾时我们预测今天应该发生什么这个预测和实际开头的偏差就是先验误差。如果没有这项惩罚每个窗口的起点状态可以任意漂移估计结果就会一段一段地接不上状态曲线出现折点。把先验误差放进代价函数后MHE输出的就是一条既尊重历史又贴合最新量测的平滑状态轨迹。这个先验值的均值和协方差在文献里叫到达代价工程上最常用EKF一步预测去近似更新。2. 滚动时域估计在Matlab中的落地滑窗QP与到达代价更新2.1 把滑窗估计问题写成标准QPMHE的实现核心不是算法而是把一个受限的最优估计问题翻译成二次规划QP。以线性时不变系统为例模型为x(k1)A x(k)B u(k)w(k)量测y(k)C x(k)v(k)其中w和v分别是过程噪声和量测噪声协方差记为Qw和Rv。设在时刻k滑动窗口覆盖k-M到k共M个间隔决策变量取Z[x(k-M); w(k-M); w(k-M1); ...; w(k-1)]也就是窗口起点状态加窗口内的M个过程噪声向量。代价函数由三部分叠加先验误差项(x(k-M)-x̄)^T P0^{-1}(x(k-M)-x̄)过程噪声项Σw_i^T Qw^{-1} w_i量测残差项Σv_i^T Rv^{-1} v_i。其中v_i可以直接写成量测y_i减去由x(k-M)和w序列线性展开得到的预测量测所以整个代价函数是关于Z的二次型。把这三项合并后就得到标准的min 0.5 Z^T H Z f^T Z形式。如果你用Matlab内置的quadprog求解需要注意H矩阵必须对称正定。实际构造时我会分块来写H的左上角n×n块放到达代价协方差逆矩阵P0^{-1}中间放kron(eye(M), inv(Qw))量测残差项则通过一个残差矩阵G来叠加最终H H0 G * kron(eye(M1), inv(Rv)) * G线性项f也由G和量测值算出。把窗口起点状态和噪声序列作为决策变量而不是把窗口内所有状态都当变量矩阵规模能小很多quadprog求解速度也快。2.2 到达代价近似不能只给均值还得给方差到达代价的理论形式是窗口起点状态的精确后验分布这在递推框架里会变得非常复杂实际工程几乎不会维护完整分布而是用一个高斯近似来替代也就是维护均值x̄和协方差P0。均值表示此刻窗口起点最可能在哪协方差表示这个预测有多可信。我刚实现时犯过一个错误只更新均值协方差一直固定不动。结果窗口一滑动先验误差的惩罚权重就不对估计值对老量测过度信任状态曲线明显滞后。正确做法是每次MHE求解完成后把窗口起点状态的估计值x(k-M)和估计出的w(k-M)代进模型得到下一个窗口起点的均值预测协方差则用一步预测更新P_next A P0 A Qw。这个近似在文献里叫平滑约束的EKF到达代价虽然理论上有更严格的Fisher信息矩阵更新方法但对绝大多数工程场景这个近似已经足够稳定。提示P0的初始值不要拍脑袋设。若系统状态量纲差异大先对状态做归一化或给对角线协方差赋合理物理量级否则QP的数值条件会变差quadprog偶尔会报H must be positive definite。2.3 一个可复用的MHE核心代码下面这段是我在Matlab里实际使用的MHE核心逻辑简化版。它接受一个窗口的量测序列和控制序列返回当前状态估计和更新后的到达代价均值与协方差逆。function [xhat, xbar_next, Pinv_next] mhe_step(Y, U, xbar, Pinv, A, B, C, Qw, Rv, M) n size(A,1); m size(B,2); nvars n M*n; H zeros(nvars); f zeros(nvars,1); % 先验误差项到达代价 H(1:n, 1:n) Pinv; f(1:n) -Pinv * xbar; % 过程噪声惩罚 H(n1:end, n1:end) kron(eye(M), inv(Qw)); % 量测残差项y_i C * x_{k-Mi} v_i % x_{k-Mi} A^i*x_start sum_{t0}^{i-1} A^{i-1-t}(B*U_t w_t) G []; ybar []; for i 0:M row_x0 C * A^i; % 对窗口起点状态的贡献 row_w zeros(n, M*n); for t 0:i-1 row_w(:, t*n1 : t*nn) C * A^(i-1-t); end G [G; row_x0, row_w]; % 扣除已知输入贡献后的等效量测 xu zeros(n,1); for t 0:i-1 xu xu A^(i-1-t) * B * U(t1); end ybar [ybar; Y(i1) - C*xu]; end Rinv kron(eye(M1), inv(Rv)); H H G * Rinv * G; f f - G * Rinv * ybar; opts optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); Z quadprog(H, f, [], [], [], [], [], [], [], opts); x0_hat Z(1:n); w_hat Z(n1:end); % 当前时刻状态估计从窗口起点推到最后 xk A^M * x0_hat; for t 0:M-1 xk xk A^(M-1-t) * (B*U(t1) w_hat(t*n1:t*nn)); end xhat xk; % 到达代价均值与协方差传递到下一个窗口 xbar_next A * x0_hat B * U(1) w_hat(1:n); P_next A * (Pinv \ eye(n)) * A Qw; Pinv_next inv(P_next); end这个版本牺牲了一点理论上的最优性换来了代码可读性和调试方便。实际使用时我会把Pinv直接存成Cholesky因子避免每步求逆不过小规模系统这样写问题不大。MHE窗口M一般取5到20窗口太大时QP规模增长很快而且对模型误差反而更敏感。3. 面向原点镇定的MPC设计误差坐标与终端代价3.1 先把目标点平移到原点目标点镇定里一个容易忽略的预处理步骤是坐标平移。无论目标点是(1,2)还是(3,-1)都应该先定义一个偏移量把优化问题的平衡点搬到原点否则MPC的代价函数里会多出常数项约束表达也会变得啰嗦。具体做法是定义误差状态e x - x_ref并假设目标点为系统的平衡点那么开环模型就能改写成关于e的形式e(k1) A e(k) B u(k) d(k)其中d(k)是线性化残差或外部扰动。后文所有代价和约束都基于e描述。对于非线性系统如果目标点不是平衡点还需要先解平衡方程并引入常值前馈控制u_ref再把增量控制作用Δu u - u_ref作为MPC的决策变量。这一步看着简单实际工程里很多人忘了算u_ref导致MPC永远在朝一个不存在的平衡点努力稳态误差死活消不掉。3.2 终端代价比终端约束好用但别随便拿Q放大要保证MPC镇定闭环稳定理论上最干净的方法是加终端约束也就是让预测末端状态必须落在某个不变集内同时切换成局部反馈控制律u-K e。但终端约束在数值上很麻烦它会把可行域缩小quadprog的收敛性也会下降工程上实现成本高。比较务实的做法是只加终端代价也就是在代价函数末尾加一项e(kN)^T P_term e(kN)其中P_term用离散代数黎卡提方程求解来自无约束LQR问题P_term dlqr(A, B, Q, R)。为什么不能把P_term粗暴设为某个大数乘以Q因为终端代价的本质是预测时域之外那段未来的价值近似。如果P_term设置不当MPC会把大量控制能量提前透支系统靠近目标时反而出现超调。dlqr解出的P_term和预测时域N是配套的N越小P_term越要承担稳定性的兜底作用N越大终端代价的影响相对变弱但仍不可省略。实测经验是N取10到20、Q取对角权重、P_term由dlqr算出来后闭环基本能达到镇得住、不飞车的效果。如果想进一步收紧稳态精度可以在此基础上加一个小比例的高增益前馈但不要直接把Q放大否则约束会频繁激活MPC退化成bang-bang控制。3.3 MPC的QP矩阵构建我用预测矩阵方式构建MPC的QP原因是在Matlab里这种写法最直白也方便调试。将未来N步控制序列记为U[u(k); u(k1); ...; u(kN-1)]预测状态可以展开成X Phi x(k) Gamma U。代价函数为X^T Qbar X U^T Rbar U其中Qbar是分块对角阵最后一块用P_term替换。展开后就得到决策变量U的标准二次型function u mpc_solve(x0, A, B, N, Q, R, Pterm, umax) n size(A,1); m size(B,2); Phi zeros(n*N, n); Gamma zeros(n*N, m*N); for i 1:N Phi((i-1)*n1:i*n, :) A^i; for j 1:i Gamma((i-1)*n1:i*n, (j-1)*m1:j*m) A^(i-j) * B; end end Qbar kron(eye(N), Q); Qbar(end-n1:end, end-n1:end) Pterm; Rbar kron(eye(N), R); H 2 * (Gamma * Qbar * Gamma Rbar); f 2 * x0 * Phi * Qbar * Gamma; lb -umax * ones(N*m, 1); ub umax * ones(N*m, 1); options optimoptions(quadprog, Display, off); U quadprog(H, f, [], [], [], [], lb, ub, [], options); u U(1:m); end这个函数每次调用返回第一个控制量然后进入下一个采样周期滚动求解。速度约束如果需要可以写成G_ineq U h_ineq的形式加进去。我不建议在这个阶段把速度约束也做到U的上下界里因为速度是关于状态的量和U是线性关系直接写不等式矩阵更清晰。4. MHE与MPC的集成细节数据流、鲁棒性与延迟4.1 单速率联立先估计后控制的执行序列MHE和MPC集成最朴素也最稳定的方式是单速率联立每个采样周期先求解MHE用当前时刻的最优估计x̂(k)作为MPC的初始状态再求解MPC得到u(k)。由于两个优化器共享同一个离散模型A,B模型不一致带来的隐性偏差可以消除。这个执行顺序不能反过来否则控制器用的还是上一拍的状态系统会出现一拍滞后。在主循环代码里我会把MHE和MPC写成两个独立函数中间不穿插任何滤波。曾经试过在MHE和MPC之间再加一个卡尔曼滤波平滑结果三重滤波引入额外相位滞后估计曲线更平了但镇定响应急剧变慢。MHE本身已经是一个平滑器不需要在它下游再接滤波。数据流方面要注意MHE需要的Y序列是原始量测不能先过均值滤波。MHE的设计前提就是量测含白噪声它会自己处理噪声统计如果提前滤波等效于改变了Rv反而让MHE对噪声统计的设定失真。4.2 估计误差传导约束收紧与协方差利用MHE输出x̂(k)的同时我们手里其实还握着一个协方差矩阵P(k|k)它表示当前估计的不确定度。很多集成方案只把x̂喂给MPC把P丢掉了这很可惜。一个轻量且有效的做法是约束收紧从MHE协方差中提取每个状态分量的标准差σ_i在MPC约束里留下安全裕度。比如控制量约束从|u|≤u_max改成|u|≤u_max - κ·σ_u其中κ取2到3。这么做的好处是当量测噪声变大时MHE的协方差会相应变大MPC的控制边界自动收缩系统会用更保守的控制策略来换取安全当估计很可信时约束自动放松控制性能不受影响。这个自适应特性对目标点镇定特别有价值因为靠近目标点后量测信噪比往往变差如果没有约束收紧MPC很容易为了消除微小的估计误差而猛打方向。4.3 求解延迟补偿一步预测的空窗填补Matlab里quadprog求解QP的速度很快但毕竟不是毫秒级特别当MHE窗口较大或MPC预测时域较长时求解时间可能达到毫秒甚至几十毫秒。如果把这部分时间直接算进控制周期等效于控制量晚送达镇定过程会有可感知的抖动。我用的补偿方法是MHE求解完成后先用模型做一步预测把x̂(k)前推到真正的控制施加时刻比如x_comp Ax̂(k) Bu(k-1)再用这个补偿状态去初始化MPC。这一步预测虽然简单但能把求解延迟的影响从控制周期级别降到模型误差级别。注意这里必须用上一步控制量的实际输出值而不是MPC上一步计算出的优化值因为执行机构可能存在饱和或滞后。5. 双积分器仿真实战从参数表到闭环曲线5.1 为什么会拿双积分器当试金石双积分器模型x_ddotu是检验状态估计与控制集成的经典试金石它同时具备两个关键困难位置量测容易速度状态却往往不可直接量测系统本身是开环临界稳定的不加控制就漂移。把MHE和MPC放到这个模型上跑通再迁移到移动机器人或倒立摆上就有底气了。我仿真的模型取采样周期Ts0.1s连续模型离散化为A[1 Ts;0 1]B[0.5*Ts^2; Ts]量测矩阵C[1 0]也就是只量测位置速度完全交给MHE去估计。目标点是原点初始状态为p(0)5mv(0)1m/s。量测加入标准差0.05m的高斯白噪声过程噪声取标准差0.01模拟模型失配和未建模扰动。5.2 主循环代码框架主循环的逻辑很直接关键是维护一个环形缓冲去存量测序列。初始化时设置到达代价均值为首个量测值协方差设为单位阵乘以一个小系数。下面给出主循环的核心片段。Ts 0.1; A [1 Ts; 0 1]; B [0.5*Ts^2; Ts]; C [1 0]; Qw diag([0.01, 0.01]); Rv 0.05^2; M 10; N 15; Q diag([10, 1]); R 0.5; Pterm dlqr(A, B, Q, R); umax 2; x_true [5; 1]; u_prev 0; xbar [5; 1]; Pinv inv(eye(2) * 0.5); Ybuf []; Ubuf []; k 0; while k 200 % 量测生成仅位置 y C * x_true sqrt(Rv) * randn(1); % 滚动窗口填充 Ybuf [Ybuf, y]; Ubuf [Ubuf, u_prev]; if length(Ybuf) M1 Ybuf(:,1) []; Ubuf(:,1) []; end % MHE [xhat, xbar, Pinv] mhe_step(Ybuf, Ubuf, xbar, Pinv, A, B, C, Qw, Rv, M); % MPC u mpc_solve(xhat, A, B, N, Q, R, Pterm, umax); % 仿真推进 w_true mvnrnd(zeros(2,1), Qw); x_true A * x_true B * u w_true; u_prev u; k k 1; end注意我把到达代价的均值和协方差作为状态变量在循环里传递这是MHE能否持续收敛的关键。一开始窗口不满M步时MHE函数里的量测矩阵会缺行可以采用变窗口策略量测数量少时只构造对应行等缓冲满后再切到完整窗口。5.3 参数表与调参心得我调参时把参数分成了两组估计组控制状态曲线平不平控制组控制收敛快不快。下面是经过多次试验后比较稳定的一组参数以及对应表现。参数取值作用常见错误MHE窗口M10越大越平滑但模型失配影响越大过大会让估计响应迟钝Qwdiag([0.01, 0.01])模型不确定度决定信任模型的程度过小则量测噪声直接穿透Rv0.05^2量测噪声方差过大会让估计过于依赖模型MPC预测时域N15越大约束越好处理但QP变慢过小会牺牲稳定性Qdiag([10, 1])位置误差权重为主速度权重太低会超调R0.5控制量惩罚过大会收敛过慢Ptermdlqr(A,B,Q,R)终端代价不能随意取大数调参顺序上我建议先固定M和Qw、Rv纯调MPC的Q、R、N让系统在状态完全已知的假设下能稳定镇定然后再引入量测噪声去调MHE参数。如果反过来两边同时乱调出了问题根本分不清是估计滞后还是控制振荡。6. 调试中反复踩的坑与规避方法6.1 到达代价初始协方差定得太大第一次跑通MHE时我把P0初始化为100倍单位阵理由是先验不确定就放宽惩罚结果状态估计曲线在开头几百拍剧烈摆动。原因很直接先验误差项权重太小窗口起点状态几乎只由第一个量测决定到达代价形同虚设。后来把P0初始协方差缩到和量测噪声同数量级估计曲线立刻稳定。正确的直觉是到达代价协方差表示历史信息对新窗口起点的约束强度太小会僵化、太大会健忘需要量级匹配。这里我还会做一件事每50步打印一次Pinv的对角线观察它是否漂移。正常情况应该是缓慢变化如果出现指数级增长或衰减说明模型或噪声统计设置有误而不是Numerical的问题。6.2 终端代价与预测时域不匹配我试过把N从15改成5但没同步调整Pterm结果闭环出现持续振荡。5步预测时域太短末端状态还离原点很远终端代价想硬拉也拉不住。后来我把N和Pterm捆绑起来调短时域就必须用更靠近原点的终端集否则要加大Pterm里的位置权重让末端状态更早收敛。严格来说应该用终端代价加终端约束才能理论上保证稳定工程上我倾向于选一个足够大的N比如10到20让终端代价只承担锦上添花的作用这样即使模型失配鲁棒性也更好。6.3 Qw过小/Rv过大的连锁反应MHE和MPC参数之间不是独立的。有一轮我为了追求估计曲线平滑把Qw压到1e-4Rv设成0.5MHE确实输出了极平滑的位置估计但代价是速度估计完全跟不上真实运动。MPC拿到滞后的速度估计后产生了一个持续的相位差镇定任务直接失败。这个现象说明MHE的Qw和Rv本质上在回答你更信模型还是更信量测选哪边都不能极端必须和真实物理系统的噪声水平匹配。我会用一段离线数据跑一次卡尔曼滤波做噪声统计标定再把它换算成Qw和Rv的初值而不是纯靠手感。如果让我重新做一次这个项目我会把MHE窗口起点处保存的到达代价协方差逆矩阵直接传给MPC做约束收紧这样等于把估计不确定性显式带进了控制设计。这套方法虽然比MHEMPC各自为政多花一点调试时间但换来的是系统在噪声变大、模型失配时不会突然失控镇定过程始终有底线。