☰
GPOPS4.1轨迹优化实战:从伪谱法原理到MATLAB实现与排坑
2026/10/14 2:26:34 网站建设 项目流程

简介:GPOPS4.1是一款面向科研工作者的MATLAB轨迹优化工具箱,以庞特里亚金最大值原理为核心算法,用于求解航天、航空、机器人导航等领域的大规模非线性最优控制与弹道优化问题。工具内置丰富示例和详细文档,支持自定义成本函数与约束条件,无需依赖SNOP等额外软件即可独立运行。资源包共291个文件,主体为250个m格式源码,另有20个pdf说明文档、tex源文件、mex编译文件、安装及授权说明、readme等,压缩后仅3.22MB,目录结构清晰,便于快速配置与部署。已有459人学习下载。通过这套资源,可获取完整的工具箱源码、典型轨迹优化示例、底层算法实现及配套说明文档,既能直接复现经典案例,也能在此基础上开展二次开发,满足科研与工程实践中的高维优化需求。

1. gpops4.1轨迹优化软件:先看懂它到底帮你解了哪道题

做轨迹优化的人最熟悉的场景是:动力学方程能写出来,约束也列得清,目标函数就是从A飞到B燃料最少或时间最短,但真去算的时候往往卡在“怎么把最优控制问题变成一个能迭代的数值问题”。传统打靶法猜协态变量,猜错一个初值就发散,纯属玄学;自己写直接配点法又要处理网格加密、NLP求解器接口、雅可比稀疏结构,工程量不比写论文小。gpops4.1就是把这个门槛打包成一个MATLAB工具箱:你只需要提供状态方程、路径约束、边界条件和目标函数,它用Radau伪谱法把微分方程配点离散化成非线性规划,再交给IPOPT或SNOPT去解,最后返回最优状态、控制和网格信息。这套软件适合所有被“运动规划”卡住的从业者:无人机航迹规划、机械臂时间最优、运载器入轨或着陆、车辆避障都适用。下面按我实际拆解的路径,从原理写到排坑。

2. 伪谱法与接口约定:为什么Radau配点是默认答案

2.1 从间接法到直接配点:三种思路的取舍

最优控制数值解法大体分间接法和直接法。间接法把庞特里亚金极值条件列成两点边值问题,求解未知量是协态变量和状态变量,精度高,但初值极其敏感。我最早用间接法做机械臂时间最优,光猜初始协态就耗了两个星期,某导师的原话是“猜协态比猜彩票还难”。原因是协态变量没有物理意义,只能靠经验试。

直接法放弃解析推导必要条件,直接把状态和控制参数化,把原问题转成带约束的非线性规划(NLP)。直接法里又分两种主流:多重打靶和直接配点。多重打靶把时间轴分成多段,每段独立积分,然后施加连续性条件;实现较复杂,但网格和变量规模可控。直接配点法则把状态和控制同时离散在节点上,用多项式近似状态,再让多项式在配置点处满足动力学,这样雅可比结构更规整,适合交给通用NLP求解器。

伪谱法是直接配点的一个特殊分支:它用全局正交多项式插值状态,配点取高斯型节点。GPOPS系列采用Radau伪谱法,节点取自Radau求积公式,特点是包含区间右端点。这个端点属性对工程问题很重要——你往往需要精确满足末端边界条件,比如软着陆的终端速度为零,右端点如果被包含在节点里,约束就不会被插值误差抹掉。此外,hp自适应策略会自动判断“是增加段数还是增加每段阶数”,相当于把网格加密从手工调参变成了求解器自动决策,这是老式固定节点法最大的优势。

选型结论很明确:如果没有特殊原因,直接法伪谱配上自动网格加密,是当前工程上最低门槛、最不容易翻车的默认路径。手写代码做科研演示可以选简单欧拉配点,但要快速得到可信结果,用成型工具更稳。

