简介一套基于hp-自适应伪谱法的系绳系统最优控制源码面向从事航天器、无人机或风筝等系绳系统轨迹优化研究的工程师与高年级研究生主要解决强非线性和复杂动态约束下的最优控制问题。代码完整覆盖二阶动力学模型、初值终值设定、边界与路径约束以及目标函数定义共3个M文件分别对应主控制流程、微分代数方程DAE处理和代价函数计算三者协同完成hp自适应网格细化与伪谱离散求解整个RAR包仅4KB体量小巧、结构清晰。已有704人学习下载适合具备最优控制或伪谱法基础、希望深入理解自适应伪谱法的中高级MATLAB开发者。借助这份代码可以快速理解hp自适应伪谱法如何自动调整基函数阶数与节点密度掌握系绳系统轨迹规划与参数优化的核心思路并迁移至其他非线性动态优化任务。1. 绳系最优控制为什么要用自适应伪谱法先看一个让打靶法崩掉的展开问题想象一个真实的绳系卫星任务绳系从舱体向外释放绳长要从几十米一路拉到几公里摆角在引力梯度和科氏力的耦合下来回震荡张力还被限制在材料安全范围之内。建立最优控制问题之后你会发现不光滑点无处不在约束切换、张力松弛、末端阻尼制动每一处都会让基于固定网格的数值方法失效。传统打靶法需要先猜一整条控制曲线在绳系这种强非线性模型里初值几乎给不准第一步迭代就翻车是常态间接法要解两点边值问题横截条件初猜对了才勉强能跑tether_optimal_hp这类做法的核心思路完全不同把时间轴切成若干区间每个区间用插值多项式逼近状态和控制将最优控制问题整体离散成一个非线性规划NLP再让网格宽度和多项式阶数同时自适应。它解决的问题非常具体——在张力约束、摆角约束和控制饱和都存在的前提下找到一条能落地的绳长控制曲线。适合正在做绳系卫星动力学与控制的研究生也适合刚接手轨迹优化项目、需要快速让求解器跑起来的工程师。2. 把最优控制变成 NLP自适应伪谱法与绳系模型的匹配逻辑做绳系最优控制第一反应通常是上间接法构造哈密顿函数、写横截条件、解两点边值问题。理论上是漂亮的但工程上你很快会发现BVP 求解器对初值极其敏感绳系模型中摆角往往存在多个局部吸引域给定初猜稍微差一点就发散。直接打靶法也有自己的麻烦状态积分嵌套在每一次迭代里绳系方程的刚性和状态量之间的量级差会让每一步积分都变成负担。我一般先判断问题的解是光滑还是分段光滑再决定选哪条路线——这一步定错后面所有参数都白调。2.1 直接配点为什么在绳系问题上比打靶法稳得多所谓直接法就是不推导最优性必要条件直接在时间轴上把状态和控制离散化把原问题变成一个有限维约束优化问题。其中直接配点法比直接打靶法更适合绳系问题原因集中在两点。第一打靶法要对状态做数值积分而绳系动力学中绳长 l 出现在分母上摆角加速度表达里含有 vl/l 这样的项积分器步长会被迫压到很小刚性体现得淋漓尽致。配点法把微分方程在每个配点上写成代数残差并不需要数值积分避开了最容易数值爆炸的那一环。第二直接配点法产生的 NLP 具有稀疏结构。绳系问题里状态六个、控制一个、路径约束两三个离散到几百个节点时雅可比矩阵是带状的稀疏求解器处理起来非常快。打靶法和多重打靶法产生的灵敏度矩阵则要密得多维度稍高就难以收敛。对非线性强、又带着多个约束的绳系模型配点法的鲁棒性优势是实打实的。伪谱法就是配点法的一个特例它把每个区间上的基函数选成全局插值多项式配点选在正交多项式的零点比如 LGL 或 LGR 点上。这样做的好处是精度高光滑问题上可以用很少的配点达到很高的逼近精度代价是对解的不光滑性非常敏感。绳系展开过程中的张力切换、松弛段与张紧段的分界恰恰是这种不光滑性最爱出现的地方所以直接用单一区间的全局伪谱法常常在切换点附近产生高频振荡。2.2 hp 自适应的核心区间宽度与多项式阶数谁先动hp 自适应这个名字在有限元里很常见h 指网格宽度p 指多项式阶数。伪谱法的 hp 自适应是指把整个时间域先分成若干区间每个区间独立地决定两件事这个区间要不要再剖分得更细h 细化以及这个区间内的插值多项式阶数要不要提高p 细化。判断依据是误差估计器给出的局部分段残差。具体做法是在当前网格上解完 NLP 之后把求得的解代回动力学方程看每个区间内动力学残差有多大。某个区间的残差超过容差就触发细化。关键策略在于线性 p 优先还是 h 优先。我常用的准则是如果该区间的解是光滑的优先提高多项式阶数也就是做 p 细化因为光滑情况下高阶多项式收敛极快如果区间边界附近应变较大或解本身带转折优先把区间对半剖分也就是 h 细化用多个低阶区间去逼近。这个规则落实到实现里就是一段非常简单的分支逻辑但它的效果正是一只假想的“自适应手”把有限的自由度分配到真正需要它的地方。对绳系问题来说这个特性正好对上了模型的两个痛点——强非线性导致的局部剧烈变化以及路径约束激活产生的斜率转折。固定网格要么在切换点附近分辨率不足要么为了照顾局部精度把整个网格加密得过度庞大。hp 自适应可以只增加问题区间附近的局部密度其他光滑区段保持低阶高阶混合整体 NLP 规模会小一个量级。2.3 绳系模型的两个标度特征量级差和约束切换绳系系统在数值上还有一个天然让人头疼的地方状态量之间的量级差非常大。轨道半径 r 的量级是 7×10⁶ 米绳长 l 通常是几百米到几十公里摆角 θ 是 0.01 弧度量级角速度可能在 10⁻³ 量级。如果直接用国际单位制丢给 NLP 求解器约束矩阵和雅可比矩阵里的各项会横跨八九个数量级数值条件的恶化程度足以让任何内点法难以下手。所以用伪谱法解绳系最优控制第一步必须是无量纲化。常见做法是取初始轨道半径 a₀ 为长度基准取 t₀ √(a₀³/μ) 为时间基准速度、角速度、张力都随之归一化。无量纲化之后r 在 1 左右l 在 0.001 到 0.01 量级θ 天然无量纲再给张力选一个合适的参考量整个 NLP 的条件数会变得友好得多。这一步不做后面的一切调参都是空中楼阁。另一个特征是约束切换。绳系展开的张力约束通常写成 0 ≤ T ≤ T_max当张力从上界落到自由飞行段时控制变量的最优解会出现明显的折点。这类折点用单个高次多项式去逼近一定会出现配备点之间的振荡用 hp 自适应配合约束感知的初始分段提前把可能发生切换的位置留成独立区间才能让求解器稳定捕捉到真实的拐点。3. tether_optimal_hp 求解流程从状态方程到自适应网格原理讲清楚之后直接落到流程。这一章的思路是我自己搭绳系最优控制问题时最常用的模板先建立可复现的动力学模型再定义目标函数和路径约束最后用 hp 自适应主循环把问题交给 NLP 求解器。3.1 二维绳系状态方程一套最小可复现的动力学模型为了不把问题复杂化我采用二维平面内的点质量模型母星在圆轨道上运动子星通过一根无质量刚性绳连接忽略绳的弹性和面外摆动。状态向量取六个量r轨道半径l绳长θ摆角相对于当地竖直方向vr径向速度vl绳长变化率dθ摆角角速度控制量是绳系张力 T。代码如下import numpy as np def tether_dynamics(state, t, params): 二维绳系卫星动力学方程。 state [r, l, theta, vr, vl, dtheta] 返回值是状态对时间的导数 mu params[mu] m params[m] T params[T_control] # 当前控制输入由外部的 NLP 求解器给出 r, l, theta, vr, vl, dtheta state omega np.sqrt(mu / r**3) # 圆轨道角速度 eps 1e-10 # 防止 l 过小导致除零 dr vr dl vl dtheta_dot dtheta # 摆角加速度科氏项 引力梯度项 张力项 ddtheta (-2.0 * dtheta * vl / (l eps) - 3.0 * omega**2 * np.sin(theta) * np.cos(theta) T / (m * (l eps))) # 径向加速度中心引力 沿摆角方向的轨道离心分量 dvr -mu / r**2 l * omega**2 * np.cos(theta)**2 # 绳长加速度摆锤向心项 迎风轨道项 - 张力加速度 dvl l * dtheta**2 r * omega**2 * np.sin(theta)**2 - T / m return np.array([dr, dl, dtheta_dot, dvr, dvl, ddtheta])这段模型虽然简化但保留了绳系系统最要命的两个特征。摆角加速度中的 -2·dθ·vl/l 来自绳长变化引起的角动量效应绳越长或收绳越快这个耦合项越明显引力梯度项 -3ω²sinθcosθ 是绳系摆动的恢复力来源在 θ0 附近它是线性的但大摆角下非线性迅速增强。张力项 T/(m·l) 的分母带 l这是整个系统刚性的根源之一也是伪谱法网格最敏感的地方。参数说明mu 取地球引力常数 3.986×10¹⁴ m³/s²m 是子星质量T_control 不是常数而是控制轨迹由伪谱法配点值给出。实际使用时要配合无量纲化r、l 都除以初始轨道半径 a₀时间除以 t₀√(a₀³/μ)这样 omega 变成 1方程数值条件大幅改善。3.2 目标函数和路径约束怎样定义“最优”和“安全”绳系展开的最优控制问题目标函数五花八门但最常见的组合是固定终端时刻最小化终端摆角和绳长误差同时把张力限制在安全范围。这对应的是一个 Mayer 型加上 Lagrange 型混合的问题。我常用的目标函数表达为def tether_objective(z_seg, params): 目标是终端摆角尽量小、终端绳长到达目标值。 也可以换成最小时间问题只要把积分项换成终端时刻即可。 zf z_seg[-1] theta_f zf[2] l_f zf[1] L_target params[L_target] w params[w_theta] # 摆角权重一般取 10~100 return 0.5 * (l_f - L_target)**2 0.5 * w * theta_f**2路径约束部分是重点。张力约束 0 ≤ T ≤ T_max 直接写在控制变量上摆角约束 |θ| ≤ θ_max 要写成代数路径约束加在整个区间上。常见参数T_max 取系绳极限拉力的安全系数折减比如 0.15~0.3 倍极限拉力θ_max 取 30°~45°取决于目标任务允许的摆动幅度。还有终端约束终端时刻摆角角速度要归零否则展开结束后残留摆动会直接影响载荷精度。调参时要注意权重 w 不要拍脑袋给。先跑一版 w1看终端摆角是多少如果摆角过大再按一个数量级一个数量级往上加加到头终端绳长误差开始增大为止。这样做比一开始就给大权重更可控因为你随时知道是哪个目标在主导解的形状。3.3 网格初始化和 hp 自适应主循环先粗网格再自动加密伪谱法的主循环可以抽象成下面这段逻辑。初始化阶段给一个很粗的时间分段比如 4 段、每段 p4然后进入迭代解 NLP、算误差、细化网格直到所有区间残差低于容差。def hp_pseudospectral_solve(problem, tol1e-5, p_max8, seg_max64): 自适应伪谱法主循环 在时间轴分段上反复求解 NLP根据残差决定加密方式。 grid initial_grid(n_seg4, p_init4) # 初始分段4 段每段 p4 for it in range(30): sol solve_nlp(problem, grid) # 调用 IPOPT 等 NLP 求解器 err_seg estimate_error(sol, problem) # 每段动力学残差 if max(err_seg) tol: return sol, grid grid refine_grid(grid, err_seg, p_maxp_max, seg_maxseg_max) raise RuntimeError(网格迭代超过上限检查误差估计与约束定义)逻辑说明solve_nlp 这一步把每段插值多项式系数和每个配点上的控制值作为优化变量动力学方程在每个配点上写成残差路径约束也作用在配点上。estimate_error 是把解代回微分方程在比配点更密的采样点上算残差最大值。refine_grid 是核心它根据误差分布对每个区间独立决策。细化规则我固定这样写误差超过 tol 但小于 3 倍 tol 的区间优先 p 细化每段多项式阶数加 2误差超过 3 倍 tol 的区间说明这一段本身形状太差直接对半剖分误差已经小于 0.1 倍 tol 的区间可以考虑降阶减小整体变量个数。p_max 设置在 8 到 10 之间超过这个值高次多项式会出现数值病态不如做 h 细化。seg_max 是保险丝防止自适应失控把网格铺到内存都装不下。4. 三个必调参数容差阈值、最大多项式阶数与网格上限用伪谱法解绳系问题真正需要反复调的就三个参数。其余参数比如初始猜测、NLP 求解器容差都按惯例设定即可但这三个是决定收敛性和精度的主开关。4.1 误差容差 tol它决定自适应何时停手误差容差直接控制最终求解精度和迭代次数。对绳系任务我的经验是如果只做方案论证tol 放到 1e-3 就够要拿来做详细设计或控制系统离线验证需要压到 1e-5 甚至 1e-6。tol 设置偏低时会出现一个很隐蔽的坑自适应会把网格不断细化去追赶那些物理意义不大的微小残差比如摆角接近零点的数值噪音被误判成需要加密的区域。结果 NLP 规模膨胀求解时间翻倍解却和前一个版本几乎没区别。建议先跑一版 tolerance 1e-3看误差分布图把注意力集中在那几个残差尖峰上再决定要不要整体压小还是只针对局部区间手动加分段。4.2 最大多项式阶数 p_max光滑区间能不能一步到位p_max 决定高频捕捉能力同时也决定了数值稳定性。光滑区间上高阶多项式收敛极快但超过一定阶数后插值多项式的龙格效应会让配点之间的振荡急剧放大。绳系问题里摆角 θ 的动力学相对光滑张力曲线则带有折点。我常用的 p_max 是 8。特殊情况下会调到 12但这时必须配合区间细分使用绝不在一个区间上用超过 12 阶的多项式去逼近约束切换点。如果求解结果里观察到配点之间的高频振荡第一反应不该是加阶数而是检查该区间是否跨过了约束激活点——真跨过了就把这个区间切开让折点落在区间边界上。4.3 网格上限与首网格分段数给自适应留多大的自由初始分段数 n_seg_init 会影响整个自适应的走向。给太少误差估计器第一次迭代就会报出大残差网格被劈成两半然后继续劈迭代次数多但结果稳定给太多NLP 初始变量就很多求解器每一步都慢且早期收敛阶段的网格重分配意义不大。常用做法先给 4 到 6 段每段 p4跑一版看误差分布。如果误差集中在一两个区段说明初始分段已经接近合理如果误差均匀分布在整个时间轴上说明问题整体是光滑的考虑把每段 p 提升到 6 而不是去切更多段。网格上限 seg_max 通常设 64几乎所有绳系展开问题在这个量级都能收敛。5. 避坑清单绳系伪谱法最常见的四个坑下面几个坑是我在 tether_optimal_hp 方案落地过程中实际踩过、并且见过同行反复踩的。每条按现象、原因、解决三段写方便排查时直接对照。5.1 无量纲化没做求解器直接报“无可行解”现象把国际单位制的绳系模型直接丢给 NLP 求解器IPOPT 在几十次迭代后报出“收敛到不可行点”不管初始猜测怎么换都救不回来看起来像问题本身没有解。原因状态量 r 在 7×10⁶ 量级l 在 10³ 量级dθ 在 10⁻³ 量级控制 T 在 10⁰ 到 10³ 量级。约束矩阵各行量级差异巨大KKT 系统的条件数被差到几千万求解器内部所有对偶变量更新都失去意义线性系统的数值误差直接把迭代方向带偏。解决在建模阶段统一做无量纲化长度量纲取 a₀初始轨道半径时间量纲取 t₀√(a₀³/μ)质量量纲用子星质量 m₀。写模型文件时把 r、l、t、T 全部换成无量纲变量。改完之后 omega 归一化成 1r 初始值也是 1整个问题所有变量都在 10⁻² 到 10¹ 量级内浮动求解器立刻变得听话。5.2 配点上约束全满足前向积分却越界现象NLP 求解结果里每个配点上的张力 T 都严格满足 T ≤ T_max看起来路径约束全部生效但把解插值到稠密时间轴并用 ODE45 前向积分中间段的张力超过上界 15%甚至出现负值。原因直接配点法的路径约束只在离散点处施加配点之间完全靠插值多项式决定行为。张力曲线在切换点附近有陡峭变化插值多项式在配点之间产生过冲类似龙格振荡虽然配点处合规但区间中段已经越过约束边界。这是所有配点法的通病绳系问题因为存在折点和奇异分子项表现得格外明显。解决三种方式组合使用。第一后处理时把配点密度提高四到五倍逐点检查路径约束找到越界区段第二在 NLP 里把约束上界适当收紧比如 T_max5kN 就改成 4.6kN给过冲留余量第三手动把张力折点所在的区间对半切开让不光滑点落在区间边界而不是插值多项式内部。5.3 局部误差卡住不降自适应网格反复细化也不收敛现象自适应迭代前几轮误差下降明显迭代到后几轮某个区间的误差一直稳定在容差的 1.2 到 1.5 倍之间每次都被判断为需要细化但细化之后误差变化极小看起来像是自甄别器发疯。原因最常见的情形是误差估计器把高次多项式的边界效应误判为真实动力学误差。区间连接处的状态连续性约束强迫相邻区间多项式在节点上对齐但节点附近的导数并不连续误差估计器在这个不连续点上始终报出高残差。每细化一次不连续点还在那里只是被包进了一个更小的区间误差幅度不变。解决先人工检查该区间有没有真实的物理分界点比如从自由飞行段切换到张力控制段的时刻。如果有直接把区间边界拖到这个切换时刻上让多项式在切换点两侧各玩各的不要让它穿过折点。如果确认没有物理分界点那就是节点处的 C¹ 不连续导致的伪误差处理办法是把误差估计的采样点略过区间端点只统计区间内部残差。5.4 摆角接近 ±π 时解剧烈振荡现象任务要求大角度摆动摆角在 170° 附近变化伪谱法解里每隔几个配点就出现一次正负大幅跳变自适应网格拼命加密也压不住最终求解器要么发散要么给出一条工程上完全不能用的控制曲线。原因θ 是角度量数学上是无界的但在数值上相邻采样点从 179° 变到 -179° 时数值差是 -358°NLP 看到的是一个巨大跳跃会试图用高次多项式去拟合这个假跳变激发出高频振荡。实际上这两个角度物理上只差 2° 的远角。角度周期性导致的状态跳跃线性多项式再怎么加密也无法光滑逼近。解决把 θ 从状态变量里去掉改用方向余弦对s sinθc cosθ增加一个单位约束 s² c² 1。运动学关系变成 ds/dt c·dθdc/dt -s·dθ摆角本身作为辅助量在后处理里提取。这样 NLP 里不再出现 359 度的伪装角度跳跃只有 s 和 c 的光滑变化。代价是多一个状态变量和一个非线性等式约束但这比对付角度跳变要便宜得多。6. 收敛之后别急着交差三条验证手段逼出隐藏问题解跑通只是起点。我见过太多方案在伪谱解上看起来完美一上高精度积分器就原形毕露。我的经验是三道验证工序全部通过才敢说这个解可用。第一道是残差复检把最优解插值到比配点密 10 倍的时间轴上算动力学残差曲线看最大残差落在哪。如果残差尖峰全部集中在某个区间内部说明该区间还有物理信息没被捕捉回到第 4 章的细化规则把这一段重新处理如果残差分布均匀且幅度接近容差才算真正收敛。第二道是前向积分回放用最优控制曲线驱动高精度求解器做前向仿真对比终端状态和伪谱解的终端状态。两者的差要在控制精度的容差范围内通常要求位置误差在米级、速度误差在厘米每秒级差得多就说明配点间插值引入了虚假动力学。第三道是网格敏感性验证把容差从 1e-5 收紧到 1e-6 再跑一版把容差放宽到 1e-4 再跑一版三条最优轨迹做对比。真正收敛的解对网格参数不敏感三次前沿曲线应几乎重合。我在最后一个绳系项目里吃过这个亏伪谱解比前向积分的终端摆角差了 4 度查了半天才发现是张力约束在配点之间的过冲累积成的误差。后来把界从 5.0kN 收到 4.55kN误差立刻回到合理范围。从那以后我就多了一道习惯所有配点法解出来的最优控制必须先过前向积分这一关才进方案评审配点之间的“黑匣子”行为不值得信任。希望这些方法也能帮你把绳系伪谱法这趟路走得少些折腾。本文还有配套的精品资源点击获取