SQP算法Matlab完整实现：求解非线性约束优化问题

简介：这套基于Matlab的SQP（序列二次规划）完整实现代码，面向需要求解非线性约束优化问题的学习者和科研人员，适合具备微积分、线性代数基础及Matlab编程经验的读者。压缩包共8个文件，包含4个.m主程序与4个.asv自动保存副本，整体仅12KB，体积轻巧，便于逐行研读与调试。已有1152人学习下载。代码完整覆盖SQP迭代核心链条：初始解选取、目标函数与约束的泰勒线性化、拉格朗日Hessian近似、二次规划子问题求解、Armijo线性搜索及收敛判定，并配有子问题求解与拉格朗日更新辅助函数。通过这套代码，读者既能从底层理解SQP的数学原理，也能直接将其作为Matlab自定义优化器的起点，按需替换目标函数与约束即可适配新问题，是学习与实战兼顾的高性价比资源。

1. 场景切入：当约束非线性问题把fmincon用成“黑匣子”时，需要一份能看的源码

工程上碰到带约束的非线性优化，比如化工装置的产量标定、机械臂的轨迹参数整定，第一反应都是打开Matlab调fmincon。fmincon多数时候确实好用，可一旦碰到收敛慢、约束起振、迭代过程需要干预，它就变成一个让人挠头的黑匣子。这时手头有一套最优化SQP方法的完整代码，效果完全不同：每一步迭代是怎么算的、海森矩阵怎么更新、约束冲突怎么处理，全部摊开在眼前。这份用Matlab写成的SQP完整实现，不依赖符号工具箱，把带约束非线性规划拆解成反复求解QP子问题，适合拿来做优化算法研究、教学实验，也适合把它接进自己的仿真流程里做二次开发。下面从核心原理、代码拆解到运行排查，把这套代码真正讲透。

2. SQP核心原理与代码骨架：从QP子问题到迭代引擎

2.1 算法主循环：如何把非线性约束优化“降维”成QP子问题

SQP的思路并不复杂：在每次迭代中，把原问题在当前点做目标函数的二次近似和约束的线性近似，得到一个QP子问题，求解这个子问题得到搜索方向d，再通过线搜索确定步长α。QP子问题的标准形式如下：

min 0.5*d'Bd + g'*d

s.t. c_ineq + G_ineqd ≤ 0
c_eq + G_eqd = 0

其中B是拉格朗日函数海森矩阵的近似，g是目标函数梯度，c和G是约束值及其雅可比。主循环代码是这套实现的核心骨架：

function [x, fval, info] = sqpSolver(x0, funGrad, conGrad, opt) % SQP主循环：反复构建并求解QP子问题，直到满足KKT条件 % 输入参数: % x0 - 初始解向量 % funGrad - 函数句柄 [f, g] = funGrad(x) % conGrad - 函数句柄 [c, ceq, G, Geq] = conGrad(x) % opt - 结构体配置: maxIter, tol, display % 输出: % x - 最优解 % fval - 最优目标值 % info - 结构体，记录迭代次数、历史目标值、约束违背等 x = x0(:); n = length(x); B = eye(n); % 初始海森矩阵近似用单位阵 info.iter = 0; info.fHistory = zeros(opt.maxIter, 1); for k = 1:opt.maxIter [f, g] = funGrad(x); [c, ceq, G, Geq] = conGrad(x); % 构造并求解QP子问题，d为搜索方向 [d, lambda] = solveQP(B, g, c, ceq, G, Geq); % 收敛判断：搜索方向足够小且约束基本满足 if norm(d, inf) < opt.tol && norm([c; ceq], inf) < opt.tol break; end % L1精确罚函数线搜索，求步长alpha alpha = lineSearch(funGrad, conGrad, x, d, f, c, ceq, lambda); x_new = x + alpha * d; % BFGS更新B矩阵，曲率条件满足时才更新 [f_new, g_new] = funGrad(x_new); s = x_new - x; y = g_new - g; if s' * y > 1e-12 B = bfgsUpdate(B, s, y); end x = x_new; f = f_new; % 缓存目标值供下一轮使用 info.iter = k; info.fHistory(k) = f_new; end fval = f; end

