GPOPS II 实战：高斯伪谱法与最优控制轨迹优化完整指南

简介：GPOPS II 是基于 Matlab 的高斯伪谱法最优控制求解程序包，面向航空航天轨迹优化、机械控制等领域的研究人员与工程师，能将连续时间最优控制问题离散化为有限维非线性规划并高效求解。压缩包内共293个文件，以 m 源文件为主，配套 pdf 与 tex 文档说明算例原理，eps 矢量图展示结果曲线，mat 数据保存运行数据，另含 mexw64、mexa64 等多平台编译接口，包体约10.57MB，目录结构便于按功能查阅。该资源已有4485人学习，是系统掌握 GPOPS II 的实用资料。从源码与文档可完整了解动态模型定义、高斯节点插值、状态与控制参数化、IPOPT 求解、后处理等关键流程；自带算例可快速迁移到实际课题，同时附带的参考文献与期刊模板也有助于成果整理成文。

1. GPOPS II 到底解决什么问题：它不是一个求解器，而是一个转录器

第一次用 GPOPS II 的人，多数是被「史上最牛逼」这五个字吸引来的。装上之后点开示例，你很快会意识到它不是一个能双击运行的求解器，而是一个把连续时间最优控制问题翻译成代数方程组的黑匣子。我在某一个机械臂轨迹优化项目里第一次正经用它，折腾了两周才捅破这层窗户纸：GPOPS II 负责转录，把微分方程约束、积分型目标函数全部离散成非线性规划问题，真正的求解放到了 IPOPT 或 Hpipm 这类 NLP 求解器里。这篇笔记按我自己的踩坑顺序来写，从高斯伪谱法的原理，到最小可运行例子，再到收敛判断和验证习惯，给已经有一点最优控制基础、想用 GPOPS II 提速的工程师一条能直接照做的路径。

2. 高斯伪谱法的核心机制：为什么 LG 配点插值比直接打靶更省

2.1 直接打靶法慢在哪，伪谱法快在哪

做轨迹优化，最朴素的想法是把时间轴均匀切成一万个点，用差分代替微分，然后交给求解器。这个办法在变量数量上是非常吃亏的：状态有多少维，每个时间点就要复制一份，控制向量再来一份，再乘以时间网格数，非线性规划问题的变量规模立刻飙到几万甚至几十万。多重打靶稍微好一点，把时间分成若干段，每段内部用一个低阶积分公式递推，但段间连续性和边界约束照样要写一大堆等式。你算一次可能还行，放在需要反复调整约束、反复重算的场景里，这个代价就非常难受。

高斯伪谱法的思路完全不同：它假设状态轨迹在配点区间上可以用一个全局插值多项式近似，动力学方程在配点处精确满足。也就是说，原来需要写出来的微分方程约束，被转成了「配点上的多项式导数等于动力学函数值」这一组代数方程。未知量从每个时间点上的状态，缩减成多项式在这一组配点上的取值，规模一下子小二个数量级。配点之间没有未知变量，曲线靠插值多项式连接。这就是伪谱法为什么在轨迹优化里显得「快」的根源：不是求解器变快了，而是问题本身变小了。

GPOPS II 在这个框架之上还帮你省掉了最繁琐的部分。你不需要自己去推导离散后的雅可比矩阵，不需要手写每个约束的稀疏结构，只需要把连续时间的动力学、目标函数、路径约束原样写出来，剩下的转录工作它全包了。我第一次用的时候，最不习惯的就是这一点——总觉得自己漏了什么，实际上「交给转录器」就是它的正常用法。

2.2 Legendre-Gauss 配点：为什么节点不能等距取

伪谱法里最关键的选择是配点。如果偷懒把时间轴等距切分、节点均匀分布，然后用多项式插值，就会撞上数值分析里非常经典的 Runge 现象：多项式阶数越高，插值结果在区间两端震荡得越夸张，根本没法用。这也是很多人自己手写伪谱法程序时觉得「玄学」的原因——看着原理很清楚，算出来曲线就是抖。

