简介这份压缩包提供了一套基于hp-自适应伪谱法的MATLAB源码用于求解系绳tether系统的最优控制问题适合航空航天、无人机、风筝缆绳等领域的研究者与工程师。伪谱法先将连续时间最优控制问题离散化为非线性规划再通过数值迭代求取满足初值、终值、边界与路径约束的最优轨迹hp策略则根据误差指示自动调整网格划分的密度与多项式阶数在精度和计算成本之间取得平衡。包内共三个脚本文件均采用MATLAB编写文件总体积约4KB其中主程序定义模型维度及整体求解流程微分代数方程处理模块负责将动力学约束和等式/不等式约束转换为离散方程成本计算模块用于构建目标函数并评估代价三者可直接配合运行。这套代码不仅展示了hp自适应伪谱法的完整实现步骤也提供了系绳系统建模与约束处理的典型范例便于读者迁移到类似的最优控制任务中。资源已有七百余人学习浏览适合需要快速上手自适应伪谱法或开展系绳轨迹优化验证的中高级用户。1. 用 hp-自适应伪谱法解绳系最优控制为什么这个组合比打靶法稳做绳系卫星和空间拖拽任务时我手里最常出现的最优控制问题是这样系绳拖着子星在圆轨道里摆动需要在有限时间内从当前姿态转移到锁定姿态同时满足系绳张力、时间或燃料约束。这类模型强非线性积分类算法很容易被初始猜测带偏而伪谱法把状态和控制放在 Legendre-Gauss-Lobatto 配点上离散成有限维 NLP收敛性比打靶法稳很多。hp-自适应伪谱法则进一步把时间域切段逐段决定升多项式阶数p还是加密网格h正好吃下绳系轨迹“大部分平滑、某几段有陡变”的特点。这篇文章按我实际落地的顺序来先讲选型再讲建模、离散和参数最后把我在绳系问题上踩过的坑摊开。适合做航天 GNC 和轨迹优化的工程师参考。2. 伪谱法的选型逻辑为什么绳系最优控制不直接交给打靶法2.1 从直接打靶到配点解法从“积分加迭代”变成“一次性求解”我最早接触轨迹优化时接触的第一种方案是直接打靶法给定初始状态沿时间积分把终端误差和路径约束丢给数值优化器。它的本质是“积分器加根求解”每一次迭代都要完整积一遍轨迹。绳系动力学刚好是这套方法的克星——面内摆动的方程特征值分布在虚轴上数值积分对这种弱阻尼振荡很敏感稍微给错初值灵敏度矩阵就病态优化器一步跨出去轨迹直接飞到物理上不可解释的区域。多重打靶法是对直接打靶的一步妥协把时间域切成 M 段段内各自积分段与段之间用连续性条件接起来连续条件作为等式约束进 NLP。变量规模变大但求解稳定性提升了一个量级。可是到这一步段内的“积分”动作还在只要某段里状态变化剧烈积分器依然会被局部步长卡住。配点法走的则是另一条路干脆不积分。状态在离散节点上本身就是决策变量轨迹由插值多项式表示微分方程被转成节点处的代数残差。伪谱法就是配点法里选择了 Gauss 型节点和全局多项式基的那一派。同样是配点均匀网格配点在节点加密时误差只按幂律下降Legendre 谱基则能让光滑解的误差随配点数指数下降。绳系姿态场和系绳力场大部分区段是光滑的这正好是伪谱法的自留地。方法是否段内积分对初始猜测敏感度处理陡变的能力典型变量规模直接打靶是很高差小多重打靶是中一般中均匀配点否中低中中全局伪谱否低差中hp-自适应伪谱否低好中2.2 绳系动力学的“时间尺度撕裂”只有隐式格式能兜住绳系问题里有两个时间尺度在打架。面内摆角 θ 的无量纲固有频率大约是 √3 倍轨道角速度这个不快但如果把系绳纵向弹性建进来张力波的频率可能比它高两个数量级。显式积分器为了满足稳定性条件会被小步长拖死而伪谱离散本质上是隐式的——它没有“步长”概念每个子区间的时间尺度由多项式阶数吸收。我在项目里常用的模型是刚性杆假设把绳长高频模态从动力学里消掉。可这个假设会带来高指数 DAE一旦绳长约束和姿态约束耦合在一起打靶法的变分方程极其难求。伪谱配点儿干脆把代数变量留在离散方程组里DAE 和 ODE 一视同仁地变成等式约束求解难度下降不少。实际的经典案例是平台下面挂一根 100 m 量级的系绳摆角 60° 拉回 0°控制周期只有轨道周期的四分之一。我用显式 RK45 加多重打靶每轮灵敏度计算把步长压得难以接受换成 LGL 伪谱后每段 8 个配点、4 个段、总计 32 个配点就拿到收敛解。配点数不是越多越好后面避坑章会详细讲这个边界。2.3 hp 自适应解决的是“一段多项式到处光滑”的错误假设固定全局伪谱的弱点在 N 增大到 50 到 60 之间会明显暴露边界附近出现 Runge 型振荡NLP 雅可比矩阵变得稠密稀疏求解器的优势全废。绳系任务天然分阶段——展开、摆动、锁定——每段的动力学特性不同用一段多项式硬拟合整个时间轴既不经济也不稳定。hp 自适应的思路是分段多项式K 个子区间每段一个低阶伪谱离散常见配点数是 4 到 10。求解完成后用稠密残差检查每段误差误差超限的段再判类型光滑就升 p有陡变就劈 h。p 细化只改本段配点数量、段边界不动段间连续性约束不需要重排h 细化要把一个段一分为二新增连续性约束变量编号全变。细化策略里我优先走 p 再走 h因为 p 的代价只是矩阵变高几阶h 的代价是整个 NLP 结构调整。我习惯初始网格给 3 到 4 段、每段 8 个配点第一轮先摸到解的粗糙形状再让细化器去动个别段而不是一上来给 20 段均匀网格。这里有个经验段边界尽量贴近任务阶段分界点比如展开段结束、锁定段开始hp 细化迭代轮次能省一半以上。2.4 “模型伪谱法”里的模型到底指什么标题里“模型伪谱法”这个词在项目里通常指基于显式动力学模型——刚杆模型或哑铃模型——的伪谱求解与近年流行的“数据驱动伪谱法”相对。数据驱动适合做黑匣子补偿但绳系任务做标称轨迹优化时必须有一个物理自洽的模型否则伪谱配点上解出来的控制量放到实物上根本不敢用。模型伪谱法的另一个隐含意思是伪谱法本身不负责建模它只负责把模型离散化。模型里哪一项是控制输入、哪一项是路径约束、哪一项是终端条件直接决定离散后 NLP 的规模和条件数。建模阶段的错误比如漏了系绳张力下限伪谱法会在你浑然不觉时找到一个数学上满足约束、物理上荒谬的解。这一点我在避坑章里专门展开。3. 把绳系轨迹优化写成离散 NLP建模、无量纲化与 LGL 离散化3.1 先定义能收敛的模型面内刚性摆不展开用刚性绳系摆做原型。质心做匀速圆轨道运动系绳长度固定只考虑面内摆角。令 θ 为系绳相对当地垂线的摆角ω 为其角速度采用轨道角速度归一化的无量纲时间动力学方程为θ ω ω -3 sinθ cosθ usinθ cosθ 在 45 度附近给出最大的重力梯度力矩θ0 是稳定的径向平衡θ90 度是不稳定平衡点。控制目标是“最小控制能量 终端姿态锁定”J ∫₀^T u² dτ边界条件取θ(0)60 度ω(0)0θ(T)0ω(T)0。T 固定为无量纲时间 10。这是原型验证的起点。工程要更真实把绳长 λ 加进状态变成绳系卫星哑铃模型状态变成 λ、λ、θ、θ控制同时包含系绳拉力的径向分量和摆角的侧向推力但离散流程完全相同。我先用两状态版本讲清楚流程参数后面都换得动。3.2 无量纲化与时间映射把物理时间压进 [-1,1]伪谱法大部分实现都定义在参考区间 [-1,1] 上。若物理时间 t 属于 [t₀, t₁]映射写成t (T/2) τ (t₀ t₁)/2T 是段的总时长微分算子 d/dt (2/T) d/dτ。离散时用 LGL 点和微分矩阵算出的导数必须乘上 2/T 这个尺度因子否则动力学残差差了量级收敛后终端约束会偏。绳系问题常用做法是把时间和轨道周期一起归一化无量纲时间 τ Ω t方程里的系数 3 就是这么来的。目标函数也按无量纲时间定义出图时把时间轴标清楚就行。以前的教训是忘记把 dt/2 写进残差结果解出来的轨迹时间尺度整体缩了一半看起来像稳定解实际物理上一塌糊涂。3.3 LGL 配点与微分矩阵先拿到这两个矩阵伪谱法里最常打交道的是三样东西LGL 节点、微分矩阵 D、积分权重 w。专业工具里是一行调用自建原型时用 Newton 迭代求内部节点。下面的实现是完整的可以直接运行。import numpy as np from numpy.polynomial.legendre import leggauss, legval, legder def lgl_nodes(n): 返回 n 个 Legendre-Gauss-Lobatto 节点升序含 -1 和 1 if n 2: return np.array([-1.0, 1.0]) # 内部节点是 P_{n-1}(x)0 的根先用 Gauss 节点做初值 x0, _ leggauss(n - 2) # 构造 P_{n-1} 系数最高次系数为 1 即可 pcoef np.zeros(n) pcoef[-1] 1.0 dpcoef legder(pcoef) ddpcoef legder(dpcoef) # 对 f P_{n-1} 做 Newton 迭代 for _ in range(40): p legval(x0, dpcoef) dp legval(x0, ddpcoef) x0 x0 - p / dp return np.concatenate(([-1.0], np.sort(x0), [1.0])) def lgl_diff_matrix(x): 构造 LGL 节点的伪谱微分矩阵 DD f 给出 f 在节点处的导数 n len(x) D np.zeros((n, n)) pcoef np.zeros(n) pcoef[-1] 1.0 pprime legval(x, legder(pcoef)) # P_{n-1} 在各节点的值 for i in range(n): for j in range(n): if i ! j: D[i, j] pprime[i] / (pprime[j] * (x[i] - x[j])) # 内部点对角元用行和补零 for i in range(1, n - 1): D[i, i] -np.sum(D[i, :]) # 边界点对角元是标准结果 D[0, 0] -n * (n - 1) / 4.0 D[-1, -1] n * (n - 1) / 4.0 return D def lgl_weights(x): LGL 积分权重用于目标函数离散 n len(x) pcoef np.zeros(n) pcoef[-1] 1.0 P legval(x, pcoef) # P_{n-1} return 2.0 / ((n - 1) * n * P**2)逻辑说明LGL 节点包含端点 -1、1 以及 P_{n-1} 的根共 n 个。Newton 迭代的初值用同数量的 Gauss 节点收敛极快40 次迭代在这里只是兜底的保险。微分矩阵的非对角元用 P_{n-1} 的值之比乘上 1/(x_i-x_j)这是谱方法教材里的标准闭式公式内部点对角线由“行和为零”的性质补出来边界点对角线写成 -N(N-1)/4 和 N(N-1)/4其中 Nn-1 是多项式阶数。权重公式对应 LGL 积分精度 2n-3 阶离散目标函数误差已经足够小。3.4 单段离散把连续 ODE 改成代数残差对第 k 段配点集合 x_k 有 N_k 个点微分矩阵 D_k 是 N_k×N_k时长 Δt_k。状态按配点排列成 X_k维度 N_k×2。状态的时间导数在配点处为 D_k X_k转成物理时间再乘 2/Δt_k。动力学方程 dX/dt f 就变成一组代数等式约束D_k X_k / (Δt_k/2) - f(X_k, U_k) 0段间加连续性约束X_k 的最后一行等于 X_{k1} 的第一行。目标函数用 LGL 权重求和∫₀^T u² dt ≈ Σ (Δt_k/2) Σ w_i u_{k,i}²下面是一段的残差装配函数def segment_residual(D, dt, X, U): 返回该段动力学残差数值上应趋于零 D : N x N LGL 微分矩阵参考区间 [-1,1] dt : 该段物理时长配合 [-1,1] 映射时尺度因子为 dt/2 X : N x 2状态 [theta, omega] 在配点上的值 U : N x 1控制 u 在配点上的值 scale dt / 2.0 dX (D X) / scale theta X[:, 0] omega X[:, 1] u U[:, 0] # 刚性绳系面的动力学omega -3 sinθ cosθ u rhs np.stack([omega, -3.0 * np.sin(theta) * np.cos(theta) u], axis1) return dX - rhsscale 是 dt/2不是 dt这个因子我丢过一次结果整个时间尺度错了解虽然好看但完全不能用。残差函数返回的形状是 N×2拼 NLP 约束时要拉平成向量。每段各算各的残差段间连续性另起一个约束函数。3.5 决策变量、初猜和 NLP 求解器选择变量布局是一维向量把所有段的状态和控制按段拼接。两状态单控制、3 段×8 配点的规模是 3×8×372 个变量SLSQP 能直接跑。工程问题建议用 IPOPT 配稀疏雅可比绳系模型进 NLP 就带病态雅可比必须用符号推导或自动微分别用有限差分否则求解器每一步都在和数值噪声搏斗。初猜是一条血泪经验控制全零、状态线性插值一般能收敛但偶尔 SLSQP 会因为初始雅可比奇异而退出。我的做法是先跑一个 3 段×4 配点的低阶离散把粗解插值到 8 配点网格作为第二轮初猜。这个“二阶启动”比任何精调都管用基本消灭了“初猜翻车”这一类问题。求解骨架在下面逻辑是标准的 NLP 组装import scipy.optimize as opt def solve_pseudospectral(segs, dt_seg, tol1e-6): # segs: 每段配点数dt_seg: 每段物理时长 z0 init_guess(segs, dt_seg) # 状态线性插值控制置零 cons [ {type: eq, fun: lambda z: dynamics_cons(z, segs, dt_seg)}, {type: eq, fun: lambda z: continuity_cons(z, segs, dt_seg)}, {type: eq, fun: boundary_cons} ] res opt.minimize( lambda z: cost(z, segs, dt_seg), z0, methodSLSQP, constraintscons, options{maxiter: 500, ftol: tol} ) return resdynamics_cons 内部遍历段逐个调用 segment_residualcontinuity_cons 取每段端点做差boundary_cons 把 θ₀、ω₀、θ_T、ω_T 的设定值钉死。SLSQP 在用例子上够用但工程规模建议换 IPOPT 或 CasADi我抓到过 SLSQP 在终端角速度约束上提前宣称收敛返回一个约束违反很小的假解。4. hp-自适应伪谱法的工程参数网格、容差和 p/h 细化判据4.1 初始网格配给与阶段对齐初始网格我的经验值是3 到 4 段每段 8 个 LGL 配点。若知道任务阶段边界——比如展开段结束、摆动段开始——把段边界放在分界点附近细化迭代能少跑两轮。不知道也没关系hp 细化器会自己找只是多花几次求解。每段配点数 N_k 有上下界。下限 4低于 4 时多项式刚能盖住约束误差顶不上去上限 12超过 12 后微分矩阵条件数开始抬头升 p 的收益被舍入误差吃掉了。总段数我一般允许到 12 段左右再往上其实退化成有限差分配点丢掉了伪谱法变量少的红利。网格细分的目标是局部不是全局。4.2 稠密残差评估不要只在配点上自欺欺人伪谱解只在配点上被等式约束强制满足配点之间轨迹质量不直接可知。配点等式约束残差可能到 1e-10但把解插值回稠密均匀点中间残差可能到 1e-2。原因是 LGL 节点两端密、中间疏配点间插值误差根本没有进入约束。我拿到解以后把每段解插值到 201 个均匀样本点在插值点上计算 dX/dt - f取 L∞ 范数作为该段的误差。插值用拉格朗日基函数对 LGL 节点做样本点不低于 100 个。低于 100 会把残差尖峰滤掉我见过用 25 点评估漏判的案例。def segment_error(D, dt, X, U): 稠密残差评估返回最大残差及各点残差曲线 tau_d np.linspace(-1.0, 1.0, 201) X_d interpolate_lgl(X, tau_d) # 状态插值 U_d interpolate_lgl(U, tau_d) # 控制插值 dX_d interpolate_lgl(D X / (dt / 2.0), tau_d) # 导数也插值 theta X_d[:, 0] omega X_d[:, 1] u U_d[:, 0] rhs np.stack([omega, -3.0 * np.sin(theta) * np.cos(theta) u], axis1) err np.linalg.norm(dX_d - rhs, axis1) return np.max(err), err这里的易错点是插值对象要同时包含状态、控制和时间导数 dX。残差公式里 dX 也按插值求值如果直接把配点上的导数搬过去等于又回到配点验证误差尖峰还是会被插值抹平。这一步是整个误差估计里最偷懒不得的。4.3 p 细化还是 h 细化用光滑性探针定方向细化决策看两个信号。第一个是该段的插值多项式系数衰减对每段状态 θ 的 LGL 插值系数按 Legendre 系数从高阶往低阶看如果系数衰减率快——相邻高阶系数比值稳定在 0.8 以下——说明解在段内光滑误差主要来自阶数不足升 p。第二个是衰减率慢、高阶系数振荡说明段内有非光滑点或陡变升 p 会触发 Runge 现象得改 h。实操中我常用一个探针该段先试算 p4 阶看残差变化。p4 后残差下降一个数量级判光滑段走 p 细化残差基本不动判陡变段走 h 细化。用 p4 而不是 p1是因为相邻阶次的残差差异在图上区分度不够容易被舍入误差盖住。段内信号残差随 p 增加的变化细化策略光滑指数下降pN_k2 或 4非光滑/陡变基本不动h段一分为二边界层略降但尾部翘hp 混合4.4 两层容差与求解器参数两层容差要拉开。伪谱残差阈值我常设 1e-6 到 1e-8NLP 求解器的 KKT 容差要提高一到两个量级。IPOPT 里我习惯设 tol1e-10、max_iter500。如果 NLP 收敛在 1e-7伪谱残差阈值却设 1e-9细化器会一直反复触发让你误以为网格出了问题。注意p 细化只改本段节点数段边界不动h 细化会把段劈开连续性约束和变量编号全变。细化器里优先 p 再 h把 h 留给真正不光滑的段。5. 绳系伪谱法五个高频翻车现场现象、原因、解决5.1 配点数抬不上去N20 之后比 N12 更差现象单段配点数超过 20残差不降反升NLP 求解速度肉眼可见地下降。原因LGL 微分矩阵条件数按 O(N²) 增长节点本身在边界堆积、内部稀疏。提高 N 对病态问题没有帮助反而放大舍入误差和插值振荡。绳系解光滑段用高 N 有指数收益遇到陡变段就是灾难。解决劈段而不是继续升同一段的 p。我在绳系离轨任务里对比过全局 N60 和 6 段×10 配点后者变量更多但条件数小得多收敛更快。配点数的上限就定在 12 左右超过就触发 h 细化。5.2 平坦初猜让优化器直接缴械现象所有状态初值填 0 或常数第一轮 NLP 报奇异或陷入局部。原因伪谱约束的非线性在错误猜测下雅可比退化。恒零猜测让 sinθ cosθ 项全为零约束从数值上少了一条有效信息求不出可行方向。解决先用 3 段×4 配点低阶离散收敛插值到 8 配点网格作为第二轮初猜。这是我最常拿出来救场的操作几乎每次都能把失败拉回来。等价做法是用配点法先算粗网格解再插值到 hp 网格。别指望平坦猜测能靠求解器自己爬出来。5.3 在配点上验证残差导致内部失稳被忽视现象配点等式约束残差 1e-10把解插值回 200 个稠密点区间内部残差 1e-2。原因LGL 节点两端密、中间疏配点间插值误差不参与约束整段轨迹其实没有被真正看到。仅看配点残差等于只检查了解被钉住的那几个点。解决按 4.2 的方案在 201 个均匀插值点上重新评估残差以稠密残差为准做细化判据。插值函数要写得跟求解器一样小心我自己的习惯是插值后先目测一遍残差曲线再让细化器做决定。残差曲线如果有周期性尖峰多半是段边界连续性没接好。5.4 绳长压进不可行域哑铃模型变成橡皮筋现象可伸绳模型下最优解把绳长压到标称长度的 0.3 倍张力几乎为零物理上系绳早就松塌了。原因伪谱法不在意模型背后的物理假设。哑铃模型假设绳始终绷紧但目标函数和约束里没写这一条优化器发现缩短绳长能省控制能量就往模型没定义的地方钻。解决在离散约束里显式加绳长上下界 λ ∈ [λ_min, λ_max]以及张力非负或绳长下限约束。同时一定要看最优解的拉格朗日乘子乘子能告诉你哪些约束正被边界撑着。乘子全为零的路段说明约束根本没起作用回头检查建模。这个坑很隐蔽因为解的形状看起来平滑合理实际已经在物理上不可用了。5.5 细化判据只看误差p/h 反复横跳直到网格爆炸现象误差超限就 h 细分下一轮误差还在继续细分段数从 12 涨到 40求解器跑不动。原因判据里少了光滑性维度。很多段误差超限其实是 p 不足给 h 属于药不对症。伪谱法里 h 细化只能降局部误差不能恢复指数收敛特性最后算出一个有限差分味道的高密度网格伪谱省变量的红利全丢。解决细化前做一次 p4 探针试算看残差是否明显下降。这个判据固化到细化器里之后我的迭代轮次从 6 到 8 轮降到 2 到 3 轮。探针只改本段配点数、不动段边界变量重排代价小值得做。6. 让 hp 自适应更稳的联动技巧残差-阶数曲线与网格重分探针最后分享一个细化器层面的细节也是我后来每次做 hp 自适应都保留的习惯每个段维护一张“残差随 p 变化”的小表。第一轮求解完对误差超限的段先做 p4 探针记录残差从多少掉到多少。如果残差下降超过一个数量级判光滑段走 p 细化如果下降不到 0.5 倍判陡变段走 h 细化。这张表同时告诉你该段需要多少阶才够。探针试算时只改 N_k不重排段边界。段边界是所有子区间共享的接缝动了它相邻段的插值基函数全要重算连续性约束重新编号上一轮的大部分求解信息浪费掉。p 探针的代价只是矩阵高几阶换来的是细化方向的确定性这笔账很划算。细化器还要加一个回滚机制。如果新一轮网格重分后全局残差反而比上一轮大说明判据踩进了雷区——常见原因是某段被 h 细化劈开后新边界落在了一个不该落的位置。回滚到上一轮网格在该段改走 p 细化通常能绕过去。我遇到过一次细化后数值解跳变回滚救回。回滚条件写进细化器比手动干预可靠。最后一件事细化器永远要比求解器保守。伪谱解本身带数值噪声残差在 1e-7 附近上下波动很正常细化阈值别贴着噪声设。我会把细化阈值定在 NLP 收敛容差的 10 倍以上给求解器留出余量。这条纪律保了我很多次。希望帮到你。本文还有配套的精品资源点击获取