2.2 数学要素:代价函数、动力学、路径约束与边界条件的接口约定

GPOPS求解的对象是Bolza型最优控制问题,通用形式为:

  • 代价:J = M(x(t0), t0, x(tf), tf) + ∫ L(x(t), u(t), t) dt

前半部分叫Mayer项,对应末端代价,比如燃料最优直接取终点质量;后半部分叫Lagrange项,对应过程代价,比如能耗积分。GPOPS把这两项拆开放进不同接口里:连续动力学函数里写积分被积函数 output.integrand,端点函数里写末端代价 output.objective。这个拆分是为了配合打靶法的精度控制——积分项在网格内做高斯求积,端点项则依赖边界节点。

动力学约束统一写成状态导数形式:\dot{x}(t) = f(x(t), u(t), t),用户在 continuous 函数里通过 output.dynamics 返回状态导数矩阵,行数等于配点数量,列数等于状态维度。路径约束写成 g_L ≤ g(x,u,t) ≤ g_U,在 continuous 函数里用 output.path 返回,上下界在bounds结构里配置。事件边界条件(初始位置、末端速度、质量限制等)在 endpoint 函数里用 output.eventgroup(k).event 返回一组标量方程,GPOPS会把它们作为等式或不等式约束交给NLP。所有变量界限,包括状态上下限、控制上下限、初始和末端时间界限,统一放在 bounds 结构体里。

这个接口约定的核心设计是“函数分开写、边界集中管”。工程上常见错误是:把初始状态当普通状态界限写进 bounds.phase.state.lower 和 upper,结果末端状态也被同一界限束缚,解被限制得没法动。正确做法是初始和末端条件进 eventgroup,状态上下限只负责给求解器提供搜索空间。

2.3 setup结构体:决定求解器行为的十几个关键字段

GPOPS的主入口是 gpops4(setup),所有控制参数都通过一个结构体传递。初次接触的人最容易照抄示例demo,不理解字段含义,一旦换问题就不会改。最关键的字段我用表列出来:

字段作用常见取值
setup.name给问题起名,会出现在输出文件里任意字符串
setup.functions.continuous动力学、路径约束、被积函数的函数句柄@myContinuous
setup.functions.endpoint端点事件和目标函数的函数句柄@myEndpoint
setup.bounds各变量界限的结构体见模块bounds
setup.guess初值猜测,包含时间、状态、控制guess.phases(1).state
setup.nlp.solver底层NLP求解器'ipopt' 或 'snopt'
setup.mesh.method网格细化方法'hp'
setup.mesh.tolerance网格细化收敛误差1e-4 ~ 1e-7
setup.mesh.phase.fraction各段占总时长比例[1] 或分段 [0.4, 0.6]
setup.mesh.phase.colpoints每段配点个数6~10
setup.derivatives.supplier导数来源'sparseCD' 或 'analytic'
setup.derivatives.dependencies雅可比依赖方式'full' 或 'sparse'
setup.scales.method变量缩放方式'automatic' 或 'none'

mesh.method 后面的 'hp' 代表同时允许段数和每段阶数变化;有些版本里还会写成 'hp-Radau',含义相同。setup.derivatives.supplier 选 'sparseCD' 时用复步有限差分,精度比普通有限差分高非常多,几乎不会出现数值噪声导致收敛停顿的问题,代价是计算量略增。setup.scales.method 我一般默认开 'automatic',它会在求解前自动分析状态和控制量级生成缩放因子,能帮你解决一大半初始化崩溃问题,后面避坑章会详细展开。

提示:setup 里的字段名在不同版本间略有变化,比如 colpoints 旧版本叫 collocationPoints。拿到新版本后先翻自带demo的setup部分,以 demo 里的实际字段为准。

3. 安装与配置:拿到 gpops4.1 到跑通第一个示例

3.1 目录结构和MATLAB路径:五个核心目录分别干什么