GPOPS II 里的「高斯」二字，指的就是把配点选在 Legendre-Gauss（LG）点上，也就是 Legendre 多项式的零点。这套节点分布有两个实打实的好处。第一，插值误差在整个区间内比较均匀，不会出现两端剧烈震荡；第二，在这个节点上做高斯求积，数值积分精度非常高——同样的配点数量，积分误差比等距节点小好几个量级。你写目标函数的时候，里面有积分项，比如最小燃料、最小功，这些积分在离散后都变成配点上的加权求和，节点选得好不好直接决定积出来的数准不准。

实际写代码的时候你不需要关心「这些节点具体在哪个时间位置」，这是 GPOPS II 内部处理的。但在调试阶段有一个常识应该记住：GPOPS II 输出的 solution.phase.time 不是均匀网格。你看结果第一个反应如果是「这些时间点怎么不是等距的」，说明你对伪谱法的预期还停留在直接打靶的年代。配点分布不均匀是正常现象，靠近两端密一些，中间稀疏一些，这是 LG 节点天生的性质。

2.3 网格细化：h 细分段，p 升阶

GPOPS II 区别于很多伪谱法工具箱的另一大特色，是它的自适应网格细化。单段高阶级伪谱法确实能逼近光滑问题，但真实工程问题里状态轨迹经常有局部剧烈变化，比如 Bang-Bang 控制的切换点、路径约束的激活边界。在这些地方，单一全局多项式再怎么升阶都难以同时照顾到「整体光滑区」和「局部陡变区」。

这时候就要用到 h/p 细化机制。h 指的是把时间轴拆成多少段，p 指的是每一段内多项式阶数。GPOPS II 第一遍先用一个相对均匀的网格求解，然后检查各段的插值残差：如果某一段残差普遍偏大，就把这一段继续细分成更多子段；如果残差在该段内部呈现周期性起伏，说明多项式阶数不够，需要单独提升这一段的阶数。这个过程完全自动，你只需要在 setup 里给一个 mesh tolerance，比如 1e-3，求解器会在每次迭代后不断调整网格，直到残差低于这个阈值。

这相当于给你吃了颗后悔药：不需要一开始就猜对网格布局。我一般会让它先用一个比较粗糙的初值快速跑通解的大致形状，然后观察自动细化后的段数和阶数分布，以此反推问题的复杂度。段数集中堆在哪个时间段，哪个时间段就是问题最棘手的地方。这个信息比单纯看优化目标值有用得多——它直接告诉你模型里哪个物理过程在跟离散精度较劲。

3. 用 GPOPS II 跑通第一个最优控制问题：双积分器最小时间的完整 setup

3.1 最小文件结构：setup、连续函数、端点函数

GPOPS II 的使用习惯和普通 MATLAB 函数不太一样。它不接收你传进来的矩阵，而是接收一个叫做 setup 的结构体。这个结构体里装了三类东西：问题的名字、动力学和目标函数句柄、边界约束和初值猜测。写代码的时候，大多数情况是你搭好 setup，然后调用一次 gpops2(setup)，剩下的一切交给它内部处理。

最常见的做法是把文件拆成三份：一个主脚本用来填 setup，一个 continuous 函数用来写动力学和拉格朗日型成本，一个 endpoint 函数用来写端点成本（比如最小终端时间）。这个分层不是 GPOPS II 强制的，但这样拆的好处是改模型时不需要翻整个脚本。我习惯先固定住 continuous 和 endpoint 的函数签名，因为它们被 setup.functions 字段引用，改签名容易导致死活对不上号的问题。

下面这个例子是最经典的最小时间控制：一个双积分器系统，状态是位置和速度，控制是加速度，约束加速度绝对值不超过一，目标是让系统从静止最快到达距离一的位置并停在原点。这个问题有解析解，控制曲线是标准的 Bang-Bang，最适合用来验证 GPOPS II 的行为。

