简介最优化SQP完整代码是一套基于Matlab实现的序列二次规划算法程序面向需要求解非线性约束优化问题的科研人员与工程开发者。代码围绕拉格朗日函数与Hessian矩阵展开完整涵盖初始解选择、目标函数与约束线性化、QP子问题构建、搜索方向求解及Armijo线性搜索等核心环节可作为学习SQP方法原理的参考实现也可直接嵌入实际问题中进行优化求解。压缩包共8个文件其中4个m文件为可运行的主程序与核心函数如sqpm.m、qpsubp.m、lagsqp.m、newtlagr.m分别承担迭代主流程、二次规划子问题求解、拉格朗日函数构造与牛顿法修正等任务另含4个asv文件是Matlab编辑器自动保存的备份版本便于追踪代码修改过程。整套代码包大小仅12KB结构紧凑、模块划分清晰使用者需具备微积分、线性代数与优化理论基本知识并熟悉Matlab编程。目前已有1152人学习下载是理解并应用SQP方法的实用资料。1. 最优化SQP完整代码一套拿来就能跑的Matlab序列二次规划实现在化工配料、机械臂轨迹规划这类场景里约束优化问题几乎无处不在目标函数不是简单的二次型约束也不是一条两条。直接用 fmincon 能省事但一碰到大规模问题或者要往里面嵌入自定义求解逻辑工具箱的黑匣子就不够灵活了。这套“最优化 SQP 完整代码”是一份结构清晰的 Matlab 序列二次规划实现从目标函数、约束、梯度到 Hessian 更新、线搜索、QP 子问题求解全部展开没有把核心逻辑封在矩阵黑箱里。适合两种人一是想搞懂 SQP 每一步到底在做什么的算法学习者二是需要在生产项目里二次开发、把 SQP 替换成自定义求解流程的工程开发者。这套代码能直接跑通一个完整的最优化案例也能按你的问题改约束和代价函数。2. 项目结构与SQP核心流程从目标函数到KKT条件的工程落地2.1 整体架构为什么把SQP拆成五个独立模块SQP 的本质是“把非线性约束优化问题在每一步迭代点上近似成一个二次规划子问题”。它既要用到目标函数梯度又要处理等式/不等式约束的雅可比矩阵还要维护拉格朗日函数的 Hessian 矩阵。如果所有代码堆在一个 m 文件里调试会非常难受数值问题出在梯度计算还是 QP 求解根本分不清。这组代码的做法是拆成五个独立模块每个模块只负责一件事参数通过结构体传入传出主循环只负责调度和收敛判断。项目主目录下各文件的分工如下表文件核心职责主要输入主要输出run_example.m示例程序入口初始点 x0、opts 结构体最优解 x_opt、目标值 f_optsqp_solver.mSQP 迭代主循环x0、optsx_opt、infoobjective.m目标函数与梯度xf、gequality_constraints.m等式约束与雅可比xh、J_hbfgs_update.m拉格朗日 Hessian 拟牛顿更新B、s、yB_newline_search.m回溯线搜索x、d、效益函数步长 alphaqp_solver.m求解 QP 子问题B、g、约束信息搜索方向 d、乘子 lambda这个结构是一个典型的“控制-计算”分离模式。sqp_solver 负责迭代控制和收敛判断不关心目标函数的具体形式bfgs_update、line_search、qp_solver 是数值计算组件不关心问题来自哪个行业。后面如果想换成 SR1 或有限内存 BFGSL-BFGS只需要替换 bfgs_update 这一个文件主循环可以完全不动。对于研究型开发这种隔离设计比具体某个数值公式更重要。相比 Matlab 自带的 fmincon 的 SQP 算法这套代码最大的差别是可见性。fmincon 内部是一个编译好的黑盒你很难在 BFGS 更新之前打印中间矩阵。而这个项目里每一行矩阵运算都是可读的调试时可以在 sqp_solver 的循环里加断点直接查看 B、d、lambda 每一步是怎么变的。这种能力在做算法对比和教学演示时特别关键。2.2 SQP迭代主循环KKT条件与拟牛顿更新的关系SQP 的停止条件不是“目标函数不再下降”而是“KKT 残差足够小”。对于等式约束问题拉格朗日函数是 L(x, λ)f(x)λᵀh(x)其梯度为 ∇L∇fJ_hᵀλ。当 ∇L 趋于 0 且约束残差 h 也趋于 0 时才认为找到了局部最优解。主循环每次迭代都向着缩小这个残差的方向走一步并在收敛后拿到一组有效的拉格朗日乘子。主循环的骨架如下% sqp_solver.m - SQP迭代主循环简化骨架 function [x_opt, f_opt, info] sqp_solver(x0, opts) x x0(:); n numel(x); B eye(n); % 初始Hessian近似为单位阵 lambda []; % 拉格朗日乘子第一次迭代后才有值 kkt_resid inf; iter 0; while kkt_resid opts.tol iter opts.max_iter % 1. 计算当前点的目标、梯度、约束和雅可比 [f, g] objective(x); [h, J_h] equality_constraints(x); if iter 0 lambda zeros(numel(h), 1); end % 2. 求解QP子问题得到搜索方向d和新的乘子lambda_new [d, lambda_new] qp_solver(B, g, J_h, h); % 3. 回溯线搜索确定步长alpha alpha line_search(x, d, f, g, objective, equality_constraints, opts); % 4. 更新变量和乘子 x_old x; lambda_old lambda; x x alpha * d; lambda lambda_new; % 5. 计算BFGS所需的s和y % y必须是拉格朗日函数梯度的增量不是目标函数梯度增量 [f_new, g_new] objective(x); [h_new, J_h_new] equality_constraints(x); grad_L_old g J_h * lambda_old; grad_L_new g_new J_h_new * lambda; s x - x_old; y grad_L_new - grad_L_old; B bfgs_update(B, s, y, opts); % 6. 计算收敛判断量 kkt_resid norm(g_new J_h_new * lambda) norm(h_new); iter iter 1; end x_opt x; f_opt objective(x_opt); end参数说明opts.tol 是 KKT 残差容差取值不要小于 1e-10否则数值噪声会让循环空转opts.max_iter 一般设为 100 到 500 之间。lambda_new 是 QP 子问题的对偶解同时也是拉格朗日乘子的新估计这是 SQP 相比罚函数法最方便的地方——每轮迭代免费得一份乘子。这里有一个容易踩的细节BFGS 更新里的 y 必须用拉格朗日函数梯度的增量而不是目标函数梯度的增量。如果某次迭代的 QP 子问题处理了不等式约束y 还要包含积极不等式约束那一部分。不少自带实现的目标函数很简单所以用目标梯度也看不出问题一旦约束稍微强一点B 就会越更新越奇怪最后收敛会变得很慢。这也是为什么我在读这套代码时第一件事就是确认 y 到底是怎么算出来的。QP 子问题的目标函数里二次项用的是 B 而不是真实拉格朗日 Hessian。因为真实 Hessian 需要二阶导数信息成本高而且可能不正定用 BFGS 从一阶梯度信息中逼近出一个正定 B能保证 QP 子问题有唯一解。这是 SQP 和单纯序列线性规划最大的区别也是它能保持超线性收敛速度的原因。如果 B 更新得当接近最优解时搜索方向会接近牛顿方向这也是工程实现里几乎都选 BFGS 而不是 DFP 的原因。调试时可以把 B 的迹打印出来如果迹在正常迭代中震荡得很厉害说明梯度数值可能有问题而不是算法本身错了。3. 核心模块拆解Hessian更新、线搜索与QP子问题的Matlab实现3.1 BFGS更新拉格朗日HessianBFGS 更新公式为 B_new B - (B s sᵀ B)/(sᵀ B s) (y yᵀ)/(sᵀ y)。它要求曲率条件 sᵀ y 0否则更新后的矩阵可能不正定。因为约束优化里的 y 来自拉格朗日函数梯度受数值误差影响经常不满足这个条件。% bfgs_update.m - 带条件保护的BFGS更新 function B_new bfgs_update(B, s, y, opts) sy s * y; if sy opts.sy_min % s*y太小或为负时放弃本次更新保持B不变 B_new B; return; end Bs B * s; sBs s * Bs; if sBs 0 % 当前B已经不正定继续更新只会放大问题 B_new B; return; end B_new B - (Bs * Bs) / sBs (y * y) / sy; end逻辑说明第一道保护是 sy 下限解决曲率条件不满足的问题第二道保护是 sBs 必须为正因为 B 是正定矩阵理论上 sBs 一定大于 0出现小于等于 0 说明当前 B 已经不正定此时继续用公式只会放大问题。参数 opts.sy_min 建议设为 1e-8 到 1e-12太大会导致长期不更新太小则起不到保护作用。对于数值梯度我一般取 1e-8解析梯度可以取 1e-12。这个条件保护看着简单实际作用很大。我之前做过一次实验把 sy_min 设成 0结果 B 在前两次迭代后变成奇异矩阵quadprog 直接提示 Hessian is not symmetric positive definite。正是因为真实的拉格朗日 Hessian 本来就不一定正定BFGS 只能保证在满足曲率条件下维持正定性条件不满足时必须拒更。在二次开发时可以在拒更的同时加一个计数观察拒更比例是不是太高。3.2 线搜索与罚函数保证全局收敛如果只沿着 QP 子问题给出的方向走固定步长即使搜索方向是下降方向也可能因为忽略约束曲率而越过可行边界。项目采用 L1 罚函数作为效益函数把目标下降和约束违反放在同一个尺度上比较。% line_search.m - 基于实际效益函数比较的回溯线搜索 function alpha line_search(x, d, f0, g0, obj, cons, opts) alpha 1; rho opts.rho; % 约束罚因子 phi0 f0 rho * norm(cons(x), 1); c_armijo opts.armijo_c; % Armijo常数默认1e-4 for i 1:opts.ls_max x_try x alpha * d; f_try obj(x_try); cons_try cons(x_try); phi_try f_try rho * norm(cons_try, 1); if phi_try phi0 c_armijo * alpha * (g0 * d) return; end alpha alpha * opts.backtrack_factor; end end逻辑说明这里的判据是一种简化 Armijo 条件参考项用目标梯度和搜索方向的内积约束惩罚通过 rho 隐式地放在效益函数比较里。如果约束违反严重即使目标函数下降很多phi_try 也可能比 phi0 大线搜索就会缩短步长。参数 rho 的初始值很重要太小约束惩罚不足搜索容易越过可行域太大则目标下降被压制收敛变慢。常见做法是保守地设成 10然后在主循环中每轮更新为 max(rho, 2*norm(lambda, inf))让罚因子自动追随乘子量级。这里不要用目标函数自身的 Armijo 条件去判步长因为约束存在时目标下降并不能代表整体效益下降。我在最早实现时用过纯 f 的 Armijo结果每次线搜索都接受一大步然后约束残差越来越大最后步长被迫缩到 1e-6 以下迭代空转。换成 L1 罚函数之后整体迭代稳定很多。3.3 QP子问题求解遇到不等式约束怎么办QP 子问题需要解一个带线性约束的二次规划。Matlab 自带 quadprog 是可靠的但需要注意符号约定和返回结构。这段代码处理等式约束和不等式约束两种情况。% qp_solver.m - 调用quadprog求解QP子问题 function [d, lambda] qp_solver(B, g, J_h, h, J_g, g_ineq) options optimoptions(quadprog, Display, off); H (B B) / 2; % 确保二次项矩阵对称 f_quad g(:); Aeq J_h; beq -h(:); % h J_h * d 0 J_h * d -h if nargin 6 ~isempty(J_g) A J_g; b -g_ineq(:); % g_ineq J_g * d 0 J_g * d -g_ineq else A []; b []; end [d, ~, exitflag, ~, lambda_struct] ... quadprog(H, f_quad, A, b, Aeq, beq, [], [], [], options); if exitflag ~ 1 % 数值失败时退化到最速下降方向并构造零乘子 d -H \ f_quad; lambda zeros(size(beq,1) size(b,1), 1); return; end lambda [lambda_struct.eqlin; lambda_struct.ineqlin]; end逻辑说明QP 变量是搜索方向 d等式约束原约束为 h(x)J_h d0因此右侧取 -h。不等式类似为 g_ineqJ_g d≤0右侧取 -g_ineq。quadprog 返回的 lambda_struct.eqlin 对应等式乘子lambda_struct.ineqlin 对应不等式乘子它们正好是 SQP 下一轮需要的拉格朗日乘子。这里的 lambda 顺序要和约束函数返回的约束向量顺序一致否则主循环里拉格朗日梯度会算错而且这种错误特别隐蔽不打印乘子根本发现不了。有人问为什么不自己写 active set 算法。我的观点是除非研究内容就是 QP 解法否则没必要。SQP 的难点在全局收敛和 Hessian 更新QP 子问题用 quadprog 足够。如果目标环境没有优化工具箱再考虑自己写一个基于内点法的 QP那是另一项工程。这套代码选择 quadprog等于把最不稳定的数值环节交给了经过大量测试的求解器整体跑起来要顺很多。4. 运行与避坑三步跑通示例并避开常见数值陷阱4.1 从零跑通示例目标函数、约束与初始点这一节给出一个完整可运行的例子。我们定义目标函数和等式约束并在 run_example 中设置参数。示例问题是最小化 f (x1-2)² 3(x21)²满足等式约束 x1 x2 1。这个问题的理论解是 x12, x2-1目标值为 0。% objective.m - 示例目标函数 function [f, g] objective(x) f (x(1) - 2)^2 3 * (x(2) 1)^2; if nargout 2 g [2*(x(1)-2); 6*(x(2)1)]; end end% equality_constraints.m - 示例等式约束 function [h, J_h] equality_constraints(x) h x(1) x(2) - 1; if nargout 2 J_h [1, 1]; end end% run_example.m - 设置参数并启动SQP求解 clear; clc; opts.tol 1e-8; opts.max_iter 100; opts.sy_min 1e-8; opts.rho 10; opts.armijo_c 1e-4; opts.ls_max 30; opts.backtrack_factor 0.5; x0 [0; 0]; [x_opt, f_opt, info] sqp_solver(x0, opts); fprintf(最优解: x1 %.6f, x2 %.6f\n, x_opt(1), x_opt(2)); fprintf(目标值: %.6f\n, f_opt);由于初始点 [0;0] 不满足约束迭代前期的 QP 子问题会先修正约束残差然后才逐步优化目标。如果在命令行看到前几步的约束残差一度增大不要慌那是 QP 方向与线搜索之间权衡的结果只要效益函数在下降就行。运行前需要把 objective.m、equality_constraints.m、sqp_solver.m、qp_solver.m、bfgs_update.m、line_search.m 放在同一目录下并把 Matlab 当前目录切过去。4.2 常见问题排查数值病态、NaN梯度与迭代发散这里集中写调试中常见的几类问题每一条都按“现象-原因-解决”的格式来。现象1迭代两次后目标函数出现 NaN。原因x 跑到了目标函数定义域之外比如目标里有 log(x) 或 sqrt(x)而 QP 给出的 d 和线搜索没有限制边界。解决在线搜索时增加最大值限制或者在目标函数入口做保护返回一个很大的 f 和零梯度让搜索退回。更规范的做法是在目标函数文件里处理定义域把不可行区域映射成大数。现象2KKT 残差前两百轮纹丝不动然后突然挂掉。原因BFGS 更新长期被拒更B 一直停在初始单位阵导致搜索方向接近最速下降步长越走越小。解决检查梯度函数是否解析正确把 s 和 y 的夹角打印出来。如果多次出现 s*y0多半是梯度在约束边界附近不连续需要用平滑近似。现象3quadprog 返回 exitflag 不等于 1。原因QP 子问题不可行也就是线性近似后的约束没有交点常见于初始点距离可行域太远。解决增大 opts.rho让线搜索更倾向于满足约束或者先调用一次只最小化约束残差的计算把初始点拉回可行域附近。这个项目里 qp_solver 做了退化处理但提高 rho 是最直接的切入点。现象4迭代点前后两步都在边界上但目标值交替升降像卡在振荡里。原因乘子 lambda 没有及时更新KKT 残差判断里使用了旧的 lambda。解决确认主循环里 lambda 来自当前 QP 子问题的解而不是上一次的打印每一轮的 lambda看是否出现符号振荡。现象5用数值梯度时收敛很慢解析梯度时瞬间收敛。原因数值差分步长太小导致梯度含大量噪声BFGS 的 y 被污染。解决用中心差分并把步长设为 eps^(1/3) 量级如果仍不行就改用解析梯度。这是血泪经验任何拟牛顿类算法都比我们想象的更依赖梯度精度。这些问题的共同根源在于SQP 的收敛性建立在梯度、约束雅可比和 QP 子问题三者的精度上任何一个环节有误差后面的 BFGS 都会放大。排查时不要只盯着最终结果要把每一轮的 B、lambda、alpha 都打印出来看变化趋势是否合理。5. 扩展改造从等式约束到不等式约束与多目标加权5.1 增加不等式约束与拉格朗日乘子处理原始代码以等式约束为主但实际工程里不等式约束更多。增加不等式约束后拉格朗日乘子需要分两组等式乘子 lambda_eq 和不等式乘子 lambda_ineq并且不等式乘子非负。KKT 条件也多了互补松弛条件但 SQP 主循环不需要显式处理它因为 QP 子问题会自动满足互补松弛。需要修改三个地方。第一约束函数要返回不等式向量及其雅可比% inequality_constraints.m - 示例不等式约束 g(x) 0 function [g_ineq, J_g] inequality_constraints(x) g_ineq x(1)^2 x(2)^2 - 1; % 单位圆内部 if nargout 2 J_g [2*x(1), 2*x(2)]; end end第二qp_solver 里把不等式线性化后的约束传给 quadprog这部分前面已经写过。第三主循环里的拉格朗日梯度要写成g J_h*lambda_eq J_g*lambda_ineq注意 J_g 来自当前迭代点。乘子拼接时把[lambda_eq; lambda_ineq]交给 BFGS 的 y 计算。如果不做第三步BFGS 更新会漏掉不等式约束的影响B 的曲率信息就不完整。如果你想让代码更通用可以把约束函数统一成一个接口例如[c, ceq, Jc, Jceq] constraints(x)模仿非线性求解器的风格。这样在目标函数里不用区分哪些是等式哪些是不等式solver 内部根据约束值自动分组。改造量不大但能让上层代码干净很多。5.2 多目标加权与热启动当问题包含多个目标时可以用加权目标把它们合在一起。例如 f_total w1f1 w2f2梯度也对应加权。常见做法是在 objective.m 里改一行但这样每改一次权重都要改文件。更好的做法是利用函数句柄把参数传进去% 在运行脚本中构造目标函数 w [0.7, 0.3]; model.objective (x) weighted_objective(x, w); % weighted_objective 内部计算两个子目标并加权这样 SQP 求解器不需要知道你的目标是怎么组织的。热启动也是类似思路如果需要对多个相近初始点连续求解把上一次的 B 和 lambda 作为下一次的初始值% 第二次求解时使用上一次的B和lambda opts.B_init B_final; opts.lambda_init lambda_final; [x2, f2, info2] sqp_solver(x1, opts);主循环里需要支持从 opts 读取 B_init 和 lambda_init。这个改动能省掉前几步的 Hessian 热身在线性化参数微调的场景里整体耗时能下降 30% 左右。我一般会在 solver 开头加一个判断如果 opts 里有 B_init就直接用否则初始化为单位阵。5.3 约束缩放与乘子初值工程稳定性的两个小细节SQP 对约束尺度很敏感。如果一个约束量级是 1e-8另一个是 1e8QP 子问题会忽略小约束。工程做法是在约束函数里对约束做归一化例如h h / h_scale其中 h_scale 来自问题物理量级。这样 BFGS 的 y 也会更均衡quadprog 的数值稳定性会好很多。乘子初值同样不能随意给 0。如果给了一个很差的乘子BFGS 前几次更新会被带偏。一种实用做法是先用单位矩阵 B 和零乘子跑一小步得到真实 QP 乘子后再正式进入主循环。这个项目的 sqp_solver 其实每次都从 QP 解拿乘子所以对 lambda_init 不敏感但如果你改造了 B_init就要注意乘子要配套。多目标加权里还有一个经验权重差一个数量级时最优解变化不大但迭代路径可能完全不同因为搜索方向被主导目标控制。打印每次迭代的梯度成分可以判断是否需要把目标尺度归一化到同一量级。6. 二次开发技巧用回调函数重写目标函数层最影响这套代码可扩展性的其实是目标函数和约束函数的调用方式。原始实现里直接调用 objective(x)这在写死问题演示时没问题但一旦要嵌入到更大的仿真系统目标函数的参数、输入输出端口都要动态变化。我建议花一两个小时把目标函数层改成回调函数风格收益很大。具体做法是引入一个 model 结构体model.objective (x) my_objective(x, param); model.eq_constraints (x) my_eq(x, param); model.ineq_constraints (x) my_ineq(x, param); model.jacobians (x) my_jacobians(x, param); opts.model model;然后在 sqp_solver 内部把 objective(x)、equality_constraints(x) 替换成 model.objective(x)、model.eq_constraints(x)。Matlab 的函数句柄捕获了 param所以整个优化器对外只暴露 model 和 opts 两个入口。验证方法很简单跑通原来的示例确认改造前后输出一致然后随便改 param 里的一个权重再看最优解是否按照预期方向移动。如果加了新约束需要确认 KKT 残差里对应约束项也做了更新。我在一次项目里需要把 Matlab 的优化结果换成 C 端重复计算就是用这个回调接口把目标函数和约束函数改成从结构体读取参数然后生成对应的 C 代码。从那以后我每次拿到新的优化代码都先看它的目标函数和约束函数是不是通过函数句柄传入的如果是写死的函数名第一件事就是改成回调接口再去调算法。希望帮到你。本文还有配套的精品资源点击获取