GPOPS-II轨迹优化模板：从跑不通到跑得稳的工程化实践

简介：这份资源面向轨迹优化初学者与航空航天、机器人方向的研究者，围绕GPOPS-II这一基于伪谱方法的通用最优路径规划工具，提供快速上手与模板化使用支持。GPOPS-II擅长处理多阶段动力系统问题，如航天器轨道设计、再入路径规划与机器人避障路径求解，资源中的快速参考文档可帮助读者理解基函数、节点、约束与目标函数等核心概念。压缩包共191个文件，约12.74MB，以160个m脚本文件为主体，辅以png与eps结果图、pdf参考文档、txt说明及多平台mex可执行文件，覆盖算法实现、绘图输出与跨系统运行需求。目前已有1654人学习下载。读者可借助模板快速搭建优化任务，通过脚本与示例理解伪谱离散化流程，并参考结果图验证轨迹光滑度与约束满足情况，为深入研究或二次开发提供可复用的工程起点。

1. GPOPS-II 轨迹优化模板：从“跑不通”到“跑得稳”的第一道坎

如果你做的是机械臂轨迹规划、无人机航迹生成或者车辆轨迹数据处理，大概率听过 GPOPS-II 这个名字。它是一套基于 MATLAB 的最优控制求解工具箱，核心思路是把连续时间的轨迹优化问题，通过高斯伪谱法离散成非线性规划问题，再交给 IPOPT 或 SNOPT 求解。听起来很顺，但真正上手的人都知道，第一次跑官方示例之外的自己的问题，十有八九会翻车——要么报错说边界条件不匹配，要么迭代几千步不收敛，要么解出来的轨迹抖得像心电图。

这篇东西不讲教科书式的原理推导，而是围绕“GPOPS-II 轨迹模板”这个关键词，把一套能复用的工程化模板拆开讲清楚。所谓模板，不是复制粘贴就完事，而是把连续系统动力学、路径约束、事件约束、初值猜测这几个最容易出问题的环节，做成可替换、可调试的结构。适合已经装好 GPOPS-II、能跑通官方例子，但一到自己问题就卡住的从业者。读完你应该能搭出一个自己的轨迹优化模板，知道每个参数改了会发生什么，以及不收敛时先查哪里。

2. 拆解 GPOPS-II 模板的四个核心结构：动力学、约束、猜测、求解器

2.1 连续动力学函数为什么必须单独成文件

GPOPS-II 的入口是gpops函数，但真正决定问题规模的是你写的连续动力学函数。常见做法是把动力学写成一个独立的.m文件，输入是t、x、u、p，输出是dx。很多人图省事，直接把动力学写成匿名函数塞进主脚本，结果调试时连断点都打不进去。

我一般会这样组织：

function dx = dynamics_arm(t, x, u, p) % 二连杆机械臂动力学，状态为 [q1; q2; dq1; dq2] % 输入为关节力矩 [tau1; tau2] % p 为物理参数结构体：m1, m2, l1, l2, g q = x(1:2); dq = x(3:4); tau = u; % 质量矩阵 M11 = (p.m1 + p.m2) * p.l1^2 + p.m2 * p.l2^2 + 2 * p.m2 * p.l1 * p.l2 * cos(q(2)); M12 = p.m2 * p.l2^2 + p.m2 * p.l1 * p.l2 * cos(q(2)); M22 = p.m2 * p.l2^2; M = [M11, M12; M12, M22]; % 科氏力与重力项 C = [-p.m2 * p.l1 * p.l2 * sin(q(2)) * (2 * dq(1) * dq(2) + dq(2)^2); p.m2 * p.l1 * p.l2 * sin(q(2)) * dq(1)^2]; G = [(p.m1 + p.m2) * p.g * p.l1 * cos(q(1)) + p.m2 * p.g * p.l2 * cos(q(1) + q(2)); p.m2 * p.g * p.l2 * cos(q(1) + q(2))]; ddq = M \ (tau - C - G); dx = [dq; ddq]; end

逻辑说明：这个函数把状态导数显式写出来，方便在gpops报错时单独调用检查。参数p用结构体传，避免位置参数顺序写错。注意M \ (tau - C - G)比inv(M)数值上更稳，这是血泪经验。

参数说明：m1、m2是连杆质量，l1、l2是长度，g取 9.81。这些值如果量级差太多，比如一个 0.1 一个 100，求解器会很难受，建议先归一化。