解压后的包一般包含核心源码目录、驱动工具目录、示例目录、求解器接口目录和文档目录。常见结构是:核心源码目录放伪谱离散化和网格细化的实现,路径必须加进MATLAB路径;示例目录放十来个官方demo,从一维小车到复杂航天器都有,建议先跑一遍确认环境没问题;求解器接口目录放IPOPT或SNOPT的mex封装;文档目录含PDF手册和版本说明。有些第三方整合包会自带一个 start 或 setup 脚本,打开后显示“添加路径完成”之类输出。最朴素的配置方式是:

addpath(genpath('D:/tools/gpops4.1')); savepath;

genpath 会把目标文件夹下所有子目录递归加载,保证核心源码里的私有函数也能被找到。savepath 的作用是把当前路径保存到MATLAB的pathdef.m,下次启动不用重复加。如果机器上有多个MATLAB版本并存,注意 savepath 只对当前版本生效。添加路径之后立刻验证:

which gpops4

如果返回了完整路径说明加载成功,如果返回“未找到”就要检查解压目录是不是包含中文名或空格,MATLAB对这两者兼容性很差。

3.2 依赖检查:IPOPT/SNOPT、mex文件与六大常见错误

GPOPS本体只是做离散化,真正求解NLP需要外部求解器。IPOPT是开源免费求解器,适合大部分场景;SNOPT是商业软件,在大规模问题上通常更稳,但需要授权文件。拿到资源包后先看求解器接口目录里是否已经编译好的mex文件,不同操作系统的mex后缀不同:Windows是mexw64,Linux是mexa64,macOS是mexmaci64。

检查IPOPT能否被MATLAB识别:

# 在MATLAB命令窗口执行 which ipopt getIpoptOptions # 部分版本提供该函数

如果提示找不到,最常见原因是IPOPT的动态库没有加入系统PATH。我的处理流程是:先把IPOPT解压到固定目录,例如 D:/solvers/ipopt,然后在MATLAB里设置环境变量:

setenv('PATH', ['D:/solvers/ipopt/bin;' getenv('PATH')]);

注意必须在启动MATLAB之前设置系统PATH,之后设置对已启动的会话不一定生效。如果mex文件是源码自带的,通常编译好的版本对应某个固定MATLAB大版本,跨版本经常出现“无法加载动态库”的报错。解决办法只有两条路径:一是安装匹配版本的MATLAB,二是用自带源码重新编译mex。编译IPOPT的mex对新手不太友好,需要装MinGW-w64编译器和对应依赖库,所以我更推荐优先找编译好的匹配版本。

3.3 冒烟测试:跑通自带示例的三个判断标准

配置完成后别急着上手自己的模型,先跑一个自带demo做冒烟测试。一般选最简单的动力学示例,运行之后看三个指标。第一是是否正常迭代并退出,如果IPOPT显示迭代步数并且最终返回“Optimal Solution Found”或类似消息,说明NLP问题求解成功。第二是网格细化是否收敛,GPOPS会自动执行多轮网格细化,每一轮结束时显示最大配点误差,如果误差最终低于setup.mesh.tolerance,输出里会有“Mesh Refinement Converged”的标志。第三是结果是否有意义:把状态和控制画出来,目视检查有没有超界、跳变或明显的数值振荡。

% 假设demo名是 demoBasic demoBasic;

跑通这三个标准就说明核心依赖、路径、NLP求解器接口全部正常。如果在第一轮迭代就崩,先关掉自动缩放、减小配点数量、降低网格容差到1e-3,逐项隔离问题。我遇到过一种情况:IPOPT报“Invalid number in NLP function or derivative”,概率是连续函数里返回了NaN,后面有专门一节讲排查。冒烟测试通过之后,才建议开始改边界条件、换动力学模型。

4. 从零写一个轨迹优化案例:一维垂直着陆的燃料最优问题

4.1 问题定义与无量纲化:为什么轨迹优化要先“去量纲”