% main_double_integrator.m clear; clc; setup.name = 'double_integrator_min_time'; setup.functions.continuous = @(x, u, t) continuous_double_int(x, u, t); setup.functions.endpoint = @(x0, xf, t0, tf) endpoint_double_int(x0, xf, t0, tf); % 时间边界 setup.bounds.phase.initialtime.lower = 0; setup.bounds.phase.initialtime.upper = 0; setup.bounds.phase.finaltime.lower = 0; setup.bounds.phase.finaltime.upper = 10; % 给一个宽松的上界，让优化器自己决定终端时间 % 状态边界：起点固定，终点固定 setup.bounds.phase.initialstate.lower = [0; 0]; setup.bounds.phase.initialstate.upper = [0; 0]; setup.bounds.phase.finalstate.lower = [1; 0]; setup.bounds.phase.finalstate.upper = [1; 0]; % 状态路径边界：位置不限，速度限制在正负 5 setup.bounds.phase.state.lower = [-Inf; -5]; setup.bounds.phase.state.upper = [Inf; 5]; % 控制边界：加速度限制在正负 1 setup.bounds.phase.control.lower = -1; setup.bounds.phase.control.upper = 1; % 初始网格：一段，20 个配点 setup.mesh.colpoints = 20; setup.mesh.tolerance = 1e-3; % 猜测：线性连接起点与终点，控制全程为零 setup.guess.phase.time = [0; 1]; setup.guess.phase.state = [0 0; 1 0]; setup.guess.phase.control = [0; 0]; output = gpops2(setup);

这里最值得注意的参数是 finaltime.upper 和 guess。终端时间上界不要设得太紧，因为最小时间问题是让优化器自己去权衡时间成本和约束可行性，上界太靠近真实解反而会给求解器施加不必要的压力。初值猜测只用两行，表示从初始状态到终点状态的线性插值，控制猜测为零。GPOPS II 会在这个两行猜测的基础上自动生成初始网格上的插值。

3.2 continuous 与 endpoint 函数：目标函数怎么写

continuous 函数里要返回三样东西：动力学导数、拉格朗日型成本、路径约束。注意这里的路径约束与 bounds 里写的框式约束不一样，它是关于状态和控制任意非线性组合的约束，这一项如果不需要可以返回空数组或者零向量。对双积分器问题，路径约束用不上，因为控制上下界已经在 bounds 里写清楚了。

endpoint 函数是另一个关键，最小时间问题的目标函数写在这里：返回 tf 本身。这意味着优化器要把终端时间最小化，同时还要满足 tf 的状态要落在目标状态上。

% continuous_double_int.m function [cost, dynamics, path] = continuous_double_int(x, u, t) x1 = x(:,1); % 位置 x2 = x(:,2); % 速度 dynamics(:,1) = x2; dynamics(:,2) = u; cost = zeros(size(t)); % 时间成本由 endpoint 负责 path = []; end

% endpoint_double_int.m function [obj, event] = endpoint_double_int(x0, xf, t0, tf) obj = tf; % 目标：终端时间最小 event = []; % 不需要额外事件约束 end

这段代码里最容易被忽视的是 cost 这一项。如果 endpoint 已经返回了 tf，而你在 continuous 里又把 cost 填成 ones(size(t))，那么目标函数就变成了 tf 加整个积分区间长度，物理意义会变得混乱。虽然数值上只是差一个倍数，但有梯度的语义已经不对了。我见过有人在论坛里问「为什么 GPOPS II 给出的最小时间偏大」，大概率就是这种重复计入成本导致的。所以建议在一个问题的开始阶段，只在 endpoint 里写目标，连续项成本能不用就不用。

3.3 怎么判断第一次算出来的结果对不对

运行完 gpops2(setup) 之后，结果都放在 output.result.solution.phase 这个结构里，里面有 time、state、control 三个核心字段。第一步不要看目标函数值，先画控制曲线。如果这是一个双积分器 Bang-Bang 问题，你看到控制曲线是先从 1 跳到 -1、中间有一次切换、两端维持约束边界，那么这个结果基本是对的。

% 画结果 sol = output.result.solution.phase; figure; subplot(2,1,1); plot(sol.time, sol.state(:,1)); hold on; plot(sol.time, sol.state(:,2)); legend('x1 位置', 'x2 速度'); subplot(2,1,2); plot(sol.time, sol.control(:,1)); title('控制: 加速度');

注意输出结果奇点一个比较反直觉的现象：你能拿到的只是配点上的值，不是等距时间序列。画图时最稳妥的是直接用 sol.time 作为横轴，不要自己做线性插值到均匀网格再画，那样会无谓地制造波动。如果你发现控制曲线在某个节点之间出现了异常的锯齿，大概率不是求解器的问题，而是你自己插值时把配点的非均匀性质放大到了图上。

