Gauss伪谱法火箭轨迹优化：Matlab实现与调试实战

简介面向航空航天与计算力学领域的研究者和工程师这套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.mNLP装配把连续问题离散成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_{i1}^{N} D_{k,i} X_i ]在我们这套实现里动力学约束就变成了[ \sum_{i1}^{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_{k1}^{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*N1:nx*Nnu*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里几百行代码就能跑通一条完整火箭飞行轨迹。后续如果想继续深挖建议把注意力放在多段离散和应用场景扩展上而不是反复纠结算法本身。最后再多说一句遇到求解器不收敛别硬扛先回到简化模型上确认算法正确性再逐步加约束往往比盲目调参数有效得多。本文还有配套的精品资源点击获取