企业官网建设流程全解析

去年做移动机器人定点停靠的仿真项目时，我遇到一个非常典型的现象：车辆离目标还有三米时一切正常，靠近目标之后反而开始原地画圈。起初我把锅全甩给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 把滑窗估计问题写成标准QP

MHE的实现核心不是算法，而是把一个受限的最优估计问题翻译成二次规划（QP）。以线性时不变系统为例，模型为x(k+1)=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-M+1); ...; 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(M+1), 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(n+1:end, n+1:end) = kron(eye(M), inv(Qw)); % 量测残差项：y_i = C * x_{k-M+i} + v_i % x_{k-M+i} = A^i*x_start + sum_{t=0}^{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*n+1 : t*n+n) = 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(t+1); end ybar = [ybar; Y(i+1) - C*xu]; end Rinv = kron(eye(M+1), 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(n+1:end); % 当前时刻状态估计：从窗口起点推到最后 xk = A^M * x0_hat; for t = 0:M-1 xk = xk + A^(M-1-t) * (B*U(t+1) + w_hat(t*n+1:t*n+n)); 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(k+1) = 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(k+N)^T P_term e(k+N)，其中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(k+1); ...; u(k+N-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)*n+1:i*n, :) = A^i; for j = 1:i Gamma((i-1)*n+1:i*n, (j-1)*m+1:j*m) = A^(i-j) * B; end end Qbar = kron(eye(N), Q); Qbar(end-n+1:end, end-n+1: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_ddot=u是检验状态估计与控制集成的经典试金石，它同时具备两个关键困难：位置量测容易，速度状态却往往不可直接量测；系统本身是开环临界稳定的，不加控制就漂移。把MHE和MPC放到这个模型上跑通，再迁移到移动机器人或倒立摆上就有底气了。

我仿真的模型取采样周期Ts=0.1s，连续模型离散化为A=[1 Ts;0 1]，B=[0.5*Ts^2; Ts]，量测矩阵C=[1 0]，也就是只量测位置，速度完全交给MHE去估计。目标点是原点，初始状态为p(0)=5m，v(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) > M+1 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 参数表与调参心得

我调参时把参数分成了两组：估计组控制"状态曲线平不平"，控制组控制"收敛快不快"。下面是经过多次试验后比较稳定的一组参数，以及对应表现。

调参顺序上我建议先固定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-4，Rv设成0.5，MHE确实输出了极平滑的位置估计，但代价是速度估计完全跟不上真实运动。MPC拿到滞后的速度估计后，产生了一个持续的相位差，镇定任务直接失败。这个现象说明，MHE的Qw和Rv本质上在回答"你更信模型还是更信量测"，选哪边都不能极端，必须和真实物理系统的噪声水平匹配。我会用一段离线数据跑一次卡尔曼滤波做噪声统计标定，再把它换算成Qw和Rv的初值，而不是纯靠手感。

如果让我重新做一次这个项目，我会把MHE窗口起点处保存的到达代价协方差逆矩阵直接传给MPC做约束收紧，这样等于把估计不确定性显式带进了控制设计。这套方法虽然比"MHE+MPC各自为政"多花一点调试时间，但换来的是系统在噪声变大、模型失配时不会突然失控，镇定过程始终有底线。