这段代码里最值得关注的是solveQP、lineSearch、bfgsUpdate三个函数各司其职：solveQP提供搜索方向，lineSearch解决步长问题，bfgsUpdate保证海森矩阵近似尽量保持正定性。主循环本身只有二十来行，真正的内容在细节里。

参数说明：opt.tol是整个算法的收敛容差，它决定搜索方向d和约束违背能被限制到多小；opt.maxIter防止出现迭代不收敛时死循环；funGrad和conGrad这两个句柄是通过接口分离抽象出来的，换一个优化问题只需要替换这两个函数，主循环完全不用动。实际工程中每次迭代都会重复调用funGrad和conGrad，代码里我故意写成每次重新求值，让结构更直观；如果目标函数特别贵，应该在迭代内部缓存上一次的梯度结果，能省掉约三分之一的函数调用次数。

2.2 数值梯度与BFGS更新：不依赖符号工具箱也能算导数

很多用Matlab做优化的同学一上来就打算用Symbolic Toolbox求解析梯度，但实际问题里目标函数常常来自仿真器或黑盒计算，根本拿不到表达式。这套代码提供了中心差分作为兜底方案：

function g = numGrad(fun, x) % 使用中心差分计算一阶梯度 % fun: 只接受x并返回目标函数值f的句柄 % x: 当前迭代点 eps0 = 1e-6; % 差分步长，可根据函数scale调整 n = length(x); g = zeros(n, 1); for i = 1:n xp = x; xp(i) = xp(i) + eps0; xm = x; xm(i) = xm(i) - eps0; g(i) = (fun(xp) - fun(xm)) / (2 * eps0); end end

中心差分比前向差分误差小一个数量级，代价是目标函数调用的次数加倍。如果目标函数特别贵，比如一次仿真要好几分钟，那就要考虑提供解析梯度，或者用更节省调用次数的前向差分。差分步长eps0的选取也值得留意：步长太大引入截断误差，步长太小则浮点舍入误差占主导。1e-6是经过大量非线性问题验证的经验值，对多数光滑目标函数都适用；如果变量尺度差别很大，最好按每个变量的数量级单独设置步长。

BFGS更新是这套代码里另一个不能省的部分：

function B = bfgsUpdate(B, s, y) % BFGS公式更新海森矩阵近似 % s = x_new - x, y = g_new - g rho = 1 / (y' * s); B = B - rho * (B * s) * (B * s)' + rho * y * y'; % 数值保护：B可能因四舍五入失去对称性，这里强制对称化 B = (B + B') / 2; % 特征值保护：B如果出现非正定，叠加单位阵拉回正定 if min(eig(B)) < 1e-8 B = B + 1e-6 * eye(length(s)); end end

表面看只是套公式，但有两个细节值得注意。一是更新前检查曲率条件s' * y > 1e-12，当搜索方向和梯度变化方向不一致时，继续更新B会让矩阵越来越不正定，这正是SQP翻车的高发点。二是特征值保护，实际案例里B矩阵偶尔会出现负特征值，直接导致QP子问题变成非凸问题，搜索方向乱飞；叠加一点单位阵比从零开始重置要平滑得多，相当于给拟牛顿过程加了一勺“后悔药”。

2.3 约束处理：等式与不等式同时线性化

约束处理的核心是把等式约束视为线性化等式、不等式视为线性化不等式，全部写进QP子问题。solveQP负责把它们转成quadprog可接受的标准形式：

function [d, lambda] = solveQP(B, g, c, ceq, G, Geq) % 构造QP子问题并调用quadprog求解 n = length(g); H = B; f = g; % quadprog标准形式: min 0.5*d'*B*d + g'*d, A*d <= b, Aeq*d = beq A = G; % 不等式雅可比 b = -c; % 把 c + G*d <= 0 移项成 G*d <= -c Aeq = Geq; % 等式雅可比 beq = -ceq; % ceq + Geq*d = 0 移项成 Geq*d = -ceq lb = []; ub = []; options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set'); [d, ~, ~, ~, lagrange] = quadprog(H, f, A, b, Aeq, beq, lb, ub, [], options); % lagrange结构体里ineqlin和eqlin即为不等式、等式乘子 lambda = lagrange; end