4. IPOPT 与网格细化：GPOPS II 的必调参数和收敛判断

4.1 NLP 求解器选型：IPOPT 稳，Hpipm 快

GPOPS II 把伪谱离散做完之后，真正吃计算量的是非线性规划求解器，常见选项是 IPOPT 和 Hpipm。前者是内点法加滤波器线性搜索的经典实现，对各种病态问题容忍度比较高；后者是用来求解大规模稀疏问题的增广拉格朗日内点法，在变量规模大、约束结构稀疏时优势明显。

这两个求解器不是随便换一换那么简单。同一个问题，IPOPT 迭代几十次能收敛，Hpipm 可能前几步就当机；反过来，变量规模特别大的轨迹优化问题里，Hpipm 的内存占用和单步开销比 IPOPT 低不少。我自己的选择习惯是：第一遍调试用 IPOPT，因为它报错信息更直观，失败时更容易定位到是动力学函数写错还是网格给错；等模型稳定、进入批量计算阶段再切到 Hpipm 提速。IPOPT 报出的 restore failed 这类错误，绝大多数情况是问题本身病态，换个求解器只是延迟问题的暴露。

% 在 setup 中指定 NLP 求解器与求解器容差 setup.nlp.solver = 'ipopt'; setup.nlp.ipopt.tol = 1e-6; setup.nlp.ipopt.max_iter = 500;

这里的 nlp.ipopt.tol 是给 NLP 求解器看的终止容差，它和 setup.mesh.tolerance 是两个完全不同的东西。前者控制配点离散后的代数方程组求解精度，后者控制网格细化时残差的收敛标准。很多人把这两个容差混为一谈，结果把 mesh tolerance 调到 1e-8，NLP 内部永远达不到那么高的精度，程序几个小时跑不完，其实问题不出在网格，而是把两个标准搞串了。

4.2 mesh tolerance、colpoints 与初值：三个必调字段

真正需要反复调的核心参数其实就三个：mesh.tolerance、mesh.colpoints、guess。mesh.tolerance 管网格细化的终止条件，建议从 1e-3 起步，跑通之后再收紧到 1e-5。不要一上来就追求 1e-6，因为伪谱法的局部残差在约束切换点附近本来就难降，过低的容差会逼着网格细化器无限加段，消耗的时间会让人怀疑人生。

colpoints 是初始网格每段的多项式阶数。这个数值不需要给大，对光滑问题 10 到 20 就够，越大的初值意味着初始 NLP 问题的变量越多、第一次计算越慢。它的作用只是给网格细化器一个起点，最终的分段方案由自适应过程决定。我见过有人把 colpoints 调到 100，理由是「阶数越高越准」，实际上这会白白增加第一轮求解器的负担，而且高阶多项式的微分矩阵更密集，计算代价是非线性的。

guess 初值猜测这件事，重要性经常被低估。GPOPS II 是局部优化算法，它的迭代收敛依赖于从初始猜测出发的下降路径。一个纯零猜测很可能让 NLP 求解器第一步就算出 NaN，或者卡在某个虚假的局部最优解上。对双积分器这种简单问题，两行线性插值就够了；对更复杂的受控运动问题，我会用上一轮粗糙网格的解作为下一轮初值，或者先用一个简单的比例控制器仿真一段轨迹作为 guess，效果比瞎猜好得多。

4.3 缩放：GPOPS II 里最容易被忽略的翻车点

GPOPS II 的另一个坑是状态量纲差异带来的数值病态。假设你的状态里既有位置（数值在千的量级），又有速度（数值在个位），动力学方程的雅可比矩阵各项相差三个数量级，IPOPT 在内部计算对偶变量时会非常吃力，表现就是迭代步长越来越小、目标函数变化缓慢、甚至收敛到不满足约束的伪最优解。

处理缩放没有统一的公式，常见做法是让所有状态和控制都落在大致 0.1 到 10 的范围内。位置除以 100、速度除以 5、控制保持原样，这些系数要写进动力学函数里，而不是在 setup.bounds 里手动换算后把物理含义丢掉。

