★ FEATURED ARTICLE 模型预测控制这几年在工业界和学术界的讨论热度一直居高不下尤其是做自动驾驶轨迹跟踪、机器人运动控制、化工过程优化的朋友几乎绕不开MPC这个词。但很多刚入门的朋友跟我一样最初看论文时被那一堆预测时域、控制时域、滚动优化、二次规划搞得云里雾里公式能看懂但真到写代码实现的时候又不知道从哪下手。这篇内容就是把我自己从零实现MPC的完整过程拆开来讲从核心思想到QP求解器的选型再到一个完整的轨迹跟踪案例尽量把每个环节的“为什么”说清楚。适合有一定控制理论基础、想动手实现MPC的工程师和学生也适合已经用过MPC但对其内部机制一知半解的从业者。1. 模型预测控制的核心思想与方案选型1.1 为什么是“预测”而不是“反馈”传统PID控制的核心逻辑是“误差出现后再修正”它不关心系统未来会怎样只看当前误差。这在很多场景下够用但遇到有约束、有滞后、多变量耦合的系统时PID就显得力不从心。MPC的出发点完全不同它在每个控制周期内利用系统的数学模型去预测未来一段时间内系统的行为然后在这个预测的基础上求解一个优化问题找到一串最优的控制输入序列但只把第一个控制量施加给系统下一个周期再重新预测、重新优化。这个“滚动时域”的机制是MPC最本质的特征。你可以把它理解成开车时不断看前方路况调整方向盘而不是等到车偏离车道了才猛打方向。预测时域就是你“看多远”控制时域就是你“规划多远的动作”。看多远和规划多远这两个参数直接决定了MPC的性能和计算量后面我会详细讲怎么选。1.2 MPC相比LQR和PID的优势在哪LQR也是一种基于模型的最优控制方法它通过求解Riccati方程得到一个全局最优的静态反馈增益矩阵。但LQR有两个硬伤一是无法显式处理约束二是它优化的是无限时域的二次型代价对时变参考轨迹的跟踪不够灵活。MPC则天然支持约束你可以把执行器的物理限制、状态的安全边界都写进优化问题里求解器会在满足这些约束的前提下找最优解。PID的优势在于实现简单、不需要模型但它的参数整定依赖经验面对多输入多输出系统时耦合严重调参难度指数级上升。MPC虽然需要模型但一旦模型建立起来约束和代价函数的调整非常直观而且能统一处理多变量系统。1.3 线性MPC与非线性MPC的取舍MPC按模型类型分为线性MPC和非线性MPC。线性MPC假设系统动态可以用线性状态空间方程描述优化问题转化为二次规划QP求解速度快有成熟的求解器可用实时性有保障。非线性MPC直接用非线性模型做预测优化问题是非线性规划NLP求解慢且可能陷入局部最优但对强非线性系统的控制效果更好。我的建议是如果你的系统在工作点附近可以用线性模型较好地近似优先选线性MPC。绝大多数轨迹跟踪、过程控制场景线性MPC已经足够。只有当系统非线性非常强、线性化误差不可接受时才考虑非线性MPC。这篇内容主要围绕线性MPC展开因为它是入门和工程落地的主力。1.4 优化问题的数学形式线性MPC的标准优化问题可以写成如下形式。假设系统模型为x(k1) A x(k) B u(k)其中x是状态向量u是控制输入。在每个时刻k我们求解min J Σ [x(ki|k)^T Q x(ki|k) u(ki|k)^T R u(ki|k)]约束条件包括系统动态约束x(ki1|k) A x(ki|k) B u(ki|k)状态约束x_min ≤ x(ki|k) ≤ x_max输入约束u_min ≤ u(ki|k) ≤ u_maxQ和R是权重矩阵Q越大越注重状态收敛R越大越注重控制量平滑。这个优化问题在每一时刻求解一次得到最优控制序列后只取第一个元素施加。2. 二次规划求解器的选型与实操要点2.1 为什么MPC最终变成QP问题线性MPC的代价函数是二次型约束是线性的这正好是二次规划QP的标准形式。QP问题的数学表达是min (1/2) z^T H z f^T z s.t. A_eq z b_eq A_ineq z ≤ b_ineq其中z是决策变量在MPC里就是未来N步的控制输入序列。H是Hessian矩阵由Q、R和系统矩阵A、B组合而成。把MPC问题转化成QP标准形式是代码实现的关键一步转化得好不好直接影响求解效率和数值稳定性。2.2 常用QP求解器对比求解器语言支持特点适用场景OSQPC/Python/MATLAB算子分裂法稀疏矩阵友好速度快嵌入式MPC、大规模稀疏问题quadprogMATLABMATLAB自带稳定可靠快速原型验证qpOASESC在线主动集法适合小规模稠密问题实时MPC、嵌入式CasADiPython/C/MATLAB符号建模自动微分支持NLP非线性MPC、快速原型Gurobi多语言商业求解器性能极强对求解速度要求极高的场景我个人的选择习惯是MATLAB环境下用quadprog做验证Python环境下用OSQP做部署C环境下用qpOASES。OSQP的稀疏矩阵支持非常好MPC问题天然稀疏用OSQP能获得很好的性能。2.3 把MPC问题转成QP标准形式的详细步骤这一步是很多初学者卡住的地方。我以预测时域N10、状态维度n2、输入维度m1为例把转化过程拆开讲。首先把预测方程展开。给定当前状态x0未来N步的状态可以写成X Φ x0 Γ U其中X [x1, x2, ..., xN]^T是堆叠的状态向量U [u0, u1, ..., u_{N-1}]^T是堆叠的输入向量。Φ和Γ是由A、B递推得到的矩阵Φ [A; A^2; ...; A^N] Γ [B, 0, ..., 0; AB, B, ..., 0; ...; A^{N-1}B, A^{N-2}B, ..., B]然后代价函数J X^T Q_bar X U^T R_bar U把X的表达式代入整理成U的二次型J U^T (Γ^T Q_bar Γ R_bar) U 2 x0^T Φ^T Q_bar Γ U const这样H 2(Γ^T Q_bar Γ R_bar)f 2 Γ^T Q_bar^T Φ x0。约束条件也类似地写成U的线性不等式。注意Q_bar和R_bar是块对角矩阵由单步的Q和R扩展而来。构造这两个矩阵时要注意维度对齐状态和输入的排列顺序要和Φ、Γ一致。2.4 稀疏性与求解效率的优化MPC问题的H矩阵通常是稀疏的因为每个时刻的状态只和相邻时刻有关。OSQP利用这个稀疏性可以把求解速度提升一个数量级。构造H时用稀疏矩阵格式如scipy.sparse.csc_matrix而不是稠密矩阵能显著减少内存占用和计算时间。另一个优化点是热启动。相邻两个控制周期的QP问题非常相似把上一周期的解作为本周期的初始猜测能大幅减少迭代次数。OSQP和qpOASES都支持热启动实测下来在轨迹跟踪场景中能减少30%到50%的求解时间。3. 完整案例二维轨迹跟踪的MPC实现3.1 问题描述与系统建模我选一个经典的二维轨迹跟踪案例一个质点模型状态是位置(x, y)和速度(vx, vy)控制输入是加速度(ax, ay)。这个模型虽然简单但足够说明MPC的完整流程而且和自动驾驶、无人机轨迹跟踪的本质是一样的。系统方程写成状态空间形式x(k1) A x(k) B u(k)其中状态向量 x [px, py, vx, vy]^T输入向量 u [ax, ay]^TA [[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]B [[0.5dt^2, 0], [0, 0.5dt^2], [dt, 0], [0, dt]]dt是采样时间我取0.1秒。这个模型假设加速度在两个采样点之间保持不变是零阶保持器的离散化结果。3.2 参数选择与权重整定参数选择是MPC调参的核心。预测时域N选多大控制时域选多长Q和R怎么定这些问题没有标准答案但有规律可循。预测时域N决定了MPC“看多远”。N太小系统看不到远处的约束和参考轨迹变化控制会短视N太大计算量线性增长而且远处的预测精度下降。经验法则是N * dt应该覆盖系统的主要动态响应时间。对于这个质点模型我选N20对应2秒的预测窗口足够覆盖从静止加速到目标速度的过程。控制时域通常小于等于预测时域。为了简化我让控制时域等于预测时域即每一步都优化控制量。如果计算资源紧张可以只优化前M步后面M到N步的控制量保持不变。Q和R的整定Q是4x4矩阵对应位置和速度的权重。位置权重设大一些比如100速度权重小一些比如10因为我们更关心位置跟踪精度。R是2x2矩阵对应两个加速度输入的权重设为单位矩阵的0.1倍表示对控制量变化有一定惩罚但不苛刻。实操心得Q和R的比例比绝对值更重要。如果发现控制量抖动厉害增大R如果发现跟踪滞后增大Q。我通常先把R设得很小调Q到跟踪效果满意再逐步增大R来平滑控制量。3.3 Python代码实现下面是完整的Python实现用OSQP求解QP问题。代码可以直接运行依赖numpy、scipy和osqp。import numpy as np import scipy.sparse as sp import osqp # 系统参数 dt 0.1 A np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) B np.array([[0.5*dt**2, 0], [0, 0.5*dt**2], [dt, 0], [0, dt]]) n 4 # 状态维度 m 2 # 输入维度 N 20 # 预测时域 # 权重矩阵 Q np.diag([100, 100, 10, 10]) R np.diag([0.1, 0.1]) # 构造预测矩阵 Phi np.zeros((N*n, n)) Gamma np.zeros((N*n, N*m)) A_pow np.eye(n) for i in range(N): A_pow A_pow A if i 0 else A Phi[i*n:(i1)*n, :] A_pow for j in range(i1): A_pow_j np.linalg.matrix_power(A, i-j) Gamma[i*n:(i1)*n, j*m:(j1)*m] A_pow_j B # 构造Q_bar和R_bar Q_bar sp.kron(sp.eye(N), Q, formatcsc) R_bar sp.kron(sp.eye(N), R, formatcsc) # 构造H和f的矩阵部分 H 2 * (Gamma.T Q_bar.toarray() Gamma R_bar.toarray()) H_sparse sp.csc_matrix(H) # 约束加速度范围 [-1, 1] u_min -1.0 u_max 1.0 A_ineq sp.vstack([sp.eye(N*m), -sp.eye(N*m)], formatcsc) l_ineq np.full(N*m, -np.inf) l_ineq np.concatenate([np.full(N*m, u_min), np.full(N*m, -u_max)]) u_ineq np.concatenate([np.full(N*m, u_max), np.full(N*m, -u_min)]) # 创建OSQP求解器 prob osqp.OSQP() prob.setup(PH_sparse, qnp.zeros(N*m), AA_ineq, ll_ineq, uu_ineq, verboseFalse, warm_startTrue) # 仿真参数 T_sim 10.0 steps int(T_sim / dt) x np.array([0, 0, 0, 0], dtypefloat) # 初始状态 trajectory [] for k in range(steps): # 参考轨迹圆形 t k * dt ref_x 5 * np.cos(0.5 * t) ref_y 5 * np.sin(0.5 * t) ref_vx -2.5 * np.sin(0.5 * t) ref_vy 2.5 * np.cos(0.5 * t) x_ref np.tile(np.array([ref_x, ref_y, ref_vx, ref_vy]), N) # 更新f向量 f 2 * Gamma.T Q_bar.toarray() (Phi x - x_ref) prob.update(qf) # 求解 res prob.solve() u_opt res.x[:m] # 施加第一个控制量 x A x B u_opt trajectory.append(x.copy()) trajectory np.array(trajectory)这段代码的核心逻辑是每个控制周期更新f向量因为x0变了然后调用OSQP求解取第一个控制量施加给系统。热启动通过warm_startTrue开启OSQP会自动利用上一次的解。3.4 仿真结果分析跑完这段代码你会看到质点从原点出发逐渐跟踪上圆形参考轨迹。前几秒会有明显的跟踪误差因为初始状态和参考轨迹差距大MPC在约束范围内全力加速。大约2到3秒后跟踪误差收敛到很小的范围。如果发现跟踪效果不理想可以从这几个方向排查预测时域N是否足够覆盖动态过程Q矩阵中位置权重是否够大加速度约束是否太紧导致无法及时跟踪。我实测下来N20、Q位置权重100、加速度限制±1这个配置跟踪半径5米、角速度0.5rad/s的圆形轨迹稳态误差在0.05米以内。4. 常见问题与排查技巧实录4.1 QP求解失败或无解怎么办这是MPC落地时最常见的问题。求解失败通常有几个原因约束之间互相矛盾导致可行域为空H矩阵不是正定的导致QP非凸数值精度问题矩阵条件数太大。排查步骤先检查约束是否合理。比如加速度上下限是否写反了状态约束是否和初始状态冲突。然后检查H矩阵的正定性H 2(Γ^T Q_bar Γ R_bar)只要Q和R是正定的H就是正定的。如果Q或R有零特征值加一个小正则项如1e-6 * I保证正定。如果约束确实可能导致无解可以引入软约束在代价函数里加一个松弛变量允许约束被轻微违反但违反量会被惩罚。这样即使原问题无解也能得到一个次优但可用的解。4.2 控制量抖动严重怎么调控制量抖动通常是因为R太小或者预测时域太短。R太小意味着对控制量变化的惩罚不够求解器会倾向于用剧烈的控制动作来快速消除误差。增大R能平滑控制量但会牺牲跟踪速度。另一个原因是模型和实际系统不匹配。如果模型预测的状态和实际状态偏差大MPC会不断修正导致控制量振荡。这时候需要重新辨识模型参数或者降低模型精度要求增大R来容忍模型误差。我踩过的一个坑是采样时间dt选得太小导致离散化后的B矩阵数值很小控制量对状态的影响被削弱求解器为了达到同样的控制效果会输出很大的控制量进而引发抖动。后来把dt从0.01调到0.1问题就解决了。4.3 预测时域和控制时域怎么选这个问题没有万能答案但有几个经验规则。预测时域N * dt应该至少覆盖系统阶跃响应的上升时间。对于一阶系统上升时间约3倍时间常数对于二阶系统约4到5倍。控制时域M通常取N的10%到20%如果计算资源充足取MN效果最好。如果发现系统响应慢、跟踪滞后先增大N。如果发现计算时间太长先减小M。N和M的调整会相互影响建议固定一个调另一个观察效果变化。4.4 常见问题速查表问题现象可能原因排查方法解决方案求解失败约束冲突检查约束上下限放宽约束或加软约束控制量抖动R太小增大R观察增大R或增大dt跟踪滞后Q太小或N太短增大Q或N调整权重或时域稳态误差大模型失配对比模型输出和实际重新辨识模型计算超时N或M太大减小N或M优化代码或降维数值不稳定矩阵条件数大检查H特征值加正则项或缩放4.5 几个容易被忽略的实操细节第一个细节是状态约束的处理。很多人只加输入约束不加状态约束结果系统状态跑到了物理上不可能的区域。状态约束在QP里体现为对X的线性不等式而X Φ x0 Γ U所以约束要转化成对U的约束。转化过程中要注意Φ x0是已知量移到不等式右边。第二个细节是参考轨迹的预处理。如果参考轨迹有跳变MPC会试图用有限的控制量去跟踪一个不可能跟踪的信号导致求解器输出饱和。实际使用中要对参考轨迹做平滑滤波或者用参考轨迹的变化率作为前馈。第三个细节是求解器的终止条件。OSQP默认的终止精度是1e-3对于大多数控制场景够用。如果发现求解结果精度不够可以调到1e-4或1e-5但求解时间会增加。我通常先用默认精度跑通再根据效果微调。4.6 从仿真到实机的迁移经验仿真跑通只是第一步实机上还有几个坑要填。首先是传感器噪声仿真里状态是精确已知的实机上要用状态估计器如卡尔曼滤波从带噪声的测量中估计状态。状态估计的滞后和误差会直接影响MPC的性能需要在Q矩阵里适当降低对速度状态的权重因为速度通常估计得不如位置准。其次是执行器延迟仿真里假设控制量立即生效实机上从计算完到执行器响应有延迟。补偿方法是在预测模型里把延迟建模进去或者用Smith预估器。我通常先在模型里加一个纯延迟环节看MPC能否补偿如果不行再考虑更复杂的方案。最后是计算平台的算力。仿真在PC上跑实机可能在嵌入式平台上跑算力差几十倍。迁移前要评估QP求解时间是否满足控制周期要求。如果不够可以减小N和M或者用C语言重写求解器或者换用更高效的求解器如qpOASES。5. MPC的扩展方向与进阶思路5.1 从线性MPC到自适应MPC线性MPC假设模型参数固定但实际系统参数可能随时间变化。自适应MPC在线辨识模型参数并实时更新预测模型。实现方式有两种一是递推最小二乘法在线辨识A、B矩阵二是用多个模型切换。自适应MPC的难点在于辨识的稳定性和计算量参数变化太快会导致辨识发散变化太慢又跟不上。5.2 从确定性MPC到鲁棒MPC实际系统有扰动和模型不确定性确定性MPC在这些情况下可能违反约束。鲁棒MPC考虑最坏情况下的扰动保证约束在所有可能情况下都满足。常见方法有管状MPCTube MPC和min-max MPC。管状MPC把实际状态约束在一个以标称轨迹为中心的管子里管子半径由扰动上界决定。鲁棒MPC的代价是保守性增加控制性能下降。5.3 MPC与学习方法的结合最近几年学习MPC是个热点用神经网络学习MPC的控制律或者用强化学习调MPC的权重。学习MPC的优势是推理速度快训练好后不需要在线求解QP适合算力受限的场景。但缺点是泛化能力有限训练分布外的场景可能失效。我的看法是学习MPC适合特定场景的定制化部署通用性不如传统MPC。5.4 显式MPC的思路显式MPC把QP的求解过程离线化预先计算所有可能状态下的最优控制律在线时只需要查表。显式MPC适合状态维度低、约束简单的场景因为状态维度一高离线计算的复杂度指数增长。对于状态维度小于等于3的系统显式MPC非常实用在线计算时间可以做到微秒级。6. 工程落地中的性能优化技巧6.1 代码层面的优化QP求解是MPC的计算瓶颈优化求解器调用是关键。第一用稀疏矩阵格式构造H和A_ineqOSQP对稀疏矩阵的处理效率远高于稠密矩阵。第二开启热启动相邻周期的解非常接近热启动能减少迭代次数。第三预分配所有数组避免在控制循环里动态分配内存。第四如果用的是Python把求解器调用封装成C扩展或者用Cython加速。6.2 问题规模的缩减如果计算资源实在紧张可以从这几个方向缩减问题规模。降低预测时域N但要注意N太小会影响稳定性。增大采样时间dt但dt太大会降低控制精度。只优化控制时域M步后面N-M步的控制量保持不变。对状态进行降维去掉对控制目标影响小的状态。6.3 多速率MPC的设计有些系统不同状态的动态时间尺度差异很大比如位置变化慢、电流变化快。这时候可以用多速率MPC慢状态用大采样时间快状态用小采样时间。实现上可以用两个不同频率的MPC级联慢MPC输出参考给快MPC。这种设计能兼顾计算效率和控
阅读完成 · 觉得有帮助？