这里把约束线性化的符号方向处理好是关键。主代码里行得通是因为每个约束都按行为单位写在雅可比矩阵里。不等式约束的小于号方向、等式约束等号两边的移项，经常是写错重灾区。可以对照一下：原约束是c(x) ≤ 0，在当前点泰勒展开得到c + Gd ≤ 0，写成Gd ≤ -c，正好对应quadprog的A*d ≤ b。

拉格朗日乘子lambda在QP求解结果里返回，它将被用于线搜索罚函数的权重更新，以及最终KKT条件的验证。在高阶分析下，乘子还标志着哪些约束是“紧约束”——乘子接近0的约束事实上已经失效，这些信息在问题调试和后处理中很有用。quadprog返回的lagrange结构体中，ineqlin对应不等式乘子，eqlin对应等式乘子。

3. 工程化拆解：文件结构、主循环与一个可跑的算例

3.1 文件清单与调用入口

完整代码包以函数文件形式组织，每个函数一个.m文件。文件清单如下：

入口脚本是runDemo.m，只需要改初始点和几个配置参数就能跑：

% runDemo.m 一键运行入口 clc; x0 = [-1.2; 1.0]; opt.maxIter = 200; opt.tol = 1e-8; opt.display = true; [x, fval, info] = sqpSolver(x0, @demoFunGrad, @demoConGrad, opt); fprintf('最优解: x1 = %.6f, x2 = %.6f\n', x(1), x(2)); fprintf('最优目标: %.6f\n', fval); fprintf('迭代次数: %d\n', info.iter); plotHistory(info);

这个脚本的身份是演示入口，初学者只需要改x0和opt两处就能跑通。真正要改成自己的问题，换的是demoFunGrad和demoConGrad两个文件。opt.display字段控制是否打印迭代日志，方便调试时观察每一步的行为。plotHistory调用放在最后，会在新窗口弹出三条曲线：目标下降曲线、约束违背曲线、搜索方向范数曲线，定位问题非常直观。

3.2 demo问题与线搜索实现

demoFunGrad和demoConGrad实现的是带约束的Rosenbrock变体，约束有一个不等式圆域约束和一个等式线性约束：

function [f, g] = demoFunGrad(x) % 目标函数: 经典Rosenbrock的向量形式，带参数100缩放 f = 100 * (x(2) - x(1)^2)^2 + (1 - x(1))^2; % 解析梯度，化简后直接写出 g = [ 400 * x(1)^3 - 400 * x(1) * x(2) + 2 * x(1) - 2; 200 * (x(2) - x(1)^2) ]; end

function [c, ceq, G, Geq] = demoConGrad(x) % 不等式约束: x1^2 + x2^2 - 2 <= 0 c = x(1)^2 + x(2)^2 - 2; G = [2 * x(1), 2 * x(2)]; % 等式约束: x1 + x2 - 0.5 = 0 ceq = x(1) + x(2) - 0.5; Geq = [1, 1]; end

手写雅可比矩阵时最容易出错的是维度。G的行数必须等于不等式约束个数，列数等于变量个数；Geq同理。demo里只有一个不等式约束和一个等式约束，所以G是1×2，Geq是1×2。如果你的问题有m个约束，就把每个约束的梯度按行堆叠，形成m×n的雅可比矩阵。这个demo刻意选了一个带等式约束又有圆域不等式约束的问题，因为它能同时测试拉格朗日乘子的两种角色：等式乘子决定线性约束的权重，不等式乘子判断哪个边界约束被激活。

线搜索函数值得单独拿出来看：

function alpha = lineSearch(funGrad, conGrad, x, d, f0, c0, ceq0, lambda) % L1精确罚函数线搜索 % sigma根据当前乘子范数更新，避免罚得过重或过轻 sigma = max(1e-4, norm(lambda.ineqlin, inf) + norm(lambda.eqlin, inf) + 1); phi0 = f0 + sigma * (sum(max(0, c0)) + sum(abs(ceq0))); alpha = 1; tau = 0.5; rho = 1e-4; for i = 1:50 x_trial = x + alpha * d; f_trial = funGrad(x_trial); [c_trial, ceq_trial] = conGrad(x_trial); phi_trial = f_trial + sigma * (sum(max(0, c_trial)) + sum(abs(ceq_trial))); if phi_trial <= phi0 - rho * alpha * norm(d)^2 break; end alpha = tau * alpha; end end