用一个最经典但代码量最小的案例讲全程:一维垂直着陆器从高空减速到地面,目标是最小化燃料消耗,也就是最大化终端质量。状态取高度h、速度v、质量m,控制取推力T,动力学方程如下:

  • 高度导数:\dot{h} = v
  • 速度导数:\dot{v} = T/m - g
  • 质量导数:\dot{m} = -k·T

其中 k 反映比冲和重力加速度的组合关系。轨迹优化里最坑的不是方程写不出来,而是量纲问题:高度数量级可能是1000,推力可能是几万,质量可能是几十万,数量级相差10^4以上,NLP求解器在标量梯度面前直接瘫痪。常见做法是全部无量纲化。我一般取初始质量m0、重力加速度g、参考高度h_ref作为基本量,时间尺度取sqrt(h_ref/g),推力尺度取m0·g。无量纲化之后,状态、控制都落在0.1到10这个范围,求解器收敛会快一个量级。

4.2 写连续系统函数 continuous.m:状态导数和路径约束怎么写

连续系统函数接收输入结构体,内部从 input.phase 里取出时间、状态、控制矩阵,然后计算动力学、路径约束和积分项。核心代码:

function output = landingContinuous(input) % 取配点上的时间、状态、控制 t = input.phase.time; x = input.phase.state; u = input.phase.control; % 状态分解:h, v, m h = x(:,1); v = x(:,2); m = x(:,3); % 无量纲参数:g为1,比冲系数k为1 g = 1; k = 1; % 动力学方程,每行对应一个配点 dh = v; dv = u ./ m - g; dm = -k .* u; % 返回动力学状态导数 output.dynamics = [dh, dv, dm]; % 路径约束:推力不超过上限,这里不需要额外路径约束 output.path = []; % 留空表示无路径约束 % 无过程代价,积分为空 output.integrand = []; end

代码里u ./ m是逐元素除法,因为控制矩阵u和质量矩阵m都是列向量,数量等于配点数。这里有个容易翻车的细节:如果某次迭代质量 m 的数值逼近0,u ./ m会产生Inf,直接导致整个雅可比矩阵报废。所以在变量界限里一定要把质量下限设为一个正数,比如0.2,绝不允许它到0附近。output.path留空代表没有普通路径约束,如果有着陆段高度下限要求,比如 h ≥ 0.05,就应该在这里返回output.path = h - 0.05;并在 bounds 里给上下界。

4.3 端点函数 events.m:边界条件与代价函数耦合

端点函数处理所有只跟起止时刻相关的量。事件组返回的是需要被满足的方程,目标函数返回的是Bolza问题里的Mayer项:

function output = landingEndpoint(input) % 初末状态 t0 = input.phase.initialtime; tf = input.phase.finaltime; x0 = input.phase.initialstate; xf = input.phase.finalstate; % 初始状态边界:h0=1, v0=0, m0=1 eventInit = [x0(:,1) - 1; % 初始高度为1 x0(:,2) - 0; % 初始速度为0 x0(:,3) - 1]; % 初始质量为1 % 末端状态边界:高度为0,速度为0 eventFinal = [xf(:,1) - 0; % 终端高度为0 xf(:,2) - 0]; % 终端速度为0 % 打包所有事件方程 output.eventgroup(1).event = [eventInit; eventFinal]; % 目标函数:最大化终端质量,即最小化 -m_f output.objective = -xf(:,3); % 无积分代价 output.integrand = []; end

这里的逻辑是:GPOPS要求事件方程最终等于零或者落在bounds指定区间。我把初始条件写成“实际值减给定值”,如果实际初始高度是1,事件值为0,落在下界0上界0的区间内,约束满足。目标函数写成-xf(:,3),因为求解器默认最小化,取负号就把“最大化终端质量”变成最小化负质量。这个负号非常容易漏,漏掉之后软件会拼命把质量往小了算,解出来是燃料“最多”而不是最少。

