简介：面向航空航天与计算力学领域的研究者和工程师，这套MATLAB代码基于Gauss伪谱法求解火箭飞行轨迹，将连续最优控制问题离散为有限维非线性规划，适用于上升段轨迹优化、制导算法验证及多阶段飞行方案设计等场景。压缩包内含9个文件，包括7个.m源码文件与2个.mat数据文件，整体仅25KB，结构清晰、便于阅读。源码完整覆盖离散配点生成（如LG点）、目标函数与非线性约束定义、火箭动力学建模、优化求解主流程等关键环节。方法上，Gauss伪谱法利用高斯求积节点同时离散状态与控制变量，相比传统打靶法具有更高精度与效率，代码对核心步骤均有模块化实现。当前已有1940人学习下载，是理解伪谱法原理、掌握航天轨迹优化工程实现的实用参考，尤其适合希望从理论推导走向代码实践的初学者循序渐进地学习。
1. 为什么要用Gauss伪谱法做火箭轨迹优化
1.1 从打靶法的崩溃说起
我在本科阶段第一次做火箭轨迹优化时，用的还是传统的打靶法。打靶法的思路很直白：把初始状态当作“待射的炮弹”，不断调整初值，让终端状态命中目标。听起来简单，实际用起来却很折磨。火箭飞行是一个强非线性、强耦合过程，初值稍微给偏一点，积分到终点时速度和位置偏差可能放大好几个数量级。更麻烦的是，如果问题带有路径约束，比如动压不超过某个上限，每一轮打靶还得额外判断约束是否被突破，代码越写越乱，收敛却越来越差。
后来我理解了问题本质：轨迹优化并不是非要每一步都严格“射准”才叫最优，它本质上是一个在无穷维函数空间里找最优解的问题。打靶法是把无穷维问题压缩成有限个初值参数，而伪谱法走的路线完全不同——它直接对整条轨迹做离散近似，把无穷维问题一次性转化为有限维非线性规划。这种思路上的转变，让一大批原本很难啃的轨迹优化问题变得容易上手。
1.2 伪谱法的核心思想
伪谱法属于直接配点法的一种，但和普通的直接配点法又有明显区别。普通配点法常用局部多项式或差分格式逼近导数，相邻节点之间的信息传递依赖递推关系；而Gauss伪谱法用的是全局插值多项式，在精心选取的配点上，把状态变量、控制变量都表示成拉格朗日插值的形式，状态对时间的导数则通过导数矩阵直接算出来。
Gauss伪谱法名字里的“Gauss”来自Gauss型求积节点，这些节点是Legendre多项式的根。如果配点选取合适，插值多项式逼近光滑函数时，误差会随着配点数增加以指数速度衰减。对于火箭轨迹这类相对光滑的曲线，通常只需要二三十个配点，就能得到相当满意的分辨率。这个特性非常宝贵，因为配点数直接决定了非线性规划的变量规模，配点少意味着NLP求解快、占用内存小，调试也轻松很多。
从求解器视角看，Gauss伪谱法把最优控制问题转换为如下形式的标准NLP：
在配点上满足动力学残差约束，同时满足边界条件、路径约束与目标函数最小化。
转换完成后，数学上可以证明，当配点数趋于无穷时，NLP的解会趋于原连续时间最优控制问题的解。这就是为什么工程界对伪谱法的收敛性有信心。
2. 火箭轨迹优化的数学模型搭建
2.1 火箭运动方程怎么列
在Matlab里写轨迹优化，第一步不是急着写代码，而是先把数学模型列清楚。这里我以常用的平面运动模型为例。假设火箭在一个固定平面内飞行，把地球视为平面、忽略地球自转，状态量可以取：
- 位置坐标分量 ( x, y )
- 速度分量 ( v_x, v_y )
- 火箭总质量 ( m )
运动方程如下：
[ \begin{aligned} \dot{x} &= v_x \ \dot{y} &= v_y \ \dot{v}_x &= \frac{T \cos u - D \cos \gamma}{m} \ \dot{v}y &= \frac{T \sin u - D \sin \gamma - m g}{m} \ \dot{m} &= -\frac{T}{I{sp} g_0} \end{aligned} ]
其中 ( T ) 是发动机推力，( u ) 是推力方向角，( D ) 是气动阻力，( \gamma ) 是速度倾角，( g ) 是重力加速度，( I_{sp} ) 是比冲，( g_0 ) 是标准重力加速度。这里气动阻力通常写作 ( D = \frac{1}{2} \rho v^2 C_D A )，密度 ( \rho ) 随高度变化，可能用指数大气模型 ( \rho = \rho_0 e^{-y/H} ) 来近似。
在代码里我们会把上述方程封装成一个函数：
function Xdot = rocketDynamics(X, U, t, param) % X = [x; y; vx; vy; m] % U = [throttle; thrustAngle] x = X(1); y = X(2); vx = X(3); vy = X(4); m = X(5); T = param.Tmax * U(1); alpha = U(2); v = sqrt(vx^2 + vy^2); rho = param.rho0 * exp(-y / param.H); D = 0.5 * rho * v^2 * param.CD * param.A; g = param.g0; Xdot = [vx; vy; (T*cos(alpha) - D*vx/v)/m; (T*sin(alpha) - D*vy/v)/m - g; -T/(param.Isp*g0)]; end2.2 性能指标与约束条件
火箭轨迹优化的目标函数常见有三种：最小化飞行时间、最小化燃料消耗、最大化终点速度。工程上经常使用最小化燃料消耗，因为燃料直接对应成本。在伪谱法框架里，目标函数很容易写：
[ J = -m(t_f) ]
因为终点质量越大意味着消耗燃料越少。如果写成最小化时间，则 ( J = t_f )。有时候为了兼顾最优性和数值稳定性，也会把目标函数写成“终点质量 + 松弛变量惩罚”的形式。
约束条件分几类：
- 边界约束：发射时位置速度给定，终端高度或速度给定。
- 路径约束：动压 ( q < q_{max} )、过载 ( n < n_{max} )、热流密度受限。
- 控制约束：推力大小限制在 ( [T_{min}, T_{max}] )，推力方向角限制在合理区间内。
这些约束在Matlab里就是一组函数，伪谱法离散化后，它们会变成NLP中的等式约束和不等式约束。值得一提的是一条经验：路径约束不要写得过紧，比如动压上限加个1%裕度，否则求解器会卡在约束边界上来回挣扎，收敛速度骤降。
2.3 整理成Gauss伪谱法需要的标准形式
所有轨迹优化问题，最终都要归纳为下面的标准形式：
[ \begin{aligned} \min \quad & J = \Phi(X(t_0), t_0, X(t_f), t_f) + \int_{t_0}^{t_f} L(X,U,t) dt \ \text{s.t.} \quad & \dot{X} = f(X, U, t) \ & C(X,U,t) \le 0 \ & \phi(X(t_0), t_0, X(t_f), t_f) = 0 \end{aligned} ]
Gauss伪谱法做的事情，就是把连续时间 ( t \in [t_0, t_f] ) 映射到归一化区间 ( \tau \in [-1, 1] )，然后在 ( \tau ) 域上选取配点。映射关系非常简单：
[ t = \frac{t_f + t_0}{2} + \frac{t_f - t_0}{2} \tau ]
这个归一化处理不仅仅是为了数学上的便利，更重要的是能让所有状态量在差不多的尺度范围内参与计算，避免由于时间区间过长导致的缩放不适。后面调试时你就会发现，单位不统一导致的问题远比算法本身的问题多。
3. Matlab实现全流程
3.1 整体代码架构设计
Matlab里实现Gauss伪谱法，我习惯拆成四个模块，分别是：
| 模块 | 职责 | 对应文件 |
|---|---|---|
| 配点生成 | 计算LGL节点、微分矩阵、积分权值 | getGaussNodes.m |
| 问题定义 | 目标函数、动力学、约束、边界条件 | problemDef.m |
| NLP装配 | 把连续问题离散成NLP变量和约束 | buildNLP.m |
| 求解与后处理 | 调用fmincon/ipopt，画轨迹图 | runTrajectory.m |
这种拆分方式的好处是，你换一个火箭模型或者换一组约束，只需要改problemDef.m，配点和装配模块完全不用动。如果以后想让代码更专业，可以在这个基础上把getGaussNodes.m替换成更高效的节点计算方法，其他模块不受影响。
Matlab里用fmincon就能跑通整个流程，因为fmincon内置了内点法和序列二次规划法。但是要注意，fmincon要求提供梯度信息才会快，否则会反复用有限差分去近似梯度，配点数一多就慢得难以接受。我建议至少给目标函数和约束函数提供解析梯度，或者使用Matlab的自动微分工具箱把它们算出来。如果问题规模超过几百个变量，换用IPOPT配合Matlab接口是更靠谱的选择。
3.2 微分矩阵与离散化装配
Gauss伪谱法的核心操作，就是用微分矩阵 ( D ) 把一个状态序列的导数近似成矩阵乘法。具体来说，如果状态序列是 ( X_1, X_2, \ldots, X_N )（( N ) 个配点），那么：
[ \dot{X}(\tau_k) \approx \sum_{i=1}^{N} D_{k,i} X_i ]
在我们这套实现里，动力学约束就变成了：
[ \sum_{i=1}^{N} D_{k,i} X_i - \frac{t_f - t_0}{2} f(X_k, U_k, t_k) = 0 ]
注意这里多乘了一个 ( \frac{t_f - t_0}{2} )，这是时间映射产生的尺度因子，漏掉它会导致所有导数量级出错。
装配NLP时的代码逻辑大致如下：
% 假设配点数为 N % 状态量全部堆叠成向量 Xvec = [x1;y1;vx1;vy1;m1; x2;y2;...; xN;yN;vxN;vyN;mN] % 控制量堆叠成 Uvec % 时间映射: t = (tf + t0)/2 + tau*(tf - t0)/2 % 动力学残差约束 Aeq_dyn * [Xvec; Uvec; t0; tf] = 0 Aeq_dyn = zeros(N * nx, nx*N + nu*N + 2); for k = 1:N rowIdx = (k-1)*nx + 1 : k*nx; % D矩阵作用于所有配点上的状态 for i = 1:N colIdx_state = (i-1)*nx + 1 : i*nx; Aeq_dyn(rowIdx, colIdx_state) = D(k,i) * eye(nx); end colIdx_control = nx*N + (k-1)*nu + 1 : nx*N + k*nu; % 非线性项放到函数里，fmincon只支持线性等式矩阵， % 更推荐把动力学残差放到非线性约束而非线性等式约束里 end实际编码时，动力学约束通常不是线性相关的，因为方程右侧包含状态和控制相乘的项。因此最终要把动力学残差写入nonlcon函数，而不是试图构造一个线性等式矩阵。这一点新手很容易走弯路：看到别人代码里有Aeq矩阵，以为什么问题都能用线性约束表达，实际那是已经线性化过的特例。
3.3 构造目标函数与约束函数
在Matlab中，伪谱法离散后的目标函数一般由两项组成：终点性能（比如质量）和高斯积分项。高斯积分项的近似式是：
[ \int_{t_0}^{t_f} L dt \approx \frac{t_f - t_0}{2} \sum_{k=1}^{N} w_k L(X_k, U_k, t_k) ]
其中 ( w_k ) 是Gauss求积权值。如果目标是最大化终点质量，就没有积分项，只写 ( J = -m_f )。如果目标是时间最短，则直接 ( J = t_f )。如果问题中包含需要积分的中间项，比如燃料消耗的积分，就用上面的高斯积分公式近似。
需要传给fmincon的目标函数写成：
function [J, gradJ] = objFun(z) Xvec = z(1:nx*N); Uvec = z(nx*N+1:nx*N+nu*N); t0 = z(end-1); tf = z(end); % 重构状态矩阵 X = reshape(Xvec, nx, N)'; U = reshape(Uvec, nu, N)'; J = -X(end, 5); % 最大化终端质量 end约束函数同样分成等式约束和不等式约束两部分：
function [c, ceq] = nonlcon(z) % 拆包 % ceq: 动力学残差, 初始状态约束, 终端状态约束 % c: 动压/过载/控制幅值等不等式约束 end这里有个重要细节：fmincon默认将等式约束的容差和不等式约束的容差都设置为1e-6左右。对于实际轨迹优化来说，这个精度足够了。但如果你发现终端位置总差几米，先把容差调到1e-8试试，通常就能解决，不要一上来就怀疑算法错了。
4. 调试实战：我踩过的坑和解决办法
4.1 初值猜测到底多重要
伪谱法对初值的敏感度虽然比打靶法低，但不代表可以随便给。我最早做垂直起降火箭轨迹时，初值猜测直接给了一条水平直线，结果NLP求解器一直说“No feasible solution found”。后来我先把问题简化成无约束情况，给一条抛物线轨迹作为初始猜测，再用这个解作为带约束问题的初值，很快就收敛了。
这里分享一个比较稳妥的初值猜测策略：
- 先用解析方法或简单的最短时间估计得到一个粗略轨迹。
- 给状态变量做一个单调递增或递减的插值，保证初始猜测满足边界条件。
- 控制变量给一个常数猜测，比如推力0.8倍最大推力，角度0度。
- 如果路径约束导致收敛困难，先把约束放得很宽，求出初解后再逐步收紧约束。
在工程实践中，初值猜测的工程意义超过算法本身。哪怕你对最优解一无所知，也要尽量让初始猜测在物理上合理，比如速度不能为负、质量单调递减、轨迹单调上升。给一个物理上违反常识的初值，再强的求解器也救不回来。
4.2 无量纲化与缩放处理
这一节值得单独拿出来说，因为十次轨迹优化有八次问题出在缩放上。火箭轨迹里的状态量跨度极大：位置是千米量级，速度是千米每秒量级，质量是百吨量级，时间常数却可能是几十秒。如果直接把国际单位制放进NLP，雅可比矩阵的元素数值差异可能超过10个数量级，导致求解器的变量归一化机制失效，收敛精度大打折扣。
我的做法是定义一组基准量，把所有状态转化成无量纲量：
- 长度基准 ( L_{ref} = R_0 )（地球半径或参考航程）
- 速度基准 ( V_{ref} = \sqrt{g_0 R_0} )
- 时间基准 ( T_{ref} = L_{ref} / V_{ref} )
- 质量基准 ( m_{ref} = m_0 )（起飞质量）
无量纲化之后，状态量都在0到1的范围内。控制量推力比范围已经是[0,1]或[0.2,1]，角度本身就是无量纲弧度制，不需要再缩放。这套处理做完，求解速度能提升一个数量级，而且解的稳定性显著提高。
经验法则：如果你发现IPOPT或fmincon迭代几百次KKT残差仍在 ( 10^{-3} ) 徘徊，先别调算法参数，回去检查变量缩放。
4.3 配点数N怎么选
配点数的选择直接影响求解效率和精度。我从试算经验中总结出的规律是：
| 配点数N | 适用场景 | 典型精度 |
|---|---|---|
| 8-12 | 可行性研究，粗略估计轨迹形状 | 位置误差百米级 |
| 16-24 | 标准轨迹优化，论文演示 | 位置误差米级 |
| 30-50 | 高精度轨控策略，需要精确复现 | 位置误差亚米级 |
| 80以上 | 很少用，容易产生龙格现象和病态 | 不建议 |
配点数太多并不总是好事。Gauss伪谱法使用全局多项式，当N超过50后，矩阵条件数会迅速增长，数值误差反而可能增加。如果确实需要很高的分辨率，正确做法是把轨迹分段，每段用独立的伪谱离散，段间通过连续性条件连接。这个思路在工程上叫“多段伪谱法”。
4.4 常见报错速查表
我整理了一份自己在调试过程中常用的排查表：
| 现象 | 可能原因 | 处理方式 |
|---|---|---|
| 求解器报Infeasible | 初值不满足边界约束 | 检查Xvec初始值是否为边界条件的线性插值 |
| 迭代不动，目标值一直不变 | 梯度缺失，数值差分精度不足 | 开启fmincon的SpecifyConstraintGradient，认真写解析梯度 |
| 动力学残差总在特定配点偏大 | 斜率突变或参数变化剧烈 | 增加配点数或采用多段伪谱法 |
| 结果抖动剧烈，不光滑 | 配点数过少或控制量限制过松 | 增大N或检查动力学单位 |
| KKT残差收敛很慢 | 缩放问题 | 做无量纲化处理，检查雅可比矩阵条件数 |
5. 扩展思路：从仿真到工程应用
5.1 轻量化实现与替代工具箱
如果时间充裕，完全可以自己实现整套流程；如果时间紧张，建议直接参考成熟工具箱。GPOPS是Gauss伪谱法领域知名度很高的Matlab工具箱，内部实现了配点生成、缩比变换、NLP接口，用户只需要提供问题定义函数，省去大量底层工作。它用的核心配点方案就是Gauss-Lobatto和Gauss配点，原理和我前面讲的完全一致。
不过，我仍然推荐自己动手实现一遍基础版本。原因很简单：用工具箱能解决眼前问题，但遇到工具箱不支持的奇异工况，比如变比冲、变推力上限、质量突变分离，或者多级火箭分离环节，你还是得理解底层逻辑才能正确建模。我自己做的垂直起降轨迹优化，最后一部分边界条件就是工具箱不好处理的，必须手工装配NLP。
5.2 多级火箭与分离过程的处理思路
真实火箭飞行的最大特点是不连续：级间分离的一瞬间，质量和推力突变，不能简单当作光滑函数处理。标准Gauss伪谱法要求状态光滑，遇到不连续问题就会失效。工程上一般把飞行过程按事件拆分成多个段：
- 一级推进段
- 级间滑行段
- 二级点火段
- 关机段
每个段单独做伪谱离散，段间通过连续性条件连接：上一段终端位置速度等于下一段起始位置速度，但质量可以有跳变，因为级间分离抛掉了结构质量。用多段伪谱法做这件事，需要额外引入连接条件和箱约束，NLP变量的规模也会对应增加。
5.3 在线轨迹重规划方向
火箭飞行中如果遇到突发风场或者推力偏差，需要在线重新规划轨迹。传统伪谱法单次求解可能需要百毫秒甚至秒级时间，直接在飞行计算机上做在线规划压力很大。但这几年有个思路逐渐成熟：预先离线计算大量不同工况下的最优轨迹，用机器学习或者查表插值的方式生成一个近似解，作为在线Gauss伪谱法求解的初值，这样迭代次数能大幅压缩。这也是我认为未来几年把伪谱法推向实际飞行控制最可行的一条路。
实际操作中我最大的体会是，Gauss伪谱法这么好用的原因不是“配点法本身有多神秘”，而是它把工程问题建模成NLP的路径足够短，调试工具足够成熟。只要模型建对、初值给稳、缩放做足，Matlab里几百行代码就能跑通一条完整火箭飞行轨迹。后续如果想继续深挖，建议把注意力放在多段离散和应用场景扩展上，而不是反复纠结算法本身。最后再多说一句，遇到求解器不收敛别硬扛，先回到简化模型上确认算法正确性，再逐步加约束，往往比盲目调参数有效得多。
本文还有配套的精品资源，点击获取