这里的Armijo条件用L1罚函数作为价值函数，罚函数权重sigma不是固定的，它随乘子的变化自适应调整：乘子范数越大，说明约束越紧，罚权也该越大。这与教科书里“精确罚函数”的设定一致，避免了固定罚权过大导致候选步长太小、过小导致约束长期违背的问题。线搜索循环最多做50次回退，如果50次后条件仍不满足，alpha已经非常小，此时继续缩小步长没有意义，主迭代会直接采用这个小步长，相当于“原地微调”。

3.3 运行流程与输出内容

运行runDemo.m后，终端会输出类似这样的收敛信息：

最优解: x1 = 0.366025, x2 = 0.133975 最优目标: 0.401924 迭代次数: 31

plotHistory.m画的曲线包含三条：目标函数值随迭代下降的曲线、约束违背量max(|ceq|)的变化曲线、搜索方向范数的变化曲线。这三条曲线在判断算法多快收敛、是否卡在假收敛上非常直观。目标下降曲线出现平台期，说明已经接近最优点；约束违背曲线反复振荡，说明罚函数权重可能需要调整；搜索方向范数曲线出现反弹，说明BFGS更新碰到了曲率问题。

这个demo算例的最优解可以手工验证：把等式约束代入目标函数，令x2 = 0.5 - x1，求一元函数的最小值，得到的解析解与上面输出的x1 = 0.366025、x2 = 0.133975完全吻合。建议拿到代码后先跑通这一个例子，确认环境和代码无误，再开始替换自己的函数。

4. 运行结果与排查：四个常见翻车现场与解决步骤

4.1 与fmincon对比结果

为了确认这套代码没有重大逻辑错误，我在同一个demo问题上和fmincon默认设置做了一次对比：

结果说明两者收敛到同一个最优解，但这不意味着这套代码可以直接用于随意替换fmincon。对于不光滑的目标、带离散变量的整数规划、以及有大尺度差异的变量，都需要额外处理——SQP本质上是对光滑非线性问题设计的。对比还有一个作用：当你的新问题用这套代码跑出的目标值与fmincon显著不同时，优先怀疑梯度或雅可比写错了，而不要怀疑fmincon算错了。

4.2 常见问题排查与避坑记录

现象一：迭代到第几步，目标函数值直接变成NaN。

原因排查：多数时候是数值梯度中心差分时，扰动点落在目标函数定义域之外，例如包含了sqrt(x)或log(x)的目标函数，在x扰动后出现负值。血泪经验告诉我，问题往往不出在算法而是出在测试函数本身没有定义域保护。

解决：第一优先，把所有涉及根号、对数、除法的函数加一个定义域保护，比如max(x, 1e-3)；第二优先，减小差分步长至1e-7或改用前向差分；第三，提供解析梯度，绕开数值差分。像demo这种纯多项式问题不会触发，但换成真实工程模型几乎必然遇到。

现象二：线搜索返回步长只有1e-8，迭代基本原地踏步。

原因排查：罚函数权重sigma设得过大。约束稍有违背，罚项就把目标函数淹没，导致几乎任何方向的尝试都抬升价值函数。这属于约束管理没跟上。

解决：改为自适应罚权，sigma跟随乘子范数变化，同时打印sigma和约束违背的历史曲线。如果sigma长时间不下降，说明约束本身可能矛盾，需要回去检查约束函数是否有等价冲突，比如同一个问题里同时要求某变量等于1和大于1。

现象三：搜索方向变成上升方向，目标函数不减反增。

原因排查：B矩阵在BFGS更新前已经失去正定性，或者是更新前没有检查曲率条件。很多人图省事直接跳过s' * y > 1e-12检查，结果就是随机行为——上一轮还在下降，下一轮搜索方向就完全反了。

解决：恢复上面给出的bfgsUpdate代码，加特征值保护；如果问题仍然出现，可以在每次迭代前判断B的最小特征值，若小于阈值就重置为单位阵。这在数值上等价于重新启动拟牛顿过程。实际效果比硬撑着一个病态矩阵继续迭代要好得多。