4.4 主脚本setup:mesh、变量界限、初值与求解调用

主脚本负责把上述函数和所有参数组织成setup结构体:

clear; clc; % 定义初始猜测:时间0到1,状态线性变化 guess.phases(1).time = [0; 1]; guess.phases(1).state = [1, 0, 1; % 起点:h=1, v=0, m=1 0, 0, 0.6]; % 终点:h=0, v=0, m=0.6 guess.phases(1).control = [0.5; 0.5]; % 变量界限 bounds.phase.initialtime.lower = 0; bounds.phase.initialtime.upper = 0; bounds.phase.finaltime.lower = 0.5; bounds.phase.finaltime.upper = 5; bounds.phase.state.lower = [-inf, -inf, 0.2]; bounds.phase.state.upper = [inf, inf, 1]; bounds.phase.control.lower = [0]; bounds.phase.control.upper = [2.5]; % 事件约束:初始和末端条件都是等式 bounds.eventgroup(1).lower = zeros(5,1); bounds.eventgroup(1).upper = zeros(5,1); % setup结构体 setup.name = 'landing_fuel_optimal'; setup.functions.continuous = @landingContinuous; setup.functions.endpoint = @landingEndpoint; setup.bounds = bounds; setup.guess = guess; % 求解器与网格 setup.nlp.solver = 'ipopt'; setup.mesh.method = 'hp'; setup.mesh.tolerance = 1e-6; setup.mesh.phase.fraction = [1]; setup.mesh.phase.colpoints = [6]; setup.derivatives.supplier = 'sparseCD'; setup.derivatives.dependencies = 'full'; setup.scales.method = 'automatic'; % 求解 output = gpops4(setup); solution = output.result.solution; % 提取结果画图 t = solution.phase.time; h = solution.phase.state(:,1); v = solution.phase.state(:,2); m = solution.phase.state(:,3); u = solution.phase.control; plot(t, [h, v, m, u], 'LineWidth', 1.5); legend({'h', 'v', 'm', 'T'});

这个脚本里最值得解释的是bounds.eventgroup(1).lower = zeros(5,1)那一句。事件方程一共5个(3个初始 + 2个末端),所以上下界必须是5维向量。如果这里写成漏掉一个维度,GPOPS会在预处理阶段报维度不匹配,这个我踩过一次,卡了半小时才发现。setup.mesh.phase.fraction = [1]表示初始网格只有一段,占比100%;colpoints = [6]表示该段有6个配点。等求解完第一轮,hp自适应会自动决定是否加密。

初值猜测要把物理可行的大致趋势给出来:高度从1降到0,速度从0到0,质量从1降到某个非零值。即使猜得粗糙,只要量级对,自动缩放加上IPOPT都能拉回来。如果猜得太离谱,比如质量初始给100,自动缩放会生成错误的缩放因子,解出来的轨迹可能完全反直觉。

5. 避坑与常见问题排查:求解器翻车时先查这八处

5.1 NAN与Inf:病态无量纲化是最大元凶

现象:第一次迭代后输出窗口全是NaN,或者IPOPT报 “Invalid number in NLP function or derivative”。

原因:绝大多数情况是动力学函数里有分母、开方或对数运算,中间量越界。我见过最典型的是质量作为分母,迭代过程中质量被NLP求解器试探到接近0,产生Inf,雅可比矩阵被污染后就再也回不来。

解决:先检查动力学函数中所有可能出现除法的表达式,确认分母有正数下界;然后在bounds里给相关状态设定远离奇点的硬下限;再打开自动缩放 setup.scales.method = 'automatic'。这三步做完,NAN出现的概率会降到极低。如果还出现,再辅助排查路径约束有没有返回空数组但bounds里却声明了上下界,这种不一致也会导致内部函数传参错乱。