2.2 路径约束和事件约束的写法差异

GPOPS-II 的约束分两类：路径约束在path函数里写，事件约束在event函数里写。路径约束是整条轨迹上都要满足的，比如关节力矩上限、末端执行器不能碰地面；事件约束是起点和终点要满足的，比如初始位置、终端速度为零。

function phaseout = path_constraint(input) % 路径约束：力矩上限和末端位置限制 tau = input.phase.control; q = input.phase.state(:, 1:2); % 力矩约束 phaseout.path = [tau(:, 1) - 50; -tau(:, 1) - 50; tau(:, 2) - 30; -tau(:, 2) - 30]; % 末端 y 方向位置不低于 0.1 l1 = 0.5; l2 = 0.4; y_end = l1 * sin(q(:, 1)) + l2 * sin(q(:, 1) + q(:, 2)); phaseout.path = [phaseout.path; 0.1 - y_end]; end

逻辑说明：phaseout.path是一个列向量，每个元素都要求小于等于零。所以写约束时统一写成表达式 <= 0的形式，不要一会儿大于一会儿小于，否则很容易搞反。

参数说明：力矩上限 50 和 30 是示例值，实际要按你的电机堵转力矩留 20% 余量。末端位置约束用0.1 - y_end，表示y_end >= 0.1。

事件约束的写法类似，但输入是input.phase.initialstate和input.phase.finalstate，输出是phaseout.event。常见错误是把事件约束写成路径约束，结果求解器在中间点也强行满足，导致无解。

2.3 初值猜测：模板里最容易被忽略的“后悔药”

GPOPS-II 对初值猜测非常敏感。官方例子通常给一个简单的线性猜测就能收敛，但你的问题如果非线性强，线性猜测基本等于自杀。我一般会在模板里留一个guess函数，用简单的比例控制或者多项式插值生成初值。

function guess = initial_guess(t, p) % 用五次多项式生成关节角度初值，速度初值取零 tf = t(end); q0 = [0; 0]; qf = [pi/2; pi/4]; s = t / tf; h = 10 * s.^3 - 15 * s.^4 + 6 * s.^5; q_guess = q0 + (qf - q0) * h'; dq_guess = zeros(size(q_guess)); guess.state = [q_guess, dq_guess]; guess.control = zeros(length(t), 2); end

逻辑说明：五次多项式保证起止速度和加速度为零，比线性猜测平滑得多。guess.state的列顺序必须和bounds里状态定义顺序一致，否则会报维度错误。

参数说明：q0和qf是起止关节角，按你的任务改。如果轨迹中间有障碍，可以在h上叠加一个正弦项绕开，但别加太多，否则初值本身就不满足动力学。

2.4 求解器选项：IPOPT 和 SNOPT 怎么选

GPOPS-II 支持 IPOPT 和 SNOPT 两种 NLP 求解器。IPOPT 开源、免费、对初值宽容一些，但速度慢；SNOPT 快、精度高，但对初值敏感，且需要许可证。我一般先用 IPOPT 调通，再换 SNOPT 加速。

setup.name = 'arm_trajectory'; setup.functions.continuous = @dynamics_arm; setup.functions.phaseout = @path_constraint; setup.functions.initialguess = @initial_guess; setup.nlp.solver = 'ipopt'; setup.nlp.ipoptoptions.maxiterations = 2000; setup.nlp.ipoptoptions.tolerance = 1e-6; setup.derivatives.supplier = 'sparseCD'; setup.mesh.tolerance = 1e-4; setup.mesh.maxiterations = 10; output = gpops2(setup);

逻辑说明：sparseCD是稀疏中心差分，比默认的sparseFD精度高，但计算量稍大。mesh.tolerance控制网格自适应迭代的收敛阈值，设得太小会一直细化网格，设得太大解不精确。

参数说明：maxiterations2000 是 IPOPT 的上限，如果经常撞到这个数，说明初值或约束有问题。tolerance1e-6 是 NLP 收敛容差，一般够用。mesh.maxiterations10 表示最多细化 10 次，超过还不收敛就停。

3. 从零搭一个 GPOPS-II 轨迹模板：文件结构与调试顺序

3.1 模板目录怎么放才能一眼看懂

我习惯把模板拆成五个文件，放在同一个目录下：

