简介：这份压缩包提供了一套基于hp-自适应伪谱法的MATLAB源码，用于求解系绳（tether）系统的最优控制问题，适合航空航天、无人机、风筝缆绳等领域的研究者与工程师。伪谱法先将连续时间最优控制问题离散化为非线性规划，再通过数值迭代求取满足初值、终值、边界与路径约束的最优轨迹；hp策略则根据误差指示自动调整网格划分的密度与多项式阶数，在精度和计算成本之间取得平衡。包内共三个脚本文件，均采用MATLAB编写，文件总体积约4KB，其中主程序定义模型维度及整体求解流程，微分代数方程处理模块负责将动力学约束和等式/不等式约束转换为离散方程，成本计算模块用于构建目标函数并评估代价，三者可直接配合运行。这套代码不仅展示了hp自适应伪谱法的完整实现步骤，也提供了系绳系统建模与约束处理的典型范例，便于读者迁移到类似的最优控制任务中。资源已有七百余人学习浏览，适合需要快速上手自适应伪谱法或开展系绳轨迹优化验证的中高级用户。

1. 用 hp-自适应伪谱法解绳系最优控制：为什么这个组合比打靶法稳

做绳系卫星和空间拖拽任务时，我手里最常出现的最优控制问题是这样：系绳拖着子星在圆轨道里摆动，需要在有限时间内从当前姿态转移到锁定姿态，同时满足系绳张力、时间或燃料约束。这类模型强非线性，积分类算法很容易被初始猜测带偏，而伪谱法把状态和控制放在 Legendre-Gauss-Lobatto 配点上离散成有限维 NLP，收敛性比打靶法稳很多。hp-自适应伪谱法则进一步把时间域切段，逐段决定升多项式阶数（p）还是加密网格（h），正好吃下绳系轨迹“大部分平滑、某几段有陡变”的特点。这篇文章按我实际落地的顺序来：先讲选型，再讲建模、离散和参数，最后把我在绳系问题上踩过的坑摊开。适合做航天 GNC 和轨迹优化的工程师参考。

2. 伪谱法的选型逻辑：为什么绳系最优控制不直接交给打靶法

2.1 从直接打靶到配点：解法从“积分加迭代”变成“一次性求解”

我最早接触轨迹优化时，接触的第一种方案是直接打靶法：给定初始状态，沿时间积分，把终端误差和路径约束丢给数值优化器。它的本质是“积分器加根求解”，每一次迭代都要完整积一遍轨迹。绳系动力学刚好是这套方法的克星——面内摆动的方程特征值分布在虚轴上，数值积分对这种弱阻尼振荡很敏感，稍微给错初值，灵敏度矩阵就病态，优化器一步跨出去，轨迹直接飞到物理上不可解释的区域。

多重打靶法是对直接打靶的一步妥协：把时间域切成 M 段，段内各自积分，段与段之间用连续性条件接起来，连续条件作为等式约束进 NLP。变量规模变大，但求解稳定性提升了一个量级。可是到这一步，段内的“积分”动作还在，只要某段里状态变化剧烈，积分器依然会被局部步长卡住。

配点法走的则是另一条路：干脆不积分。状态在离散节点上本身就是决策变量，轨迹由插值多项式表示，微分方程被转成节点处的代数残差。伪谱法就是配点法里选择了 Gauss 型节点和全局多项式基的那一派。同样是配点，均匀网格配点在节点加密时误差只按幂律下降，Legendre 谱基则能让光滑解的误差随配点数指数下降。绳系姿态场和系绳力场大部分区段是光滑的，这正好是伪谱法的自留地。

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θ + u

sinθ 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₁)/2

T 是段的总时长，微分算子 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 节点的伪谱微分矩阵 D，D @ 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，其中 N=n-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_{k+1} 的第一行。目标函数用 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], axis=1) return dX - rhs

scale 是 dt/2，不是 dt，这个因子我丢过一次，结果整个时间尺度错了，解虽然好看但完全不能用。残差函数返回的形状是 N×2，拼 NLP 约束时要拉平成向量。每段各算各的残差，段间连续性另起一个约束函数。

3.5 决策变量、初猜和 NLP 求解器选择

变量布局是一维向量：把所有段的状态和控制按段拼接。两状态单控制、3 段×8 配点的规模是 3×8×3=72 个变量，SLSQP 能直接跑。工程问题建议用 IPOPT 配稀疏雅可比，绳系模型进 NLP 就带病态，雅可比必须用符号推导或自动微分，别用有限差分，否则求解器每一步都在和数值噪声搏斗。

初猜是一条血泪经验：控制全零、状态线性插值一般能收敛，但偶尔 SLSQP 会因为初始雅可比奇异而退出。我的做法是先跑一个 3 段×4 配点的低阶离散，把粗解插值到 8 配点网格作为第二轮初猜。这个“二阶启动”比任何精调都管用，基本消灭了“初猜翻车”这一类问题。求解骨架在下面，逻辑是标准的 NLP 组装：

