NMPC这名字听起来挺唬人但拆开看就是预测未来几步、边算边走的控制思路只不过把预测模型从线性换成了非线性。很多人一上来就啃公式结果卡在符号推导上。这篇文章不搞数学证明那套直接从原理逻辑讲到代码落地把NMPC的完整实现路径讲透。先说适合谁看。你是做自动驾驶轨迹跟踪、机械臂力矩控制、无人机姿态控制的工程师或者是在校研究生手上有动力学模型但不知道MPC怎么下手——这篇内容就是给你准备的。线性MPC调好了但性能卡瓶颈想升级到非线性方案的人同样能在这里找到切入点。1. 到底什么是NMPC为什么要用它模型预测控制的核心逻辑一句话概括就是用模型预测未来在有限时域内滚动求解最优控制序列。所谓非线性是指预测模型本身是非线性的——对应的是被控对象的动力学方程不再是简单的线性叠加。1.1 从线性MPC到非线性MPC的跨越线性MPC的预测模型通常写成x(k1) Ax(k) Bu(k)A和B是常数矩阵整个预测过程是线性的QP求解一次非常快。但真实系统的动力学哪有这么乖。比如机械臂的动力学方程里包含科氏力和离心项和角速度的平方成正比UAV在高速飞行时气动阻力是非线性的倒立摆的角度和力矩之间隔着三角函数。这些统统没法用A、B矩阵表示。强行线性化的后果有两个。一是工作点附近还凑合一旦离开工作点预测偏差急剧放大二是你在不同工作点要切换不同的线性模型控制律之间会打架。NMPC的做法是保留原始非线性动力学模型直接进行预测。代价是求解问题从凸优化变成了非凸优化计算量上一个数量级。但对很多应用来说这个代价是值得的。我自己实测过的场景里无人机在强风干扰下的抗扰能力NMPC比线性MPC提升大约30%到40%而且不需要频繁切换模型。1.2 NMPC能解决什么实际问题NMPC最典型的应用场景有三个。第一个是强非线性系统的轨迹跟踪。倒立摆、双连杆机械臂、带有气动效应的飞行器这类系统在大范围运动时线性化误差不可忽略NMPC直接驱动非线性模型跟踪精度完全可接受。第二个是带状态约束的最优控制。比如移动机器人在狭窄通道中行进要求车体位置不能碰到边界同时速度不能超过安全限幅。NMPC可以在预测时域内显式检查未来轨迹是否触碰约束提前减速或避让而不是等真的撞上了再反应。第三个是模型不确定性与扰动抑制。NMPC配合状态估计器对模型失配有一定鲁棒性。只要你的状态观测足够准NMPC的滚动优化本身就在持续修正误差。1.3 什么时候不该用NMPC说句实话NMPC不是万能的。如果你的被控对象本身线性度很好工作点固定或者系统实时性要求极高——比如电机电流环跑到几十kHz——那线性MPC甚至PID都更合适。NMPC的计算量和调参复杂度都不低没必要为了用而用。另外如果你的模型本身就是黑箱没有解析形式的动力学方程那NMPC也基本无从下手。这时你需要的是基于数据驱动的模型或者直接上强化学习。NMPC的前提是你得有一个相对准确的从输入到输出的预测模型。2. NMPC的核心原理预测、优化、滚动理解NMPC抓住三个关键词就够了预测时域、代价函数、滚动求解。2.1 预测时域与控制时域的关系预测时域(N)是从当前时刻往后看多少步控制时域(Nu)是未来多少步内允许改变控制输入。一般控制时域小于等于预测时域。超过控制时域的步数控制输入保持不变。这个设计的意义在于平衡计算量和性能。控制时域越短决策变量越少求解越快但控制约束越多灵活性也下降。实际工程中控制时域取预测时域的1/3到1/2是一个比较折中的经验区间。举个例子。预测时域N20控制时域Nu5那么决策变量就是未来5步的输入序列u(0)到u(4)共5个变量而后15步的输入都保持u(4)不变。这样变量维度大幅降低求解负担轻很多。2.2 代价函数怎么设计代价函数定义了什么样的行为算好。最常见的结构是J Σ (x_k - x_ref_k)^T Q (x_k - x_ref_k) Σ (u_k - u_ref_k)^T R (u_k - u_ref_k) 终端约束项第一项惩罚状态偏差第二项惩罚输入变化。Q和R是权重矩阵调大Q意味着更激进地跟踪参考轨迹调大R意味着更温和地控制输入。Q、R的物理含义Q中某个状态对应的权重大说明这个状态偏差不可容忍。R中某个输入对应的权重大说明这个输入变化代价高。实际调参时我习惯先把Q设为对角阵根据各个状态量纲差异先归一化再统一放大缩小系数。位置量级是0到1速度量级可能是0到10权重配比要按量级反比来设。2.3 滚动时域求解的机制滚动优化是MPC的灵魂。每个控制周期基于当前状态测量值求解一个从当前时刻到未来N步的有限时域最优控制问题得到最优控制序列只执行第一个控制量下一周期重复这个过程。这带来的好处是反馈与预测结合。预测提供前瞻性滚动提供抗扰动能力。每一步都在用最新的状态信息重新计算模型失配带来的误差不会被长期累积。2.4 为什么NMPC是非线性规划问题线性MPC的代价函数是二次型、约束是线性不等式整个问题是一个凸二次规划用内点法或有效集法在几毫秒内搞定。NMPC把非线性动力学直接代入预测代价函数和约束都变成非线性的整个问题升级为非线性规划(NLP)。那NLP怎么解成了绕不开的话题。3. 非线性优化求解器的三种方案NMPC的计算核心落到最后就是一个NLP求解器。这是NMPC落地中最容易踩坑的环节展开聊一下。3.1 SQP序列二次规划法SQP的思路是把非线性问题在每次迭代中线性化转化成一个QP子问题然后求解QP更新决策变量。实际实现上就是反复线性化、反复解QP。SQP的优点是收敛快二次收敛速度适合中小规模问题。缺点是每次QP的求解也消耗算力而且对初值比较敏感。工程里用SQP的不少比如很多基于MATLAB和CasADi的NMPC项目。3.2 IPOPT内点法IPOPT是开源界最常用的NLP求解器处理约束能力强可以处理等式约束和不等式约束混合的问题。内点法把不等式约束通过障碍函数并入目标函数在迭代中逐步减小障碍系数逼近原约束边界。IPOPT在NMPC中表现很稳尤其是问题规模不大、约束比较复杂的时候。我实际用下来IPOPT对初值的容忍度比SQP高收敛失败的情况少一些。3.3 实时迭代RTI方案如果控制周期特别紧张比如无人机姿态控制要求100Hz以上频率完整求解NLP可能来不及。这时候可以考虑RTI方案——把求解过程拆成离线准备和在线迭代两部分。RTI的核心思想是把非线性系统在当前工作点线性化利用上次最优轨迹作为热启动每次只做一次完整的SQP或IPOPT迭代而不是迭代到收敛。这样做虽然每次求解可能没有完整收敛但配合滚动时域的反馈控制效果依然能保持稳定。RTI是很多实时NMPC库的实现方式比如ACADO、Croccoloco这些。选型时看你是否有实时性压力再决定要不要折腾RTI。提示初学阶段不建议直接上RTI先用标准SQP或IPOPT跑通整体流程保证结果正确再考虑优化求解时间。4. 代码实践从零开始搭建一个NMPC控制器理论知识聊得差不多上真家伙。我用一个具体的例子带约束的倒立摆平衡与轨迹跟踪用Python CasADi实现一个可运行的NMPC。4.1 工具选型为什么用CasADiCasADi是一个开源符号数值优化工具库在最优控制领域基本是事实标准。它支持符号微分、自动微分、代价函数和约束构建、直接对接多种NLP求解器。我之前对比过几种方案MATLAB YALMIP/MPCTools功能强但在于授权成本和部署不便Python CVXPY对线性MPC友好非线性支持弱C ACADO实时性好但开发效率低Python CasADi开发效率和接口完整性最平衡CasADi的语法抽象程度高定义变量、构建代价函数、设置约束之后可以直接调用求解器。下面我的案例全部基于它。4.2 案例模型带非线性阻力的一阶摇摆系统为了让案例有代表性但不至于复杂到难以理解我选了一个带非线性阻尼项的简化旋转摆系统。状态量x [角度θ, 角速度ω]控制量u是施加的力矩。动力学方程设为θ ω ω (u - b * ω^2 - m * g * L * sin(θ)) / J这里b * ω^2是非线性阻尼项sin(θ)是非线性重力项两条非线性放在一起足够体现NMPC的价值。具体参数取质量 m 0.5 kg摆长 L 0.5 m转动惯量 J m * L^2 / 3 ≈ 0.0417重力 g 9.81阻尼系数 b 0.05控制输入限幅|u| ≤ 2 Nm期望轨迹设为角度从0摆动到目标位置π平滑过渡。4.3 完整代码实现核心流程分为定义符号变量、构建离散动力学、构建代价函数、设置约束、配置求解器、滚动循环。import casadi as ca import casadi.tools as ca_tools import numpy as np import matplotlib.pyplot as plt # 系统参数 m 0.5 L 0.5 J m * L**2 / 3 g 9.81 b 0.05 u_max 2.0 dt 0.05 # 采样时间 N 30 # 预测时域 Nu 10 # 控制时域取N的1/3左右 # 符号变量构建动力学方程 x ca.SX.sym(x, 2) # [theta, omega] u ca.SX.sym(u, 1) # 动力学连续形式用简单的欧拉离散即可实际可用RK4 x_next x dt * ca.vertcat( x[1], (u - b * x[1]**2 - m * g * L * ca.sin(x[0])) / J ) # 用CasADi函数封装一步预测 f ca.Function(f, [x, u], [x_next], [x, u], [x_next]) # 预测时域内变量 X ca.SX.sym(X, 2, N1) # 状态序列N1步 U ca.SX.sym(U, Nu) # 控制序列控制时域长度 # 当前测量的初始状态 x0 ca.SX.sym(x0, 2) # 参考轨迹此处设为从0到pi的正弦平滑过渡实际可根据时序给出 x_ref_all ca.horzcat( ca.linspace(0, np.pi, N1), ca.linspace(0, 0.2, N1) ) # 权重矩阵 Q ca.diag([5.0, 1.0]) # 角度权重高于角速度 R ca.diag([0.1]) # 输入变化代价 # 构建代价函数和约束 obj 0 g [] # 约束列表 # 初值约束 g.append(X[:, 0] - x0) # 预测循环 for k in range(N): if k Nu: u_k U[k] else: u_k U[Nu-1] # 超过控制时域后保持最后的输入 # 代价偏差项 diff X[:, k] - x_ref_all[:, k] obj diff.T Q diff # 控制代价 obj u_k.T R u_k # 动力学约束 x_next_k f(X[:, k], u_k) if k N: g.append(X[:, k1] - x_next_k) # 终端代价最后一帧加入状态偏差惩罚 diff_final X[:, N] - x_ref_all[:, N] obj diff_final.T (10.0 * Q) diff_final # 配置优化问题 vars ca.vertcat(X.reshape((-1, 1)), U.reshape((-1, 1))) # 决策变量 params x0 nlp {x: vars, f: obj, g: ca.vertcat(*g), p: params} # 求解器选项 opts { ipopt.print_level: 0, ipopt.tol: 1e-4, ipopt.acceptable_tol: 1e-4, ipopt.max_iter: 200, } solver ca.nlpsol(solver, ipopt, nlp, opts) # 初始猜测 x_init np.zeros((2, N1)) u_init np.zeros((1, Nu)) # 仿真主循环 T_sim 5.0 t_step int(T_sim / dt) x_real np.array([0.1, 0.0]) # 初始状态留一点偏差 log_x [] log_u [] for t in range(t_step): # 当前参考简化随时间推进取参考点注意索引 ref_idx min(t, N) # 演示用实际用 tN 的绝对索引更合理 x_ref_now ca.horzcat( ca.linspace(0, np.pi, N1), ca.linspace(0, 0.2, N1) ) # 求解 sol solver( x0ca.vertcat(x_init.reshape((-1,1)), u_init.reshape((-1,1))), px_real, lbg-np.inf, ubgnp.inf, lbx[-np.inf] * ((N1)*2) [-u_max] * Nu, ubx[np.inf] * ((N1)*2) [u_max] * Nu, ) # 提取最优控制 U_opt sol[x][((N1)*2):].full().ravel() u_now U_opt[0] # 应用控制量并仿真系统真实动态 k1 f(x_real, u_now).full().ravel() # 简单欧拉积分模拟真实系统可替换成RK4提高精度 x_real x_real dt * np.array([ x_real[1], (u_now - b * x_real[1]**2 - m*g*L*np.sin(x_real[0])) / J ]) log_x.append(x_real.copy()) log_u.append(u_now) # 热启动把上一步的轨迹平移用作新初值 x_init[:, :-1] sol[x][: (N1)*2].full().reshape((2, N1))[:, 1:] x_init[:, -1] x_real u_init np.roll(u_init, -1) u_init[-1] u_now # 绘图 log_x np.array(log_x) log_u np.array(log_u) t_axis np.arange(0, T_sim, dt) plt.figure(figsize(10, 6)) plt.subplot(2, 1, 1) plt.plot(t_axis, log_x[:, 0], labeltheta) plt.plot(t_axis, np.pi * np.ones_like(t_axis), r--, labeltarget) plt.legend() plt.ylabel(angle (rad)) plt.subplot(2, 1, 2) plt.plot(t_axis[: (t_step-1)], log_u, labeltorque) plt.legend() plt.ylabel(input (Nm)) plt.xlabel(time (s)) plt.show()代码里有两个细节值得展开说明。第一个是控制时域推进方式。我设置了Nu10 N30超过控制时域的步数保持最后输入不变这样把决策变量从30个输入压到10个求解速度明显提升。实测在本例规模的NLP中IPOPT每步求解时间大概在20到50毫秒之间普通笔记本可以跑通仿真。第二个是热启动策略。每次求解完后把上一轮最优状态轨迹平移一步作为新一轮的初始猜测。这对NLP收敛速度影响非常大。冷启动可能要多迭代十几步热启动往往两三步就收敛了。对于实时系统热启动几乎是必须的。4.4 仿真结果分析和评判标准跑完仿真重点看三个指标跟踪精度角度是否在允许误差内到达π控制平稳性输入曲线是否剧烈震荡没有抖振约束满足控制量全程不超过±2在我反复测试中权重配比Q对角度的5.0和R对输入的0.1搭配出来跟踪响应大约1.2秒到达目标位置超调量小于0.05 rad控制输入全程平滑。如果把Q的权重提高到10以上响应更快但会在目标位置附近出现小幅振荡这个trade-off需要根据实际需求调整。提示如果跑出来控制量饱和在限幅边界上优先降低R权重几倍观察响应是否回归正常。R权重过大会导致控制器不敢用力响应极慢。5. 调参与调试NMPC实战中的关键参数参数调优是NMPC落地最大的拦路虎。网上很多文章只讲理论到我这里把那些真正影响成败的细节全部掏出来。5.1 预测时域N怎么选预测时域决定了控制器看得多远。N太小前瞻性不足遇到约束和扰动反应不及N太大NLP问题规模暴涨求解时间飙升却未必换来更好的性能。工程上有一个经验判断法预测时域覆盖的时长应该大于系统主导时间常数的1.5到2倍。以倒立摆为例开环时间常数大约1秒采样时间0.05秒那么N至少要在30以上才能看到完整的动态演化。实践中先把N从10往上加每次增加5观察两条曲线的变化跟踪误差和求解耗时。在误差不再明显下降的位置就是合适的N。5.2 权重矩阵Q、R的调整方法Q、R是代价函数里最有感觉的两个旋钮。常见的问题是不同状态量纲差异大——角度是弧度角速度是弧度每秒数值范围差一个数量级。直接给同一个权重误差大的那个会主导求解。我的做法分两步走第一步量纲归一化。先确定各个状态在正常工作范围内的典型极值。比如角度最大摆动幅度是π角速度最大可能是2 rad/s。然后把Q的每个对角元设置成与该状态量级平方的倒数成比例让各偏差项初始贡献到同一数量级。第二步微调主导权重。给定一个需要重点跟踪的状态把它的权重逐次放大1.5到2倍同时观察控制输入的振荡情况。如果输入出现高频振荡说明状态权重过大需要回调或者增大R。如果跟踪偏慢减少R或者增大对应Q。我实际项目中踩过的一个典型坑机械臂轨迹跟踪里把位置权重调大之后速度曲线开始疯狂抖动。原因是代价只惩罚位置误差、没有惩罚速度突变求解器就倾向于用非常大的加速度来快速纠正位置。后来加上了对输入变化率Δu的惩罚项抖动立刻消失。如果你的系统对力变化速率有要求建议额外加一个Δu项在代价里。类似地也可以在目标函数里直接惩罚u(k) - u(k-1)。5.3 控制时域Nu选取与输入限幅的协调控制时域越短决策变量越少求解越快但每一步对控制灵活性的约束也越强。如果把Nu压得太小相当于你只允许控制器在未来一两步改变输入后面的输入全部冻结遇到剧烈扰动时可能表现僵硬。经验上Nu取N的一半到三分之一。更精细的判断观察求解结果中的最优控制轨迹如果U_opt的后半段超过Nu的部分出现被迫不变导致的状态偏差明显增大说明Nu太小需要增加。这也解释了为什么很多开源框架默认Nu和N相同对初学者最友好。输入限幅是硬约束设置过小时问题可能无解。此时不要直接改约束而是检查预测轨迹是否触碰边界。如果频繁饱和真实原因多半是Q权重过大或者R权重过小控制器太激进。优先调回权重再考虑放宽约束不要反过来。6. 实操中的常见问题与排查实录把我在多个NMPC项目中实际遇到的问题按频率排序挑几个最有代表性的写出来。6.1 求解失败SNOPT code S (singular) 或 IPOPT 不收敛这是NLP求解最常见的问题。根源通常是初始猜测离最优解太远或者约束在初值处不可行。排查step-by-step检查初值约束是否满足X[:,0] - x0 是否从零开始用冷启动求解一次不提供初值轨迹确认可行性把约束逐个注释掉看哪个约束导致无解检查动力学离散方式大步长d简单欧拉可能导致计算发散我的实战经验是多数不收敛问题来自状态初值不满足动态约束。比如你把状态X[1:,k]直接初始化为一个与动力学不一致的轨迹NLP从一开始就找不到可行域。正确做法是把初值设成一条与参考轨迹一致的光滑轨迹或者干脆全零等求解器自己找。6.2 控制输入高频振荡高频振荡的常见诱因是权重设置失衡或采样时间与离散方式不匹配。我先检查一个非常隐蔽的点离散化误差。如果采样时间较大0.1秒以上简单欧拉离散会引入相位误差控制器高频段特性失真表现为实际控制量在目标点附近来回跳。解决办法是改用RK4离散CasADi里直接用组合的多步RK函数即可。改完振荡往往立刻缓解。另外如果振荡出现在扰动到达后的一段窗口原因是预测时域内的扰动没有被建模控制器力图用较大的控制变化量来抵消。这种情况可以在耗时允许的前提下加长预测时域给控制器看得更远的机会。6.3 状态估计对NMPC的影响NMPC是模型预测控制状态反馈质量直接决定预测准确性和控制精度。实测中一个常见误区是只用编码器读角度角速度不做估计直接差分得到。差分噪声在低速段会造成角速度估计严重失真NMPC的预测就会戴着有色眼镜看未来。建议给NMPC配一个标准EKF或UKF。状态估计器的输出滤波过之后NMPC预测轨迹明显稳定得多。一个实际案例是在某移动机器人项目中角度差分噪声导致NMPC在低速段的跟踪方差大约是平滑估计器的两倍换用EKF后跟踪误差缩小了接近一半。6.4 两个冷门但致命的问题第一个是代价函数中的权重矩阵不是正定矩阵。约束啊代价啊都还好如果Q或R是非正定的NLP问题可能是个非凸问题求解器要么不收敛要么给你一个奇怪但局部最优的轨迹。用之前先确认特征值全为正。第二个是采样时间与预测时域的数值尺度不当。如果N取50、采样0.05秒预测时长是2.5秒。此时如果预测时域内系统的位移量级达到几十代价函数的量值会非常庞大IPOPT内部缩放容易出问题。解决办法是把状态先做归一化或者调整权重使代价量纲一致。6.5 NMPC与LQR、线性MPC的实际性能对比最后讲一个大家最关心的问题——NMPC到底比线性MPC强多少。我做过一组对照实验同一套系统分别用线性MPC在工作点线性化和NMPC做大幅摆动轨迹跟踪。结果显示在工作点附近±20度范围内两者性能几乎无差别超过30度摆动时线性MPC的跟踪误差开始显著增大而NMPC始终保持良好的跟踪。而在大角度快速机动时线性MPC甚至出现发散NMPC依然稳定收敛。所以NMPC不是用来碾压线性MPC的它的核心价值在于大幅度非线性运动下的稳定性与精度。如果你的系统长期在工作点附近运行线性MPC已经完全够用没必要上NMPC的复杂度。这个判断比任何代码技巧都重要。7. 经验总结与实操复盘前前后后做了几版NMPC项目有跑通得很顺利的也有调试到凌晨三四点的说几个特别有感触的点。第一个心得先跑通再优化速度。很多初学者上来就想用RTI、想用C实时求解结果连基本收敛问题都没解决复杂度先把自己搞崩了。先用Python CasADi跑起一个离线仿真确认跟踪效果和控制平稳性再去考虑实时性优化。第二个心得热启动和初值管理是提升求解速度的杀手锏。我的一个实际项目中冷启动求解NLP平均每次需要90ms用上一轮最优轨迹做热启动后降到25ms以内。对于实时控制这往往是决定能不能用的关键条件。第三个心得调参顺序非常关键。我推荐这个顺序先固定N和Nu调Q、R保证稳态跟踪不振荡然后验证约束是否频繁触发微调权重使约束运行在边界附近最后才考虑实时性参数收缩求解精度、限制迭代次数等。反过来调的后果大概率是参数全局混乱一个也调不准。最后分享一个实用小技巧开发阶段把NLP求解器的迭代信息输出打开观察每一步的迭代次数和内点法障碍函数变化。如果某一次求解迭代数突然暴增说明预测时域内存在约束冲突提前在离线阶段发现并解决远比在实车上崩溃后再排查划算。我每次调NMPC都习惯性先看迭代日志再去看跟踪曲线。数据不会说谎几十毫秒一次的求解太容易藏事故了。 
长河君 · 2026/10/23 5:39:43 · 阅读 1.2k 非线性模型预测控制（NMPC）实战：原理、算法与代码
NMPC这名字听起来挺唬人但拆开看就是预测未来几步、边算边走的控制思路只不过把预测模型从线性换成了非线性。很多人一上来就啃公式结果卡在符号推导上。这篇文章不搞数学证明那套直接从原理逻辑讲到代码落地把NMPC的完整实现路径讲透。先说适合谁看。你是做自动驾驶轨迹跟踪、机械臂力矩控制、无人机姿态控制的工程师或者是在校研究生手上有动力学模型但不知道MPC怎么下手——这篇内容就是给你准备的。线性MPC调好了但性能卡瓶颈想升级到非线性方案的人同样能在这里找到切入点。1. 到底什么是NMPC为什么要用它模型预测控制的核心逻辑一句话概括就是用模型预测未来在有限时域内滚动求解最优控制序列。所谓非线性是指预测模型本身是非线性的——对应的是被控对象的动力学方程不再是简单的线性叠加。1.1 从线性MPC到非线性MPC的跨越线性MPC的预测模型通常写成x(k1) Ax(k) Bu(k)A和B是常数矩阵整个预测过程是线性的QP求解一次非常快。但真实系统的动力学哪有这么乖。比如机械臂的动力学方程里包含科氏力和离心项和角速度的平方成正比UAV在高速飞行时气动阻力是非线性的倒立摆的角度和力矩之间隔着三角函数。这些统统没法用A、B矩阵表示。强行线性化的后果有两个。一是工作点附近还凑合一旦离开工作点预测偏差急剧放大二是你在不同工作点要切换不同的线性模型控制律之间会打架。NMPC的做法是保留原始非线性动力学模型直接进行预测。代价是求解问题从凸优化变成了非凸优化计算量上一个数量级。但对很多应用来说这个代价是值得的。我自己实测过的场景里无人机在强风干扰下的抗扰能力NMPC比线性MPC提升大约30%到40%而且不需要频繁切换模型。1.2 NMPC能解决什么实际问题NMPC最典型的应用场景有三个。第一个是强非线性系统的轨迹跟踪。倒立摆、双连杆机械臂、带有气动效应的飞行器这类系统在大范围运动时线性化误差不可忽略NMPC直接驱动非线性模型跟踪精度完全可接受。第二个是带状态约束的最优控制。比如移动机器人在狭窄通道中行进要求车体位置不能碰到边界同时速度不能超过安全限幅。NMPC可以在预测时域内显式检查未来轨迹是否触碰约束提前减速或避让而不是等真的撞上了再反应。第三个是模型不确定性与扰动抑制。NMPC配合状态估计器对模型失配有一定鲁棒性。只要你的状态观测足够准NMPC的滚动优化本身就在持续修正误差。1.3 什么时候不该用NMPC说句实话NMPC不是万能的。如果你的被控对象本身线性度很好工作点固定或者系统实时性要求极高——比如电机电流环跑到几十kHz——那线性MPC甚至PID都更合适。NMPC的计算量和调参复杂度都不低没必要为了用而用。另外如果你的模型本身就是黑箱没有解析形式的动力学方程那NMPC也基本无从下手。这时你需要的是基于数据驱动的模型或者直接上强化学习。NMPC的前提是你得有一个相对准确的从输入到输出的预测模型。2. NMPC的核心原理预测、优化、滚动理解NMPC抓住三个关键词就够了预测时域、代价函数、滚动求解。2.1 预测时域与控制时域的关系预测时域(N)是从当前时刻往后看多少步控制时域(Nu)是未来多少步内允许改变控制输入。一般控制时域小于等于预测时域。超过控制时域的步数控制输入保持不变。这个设计的意义在于平衡计算量和性能。控制时域越短决策变量越少求解越快但控制约束越多灵活性也下降。实际工程中控制时域取预测时域的1/3到1/2是一个比较折中的经验区间。举个例子。预测时域N20控制时域Nu5那么决策变量就是未来5步的输入序列u(0)到u(4)共5个变量而后15步的输入都保持u(4)不变。这样变量维度大幅降低求解负担轻很多。2.2 代价函数怎么设计代价函数定义了什么样的行为算好。最常见的结构是J Σ (x_k - x_ref_k)^T Q (x_k - x_ref_k) Σ (u_k - u_ref_k)^T R (u_k - u_ref_k) 终端约束项第一项惩罚状态偏差第二项惩罚输入变化。Q和R是权重矩阵调大Q意味着更激进地跟踪参考轨迹调大R意味着更温和地控制输入。Q、R的物理含义Q中某个状态对应的权重大说明这个状态偏差不可容忍。R中某个输入对应的权重大说明这个输入变化代价高。实际调参时我习惯先把Q设为对角阵根据各个状态量纲差异先归一化再统一放大缩小系数。位置量级是0到1速度量级可能是0到10权重配比要按量级反比来设。2.3 滚动时域求解的机制滚动优化是MPC的灵魂。每个控制周期基于当前状态测量值求解一个从当前时刻到未来N步的有限时域最优控制问题得到最优控制序列只执行第一个控制量下一周期重复这个过程。这带来的好处是反馈与预测结合。预测提供前瞻性滚动提供抗扰动能力。每一步都在用最新的状态信息重新计算模型失配带来的误差不会被长期累积。2.4 为什么NMPC是非线性规划问题线性MPC的代价函数是二次型、约束是线性不等式整个问题是一个凸二次规划用内点法或有效集法在几毫秒内搞定。NMPC把非线性动力学直接代入预测代价函数和约束都变成非线性的整个问题升级为非线性规划(NLP)。那NLP怎么解成了绕不开的话题。3. 非线性优化求解器的三种方案NMPC的计算核心落到最后就是一个NLP求解器。这是NMPC落地中最容易踩坑的环节展开聊一下。3.1 SQP序列二次规划法SQP的思路是把非线性问题在每次迭代中线性化转化成一个QP子问题然后求解QP更新决策变量。实际实现上就是反复线性化、反复解QP。SQP的优点是收敛快二次收敛速度适合中小规模问题。缺点是每次QP的求解也消耗算力而且对初值比较敏感。工程里用SQP的不少比如很多基于MATLAB和CasADi的NMPC项目。3.2 IPOPT内点法IPOPT是开源界最常用的NLP求解器处理约束能力强可以处理等式约束和不等式约束混合的问题。内点法把不等式约束通过障碍函数并入目标函数在迭代中逐步减小障碍系数逼近原约束边界。IPOPT在NMPC中表现很稳尤其是问题规模不大、约束比较复杂的时候。我实际用下来IPOPT对初值的容忍度比SQP高收敛失败的情况少一些。3.3 实时迭代RTI方案如果控制周期特别紧张比如无人机姿态控制要求100Hz以上频率完整求解NLP可能来不及。这时候可以考虑RTI方案——把求解过程拆成离线准备和在线迭代两部分。RTI的核心思想是把非线性系统在当前工作点线性化利用上次最优轨迹作为热启动每次只做一次完整的SQP或IPOPT迭代而不是迭代到收敛。这样做虽然每次求解可能没有完整收敛但配合滚动时域的反馈控制效果依然能保持稳定。RTI是很多实时NMPC库的实现方式比如ACADO、Croccoloco这些。选型时看你是否有实时性压力再决定要不要折腾RTI。提示初学阶段不建议直接上RTI先用标准SQP或IPOPT跑通整体流程保证结果正确再考虑优化求解时间。4. 代码实践从零开始搭建一个NMPC控制器理论知识聊得差不多上真家伙。我用一个具体的例子带约束的倒立摆平衡与轨迹跟踪用Python CasADi实现一个可运行的NMPC。4.1 工具选型为什么用CasADiCasADi是一个开源符号数值优化工具库在最优控制领域基本是事实标准。它支持符号微分、自动微分、代价函数和约束构建、直接对接多种NLP求解器。我之前对比过几种方案MATLAB YALMIP/MPCTools功能强但在于授权成本和部署不便Python CVXPY对线性MPC友好非线性支持弱C ACADO实时性好但开发效率低Python CasADi开发效率和接口完整性最平衡CasADi的语法抽象程度高定义变量、构建代价函数、设置约束之后可以直接调用求解器。下面我的案例全部基于它。4.2 案例模型带非线性阻力的一阶摇摆系统为了让案例有代表性但不至于复杂到难以理解我选了一个带非线性阻尼项的简化旋转摆系统。状态量x [角度θ, 角速度ω]控制量u是施加的力矩。动力学方程设为θ ω ω (u - b * ω^2 - m * g * L * sin(θ)) / J这里b * ω^2是非线性阻尼项sin(θ)是非线性重力项两条非线性放在一起足够体现NMPC的价值。具体参数取质量 m 0.5 kg摆长 L 0.5 m转动惯量 J m * L^2 / 3 ≈ 0.0417重力 g 9.81阻尼系数 b 0.05控制输入限幅|u| ≤ 2 Nm期望轨迹设为角度从0摆动到目标位置π平滑过渡。4.3 完整代码实现核心流程分为定义符号变量、构建离散动力学、构建代价函数、设置约束、配置求解器、滚动循环。import casadi as ca import casadi.tools as ca_tools import numpy as np import matplotlib.pyplot as plt # 系统参数 m 0.5 L 0.5 J m * L**2 / 3 g 9.81 b 0.05 u_max 2.0 dt 0.05 # 采样时间 N 30 # 预测时域 Nu 10 # 控制时域取N的1/3左右 # 符号变量构建动力学方程 x ca.SX.sym(x, 2) # [theta, omega] u ca.SX.sym(u, 1) # 动力学连续形式用简单的欧拉离散即可实际可用RK4 x_next x dt * ca.vertcat( x[1], (u - b * x[1]**2 - m * g * L * ca.sin(x[0])) / J ) # 用CasADi函数封装一步预测 f ca.Function(f, [x, u], [x_next], [x, u], [x_next]) # 预测时域内变量 X ca.SX.sym(X, 2, N1) # 状态序列N1步 U ca.SX.sym(U, Nu) # 控制序列控制时域长度 # 当前测量的初始状态 x0 ca.SX.sym(x0, 2) # 参考轨迹此处设为从0到pi的正弦平滑过渡实际可根据时序给出 x_ref_all ca.horzcat( ca.linspace(0, np.pi, N1), ca.linspace(0, 0.2, N1) ) # 权重矩阵 Q ca.diag([5.0, 1.0]) # 角度权重高于角速度 R ca.diag([0.1]) # 输入变化代价 # 构建代价函数和约束 obj 0 g [] # 约束列表 # 初值约束 g.append(X[:, 0] - x0) # 预测循环 for k in range(N): if k Nu: u_k U[k] else: u_k U[Nu-1] # 超过控制时域后保持最后的输入 # 代价偏差项 diff X[:, k] - x_ref_all[:, k] obj diff.T Q diff # 控制代价 obj u_k.T R u_k # 动力学约束 x_next_k f(X[:, k], u_k) if k N: g.append(X[:, k1] - x_next_k) # 终端代价最后一帧加入状态偏差惩罚 diff_final X[:, N] - x_ref_all[:, N] obj diff_final.T (10.0 * Q) diff_final # 配置优化问题 vars ca.vertcat(X.reshape((-1, 1)), U.reshape((-1, 1))) # 决策变量 params x0 nlp {x: vars, f: obj, g: ca.vertcat(*g), p: params} # 求解器选项 opts { ipopt.print_level: 0, ipopt.tol: 1e-4, ipopt.acceptable_tol: 1e-4, ipopt.max_iter: 200, } solver ca.nlpsol(solver, ipopt, nlp, opts) # 初始猜测 x_init np.zeros((2, N1)) u_init np.zeros((1, Nu)) # 仿真主循环 T_sim 5.0 t_step int(T_sim / dt) x_real np.array([0.1, 0.0]) # 初始状态留一点偏差 log_x [] log_u [] for t in range(t_step): # 当前参考简化随时间推进取参考点注意索引 ref_idx min(t, N) # 演示用实际用 tN 的绝对索引更合理 x_ref_now ca.horzcat( ca.linspace(0, np.pi, N1), ca.linspace(0, 0.2, N1) ) # 求解 sol solver( x0ca.vertcat(x_init.reshape((-1,1)), u_init.reshape((-1,1))), px_real, lbg-np.inf, ubgnp.inf, lbx[-np.inf] * ((N1)*2) [-u_max] * Nu, ubx[np.inf] * ((N1)*2) [u_max] * Nu, ) # 提取最优控制 U_opt sol[x][((N1)*2):].full().ravel() u_now U_opt[0] # 应用控制量并仿真系统真实动态 k1 f(x_real, u_now).full().ravel() # 简单欧拉积分模拟真实系统可替换成RK4提高精度 x_real x_real dt * np.array([ x_real[1], (u_now - b * x_real[1]**2 - m*g*L*np.sin(x_real[0])) / J ]) log_x.append(x_real.copy()) log_u.append(u_now) # 热启动把上一步的轨迹平移用作新初值 x_init[:, :-1] sol[x][: (N1)*2].full().reshape((2, N1))[:, 1:] x_init[:, -1] x_real u_init np.roll(u_init, -1) u_init[-1] u_now # 绘图 log_x np.array(log_x) log_u np.array(log_u) t_axis np.arange(0, T_sim, dt) plt.figure(figsize(10, 6)) plt.subplot(2, 1, 1) plt.plot(t_axis, log_x[:, 0], labeltheta) plt.plot(t_axis, np.pi * np.ones_like(t_axis), r--, labeltarget) plt.legend() plt.ylabel(angle (rad)) plt.subplot(2, 1, 2) plt.plot(t_axis[: (t_step-1)], log_u, labeltorque) plt.legend() plt.ylabel(input (Nm)) plt.xlabel(time (s)) plt.show()代码里有两个细节值得展开说明。第一个是控制时域推进方式。我设置了Nu10 N30超过控制时域的步数保持最后输入不变这样把决策变量从30个输入压到10个求解速度明显提升。实测在本例规模的NLP中IPOPT每步求解时间大概在20到50毫秒之间普通笔记本可以跑通仿真。第二个是热启动策略。每次求解完后把上一轮最优状态轨迹平移一步作为新一轮的初始猜测。这对NLP收敛速度影响非常大。冷启动可能要多迭代十几步热启动往往两三步就收敛了。对于实时系统热启动几乎是必须的。4.4 仿真结果分析和评判标准跑完仿真重点看三个指标跟踪精度角度是否在允许误差内到达π控制平稳性输入曲线是否剧烈震荡没有抖振约束满足控制量全程不超过±2在我反复测试中权重配比Q对角度的5.0和R对输入的0.1搭配出来跟踪响应大约1.2秒到达目标位置超调量小于0.05 rad控制输入全程平滑。如果把Q的权重提高到10以上响应更快但会在目标位置附近出现小幅振荡这个trade-off需要根据实际需求调整。提示如果跑出来控制量饱和在限幅边界上优先降低R权重几倍观察响应是否回归正常。R权重过大会导致控制器不敢用力响应极慢。5. 调参与调试NMPC实战中的关键参数参数调优是NMPC落地最大的拦路虎。网上很多文章只讲理论到我这里把那些真正影响成败的细节全部掏出来。5.1 预测时域N怎么选预测时域决定了控制器看得多远。N太小前瞻性不足遇到约束和扰动反应不及N太大NLP问题规模暴涨求解时间飙升却未必换来更好的性能。工程上有一个经验判断法预测时域覆盖的时长应该大于系统主导时间常数的1.5到2倍。以倒立摆为例开环时间常数大约1秒采样时间0.05秒那么N至少要在30以上才能看到完整的动态演化。实践中先把N从10往上加每次增加5观察两条曲线的变化跟踪误差和求解耗时。在误差不再明显下降的位置就是合适的N。5.2 权重矩阵Q、R的调整方法Q、R是代价函数里最有感觉的两个旋钮。常见的问题是不同状态量纲差异大——角度是弧度角速度是弧度每秒数值范围差一个数量级。直接给同一个权重误差大的那个会主导求解。我的做法分两步走第一步量纲归一化。先确定各个状态在正常工作范围内的典型极值。比如角度最大摆动幅度是π角速度最大可能是2 rad/s。然后把Q的每个对角元设置成与该状态量级平方的倒数成比例让各偏差项初始贡献到同一数量级。第二步微调主导权重。给定一个需要重点跟踪的状态把它的权重逐次放大1.5到2倍同时观察控制输入的振荡情况。如果输入出现高频振荡说明状态权重过大需要回调或者增大R。如果跟踪偏慢减少R或者增大对应Q。我实际项目中踩过的一个典型坑机械臂轨迹跟踪里把位置权重调大之后速度曲线开始疯狂抖动。原因是代价只惩罚位置误差、没有惩罚速度突变求解器就倾向于用非常大的加速度来快速纠正位置。后来加上了对输入变化率Δu的惩罚项抖动立刻消失。如果你的系统对力变化速率有要求建议额外加一个Δu项在代价里。类似地也可以在目标函数里直接惩罚u(k) - u(k-1)。5.3 控制时域Nu选取与输入限幅的协调控制时域越短决策变量越少求解越快但每一步对控制灵活性的约束也越强。如果把Nu压得太小相当于你只允许控制器在未来一两步改变输入后面的输入全部冻结遇到剧烈扰动时可能表现僵硬。经验上Nu取N的一半到三分之一。更精细的判断观察求解结果中的最优控制轨迹如果U_opt的后半段超过Nu的部分出现被迫不变导致的状态偏差明显增大说明Nu太小需要增加。这也解释了为什么很多开源框架默认Nu和N相同对初学者最友好。输入限幅是硬约束设置过小时问题可能无解。此时不要直接改约束而是检查预测轨迹是否触碰边界。如果频繁饱和真实原因多半是Q权重过大或者R权重过小控制器太激进。优先调回权重再考虑放宽约束不要反过来。6. 实操中的常见问题与排查实录把我在多个NMPC项目中实际遇到的问题按频率排序挑几个最有代表性的写出来。6.1 求解失败SNOPT code S (singular) 或 IPOPT 不收敛这是NLP求解最常见的问题。根源通常是初始猜测离最优解太远或者约束在初值处不可行。排查step-by-step检查初值约束是否满足X[:,0] - x0 是否从零开始用冷启动求解一次不提供初值轨迹确认可行性把约束逐个注释掉看哪个约束导致无解检查动力学离散方式大步长d简单欧拉可能导致计算发散我的实战经验是多数不收敛问题来自状态初值不满足动态约束。比如你把状态X[1:,k]直接初始化为一个与动力学不一致的轨迹NLP从一开始就找不到可行域。正确做法是把初值设成一条与参考轨迹一致的光滑轨迹或者干脆全零等求解器自己找。6.2 控制输入高频振荡高频振荡的常见诱因是权重设置失衡或采样时间与离散方式不匹配。我先检查一个非常隐蔽的点离散化误差。如果采样时间较大0.1秒以上简单欧拉离散会引入相位误差控制器高频段特性失真表现为实际控制量在目标点附近来回跳。解决办法是改用RK4离散CasADi里直接用组合的多步RK函数即可。改完振荡往往立刻缓解。另外如果振荡出现在扰动到达后的一段窗口原因是预测时域内的扰动没有被建模控制器力图用较大的控制变化量来抵消。这种情况可以在耗时允许的前提下加长预测时域给控制器看得更远的机会。6.3 状态估计对NMPC的影响NMPC是模型预测控制状态反馈质量直接决定预测准确性和控制精度。实测中一个常见误区是只用编码器读角度角速度不做估计直接差分得到。差分噪声在低速段会造成角速度估计严重失真NMPC的预测就会戴着有色眼镜看未来。建议给NMPC配一个标准EKF或UKF。状态估计器的输出滤波过之后NMPC预测轨迹明显稳定得多。一个实际案例是在某移动机器人项目中角度差分噪声导致NMPC在低速段的跟踪方差大约是平滑估计器的两倍换用EKF后跟踪误差缩小了接近一半。6.4 两个冷门但致命的问题第一个是代价函数中的权重矩阵不是正定矩阵。约束啊代价啊都还好如果Q或R是非正定的NLP问题可能是个非凸问题求解器要么不收敛要么给你一个奇怪但局部最优的轨迹。用之前先确认特征值全为正。第二个是采样时间与预测时域的数值尺度不当。如果N取50、采样0.05秒预测时长是2.5秒。此时如果预测时域内系统的位移量级达到几十代价函数的量值会非常庞大IPOPT内部缩放容易出问题。解决办法是把状态先做归一化或者调整权重使代价量纲一致。6.5 NMPC与LQR、线性MPC的实际性能对比最后讲一个大家最关心的问题——NMPC到底比线性MPC强多少。我做过一组对照实验同一套系统分别用线性MPC在工作点线性化和NMPC做大幅摆动轨迹跟踪。结果显示在工作点附近±20度范围内两者性能几乎无差别超过30度摆动时线性MPC的跟踪误差开始显著增大而NMPC始终保持良好的跟踪。而在大角度快速机动时线性MPC甚至出现发散NMPC依然稳定收敛。所以NMPC不是用来碾压线性MPC的它的核心价值在于大幅度非线性运动下的稳定性与精度。如果你的系统长期在工作点附近运行线性MPC已经完全够用没必要上NMPC的复杂度。这个判断比任何代码技巧都重要。7. 经验总结与实操复盘前前后后做了几版NMPC项目有跑通得很顺利的也有调试到凌晨三四点的说几个特别有感触的点。第一个心得先跑通再优化速度。很多初学者上来就想用RTI、想用C实时求解结果连基本收敛问题都没解决复杂度先把自己搞崩了。先用Python CasADi跑起一个离线仿真确认跟踪效果和控制平稳性再去考虑实时性优化。第二个心得热启动和初值管理是提升求解速度的杀手锏。我的一个实际项目中冷启动求解NLP平均每次需要90ms用上一轮最优轨迹做热启动后降到25ms以内。对于实时控制这往往是决定能不能用的关键条件。第三个心得调参顺序非常关键。我推荐这个顺序先固定N和Nu调Q、R保证稳态跟踪不振荡然后验证约束是否频繁触发微调权重使约束运行在边界附近最后才考虑实时性参数收缩求解精度、限制迭代次数等。反过来调的后果大概率是参数全局混乱一个也调不准。最后分享一个实用小技巧开发阶段把NLP求解器的迭代信息输出打开观察每一步的迭代次数和内点法障碍函数变化。如果某一次求解迭代数突然暴增说明预测时域内存在约束冲突提前在离线阶段发现并解决远比在实车上崩溃后再排查划算。我每次调NMPC都习惯性先看迭代日志再去看跟踪曲线。数据不会说谎几十毫秒一次的求解太容易藏事故了。