这样拆的好处是，当gpops2报错时，你可以单独调用dynamics_xxx检查导数是否合理，单独调用guess_xxx看初值是否满足边界。很多人把所有东西塞进一个脚本，报错后只能一行行注释，效率极低。

3.2 边界条件设置：bounds里的坑比想象多

bounds结构体是 GPOPS-II 最容易写错的地方。它分phase.initialtime、phase.finaltime、phase.initialstate、phase.finalstate、phase.control、phase.path等字段。每个字段都是两列，第一列是下界，第二列是上界。

bounds.phase.initialtime.lower = 0; bounds.phase.initialtime.upper = 0; bounds.phase.finaltime.lower = 1; bounds.phase.finaltime.upper = 3; bounds.phase.initialstate.lower = [0, 0, 0, 0]; bounds.phase.initialstate.upper = [0, 0, 0, 0]; bounds.phase.finalstate.lower = [pi/2, pi/4, 0, 0]; bounds.phase.finalstate.upper = [pi/2, pi/4, 0, 0]; bounds.phase.control.lower = [-50, -30]; bounds.phase.control.upper = [50, 30]; bounds.phase.path.lower = [-inf, -inf, -inf, -inf, -inf]; bounds.phase.path.upper = [0, 0, 0, 0, 0];

逻辑说明：初始状态上下界相等表示固定，如果想让某个状态自由，把上下界设成-inf和inf。路径约束的上下界要和path_xxx函数输出的维度一致，否则会报path dimension mismatch。

参数说明：finaltime给一个范围而不是固定值，让求解器自己找最优时间。如果任务要求固定时间，上下界设成相等即可。控制上下界就是力矩限幅，别设太大，否则解出来的力矩虽然数学上最优，但物理上电机根本给不出来。

3.3 调试顺序：先让动力学跑通，再谈优化

我见过太多人一上来就调gpops2，报错后完全不知道从哪查。正确的顺序是：

第一步，单独调用dynamics_xxx，给一组合理的x和u，看dx是不是有限值。如果出现NaN或Inf，说明动力学里有除零或开方负数。

第二步，单独调用guess_xxx，把guess.state和guess.control画出来，看是否满足边界。如果初值本身就不满足边界，求解器第一步就会卡住。

第三步，把setup.mesh.maxiterations设成 1，只跑一次网格，看 NLP 是否收敛。如果不收敛，把setup.nlp.ipoptoptions.maxiterations设成 100，看迭代过程卡在哪。

第四步，逐步放开mesh.maxiterations，观察解的变化。如果每次细化后解都剧烈变化，说明网格自适应有问题，可以手动加密网格。

% 手动加密网格示例 setup.mesh.phase.fraction = [1/5, 1/5, 1/5, 1/5, 1/5]; setup.mesh.phase.colpoints = [4, 4, 4, 4, 4];

逻辑说明：fraction是每段网格占总时间的比例，colpoints是每段的配点数。配点数越多，离散精度越高，但 NLP 规模也越大。一般 4 到 6 个配点够用。

参数说明：如果轨迹变化剧烈，比如有快速启停，把对应时间段的fraction调大，或者增加colpoints。但别一上来就全局加配点，先试 4 个，不够再加。

4. GPOPS-II 轨迹模板避坑：五个让求解器翻车的常见操作

4.1 现象：报错Error using gpops2 (line xxx) Path constraint dimension mismatch

原因：path_xxx函数输出的phaseout.path列数和bounds.phase.path的上下界列数不一致。常见于你改了路径约束数量，但忘了同步改bounds。

解决：在path_xxx函数里加一句size(phaseout.path)打印，和bounds.phase.path.lower的长度对一下。建议把路径约束数量写成一个变量，在main.m和path_xxx里共用。

4.2 现象：求解器迭代几千步不收敛，exitflag为负

原因：初值猜测太差，或者约束之间互相矛盾。比如你要求末端位置精确到某个点，同时又要求末端速度为零，但动力学根本做不到。

解决：先把事件约束放宽，比如末端位置允许 ±0.01 误差，看是否能收敛。如果能，再逐步收紧。另外检查初值猜测是否满足动力学，可以用ode45从初值积分一遍，看轨迹是否合理。

4.3 现象：解出来的控制量抖得厉害，像方波

原因：网格配点太少，或者路径约束在控制量上加了硬限幅，导致解在边界上反复切换。