5.2 mesh refinement无限循环:误差单调下降才正常

现象:求解器每轮都提示“Mesh Refinement”并重新计算,但最大误差始终卡在某个值,网格却越切越细,总配点数从几十涨到几百,计算时间爆炸。

原因:要么是问题本身存在控制切换点,比如推力从最大突跳到最小,这种非光滑控制需要网格把切换点当成段边界;要么是容差设得太苛刻,比如1e-8,但NLP求解器用的终止容差只有1e-6,两边的误差标准打架,网格永远追不上。

解决:先看误差分布。如果误差集中在某个时间段,大概率是切换点问题,这时候不要依赖自动网格,手动把 time 分成两段或三段,设置 setup.mesh.phase.fraction = [0.4, 0.6],并在切换点附近把 colpoints 调高,比如 [10, 10]。如果误差分布均匀,就把 setup.mesh.tolerance 从1e-6放宽到5e-5,先拿到一个可用解再逐步收紧。整体原则是:网格误差要单调下降才正常,连续两轮误差不降反升,就要停下来改用人工分段。

5.3 初值“改不动”结果:scale才是幕后黑手

现象:把guess里的初始速度从0改成1,或者把初始高度从1改成10,重新求解后最优轨迹几乎不变,像初值没有生效一样。

原因:这不是初值被忽略,而是自动缩放机制接管了数值尺度。特别是 setup.scales.method 设为 automatic 时,求解器会基于guess统计状态范围,再把问题变换到约[-1,1]的尺度。如果guess里的量级跟自动缩放统计值相差太大,实际送入NLP的初值已经被缩放扳回同一个点。另外NLP求解器本身的机制也会把初值“拉”到可行域附近,初值的影响力有限。

解决:这一步要分清目标。初值的作用不是微调结果,而是让求解器落入期望的吸引域。如果两个不同的guess得到同一解,说明该解是强吸引子,通常可信;但如果怀疑存在多个局部解,就刻意把guess改成差距非常大的形态,比如从“先下降再上升”改成“一直上升再急降”,看是否收敛到另一个解。同时关闭自动缩放,改用自己设定的缩放参数,能更直观地观察初值影响。查证方法是在求解前输出 setup.scales 的生成值,确认状态和控制的scale是否与预期一致。

5.4 边界条件不匹配报错:事件函数维度必须等于bounds维度

现象:报错信息类似 “Number of event functions does not match bounds” 或者 “Index exceeds matrix dimensions”。

原因:endpoint 函数里 eventgroup 返回的事件向量长度,跟 bounds.eventgroup(k).lower / upper 的长度不一致。事件方程个数少一个,bounds里多一个;或者事件向量在某个配点上返回的是矩阵,被拉成了包含大量元素的超长向量。另一种常见来源是:初始条件和末端条件里的状态顺序写反,把 x0 和 xf 用混了,维度虽然一致但语义错误,这种报错更隐蔽。

解决:写一段调试代码,在调用求解器前手动传入一个假的输入结构体,直接调用 endpoint 函数:

% 构造假输入 input.phase.initialtime = 0; input.phase.finaltime = 1; input.phase.initialstate = [1, 0, 1]; input.phase.finalstate = [0, 0, 0.6]; out = landingEndpoint(input); disp(size(out.eventgroup(1).event)); % 确认向量长度 disp(size(bounds.eventgroup(1).lower)); % 对比bounds长度

这个习惯帮我避开了无数次低级错误。所有自动生成的事件函数一定要先单独调一次,确认返回维度后再放进去求解。连续函数也同样处理,输入单个时间点的状态控制,确认输出是1行3列而不是反成3行1列。

5.5 求解成功但结果明显不对:目标函数符号与约束方向

现象:NLP求解器正常收敛,但画出来的轨迹高度往下飞到负值,或者质量在增加,完全违背物理直觉。