import scipy.optimize as opt def solve_pseudospectral(segs, dt_seg, tol=1e-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, method='SLSQP', constraints=cons, options={'maxiter': 500, 'ftol': tol} ) return res

dynamics_cons 内部遍历段，逐个调用 segment_residual；continuity_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], axis=1) err = np.linalg.norm(dX_d - rhs, axis=1) return np.max(err), err

这里的易错点是：插值对象要同时包含状态、控制和时间导数 dX。残差公式里 dX 也按插值求值，如果直接把配点上的导数搬过去，等于又回到配点验证，误差尖峰还是会被插值抹平。这一步是整个误差估计里最偷懒不得的。

4.3 p 细化还是 h 细化：用光滑性探针定方向

细化决策看两个信号。第一个是该段的插值多项式系数衰减：对每段状态 θ 的 LGL 插值系数，按 Legendre 系数从高阶往低阶看，如果系数衰减率快——相邻高阶系数比值稳定在 0.8 以下——说明解在段内光滑，误差主要来自阶数不足，升 p。第二个是衰减率慢、高阶系数振荡，说明段内有非光滑点或陡变，升 p 会触发 Runge 现象，得改 h。

实操中我常用一个探针：该段先试算 p+4 阶，看残差变化。p+4 后残差下降一个数量级，判光滑段，走 p 细化；残差基本不动，判陡变段，走 h 细化。用 p+4 而不是 p+1，是因为相邻阶次的残差差异在图上区分度不够，容易被舍入误差盖住。

4.4 两层容差与求解器参数

两层容差要拉开。伪谱残差阈值我常设 1e-6 到 1e-8，NLP 求解器的 KKT 容差要提高一到两个量级。IPOPT 里我习惯设 tol=1e-10、max_iter=500。如果 NLP 收敛在 1e-7，伪谱残差阈值却设 1e-9，细化器会一直反复触发，让你误以为网格出了问题。

注意：p 细化只改本段节点数，段边界不动；h 细化会把段劈开，连续性约束和变量编号全变。细化器里优先 p 再 h，把 h 留给真正不光滑的段。

5. 绳系伪谱法五个高频翻车现场：现象、原因、解决

5.1 配点数抬不上去：N=20 之后比 N=12 更差

现象：单段配点数超过 20，残差不降反升，NLP 求解速度肉眼可见地下降。

原因：LGL 微分矩阵条件数按 O(N²) 增长，节点本身在边界堆积、内部稀疏。提高 N 对病态问题没有帮助，反而放大舍入误差和插值振荡。绳系解光滑段用高 N 有指数收益，遇到陡变段就是灾难。

解决：劈段而不是继续升同一段的 p。我在绳系离轨任务里对比过全局 N=60 和 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 细化只能降局部误差，不能恢复指数收敛特性，最后算出一个有限差分味道的高密度网格，伪谱省变量的红利全丢。

解决：细化前做一次 p+4 探针试算，看残差是否明显下降。这个判据固化到细化器里之后，我的迭代轮次从 6 到 8 轮降到 2 到 3 轮。探针只改本段配点数、不动段边界，变量重排代价小，值得做。

6. 让 hp 自适应更稳的联动技巧：残差-阶数曲线与网格重分探针

最后分享一个细化器层面的细节，也是我后来每次做 hp 自适应都保留的习惯：每个段维护一张“残差随 p 变化”的小表。第一轮求解完，对误差超限的段先做 p+4 探针，记录残差从多少掉到多少。如果残差下降超过一个数量级，判光滑段，走 p 细化；如果下降不到 0.5 倍，判陡变段，走 h 细化。这张表同时告诉你该段需要多少阶才够。

探针试算时只改 N_k，不重排段边界。段边界是所有子区间共享的接缝，动了它，相邻段的插值基函数全要重算，连续性约束重新编号，上一轮的大部分求解信息浪费掉。p 探针的代价只是矩阵高几阶，换来的是细化方向的确定性，这笔账很划算。

细化器还要加一个回滚机制。如果新一轮网格重分后，全局残差反而比上一轮大，说明判据踩进了雷区——常见原因是某段被 h 细化劈开后，新边界落在了一个不该落的位置。回滚到上一轮网格，在该段改走 p 细化，通常能绕过去。我遇到过一次细化后数值解跳变，回滚救回。回滚条件写进细化器比手动干预可靠。

最后一件事：细化器永远要比求解器保守。伪谱解本身带数值噪声，残差在 1e-7 附近上下波动很正常，细化阈值别贴着噪声设。我会把细化阈值定在 NLP 收敛容差的 10 倍以上，给求解器留出余量。这条纪律保了我很多次。希望帮到你。

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