function [cost, dynamics, path] = continuous_double_int_scaled(x, u, t) % 物理量到无量纲量的缩放 L = 100; % 位置特征值 V = 5; % 速度特征值 x1 = x(:,1) * L; % 无量纲转回物理量 x2 = x(:,2) * V; dynamics(:,1) = x2 / L; % dx1/dt 的无量纲形式 dynamics(:,2) = u / V; % dx2/dt 的无量纲形式 cost = zeros(size(t)); path = []; end

这个写法看着繁琐，但它是从根源上避免病态的。缩放做对了，IPOPT 的迭代数通常会降一半以上，而且不会在收敛边缘反复横跳。判断一个模型要不要缩放，最简单的标准是看优化变量的初始猜测值之间差了几个数量级。只要超过两个数量级，大概率需要缩放；只要是同一个量级，通常可以不动。

5. GPOPS II 避坑记录：收敛失败、数值缩放与配点间约束的踩坑

5.1 IPOPT 报 restore failed，第一步先查 sqrt 和 log

现象：求解器迭代到第几次之后突然打印 restore failed，然后 wpd 字段直接出现大量 NaN，任务中止。解决过程一度让我以为问题出在网格上，实际上问题出在动力学函数本身。

原因：我的动力学模型里有一个开平方的阻力项，sqrt 里面是速度的平方和某个常量之差。IPOPT 在试探过程中会走到物理上不合理但是数值上合法的区域，速度超过阈值导致平方根里变成负数，整个雅可比矩阵瞬间污染。GPOPS II 的配点离散不会阻止这种非法域，它只能把约束管理在配点残差层面。

解决：在动力学函数入口处把状态做饱和，然后重算导数。不是简单 clamp 掉就完事，对饱和后的表达式重新求导，保证雅可比矩阵连续。这一步做完之后，restore failed 基本消失。从此我的动力学函数所有出现除法、开方、对数的地方，一定先在函数头部加一行安全保护：如果是 sqrt(t)，就写 sqrt(max(t, 1e-6))。

5.2 目标函数收敛了，但控制曲线抖动严重

现象：目标函数稳定在一个值附近，优化提示成功收敛，但画出来的控制曲线在时间轴上呈现明显的锯齿状，尤其在切换点附近，一会儿正一会儿负，看起来很「碎」。

原因：这是网格细化不够的表现。伪谱法的特性是配点处约束严格满足，但配点之间只能靠插值曲线填补。如果该段多项式阶数偏低，而控制的最优解在段内有一次快速切换，插值曲线就会在配点之间产生波动。目标函数收敛是因为离散后的配点残差达标了，但这个达标是离散意义上的达标，不是连续意义上的达标。

解决：收紧 mesh.tolerance，强制网格细化器在切换点附近加段；或者在连续函数里给控制加上导数代价，逼迫控制轨迹变得光滑。第二个方案适合本意就不希望控制频繁抖动的场景。注意不要在调参之前先把目标函数精度调高，那只会延长计算时间而不会让控制变光滑，问题出在离散分辨率而不是优化精度。

5.3 控制曲线的每一段都平直，但切换点的位置不对

现象：Bang-Bang 控制的角度看，曲线形式完全正确，一个正段一个负段，切换干净利落，但是切换时刻跟解析解对不上，差了百分之十，而且随着初始配点数量变化，这个偏差还在变。

原因：终端时间是最优变量之一，而初值猜测里给的时间上界和状态猜测会让求解器收敛到一个局部最优解。对一个高度非凸的最小时间问题，可能真的存在不止一个局部解，但更常见的解释是初始网格太粗，导致切换点时间无法被配点位置覆盖，反正切点落在了配点之间的插值区段里，时间精度天然受限。

解决：把 mesh.tolerance 收紧之后网格细化器会在切换点附近自动加段。我当时的做法是先跑一遍粗网格，拿到大致切换时间，然后手动把初始网格的 colpoints 提高到 30 左右，并且给 guess 的时间轴上在切换点附近多插几个点。第二次算出来的切换时刻就和解析解对上了。记住一个经验：看到切换点位置不对时，先怀疑网格，再怀疑模型。

5.4 配点上满足路径约束，配点之间却越界

