为什么复现这篇论文
Hargraves 和 Paris 在 1987 年发表的 Direct Trajectory Optimization Using Nonlinear Programming and Collocation 是直接轨迹优化路线中的一篇经典短文。它的核心思想很工程化：
不先推导庞大的协态方程，而是直接把状态、控制和时间离散成有限维变量，再用非线性规划求解。
这条路线后来成为很多轨迹优化工具的基础思路。现代工程中常见的 direct collocation、direct transcription、Hermite-Simpson、pseudospectral method，本质上都沿着这条路继续发展。
我这次复现的目标不是复制 Boeing 当年的 NPDOT 程序，而是复现论文中最关键的数值转录方法：
- 状态变量用三次 Hermite 多项式表示；
- 控制变量在节点之间线性插值；
- 动力学方程在每段中心点强制满足；
- 轨迹优化问题转成 NLP；
- 用 SQP 类优化器求解。
代码和验证结果保存在本地：
/home/ubuntu/repro/direct-collocation
论文方法：把最优控制问题变成 NLP
论文考虑一般形式的轨迹优化问题：
$$ \begin{aligned} \min \quad & J = \Phi(x(E), u(E), \omega, E) \\ \text{s.t.} \quad & \dot{x} = f(x, u, \omega, t) \end{aligned} $$其中状态、控制、事件时间、设计参数一起构成 NLP 的决策变量。论文把所有变量收集为一个向量：
$$ P = \begin{bmatrix} Z^T & E^T & u^T \end{bmatrix}^T $$然后把动力学缺陷、边界条件和路径约束都写成非线性约束：
$$ C(P) = 0 \quad \text{or} \quad \ell \le C(P) \le u $$最终得到标准非线性规划问题。
三次 Hermite 状态插值
对每一个离散段，状态用三次多项式表示：
$$ x(s) = C_0 + C_1 s + C_2 s^2 + C_3 s^3, \qquad s \in [0, 1] $$端点状态为 x_i, x_{i+1}，端点导数由动力学给出：
段长为 h。根据 Hermite 插值，中心点状态为：
中心点处由多项式导出的斜率为：
$$ \dot{x}_c^H = \frac{3}{2h}(x_{i+1} - x_i) - \frac{1}{4}(f_i + f_{i+1}) $$控制变量线性插值得到中心控制：
$$ u_c = \frac{1}{2}(u_i + u_{i+1}) $$于是每一段的 collocation defect 是：
$$ \Delta_i = \dot{x}_c^H - f(x_c, u_c) = 0 $$这就是我复现代码中的核心。
Python 实现
使用 Python + SciPy 实现 NLP。核心函数如下：
def defects(z, p):
x, y, v, theta, tf = unpack(z, p)
h = tf / p.n_segments
cons = []
for i in range(p.n_segments):
yi = np.array([x[i], y[i], v[i]])
yj = np.array([x[i + 1], y[i + 1], v[i + 1]])
fi = dynamics(yi, theta[i], p)
fj = dynamics(yj, theta[i + 1], p)
yc = 0.5 * (yi + yj) + h / 8.0 * (fi - fj)
thetac = 0.5 * (theta[i] + theta[i + 1])
fc = dynamics(yc, thetac, p)
slope_c = 1.5 * (yj - yi) / h - 0.25 * (fi + fj)
cons.extend(slope_c - fc)
return np.asarray(cons)
优化器使用 scipy.optimize.minimize(method="SLSQP")。这和论文里使用 NPSOL 的路线一致：都是 SQP 类非线性规划求解器。
验证问题：brachistochrone
论文提到 NPDOT 验证过四类例子：
- Van der Pol；
- Brachistochrone；
- Supersonic interceptor minimum-time climb；
- Advanced booster trajectory。
但这篇论文只有 5 页，很多工程例子的气动、推进、燃耗表并没有直接给出。因此最适合独立验证的是 brachistochrone，因为无约束版本有解析 cycloid 解。
我设置的验证问题为：
$$ \begin{aligned} x(0) &= 0, & y(0) &= 1, & v(0) &= 0, \\ x(t_f) &= 1, & y(t_f) &= 0, & g &= 1, \\ \min \quad & t_f \end{aligned} $$动力学为：
$$ \begin{aligned} \dot{x} &= v\cos\theta, \\ \dot{y} &= v\sin\theta, \\ \dot{v} &= -g\sin\theta. \end{aligned} $$这里 theta 是控制角，y 正方向向上。为了下降，优化器会选择负的 theta。
解析解对照
无约束 brachistochrone 的解析解是摆线：
$$ \begin{aligned} x &= a(\phi - \sin\phi), \\ y_{\mathrm{drop}} &= a(1 - \cos\phi), \\ t_f &= \phi_f \sqrt{\frac{a}{g}}. \end{aligned} $$对于起点 (0, 1)、终点 (1, 0)、g = 1，求得：
这给了我们一个很干净的 benchmark：数值配点结果应该随着段数增加逐渐收敛到 1.825682189397。
收敛结果
使用不同段数运行直接配点 NLP：
| segments | success | final time | analytic time | abs error | rel error | max defect residual |
|---|---|---|---|---|---|---|
| 5 | True | 1.825708717 | 1.825682189 | 2.653e-05 | 1.453e-05 | 3.525e-11 |
| 10 | True | 1.825693832 | 1.825682189 | 1.164e-05 | 6.377e-06 | 1.129e-10 |
| 20 | True | 1.825687905 | 1.825682189 | 5.715e-06 | 3.131e-06 | 7.892e-11 |
可以看到两个结果：
- 终端时间随着段数增加收敛到解析解；
- 动力学配点残差保持在
1e-10量级。
这说明 Hermite midpoint collocation 的转录本身是正确工作的。
下图是 N=20 时，数值轨迹与解析摆线的对比：
单独看数值节点和 Hermite 轨迹：
速度和控制角变化：
路径约束验证
论文中特别强调直接法的一个优势：容易处理路径约束。为此我也做了一个 constrained brachistochrone-like 版本，在节点和中心点都检查路径约束：
$$ y \ge y_{\mathrm{floor}}(x) $$对应代码中，节点约束和中心点约束都会进入 NLP：
vals.extend(y - floor_y(x, p))
...
for each segment:
yc = Hermite midpoint state
vals.append(yc[1] - floor_y(yc[0], p))
收敛结果：
| segments | success | final time | max defect residual |
|---|---|---|---|
| 5 | True | 1.825708717 | 8.431e-13 |
| 10 | True | 1.825693831 | 7.761e-11 |
| 20 | True | 1.825687905 | 1.026e-10 |
当前这个路径约束设置没有显著改变最优轨迹，因此数值和无约束情况接近。但这个实验验证了两点：
- 路径约束能以普通 NLP inequality 的形式加入；
- 约束可以同时在节点和段中心点检查，这和论文描述一致。
论文中工程例子的复现边界
复现过程中一个重要原则是：论文没给的数据，不要自己编。
Supersonic interceptor minimum-time climb
论文中给出的可用信息包括：
初始：sea level, Mach 0.38
终端：altitude 20 km, Mach 1.0
初始重量：42,000 lb
控制：pitch function
初始猜测：final range = 360,000 ft
初始猜测：final mass = 1204 slugs
初始控制：pitch = 0.18 rad
8 段 Chebyshev 分布结果：time to climb = 325.2 s
15 段非均匀分布结果：time to climb = 317.3 s
但它没有给出完整的：
- 气动系数表；
- 推力随 Mach/高度变化表；
- 燃耗数据；
- NACA-1962 atmosphere 的具体实现细节；
- Ref. 35 中使用的原始数据。
所以我没有声称复现了 317.3 s。准确复现这个例子，需要继续找到 Bryson、Desai、Hoffman 1969 年那篇 Energy-State Approximation in Performance Optimization of Supersonic Aircraft 的数据表。
Advanced booster
论文给出的信息是：
Mach 0-2：第一推进阶段
Mach 2-20：第二推进阶段
Mach >20：火箭阶段
三维球形非旋转地球动力学
路径约束：动压、高度、攻角
NPDOT 使用 22 段
最终比 baseline 重量提升 10%
但同样缺少车辆、气动、推进、目标轨道和 baseline 轨迹数据。因此只能记录论文报告值，不能精确数值复现。
这次复现得到的经验
1. 直接配点法的核心很短
真正核心的数学公式只有几行：
$$ \begin{aligned} x_c &= \frac{1}{2}(x_i + x_{i+1}) + \frac{h}{8}(f_i - f_{i+1}), \\ \dot{x}_c^H &= \frac{3}{2h}(x_{i+1} - x_i) - \frac{1}{4}(f_i + f_{i+1}), \\ \Delta_i &= \dot{x}_c^H - f(x_c, u_c). \end{aligned} $$但这几行把连续时间最优控制问题变成了有限维 NLP。
2. 路径约束是直接法的强项
间接法处理路径约束时通常会牵涉复杂的切换结构、乘子条件和边界弧。直接法则可以直接写成：
$$ g(x_i, u_i) \ge 0, \qquad g(x_c, u_c) \ge 0. $$然后交给 NLP 求解器。
3. 论文复现要区分“方法复现”和“数据复现”
这篇论文的方法已经可以完整复现；但工程例子因为缺数据，不能完整复现论文中的数值表。把这两件事区分开很重要：
- 方法复现：实现 Hermite collocation + NLP，并用可验证问题证明正确；
- 数据复现：需要论文或引用文献中完整的气动、推进、环境模型和约束数据。
4. SLSQP 能跑通，但不是最终形态
SciPy SLSQP 足够验证方法，但对于更大的飞行器轨迹优化问题，建议迁移到：
- CasADi + IPOPT；
- JAX / CasADi 自动微分；
- 稀疏 Jacobian；
- mesh refinement。
这会更接近现代轨迹优化工具链。
下一步
后续可以沿两条路线继续：
- 算法路线：实现自动网格细化，比较 10、20、40、80 段下的误差和计算时间；
- 工程路线：寻找 Ref. 35 的超音速飞机数据表，尝试复现论文报告的
317.3 sminimum-time climb。
这篇论文的价值在于，它把最优控制问题从“解析推导协态方程”推进到了“构造有限维 NLP 并交给优化器”。这也是现在很多工程轨迹优化工具仍在使用的基本思想。
参考文献
- Hargraves, C. R., & Paris, S. W. (1987). Direct Trajectory Optimization Using Nonlinear Programming and Collocation. Journal of Guidance, Control, and Dynamics, 10(4), 338–342.
- Bryson, A. E., & Ho, Y. C. (1969). Applied Optimal Control. Blaisdell.
- Bryson, A. E., Desai, M. N., & Hoffman, W. C. (1969). Energy-State Approximation in Performance Optimization of Supersonic Aircraft. Journal of Aircraft, 6(6), 481–488.