解决：增加colpoints到 6 或 8，或者在控制量上加入平滑惩罚项。GPOPS-II 支持在setup.functions.continuous里返回phaseout.integrand，可以把控制量的平方积分进去，让解更平滑。

phaseout.integrand = 0.01 * (u(:,1).^2 + u(:,2).^2);

逻辑说明：这个积分项会加到目标函数里，权重 0.01 是我试出来的经验值，太大轨迹会变慢，太小起不到平滑作用。

参数说明：权重按你的控制量量级调。如果力矩是几十牛米，0.01 合适；如果是几百，用 0.001。

4.4 现象：gpops2跑了一半 MATLAB 卡死，内存爆了

原因：网格细化次数太多，NLP 变量数指数增长。或者动力学函数里有大矩阵求逆，每次调用都分配新内存。

解决：限制mesh.maxiterations不超过 10，colpoints不超过 8。动力学里预分配矩阵，避免在函数内动态扩容。如果还爆，把setup.derivatives.supplier从sparseCD改成sparseFD，牺牲一点精度换内存。

4.5 现象：换了一组物理参数后，之前能收敛的模板突然不收敛了

原因：参数量级变化导致 NLP 条件数变差。比如质量从 1 变成 1000，力矩约束还是 ±50，相当于把可行域压扁了。

解决：把所有参数归一化到 1 附近。质量除以最大质量，长度除以最大长度，时间除以总时间。归一化后重新调约束上下界。这是最容易被忽略但最有效的一招。

5. 用 GPOPS-II 做机械臂轨迹规划的进阶技巧：从能跑到好用

5.1 把绝对轨迹误差（ATE）作为评价指标嵌进模板

轨迹优化跑通只是第一步，解出来的轨迹好不好用，得有个量化指标。做机械臂的时候，我习惯在模板里加一个后处理函数，计算实际轨迹和期望轨迹的绝对轨迹误差（ATE）。具体做法是：用ode45把优化得到的控制量代回动力学，积分出实际状态，再和优化解的状态逐点做差。

function ate = compute_ate(output, dynamics, p) % output 是 gpops2 的返回结构 % 取优化解的时间和状态 t_opt = output.result.solution.phase.time; x_opt = output.result.solution.phase.state; u_opt = output.result.solution.phase.control; % 用 ode45 重新积分 [t_sim, x_sim] = ode45(@(t,x) dynamics(t, x, interp1(t_opt, u_opt, t, 'linear', 'extrap'), p), ... t_opt, x_opt(1,:)'); % 计算位置部分的 ATE pos_error = x_sim(:, 1:2) - x_opt(:, 1:2); ate = mean(sqrt(sum(pos_error.^2, 2))); end

逻辑说明：interp1把优化得到的离散控制量插值成连续函数，ode45重新积分。如果 ATE 很大，说明优化解对动力学不满足，可能是配点太少或者约束太松。

参数说明：interp1的'linear'可以换成'spline'，但样条插值可能引入过冲，导致积分不稳定。我一般先用线性，不够再换。

5.2 用热启动加速相似任务

如果你要反复求解结构相同、只是起止点不同的轨迹，可以用上一次的解作为下一次的初值猜测。这就是热启动。GPOPS-II 的output.result.solution里存了网格和状态，可以直接喂给下一次的setup.guess。

% 第一次求解 output1 = gpops2(setup); % 第二次求解，用第一次的解作为初值 setup.guess = output1.result.solution; output2 = gpops2(setup);

逻辑说明：热启动能大幅减少迭代次数，尤其是任务相似的时候。但要注意，如果两次任务的边界条件差太多，热启动反而会拖慢收敛，甚至导致不收敛。

参数说明：判断能不能热启动，看两次任务的起止状态差是否小于总行程的 20%。超过这个数，老老实实用多项式初值。

5.3 一个我踩过的坑：别在动力学里做插值

早期为了省事，我在动力学函数里直接对实验数据做插值，结果每次求解器调用动力学都要查表，速度慢不说，还因为插值不光滑导致导数不连续，求解器直接罢工。后来改成先把实验数据拟合成分段多项式，把系数存成参数传进去，问题才解决。

教训就是：动力学函数里只做加减乘除和三角函数，任何查表、插值、文件读取都放到外面。GPOPS-II 会频繁调用动力学，里面每多一个操作，求解时间就翻倍。

希望帮到你。

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