现象：所有配点处都满足路径约束，优化器也显示成功，但用很密的时间轴重放这条轨迹时，发现某一段状态明显越过约束边界，比如高度低于最低限制，或者距离障碍物太近。

原因：伪谱法保证的是「配点处」的约束成立，配点之间只是多项式插值。如果路径约束在配点之间的插值曲线上出现一个深度不大的波谷，就可能形成越界而求解器完全不知道。网格细化器检查的是残余动力学误差，它并不会主动检查插值曲线上的路径约束最大值。

解决：做一次事后校验，用密集时间点对最优轨迹重新插值，再逐点检查路径约束。如果越界幅度在一个小范围内，直接把原来的保守边界内缩，比如把高度下限从 5 改成 5.2，给插值曲线留出安全余量。如果越界幅度大，说明该段的插值曲线本身已经严重失真，需要提高该段多项式阶数。GPOPS II 结果里配点数量不会太多，这种事后重采样校验成本很低，值得在每次得到最优解后当标准流程来走。

6. 验证 GPOPS II 结果的硬习惯：Hamiltonian 常数校核与热启动复用

6.1 Hamiltonian 常数校核：不用白不用的伪谱法红利

GPOPS II 的一个额外收获是：它算完之后能直接给出伴随变量，伪谱法的离散结构天然把它和最优解放在同一个解向量里。对自治系统、终端固定的问题，最优解的 Hamiltonian 函数在整个时间轴上应该是一个常数。这个性质可以直接拿来当质量测试标准。

% Hamiltonian 校核伪代码 lambda = output.result.solution.phase.costate; % 字段名随版本略有出入 u = sol.control; H = lambda(:,1) .* x2 + lambda(:,2) .* u; % 对双积分器 H_std = std(H) / mean(abs(H)); if H_std < 1e-2 disp('H 常数性满足，结果可信'); else disp('H 波动偏大，检查网格或约束设置'); end

这一条的价值在于，它不是看目标函数有多少位小数，而是检验解是否满足最优性的一阶必要条件。很多隐性错误，比如成本函数写重复、某条路径约束忘记激活，都会反映为 H 的波动。我会把这一段写进每次批量处理之后的标准检查里。H 波动大的时候，先别急着继续调网格，回到模型定义里找语义问题。

6.2 热启动复用：把前一次解灌进新场景

批量场景里最省时间的技巧是热启动。同一个系统换一组终端条件，GPOPS II 的冷启动可能又要迭代几百步，但把上一次收敛的解当作 guess 传进去，迭代数常常直接砍半。做法是把上一次输出结果的结构体字段直接填到新问题的 setup.guess 里。

setup_new.guess.phase.time = sol.time; setup_new.guess.phase.state = sol.state; setup_new.guess.phase.control = sol.control;

这里要注意坐标维度和缩放要与新场景匹配。如果新场景的终端状态变了，直接从旧解灌进去可能会让初值猜测的落点离可行域更远，反而比线性插值更慢。一种更稳妥的做法是先把旧解做一次线性变形，让初末端点落在新目标状态上，再作为 guess 传入。这个小技巧在批量算一条轨迹库的时候能省下大量时间。

6.3 把 GPOPS II 当轨迹生成器，而不是在线求解器

我在做机械臂轨迹规划时拿到的最重要的教训是：不要让 GPOPS II 跑到在线环节里去。它的价值在离线阶段，把各种工况下的最优轨迹先批量算好，存成 .mat 文件，当作在线求解器的高质量初值或者监督学习的训练标签。真正在线重规划时，用一个轻量的直接打靶法加 SQP 去复现轨迹形状，比硬扛伪谱法稳定得多。

有一次我做某图像处理 Demo 的辅助定位轨迹时，跳过 Hamiltonian 校核直接把结果交给了后续环节，后来换密网格发现控制形状变了百分之二十，定位精度也随之下滑。从此我把「H 常数校核」和「事后密网格重采样」写成了每个 GPOPS II 任务交付前的固定动作。这两步加起来不过几分钟，却能把伪谱法解背后的隐含误差挡在交付之前。这套流程用顺了之后，你会越来越敢把 GPOPS II 当成生产工具用，而不是一个调着玩的黑匣子。希望帮到你。

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