原因:目标函数负号写反是头号原因。比如要把末端质量最大化,代码写成output.objective = xf(:,3),求解器自然去最小化终端质量,把所有燃料烧完只剩0.2。第二个原因是状态界限写反,把质量下界写成1上界写成0,NLP直接告诉你不可行但也会返回某个人造可行点。

解决:每次拿到新问题,先用一个极端简单的初值测试目标函数方向:把控制全设0,看看目标值变化是否符合预期。如果最小化问题时目标值应当比初始点小,结果反而变大,说明符号反了。再检查约束方向:对等式约束,事件函数上下界都设0;对不等式约束,要确认上下界顺序没有被交换。这两个检查最多花费五分钟,能省掉后面两小时的迷茫时间。

5.6 局部解误当全局解:换初值交叉验证

现象:某次求解得到一条看起来很漂亮的轨迹,目标值也在合理范围,但换一个初值再解,得到的目标值明显更小,原解是局部最优。

原因:NLP求解器都是找局部解,伪谱法无法保证全局最优。轨迹优化问题里,碰到有障碍物或多模态问题的场景,局部解极其常见。

解决:养成至少用三个初值交叉验证的习惯。三个初值的形态差异要大:常值控制、线性插值控制、正弦波控制。每个初值解完之后对比目标函数值和终端状态的约束违反量。如果三个初值都收敛到同一个目标值,大概率可以认为是比较可靠的解;如果目标值不同,取最优那个再细查约束。这个交叉验证流程我已经写进了固定的提交流程,血泪经验,不值得再栽第二次。

6. 用间接法验证伪谱法结果:对最优性做一次交叉检验

伪谱法输出的结果虽然满足NLP的KKT条件,但NLP的收敛点未必满足原连续时间问题的最优性条件。我习惯在拿到解之后加一道“间接法检验”,用庞特里亚金必要条件反向验证。构造哈密顿函数 H = λ^T f,其中 λ 是伪谱法输出的costate,f 是动力学。最优控制应当满足 H 对 u 的梯度接近零。

对前面的垂直着陆案例,哈密顿函数 H = λ1·v + λ2·(u/m - g) + λ3·(-k·u)。对控制求导得到最优条件 dH/du = λ2/m - k·λ3 = 0。GPOPS求解器在 solution 结构里提供了 costate,可以直接提取:

lambda = solution.phase.costate; t = solution.phase.time; u = solution.phase.control; m = solution.phase.state(:,3); % 无量纲参数 k=1 dHdu = lambda(:,2) ./ m - 1 * lambda(:,3); % 数值上检查最优条件 max_abs_dHdu = max(abs(dHdu)); disp(['max dH/du = ', num2str(max_abs_dHdu)]);

如果 max_abs_dHdu 的数值量级在1e-3以下,说明控制量确实是当前costate下的最优选择,解的可信度大增。如果这个值很大,例如超过0.1,则说明网格不够密,或者求解器收敛到了一个伪最优解,有必要加密网格重解。除了控制最优性,还要检查横截条件:对自由终端时间问题,哈密顿函数在终点应满足 H(tf) + ∂M/∂t ≈ 0。这个检验同样可以用costate和时间输出完成。

这套交叉检验的做法来自我拆解某次模拟项目时的经验。当时做一个机械臂时间最优轨迹,伪谱法解出的控制律在物理仿真器里跑出明显抖动,但NLP报告收敛。后来补做costate检验,发现 dH/du 量级高达0.5,网格误差也超标。强制把切换点手动分段后重解,抖动消失,检验值降到1e-4。从那以后我每次跑完轨迹优化都强制走一遍 costate 检验,如果某个结果连 dH/du 都过不了,不管画面多好看我都不敢直接用。轨迹优化这门手艺,一半在建模一半在验证,希望帮到你。

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

需要专业的网站建设服务?

联系我们获取免费的网站建设咨询和方案报价,让我们帮助您实现业务目标

立即咨询