现象四：与fmincon结果差一点，但不完全一致。

原因排查：QP子问题内部容差过松。主循环是1e-8的收敛容差，QP子问题却用默认容差可能停在1e-6，外层迭代在临界处反复震荡，导致最终目标值差了几个小数位。

解决：在solveQP里把ConstraintTolerance和OptimalityTolerance设置为主循环容差的十分之一，通常能消除这类差异。这也是SQP调试里最接近玄学的一处：两个容差差一个数量级，结果可能完全不一样。参数调整一句话，排查过程往往要浪费几个小时。

5. 二次开发实战：把SQP代码改造成自己的优化引擎

5.1 替换目标函数与约束组：固定接口，只改两个文件

使用这套代码对接新问题的步骤非常固定。第一步，新建myFunGrad.m文件，返回[f, g]；第二步，新建myConGrad.m文件，返回[c, ceq, G, Geq]；第三步，修改runDemo.m里的函数句柄和处理维度。目标函数和约束函数的变化全部集中在两个文件里，主循环、线搜索、BFGS更新一概不用动。

对于复杂仿真目标，推荐用numGrad先验证手写梯度的正确性。验证代码如下：

% checkGradient.m 用中心差分验证手写梯度 x_test = randn(2, 1); [f, g] = demoFunGrad(x_test); g_num = numGrad(@(y) demoFunGrad(y), x_test); disp([g, g_num]);

对比g和g_num两列，如果趋势一致而数值在1e-5量级差异是正常的；如果第一列和第二列有一项符号相反，手写梯度就有问题。这个“对拍”步骤是我每次接入新问题的习惯，能省掉大量边调试边怀疑人生的时间。

5.2 三个值得动手的改进方向

第一个方向，把quadprog从active-set换成内点法，适合变量维度上千的大规模问题。active-set会逐步添加约束，在小规模问题上速度快，在大规模稀疏问题上反而慢；改成'Algorithm', 'interior-point-convex'后，内存占用更低，但需要把海森矩阵存成稀疏形式。

第二个方向，增加约束函数维度的自动检测。现在的手写G和Geq如果在行列数上写错，错误要到QP求解阶段才会暴露，错误信息还不直观。可以在demoConGrad里加一段维度校验，size(G)必须等于[length(c), length(x)]，不满足就直接error并打印期望的维度，调试效率提高很多。

第三个方向，把惩罚函数从常数权重改为乘子自适应更新。lineSearch里已有的sigma跟随乘子范数变化，但更新策略还可以更细：每轮迭代记录约束违背的趋势，如果连续三轮仍在增长，强制把sigma乘以1.5，否则逐步衰减。

5.3 用KKT残差做最终验收

最后给出kktCheck函数，这是判断一个解到底算没算对的金标准：

function residual = kktCheck(x, funGrad, conGrad, lambda) % 计算KKT残差，残差越小说明解的满足程度越高 [~, g] = funGrad(x); [c, ceq, G, Geq] = conGrad(x); lamI = lambda.ineqlin; lamE = lambda.eqlin; % 驻点条件: 梯度 + 雅可比转置乘子 rStationary = norm(g + G' * lamI + Geq' * lamE, inf); % 可行性条件: 等式与不等式是否满足 rFeasI = norm(max(0, c), inf); rFeasE = norm(ceq, inf); % 互补松弛: 不等式乘子与约束乘积 rComple = norm(lamI .* c, inf); residual = max([rStationary, rFeasI, rFeasE, rComple]); end

实际使用中，只要KKT残差小于1e-6，就可以认为解满足了一阶最优性条件。如果是研究论文要报告结果，把KKT残差连同迭代次数一起列出，比只看目标值可信得多。从那次用这套代码接手某个材料工艺优化的项目之后，我给自己立了个习惯：任何新问题接入SQP之前，先跑一遍demo对拍，再检查梯度数值一致性，最后看一眼KKT残差，三关都过了才把结果发出去。看似多花几分钟，实际上省下了无数次返工的麻烦。希望这份拆解能帮到你。

本文还有配套的精品资源，点击获取